, ,

(浙江工業(yè)大學 信息工程學院，浙江 杭州 310023)

基于RGB-D相機的視覺里程計實現(xiàn)

仇翔,王強,俞立

(浙江工業(yè)大學 信息工程學院，浙江 杭州 310023)

針對移動機器人在未知環(huán)境中的自身定位問題，提出了一種基于RGB-D相機的移動機器人運動軌跡估計方法.首先，提取當前圖像的ORB特征并與關(guān)鍵幀進行特征匹配；然后，采用結(jié)合特征匹配質(zhì)量和深度信息的PROSAC算法對幀間運動進行迭代估計；最后，提取關(guān)鍵幀并利用g2o求解器進行局部優(yōu)化，得到關(guān)鍵幀位姿的最優(yōu)估計，進而得到機器人的運動軌跡.實驗結(jié)果表明：與RANSAC+ICP算法相比，該方法能有效提高移動機器人的定位精度.

視覺里程計；ORB；PROSAC；關(guān)鍵幀

視覺里程計(Visual odometry, VO)[1]作為視覺SLAM[2]的前端，一直被廣泛的研究.它是通過搭載在移動機器人上的相機獲取的一系列連續(xù)視覺信息來推算出機器人的相對運動.和傳統(tǒng)輪式里程計(Wheel odometry)相比，視覺里程計不存在車輪打滑而造成的誤差，適用于非平面運動環(huán)境和非輪式移動機器人[3]，并且能夠獲取更多的環(huán)境信息來用于三維重建[4]、伺服控制[5]和目標跟蹤[6]等，具有廣泛的應(yīng)用前景.

隨著微軟和華碩相繼推出成本低廉且能夠同時采集色彩和深度信息的相機(RGB-D相機)，利用該類型相機實現(xiàn)的VO得到廣泛的應(yīng)用.基于RGB-D相機的VO研究方法一般分為兩類：一類是基于稀疏特征點的方法，另一類是稠密方法.基于稀疏特征點的方法是根據(jù)SIFT，SURF，ORB等特征點的匹配來建立幀間聯(lián)系，Henry等[7]利用隨機采樣一致性(RANSAC)算法[8]剔除誤匹配并把計算得到的相對位姿結(jié)果作為迭代最近點算法(Iterative closest point, ICP)[9]的迭代初值，提高了幀間運動估計的精確度.但是由于由空間三維點組成的點云的迭代計算的數(shù)據(jù)量大，需要較高的硬件成本.Dryanovski等[10]考慮了像素點深度的不確定性并建立高斯混合模型，采用幀到模型的特征點云配準方法，并利用卡爾曼濾波對模型進行動態(tài)更新，該方法的實時性高，能夠在室內(nèi)小范圍實現(xiàn)閉環(huán)而不需要額外的SLAM后端技術(shù)，但無法應(yīng)用于大范圍環(huán)境.另外，從深度信息中提取的平面信息具有很好的特性，Pathak等[11]提出了一種基于平面的ICP配準算法，高翔等[12]在提取的平面上進行特征點匹配，很大程度上減少了誤匹配的發(fā)生，但此方法應(yīng)用環(huán)境較為局限.Silva等[13]利用光流法來追蹤當前幀的特征點，相比于在兩幀上分別提取特征點后進行匹配的方法計算速度更快，但在運動速度較快時方法容易失效.稠密方法是基于圖像一致性假設(shè)，相比于稀疏特征點方法，它能夠充分利用圖像的像素信息，最早由Steinbruecker等[14]提出的，由于數(shù)據(jù)量大，需要利用GPU加速技術(shù)實現(xiàn)[15].

綜上所述，目前已有的視覺里程計方法大多精度不夠或者對硬件要求高.筆者提出了一種基于RGB-D相機的視覺里程計實現(xiàn)方法,利用融合特征匹配質(zhì)量和深度信息的改進隨機采樣一致性(PROSAC)算法[16]快速估計幀間運動，采用g2o(General graph optimization)[17]求解器對關(guān)鍵幀進行局部優(yōu)化，提高了定位精度，并且由于計算復(fù)雜度不高，對硬件的要求較低，最后通過實驗驗證該方法的有效性.

