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

孟特 李富才 劉邦彥 張鳳生

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

關(guān)鍵詞：RGB-D SLAM;Kinect V2;EPnP;稠密點(diǎn)云地圖

中圖分類號：TP391??????? 文獻(xiàn)標(biāo)志碼：A

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

1 視覺SLAM系統(tǒng)框架

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

2 RGB-D SLAM算法的改進(jìn)

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

2.1 基于四叉樹（Q_tree）算法改進(jìn)ORB特征提取

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

由于不涉及尺度，梯度等復(fù)雜運(yùn)算，F(xiàn)AST檢測器速度非?？?，缺點(diǎn)是不具有方向性和尺度不變性。為了克服這一缺點(diǎn)，ORB算法通過構(gòu)建圖像金字塔來實(shí)現(xiàn)尺度不變性，而旋轉(zhuǎn)不變性通過灰度質(zhì)心法[14]來實(shí)現(xiàn)。所謂質(zhì)心是指以圖像塊的灰度值作為權(quán)重的中心，質(zhì)心的計算步驟：

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

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

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

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

Step 1 初始化父節(jié)點(diǎn)O，將圖像均勻分割為四個子區(qū)域（A，B，C，D），得到初始化的Q_tree結(jié)構(gòu);

Step 2 按照每個子區(qū)域的角點(diǎn)數(shù)量進(jìn)行判斷，若角點(diǎn)數(shù)量小于或等于1，則停止劃分，否則繼續(xù)均勻的四分該子區(qū)域;

Step 3 重復(fù)Step 2，直到遞歸出的子區(qū)域存在的角點(diǎn)數(shù)小于或等于1，或達(dá)到期望提取的角點(diǎn)數(shù);對于角點(diǎn)簇集的情況，通過非極大值抑制條件，只保留Harris響應(yīng)值最大的較優(yōu)角點(diǎn)，去除其他冗余點(diǎn)。

2.2 相機(jī)位姿估計

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

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

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

根據(jù)相機(jī)投影模型，有

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

其中，si為尺度因子，uiviT表示pci對應(yīng)的2D投影坐標(biāo)，K為相機(jī)內(nèi)參矩陣，通過相機(jī)標(biāo)定獲得。

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

Mx=0（3）

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

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

x=∑Ni=1βivi（4）

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

求解出βi后，根據(jù)式（4）可得到x，再將x帶入式（1）就可計算出匹配點(diǎn)在相機(jī)坐標(biāo)系下的3D點(diǎn)。

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

（1）分別計算匹配點(diǎn)在世界坐標(biāo)系和相機(jī)坐標(biāo)系下的質(zhì)心

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

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

A=pwTo-pwTopwTnpwTn B=pcTo-pcTopcTnpcTn

（3）計算本質(zhì)矩陣H：H=BTA;

（4）計算H的SVD分解

H=U∑VT（5）

其中，U，V為正交陣;

（5）計算位姿的旋轉(zhuǎn)R;

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

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

2.3 地圖構(gòu)建

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

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

為驗(yàn)證本文對RGB-D SLAM算法的改進(jìn)效果，應(yīng)用本文的算法在德國慕尼黑工業(yè)大學(xué)發(fā)布的用于RGB-D SLAM實(shí)驗(yàn)的開源TUM數(shù)據(jù)集進(jìn)行處理，選用的3個數(shù)據(jù)集序列分別為：fr1_desk、fr1_desk2、fr2_360，并利用SLAM算法評測工具EVO從特征點(diǎn)檢測、軌跡輸出和系統(tǒng)精度3個方面評價算法的效果。

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

3.1 Q_tree特征點(diǎn)提取算法驗(yàn)證

為驗(yàn)證特征點(diǎn)提取的Q_tree算法，利用Kinect V2相機(jī)采集實(shí)驗(yàn)室真實(shí)環(huán)境的RGB圖片，分別利用Q_tree算法和ORB算法對同一幀圖片提取特征點(diǎn)，結(jié)果如圖3所示?？梢?，ORB特征點(diǎn)有明顯的扎堆、簇集現(xiàn)象，Q_tree算法實(shí)現(xiàn)的ORB關(guān)鍵點(diǎn)分布均勻，更利于之后的特征匹配。

圖3 角點(diǎn)對比圖

（a）Q_tree算法提取結(jié)果;（b） ORB算法提取結(jié)果

3.2 軌跡驗(yàn)證

選用TUM數(shù)據(jù)集中的fr1_desk、fr1_desk2進(jìn)行相機(jī)真實(shí)運(yùn)行軌跡與估計軌跡的驗(yàn)證，并用真實(shí)數(shù)據(jù)集進(jìn)行軌跡的跟蹤，實(shí)驗(yàn)結(jié)果如圖4、圖5所示，實(shí)線為相機(jī)的位姿估計軌跡，虛線為真實(shí)軌跡?？梢钥闯鲈趦蓚€不同的數(shù)據(jù)集中估計軌跡與真實(shí)軌跡的偏移量都非常小，估計軌跡與真實(shí)軌跡整體方向是一致的。

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

3.3 精度對比

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

3.4 稠密地圖構(gòu)建

本文算法的應(yīng)用場景為室內(nèi)的靜態(tài)場景，所以稠密地圖構(gòu)建實(shí)驗(yàn)選用的數(shù)據(jù)集為fr1與fr2序列，并在真實(shí)環(huán)境中進(jìn)行驗(yàn)證，實(shí)驗(yàn)結(jié)果如圖7、圖8所示?？梢钥闯觯疚乃惴梢栽谕暾Ａ魯?shù)據(jù)集信息的情況下構(gòu)建出稠密點(diǎn)云地圖。圖9是在實(shí)際環(huán)境中初始化和建圖的情況。實(shí)驗(yàn)過程中，算法運(yùn)行順暢、穩(wěn)定，能夠正確構(gòu)建出實(shí)際環(huán)境的稠密點(diǎn)云地圖，且能夠保留完整的信息，用于機(jī)器人的路徑規(guī)劃。

4 結(jié)論

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

參考文獻(xiàn)

[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]賈棋，高新凱，羅鐘鉉，等.基于幾何關(guān)系約束的特征點(diǎn)匹配算法[J].計算機(jī)輔助設(shè)計與圖形學(xué)學(xué)報，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數(shù)據(jù)的移動機(jī)器人室內(nèi)定位與建圖研究[D]. 哈爾濱：哈爾濱工程大學(xué)， 2016.

[6]朱東林，徐光輝，周捷，等. 視覺同時定位與建圖方法綜述[J].通信技術(shù)，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前端視覺里程計的優(yōu)化算法[J]. 哈爾濱理工大學(xué)學(xué)報， 2020，25（6）：157-164.

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

[20] 伍錫如，黃榆媛，王耀南. 改進(jìn)ORB特征的機(jī)器人RGB-D SLAM算法[J]. 計算機(jī)工程與應(yīng)用， 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圖像的室內(nèi)機(jī)器人同時定位與地圖構(gòu)建[J]. 計算機(jī)應(yīng)用， 2020， 40（12）：3637-3643.

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

[25] 楊世強(qiáng)，白樂樂，趙成，等.一種改進(jìn)的Qtree_ORB算法[J].計算機(jī)工程與應(yīng)用，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

通信作者：張鳳生，博士，教授，主要研究方向?yàn)闄C(jī)電系統(tǒng)設(shè)計與控制、機(jī)器視覺技術(shù)應(yīng)用。E-mail： fszhang1994@163.com

2519501186272