行星著陸動力下降段相對視覺導航方法

秦 同，朱圣英，崔平遠，欒恩杰,2

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

0 引 言

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

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

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

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

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

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

1 相對導航方案

本節首先給出光學導航相機與三波束雷達的觀測模型，然后利用兩者的觀測量構建相對導航坐標系，再結合動力下降段著陸器姿軌耦合動力學方程設計相對導航系統。

1.1 敏感器觀測模型

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

圖1 光學導航原理示意圖Fig.1 The geometric schematic of optical navigation

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

(1)

(2)

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

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

(3)

(4)

(5)

(6)

(7)

1.2 相對導航坐標系

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

圖3 相對導航坐標系Fig.3 The relative navigation reference frame

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

(8)

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

(9)

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

(10)

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

(11)

由此可構建相對導航坐標系的三個特征點之間的位置矢量為：

(12)

(13)

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

(14)

(15)

ey=ez×ex

(16)

則導航坐標系到本體坐標系的旋轉矩陣為：

(17)

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

(18)

至此便得到相對導航坐標系下的導航參考基準位置。

1.3 相對導航系統

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

(19)

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

(20)

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

(21)

式中,ωx,ωy,ωz為ω的三軸分量。根據著陸器的動力學方程可得導航系統狀態方程如式(22)所示。

(22)

式中,w為系統噪聲，在此假設為高斯白噪聲。

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

(23)

(24)

式中，q0為姿態四元數的標量部分，q1,q2,q3為姿態四元數的矢量部分。

(25)

式中,下標表示時刻，Qk表示系統噪聲協方差陣，Rk表示測量噪聲協方差陣，為對稱正定矩陣，δkj表示Kronecker函數。tk時刻的狀態可通過狀態預測與觀測更新得到。具體過程如下：

一步預測：

(26)

(27)

(28)

觀測更新：

(29)

(30)

(31)

(32)

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

圖4 相對導航流程Fig.4 The operation sequence of relative navigation

2 可觀測度分析

在第1節構建的導航系統中，相機視場中的特征點數量及位置是影響導航性能的關鍵因素。本節從導航可觀性的角度對導航系統進行理論分析，通過構建可觀測度矩陣，解耦分析著陸器位置及姿態的可觀測性與特征點數量及位置的關系，解析獲得滿足著陸器全狀態可觀的基本條件。

2.1 位置可觀性

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

(33)

(34)

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

(35)

式中,

(36)

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

(37)

式中,n為系統狀態維數。

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

(38)

本小節只分析著陸器位置和速度的可觀性，根據可觀測度矩陣的性質，前6列的線性關系對應了著陸器位置和速度的可觀測度。在狀態方程中，姿態與位置的耦合體現在速度微分方程中，A3×4正是由于這種耦合關系產生的，而在構建式(38)所示的可觀性矩陣時將A3×4消掉，因此可以解耦分析位置和速度的可觀性。在本小節中，假設姿態可觀，忽略可觀測度矩陣中與姿態相關的后四列，取式(38)前六列作為位置速度的可觀測度矩陣，如式(39)所示。

(39)

(40)

(41)

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

2.2 姿態可觀性

(42)

(43)

式中：

(44)

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

(45)

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

(46)

(47)

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

(48)

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

(49)

可知觀測量中不包含著陸器姿態在導航坐標系下沿ri-r方向的旋轉信息。

當觀測兩個特征點時，可得觀測信息矩陣為：

(50)

(51)

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

(52)

式(52)要求：

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

(53)

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

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

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

(54)

式中,Φk,k-1為tk-1時刻到tk時刻的狀態轉移矩陣。根據文獻[14]可知，狀態轉移矩陣具有如下形式：

(55)

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

(56)

(57)

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

在2.1節中分析位置速度可觀性時假設姿態可觀，并得到了一個特征點就可式位置速度可觀測結論，而姿態的可觀性分析證明了僅有一個特征點時姿態可觀，因此綜合2.1節與2.2節的分析可知，當僅有一個特征點可用時，相對導航系統完全可觀。

3 仿真校驗

本節通過數學仿真校驗相對導航系統可觀性以及相對導航可行性，并結合姿態和軌跡制導分析著陸器通過相對導航實現定點著陸的能力。

3.1 仿真參數設置

著陸器在相對導航坐標系下的初始標稱狀態及狀態標準差如表1所示，姿態四元數可根據初始姿態歐拉角計算得到[15]。導航敏感器參數如表2所示。

表1 初始標稱狀態及狀態標準差Table 1 Nominal values and standard deviations of initial states

表2 導航敏感器參數Table 2 Parameters of navigation sensors

(58)

(59)

則期望的姿態角速度為：

(60)

3.2 仿真結果

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

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

(61)

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

圖5 姿態可觀性及可觀測度仿真結果Fig.5 The simulation results of lander’s observability and observability degree

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

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

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

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

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

位置分量3σ精度速度分量3σ精度姿態分量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所示?？梢钥闯?，在施加控制的作用下，著陸器可以從不同的初始狀態到達目標著落點附近，實現定點軟著陸，且著陸精度與導航精度近似。

4 結 論

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

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

通過可觀測度分析可知，僅利用一個特征點的觀測量即可使導航系統可觀。數學仿真表明，著陸器的三軸位置估計精度可優于5 m，速度估計精度可優于0.5 m/s，姿態角估計精度可優于0.3°。在相對導航的基礎上，結合軌跡制導與控制，可實現火星定點軟著陸。