一种带有3D运动状态估计的slam系统

A Flexible and Scalable SLAM System with Full 3D Motion Estimation

摘要：

在许多应用场景中，比如城市搜救和搜索（USAR）机器人，需要去获取未知环境的地图。我们提出了一个快速在线学习占用栅格地图、占用较少计算资源的系统。它利用激光雷达系统与基于惯性传感器的3D位姿估计系统进行融合，实现了一种鲁棒的扫描匹配方法。通过地图变化的快速近似和多分辨率栅格地图，在各种有挑战性的环境中实现了可靠的定位与建图。提供了多种数据集以适应嵌入式手持建图系统。我们表明，该系统是足够准确的，在我们考虑的应用场景中，不需要显式闭环检测技术。该软件可作为ROS的开源代码包。

I.介绍

学习环境模型并定位自身是一个真正的机器人在真实世界运行的最重要的能力。在本文中，我们提出了一种灵活的、可升级的系统来解决SLAM问题，已成功的运用在了UGV、USV和一个小型的室内导航系统上。该方法消耗的计算资源较少，故可以应用于低成本、低功耗的嵌入式系统。该方法是在ROS上实现的开源软件。它适应ROS上的API和导航stack，并可以在ROS的生态中替代其他SLAM方法。

本文介绍的系统旨在保证计算力要求低的前提下，实现足够精确的环境感知和自我定位。它可以应用在小尺度的、不必做大的闭环的系统中，并需要使用高更新速率的激光雷达系统。类似的场景包括RoboCup搜救比赛，可能需要在模拟的地震场景中找到受害者，因此需要对车辆在6Dof上进行姿态估计。或者，比地面机器人更灵活的室内飞行器的导航。有关USAR的结果和模型在参考【2】中可以找到。

我们的方法结合了2D SLAM（基于激光雷达的平面地图）和3D导航（基于IMU）融合了2D的SLAM信息作为辅助（FIG.I）。SLAM过程通常是由激光雷达的数据更新来触发的，而整个3D导航解决方案是需要实时计算的，构成车辆控制系统的一部分。

II.相关事宜

近些年已经有大量的研究关于SLAM的问题，例如作为开源软件的gmapping使用的是Rao-Blackwellized粒子过滤器，可以可靠在的典型办公室室内场景使用。然而，这些解决方案的工作最好在平面环境，依赖于现有的，足够精确的航迹以及不利用现代雷达系统提供的高更新率。对于非结构化环境，会导致载体的显著滚转和俯仰运动，或在空中平台上实现这种系统不适用或必须进行显著修改。

一个SLAM的前端和后端系统之间的区别。在大满贯的前端，用于实时在线估计机器人运动，后端用于优化位姿图，和在使用的前端产生的位姿之间的约束。本文提出的方法可以作为一个SLAM的前端和不提供姿势图优化像[ 4 ]和[ 5 ]提出的解决方案。然而，我们表明，在许多情况下，这种优化并不需要在现实中的一些条件，因为这种方法是足够准确的，可用于机器人来执行他们的任务。

基于激光扫描匹配的室内导航系统提出了对旋翼无人机使用[ 6 ]、[ 7 ]、[ 8 ]。在这里，采用两阶段的方法，前端快速扫描的位姿估计，和用于在后台或远程计算机上进行的较慢的后端建图步骤。从雷达扫描对准的位姿估计不直接纳入车辆的控制回路，因此他们只在低速行驶。

在[ 9 ]和[ 10 ]中描述了移动机器人上使用的其他前端系统。在本文的对比，他们没有提供完整的六自由度位姿估计和开源软件。

使用扫描匹配进行定位的工作始于ICP[ 11 ]，它起源于注册三维点云的一般方法。许多基于ICP的方法的主要缺点是对点对应的高复杂度搜索，这必须在每次迭代中进行。极坐标扫描匹配（PSM）[ 12 ]避免了利用激光扫描的自然极坐标系统来估计它们之间的匹配的对应搜索。扫描进行预处理，可用于极性扫描匹配。实时相关扫描匹配方法[ 13 ]采用穷举抽样方法进行扫描匹配。通过多种优化，这种方法能够实时应用。基于正态分布变换（NDT）[ 14 ]的扫描匹配将扫描对齐到代表前扫描的正态分布混合。

对于沿海的情况，有研究使用昂贵的多传感器扫描仪[ 15 ]，但据笔者的知识，没有单发射器激光雷达为基础的SLAM方法，可在现实世界中的条件下进行测试。

