基于未知陸標(biāo)圖像測(cè)量的自主相對(duì)導(dǎo)航方法研究

徐 超，黃翔宇，李茂登

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

引 言

隨著深空探測(cè)技術(shù)的發(fā)展，越來越多的行星探測(cè)任務(wù)要求探測(cè)器能夠在復(fù)雜未知地形環(huán)境下安全著陸，這就要求探測(cè)器具有在未知環(huán)境下進(jìn)行自主相對(duì)導(dǎo)航的能力[1-2]。

傳統(tǒng)的深空探測(cè)軟著陸任務(wù)中主要采用慣性導(dǎo)航為主的自主導(dǎo)航方式，但慣性導(dǎo)航誤差會(huì)隨時(shí)間累積，因此需要利用其它敏感器測(cè)量對(duì)慣性導(dǎo)航進(jìn)行修正?！版隙鹑?hào)”（Chang'E-3，CE-3）和“嫦娥四號(hào)”（Chang'E-4，CE-4）軟著陸采用了基于慣性導(dǎo)航配以測(cè)距測(cè)速修正的導(dǎo)航方法[3-4]。盡管測(cè)距測(cè)速敏感器可對(duì)慣性導(dǎo)航的高度和速度進(jìn)行修正，但是其不能抑制導(dǎo)航系統(tǒng)水平位置誤差的發(fā)散，可能導(dǎo)致著陸器偏離目標(biāo)著陸點(diǎn)。

針對(duì)慣性導(dǎo)航配以測(cè)距測(cè)速修正的導(dǎo)航方法無(wú)法估計(jì)水平位置的問題，一種最為可行的解決方法是引入行星表面陸標(biāo)圖像信息進(jìn)行自主導(dǎo)航。目前采用的陸標(biāo)圖像信息的地外天體著陸自主導(dǎo)航方法的研究大都是在已有著陸區(qū)域參考地形圖的假設(shè)下進(jìn)行的，通過在下降的過程中，將圖像中提取的陸標(biāo)與包含陸標(biāo)位置信息的參考地形圖進(jìn)行匹配獲得下降圖像中導(dǎo)航陸標(biāo)的位置信息，進(jìn)而利用這些陸標(biāo)的位置信息結(jié)合其圖像測(cè)量信息得到探測(cè)器的導(dǎo)航信息[5]。然而，當(dāng)事先沒有著陸區(qū)域參考地形圖或由于參考地形圖與下降圖像的分辨率差異過大導(dǎo)致無(wú)法進(jìn)行特征匹配時(shí)，下降序列圖像中觀測(cè)和跟蹤到的陸標(biāo)位置信息將無(wú)法獲取，需要解決在未知環(huán)境下利用未知位置信息的陸標(biāo)進(jìn)行導(dǎo)航的問題。

針對(duì)未知環(huán)境下的行星著陸導(dǎo)航問題，美國(guó)國(guó)家航空航天局（National Aeronautics and Space Administration，NASA）為“火星探測(cè)漫游者”（Mars Exploration Rover，MER）任務(wù)研究了下降圖像運(yùn)動(dòng)估計(jì)子系統(tǒng)（Descent Image Motion Estimation System，DIMES），該系統(tǒng)通過跟蹤3幅下降序列圖像中的未知位置信息的特征點(diǎn)結(jié)合高度計(jì)測(cè)高信息來估計(jì)探測(cè)器相對(duì)著陸面的水平速度，通過實(shí)際的火星著陸任務(wù)驗(yàn)證了該系統(tǒng)的有效性[6-7]。然而，DIMES系統(tǒng)僅能估計(jì)探測(cè)器相對(duì)著陸面的水平速度信息。文獻(xiàn)[8～9]利用同時(shí)定位與地圖創(chuàng)建方法（Simultaneous Localization and Mapping，SLAM），將陸標(biāo)位置作為系統(tǒng)狀態(tài)進(jìn)行估計(jì)從而實(shí)現(xiàn)飛行器在未知環(huán)境下的自主導(dǎo)航，但是SLAM方法計(jì)算量大不利于實(shí)時(shí)計(jì)算；文獻(xiàn)[10～11]給出一種多狀態(tài)約束的未知環(huán)境下視覺輔助慣性導(dǎo)航的自主導(dǎo)航方法；文獻(xiàn)[12]給出一種利用未知陸標(biāo)在連續(xù)2幅下降圖像中的測(cè)量建立隱式測(cè)量模型對(duì)著陸器狀態(tài)進(jìn)行估計(jì)的方法，但該方法只適用于著陸面近似為平面的情況。上述方法雖然都避免了需要陸標(biāo)位置的先驗(yàn)信息，但這些導(dǎo)航方法系統(tǒng)狀態(tài)都不完全可觀，因而探測(cè)器某些狀態(tài)在著陸過程中并不能得到精確估計(jì)。

