SLAM算法之HectorSLAM，Gmapping，KartoSLAM，CoreSLAM和LagoSLAM

文章将介绍使用的基于机器人操作系统（ROS）框架工作的SLAM算法。 在ROS中提供的五种基于2D激光的SLAM算法分别是：HectorSLAM，Gmapping，KartoSLAM，CoreSLAM和LagoSLAM。当然最后还有比较经典的google开源的cartographer，虽然不是基于ROS的但是大牛们已经将它修改为基于ＲＯＳ的版本的cartographer_ros,

ROS(Robot Operating System)是一个基于Linux的开源操作系统，目的在于构建一个能够整合不同研究成果，实现算法，代码重用的机器人软件平台。ROS是当前比较流行的机器人控制框架，它让我们节省时间构建机器人的系统框架，更专注算法层面的研究。极大的提高了开发效率。提供了包括硬件抽象描述、底层设备控制、常用功能的实现，程序间信息的传递、程序包管理以及一些可视化数据程序和软件库，并在此平台的基础上开源了很多诸如定位构图，运动与规划，感知与决策等应用软件包。

  机器人爆炸式增长的一个主要问题是不能在不同的机器人平台上重复使用代码。然而，ROS中的硬件抽象层及其消息服务允许创建可用于许多不同机器人平台的新代码。而且，ROS提供了一套稳定的机器人软件包，公认的SLAM评估方法都依赖于机器人社区可用的标准数据集。 本文中研究了基于激光的主要二维SLAM算法，所有SLAM的结果都使用占用网格作为最终输出，使用地图相似性的性能指标进行分析。 重点是放在地图质量，而不是姿态估计误差，因为映射输出受到本地化问题的高度影响。 主要目标是提供ROS中所有五种算法的优缺点概述，提供简单而准确的定量比较，从而为ROS开发者定义一个通用的指导方针，以选择最符合他们需求的算法。

（1）HectorSLAM

HectorSLAM是一种结合了鲁棒性较好的扫描匹方法2D SLAM方法和使用惯性传感系统的导航技术。传感器的要求：高更新频率小测量噪声的激光扫描仪.不需要里程计,使空中无人机与地面小车在不平坦区域运行存在运用的可能性.作者利用现代激光雷达的高更新率和低距离测量噪声，通过扫描匹配实时地对机器人运动进行估计。所以当只有低更新率的激光传感器时，即便测距估计很精确，对该系统都会出现一定的问题，如下图是该系统生成的二维地图。

http://kaspar.informatik.uni-freiburg.de/~slamEvaluation/datasets.php

http://www.ros.org/wiki/hector_slam

利用已经获得的地图对激光束点阵进行优化, 估计激光点在地图的表示,和占据网格的概率.，其中扫描匹配利用的是高斯牛顿的方法进行求解. 找到激光点集映射到已有地图的刚体转换(x,y,theta).具体的公式如下：

             

 下图二维地图是hectorSLAM 多分辨率地图的表示。 网格单元格长度（从左到右）：20厘米，10厘米和5厘米 ，使用多分辨率地图表示，以避免陷入局部最小化。 这个解决方案背后的想法是在内存中有不同的地图，同时使用之前估计的姿势来更新它们。 计算成本 仍然很低，所以地图总是一致的

 最后，3D空间导航状态估计是基于EKF滤波器。 但是，这种情况是仅在IMU存在时才需要，例如在空中机器人的情况下。 因此，这里我们只讨论二维的SLAM

它会将不再这项工作中使用。如下图显示了HectorSLAM方法的概述。

 （2）Gmapping

Gmapping是一种基于激光的SLAM算法，它已经集成在ROS中，是移动机器人中使用最多的SLAM算法。这个算法已经由Grisetti等人提出是一种基于 Rao-Blackwellized的粒子滤波的 SLAM方法。基于粒子滤波的算法用许多加权粒子表示路径的后验概率，每个粒子都给出一个重要性因子。但是，它们通常需要大量的粒子才能获得比较好的的结果，从而增加该算法的的计算复杂性。此外，与PF重采样过程相关的粒子退化耗尽问题也降低了算法的准确性。粒子退化问题包括在重采样阶段从样本集粒子中消除大量的粒子。发生这种情况是因为它们的重要性权重可能变得微不足道。因此，这意味着有一定的小概率时间会消除正确的假设的粒子。为了避免粒子的退化问题，已经开发了自适应重采样技术。

作者还提出了一种计算精确分布的方法，不仅考虑机器人平台的运动，还考虑最近的观察。提出了在使用测距运动模型时结合观测值提出建议分布。然而，当一个移动机器人装备一个非常精确的传感器LRF时，可以使用该传感器的模型，因为它可以实现了极高的似然函数。 基于此，作者整合了最近的传感器观测zt，并且他们计算高斯分布近似有效地获得下一代粒子分布。

 

这种自适应重采样降低了PF预测步骤中机器人姿态的不确定性。 结果，由于扫描匹配过程，所需的粒子数量减少，因为不确定性较低。 实验中，Gmapping使用的粒子数量是30，与普通PF方法相比，这是非常低的。

 （3）LagoSLAM

   LagoSLAM 是线性近似图优化,不需要初始假设。基本的图优化slam的方法就是利用最小化非线性非凸代价函数.每次迭代, 解决局部凸近似的初始问题来更新图配置,过程迭代一定次数直到局部最小代价函数达到. (假设起始点经过多次迭代使得局部代价函数最小). 。假设图中每个节点的相对位置和方向都是独立的，作者求解了一个等价于非凸代价函数的方程组。为此，提出了一套基于图论的程序，通过线性定位和线性位置估计，得到非线性系统的一阶近似。

 （4）KartoSLAM

KartoSLAM是基于图优化的方法，用高度优化和非迭代 cholesky矩阵进行稀疏系统解耦作为解．图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，箭头的指向的连接表示连续机器人位置点的运动，每个新节点加入，地图就会依据空间中的节点箭头的约束进行计算更新．

　　KartoSLAM的ROS版本，其中采用的稀疏点调整（the Spare Pose Adjustment(SPA)）与扫描匹配和闭环检测相关．landmark越多,内存需求越大,然而图优化方式相比其他方法在大环境下制图优势更大.在某些情况下KartoSLAM更有效,因为他仅包含点的图(robot pose),求得位置后再求map.

 （5）CoreSLAM

        COreSLAM最小化性能损失的一种slam算法.将算法简化为距离计算与地图更新的两个过程,  第一步,每次扫描输入,基于简单的粒子滤波算法计算距离,粒子滤波的匹配器用于激光与地图的匹配,每个滤波器粒子代表机器人可能的位置和相应的概率权重,这些都依赖于之前的迭代计算. 选择好最好的假设分布,即低权重粒子消失,新粒子生成..在更新步骤,扫描得到的线加入地图中,当障碍出现时,围绕障碍点绘制调整点集,而非仅一个孤立点。

　　　　　　　　　　　

 以上各种激光测距仪的ＳＬＡＭ的算法的对比：

（6）cartographer

cartographer是Google的实时室内建图项目，传感器安装在背包上面，可以生成分辨率为5cm的2D格网地图。获得的每一帧laser scan数据，利用scan match在最佳估计位置处插入子图（submap）中，且scan matching只跟当前submap有关。在生成一个submap后，会进行一次局部的回环（loop close），利用分支定位和预先计算的网格，所有submap完成后，会进行全局的回环。