刘安睿劼，王耀力

太原理工大学 信息与计算机学院，太原 030024

1 引言

SLAM（Simultaneous Localization and Mapping）同时定位与地图构建是机器人自主导航中的一个非常重要的领域，它是机器人在未知环境中通过传感器信息确定自身的空间位置，并建立所处空间的环境模型。随着机器人技术的不断发展，作为机器人的一个重要分支，SLAM技术也越来越重要。无论是室内机器人导航还是旋翼无人机自主导航又或是现在火热的自动驾驶，SLAM 技术都作为高效的建图和定位工具。传统的SLAM 技术大多采用激光雷达或者声呐系统进行二维地图的创建[1-3]。虽然现在激光雷达在室内和室外应用不惧任何挑战，但是由于激光雷达成本过高，所以它不能够在低成本系统中应用。因此SLAM 领域出现了以视觉传感器为基础的VSLAM（Visual Simultaneous Localization and Mapping）。VSLAM 能够对环境中的复杂结构进行完整的表示。现在比较流行的系统有MSCKF[4]、OKVIS[5]、LSD-SLAM[6]、ORB_SLAM2[7-8]、VINS-Mono[9]等。但是这些系统只是简单地输出了位姿轨迹和环境三维点云，并没有具体构建能用于机器人导航的地图。

Santana 等人[10]提出了一种基于单目相机使用单应矩阵构建2D 地图的方法，该方法在分割的时候判断图像的特征点是否在同一平面来快速判断是否有障碍物，但是由于单目视觉没有尺度信息，所以建立的2D 栅格地图并不适用于现实中的机器人定位和导航。Dia 等人[11]提出了一种新型的用于占有网格地图的逆传感器模型（ISM），但是并没有SLAM 方面的实际应用。Hull[12]提出了一种基于LSD-SLAM 的实时占据网格建图，与文献[10]的方法类似，使用平面信息构建单目SLAM 地图，利用点云地图投影的方式生成栅格地图。Gregorio 等人[13]提出了一个高效的机器人导航建图框架，一种3D 栅格地图构建算法，但是其计算量太大，对于大型机器人系统来说，并不希望部分功能占用太多系统资源。

ORB-SLAM2[7-8]是一个比较完善的VSLAM 系统，包含跟踪、局部建图、回环检测和全局BA，并且提供RGB-D相机的使用接口，但是它只能输出关键帧、稀疏点云和位姿信息。稀疏点云并不能用于轮式机器人的导航，因此针对ORB_SLAM2 的这些缺点，利用它提供的关键帧、位姿信息以及RGB-D 相机接口，通过3D 栅格地图构建算法生成可用于轮式机器人导航的3D栅格地图。

2 逆传感器模型

机器人在没有先验的未知环境中，通过自身的传感器建立考虑噪声和不确定因素的概率分布模型来估计外部环境，这个模型称为传感器模型（Sensor Model，SM）。在传感器观测到外部环境数据之后，利用这些数据建立环境地图的模型称为逆传感器模型（Inverse Sensor Model，ISM）

逆传感器模型最初由Elfes[14]在1989 年提出，后来由Thrun[15]进行了详细推导。ISM模型主要是利用传感器的测量数据来表示占用网格地图的单元格是否被占用的概率。从传感器角度来说，它与测量的障碍物之间的所有单元格都需要一个概率值，该概率值表示空闲或被占据的二元状态的可能性。超出障碍物的任何单元格或者并没有被传感器观测到的单元格都可以被视为未知。传感器测量通常会受到某种形式的噪声和不确定性的影响，所以单元格的概率状态分配有助于将噪声和不确定性结合到地图中[16]。

2.1 理想的逆传感器模型

进行ISM 模型建立之前，先需要做出一些假设，使得传感器模型易于构建。

假设1 独立性假设。每个单元格的占用概率独立于地图中的每个其他单元。

假设2 使用高斯分布来表示观测数据的噪声。

噪声测量的高斯概率分布函数，用于模拟传感器测量的噪声。方差σz表示测量z的不确定性，主要由正态分布曲线的宽度表示。方差越大表明测量的不确定性越大，导致曲线越宽。相反，较小的方差表明测量的不确定性较小，曲线较薄。