1 問題描述

1.1 相機模型

對于一般的相機，可以采用針孔相機模型，如圖1所示，任意空間點P=(x,y,z,1)T的坐標可以由它在圖像上投影點p=(u,v)T和該點的深度d=d(p)通過逆向投影函數(shù)π-1得到

(1)

式中：fx,fy為相機在x,y兩個軸上的焦距；cx,cy為相機的光圈中心；s為深度圖的縮放因子，這些參數(shù)可以事先通過標定得到.

圖1 針孔相機模型Fig.1 The model of pinhole camera

同理，通過正向投影得到空間點的像素坐標為

(2)

1.2 幀間相機運動模型

當相機運動時，所采集到的圖像也會發(fā)生相應(yīng)變化，根據(jù)相同特征點的關(guān)聯(lián)性，可以計算出前后兩幀間的相機相對位姿，并表示為T4×4∈SE(3)，即

(3)

式中：R∈SO(3)為旋轉(zhuǎn)矩陣；t為平移向量.

(4)

令e=0，根據(jù)3個匹配好的特征點對，可以求解出Tk-1,k的四個可能值，然后利用第四特征點對來消除歧義，得到幀間相對運動Tk-1,k.

根據(jù)幀間運動模型計算得到的Tk-1,k的依次遞推，可以得到移動機器人當前位姿，進而解決機器人的定位問題.

圖2 幀間運動模型Fig.2 The motion model of inter-frame

2 視覺里程計算法流程

為了實現(xiàn)移動機器人的精確定位，設(shè)計了視覺里程計系統(tǒng)，如圖3所示.

圖3 視覺里程計系統(tǒng)框架Fig.3 The system framework of VO

視覺里程計系統(tǒng)的流程：1)提取當前幀圖像ORB特征點，并與當前關(guān)鍵幀進行特征匹配；2)根據(jù)相機模型，將二維特征像素點坐標投影成空間三維點坐標，并采用結(jié)合特征匹配質(zhì)量和深度信息的PROSAC算法對幀間運動進行迭代估計；3)提取關(guān)鍵幀，建立圖優(yōu)化模型，并采用g2o求解器進行局部優(yōu)化，得到關(guān)鍵幀的最優(yōu)估計.具體實現(xiàn)如下.

2.1ORB特征提取與匹配

ORB特征是在FAST角點檢測算法和BRIEF特征描述算法基礎(chǔ)上改進而來的，相比于SIFT和SURF，其實時性更高.ORB特征提取可以分為以下兩步進行：

θ=atan2(m01,m10)

(5)

然后，構(gòu)建具有旋轉(zhuǎn)不變性的rBRIEF(rotated BRIEF)描述子.BRIEF描述子自身不具有方向信息，為其添加特征點的主方向θ來解決旋轉(zhuǎn)不變性問題.BRIEF描述子是以二進制比特串的方式存儲的，對于任意n個二進制準則特征集，定義了一個2n矩陣為

(6)

那么具有旋轉(zhuǎn)不變性的BRIEF描述子為

gn(p,θ)=fn(p)|(xi,yi)∈Sθ

(7)

ORB特征是否匹配是根據(jù)rBRIEF描述子的Hamming距離來判斷的，根據(jù)快速最近鄰算法FLANN檢測得到最近鄰點和次近鄰點，令特征點與最近鄰點的Hamming距離為d1，與次近鄰點的Hamming距離為d2，當距離比率γ=d1/d2小于給定閾值時，匹配成功，否則失敗.且γ越小，匹配程度越高.

2.2 幀間相機運動估計

在前后兩幀中提取特征點后，利用匹配好的特征點對，根據(jù)幀間運動模型求解出相對位姿變換.但是由于特征點對存在誤匹配情形，需要排除這些誤匹配的影響，來得到最優(yōu)的幀間運動估計.

筆者采用基于PROSAC算法的P3P模型來估計幀間運動.PROSAC算法是隨機采樣一致性算法的改進，兩者的區(qū)別在于RANSAC算法對采樣點等概率采樣，而PROSAC算法根據(jù)采樣點與模型的匹配程度進行排序，匹配質(zhì)量高的優(yōu)先采樣，因此算法的實時性和穩(wěn)定性更高.

