荊樹旭，盧鵬宇，翟曉惠，高 濤

(長安大學(xué) 信息工程學(xué)院，陜西 西安 710064)

0 引 言

隨著智能機(jī)器人技術(shù)的不斷發(fā)展，研究者們希望機(jī)器人可以像人類一樣，通過眼睛去觀察和理解周圍的世界，完成自身的定位和對(duì)周圍環(huán)境的建模。這個(gè)問題被稱為即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)[1-4]。視覺SLAM[5]是指通過視覺傳感器的方式完成即時(shí)定位與地圖構(gòu)建。相比使用激光雷達(dá)來獲取周圍環(huán)境信息，視覺傳感器具有體積小，重量輕，功耗小，價(jià)格便宜等優(yōu)點(diǎn)[4]。2010年，微軟推出了可以同時(shí)獲取彩色圖和深度圖的相機(jī)Kinect。由于其低廉的價(jià)格，使得RGB-D相機(jī)在視覺SLAM領(lǐng)域得到了廣泛的應(yīng)用[6-8]。

目前，國內(nèi)外主流的方法是基于特征點(diǎn)法和圖優(yōu)化法的RGB-D SLAM。2010年，Henry等人[9]最早提出了一種基于深度相機(jī)的SLAM算法，該算法首先提取SIFT特征點(diǎn)，然后采用最近迭代點(diǎn)(iterative closest point，ICP)算法求解兩幀圖像之間的最優(yōu)變換，最后利用圖優(yōu)化工具TORO進(jìn)行全局優(yōu)化。2012年，Henry等人[10]提出了一種改進(jìn)的算法，該算法利用重投影誤差改進(jìn)隨機(jī)采樣一致性(radom sample consensus，RANSAC)算法[11]的配準(zhǔn)過程，使用Fast特征提取和Calonder特征描述符代替之前的SIFT，用稀疏光速平差法(sparse bundle adjustment，SBA)代替TORO進(jìn)行優(yōu)化。2011年，Newcombe等人[12]提出的KinectFusion是最早能實(shí)現(xiàn)實(shí)時(shí)三維重建的方案，可以在不需要RGB圖只需要深度圖的情況下就能實(shí)時(shí)地建立三維模型。2014年，Whelan等人[13]在KinectFusion的基礎(chǔ)上增加了回環(huán)檢測模塊來增強(qiáng)魯棒性。同年，Endres等人[14]測評(píng)了SIFT[15]、SURF[16]和ORB[17]這3種方法在RGB-D SLAM中的表現(xiàn)，在后端采用通用圖優(yōu)化工具g2o進(jìn)行全局優(yōu)化，最后采用了八叉樹地圖OctoMap進(jìn)行地圖的構(gòu)建。2017年，Mur-Artal R等人[18]提出了ORB-SLAM2，這是一個(gè)基于單目、雙目和RGB-D相機(jī)的完整的SLAM系統(tǒng)。2018年，張震等人[19]利用ORB特征和詞袋(bag of words，BoW)模型增強(qiáng)了RGB-D SLAM算法的魯棒性。

上述算法存在的部分問題：采用暴力匹配算法對(duì)特征點(diǎn)進(jìn)行匹配，需要每個(gè)特征與其他所有的特征進(jìn)行匹配，匹配范圍大，耗時(shí)嚴(yán)重。RANSAC處理數(shù)據(jù)時(shí)具有隨機(jī)性的特點(diǎn)，忽略了待匹配點(diǎn)質(zhì)量的差別。BoW模型不考慮圖像中視覺單詞的位置信息，會(huì)導(dǎo)致回環(huán)檢測的精度不足。針對(duì)上述問題，該文利用BoW模型結(jié)合型樹層次結(jié)構(gòu)，將特征點(diǎn)劃分為特征字典某一特定層的節(jié)點(diǎn)，只有屬于同一父節(jié)點(diǎn)的特征才是潛在的正確匹配，從而縮小匹配范圍，提高算法的實(shí)時(shí)性。利用順序抽樣一致性(progressive sample consensus，PROSAC)算法[20]對(duì)圖像匹配誤差進(jìn)行剔除并且求解相機(jī)位姿，利用視覺詞典選出一系列閉環(huán)候選幀，對(duì)閉環(huán)候選幀進(jìn)行嚴(yán)格的篩選，從而篩選出真正的閉環(huán)幀，提高算法的魯棒性。

