伍錫如，黃榆媛，王耀南

1.桂林電子科技大學(xué) 電子工程與自動化學(xué)院，廣西 桂林 541004 2

.湖南大學(xué) 電氣與信息工程學(xué)院，長沙 410082

1 引言

隨著魚眼相機、深度相機和全景相機等先進(jìn)設(shè)備的出現(xiàn)與計算機視覺技術(shù)的日益成熟，視覺同時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）成為國內(nèi)外學(xué)者的重點研究領(lǐng)域。特別是在機器人、AR/VR、自動駕駛和無人機等應(yīng)用中涌現(xiàn)出許多創(chuàng)新性成果[1-5]。SLAM技術(shù)是機器人實現(xiàn)自主移動的關(guān)鍵技術(shù)[6]，基于視覺的SLAM相比于激光SLAM具有功耗低、成本低、小型化等優(yōu)點，理論和應(yīng)用價值突出[7]。2010年微軟公司推出的RGB-D深度相機Kinect，由于其低成本、輕量化、能采集豐富的圖像信息等優(yōu)勢，廣泛應(yīng)用到視覺SLAM技術(shù)的研究和開發(fā)中[8-11]。文獻(xiàn)[8]首次采用深度相機進(jìn)行室內(nèi)環(huán)境3D密集地圖的構(gòu)建。Endres F等人[10]設(shè)計了一個經(jīng)典的RGB-D SLAM系統(tǒng)，該系統(tǒng)具有較好的魯棒性和準(zhǔn)確性，能在較長軌跡和有挑戰(zhàn)性的場景中追蹤機器人的運動。文獻(xiàn)[11]中介紹了第一個基于單目、雙目和RGB-D相機的完整SLAM系統(tǒng)，全程使用ORB[12]特征，兼具地圖重用、閉環(huán)檢測與重定位的功能，并且可實時在CPU上工作。

經(jīng)過30多年的發(fā)展，機器人視覺SLAM的整體算法框架包括：前端視覺里程計、后端優(yōu)化、回環(huán)檢測和地圖構(gòu)建。從概念上來說，機器人視覺SLAM需要完成的工作是用視覺傳感器采集到的圖片數(shù)據(jù)，精確地估計出機器人的運動位姿，并同步構(gòu)建周圍場景的地圖，以便于進(jìn)行后續(xù)的路徑規(guī)劃或運動控制[13]。早期的視覺SLAM沿用機器人領(lǐng)域的濾波方法，如卡爾曼濾波[14]、擴展卡爾曼濾波[15]、粒子濾波[16]等，而現(xiàn)在應(yīng)用計算機視覺領(lǐng)域的圖優(yōu)化方法成為主流[17-21]。文獻(xiàn)[18]的視覺SLAM系統(tǒng)最早引入關(guān)鍵幀機制，通過串接關(guān)鍵幀而非采集到的全部圖片完成軌跡和地圖的優(yōu)化。文獻(xiàn)[19]總結(jié)和分析了基于關(guān)鍵幀的SLAM系統(tǒng)，并對SLAM研究中存在的外部干擾、地圖維護(hù)、重定位等問題提供指導(dǎo)意見。KinectFusion[20]是Newcombe等人設(shè)計的首個用Kinect深度相機完成的三維重建系統(tǒng)，它從深度圖中讀取點云數(shù)據(jù)，根據(jù)迭代最近點（Iterative Closest Point，ICP）算法估計位姿來拼接點云，基于網(wǎng)格模型實時重建場景。Kintinuous[21]是KinectFusion的改進(jìn)版本，增加回環(huán)檢測與優(yōu)化，融合直接法使得處理速度更快，適用于大場景重建。文獻(xiàn)[10]的RGB-D SLAMv2是一個優(yōu)秀且完整的SLAM系統(tǒng)，有前后端、閉環(huán)檢測和地圖構(gòu)建，在使用SIFT特征時要用GPU加速。從這些優(yōu)秀的文獻(xiàn)工作來看，在靜態(tài)不變、無人為干擾、光照不明顯的應(yīng)用環(huán)境中，視覺SLAM技術(shù)已經(jīng)發(fā)展得較為成熟。但是現(xiàn)實的工作環(huán)境往往更加復(fù)雜多變，如何在已有的系統(tǒng)框架上通過改進(jìn)算法提升視覺SLAM技術(shù)的實時性、魯棒性和準(zhǔn)確性，從而應(yīng)用到實際場景中，成為該技術(shù)領(lǐng)域的一大挑戰(zhàn)和難題。