考慮到RGB-D相機的深度信息具有一定的誤差，且誤差值隨著采樣點深度值d增加而加大，近處的特征匹配點更加可靠.因此，筆者提出了一種融合深度信息d和特征點對匹配程度γ的排序算法，設(shè)特征匹配點可信度函數(shù)為

q=1/(αγ+βd)

(8)

式中：α，β為比例因子.

基于特征匹配點可信度的PROSAC算法步驟：

1)根據(jù)q從高到低對特征匹配點進行排序.

2)設(shè)置初值，包括內(nèi)點誤差閾值、內(nèi)點數(shù)量閾值和最大迭代次數(shù).

3)根據(jù)抽樣次數(shù)t和假定樣本集合增長函數(shù)n=g(t)來確定樣本集M.

4)從M中選取4個特征匹配點，利用幀間運動模型計算得到模型參數(shù)，利用其余所有特征匹配點進行模型驗證，得到滿足該模型的內(nèi)點集.

5)判斷是否滿足循環(huán)終止條件，若否，則循環(huán)執(zhí)行3)～5)，反之循環(huán)結(jié)束.循環(huán)終止條件：迭代次數(shù)達到最大迭代次數(shù)；最大內(nèi)點集中內(nèi)點數(shù)量大于內(nèi)點數(shù)量閾值；k次采樣后的內(nèi)點數(shù)與k-1次抽樣后得到的內(nèi)點數(shù)沒有增加.

得到最大內(nèi)點集后，利用該集合中所有特征匹配點，重新利用非線性最小二乘法計算幀間相機運動的最優(yōu)估計.

2.3 關(guān)鍵幀選取與局部優(yōu)化

由于幀間運動估計不可避免存在一定的誤差，為了減輕累積誤差對移動機器人定位精度的影響，將采用基于關(guān)鍵幀提取和局部優(yōu)化的算法.

圖4給出了該算法的基本表示，nt表示當前幀，Ki表示關(guān)鍵幀，每個當前幀都是和之前最近的關(guān)鍵幀進行幀間匹配得到自身位姿狀態(tài)，而不是相鄰幀.關(guān)鍵幀提取方法一般根據(jù)當前幀與前一關(guān)鍵幀匹配時的內(nèi)點數(shù)和運動大小來選定，當內(nèi)點數(shù)小于設(shè)定閾值或者運動大于設(shè)定閾值時，則認為機器人位姿變化較大，而把當前幀設(shè)為關(guān)鍵幀.

圖4 關(guān)鍵幀提取與局部優(yōu)化Fig.4 Key frame extraction and local optimization

在系統(tǒng)運行過程中，若檢測到新的關(guān)鍵幀，則將該幀與關(guān)鍵幀序列中后m個關(guān)鍵幀進行匹配，匹配是否成功取決于兩幀間的特征匹配點數(shù)是否大于設(shè)定閾值.若大于則可以建立新的約束，實現(xiàn)局部的位姿優(yōu)化，提高系統(tǒng)的定位精度.將該問題轉(zhuǎn)換為圖優(yōu)化模型，建立模型的誤差函數(shù)為

(9)

式中：K為所選取的關(guān)鍵幀位姿狀態(tài)集合；Ki,Kj∈K；Ti,j為Ki和Kj間狀態(tài)轉(zhuǎn)移矩陣；Ωi,j為信息矩陣；e(Ki,Kj,Ti,j)為由Ki和Kj組成的邊所產(chǎn)生的誤差.

g2o是通用的圖優(yōu)化求解器，其中包括Gauss-Newton，Levenberg-Marqudart(LM)等非線性最小二乘求解算法.對于筆者所建立的圖優(yōu)化模型，將關(guān)鍵幀的位姿設(shè)置為頂點，關(guān)鍵幀之間的約束設(shè)置為邊，在g2o求解器中采用LM算法最小化f(K)可以得到關(guān)鍵幀位姿的局部最優(yōu)估計.

3 實 驗

3.1 實驗環(huán)境

