自動駕駛與機器人中的SLAM技術：從理論到實踐

第一部分 基礎數(shù)學知識 1
第1章 自動駕駛 3
1.1 自動駕駛技術 3
1.1.1 自動駕駛能力與分級 3
1.1.2 L4 的典型業(yè)務 6
1.2 自動駕駛中的定位與地圖 10
1.2.1 為什么L4自動駕駛需要定位與地圖 10
1.2.2 高精地圖的內容與生產 12
1.3 本書內容的介紹順序 14
 
第2章 基礎數(shù)學知識回顧 17
2.1 幾何學 19
2.1.1 坐標系 19
2.1.2 李群與李代數(shù) 26
2.1.3 SO(3)上的BCH線性近似式 27
2.2 運動學 27
2.2.1 李群視角下的運動學 28
2.2.2 四元數(shù)視角下的運動學 29
2.2.3 四元數(shù)的李代數(shù)與旋轉矢量間的轉換 30
2.2.4 其他幾種運動學表達方式 32
2.2.5 線速度與加速度 34
2.2.6 擾動模型與雅可比矩陣 35
2.3 運動學演示案例：圓周運動 37
2.4 濾波器與最優(yōu)化理論 40
2.4.1 狀態(tài)估計問題與最小二乘法 40
2.4.2 卡爾曼濾波器 40
2.4.3 非線性系統(tǒng)的處理方法 41
2.4.4 最優(yōu)化方法與圖優(yōu)化 42
2.5 本章小結 44
習題 44
 
第3章 慣性導航與組合導航 47
3.1 IMU系統(tǒng)的運動學 49
3.1.1 關于IMU測量值的解釋 51
3.1.2 IMU測量方程中的噪聲模型 51
3.1.3 IMU的離散時間噪聲模型 53
3.1.4 現(xiàn)實中的IMU 54
3.2 使用IMU進行航跡推算 56
3.2.1 利用IMU數(shù)據(jù)進行短時間航跡推算 56
3.2.2 IMU遞推的代碼實驗 57
3.3 衛(wèi)星導航 61
3.3.1 GNSS的分類與供應商 61
3.3.2 實際的RTK安裝與接收數(shù)據(jù) 63
3.3.3 常見的世界坐標系 64
3.3.4 RTK讀數(shù)的顯示 66
3.4 使用誤差狀態(tài)卡爾曼濾波器實現(xiàn)組合導航 72
3.4.1 ESKF的數(shù)學推導 72
3.4.2 離散時間的ESKF運動方程 77
3.4.3 ESKF的運動過程 78
3.4.4 ESKF的更新過程 79
3.4.5 ESKF的誤差狀態(tài)后續(xù)處理 80
3.5 實現(xiàn)ESKF的組合導航 82
3.5.1 ESKF的實現(xiàn) 82
3.5.2 實現(xiàn)預測過程 83
3.5.3 實現(xiàn)RTK觀測過程 84
3.5.4 ESKF系統(tǒng)的初始化 87
3.5.5 運行ESKF 90
3.5.6 速度觀測量 95
3.6 本章小結 98
習題 98
 
第4章 預積分學 99
4.1 IMU 狀態(tài)的預積分學 101
4.1.1 預積分的定義 101
4.1.2 預積分測量模型 103
4.1.3 預積分噪聲模型 106
4.1.4 零偏的更新 109
4.1.5 預積分模型歸結至圖優(yōu)化 112
4.1.6 預積分的雅可比矩陣 113
4.1.7 小結 115
4.2 實踐：預積分的程序實現(xiàn) 116
4.2.1 實現(xiàn)預積分類 116
4.2.2 預積分的圖優(yōu)化頂點 120
4.2.3 預積分方案的圖優(yōu)化邊 121
4.2.4 實現(xiàn)基于預積分和圖優(yōu)化的GINS 126
4.3 本章小結 133
習題 133
 
第二部分 激光雷達的定位與建圖 135
第5章 基礎點云處理 137
5.1 激光雷達傳感器與點云的數(shù)學模型 139
5.1.1 激光雷達傳感器的數(shù)學模型 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 平面擬合的實現(xiàn) 184
5.3.3 直線擬合185
5.3.4 直線擬合的實現(xiàn) 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的實現(xiàn)（高斯-牛頓法）199
6.2.3 點到線的掃描匹配算法 203
6.2.4 點到線ICP的實現(xiàn)（高斯-牛頓法）204
6.2.5 似然場法 207
6.2.6 似然場法的實現(xiàn)（高斯-牛頓法）209
6.2.7 似然場法的實現(xiàn)（g2o）212
6.3 占據(jù)柵格地圖 215
6.3.1 占據(jù)柵格地圖的原理215
6.3.2 基于Bresenham算法的地圖生成 216
6.3.3 基于模板的地圖生成 218
6.4 子地圖 223
6.4.1 子地圖的原理 223
6.4.2 子地圖的實現(xiàn) 224
6.5 回環(huán)檢測與閉環(huán) 228
6.5.1 多分辨率的回環(huán)檢測 229
6.5.2 基于子地圖的回環(huán)修正 233
6.5.3 討論 238
6.6 本章小結 241
習題 241
 
第7章 3D SLAM 243
7.1 多線激光雷達的工作原理 245
7.1.1 機械旋轉式激光雷達 245
7.1.2 固態(tài)激光雷達 246
7.2 多線激光雷達的掃描匹配 248
7.2.1 點到點ICP 248
7.2.2 點到線、點到面ICP 254
7.2.3 NDT方法 258
7.2.4 本節(jié)各種配準方法與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 特征提取部分的實現(xiàn) 281
7.4.4 特征法激光雷達里程計的實現(xiàn) 286
7.5 松耦合LIO系統(tǒng) 293
7.5.1 坐標系說明 293
7.5.2 松耦合LIO系統(tǒng)的運動與觀測方程 294
7.5.3 松耦合LIO系統(tǒng)的數(shù)據(jù)準備 294
7.5.4 松耦合LIO系統(tǒng)的主要流程 297
7.5.5 松耦合LIO系統(tǒng)的配準部分 301
7.6 本章小結 304
習題 304
 
第三部分 應用實例 305
第8章 緊耦合LIO系統(tǒng) 307
8.1 緊耦合的原理和優(yōu)點 309
8.2 基于IEKF的LIO系統(tǒng) 309
8.2.1 IEKF狀態(tài)變量與運動方程 309
8.2.2 觀測方程中的迭代過程 311
8.2.3 高維觀測的等效處理 313
8.3 實現(xiàn)基于IEKF的LIO系統(tǒng) 315
8.4 基于預積分的LIO系統(tǒng) 319
8.4.1 預積分LIO系統(tǒng)的原理 319
8.4.2 代碼實現(xiàn) 321
8.5 本章小結 327
習題 327
 
第9章 自動駕駛車輛的離線地圖構建 329
9.1 點云建圖的流程 331
9.2 前端實現(xiàn) 332
9.3 后端位姿圖優(yōu)化與異常值檢驗 337
9.4 回環(huán)檢測 339
9.5 地圖的導出 345
9.6 本章小結 347
習題 348
 
第10章 自動駕駛車輛的實時定位系統(tǒng) 351
10.1 點云融合定位的設計方案 353
10.2 算法實現(xiàn) 354
10.2.1 RTK初始搜索 354
10.2.2 外圍測試代碼 358
10.3 本章小結 360
習題 361
參考文獻 363