基于RGB-D圖像的視覺SLAM算法研究

孟特 李富才 劉邦彥 張鳳生

摘要：針對主流的RGB-D SLAM系統精度較低并且僅生成稀疏點云地圖的問題，提出一種改進的SLAM系統。前端采用改進的ORB特征提取算法，改進特征點簇集的問題;后端綜合利用EPnP與ICP算法進行相機位姿優化，提高位姿估計精度;并增加稠密點云地圖構建線程，得到場景的稠密點云地圖，以用于機器人的導航與路徑規劃。在TUM數據集上，使用Kinect V2相機將改進的SLAM算法與ORB-SLAM2算法進行實驗比對，驗證了改進SLAM算法的綜合性能優于ORB-SLAM2算法。

關鍵詞：RGB-D SLAM;Kinect V2;EPnP;稠密點云地圖

中圖分類號：TP391??????? 文獻標志碼：A

視覺SLAM可從環境中獲取海量的、富于冗余的紋理信息，擁有超強的場景辨識能力。早期的視覺SLAM基于濾波理論，其非線性的誤差模型和巨大的計算量限制了它的實用性[1-2]。近年來，隨著具有稀疏性的非線性優化理論及深度相機技術、計算機性能與算法的進步，視覺SLAM得到了越來越廣泛的研究及應用[3-7]。視覺SLAM根據相機不同可分為單目SLAM（Monocular SLAM）、雙目SLAM（Binocular SLAM）和深度相機SLAM（RGB-D SLAM）三類，其中，RGB-D SLAM可直接獲得深度信息，得到了越來越多的應用。Endres等[8]最早提出了一套完整的RGB-D SLAM系統，能夠在較長軌跡和復雜環境中追蹤機器人位姿，具有較好的魯棒性和準確性。而ORB-SLAM系統只提供了單目接口[9]。ORB-SLAM2系統提供了單目、雙目和深度相機接口，全程使用ORB（Oriented FAST and Rotated BRIEF）特征[10]，相比于尺度不變特性（Scale Invariant Feature Transform， SIFT）算法[11]和加速穩健性特性（Speed-up Robust Feature Transform， SURF）算法[12]，在保證具備旋轉和尺度不變性的基礎上又具有良好的實時性，成為目前視覺SLAM的主流方案[13-15]。但該系統使用EPnP（Effective Perspective-n-Point）算法進行相機位姿估計，存在較大誤差;而且僅構建出了稀疏點云地圖，無法直接用于導航與路徑規劃。本文的研究旨在解決上述問題。

1 視覺SLAM系統框架

ORB-SLAM2系統具有跟蹤、局部建圖和回環檢測三個線程，在此基礎上，本文的SLAM系統增加了稠密點云地圖構建線程，系統框架如圖1所示。跟蹤線程采用ORB進行特征點提取，特征匹配時采用隨機采樣一致性（RANSAC）算法[16]，提高匹配的準確性，并利用PnP（Perspective-n-Point）算法對匹配成功的地圖點和彩色圖像進行位姿估計和優化[17-18]，計算出關鍵幀點云地圖;在局部建圖線程中通過對關鍵幀的處理生成稀疏點云地圖。回環檢測線程進行地圖的檢測，減少累積誤差，優化地圖;稠密地圖構建線程完成稀疏點云地圖到稠密點云地圖的構建。

2 RGB-D SLAM算法的改進

針對目前的RGB-D SLAM系統精度較低，且只能生成稀疏點云地圖的問題，本文利用RGB-D深度相機所獲得的深度灰度圖，基于四叉樹（Q_tree）算法改進ORB特征提取，以減少冗余特征點;綜合應用EPnP加ICP（Iterative Closest Point）的方法求解相機位姿，既保證求解的快速性，又因為引入3D-3D的ICP算法，提高求解的精度;此外，增加稠密點云地圖的構建。

