婁 路

重慶交通大學(xué)信息科學(xué)與工程學(xué)院，重慶400074

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

文獻(xiàn)［3］研究了利用太陽(yáng)、地球的天文導(dǎo)航方法與航位推算法相結(jié)合獲得位置的月球車組合導(dǎo)航方法。針對(duì)月球車導(dǎo)航的特殊要求，文獻(xiàn)［4］提出了天文導(dǎo)航與航位推算有機(jī)組合的月球車自主導(dǎo)航系統(tǒng)。文獻(xiàn)［5］提出一種利用對(duì)太陽(yáng)高度的連續(xù)觀測(cè)，通過(guò)最小二乘法求解天文位置圓非線性方程組，獲得漫游車固定點(diǎn)位置的天文導(dǎo)航方法。美國(guó)國(guó)家航空航天局的“勇氣號(hào)”和“機(jī)遇號(hào)”火星車綜合利用了里程計(jì)、慣性導(dǎo)航系統(tǒng)、視覺(jué)里程計(jì)和太陽(yáng)敏感器這些相對(duì)定向和絕對(duì)定向技術(shù)進(jìn)行導(dǎo)航定位［6］。

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

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

因此，針對(duì)MEMS-IMU 的發(fā)展趨勢(shì)和視覺(jué)測(cè)程法采用標(biāo)定立體攝像機(jī)、算法復(fù)雜且依賴于地形特征點(diǎn)等問(wèn)題，本文提出一種基于MEMS-IMU 和單目攝像頭的傳感器融合方法，該方法采用加速度計(jì)的重力場(chǎng)分量去補(bǔ)償修正陀螺儀的漂移偏差，獲得較高的俯仰角和橫滾角精度。采用單目攝像機(jī)，無(wú)需標(biāo)定，設(shè)計(jì)了一個(gè)簡(jiǎn)單高效基于表象的視覺(jué)定向算法獲取火星車行駛的航向角，該視覺(jué)定向方法能適用于各種地表環(huán)境。

1 基于傳感器數(shù)據(jù)補(bǔ)償融合的姿態(tài)估計(jì)

本文采用一個(gè)軟件開(kāi)源的MEMS-IMU 開(kāi)發(fā)板作為姿態(tài)導(dǎo)航系統(tǒng)，該開(kāi)發(fā)板是由一個(gè)8 位微處理器(ATmage328，8MHz 時(shí)鐘)、三軸陀螺儀、三軸加速度計(jì)和三軸磁場(chǎng)計(jì)芯片組成。姿態(tài)計(jì)算可由微處理器單獨(dú)完成，也可將傳感器數(shù)據(jù)通過(guò)串口上傳火星車主計(jì)算機(jī)，由其完成姿態(tài)計(jì)算。由于火星不存在固定的磁場(chǎng)，所以在本研究中三軸磁場(chǎng)計(jì)的定向輸出只作為地面模擬實(shí)驗(yàn)環(huán)境下一個(gè)航向參考值。

將此開(kāi)發(fā)板固定安裝到輪式火星車上，構(gòu)成一個(gè)捷聯(lián)慣導(dǎo)系統(tǒng)。移動(dòng)機(jī)器人運(yùn)動(dòng)過(guò)程中的姿態(tài)測(cè)算，通常用到2個(gè)坐標(biāo)系統(tǒng):一個(gè)是用來(lái)分析機(jī)器人運(yùn)動(dòng)的地面坐標(biāo)系E;另一個(gè)是機(jī)器人自身的車體坐標(biāo)系B，如圖1 所示，其中θ 表示俯仰角，φ 表示橫滾角，ψ 表示航向角。

圖1 地面和車體坐標(biāo)系

本文采用四元數(shù)法求解姿態(tài)。對(duì)于從地面坐標(biāo)系E 到車體坐標(biāo)系B 旋轉(zhuǎn)一個(gè)角度θ，可以用一個(gè)四元數(shù)表示為:

于是采用四元數(shù)表示的旋轉(zhuǎn)矩陣R 可以表示為:

進(jìn)一步，車體姿態(tài)的歐拉角表達(dá)方式θ，φ，ψ可以用下式求解:

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

則四元數(shù)描述的車體相對(duì)地面坐標(biāo)系的旋轉(zhuǎn)可以用下式計(jì)算:

在初始姿態(tài)已知的情況下，利用時(shí)步積分法可以估測(cè)計(jì)算得到時(shí)刻t 車體的旋轉(zhuǎn)狀態(tài)

