移动机器人CartographerSLAM 建图改进研究

赵 芸，赵 敏

（上海理工大学光电信息与计算机工程学院，上海 200093）

0 引言

即时定位与建图（Simultaneous Localization and Mapping，SLAM）最早由Smith 等［1］在1988 年提出，它是机器人技术研究方向。机器人在应用中能自己移动的前提是需要扫描当下环境并生成地图。实现机器人SLAM 需要知道其在当前地图中的姿势和位置。因此，SLAM 是移动机器人领域的一个关键问题。

早期SLAM 研究多用激光雷达作为主要传感器，将扩展卡尔曼滤波（EKF）应用于机器人位姿估计，但效果不理想。对于一些强非线性系统，这种方法会带来更多的截断误差，从而导致定位和映射不准确。2007 年，Grisetti 等［2］提出改进的Rao-Blackwellized particle filter（RBPF）激光雷达Gmapping 方法，这在SLAM 领域堪称里程碑通过改进所提出的分布和自适应采样技术，提高了定位精度，降低了计算复杂度；优化方法作为概率方法的一种有效替代，近年得到广泛应用；2010 年Konolige 等［3］提出一种具有代表性的方法Karto-SLAM，该方法利用稀疏位姿调整解决非线性优化中的矩阵直接求解问题；2011 年提出的Hector SLAM［4］采用高斯—牛顿法解决扫描匹配问题，该方法不需要里程表信息，但需要高精度的激光雷达；2016 年，谷歌提出Cartographer［5］，通过对子图和全局图同时使用激光闭环，减少累积误差。

本文针对Cartographer SLAM 算法在回环匹配时使用Scan to Map（帧与子图）方法容易在环境相似的地方或者长距离走廊等处出现匹配出错高的缺点，提出改进方法，将Scan to Map 用Map to Map（子图与子图）方法替代，优化Cartographer SLAM 算法构建更精确的地图；阐述了Map to Map 方法理论，利用实验对比改进前后算法，对实验结果进行分析。

1 Cartographer SLAM 算法

在分析Cartographer SLAM 算法之前先分析基于图优化的SLAM 框架原理。图优化的SLAM 是通过后端的回环检测对前端位姿进行调整，最终得到当下最接近真实值的机器人位置和姿势［6］。

图1 是基于图优化SLAM 的架构［7］，主要包括前端和后端两个部分，其中数据关联和闭环检测由前端完成。数据关联主要通过传感器对机器人运动过程中的环境进行观测获得观测值，对获得的观测值和已经在地图中存在的特征值进行对比，如果是新的环境特征则将其添加到建立的地图中，如果与已建立的地图中的特征符合则通过对比不断修正特征，解决匹配前帧和后帧问题并估计相关位置和姿势，把控局部数据关联［8］。闭环检测利用传感器获得观测量，比较当前位姿与之前位姿是否匹配，并不断优化位姿，把控全局数据关系。

Fig.1 SLAM algorithm framework based on graph optimization图1 基于图优化的SLAM 算法框架

其中位姿及约束关系如图2 所示。

Fig.2 Representation method of transforming robot pose into graph图2 机器人位姿转化为图的表示方法

向量X=(xi,xj,…)T是图中显示点集，表示机器人位姿序列。其中，xi，xj为i节点和j节点的位姿；zij为i节点和j节点的相对位姿，表示不同时刻下的关联信息；Ωij为i节点和j节点的信息矩阵；fij(x)为无干扰理想环境测量函数值；eij(x)为预测量与测量之间的差量，即误差函数值。

设eij(x)遵守高斯分布，Fij(x)即目标函数，可用如下公式表示：

由公式（1）可知，当存在节点x*令目标函数Fij(x)取到最小值，则此时的解为最优，用公式表示如下：

因此，图优化的SLAM 是将其变成求最优解，Cartographer SLAM 则通过对公式求解得到优化后的位姿。

