基于改進(jìn)預(yù)測(cè)濾波的小天體精確著陸自主導(dǎo)航方法研究

冀紅霞，宗 紅，黃翔宇,2

（1.北京控制工程研究所，北京100190；2.空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京100190）

引 言

小天體探測(cè)將是未來深空探測(cè)的熱點(diǎn)之一[1]。由于深空天體與地球的距離遙遠(yuǎn)，導(dǎo)致地面測(cè)控站所發(fā)出的測(cè)控信息的傳輸出現(xiàn)時(shí)滯,難以實(shí)現(xiàn)對(duì)著陸過程進(jìn)行實(shí)時(shí)測(cè)控，因此深空探測(cè)著陸任務(wù)必須利用探測(cè)器上的敏感器和導(dǎo)航算法自主地完成[2]。傳統(tǒng)的慣性導(dǎo)航系統(tǒng)由于初始對(duì)準(zhǔn)誤差、常值漂移以及模型不確知參數(shù)的存在，導(dǎo)致導(dǎo)航精度較低，不能滿足精確著陸的任務(wù)需要。為提高探測(cè)器導(dǎo)航精度，需要引入相對(duì)導(dǎo)航傳感器直接測(cè)量探測(cè)器相對(duì)目標(biāo)天體的信息。日本空間科學(xué)研究所（Institute of space and Aeronau‐tical Science，ISAS）針對(duì)MUSES-C 任務(wù)提出了利用光學(xué)測(cè)量的自主著陸小行星導(dǎo)航方案。該導(dǎo)航方案通過導(dǎo)航相機(jī)和激光測(cè)距儀進(jìn)行測(cè)量，經(jīng)過導(dǎo)航濾波器處理這些圖像和距離信息來確定探測(cè)器的位置和速度[3]，歐洲航天局（European Space Agency，ESA）的“羅塞塔號(hào)”（Rosetta）任務(wù)利用激光測(cè)距儀與多普勒雷達(dá)確定探測(cè)器相對(duì)著陸區(qū)的高度、速度和姿態(tài)信息[4]。噴氣推進(jìn)實(shí)驗(yàn)室（Jet Propulsion Laboratory，JPL）Johnson 等[5]提出了一套基于計(jì)算機(jī)視覺的小天體軟著陸導(dǎo)航方案，通過跟蹤多幀圖像特征點(diǎn)，結(jié)合激光高度計(jì)數(shù)據(jù)對(duì)探測(cè)器運(yùn)動(dòng)進(jìn)行估計(jì)。

由于小天體尺寸小，形狀不規(guī)則，表面性質(zhì)多變，很難得到精確的探測(cè)器運(yùn)動(dòng)學(xué)模型，因此為了在不能精確確定引力模型和著陸地點(diǎn)狀況的小天體表面著陸，必須有自主性強(qiáng)、精度高和魯棒性強(qiáng)的導(dǎo)航算法。為了確定探測(cè)器位置、速度及姿態(tài)狀態(tài)，并抑制模型誤差及觀測(cè)噪聲與系統(tǒng)噪聲的影響，考慮到系統(tǒng)的非線性，需要設(shè)計(jì)非線性濾波器。非線性估計(jì)領(lǐng)域最常用的方法是擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）[6]，它具有收斂速度快、算法簡(jiǎn)單等優(yōu)點(diǎn)。盡管EKF 在建模準(zhǔn)確且系統(tǒng)噪聲為零均值白噪聲的情況下，可以獲得良好的估計(jì)效果。但在實(shí)際應(yīng)用中，難以獲得精確的系統(tǒng)模型，且最初建好的模型在系統(tǒng)運(yùn)行過程中受環(huán)境變化的影響可能變得不準(zhǔn)確，進(jìn)而造成模型誤差，這時(shí)采用EKF 將導(dǎo)致濾波精度下降甚至發(fā)散。

非線性預(yù)測(cè)濾波算法（Nonlinear Predictive Fil‐ter,NPF）[7-8]對(duì)不確定模型具有適應(yīng)性，該算法通過使預(yù)測(cè)輸出與測(cè)量輸出的誤差和模型誤差的加權(quán)和最小值來估計(jì)模型誤差，進(jìn)而修正狀態(tài)估計(jì)，形成一種遞推的在線估計(jì)算法，由于對(duì)模型誤差實(shí)時(shí)進(jìn)行跟蹤，故收斂速度較慢。