III.系统概述

相对许多其他基于网格的2D-SLAM来说，本文提供了一种可用的，具有完整的6自由度运动的平台。我们的系统可以预测6自由度的平移和旋转状态。为了实现这一点，我们的系统由两个主要部分组成，导航滤波器融合了来自于惯性传感器和其他可用的传感器到一个可用的3D数据，而2D SLAM则提供平面的位姿信息。这两部分的更新都是单独的，是松耦合系统，他们会定时保持同步。

我们定义导航坐标系统是一个右手系统，它的原点在平台的起点上，Z轴指向上方，X轴在启动时指向平台的偏航方向。完整的3D状态表示为：，其中是欧拉角的roll、pitch和yaw，和是位置和速度在导航坐标系中的表示。

惯性的测量构成了输入矢量，其中角速度矢量以及加速度矢量。由非线性微分方程描述刚体运动：

 (1)

(2)

(3)

是映射自身坐标系到导航坐标系的方向余弦矩阵，是欧拉角到角速度的导数，g是固定重力向量[16]。当使用低成本传感器时，通常会忽略由于地球自转而产生的伪力的影响。

由于传感器噪声，速度和位置表现出明显的漂移。因此，必须进一步融合传感器信息。在本文中，这个信息是通过扫描匹配，非常适合用在室内场景。其他可能的来源是用于航向信息的磁场传感器或用于海拔高度估计的气压传感器。如果可用，odom航迹信息可以用来提供速度的测量。在室外场景中，卫星导航系统通常被用作辅助系统以防止惯性导航解决方案漂移[ 17 ]。

根据不同的平台，系统方程可以引入更多的约束，从而化简系统状态空间。

IV.2D SLAM

能够代表任意环境的栅格地图的使用，这是在现实世界环境中，利用激光雷达的移动机器人定位的一种行之有效的方法[18]。由于雷达平台可能具有6自由度运动，扫描已被转化成一个估计位姿在局部稳定的坐标系。使用估计的方位和联合值，扫描被转换成扫描端点的点云。

根据具体情况，这个点云可以进行预处理，例如对点数进行降采样或移除无效点。对于所提出的方法，只使用基于端点z坐标的滤波，以便在扫描匹配过程中仅使用预期扫描平面的阈值内的端点。

A.Map Access

FIG.2.  (a) 占用栅格地图的双线性滤波。点是其值应插值的点。(b) 占用栅格地图和空间导数。

占用栅格地图的离散性限制了可以实现的精度，也不允许直接计算插值值或导数。出于这个原因，一个插值方案允许通过双线性滤波子网格单元的精度估计占用概率和衍生工具。直观地说，网格映射单元格值可以看作是底层连续概率分布的样本。

给定一个连续的地图坐标，占用值以及梯度可以像FIG.2(a)一样用四个临近点近似,沿着x轴和y轴进行线性插值，然后得到:

(4)

导数可以近似为：

(5)

(6)

需要注意的是，地图的采样点、栅格单元位于单位距离为1的规则网格上，从而简化了梯度近似方程。

B.Scan Matching

扫描匹配是将激光雷达扫描与现有地图对齐的过程。现代激光扫描仪具有低测量噪声和高扫描速度。由于这种原因，注册扫描的方法可能会产生非常精确的结果。多机器人系统的激光扫描仪的精度和精度比里程计数据高很多，如果在所有。第VI部分显示的例子，对为什么不可用odom里程计数据提出了评价。

迄今为止，我们的方法是基于带有地图记忆的端点对齐的优化方法。根据计算机视觉工作带来的启发，我们的基本思路是使用高斯-牛顿法。使用这种方法，不需要在激光端点之间进行数据关联搜索或穷举搜索。当扫描与现有地图对齐时，匹配将通过所有前面的扫描隐式进行。

我们寻求一个刚性变换的函数最小值：

(7)

也就是说，我们需要找到一个转换，使得激光扫描与地图有最佳的对齐。在这里，是扫描端点的世界坐标。有一个函数，可以表示机器人在世界坐标系中的坐标：

(8)

函数返回给出的地图坐标值。鉴于一些的的初始估计，我们要估计，它根据以下方程去优化测量误差：

(9)

我们对做一阶泰勒展开得：

（10）

通过将的偏导数设置为0而使以下方程最小化：

(11)

解得到高斯-牛顿方程的最小化问题：

(12)

其中：

(13)

