基于雙未知輸入擴展自校準濾波的火星大氣進入段自主導(dǎo)航算法

楊海峰， 傅惠民

(北京航空航天大學(xué) 小樣本技術(shù)研究中心， 北京 100191)

0 引 言

火星探測是人類深空探測的熱點之一，更是各航天大國展現(xiàn)自身綜合國力和科技水平的平臺。中國在順利實施“嫦娥”系列月球探測任務(wù)之后，與火星探測相關(guān)的技術(shù)研究也正在逐步進行。不同于月球探測，火星探測任務(wù)由于地球與火星之間的距離太過遙遠，無法進行實時控制，這就要求火星探測器必須具備高精確的自主導(dǎo)航系統(tǒng)。正因如此，火星探測的高精度自主導(dǎo)航一直都是國內(nèi)外研究的前沿與熱點問題[1-5]。

探測器著陸火星需經(jīng)歷進入、下降和著陸(Entry, Descent and Landing, EDL)過程，而火星大氣進入段是最重要的階段之一，其自主導(dǎo)航精度直接影響最終著陸的成功與否[6-8]。由于火星大氣進入段是EDL過程中歷時最長、氣動環(huán)境最為惡劣的階段，其探測器的運動狀態(tài)將受到突風、沙塵暴等諸多未知因素的影響，導(dǎo)致其動力學(xué)特性無法精確建模[9-12]；同時，慣性測量裝置(Inertial Measurement Unit，IMU)存在漂移，導(dǎo)致量測數(shù)據(jù)中不可避免地存在未知輸入(偏差)。因此，亟需建立一種能夠消除上述兩種未知輸入影響的火星大氣進入段自主導(dǎo)航算法。

自適應(yīng)Kalman 濾波方法(Adaptive Kalman Filter)[13]， 試圖通過調(diào)用前N步的狀態(tài)估計來修正未知輸入的影響， 但這些數(shù)據(jù)往往與當前時刻的狀態(tài)關(guān)系不大，有時反而會造成更大的誤差，甚至導(dǎo)致濾波發(fā)散。為此，傅惠民等提出一種自識別自校準濾波方法[14-15]，該方法能夠在系統(tǒng)的動力學(xué)方程和量測方程均含有未知輸入時，自動對其未知輸入進行識別、估計和補償，消除偏差影響，并通過數(shù)據(jù)融合減小偶然誤差影響，提高濾波精度?；谶@一理論，結(jié)合火星大氣進入段著陸導(dǎo)航的工程實際，本文建立了一種新的火星大氣進入段自主導(dǎo)航算法，該算法可以有效消除突風等未知環(huán)境因素和量測設(shè)備未知漂移的影響，大幅提升火星大氣進入段自主導(dǎo)航的精度。

1 含雙未知輸入的火星大氣進入段自主導(dǎo)航模型

火星大氣進入段自主導(dǎo)航模型通常由動力學(xué)方程和量測方程組成，考慮到突風等不確定因素以及量測設(shè)備不精確的影響，含雙未知輸入的自主導(dǎo)航模型為：

Xk=fk-1(Xk-1)+bk-1+Wk-1,

(1)

Yk=hk(Xk)+dk+Vk,

(2)

式中，fk(·)和hk(·)均為非線性向量函數(shù)；Xk為m維狀態(tài)向量，Yk為n維量測向量；bk和dk均為未知輸入；Wk是方差矩陣為Qk的狀態(tài)噪聲向量；Vk是方差矩陣為Rk的量測噪聲向量，并且滿足：

(3)

1.1 含未知輸入的動力學(xué)方程

式(1)中，X=(r,v,γ,θ,λ,ψ)T為探測器運動狀態(tài)向量，其微分形式為[16-17]：

(4)

式中，r表示探測器質(zhì)點到火星中心的距離;v表示探測器質(zhì)點在火星中心固聯(lián)坐標系下的速度；σ為探測器的滾轉(zhuǎn)角，γ和ψ分別為航跡傾角和航向角；θ和λ分別表示探測器位置對應(yīng)的火星經(jīng)度和緯度；gM為火星重力加速度，D和L分別表示探測器的氣動阻力加速度和升力加速度，其具體的計算公式如下所示：

(5)

(6)

(7)

其中，μ=42 828.29×109m3/s2為火星引力常數(shù)；CD與CL分別為探測器的阻力系數(shù)與升力系數(shù)；Sr為探測器參考表面積；mc為探測器質(zhì)量；ρ為火星大氣密度，由下式給出：

(8)

其中，ρ0=2×10-4kg/m3為火星標準大氣密度；r0=3 437.2 km為距離火星表面40 km的徑向基準位置；hs=7.5 km為火星大氣定標高度。

