基于ROS 的ORB-SLAM2 建圖算法研究

姜衍超，李天順，孫佳東，朱建軍

（吉林化工學院 信息與控制工程學院，吉林吉林，132022）

0 引言

同步定位與地圖構建（SLAM）[1]技術自出現以來，始終是計算機視覺和機器人領域的重點研究內容之一，國內外高科技企業一直保持對其高度關注。SLAM 技術不僅能夠實現地圖的構建，還可以實時估計機器人的位置。視覺SLAM將相機作為主要傳感器，相機不僅可以提供相對豐富的環境信息，而且價格相對較低，移動機器人通過視覺傳感器在新環境中估計自身運動軌跡，實現對周圍場景地圖的重建，目前已廣泛應用于VR、AR、三維重建、自動駕駛等場景。

目前經典VSLAM 算法有ORB-SLAM[2]、SVO[3]、LSDSLAM[4]、RTAB-MAP[5]等。ORB-SLAM 算法運用特征點法，所有步驟均使用ORB 特征，支持單目相機，魯棒性良好，結構清晰及運行復雜度較低，自提出以來一直是研究者們的重點關注對象。Mur-Artal 等人[6]在原有算法基礎上提出了ORB-SLAM2，該算法增加了雙目相機和深度相機模式，提高算法適用性，通過深度相機可以直接得到機器人與周圍環境的位置信息，減少初始化部分，提高算法精度以及準確性，但ORB-SLAM2只能構建稀疏的地圖，過于稀疏的點云地圖實用性較差，無法實現室內導航等實用性功能。針對ORB-SLAM算法的不足，高翔[7]修改增加點云地圖線程，使ORBSLAM 能直接顯示地圖環境點云信息，但無法直接應用于機器人導航，未添加地圖保存功能。稠密點云地圖包含地圖點數量多，信息含量豐富，包含地圖點的x、y、z、r、g、b 等信息，通過稠密點云地圖可以向其他地圖進行轉化，從而實現目標功能。由稠密點云地圖可以轉換為八叉樹地圖，能夠支撐三維空間內機器人進行路徑規劃，如無人機，機械臂等[8]；轉化為二維柵格地圖[9～10]，可以實現二維空間下的移動機器人路徑規劃。

故本文在ORB-SLAM2 算法的基礎上，引入構建稠密點云地圖算法和實時構建二維柵格地圖算法，生成稠密點云地圖以及二維柵格地圖，為實現室內移動機器人路徑規劃、導航、避障的功能提供技術支持。

1 ORB-SLAM2 算法原理

ORB-SLAM2 由跟蹤、局部建圖、回環檢測、全局BA、位置識別、地圖等部分組成，其中跟蹤線程、局部建圖線程和回環檢測線程為主要的并行線程。跟蹤線程（Tracking）根據到的每一幀圖像的特征點估計相機位姿，經過局部地圖跟蹤進行對相機位姿優化，并判斷當前幀是否能成為關鍵幀；局部建圖線程（Local Mapping）在跟蹤線程關鍵幀的基礎上，利用局部BA 優化關鍵幀位姿和地圖點，是用來管理和優化局部地圖；回環檢測線程（Loop Closing）通過計算關鍵幀與候選關鍵幀的位姿進行閉環融合，并執行位姿圖優化來修正累積誤差。算法原理框架如圖1 所示。

圖1 ORB-SLAM2 原理框架

2 改進ORB-SLAM2 原理框架

本文在ORB-SLAM2 算法的基礎上，通過添加兩大線程實現算法的改進，完成對周圍環境的地圖重建。改進主要框架如圖2 各個部分所示，本文算法方法由如下三個部分構成，圖中第一部分圓形框為原始ORB-SLAM2 算法部分，方形虛線框為原算法的基礎上新增的部分。

圖2 改進ORB-SLAM2 原理框架圖

圖3 原理示意圖

(1)RGB-D 模式的ORB-SLAM2 算法：獲取關鍵幀信息和地圖點。

(2)稠密點云地圖算法：提取關鍵幀的地圖點云數據，匹配各個關鍵幀點云數據進行濾波處理及稠密點云地圖拼接。

(3)柵格地圖算法：三維稠密點云向二維平面進行投影，依據占據柵格信息，重建環境的二維地圖模型。

■2.1 稠密點云地圖構建

由于ORB-SLAM2 生成的地圖點云稀疏，結構不清晰，因此在ORB-SLAM2 的基礎上通過稠密點云地圖線程獲取點云數據，實現稠密點云地圖構建。構建流程如下：新建pointcloudmapping.cc 文件，設定參數初始值如濾波分辨率resolution、讀取圖像步長skipSpan、機器人相機高度robotCameraHeight 等。在ORB-SLAM2 生成關鍵幀的同時，根據當前關鍵幀位置、機器人姿態，結合深度相機獲取的圖像數據生成當前關鍵幀點云數據。信息采集過程中，機器人獲取的彩色圖像提供當前關鍵幀圖像點信息的u(x)、v(y)坐標，深度圖像可提供機器人與環境的距離信息Z 坐標，根據相機參數K 結合采集的RGB-D 圖像信息，通過式1：

整理得式2：

