目录
1. 写在前面
好久没有更新SLAM系列的文章了，由于近期项目涉及激光SLAM和多传感器融合技术，故更新。基于激光雷达的同时定位与地图构建技术（simultaneous localization and mapping， SLAM）以其准确测量障碍点的角度与距离、 无须预先布置场景、可融合多传感器、 在光线较差环境工作、 能够生成便于导航的环境地图等优势，成为目前定位方案中不可或缺的新技术。
激光 SLAM 任务是搭载激光雷达的主体于运动中估计自身的位姿，同时建立周围的环境地图。而准确的定位需要精确的地图，精确的地图则来自于准确的定位，定位侧重自身位姿估计，建图侧重外部环境重建。
SLAM技术正是为了实现这个目标涉及到的诸多技术的总和。由于其重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。
SLAM系统一般分为五个模块：传感器数据、视觉里程计、后端、建图及回环检测。
- 传感器数据：主要用于采集实际环境中的各类型原始数据。包括激光扫描数据、视频图像数据、点云数据等。
- 视觉里程计：主要用于不同时刻间移动目标相对位置的估算。包括特征匹配、直接配准等算法的应用。
- 后端：主要用于优化视觉里程计带来的累计误差。包括滤波器、图优化等算法应用。
- 建图：用于三维地图构建。
- 回环检测：主要用于空间累积误差消除
其工作流程大致为：
传感器读取数据后，视觉里程计估计两个时刻的相对运动（Ego-motion），后端处理视觉里程计估计结果的累积误差，建图则根据前端与后端得到的运动轨迹来建立地图，回环检测考虑了同一场景不同时刻的图像，提供了空间上约束来消除累积误差。
目前，SLAM（即时定位与地图构建）技术主要被运用于无人机、无人驾驶、机器人、AR、智能家居等领域。
按照核心的功能模块来区分，目前常见的移动机器人SLAM系统一般具有两种形式：基于激光雷达的SLAM(激光SLAM)和基于视觉的SLAM(Visual SLAM或VSLAM)。
激光SLAM脱胎于早期的基于测距的定位方法(如超声和红外单点测距)。激光雷达(Light Detection And Ranging)的出现和普及使得测量更快更准，信息更丰富。
激光雷达采集到的物体信息呈现出一系列分散的、具有准确角度和距离信息的点，被称为点云。通常，激光SLAM系统通过对不同时刻两片点云的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，也就完成了对机器人自身的定位。
激光雷达距离测量比较准确，误差模型简单，在强光直射以外的环境中运行稳定，点云的处理也比较容易。同时，点云信息本身包含直接的几何关系，使得机器人的路径规划和导航变得直观。激光SLAM理论研究也相对成熟，落地产品更丰富。
2. 开源激光SLAM方案
根据所采用的凸优化框架， 激光 SLAM 可分为两大类：基于滤波器（Filter-based）和基于图优化（Graph-based）的激光 SLAM。
2.1 基于滤波器的激光SLAM方案
由 Smith R 等人提出的扩展卡尔曼滤波 SLAM 方案（EKF-SLAM），使用最大似然算法进行数据关联，该方案的缺点是计算量复杂，鲁棒性较差，构建的地图是特征地图而不是栅格地图，无法应用在导航避障上。
针对 EKF-SLAM 方案的不足， Montemerlo M 等人提出了 FastSLAM 方案，该方案将 SLAM 问题分解成机器人定位问题和基于已知机器人位姿的构图问题， 是最早能够实时输出栅格地图的激光 SLAM 方案。
用粒子滤波来估计机器人位姿， 将每个粒子用运动学模型进行传播，对于传播后的粒子用观测模型进行权重计算并根据估计的位姿构建地图。
该方案存在两个问题， 第一， 由于每个粒子包含机器人的轨迹和对应的环境地图，对于大尺度环境，若里程计误差较大即预测分布与真实分布差异较大， 则需要较多粒子来表示机器人位姿的后验概率分布，严重消耗内存；第二，由于重采样的随机性，随着重采样次数增多，粒子多样性散失，粒子耗散问题会严重影响地图的构建。
为了对 FastSLAM 方案进行优化， Grisetti G 等人提出Gmapping 方案， 以 FastSLAM 方案为基本原理，在较小的环境中能实现较好的建图效果，是目前使用最为广泛的2D激光 SLAM 方案。
为了解决内存消耗严重问题， 将粒子的数量保持在一个比较小的数值， 对预测分布采样，然后基于优化扫描匹配来优化位姿。对于缓解粒子耗散问题，采用减少重采样次数，用一个度量表示预测分布与真实分布的差异性，当差异性很小时，不进行重采样，当差异性很大时，进行重采样。该方案不足是在里程计模型在传播时， 对所有的粒子同等对待，优的粒子在传播时可能变成差的粒子， 粒子退化问题严重。因此 Gmapping 方案非常依赖于里程计信息，构建的地图也取决于里程计的精度。
针对 Gmapping 方案的不足， Blanco J L 等人在 2010 年提出了更加优化的 Optimal RBPF 方案，其在里程计模型传播时，一个粒子每次传播会得到 N 个粒子， 在 N 个粒子里面选择最优粒子， 作为此次的真实传播， 相当于每个粒子给了 N次机会，大大减少最优粒子退化为噪声很大的粒子情况。
2.2 基于图优化激光SLAM方案
在激光雷达领域， Lu F， Milios E 等人首次提出利用图优化（graph-based optimization） 的数学框架优化 SLAM 问题，通过非线性最小二乘方法来优化建图过程中累积的误差。其存在的问题在于没有认识到系统的稀疏性， 离线处理SLAM 问题。
由 Gutmann J 等人提出的图优化框架与当前的图优化框架相似，包括局部扫描匹配、全局优化、以及子图与子图的闭环检测，不足是同样没有认识到系统的稀疏性，并非实时 SLAM。
为了进一步改善前面工作的不足， Konolige K 等人提出首个基于图优化框架的开源方案 Karto SLAM，该方案认识到了系统稀疏性，在一定程度上替代了基于滤波器的激光SLAM 方案。该方案的不足是采用局部子图匹配之前都要构建子图，耗费时间较长；若采用全局匹配方法，则在搜索范围大的时候速度会变慢。
谷歌的 Cartographer开源方案，是对 Karto SLAM 的优化方案， 核心内容是融合多传感器数据的局部子图创建以及用于闭环检测的扫描匹配策略。
该方案中前端扫描匹配算法是结合 CSM 与梯度优化来实现的。在生成一个子地图后，会进行一次局部的闭环检测；当全部子地图构建完成后，利用分枝定界和预先计算的网格的算法，进行全局闭环检测，从而保证闭环检测的速度。该方案的不足是没有对闭环检测结果进行验证， 在几何对称的环境中，容易引起错误的闭环。
另外， Kohlbrecher S 等人提出了 Hector-SLAM 方案，该方案利用高斯牛顿方法解决前端扫描匹配问题，把每一帧采集到的激光雷达数据和地图进行匹配，该方案仅有前端扫描匹配的模块，无后端优化的过程。与 Gmapping 方案最大的区别在于不需要里程计数据，里程计信息通过激光雷达数据估算出来， 所以对传感器的测量频率要求较高。 在 ROS仿真环境中运行 Hector-SLAM 方案， 若机器人速度过快尤其是在强旋转的时候， Hector-SLAM 方案会发生漂移现象。在真实环境中，由于周围特征点能够辅助机器人定位和建图，构建的栅格地图会比仿真环境中效果好。Hector-SLAM 方案的缺点是对初值敏感，同时难以处理闭环问题。 Hector-SLAM 的整体建图精度高于Gmapping，但对参数配置要求较高。Hector -SLAM 适用于对地图要求较高的场合， Gmapping 易用性更好。
2.3 3D激光SLAM
在 3D 激光 SLAM 领域中， 由 Zhang J 等人提出的LOAM 方案，利用 3D 激光雷达采集数据， 进行基于特征点的扫描匹配， 利用非线性优化方法进行运动估计， 激光里程计的输出与地图进行匹配， 包括直线匹配和平面匹配， 无回环检测模块，点面特征还不够可靠。
3. 小结
以上为激光SLAM相关开源架构及算法介绍，激光SLAM数学描述将在本系列下一篇文章中详细介绍，巨额数学公式推导警告。
参考：
1. Montemerlo M, Thrun S, Koller D, et al. FastSLAM: a factored solution to the simultaneous localization and mapping problem [C]//Proc of the AAAI National Conference on Artificial Intelligence.California: AAAI press, 2002: 593-598.
2. Grisetti G, Stachniss C, Burgard W. Improved techniques for grid mapping with Rao-Blackwellized Particle Filters [J]. IEEE Trans on Robotics, 2007, 23 (1): 34-46.
3. 《激光SLAM理论与实践》深蓝学院 曾书格