2.1 基于四叉樹（Q_tree）算法改進ORB特征提取

ORB特征檢測算法由FAST（Features from Accelerated Segment Test）特征點檢測和BRIEF（Binary Robust Independent Elementary Feature）描述子構成。FAST是一種特征點檢測算法，FAST角點定義為：若某像素與其周圍鄰域內足夠多的像素點相差較大，則該像素可能是角點。以某一像素點P為圓心，以3個像素單位為半徑的圓上16個像素點與像素點P的差來進行角點提取。若圓周上有連續N個像素點的亮度大于Ip+T，或者小于Ip-T，則將該圓心設為候選角點。其中，Ip為圓心P的亮度值，T為閾值，一般為Ip的20%，N一般取值為12。

由于不涉及尺度，梯度等復雜運算，FAST檢測器速度非常快，缺點是不具有方向性和尺度不變性。為了克服這一缺點，ORB算法通過構建圖像金字塔來實現尺度不變性，而旋轉不變性通過灰度質心法[14]來實現。所謂質心是指以圖像塊的灰度值作為權重的中心，質心的計算步驟：

Step 1 在一個小的圖像塊B中，定義圖像的矩為：mpq=∑x，y∈BxpyqIx，y，p，q=0，1;

Step 2 通過矩找到B的質心C為：C=m10m00，m01m00;

Step 3 連接B的幾何中心O和質心C，得方向向量OC，故特征點的方向定義為：θ=tan-1（m01/m10）。

圖2為Q_tree區域分割和數據存儲的結構圖。Q_tree算法對提取到的FAST角點進行均勻的分割，方便后期的特征點匹配[15]，該算法改變了特征點的存儲方式，在保留了ORB提取算法快速性的同時改善了特征點簇集和冗余的問題。算法步驟如下：

Step 1 初始化父節點O，將圖像均勻分割為四個子區域（A，B，C，D），得到初始化的Q_tree結構;

Step 2 按照每個子區域的角點數量進行判斷，若角點數量小于或等于1，則停止劃分，否則繼續均勻的四分該子區域;

Step 3 重復Step 2，直到遞歸出的子區域存在的角點數小于或等于1，或達到期望提取的角點數;對于角點簇集的情況，通過非極大值抑制條件，只保留Harris響應值最大的較優角點，去除其他冗余點。

2.2 相機位姿估計

RGB-D相機可直接獲得環境的深度信息，可以根據特征匹配結果進行相機位姿估計。通常有兩種情況，一種是3D-2D問題，即已知環境的3D點與其在相機中的2D點投影來求解位姿;另一種為3D-3D問題，即用已經匹配好的兩組3D點進行位姿求解[19]。在SLAM問題中，常用EPnP算法來解決第一種問題，用ICP算法來解決第二種問題。但第一種情況沒有有效利用更多的匹配點信息，而ICP算法是通過在待匹配的目標點云M和源點云Q中，按一定的約束條件找到最鄰近點pi，qi，故該算法非常依賴待匹配目標點云初始的旋轉向量R與平移向量t，否則容易陷入局部最優迭代。為此，本文采用EPnP算法與ICP算法相結合，利用EPnP算法獲得目標點云較好的初值，由ICP算法有效利用匹配點信息。EPnP算法是利用四個非共面的控制點的加權和來組成新的坐標系，從而可用世界坐標系或相機坐標系下的四個控制點來表示世界坐標系或相機坐標系下的所有3D點。

假設匹配點在世界坐標系下的坐標為pwi，i=1，2，…，n，在相機坐標系下的坐標為pci，i=1，2，…，n;同時，每個控制點的坐標為ccj=xci，yci，zciT（j=1，2，3，4），則在世界坐標系下，匹配點與控制點的關系為

pwi=∑4j=1aijcwj，∑4j=1aij=1（1）

根據相機投影模型，有

siuivi1=Kpci=K∑4j=1aijccj（2）