本文提出一種新的RGB-D SLAM算法，旨在提高機器人視覺SLAM算法實時性、定位和建圖精度。主要貢獻(xiàn)在于：（1）基于四叉樹（Quad-tree，Q-tree）算法改進(jìn)ORB特征提取，減少冗余點，加速誤匹配點的剔除；（2）應(yīng)用EPnP和ICP組合算法求解得到較精準(zhǔn)的機器人位姿；（3）基于關(guān)鍵幀和詞袋（Bag of Words，BoW）模型構(gòu)建快速回環(huán)檢測，減小累積誤差；（4）使用光速法平差（Bundle Adjustment，BA）優(yōu)化位姿圖得到最優(yōu)軌跡，構(gòu)建3D彩色點云地圖，并以八叉樹地圖（Octomap）形式進(jìn)行存儲。通過公開數(shù)據(jù)集和實際移動機器人平臺驗證算法性能。

2 機器人RGB-D SLAM系統(tǒng)架構(gòu)

圖1 機器人RGB-D SLAM系統(tǒng)框圖

根據(jù)文獻(xiàn)[10]的系統(tǒng)框架，本文的機器人RGB-D SLAM系統(tǒng)框圖如圖1所示。前端負(fù)責(zé)讀取傳感器數(shù)據(jù)和估計位姿。Kinect相機作為環(huán)境感知設(shè)備能同時讀取RGB彩色圖和深度圖。本文選擇特征法開展研究，因為它不易受動態(tài)障礙物、光線等的影響，可穩(wěn)定運行在移動機器人平臺。常用的圖像特征有SIFT[22]、SURF[23]和ORB，其中的ORB算法因為較優(yōu)的快速性和實時性得到廣泛應(yīng)用。為使提取的ORB特征點均勻分布，本文先引入Q-tree來管理FAST角點，然后采用快速最近似最近鄰（Fast Library for Approximate Nearest Neighbors，F(xiàn)LANN）算法匹配特征，最后在隨機采樣一致性（Random Sample Consensus，RANSAC）算法的基礎(chǔ)上，迭代使用EPNP和ICP方法求解不同坐標(biāo)系下的幾何關(guān)系，獲得機器人位姿。由于相機獲取的數(shù)據(jù)存在不可避免的外界干擾，位姿估計必然會出現(xiàn)累積漂移，所以需要回環(huán)測和后端優(yōu)化來減小累積誤差，進(jìn)一步優(yōu)化位姿?；丨h(huán)檢測的目的是判別機器人是否重新回到之前已經(jīng)走過的區(qū)域，檢測到的有效回環(huán)信息能將具有累積誤差的軌跡拉回正確的地方。實現(xiàn)回環(huán)檢測的原理是對關(guān)鍵幀進(jìn)行數(shù)據(jù)相似性的計算，具體的方法是使用BoW模型來創(chuàng)建字典，通過字典對關(guān)鍵幀之間的相似性做出量度，進(jìn)而判斷回環(huán)的存在。在后端中，需要用到優(yōu)化理論構(gòu)造圖優(yōu)化模型，這個模型使用估計出的位姿和回環(huán)檢測數(shù)據(jù)構(gòu)造位姿圖，通過BA算法優(yōu)化位姿圖的結(jié)構(gòu)，得到長時間內(nèi)機器人的最優(yōu)軌跡，從而構(gòu)建全局一致的機器人運動軌跡和環(huán)境地圖。這部分工作將基于通用圖形優(yōu)化器G2O進(jìn)行求解。在已知機器人的最優(yōu)軌跡后，可通過軌跡來重建環(huán)境地圖。3D點云地圖數(shù)據(jù)規(guī)模較大，為降低硬件內(nèi)存要求，使用緊湊的Octomap進(jìn)行數(shù)據(jù)存儲。Octomap易于信息的壓縮和更新，同時也可以根據(jù)應(yīng)用需求調(diào)節(jié)分辨率得到不同精度的導(dǎo)航地圖。

3 機器人RGB-D SLAM算法實現(xiàn)