理想的逆传感器模型是在无噪声、完美的传感器的前提下建模的。如图1 所示。在网格地图中，mi代表相应的网格单元是否被占用的随机变量。P(mi)代表i单元格的占用概率。P(mi)=0 代表绝对空闲，P(mi)=1代表绝对占据，P(mi)=0.5 代表未知。在这种理想情况下，在障碍物或距离测量之前的每个单元被设置为空闲，并且在障碍物之后的每个单元被设置为未知。在障碍物的实际范围测量中，设置缓冲范围L以考虑测量不以网格内的单元为中心的可能性。使用进行测量的单元的中心计算到传感器的距离。理想情况下，在此单元格内的任何位置进行的测量都会为此单元格指定1。

理想的逆传感器模型可表示为：

其中，r表示传感器的观测范围，L为缓冲范围；区间内g(r)=0 表示传感器没有观测到障碍物。z-区间内g(r)=1 表示障碍物已经被传感器观测到。otherwiser>0 区间内g(r)=0.5 代表未知，传感器无法对该区域进行观测。r必须为正的，因为传感器永远不应该返回它后面的测量值。缓冲范围L用来考虑测量不以网格内的单元为中心的情况。通常L的大小选择为单元格对角之间的距离。

范围测量z包含高斯噪声，因此可以写成：

标准的噪声测量的高斯概率分布函数如图2所示，其中r为沿测量线到测量z的范围，所以：

图2 具有正态分布不确定性的高斯概率分布函数（方差σ=0.5）

2.2 实际逆传感器模型

实际中的ISM 模型是将高斯噪声融入理想传感器模型得到的，所以对它们进行卷积即可得到实际ISM模型：

由公式（1）能很明显地看出来g(r)是分段函数，因此必须以分段的方式进行模型的卷积，这意味着g(r)必须分割积分，因此完整的卷积写成：

其中r的范围为(a,b)，并定义分段代表的单元格占用概率，所以可以得到：

取误差函数：

所以公式（7）可以写成：

这时候考虑g(r)的分段特性，由公式（1）给出，所以可以得到完整的分段卷积：

公式（11）显示了最终推导出来的逆传感器模型公式。

高斯逆传感器模型如图3所示。

2.3 逆传感器模型参数选择

2.3.1L值的选取

在评估L值对ISM模型的影响时，采用固定变量的方法。首先固定z=2 m 值和高斯噪声模型σ=0.15，通过改变L值的大小，评估ISM 的变化。不同L值对ISM模型的影响，如图4所示。

图4 显示了不同的L值如何缩小或者加宽曲线的峰值。随着L值的增大，波峰高度在不断上升，说明ISM 模型的网格单元被占用的概率在不断增大。因此如果L值过大，那么将会有更多的单元格被判断为有障碍物，现实中没有障碍物的地方可能会被判断成有障碍物，造成比较大的偏差。如果L值过小，就会释放比较多的单元格变成未占用，可能会把有障碍物的单元格释放，造成误差。通过实验，当L的取值为单元格的对角线长度的时候，概率偏差最小[17]。

2.3.2σ值对ISM的影响

在研究σ值对ISM 的影响时，先固定变量z=2 m和缓冲范围L=0.5 m，通过改变σ的不同值，评估ISM模型。对于ISM 模型来说标准偏差σ表示占用预测的不确定性。如图5所示，较小的σ值对应较高较窄的峰值，较大的σ值对应较低较宽的峰值。随着σ值的不断增大，不确定因素和噪声的影响越来越大，实际测量周围的更多单元可能被认为是占用的。经过实验测试，σ=0.15 时，构建出来的地图效果最好[17]。

2.3.3观测范围的不确定性

对于不同的观测范围z，也会影响ISM模型，如图6所示。从图中可以看出，随着观测范围的不断扩大，相同模型的网格单元被占用的概率越来越小。这可能导致实际包含障碍物的单元被分配的概率低于总测量范围的峰值概率，ISM模型的判断会越来越不准确。经过实验测试，观测范围z=0.3～5 m 时，ISM 模型判断准确率最高[17]。

图3 卷积得到高斯逆传感器模型(z=2 m,L=1 m,σ=0.5)

图4 参数L 的不同取值对ISM模型的影响（z=2 m,σ=0.15）

图5 σ 值对逆传感器模型的影响（z=2 m,L=0.5 m）

图6 测量距离的变化对ISM模型的影响（L=0.4 m）

3 栅格地图模型

栅格图旨在将环境划分为均匀大小的单元网格。网格单元表示环境的一部分，并且可以存储是被占用、空闲还是未知相对应的信息。在占用的情况下，它表示在单元格内存在障碍物，并且对于路径规划算法或机器人，该空间不可穿越；在空闲的情况下，环境的这一部分是可穿越的，没有障碍；在未知的情况下，没有关于该区域的环境状况的信息，表示需要探索该区域。根据环境所需的细节不同，可以将网格划分为各种分辨率，例如1 cm 至5 m 的范围内。网格单元可以各自包含被占用概率的值，其中概率值的范围从0（空闲）到1（占用），而值0.5 意味着单元的状态不明，这允许将具有噪声或不确定性的传感器测量建模到地图中。