其中，si為尺度因子，uiviT表示pci對應的2D投影坐標，K為相機內參矩陣，通過相機標定獲得。

將式（1）帶入式（2），矩陣形式為

Mx=0（3）

其中，待求量x是一個12維向量，代表控制點的坐標;M是2n×12的矩陣，其系數通過計算匹配點獲取。

滿足式（3）的x所有解的集合成為M的零空間，存在以下關系

x=∑Ni=1βivi（4）

其中，vi通過計算MTM的特征向量來求得，βi的求解與N的取值有關，N一般取1～4。

求解出βi后，根據式（4）可得到x，再將x帶入式（1）就可計算出匹配點在相機坐標系下的3D點。

接下來就變成了3D-3D問題，利用ICP算法進行求解。

（1）分別計算匹配點在世界坐標系和相機坐標系下的質心

pwo=1n∑4i=1pwi， pco=1n∑4i=1pci

（2）計算pwi的去質心pwo后的矩陣A，計算pci的去質心pco后的矩陣B

A=pwTo-pwTopwTnpwTn B=pcTo-pcTopcTnpcTn

（3）計算本質矩陣H：H=BTA;

（4）計算H的SVD分解

H=U∑VT（5）

其中，U，V為正交陣;

（5）計算位姿的旋轉R;

（6）計算位姿的平移t;

至此，可求得相機的估計位姿R，t。

2.3 地圖構建

在ORB-SLAM2系統中，通過Q_tree算法提取圖像均勻的特征點，采用快速近似最近鄰算法（FLANN）進行特征點的匹配，并使用隨機抽樣一致算法（random sample consensus， RANSAC）剔除誤匹配。通過局部BA優化選取關鍵幀構建出局部地圖，并由PCL點云庫構建出稀疏點云地圖[20-23]，此法雖然可用于機器人的定位，但卻很難實現機器人的導航和路徑規劃。為克服這一不足，本文基于RGB-D圖像信息進行了稠密點云地圖的構建，使之可以用于機器人的導航與路徑規劃[24]。

3 RGB-D SLAM實驗與結果分析

為驗證本文對RGB-D SLAM算法的改進效果，應用本文的算法在德國慕尼黑工業大學發布的用于RGB-D SLAM實驗的開源TUM數據集進行處理，選用的3個數據集序列分別為：fr1_desk、fr1_desk2、fr2_360，并利用SLAM算法評測工具EVO從特征點檢測、軌跡輸出和系統精度3個方面評價算法的效果。

實驗環境：操作系統為64位Ubuntu 18.04.5 LTS，CPU為Intel core i5-8250u CPU@1.60GHZ * 8，內存為8GB;視覺傳感器為微軟公司的Kinect V2相機。實驗前，在ROS melodic環境下采用張正友標定法，使用9×11棋盤格（每格20 mm×20 mm），對相機進行標定，得到相機的內參矩陣K和畸變參數。

3.1 Q_tree特征點提取算法驗證

為驗證特征點提取的Q_tree算法，利用Kinect V2相機采集實驗室真實環境的RGB圖片，分別利用Q_tree算法和ORB算法對同一幀圖片提取特征點，結果如圖3所示。可見，ORB特征點有明顯的扎堆、簇集現象，Q_tree算法實現的ORB關鍵點分布均勻，更利于之后的特征匹配。

圖3 角點對比圖

（a）Q_tree算法提取結果;（b） ORB算法提取結果

3.2 軌跡驗證

選用TUM數據集中的fr1_desk、fr1_desk2進行相機真實運行軌跡與估計軌跡的驗證，并用真實數據集進行軌跡的跟蹤，實驗結果如圖4、圖5所示，實線為相機的位姿估計軌跡，虛線為真實軌跡。可以看出在兩個不同的數據集中估計軌跡與真實軌跡的偏移量都非常小，估計軌跡與真實軌跡整體方向是一致的。