1 基于BoW模型的增強(qiáng)RGB-D SLAM算法

算法的整體流程如圖1所示。在前端，采取ORB算法提取圖像的特征點(diǎn)，基于BoW模型改進(jìn)匹配，在保證匹配效果的同時(shí)，減少匹配的時(shí)間，提高算法的實(shí)時(shí)性，結(jié)合PROSAC算法和透視n點(diǎn)(PnP)算法計(jì)算相機(jī)的初始位姿變換，利用bundle adjustment(BA)進(jìn)行非線性優(yōu)化，得到更精確的相機(jī)位姿。通過關(guān)鍵幀篩選方案判斷該幀是否成為關(guān)鍵幀。在后端，通過BoW模型[21]對(duì)關(guān)鍵幀序列中的每一幀與當(dāng)前關(guān)鍵幀圖像進(jìn)行相似度得分計(jì)算，得分較高的列為閉環(huán)候選幀，利用結(jié)構(gòu)一致性對(duì)候選幀序列進(jìn)行回環(huán)判定，篩選出真正的閉環(huán)幀，將閉環(huán)幀作為新的約束加入到位姿圖中，增強(qiáng)算法的魯棒性。利用通用圖優(yōu)化工具g2o對(duì)位姿圖進(jìn)行全局優(yōu)化，得到全局一致的位姿。最后利用全局優(yōu)化后的位姿和對(duì)應(yīng)的點(diǎn)云數(shù)據(jù)，采用貪心三角化算法重建出網(wǎng)格地圖。

圖1 算法流程

1.1 ORB特征提取與基于BoW的圖像特征匹配

1.1.1 ORB特征提取

該文選用ORB特征算法進(jìn)行特征提取。ORB算法采用改進(jìn)的FAST角點(diǎn)檢測方法獲取特征點(diǎn)，采用BRIEF特征描述子生成特征描述符。與SIFT和SURF相比，ORB算法在速度上擁有絕對(duì)的優(yōu)勢，不需要使用GPU加速，適合在嵌入式設(shè)備或缺少GPU運(yùn)算能力的設(shè)備上使用。

1.1.2 基于BoW的圖像特征匹配

當(dāng)兩幅圖像進(jìn)行匹配時(shí)，如果采用暴力匹配法，將每幅內(nèi)的每一個(gè)特征與另一幅圖像內(nèi)的每一個(gè)特征進(jìn)行匹配，其時(shí)間復(fù)雜度為O(N2)。而BoW的正向索引存儲(chǔ)了圖像特征描述子和它們在字典樹中關(guān)聯(lián)的節(jié)點(diǎn)，這種方式可以極大地縮小圖像特征的匹配范圍，加快匹配速度。算法步驟如下：

(1)對(duì)任意兩幅圖像I1和I2，提取它們的ORB特征，得到它們的特征點(diǎn)和描述子，即(keypoints1i，descriptors1i)和(keypoints2i，descriptors2i)。其中keypoints1i表示I1的特征點(diǎn)，descriptos1i表示與keypoints1i對(duì)應(yīng)的特征描述子；keypoints2i表示I2的特征點(diǎn)，descriptos2i表示與keypoints2i對(duì)應(yīng)的特征描述子。

(2)對(duì)descriptors1i和descriptors2i求其對(duì)應(yīng)的BoW特征向量(node1i，f1ij)和(node2i，f2ij)。其中node1i和node2i表示第l層的節(jié)點(diǎn)，f1ij表示以node1i為父節(jié)點(diǎn)的特征，f2ij表示以node2i為父節(jié)點(diǎn)的特征。

(3)當(dāng)node1i=node2i時(shí)，計(jì)算f1ij和f2ij對(duì)應(yīng)的描述子descripto1ij與descriptor2ij之間的漢明距離Dist，設(shè)置最小值為bestDist1，次小值為bestDist2。

設(shè)定閾值H和h，若滿足：

bestDist1

(1)

bestDist1

(2)

則認(rèn)為該匹配是正確的匹配，存入Matches。

