基于立体视觉的2D 占据网格地图构建

王永祥，常 青，黄良红，孙永明

（1.太原理工大学信息与计算机学院，山西晋中 030600；2.民航山西空管分局，山西太原 030031；3.山西省林业科学研究院，山西太原 030000）

科学技术的快速发展，对机器人的要求也越来越高，机器人不仅可以代替重复的工作，还可以通过传感器采集数据，判断决策。当在未知环境中工作时，就必须对自身进行定位，并且对当前所处环境建图。机器人同时定位与建图(Simultaneous Localization and Mapping，SLAM)技术可以使机器人得到未知环境的信息。机器人在未知环境中通过传感器测量得到自身在环境中所处的位置和周围的环境信息[1-3]。当前，根据传感器的不同，SLAM 算法大致可分为视觉SLAM[4]与激光SLAM[5]，采用激光SLAM 算法构建2D 占据网格地图相对成熟，但是激光测距仪价格昂贵，很难适用于低成本设备。相机成本低，而且采集的视频数据描述的信息丰富。视觉SLAM 算法只能输出稀疏点云地图，比较典型的方案有DSO[6]、ORBSLAM2[7-8]、VINS-mono[9]、LSD-SLAM[10]等，但这些开源算法并不适用于机器人进行路径规划、导航任务。所以要建立可用于机器人导航避障的2D 占据网格地图[11-13]。

文献[14]中将RGB-D 相机的深度图像转换为laser 数据，来代替昂贵的激光传感器构建网格地图，但是RGB-D 相机在室外使用时易受环境光影响[14]。文献[15]中提出了一种方法来建立单目相机的逆传感器(Inverse Sensor Model，ISM)概率模型。评估了传感器精度和网格分辨率变化对逆传感器模型的影响，但并没有通过占据概率构建可用于导航的网格地图[15]。文献[16]中提出了一种基于LSD-SLAM 的实时占据网格建图，采用单目相机采集图像，利用点云地图投影的方式生成网格地图[16]。

1 基于立体视觉的逆传感器模型

1.1 双目立体视觉几何模型

双目相机类似人的眼睛，由左右两个相机组成，两个相机的光圈中心都位于x轴上，两个光圈中心的连线为双目相机的基线，它在相机出厂后就是固定的，真实尺度可根据这个基线计算，是双目相机的重要参数，立体视觉几何模型如图1 所示[17-19]。

图1 立体视觉几何模型

其中，f为焦距，b为基线，uL为左像素点的横坐标，uR为右像素点横坐标，d为左右像素点的横坐标之差，称为视差，z即为通过视差值计算出的测量距离。

1.2 双目立体视觉的逆传感器模型

逆传感器模型描述了传感器数据到地图的转换，传感器到障碍物之间所有的网格单元都需要填充一个概率值，通过这个概率得知这些单元网格是占据还是空闲状态的可能性，而没有被传感器探测到的单元网格则被描述为未知状态。单元网格的概率状态分布有助于在地图中引入噪声和不确定度。传感器测量的噪声与不确定度通常会影响测量结果，所以，在设计逆传感器模型的时候，需要将噪声与不确定度考虑进来。

深度图像的每个像素到物体的距离都被认为是逆传感器模型的测量距离。假设空间中的一点P在相机坐标系下的坐标为Pc=(xc,yc,zc)T，测量距离lP定义如下：

理想状态下的传感器模型为：

Pmin是单元网格中的初始概率，为了减少不确定度的影响，在此引入了一个二次指数函数来估计测量距离。

那么实际环境中双目视觉系统单次测量单元网格地图的占据概率为：

显著性因子k表明了单次测量的权重，它确保远处的测量结果影响小，近处的测量结果影响大，ΔlP为深度相关不确定度，lP为测量距离，r表示自变量。

1.3 逆传感器模型参数讨论

1.3.1 显著性因子k对ISM的影响

在讨论k值影响的时候，采用控制变量的方法，设定测量距离z=3 m，深度相关不确定度ΔlP=0.1。通过改变k值来分析ISM 的变化，如图2 所示。

图2 显著性因子k对ISM的影响

由图2 可知，显著性因子k值越大，测量结果的概率就越大，k值越小，测量结果的概率就越小。它描述了单次测量的重要性，所以选择一个合适的k值对于传感器模型的好坏至关重要。经过实验分析，当k=0.1 时，逆传感器模型的效果较好。

1.3.2 深度相关不确定度ΔlP对ISM的影响

在讨论深度相关不确定度对ISM 模型的影响时，同样采取控制变量的方法，显著性因子k=0.1，测量距离z=3 m，通过选取不同的ΔlP分析ISM 的性能，如图3 所示。

图3 深度相关不确定度ΔlP对ISM的影响

由图3 可知深度相关不确定度ΔlP越高，波峰越低，同时波峰也越宽，而不确定度ΔlP越低，波峰越高，同时波峰也越窄。

1.3.3 测量距离z对ISM的影响

由于测量的距离不同，就会导致测量不确定度不同，那么测量结果也就不同，由于z的变化导致了ΔlP的变化，选取k=0.1，通过z与ΔlP之间的变换关系计算出ΔlP，如图4 所示。

图4 测量距离z对ISM的影响

由图4 可知，测量值越小被占用的可能性就越高，并且每次测量的概率都会随着距离的增大而减小，直到到达最大的距离为止，此时单元格变得未知。

2 2D网格地图模型

占据网格地图的目的是将环境分成尺寸大小均匀的单元，这些单元存储着环境的相关信息，通过这些信息来判断当前环境状况，环境有3 种状态：占据、空闲、未知。

