自动驾驶与机器人中的SLAM技术：从理论到实践

本书系统介绍自动驾驶与机器人中的 SLAM 技术，从零开始搭建一套完整的激光雷达与惯性导航定位 建图方案。理论方面使用现代化流形方法进行推导，代码方面则使用简洁明快的现代 C++ 语言实现。本书 从最基本的理论与程序代码开始，一步步增加各种模块，省略复杂的工程细节，最后形成一个完整的系统。本 书在逻辑上是完整自洽的，在内容上则是通俗易懂的。 本书从经典的卡尔曼滤波器讲到现代的预积分和图优化理论。读者可以通过实际操作，将这些算法重新 实现一遍，并比较它们之间的异同。本书内容包括惯性导航、组合导航、误差拓展卡尔曼滤波器、预积分和 图优化、二维和三维激光点云的表达、最近邻数据结构、点云配准算法，等等。最后，本书将各种算法模块 组合起来，形成完整的惯性里程计、离线地图构建和实时定位系统。 本书可作为自动驾驶和机器人定位领域的教材，适用于对该方向感兴趣的学生、教师和科研人员。

高翔，慕尼黑工业大学博士后，清华大学自动化系博士。长期从事SLAM的研究工作，研究兴趣为机器人中的视觉SLAM技术、机器学习与SLAM的结合。主要著、译作包括《视觉SLAM十四讲：从理论到实践》《机器人学中的状态估计》，在ICRA、IROS、IEEE RA-Letters、Transactions on Mechatronics、IEEE-ASME Robotics and Autonomous Systems、Autonomous Robots等国际知名期刊和会议上发表了多篇论文。

适读人群 ：本书可作为自动驾驶和机器人定位领域的教材，适用于对该方向感兴趣的学生、教师和科研人员。

本书特色

√主要用C++17作为C++标准

√追求理论与实践统一

√注重原理层面的代码实现

√提到的算法都有对应代码实现

√让算法运行得比经典实现更流畅

√从零开始搭建一套完整的激光SLAM方案

√本书配套代码全部开源

√配习题

本书内容架构

第1部分 基础数学知识

√介绍基础的几何学、运动学知识

√回顾状态估计理论的基础知识

√介绍主流的处理惯性测量单元的方法：经典的误差状态卡尔曼滤波器；预积分方法

第2部分 激光雷达的定位与建图

介绍激光SLAM的内容

√基础的点云处理方法

√实现经典的数据结构

√2D SLAM

√3D SLAM

√介绍松耦合的激光雷达–惯性导航里程计

第3部分 应用实例

介绍经典的SLAM应用

√实现一个紧耦合的激光雷达–惯性导航里程计

√介绍离线的点云地图构建方法

√介绍在已有点云地图中进行高精定位的方法

√介绍自动驾驶车辆的实时定位系统

本书的内容和特点

本书介绍自动驾驶和机器人中使用到的SLAM相关技术。这里谈到的SLAM是比较广义的，笔者会把定位、建图相关的传感器与处理方法都包含进来。一辆常见的自动驾驶车辆会含有惯性测量单元、轮速计、车速计、多线激光雷达传感器等多种传感器，本书中的定位和建图也会涉及这些传感器的处理方法。于是，读者会在本书中看到诸如惯性导航的基础算法、以卡尔曼滤波为代表的滤波器、激光点云的匹配方式、轨迹融合算法，等等。

本书的写作思路和笔者之前写的书一样，追求理论与实践统一，且非常注重原理层面的代码实现。本书提到的所有算法，都会在对应章节中给出具体的代码实现。读者将和笔者一起，从头实现这个领域里的一些重要的、基础的算法结构，并且使用现代的编程方式，充分地利用并行化原理，让算法运行得比经典实现更流畅。相应地，笔者不会拘泥于某个开源代码的具体实现。例如，本书避免介绍 LOAM 的某几行做了什么，或者Cartographer的某个文件引用了哪个库。不谈论线程池、参数文件格式这些工程化的内容。是的，那样太琐碎，而且每个人的实现方式都不一样。

本书尽量只保留核心部分的算法代码，让读者自己调试、理解整个过程。笔者仍然会使用以往的写作风格。了解笔者的读者应该会快速适应，而不了解笔者的读者也不会觉得学习本书中的知识过于困难。笔者希望自己的写作就像谈话那样通俗易懂。在介绍内容的过程中，希望读者能够清晰地理解笔者的思路，而非堆砌零散的信息。尽管这种写作风格可能导致文字上有些啰嗦，但笔者相信这样做对表达观点是有益的。本书的绝大多数重点内容配有对应的实现代码。这是本书最大的特点之一。笔者相信对于教学用书，给出所见即所得的代码永远是一个明智的选择。尽管笔者尽量精简了代码，但本书的代码部分仍然比笔者写的上一本书多了很多。