Cartographer SLAM 是谷歌的开源算法，利用非线性最小二乘法将闭环检测后的数据图进行后端优化，从而获得全局地图一致性导航［9］。谷歌提供Cartographer SLAM 算法配备传感设备包方式给出一种搭建室内地图的即时解决方法，该包能产生r=5 cm 为单位的2D 栅格地图。此算法结合单独的本地方法和全局方法处理2D SLAM，两种方法都优化了由LIDAR 观测值的平移和旋转组成的姿态。

ξx、ξy为移动机器人在x,y方向的平移，ξθ为2D 平面旋转量，该姿态进一步称为扫描。在稳定性不够好的平台如Cartographer 上，利用IMU 预估重力方向，把水平安装的激光雷达扫描投影到2D 地图中［10］。在局部法中，每个连续扫描都是针对地图上一小部分进行匹配的，这种小块区域称为子图M，非线性优化可将扫描与子图对齐，这个过程称为扫描匹配。扫描匹配会因为时间变长而累积错误，而全局方法会消除这些误差。

子映射构造是对观测数据帧和子映射坐标数据帧进行多次对齐的更替行为。激光扫描的起始点在0 ∈R2。将扫描数据记为

扫描数据帧的子映射位姿ξ变换为Tξ，将激光扫描点的扫描帧转换到子图坐标下，公式定义如下：

使用几个连续的雷达扫描数据构建子映射，这些子映射采用概率网格M:rZ×rZ→形式，从给定分辨率r如5 cm 的离散网格点映射值，这些值可被认为是网格点被阻塞的概率。针对每个网络点定义对应的像素，由最靠近该网络点的所有点构成［11］。

无论什么时候把激光扫描帧插进概率网络，都将计算得出一对命中（hit）网络点和一组丢失（miss）网络点。对于每一次hit栅格，把最近网络点插入hit集合。针对每一个hit点插进与每一个像素相关的点，这些像素以及激光扫描数据点和每个扫描数据点之间的一条射线交叉，排除那些在hit集合中的网络点［12］。对每个未被扫描到的网格点赋予一个概率值phit或pmiss。如果网格点x已被观察到，那么命中和丢失的概率则变为：

将扫描数据帧插入到子映射前，扫描位姿ξ相对于当前时刻的子映射，需要通过Ceresbased［13］扫描匹配器进行优化。扫描设备查找一个扫描姿势，该姿势使子映射里扫描点的概率变得最大，即把这个变为一个非线性最小二乘［14］：

其中Tξ根据位姿扫描将hk从扫描帧格式转换为子图。函数Msmooth:R2→R是局部子图中概率值的平滑版本，这里用双立方插值。因此，区间[0,1]的值可能出现，但对结果没有影响。IMU 能够测量角速度用来估计移动机器人的旋转角度θ和激光雷达之间的位姿［15］。

2 Cartographer SLAM 算法优化策略

Cartographer 算法利用回环检测改善位姿进而消除累计误差。机器人在运动时会把特征点以及地面标注点和之前观测到的地面标注点以及特征点形成数据关联的回环。因此，只要有新的特征点加入，都可使回环链添加边缘，再构建新的回环链。闭环问题是数据关联至关重要部分，利用闭环检测判别机器人是不是已经走过现在的位置，优化已经完成的地图，并且通过这个条件约束完成拓扑等价的路径图，激光扫描数据匹配相近性即为闭环检测核心。

从上述原理可知匹配特征点非常重要，只要有错误出现就会使地图构建的图形发散。传统Cartographer 回环检测采用帧与子图（Scan to Map）方式进行对比，消去构图时出现的误差积累，虽然帧与子图的匹配方式可适当提高匹配效率，但这种方式容易因为一帧的量过少而使单个激光帧匹配时受限导致回环出错。

Fig.3 Similarity of data between frames图3 帧间数据相似情况

当两帧数据比较相似，在进行匹配时会认为是一个闭环从而造成回环匹配错误，如图3 虚线内所示。因此，本文设计一种子图与子图（Map to Map）回环检测法，这种方式能优化激光帧少的缺陷，将激光雷达当前扫描的N 帧数据缓存起来形成一个局部子图，通过该子图和前阶段的子图进行再匹配［16］。

