伍錫如，黃榆媛，王耀南

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

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

1 引言

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

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

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

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

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

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

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

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

圖2 整體算法流程圖

3.1 前端

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

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

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

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

圖3 多尺度高斯金字塔

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

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

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

連接O和C獲得方向向量OC，則關(guān)鍵點(diǎn)O的主方向?yàn)椋?/p>

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

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

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

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

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

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

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

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

（2）BRIEF描述子計(jì)算

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

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

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

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

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

3.1.2 機(jī)器人位姿估計(jì)

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

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

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

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

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

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

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

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

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

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

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

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

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

（2）第i對(duì)點(diǎn)的誤差定義為：

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

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

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

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

至此，就能求得機(jī)器人的估計(jì)位姿R,t。

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

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

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

3.2.1 關(guān)鍵幀選擇

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

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

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

3.2.2 BoW模型

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

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

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

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

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

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

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

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

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

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

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

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

3.3.2 地圖構(gòu)建

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

4 實(shí)驗(yàn)與分析

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

4.1 實(shí)驗(yàn)準(zhǔn)備

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

表1 實(shí)驗(yàn)數(shù)據(jù)的信息

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

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

畸變參數(shù)：

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

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

圖7 Kinect v1與標(biāo)定實(shí)驗(yàn)

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

4.2.1 基于Q-tree的ORB算法檢測(cè)性能

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

圖8 角點(diǎn)檢測(cè)對(duì)比

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

4.2.2 實(shí)時(shí)性和定位誤差對(duì)比

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

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

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

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

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

圖10 估計(jì)軌跡與真實(shí)軌跡對(duì)比圖

圖11 絕對(duì)位姿誤差變化趨勢(shì)圖

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

4.3 建圖效果評(píng)估

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

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

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

4.3.2 自測(cè)實(shí)驗(yàn)場(chǎng)景建圖

圖13 實(shí)驗(yàn)平臺(tái)及環(huán)境

圖14 自測(cè)實(shí)驗(yàn)場(chǎng)景下的建圖對(duì)比

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

5 結(jié)束語(yǔ)

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

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