基于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相机获取的关键帧环境信息以及相机位姿信息，实时生成稠密点云地图与栅格地图。实验结果表明，本文算法生成的地图，既保留了原有环境的地图结构，又降低内存占用，可用做机器人的路径规划研究，为室内移动机器人导航和避障方面提供技术支持。