谷程鵬 邵濟(jì)明 李廣興 施 梨

上海宇航系統(tǒng)工程研究所，上海201100

月球作為地球的天然衛(wèi)星，有著豐富的資源[1]，是初期深空探測(cè)的理想目標(biāo)。在探月任務(wù)中，月球車至關(guān)重要。月球車要在完全陌生的、非結(jié)構(gòu)化的及復(fù)雜的月面環(huán)境中實(shí)現(xiàn)路徑規(guī)劃和給定的探測(cè)任務(wù)，需要精確獲取自身的位置和姿態(tài)信息。現(xiàn)有月球車的自主導(dǎo)航主要采用慣性和視覺(jué)導(dǎo)航方式。慣性導(dǎo)航自主性強(qiáng)，姿態(tài)精度高，但其誤差隨時(shí)間累積，難以獨(dú)立完成長(zhǎng)時(shí)間的導(dǎo)航任務(wù)。視覺(jué)導(dǎo)航[2]自主性強(qiáng)，定位精度高，但其誤差隨距離累積。此外，天文導(dǎo)航[3]誤差較高，且易受可觀測(cè)性影響,在月面上定位性能較低。慣性和視覺(jué)導(dǎo)航均為相對(duì)導(dǎo)航方式，提高長(zhǎng)時(shí)間、長(zhǎng)距離下的定位精度，是月球車導(dǎo)航的關(guān)鍵技術(shù)之一。

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

1 組合導(dǎo)航方案設(shè)計(jì)

1.1 組合導(dǎo)航系統(tǒng)狀態(tài)方程

以慣性導(dǎo)航系統(tǒng)的導(dǎo)航輸出參數(shù)誤差和慣性元件誤差作為狀態(tài)向量，包括3個(gè)平臺(tái)誤差角、3個(gè)速度誤差、3個(gè)位置誤差、3個(gè)陀螺常值漂移和3個(gè)陀螺隨機(jī)漂移和3個(gè)加速度計(jì)漂移共18維。推導(dǎo)出慣導(dǎo)系統(tǒng)誤差方程[14]作為組合導(dǎo)航狀態(tài)方程。

(1)

1.2 組合導(dǎo)航系統(tǒng)量測(cè)方程

1.2.1 慣導(dǎo)觀測(cè)信息

根據(jù)前后時(shí)刻的慣導(dǎo)輸出，計(jì)算k到k+1時(shí)刻的旋轉(zhuǎn)和平移參數(shù)

(2)

(3)

式中：

1.2.2 視覺(jué)觀測(cè)信息

(4)

由于月球車的位姿信息是遞推獲得，前一時(shí)刻的誤差會(huì)不斷累積，造成定位誤差逐漸變大，難以用模型描述。然而，前后時(shí)刻的相對(duì)運(yùn)動(dòng)參數(shù)的解算誤差只與算法誤差、特征匹配誤差及成像誤差有關(guān)，其誤差分布特性可以認(rèn)為是不變的?；谠撉昂髸r(shí)刻的相對(duì)運(yùn)動(dòng)參數(shù)，設(shè)計(jì)慣性/視覺(jué)組合導(dǎo)航方案。因此，視覺(jué)觀測(cè)信息可表示為

(5)

(6)

1.2.3 組合導(dǎo)航量測(cè)模型

利用慣導(dǎo)觀測(cè)信息式(2)～(3)和視覺(jué)觀測(cè)信息式(5)～(6)之差構(gòu)造量測(cè)方程

(7)

(8)

顯然，該差值與狀態(tài)成線性關(guān)系，并疊加未知噪聲。取旋轉(zhuǎn)矩陣上三角三參數(shù)與平移矢量為觀測(cè)量，得到最終的量測(cè)方程

(9)

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

1.3 自適應(yīng)卡爾曼濾波方程

由于系統(tǒng)狀態(tài)方程和量測(cè)方程均為線性，但量測(cè)噪聲模型未知，濾波同時(shí)需要對(duì)量測(cè)噪聲的均值和方差進(jìn)行在線估計(jì)，故采用自適應(yīng)卡爾曼濾波。噪聲估計(jì)方程為

(10)

(11)

(12)

1.4 協(xié)方差傳遞

為了更好地說(shuō)明各導(dǎo)航方式的性能，推導(dǎo)出相應(yīng)的協(xié)方差傳遞方程為

(13)

(14)

(15)

(16)

式中：HV和Hk,k+1為式(14)求得的對(duì)應(yīng)雅克比矩陣，且由于噪聲未知，這里用組合導(dǎo)航估計(jì)的噪聲矩陣來(lái)求視覺(jué)導(dǎo)航的過(guò)程噪聲矩陣。

某一時(shí)刻的協(xié)方差代表了導(dǎo)航誤差下限，以此來(lái)證明組合導(dǎo)航的優(yōu)勢(shì)。

2 仿真

2.1 仿真條件

仿真中月球車的軌跡選用美國(guó)勘探者3號(hào)探測(cè)器的著陸地點(diǎn)(月球2°56′N,336°40′E)作為起點(diǎn)，月球車以約0.2m/s的速度勻速運(yùn)行。仿真進(jìn)行100次，取均方誤差作為各導(dǎo)航方式的誤差結(jié)果。

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