通过传感器的测量值z以及从开始到t时刻的姿态x，就可以计算地图m的后验概率，可以表示为p(m|z1:t,x1:t)。

假设当前处于世界坐标系下，同时传感器测量值与机器人姿态x相关，那么将姿态合并到测量值中去，则地图m的后验概率又可以表示为p(m|z1:t)。

如果地图中的单元网格数量很多，则地图就会有很多种可能，会导致地图很复杂，所以就需要研究每一个单元网格的后验分布，地图m被分成相同尺寸的单元网格，将第i个单元网格的占据变量记为mi，p(mi=1)表示第i个单元网格被占据的概率，p(mi=0)表示第i个单元网格空闲，第i个单元网格的后验分布表示为p(mi|z1:t)。

假设地图单元网格之间是相互独立的，就可以将地图m表示为每一个单元网格后验分布的乘积：

当传感器有了新的测量结果时，地图中的一部分单元网格的占据概率发生变化，就需要对地图进行更新。由于第i个单元网格不同时刻的传感器测量值z条件独立，故采用二进制贝叶斯滤波器来解决二进制状态表示的静态环境的占据概率，通过贝叶斯法可得：

同理可得第i个单元网格空闲时的概率为，与式（6）作商可得：

为了避免数值上的不稳定，将占据概率的值稳定地限制在0～1 之间，采用log-odd 率来表示占据概率，如下所示：

则式（7）可写为:

分析式（9）可知，L(mi|z1:t)代表的是地图更新后的第i个单元网格的占据概率，L(mi|zt)是通过传感器测量值得到的t时刻的概率，该概率可以通过ISM 传感器模型确定L(mi|z1:t-1)为前一时刻的占据概率，L(mi)是地图的先验信息。

3 基于ORB-SLAM2算法构建2D网格地图

3.1 ORB-SLAM2算法

ORB-SLAM2 算法是一个基于特征点匹配的实时SLAM 系统，室内外都可以使用，支持单目、双目以及RGB-D 相机，该系统分为跟踪、建图、重定位、闭环检测4 个部分。该系统能够实时计算出相机的轨迹，同时生成稀疏的点云地图。

3.2 2D网格地图的构建

采用ORB-SLAM2 作为视觉里程计，但是ORBSLAM2 只能生成稀疏点云地图，该地图并不能满足机器人对障碍物判断的需求，所以为了达到机器人避障的目的，需要提供丰富的环境信息，就需要得到一个2D 网格地图，可以利用ORB-SLAM2 作为一个视觉里程计，提供当前位姿。此外还需要在此位姿状态下的环境信息，利用双目相机提供深度图，来描述此位姿下的深度信息。而双目相机提供的深度图不能直接使用，需通过ROS 功能包DepthToScan 将双目相机提供的深度图转化为ROS平台下可使用的2D laser scan 格式的深度信息。每个关键帧对应一个当前位姿下深度图转化的2D laser scan 深度信息。

采用的双目相机为MYNTEYE s1030-IR，该相机通过一个ROS 节点发布左目、右目图像与深度图像等主题，ORB-SLAM2 算法将MYNTEYE 相机发布的主题作为ORB-SLAM2 系统的输入，输出当前相机的位姿x。该方案直接利用MYNTEYE 相机发布的深度图像，深度图的计算是通过相机内部计算的，该方案并没有再次通过视差图来计算深度图，这样大大减少了因计算深度图而消耗的时间，得到的深度图通过DepthToScan ROS 功能包转化为Scan 格式的数据。ISM 逆传感器模型通过相机位姿x与Scan深度数据，计算得到占据概率，并通过ROS 平台下Occupancy Grid Map 构建2D 网格地图，算法流程如图5 所示。

图5 基于ORB-SLAM2算法构建2D网格地图算法流程图

4 实验分析

4.1 室内环境实验

室内实验场景选择在实验室，如图6 所示。

图6 室内场景图

通过ORB-SLAM2 算法构建了实验室的稀疏点云地图与相机的运动轨迹，如图7 所示，可以看出，该地图并不适应于机器人导航。

图7 ORB-SLAM2点云地图

通过文中算法构建该场景下的2D 网格地图，可以看判断出障碍物的位置，有利于机器人的导航，如图8 所示。

图8 室内2D网格地图

4.2 室外环境实验

室外实验场景选取两边有树的公园小路，如图9所示。

图9 室外场景图

该场景下的稀疏点云地图以及运动轨迹如图10所示。

图10 ORB-SLAM2点云地图

通过文中算法构建该场景下的2D 网格地图，并在ROS 中的RVIZ 可视化平台下显示，可以判断出小路两边的障碍物被标记了出来，适用于机器人判断障碍物，如图11 所示。

图11 室外2D网格地图

4.3 对比分析实验

将文中建图算法与RTAB-map 建图算法选择室外环境2D 网格地图与真实环境进行对比，通过ROS平台下的RVIZ 可视化界面中的测量工具对2D 地图的3 处位置进行测量，并与真实环境下同样的位置进行对比，如图12 与图13 所示。

图12 室外2D网格地图距离测量

图13 RTAB-map网格地图

以真实环境作为参考，将测量结果进行对比，如表1 所示。

表1 2D网格地图与真实环境对比

由表1 可知，文中算法相比于RTAB-map 算法，精度提高了3%左右，而且测量值都比实际值小，表明机器人不会触碰到障碍物。所以此地图可用于室外机器人导航。

5 结束语

该文在ORB-SLAM2 算法的基础上采用MYNTEYE s1030-IR 相机构建可用于室外机器人导航的2D 网格地图，采用了适用于双目相机的ISM 模型，详细描述了地图的构建过程。通过与真实环境对比，所建地图的精度满足机器人的导航、避障。