由于突風、模型參數(shù)選取不當?shù)纫蛩氐挠绊?，在探測器運動狀態(tài)的6個維度上都有可能產(chǎn)生未知輸入，所以b一般表示為：

b=(b1,b2,b3,b4,b5,b6)T.

(9)

1.2 含未知輸入的量測方程

火星探測器可基于IMU對自身加速度進行測量，進而得到探測器的運動狀態(tài)；此外，探測器還可通過與已知位置信息的火星導(dǎo)航信標之間的無線電通訊得到自身的運動狀態(tài)。因此，本文選用IMU加速度計和基于3個導(dǎo)航信標的無線電測距、測速的組合量測模型。

式(2)中量測向量Y的具體形式為[16,18-19]：

Y=(aTy,rTy,vTy)T,

(10)

其中，ay為加速度計在火星中心固聯(lián)坐標系下的量測結(jié)果，其量測方程如下所示：

ay=CpCvav+da+Va，

(11)

式中，da=(dk,1,dk,2,dk,3)T,Va=(Vk,1,Vk,2,Vk,3)T, 而dk, j和Vk, j分別為dk和Vk的第j個分量;av為探測器在速度坐標系下的實際加速度向量；Cv是由速度坐標系到導(dǎo)航坐標系的轉(zhuǎn)換矩陣；Cp是由導(dǎo)航坐標系到火星中心固聯(lián)坐標系的轉(zhuǎn)換矩陣，其具體形式由下式給出：

av=(-D,-Lsinφ,Lcosφ)T,

(12)

(13)

(14)

式(10)中的ry和vy分別為由無線電測距和多普勒測速得到的量測結(jié)果，其具體形式為[16,18-19]：

ry=(ry,1,ry,2,ry,3)T，

(15)

vy=(vy,1,vy,2,vy,3)T，

(16)

式中，ry, j和vy, j分別為探測器相對于第j個火星導(dǎo)航信標的距離和徑向速度，其量測方程由下式給出：

ry, j=rc, j+dk, j+3+Vk, j+3,j=1,2,3，

(17)

vy, j=vc, j+dk, j+6+Vk, j+6,j=1,2,3,

(18)

其中，rc, j和vc, j分別為相對距離與徑向速度的實際值，其計算公式為：

(19)

(20)

其中，rc和vc、rb,j和vb,j分別為探測器和第j個導(dǎo)航信標在火星中心固聯(lián)坐標系下的位置和速度向量，前者由下式計算得到：

rc=(rcosλcosθ,rcosλsinθ,rsinλ)T,

(21)

(22)

需特別指出的是，由于量測數(shù)據(jù)中只有IMU的加速度計中存在未知輸入，而無線電測距與多普勒測速只有隨機誤差的影響，不存在未知輸入。因此，量測方程未知輸入向量的9個維度中，與導(dǎo)航信標相關(guān)的后6個維度取值均為0，即

dk=(dk,1,dk,2,dk,3,0,0,0,0,0,0)T.

(23)

2 雙未知輸入自主導(dǎo)航濾波算法

下面對式(1)和式(2)給出的由動力學(xué)方程、IMU加速度計和基于3個導(dǎo)航信標的無線電測距測速組成的含雙未知輸入的導(dǎo)航模型(m=6,n=9)進一步建立其相應(yīng)的導(dǎo)航濾波算法。

2.1 雙未知輸入自識別自校準

(1)狀態(tài)方程未知輸入自識別自校準

(24)

j=1, 2,…，m,

(25)

(26)

(2)量測方程未知輸入自識別自校準

(27)

(28)

(29)

2.2 雙未知輸入自主導(dǎo)航濾波

(1)一步自校準預(yù)測

Xk=Φk-1Xk-1+UX,k-1+bk-1+Wk-1，

(30)

其中，

(31)

(32)

非線性系統(tǒng)一步自校準預(yù)測為：

(33)

一步預(yù)測誤差協(xié)方差矩陣Pk/(k-1)為：

Pk/(k-1)=Φk-1Pk-1ΦTk-1+Qk-1+Ωk-1+ΩTk-1+Ω*k-1，

(34)

其中，

Ωk-1=Φk-1[Pk-1-Sk-1ΦTk-2-(I-Kk-1Hk-1)Qk-2]T*k-1，

(35)

Ω*k-1=T*k-1[Pk-1+Φk-2Pk-2ΦTk-2+Qk-2-Sk-1ΦTk-2-

(I-Kk-1Hk-1)Qk-2-Φk-2STk-1-QTk-2(I-Kk-1Hk-1)T]T*k-1，