在自建的真實環境數據集中的運行結果如圖6所示。盡管由于缺少設備追蹤真實的軌跡，無法比較軌跡的效果，但從圖中可以看出，本文的算法在實際環境中可以正常運行并產生自身估計的位姿軌跡。

3.3 精度對比

絕對位姿誤差（APE）描述的是系統每一幀相機的真實位姿和估計位姿的差值，可以直觀的顯示系統的精度[25]，利用均方根誤差來衡量系統的精度。本文的SLAM系統與ORB-SLAM2系統的精度對比見表1。在fr1_desk、fr1_desk2和fr2_360三個序列上，本文系統的均方根誤差均比ORB-SLAM2系統的小，說明本系統在不同環境中的穩定性要優于原ORB-SLAM2系統，精度更好。

3.4 稠密地圖構建

本文算法的應用場景為室內的靜態場景，所以稠密地圖構建實驗選用的數據集為fr1與fr2序列，并在真實環境中進行驗證，實驗結果如圖7、圖8所示。可以看出，本文算法可以在完整保留數據集信息的情況下構建出稠密點云地圖。圖9是在實際環境中初始化和建圖的情況。實驗過程中，算法運行順暢、穩定，能夠正確構建出實際環境的稠密點云地圖，且能夠保留完整的信息，用于機器人的路徑規劃。

4 結論

通過對RGB-D SLAM算法的改進與實驗研究，可知，RGB-D圖像特征提取采用Q_tree算法，與ORB算法相比，所提取的特征點更均勻，減少了特征點的冗余和簇集;將EPnP與ICP算法相結合進行相機位姿估計的優化，提高了位姿估計的準確性;利用PCL點云庫進行稀疏點云的拼接，構建了能夠用于機器人導航的稠密點云地圖。進一步的研究是利用八叉樹結構，將稠密點云地圖壓縮成八叉樹地圖，剔除與導航無關的冗余特征，提高算法的實時性;并研究如何生成二維柵格地圖，以用于AGV的路徑規劃。

參考文獻

[1]DISSANYAKE G， WILLIAMS S B， DURRANT-WHYTE H， et al. Map management for efficient simultaneous localization and mapping （SLAM）[J]. Autonomous Robots， 2002， 12（3）：267-286.

[2]賈棋，高新凱，羅鐘鉉，等.基于幾何關系約束的特征點匹配算法[J].計算機輔助設計與圖形學學報，2015，27（8）：1388-1397.

[3]NAMITHA N， VAITHEESWARAN S M， JAYASREE V K， et al. Point cloud mapping measurements using kinect RGB-D sensor and kinect fusion for visual odometry[C]//12th International Cenference on Communication Networks（ICCN），Bangalore， 2016：209-212.

[4]YU， H Y， ZHU H J， HUANG F R. Visual simultaneous localization and mapping（SLAM） based on blurred image detection[J]. Journal of Intelligent & Robotic Systems，2021，103（1）：12.

[5]李亞賓.基于RGB-D數據的移動機器人室內定位與建圖研究[D]. 哈爾濱：哈爾濱工程大學， 2016.

[6]朱東林，徐光輝，周捷，等. 視覺同時定位與建圖方法綜述[J].通信技術，2021，54（3）：523-533.

[7]ZHOU F， ZHANG L M， DENG C L， et al. Improved point-line feature based visual SLAM method for complex environments[J]. Sensors，2021，21（13）： 4604.

[8]ENDRES F， HESS J， STURM J， et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics， 2014， 30（1）：177-187.

[9]MUR-ARTLA R， MONTIEL J M M， TARDOS J D. ORB-SLAM： A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics， 2015， 31（5）：1147-1163.

[10] CHEN S B， ZHOU B D， JIANG C H， et al. A LiDAR/visual SLAM backend with loop closure detection and graph optimization[J]. Remote Sensing，2021，13（14）： 2720.

