汪兆永，何炳蔚

(福州大學(xué) 機械工程及自動化學(xué)院，福建 福州 350108)

0 引言

近年來，移動機器人技術(shù)迅速發(fā)展，在未知環(huán)境中實現(xiàn)自主導(dǎo)航是移動機器人的一項重要任務(wù)，自主導(dǎo)航也一直是機器人研究的熱點問題。特別是在未知環(huán)境下的導(dǎo)航問題，機器人在快速自定位和獲取周圍環(huán)境信息方面仍然存在很大困難。學(xué)者們針對SLAM 領(lǐng)域展開了大量的研究，提出了許多研究方法。比較成熟的SLAM方法有基于激光傳感器、聲吶傳感器等獲取環(huán)境信息［1］，但是得到的信息量較小，所需探測環(huán)境時間較長，對環(huán)境條件要求較高，不適用于復(fù)雜的環(huán)境。而視覺傳感器系統(tǒng)具有信息量大，性價比高等優(yōu)點，隨著計算機圖像處理技術(shù)能力的提高，學(xué)者們開展了大量基于視覺傳感器的SLAM 相關(guān)研究，也表明了視覺傳感器具有很好的發(fā)展前景和使用價值。基于視覺傳感器的自定位研究主要有單目視覺、雙目立體視覺系統(tǒng)、全景視覺和單目全景混合視覺系統(tǒng)［2］，存在的主要問題是連續(xù)性不足、系統(tǒng)和處理算法較為復(fù)雜。

2010 年微軟公司開發(fā)的Kinect 傳感器，結(jié)合了普通攝像頭和激光傳感器，功能強大，具有較高的性價比，同時可獲得環(huán)境顏色信息和深度值。其中，獲取的環(huán)境深度值具有連續(xù)性、信息量豐富、受光線影響小等特點。因此，被廣泛運用于機器人視覺研究領(lǐng)域［3］。在機器人導(dǎo)航研究中，若單獨采用普通視覺系統(tǒng)則存在獲取環(huán)境深度信息算法復(fù)雜的缺陷，而單獨采用激光傳感器則在垂直方向上的視角限制大，且無法感知環(huán)境顏色信息，如果能夠很好的把普通攝像頭和激光傳感器結(jié)合起來運用于機器人導(dǎo)航過程中，提高獲取信息的效率和實時性，對于移動機器人研究領(lǐng)域具有重大的意義。但是，目前Kinect 研究尚處于起步階段，它的缺點是獲得的深度數(shù)據(jù)一定程度上受到噪聲的影響，且獲取深度信息的環(huán)境距離也存在限制，應(yīng)用在機器人自主導(dǎo)航中還存在許多困難。

本文主要開展基于Kinect 傳感器的室內(nèi)機器人自定位研究，首先用Kinect 傳感器采集環(huán)境的顏色和深度信息，然后采用SIFT 匹配方法獲取匹配特征點和RANSAC方法去除誤匹配，最后通過ICP 算法計算，不斷更新機器人運動參數(shù)，從而更新機器人位姿。比較采用其他視覺傳感器進(jìn)行機器人自定位的方法，本文方法僅用Kinect 傳感器便可同時獲得環(huán)境顏色和深度信息，引入的RANSAC方法可去除不可靠的匹配點，算法簡單，設(shè)備簡易且機器人自定位精度較高。

1 KINECT 簡介

Kinect(深度測量傳感器)傳感器是由紅外線發(fā)射器(IR projector)和紅外攝像頭(IR camera)組合而成的3D結(jié)構(gòu)光深度測量傳感器［4］，可用于測量空間三維點數(shù)據(jù)。Kinect 既含有普通RGB 攝像頭，又含有深度攝像頭，如圖1。作為一個深度測量傳感器，Kinect 具有三個輸出:IR 圖像、RGB 圖像、深度信息。

圖1 Kinect 傳感器

1)Kinect 原理介紹

IR 攝像頭接收和解碼IR 發(fā)射器在空間投影的IR 三維點信息，生成IR 圖像。RGB 圖像是由一個標(biāo)準(zhǔn)的分辨率為640 ×480 攝像頭拍攝的圖像。攝像頭的水平視角為57°，垂直視角為43°，獲取圖像頻率是30 fps。通過標(biāo)定獲取攝像頭的內(nèi)參數(shù)，從而將RGB 圖像和IR 圖像特征點集的像素值和深度值(x，y，d)轉(zhuǎn)化為對應(yīng)的空間三維數(shù)據(jù)(x，y，z)。

Kinect 傳感器主要的作用是輸出場景對應(yīng)的深度信息，返回“深度倒數(shù)”值d，而不是直接返回實際深度值z。由紅外線發(fā)射器和紅外攝像頭的組合可獲取深度圖像素值和深度信息，如式(1)。