本章介紹整個算法的實現(xiàn)流程。圖2是整體算法流程圖。該算法的輸入數(shù)據(jù)為Kinect深度相機于同一時間戳采集到彩色RGB圖像幀和深度圖像幀，它們在時空上是對齊的。算法前端，對RGB圖像幀使用基于四叉樹的ORB特征算法來提取均勻的Fast關(guān)鍵點，獲得這些關(guān)鍵點的BRIEF描述子；通過FLANN算法完成描述子的特征匹配，并根據(jù)篩選條件剔除錯誤匹配點對；按照RANSAC迭代方式，結(jié)合EPNP和ICP算法估計出機器人的位姿。同時，每一時間戳的圖像幀經(jīng)過關(guān)鍵幀判別后存儲到關(guān)鍵幀數(shù)據(jù)集中，該集合經(jīng)冗余處理后用于詞袋模型的回環(huán)檢測。在后端，前端計算的機器人位姿和回環(huán)檢測數(shù)據(jù)被構(gòu)造成位姿圖，基于BA算法求解得到最優(yōu)的機器人位姿軌跡。根據(jù)最優(yōu)軌跡和對應(yīng)的3D點云重建出運行環(huán)境的彩色點云地圖，點云地圖在轉(zhuǎn)換后被表示成緊湊的Octomap地圖。

圖2 整體算法流程圖

3.1 前端

3.1.1 基于Q-tree的ORB特征提取

相比于像素或直方圖的圖像內(nèi)容描述方法，基于外觀的圖像底層特征保留了豐富的原始信息，能更好地區(qū)分和表達(dá)圖像內(nèi)容。根據(jù)硬件條件和實時性的需求，選擇同時兼顧特征檢測速度和穩(wěn)定性能的ORB算法對場景內(nèi)容進(jìn)行描述。

（1）基于Q-tree的FAST關(guān)鍵點檢測

對同一張圖片提取不同的空間尺度信息，得到圖像局部和全局尺度上的一系列多分辨率圖像集合，構(gòu)建高斯金字塔（見圖3），再從每一層中進(jìn)行FAST角點檢測，解決FAST算法本身沒有尺度不變特性的問題。

圖3 多尺度高斯金字塔

采用灰度質(zhì)心法計算FAST關(guān)鍵點的主方向，增加旋轉(zhuǎn)描述。假設(shè)點O(x,y)是某一待檢測的特征點，取包含點O的一個小圖像塊U計算(p+q)階鄰域矩：

式中，I(x,y)為點O的灰度值。

通過鄰域矩來求圖像塊U的質(zhì)心C：

連接O和C獲得方向向量OC，則關(guān)鍵點O的主方向為：

均勻地對環(huán)境進(jìn)行采樣是進(jìn)行良好特征匹配的前提，但在研究中發(fā)現(xiàn)檢測出的FAST角點在一張圖片中會堆積成點斑，不利于后期的特征匹配。本文結(jié)合Q-tree均勻算法[24]加以改進(jìn)，減少冗余特征點的同時，能使提取的關(guān)鍵點更均勻地遍布在整個圖像中。

圖4是Q-tree區(qū)域分割及數(shù)據(jù)存儲的示意圖。對每張圖像提取到的FAST角點進(jìn)行Q-tree均勻處理，步驟如下（一個子區(qū)域?qū)?yīng)樹狀結(jié)構(gòu)中的一個子節(jié)點）：

步驟1初始化父節(jié)點O，將2D空間均勻分割成4個子區(qū)域(A,B,C,D)，得到初始的Q-tree結(jié)構(gòu)。

圖4 Q-tree數(shù)據(jù)結(jié)構(gòu)示意圖

步驟2將檢測出的FAST角點映射到初始Q-tree中，根據(jù)角點位置把角點劃分到不同的子區(qū)域。

步驟3按照子區(qū)域存在的角點數(shù)量判斷是否進(jìn)行再分割：若角點個數(shù)小于等于1，則不再分割；反之，則再次均勻四分割并劃分角點。

步驟4重復(fù)步驟3，直至遞歸劃分出的子區(qū)域中存在的角點個數(shù)小于等于1或達(dá)到期望提取的角點數(shù)量為止。

步驟4中停止分割的條件是每個子區(qū)域中的角點數(shù)都不大于1，這在實際中很難達(dá)到，因為檢測出的角點如存在扎推現(xiàn)象，相鄰的多個角點幾乎是重疊的，所以當(dāng)子區(qū)域的角點個數(shù)大于1時，需要通過非極大值抑制[23]條件，只保留Harris響應(yīng)強度最大的較優(yōu)角點，去除Harris響應(yīng)強度小的冗余點。