理想情况下，从开始到时间t的给定传感器测量值z和位姿估计x已知的情况下，系统将计算地图m上的后验分布：

为了计算方便，把所有相对于机器人位姿x的传感器测量值z转换为世界坐标系中的坐标，因此可以将位姿并入测量值z。消去x得到如下公式：

栅格地图m可以被划分成i个大小相等的网格单元，其中单个单元格可以表示为。每个单元格mi被占用的概率（简写成（简写成）表示单元格空闲的概率。对于拥有1 000 个单元格的网格地图来说，它拥有21000种可能，这个计算量过大，因此将单元格独立统计是一个比较好的选择。公式（12）可以写成：

把公式（16）代入公式（15）可得：

根据相同原理互补概率为：

为了方便计算，可以用公式（17）除以公式（18），可得：

为了避免概率在接近0 或1 时数值不稳定，对公式（19）取对数：

4 ORB_SLAM2栅格地图

ORB_SLAM2 是 2017 年 Raul Mur-Artal 等人提出的一个完整的SLAM 系统，它在ORB_SLAM 的基础上增加了双目和RGB-D 相机的使用算法，并且增加了重定位模块。该系统包含地图复用、闭环检测以及重定位功能，并且可以在CPU中实时工作。

ORB_SLAM2 系统有三个主要的并行线程：（1）跟踪线程。跟踪线程通过寻找与本地地图匹配的特征，应用BA（Bundle Adjustment）最小化重投影误差，实现了相机每帧的定位。（2）局部地图线程。局部地图线程映射管理局部地图并对其进行优化，执行局部BA 优化。（3）循环关闭线程。循环关闭线程检测大循环并通过执行姿势图优化来校正累积漂移。该线程启动第四个线程以在姿势图优化之后执行完整BA，以计算最佳结构和运动解决方案。RGB-D相机预处理如图7所示；ORB_ SLAM2系统框架如图8所示。

图7 ORB_SLAM2 RGB-D相机预处理

图8 ORB_SLAM2算法框架

本文算法以ORB_SLAM2 为前端，ROS 为中间层，ISM和栅格地图构建算法为后端，实时构建能用于机器人导航的3D栅格地图。前端ORB_SLAM2利用RGBD相机完成关键帧选取、相机位姿估计以及稀疏点云的计算，并在ROS 上发布这些话题。ISM 算法从ROS 订阅这些数据，作为传感器观测数据zt和机器人姿态xt，进而通过栅格地图构建算法生成所需的地图，地图采用八叉树进行储存以减少存储空间和内存使用[18]。具体流程图如图9所示。

图9 ORB_SLAM2实时栅格地图构建算法流程图

5 实验

5.1 实验说明

本文采用ROS 系统作为中间平台，采用Rviz（Ros Visualization）作为实时显示平台。本文实验Rviz 中地图网格大小选取为1 m2。3D 栅格地图精度（单元格大小）决定了地图的精细程度，体现更多的细节单元格需要小一点（不小于0.01 m），如果为了体现地图的大轮廓，那么单元格就要选择的大一点。本文所有实验中地图单元格都选取为0.05 m，因此根据本文2.3.1小节可得，σ取值为经验值0.15。

5.2 TUM数据集实验

慕尼黑工业大学（TUM）的计算机视觉实验室提供了RGB-D 大型数据集。数据集采用Microsoft Kinect传感器以全帧速率（30 Hz）和传感器分辨率（640×480）记录彩色图像和深度图像数据，并且提供了地面实况轨迹。本文采用轮式机器人所采集的数据集rgbd_dataset_freiburg2_pioneer_slam作为本次实验图像输入。RGB-D相机运动轨迹及点云图如图10所示。图10显示了轮式机器人的运动轨迹以及稀疏点云，但是由于点云太过稀疏无法进行导航以及路径规划。实时生成的3D 栅格地图如图11所示。该图能够清楚地表示出障碍物所在的位置，以及房间的墙面信息，但是障碍物和墙面略有不规则是因为在计算特征点的时候，有的地方缺少纹理，有的地方超出深度传感器的探测范围，所以无法计算出足够的特征点，因此输入到ISM 模型中的数据有部分偏差，使得生成的墙面和障碍物有些不规则。图11 下方没有墙面是因为数据集中并没有拍摄到该处的图像。

