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

楊海峰， 傅惠民

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

0 引 言

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

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

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

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

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

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

(1)

Yk=hk(Xk)+dk+Vk,

(2)

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

(3)

1.1 含未知輸入的動力學方程

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

(4)

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

(5)

(6)

(7)

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

(8)

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

由于突風、模型參數選取不當等因素的影響，在探測器運動狀態的6個維度上都有可能產生未知輸入，所以b一般表示為：

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

(9)

1.2 含未知輸入的量測方程

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

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

Y=(aTy,rTy,vTy)T,

(10)

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

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是由速度坐標系到導航坐標系的轉換矩陣；Cp是由導航坐標系到火星中心固聯坐標系的轉換矩陣，其具體形式由下式給出：

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

(12)

(13)

(14)

式(10)中的ry和vy分別為由無線電測距和多普勒測速得到的量測結果，其具體形式為[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個火星導航信標的距離和徑向速度，其量測方程由下式給出：

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個導航信標在火星中心固聯坐標系下的位置和速度向量，前者由下式計算得到：

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

(21)

(22)

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

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

(23)

2 雙未知輸入自主導航濾波算法

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

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

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

(24)

j=1, 2,…，m,

(25)

(26)

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

(27)

(28)

(29)

2.2 雙未知輸入自主導航濾波

(1)一步自校準預測

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

(30)

其中，

(31)

(32)

非線性系統一步自校準預測為：

(33)

一步預測誤差協方差矩陣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的情況，系統一步預測為：

(39)

一步預測誤差協方差矩陣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)

量測估計誤差協方差矩陣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)

量測估計誤差協方差矩陣PY, k為：

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

(51)

(3)狀態估計

(52)

狀態估計誤差協方差矩陣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 算例對比與結果分析

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

火星探測器的初始運動狀態和狀態估計見表1，3個火星導航信標的分布情況見表2。

表1 探測器初始運動狀態及其估計

表2 火星導航信標分布

火星大氣進入段導航持續時間300 s，取相鄰兩步之間的時間間隔為1 s，設動力學方程和量測方程中的未知輸入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導航算法的RMSE均值比較

圖1 本文算法(實線)與EKF導航算法(虛線)的狀態估計RMSE比較

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

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

4 結束語

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