SLAM | 激光SLAM中开源算法对比

前面的话

好久没有更新SLAM系列的文章了，前面我们讲到了激光SLAM技术。基于激光雷达的同时定位与地图构建技术（simultaneous localization and mapping， SLAM）以其准确测量障碍点的角度与距离、 无须预先布置场景、可融合多传感器、 在光线较差环境工作、 能够生成便于导航的环境地图等优势，成为目前定位方案中不可或缺的新技术。

激光 SLAM 任务是搭载激光雷达的主体于运动中估计自身的位姿，同时建立周围的环境地图。而准确的定位需要精确的地图，精确的地图则来自于准确的定位，定位侧重自身了解，建图侧重外在了解。

具体细节可以参考文章：2020年最新 iPad Pro上的激光雷达是什么？来激光SLAM技术中找答案

今天来讲讲激光SLAM开源方案对比。

1. 激光SLAM开源方案对比

根据所采用的数学优化框架， 激光 SLAM 可分为两大类：基于滤波器（Filter-based）和基于图优化（Graph-based）的激光 SLAM。

1.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次机会，大大减少最优粒子退化为噪声很大的粒子情况。

1.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 易用性更好。

1.3 3D激光SLAM

在 3D 激光 SLAM 领域中， 由 Zhang J 等人提出的LOAM 方案，利用 3D 激光雷达采集数据， 进行基于特征点的扫描匹配， 利用非线性优化方法进行运动估计， 激光里程计的输出与地图进行匹配， 包括直线匹配和平面匹配， 无回环检测模块，点面特征还不够可靠。

为了进一步改进 LOAM 方案的， Zhang J 等人提出视觉结合 3D 激光雷达实时建图的 V-LOAM 方案。利用视觉里程计以高频率估计位姿变换，激光里程计以低频率优化运动估计，并校准漂移。在公开的 KITTI 数据集上，V-LOAM 算法精度排名第一， 而且当传感器高速运动并受到明显的光照变化时，该方法的鲁棒性较好。

Zhang J 等人提出了一种用于自我运动估计和建图的数据处理方案 LVIO。该方案连接 3D 激光扫描仪， 相机和 IMU，顺序多层运行三个模块以产生实时自我运动估计。粗到精数据处理产生高速率估计并在长距离中构建低漂移的地图。

1.4 发展趋势

几种激光SLAM算法对比：

面对复杂的周围环境，多传感器融合的SLAM是必然趋势。视觉会提供高精度的里程计以及信息量丰富的地图信息，激光雷达为视觉特征提供准确的深度信息。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理论与实践》深蓝学院 曾书格