針對(duì)NPF 和EKF 的局限性，本文采用改進(jìn)的預(yù)測(cè)濾波方法，首先利用NPF 的預(yù)測(cè)輸出與實(shí)際測(cè)量輸出之間的方差最小的原則來估計(jì)模型誤差并修正系統(tǒng)模型，再利用EKF 的時(shí)間更新和測(cè)量更新來進(jìn)一步估計(jì)系統(tǒng)狀態(tài)，并針對(duì)小天體精確著陸精度的要求，建立小天體著陸運(yùn)動(dòng)學(xué)模型，結(jié)合陀螺、光學(xué)相機(jī)和測(cè)速儀測(cè)量信息來確定探測(cè)器位置、速度和姿態(tài)，推導(dǎo)了小天體探測(cè)器軟著陸系統(tǒng)中應(yīng)用的預(yù)測(cè)濾波算法。在分析導(dǎo)航系統(tǒng)可觀性的基礎(chǔ)上，通過仿真驗(yàn)證在小天體引力變化引起的模型誤差下的導(dǎo)航精度效果，并與EKF 的估計(jì)效果進(jìn)行比較分析。

1 小天體探測(cè)器自主光學(xué)導(dǎo)航系統(tǒng)

1.1 小天體引力場(chǎng)模型

為了建立小行星探測(cè)器著陸段仿真模型，需要對(duì)小行星引力場(chǎng)進(jìn)行建模，以模擬探測(cè)器在著陸段的運(yùn)動(dòng)。小行星一般都是不規(guī)則的橢球體，一般采用球諧項(xiàng)展開來建立其引力勢(shì)函數(shù)[9]。假設(shè)小天體密度為常值，利用小天體形狀模型，通過數(shù)值積分可計(jì)算出小天體引力場(chǎng)諧項(xiàng)系數(shù)[10]。式（1）給出了小天體引力勢(shì)函數(shù)模型（四階）。

其中：μA=GM為萬有引力常數(shù)和小天體質(zhì)量的乘積；a為小天體的名義半徑；ra為探測(cè)器到小天體質(zhì)心的距離；θ,?分別為赤經(jīng)、赤緯；C20,C22,C40,C42,C44為小天體引力勢(shì)函數(shù)調(diào)諧項(xiàng)系數(shù)。

小天體引力加速度分量可以表示為

其中：下標(biāo)b表示探測(cè)器三軸位置分量定義在小天體固連坐標(biāo)系中。

1.2 小天體著陸運(yùn)動(dòng)學(xué)模型與測(cè)量模型

探測(cè)器導(dǎo)航的任務(wù)就是確定探測(cè)器相對(duì)于所選定的參考坐標(biāo)系的位置、速度和飛行姿態(tài)，需找到一個(gè)合適的參考坐標(biāo)系（參見圖1）。所以這里定義了涉及到的參考坐標(biāo)系：著陸點(diǎn)坐標(biāo)系、本體坐標(biāo)系和相機(jī)坐標(biāo)系。

圖1 坐標(biāo)系相對(duì)幾何關(guān)系示意圖Fig.1 Relative geometric relations of coordinate system

1）著陸點(diǎn)坐標(biāo)系{l}：原點(diǎn)定義為著陸點(diǎn)，基準(zhǔn)平面取為當(dāng)?shù)厮矫?，x軸指向東，y軸指向北，z軸與x、y軸構(gòu)成右手坐標(biāo)系。

2）本體坐標(biāo)系：原點(diǎn)定義為探測(cè)器質(zhì)心，x，y，z軸分別沿探測(cè)器3個(gè)慣性主軸方向構(gòu)成右手坐標(biāo)系。

3）相機(jī)坐標(biāo)系{c}：原點(diǎn)定義為相機(jī)的光心，x,y軸平行于像平面，z軸與x，y軸構(gòu)成右手坐標(biāo)系。

1.2.1 小天體探測(cè)器運(yùn)動(dòng)學(xué)模型

