秦 同，朱圣英，崔平遠(yuǎn)，欒恩杰,2

(1. 北京理工大學(xué)宇航學(xué)院，北京 100081；2. 國家國防科技工業(yè)局，北京 100048)

0 引 言

動力下降段是行星著陸的最終階段，直接決定了最終的著陸精度。為實現(xiàn)精確著陸，著陸器在該階段需通過自主導(dǎo)航實現(xiàn)高精度姿軌估計，并以此為基礎(chǔ)實施制導(dǎo)控制，到達(dá)預(yù)定著陸點[1]。光學(xué)導(dǎo)航相機觀測量中同時包含了著陸器的位置與姿態(tài)信息，且視覺導(dǎo)航自主性強，技術(shù)成熟，是實現(xiàn)動力下降段著陸器姿軌估計的有效途徑[2]。

對行星著陸視覺導(dǎo)航的研究主要分為三類。第一類為絕對視覺導(dǎo)航。相機提取大型自然路標(biāo)，通過與行星地形數(shù)據(jù)庫匹配獲得路標(biāo)在行星固連坐標(biāo)系下的位置，以此為導(dǎo)航參考估計著陸器的狀態(tài)[3-4]。該方法可以獲得著陸器在固連坐標(biāo)系下的絕對位置、速度及姿態(tài)信息。然而，由于在行星著陸動力下降段，著陸器高度較低，相機視場受限，著陸區(qū)域通常又選在自然路標(biāo)匱乏的大范圍平坦區(qū)[5]，因此難以觀測到可用的自然路標(biāo)。且行星全局地形數(shù)據(jù)庫本身存在一定的誤差，影響了導(dǎo)航系統(tǒng)精度[6]。

第二類為相對位姿估計。光學(xué)相機只需提取圖像中的隨機特征點，通過序列圖像中特征點匹配，估計著陸器的位置及姿態(tài)變化，進而獲得著陸器的速度與角速度信息[7-8]。該方法不依賴地形數(shù)據(jù)庫，隨機特征點可以為隕石坑、巖石等，數(shù)量充足。然而該方法無法獲得著陸器相對著陸點的狀態(tài)信息，不能滿足定點著陸的要求。

第三類為相對著陸點的視覺導(dǎo)航。著陸器通過相機跟蹤預(yù)定著陸點或在線選取著陸點[9]，并提取著陸點周圍隨機特征點，利用隨機特征點像素坐標(biāo)估計著陸器相對著陸點的狀態(tài)，實現(xiàn)定點著陸[10-12]。該方法既有充足的特征點做導(dǎo)航參考，又可實現(xiàn)相對著陸點的狀態(tài)估計，滿足行星精確軟著陸的需求。本文的研究即屬于此類視覺導(dǎo)航方法。

在實現(xiàn)相對視覺導(dǎo)航時，由于單一相機無法獲得景深信息，因此需其他輔助敏感器。文獻(xiàn)[12]通過雙目視覺相機實現(xiàn)了相對導(dǎo)航，獲得了著陸器的全狀態(tài)高精度估計。然而雙目相機的應(yīng)用受可視區(qū)域、多特征點可分辨率約束以及視差可分辨率約束，且圖像處理過程較單目相機復(fù)雜，實時性較差。文獻(xiàn)[11]研究了利用三波束雷達(dá)測量輔助的相對視覺導(dǎo)航方法，該方法雖然可估計出著陸器的位置和速度信息，但未能充分利用觀測信息，無法估計出著陸器的全部姿態(tài)。

本文研究基于單目相機與三波束雷達(dá)的相對視覺導(dǎo)航方法，以實現(xiàn)著陸器全狀態(tài)估計。本文提出的方法利用相機和雷達(dá)的測量信息構(gòu)建相對導(dǎo)航坐標(biāo)系并求解隨機視覺特征點在該坐標(biāo)系下的位置矢量，利用求解得到的特征點為參考，設(shè)計導(dǎo)航系統(tǒng)，估計著陸器在相對導(dǎo)航坐標(biāo)系下的位置、速度及姿態(tài)信息。同時，對導(dǎo)航系統(tǒng)進行可觀測度分析，通過構(gòu)建可觀測度矩陣，解耦分析位置和姿態(tài)的可觀測性，獲得滿足全狀態(tài)可觀的最少特征點數(shù)量及位置要求。最后通過仿真分析著陸器狀態(tài)誤差，驗證可觀測度理論分析及導(dǎo)航性能。

1 相對導(dǎo)航方案

