基于RGB—D數據的SLAM算法

顏義鵬　許志強　翟漪璇　韓金鑫　成怡

【摘 要】本文提出了一種基于RGB-D數據的SLAM算法，通過對相機獲取的圖像進行ORB特征點的提取與匹配，估計相機運動關系，實現點云拼接，最后會得到全局一致的點云地圖和軌跡。為了消除誤差積累引起的干擾，引入通用圖優化庫g2o，得到光滑的優化軌跡；在后端回環檢測的過程中，引入關鍵幀選取機制，提高點云地圖的生成效率，減少其消耗的存儲空間。實驗結果表明，本文的研究方法在RGB-D SLAM 算法中具有可行性，并且能夠滿足實時性要求，具備較高的精度。

【關鍵詞】RGB-D；ORB特征點提取；SLAM；圖優化

中圖分類號： TP391.41 文獻標識碼： A 文章編號： 2095-2457（2018）14-0152-002

DOI：10.19694/j.cnki.issn2095-2457.2018.14.069

SLAM algorithm research based on RGB-D data

YAN Yi-peng XU Zhi-qiang ZHAI Yi-xuan HAN Jin-xin CHENG Yi

（Tianjin polytechnic university，Tianjin 300387， China）

【Abstract】This paper proposes a SLAM algorithm based on RGB-D data. By extracting and matching the ORB keypoints of the images obtained by the camera， the camera motion relationship is estimated and the point clouds are stitched. Finally， a globally consistent point cloud map and trajectory are obtained. In the process of detection of the backend end loop closure， a key frame selection mechanism is introduced to improve the generation efficiency of the point cloud map to reduce the amount of storage space. The experimental results show that the research method of this paper is feasible in the RGB-D SLAM algorithm， and it can meet the real-time requirements with high accuracy.

【Key words】RGB-D； ORB keypoint extraction； SLAM； Graph optimization

0 引言

因激光雷達具有測距精度高的特點，所以早期的Slam多使用激光雷達作為傳感器，但激光雷達存在價格昂貴、數據難以處理等固有缺陷。近年來隨著Kinect等深度攝像機的發展，由深度攝像機采集而來的RGB-D圖像越來越多的被用于SLAM領域。

通常情況下，RGB-D SLAM方法包括前端和后端兩部分。前端的主要任務是對RGB-D相機獲得的彩色圖像和深度圖像進行預處理，從而獲得點云數據；首先根據RGB-D相機提供的彩色圖像提取圖像的特征點，進行特征匹配；然后根據特征匹配的結果以及RGB-D相機提供的深度圖像估算兩幀圖像對應相機位姿的變換關系。后端的主要任務是通過圖優化方法，減小軌跡估計過程中出現的非線性誤差，從而獲得更加精確的相機位姿。

1 特征點提取與匹配

常用的特征點提取算法包括SIFT、SURF、ORB等。考慮到RGB-D SLAM算法對實時性的要求較高，因此本文在圖像特征點提取與匹配環節采用ORB方法。ORB特征提取方法由Rublee等于2011年提出，其全稱是Oriented Brief，由于其計算速率更快、精度更高且具有更好的魯棒性（尺度和旋轉不變性），因此被廣泛應用在圖像處理研究領域中。

設F1和F2為相鄰兩幀，它們的兩組一一對應的特征點為：

P={p1，p2，…，pn}∈F1（1）

Q={q1，q2，…，qn}∈F2（2）

求解旋轉矩陣r和位移矢量t，使得：

？坌i，pi=Rqi+t（3）

然而由于誤差的存在，等號不可能成立，因此通過最小化一個誤差來求解r，t：

此問題可以用經典的ICP算法求解，本文調用Opencv中的函數求解此問題。

2 點云拼接

點云的拼接，實質上是對點云做變換的過程。這個變換可用變換矩陣（transform matrix）來描述：

該矩陣的左上部分是一個3×3的旋轉矩陣，它是一個正交陣。右上部分是3×1的位移矢量。左下是3×1的縮放矢量，在SLAM中通常取成0。右下角是個1.這樣的一個陣可以對點或者其他東西進行齊次變換：

由于變換矩陣結合了旋轉和縮放，是一種較為經濟實用的表達方式。它在機器人和許多三維空間相關的科學中都有廣泛的應用。

3 后端優化

3.1 關鍵幀

