基于多帧局部地图的室内TSDF 建图算法*

曹一波 张智辉 赵鹏飞 朱海文

（华南师范大学软件学院 佛山 538200）

1 引言

随着机器人工业的快速发展，越来越多的移动机器人走入人们的生活，目前比较常见的包括家用的扫地机器人，餐厅的送餐机器人等。不管是哪一种移动机器人，它们都有避免碰撞-感知周围障碍物，同时利用障碍物地图来规划自己运动的需求。相关的问题最早在20 世纪80 年代被提出，要求机器仅利用自身携带的传感器，可以在陌生环境中构建出环境地图，同时确定自己在地图中的位置。这个问题也被叫做同时定位与建图（Simultaneous Localization and Mapping，SLAM）［1］。

由于SLAM 要求建模周围的障碍物信息，所以机器人上面必须要装备一个可以获取深度的传感器，常用的传感器包括超声波、激光雷达、相机、RGBD相机等［2］。激光雷达是目前在室内使用的比较广泛的传感器，其特点为水平视场角大、能够直接获取到精确的测距数据。通过快速旋转的激光雷达，我们可以获取到一帧表征周围障碍物的点云，而为了获取到相对较大范围内的环境地图，我们必须将这些点云正确地对齐到一起。因此，基于激光雷达的SLAM 问题的核心是点云对准的问题［3］。

最早的对准方法是基于概率的，这些方法通过扩展卡尔曼滤波（Extended Kalman Filter，EKF）或粒子滤波（Rao-Blackwellized Particle Filter，RBPF）［4］计算得到最有可能的机器人位姿，然后基于位姿来对点云进行添加。卡尔曼滤波是较早的基于概率的方法，由于它只能模拟高斯分布，不能很好地表示位姿的概率分布，因此后续被表现更好的粒子滤波替代。粒子滤波用大量的粒子来表示机器人位姿分布，由于粒子的灵活性，粒子滤波可以模拟任意形状的概率分布，且模拟的准确度随粒子数量的增加而增加。在粒子滤波的使用过程中，人们发现了一个很直觉的现象：随着传感器精度的上升，机器人定位的精度反而下降甚至出现定位丢失，这就是所谓的粒子耗散问题［5］。

为了避免粒子耗散问题，人们开始使用非概率的纯匹配方法，最著名的纯匹配方法是用在谷歌开源算法Cartographer［6］中的CSM 加非线性优化的组合，其中CSM 使用暴力搜索来实现粗配准，非线性优化则用于实现精确配准。另一种被广泛使用的非概率配准方法则是ICP（迭代最近点）［7］。ICP 是点云处理的常见算法，目的是为了解决两组形状相似、位置上有所偏差的点云的配准问题。

ICP 能以较高的精度配准激光雷达先后获取到两帧点云，这正好是基于激光雷达的SLAM 所需要的［8］。PL-ICP 是ICP 的优化版本［9］，不同于ICP使用点与点的距离作为优化函数，PL-ICP 使用点线距离作为优化函数，这不仅提高了它的精度，还让它有比普通ICP 更快的收敛速度。但是不管是ICP还是PL-ICP，它们都有一个很严重的缺点。由于ICP 一般是直接配准相邻的两帧，新的一帧位置仅仅受上一帧的位置所影响，这就导致误差很容易会随着匹配帧数量的增加而线性增长。机器人静止的时候使用ICP 匹配，过一阵子发现机器人的位置已经发生了偏移，这便是这种情况的真实写照。更糟糕的是，由于无法使用历史帧来进行纠正，如果某一帧的匹配效果比较差，那么后续所有帧的位置都会收到影响，从而严重降低最后得到地图的效果［10］。