最終可得到魯棒性強(qiáng)的匹配Matches，Matches被用來進(jìn)行下一步的圖像配準(zhǔn)和位姿估計(jì)。

1.2 位姿求解與優(yōu)化

完成兩幅圖像的匹配后，可以進(jìn)行相機(jī)位姿的求解。該文使用PROSAC算法與PnP算法結(jié)合計(jì)算相機(jī)的初始位姿，同時(shí)認(rèn)為最鄰近匹配的漢明距離越小，匹配點(diǎn)的質(zhì)量越高。得到相機(jī)的初始位姿后，采用BA對(duì)初始位姿進(jìn)行非線性優(yōu)化，得到精確的位姿，BA問題即求解最小化重投影誤差，如式(3)所示。

(3)

其中，(R,t)表示相機(jī)位姿，Xi表示空間點(diǎn)的坐標(biāo)，xi表示該點(diǎn)在像素內(nèi)的坐標(biāo)，Φ表示相機(jī)內(nèi)參，ρ表示Huber核函數(shù)。

BA將在g2o中采用Levenberg-Marquadt實(shí)現(xiàn)。位姿求解與優(yōu)化的具體算法如下：

(1)對(duì)Matches中的匹配點(diǎn)按照bestDist1的值從小到大進(jìn)行排序。

(2)在排序后的點(diǎn)中選取前n個(gè)點(diǎn)作為高質(zhì)量匹配點(diǎn)作為RANSAC算法和PnP算法的輸入，求解出相機(jī)位姿的變換矩陣(R,t)和內(nèi)點(diǎn)數(shù)Inliers。

(3)將(R,t)代入式(3)求得優(yōu)化后的相機(jī)位姿(R,t)*。

1.3 關(guān)鍵幀選擇

該文使用關(guān)鍵幀進(jìn)行定位和建圖，通過計(jì)算幀間相對(duì)運(yùn)動(dòng)距離來判斷該幀是否為關(guān)鍵幀。幀間相對(duì)運(yùn)動(dòng)距離的計(jì)算公式如下：

D=t+min(2π-R,R)

(4)

其中，(R,t)表示相機(jī)位姿。

對(duì)當(dāng)前幀F(xiàn)ramecurr，判斷該幀是否為關(guān)鍵幀的方法如下所示：

(1)判斷Inliersi>Inliersmin，如果該式成立，則進(jìn)行下一步判斷，否則舍棄該幀。

(2)將(R,t)*作為初值輸入式(4)，求得幀間相對(duì)運(yùn)動(dòng)距離D。

(3)設(shè)定閾值Dmin和Dmax，若Dmin

該文引入了冗余關(guān)鍵幀刪除機(jī)制，有助于地圖的構(gòu)建和提高SLAM算法的魯棒性。對(duì)當(dāng)前關(guān)鍵幀KeyFramecurr，如果KeyFramecurr上超過90%以上的特征點(diǎn)可以被至少其他3個(gè)關(guān)鍵幀檢測到時(shí)，認(rèn)為KeyFramecurr為冗余關(guān)鍵幀，將其從關(guān)鍵幀序列里刪除。

1.4 回環(huán)檢測

該文通過BoW模型對(duì)關(guān)鍵幀序列進(jìn)行回環(huán)檢測，首先計(jì)算待檢索圖像和數(shù)據(jù)庫圖像的BoW向量，計(jì)算方式如下：

(5)

其中，ni和mi分別表示待檢索圖像和數(shù)據(jù)庫圖像中單詞i的數(shù)目，ωi表示單詞i的權(quán)重。然后計(jì)算待檢索圖像和數(shù)據(jù)庫圖像之間的相似性分?jǐn)?shù)，計(jì)算方式如下：

(6)

利用式(6)可以計(jì)算當(dāng)前幀F(xiàn)ramecurr與舊的關(guān)鍵幀KeyFramei之間的相似度。通過建立關(guān)鍵幀的倒排索引，記錄包含特定單詞的關(guān)鍵幀。對(duì)于新的一幀圖像，首先計(jì)算該幀圖像包含的單詞表，對(duì)于每一個(gè)單詞，通過倒排索引查找具有該單詞的所有關(guān)鍵幀，同時(shí)記錄每個(gè)關(guān)鍵幀與當(dāng)前幀的共同單詞個(gè)數(shù)。對(duì)那些共同單詞個(gè)數(shù)超過N的關(guān)鍵幀，計(jì)算其與當(dāng)前幀之間的相似性得分Score。設(shè)定閾值sTH，若相似度分?jǐn)?shù)Scorei滿足：

