基于視覺和MEMS-IMU融合的火星車導航定向技術*

婁 路

重慶交通大學信息科學與工程學院，重慶400074

星際探測車是一種具有高度自主能力、適合于在復雜的非結構化行星地表環境中(比如火星或月球)工作的探測移動機器人。為確保探測車在這樣的復雜環境中執行探測任務，要求它具備高度的自主導航定向性能。近幾年隨著深空探測技術的發展，星際探測車的導航定向方法越來越多，根據測試手段、應用環境和誤差累計特性的差別，一般可以將這些方法歸納為相對定向技術和絕對定向技術兩類。相對定向技術主要包括以里程計、慣性導航系統為主體進行開發的導航技術［1］，該技術具有設備簡單、實時性和完全自主不依賴外界環境的優點，能滿足可靠的短距離導航精度。但是由于里程計、慣性導航系統具有一定的測量誤差，隨著測量誤差的積累，導航精度隨時間而降低，所以不適合于長時間、長距離導航定向。絕對定向方法主要有視覺導航、無線電導航以及利用太陽敏感器、星敏感器等進行定向［2］，其定向誤差不隨時間而增大，適合長距離的定向，但是自主性和實時性較差，容易受到周圍環境的影響。

文獻［3］研究了利用太陽、地球的天文導航方法與航位推算法相結合獲得位置的月球車組合導航方法。針對月球車導航的特殊要求，文獻［4］提出了天文導航與航位推算有機組合的月球車自主導航系統。文獻［5］提出一種利用對太陽高度的連續觀測，通過最小二乘法求解天文位置圓非線性方程組，獲得漫游車固定點位置的天文導航方法。美國國家航空航天局的“勇氣號”和“機遇號”火星車綜合利用了里程計、慣性導航系統、視覺里程計和太陽敏感器這些相對定向和絕對定向技術進行導航定位［6］。

“勇氣號”和“機遇號”火星車所用的慣性導航設備Litton LN-200，是一個小型化、高可靠性的光纖導航單元(IMU)，其在一個緊湊的盒體內包含3個光纖陀螺和3個微機電系統(MEMS)加速度計。IMU 姿態航向軟件以8Hz 的頻率計算和更新其姿態和位置，其中姿態更新由三軸加速度計和三軸陀螺儀測量獲得，位置則通過IMU 結合里程計數器采用航跡推算法獲得［7］。“勇氣號”也采用視覺測程法(Visual Odometry)測算方向和位置，其基于連續拍攝的導航相機立體像對在二維影像平面和三維地面空間追蹤特征點，并估計相對的位置和姿態實現車上實時定位，改正由航跡推算方法在車輪打滑時帶來的較大定位誤差［8］。由于火星車上計算機速度的限制，視覺測程法獲取和處理一個立體像對，然后更新位置和姿態的整個過程需要近3min 的時間，速度太慢，因而無法用于“勇氣號”和“機遇號”的全行程，只用于部分短距離關鍵路徑上的局部定位，比如預計車輪打滑時、或接近指定的科學目標時［9］。除此之外，視覺測程法也有失敗的時候，這主要發生在地形沒有足夠的特征點、特征點數目太少、火星車自身陰影影響等情況下。視覺測程法的缺點是計算速度慢、成功與否依賴于地形特征，因此只能應用于局部定位［10］。

近年來微電子機械慣性測試單元MEMS-IMU日益成熟，與傳統高端慣性導航系統相比，MEMSIMU 可靠性差、精度低，但卻具備體積小、成本低、功耗低的優勢。比如“勇氣號”火星車使用的光纖陀螺儀LN-200，重量接近1kg，功耗高達12W［11－12］，明顯對探測車的行走控制和電源管理都不利。而歐洲空間局太空科學研究和技術中心正在研制的基于MEMS-IMU 芯片的行星探測車導航系統，總重量不超過200g，功耗小于1W，各項仿真測試參數的精度接近或超過設計期望指標［12］。