利用慣性敏感器測(cè)量建立探測(cè)器著陸運(yùn)動(dòng)學(xué)方程為

其中r和v分別為探測(cè)器在著陸點(diǎn)坐標(biāo)系下的位置和速度；af為IMU 測(cè)量得到的加速度，ωm為小天體的自旋角速度；q為表示探測(cè)器本體系相對(duì)著陸點(diǎn)坐標(biāo)系的姿態(tài)四元數(shù)；U為上一節(jié)提到的小天體引力加速度；(C(q))LB為本體系到著陸點(diǎn)坐標(biāo)系的方向余 弦陣，簡(jiǎn)寫為C(q)。

Ω(ω)定義如下

1.2.2 IMU測(cè)量模型

假設(shè)IMU 固連于著陸器體坐標(biāo)系，可直接敏感著陸器相對(duì)于慣性空間的角速度和非引力加速度。則加速度計(jì)和陀螺的測(cè)量模型分別表示為

其中，ba和bw分別為加速度計(jì)零偏和陀螺零偏。

其中：a為除引力外所有作用于探測(cè)器上的合力產(chǎn)生的加速度，na、nw分別是加速度計(jì)和陀螺測(cè)量噪聲。

1.2.3 導(dǎo)航相機(jī)測(cè)量模型

探測(cè)器在著陸點(diǎn)固連坐標(biāo)系下的狀態(tài)參數(shù)為(r,q)，著陸點(diǎn)固連坐標(biāo)系下第i個(gè)特征點(diǎn)的位置矢量為L(zhǎng) pi，探測(cè)器到第i個(gè)特征點(diǎn)的位置矢量在Σl，Σb和Σc3個(gè)坐標(biāo)系下的表示分別為

1）著陸點(diǎn)固連坐標(biāo)系Σl下的表示為

2）在探測(cè)器體固連坐標(biāo)系Σb下的表示為

3）在相機(jī)固連坐標(biāo)系Σc下的表示為

為簡(jiǎn)化問題起見，這里假設(shè)相機(jī)坐標(biāo)系與探測(cè)器本體系重合，即Ib= 0，=I3×3,則探測(cè)器到第i 個(gè)特征點(diǎn)的位置矢量在相機(jī)坐標(biāo)系下的表示為

假設(shè)光學(xué)導(dǎo)航相機(jī)模型為理想的小孔成像模型，由射影變換可得

其中：[xiyizi]T是相機(jī)固連系下探測(cè)器到第i個(gè)（i = 1,2,3）特征點(diǎn)的位置坐標(biāo)，跟蹤的特征點(diǎn)在像平面中的位置坐標(biāo)即為導(dǎo)航測(cè)量量，可表示如下

其中；m為為像素；[ui,vi]為成像平面上的像元像素坐標(biāo)值，坐標(biāo)mi=[uivi]T=f[xi/ziyi/zi]T。

則導(dǎo)航測(cè)量方程為

1.2.4 測(cè)速敏感器測(cè)量模型

測(cè)速敏感器輸出的相對(duì)速度的測(cè)量方程[11]如下

其中，nu為測(cè)速敏感器測(cè)量噪聲。

2 基于預(yù)測(cè)濾波的小天體探測(cè)器自主光學(xué)導(dǎo)航系統(tǒng)

2.1 狀態(tài)方程

導(dǎo)航系統(tǒng)狀態(tài)包括探測(cè)器當(dāng)前位置、速度和姿態(tài)以及成像時(shí)刻探測(cè)器位置和姿態(tài)，即

其中：xE為系統(tǒng)當(dāng)前狀態(tài)；和分別為成像時(shí)刻探測(cè)器位置和姿態(tài)，其不隨時(shí)間變化，當(dāng)生成新圖像時(shí)由新圖像成像時(shí)刻探測(cè)器位置和姿態(tài)代替前一圖像成像時(shí)刻探測(cè)器位置和姿態(tài)作為系統(tǒng)狀態(tài)。

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

線性化式（3）可得系統(tǒng)當(dāng)前誤差狀態(tài)方程如下