針對(duì)未知環(huán)境下行星表面導(dǎo)航問題，本文提出一種利用未知陸標(biāo)序列圖像測(cè)量的行星表面相對(duì)導(dǎo)航方法。該方法可有效估計(jì)探測(cè)器相對(duì)行星表面的速度，較為精確地估計(jì)探測(cè)器相對(duì)于著陸點(diǎn)或陸標(biāo)的位置，從而可滿足精確避障著陸的導(dǎo)航要求。以火星著陸為例的數(shù)學(xué)仿真表明該方法是有效可行的。

1 基于慣性測(cè)量的行星著陸運(yùn)動(dòng)學(xué)模型

以初始時(shí)刻星下點(diǎn)天南東系為著陸參考坐標(biāo)系，假設(shè)慣性敏感器安裝和零偏等偏差已經(jīng)通過標(biāo)定消除，探測(cè)器相對(duì)著陸坐標(biāo)系的姿態(tài)可通過陀螺和星敏等敏感器精確確定，則利用加速度計(jì)測(cè)量建立探測(cè)器著陸運(yùn)動(dòng)學(xué)方程[12]

加速度計(jì)測(cè)量模型為

2 未知陸標(biāo)圖像隱式測(cè)量模型

隱式測(cè)量模型利用陸標(biāo)序列圖像測(cè)量信息結(jié)合探測(cè)器估計(jì)位姿構(gòu)建隱式測(cè)量方程，該測(cè)量模型不需要陸標(biāo)位置先驗(yàn)信息。

圖1 未知陸標(biāo)序列圖像測(cè)量示意Fig.1 Schematic diagram of a unknown landmark observed in three sequential images

由式（4）可求解得到ti和tj時(shí)刻相機(jī)到陸標(biāo)Pj的距離

根據(jù)式（3）和式（5）可計(jì)算ti+1時(shí)刻陸標(biāo)Pj的位置為

則ti+2時(shí)刻陸標(biāo)Pj在相機(jī)系下的視線矢量為

由式（7）和式（8）可看出，對(duì)任意位置未知的陸標(biāo)，只要在3幅下降圖像中觀測(cè)到該陸標(biāo)，即可建立該陸標(biāo)的隱式測(cè)量方程用于探測(cè)器的狀態(tài)估計(jì)。

3 基于未知陸標(biāo)圖像測(cè)量的導(dǎo)航濾波算法

3.1 狀態(tài)方程

由（7）和式（8）可知，未知陸標(biāo)的隱式測(cè)量模型同時(shí)與探測(cè)器在3個(gè)時(shí)刻的狀態(tài)相關(guān)，故基于未知陸標(biāo)圖像隱式測(cè)量模型的自主導(dǎo)航系統(tǒng)狀態(tài)向量取為

當(dāng)相機(jī)拍攝一幅新圖像后系統(tǒng)狀態(tài)按如下方法確定

其中：xi+1和xi+2分別為成像時(shí)刻ti+1和ti+2之后的系統(tǒng)狀態(tài)，M為

根據(jù)式（9）定義的探測(cè)器狀態(tài)，可得探測(cè)器誤差狀態(tài)為

線性化式（1）得誤差狀態(tài)方程

3.2 隱式測(cè)量方程

由第2部分可得，ti+2時(shí)刻導(dǎo)航陸標(biāo)Pj估計(jì)觀測(cè)量為

則ti+2時(shí)刻觀測(cè)量殘差

根據(jù)狀態(tài)誤差定義，式（18）可寫為

3.3 導(dǎo)航濾波器

采用EKF作為濾波算法對(duì)探測(cè)器狀態(tài)進(jìn)行估計(jì)。當(dāng)慣性敏感器得到加速度測(cè)量數(shù)據(jù)時(shí)，系統(tǒng)狀態(tài)可按式（1）進(jìn)行遞推，同時(shí)系統(tǒng)狀態(tài)誤差協(xié)方差可由下式積分得到

由于導(dǎo)航系統(tǒng)每獲得一幅圖像時(shí)均對(duì)系統(tǒng)狀態(tài)按式（10）重置，故系統(tǒng)狀態(tài)誤差協(xié)方差P在每次成像時(shí)刻時(shí)按式（20）更新

假設(shè)有N個(gè)未知陸標(biāo)同時(shí)在ti、ti+1和ti+2時(shí)刻拍攝的下降圖像中被檢測(cè)到，則可得觀測(cè)殘差向量

卡爾曼濾波增益計(jì)算為

導(dǎo)航系統(tǒng)狀態(tài)和協(xié)方差更新為

4 仿真分析

為了驗(yàn)證本文提出的導(dǎo)航方案，以火星著陸為例進(jìn)行仿真，參考“好奇號(hào)”（Curiosity）火星著陸探測(cè)器的軌道數(shù)據(jù)[13]以及降落傘和主發(fā)動(dòng)機(jī)的減速能力，設(shè)置仿真初始條件：探測(cè)器在初始時(shí)刻星下點(diǎn)天南東系下位置和速度如表1所示。

表1 探測(cè)器仿真初始狀態(tài)Table 1 Initial state parameters

