A Flexible and Scalable SLAM System with Full 3D Motion Estimation
摘要
在许多应用场景中，比如城市搜救和搜索（USAR）机器人，需要去获取未知环境的地图。我们提出了一个快速在线学习占用栅格地图、占用较少计算资源的系统。它利用激光雷达系统与基于惯性传感器的3D位姿估计系统进行融合，实现了一种鲁棒的扫描匹配方法。通过地图变化的快速近似和多分辨率栅格地图，在各种有挑战性的环境中实现了可靠的定位与建图。提供了多种数据集以适应嵌入式手持建图系统。我们表明，该系统是足够准确的，在我们考虑的应用场景中，不需要显式闭环检测技术。该软件可作为ROS的开源代码包。
介绍
学习环境模型并定位自身是一个真正的机器人在真实世界运行的最重要的能力。在本文中，我们提出了一种灵活的、可升级的系统来解决SLAM问题，已成功的运用在了UGV、USV和一个小型的室内导航系统上。该方法消耗的计算资源较少，故可以应用于低成本、低功耗的嵌入式系统。该方法是在ROS上实现的开源软件。它适应ROS上的API和导航stack，并可以在ROS的生态中替代其他SLAM方法。
本文介绍的系统旨在保证计算力要求低的前提下，实现足够精确的环境感知和自我定位。它可以应用在小尺度的、不必做大的闭环的系统中，并需要使用高更新速率的激光雷达系统。类似的场景包括RoboCup搜救比赛，可能需要在模拟的地震场景中找到受害者，因此需要对车辆在6Dof上进行姿态估计。或者，比地面机器人更灵活的室内飞行器的导航。有关USAR的结果和模型在参考【2】中可以找到。
我们的方法结合了2D SLAM（基于激光雷达的平面地图）和3D导航（基于IMU）融合了2D的SLAM信息作为辅助（FIG.I）。SLAM过程通常是由激光雷达的数据更新来触发的，而整个3D导航解决方案是需要实时计算的，构成车辆控制系统的一部分。
相关工作
近些年已经有大量的研究关于SLAM的问题，例如作为开源软件的gmapping使用的是Rao-Blackwellized粒子过滤器，可以可靠在的典型办公室室内场景使用。然而，这些解决方案的工作最好在平面环境，依赖于现有的，足够精确的航迹以及不利用现代雷达系统提供的高更新率。对于非结构化环境，会导致载体的显著滚转和俯仰运动，或在空中平台上实现这种系统不适用或必须进行显著修改。
一个SLAM的前端和后端系统之间的区别。在大满贯的前端，用于实时在线估计机器人运动，后端用于优化位姿图，和在使用的前端产生的位姿之间的约束。本文提出的方法可以作为一个SLAM的前端和不提供姿势图优化像[ 4 ]和[ 5 ]提出的解决方案。然而，我们表明，在许多情况下，这种优化并不需要在现实中的一些条件，因为这种方法是足够准确的，可用于机器人来执行他们的任务。
基于激光扫描匹配的室内导航系统提出了对旋翼无人机使用[ 6 ]、[ 7 ]、[ 8 ]。在这里，采用两阶段的方法，前端快速扫描的位姿估计，和用于在后台或远程计算机上进行的较慢的后端建图步骤。从雷达扫描对准的位姿估计不直接纳入车辆的控制回路，因此他们只在低速行驶。
在[ 9 ]和[ 10 ]中描述了移动机器人上使用的其他前端系统。在本文的对比，他们没有提供完整的六自由度位姿估计和开源软件。
使用扫描匹配进行定位的工作始于ICP[ 11 ]，它起源于注册三维点云的一般方法。许多基于ICP的方法的主要缺点是对点对应的高复杂度搜索，这必须在每次迭代中进行。极坐标扫描匹配（PSM）[ 12 ]避免了利用激光扫描的自然极坐标系统来估计它们之间的匹配的对应搜索。扫描进行预处理，可用于极性扫描匹配。实时相关扫描匹配方法[ 13 ]采用穷举抽样方法进行扫描匹配。通过多种优化，这种方法能够实时应用。基于正态分布变换（NDT）[ 14 ]的扫描匹配将扫描对齐到代表前扫描的正态分布混合。
对于沿海的情况，有研究使用昂贵的多传感器扫描仪[ 15 ]，但据笔者的知识，没有单发射器激光雷达为基础的SLAM方法，可在现实世界中的条件下进行测试。
系统概述
相对许多其他基于网格的2D-SLAM来说，本文提供了一种可用的，具有完整的6自由度运动的平台。我们的系统可以预测6自由度的平移和旋转状态。为了实现这一点，我们的系统由两个主要部分组成，导航滤波器融合了来自于惯性传感器和其他可用的传感器到一个可用的3D数据，而2D SLAM则提供平面的位姿信息。这两部分的更新都是单独的，是松耦合系统，他们会定时保持同步。
我们定义导航坐标系统是一个右手系统，它的原点在平台的起点上，Z轴指向上方，X轴在启动时指向平台的朝向。
完整的3D状态表示为\(\mathbf{x}=\left(\begin{array}{lll}\mathbf{\Omega}^{\mathrm{T}} & \mathbf{p}^{\mathrm{T}} & \mathbf{v}^{\mathrm{T}}\end{array}\right)^{\mathrm{T}}\)，其中，\(\mathbf{\Omega}=(\phi, \theta, \psi)^{\mathrm{T}}\)表示欧拉角的roll，pitch和yaw，\(\mathbf{p}=\left(p_{x}, p_{y}, p_{z}\right)^{\mathrm{T}}\)和\(\mathbf{v}=\left(v_{x}, v_{y}, v_{z}\right)^{\mathrm{T}}\)分别表示在导航坐标系下的位置和速度。
惯性测量使用\(\mathbf{u}=\left(\begin{array}{ll}\omega^{T} & \mathbf{a}^{T}\end{array}\right)^{T}\)来表示，其中\(\mathbf{a}=\left(a_{x}, a_{y}, a_{z}\right)^{\mathrm{T}}\)和\(\mathbf{w}=\left(w_{x}, w_{y}, w_{z}\right)^{\mathrm{T}}\)分别表示加速度和角速度。任意刚体的运动可以表示为如下非线性微分方程（简化版）：
\[ \begin{aligned} \dot{\mathbf{\Omega}} &=\mathbf{E}_{\Omega} \cdot \boldsymbol{\omega} \\ \dot{\mathbf{p}} &=\mathbf{v} \\ \dot{\mathbf{v}} &=\mathbf{R}_{\Omega} \cdot \mathbf{a}+\mathbf{g} \end{aligned} \]