張曉文，李 驥，黃翔宇，王大軼

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

0 引言

未來的地外天體著陸探測(cè)任務(wù)出于具備更高安全性和獲得更多高科學(xué)價(jià)值數(shù)據(jù)考慮，對(duì)探測(cè)器落點(diǎn)的精度提出了越來越高的要求，尤其是載人探測(cè)任務(wù)［1-2］.這就對(duì)星上的導(dǎo)航系統(tǒng)提出了新的挑戰(zhàn)，因?yàn)榫_制導(dǎo)的前提條件是探測(cè)器位置速度實(shí)時(shí)精確可知.

20世紀(jì)60年代末期美國阿波羅(Apollo)載人飛船登月時(shí)，星上導(dǎo)航系統(tǒng)主要由慣性測(cè)量單元和微波雷達(dá)構(gòu)成［3］.2005年日本隼鳥號(hào)(Hayabusa)探測(cè)器附著Itokawa小行星時(shí)利用激光測(cè)距敏感器確定其高度，利用光學(xué)成像敏感器拍攝事先投放到小行星表面的人工陸標(biāo)確定其相對(duì)小行星的水平速度［4-5］.2013年中國首個(gè)月球著陸探測(cè)器嫦娥三號(hào)成功登陸月球虹灣，其導(dǎo)航系統(tǒng)與阿波羅飛船相比，新增了激光測(cè)距敏感器和避障敏感器［6］.使用激光或微波作介質(zhì)的測(cè)距敏感器和測(cè)速敏感器能夠?qū)Ω叨群退俣茸鞒黾皶r(shí)修正，與慣性導(dǎo)航相配合極大地提高了著陸過程的自主導(dǎo)航精度［7］.但是測(cè)距信息僅能對(duì)高度作出修正，不能提供相對(duì)天體表面的水平位置信息.利用光學(xué)成像敏感器拍攝的天體表面二維圖像［8］，或是利用激光掃描雷達(dá)獲取的天體地表三維圖像［9］，則可以提供相對(duì)方位信息.此外，二維或三維地面圖像除了用于導(dǎo)航還可用于對(duì)大坑或凸起等障礙的識(shí)別［10］，實(shí)現(xiàn)了敏感器的復(fù)用.

在獲取了各個(gè)敏感器測(cè)量數(shù)據(jù)之后還需利用信息融合方法統(tǒng)一處理以給出著陸器位置速度最優(yōu)估計(jì).集中式濾波方法雖然能夠?qū)λ袦y(cè)量信息作最優(yōu)融合處理，但是其濾波方程維數(shù)很高，不利于星上實(shí)時(shí)計(jì)算，同時(shí)該方法的容錯(cuò)性差，不易處理不同敏感器的時(shí)間同步性問題，因此不適合工程應(yīng)用.基于協(xié)方差交叉理論的分布式濾波方法在眾多信息融合方法中獲得了越來越多的關(guān)注和認(rèn)可.該方法與聯(lián)邦濾波等分布式濾波方法相比主要優(yōu)點(diǎn)是不要求局部狀態(tài)估計(jì)相互獨(dú)立或已知其相關(guān)性，同時(shí)其融合估計(jì)精度高于每個(gè)局部估計(jì)的精度［11-12］.

針對(duì)慣性導(dǎo)航誤差不斷累計(jì)，以及測(cè)距測(cè)速信息僅能修正高度和速度不能修正相對(duì)位置的缺點(diǎn)，將光學(xué)成像敏感器加入到導(dǎo)航系統(tǒng)中，通過拍攝天體表面已知特征點(diǎn)的二維灰度圖像并與星上數(shù)字地形圖匹配比較獲取相對(duì)水平位置信息，從而顯著提高了下降過程中相對(duì)水平位置估計(jì)的精度.針對(duì)集中式濾波以及聯(lián)邦濾波的各種固有缺陷，設(shè)計(jì)了基于協(xié)方差交叉的分布式擴(kuò)展卡爾曼濾波器，對(duì)多源測(cè)量信息作最優(yōu)信息融合，提高了狀態(tài)估計(jì)精度和算法魯棒性.

1 導(dǎo)航系統(tǒng)方案