本節(jié)首先給出光學(xué)導(dǎo)航相機與三波束雷達(dá)的觀測模型，然后利用兩者的觀測量構(gòu)建相對導(dǎo)航坐標(biāo)系，再結(jié)合動力下降段著陸器姿軌耦合動力學(xué)方程設(shè)計相對導(dǎo)航系統(tǒng)。

1.1 敏感器觀測模型

不失一般性，將導(dǎo)航相機假設(shè)為針孔相機模型，在動力下降段，相機跟蹤拍攝著陸點及其周圍地形的圖像，或者通過相機圖像在線選擇著陸點，提取著陸點及圖像中的光學(xué)特征點，其中著陸點為光學(xué)特征點之一。光學(xué)觀測原理如圖1所示。圖中，oc-xcyczc表示相機坐標(biāo)系，f為相機焦距，(p,l)為目標(biāo)點在像平面上的像素點坐標(biāo)。

圖1 光學(xué)導(dǎo)航原理示意圖Fig.1 The geometric schematic of optical navigation

光學(xué)相機的觀測模型如式(1)所示。

(1)

(2)

圖2 三波束雷達(dá)幾何示意圖Fig.2 The geometric schematic of the three-beam radar

除光學(xué)導(dǎo)航相機外，著陸器一般載有多波束雷達(dá)，可實現(xiàn)多方向激光測距，考慮文獻(xiàn)[13]中的多波束雷達(dá)，如圖2所示。圖中，ob-xbybzb為著陸器本體坐標(biāo)系，三波束在obxbyb內(nèi)均勻分布，夾角θ=120°，其中一條波束在obxbyb平面內(nèi)的投影沿xb軸方向；每一條波束與本體系Z軸的夾角均為φ=30°。雷達(dá)可測量著陸器沿波束方向到火星表面的距離，根據(jù)著陸區(qū)平面假設(shè)，觀測模型如式(3)所示。

(3)

(4)

(5)

(6)

(7)

1.2 相對導(dǎo)航坐標(biāo)系

由于動力下降段著陸器高度較低，相機視場范圍較窄，且著陸區(qū)域一般在大范圍平坦區(qū)，相機觀測到的光學(xué)特征點一般為巖石等尺度較小的地形特征，而難以觀測到火星地形數(shù)據(jù)庫中已有的地形特征。因此無法獲得特征點在火星全局坐標(biāo)系下的位置。本小節(jié)將基于光學(xué)特征點建立相對導(dǎo)航坐標(biāo)系，并利用相機與雷達(dá)的觀測量計算特征點在相對導(dǎo)航坐標(biāo)系下的位置，將此位置作為著陸器在相對導(dǎo)航坐標(biāo)系下的導(dǎo)航基準(zhǔn)。

圖3 相對導(dǎo)航坐標(biāo)系Fig.3 The relative navigation reference frame

在相對導(dǎo)航坐標(biāo)系內(nèi)進行導(dǎo)航，需獲取特征點在該坐標(biāo)系下的位置信息。根據(jù)導(dǎo)航相機的觀測量可獲得特征點在本體系下的單位方向矢量，如式(8)所示。

(8)

結(jié)合三波束雷達(dá)的測距信息及雷達(dá)安裝方向信息可得到本體系下波束照射點相對著陸器的三維位置矢量，如式(9)所示。

(9)

假設(shè)著陸區(qū)域近似為平面，則根據(jù)式(9)可得到本體下的著陸平面單位法向量，如式(10)所示。

(10)

結(jié)合式(8)與式(10)，可得到特征點在本體系下的三維位置矢量，如式(11)所示。

(11)

由此可構(gòu)建相對導(dǎo)航坐標(biāo)系的三個特征點之間的位置矢量為：

(12)

(13)

易得到on-xnynzn三軸在本體系下的單位矢量為：

(14)

(15)

ey=ez×ex

(16)

則導(dǎo)航坐標(biāo)系到本體坐標(biāo)系的旋轉(zhuǎn)矩陣為：

(17)

特征點在導(dǎo)航坐標(biāo)系下的三維位置矢量如式(18)所示。

(18)

至此便得到相對導(dǎo)航坐標(biāo)系下的導(dǎo)航參考基準(zhǔn)位置。

1.3 相對導(dǎo)航系統(tǒng)

相對導(dǎo)航系統(tǒng)需估計著陸器相對目標(biāo)點的位置、速度以及在相對導(dǎo)航坐標(biāo)系下的姿態(tài)信息。給定導(dǎo)航狀態(tài)矢量如式(19)所示。

(19)