因此，針對MEMS-IMU 的發展趨勢和視覺測程法采用標定立體攝像機、算法復雜且依賴于地形特征點等問題，本文提出一種基于MEMS-IMU 和單目攝像頭的傳感器融合方法，該方法采用加速度計的重力場分量去補償修正陀螺儀的漂移偏差，獲得較高的俯仰角和橫滾角精度。采用單目攝像機，無需標定，設計了一個簡單高效基于表象的視覺定向算法獲取火星車行駛的航向角，該視覺定向方法能適用于各種地表環境。

1 基于傳感器數據補償融合的姿態估計

本文采用一個軟件開源的MEMS-IMU 開發板作為姿態導航系統，該開發板是由一個8 位微處理器(ATmage328，8MHz 時鐘)、三軸陀螺儀、三軸加速度計和三軸磁場計芯片組成。姿態計算可由微處理器單獨完成，也可將傳感器數據通過串口上傳火星車主計算機，由其完成姿態計算。由于火星不存在固定的磁場，所以在本研究中三軸磁場計的定向輸出只作為地面模擬實驗環境下一個航向參考值。

將此開發板固定安裝到輪式火星車上，構成一個捷聯慣導系統。移動機器人運動過程中的姿態測算，通常用到2個坐標系統:一個是用來分析機器人運動的地面坐標系E;另一個是機器人自身的車體坐標系B，如圖1 所示，其中θ 表示俯仰角，φ 表示橫滾角，ψ 表示航向角。

圖1 地面和車體坐標系

本文采用四元數法求解姿態。對于從地面坐標系E 到車體坐標系B 旋轉一個角度θ，可以用一個四元數表示為:

于是采用四元數表示的旋轉矩陣R 可以表示為:

進一步，車體姿態的歐拉角表達方式θ，φ，ψ可以用下式求解:

MEMS-IMU 的三軸陀螺儀可以測量輸出車體在車體坐標系x，y，z 方向上的角速度ωx，ωy，ωz，如果用向量ω 表示為:

則四元數描述的車體相對地面坐標系的旋轉可以用下式計算:

在初始姿態已知的情況下，利用時步積分法可以估測計算得到時刻t 車體的旋轉狀態

加速度計也可以單獨測算探測車的旋轉姿態，用與陀螺儀類似的方式，我們可以推導出與式(7)加速度計表示的時刻t 車體的旋轉狀態:

式中，▽f 為加速度計所計算獲得的重力場分量。

陀螺儀存在一個固有的技術缺陷，就是隨機漂移，特別是低成本MEMS-IMU 器件，陀螺儀長時精度很低，導致陀螺儀的輸出值不可信。為解決陀螺儀漂移問題，本文采用了一種補償濾波器算法(該算法在文獻［13］中被提出并得到數學證明)，主要思路是:利用探測車姿態發生改變時，加速度計會測量獲得俯仰角和橫滾角坐標方向上產生的重力加速度分量，根據此分量計算得到的車體旋轉狀態，然后去更正陀螺儀產生的計算誤差。

根據式(7)和式(8)我們設計了一個補償融合公式:

式中，γt是權重系數，它是根據對陀螺儀誤差與測算值之間的估值比自動調整。

該算法的原理可用圖2 概述。

圖2 補償濾波器原理示意

本文采取的這種補償融合算法計算量小，單片機可以完成整個計算過程，實時求解輸出姿態角，工作頻率達到50Hz。模擬實驗的結果顯示俯仰角(θ)、橫滾角(φ)精度較高，而航向角在轉彎時誤差偏大。文獻［9］的研究中采用卡爾曼濾波做類似融合算法，也同樣提及航向角誤差偏大的問題，但是沒有分析原因。本文分析認為其原因也與本算法一樣，是由于加速度計無法在航向角(水平方向)上對此方向軸上的陀螺儀漂移誤差輸出進行補償修正所導致的。

2 基于單目攝像頭的航向角測算

為了解決上述航向角誤差偏大的問題，本文進一步提出利用單目視覺測算航向的思路，采取類似文獻［10］中視覺里程計的算法，但與其采用立體攝像機獲取視覺圖片對、用角點特征算法進行圖片特征提取、配對、跟蹤和運動跟蹤的復雜算法不同，本文采用了一種基于表象(appearance)模板匹配的算法，計算量小、實時性好。