加速度計(jì)也可以單獨(dú)測(cè)算探測(cè)車的旋轉(zhuǎn)姿態(tài)，用與陀螺儀類似的方式，我們可以推導(dǎo)出與式(7)加速度計(jì)表示的時(shí)刻t 車體的旋轉(zhuǎn)狀態(tài):

式中，▽f 為加速度計(jì)所計(jì)算獲得的重力場(chǎng)分量。

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

根據(jù)式(7)和式(8)我們?cè)O(shè)計(jì)了一個(gè)補(bǔ)償融合公式:

式中，γt是權(quán)重系數(shù)，它是根據(jù)對(duì)陀螺儀誤差與測(cè)算值之間的估值比自動(dòng)調(diào)整。

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

圖2 補(bǔ)償濾波器原理示意

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

2 基于單目攝像頭的航向角測(cè)算

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

一般而言，運(yùn)動(dòng)圖片的位移可以用光流法計(jì)算，但是在地表圖片比較一致、沒(méi)有明顯紋理特征的環(huán)境下，光流法無(wú)法獲得特征點(diǎn)［14］。文獻(xiàn)［10］也提到機(jī)遇號(hào)在它的火星探測(cè)區(qū)域中面臨許多平坦、缺少圖像特征的環(huán)境，使得基于角點(diǎn)特征的視覺(jué)里程計(jì)方法失效，只能依賴傳統(tǒng)的里程計(jì)和慣性導(dǎo)航器件。

另一種計(jì)算運(yùn)動(dòng)圖片位移的方法是模板匹配法［15］，在相鄰圖片幀里查找相似的區(qū)域圖案，可以有效地工作于更廣泛的地面環(huán)境中，包括沙礫、草地等無(wú)明顯圖像特征的地面。

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

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

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

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

在本算法中，當(dāng)所安裝的攝像機(jī)(定焦距)對(duì)地面的探測(cè)角度確定之后，式(11)和(12)中M 只與攝像機(jī)視場(chǎng)角(Field of View)相關(guān)，是一個(gè)常量，測(cè)量一次即可獲得。因此本算法有以下幾個(gè)優(yōu)勢(shì):

1)無(wú)需對(duì)攝像機(jī)做標(biāo)定工作，方法簡(jiǎn)單、可靠;

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

3)與基于立體視覺(jué)的利用圖片特征點(diǎn)(如SIFT)進(jìn)行地表匹配的算法相比，本算法對(duì)環(huán)境的要求更低。實(shí)驗(yàn)證明在地貌相似沒(méi)有明顯特征的環(huán)境中(如沙化、灰塵地表)，該算法一樣有效可靠。

3 實(shí)驗(yàn)結(jié)果及分析

在地面模擬試驗(yàn)環(huán)境中將上述方法應(yīng)用到輪式火星車上，并進(jìn)行了姿態(tài)估計(jì)和航向定位測(cè)試。同時(shí)在車上安裝了光學(xué)跟蹤球，通過(guò)一個(gè)室內(nèi)運(yùn)動(dòng)光學(xué)跟蹤系統(tǒng)(VICON)對(duì)火星車姿態(tài)和位置進(jìn)行測(cè)算，得到真實(shí)基準(zhǔn)數(shù)據(jù)。在試驗(yàn)中，采用地球重力加速度值代替火星重力加速度值。模擬實(shí)驗(yàn)結(jié)果表明，本文所提出的傳感融合算法具有較好的姿態(tài)和航向估計(jì)精度。

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

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

圖4 姿態(tài)航向測(cè)試典型數(shù)據(jù)

表1 姿態(tài)航向測(cè)量誤差(RMS)

4 結(jié)語(yǔ)

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

［1］岳富占，崔平遠(yuǎn)，崔祜濤.基于地球敏感器和加速度計(jì)的月球車自主定向算法研究［J］.宇航學(xué)報(bào)，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］岳富占，崔平遠(yuǎn)，崔祜濤.月球巡視探測(cè)器定位技術(shù)研究［J］.機(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］寧曉琳，房建成.一種基于天體觀測(cè)的月球車位置姿態(tài)確定方法［J］. 北京航空航天大學(xué)學(xué)報(bào)，2006，32(7):756-759. (Ning Xiao-lin，F(xiàn)ang 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］裴?？?，居鶴華，崔平遠(yuǎn).基于聯(lián)邦卡爾曼濾波的月球車自主導(dǎo)航方法［J］. 宇航學(xué)報(bào)，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］邸凱昌.勇氣號(hào)和機(jī)遇號(hào)火星車定位方法評(píng)述［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.