(36)

Sk-1=(I-Kk-1Hk-1){Φk-2Pk-2+T*k-2[Pk-2-Φk-3STk-2-

QTk-3(I-Kk-2Hk-2)T]}+Kk-1Tk-1(Hk-2Pk-2-RTk-2KTk-2)，

(37)

S2=(I-K2H2)Φ1P1，

(38)

對于k=1,2的情況，系統(tǒng)一步預(yù)測為：

(39)

一步預(yù)測誤差協(xié)方差矩陣Pk/(k-1)為：

Pk/(k-1)=Φk-1Pk-1ΦTk-1+Qk-1，

(40)

濾波初始化為：

(41)

(42)

(2)量測自校準估計

Yk=HkXk+UY,k+dk+Vk，

(43)

式中，

(44)

(45)

(46)

量測估計誤差協(xié)方差矩陣PY, k為：

PY, k=HkPk/(k-1)HTk+Rk+HkΨk+ΨTHTk+Ψ*k，

(47)

其中，

Ψk=-{Φk-1Pk-1HTk-1+T*k-1[Pk-1-Φk-2STk-1-QTk-2(I-

Kk-1Hk-1)T]HTk-1-Φk-1Kk-1Rk-1-T*k-1Kk-1Rk-1}Tk，

(48)

Ψ*k=Tk(Hk-1Pk-1HTk-1+Rk-1-Hk-1Kk-1Rk-1-RTk-1KTk-1HTk-1)Tk，

(49)

對于k= 1,2的情況，量測估計為：

(52)

量測估計誤差協(xié)方差矩陣PY, k為：

PY, k=HkPk/(k-1)HTk+Rk，

(51)

(3)狀態(tài)估計

(52)

狀態(tài)估計誤差協(xié)方差矩陣Pk為：

Pk=Pk/(k-1)-KkPTXY, k，

(53)

其中，Kk為濾波增益矩陣，由下式計算得到

Kk=PXY, kP-1Y, k，

(54)

PXY, k=Pk/(k-1)HTk+Ψk.

(55)

式中，Ψ1=Ψ2=0。

3 算例對比與結(jié)果分析

采用美國火星科學(xué)實驗室(Mars Science Laboratory, MSL)任務(wù)中好奇號(Curiosity)著陸探測器在大氣進入段的實際數(shù)據(jù)[11,16]，分別對本文算法和基于EKF的自主導(dǎo)航算法進行500次獨立數(shù)字仿真，并將狀態(tài)各維度上的均方根誤差(RMSE)作為評價導(dǎo)航性能的指標。

火星探測器的初始運動狀態(tài)和狀態(tài)估計見表1，3個火星導(dǎo)航信標的分布情況見表2。

表1 探測器初始運動狀態(tài)及其估計

表2 火星導(dǎo)航信標分布

火星大氣進入段導(dǎo)航持續(xù)時間300 s，取相鄰兩步之間的時間間隔為1 s，設(shè)動力學(xué)方程和量測方程中的未知輸入bk-1和dk分別為：

(56)

dk=(0.003,0.003,0.003,0,0,0,0,0,0)T, 1≤k≤300.

(57)

500次蒙特卡洛仿真所得兩種算法在各維度上均方根誤差對比情況如圖1所示，均方根誤差均值的對比見表3。

表3 本文算法與EKF導(dǎo)航算法的RMSE均值比較

圖1 本文算法(實線)與EKF導(dǎo)航算法(虛線)的狀態(tài)估計RMSE比較

Fig. 1 State estimationRMSEscomparison of the proposed algorithm and the EKF navigation algorithm

由圖1和表3中的計算結(jié)果可以看出，當著陸探測器在大氣進入段受到突風等不確定因素影響且加速度計存在未知漂移時，本文算法可以很好地消除這些未知輸入的影響，狀態(tài)估計誤差遠小于基于EKF的自主導(dǎo)航算法，且導(dǎo)航全程估計結(jié)果都保持穩(wěn)定，具有很強的魯棒性。

4 結(jié)束語

為進一步提升火星大氣進入段自主導(dǎo)航精度，消除動力學(xué)模型中突風、沙塵暴以及量測模型中IMU漂移等未知輸入對探測器運動狀態(tài)估計的影響，本文將雙未知輸入擴展自校準濾波引入火星自主導(dǎo)航工程實際中，建立了一種新的火星大氣進入段自主導(dǎo)航算法。大量計算結(jié)果表明，與傳統(tǒng)算法相比，本文算法導(dǎo)航精度更高，魯棒性更強，能更好地滿足未來火星高精度定點著陸的需求。