基于陸標(biāo)圖像的組合導(dǎo)航系統(tǒng)的組成見圖1.圖1中的左側(cè)部分為敏感器，分別為慣性測(cè)量單元、測(cè)距敏感器、測(cè)速敏感器和光學(xué)成像敏感器.圖1中右側(cè)部分為星上軟件算法模塊，分別為姿態(tài)確定、圖像處理、數(shù)字地形圖和位置速度確定.慣性測(cè)量單元中的陀螺組件測(cè)量和輸出3軸角度增量，經(jīng)姿態(tài)確定算法處理得到姿態(tài).慣性測(cè)量單元中的加速度計(jì)組件測(cè)量和輸出3軸速度增量.慣性測(cè)量單元在初始對(duì)準(zhǔn)之后不再依賴外部輸入，因此慣性導(dǎo)航在著陸下降全程運(yùn)行.測(cè)距敏感器向探測(cè)器下方發(fā)射波束測(cè)量該方向斜距.測(cè)速敏感器向探測(cè)器下方發(fā)射3束不共面的波束測(cè)量相對(duì)速度.光學(xué)成像敏感器拍攝前下方地面特征點(diǎn)的二維灰度圖像.圖像經(jīng)圖像處理算法計(jì)算給出陸標(biāo)像素坐標(biāo).速度增量、斜距、相對(duì)速度和陸標(biāo)像素等測(cè)量最終由位置速度確定算法統(tǒng)一處理給出探測(cè)器位置速度的實(shí)時(shí)精確估計(jì).

圖1 自主導(dǎo)航系統(tǒng)組成與信號(hào)流圖Fig.1 Composition and signal flow of the autonomous navigation system

2 動(dòng)力學(xué)方程

地外天體探測(cè)器的軌道動(dòng)力學(xué)方程如下:

式中:r為探測(cè)器在慣性系中位置矢量，r為其模;v為探測(cè)器在慣性系中速度矢量;μ為中心天體引力常數(shù);F為除引力外作用于探測(cè)器的合推力;m為探測(cè)器質(zhì)量;aε為天體非球形引力和第三體引力等引起的攝動(dòng)加速度.

3 測(cè)量方程

3.1 斜 距

測(cè)距敏感器輸出的斜距的測(cè)量方程如下:

式中:sl為測(cè)距敏感器含誤差的標(biāo)度因數(shù);Rm為天體參考半徑;h(r)為天體數(shù)字高程函數(shù);“·”為求數(shù)量積符號(hào);Cib為本體系到慣性系的姿態(tài)轉(zhuǎn)換陣;Al為測(cè)距波束在本體系指向;bl為測(cè)距敏感器常值偏差;nl為測(cè)距敏感器測(cè)量噪聲，與距離相關(guān).

3.2 相對(duì)速度

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

式中:su為測(cè)速敏感器含誤差的標(biāo)度因數(shù);Au為測(cè)速波束在本體系指向;Cbi為慣性系到本體系的姿態(tài)轉(zhuǎn)換陣;ωm為天體自轉(zhuǎn)角速度矢量;“×”為求向量積符號(hào);bu為測(cè)速敏感器常值偏差;nu為測(cè)速敏感器測(cè)量噪聲.

3.3 陸標(biāo)像素坐標(biāo)

陸標(biāo)像素坐標(biāo)的測(cè)量方程如下:

式中:Kp為光學(xué)成像敏感器像素坐標(biāo)轉(zhuǎn)換陣;Cpb為本體系到光學(xué)成像敏感器測(cè)量系的姿態(tài)轉(zhuǎn)換陣;ro為陸標(biāo)在慣性系中位置矢量;p0為焦平面中心的像素坐標(biāo);np為光學(xué)成像敏感器測(cè)量噪聲.

4 導(dǎo)航算法

導(dǎo)航算法中狀態(tài)變量x取為探測(cè)器在慣性系中的位置矢量r和速度矢量v.由于式(1)所示的系統(tǒng)方程和式(2)及(4)所示的測(cè)量方程都是狀態(tài)變量的非線性方程，因此子濾波器采用擴(kuò)展卡爾曼濾波.各子濾波器給出的狀態(tài)估計(jì)和誤差協(xié)方差陣?yán)脜f(xié)方差交叉法進(jìn)行信息融合.具體計(jì)算步驟如下.

