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

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

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

0 引言

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

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

故本文在ORB-SLAM2 算法的基礎(chǔ)上，引入構(gòu)建稠密點云地圖算法和實時構(gòu)建二維柵格地圖算法，生成稠密點云地圖以及二維柵格地圖，為實現(xiàn)室內(nèi)移動機(jī)器人路徑規(guī)劃、導(dǎo)航、避障的功能提供技術(shù)支持。

1 ORB-SLAM2 算法原理

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

圖1 ORB-SLAM2 原理框架

2 改進(jìn)ORB-SLAM2 原理框架

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

圖2 改進(jìn)ORB-SLAM2 原理框架圖

圖3 原理示意圖

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

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

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

■2.1 稠密點云地圖構(gòu)建

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

整理得式2：

得出像素點在相機(jī)坐標(biāo)系下的X，Y，Z 坐標(biāo)，即當(dāng)前關(guān)鍵幀地圖點的信息。為減少噪音干擾，在不影響點云地圖結(jié)構(gòu)特征的前提下，對構(gòu)建的點云數(shù)據(jù)進(jìn)行體素濾波處理，以體素中心點代替該體素區(qū)域點云數(shù)據(jù)，去除冗余點云，減少點云數(shù)據(jù)，形成當(dāng)前關(guān)鍵幀點云數(shù)據(jù)。

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

■2.2 柵格地圖構(gòu)建

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

圖4 柵格地圖

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

根據(jù)xMin、xMax、yMin、yMax 確定柵格地圖的長和寬，根據(jù)不同位置單元格的數(shù)據(jù)構(gòu)建柵格地圖，將生成的柵格地圖信息實時發(fā)布到ROS 的話題節(jié)點中，在Rviz 中顯示柵格地圖信息，通過map_server（地圖服務(wù)器）將柵格地圖進(jìn)行保存。在仿真環(huán)境中導(dǎo)入地圖文件，可啟動移動機(jī)器人的路徑規(guī)劃功能進(jìn)行目標(biāo)點導(dǎo)航。

3 實驗結(jié)果與分析

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

圖6 Gazebo 仿真環(huán)境

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

圖7 ORB-SLAM2 方法

圖8 本文方法

通過map_server 保存生成的柵格地圖，保存的柵格地圖效果如圖9 所示。經(jīng)與仿真環(huán)境對比，柵格地圖結(jié)構(gòu)完整，有效標(biāo)記椅子、書櫥、電視機(jī)柜等室內(nèi)家具的地圖位置信息。

圖9 柵格地圖

實驗結(jié)果表明，經(jīng)算法改進(jìn)后生成的點云地圖和柵格地圖結(jié)構(gòu)完整，有效表示地圖特征信息。在ORB-SLAM2 的運(yùn)動模式下，調(diào)用生成的柵格地圖，即.pgm 和.yaml 文件，利用ROS 的move_base 功能包可以配置路徑規(guī)劃算法，如A＊、RRT、DWA 等，實現(xiàn)機(jī)器人的導(dǎo)航和運(yùn)動控制。

4 結(jié)語

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