室內(nèi)移動機(jī)器人視覺里程計研究

高云峰，李偉超，李建輝

（哈爾濱工業(yè)大學(xué)機(jī)電工程學(xué)院，黑龍江哈爾濱 150001）

0 引言

在室內(nèi)移動機(jī)器人的研究中，實時獲得機(jī)器人高精度的運動參數(shù)是極其重要的，關(guān)系到機(jī)器人定位、三維全局地圖構(gòu)建等重要導(dǎo)航任務(wù)的實現(xiàn)。由于機(jī)器人行進(jìn)過程中的碰到障礙物、輪子存在長期磨損、打滑現(xiàn)象，使得光電碼盤、測速電機(jī)等不能準(zhǔn)確測定機(jī)器人的位移準(zhǔn)確信息;采用GPS定位，存在分辨率低，且在室內(nèi)的信號弱，不適合于室內(nèi)移動機(jī)器人。視覺里程計可有效彌補(bǔ)這些缺點。

視覺里程計算法是由Matthies L［1］在1987年提出的，之后經(jīng)過多年的研究，已經(jīng)有很多優(yōu)秀的里程計出現(xiàn)。Nister D等人［2，3］分別采用單目和雙目實現(xiàn)里程計，并取得較好的效果。

Navid N V等人［4］設(shè)計的視覺里程計用在室外馬路上的車輛自身定位，這個里程計用攝像機(jī)拍攝地面，并采用光流法作圖像配準(zhǔn)，Zhang Tianguang［5］在隨后采用類似的方法設(shè)計的里程計，他也采用了光流法，最后他使用了卡爾曼濾波器將常規(guī)里程計測得的值和視覺里程計測得的值作了信息融合，在室內(nèi)環(huán)境達(dá)到了2.5%的精度，由于他們的里程計的工作模型是二維模型，對攝像機(jī)的安裝提出了比較高的要求。

1 視覺里程計工作模型

里程計裝在室內(nèi)移動機(jī)器人底部拍攝地面，配置合適光源進(jìn)行照明。

里程計用到的坐標(biāo)系有:

1）世界坐標(biāo)系（OwXwYwZw），原點位于標(biāo)定時棋盤圖的某一固定點上，OwXwYw與地平面重合，OwZw豎直向上。

2）機(jī)器人坐標(biāo)系（OrXrYrZr），原點位于機(jī)器人的后驅(qū)動輪軸心處，OrXr指向與后驅(qū)動輪軸線垂直方向，豎直向上。

3）機(jī)器人起始點坐標(biāo)系（OroXroYroZro），原點和方向與機(jī)器人坐標(biāo)系的初始位置重合。

4）攝像機(jī)坐標(biāo)系（OcXcYcZc），原點位于攝像機(jī)主點，與幀存坐標(biāo)系平面平行。

5）攝像機(jī)起始點坐標(biāo)系（OcoXcoYcoZco），原點和方向與攝像機(jī)坐標(biāo)系的初始位置重合。

6）幀存坐標(biāo)系（OoUV），原點位于圖像左上角，OoU向右，OoV向下。

2 視覺里程計軟件流程與關(guān)鍵算法

2.1 軟件流程

該里程計軟件流程為:在t時刻采集一幀圖像，然后提取SURF特征點，并與前一幀圖像的SURF特征點進(jìn)行匹配，匹配的準(zhǔn)則是最近鄰向量匹配法;匹配得到特征點對之后，再將幀存坐標(biāo)系下的點轉(zhuǎn)換到攝像機(jī)坐標(biāo)系下并進(jìn)行傾斜變換修正，然后使用RANSAC算法選取高精度的特征點對并計算機(jī)器人的轉(zhuǎn)換矩陣，以用于在攝像機(jī)起始點坐標(biāo)系下的運動估計;最后，再將攝像機(jī)起始點坐標(biāo)系下的機(jī)器人的坐標(biāo)轉(zhuǎn)換到機(jī)器人起始點坐標(biāo)系下。

2.2 攝像機(jī)模型與攝像機(jī)標(biāo)定

為了得到幀存坐標(biāo)系、攝像機(jī)坐標(biāo)系與世界坐標(biāo)系的關(guān)系，需要進(jìn)行攝像機(jī)的建模與標(biāo)定。

攝像機(jī)的模型有線性模型和非線性模型2種，由于采用的攝像機(jī)畸變較小，所以，選用線性模型，采用張正友的模型來進(jìn)行標(biāo)定，得到攝像機(jī)的參數(shù)為

其中，fx，fy，u0，v0為張正友模型下的攝像機(jī)內(nèi)參，Rc，Tc為攝像機(jī)起始位置時的外參。

2.3 SURF算法與幀間圖像特征點匹配

SURF算法是Bay H等人［6］提出的，它是對SIFT算法的改進(jìn)算法，理論基礎(chǔ)是尺度空間理論和尺度選擇理論，SURF算法中使用箱式濾波器代替二階高斯濾波器，這樣做的好處是，可以使用積分圖像來加速卷積過程，極大地減少了卷積所使用的時間，達(dá)到比SIFT更快的計算速度。

