# [读书笔记2]：激光雷达2D SLAM基本框架

[读书笔记]

激光雷达2D SLAM基本框架¹

1.前端：传感器获取数据和扫描匹配
2.后端：优化和闭环检测
3.地图创建

扫描匹配

1.基于点的扫描匹配

ICP算法
寻找一致点对=>剔除冗余点对=>根据对应点计算RT=>点云转换，计算误差=>迭代至最小阈值

2.基于特征的扫描匹配

基于特征的扫描匹配是指对接收到的激光雷达数据中的关键因素进行匹配，关键因素可以是点、线、面，也可是它们的组合。

3.基于数学特性的扫描匹配

基于数学特性的扫描匹配是指使用各种数学性质来刻画扫描数据及帧间位姿变化的扫描匹配方法，其中正态分布变换是最常用的一种方法NDT

4.基于优化问题的扫描匹配

基于优化问题的扫描匹配是指把激光雷达数据的扫描匹配问题建模成非线性最小二乘优化问题.

优化

1.基于滤波器的方法

基于滤波器的2D SLAM方法采用的是递归贝叶斯估计，构建增量式地图并实现定位。根据贝叶斯法则，用后验概率函数描述SLAM问题，根据时间和观测信息的更新对后验概率函数进行估计和更新。
KF PF

2.基于图优化的方法

基于图优化的SLAM通过位姿图（Pose Graph ）来表示机器人的运动轨迹，位姿图中的节点（Node）表示不同时刻机器人的位置与姿态信息，节点与节点之间的连线表示位姿间的约束关系，根据位姿间的约束关系对节点位姿进行优化。

闭环检测

闭环是指机器人运动了一圈之后又回到了它去过的地方。通过闭环机器人可以校正不正确的路标，消除累积误差，从而提高所建地图的精度。
1.帧与帧的闭环检测

使用相关性扫描匹配的方法对比两帧激光雷达数据之间的相似性，以此判断其是否达到闭环。但一帧激光雷达的数据所提供的信息很少，容易与其他相似的帧混淆，导致错误匹配。

2.帧与子图的闭环检测

Cartographer将激光雷达数据帧与子图（连续的若干激光数据帧组成一个子图）中的激光雷达数据喊进行匹配，限制了用作匹配的数据帧的范围，降低了错误匹配的概率。在Cartographer中还使用了分支定界（Branch and Bound）算法，加速了闭环的形成。

3.子图与子图的闭环检测

使用子图与子图进行匹配的方法进行闭环检测，弥补了单帧激光雷达数据信息量少的缺点，进一步减少了错误匹配的发生。

4.其他闭环检测

像素精确扫描匹配
基于直方图的匹配
扫描数据中的特征点提取出来，结合深度学习的方法进行匹配
…

地图构建

1.栅格地图：

栅格地图是指将整个地图按照一定的分辨率进行网格划分，使用网格来显示环境地图。

2.拓扑地图：

拓扑地图是指把环境中的空间（路口、拐角等）抽象为节点，空间与空间之间的通道（走廊、公路等）抽象为边，使用节点和边构成一幅完整的地图。拓扑地图适合用来做路径规划，其典型应用是公交路线图和地铁路线图

3.几何特征地图:

几何特征地图是指把环境中的几何特征提取出来，例如直线、弧线、圆弧、角点等，用这些简单的几何形状来表示地图。几何特征地图的优点是简洁，占用内存少；但是在提取几何特征的过程中会累积误差，不适用于大场景。

定位

1.局部定位

局部定位是指机器人根据自身携带的传感器来确定自己的位置。局部定位所依赖的传感器通常是指里程计，机器人通过里程计的信息来推算自己的位置。

2.全局定位

全局定位是使用路标辅助定位的。首先在环境中的固定位置摆放路标，机器人在运动过程中识别出这些路标，并根据自身与路标的相对距离和方向计算出自身的位置。

常见2D SLAM 方法

1.EKF SLAM