Scorei>sTH×Scoremax

(7)

則將存儲(chǔ)到候選閉環(huán)幀序列。

若閉環(huán)候選幀序列不為空，對(duì)采用1.1.2節(jié)的方法得到匹配點(diǎn)對(duì)Matches，之后利用RANSAC算法計(jì)算出基礎(chǔ)矩陣F和內(nèi)點(diǎn)數(shù)inlier，利用F驗(yàn)證對(duì)極幾何約束[22]，如果滿足幾何約束關(guān)系并且inlier大于閾值fTH，則認(rèn)為構(gòu)成回環(huán)。

1.5 全局優(yōu)化與地圖構(gòu)建

該文通過位姿圖進(jìn)行全局優(yōu)化，將每個(gè)關(guān)鍵幀的相機(jī)位姿作為節(jié)點(diǎn)，將兩個(gè)位姿的變換關(guān)系T作為邊構(gòu)建位姿圖。這樣對(duì)于兩個(gè)有邊連接的節(jié)點(diǎn)，會(huì)存在一個(gè)誤差：

ei,j=xi-T×xj

(8)

其中，xi,xj表示i,j節(jié)點(diǎn)的估計(jì)值。將所有的頂點(diǎn)和邊考慮進(jìn)來，總的誤差函數(shù)為：

(9)

其中，Ωi,j表示邊的信息矩陣。這樣整個(gè)問題被轉(zhuǎn)換為一個(gè)最小二乘問題。該文采用g2o中的Levenberg-Marquadt法進(jìn)行求解。

在相機(jī)運(yùn)動(dòng)過程中，可能會(huì)出現(xiàn)運(yùn)動(dòng)丟失的情況。為此，該文加入了重定位功能：當(dāng)連續(xù)M幀圖像都出現(xiàn)失配的時(shí)候，采用回環(huán)檢測的方法尋找關(guān)鍵幀序列中與當(dāng)前幀最為相似的圖像，之后通過1.2節(jié)的方法建立幀間約束并將當(dāng)前幀設(shè)置為關(guān)鍵幀。

該文選用網(wǎng)格地圖來進(jìn)行稠密地圖的構(gòu)建，其構(gòu)建算法如下所示：

(1)通過濾波對(duì)點(diǎn)云進(jìn)行處理，減少點(diǎn)云數(shù)據(jù)容量，去除點(diǎn)云數(shù)據(jù)中的噪聲、離群點(diǎn)。

(2)通過重采樣對(duì)物體表面進(jìn)行平滑處理和漏洞修復(fù)。

(3)計(jì)算點(diǎn)云法線，并將點(diǎn)云位姿、顏色、法線信息合并到一起，構(gòu)建有向點(diǎn)云。

(4)使用貪心投影三角化算法對(duì)有向點(diǎn)云進(jìn)行三角化，構(gòu)建三角化網(wǎng)格地圖。

2 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證算法的有效性，該文利用Sturm等人[23]提供的RGB-D基準(zhǔn)包進(jìn)行實(shí)驗(yàn)，該基準(zhǔn)包中包含Kinect產(chǎn)生的彩色圖像和深度圖像序列，以及機(jī)器人的真實(shí)運(yùn)動(dòng)位姿。同時(shí)，文獻(xiàn)[23]還提供了一個(gè)自動(dòng)評(píng)估工具，該工具可以自動(dòng)計(jì)算出真實(shí)軌跡與算法估計(jì)軌跡之間的均方根誤差(RMSE)。該文將從準(zhǔn)確性和實(shí)時(shí)性與文獻(xiàn)[19]、文獻(xiàn)[24]和文獻(xiàn)[25]進(jìn)行對(duì)比。算法的實(shí)驗(yàn)平臺(tái)為：Intel i5 9300H處理器(2.4 GHz)，16 GB RAM，64位Ubuntu16.04操作系統(tǒng)。

