一種月球車慣性/視覺組合導航方法研究

谷程鵬 邵濟明 李廣興 施 梨

上海宇航系統工程研究所，上海201100

月球作為地球的天然衛星，有著豐富的資源[1]，是初期深空探測的理想目標。在探月任務中，月球車至關重要。月球車要在完全陌生的、非結構化的及復雜的月面環境中實現路徑規劃和給定的探測任務，需要精確獲取自身的位置和姿態信息。現有月球車的自主導航主要采用慣性和視覺導航方式。慣性導航自主性強，姿態精度高，但其誤差隨時間累積，難以獨立完成長時間的導航任務。視覺導航[2]自主性強，定位精度高，但其誤差隨距離累積。此外，天文導航[3]誤差較高，且易受可觀測性影響,在月面上定位性能較低。慣性和視覺導航均為相對導航方式，提高長時間、長距離下的定位精度，是月球車導航的關鍵技術之一。

目前在慣性/視覺組合導航方面，已有了較多的研究。慣性/視覺組合導航一般以慣性導航為主，以位置、速度和姿態為狀態量建立PVA方程[4],或者以誤差項為狀態建立誤差方程[5]；基于特征點圖像坐標[6]、視覺導航的位姿輸出[7]、視覺導航前后站點相對運動參數[8]、慣導和視覺導航測量之差[9-10]建立量測方程。文獻[9]利用視覺位置輸出估算載體的運動速度，并將其與慣性導航速度之差作為量測信息，從而完成組合導航方案設計，但是它需要構造視覺里程計的速度誤差模型并將狀態進行增廣。文獻[10]基于上一時刻組合導航的位置和姿態估計值構造觀測量，從而避免了對狀態的增廣，簡化了測量模型。但這是在假設組合導航估計值和真實值接近的條件下得到的。實際上，位置估計偏差可能就會引起濾波的發散。文獻[11]用單目攝像頭測算航向，對慣性導航解算結果進行修正，但這一算法只融合航向信息，且組合結構比較簡單。文獻[12]提出雙目視覺加慣性元件的航天器相對導航方法，但該視覺測量是基于航天器上設置的已知特征點的，而在月球探測初期，月面上難以布置特征站點，無法依靠視覺來絕對定位。本文以誤差項為狀態量，利用前后2個時刻的慣性導航輸出計算前后時刻相對運動參數，并將其與視覺導航輸出之差作為濾波量測，既能融合所有的導航信息，又能避免對狀態的增廣。同時視覺導航的輸出為相對量，故不需要絕對的特征點信息。針對測量噪聲未知的問題，采用自適應卡爾曼濾波[13]，對噪聲均值和方差進行在線估計。仿真結果表明，該慣性/視覺組合導航精度相比單一的導航方式有明顯的提高，方法有效。

1 組合導航方案設計

1.1 組合導航系統狀態方程

以慣性導航系統的導航輸出參數誤差和慣性元件誤差作為狀態向量，包括3個平臺誤差角、3個速度誤差、3個位置誤差、3個陀螺常值漂移和3個陀螺隨機漂移和3個加速度計漂移共18維。推導出慣導系統誤差方程[14]作為組合導航狀態方程。

(1)

1.2 組合導航系統量測方程

1.2.1 慣導觀測信息

根據前后時刻的慣導輸出，計算k到k+1時刻的旋轉和平移參數

(2)

(3)

式中：

1.2.2 視覺觀測信息

(4)

由于月球車的位姿信息是遞推獲得，前一時刻的誤差會不斷累積，造成定位誤差逐漸變大，難以用模型描述。然而，前后時刻的相對運動參數的解算誤差只與算法誤差、特征匹配誤差及成像誤差有關，其誤差分布特性可以認為是不變的。基于該前后時刻的相對運動參數，設計慣性/視覺組合導航方案。因此，視覺觀測信息可表示為

(5)

(6)

1.2.3 組合導航量測模型

利用慣導觀測信息式(2)～(3)和視覺觀測信息式(5)～(6)之差構造量測方程

(7)

(8)

顯然，該差值與狀態成線性關系，并疊加未知噪聲。取旋轉矩陣上三角三參數與平移矢量為觀測量，得到最終的量測方程

(9)

式中：Hr由式(7)計算得到；Fd和Gd為離散狀態矩陣和過程噪聲矩陣。

1.3 自適應卡爾曼濾波方程