该算法是基于KF改进提出。EKF SLAM主要包括状态预测和状态更新这两个过程。每当有新的控制量输人，机器人就会根据输人的控制量进行一次判断，然后对机器人的位姿进行估计，最后通过雷达采集到的观测数据进行第二次判断，并根据观测数据对之前估计的位姿进行调整，从而得到更精确的位姿。

2.FastSLAM

FastSLAM包含了卡尔曼滤波和粒子滤波的特点，卡尔曼滤波用来进行路标位置的估计，粒子滤波用来对系统的位姿进行估计。FastSLAM有FastSLAM1.0和FastSLAM2.0两种，其区别在于建议分布函数的不同。FastSLAM算法一般分为４个步骤：１）采样阶段，根据建议分布对每一个粒子进行采样；２）地图估计，将地图估计分解为Ｎ个路标的估计问题，通过EKF估计地图中路标的条件概率分布；３）权重计算，根据SLAM的系统状态后验概率和建议分布计算每个粒子的权重，并将粒子的权重归一化；４）重采样，用权重高的粒子代替权重低的粒子。

3.Gmapping

Gmapping利用RBPF来解决SLAM问题，先定位，后建图，依赖于传感器和里程计将SLAM问题分为两步，即先通过里程计数据和传感器观测数据计算机器人轨迹，再通过机器人轨迹和已知观测值来计算地图的概率分布。RBPF使用粒子滤波来估计后验概率，因为轨迹与环境地图具有高度相关性，因此其中的每一个粒子都对应一条机器人轨迹和环境中一张单独的地图，最后筛选出最高概率的粒子。基于粒子滤波的SLAM需要大量的粒子来获取一个较好的结果，从而增加计算量。Gmapping算法通过自适应重采样减少了粒子耗散（错误删除包含机器人轨迹和环境地图的粒子），计算粒子分布的时候不仅考虑到了机器人的运动，而且将当前的观测信息也考虑了进去，减小了机器人位置在滤波中的不确定性。

4.HectorSLAM

与Gmapping相比，SLAM不需要里程计数据，仅使用激光雷达就可以完成实时地图创建和定位，但要求激光雷达的更新频率较高，测量噪声较小。该算法在初始时刻利用激光雷达的第一帧数据直接进行建图，然后利用激光雷达新获取的数据与已经获得的地图进行匹配，推导出机器人当前的优位姿,最后用高斯牛顿方法求解位姿的最优估计，找到激光雷达数据点集映射到已有地图的刚体转换。为了避免出现局部最优，地图采用多分辨率的形式

5.Cartographer

Cartographer的主要思路是利用闭环检测来减少构图过程中的累积误差。激光雷达扫描（扫描由雷达的观测量组成）插人到最佳位置估计的子图（子图由若干连续的扫描构成）中，当不再有新的扫描插人到子图中时，视为一个子图构建完成，创建完成的子图数量会随着时间增多，子图之间的累积误差会越来越大，构建完成的子图会进入到闭环检测中，利用闭环检测来消除子图位姿的累积误差。当已完成的子图和扫描与当前位姿估计值距离足够近的时候，扫描匹配就可以找到该闭环。如果在当前估计的位姿周围找到了更好的匹配，则将其作为闭环约束添加到闭环优化问题中。

方法对比

方法	优缺点
EKFSLAM	优点：输出特征地图，计算量大。缺点：鲁棒性较差。
FastSLAM	优点：实时输出栅格地图。缺点：内存消耗大，粒子耗散严重。
Gmapping	优点：粒子耗散较小。缺点：对里程计数据的依赖很大。
Hector SLAM	优点：不依赖于里程计数据。缺点；旋转漂移大，初值敏感，难处理闭环。
Cartographer	优点：前端使用CSM与梯度优化，后端使用图优化与分支定界，加速了闭环检测。

---

1. 罗恒杰. 基于激光雷达的2D SLAM方法综述[C]. 中国计算机用户协会网络应用分会.中国计算机用户协会网络应用分会2019年第二十三届网络新技术与应用年会论文集.中国计算机用户协会网络应用分会:北京联合大学北京市信息服务工程重点实验室,2019:165-171.文章链接 ↩︎