李 琳 輝， 趙 一 兵， 郭 烈， 張 明 恒

（1.大連理工大學 工業(yè)裝備結構分析國家重點實驗室，遼寧 大連 116024；2.大連理工大學 運載工程與力學學部 汽車工程學院，遼寧 大連 116024）

0 引 言

視覺里程計是一種基于單目視覺或雙目視覺傳感器，通過跟蹤圖像中的特征點來進行車體運動估計的定位裝置[1].目前，除了應用于無人地面車輛[2、3]（unmanned ground vehicle，UGV）外，視覺里程計最為成功的應用是在美國2004年著陸火星的勇氣號和機遇號火星巡視器（Mars exploration rovers，MERs）[4]上.與傳統(tǒng)的相對定位傳感器（如車輪里程計、慣導傳感器）相比，視覺里程計的定位精度僅依靠視覺輸入信息，不受滑轉、傾斜、下陷等因素的影響，在松軟環(huán)境下具有較高的相對定位精度和巡視里程統(tǒng)計結果.此外，通過車輪里程計和慣導信息的結合，能夠對滑轉率進行估計，可以為導航控制提供狀態(tài)反饋量，提高巡視器的行駛安全性和通過性[5].

實現(xiàn)視覺里程計的基本框架是特征點提取→特征點匹配與跟蹤→運動估計，目前比較常用的特征是 Harris角點[3～6]和SIFT 算子[7、8].前者能夠提高算法的實時性；后者能夠適應較大的視角差異，但需要較長的算法時間，不適合在計算能力有限的月球車上使用.特征點的匹配和跟蹤包括左、右圖像特征點的匹配和前、后連續(xù)幀圖像特征點的跟蹤兩個部分，一般引入多種約束條件從減少誤匹配的角度來提高運動估計的精度；在運動估計部分，為了提高求解精度，常用的運動求解算法有最小二乘、隨機抽樣一致（RANSAC）[3]和最大似然估計[4、5]方法等.

雖然視覺里程計的理論已基本完善，但針對外星探測而言，仍存在許多問題需要解決.例如，在沙質環(huán)境中難以找到足夠的特征點進行跟蹤，特征點位置變化影響特征跟蹤穩(wěn)定性，需要提高里程計對不收斂問題的魯棒性[4]等.

本文針對上述問題開展月球車用視覺里程計的開發(fā)，首先研究快速的圖像增強算法，保證提取足量的特征點；其次通過預估搜索方向和搜索窗口的方法提高跟蹤匹配的效率；同時，采用RANSAC方法求解基礎矩陣并進行運動估計，提高里程計對不收斂問題的魯棒性；最后在特征點缺乏的月面模擬試驗環(huán)境中進行試驗，以驗證所提方法的有效性.

1 特征點提取與增強

視覺里程計作為一種依賴特征點跟蹤進行運動估計的相對定位方法，特征點提取的結果影響運動估計的穩(wěn)定性和精度.從月面的有關研究資料中可知，月壤的粒度與淤沙相似，如果攝像機視野中沒有月巖、車轍等特征明顯的目標，則容易產生特征點稀少并且不夠強壯的問題，將難以實現(xiàn)特征點的跟蹤，或者降低運動估計的精度.所以，需要選擇合適的特征點提取方法和圖像預處理方法以提高特征點提取的數(shù)量和質量，并且使特征點的分布盡量均勻，以提高運動估計的精度.

1.1 Harris角點檢測

Harris角點具有快速、穩(wěn)定，對光照條件、噪聲等情況不敏感，能定量提取，且所得特征點分布合理的特點，在MERs視覺里程計應用中取得了良好的效果.Harris算子的卷積表達式如下：式中：gx為圖像坐標系x方向的梯度，gy為y方向的梯度；G（槇s）為高斯模板，為卷積操作；E為每像素點的能量函數(shù)；det為矩陣的行列式；trace為矩陣的跡；k為常數(shù)，一般取0.04.