（2）BRIEF描述子計算

BRIEF[25]用隨機點對生成的描述子的抗噪性能不佳，處理方法是使用積分圖做平滑處理，通過高斯濾波降噪。依據(jù)取法之間相關(guān)性最小的規(guī)則，在特征點的31×31像素鄰域中選擇5×5子窗口對，比較隨機N對子窗口對的像素灰度值之和得到一個N維的二進(jìn)制描述子，對于一個平滑后的圖像鄰域Δ：

其中，λ(Δ;g,h)表示二值化的結(jié)果，I(Δ,g)和 I(Δ,h)是鄰域Δ內(nèi)的兩個對應(yīng)子窗口g、h各自的灰度值之和。fN(Δ)是獲得的二進(jìn)制比特串，N=256。

依據(jù)FAST關(guān)鍵點的主方向?qū)γ枋鲎舆M(jìn)行旋轉(zhuǎn)，克服BRIEF描述子不具備旋轉(zhuǎn)不變性的缺點。這個方向信息用關(guān)鍵點31×31像素鄰域內(nèi)的N對(xj,yj)構(gòu)成2×N的矩陣Τ：

由旋轉(zhuǎn)矩陣R和式（3）中的主方向φ對Τ旋轉(zhuǎn)，得到校正后的Τφ矩陣：帶方向的描述子[12]可表示為：

在檢測出特征點并計算對應(yīng)描述子后，通過描述子計算圖片間的漢明距離進(jìn)行匹配。暴力匹配法對于特征點多的場景運算成本高，不利于機器人SLAM的實時運行，選用FLANN[26]算法完成特征匹配。同時根據(jù)工程經(jīng)驗，用漢明距離小于最小距離的2倍為篩選依據(jù)，去除錯誤的匹配點對。

3.1.2 機器人位姿估計

在使用深度相機獲得深度圖的情況下，可以得到特征點的距離信息，所以根據(jù)ORB特征匹配后的結(jié)果對機器人的位姿進(jìn)行估計，通?？梢钥紤]兩種情形，一種是根據(jù)已知3D點與其在相機中的投影2D點求解PNP問題，另一種就是使用匹配好的兩組3D點云求解ICP問題。在SLAM中，常見的一些做法是使用P3P或者非迭代的EPNP算法估計位姿再進(jìn)行優(yōu)化，這種做法沒有有效利用更多的匹配點信息，而直接使用ICP算法求解則需要依賴較好的初始值，不然極易在迭代中陷入局部最優(yōu)，且在深度相機不能獲得距離信息后難以執(zhí)行，可見這些方法在噪聲干擾或誤匹配時會導(dǎo)致算法失效。本研究在這些算法的基礎(chǔ)上進(jìn)行優(yōu)化，使用RANSAC算法簡單有效地過濾誤匹配，通過迭代的方式提高匹配點對的信息利用率，把良好的匹配數(shù)據(jù)應(yīng)用到EPNP[27]算法中改善穩(wěn)健性，在有了較好的初始值情況下，進(jìn)一步使用ICP[28]算法求解機器人位姿。

EPNP算法的思想是用4個非共面虛擬控制點的加權(quán)和表示世界坐標(biāo)系下的3D點坐標(biāo)。為保證算法的穩(wěn)定性，需要根據(jù)世界坐標(biāo)系下匹配點集的質(zhì)心以及主成分分析法得到的三個主方向選擇控制點。它每次隨機地從所有匹配好的點對中選擇四個點對進(jìn)行計算，匹配點相對于世界坐標(biāo)系的坐標(biāo)為,i=1,2,…,n ，相對于相機坐標(biāo)系的坐標(biāo)為,i=1,2,…,n，同時，假設(shè)4個控制點的坐標(biāo)表示那么在世界坐標(biāo)系下，匹配點與4個虛擬控制點的線性關(guān)系為：

由于相機坐標(biāo)系與世界坐標(biāo)系是相似變換的關(guān)系，在相機坐標(biāo)系下也存在：

根據(jù)相機的投影模型有：

式中，si為尺度因子，[uivi]T表示 pci對應(yīng)的2D投影坐標(biāo)；K是相機的內(nèi)參數(shù)矩陣，其中 fu,fv代表相機焦距，(uc,vc)為相機中心，該矩陣可以通過標(biāo)定獲得。