假設(shè)k 時(shí)刻相機(jī)拍攝一幅圖像，則狀態(tài)方程xE|k和協(xié)方差PEEk|k分別擴(kuò)維為xk和Pk|k。

其中

2.2 觀測(cè)方程

對(duì)于導(dǎo)航相機(jī)，以特征點(diǎn)在像平面的位置為觀測(cè)量，對(duì)于特征點(diǎn)由式（10）可得觀測(cè)方程為

其中：vi為觀測(cè)噪聲，設(shè)其協(xié)方差陣為Rv。第i時(shí)刻導(dǎo)航陸標(biāo)pj估計(jì)觀測(cè)量為

則第i時(shí)刻的觀測(cè)量殘差為

測(cè)量方程為

其中：H1=[H1103×3H1303×303×3]

其中

對(duì)于測(cè)距測(cè)速敏感器，由（11）可得觀測(cè)方程為

其中測(cè)量敏感矩陣為

其中

2.3 小天體探測(cè)器自主導(dǎo)航濾波器

2.3.1 預(yù)測(cè)濾波算法基本原理

設(shè)一個(gè)非線性系統(tǒng)方程為

其中：x(t)∈是狀態(tài)向量；f∈是模型向量 ；d(t)∈是模型誤差向量；g∈是模型誤差擾動(dòng)矩陣；h∈是測(cè)量向量；y(t)∈是測(cè)量輸出向量；v(t) 是零均值的高斯白噪聲，其方差陣R ∈。將輸出估計(jì)方程泰勒展開，并運(yùn)用李導(dǎo)數(shù)知識(shí)，得

其中：T為時(shí)間間隔；z[(t),T]的第i個(gè)元素為

其中：ri是與第i個(gè)輸出對(duì)應(yīng)的相對(duì)階，為d(t)出現(xiàn)在hi[x(t)]的微分中的最低階數(shù)；Λ(T)∈是對(duì)角矩陣，對(duì)角元λii=Ti/ri!,i=1,2,…,m；U(x)∈為靈敏度矩陣。

基于預(yù)測(cè)輸出與實(shí)際測(cè)量輸出的殘差最小的思想來估計(jì)系統(tǒng)的模型誤差。由此建立的目標(biāo)函數(shù)是由測(cè)量輸出與預(yù)測(cè)輸出間殘差以及模型誤差修正項(xiàng)的加權(quán)平方和組成的，即

其中：括號(hào)中的k表示第k個(gè)采樣時(shí)刻；y(k+ 1)是t(k + 1)時(shí)刻的測(cè)量輸出。預(yù)測(cè)輸出為

將（24）式代入（23）式，并令?J/?d(k)= 0，可得模型誤差估計(jì)為

其中

2.3.2 改進(jìn)的預(yù)測(cè)濾波算法

基于預(yù)測(cè)濾波原理，并針對(duì)探測(cè)器自主光學(xué)導(dǎo)航系統(tǒng)運(yùn)動(dòng)學(xué)、運(yùn)動(dòng)學(xué)模型和測(cè)量噪聲的非線性特性，以及小天體模型中存在的引力參數(shù)不確定性，將NPF與EKF 進(jìn)行結(jié)合，在狀態(tài)方程中引入模型誤差，將探測(cè)器的位置、姿態(tài)和四元數(shù)作為狀態(tài)變量，使用測(cè)速敏感器和光學(xué)相機(jī)的測(cè)量值，求得探測(cè)器的位置、速度和姿態(tài)的估計(jì)值。

為了表達(dá)方便，將同時(shí)考慮模型誤差和過程噪聲的非線性系統(tǒng)的狀態(tài)方程寫成如下形式

令采樣時(shí)間間隔為T，預(yù)測(cè)濾波和擴(kuò)展卡爾曼濾波相結(jié)合的濾波步驟如下：

1）由（25）式計(jì)算模型誤差d(k)；

對(duì)于2.1節(jié)、2.2節(jié)組成的導(dǎo)航系統(tǒng)進(jìn)行推導(dǎo)，靈敏度矩陣U、對(duì)角陣Λ(T)和在預(yù)測(cè)輸出時(shí)用到的z[(t),t,T]的計(jì)算公式分別如下