由于系統狀態方程和量測方程均為線性，但量測噪聲模型未知，濾波同時需要對量測噪聲的均值和方差進行在線估計，故采用自適應卡爾曼濾波。噪聲估計方程為

(10)

(11)

(12)

1.4 協方差傳遞

為了更好地說明各導航方式的性能，推導出相應的協方差傳遞方程為

(13)

(14)

(15)

(16)

式中：HV和Hk,k+1為式(14)求得的對應雅克比矩陣，且由于噪聲未知，這里用組合導航估計的噪聲矩陣來求視覺導航的過程噪聲矩陣。

某一時刻的協方差代表了導航誤差下限，以此來證明組合導航的優勢。

2 仿真

2.1 仿真條件

仿真中月球車的軌跡選用美國勘探者3號探測器的著陸地點(月球2°56′N,336°40′E)作為起點，月球車以約0.2m/s的速度勻速運行。仿真進行100次，取均方誤差作為各導航方式的誤差結果。

基本參數：月球半徑1738km，重力加速度g=1.633m/s2，自轉周期27.32166天。慣性元件參數：陀螺常值漂移0.2(°)/h，隨機漂移0.1(°)/h，相關時間Tg=3600s；加速度計隨機漂移10-4gm，相關時間Ta=1800s；采樣頻率均為100Hz。立體視覺參數：焦距f=6mm，分辨率768×576，像素大小dx=dy=1/120mm，成像誤差Δε=0.6pixel，匹配誤差Δδ=0.6pixel，采樣頻率為0.25Hz。雙目相機平行安裝，基線距離0.2m，相機俯視角為20°，左相機坐標系相對本體坐標系的轉換參數為

2.2 仿真結果

圖1～2為視覺導航在不同運動下的前后時刻相對運動參數解算誤差，橫坐標為250次隨機運動，解算過程中的圖像特征點位置和數量同樣隨機。這里列出了經度誤差和橫滾角誤差。可以看出，前后時刻相對運動參數的解算誤差在一定區域之內，故用式(5)～(6)來描述視覺觀測信息是合理的。

圖1 視覺導航單步解算經度誤差

圖2 視覺導航單步解算橫滾角誤差

圖3～8分別列出了慣性導航、視覺導航和組合導航的位置及姿態誤差(不失一般性，圖中列出前1000s時間結果)。從經緯度上看，慣性導航誤差高于視覺導航，且均呈現高次增長現象。本文提出的組合導航方法能夠明顯地提高定位精度，同時慣性導航和視覺導航均為相對導航方式，組合導航的誤差同樣不斷增長，但其增長趨勢較單一，導航方式緩慢，減小了誤差的發散程度。在高度通道上，由于慣性導航的誤差已較小，組合導航的精度提升效果并不明顯。從姿態上看，視覺導航誤差明顯高于慣性導航，而組合導航則能顯著提升橫滾角和俯仰角的精度，但航向角的精度并沒有得到改善。此外，由于慣性導航的姿態精度較高，漂移較小，組合導航的姿態誤差也較為平穩，能夠控制在一定的范圍之內。

圖3 經度誤差

圖4 緯度誤差

圖5 高度誤差

圖6 橫滾角誤差

圖7 俯仰角誤差

圖8 航向角誤差

表1統計了各導航方法在200m(1000s)處的位置、姿態誤差及協方差。由表1看出，視覺導航的位置精度相對慣性導航高，慣性導航的姿態精度相對視覺導航高。組合導航的位置和姿態精度均顯著優于單一導航方式。在200m處，由于慣性和視覺導航的誤差發散嚴重，組合導航精度提升效果較為明顯。本文提出的組合導航方法的位置誤差為1.092m，相比慣性導航減小了99.7%，相比視覺導航減小了95.1%。組合導航的橫滾角和俯仰角精度提升2個數量級。從協方差傳遞情況上看，組合導航相比單一導航方式確有很大提高，證明了該組合導航方式的有效性。

表1 3種導航方式在200m處位置姿態誤差和協方差

3 結論

提出一種基于前后時刻慣導觀測量的月球車慣性/視覺組合導航方法，將前后時刻相對運動的慣導觀測量轉換為只與當前時刻慣性導航誤差相關，從而避免了對狀態的增廣。利用視覺導航相對運動參數誤差特性不變的特點，將相對運動的慣導和視覺觀測量之差作為組合導航量測值，采用自適應卡爾曼濾波解決量測噪聲未知的問題。仿真證明，該方法可以有效提高位置和姿態的精度，實現慣性導航和視覺導航的互補。