將公式（10）和（11）代入公式（12）中展開得：

把式（14）寫成矩陣形式：

向量x位于M的右零空間[27]，存在下列關(guān)系：

其中，vi是矩陣M的零特征值相應(yīng)的N特征向量，通過計算MTM的特征向量可以解出vi。βi的求解與N的取值有關(guān)，詳細(xì)求解參照文獻(xiàn)[27]。

求解出βi后，依據(jù)公式（16）就能得到x，即相機坐標(biāo)系下4個虛擬控制點的坐標(biāo)。再將x代入式（11）計算出匹配點在相機坐標(biāo)系下的3D坐標(biāo)。

在已知匹配點在世界坐標(biāo)系下和相機坐標(biāo)系下的兩組3D點后，使用ICP[28]算法估計機器人的位姿，獲得旋轉(zhuǎn)矩陣R和平移向量t。算法的操作流程為：

（1）假設(shè)兩組點分別為P和Q：

存在歐式變換R,t，滿足?i,pi=Rqi+t。

（2）第i對點的誤差定義為：

構(gòu)建最小二乘的目標(biāo)函數(shù)F，求出一個R,t使誤差平方和得到極小值：

（3）求解式（19），先計算兩組點的質(zhì)心坐標(biāo) ?,?′，得到每個點的去質(zhì)心坐標(biāo)κi,κ′i：

（4）由優(yōu)化的目標(biāo)函數(shù)求旋轉(zhuǎn)矩陣R：

（5）按照（4）中的 R，進(jìn)一步求出t：

至此，就能求得機器人的估計位姿R,t。

用每次估計出的位姿計算反投影誤差，確定內(nèi)點的數(shù)量，然后通過內(nèi)點數(shù)量判斷出有效的位姿估計，最后選擇具有最大內(nèi)點數(shù)的位姿作為此次的最準(zhǔn)確估計。在RANSAC算法框架中，以上過程不斷迭代，獲得當(dāng)前幀對應(yīng)的最優(yōu)位姿估計。

3.2 基于關(guān)鍵幀的回環(huán)檢測

根據(jù)降低控制問題規(guī)模和保證稀疏性的需要，采用關(guān)鍵幀技術(shù)進(jìn)行有效采樣能提高視覺SLAM系統(tǒng)的實時性和穩(wěn)定性。而且在后端優(yōu)化中使用成功的回環(huán)信息有利于構(gòu)建全局一致的軌跡和地圖。

3.2.1 關(guān)鍵幀選擇

兩張圖片之間相似性的可靠測量是許多關(guān)鍵幀選擇算法的基礎(chǔ)。本文研究了基于特征匹配的相似性量度S[29]的方法。S與兩張圖片的距離函數(shù)Λ相關(guān)：

式中，Σt和Σi分別表示當(dāng)前圖像幀F(xiàn)t及該幀對應(yīng)的最近上一關(guān)鍵幀(KF)i中視覺特征的個數(shù)，M表示兩個圖像幀匹配的視覺特征個數(shù)。Λmax和Λmin是圖像之間距離函數(shù)Λ期望取到或者能夠取到的最大值和最小值，S 的取值范圍是[0，1]。

將系統(tǒng)輸入的第一幀作為初始關(guān)鍵幀，下一個關(guān)鍵幀的選擇判據(jù)有兩個：（1）距離上一個關(guān)鍵幀插入已經(jīng)過去超過20幀；（2）檢測到的內(nèi)點數(shù)要大于設(shè)定的最小閾值，并且相似性量度S小于設(shè)定的最大閾值。滿足（1）、（2）中一個條件即將當(dāng)前幀設(shè)為關(guān)鍵幀。

3.2.2 BoW模型

BoW模型的建立是基于外觀的，它應(yīng)用于回環(huán)檢測時可以脫離視覺SLAM系統(tǒng)前、后端估計的幾何信息，比基于圖像特征的回環(huán)檢測方式更加快速和有效。在BoW中涉及單詞和字典的概念，每個單詞代表某一類特征的組合，多個單詞組成字典。根據(jù)文獻(xiàn)[30]，使用類似于層次聚類的k叉樹來生成字典。圖5是深度為l的k叉樹字典數(shù)據(jù)結(jié)構(gòu)圖。從所有訓(xùn)練圖片中提取ORB特征，在根節(jié)點用k-means++算法均勻地將所有特征點進(jìn)行層次聚類。這里使用文獻(xiàn)[11]中的ORB特征類型字典用于回環(huán)的檢索。

