基于視覺(jué)傳感器的移動(dòng)機(jī)器人定位算法*

張一博, 馬 磊

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

0 引 言

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

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

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

1 視覺(jué)里程計(jì)卡爾曼融合

1.1 基于光流法的運(yùn)動(dòng)估計(jì)

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

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

(1)

對(duì)左邊進(jìn)行泰勒展開(kāi)，保留一階項(xiàng)，得

(2)

整理可得約束方程

(3)

圖像在該點(diǎn)處灰度值梯度變化分別記為Ix和Iy，圖像對(duì)時(shí)間的變化記為It，而圖像的運(yùn)動(dòng)也可以記為u,v，則約束方程寫成矩陣形式

(4)

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

(5)

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

(6)

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

1.2 基于特征點(diǎn)匹配的相機(jī)運(yùn)動(dòng)估計(jì)

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

(7)

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

1.3 光流法與特征點(diǎn)法融合

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

圖1 融合算法整體框架

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

(8)

zk=Tkxk+vk

(9)

式中Tk為當(dāng)前關(guān)鍵幀下視覺(jué)傳感器的外參，zk為相機(jī)坐標(biāo)系下點(diǎn)的坐標(biāo)，vk為噪聲

(10)

(11)

圖2 關(guān)鍵幀選取與局部地圖更新

2 實(shí)驗(yàn)結(jié)果和分析

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

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

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

圖4 不同視覺(jué)里程計(jì)算法的誤差

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

圖5 室內(nèi)場(chǎng)景下視覺(jué)里程計(jì)位置估計(jì)

3 結(jié)束語(yǔ)

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