在RGB-D SLAM算法中，若是把機器人采集到的所有圖像均用來生成點云地圖，這種做法有些欠缺。因此，在標準的RGB-DSLAM 算法中會有回環檢測這一環節。由于用所有的圖像進行拼接時會產生很大的累積誤差，并且計算量較大，降低該算法的實時性。因此本文考慮在回環檢測環節使用提取圖像關鍵幀的方法進行點云地圖的拼接，提高系統的準確性。同時，在回環檢測的過程中，使用關鍵幀拼接點云還能顯著地減少計算量和壓縮存儲點云數據所消耗的空間。

3.2 位姿圖

完成圖構建后，通過優化位姿圖可以求解相機的最優位姿。常用的圖優化方法包括非線性最小二乘法、基于松弛的優化方法、基于隨機梯度下降法以及流形優化方法。非線性最小二乘法、基于松弛的優化方法和基于隨機梯度下降法都是在歐氏空間中進行優化，但機器人的真實位姿變化卻是在非歐氏空間中的。為了防止歐氏空間中奇異值的出現，本文使用四元數來表示機器人的位姿變化并在流形空間中進行優化。

4 實驗與結果分析

本文實驗操作環境基于ROS（Robot Operating System）開源機器人操作系統，算法代碼為C++，均可通過gcc編譯。實驗運行的硬件設備平臺是配置為4.00GB內存和1.70G Hz主頻的32位Ubuntu 14.04 PC。實驗使用的實驗數據來自于標準開源測試數據集nyuv2。從中選取780幀彩色圖像與780幀深度圖像。彩色圖像與深度圖像均為640480的png格式。

本文首先分別選取較為相鄰的彩色圖像與深度圖像各兩幀，其中每一張彩色圖形都有與其相對應的深度圖像。如圖1和圖2所示。

使用ORB特征提取與匹配算法對圖1和2的圖像進行特征點的提取和匹配，匹配之后的結果如圖3所示。通過前述的實驗結果，可以計算出兩張圖像間的運動關系。將提取出的圖像特征點的二維像素坐標轉換為三維點云坐標，并采用相機內參參數，構建相機矩陣。調用Opencv中的SolvePnPRansac函數求解pnp，得到旋轉向量rvec和平移向量tvec，然后再組裝成變換矩陣，將一幀幀圖像轉換為點云，最后拼接起來就得到了點云圖，如圖4所示。本篇文章的實驗為了能夠保證局部運動的正確性，添加了回環檢測環節，識別機器人曾經到過的地方，生成的點云地圖如圖5所示。對比可知，經回環檢測，采取保留關鍵幀的點云地圖的處理效果較好，采用g2o圖優化工具求解的優化軌跡運動圖，如圖6所示。

5 總結

本文采用標準開源測試數據集nyuv2并對文中提出的一種基于RGB-D SLAM算法進行了一系列的實驗驗證：基于ORB算法的圖像特征點提取與匹配、估計圖像間的運動關系、生成點云地圖與圖優化軌跡。并在生成點云地圖部分，將未引入回環檢測環節與引入回環檢測環節生成的點云地圖進行了對比分析。結果表明，本文的研究方法具有可行性。

【參考文獻】

[1]李海洋，李洪波，林穎，等.基于Kinect的SLAM方法[J]，中南大學學報：自然科學版，2013（S2）.

[2]賈松敏，王可，郭兵，等.基于RGB-D相機的移動機器人三維SLAM[J].華中科技大學學報：自然科學版，2014（I）：103-109.

[3]Gao X，Zhang T.Robust RGB-D simultaneous localization and mapping using planar point featuresfJl. Robotics&Autonomous; Systems， 2015， 72：1-14.

[4]Wang Y，Zhang Q，Zhou Y.Dense 3D mapping for indoor environment based on Kinect-style depth cameras[M].Robot Intelligence Technology and Applications 3.SDrinQer International Publishine. 2015：317-330.

[5]羅元，熊艷，張毅.基于改進迭代無跡粒子濾波的同時定位與地圖構建[J].光學 精密工程（增），2015，23（10）：559-565.

[6]Kohlbrecher S，Stryk O V，Meyer J，et al.A flexible and scalable SLAM system with full 3D motion estimation[C].IEEE International Symposium on Safety， Security， and Rescue Robotics.IEEE， 2011：155-160.