(1)慣導(dǎo)外推

式中:下標(biāo)k為迭代計(jì)算序號(hào);為陀螺組合三軸等效常值漂移;Δt為導(dǎo)航周期.

式中Cib，k為由轉(zhuǎn)換得到的姿態(tài)轉(zhuǎn)換陣.

慣導(dǎo)外推為探測(cè)器提供了獨(dú)立實(shí)時(shí)的外力作用下位置速度的基本估計(jì).在沒有引入斜距、相對(duì)速度和陸標(biāo)像素坐標(biāo)等測(cè)量信息之前導(dǎo)航系統(tǒng)僅執(zhí)行慣導(dǎo)外推.

(2)誤差協(xié)方差一步預(yù)測(cè)

式中:Φk，k-1為狀態(tài)轉(zhuǎn)移矩陣;Pk-1為誤差協(xié)方差估計(jì);Qk-1為系統(tǒng)噪聲方差陣.

狀態(tài)轉(zhuǎn)移矩陣的計(jì)算公式為

式中I為單位陣.

(3)子濾波器測(cè)量更新

對(duì)當(dāng)前導(dǎo)航計(jì)算周期內(nèi)的每項(xiàng)有效測(cè)量構(gòu)造一個(gè)基于擴(kuò)展卡爾曼濾波的子濾波器，并用上標(biāo)i來表示子濾波器的序號(hào).記第i個(gè)有效測(cè)量的測(cè)量方程的右端表達(dá)式為hi(x)，對(duì)其求偏導(dǎo)得到線性化的k時(shí)刻的測(cè)量矩陣如下:

計(jì)算子濾波器的濾波增益矩陣和狀態(tài)估計(jì):

計(jì)算子濾波器的誤差協(xié)方差陣

(4)基于協(xié)方差交叉的信息融合估計(jì)

計(jì)算子濾波器的誤差協(xié)方差陣的跡

式中函數(shù)trace()表示求矩陣跡.

計(jì)算子濾波器的非負(fù)加權(quán)系數(shù)

式中n為子濾波器總數(shù)，即有效測(cè)量總數(shù).

計(jì)算信息融合后的誤差協(xié)方差陣和狀態(tài)估計(jì)

下一輪導(dǎo)航計(jì)算重復(fù)上述步驟直到任務(wù)結(jié)束.

5 數(shù)學(xué)仿真

數(shù)學(xué)仿真采用文獻(xiàn)［7］中描述的常推力顯示制導(dǎo)下的著陸軌跡，下降初始高度為15 km，終端高度為100 m.導(dǎo)航系統(tǒng)在徑向、法向和前向分別有1.2 km和1.2 m/s的初始位置速度誤差.慣性測(cè)量單元中陀螺的隨機(jī)漂移為0.2(°)/h(σ)，常值漂移為0.2(°)/h;加速度計(jì)的測(cè)量噪聲為 0.002 m/s2(σ).測(cè)距敏感器的標(biāo)度因數(shù)誤差為0.10%(σ)，常值偏差為0.1 m，測(cè)量噪聲為2%(σ).測(cè)速敏感器的標(biāo)度因數(shù)誤差為 0.13%(σ)，常值偏差為0.01 m/s，測(cè)量噪聲為0.16 m/s(σ).陸標(biāo)像素坐標(biāo)的圖像處理提取誤差為0.1像素(σ).測(cè)距敏感器和測(cè)速敏感器的標(biāo)度因數(shù)誤差與常值偏差按照時(shí)間常數(shù)為100 s的一階馬爾科夫過程建模.除光學(xué)導(dǎo)航敏感器采樣頻率為0.1 Hz外，其余導(dǎo)航敏感器采樣頻率均為20 Hz.