2）將d(k)代入一步預(yù)測(cè)估計(jì)值中

3）由EKF計(jì)算濾波值為

4）計(jì)算濾波增益陣

5）一步預(yù)測(cè)方差陣為

估計(jì)方差陣為

2.4 導(dǎo)航系統(tǒng)的可觀性分析

測(cè)量系統(tǒng)的可觀性，反映系統(tǒng)通過測(cè)量在有限時(shí)間內(nèi)的觀測(cè)量確定系統(tǒng)狀態(tài)的能力。導(dǎo)航系統(tǒng)的基本任務(wù)就是確定航天器的位置和速度，所以判斷導(dǎo)航系統(tǒng)能否滿足任務(wù)要求的第一步，就是分析其可觀性。為了分析導(dǎo)航系統(tǒng)的可觀性，這里引入非線性系統(tǒng)局部弱可觀的概念[12-13]。

定義1：如果對(duì)于定義區(qū)間內(nèi)的每個(gè)X,系統(tǒng)Σ 都局部弱可觀，那么稱系統(tǒng)Σ局部弱可觀。記系統(tǒng)Σ的觀測(cè)矩陣為

其中：n為狀態(tài)向量X的維數(shù)；(X)為k階李導(dǎo)數(shù)。

定義2：如果rank(Q(X0))=n，那么稱系統(tǒng)Σ在X0點(diǎn)滿足可觀性秩條件。

這里利用非線性系統(tǒng)的可觀性秩條件來分析自主導(dǎo)航系統(tǒng)的可觀性。

容易看出，的具體表達(dá)式將非常復(fù)雜，所以，觀測(cè)矩陣的數(shù)學(xué)表達(dá)式也非常復(fù)雜，但是，已可決定Q的秩，且rank(Q0)= 9，故在此將不再計(jì)算，且定義。

需要估計(jì)的參數(shù)維數(shù)m= 9（位置、速度和姿態(tài)信息），由上述分析可知rank(Q0)= 9，根據(jù)定義2可得，對(duì)應(yīng)的導(dǎo)航系統(tǒng)在X0點(diǎn)是局部弱可觀的。

3 仿真結(jié)果

3.1 仿真驗(yàn)證

為驗(yàn)證本文提出的采用預(yù)測(cè)濾波的小天體探測(cè)自主光學(xué)導(dǎo)航系統(tǒng)的有效性和可行性，取小行星Eros433作為目標(biāo)天體進(jìn)行數(shù)學(xué)仿真，Eros433小天體物理參數(shù)如表1所示。

表1 仿真分析參數(shù)列表Table 1 The parameter list of simulation analysis

仿真初始條件為位置各方向存在50 m 的隨機(jī)誤差，速度各方向存在1 m/s 隨機(jī)誤差，姿態(tài)各軸指向存在1°的隨機(jī)誤差，導(dǎo)航陸標(biāo)位置各方向存在1 m隨機(jī)誤差，測(cè)速儀各方向存在0.01 m/s隨機(jī)誤差。設(shè)導(dǎo)航相機(jī)焦距為3.5 mm，導(dǎo)航相機(jī)和測(cè)速儀以1 s的采樣間隔輸出，圖像處理精度為1個(gè)像素，仿真采用模擬陸標(biāo)進(jìn)行。探測(cè)器在著陸過程中采用Apollo多項(xiàng)式制導(dǎo)方法，仿真初始狀態(tài)如表2所示，陀螺相關(guān)參數(shù)和加速度計(jì)相關(guān)參數(shù)如表3～4 所示，狀態(tài)誤差協(xié)方差陣初值P0= 103?I15×15。

表2 探測(cè)器仿真初始狀態(tài)Table 2 The initial simulation state of the detector

表3 陀螺相關(guān)參數(shù)Table 3 The parameters of the gyros

表4 加速度計(jì)相關(guān)參數(shù)Table 4 the Parameters of the Accelerometers

在上述設(shè)定參數(shù)下，采用改進(jìn)的預(yù)測(cè)濾波算法的仿真結(jié)果如圖2（a）和（b）所示。從圖2 中的仿真結(jié)果可以看出，使用改進(jìn)的NPF 算法較EKF 實(shí)時(shí)性更好，位置、速度和姿態(tài)曲線收斂速度與EKF 相比更快趨于收斂，且具有較高的導(dǎo)航精度。