式中:(x，y)為深度圖像像素坐標(biāo)，(u，v)為IR 圖像像素坐標(biāo)，(u0，v0)為IR 攝像頭焦點坐標(biāo)。

2)Kinect 標(biāo)定技術(shù)

本文采用張正友的方法［5］和matlab 標(biāo)定工具箱［6］標(biāo)定Kinect 普通RGB 攝像頭的內(nèi)參數(shù)。標(biāo)定模板為平面黑白棋盤格，棋盤格方格大小為40 mm ×40 mm，圖2 為用Kinect 普通RGB 攝像頭在5 個不同角度拍攝的棋盤圖像對。標(biāo)定的結(jié)果RGB 攝像頭內(nèi)參數(shù)為:

不考慮攝像頭畸變。

圖2 用于標(biāo)定普通攝像頭的圖片

2 SIFT 特征介紹

SIFT 特征匹配算法［7－8］能夠提取穩(wěn)定的特征，是指該特征能對旋轉(zhuǎn)、尺度、仿射、視角、光照等圖像變化因素保持一定的不變性。一般SIFT 匹配包括四個步驟:1)特征點檢測:Lowe［7］在圖像二維平面空間和DoG(Difference－of－Gaussian)尺度空間中同時檢測局部極值以作為特征點，DoG 算子定義為兩個不同尺度的高斯核的差分，DoG 算子如式(3)所示。

在檢測尺度空間極值時，某個像素需要跟同一尺度周圍領(lǐng)域的8 個像素以及相鄰尺度對應(yīng)位置的領(lǐng)域9 ×2 個像素(總共26 個像素)進(jìn)行比較，以確保在尺度空間和2 維圖像空間都能檢測到局部極值。2)特征點描述，即建立特征向量:每個關(guān)鍵點由2 ×2 共4 個種子點構(gòu)成，每個種子點包含8 個方向向量信息。為了增強匹配穩(wěn)定性，Lowe［7］對每個關(guān)鍵點采用4 ×4 共16 個種子點來描述，這樣對于每個關(guān)鍵點就可以產(chǎn)生128 個數(shù)據(jù)，即最后每個關(guān)鍵點建立128 維的SIFT 特征向量。3)進(jìn)行特征匹配以獲取候選匹配點:當(dāng)兩幅圖像的SIFT 特征向量生成后，下一步采用關(guān)鍵點特征向量間的歐式距離來作為兩幅圖像中關(guān)鍵點的相似性判定度量。取兩幅圖像歐式距離最近的前兩對關(guān)鍵點，在這兩對關(guān)鍵點中，如果最近的距離除以次近的距離少于某個比例閾值，則接受這一對匹配點。4)消除誤匹配:本文采用RANSAC 算法［9］去除外點，RANSAC 算法是通過多次隨機抽取一定的樣本對參數(shù)進(jìn)行估計，先得出初始參數(shù)模型，然后根據(jù)估計參數(shù)將所有數(shù)據(jù)進(jìn)行分類，一部分?jǐn)?shù)據(jù)在設(shè)定誤差范圍內(nèi)，則稱為內(nèi)點，反之稱為外點，經(jīng)過多次迭代計算出最優(yōu)模型參數(shù)。

本文在采用SIFT 匹配的同時，引入RANSAC 去除誤匹配，提高匹配點可靠性。若不采用RANSAC 方法消除誤匹配，也可以完成SIFT 特征點匹配，但是當(dāng)存在較多不可靠匹配點時，對于運動參數(shù)計算存在很大影響，從而影響機器人自定位精度，RANSAC 方法相對于其他方法僅需兩幅圖像顏色信息便可消除誤匹配，且能穩(wěn)定并準(zhǔn)確地去除誤匹配，提高機器人運動參數(shù)的計算精度。

3 迭代就近點法(ICP 算法)

計算機視覺研究者Besl 和Mckay［10］介紹了一種高層次的基于自由形態(tài)曲面的配準(zhǔn)方法，也稱為迭代就近點法ICP(Iterative Closest Point)。ICP 算法可以由兩幅不同RGB 圖像或者深度圖像的對應(yīng)點云數(shù)據(jù)計算兩者之間的變換矩陣(R，T)。迭代就近點法ICP 算法經(jīng)過十幾年的發(fā)展，不斷地得到了完善和補充。本文采用奇異值分解算法(SVD)來求解變換矩陣(R，T)，因為采用SVD 算法計算相對簡單［11］，本節(jié)將闡述采用的ICP 算法，如何通過SVD 算法獲得特征點對的最優(yōu)匹配參數(shù)。