系統(tǒng)噪聲方差陣取為對(duì)角陣，前3個(gè)對(duì)角元素為0，后3個(gè)對(duì)角元素取1×10-6(m2/s4)，選擇依據(jù)是慣性導(dǎo)航誤差，以及數(shù)學(xué)仿真中實(shí)際濾波效果.記斜距估計(jì)為.斜距、相對(duì)速度、陸標(biāo)像素坐標(biāo)的測(cè)量噪聲方差取值依次為0.1(像素)2，選擇依據(jù)是敏感器誤差源類型和大小，以及數(shù)學(xué)仿真中實(shí)際濾波效果.

數(shù)學(xué)仿真設(shè)定3種工況.工況1是導(dǎo)航系統(tǒng)僅進(jìn)行慣性導(dǎo)航.工況2是導(dǎo)航系統(tǒng)不僅進(jìn)行慣性導(dǎo)航，同時(shí)還在下降開始100 s后加入斜距和相對(duì)速度測(cè)量對(duì)高度和相對(duì)速度估計(jì)的修正.工況3是本文提出導(dǎo)航方法，即在工況2的基礎(chǔ)上從110 s起加入陸標(biāo)像素坐標(biāo)測(cè)量對(duì)相對(duì)水平位置估計(jì)的修正.仿真結(jié)果見圖2至圖7所示的導(dǎo)航誤差曲線.圖2和圖3是工況1的結(jié)果，其位置速度誤差曲線隨著時(shí)間推移而不斷增大.圖4和圖5是工況2的結(jié)果.圖4中天向位置誤差在100 s后迅速下降，東向和北向位置誤差在經(jīng)過200 s的震蕩后有所下降.圖5中3個(gè)方向的速度估計(jì)誤差在100 s后均迅速下降，北向速度誤差有約60 s振蕩過程.圖4和圖5結(jié)果表明斜距和相對(duì)速度測(cè)量引入后有效地修正了高度和速度.圖6和圖7是工況3的結(jié)果.圖6中天向位置誤差在100 s后迅速下降，東向和北向位置誤差在110 s后迅速下降，表明引入陸標(biāo)像素信息后位置估計(jì)立刻得到提高.圖7中3個(gè)方向的速度估計(jì)誤差在100 s后均迅速下降，北向速度誤差有約20 s振蕩過程.

圖2 慣性導(dǎo)航位置誤差Fig.2 Inertial navigation position estimate errors

圖3 慣性導(dǎo)航速度誤差Fig.3 Inertial navigation velocity estimate errors

圖4 引入測(cè)距測(cè)速信息的導(dǎo)航位置誤差Fig.4 Position estimate errors when slant rang and relative velocity measurements are uesd

圖5 引入測(cè)距測(cè)速信息的導(dǎo)航速度誤差Fig.5 Velocity estimate errors when slant rang and relative velocity measurements are used

將3種工況的終端導(dǎo)航誤差列于表1中作比較.從表1可知工況1無論是位置誤差還是速度誤差均遠(yuǎn)大于另外2種工況.工況2的位置估計(jì)誤差大于工況3結(jié)果達(dá)2個(gè)數(shù)量級(jí)，且主要是由水平位置估計(jì)誤差引起，兩者的高度估計(jì)誤差相當(dāng).工況2和工況3的速度估計(jì)誤差均小于0.1 m/s.該結(jié)果表明本文提出的自主導(dǎo)航方法獲得導(dǎo)航結(jié)果在3個(gè)仿真工況中最優(yōu).

圖6 引入陸標(biāo)圖像的導(dǎo)航位置誤差Fig.6 Position estimate errors when landmark images are uesd

圖7 引入陸標(biāo)圖像的導(dǎo)航速度誤差Fig.7 Velocity estimate errors when landmark images are uesd

表1 終端導(dǎo)航誤差Tab.1 Terminal navigation errors

6 結(jié)論