圖5 k叉樹字典結(jié)構(gòu)

回環(huán)檢測前要刪除關(guān)鍵幀集合中的冗余幀，刪除的標(biāo)準(zhǔn)是如果有不少于3個的其他關(guān)鍵幀能夠觀測到某個關(guān)鍵幀90%以上的路標(biāo)點，則把該關(guān)鍵幀視作冗余關(guān)鍵幀并剔除[11]?；丨h(huán)檢測在去除冗余幀的關(guān)鍵幀集合中進(jìn)行，通過比較不同關(guān)鍵幀間的相似性判斷回環(huán)，將回環(huán)信息作為約束交給后端處理。

3.3 后端優(yōu)化與建圖

3.3.1 基于BA的圖優(yōu)化

視覺SLAM中的優(yōu)化問題可以用一個由頂點和邊組成的圖表示，通過優(yōu)化圖的結(jié)構(gòu)獲得待優(yōu)化變量的最優(yōu)解。圖6給出了簡單的圖優(yōu)化模型。路標(biāo)點和位姿作為頂點是待優(yōu)化的變量，使用運動模型和觀測模型得到頂點之間的約束邊，可以是估計位姿約束或者回環(huán)約束。

圖6 圖優(yōu)化模型

基于圖優(yōu)化模型，視覺SLAM的優(yōu)化能直觀地用數(shù)學(xué)語言表達(dá)成一個非線性最小二乘問題，通過BA[31]方法在G2O庫中求最優(yōu)解。視覺SLAM過程的基本數(shù)學(xué)表示包括運動方程xk和觀測方程zk。

這里的uk是傳感器的輸入，wk為噪聲。pj是在位置xk觀測到的路標(biāo)點，vk,j為觀測中的噪聲。

由公式（25）知道，運動和觀測過程都存在噪聲干擾，實際的觀測值和用算法計算出來的觀測值存在誤差，圖模型的邊就代表了這個誤差項e。本研究只對位姿xk做優(yōu)化處理，優(yōu)化的目標(biāo)函數(shù)F(X)為：

其中，X是全部位姿變量xk的集合，D代表估計位姿的時間序列；xi,xj是i、j時刻估計出的機器人位姿，zij表示這兩個頂點的實際約束，而z?ij是通過計算得到的約束；Ωij代表約束的信息矩陣，即兩頂點不確定性的度量，其中的每個Ωij作為ei、ej的系數(shù)，是對ei、ej誤差項相關(guān)性的預(yù)估。此時，優(yōu)化問題的求解變成尋找一個最優(yōu)位姿序列X?，使得目標(biāo)函數(shù)F(X)取得最小值，即：

考慮優(yōu)化算法的魯棒性，對目標(biāo)函數(shù)用魯棒Huber核函數(shù)包裝，避免優(yōu)化過程專注于錯誤數(shù)據(jù)的優(yōu)化，而忽略正確約束的影響。整體實現(xiàn)在G2O庫中進(jìn)行，使用列文伯格-馬夸爾特下降策略和設(shè)置稀疏求解器完成。

3.3.2 地圖構(gòu)建

構(gòu)建地圖是對機器人運行環(huán)境進(jìn)行建模的過程。把關(guān)鍵幀數(shù)據(jù)處理成點云，根據(jù)優(yōu)化后的位姿使用點云庫PCL拼接點云，得到3D彩色點云地圖。目前由離散路標(biāo)點構(gòu)建的稀疏點云地圖很難與導(dǎo)航、避障、路徑規(guī)劃等應(yīng)用接軌，并且數(shù)據(jù)的存儲對硬件內(nèi)存要求高。研究中將點云地圖轉(zhuǎn)化成Octomap，八叉樹節(jié)點能保留該點的占據(jù)情況，樹狀存儲結(jié)構(gòu)占據(jù)內(nèi)存小，也使得數(shù)據(jù)的更新和維護(hù)靈活和快速。在Octomap庫實現(xiàn)點云地圖到Octomap的轉(zhuǎn)換，其可視化界面有查看、縮放旋轉(zhuǎn)、調(diào)節(jié)分辨率等操作。

4 實驗與分析