式中，r,v,q分別為探測的位置、速度及姿態(tài)四元數(shù)。相對導(dǎo)航坐標(biāo)系并非嚴(yán)格意義上的導(dǎo)航坐標(biāo)系，然而在著陸動力下降段，由行星轉(zhuǎn)動產(chǎn)生的科氏加速度遠(yuǎn)小于行星引力加速度及控制加速度，因此可將其忽略，將相對導(dǎo)航坐標(biāo)系近似為慣性坐標(biāo)系，則著陸器的軌道動力學(xué)及姿態(tài)運動學(xué)方程如式(20)所示。

(20)

式中,ac為本體系下的控制加速度，g為行星重力加速度矢量，ω為姿態(tài)角速度，Ω(ω)為四元數(shù)運動學(xué)矩陣，如式(21)所示。

(21)

式中,ωx,ωy,ωz為ω的三軸分量。根據(jù)著陸器的動力學(xué)方程可得導(dǎo)航系統(tǒng)狀態(tài)方程如式(22)所示。

(22)

式中,w為系統(tǒng)噪聲，在此假設(shè)為高斯白噪聲。

導(dǎo)航系統(tǒng)的觀測量為特征點在相機坐標(biāo)系下的三維位置矢量，觀測方程如式(23)所示。

(23)

(24)

式中，q0為姿態(tài)四元數(shù)的標(biāo)量部分，q1,q2,q3為姿態(tài)四元數(shù)的矢量部分。

(25)

式中,下標(biāo)表示時刻，Qk表示系統(tǒng)噪聲協(xié)方差陣，Rk表示測量噪聲協(xié)方差陣，為對稱正定矩陣，δkj表示Kronecker函數(shù)。tk時刻的狀態(tài)可通過狀態(tài)預(yù)測與觀測更新得到。具體過程如下：

一步預(yù)測：

(26)

(27)

(28)

觀測更新：

(29)

(30)

(31)

(32)

相對導(dǎo)航流程如圖4所示。需注意，本文的導(dǎo)航方法使用的觀測量并非相機與雷達(dá)的原始觀測量，而是根據(jù)原始觀測量計算得到的特征點在本體系下的三維位置矢量。濾波器中采用的觀測方程，即式(23)中用到的特征點位置矢量也是通過原始測量計算得到的，計算公式即為式(18)。

圖4 相對導(dǎo)航流程Fig.4 The operation sequence of relative navigation

2 可觀測度分析

在第1節(jié)構(gòu)建的導(dǎo)航系統(tǒng)中，相機視場中的特征點數(shù)量及位置是影響導(dǎo)航性能的關(guān)鍵因素。本節(jié)從導(dǎo)航可觀性的角度對導(dǎo)航系統(tǒng)進行理論分析，通過構(gòu)建可觀測度矩陣，解耦分析著陸器位置及姿態(tài)的可觀測性與特征點數(shù)量及位置的關(guān)系，解析獲得滿足著陸器全狀態(tài)可觀的基本條件。

2.1 位置可觀性

對于式(22)～式(23)構(gòu)成的非線性系統(tǒng)，可通過線性化求解雅克比矩陣，進而構(gòu)建可觀測度矩陣。在tk時刻，狀態(tài)方程的雅克比矩陣具體形式如式(33)所示。

(33)

(34)

觀測方程雅克比矩陣形式如式(35)所示。

(35)

式中,

(36)

通過線性化矩陣得到的系統(tǒng)可觀測度矩陣一般形式如式(37)所示。

(37)

式中,n為系統(tǒng)狀態(tài)維數(shù)。

將式(33)與式(35)代入式(37)可得導(dǎo)航系統(tǒng)可觀測度矩陣，取式(37)的前兩項得：

(38)

本小節(jié)只分析著陸器位置和速度的可觀性，根據(jù)可觀測度矩陣的性質(zhì)，前6列的線性關(guān)系對應(yīng)了著陸器位置和速度的可觀測度。在狀態(tài)方程中，姿態(tài)與位置的耦合體現(xiàn)在速度微分方程中，A3×4正是由于這種耦合關(guān)系產(chǎn)生的，而在構(gòu)建式(38)所示的可觀性矩陣時將A3×4消掉，因此可以解耦分析位置和速度的可觀性。在本小節(jié)中，假設(shè)姿態(tài)可觀，忽略可觀測度矩陣中與姿態(tài)相關(guān)的后四列，取式(38)前六列作為位置速度的可觀測度矩陣，如式(39)所示。

(39)

(40)

(41)

式(41)的所有特征值均為N，因此位置速度可觀測度始終為1，且與特征點數(shù)量無關(guān)。

2.2 姿態(tài)可觀性

(42)

(43)

式中：

(44)

定義觀測誤差如式(45)所示。

