RGB-D SLAM V2系統算法分析與性能評估*

李明燦，賈永興，王 聰

（陸軍工程大學，江蘇 南京 210000）

0 引言

隨著芯片技術和傳感器技術的發展，移動機器人技術逐步進入生活的方方面面。無人機、掃地機器人等功能各異的機器人正深刻改變著人們的生產生活方式。想要完成復雜的任務，機器人需要能夠自主規劃自身的運動。在沒有操作者的情況下，自主移動機器人失去了輔助判斷狀態的先決條件，所以必須具備感知外部信息的能力，憑借傳感器數據估算自身的位置與周圍環境的信息，從而依靠這些信息計算出最佳的運動路線和動作軌跡，而準確的機器人信息感知是困難的。機器人搭載的傳感器只能測量到局部的環境數據，不能直觀了解環境的整體情況，必須依靠位置狀態進行環境信息的提取與合成。這種相互依存的關系導致移動機器人學中一個重大的開放性問題——要精確定位，因此機器人必須有精確的環境地圖。為了構建精確的地圖，必須精確知道機器人觀測的位置。為了實現高精度的信息感知，研究者們進行了長期探索，經過建模分析與實際環境實驗，提出了許多有意義的解決方案，其中，一個重要的領域是同時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）技術，目前已成為整個問題框架中的核心研究方向。

1 視覺SLAM技術介紹

SLAM技術旨在利用傳感器數據，通過計算機估算出機器人平臺的位置變化，并同時性地構建出周圍環境的仿真地圖。其中，利用視覺傳感機器獲取外部信息的SLAM技術稱為視覺SLAM。視覺SLAM前端被叫做視覺里程計，目的是通過計算機視覺方法估算出機器人的位姿變化；視覺SLAM后端使用圖優化[1]或束調整[2]等方式，優化機器人的運動軌跡。在視覺里程計研究領域，不少開發者推出許多優秀的算法，實現了使用不同類型的攝像頭估算運動中位姿的變化。按是否需要提取圖像特征[3]，它主要分為特征點法和直接法。

在當前視覺SLAM領域，最受關注的是利用深度相機完成感知任務。深度相機又稱RGBD相機，可以用來觀測視角范圍的彩色圖像和深度圖像，輸出一連串的三維圖像?；赗GBD數據的特征點法利用彩色圖像拾取特征點，而后利用對應的距離信息還原出其真實的空間分布，并根據3D-3D空間位姿變換的約束關系，依靠部分特征點使用純幾何手段估算關鍵幀之間的變換矩陣，進而分解出機器人運動的平移向量和旋轉矩陣。在得到一段時期的位姿變換估計后，算法后端利用優化算法對輸出結果進行高維度濾波，得到更符合實際變化特點的定位信息。系統的另一個任務是建圖，即利用優化獲得的定位信息，將三維特征點或者點云圖變換參考系并進行耦合，得到稀疏或者稠密的仿真地圖。

基礎的視覺SLAM算法在結構上已趨于完善，但是在實際的應用實現中并不能完全穩定的工作，需要使用許多特別的技巧強化系統的健壯性。經過長期的探索，研究者推出了許多優秀的框架，在原有理論的基礎上采用了新的理念，完成了準確的定位和建圖。考慮到傳感器適配、稠密建圖以及準確性等方面，本文選擇最新的RGB-D SLAM V2來進行典型算法的分析和評估。

2 算法結構

RGB-D SLAM V2由Endres提出，是一個基于深度相機的稠密視覺SLAM系統。RGB-D SLAM V2算法流程，如圖1所示。它的代碼直接兼容機器人操作系統（Robot Operating System，ROS），基本不用配置就可以非常方便地運行。對比基礎的系統架構，它在位姿估計、重定位、回環檢測和建圖等方法上使用了創新算法，提高了系統的容錯性，使系統對機器人的運動追蹤更加快速和穩定，同時能夠產生較為完整的環境地圖，為后續的應用提供了更全面的信息。

圖1 RGB-D SLAM V2流程

2.1 位姿估計

RGB-D SLAM V2使用的是標準的特征點法，支持SIFT、SUFT以及ORB[4]等特征選取方法，利用稀疏的3維特征點估計幀與幀之間的變換矩陣。如圖2所示，比較3種特征點，有GPU時SIFT綜合表現最好，而綜合實時性、硬件成本和準確率來看，ORB具有均衡的優勢。

圖2 3種特征點效果比較

視覺SLAM基礎的位姿估算方法利用的是連續兩幀的空間變換關系。假設上一幀觀測位置為p1，當前幀觀測位置為p2，兩幀之間的變換矩陣描述為T12，則觀測位置之間的關系為：

RGB-D SLAM V2采用的估計算法不僅利用了一種策略，而且采用RANSAC[5]和ICP相結合的方式進行運動估計。初始階段，前端使用3對特征點進行快速計算作為RANSAC的參考。在每一輪迭代中，利用最小二乘法估計其余點對相關程度，不斷優化得到最佳的變換矩陣。

2.2 環境測量模型

Endres提出了一個專屬的環境測量模型（Environmental Measurement Model，EMM），如圖3所示，指出了觀測圖像的特征點服從高斯分布，開創性地利用高斯分布的吻合特性來考慮點位之間的一致性問題。模型把兩幀之間的特征點進行空間初始估計后，將特征點的高斯分布空間按照設定的閾值劃分邊界?？臻g能夠重疊的相同特征會被視作在可接受誤差范圍內的可靠點，可以被用來進行進一步的優化，而無法重疊的匹配點會被視作誤匹配或者不合格的特征而被丟棄。另外，要考慮在當前幀中出現的新特征點，既沒有舊特征點與之匹配，也不存在未匹配特征點與之高斯分布重合。