實驗采用的是華碩的XtionPRO LIVE深度相機，圖像分辨率為640×480，深度量程為0.8～3.5 m,采樣頻率為30 Hz.計算機配置為AMD A6-3400M處理器，主頻為1.40 GHz，內(nèi)存4.00 GB，操作系統(tǒng)為64 位Ubuntu14.04.使用的移動機器人為Kuka公司的Youbot移動機器人，如圖5所示，另外它上面搭載了激光傳感器，可以利用激光定位得到更高精度的定位結(jié)果作為真實軌跡.

圖5 實驗機器人Fig.5 The experimental robot

3.2 幀間匹配內(nèi)點數(shù)對比

基于稀疏特征點的方法，幀間匹配內(nèi)點數(shù)是反映計算準確性的重要指標.在相同的ORB特征提取和FLANN匹配的參數(shù)設(shè)置下，對比了分別利用RANSAC+ICP算法和筆者算法對采集到的數(shù)據(jù)處理得到的內(nèi)點數(shù)，如圖6所示.其中筆者方法中匹配程度函數(shù)兩個系數(shù)分別設(shè)置為α=1，β=0.2.從圖6可知：滿足筆者算法得到的幀間運動模型的特征匹配點的數(shù)量更多.

圖6 幀間匹配內(nèi)點數(shù)Fig.6 The number of inliers between inter-frame matching

3.3 直線運動實驗

本實驗是在Youbot移動機器人平臺上進行的，其直線運動的速度為0.2 m/s，運動距離為2.8 m.分別利用RANSAC+ICP算法[9]和筆者算法估計移動機器人運動軌跡，如圖7所示.實驗結(jié)果表明：在直線運動中，筆者算法平均定位誤差為1.03%，相比于RANSAC+ICP算法的3.57%誤差更小.

圖7 直線運動軌跡Fig.7 The trajectory of linear motion

3.4 圓周運動實驗

本實驗移動機器人進行圓周運動，其線速度為0.2 m/s，角速度為0.2 rad/s，運動半徑為1 m，運動距離為6.28 m.實驗結(jié)果如圖8所示，RANSAC+ICP算法平均的定位誤差為8.30%，而筆者算法的平均定位誤差為2.68%.

圖8 圓周運動軌跡Fig.8 The trajectory of circular motion

3.5 實驗分析

實驗表明：與RANSAC+ICP算法相比，筆者算法的定位精度更高，通過比較和分析，原因有兩點：1) 通過對特征匹配點進行可信度排序，優(yōu)先采樣可信度高的特征匹配點，用更少的迭代次數(shù)獲得最佳的幀間運動估計；2) 通過提取關(guān)鍵幀并對關(guān)鍵幀進行局部的優(yōu)化，可以盡量減輕累積誤差對定位精度的影響.

4 結(jié) 論

利用RGB-D相機獲得的圖像和深度信息，提出了一種移動機器人視覺里程計實現(xiàn)方法.根據(jù)ORB特征點建立幀間聯(lián)系，采用融合匹配質(zhì)量和深度信息的PROSAC算法求解P3P模型并得到幀間運動估計，結(jié)合關(guān)鍵幀提取和g2o求解器進行局部狀態(tài)優(yōu)化，實現(xiàn)了移動機器人較高精度的自主定位.實驗證明：該算法具有可行性和有效性.考慮到系統(tǒng)運行過程中仍存在誤差累積，下一步工作將會考慮全局優(yōu)化問題，將該方法應(yīng)用到視覺SLAM中，構(gòu)建具有全局一致性的運動軌跡和地圖模型.

[1] SCARAMUZZA D, FRAUNDORFER F. Visual odometry: partⅠ: the first 30 yearccs and fundamentals[J]. IEEE robotics & automation magazine,2011,18(4):80-92.

[2] DURRANTWHYTE H, BAILEY T. Simultaneous localization and mapping: partⅠ[J]. IEEE robotics & automation magazine,2006,13(2):99-110.

[3] 曹美會，鮮斌，張旭，等.基于視覺的四旋翼無人機自主定位與控制系統(tǒng)[J].信息與控制,2015,44(2):190-196.