[1] 歐陽自遠.我國月球探測的總體科學目標與發展戰略[J].地球科學進展,2004,19(3):351-357.(Ouyang Ziyuan. Scientific Objectives of Chinese Lunar Exploration Project and Development Dtrategy[J]. Advance in Earth Science, 2004,19(3):351-357.)

[2] 劉傳凱,王保豐,王鎵,等.嫦娥三號巡視器的慣導與視覺組合定姿定位[J].飛行器測控學報,2014,33(3): 250-257.(Liu Chuankai, Wang Baofeng, Wang Jia, et al. Integrated INS and Vision-based Orientation Determination and Positioning of CE-3 Lunar Rover[J]. Journal of Spacecraft TT&C Technology, 2014,33(3): 250-257.)

[3] 施麗娟,熊智,王融,等.基于H∞濾波的空天飛行器慣性/天文組合導航方法[J].航天控制，2016,34(5):46-51.(Shi Lijuan, Xiong Zhi, Wang Rong, et al. An INS/CNS Integrated Navigation Method of Aerospace Vehicle Based on theH∞ Filter. Aerospace Control, 2016,34(5):46-51.)

[4] Wang T M, Wang C L, Liang J H, et al. Vision-aided Inertial Navigation for Small Unmanned Aerial Vehicles in GPS-denied Environments[J].International Journal of Advanced Robotic Systems,2013,10(5):1-12.

[5] Kim Y, Hwang D H. Vision/INS Integrated Navigation System for Poor Vision Navigation Environments[J]. Sensors, 2016, 16(10):1672.

[6] Kim Y, Hwang D H. Design of Vision/INS Integrated Navigation System in Poor Vision Navigation Environments[J]. International Conference on Control, Automation and Systems, Gwangju: ICCAS,2013:531-535.

[7] Corato F D, Innocenti M, Pollini L. An Entropy-like Approach to Vision-aided Inertial Navigation[J]. Ifac World Congress, 2011,44(1):13789-13794.

[8] Tardif J P, George M, Laverne M, et al. A New Approach to Vision-aided Inertial Navigation[C]. IEEE/RSJ International Conference on Intelligent Robot & Systems, Taipei: IEEE,2010,25(1):4161-4168.

[9] 逯建軍,任曉軍,孫偉,等.慣性/雙目視覺里程計深組合導航方法[J].導航定位與授時,2016,3(3): 37-43.(Lu Jianjun, Ren Xiaojun, Sun Wei, et al. INS/Stereo Vision Odometry Deeply Integrated Navigation Method[J]. Navigation Positioning & Timing,2016,3(3): 37-43.)

[10] 徐勇志,寧曉琳.一種巡視器慣性/視覺組合導航新方法[J].空間科學學報,2015,35(6):721-729.(Xu Yongzhi, Ning Xiaolin. A New INS/VNS Integrated Navigation Method for Planetary Exploration Rover[J]. Chin. J. Space Sci, 2015,35(6):721-729.)

[11] 婁路.基于視覺和MEMS-IMU融合的火星車導航定向技術[J].航天控制,2012,30(4):32-36.(Lou Lu. An Approach of Attitude Estimation by Using Vision-aided IMU for Mars Rover[J]. Aerospace Control, 2012,30(4):32-36.)

[12] 趙逸倫,喬兵,靳永強,等.一種采用雙目視覺加慣性測量的航天器組合相對導航方法[J].航天控制,2016,34(4): 47-52.(Zhao Yilun, Qiao Bing, Jin Yongqiang, et al. An Integrated Relative Navigation Algorithm for Spacecraft Based on Binocular Vision and Inertial Measurement[J]. Aerospace Control, 2016,34(4):47-52.)

[13] 周啟帆,張海,王嫣然.一種基于冗余測量的自適應卡爾曼濾波算法[J].航空學報,2015,36(5):1596-1605.(Zhou Qifan, Zhang Hai, Wang Yanran. A Redundant Measurement Adaptive Kalman Filter Algorithm[J]. Acta Aeronautica et Astronautica Sinica,2015,36(5):1596-1605.)

[14] 劉建業,曾慶化,趙偉,等.導航系統理論與應用[M].西安:西北工業大學出版社,2010.(Liu Jianye, Zeng Qinghua, Zhao Wei, et al. Navigation System Theory and Application[M]. Xi′an: Northwestern Polytechnical University Press, 2010.)