(45)

式中：Yi表示針對第i個特征點的觀測量。將觀測誤差方程線性化可得：

(46)

(47)

通過式(47)可分析觀測量中的姿態(tài)信息。記觀測信息矩陣為：

(48)

(xi-x)Q(1)+(yi-y)Q(2)+(zi-z)Q(3)=0

(49)

可知觀測量中不包含著陸器姿態(tài)在導(dǎo)航坐標(biāo)系下沿ri-r方向的旋轉(zhuǎn)信息。

當(dāng)觀測兩個特征點時，可得觀測信息矩陣為：

(50)

(51)

式(50)滿秩的充要條件為

(52)

式(52)要求：

(r2-r)≠k(r1-r)

(53)

式中,k為比例因子。通過上述分析可知：觀測兩個特征點，且兩個特征點相對著陸器的視線矢量不共線時，觀測量中包含了著陸器的三軸姿態(tài)信息。同理，當(dāng)著陸器觀測三個及以上不同的特征點時，觀測信息中也包含完整的三軸姿態(tài)信息。

通過上述分析可知某一觀測時刻觀測量中包含的姿態(tài)信息，對于觀測2個及以上不同特征點的情況，由于各個時刻的觀測量中都包含了三軸姿態(tài)信息，因此通過序列觀測可以準(zhǔn)確估計出姿態(tài)。當(dāng)觀測一個特征點時，通過式(49)可知，每個時刻的觀測量都缺少了著陸器姿態(tài)沿某個方向的旋轉(zhuǎn)信息。由于在動力下降過程中，著陸器的姿態(tài)及特征點相對著陸器的位置矢量不斷變化，因此需要分析導(dǎo)航系統(tǒng)通過序列觀測獲得著陸器姿態(tài)的能力。

為分析在序列觀測量及狀態(tài)方程作用下的導(dǎo)航系統(tǒng)可觀性，需構(gòu)建分段線性定常系統(tǒng)可觀測度矩陣。對于式(43)所示的微分方程，其離散化狀態(tài)方程如式(54)所示。

(54)

式中,Φk,k-1為tk-1時刻到tk時刻的狀態(tài)轉(zhuǎn)移矩陣。根據(jù)文獻(xiàn)[14]可知，狀態(tài)轉(zhuǎn)移矩陣具有如下形式：

(55)

式中,Δq為tk-1時刻到tk時刻的四元數(shù)變化。綜合t0到tk時刻的觀測量，得到姿態(tài)可觀測度矩陣如式(56)所示。

(56)

(57)

在動力下降過程中，若特征點相對著陸器的位置矢量發(fā)生變化，或著陸器存在姿態(tài)運動，著陸器相當(dāng)于觀測了不同的特征點，可觀測度矩陣Oa滿秩。因此從序列觀測的角度出發(fā)，著陸器只觀測一個特征點也可實現(xiàn)姿態(tài)可觀。具體的分析將在下一節(jié)中通過數(shù)學(xué)仿真分析。

在2.1節(jié)中分析位置速度可觀性時假設(shè)姿態(tài)可觀，并得到了一個特征點就可式位置速度可觀測結(jié)論，而姿態(tài)的可觀性分析證明了僅有一個特征點時姿態(tài)可觀，因此綜合2.1節(jié)與2.2節(jié)的分析可知，當(dāng)僅有一個特征點可用時，相對導(dǎo)航系統(tǒng)完全可觀。

3 仿真校驗

本節(jié)通過數(shù)學(xué)仿真校驗相對導(dǎo)航系統(tǒng)可觀性以及相對導(dǎo)航可行性，并結(jié)合姿態(tài)和軌跡制導(dǎo)分析著陸器通過相對導(dǎo)航實現(xiàn)定點著陸的能力。

3.1 仿真參數(shù)設(shè)置

著陸器在相對導(dǎo)航坐標(biāo)系下的初始標(biāo)稱狀態(tài)及狀態(tài)標(biāo)準(zhǔn)差如表1所示，姿態(tài)四元數(shù)可根據(jù)初始姿態(tài)歐拉角計算得到[15]。導(dǎo)航敏感器參數(shù)如表2所示。

表1 初始標(biāo)稱狀態(tài)及狀態(tài)標(biāo)準(zhǔn)差Table 1 Nominal values and standard deviations of initial states

表2 導(dǎo)航敏感器參數(shù)Table 2 Parameters of navigation sensors

(58)

(59)

則期望的姿態(tài)角速度為：

(60)

3.2 仿真結(jié)果