[4] 管秋，金俊杰，張劍華，等.基于最優(yōu)RANSAC算法的非增加式多視圖三維重建[J].浙江工業(yè)大學學報,2015,43(5):473-478.

[5] 孫建輝，劉坤.基于序列圖像特征識別實時反饋伺服跟蹤研究[J].浙江工業(yè)大學學報，2015，43(2):185-189.

[6] 楊東勇，馮形松.采用形態(tài)學算子的運動目標檢測跟蹤方法研究及實現(xiàn)[J].浙江工業(yè)大學學報,2010,38(2):149-154.

[7] HENRY P, KRAININ M, HERBST E, et al. RGB-D mapping: using depth cameras for dense 3D modeling of indoor environments[J]. International journal of robotics research,2014,31(5):647-663.

[8] FISCHLER M A, BOLLES R C. Random sample consensus: a paradigm for model fitting with application to image analysis and automated cartography[J]. Communications of the association for computing machinery,1981,24(6):381-395.

[9] BESL P J, MCKAY N D. Method for registration of 3-D shapes[J]. IEEE transactions on pattern analysis & machine intelligence,1992,14(2):239-256.

[10] DRYANOVSKI I, VALENTI R G, XIAO J Z. Fast visual odometry and mapping from RGB-D data[C]//2013 IEEE International Conference on Robotics and Automation. Piscataway: IEEE,2013:2305-2310.

[11] PATHAK K, BIRK A, VASKEVICIUS N, et al. Fast registration based on noisy planes with unknown correspondences for 3-D mapping[J]. IEEE transactions on robotics,2010,26(3):424-441.

[12] GAO X, ZHANG T. Robust RGB-D simultaneous localization and mapping using planar point features[J]. Robotics & autonomous systems,2015(72):1-14.

[13] SILVA B M F, GONCALVES L M G. A fast visual odometry and mapping system for RGB-D cameras[C]//Joint Conference on Robotics: SBR-LARS Robotics Symposium and Robocontrol. Piscataway: IEEE,2014:55-60.

[14] STEINBRUCKER F, STURM J, CREMERS D. Real-time visual odometry from dense RGB-D images[C]//IEEE International Conference on Computer Vision Workshops. Piscataway: IEEE,2011:719-722.

[15] WHELAN T, JOHANNSSON H, KAESS M, et al. Robust real-time visual odometry for dense RGB-D mapping[C]//IEEE International Conference on Robotics & Automation. Piscataway: IEEE,2013:5724-5731.

[16] CHUM O, MATAS J. Matching with PROSAC-progressive sample consensus[C]//IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Piscataway: IEEE,2005:220-226.

[17] KUMMERLE R, GRISETTI G, STRASDAT H, et al. G2o: a general framework for graph optimization[C]//IEEE International Conference on Robotics & Automation. Piscataway: IEEE,2011:3607-3613.

RealizationofvisualodometrybasedonRGB-Dcamera

QIU Xiang, WANG Qiang, YU Li

(College of Information Engineering, Zhejiang University of Technology, Hangzhou 310023, China)

Aiming at the problem of localization of mobile robot in unknown environment, a method to estimate the trajectory of robot based on RGB-D camera was proposed. Firstly, ORB(oriented FAST and rotated BRIEF) features were extracted in current frame and matched with key frame. Secondly, combined the quality of feature matching with depth information, the PROSAC(Progressive sample consensus) algorithm was adopted to iteratively estimate inter-frame motion. Finally, key frame was detected and g2o(general graph optimization) solver was used to make local optimization to get the best pose estimation of key frame, and then the trajectory of robot was gained. The result shows that this method can effectively improve the positioning accuracy of mobile robot compared with RANSAC+ICP algorithm.

visual odometry; ORB; PROSAC; key frame

2017-02-22

浙江省自然科學基金重點項目(LZ15F030003)

仇 翔(1980—)，男，浙江湖州人，講師，研究方向為先進控制理論、機器人控制技術(shù)，E-mail: qiuxiang@zjut.edu.cn.

TP24

A

1006-4303(2017)06-0634-05

(責任編輯：陳石平)