激光SLAM(一):概述

激光SLAM

pipeline

1. 数据处理（非常重要）
激光雷达去畸变、里程计标定
2. 帧间匹配（核心部分）
ICP计算两帧激光之间的相对位姿
3. 回环检测
识别回到访问过的环境
4. 后端优化
位姿图、非线性最小二乘优化

1.数据处理（非常重要）

里程计标定

• 机械标称值不代表实际值，实际误差可能较大
• 里程计的精度对于机器人定位和建图至关重要

激光雷达运动畸变去除

• 一帧激光数据的采集需要时间
• 运动畸变会让数据严重失真，影响匹配精度

2.帧间匹配（核心部分）

算法

简称	全称
ICP	Iterative Closest Point
PI-ICP	Point-to-Line Iterative
NICP	Normal Iterative Closest Point
CSM	Correlation Scan Match
NDT	Normal Distance Transform
	Optimization-Based
	Feature-Based

3.回环检测

方法	特点
Scan-to-Scan	计算量最小，容易产生歧义
Scan-to-Map	计算量中等，中等歧义
Map-to-Map	计算量最大，基本不产生歧义

4.后端优化

高斯牛顿方法
LM方法

2D激光SLAM

输入

• IMU数据
• 里程计数据
• 2D激光雷达数据

输出

• 覆盖珊格地图（建图）
• 机器人的轨迹或位姿图（定位）

数据处理（非常重要）

• 轮式里程计的标定
• 激光雷达运动畸变去除
• 不同系统之间的时间同步

帧间匹配

方法	特点
PL-ICP	点到线的距离，更符合室内结构化的场景，依赖初始解
CSM	暴力搜索，能找到最优解
Optimization-Based	依赖于地图的梯度，可以轻易的引入额外约束

回环检测

• Scan-to-Map
• Map-to-Map
• Branch and Bound & Lazy Decison

最新技术：CSM+梯度优化

实际环境中的问题

1. 环境变化
2. 几何结构相似环境
3. 建图的操作复杂
4. 全局定位
5. 地面材质的变化
6. 机器人载重的改变

趋势–与视觉融合

视觉提供的信息量较丰富

视觉提供的信息

• 高精度的里程信息
• 信息量丰富的视觉地图

融合解决的问题

高精度的里程信息

• 地面材质的变化
• 机器人载重的改变

信息量丰富的视觉地图

• 几何结构相似环境
• 全局定位

3D激光SLAM

输入

• IMU数据
• 里程计数据
• 3D激光雷达数据

输出

• 3D点云地图（建图）
• 机器人的轨迹或位姿图（定位）

数据处理（非常重要）

• 轮式里程计的标定
• 激光雷达运动畸变去除
• 不同系统之间的时间同步

帧间匹配

方法	特点
Point-to-Plane ICP	点到面的距离，类似于2D的点线
Plane-to-Plane ICP(GICP)	面到面的距离
NDT	划分网格，拟合高斯分布，速度快
NICP	在ICP的基础上，引入法向量信息，精度高
IMLS-ICP	对点云进行局部曲面拟合
Feature-based Method	提取特征点和描述符进行匹配

与视觉融合

• 3D激光雷达为视觉特征提供深度信息
• 视觉辅助激光雷达进行运动畸变去除
• 视觉辅助回环检测
• 视觉提供精确里程信息

问题

• 退化环境
• 地图的动态更新
• 全局定位
• 动态环境定位

相关数学知识

坐标系变换

如图，假设机器人坐标系$B$在世界坐标系$W$下的位姿为(a,b,)，令$B$系到$W$系的转换矩阵为$T_{WB}$，则有
$$T_{WB}=\left[\begin{array}{ccc}cos\theta & -sin\theta & a\\ sin\theta & cos\theta & b\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\text{\hspace{1em}(1)}$$
已知某个向量在$B$系中的表达为$x_B$，在$W$系中的表达为$x_W$，则有
$$x_W=T_{WB}\ast x_B\text{\hspace{1em}(2)}$$
所以
(1) $T_{WB}$这个形式是怎么来的？
(2) 为什么可以这样转换得到$x_W$？

(1)
主要看$T_{WB}$的$R$部分和$t$部分
$$R=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right],t=\left[\begin{array}{c}a\\ b\end{array}\right]$$
$R$这个形式是怎么来的？
旋转分为主动旋转和被动旋转。
主动旋转


被动旋转



(2)
如(2)式，加$t$部分的原因是，经$R$旋转之后的向量在方向上已经跟$W$系对齐了（想象$B$系转一转跟$W$系对齐），剩下的就是需要把$B$系原点相对于$W$系原点的部分补上。

部分图片来自深蓝学院课件，侵删。