為了提高角度分布的均勻性，在計算出每個像素點的E后，首先按照非最大值抑制原則，取3×3窗口中各點E最大的像素點作為候選角點，并將候選角點按E由高到低進行排序.然后，設定一個候選角點間的最小距離Dmin，以其為約束條件對順序排列的各候選角點進行篩選，便可以按需要的角點的數(shù)目進行定量提取.經(jīng)過該種處理后，所保留的角點就是用于匹配跟蹤的特征點.

1.2 Top－Hat變換

從Harris角點提取的原理可以看出，如果增大角點在兩個正交方向上的梯度，將能夠增大能量函數(shù)的值，得到更強壯的角點，這需要研究與Harris角點提取相匹配的局部圖像增強算法.

局部圖像增強算法以局部直方圖均衡算法為代表，能夠提高圖像每個局部細節(jié)的對比度，但需要多次統(tǒng)計窗口內的直方圖，計算量較大.為了達到快速局部圖像增強的目的，本文采用形態(tài)學的Top－Hat變換來提高圖像每個像素點的鄰域梯度.該方法具有快速且易于并行實現(xiàn)等優(yōu)點.

基于Top－Hat變換的對比度提高利用了形態(tài)學白Top－Hat和黑Top－Hat變換分別能夠檢測圖像的峰值和谷值的性質.一幅圖像f的白Top－Hat變換，用 WTH變換表示，定義為原始圖像f與開運算γ（f）的差，即

開運算是一種非擴展變換，處理過程中在原始圖像的下方，所以WTH變換用來檢測圖像的峰值.

一幅圖像f的黑Top－Hat變換，用BTH變換表示，定義為原始圖像f與閉運算 （f）的差，即

根據(jù)對偶性，BTH變換的結果是信號的波谷，因此BTH變換用來檢測圖像中的谷值.

根據(jù)WTH變換和BTH變換能分別檢測出峰和谷的性質，如果將WTH變換的結果與原圖像相加，就會增加圖像中亮目標；而從得到的結果中減去BTH變換的結果，就會加強圖像中的暗目標，這就提高了整幅圖像的對比度，用kTH表示為

綜上，本文圖像特征提取部分的實現(xiàn)流程如下：設定能量函數(shù)的最小閾值Emin、角點間的最小距離Dmin和需提取的角度數(shù)目n進行角點提取，若能量值大于Emin的角點個數(shù)不足n，則進行Top－Hat變換后重新對角點進行提取，直至獲取n個能量大于Emin的角點.

2 特征點匹配與跟蹤

特征點的匹配與跟蹤是里程計算的關鍵環(huán)節(jié)，需要首先對同時刻左、右圖像上的特征點進行匹配，然后對不同時刻同視角圖像上的特征點進行跟蹤，最后建立同一物點在不同時刻不同視角圖像上的對應關系.特征點匹配與跟蹤的準確率直接影響運動估計的精度，同時也是影響算法實時性的關鍵環(huán)節(jié).因此，本文從降維、合理選擇搜索窗口和預估搜索方向等角度出發(fā)，提高特征匹配與跟蹤的效率.

2.1 對極線約束匹配

對極線約束可描述為一幅圖像上的點槇m在另一幅圖像上的匹配點槇m′位于該點的對極線l′m上.數(shù)學上這一幾何關系表示為一個3×3的基礎矩陣F，如式（6）所示.通過基礎矩陣的求解，可以將特征點匹配的搜索空間從二維降到一維的對極線上.

其中I表示匹配窗口的灰度值，fmw表示匹配窗口，上標A和B分別表示不同的圖像.

（3）循環(huán)重復①～③m次，如果ε發(fā)生變化，使用式（8）作相應調整.其中Γ為至少取得一個良性取樣子集的概率，應大于0.95；ε表示錯誤點占數(shù)據(jù)集的比例；p表示基本子集中數(shù)據(jù)點的數(shù)目.

① 在數(shù)據(jù)集中，取樣基本子集p＝8；

