基于視覺傳感器的移動機器人定位算法*

張一博, 馬 磊

(西南交通大學 電氣工程學院，四川 成都 610031)

0 引 言

傳統(tǒng)傳感器定位導航過程中輪子打滑摩擦等現(xiàn)象的影響較為嚴重[1，2]，且全球定位系統(tǒng)(global positioning system,GPS)無法在室外使用，基于視覺傳感器的里程計重要性就顯得較為突出。根據(jù)所使用的傳感器的不同，視覺里程計大致上又可以分為三類，其中雙目立體視覺里程計由于精度較高，被廣泛應用到移動機器人的定位中[3]，但其計算量太大，需要GPU或者現(xiàn)場可編程門陣列(field programmable gate array,FPGA)處理才能實時輸出距離信息。單目傳感器[4]由于不能直接計算出圖像的距離，得到的只是一個相對的值。深度視覺傳感器是2010年之后興起的一種傳感器，其可以通過紅外結構光或者飛行時間(time-of-flight,TOF)原理，測出物體與傳感器之間的距離。

2004年Nistér D等人[5,6]設計了一種實時的視覺里程計，同時還提出了2種基于特征點匹配的里程計實現(xiàn)流程，即單目視覺里程計和立體視覺里程計。另一種比較普遍的光流法最早由Silveira提出，隨著一些LSD-SLAM[8]等開源項目的出現(xiàn)，光流法逐漸登上V-SLAM的主流舞臺。

本文將慣性測量單元(inertial measurement unit,IMU)和GPS組合導航中的卡爾曼融合思想[9]運用到視覺里程計中，對光流法和特征點法進行融合，并提出局部地圖快速匹配策略，經過開源數(shù)據(jù)集和實驗室環(huán)境的測試，結果表明:該方法在保證魯棒性同時，較一般的視覺里程計計算時間更短，定位精度更高，有效地降低了匹配跟蹤失敗率。

1 視覺里程計卡爾曼融合

1.1 基于光流法的運動估計

Lucas-Kanade算法是光流法的經典算法，該類算法的前提是同一個空間點對應的不同圖像中像素點灰度值固定不變，在此前提的基礎上，像素點的灰度可以寫成I(x,y,t)。假設空間中有一固定點，t時刻對應在圖像中的位置為(x,y)，由灰度不變前提可以得出

I(x+dx,y+dy,t+dt)=I(x,y,t)

(1)

對左邊進行泰勒展開，保留一階項，得

(2)

整理可得約束方程

(3)

圖像在該點處灰度值梯度變化分別記為Ix和Iy，圖像對時間的變化記為It，而圖像的運動也可以記為u,v，則約束方程寫成矩陣形式

(4)

若該點P在兩幀圖像中的非齊次坐標p1和p2，光流估計相機的主體思路是使得光度誤差e=I1(p1)-I2(p2)盡可能最小，因此,BA優(yōu)化的目標誤差二范數(shù)函數(shù)為

(5)

為了求得優(yōu)化變量也就是相機位姿ξ，使用李代數(shù)求解導數(shù)時的擾動模型思想，最終可以計算出誤差項ei關于位姿ξ的雅可比矩陣

(6)

最后利用高斯牛頓法迭代計算出相機位姿ξ，進一步解算出移動機器人的旋轉矩陣R和平移向量t，為后續(xù)的匹配法融合提供先驗預測值。

1.2 基于特征點匹配的相機運動估計

特征點有關鍵點和描述子兩部分組成，為了適當減少精確度和魯棒性以提升計算速度，選擇ORB特征[9]。ORB特征改進了FAST特征點檢測中方向不確定問題，且具有快速描述子BRIEF[10]。

(7)

其中,誤差項關于位姿的雅可比矩陣使用李代數(shù)擾動模型算出，最后在非線性優(yōu)化中只要不斷迭代，即可得出當目標函數(shù)(7)取得極小值時位姿ξ，從而解算出旋轉矩陣R和平移向量t。

1.3 光流法與特征點法融合