本文研究了一種基于陸標(biāo)圖像和信息融合的天體定點(diǎn)著陸自主導(dǎo)航方法.該方法首先在慣性導(dǎo)航加測(cè)距測(cè)速修正的基礎(chǔ)上，將光學(xué)成像敏感器加入到導(dǎo)航系統(tǒng)，通過在下降過程中拍攝天體表面陸標(biāo)圖像以獲得著陸器相對(duì)陸標(biāo)的方位信息，顯著提高水平位置確定精度.其次在進(jìn)行狀態(tài)估計(jì)時(shí)設(shè)計(jì)了基于協(xié)方差交叉的分布式擴(kuò)展卡爾曼濾波器，對(duì)多源測(cè)量信息作最優(yōu)融合，除了提高狀態(tài)估計(jì)精度，還提高了算法魯棒性，使得動(dòng)態(tài)濾波過程適應(yīng)多敏感器異步采樣或是部分測(cè)量無效的情形.數(shù)學(xué)仿真結(jié)果驗(yàn)證了該方法的有效性，獲得的位置速度估計(jì)在各個(gè)方向精度均很高.結(jié)果對(duì)比表明陸標(biāo)圖像的引入不僅起到了大幅減小水平位置估計(jì)誤差的作用，同時(shí)還加快了濾波收斂速度.

［1］TODD A E，MARTIN H，JOSEPH E R.Altair navigation during trans-lunar cruise，lunar orbit，descent and landing［C］//AIAA Guidance，Navigation，and Control Conference.Washington D.C.:AIAA，2010:1-33.

［2］JOSEPH E R，ANDREW T V，ROBERT A W，et al.Optical navigation plan and strategy for the lunar lander Altair［C］//AIAA Guidance，Navigation，and Control Conference.Washington D.C.:AIAA，2010:1-40.

［3］EICHLER J.A performance study of the lunar module's landing radar system ［C］//AIAA Guidance，Control and Flight Dynamics Conference.Washington D.C.:AIAA，1967:1-24.

［4］GASKELL R，BARNOUIN-JHA O，SCHEERES D，et al.Landmark navigation studies and target characterization in the Hayabusa encounter with Itokawa［C］//AIAA/AAS Astrodynamics Specialist Conference and Exhibit.Washiongton D.C.:AIAA，2006:1-12.

［5］TAKASHI K，MASATOSHI M，MASASHI U，et al.Optical hybrid navigation and station keeping around Itokawa［C］//AIAA/AAS Astrodynamics Specialist Conference and Exhibit.Washiongton D.C.:AIAA，2006:1-15.

［6］張洪華，關(guān)軼峰，黃翔宇，等.嫦娥三號(hào)著陸器動(dòng)力下降的制導(dǎo)導(dǎo)航與控制［J］.中國科學(xué):技術(shù)科學(xué)，2014，44(4):377-384.ZHANG H H，GUAN Y F，HUANG X Y，et al.Guidance navigation and control for Chang'E-3 powered descent［J］.Scientia Sinica Technologica，2014，44(4):377-384.

［7］黃翔宇，張洪華，王大軼，等.月球軟著陸的高精度自主導(dǎo)航與控制方法研究［J］.空間控制技術(shù)與應(yīng)用，2012，38(2):5-9.HUANG X Y，ZHANG H H，WANG D Y，et al.Pinpoint autonomous navigation and control for lunar soft landing［J］.Aerospace Control and Application，2012，38(2):5-9.

［8］NEVEU D，BRUNEAN-DUBUC S，ALGER M，et al.Pinpoint lunar landing navigation using crater detection and matching:design and laboratory validation［C］//AIAA Guidance，Navigation，and Control Conference.Washington D.C.:AIAA，2012:1-25.

［9］崔平遠(yuǎn)，馮軍華，朱圣英，等.基于三維地形匹配的月球軟著陸導(dǎo)航方法研究［J］.宇航學(xué)報(bào)，2011，32(3):470-476.CUI P Y，F(xiàn)ENG J H，ZHU S Y，et al.3D Terrain feature matching based navigation for lunar soft landing［J］.Journal of Astronautics，2011，32(3):470-476.

［10］TONISLAV I，ANDRES H，JOHN M C.Probabilistic hazard detection for autonomous safe landing［C］//AIAA Guidance，Navigation，and Control(GNC)Conference.Washington D.C.:AIAA，2013:1-13.

［11］LIGGINS M E，HALL D L，LLINAS J.Hand book of multisensor data fusion，theory and practice［M］.New York:CRC Press，2009:319-444.

［12］JULIER S J，UHLMANN J K.A non-divergent estimation algorithm in the presence of unknow correlation［C］//Proceedings of 1997 IEEE American Control Conference.New York:IEEE，1997:2369-2373.