一般而言，運動圖片的位移可以用光流法計算，但是在地表圖片比較一致、沒有明顯紋理特征的環境下，光流法無法獲得特征點［14］。文獻［10］也提到機遇號在它的火星探測區域中面臨許多平坦、缺少圖像特征的環境，使得基于角點特征的視覺里程計方法失效，只能依賴傳統的里程計和慣性導航器件。

另一種計算運動圖片位移的方法是模板匹配法［15］，在相鄰圖片幀里查找相似的區域圖案，可以有效地工作于更廣泛的地面環境中，包括沙礫、草地等無明顯圖像特征的地面。

本文采取一種改進的模板匹配算法，首先利用安裝在探測車上的前視攝像頭(與地面有一個固定傾角)，對地面環境連續抓取圖片，分析相鄰兩幅圖片的位移變化。具體過程是:先在第一幅圖片的固定區域中隨機生成部分采樣點，然后在下一幅圖片的預設搜索區域中進行采樣像素點的循環搜索匹配，逐次縮小搜索范圍直至檢測到最相似的區域，從而確定圖片位移距離，再通過位移距離換算得到攝像頭航向偏轉角度，如圖3 所示。在進行圖片區域相似度匹配時，采用歐式距離D 為判別依據。

圖3 基于表象的模板匹配算法示意

通過模板匹配計算出相鄰兩幅圖片的位移ΔU之后，通過式(11)可以得到航向角的偏移量Δψ:

最后通過公式(12)可以獲取航向角ψ:

在本算法中，當所安裝的攝像機(定焦距)對地面的探測角度確定之后，式(11)和(12)中M 只與攝像機視場角(Field of View)相關，是一個常量，測量一次即可獲得。因此本算法有以下幾個優勢:

1)無需對攝像機做標定工作，方法簡單、可靠;

2)本算法不提取圖片視覺特征，只測量相鄰圖片之間像素位移，計算量小，能夠達到每秒15幀的實時處理能力，因而對火星車行走速度沒有限制;

3)與基于立體視覺的利用圖片特征點(如SIFT)進行地表匹配的算法相比，本算法對環境的要求更低。實驗證明在地貌相似沒有明顯特征的環境中(如沙化、灰塵地表)，該算法一樣有效可靠。

3 實驗結果及分析

在地面模擬試驗環境中將上述方法應用到輪式火星車上，并進行了姿態估計和航向定位測試。同時在車上安裝了光學跟蹤球，通過一個室內運動光學跟蹤系統(VICON)對火星車姿態和位置進行測算，得到真實基準數據。在試驗中，采用地球重力加速度值代替火星重力加速度值。模擬實驗結果表明，本文所提出的傳感融合算法具有較好的姿態和航向估計精度。

從圖4 典型實驗曲線可以看出，基于單目視覺的航向角估測值(IMG － Yaw)非常接近真實值(VICON－Yaw)，并且平均精度高于MEMS －IMU所提供的基于地磁感應計輸出的定向角輸出(IMU－YAW)。

此外，多組測試結果顯示:在平坦地面條件下能得到更好的輸出結果，航向角平均誤差(RMS)在1°左右，仰俯角和橫滾角平均誤差(RMS)在0.6°左右;在沙礫、石頭和斜坡地表條件下，姿態和航向的測試精度有所下降，但是平均誤差仍在3°左右。導致精度下降的原因可能是在沙礫、斜坡路況下輪式機器車產生的側滑和攝像機的抖動，但仍需進一步的實驗來驗證和提高。

圖4 姿態航向測試典型數據

表1 姿態航向測量誤差(RMS)

4 結語

對于火星探測車在復雜地表環境下自主導航定位的需求，本文基于低成本、低功耗MEMS-IMU 和單目攝像機，提出一種傳感器融合的簡單有效的姿態航向估測方法，克服了陀螺儀漂移問題和地表環境單一性導致視頻特征點失效的問題。模擬測試結果驗證了該方法的有效性和可靠性。進一步的工作將考慮解決探測車側滑、抖動對本算法精度的影響，以及結合其他絕對定位方法提高長距離的航向定位精度。