光流法的特點是幀間計算速度快，定位頻率高，但誤差累積是不可避免的問題；特征點匹配法計算速度比光流法慢，但定位精度高。將特征點匹配位置估計對光流法的先驗定位信息進行糾正，可以提升定位精度，同時在算法融合階段建立局部地圖，不僅能加強關鍵幀選取的嚴格性，還可以建立局部回環(huán)有效的降低誤差累積，流程如圖1所示。

圖1 融合算法整體框架

根據(jù)卡爾曼濾波器預測方程

(8)

zk=Tkxk+vk

(9)

式中Tk為當前關鍵幀下視覺傳感器的外參，zk為相機坐標系下點的坐標，vk為噪聲

(10)

(11)

圖2 關鍵幀選取與局部地圖更新

2 實驗結果和分析

采用現(xiàn)有的慕尼黑工業(yè)大學(TUM)提供的公開RBG-D數(shù)據(jù)集進行算法效果驗證。TUM數(shù)據(jù)集可以提供精確運動軌跡當作移動機器人的真實軌跡位置信息，以便和傳統(tǒng)視覺里程計算法以及融合算法進行比對，驗證該算法的精確度。該數(shù)據(jù)集所使用的視覺傳感器是深度相機Kinect 1，與本實驗所使用的一致，因此得到的算法驗證效果具有很高的參考價值。Kinect 1圖像分辨率為640×480，深度圖像和RGB圖像采集頻率都是20幀。將相鄰幀之間通過光流直接法確定的特征點，根據(jù)這些特征點估計出移動機器人的運動，作為卡爾曼濾波的先驗里程計信息，對關鍵幀之間的特征點進行匹配，為了避免特征點過多帶來的觀察不便，對特征點進行了剔除與篩選，將關鍵幀之間估計出的里程計信息與之前光流直接法解算出的先驗信息進行融合，得到最終的移動機器人位置軌跡，如圖3所示。

從圖中可以看出:對于直線運動過程的估計，光流法可以得到相對不錯的結果，但當移動機器人發(fā)生轉彎時，光流法解算出的位置信息角度誤差較大，機器人速度過快時也會造成一定的干擾，隨著時間不斷地推移誤差累積也在增加，這時特征點匹配處理大尺度移動場景的優(yōu)勢就體現(xiàn)出來了，同時速度過快和轉彎時建立的局部地圖篩選關鍵幀機制減少了誤差累積在定位精度上的干擾。

誤差曲線是計算視覺里程計算法得到的移動機器人位置點和運動捕捉系統(tǒng)(真值)之間的歐氏距離，如圖4所示。視覺里程計融合后算法誤差要明顯小于光流法的誤差。

圖4 不同視覺里程計算法的誤差

真實環(huán)境下的實驗器材為自主研發(fā)的移動機器人和Kinect 1視覺傳感器，將文中改進的視覺里程計算法運用到實驗室內場景中，觀察實驗效果。實驗室環(huán)境下的結果如圖5所示，上文中運用成熟的數(shù)據(jù)集已驗證了視覺融合算法精確度，真實環(huán)境下主要測試該算法實時性能否得到保障。根據(jù)實驗結果不難得出:在保證機器人定位精度的前提下融合算法可以滿足實時性要求，融合算法得到最終的移動機器人軌跡平滑，比光流法定位在精度上有很大的提升，特別是在機器人轉彎、速度過快等工作狀況下具有良好的魯棒性。

圖5 室內場景下視覺里程計位置估計

3 結束語

基于視覺傳感器的里程計算法研究在移動機器人定位中的地位逐步上升，該融合算法是將卡爾曼融合的思想運用在傳統(tǒng)的光流法和特征點匹配算法的基礎上，并提供了理論推導以及實現(xiàn)，提出了局部地圖的建立以減少誤差累積。在現(xiàn)有的數(shù)據(jù)集情況下經過測試滿足了移動機器人定位精度要求，真實室內場景中實驗則確保了實時性的要求，對于室外的情況，接下來會進行進一步的改進。