利用Cartographer 构建地图时，在距离较长的走廊和环境地图结构比较相似的地方很容易导致该算法在回环检测中出错。针对该问题本文设计并使用延时策略以保证回环检测准确率及加速匹配速率。

2.1 Map to Map 回环检测设计

针对回环检测中帧—子图的匹配问题，本文提出Map to Map 回环检测策略。首先对激光数据坐标转换进行分析。

以激光雷达为坐标系时，其自身转一圈得到的间距值可提供雷达的一帧数值，根据转角不一样以及扫描断电之间的计量间距，得出雷达自身与四周物障的间距［17］，但得到的数据以雷达自身为中心，需要转化到世界坐标系下［18］。假设雷达扫描端点的某一个点以S(Sx,Sy)表示，那么它在世界坐标系的位置和姿态为U=(Ux,Uy,Uθ)，可通过U转化矩阵TU将s转化到世界坐标系中，见公式（10）。

将最近的几帧扫描帧建立子图，以二维栅格地图方式显示，地图分辩率由栅格大小控制，每一个小栅格状态用0和1 表示该空闲和占据［19］。通过公式（10）把雷达扫描帧转化成全局坐标系，如图4 所示。对于每一个栅格状态，把红色点当作激光雷达中心，扫描的物障信息为黑色的点。空白栅格代表雷达扫描范畴未发现的物障。

Fig.4 Grid map图4 栅格地图

将最近连续的几帧扫描数据整合为子图。若子地图m由雷达数据l0至li组成，以N代表相应帧包括的雷达点数目，即l0至li包含的激光点数是：

由此构建的子地图m拥有激光点数为：

将式（11）及式（12）经过栅格地图化后须匹配运算的栅格数量为和Nm，因为相近帧之间的值存在多余成分，有＞Nm，因此可以利用子图的构建去掉连续数据帧的冗余数据［20］，使Map to Map 匹配时需要运算的数据少于Scan to Map，以提高匹配效率。正是因为子地图中含有多帧扫描数据，信息量较大，匹配范畴较广，所以能很好地解决由于环境布局结构比较类似引起错误的回环匹配，如图5 所示。

Fig.5 Sub-map example图5 子地图示例

图5（a）和图5（b）虚线内部分虽然相似度极高，但其它部分相似度较低，在加入Map to Map 回环检测之后，整体匹配不会被判断为一个闭环，从而提升回环匹配准确性。

2.2 延时决策设计

即使应用改进后的基于Map to Map 方式进行回环检测匹配，但在较长的走廊和环境特征点极为相似时，由于激光雷达检测到的数据帧极为相似，仍易出现回环错误，故本文使用延时决策处理策略，结合Map to Map 的回环匹配方法使改进后的算法能在任何环境下构建精确地图。

由图6 可知，移动机器人由a 点行驶到p 点过程中，当机器人走到i时发现i点和h点激光雷达观测到了极为相似的环境数据，正常情况下会进行闭环优化，并将i点和h点连接起来，但一旦此时回环出错，整个地图会被一次错的回环匹配破坏［21］。加入延时决策之后，当检测到一个回环时不会立即进行回环检测优化位姿，机器人继续行驶直到产生下一个回环检测点j和g点时进行位姿更新优化［22］。当第二次检测到回环点后，假设这两次检测到的点都是正确的，那么两次回环形成的4 条边为T1、T2、T3、T4，根据图优化SLAM 理论［23］可知T1∙T2∙T3∙T4=I。

Fig.6 Delay decision图6 延时决策

3 改进前后的Cartographer SLAM 算法对比实验

为证明在多种环境如长距离、结构相似等环境下改进后的算法比改进前的算法具有更好的建图能力，本文设计两个实验验证改进后的Cartographer SLAM 算法。针对物理结构极其相似的环境和长距离、等宽度走廊，搭建简单的物理布局，使用遮挡板挡住其两端，依次使用未改进的Cartographer 算法和改进后的优化算法进行地图构建。