［1］岳富占，崔平遠，崔祜濤.基于地球敏感器和加速度計的月球車自主定向算法研究［J］.宇航學報，2005，269(5):553-557. (YUE Fu-zhan，CUI Ping-yuan，CUI Hu-tao.Earth Sensor and Accelerometer Based Autonomous Heading Detection Algorithm Research of Lunar Rover［J］.Journal of Astronautics，2005，269(5):553-557.)

［2］岳富占，崔平遠，崔祜濤.月球巡視探測器定位技術研究［J］.機器人，2006，28(2):235-239. (YUE Fuzhan，CUI Ping-yuan，CUI Hu-tao. Research on Localization Technologies for Lunar Rovers［J］.Robot，2006，28(2):235-239.)

［3］寧曉琳，房建成.一種基于天體觀測的月球車位置姿態確定方法［J］. 北京航空航天大學學報，2006，32(7):756-759. (Ning Xiao-lin，Fang Jian-cheng. Position and Pose Estimation by Celestial Observation for Lunar Rovers［J］.Journal of Beijing University of Aeronautics and Astronautics，2006，32(7):756-759.)

［4］裴福俊，居鶴華，崔平遠.基于聯邦卡爾曼濾波的月球車自主導航方法［J］. 宇航學報，2009，30(4):1429-1434.(PEI Fu-jun，JU He-hua，CUI Ping-yuan.Autonomous Navigation Method Based on Federated Kalman Filter for Lunar Rover［J］.Journal of Astronautics，2009，30(4):1429-1434.)

［5］Y.Kuroda，T.Kurosaw.Accurate Localization in Combination with Planet Observation and Dead Reckoning for Lunar Rover［C］.Proc.of 2004 IEEE International Conference on Robotics and Automation (ICRA 2004 )，New Orleans，pp.2092-2097.

［6］邸凱昌.勇氣號和機遇號火星車定位方法評述［J］.航天器工程，2009，18(5):1-5. (DI Kai-chang. Spacecraft Engineering［J］.A Review of Spirit and Opportunity Rover Localization Methods.2009，18(5):1-5.)

［7］K.Ali，C.Vanelli ，J.Biesiadecki.Attitude and Position Estimation on the Mars Exploration Rovers［C］.Proc of the 2005 IEEE Conference on Systems Man and Cybernetics，October 2005.

［8］R.Li，S.W.Squyres，R.E. Arvidson. Initial Results of Rover localization and topographic mapping for the 2003 Mars exploration rover mission［J］. Photogram2 Metric Engineering and Remote Sensing，Special issue on Mapping Mars，2005，71 (10):1129-1142.

［9］Maimone M ，Johnson A ，Cheng Y，et al.Autonomous Navigation Results from the Mars Exploration Rover(MER)Mission ［C］. 9th International Symposium on Experimental Robotics (ISER)，June 2004.

［10］M.Maimone，Y.Cheng，L.Matthies.Two Years of Visual Odometry on the Mars Exploration Rovers［J］.Journal of Field Robotics，2007，24(3):169-186.

［11］Instrument Information［EB/OL］. http://starbrite. jpl.nasa. gov/pds/viewInstrumentProfile. jsp? INSTRUMENT_ID=imu＆INSTRUMENT_HOST_ID=MER2.

［12］J. Hidalgo，P. Poulakis，J. Kohler，A. Barrientos，and J. Del-Cerro. ESTEC Testbed Capabilities for the Performance Characterization of Planetary Rover Localization Sensors-First Results on IMU Investigations［C］.11th Symposium on Advanced Space Technologies in Robotics and Automation，2011.

［13］R. Mahony，T. Hamel，J. M. Pflimlin. Nonlinear Complementary Filters on the Special Orthogonal Group［J］.IEEE Transactions on Automatic Control，53(5):1203-1218.

［14］N. Nourani-Vatani，J. Roberts，M. Srinivasan. IMU Aided 3d Visual Odometry for Car-like Vehicles［C］. In Proceedings of the Australian Conference on Robotics and Automation，2008，Vol:1，pp.1-8.

［15］Berthold K P Horn. Robot Vision. MIT Electrical Engineering and Computer Science Series［D］. The MIT Press，1986.