2.1 特征匹配結(jié)果分析

對(duì)Fr1/xyz數(shù)據(jù)包相鄰的兩幀圖像進(jìn)行特征匹配。表1為基于BoW模型的特征匹配算法和暴力匹配算法關(guān)于Fr1/xyz數(shù)據(jù)包的特征匹配時(shí)間對(duì)比，圖2所示為BoW匹配的結(jié)果。

表1 Fr1/xyz數(shù)據(jù)包特征匹配時(shí)間對(duì)比

由表1可知，在ORB算法提取的特征點(diǎn)數(shù)量為500，F(xiàn)r1/xyz數(shù)據(jù)包共產(chǎn)生787次匹配的情況下，暴力匹配共使用了10.7 s的時(shí)間完成匹配，BoW匹配共使用了6.2 s的時(shí)間完成匹配，由此可見，BoW匹配可以極大地減少特征匹配的時(shí)間。由圖2可知，BoW匹配的結(jié)果較好，正確匹配數(shù)量多。

圖2 BoW匹配結(jié)果

2.2 定位誤差對(duì)比

該文選取了Fr1數(shù)據(jù)包中的Fr1/xyz、Fr1/rpy、Fr1/desk等9個(gè)數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)。表2為文中算法與文獻(xiàn)[19]、文獻(xiàn)[24]和文獻(xiàn)[25]的均方根誤差對(duì)比。

表2 文中算法與文獻(xiàn)[19]、文獻(xiàn)[24]和文獻(xiàn)[25]的均方根誤差對(duì)比 m

由表2可以看出，文中算法在Fr1/xyz、Fr1/rpy、Fr1/desk等6個(gè)數(shù)據(jù)集中的精度均優(yōu)于文獻(xiàn)[19]、文獻(xiàn)[24]和文獻(xiàn)[25]中提出的算法。僅在Fr1/desk2數(shù)據(jù)集，文中算法略低于文獻(xiàn)[19]和文獻(xiàn)[25]，但優(yōu)于文獻(xiàn)[24]；在Fr1/360數(shù)據(jù)集，文中算法略低于文獻(xiàn)[24]和文獻(xiàn)[25]；在Fr1/plant數(shù)據(jù)集，文中算法略低于文獻(xiàn)[25]，但優(yōu)于文獻(xiàn)[24]。基于相同數(shù)據(jù)的集性能比較，文獻(xiàn)[24]的平均RMSE為0.096 8 m，文獻(xiàn)[25]的平均RMSE為0.084 2 m，文中算法的平均RMSE為0.079 7 m，總體精度優(yōu)于文獻(xiàn)[24]和文獻(xiàn)[25]。文獻(xiàn)[19]在測試數(shù)據(jù)集的平均RMSE為0.072 8 m，文中算法對(duì)應(yīng)的數(shù)據(jù)集的平均RMSE為0.066 5 m，總體精度優(yōu)于文獻(xiàn)[19]。根據(jù)以上統(tǒng)計(jì)結(jié)果，驗(yàn)證了文中算法的總體精度優(yōu)于文獻(xiàn)[19]、文獻(xiàn)[24]和文獻(xiàn)[25]，具有更好的準(zhǔn)確性。

為了更加直觀地顯示文中算法的定位準(zhǔn)確性，對(duì)估計(jì)軌跡與真實(shí)地面軌跡進(jìn)行誤差量化分析。圖3是估計(jì)軌跡與真實(shí)軌跡的對(duì)比圖，其中的(a)、(b)、(c)、(d)分別對(duì)應(yīng)了表2的Fr1/xyz、Fr1/desk、Fr1/floor、Fr1/room。表3為誤差量化數(shù)據(jù)，包括最大值max、最小值min、中位數(shù)median、均值mean、標(biāo)準(zhǔn)差std。

(a)Fr1/xyz數(shù)據(jù)集相機(jī)軌跡恢復(fù)結(jié)果 (b)Fr1/desk數(shù)據(jù)集相機(jī)軌跡恢復(fù)結(jié)果

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

從圖3可以看出，估計(jì)軌跡能較好地貼合真實(shí)軌跡，全局一致性較好。從表3可以看出在Fr1數(shù)據(jù)包中，誤差值與變化波動(dòng)都較小，總體誤差在可接受范圍。