[11] MUR-ARTAL R， TARDOS J D. ORB-SLAM2： An open-source SLAM system for monocular， stereo and RGB-D cameras[J]. IEEE Transactions on Robotics， 2017， 33（5）：1255-1262.

[12] HUANG H Y， YE H Y， SUN Y X， et al. Incorporating learnt local and global embeddings into monocular visual SLAM[J]. Autonomous Robots， 2021，45（6）： 789-803.

[13] RUBLEE E， RABAUD V， KONOLIGE K， et al. ORB： An efficient alternative to SIFT or SURF[C]//IEEE International Conference on Computer Vision， ICCV， Barcelona， 2011： 2564-2571.

[14] LOWE D G. Distinctive image feature from scale-invariant key points[J]. International Journal of Computer Vision， 2004， 60（2）：91-110.

[15] BAY H，TUYTELAARS T，VANGOOL L. SURF： Speeded-up robust features[C]// 9th European Conference on Computer Vision （ECCV 2006）， Graz， 2006：404-417.

[16] ZHONG Q B， FANG X Y. A bigbigan-based loop closure detection algorithm for indoor visual SLAM[J]. Journal of Electrical and Computer Engineering，2021，2021： 9978022.

[17] FISCHLER M A， BOLLES R C. Random sample consensus： A paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM， 1981， 24（6）：381-395.

[18] 董靜薇，蔣燚銘，韓知雨. RGB-D SLAM前端視覺里程計的優化算法[J]. 哈爾濱理工大學學報， 2020，25（6）：157-164.

[19] 高翔，張濤.視覺SLAM十四講：從理論到實踐[M].北京：電子工業出版社，2017.

[20] 伍錫如，黃榆媛，王耀南. 改進ORB特征的機器人RGB-D SLAM算法[J]. 計算機工程與應用， 2020（14）：35-44.

[21] 房德國，王偉，李自然，等.VIO-SLAM綜述[J].電光與控制，2020，27（12）：58-62+100.

[22] ENDRES F， HESS J， STURM J， et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics， 2014， 30（1）：177-187.

[23] 趙宏，劉向東，楊永娟. 基于RGB-D圖像的室內機器人同時定位與地圖構建[J]. 計算機應用， 2020， 40（12）：3637-3643.

[24] 林輝燦，呂強，王國勝，等. 基于VSLAM的自主移動機器人三維同時定位與地圖構建[J]. 計算機應用， 2017（10）：2884-2887+2894.

[25] 楊世強，白樂樂，趙成，等.一種改進的Qtree_ORB算法[J].計算機工程與應用，2021，57（20）：82-89.

Research on Visual SLAM Algorithm Based on RGB-D Image

MENG Te1， LI Fu-cai2，LIU Bang-yan1， ZHANG Feng-sheng1

（1. School of Mechanical and Electrical Engineering， Qingdao University， Qingdao 266071， China;

2.Qingdao Institute of Aeronautical Technology，Qingdao 266400， China）

Abstract：

Aiming at the problem that the mainstream RGB-D SLAM system has low accuracy and only generates sparse point cloud map， an improved slam system is proposed. The frontend uses improved ORB feature extraction algorithm to improve the problem of feature point extraction cluster. The back-end uses EPNP and ICP algorithm to optimize the camera pose and improve the pose estimation accuracy. And adding thread of constructing dense point cloud map to get dense point cloud map of scene for navigation and path planning of robot. On TUM data set， Kinect V2 camera is used to compare the improved SLAM algorithm with ORB-SLAM2 algorithm. The result shows that the comprehensive performance of SLAM algorithm is better than that of ORB-SLAM2 algorithm.

Keywords：

RGB-D SLAM; Kinect V2; EPnP; dense point cloud map

收稿日期：2021-09-06

通信作者：張鳳生，博士，教授，主要研究方向為機電系統設計與控制、機器視覺技術應用。E-mail： fszhang1994@163.com

2519501186272