图7 为在狭长等宽走廊环境下改进前后算法实验对比图，图8 是在结构相似环境下改进前后算法实验对比。图7（a）为使用未改进算法的建图结果，从图中可以看出，在较长走廊内，由于环境基本相似，构建出来的走廊的与实际环境中的走廊长度不符，且建图宽度与实际环境不符，可以判断是回环检测匹配出现错误。相反，图7（b）是使用改进后的优化算法构建的地图，可以看出即使在狭长的走廊里也能构建出精确的地图。从图8（b）可以看出，使用改进优化后的算法进行地图构建时，当物理结构环境极为相似时，该算法也能进行正确的回环检测与匹配，从而生成精确地图。反之，图8（a）在使用未改进的算法描绘地图时出现回环检测错误，导致整个地图与真实环境布局不匹配，无法进行下一步导航。

Fig.7 Map comparison of algorithm construction under narrow and equal width corridor environment图7 狭长等宽走廊环境下算法构建地图对比

Fig.8 Map comparison of algorithm construction under similar structure environment图8 结构相似环境下算法构建地图对比

4 改进前后的Cartographer SLAM 算法建图对比实验

将上述Cartographer SLAM 优化算法在实际环境中进行整体对比实验，与未优化前的建图结果进行对比分析，证明优化后的算法对SLAM 建图确有效果。

通过上位机操控移动机器人在实验环境中行驶，ROS（移动机器人操作系统平台）将传感器如里程计、激光雷达等扫描数据记录下来，之后分别运行优化前后的Cartographer SLAM 算法得到建图结果，对比两种结果并分析优化效果。

图9、图10 是改进前后的Cartographer SLAM 算法构建地图效果。

Fig.9 Map constructed by Cartographer before improvement图9 Cartographer 未改进前构建的地图

对比图9 和图10 可以很明显看出，改进前的CartographerSLAM 算法绘制的地图存在边界毛刺不清晰情况，内部也有些许重影；而改进后的算法构建的地图边缘更加清晰，内部障碍物描画也更具体，几乎不存在边界模糊或毛刺样不清晰或重影情况。

Fig.10 Improved Cartographer SLAM map图10 改进后的Cartographer SLAM 地图

为了更好地对比改进前后算法优化程度，本文通过挑选并实际测量实验环境中的10 个特征点，与在Rviz 中建图的测量值进行比较，对这两个算法的相对误差绝对值数据、绝对误差数据进行计算，并画出两个算法相对误差的对比折线图，如图11 所示。

Fig.11 Comparison of absolute value of relative error of Cartographer algorithm before and after improvement图11 改进前后Cartographer 算法相对误差绝对值对比折线

实验采用四核处理器的CPU，改进前后算法在运行过程中采集到的每核系统内存占用率对比如图12 所示。

从图11 可以看出，改进前的算法相对误差普遍较大并且不稳定；改进后的算法相对误差稳定，一般都未超过1%，对于室内环境地图构建精度更高。由图12 可以看出，改进前的CPU 占用率比改进后的高并呈上升趋势，改进后的CPU 占用率较低，随着时间增加趋于平稳。因此，在相同实验环境下移动机器人使用Cartographer SLAM 算法效果更好。

Fig.12 Comparison of CPU occupancy rate before andafter the improvement of Cartographer algorithm图12 改进前后Cartographer 算法运行时CPU 占用率对比

5 结语

本文阐述了主流Cartographer SLAM 算法，并针对以前Scan to Map 算法对子图进行闭环检测出现的匹配度不足问题，采用Map to Map 策略进行创新，降低了激光数据信息量少的缺点。利用创新的Map to Map 算法对子图信息匹配作回环检测，通过栅格地图建图，提高匹配度。针对回环检测在相似度高的环境下匹配易出错问题，将创新的Map to Map 算法与延时决策设计结合用以更新移动机器人位姿，提高回环检测稳定性。后续研究将此算法与传感器数据融合算法相结合，以进一步提高移动机器人SLAM 建图精度。