② 由基本子集，采用最小二乘法計算基礎矩陣；

③ 評估基礎矩陣的相容性：對數(shù)據(jù)集中所有數(shù)據(jù)點，計算數(shù)據(jù)點像素與其對應極線的距離，大于某一閾值則被分類為錯誤匹配點，小于的為正確匹配點；

（4）通過與前一次循環(huán)的比較，保留具有最多正確匹配點數(shù)目的基礎矩陣；

（5）所有正確匹配的點重新計算基礎矩陣.

2.2 搜索窗口優(yōu)化

設置合理的搜索窗口和搜索方向能夠有效減少計算量，降低特征點位置變化對匹配跟蹤的干擾，最終提高特征點匹配的效率.本文針對立體視覺傳感器的特點，從以下幾個方面對特征點匹配和跟蹤的過程進行優(yōu)化.

（1）限定感興趣區(qū)域，從立體匹配的誤差模型可知，物點的重建誤差隨其距攝像機距離的增大

為提高基礎矩陣求解的精度以及對誤匹配的魯棒性，本文采用了基于RANSAC的方法.RANSAC的基本思想是：對數(shù)據(jù)進行多次隨機取樣，每次取出盡可能少但充分多個數(shù)據(jù)來確定模型參數(shù)，再根據(jù)已確定的模型對所有數(shù)據(jù)進行劃分，在此模型誤差范圍內的點稱為內點，反之為外點；經(jīng)多次采樣后，找出內點最多的點的集合進行進一步優(yōu)化，最終確定模型參數(shù).該方法理論上能在錯誤數(shù)據(jù)比例大于50%時獲得穩(wěn)定的估計結果，能夠有效解決錯誤數(shù)據(jù)過多帶來的不收斂問題.本文基于該思想的基礎矩陣求解步驟如下：

（1）提取Harris角點作為待匹配的特征點.

（2）通過對Harris角點的初始匹配來確定匹配特征點數(shù)據(jù)集.初始匹配以式（7）所示的相似性度量函數(shù)為判斷依據(jù)，引入唯一性約束來提高結果的可靠性.而增大.所以，在提取特征點進行匹配前，裁去原始圖像中的遠景部分，能夠避免不精確的特征點跟蹤對運動估計的干擾，同時也降低了計算量.

（2）對左、右圖像特征點的匹配而言，由于攝像機安裝姿態(tài)不變，具有固定的沿著極線的搜索方向.為了確定合適的搜索窗口，將圖像分成若干區(qū)域，基于前一時刻立體匹配的結果為每個區(qū)域設置可參考的平均視差值，從而建立可更新的搜索窗口查找表，能夠根據(jù)特征點的像素坐標和前一時刻匹配結果，分配合理的搜索區(qū)間.

（3）對前、后時刻同視角圖像上的特征點跟蹤而言，搜索方向和搜索窗口的確定依賴于車體的運動情況.所以，同樣將圖像分成若干區(qū)域，首先統(tǒng)計相鄰時刻各區(qū)域內特征點的大致搜索方向和平均視差，然后采用Kalman濾波器預估下一時刻的搜索方向和平均視差，從而提高特征點跟蹤的效率.

3 運動估計

根據(jù)立體視覺原理，確定左、右圖像上特征點的對應關系后，依據(jù)攝像機標定結果，可以獲取該特征點在車體坐標系下的三維坐標.定義前一時刻和當前時刻同一特征點i對應的三維坐標分別為Ppi＝ （xpiypizpi）T和Pci＝（xciycizci）T，運動估計的目標是希望找到坐標系間的旋轉矩陣R和平移向量T，使得式（9）最小，從而獲取車體位置和方向的變化.

其中N為跟蹤上的特征點的個數(shù).為避免錯誤的特征點跟蹤影響運動估計的精度和收斂性，同樣引入RANSAC思想，實現(xiàn)過程如下：

（1）在數(shù)據(jù)集中，取樣基本子集p＝4；