通過使用SURF算法得到前后兩幀的特征點組（u1i，v1i），i=1，…，n1和（u2i，v2i），i=1，…，n2。

然后進(jìn)行特征點匹配，使用最近鄰向量匹配法進(jìn)行匹配，依次選取第一幅圖像中的每一特征點與第二幅圖像中的所有特征點分別計算歐氏距離，只要最近的距離與次近的距離的比值小于一個閾值，就認(rèn)為這對特征點匹配成功，實驗結(jié)果如圖1所示。

圖1 前后兩幀圖像匹配結(jié)果Fig 1 Matching result of adjacent frames

2.4 幀存坐標(biāo)系至攝像機(jī)坐標(biāo)系變換

由SURF算法得到了一些特征點對，這些特征點對的坐標(biāo)還是在幀存坐標(biāo)系下表示的，由于需要在攝像機(jī)坐標(biāo)系下進(jìn)行運動估計，根據(jù)攝像機(jī)小孔成像模型和標(biāo)定得到的攝像機(jī)的參數(shù)進(jìn)行坐標(biāo)變換，得到特征點對在攝像機(jī)坐標(biāo)系下的坐標(biāo)，變換公式如下

2.5 基于透視變換的攝像機(jī)傾斜修正

由于CCD所在平面與地面不平行，所以，攝像機(jī)得到的圖像與實際地平面存在著變形，導(dǎo)致轉(zhuǎn)換到攝像機(jī)坐標(biāo)系中點的坐標(biāo)與地面的點的實際坐標(biāo)有所偏移。

由2.4節(jié)得到的點所在的平面是與地平面有一定偏角的平面1，平面1與攝像機(jī)光軸垂直。需要得到實際的點構(gòu)成的地平面2。

為了能將點平面 1 上得到的點P'i（x'ci，y'ci，zc，1）轉(zhuǎn)換到地平面2上的實際的點Pi（xci，yci，zci，1），需要求得直線OcP'i的方程和地平面2的方程，這樣就可以聯(lián)立方程組求得Pi（xci，yci，zci，1）。

在求地平面2的時候，由于攝像機(jī)的運動只有繞垂直于地面的方向上的軸的轉(zhuǎn)動和在平行于地面的平面內(nèi)的平移，地平面在前后兩幀的攝像機(jī)坐標(biāo)系中的表示都是一致的，所以，所有幀的攝像機(jī)傾斜修正的公式都是一致的，都可以由標(biāo)定得到的參數(shù)Rc和Tc.

2.6 幀間坐標(biāo)變換矩陣求解

求解:已知地面上的采樣點在當(dāng)前幀攝像機(jī)坐標(biāo)系下的坐標(biāo)和前一幀攝像機(jī)坐標(biāo)系下的坐標(biāo)，求當(dāng)前幀的點在前一幀攝像機(jī)坐標(biāo)系下的轉(zhuǎn)換矩陣，即求

其中，Wci=（Rci|Tci）為所求，Rci為旋轉(zhuǎn)矩陣，Tci為平移矩陣。

如果直接使用同一平面上的點來求解上面的Wci中的未知數(shù)，那么構(gòu)成的線性方程組的系數(shù)矩陣是病態(tài)矩陣，無法求得正確值，所以，采用向量的方式來解決這個問題，先求Rci，然后求Tci。

2.7 RANSAC 算法

在得到特征點對在攝像機(jī)坐標(biāo)系下的坐標(biāo)后，由于匹配的特征點存在誤匹配現(xiàn)象，于是選用RANSAC算法行運動估計［7］。

RANSAC算法應(yīng)用的基本思路是在特征點對中隨機(jī)選出3個特征點對，根據(jù)2.6所示步驟求得Wci，然后再通過統(tǒng)計適合于Wci的特征點對的個數(shù)，即內(nèi)點個數(shù)，選出適合點數(shù)目最多的Wci，認(rèn)為是所求的Wci。算法匹配結(jié)果如圖2。

圖2 RANSAC算法匹配結(jié)果Fig 2 Matching result of RANSAC algorithm

2.8 攝像機(jī)起始點下的機(jī)器人位移求解

當(dāng)前幀攝像機(jī)在OroXroYroZro下的坐標(biāo)，可根據(jù)下面公式求得

2.9 Mcr的標(biāo)定

得到攝像機(jī)起始點坐標(biāo)系下機(jī)器人的坐標(biāo)之后，需要把這個坐標(biāo)信息轉(zhuǎn)換到機(jī)器人起始點坐標(biāo)系下，因為機(jī)器人姿態(tài)變化時，不進(jìn)行轉(zhuǎn)換，會導(dǎo)致里程計給出的信息不是機(jī)器人的正確的坐標(biāo)信息。