为了增加ICP 匹配的信息量，本文章没有采用上一帧作为参考帧，而是选择创建局部地图，并以局部地图作为参考帧。这样做的一个显著好处是利用之前多帧点云的融合信息，有效提升了新一帧配准结果的全局一致性。对于普通的占用栅格地图来说，由于其中的障碍物边界比较模糊，使得我们将其用于上层的导航模块比较困难。为了解决这个问题，本篇文章引入TSDF 建图方法［11］，TSDF借用插值方法可以精确地恢复出物体表面相关信息，从而得到更加准确的环境地图。

2 基于多帧局部地图的配准方法

2.1 PL-ICP算法

ICP 是常见的点云匹配算法，其核心思路在于找到两组点云中的匹配点，通过匹配点的间的距离差建立误差函数，最后通过最小化误差函数得到原始点云到目标点云的变换矩阵。与传统ICP 算法不同，PL-ICP 采用点到线得距离作为误差量。这种误差形式更加符合实际情况，使得PL-ICP 能够达到比普通ICP 更高的精度与更快的收敛速度。但是PL-ICP 相较于ICP 对于初值更加敏感，一般需要配合里程计一起使用［7］。

图1 PL-ICP示意图

考虑前后两帧相邻的点云：参考帧X与当前帧P。我们将遍历每一个当前帧的点，并在参考帧中找到两个距离其最近的点，用这两个点建立一条直线，并将点线距离作为我们需要优化的误差量。

用pi表示点云P中的点，用xj1i，xj2i表示点云集合X中距离pi最近的两个点，再由ni表示点xj1i，xj2i组成直线的法向量，则PL-ICP 的目标函数可以描述如下：

其中有

PL-ICP的流程可以描述如下：

1）将当前帧的数据pi根据当前的位姿估计Tk进行变换，得到新的坐标表示Tkpi；

2）对于当前帧的每一个点Tkpi，在X中找到最近的两个点xj1i，xj2i，并计算法向量ni；

3）计算Tkpi到直线的距离，去除距离过大的点；

4）最小化误差函数［7］，求得新的变换Tk+1；

5）若达到收敛条件或检测到循环，将Tk+1作为结果输出，否则跳转到第一步。

2.2 局部地图匹配

局部地图区别与全局地图，是由短时间内获取的一定帧组合而成的小范围地图。局部地图对于定位和建图任务来说好处是明显的，第一，作为匹配的参与者之一，它的规模不是很大，因此不用花费过多的资源对其进行处理；其次，局部地图由于综合了最近帧的所有信息，因此它的准确度要明显好于单独使用某一帧进行匹配。

在局部地图的实现方面，本文使用重合的局部地图策略，即某一帧会同时被两个相邻的局部地图包含，分别出现在第一个局部地图靠后部分和第二个局部地图靠前部分。下面是三张部分重叠的局部地图的示意图。

图2 重叠的局部地图示意图

局部地图的创建流程如图3。

图3 局部地图创建流程图

图4 单条光线TSDF值示意图

图5 TSDF场示意图（左上栅格为障碍物，右下栅格为空闲区域，中间的线条为拟合的障碍物的表面）

图6 实验使用的机器人仿真模型

图7 实验使用的仿真环境

图8 相同数据下两个算法的建图效果

也就是说，每一个局部地图中都会包含20 帧点云。其中前10 帧将会与上一个局部地图进行匹配，同时被添加到上一个局部地图中；而后10 帧则会与当前局部地图进行匹配，这10 帧同时也是下一个局部地图的前10帧。

使用重叠的局部地图策略一个显著的好处便是出了刚开始的10 帧以外，后续的每一帧都能与一个至少有10 帧数据的局部地图进行匹配，且这个匹配是增量式的，即匹配的结果会被添加到被匹配的局部地图当中。

3 基于TSDF的建图方法

截断距离符号函数TSDF（Truncated Signed Distance Function）是物体表面重构的经典算法。其核心思想是基于栅格地图建立一个符号距离函数场，某一个栅格的函数值的大小表示栅格与表面的距离［12］。由于障碍物表面内外的距离值符号不同，因此构建出函数场后我们可以由函数符号的跳转恢复出曲面［13］。由于距离表面较远的点不用考虑，因此我们可以用1和-1约束距离符号函数的大小。