圖2 改進(jìn)的NPF和EKF濾波曲線Fig.2 The filter curve of improved NPF and EKF

3.2 誤差分析

由于濾波模型中的小天體參數(shù)存在不確定性，這里假設(shè)小天體引力場(chǎng)模型存在x%的不確定性（x=20,50，100），進(jìn)行了仿真，給出了導(dǎo)航濾波的誤差值，結(jié)果分別如下圖所示：圖3～4 為模型參數(shù)存在20%不確定度時(shí)的濾波誤差，圖5～6 為模型參數(shù)存在50%不確定度時(shí)的濾波誤差，圖7～8 為模型參數(shù)存在100%不確定度時(shí)的濾波誤差。

為研究不同模型誤差對(duì)導(dǎo)航精度的影響，此處引入均方根誤差（Root Mean Squared Error，RMSE）為評(píng)價(jià)指標(biāo)，均方根誤差按下式計(jì)算

其中：N為仿真次數(shù)，本節(jié)仿真中取N= 10；ei(t)為第i次仿真得到的狀態(tài)誤差。表5 給出了不同模型誤差下由10 次Monte Carlo 仿真得到的著陸器各狀態(tài)分量在仿真結(jié)束時(shí)刻的平均估計(jì)誤差。

從表5～6仿真數(shù)據(jù)和圖3、圖5和圖7可以看出，隨著模型誤差的增大，EKF 的估計(jì)誤差也隨之增大。而圖4、圖6 和圖8 改進(jìn)的NPF 的誤差增加幅度卻不明顯，這是由于EKF 只針對(duì)白噪聲的模型誤差進(jìn)行處理，導(dǎo)致精度下降，而改進(jìn)的NPF 對(duì)模型誤差做一步估計(jì)并進(jìn)行補(bǔ)償，不限制模型誤差的類型，從而提高了估計(jì)精度。由此驗(yàn)證了NPF 對(duì)模型誤差的估計(jì)和補(bǔ)償?shù)挠行浴?/p>

1）當(dāng)存在20%誤差不確定度時(shí)：

圖3 EKF濾波精度Fig.3 The accuracy of EKF

圖4 改進(jìn)的NPF濾波精度Fig.4 The accuracy of improved NPF

2）當(dāng)存在50%誤差不確定度時(shí)：

圖5 EKF濾波精度Fig.5 The accuracy of EKF

圖6 改進(jìn)的NPF濾波精度Fig.6 The Accuracy of improved NPF

3）當(dāng)存在100%誤差不確定度時(shí)：

圖7 EKF濾波精度Fig.7 The accuracy of EKF

圖8 改進(jìn)的NPF濾波精度Fig.8 The accuracy of improved NPF

表5 不同模型誤差下EKF蒙特卡羅導(dǎo)航誤差Table 5 The Monte Carlo navigation errors under different model error of EKF

表6 不同模型誤差下改進(jìn)的NPF蒙特卡羅導(dǎo)航誤差Table 6 The Monte Carlo navigation errors under different model error of improved NPF

4 結(jié) 論

本文提出了一種用于探測(cè)器著陸小天體的光學(xué)自主導(dǎo)航濾波方法，該方法首先在慣性導(dǎo)航加測(cè)速修正的基礎(chǔ)上，將光學(xué)成像敏感器加入到導(dǎo)航系統(tǒng)。通過下降過程中拍攝天體表面陸標(biāo)圖像以獲得著陸器相對(duì)陸標(biāo)的方位信息，在進(jìn)行狀態(tài)估計(jì)是采用基于預(yù)測(cè)濾波與擴(kuò)展卡爾曼濾波相結(jié)合的濾波器，并通過仿真驗(yàn)證了在存在較大模型誤差時(shí)，改進(jìn)的預(yù)測(cè)濾波方法可以有效地估計(jì)探測(cè)器的位置和速度信息，且算法對(duì)模型不確定性具有一定的適應(yīng)性，可適應(yīng)小天體復(fù)雜多變的環(huán)境。