如何使用本书

本书的内容安排基本遵循由浅入深的原则，但即使像第2章那样的基础内容，也需要一些铺垫。笔者希望将本书作为《视觉SLAM十四讲：从理论到实践》的后续读物。读者至少应该先读完《视觉SLAM 十四讲：从理论到实践》的前6章，熟悉一些基础的数学原理和优化库的基本使用方法。如果读者没有读过《视觉SLAM十四讲：从理论到实践》，那么至少应该具备以下几方面的知识。

?? 大学本科阶段的基础数学知识，如微积分、线性代数、概率论。

?? 大学研究生阶段的数学知识：最优化、矩阵论，一小部分李群与李代数知识。

?? 计算机科学知识：Linux 系统操作、C++ 语言。

如果读者阅读本书某部分内容有困难，建议查阅对应的参考书作为补充。整体而言，本书比《视觉 SLAM十四讲：从理论到实践》略难，知识点更密集。

本书的代码按照章节划分。例如，第3章的代码位于src/ch3目录下。各章的代码会编译为单独的库文件和可执行文件。此外，共有的代码会放置在src/common 目录下（例如一些公有结构体、消息定义、UI等）。各章的代码存在一定程度的依赖关系，后面几章的代码会复用前面几章的结果。本书的代码需要依赖ROS进行编译，但实际运行和测试过程不需要用到ROS的机制，仅使用ROS数据包进行存储。读者只需了解ROS的安装过程，不必事先熟悉ROS的相关细节。

第一部分 基础数学知识 1

第1章 自动驾驶 3

1.1 自动驾驶技术 3

1.1.1 自动驾驶能力与分级 3

1.1.2 L4 的典型业务 6

1.2 自动驾驶中的定位与地图 10

1.2.1 为什么L4自动驾驶需要定位与地图 10

1.2.2 高精地图的内容与生产 12

1.3 本书内容的介绍顺序 14

第2章 基础数学知识回顾 17

2.1 几何学 19

2.1.1 坐标系 19

2.1.2 李群与李代数 26

2.1.3 SO(3)上的BCH线性近似式 27

2.2 运动学 27

2.2.1 李群视角下的运动学 28

2.2.2 四元数视角下的运动学 29

2.2.3 四元数的李代数与旋转矢量间的转换 30

2.2.4 其他几种运动学表达方式 32

2.2.5 线速度与加速度 34

2.2.6 扰动模型与雅可比矩阵 35

2.3 运动学演示案例：圆周运动 37

2.4 滤波器与最优化理论 40

2.4.1 状态估计问题与最小二乘法 40

2.4.2 卡尔曼滤波器 40

2.4.3 非线性系统的处理方法 41

2.4.4 最优化方法与图优化 42

2.5 本章小结 44

习题 44

第3章 惯性导航与组合导航 47

3.1 IMU系统的运动学 49

3.1.1 关于IMU测量值的解释 51

3.1.2 IMU测量方程中的噪声模型 51

3.1.3 IMU的离散时间噪声模型 53

3.1.4 现实中的IMU 54

3.2 使用IMU进行航迹推算 56

3.2.1 利用IMU数据进行短时间航迹推算 56

3.2.2 IMU递推的代码实验 57

3.3 卫星导航 61

3.3.1 GNSS的分类与供应商 61

3.3.2 实际的RTK安装与接收数据 63

3.3.3 常见的世界坐标系 64

3.3.4 RTK读数的显示 66

3.4 使用误差状态卡尔曼滤波器实现组合导航 72

3.4.1 ESKF的数学推导 72

3.4.2 离散时间的ESKF运动方程 77

3.4.3 ESKF的运动过程 78

3.4.4 ESKF的更新过程 79

3.4.5 ESKF的误差状态后续处理 80

3.5 实现ESKF的组合导航 82

3.5.1 ESKF的实现 82

3.5.2 实现预测过程 83

3.5.3 实现RTK观测过程 84

3.5.4 ESKF系统的初始化 87

3.5.5 运行ESKF 90

3.5.6 速度观测量 95

3.6 本章小结 98

习题 98

第4章 预积分学 99

4.1 IMU 状态的预积分学 101

4.1.1 预积分的定义 101

4.1.2 预积分测量模型 103

4.1.3 预积分噪声模型 106