距离符号函数sdf（x）的定义：

其中laser（x）表示本次激光测量距离，dist（x）表示栅格离传感器原点的距离。

而tsdf（x）的定义为

可以看到，tsdf（x）在sdf（x）的基础上将值约束到了-1～1之间。

一般来说，构建一个TSDF 场需要综合多次激光观测，最后每个栅格的TSDF值如下所示：

其中wi(x)表示最新一次计算的权重，Wi-1(x)表示历史的权重和。多次的观测不断按上式进行融合，即可构建出整个地图的TSDF场，从地图的TSDF场中可以重构得到障碍物表面。

4 实验与分析

4.1 实验环境搭建

实验基于ROS（Robot Operating System）平台进行构建，ROS是当前机器人领域使用较多的开源社区，里面包含硬件抽象、设备控制、进程间通信等各种机器人编程常用到的功能［14］，从而为机器人研究开发提供强力的代码复用支持。本文中使用ROS中自带的Gazebo 软件进行仿真，Gazebo 是一款3D动态模拟器，通过其内部自带的物理引擎提供高保真度的环境仿真，同时提供了一整套传感器模型，通过这些传感器模型我们可以实现对现实世界中机器人的仿真［15］。

本文在Gazebo中创建了一个装备了2D激光雷达和里程计的机器人。其中2D激光雷达的扫描范围为360°，每一帧的点数为360 个，频率为5.5Hz，最小范围为0.2m，最大范围为10m。为了模拟真实环境，激光雷达的测量距离被加上了N（0，0.02m）的高斯误差。同样的，为了让里程计的表现更接近真实环境下的表现，每次里程计获取的相对位姿都被加上了N（0，0.01m）的平移误差和N（0，1deg）的旋转误差。

实验的运行平台配置为Intel Core i5 3320M +8G DDR3。仿真机器人所在的虚拟环境是一个约60m×45m 大小的多房间环境，通过控制机器人在此环境中行走，同时开启激光雷达与里程计，来获得我们所需要的数据。实验所在的仿真环境如下。

4.2 实验结果与分析

为了证明本文算法的有效性，本文使用了普通的帧间匹配方法作为对比方法，在此方法中当前帧只与上一帧进行匹配。为了避免数据差异带来的不确定性，两个算法使用相同的数据集，这个数据集由仿真小车绕仿真环境一圈收集得到。在相同数据下两个算法的建图效果如下。

可以看到两者在建图效果上有着明显差异，为了进一步比对两者在定位精度上的差距，本文选取轨迹中的10 组共20 个点，并基于这10 组点的相对位置（包含测量值与真值）得到以下数据。

表1 两种算法定位精度对比

本文使用了三个定位精度评价指标，分别是平均误差（Mean Error）、标准差（Standard Deviation）和均方根误差（Root Mean Squared Error）。从这三个指标的表现可以看出，本文中基于多帧局部地图的匹配方法能够有效提升机器人的定位精度。

5 结语

本文提出了一种基于局部地图匹配的机器人定位建图方法，对于普通的帧间匹配方法来说，由于匹配时使用的信息过少从而导致匹配的误差较大，而使用局部地图能显著提升匹配时用到的信息量，从而显著提升定位与建图的精度。为了验证该方法本文基于ROS进行了仿真实验，并在实验中证明了其有效性。实验表明，通过将普通帧间匹配替换成地图匹配，能显著减小定位算法的定位的误差，同时增强所建立地图的全局一致性。

本文的后续研究方向包括增加传感器适用类型，例如IMU、深度相机等；减少算法的计算与内存消耗，从而将算法用到更大的环境当中；进一步提升算法的定位精度与鲁棒性；增加导航系统等。