（2）由基本子集，采用奇異值分解法計算旋轉矩陣R和平移向量T；

（3）定義誤差閾值，檢測所有匹配點對R和T的支持程度，統(tǒng)計內點數(shù)；

（4）重復（1）～ （3）m次，找到內點數(shù)最多的R和T，并將這些內點作為新的匹配點集，采用奇異值分解法求解新的R和T.

用奇異值分解法求解坐標變換的過程為

（1）計算三維點集Pp和Pc的質心，并生成新的三維點集

4 試驗分析

視覺里程計算利用安裝在月球車上的一對立體相機加以實現(xiàn)，算法流程如圖1所示.圖2為立體相機在月球車原理型樣車上的安裝示意圖，相機基線距離為10 cm，鏡頭焦距為12 mm，圖像分辨率為640像素×480像素.立體視覺系統(tǒng)的標定采用了Tsai等的兩步法[9]，該方法能夠考慮鏡頭的徑向畸變，其主要優(yōu)點在于大部分求解過程是線性的，為非線性迭代部分提供了良好的初值，能同時計算出攝像機的內外參數(shù).此外，為了計算月球車的滑轉率，在6個車輪上還分別安裝了輪速傳感器.

圖1 視覺里程算法流程圖Fig.1 Flow chart of visual odometry algorithm

圖2 視覺里程計和試驗車Fig.2 Visual odometry and the test vehicle

為模擬月球車在月面的工作環(huán)境，需要對月壤進行模擬.依據(jù)參考資料[10]，月壤的粒度與淤沙相似，顆粒直徑以小于1 mm為主，絕大部分顆粒直徑在30μm～1 mm，中值直徑在40～130 μm.為此，選擇灰色的翻砂用細沙，搭建了小型試驗場進行月球車漫游試驗，圖3給出了特征點稀少的模擬場地立體圖像.從原始圖像中可以看出，圖像灰度均勻，沒有明顯的可提取特征用于匹配跟蹤，可用于模擬惡劣的里程計工況.在該種工況下，如果不進行圖像增強，則難以提取到足量的具有較大能量函數(shù)的角點；圖4給出了進行Top－Hat變換后的立體圖像，從直觀上可以看出明顯的對比度增強效果.角點提取試驗表明，在能量函數(shù)閾值一定的前提下，增強后圖像中提取的角點是原始圖像中提取角點的2倍以上，可以有效提高所提取角點的質量，從而提高特征匹配的穩(wěn)定性.

圖3 立體相機拍攝的月面試驗場圖像Fig.3 Simulated lunar surface images taken by stereo camera

圖4 Top－Hat增強后的圖像Fig.4 The enhancement results of Top－Hat processing

圖5給出了對增強圖像中角點進行跟蹤的結果，圖中每個角點都帶有一段直線，直線的另一端用于指示本特征點在另一幅圖像中的對應點在本幅圖像中的坐標投影位置.可以看出匹配跟蹤算法能夠適應特征點位置的變化，保證足量特征點的跟蹤并且特征點分布比較均勻，便于進行運動估計.

圖5 角點跟蹤結果Fig.5 Corner tracking results

為了測試視覺里程算法在缺乏特征的環(huán)境中的穩(wěn)定性和精度，本文進行了直行和彎道試驗，月球車按最高速度200 m/h行駛.圖6為直線行駛過程推算軌跡與實際軌跡的對比圖，圖7為弧線行駛過程推算軌跡與實際軌跡的對比圖.從兩組軌跡對比中可以看出，月球車實際行駛的軌跡與視覺里程計推算出的軌跡基本吻合，表明算法在松軟的沙質環(huán)境中能夠實現(xiàn)精確的定位.

圖6 直行試驗結果Fig.6 Straight experiment results