為了驗證本文算法的可行性和高效性，使用公開的RGB-D數(shù)據(jù)集進(jìn)行驗證，并在室內(nèi)環(huán)境對多履帶式全向移動機器人開展實驗。通過對比實驗分析設(shè)計算法的實時性、定位及建圖效果。

4.1 實驗準(zhǔn)備

全部實驗在Ubuntu16.04 LTS環(huán)境下完成，硬件平臺是64 bit的ThinkPad-T460 PC，處理器為Intel Core i5-6200U@2.30 GHz×4。實驗數(shù)據(jù)是德國慕尼黑工業(yè)大學(xué)的TUM數(shù)據(jù)集[32]，數(shù)據(jù)序列的詳細(xì)信息見表1。

表1 實驗數(shù)據(jù)的信息

實驗前，在ROS Kinetic環(huán)境下對Kinect v1（見圖7）進(jìn)行標(biāo)定，使用8×6×0.028的棋盤格，得到的結(jié)果如下。

相機內(nèi)參數(shù)矩陣：

畸變參數(shù)：

D=[0.005 7-0.085 5 0.012 88-0.000] 3 0 （30）

實驗中，主要參數(shù)配置有：相機幀率30 f/s，每張圖片提取的ORB特征點數(shù)量為1 000，尺度金字塔層數(shù)設(shè)為8且層與層之間的尺度因子為1.2。

圖7 Kinect v1與標(biāo)定實驗

4.2 改進(jìn)算法的相關(guān)性能分析

4.2.1 基于Q-tree的ORB算法檢測性能

圖8為使用基于Q-tree均勻算法的FAST角點檢測處理前后的角點分布情況對比。顯然，與原始的ORB算法相比，經(jīng)過改進(jìn)算法處理后，可以在整張圖像中提取出分布較均勻的角點。由式（23）、（24）可知，誤匹配點對會嚴(yán)重影響圖像相似度的計算結(jié)果，這里使用FLANN算法快速匹配特征并剔除誤匹配對，獲得良好的圖像配準(zhǔn)結(jié)果（見圖9）?？梢?，改進(jìn)處理后的ORB特征算法具有較穩(wěn)定和優(yōu)越的場景表達(dá)性能，適用于描述和匹配圖像。

圖8 角點檢測對比

圖9 改進(jìn)算法的特征匹配對比圖

4.2.2 實時性和定位誤差對比

使用表1的數(shù)據(jù)，將RGB-D SALMv2[10]算法與改進(jìn)算法的運行時間、均方根誤差（Root Mean Square Error，RMSE）進(jìn)行對比，統(tǒng)計數(shù)據(jù)于表2。

表2 RGB-D SLAMv2算法與改進(jìn)算法的性能指標(biāo)對比

由表2可以看出，本文算法的運行時間和定位誤差比RGB-D SLAMv2算法顯著減少，算法實時性和準(zhǔn)確性得到極大提升。針對這12個數(shù)據(jù)序列，從數(shù)據(jù)處理時間來看，RGB-D SLAMv2算法平均每幀數(shù)據(jù)的處理時間約為0.153 s，本文算法的處理時間約為0.080 s，從兩種算法估計出的軌跡與地面真實軌跡的RMSE來看，本文算法的平均RMSE低于RGB-D SLAMv2算法，其中RGB-D SLAMv2的平均RMSE約為0.258 m，本文算法的平均RMSE約為0.084 m。根據(jù)以上指標(biāo)的統(tǒng)計結(jié)果，證明改進(jìn)算法有更好的定位實時性和準(zhǔn)確性。

為了更加直觀地顯示本文算法的定位準(zhǔn)確性，采用文獻(xiàn)[33]的定量評價方法對估計軌跡與真實地面軌跡進(jìn)行誤差量化分析。圖10是估計軌跡與真實軌跡的對比圖，其中（a）、（b）、（c）和（d）分別對應(yīng)表1的fr1/xyz、fr1/floor、fr2/pioneer_360和fr2/pioneer_slam數(shù)據(jù)序列，灰色虛線reference表示真實軌跡，帶顏色的實線表示算法的估計軌跡，圖右側(cè)從紅到藍(lán)的色帶，顯示誤差的大小程度。圖11為4組數(shù)據(jù)序列的估計軌跡與真實軌跡絕對位姿誤差（Absolute Pose Error，APE）隨時間的變化趨勢曲線，表3為誤差量化指標(biāo)數(shù)據(jù)，包括最大值max、最小值min、均值mean、中位數(shù)median和標(biāo)準(zhǔn)差std。

