基于RGB-D相機的視覺里程計實現

, ,

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

基于RGB-D相機的視覺里程計實現

仇翔,王強,俞立

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

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

視覺里程計；ORB；PROSAC；關鍵幀

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

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

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

1 問題描述

1.1 相機模型

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

(1)

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

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

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

(2)

1.2 幀間相機運動模型

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

(3)

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

(4)

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

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

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

2 視覺里程計算法流程

為了實現移動機器人的精確定位，設計了視覺里程計系統，如圖3所示.

圖3 視覺里程計系統框架Fig.3 The system framework of VO

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

2.1ORB特征提取與匹配

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

θ=atan2(m01,m10)

(5)

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

(6)

那么具有旋轉不變性的BRIEF描述子為

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

(7)

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

2.2 幀間相機運動估計

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

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

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

q=1/(αγ+βd)

(8)

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

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

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

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

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

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

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

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

2.3 關鍵幀選取與局部優化

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

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

圖4 關鍵幀提取與局部優化Fig.4 Key frame extraction and local optimization

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

(9)

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

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

3 實 驗

3.1 實驗環境

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

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

3.2 幀間匹配內點數對比

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

圖6 幀間匹配內點數Fig.6 The number of inliers between inter-frame matching

3.3 直線運動實驗

本實驗是在Youbot移動機器人平臺上進行的，其直線運動的速度為0.2 m/s，運動距離為2.8 m.分別利用RANSAC+ICP算法[9]和筆者算法估計移動機器人運動軌跡，如圖7所示.實驗結果表明：在直線運動中，筆者算法平均定位誤差為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.實驗結果如圖8所示，RANSAC+ICP算法平均的定位誤差為8.30%，而筆者算法的平均定位誤差為2.68%.

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

3.5 實驗分析

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

4 結 論

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

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

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

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

[6] 楊東勇，馮形松.采用形態學算子的運動目標檢測跟蹤方法研究及實現[J].浙江工業大學學報,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—)，男，浙江湖州人，講師，研究方向為先進控制理論、機器人控制技術，E-mail: qiuxiang@zjut.edu.cn.

TP24

A

1006-4303(2017)06-0634-05

(責任編輯：陳石平)