高云峰，李偉超，李建輝

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

0 引言

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

視覺里程計算法是由Matthies L［1］在1987年提出的，之后經(jīng)過多年的研究，已經(jīng)有很多優(yōu)秀的里程計出現(xiàn)。Nister D等人［2，3］分別采用單目和雙目實(shí)現(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），原點(diǎn)位于標(biāo)定時棋盤圖的某一固定點(diǎn)上，OwXwYw與地平面重合，OwZw豎直向上。

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

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

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

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

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

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

2.1 軟件流程

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.7 RANSAC 算法

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

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

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

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

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

2.9 Mcr的標(biāo)定

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

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

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

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

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

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

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

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

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

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

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

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

圖5 圓周實(shí)驗(yàn)Fig 5 Experiment of circular motion

機(jī)器人末端起始坐標(biāo)為（0，0 mm），計算得到的機(jī)器人末端終點(diǎn)坐標(biāo)為（3.021，5.844 mm），相對誤差在2%以內(nèi)，而到終點(diǎn)機(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ī)器人起始點(diǎn)坐標(biāo)下的位移信息;因?yàn)椴捎昧巳S模型，所以，對攝像機(jī)的安裝位姿無特殊要求。

本視覺里程計是基于單目視覺的，由圖像特征點(diǎn)還原到攝像機(jī)坐標(biāo)系下的點(diǎn)時，采用點(diǎn)的深度信息是一個固定值，因此，該視覺里程計只適用于較為平坦的室內(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.