图10 rgbd_dataset_freiburg2_pioneer_slam运动轨迹及点云图

图11 rgbd_dataset_freiburg2_pioneer_slam 3D栅格地图

图12 机器人仿真环境

5.3 仿真轮式机器人实验

本文采用中科院软件所-重德智能机器人联合研究中心提供的机器人仿真环境，该环境包含一个完整的机器人系统。仿真环境由Gazebo构建，提供对外接口，能够通过ROS获取RGB图像数据和深度图像数据。本次实验利用该仿真环境，并与ORB_SLAM2 进行融合，通过键盘控制轮式机器人的运动。仿真环境如图12 所示，运动轨迹及点云图如图13 所示，实时3D 栅格地图如图14 所示。图14 能够非常完美地显示该仿真环境，因为该仿真环境有大量纹理丰富的墙面以及物品，并且没有特别多的杂物。在图中左下角缺少一块是因为该处墙面是光滑的。没有任何纹理存在，因此ISM模型没有接收到任何数据，也就无法生成地图。

5.4 室内轮式机器人实验

图13 仿真机器人运动轨迹及点云图

图14 仿真机器人实时3D栅格地图

对于室内的轮式机器人实验，实验设备如图15 所示，采用三轮全向轮底盘，最大移动速度0.5 m/s，最大角速度180（°）/s。采用DELLG3（i5-8300H 3.3 GHz 8 GB内存）笔记本电脑作为轮式机器人的控制器。实验摄像头为Microsoft KinectV2 RGB-D 相机，彩色图像分辨率为960×540，深度图像为320×240，摄像头采集频率为30 Hz。本实验在Ubuntu16 中利用ROS 系统作为平台进行 RGB-D 相机、ORB_SLAM2 以及 ISM 模型之间的通信。实时3D栅格地图由ROS系统内的Rviz显示。

图15 室内实验设备

室内实验场景如图16 所示，室内运动轨迹及点云地图如图17 所示，室内实时3D 栅格地图如图18 所示。室内实验生成的栅格地图能够清晰地看到障碍物和两侧的板子，能够很好地反映室内环境。通过室内实验可以看出，对于有丰富纹理的室内环境能够建立准确的3D 栅格地图，能够反映障碍物所在的空间位置。但是对于纹理不丰富的障碍物侧面以及光滑反光的钢板，就没办法获得充足的信息，因此本文的算法对于这种情况并没有办法建立栅格地图。

图16 室内实验场景

图17 室内机器人运动轨迹及点云图

5.5 算法对比分析

本文采用2017 年Gregorio[13]等人提出的Skimap 算法作为对比方法。Skimap是一种高效的机器人导航算法地图结构，能够快速获取3D地图、2.5D高度地图以及2D 占据地图。Skimap 为导航映射框架，依赖于前端提供的位姿、RGB 图像以及深度图像。因此对比实验采用ORB_SLAM2 作为前端，Skimap 作为后端地图生成算法。对比图如图19所示。

图18 室内机器人实时3D栅格地图

图19 室内机器人实时Skimap 3D栅格地图

为了评估地图的精度，本文采用长度误差lerror与角度误差θerror作为评价指标。分别从本文算法建图（图18）以及Skimap 算法建图（图19）中取出五段线段以及三个夹角（如图18），对比实际场景得到误差值，如表1所示。

表1 地图误差值

从建图的精度上看，本文算法精度和Skimap 算法精度都处于2%之内，本文算法精度略高于Skimap算法精度。ORB_SLAM2 算法有闭环检测及全局优化模块[8]，并且本文在进行实验的时候有较好的闭环条件，能够很好地减小累计误差，进一步提高了建图精度。

实时性方面，本文实时性略差于Skimap，由文献[13]可知Skimap算法树搜索时要快于八叉树和KD树，但在实验过程中Skimap算法速度为10 Hz，本文算法处理速度为10 Hz，实时性基本满足要求。

6 结束语

针对以往的视觉SLAM 系统只能生成位姿以及稀疏点云图的缺陷，本文在ORB_SLAM2 的基础上，提出了一种3D 栅格地图的实时构建算法。该算法建立了适用于视觉SLAM的逆传感器模型（ISM），详细推导了栅格地图的生成机制，并利用算法流程图清晰地表示ORB_SLAM2 与ISM 结合生成3D 栅格地图的具体流程。通过数据集实验、仿真实验以及室内轮式机器人实验，表明ISM 模型与ORB_SLAM2 的结合具有较强的实用性和较高的建图精度，并能够实时构建3D 栅格地图。