假設(shè)導(dǎo)航制導(dǎo)控制周期為1 s，動力下降過程持續(xù)30 s，著陸器從初始位置到達(dá)目標(biāo)點上方100 m。在式(61)所示的著陸區(qū)域內(nèi)隨機生成500個特征點。

D={(x,y)|x,y∈[-1500 m, 1500 m]}

(61)

利用動力下降段第一幅圖像中包括著陸點在內(nèi)的三個不共線特征點構(gòu)建相對導(dǎo)航坐標(biāo)系。首先分析著陸器僅利用本體系下著陸點位置矢量導(dǎo)航時的姿態(tài)可觀測度。式(56)中矩陣Oa的條件數(shù)倒數(shù)的變化曲線如圖5所示。從圖中可以看出，僅利用初始時刻的觀測量，可觀測度矩陣不滿秩，可觀測度為0，說明僅利用一個時刻的觀測量不能使系統(tǒng)可觀。隨著序列觀測的累積，可觀測度矩陣滿秩，系統(tǒng)可觀測度趨于穩(wěn)定。

圖5 姿態(tài)可觀性及可觀測度仿真結(jié)果Fig.5 The simulation results of lander’s observability and observability degree

根據(jù)相機參數(shù)及著陸器初始狀態(tài)參數(shù)模擬觀測到的特征點數(shù)量如圖6所示。特征點在導(dǎo)航坐標(biāo)系下的位置誤差如圖7所示。隨著探測器高度降低，可用特征點數(shù)量逐漸減少。由于探測器高度降低時，同一個像素點對應(yīng)的距離減小，因此在圖像處理精度相同的前提下，特征點測量精度提高，且雷達(dá)測距精度也提高，因此通過測量計算得到的特征點位置誤差逐漸減小。

圖6 可觀測到的特征點數(shù)量Fig.6 The number of available feature points

圖7 特征點在導(dǎo)航坐標(biāo)系下的位置誤差Fig.7 Position errors of a feature point in relative navigation reference frame

通過蒙特卡洛仿真分析相對導(dǎo)航精度，并結(jié)合制導(dǎo)方法分析最終落點精度。500次蒙特卡洛仿真的著陸器狀態(tài)誤差及3σ誤差界如圖8所示。圖中子窗口給出了動力下降段最后10 s的誤差結(jié)果。從仿真結(jié)果可以看出，著陸器最終的3σ位置估計精度優(yōu)于5 m，速度估計精度優(yōu)于0.5 m/s，姿態(tài)估計精度優(yōu)于0.5°。具體精度數(shù)據(jù)見表3。

圖8 蒙特卡洛仿真位置速度誤差及3σ誤差界Fig.8 Position, velocity and attitude errors and 3σ error bounds in Monte Carlo simulations

位置分量3σ精度速度分量3σ精度姿態(tài)分量3σ精度x/m2.6vx/(m·s-1)0.28φ/(°)0.23y/m3.2vy/(m·s-1)0.26θ/(°)0.29z/m2.9vz/(m·s-1)0.34ψ/(°)0.29

著陸器3σ著陸誤差橢圓如圖9所示?？梢钥闯?，在施加控制的作用下，著陸器可以從不同的初始狀態(tài)到達(dá)目標(biāo)著落點附近，實現(xiàn)定點軟著陸，且著陸精度與導(dǎo)航精度近似。

4 結(jié) 論

針對行星動力下降段視覺絕對導(dǎo)航自然路標(biāo)匱乏的問題，本文提出了利用光學(xué)導(dǎo)航相機與三波束雷達(dá)的相對導(dǎo)航方法。從導(dǎo)航相機觀測中選取三個不共線特征點構(gòu)建相對導(dǎo)航坐標(biāo)系，結(jié)合雷達(dá)測距信息可獲得特征點在相機坐標(biāo)系及相對導(dǎo)航系下的三維位置矢量，以特征點在相機系下的位置矢量為觀測量，結(jié)合著陸器姿軌動力學(xué)方程，利用擴展卡爾曼濾波，實現(xiàn)了著陸器相對目標(biāo)點的位姿估計。

圖9 3σ著陸誤差橢圓Fig.9 The 3σ landing error ellipse

通過可觀測度分析可知，僅利用一個特征點的觀測量即可使導(dǎo)航系統(tǒng)可觀。數(shù)學(xué)仿真表明，著陸器的三軸位置估計精度可優(yōu)于5 m，速度估計精度可優(yōu)于0.5 m/s，姿態(tài)角估計精度可優(yōu)于0.3°。在相對導(dǎo)航的基礎(chǔ)上，結(jié)合軌跡制導(dǎo)與控制，可實現(xiàn)火星定點軟著陸。