陳 航,陳敏花,蔣催催,郇文秀,潘 菲,胡慶雷
(1.北京航空航天大學 自動化科學與電氣工程學院·北京·100191;2.上海航天控制技術(shù)研究所·上?!?01109)
近年來,臨近操作已成為航天領(lǐng)域中的重要研究方向,也是各個國家的重點工程項目之一,其主要包括在軌相對觀測與監(jiān)視、在軌燃料補給,以及在軌組裝[1]等。在典型非合作場景中,相對姿態(tài)確定是重要且困難的任務(wù)之一,也是完成其他任務(wù)的重要基礎(chǔ)。一方面,由于目標非合作特性和微小衛(wèi)星對容納載荷體積的限制,無法實現(xiàn)可靠的相對角速度測量。在這種情況下,如何在缺少角速度測量的情況下完成對相對姿態(tài)的確定,是一個關(guān)鍵問題;另一方面,由于太空環(huán)境光照[2]等不確定因素和末端執(zhí)行機構(gòu)對視覺傳感器的遮擋[3],目標航天器的觀測校正信息無法及時被獲取,需對目標姿態(tài)進行預(yù)測,以保障后續(xù)任務(wù)執(zhí)行的安全可靠。因此,在姿態(tài)確定中考慮一種新型、有效的航天器姿態(tài)估計與預(yù)測方法,處理上述兩個問題,具有重要的工程實際意義。
卡爾曼濾波算法(Kalman Filter,KF,以下簡稱KF算法)被廣泛應(yīng)用于各類航天器的姿態(tài)確定任務(wù)中。KF算法通??煞譃閿U展卡爾曼濾波器[4](Extended Kalman Filter,EKF)、乘性擴展卡爾曼濾波器[5](Multiplicative Extended Kalman Filter,MEKF)和其他方法[6]。在Lefferts等[7]提出的MEKF算法中,算法采用陀螺儀測量模型確定角速度。Filipe等[8]繼承并提出了對偶角速度測量模型。但是,在非合作背景下,信息無法交互,也無法傳遞角速度信息,上述方法無法滿足任務(wù)要求。此外,為精確估計目標航天器的角速度,轉(zhuǎn)動慣量也是需要獲取的關(guān)鍵參數(shù)之一。Psiaki[9]提出了一種基于最小二乘的方法,以估算航天器姿態(tài)動力學的相關(guān)參數(shù)。在Yoon等[10]提出的姿態(tài)參數(shù)校準卡爾曼濾波算法中,除航天器的姿態(tài)和角速度外,算法還考慮了航天器和飛輪轉(zhuǎn)動慣量的估計。Xu等[11]提出了一種采用序列圖像進行慣性參數(shù)估計的方案。Setterfield[12]提出了使用本體極跡分析來估計轉(zhuǎn)動慣量的算法,但只考慮了主軸轉(zhuǎn)動慣量。上述文獻均未考慮視覺導(dǎo)航中存在的慣量積估計問題。因此,在缺少角速度測量信息的情況下估計完整轉(zhuǎn)動慣量矩陣,是一個亟待解決的問題。
視覺導(dǎo)航算法在非合作任務(wù)中廣受青睞,而另一個挑戰(zhàn)則來自于由視覺特征遮擋而導(dǎo)致的目標航天器跟蹤姿態(tài)信息的丟失。Yu等[13]為空間機器人開發(fā)了一種融合觸覺反饋和視覺觀測的框架,以解決目標跟蹤和定位問題。Cheung等[14]考慮到視線遮擋,設(shè)計了一種視覺目標運動估計算法,但該算法僅能處理地面運動物體,對空間翻滾航天器不適用。閆謙時[15]利用最小描述長度準則和貝葉斯估計方法確定了時間序列預(yù)測模型的階數(shù)和參數(shù),對航天器遙測數(shù)據(jù)的趨勢變化進行了預(yù)測。但是,該方法的長期預(yù)測精度較低。由于沒有可用的校正信息,姿態(tài)預(yù)測問題的實質(zhì)是求取姿態(tài)運動的解析解。航天器姿態(tài)運動學為強非線性函數(shù),其微分方程不具有解析解。采用傳統(tǒng)數(shù)值積分方法能夠?qū)崿F(xiàn)短時間內(nèi)的較高精度預(yù)測,但是由于隨時間累積的誤差,此類方法同樣不適用于較長時間內(nèi)的姿態(tài)預(yù)測。
針對上述問題,本文設(shè)計了一種乘性擴展卡爾曼濾波方法,通過將轉(zhuǎn)動慣量的相對比值增加到濾波狀態(tài)量中,實現(xiàn)了對完整轉(zhuǎn)動慣量矩陣比值的估計,提高了姿態(tài)和角速度估計的精度,并且無需獲取角速度測量信息;然后,分別基于函數(shù)擬合和神經(jīng)網(wǎng)絡(luò),設(shè)計了兩種預(yù)測方法,實現(xiàn)了較長時間內(nèi)對目標航天器姿態(tài)的預(yù)測。本文結(jié)構(gòu)安排如下:在第1節(jié)介紹了非合作航天器的姿態(tài)動力學和視覺導(dǎo)航問題;在第2節(jié)給出了基于MEKF的姿態(tài)和參數(shù)估計算法;在第3節(jié)設(shè)計了函數(shù)擬合和神經(jīng)網(wǎng)絡(luò)兩種姿態(tài)預(yù)測方法;在第4節(jié),通過數(shù)值仿真驗證了濾波估計算法和姿態(tài)預(yù)測算法的實時性和準確性;在第5節(jié)得出了結(jié)論。
如圖1所示,將依據(jù)視覺觀測建立的目標航天器坐標系定義為FL={XL,YL,ZL},將追蹤航天器本體系定義為FB={XB,YB,ZB}。
圖1 相對位姿確定問題的示意圖Fig.1 Schematic diagram of relative attitude
假定FB為慣性系,并定義從坐標系FB到FL的姿態(tài)四元數(shù)為q,則本文采用四元數(shù)描述目標航天器姿態(tài)運動學和動力學,具體如下
(1)
(2)
qTq=1
(3)
本文所使用的四元數(shù)運算定義如下:
四元數(shù)乘法
(4)
共軛
(5)
ω=[ω1,ω2,ω3]T,為目標航天器本體系相對慣性系的角速度,并被表示在追蹤航天器本體系下。但是,目標航天器本體系在視覺導(dǎo)航中不是先驗已知,轉(zhuǎn)動慣量主軸不一定與通過視覺觀測建立的目標航天器坐標軸重合。此時,其轉(zhuǎn)動慣量矩陣J不是對角矩陣,即慣量積不為零。
(6)
(7)
外部擾動力矩τ由重力梯度力矩、磁力矩、太陽光壓力矩等組成,可建模為高斯白噪聲。
文獻[10]指出,在視覺導(dǎo)航的任務(wù)場景中,目標航天器轉(zhuǎn)動慣量的真實值并不能從相對姿態(tài)測量中獲得。因此,本文設(shè)計了濾波算法,以估計轉(zhuǎn)動慣量的相對比值。從原理來看,轉(zhuǎn)動慣量矩陣中的六個獨立元素均可作為基準。本文選取其中的Jxx,并將式(2)改寫為如下形式
(8)
其中,J′定義為轉(zhuǎn)動慣量的相對比值矩陣
(9)
目標航天器姿態(tài)確定的過程僅用轉(zhuǎn)動慣量的相對比值便可實現(xiàn)。只需采用合適的估計算法,轉(zhuǎn)動慣量矩陣的剩余元素J5=[Jyy,Jzz,Jxy,Jxz,Jyz]便可收斂到各項相對于Jxx的比值。
本節(jié)不涉及視覺姿態(tài)解算,同時假定可獲得相對于追蹤航天器的目標航天器的相對姿態(tài)測量量。
定義目標航天器的誤差狀態(tài)量如下
(10)
(11)
(12)
將剛體航天器動力學(2)代入誤差角速度的定義式(11),得到
(13)
(14)
依次考慮上式相對各狀態(tài)量的雅可比矩陣
(15)
(16)
(17)
其中,對任意三維向量a,矩陣χ5(a)定義如下
文獻[7]給出了誤差四元數(shù)矢量部分的狀態(tài)方程
(18)
卡爾曼濾波的狀態(tài)方程為
(19)
式中,x(t)為狀態(tài)變量,F(xiàn)(t)為系統(tǒng)狀態(tài)矩陣,w(t)為系統(tǒng)過程噪聲,G(t)為系統(tǒng)噪聲矩陣。
結(jié)合公式(15)、公式(16)、公式(17)和公式(18),可推導(dǎo)乘性擴展卡爾曼濾波的狀態(tài)方程
(20)
其中
(21)
(22)
(23)
(24)
ητ、ηJ5分別對應(yīng)δω、δJ5的過程噪聲向量。
然后,計算狀態(tài)轉(zhuǎn)移矩陣Φ
(25)
其中,Δt為離散化時間間隔。
Z(t)=Hx(t)+v3(t)
(26)
其中,H=[I3×3,03×3,03×5]為測量矩陣,v3(t)為測量噪聲。
乘性擴展卡爾曼濾波器的建立分為狀態(tài)一步預(yù)測、濾波觀測量計算、濾波更新和姿態(tài)校正四個部分,具體步驟如下:
(1)狀態(tài)一步預(yù)測
(27)
(28)
(2)濾波觀測量計算
根據(jù)目標航天器姿態(tài)四元數(shù)的觀測值和預(yù)測值,計算姿態(tài)誤差四元數(shù)
(29)
(3)濾波更新
Pk|k-1=ΦPk-1ΦT+Qk-1
(30)
Kk=Pk|k-1HT(HPk|k-1HT+Rk)-1
(31)
(32)
xk=xk-1+Kk(δqv,k-Hxk-1)
(33)
其中,Pk|k-1為k時刻最優(yōu)預(yù)測估值誤差協(xié)方差陣;Pk-1為k-1時刻最優(yōu)濾波值誤差協(xié)方差陣;Kk為k時刻增益矩陣;Qk-1為k-1時刻系統(tǒng)噪聲方差矩陣;Rk為測量方差矩陣。
(4)姿態(tài)校正
利用范數(shù)約束計算誤差四元數(shù),并校正姿態(tài)
(34)
完成濾波后,根據(jù)對目標姿態(tài)的實時估計結(jié)果,本節(jié)設(shè)計了函數(shù)擬合和神經(jīng)網(wǎng)絡(luò)兩種方法,在沒有校正信息的情況下,實現(xiàn)了在未來一段時間內(nèi)對非合作目標姿態(tài)的精確預(yù)測。
擬合曲線基函數(shù),直接影響擬合效果。觀察圖2中姿態(tài)四元數(shù)的各分量波形,其周期性特征和規(guī)律特征比較明顯。
圖2 姿態(tài)四元數(shù)函數(shù)波形Fig.2 Estimated attitude quaternion
曲線函數(shù)應(yīng)建立在空間無控翻滾航天器的物理模型基礎(chǔ)上,其基本特征應(yīng)滿足如下兩個要求:
(1)姿態(tài)四元數(shù)各個分量均具備類似周期性的特征,其時間曲線圍繞0上下振蕩,且最大值不大于1,最小值不小于-1。擬合函數(shù)和擬合函數(shù)的一階導(dǎo)函數(shù)均應(yīng)為周期震蕩;
(2)擬合函數(shù)的一階導(dǎo)函數(shù)在數(shù)學形式上應(yīng)與擬合函數(shù)保持一致。
考慮將姿態(tài)四元數(shù)的各個分量擬合為正弦函數(shù)和的形式,函數(shù)的具體形式如下
(35)
式中,yk為姿態(tài)四元數(shù)的各個分量;t為時刻;ai、bi、ci均為待確定參數(shù);n為正弦函數(shù)項數(shù),擬合效果隨項數(shù)增加而提升。一般而言,項數(shù)為3或4較為合適。采用最小二乘法確定各項參數(shù)。
為保證快速收斂,選取濾波最后階段的姿態(tài)數(shù)據(jù)作為擬合目標。此時,已較好地濾除了過程和觀測噪聲。姿態(tài)四元數(shù)標量的擬合效果如圖3所示。
圖3 姿態(tài)四元數(shù)標量的擬合效果Fig.3 Function fitting effect of attitude quaternion scalar term
圖3中,黑色曲線為q0,kf,即經(jīng)濾波處理后的姿態(tài)四元數(shù)標量曲線,以該曲線為示例進行函數(shù)擬合;藍色實線為q0,predict,即為擬合預(yù)測結(jié)果,與濾波曲線q0,kf在3000s~4000s內(nèi)基本吻合。本方法獲得了姿態(tài)四元數(shù)標量的近似函數(shù)解析式,可在給定時域以外計算函數(shù)值,具備一定的預(yù)測能力。
傳統(tǒng)的數(shù)值積分方法的預(yù)測精度受初始誤差影響較大,存在誤差隨時間累積而帶來的穩(wěn)定性問題,無法滿足航天器姿態(tài)預(yù)測對精度的要求??紤]到臨近操作任務(wù)對實時性的要求、航天器姿態(tài)運動的非線性及其迅速的變化,BP神經(jīng)網(wǎng)絡(luò)具備以任意精度逼近任意非線性映射關(guān)系的能力,可以用于處理強非線性的剛體姿態(tài)運動,訓(xùn)練完成的網(wǎng)絡(luò)也具備較好的泛化能力。此外,建立BP神經(jīng)網(wǎng)絡(luò),需要足夠多、具備代表性的數(shù)據(jù),而前期姿態(tài)濾波不僅提供了充足的訓(xùn)練樣本,也完成了對訓(xùn)練樣本的預(yù)處理。因此,航天器姿態(tài)預(yù)測任務(wù)適合采用基于BP神經(jīng)網(wǎng)絡(luò)的方法。
BP神經(jīng)網(wǎng)絡(luò)的訓(xùn)練過程可劃分為信號正向傳播和誤差反向傳播兩個步驟。在正向傳播時,信號的傳播沿著輸入層—隱含層—輸出層的路徑,單個神經(jīng)元只影響與之相連的下一層神經(jīng)元。通過計算當前輸出與期望輸出的平方誤差函數(shù),判斷是否符合期望,若不符合期望,則轉(zhuǎn)入誤差反向傳播的步驟。隨后,依據(jù)梯度下降算法,將誤差沿反向分攤給各層神經(jīng)元,并調(diào)整網(wǎng)絡(luò)權(quán)值和閾值。上述兩個步驟交替進行,直至網(wǎng)絡(luò)輸出誤差減小至預(yù)設(shè)范圍。
首先,確定網(wǎng)絡(luò)選取的層次。分析時刻與預(yù)測四元數(shù)之間的映射關(guān)系,考慮到航天器姿態(tài)四元數(shù)曲線呈現(xiàn)出的明顯的規(guī)律性和周期性,選取當前時刻姿態(tài)四元數(shù)作為輸入層神經(jīng)元,將需要獲取的預(yù)測四元數(shù)作為輸出層神經(jīng)元。由于隱含層神經(jīng)元數(shù)目需要根據(jù)多次實驗進行最終確定,通過對隱含層不同神經(jīng)元數(shù)目的網(wǎng)絡(luò)進行對比,可以在兼顧誤差和訓(xùn)練時間的要求下確定最佳數(shù)目。
使用當前時刻tk的姿態(tài)四元數(shù)作為神經(jīng)網(wǎng)絡(luò)的輸入,輸出為tk+1000時刻的姿態(tài)四元數(shù),并假定完整濾波過程的時長為5000s。訓(xùn)練集不使用未完全收斂的濾波前期1000s的數(shù)據(jù),而是選取了1000s~3000s內(nèi)姿態(tài)四元數(shù)的濾波估計值。目標輸出選取為2000s~4000s姿態(tài)四元數(shù)的濾波估計值,并搭建了一個結(jié)構(gòu)層數(shù)為4-15-4的BP神經(jīng)網(wǎng)絡(luò),即輸入層為tk時刻的四元數(shù)各分量,輸出層為tk+1000時刻的四元數(shù)各分量,隱含層節(jié)點數(shù)為15。其結(jié)構(gòu)示意圖如圖4所示。
圖4 BP神經(jīng)網(wǎng)絡(luò)示意圖Fig.4 Schematic diagram of BP neural network
此外,由于航天器實際的姿態(tài)變化為連續(xù)變化,其在任意時刻的觀測值均會受到過去觀測值的影響??紤]將之前若干時刻的姿態(tài)四元數(shù)作為輸入,分別搭建結(jié)構(gòu)參數(shù)為40-70-4和20-40-4的神經(jīng)網(wǎng)絡(luò)。前者的輸入節(jié)點為40個,對應(yīng)tk至tk-9時刻的姿態(tài)四元數(shù),共包含40個參數(shù);輸出節(jié)點為4個,對應(yīng)tk+1000時刻的姿態(tài)四元數(shù);隱含層節(jié)點數(shù)目為70個。后者的輸入節(jié)點為20個,對應(yīng)當前時刻tk至tk-4時刻姿態(tài)四元數(shù)的各分量,總計20個參數(shù);輸出節(jié)點為4個,對應(yīng)tk+1000時刻的姿態(tài)四元數(shù);隱含層節(jié)點數(shù)目為40個。
為姿態(tài)測量輸入加入2度的噪聲,采樣頻率為2Hz。濾波器的參數(shù)設(shè)置如表1所示。
表1 濾波器的初始參數(shù)設(shè)置Tab.1 Initialization of the Kalman filter
其中,qnoise=π/90對應(yīng)姿態(tài)輸入測量噪聲。
圖5 姿態(tài)四元數(shù)誤差曲線圖Fig.5 Estimation errors of attitude quaternion
圖6 角速度誤差曲線圖Fig.6 Estimation errors of angular velocity
圖7 主軸轉(zhuǎn)動慣量比值估計曲線圖Fig.7 Estimation of principal moments of inertia
圖8 慣量積比值估計曲線圖Fig.8 Estimation of products of inertia
濾波估計過程為5000s,選擇3000s~4000s內(nèi)的姿態(tài)四元數(shù)各個分量曲線,擬合形式為正弦函數(shù)之和。由于擬合精度受函數(shù)項數(shù)影響,為確定最佳項數(shù),繪制項數(shù)、擬合結(jié)果與原曲線間均方根誤差(Root Mean Squared Error,RMSE)的關(guān)系曲線如圖9所示。
圖9 函數(shù)擬合項數(shù)與均方根誤差關(guān)系曲線圖Fig.9 Relationship between the numbers of terms and RMSE
圖9表明,初始時,RMSE會隨擬合項數(shù)增加而迅速減小,但隨后基本未出現(xiàn)變化,部分曲線甚至出現(xiàn)了過擬合現(xiàn)象。為了避免項數(shù)過多而導(dǎo)致計算成本增加,選擇使均方根誤差最小的項數(shù),據(jù)此分別確定四元數(shù)各分量曲線的擬合項數(shù)為8、6、4、2,然后經(jīng)計算得到各擬合函數(shù)在4000s~5000s的函數(shù)值。將濾波估計值視為姿態(tài)真實值,將誤差四元數(shù)轉(zhuǎn)為歐拉角,則翻滾、俯仰、偏航角的誤差如圖10所示。各歐拉角的預(yù)測誤差分別為2.5670°、3.2009°和2.6242°,且誤差與時間無關(guān)。
圖10 基于函數(shù)擬合的姿態(tài)預(yù)測誤差曲線圖Fig.10 Prediction errors of attitude Euler angles based on function fitting
濾波過程的姿態(tài)估計值分為兩部分:第一部分(1000s~4000s)為訓(xùn)練數(shù)據(jù)集;第二部分(4000s~5000s)為驗證數(shù)據(jù)集。在姿態(tài)預(yù)測過程中,可將濾波估計值視為真實值。將3000s~4000s內(nèi)姿態(tài)四元數(shù)的濾波估計值輸入訓(xùn)練完成的神經(jīng)網(wǎng)絡(luò),得到4000s~5000s姿態(tài)四元數(shù)的預(yù)測值;隨后,將預(yù)測四元數(shù)與濾波估計四元數(shù)間的誤差四元數(shù)轉(zhuǎn)為歐拉角誤差。3.2節(jié)所提出的三種結(jié)構(gòu)神經(jīng)網(wǎng)絡(luò)的翻滾、俯仰、偏航角誤差如表2所示。
表2 三種不同結(jié)構(gòu)參數(shù)的神經(jīng)網(wǎng)絡(luò)仿真結(jié)果Tab.2 Simulation results of three neural networks
由表2可見,由于節(jié)點大幅增加,40-70-4結(jié)構(gòu)的神經(jīng)網(wǎng)絡(luò)出現(xiàn)了過擬合現(xiàn)象。相比其他兩種結(jié)構(gòu),其預(yù)測誤差明顯上升。此外,隨著網(wǎng)絡(luò)結(jié)構(gòu)的復(fù)雜化,訓(xùn)練計算量和時間也顯著增加。該結(jié)構(gòu)與在軌任務(wù)對實時性的要求也產(chǎn)生了沖突。20-40-4結(jié)構(gòu)的網(wǎng)絡(luò)犧牲了一定的時間成本,獲得了較高的預(yù)測精度,而4-15-4結(jié)構(gòu)的神經(jīng)網(wǎng)絡(luò)兼具了高精度和低時間成本的優(yōu)點。這兩種結(jié)構(gòu)的神經(jīng)網(wǎng)絡(luò)預(yù)測歐拉角誤差曲線分別如圖11、圖12所示。
圖11 20-40-4結(jié)構(gòu)神經(jīng)網(wǎng)絡(luò)的姿態(tài)預(yù)測誤差曲線圖Fig.11 Prediction errors of attitude Euler angles of neural network (20-40-4)
圖12 4-15-4結(jié)構(gòu)神經(jīng)網(wǎng)絡(luò)的姿態(tài)預(yù)測誤差曲線圖Fig.12 Prediction errors of attitude Euler angles of neural network (4-15-4)
圖11、圖12均顯示出了誤差與時間無關(guān)的良好特性,即預(yù)測誤差不隨時間累積而逐漸增大,一直保持著較高的預(yù)測精度,能夠滿足較長時間內(nèi)對目標航天器進行姿態(tài)預(yù)測的要求。此外,以上曲線始終振蕩,沒有收斂于固定的常值。這是由于預(yù)測過程缺少了觀測信息、無法校正當前預(yù)測值所導(dǎo)致。對比圖11與圖12可以發(fā)現(xiàn),20-40-20結(jié)構(gòu)神經(jīng)網(wǎng)絡(luò)的振蕩幅值明顯小于4-15-4結(jié)構(gòu),這與表2的統(tǒng)計結(jié)果相符。上述結(jié)果表明,在前期,只要能穩(wěn)定獲取空間無控翻滾航天器在一段時間內(nèi)的姿態(tài)信息,即使在丟失跟蹤后,仍然能通過本文提出的算法實現(xiàn)較高精度的姿態(tài)預(yù)測。
本文研究了無角速度測量信息的非合作航天器姿態(tài)確定問題,以及缺少校正信息的航天器長時姿態(tài)預(yù)測問題。將姿態(tài)、角速度和轉(zhuǎn)動慣量比值列為狀態(tài)向量,并基于剛體旋轉(zhuǎn)動力學的差分形式推導(dǎo)了線性化狀態(tài)方程,提出了一種乘性擴展卡爾曼濾波算法。通過姿態(tài)測量校正更新了角速度和慣性參數(shù),避免了需要利用陀螺儀測量信息而帶來的限制。針對由末端執(zhí)行機構(gòu)遮擋視覺傳感器而導(dǎo)致的目標跟蹤丟失問題,利用濾波后的姿態(tài)數(shù)據(jù)作為訓(xùn)練樣本,分別設(shè)計了函數(shù)擬合和神經(jīng)網(wǎng)絡(luò)兩種方法,改善了傳統(tǒng)預(yù)測方法誤差隨時間累積的弊端,并提升了姿態(tài)預(yù)測精度。數(shù)值仿真結(jié)果表明,濾波算法能夠僅使用姿態(tài)測量輸入而精確估計目標航天器的相對姿態(tài)、角速度和轉(zhuǎn)動慣量比值;姿態(tài)預(yù)測算法能夠在長時段內(nèi)兼顧預(yù)測精度和計算速度;本文提出的框架能夠?qū)崟r利用目標航天器姿態(tài)信息在線實現(xiàn)較高精度的估計與預(yù)測。