对地图梯度的近似是在第IV-A提到。用方程（8）我们得到：

(14)

用和可以解出高斯-牛顿方程（12），得到一个接近最小值。重要的是要注意，该算法适用于非光滑线性逼近的地图梯度，这意味着局部二次收敛到最小不能保证。然而，该算法在实践中具有足够的精度。

在许多应用中，高斯近似的匹配不确定性是可取的。实例参数过滤器的更新以及使用图优化SLAM后端构成约束。一种方法是使用基于抽样的协方差估计，取样不同的位姿估计接近扫描匹配的位姿，并从这些构造协方差。这类似于无迹卡尔曼滤波的想法[ 20 ]。第二种方法是使用近似Hessian矩阵得到协方差估计。这里，协方差矩阵近似于：

 (15)

其中是一个比例因子，取决于激光雷达的性能。全面的推导可在[ 21 ]中获得。

C.地图的多重分辨率表示

任何爬山法或基于梯度的方法有其陷入局部极小的风险。由于所提出的方法是基于梯度上升，它也很容易陷入局部极小。使用计算机视觉中常用的类似于图像金字塔方法在多分辨率地图中表示会得到缓解。在我们的方法中，我们可以选择使用多个占用栅格地图，每个粗糙的地图具有前一个分辨率的一半。然而，多个地图的等级并不是从一个单一的高分辨率地图生成（采用高斯滤波和降采样等在图像处理中常见的做法）。相反，不同的映射被保存在内存中，同时使用对齐过程产生的位姿估计进行同步更新。这种生成方法确保地图是一致的尺度同时避免昂贵的下采样操作。扫描定位过程是在粗糙的地图的等级开始，由此估计的姿态获得作为下一级开始估计，类似于在[22]。带来的一个好处是粗粒度地图的即时可用性，例如可以用于路径规划。

Fig.3. 多分辨率地图：（a）20cm网格（b）10cm网格（c）5cm网格

V.3D状态估计

本节包括第III节的全三维状态向量估计和2D SLAM。导航滤波器以100赫兹的恒定速率实时运行，并在扫描匹配位姿和其他传感器信息一到达时就同步更新。其他传感器的融合除了扫描匹配位姿以外都不在本文的范围内。具体实现细节可在[23]中找到。

A.导航滤波器

为了估计平台的6D位姿，我们采用了带有通用模型（方程（1）-（3））的扩展卡尔曼滤波器（EKF）。此外，随着加速度随时间的变化，陀螺仪和加速度计的偏差增大了状态向量，并对结果产生显著影响。请注意，由于矩阵和中的欧拉角项，系统方程是非线性的，因此必须使用非线性滤波器。惯性测量被认为是已知的系统输入。

速度和位置更新是测得的加速度的一个纯积分，如果没有通过测量更新的额外反馈，系统将是不稳定的。防止状态估计无限增长的通常对策是在方差达到一定阈值时，伪零速度更新，否则不能保证稳定性。在本文中，在平面的二维位姿定位是通过扫描匹配更新，而全3D的位姿估计需要额外的高度气压计和距离传感器。

B.SLAM整合

为了获得最佳性能，2D SLAM解决方案和3D EKF估计之间的信息必须在两个方向上进行交换。系统不同步，EKF通常以较高的更新速率运行。为了提高扫描匹配过程的性能，EKF的位姿估计投影在XY平面，用于扫描匹配优化过程的估计初始化。另外，估计的速度和角速率也可以被整合以提供扫描匹配的估计初始化。

在相反的方向，协方差交叉（CI）是用来融合SLAM位姿与信念状态[24]。一个简单的卡尔曼测量更新会导致过于自信的估计，因为它假设统计独立的测量误差。

我们将扫描时的卡尔曼估计表示为，协方差为P。还有，方程（7）和（15）中的SLAM位姿为。融合结果有：

(16)

以及：

(17)

利用观测矩阵将整个状态空间投影到SLAM的3维子空间中。参数用来调整SLAM的更新效果。

类似于卡尔曼滤波和信息滤波之间的二元性，协方差交叉也可以用它的协方差形式来写：

(18)

以及：

(19)

其中：

(20)

根据（16）和（17）的全状态协方差的推理在计算上的开销是很大的，这是最合适的方法。

VI.结果

我们展示了三种不同用例的通用性和健壮性的例子。应该指出的是，用于基准测试的SLAM方法缺乏6自由度运动，考虑到我们的方法以及激光雷达的高更新率，它利用标准数据集。为此，我们提供ROSBAG格式的数据集进行测试。