得出像素點在相機坐標系下的X，Y，Z 坐標，即當前關鍵幀地圖點的信息。為減少噪音干擾，在不影響點云地圖結構特征的前提下，對構建的點云數據進行體素濾波處理，以體素中心點代替該體素區域點云數據，去除冗余點云，減少點云數據，形成當前關鍵幀點云數據。

利用ORB-SLAM2 的回環檢測線程和ORB-SLAM2 生成的關鍵幀位置信息，將所有關鍵幀點云數據通過PCL(Point Cloud Library)進行拼接，實現全局稠密點云地圖構建。并在ROS 中發布地圖信息話題，通過Rviz 中的PointCloud2訂閱話題信息，顯示生成的全局稠密點云地圖。

■2.2 柵格地圖構建

為實現室內移動機器人的導航功能，構建柵格地圖線程，將生成的稠密點云地圖實時轉換為柵格地圖，并添加地圖保存功能。柵格地圖由大小相同的標準單元格組成，根據單元格的數據構成地圖信息，單元格包含空閑、占據以及待觀測三種狀態。在柵格地圖中，單元格Gird(i)的狀態通過數值1、0 的概率來表示，當Gird(i)=1 時表示該單元格被占據，Gird(i)=0 時表示該單元格空閑，待觀測狀態不進行表示。如圖4 所示，在占據情況下，表示該單元格在垂直方向存在地圖信息；在空閑情況下，表示該單元格垂直方向沒有地圖信息，機器人可以通過；在待觀測情況下，則表示傳感器不能夠到該單元格狀態，即該單元格是否有地圖信息情況未知。

圖4 柵格地圖

構建流程如下：根據ORB-SLAM2 稠密點云線程生成的點云地圖，取所有點中最小的x、y 值和最大x、y 值，確定柵格地圖的xMin、xMax、yMin、yMax，構建地圖邊界。對點云地圖數據進行直通濾波處理，保留適配相機高度的點云數據，輸出新的點云信息。統計單位柵格cell 內點的數量，設置閾值為5，當地圖點的數量小于等于閾值時存儲cell 的值為0，認為該柵格沒有地圖信息；當點的數量大于閾值時認為cell 垂直方向存在地圖信息，占據整個柵格，存儲cell 的值為100，根據cell值不同確定柵格地圖信息，如圖5 所示。

根據xMin、xMax、yMin、yMax 確定柵格地圖的長和寬，根據不同位置單元格的數據構建柵格地圖，將生成的柵格地圖信息實時發布到ROS 的話題節點中，在Rviz 中顯示柵格地圖信息，通過map_server（地圖服務器）將柵格地圖進行保存。在仿真環境中導入地圖文件，可啟動移動機器人的路徑規劃功能進行目標點導航。

3 實驗結果與分析

為了達到本文的目的，即通過改進ORB-SLAM 2 算法構建柵格地圖，測試環境為筆記本電腦：Intel(R) Core(TM)i5-5200U CPU @2.20GHz，12G 內 存 與Ubuntu 18.04 操作系統。測試環境為Gazebo 搭建的室內仿真環境，使用Gazebo 中building editor 工具搭建仿真環境中所需墻體，調用模型庫添加室內常見物體，如書櫥、電視、門窗、壁畫等，仿真環境如圖6 所示。

圖6 Gazebo 仿真環境

在仿真環境中，控制裝配深度相機的室內機器人進行運動，對周圍的環境圖像進行采集，經過改進后的ORBSLAM2 算法，生成地圖效果如圖7～8 所示。圖7 為ORBSLAM2生成的地圖，圖8為改進后ORB-SLAM2生成的地圖。ORB-SLAM2 算法生成的稀疏點云地圖墻體、角落等部分地圖數據少，地圖結構缺失，無法完整表示地圖結構信息。算法改進后生成的地圖信息量豐富、結構清晰，地圖邊界明顯且完整，能夠有效完成對仿真環境的地圖重建。

圖7 ORB-SLAM2 方法

圖8 本文方法

通過map_server 保存生成的柵格地圖，保存的柵格地圖效果如圖9 所示。經與仿真環境對比，柵格地圖結構完整，有效標記椅子、書櫥、電視機柜等室內家具的地圖位置信息。

圖9 柵格地圖

實驗結果表明，經算法改進后生成的點云地圖和柵格地圖結構完整，有效表示地圖特征信息。在ORB-SLAM2 的運動模式下，調用生成的柵格地圖，即.pgm 和.yaml 文件，利用ROS 的move_base 功能包可以配置路徑規劃算法，如A＊、RRT、DWA 等，實現機器人的導航和運動控制。

4 結語

針對ORB-SLAM2 算法構建的地圖信息量少、無法保存且無法應用到機器人導航功能等問題，本文提出一種改進后的ORB-SLAM2 算法，該算法在ROS 環境下，結合RGB-D相機獲取的關鍵幀環境信息以及相機位姿信息，實時生成稠密點云地圖與柵格地圖。實驗結果表明，本文算法生成的地圖，既保留了原有環境的地圖結構，又降低內存占用，可用做機器人的路徑規劃研究，為室內移動機器人導航和避障方面提供技術支持。