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

楊海峰， 傅惠民

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

0 引 言

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

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

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

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

火星大氣進(jìn)入段自主導(dǎo)航模型通常由動(dòng)力學(xué)方程和量測(cè)方程組成，考慮到突風(fēng)等不確定因素以及量測(cè)設(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維量測(cè)向量；bk和dk均為未知輸入；Wk是方差矩陣為Qk的狀態(tài)噪聲向量；Vk是方差矩陣為Rk的量測(cè)噪聲向量，并且滿足：

(3)

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

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

(4)

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

(5)

(6)

(7)

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

(8)

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

由于突風(fēng)、模型參數(shù)選取不當(dāng)?shù)纫蛩氐挠绊懀谔綔y(cè)器運(yùn)動(dòng)狀態(tài)的6個(gè)維度上都有可能產(chǎn)生未知輸入，所以b一般表示為：

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

(9)

1.2 含未知輸入的量測(cè)方程

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

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

Y=(aTy,rTy,vTy)T,

(10)

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

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個(gè)分量;av為探測(cè)器在速度坐標(biāo)系下的實(shí)際加速度向量；Cv是由速度坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣；Cp是由導(dǎo)航坐標(biāo)系到火星中心固聯(lián)坐標(biāo)系的轉(zhuǎn)換矩陣，其具體形式由下式給出：

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

(12)

(13)

(14)

式(10)中的ry和vy分別為由無(wú)線電測(cè)距和多普勒測(cè)速得到的量測(cè)結(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分別為探測(cè)器相對(duì)于第j個(gè)火星導(dǎo)航信標(biāo)的距離和徑向速度，其量測(cè)方程由下式給出：

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分別為相對(duì)距離與徑向速度的實(shí)際值，其計(jì)算公式為：

(19)

(20)

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

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

(21)

(22)

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

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

(23)

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

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

2.1 雙未知輸入自識(shí)別自校準(zhǔn)

(1)狀態(tài)方程未知輸入自識(shí)別自校準(zhǔn)

(24)

j=1, 2,…，m,

(25)

(26)

(2)量測(cè)方程未知輸入自識(shí)別自校準(zhǔn)

(27)

(28)

(29)

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

(1)一步自校準(zhǔn)預(yù)測(cè)

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

(30)

其中，

(31)

(32)

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

(33)

一步預(yù)測(cè)誤差協(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)

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

(39)

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

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

(40)

濾波初始化為：

(41)

(42)

(2)量測(cè)自校準(zhǔn)估計(jì)

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

(43)

式中，

(44)

(45)

(46)

量測(cè)估計(jì)誤差協(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)

對(duì)于k= 1,2的情況，量測(cè)估計(jì)為：

(52)

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

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

(51)

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

(52)

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

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

(53)

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

Kk=PXY, kP-1Y, k，

(54)

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

(55)

式中，Ψ1=Ψ2=0。

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

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

火星探測(cè)器的初始運(yùn)動(dòng)狀態(tài)和狀態(tài)估計(jì)見表1，3個(gè)火星導(dǎo)航信標(biāo)的分布情況見表2。

表1 探測(cè)器初始運(yùn)動(dòng)狀態(tài)及其估計(jì)

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

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

(56)

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

(57)

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

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

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

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

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

4 結(jié)束語(yǔ)

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