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

《自动驾驶与机器人中的SLAM技术：从理论到实践》  [2]  从经典的卡尔曼滤波器讲到现代的预积分和图优化理论。本书在逻辑上是完整自洽的，在内容上则是通俗易懂的，可作为自动驾驶和机器人定位领域的教材，适用于对该方向感兴趣的学生、教师和科研人员。

第一部分  基础数学知识  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.1IMU  状态的预积分学  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  SLAM243

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

7.1.1  机械旋转式激光雷达245

7.1.2  固态激光雷达  246

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

7.2.1  点到点ICP  248

7.2.2  点到线、点到面ICP254

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