圖3 環境測量模型

2.3 重定位

定位技術的方法有很多，可以將其劃分為絕對定位和相對定位。絕對定位是以固定的坐標為參考，計算每次定位的絕對位置。相對定位以初始位置為坐標原點，依次估計相鄰兩次觀測點位間的相對運動，等效得到定位信息。但是，只是依靠連續幀簡單估計變換矩陣時，誤差累計會導致最終的定位發生嚴重偏移。重定位技術利用變換參考的方式很好地解決了這個問題，技術原理如圖4所示。

重定位的具體方法是在每次位姿估計之前對當前幀的光照條件進行一次估算，將滿足閾值的圖像作為關鍵幀抽選出來，作為接下來三維幀位姿估計的參考。這種方式的實質是將連續的相對位姿估計轉化成可變窗口的短時絕對定位，而參考的定位點是窗口的第一幀圖像的位置。在后端的優化過程中，參考幀組成的稀疏圖像集會被看作運動的節點，進一步優化估計的結果，從而實現對運動噪聲的一種數字化濾波。由于關鍵幀的低數量級和可存儲的特點，優化可以高效完成，不需要再在整段圖像序列上進行操作，節約了后端處理的時間。

圖4 重定位技術

2.4 圖優化

位姿圖優化是利用圖論的算法優化位姿的手段。在RGB-D SLAM V2中，圖優化只優化位姿圖，并沒有優化三維圖像點。具體實現中，估算的位姿被畫成了一個全向圖，使用g2o圖優化庫來對被環境測量模型接受的運動估計結果進行噪聲濾除，將兩幀相機位姿作為優化頂點，將它們之間的運動估計結果作為優化邊，利用兩者的約束關系，在多次迭代后得到最佳的估計。值得注意的是，后端模塊對檢測到的回環也加入優化頂點和邊，實際上對全局優化進行了一定的矯正。

優化邊的誤差函數：

式中，xi、xj為優化頂點，即位姿的估計值；zij是約束，也就是xi和xj之間的變換；e(xi,xj,zij)是滿足xi到xj的誤差程度；中間的Ωij是優化邊的信息矩陣（協方差矩陣的逆），表示對邊的精度的估計。

2.5 回環檢測

回環檢測是利用運動中相同路標的重復出現對軌跡進一步優化的方法，實質是一種采用反饋調節消除誤差的策略。實現回環檢測的關鍵是要依靠特征點包含的信息。健壯的圖像特征點要在保證視角變換和旋轉的情況下仍能被檢測出來，并具有相同的描述信息，因此每個特征點要同時包含點位的坐標及其描述子，用來完成特征的準確匹配?；丨h檢測就是利用特征點描述子的重復出現來判別機器人是否來到了自己走過的路線。

RGB-D SLAM V2系統的回環檢測使用基于最小生成樹的方法——一種隨機森林的隨機回環。算法使用描述子之間直接相減的差值，產生了一棵有限深度的最小生成樹，去掉連續多幀內的最近點來避免重復比較，再隨機從樹上選k個偏向于更早時間的幀來判斷閉環是否滿足。

2.6 建 圖

RGB-D SLAM V2系統適配點云地圖和八叉樹地圖兩種地圖。點云是把二維像素三維化后得到的結果，使得所有像素點的空間位置被直白表示出來。在無失準的情況下，點云保留了觀測的所有信息。經過空間變換擬合后的地圖最大限度地還原了原始環境，但是這對運算、存儲和顯示資源的消耗都是巨大的。一個小場景的點云地圖往往需要1×109bit的存儲，顯然后續應用的輸入是難以適用的。此外，這種地圖保留的信息未經進一步處理，后續信息的提取在輕量級平臺上也難以達到實時性。

對比而言，八叉樹地圖具有更高的應用優勢。如圖5所示，八叉樹地圖使用濾波方式，將設定分辨率內的像素轉換為單一的體素，利用算法中樹的結構，逐級把相鄰8個體素中顏色最多的體素作為上級分辨率節點形成一個八叉樹，從而將整個地圖更優化地存儲，提高地圖的可查找性和可利用性。

圖5 八叉樹地圖原理

3 性能評估

實驗階段，利用標準的數據集衡量了系統定位的準確性，評估輸出地圖的視覺效果。

MIT推出了使用RGB-D相機測量的數據集，結合高精度的慣導設備重現運動軌跡，用以驗證和評價SLAM算法的定位精度。由手持設備三維平行運動觀測（圖6、圖7）、圍繞目標運動觀測（圖8、圖9）和360°運動觀測（圖10、圖11）等場景測試結果可以看出，算法對光心移動的定位較為準確，基本可以還原真實的軌跡，對場景的構建較為真實，但是建圖存在區域性的空洞現象。造成這個問題的原因在于算法使用了直接耦合的方式，對觀測中的運動速度對數據的影響沒有進行適配，導致速度快速變化時像素點的漂移較大，使得擬合后的地圖具有空間點疏離的特點。

圖6 三維平行運動地圖構建效果

圖7 三維平行運動定位估計效果

圖9 圍繞目標運動定位估計效果

圖10 360°運動地圖構建效果

圖11 360°運動定位估計效果

4 結語

本文對RGB-D SLAM V2系統進行了詳細講解，并評估算法的數據集測試效果和真實場景的建圖效果。系統在基礎的視覺SLAM框架的基礎上，利用一些創新的輔助技巧提高了系統的容錯性，更健壯地完成了定位和建圖的整個流程，在適配的RGBD相機上性能表現優秀。從實驗結果可以看出，算法定位的精度基本擬合真實的運動，對環境的重現也可用于視覺觀測，具有極好的可擴展性。下一步工作將對定位和建圖的輸出結果應用機器人導航進行更加細致的研究。