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

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

引 言

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

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

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

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

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

1.1 小天體引力場模型

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

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

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

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

1.2 小天體著陸運動學(xué)模型與測量模型

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

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

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

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

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

1.2.1 小天體探測器運動學(xué)模型

利用慣性敏感器測量建立探測器著陸運動學(xué)方程為

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

Ω(ω)定義如下

1.2.2 IMU測量模型

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

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

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

1.2.3 導(dǎo)航相機測量模型

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

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

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

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

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

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

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

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

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

1.2.4 測速敏感器測量模型

測速敏感器輸出的相對速度的測量方程[11]如下

其中，nu為測速敏感器測量噪聲。

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

2.1 狀態(tài)方程

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

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

估計狀態(tài)為

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

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

其中

2.2 觀測方程

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

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

則第i時刻的觀測量殘差為

測量方程為

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

其中

對于測距測速敏感器，由（11）可得觀測方程為

其中測量敏感矩陣為

其中

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

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

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

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

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

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

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

其中：括號中的k表示第k個采樣時刻；y(k+ 1)是t(k + 1)時刻的測量輸出。預(yù)測輸出為

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

其中

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

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

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

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

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

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

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

3）由EKF計算濾波值為

4）計算濾波增益陣

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

估計方差陣為

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

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

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

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

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

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

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

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

3 仿真結(jié)果

3.1 仿真驗證

為驗證本文提出的采用預(yù)測濾波的小天體探測自主光學(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 的隨機誤差，速度各方向存在1 m/s 隨機誤差，姿態(tài)各軸指向存在1°的隨機誤差，導(dǎo)航陸標(biāo)位置各方向存在1 m隨機誤差，測速儀各方向存在0.01 m/s隨機誤差。設(shè)導(dǎo)航相機焦距為3.5 mm，導(dǎo)航相機和測速儀以1 s的采樣間隔輸出，圖像處理精度為1個像素，仿真采用模擬陸標(biāo)進(jìn)行。探測器在著陸過程中采用Apollo多項式制導(dǎo)方法，仿真初始狀態(tài)如表2所示，陀螺相關(guān)參數(shù)和加速度計相關(guān)參數(shù)如表3～4 所示，狀態(tài)誤差協(xié)方差陣初值P0= 103?I15×15。

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

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

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

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

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

3.2 誤差分析

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

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

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

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

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

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

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

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

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

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

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

圖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é) 論

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