4.1.4 零偏的更新 109

4.1.5 预积分模型归结至图优化 112

4.1.6 预积分的雅可比矩阵 113

4.1.7 小结 115

4.2 实践：预积分的程序实现 116

4.2.1 实现预积分类 116

4.2.2 预积分的图优化顶点 120

4.2.3 预积分方案的图优化边 121

4.2.4 实现基于预积分和图优化的GINS 126

4.3 本章小结 133

习题 133

第二部分 激光雷达的定位与建图 135

第5章 基础点云处理 137

5.1 激光雷达传感器与点云的数学模型 139

5.1.1 激光雷达传感器的数学模型 139

5.1.2 点云的表达 141

5.1.3 Packet的表达 143

5.1.4 俯视图和距离图 144

5.1.5 其他表达形式 148

5.2 最近邻问题 148

5.2.1 暴力最近邻法 149

5.2.2 栅格与体素方法 152

5.2.3 二分树与K-d树 160

5.2.4 四叉树与八叉树 172

5.2.5 其他树类方法 179

5.2.6 小结180

5.3 拟合问题 181

5.3.1 平面拟合 181

5.3.2 平面拟合的实现 184

5.3.3 直线拟合185

5.3.4 直线拟合的实现 187

5.4 本章小结 189

习题 190

第6章 2D SLAM 191

6.1 2D SLAM的基本原理 193

6.2 扫描匹配算法 195

6.2.1 点到点的扫描匹配 195

6.2.2 点到点ICP的实现（高斯-牛顿法）199

6.2.3 点到线的扫描匹配算法 203

6.2.4 点到线ICP的实现（高斯-牛顿法）204

6.2.5 似然场法 207

6.2.6 似然场法的实现（高斯-牛顿法）209

6.2.7 似然场法的实现（g2o）212

6.3 占据栅格地图 215

6.3.1 占据栅格地图的原理215

6.3.2 基于Bresenham算法的地图生成 216

6.3.3 基于模板的地图生成 218

6.4 子地图 223

6.4.1 子地图的原理 223

6.4.2 子地图的实现 224

6.5 回环检测与闭环 228

6.5.1 多分辨率的回环检测 229

6.5.2 基于子地图的回环修正 233

6.5.3 讨论 238

6.6 本章小结 241

习题 241

第7章 3D SLAM 243

7.1 多线激光雷达的工作原理 245

7.1.1 机械旋转式激光雷达 245

7.1.2 固态激光雷达 246

7.2 多线激光雷达的扫描匹配 248

7.2.1 点到点ICP 248

7.2.2 点到线、点到面ICP 254

7.2.3 NDT方法 258

7.2.4 本节各种配准方法与PCL内置方法的对比 265

7.3 直接法激光雷达里程计 267

7.3.1 使用NDT构建激光雷达里程计 267

7.3.2 增量NDT里程计 273

7.4 特征法激光雷达里程计 280

7.4.1 特征的提取 280

7.4.2 基于激光雷达线束的特征提取 280

7.4.3 特征提取部分的实现 281

7.4.4 特征法激光雷达里程计的实现 286

7.5 松耦合LIO系统 293

7.5.1 坐标系说明 293

7.5.2 松耦合LIO系统的运动与观测方程 294

7.5.3 松耦合LIO系统的数据准备 294

7.5.4 松耦合LIO系统的主要流程 297

7.5.5 松耦合LIO系统的配准部分 301

7.6 本章小结 304

习题 304

第三部分 应用实例 305

第8章 紧耦合LIO系统 307

8.1 紧耦合的原理和优点 309

8.2 基于IEKF的LIO系统 309

8.2.1 IEKF状态变量与运动方程 309

8.2.2 观测方程中的迭代过程 311

8.2.3 高维观测的等效处理 313

8.3 实现基于IEKF的LIO系统 315

8.4 基于预积分的LIO系统 319

8.4.1 预积分LIO系统的原理 319

8.4.2 代码实现 321

8.5 本章小结 327

习题 327

第9章 自动驾驶车辆的离线地图构建 329

9.1 点云建图的流程 331

9.2 前端实现 332

9.3 后端位姿图优化与异常值检验 337

9.4 回环检测 339

9.5 地图的导出 345

9.6 本章小结 347

习题 348

第10章 自动驾驶车辆的实时定位系统 351

10.1 点云融合定位的设计方案 353

10.2 算法实现 354

10.2.1 RTK初始搜索 354

10.2.2 外围测试代码 358

10.3 本章小结 360

习题 361

参考文献 363