2.2 仿真結(jié)果

圖1～2為視覺(jué)導(dǎo)航在不同運(yùn)動(dòng)下的前后時(shí)刻相對(duì)運(yùn)動(dòng)參數(shù)解算誤差，橫坐標(biāo)為250次隨機(jī)運(yùn)動(dòng)，解算過(guò)程中的圖像特征點(diǎn)位置和數(shù)量同樣隨機(jī)。這里列出了經(jīng)度誤差和橫滾角誤差?？梢钥闯?，前后時(shí)刻相對(duì)運(yùn)動(dòng)參數(shù)的解算誤差在一定區(qū)域之內(nèi)，故用式(5)～(6)來(lái)描述視覺(jué)觀測(cè)信息是合理的。

圖1 視覺(jué)導(dǎo)航單步解算經(jīng)度誤差

圖2 視覺(jué)導(dǎo)航單步解算橫滾角誤差

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

圖3 經(jīng)度誤差

圖4 緯度誤差

圖5 高度誤差

圖6 橫滾角誤差

圖7 俯仰角誤差

圖8 航向角誤差

表1統(tǒng)計(jì)了各導(dǎo)航方法在200m(1000s)處的位置、姿態(tài)誤差及協(xié)方差。由表1看出，視覺(jué)導(dǎo)航的位置精度相對(duì)慣性導(dǎo)航高，慣性導(dǎo)航的姿態(tài)精度相對(duì)視覺(jué)導(dǎo)航高。組合導(dǎo)航的位置和姿態(tài)精度均顯著優(yōu)于單一導(dǎo)航方式。在200m處，由于慣性和視覺(jué)導(dǎo)航的誤差發(fā)散嚴(yán)重，組合導(dǎo)航精度提升效果較為明顯。本文提出的組合導(dǎo)航方法的位置誤差為1.092m，相比慣性導(dǎo)航減小了99.7%，相比視覺(jué)導(dǎo)航減小了95.1%。組合導(dǎo)航的橫滾角和俯仰角精度提升2個(gè)數(shù)量級(jí)。從協(xié)方差傳遞情況上看，組合導(dǎo)航相比單一導(dǎo)航方式確有很大提高，證明了該組合導(dǎo)航方式的有效性。

表1 3種導(dǎo)航方式在200m處位置姿態(tài)誤差和協(xié)方差

3 結(jié)論

提出一種基于前后時(shí)刻慣導(dǎo)觀測(cè)量的月球車慣性/視覺(jué)組合導(dǎo)航方法，將前后時(shí)刻相對(duì)運(yùn)動(dòng)的慣導(dǎo)觀測(cè)量轉(zhuǎn)換為只與當(dāng)前時(shí)刻慣性導(dǎo)航誤差相關(guān)，從而避免了對(duì)狀態(tài)的增廣。利用視覺(jué)導(dǎo)航相對(duì)運(yùn)動(dòng)參數(shù)誤差特性不變的特點(diǎn)，將相對(duì)運(yùn)動(dòng)的慣導(dǎo)和視覺(jué)觀測(cè)量之差作為組合導(dǎo)航量測(cè)值，采用自適應(yīng)卡爾曼濾波解決量測(cè)噪聲未知的問(wèn)題。仿真證明，該方法可以有效提高位置和姿態(tài)的精度，實(shí)現(xiàn)慣性導(dǎo)航和視覺(jué)導(dǎo)航的互補(bǔ)。

[1] 歐陽(yáng)自遠(yuǎn).我國(guó)月球探測(cè)的總體科學(xué)目標(biāo)與發(fā)展戰(zhàn)略[J].地球科學(xué)進(jìn)展,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] 劉傳凱,王保豐,王鎵,等.嫦娥三號(hào)巡視器的慣導(dǎo)與視覺(jué)組合定姿定位[J].飛行器測(cè)控學(xué)報(bào),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∞濾波的空天飛行器慣性/天文組合導(dǎo)航方法[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] 逯建軍,任曉軍,孫偉,等.慣性/雙目視覺(jué)里程計(jì)深組合導(dǎo)航方法[J].導(dǎo)航定位與授時(shí),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] 徐勇志,寧曉琳.一種巡視器慣性/視覺(jué)組合導(dǎo)航新方法[J].空間科學(xué)學(xué)報(bào),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] 婁路.基于視覺(jué)和MEMS-IMU融合的火星車導(dǎo)航定向技術(shù)[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] 趙逸倫,喬兵,靳永強(qiáng),等.一種采用雙目視覺(jué)加慣性測(cè)量的航天器組合相對(duì)導(dǎo)航方法[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] 周啟帆,張海,王嫣然.一種基于冗余測(cè)量的自適應(yīng)卡爾曼濾波算法[J].航空學(xué)報(bào),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] 劉建業(yè),曾慶化,趙偉,等.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安:西北工業(yè)大學(xué)出版社,2010.(Liu Jianye, Zeng Qinghua, Zhao Wei, et al. Navigation System Theory and Application[M]. Xi′an: Northwestern Polytechnical University Press, 2010.)