表1給出了上述兩種工況的誤差分析結果，從中可以看出，在近4 m的行駛過程中，里程計的定位誤差整體不超過2%.試驗車上沒有安裝更精密的姿態(tài)傳感器，不能實時測量月球車行駛過程中車體俯仰角和側傾角的變化，以及高度方向（Z方向）上的位移變化，所以在表1中無法給出這3個自由度上的誤差統(tǒng)計，但從其較小的變化范圍可以看出，算法能夠輸出穩(wěn)定的結果，在運動估計過程中沒有產生不收斂問題.

圖7 彎道試驗結果Fig.7 Bend experiment results

表1 試驗數(shù)據(jù)分析Tab.1 Experiment data analysis

圖8給出了彎道行駛過程中車體偏航角φ、俯仰角θ和側傾角ψ的變化情況，通過與終點處車體的實際偏航角進行對比發(fā)現(xiàn)，偏航角的累計測量誤差為1.28°，受軟土路面的影響，車體的俯仰角和側傾角在小范圍內波動，整體變化范圍不超過3°，說明本算法能夠精確、穩(wěn)定地反映出車體在6個自由度上的位姿變化.

圖8 彎道試驗車體的偏航、俯仰和側傾角Fig.8 Yaw，pitch and roll of vehicle for bend experiment

為了計算月球車的滑轉率s，對視覺里程計和車輪里程計的輸出進行對比，圖9給出了彎道行駛過程中車輪的滑轉率變化曲線，實現(xiàn)了有效的滑轉率估計，能夠為基于滑轉率的驅動控制奠定基礎.

圖9 滑轉率檢測Fig.9 Slip ratio measurement

5 結 語

本文針對沙質的月面模擬環(huán)境，開發(fā)了可以對月球車進行精確定位的視覺里程算法，能夠推算車體在6個自由度上的姿態(tài)變化，并與車輪里程計結合實現(xiàn)了滑轉率的實時估計，為月壤參數(shù)估計以及月球車驅動控制奠定了基礎，具有重要的現(xiàn)實意義.作為一種相對定位裝置，隨著連續(xù)行駛里程的增加，視覺里程計會產生累計誤差，所以，如果要實現(xiàn)月球車長距離的精確定位，下一步需要研究能夠融合全局定位信息的視覺里程計定位誤差修正方法.

[1]MATTHIES L，SHAFER S.Error modeling in stereo navigation[J].IEEE Journal of Robotics and Automation，1987，RA－3（3）：239－248

[2]NOURANI V N，ROBERTS J，SRINIVASAN M V.Practical visual odometry for car－like vehicles[C]//IEEE International Conference on Robotics and Automation. Kobe：Institute of Electrical and Electronics Engineers Inc.，2009：3551－3557

[3]NISTER O N，BERGEN J.Visual odometry[C]//Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition.Washington D C：Institute of Electrical and Electronics Engineers Computer Society，2004：I652－I659

[4]MARK M，YANG C，LARRY M.Two years of visual odometry on the Mars exploration rovers[J].Journal of Field Robotics，2007，24（3）：169－186

[5]DANIEL M H，YANG C.Path following using visual odometry for a Mars rover in high－slip environments [C]//IEEE Aerospace Conference Proceedings.Big Sky：Institute of Electrical and Electronics Engineers Computer Society，2004：772－788

[6]彭 勃，周文暉，劉濟林.基于Harris角點檢測的立體視覺里程計[J].兵工學報，2007，28（12）：1498－1502

[7]李 智，周文暉，劉濟林.動態(tài)場景下基于視差空間的立體視覺里程計[J].浙江大學學報（工學版），2008，42（10）：1661－1665

[8]吳功偉，周文暉，顧偉康.基于視差空間的雙目視覺里程計[J].傳感技術學報，2007，20（6）：1432－1436

[9]TSAI R， LENZ R K. A technique for fully autonomous and efficient 3D robotics hand/eye calibration[J].IEEE Transactions on Robotics and Automation，1989，5（3）：345－358

[10]鄭永春.模擬月壤研制與月壤的微波輻射特性研究[D].貴陽：中國科學院地球化學研究所，2005：21－22