基于BoW模型的增強RGB-D SLAM算法

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

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

0 引 言

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

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

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

1 基于BoW模型的增強RGB-D SLAM算法

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

圖1 算法流程

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

1.1.1 ORB特征提取

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

1.1.2 基于BoW的圖像特征匹配

當兩幅圖像進行匹配時，如果采用暴力匹配法，將每幅內的每一個特征與另一幅圖像內的每一個特征進行匹配，其時間復雜度為O(N2)。而BoW的正向索引存儲了圖像特征描述子和它們在字典樹中關聯的節點，這種方式可以極大地縮小圖像特征的匹配范圍，加快匹配速度。算法步驟如下：

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

(2)對descriptors1i和descriptors2i求其對應的BoW特征向量(node1i，f1ij)和(node2i，f2ij)。其中node1i和node2i表示第l層的節點，f1ij表示以node1i為父節點的特征，f2ij表示以node2i為父節點的特征。

(3)當node1i=node2i時，計算f1ij和f2ij對應的描述子descripto1ij與descriptor2ij之間的漢明距離Dist，設置最小值為bestDist1，次小值為bestDist2。

設定閾值H和h，若滿足：

bestDist1

(1)

bestDist1

(2)

則認為該匹配是正確的匹配，存入Matches。

最終可得到魯棒性強的匹配Matches，Matches被用來進行下一步的圖像配準和位姿估計。

1.2 位姿求解與優化

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

(3)

其中，(R,t)表示相機位姿，Xi表示空間點的坐標，xi表示該點在像素內的坐標，Φ表示相機內參，ρ表示Huber核函數。

BA將在g2o中采用Levenberg-Marquadt實現。位姿求解與優化的具體算法如下：

(1)對Matches中的匹配點按照bestDist1的值從小到大進行排序。

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

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

1.3 關鍵幀選擇

該文使用關鍵幀進行定位和建圖，通過計算幀間相對運動距離來判斷該幀是否為關鍵幀。幀間相對運動距離的計算公式如下：

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

(4)

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

對當前幀Framecurr，判斷該幀是否為關鍵幀的方法如下所示：

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

(2)將(R,t)*作為初值輸入式(4)，求得幀間相對運動距離D。

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

該文引入了冗余關鍵幀刪除機制，有助于地圖的構建和提高SLAM算法的魯棒性。對當前關鍵幀KeyFramecurr，如果KeyFramecurr上超過90%以上的特征點可以被至少其他3個關鍵幀檢測到時，認為KeyFramecurr為冗余關鍵幀，將其從關鍵幀序列里刪除。

1.4 回環檢測

該文通過BoW模型對關鍵幀序列進行回環檢測，首先計算待檢索圖像和數據庫圖像的BoW向量，計算方式如下：

(5)

其中，ni和mi分別表示待檢索圖像和數據庫圖像中單詞i的數目，ωi表示單詞i的權重。然后計算待檢索圖像和數據庫圖像之間的相似性分數，計算方式如下：

(6)

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

Scorei>sTH×Scoremax

(7)

則將存儲到候選閉環幀序列。

若閉環候選幀序列不為空，對采用1.1.2節的方法得到匹配點對Matches，之后利用RANSAC算法計算出基礎矩陣F和內點數inlier，利用F驗證對極幾何約束[22]，如果滿足幾何約束關系并且inlier大于閾值fTH，則認為構成回環。

1.5 全局優化與地圖構建

該文通過位姿圖進行全局優化，將每個關鍵幀的相機位姿作為節點，將兩個位姿的變換關系T作為邊構建位姿圖。這樣對于兩個有邊連接的節點，會存在一個誤差：

ei,j=xi-T×xj

(8)

其中，xi,xj表示i,j節點的估計值。將所有的頂點和邊考慮進來，總的誤差函數為：

(9)

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

在相機運動過程中，可能會出現運動丟失的情況。為此，該文加入了重定位功能：當連續M幀圖像都出現失配的時候，采用回環檢測的方法尋找關鍵幀序列中與當前幀最為相似的圖像，之后通過1.2節的方法建立幀間約束并將當前幀設置為關鍵幀。

該文選用網格地圖來進行稠密地圖的構建，其構建算法如下所示：

(1)通過濾波對點云進行處理，減少點云數據容量，去除點云數據中的噪聲、離群點。

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

(3)計算點云法線，并將點云位姿、顏色、法線信息合并到一起，構建有向點云。

(4)使用貪心投影三角化算法對有向點云進行三角化，構建三角化網格地圖。

2 實驗結果與分析

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

2.1 特征匹配結果分析

對Fr1/xyz數據包相鄰的兩幀圖像進行特征匹配。表1為基于BoW模型的特征匹配算法和暴力匹配算法關于Fr1/xyz數據包的特征匹配時間對比，圖2所示為BoW匹配的結果。

表1 Fr1/xyz數據包特征匹配時間對比

由表1可知，在ORB算法提取的特征點數量為500，Fr1/xyz數據包共產生787次匹配的情況下，暴力匹配共使用了10.7 s的時間完成匹配，BoW匹配共使用了6.2 s的時間完成匹配，由此可見，BoW匹配可以極大地減少特征匹配的時間。由圖2可知，BoW匹配的結果較好，正確匹配數量多。

圖2 BoW匹配結果

2.2 定位誤差對比

該文選取了Fr1數據包中的Fr1/xyz、Fr1/rpy、Fr1/desk等9個數據集進行實驗。表2為文中算法與文獻[19]、文獻[24]和文獻[25]的均方根誤差對比。

表2 文中算法與文獻[19]、文獻[24]和文獻[25]的均方根誤差對比 m

由表2可以看出，文中算法在Fr1/xyz、Fr1/rpy、Fr1/desk等6個數據集中的精度均優于文獻[19]、文獻[24]和文獻[25]中提出的算法。僅在Fr1/desk2數據集，文中算法略低于文獻[19]和文獻[25]，但優于文獻[24]；在Fr1/360數據集，文中算法略低于文獻[24]和文獻[25]；在Fr1/plant數據集，文中算法略低于文獻[25]，但優于文獻[24]。基于相同數據的集性能比較，文獻[24]的平均RMSE為0.096 8 m，文獻[25]的平均RMSE為0.084 2 m，文中算法的平均RMSE為0.079 7 m，總體精度優于文獻[24]和文獻[25]。文獻[19]在測試數據集的平均RMSE為0.072 8 m，文中算法對應的數據集的平均RMSE為0.066 5 m，總體精度優于文獻[19]。根據以上統計結果，驗證了文中算法的總體精度優于文獻[19]、文獻[24]和文獻[25]，具有更好的準確性。

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

(a)Fr1/xyz數據集相機軌跡恢復結果 (b)Fr1/desk數據集相機軌跡恢復結果

表3 誤差量化指標 m

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

2.3 運行速度對比

由于文獻[25]只進行了特征點的提取，沒有進行特征描述符與特征匹配，所以文中只與文獻[19]和文獻[24]進行速度對比，對比結果如表4所示。

表4 文中算法與文獻[19]和文獻[24]算法運行速度對比 s

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

2.4 地圖構建評估

該文選用了Fr1/xyz、Fr1/desk和Fr1/floor數據包進行建圖分析。圖4的(a)、(b)和(c)分別表示使用Fr1/xyz、Fr1/desk和Fr1/floor數據包構建的三維網格地圖。

(a)Fr1/xyz數據集重建結果

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

3 結束語

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