1)匹配點搜索

采用上節(jié)提到的SIFT 特征匹配算法分別提取兩幅圖像的特征點，通過計算兩圖像間特征點歐幾里德距離來確定特征點是否為對應(yīng)的匹配點，經(jīng)過SIFT 匹配后得到一組匹配點對。用Kinect 傳感器可以獲取對應(yīng)匹配點對的深度信息，將匹配點對的坐標(biāo)歸一化后為x=［u，v，1］T，然后由已標(biāo)定的RGB 攝像頭內(nèi)參數(shù)對歸一化坐標(biāo)進(jìn)行轉(zhuǎn)換，獲得匹配點對在RGB 攝像頭坐標(biāo)系下三維坐標(biāo)x=［x，y，z］T，坐標(biāo)軸方向取為:x 軸豎直向上，y 軸水平向右，z 軸向前，計算如式(4)。

式中 (fx，fy)為標(biāo)定的RGB 攝像頭焦距，(u0，v0)為光軸中心的像素坐標(biāo)，本文計算不考慮攝像頭畸變的影響。

2)運動參數(shù)計算

目前大部分運動參數(shù)計算都是采用最小二乘法，本文采用奇異值分解算法(SVD)來求解變換矩陣(R，T)，算法相對簡單。運動參數(shù)是由3 ×3 旋轉(zhuǎn)矩陣R 和1 ×3 平移向量T 構(gòu)成，可以由式(5)求解:

式中:x1表示第一幅圖像SIFT 特征匹配點集在RGB 攝像頭坐標(biāo)系下的三維坐標(biāo)，x2表示第二幅圖像中與x1對應(yīng)的SIFT 特征點集三維坐標(biāo)，k 表示兩幅圖像經(jīng)過SIFT 匹配、RANSAC 去除誤匹配后剩余的匹配點對數(shù)。

則可以由公式(7)求得優(yōu)化解旋轉(zhuǎn)矩陣R 和平移向量T:

已經(jīng)有很多種方法計算R，T 參數(shù)，上述方法比較簡單且相對穩(wěn)定。求解出R，T 后，可計算x2經(jīng)過R，T 轉(zhuǎn)換后的坐標(biāo))，然后與x1比對重建誤差，當(dāng)計算得出的與x1的距離小于閾值，則計算結(jié)束，R，T 符合要求，否則去除誤差大于閾值的對應(yīng)點后，返回式(4)重新計算。通過這個方法可以計算出兩組三維數(shù)據(jù)點之間機器人的運動參數(shù)。

4 自定位方法的實現(xiàn)

上節(jié)中描述了SIFT 匹配，RANSAC 去除誤匹配，以及ICP 算法。本節(jié)將提出如何結(jié)合前述的方法解決機器人自定位問題。本方法將由3 部分構(gòu)成:關(guān)鍵點檢測，去除外點，計算運動參數(shù)。

1)關(guān)鍵點檢測

獲取兩幅圖像的SIFT 匹配點對及其對應(yīng)的深度信息，具體步驟是先用SIFT 方法在圖像Image1 和圖像Image2 中分別提取SIFT 特征點集Data1、Data2，然后用Kinect 傳感器提取兩幅圖像SIFT 特征點集對應(yīng)的深度信息depth1、depth2，最后對兩幅圖像采用SIFT 進(jìn)行匹配，獲得初始匹配點對。

2)外點去除

由于SIFT 提取的誤匹配點對于后續(xù)參數(shù)的計算影響很大，故采用RANSAC 方法進(jìn)一步去除初始匹配點對中的誤匹配點，從而提高參數(shù)的計算精度。

3)計算外參數(shù)

去除匹配外點后，剩余兩組為正確的對應(yīng)匹配點集，然后由匹配點集的像素坐標(biāo)和深度信息轉(zhuǎn)為攝像頭坐標(biāo)系下匹配點集的三維坐標(biāo)。假定Kinect 第一個方位拍攝圖像的特征點集三維坐標(biāo)為x1，其對應(yīng)第二個方位拍攝圖像的特征點集三維坐標(biāo)為x2，由x1、x2采用ICP 算法計算兩個方位下機器人的運動參數(shù)R、T。設(shè)定機器人初始方位為坐標(biāo)原點，機器人移動到第三個方位時，以第二個方位Kinect 拍攝圖像的特征點三維坐標(biāo)作為新的x1，以第三個方位的拍攝圖像的特征點三維坐標(biāo)作為新的x2，以新的x1、x2計算機器人第二個方位和第三方位的相對運動參數(shù)R、T，依次迭代就可以對機器人進(jìn)行自定位。此方法的流程圖如圖3。