Fig.4.平台：（a）RoboCup搜救大赛中的Hector UGV（b）整合的建图系统，可以手持建图（c）UGV建图系统装在一个USV平台上

A.USAR场景

所提出的方法是使用无人地面车辆进行自主探索和城市搜索和救援（搜救）受害者检测场景RoboCup救援机器人联赛。Hector UGV系统如Fig.4(a)所示。激光雷达稳定在滚转和俯仰轴周围，以保持激光与地面一致，最大限度地提高平面激光雷达扫描的信息增益。在Fig.5(a)和Fig.5(b)，我们的方法能够学习高度准确和一致的地图，尽管在UGV系统自由度状态的显著变化。视频显示在机器人世界杯2010竞技场救援现场的在线地图以及本节中的其他实验可在线查阅。

B.沿岸场景

测试在沿海水域由于植被茂密,GPS中断频繁，Hector UGV的定位系统安装在一个控制系统（Fig.4(c)）。没有对USV系统改造的必要性，完整的SLAM系统安装和运行了不到30分钟。在维吉尼亚的克莱特湖，部分日志文件记录和用于学习的环境地图没有GPS数据，还是没有具体的运动模型的车辆，这都将需要更紧密的集成和较长的准备时间。如Fig.5所示，我们的方法可以在GPS中断的情况下为导航提供足够精确的地图。因为它不使用提取的特征线，它在沿海植被提供了高度的非结构化环境中工作。应该指出的是，用银行utm-30lx雷达不返回有效的距离测量，当电子束击中水，这是优势，利于预防错误的测量影响的SLAM系统。一旦GPS购回，一个基于图形的SLAM方法可用于优化提供的姿势图扫描匹配通过添加GPS构成GPS停电前和额外的绝对位置约束后取得。这是今后工作的主题。需要指出的是，使用hokuyo UTM-30LX激光雷达，有时激光束照射在水面上不返回有效测量时，是有利于防止错误测量对SLAM的影响的。一旦GPS信号恢复，一个基于图形的SLAM方法可以用于优化扫描匹配（通过GPS中断之前和之后的GPS位姿作为额外的位置约束）提供的位姿图。这将是今后的工作方向。

C.嵌入式建图系统

FIg.4(b)中展示了嵌入式建图系统。它由一个hokuyo UTM-30LX激光雷达系统，基于英特尔Atom Z530 CPU板以及一个小型低成本MEMS IMU组成。如此，该系统为所提出的SLAM方法的应用提供了所有必要的传感器。它可以很容易地安装在无人驾驶的车辆上，也可以用手携带来学习环境地图。Fig.6(a)显示了通过在地面实况地图上通过机器人在RoboCup2011救援竞技场行走的地图。Fig.6(b)显示了在德国Dagstuhl castle的一个新建筑中的建图。从路径和映射中可以看出，该系统足够精确地关闭在小规模场景中通常遇到的闭环，而不使用显式循环闭包的方法，保持计算要求低，并防止运行期间对估计地图的更改。视频和ROS记录文件已经上线。记录文件可以在atom z530上以3倍速度运行而没有质量损失。因此，以SLAM使用的设置，消耗不到嵌入式建图系统的全部计算资源的一半。

Fig.5车辆实验：（a）RoboCup2010上的地图（b）0.25m精度的栅格地图覆盖在卫星照片上

Fig.6真实环境的手持建图（a）RoboCup2011搜救环境（多个小闭环）（b）Schloss Dagstuhl的一个新建筑（大闭环）

VII.结论

本文提出了一种灵活的、可伸缩的SLAM方法，可用于多种场景。我们展示的适用性不同的情况下如模拟城市搜索和救援，在USV以及手持嵌入式系统沿海建图。我们表明，该系统正确估计位姿，并整合所使用的激光雷达系统的三维运动，同时消耗低计算资源。该系统现在作为开源软件提供。

据作者所知，高更新率，同时在二维绘图和低功耗平台运行的六自由度位姿估计的组合是不可用的开放源码软件。

VIII.感谢

德国研究基金会（DFG）RTG1362“混合模式环境中的协作、自适应和响应监视”。我们感谢弗吉尼亚理工大学电气与计算机工程系的Dan Stilwell和Aditya Gadre，布拉德利，给我们机会来执行克莱特湖USV实验。感谢Christian Lindig博士提供Dagstuhl floor图。