提出一種Mcr的標(biāo)定方法，基本思路是讓機(jī)器人分別進(jìn)行純平移和繞起點與機(jī)器人起始點坐標(biāo)系原點相同、方向與OwXw軸相同的軸的純旋轉(zhuǎn)運動，運動中的各個位置的機(jī)器人坐標(biāo)系原點在機(jī)器人起始點坐標(biāo)系下的坐標(biāo)已知，當(dāng)機(jī)器人進(jìn)行純旋轉(zhuǎn)運動時，標(biāo)定得到攝像機(jī)坐標(biāo)系到機(jī)器人坐標(biāo)系的平移分量Tcr，當(dāng)機(jī)器人進(jìn)行純平移運動時標(biāo)定得到旋轉(zhuǎn)分量Rcr。

先求得當(dāng)前幀攝像機(jī)坐標(biāo)系的原點在攝像機(jī)起始點坐標(biāo)系下的坐標(biāo)，公式如下

公式中的Oci為當(dāng)前幀攝像機(jī)坐標(biāo)系的原點，Ori為當(dāng)前幀機(jī)器人坐標(biāo)系的原點，求得當(dāng)前幀機(jī)器人在攝像機(jī)起始點坐標(biāo)系下的坐標(biāo)后，就可以根據(jù)公式（7）就能得到當(dāng)前幀機(jī)器人在機(jī)器人起始點坐標(biāo)系下的坐標(biāo)

第一種方法標(biāo)定Mcr，對機(jī)器人純平移和機(jī)器人純旋轉(zhuǎn)這兩種特殊情形，先分別依據(jù)上面的原理算出當(dāng)前幀機(jī)器人在機(jī)器人起始點坐標(biāo)系下的坐標(biāo)，假定當(dāng)前幀的機(jī)器人在機(jī)器人起始點坐標(biāo)系下的坐標(biāo)已知且可信，由于測得的該坐標(biāo)含有Mcr的信息，依此作標(biāo)定，即可分別求得Mcr的旋轉(zhuǎn)分量和平移分量.最終測得的值如圖3、圖4所示。

圖3 純平移標(biāo)定實驗Fig 3 Calibration experiment of pure translational motion

圖4 純旋轉(zhuǎn)標(biāo)定實驗Fig 4 Calibration experiment of pure rotational motion

2.10 機(jī)器人起始點坐標(biāo)系下的位移求解

根據(jù)Mcr得到機(jī)器人起始點坐標(biāo)系下機(jī)器人的位移信息，由式（5）、式（6）、式（7）所示的步驟來求得。

3 實驗與結(jié)果分析

為了驗證視覺里程計算法的可行性，選用Motoman工業(yè)機(jī)器人進(jìn)行實驗，攝像機(jī)固定在機(jī)器人末端。

實驗步驟:機(jī)器人分37步沿一個直徑為300 mm的圓周行進(jìn)，測得37組數(shù)據(jù)，再根據(jù)以上算法得到機(jī)器人的末端坐標(biāo)，如圖5所示。

圖5 圓周實驗Fig 5 Experiment of circular motion

機(jī)器人末端起始坐標(biāo)為（0，0 mm），計算得到的機(jī)器人末端終點坐標(biāo)為（3.021，5.844 mm），相對誤差在2%以內(nèi)，而到終點機(jī)器人走過的距離是930.954 mm，理論上走的距離為 942.478 mm，誤差為 1.22%。

4 結(jié)束語

提出的里程計標(biāo)定方法可以精確地得到攝像機(jī)坐標(biāo)系與機(jī)器人坐標(biāo)系的轉(zhuǎn)換關(guān)系，能較為精確地得到機(jī)器人在機(jī)器人起始點坐標(biāo)下的位移信息;因為采用了三維模型，所以，對攝像機(jī)的安裝位姿無特殊要求。

本視覺里程計是基于單目視覺的，由圖像特征點還原到攝像機(jī)坐標(biāo)系下的點時，采用點的深度信息是一個固定值，因此，該視覺里程計只適用于較為平坦的室內(nèi)地面，若采用雙目視覺，可以適應(yīng)室外地面。

［1］Matthies L，Shafer S.Error modeling in stereo navigation［J］.IEEE J Robot Automat，1987，3（3）:239 －250.

［2］Nistér D，Naroditsky O，Bergen J.Visual odometry［C］//Proceedings of IEEE Computer Society Conference on Computer Vision and Pattern Recognition，2004:652 －659.

［3］Nistér D，Naroditsky O.Visual odometry for ground vehicle applications［J］.Journal of Field Robotics，2006，23（1）:3 －20.

［4］Navid N V，Roberts J，Srinivasan M V.Practical visual odometry for car-like vehicles［C］//2009 IEEE International Conference on Robotics and Automation，Kobe International Conference Center，2009.

［5］Zhang Tianguang，Liu Xiaodong，Kuhnlenz K，et al.Visual odometry for the autonomous city explorer［C］//The 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems，2009:11－15.

［6］Bay H，Ess A，Tuytelaars T，et al.Speeded-up robust features（SURF）［J］.Computer Vision and Image Understanding，2008，110:346－359.

［7］Fischler M A，Bolles R C.Random sample consensus:Aparadigm for model fitting with applications to image analysis and automated cartography［J］.ACM Commun，1981，24（6）:381 －389.