假設(shè)探測(cè)器在著陸過程中始終保持相機(jī)指向著陸點(diǎn)方向。仿真初始條件為位置各方向存在1 000 m的隨機(jī)誤差，速度各方向存在10 m/s隨機(jī)誤差。設(shè)陸標(biāo)方向矢量測(cè)量噪聲為0.1＇，當(dāng)沒有陸標(biāo)圖像測(cè)量可用于導(dǎo)航更新時(shí)探測(cè)器狀態(tài)根據(jù)慣性測(cè)量單元（Inertial Measurement Unit，IMU）測(cè)量積分得到。仿真采用模擬陸標(biāo)進(jìn)行，并在著陸過程拍攝的第一幅下降圖像中選擇安全著陸點(diǎn)，為了在下降圖像中持續(xù)跟蹤安全著陸點(diǎn)，需要在著陸點(diǎn)附近選取4個(gè)特征點(diǎn)且4個(gè)特征點(diǎn)中任意3個(gè)均不共線，從而根據(jù)射影幾何交比不變定理利用選定的4個(gè)特征點(diǎn)在前后圖像中的位置可實(shí)現(xiàn)對(duì)安全著陸點(diǎn)的連續(xù)跟蹤，因此仿真中以選定的4個(gè)特征點(diǎn)及安全著陸點(diǎn)作為導(dǎo)航陸標(biāo)進(jìn)行仿真。需要注意的是，除著陸點(diǎn)外，每連續(xù)成像3次后根據(jù)相機(jī)視場(chǎng)重新選取4個(gè)特征點(diǎn)作為導(dǎo)航陸標(biāo)，以保證可以在下降序列圖像中存在可用的導(dǎo)航陸標(biāo)。

圖2和圖3給出了探測(cè)器著陸軌跡的位置和速度變化，圖4和圖5給出了探測(cè)器著陸過程初始時(shí)刻星下點(diǎn)天南東系下位置速度的估計(jì)誤差。從圖4和圖5中可看出，采用所提出的導(dǎo)航方法可以對(duì)探測(cè)器的速度進(jìn)行精確估計(jì)，精度可優(yōu)于0.1 m/s，但無(wú)法對(duì)位置信息進(jìn)行修正，只能將位置誤差維持在初始誤差附近。值得注意的是，在未知環(huán)境著陸過程中，通常更關(guān)注探測(cè)器相對(duì)于著陸點(diǎn)的位置及速度信息。由于著陸點(diǎn)或障礙物在行星表面靜止不動(dòng)，因此探測(cè)器相對(duì)于著陸點(diǎn)的速度與著陸參考系下的速度相同，由圖5可知其可被精確估計(jì)。探測(cè)器相對(duì)于著陸點(diǎn)的位置可通過估計(jì)的探測(cè)器位置以及著陸點(diǎn)在連續(xù)3幅下降圖像中的視線矢量測(cè)量計(jì)算得到，如圖6和圖7所示，可見利用所提出的導(dǎo)航方法可有效估計(jì)探測(cè)相對(duì)于著陸點(diǎn)的位置，精度可優(yōu)于1.5 m，滿足精確避障著陸的要求。

圖2 探測(cè)器位置Fig.2 The position in up-south-east frame during landing

圖3 探測(cè)器速度Fig.3 The velocity in up-south-east frame during landing

圖4 探測(cè)器位置估計(jì)誤差Fig.4 The position estimation errors in up-south-east frame during landing

圖5 探測(cè)器速度估計(jì)誤差Fig.5 The velocity estimation errors in up-south-east frame during landing

圖6 探測(cè)器相對(duì)于著陸點(diǎn)位置Fig.6 The relative position from the lander to the target landing site in upsouth-east frame during landing

圖7 探測(cè)器相對(duì)于著陸點(diǎn)位置估計(jì)誤差Fig.7 The relative position estimation errors from the lander to the target landing site.in up-south-east frame during landing

5 結(jié) 論

本文研究了一種利用未知陸標(biāo)序列圖像測(cè)量的行星表面相對(duì)導(dǎo)航方法。該方法首先利用未知陸標(biāo)在連續(xù)3幅下降圖像中測(cè)量得到的視線方向矢量結(jié)合探測(cè)器成像時(shí)刻位置建立了未知陸標(biāo)的隱式測(cè)量模型，然后利用建立的未知陸標(biāo)隱式測(cè)量模型通過卡爾曼濾波對(duì)探測(cè)器的位置、速度進(jìn)行估計(jì)，最后利用估計(jì)的探測(cè)器位置結(jié)合著陸點(diǎn)的視線方向測(cè)量計(jì)算著陸點(diǎn)位置，進(jìn)而獲得探測(cè)器相對(duì)于著陸點(diǎn)的位置估計(jì)。仿真結(jié)果表明，提出的方法可有效估計(jì)探測(cè)器相對(duì)行星表面的速度，雖然無(wú)法直接估計(jì)探測(cè)器的位置，但可較為精確的估計(jì)出探測(cè)器相對(duì)于著陸點(diǎn)或陸標(biāo)的位置，從而可滿足精確避障著陸的導(dǎo)航要求。