2.3 運(yùn)行速度對(duì)比

由于文獻(xiàn)[25]只進(jìn)行了特征點(diǎn)的提取，沒有進(jìn)行特征描述符與特征匹配，所以文中只與文獻(xiàn)[19]和文獻(xiàn)[24]進(jìn)行速度對(duì)比，對(duì)比結(jié)果如表4所示。

表4 文中算法與文獻(xiàn)[19]和文獻(xiàn)[24]算法運(yùn)行速度對(duì)比 s

文獻(xiàn)[24]處理一幀數(shù)據(jù)平均需要0.35 s，即平均每秒鐘處理約3幀數(shù)據(jù)，文獻(xiàn)[19]處理一幀數(shù)據(jù)平均需要0.11 s，即每秒鐘處理約10幀數(shù)據(jù)，而文中算法處理一幀數(shù)據(jù)平均用時(shí)為0.04 s，即每秒鐘處理約25幀數(shù)據(jù)，是文獻(xiàn)[24]的8倍，文獻(xiàn)[19]的2.5倍。由于Kinect的幀率為30 Hz，因此文中算法基本滿足實(shí)時(shí)性要求。

2.4 地圖構(gòu)建評(píng)估

該文選用了Fr1/xyz、Fr1/desk和Fr1/floor數(shù)據(jù)包進(jìn)行建圖分析。圖4的(a)、(b)和(c)分別表示使用Fr1/xyz、Fr1/desk和Fr1/floor數(shù)據(jù)包構(gòu)建的三維網(wǎng)格地圖。

(a)Fr1/xyz數(shù)據(jù)集重建結(jié)果

Fr1/xyz和Fr1/desk是辦公室場景，為手持Kinect拍攝所得。由圖4(a)和(b)可以看到，該場景內(nèi)的辦公桌、電腦、書籍和墻壁等都得到了很好的重建。圖中的空洞部分是由于拍攝過程中沒有掃描到那部分。Fr1/floor數(shù)據(jù)包由機(jī)器人攜帶Kinect拍攝所得。由圖4(c)可以看到，整個(gè)場景得到了完整的重建。

3 結(jié)束語

該文提出了一種改進(jìn)的RGB-D SLAM算法來進(jìn)行即時(shí)定位與地圖構(gòu)建。在SLAM前端，該文采用ORB特征提取算法來進(jìn)行特征點(diǎn)的提取，使其可以運(yùn)行在嵌入式設(shè)備和缺乏GPU的設(shè)備上；采用BoW模型改進(jìn)了匹配方法，極大地減少了匹配所需要的時(shí)間，提高了算法的實(shí)時(shí)性；利用PROSAC算法結(jié)合PnP算法計(jì)算相機(jī)位姿，提高了精度的同時(shí)也節(jié)省了時(shí)間，并且利用非線性優(yōu)化進(jìn)一步提高了位姿的精度；在傳統(tǒng)的關(guān)鍵幀選取方法上加入了冗余關(guān)鍵幀刪除機(jī)制，便于關(guān)鍵幀的管理和維護(hù)。在SLAM后端，該文采用了BoW模型進(jìn)行閉環(huán)檢測，利用結(jié)構(gòu)一致性提高了閉環(huán)的準(zhǔn)確率，增強(qiáng)了算法的魯棒性；通過通用圖優(yōu)化工具g2o對(duì)位姿圖進(jìn)行優(yōu)化，得到了全局一致的位姿。該文采用貪心三角化算法來構(gòu)建三維網(wǎng)格地圖。最后，該文選取TUM數(shù)據(jù)集里的Fr1數(shù)據(jù)包進(jìn)行實(shí)驗(yàn)，并通過與文獻(xiàn)[19]、文獻(xiàn)[24]和文獻(xiàn)[25]進(jìn)行了實(shí)時(shí)性和準(zhǔn)確性的對(duì)比，表明了該算法的優(yōu)越性。在下一步的工作中，將會(huì)結(jié)合IMU(慣性測量單元)等高精度設(shè)備更進(jìn)一步優(yōu)化相機(jī)位姿，利用深度學(xué)習(xí)方法提取語義信息，構(gòu)建語義SLAM。