圖3 提出方法的流程圖

5 實驗

實驗采用的是AS－R 移動機器人，在其上安裝一個Kinect 傳感器，如圖4。Kinect 傳感器的普通攝像頭內(nèi)參數(shù)已經(jīng)事先標(biāo)定好，用Kinect 傳感器在機器人兩個方位下拍攝兩幅RGB 圖像，分別提取并保存兩幅圖像SIFT特征點及對應(yīng)的深度信息。對兩幅圖像采用SIFT 匹配如圖5 所示，初始共有181 對匹配特征點，其中包含很多錯誤匹配，然后經(jīng)過RANSAC 算法估計篩選出正確匹配特征點對數(shù)101 對，如圖6 所示。實驗設(shè)定匹配點重建后兩點距離誤差閾值為dis=30 mm，輸入機器人里程計往前直線運行距離d=250 mm。剔除重建后與x2的距離大于閾值dis 的x2點，并重新計算運動參數(shù)R、T。采用本文方法的實驗計算結(jié)果:機器人兩方位相對旋轉(zhuǎn)矩陣:

平移向量T=[7.074 5 27.090 7 253.890 3 ]T，重建誤差小于閾值，則計算結(jié)束。通過計算得出機器人往前運行距離為dc=255.429 5 mm，與機器人自帶里程計比較相差Δd=5.429 5 mm，考慮到機器人自身運行過程中輪子打滑、Kinect 傳感器獲取深度信息時本身精度及噪聲影響等誤差的存在，所以實驗結(jié)果存在一定誤差。實驗結(jié)果驗證了本文求機器人運動參數(shù)R、T 方法的可行性。

圖4 安裝有Kinect 的AS-R 機器人

圖5 用SIFT 對兩個方位下拍攝的圖像進(jìn)行初始匹配

圖6 采用RANSAC 方法去除誤匹配后最終匹配效果

6 結(jié)語

本文采用Kinect 傳感器對室內(nèi)機器人進(jìn)行定位研究，因為Kinect 傳感器同時具備普通RGB 攝像頭和深度傳感器的優(yōu)點，所以本文的方法較僅采用普通視覺系統(tǒng)的方法性價比高，設(shè)備及算法簡單，計算精度高，因此具有推廣價值。

本方法的不足是Kinect 傳感器提取深度信息受噪聲影響，這會影響計算運動參數(shù)的精度，同時Kinect 傳感器深度測量的距離有限，因此很多方面仍需要進(jìn)一步研究。

［1］Kim Ho Duck，Seo San wook，Jan Inhun.SLAM of Mobile Robot in the Indoor Environment with Digital Magnetic Compass and Ultrasonic Sensors［C］.International Conference on Control Automation and Systems in COEX，Seoul，Korea，2007.

［2］He Bingwei，Chen Zhipeng.Determination of the Common View Field in Hybrid Vision System and 3D Reconstruction Method.Robot，vol.33(5)，pp.614-620，September 2011.

［3］Takeda Y，Aoyama N，Tanaami T，et al.Study on the Indoor SLAM Using Kinect［M］.Advanced Methods，Techniques，and Applications in Modeling and Simulation.Springer Japan，2012:217-225.

［4］K.Konolige，P.Mihelich.Technical description of Kinect calibration.http://www.ros.org/wiki/kinect_calibration/technical，2011.1，3.

［5］Zhengyou Zhang.A flexible new technique for camera calibration.IEEE Transactions on Pattern Analysis and Machine Intelligence，vol.22(11)，pp.1330-1334，2002.

［6］J.Y.Bouguet.Camera calibration toolbox.http://www.vision.caltech.edu/bouguetj/calib_doc/，2010.

［7］Lowe D G.Object recognition from local scale-invariant features.IEEE International Conference on Computer Vision，Piscataway，NJ，USA，pp.130-132，1999.

［8］Yuquan Wang，Guihua Xia，Qidan Zhu，et al.Modified Scale Invariant Feature Transform in Omnidirectional Image.Proceedings 2009 IEEE International Conference on Mechatronics and Automation (ICMA)，pp.2632-2636，2009.

［9］Wang Wei，Hong Jun，Tang Yiping.Image matching for geomorphic measurement based on sift and RANSAC methods［C］.International Conference on Computer Science and Software Engineering.Wuhan，Hubei，IEEE，2008，317-320.

［10］Paul J.Besl，Neil D.McKay.A method for registration of 3-D shapes［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，1992.14(2):239-256.

［11］Eggert，D.W.，Lorusso，A.，F(xiàn)isher，R.B..Estimating 3-D rigid body transfor-mations:a comparison of four major algorithms.Machine Vison and Applications 9，272-290 (1997).