表3 誤差量化指標(biāo) m

圖10 估計軌跡與真實軌跡對比圖

圖11 絕對位姿誤差變化趨勢圖

從圖10可以看出估計軌跡能準(zhǔn)確貼合真實軌跡，全局一致性較好。分析圖11的趨勢曲線和表3的數(shù)據(jù)可知，算法在類似fr1代表的靜態(tài)室內(nèi)環(huán)境中誤差值與變化波動都較小，而類似fr2代表的移動機器人上采集的數(shù)據(jù)序列，由于機器人的運動會有一些較大的誤差波動，但經(jīng)過本文算法處理后，誤差會平緩減小，保證總體誤差在較小的可接受范圍。

4.3 建圖效果評估

本節(jié)用公開的TUM數(shù)據(jù)集和實驗室機器人平臺自測數(shù)據(jù)進(jìn)行實驗，以便評估算法的建圖效果。

4.3.1 TUM數(shù)據(jù)集建圖

將本文算法與RGB-D SLAMv2算法構(gòu)建的地圖進(jìn)行對比。圖12（a）、（b）和（c）分別表示使用fr1/xyz、fr1/desk2和fr2/pioneer_360數(shù)據(jù)序列構(gòu)建的三維地圖，每組對比圖的左邊是用RGB-D SLAMv2算法構(gòu)建的地圖，右邊是用本文算法構(gòu)建的地圖?？梢奟GB-D SLAMv2算法和本文算法都能有效地構(gòu)建環(huán)境地圖，有較好的三維重建效果。其中對于室內(nèi)具有較多物件的豐富場景，兩種算法構(gòu)建的地圖沒有明顯的優(yōu)劣差距，但是對于大范圍且較空曠的環(huán)境，改進(jìn)算法的優(yōu)勢比較明顯，構(gòu)建的地圖更清晰直觀。

4.3.2 自測實驗場景建圖

圖13 實驗平臺及環(huán)境

圖14 自測實驗場景下的建圖對比

圖13 給出了機器人平臺和實驗環(huán)境。圖（a）為自主開發(fā)的多履帶式全向移動機器人，圖（b）是實驗場景。圖14（a）、（b）為兩種算法構(gòu)建出的環(huán)境地圖對比，圖（c）是由圖（b）轉(zhuǎn)換的Octomap。在實驗過程中，發(fā)現(xiàn)機器人在跑RGB-D SLAMv2算法時會出現(xiàn)算法中斷的現(xiàn)象，為保證實際建圖，用手柄操作機器人在實驗室中間空曠地面向前移動，均勻且緩慢運行。相比之下，本文算法運行較穩(wěn)定，在一定程度上能減少大部分的光照干擾，使用關(guān)鍵幀的建圖方式忽略了不必要的部分場景，地圖更簡潔直觀。

5 結(jié)束語

本文提出了一種改進(jìn)的機器人RGB-D SLAM算法，通過Q-tree均勻算法改進(jìn)的ORB特征提取能有效剔除冗余特征點，使得保存下來的較優(yōu)特征點均勻分布，從而促進(jìn)良好的點對匹配，減少了錯誤匹配對機器人位姿估計帶來的不利影響；在解算機器人位姿時，應(yīng)用EPNP和ICP算法的組合優(yōu)化模式，充分利用更多數(shù)據(jù)在迭代過程中獲得最優(yōu)的思想，提升機器人位姿估計的準(zhǔn)確度；進(jìn)一步地應(yīng)用關(guān)鍵幀的有效回環(huán)和圖優(yōu)化技術(shù)減少累積誤差，達(dá)到使優(yōu)化后的運動軌跡和地圖全局一致的目的?；趯嶒炑芯亢蛯Ρ?，在TUM數(shù)據(jù)集和機器人平臺的實驗結(jié)果表明，算法的實時性及魯棒性提高，估計的軌跡能較好地貼合數(shù)據(jù)集的真實軌跡，重建出清晰直觀的場景地圖。

進(jìn)一步的研究會集中在聯(lián)合激光雷達(dá)、IMU等較精確的設(shè)備優(yōu)化位姿估計，引入深度學(xué)習(xí)方法賦予機器人語義識別能力并創(chuàng)建實時八叉樹地圖實現(xiàn)機器人的自主導(dǎo)航控制等。