robot-notes /核心/導航/3D LiDAR SLAM 建圖
3D LiDAR SLAM 建圖原理
2D SLAM 講透了「雞生蛋(要定位需地圖、要建圖需定位)」、占據柵格、scan matching 的加權最小二乘、loop closure、pose graph 全圖優化。這篇接續講 3D:把 2D 換成多線 / 固態 LiDAR 的點雲後,多了什麼、難在哪、代表方法怎麼一步步演進。2D 已講的基礎不重述。
前置:2D SLAM 建圖、LiDAR 完整解析(點雲從哪來)、高斯第一性原理(配準與卡爾曼的數學底)。 關鍵演算法、論文均附可查證來源(arXiv / DOI / GitHub),見文末來源清單;不確定的標「待查證」。
1. 核心問題:框架沒變,但維度爆炸
SLAM 的本質不變:同時估計機器人軌跡與環境地圖。3D 真正改變的是狀態維度與觀測形態:
- 位姿從 3-DoF
(x, y, θ)變 6-DoF(x, y, z, roll, pitch, yaw)。平地機器人近似 3-DoF,但上下坡、顛簸、無人機就得全 6-DoF。麻煩的是旋轉不能像(x,y,z)那樣直接相加:繞 X 轉 90° 再繞 Y 轉 90°,跟反過來的結果不同,角度還有 360° 環繞。數學上 3D 旋轉構成一個叫 SO(3) 的彎曲空間(流形),優化得在這個彎曲面上「小步前進」,不能當平地向量硬加——後面 iEKF、GTSAM 都要處理這件事。 - 觀測從「一圈 2D 輪廓」變「一片 3D 點雲」。2D 一幀幾百點;3D(16/32/64/128 線)一幀數萬至數十萬點,而且有掃描期間的運動畸變(motion distortion / skew)——機器人在一幀(以 10Hz 機械式 LiDAR 為例約 0.1 秒)內移動了,幀內早採跟晚採的點不在同一參考系。2D 低速常忽略,3D 高速不處理會嚴重歪斜。
- 地圖從 2D 占據柵格變 3D 表示(點雲 / 體素 / octree),記憶體與運算量高 1~2 個量級。
一句話:前端里程計 + 後端優化 + loop closure 的框架跟 2D 同構,但「點雲配準」這個地基的演算法、和「地圖怎麼存」,都因維度爆炸而需要全新工具。 後面所有創新(ikd-tree、iVox、iEKF)都是為了「在 3D 維度爆炸下還能即時」。
2. 點雲配準(Registration):3D SLAM 的地基
2D 的 scan matching 其實就是 2D 配準。3D 配準是同一件事——找一個剛體變換 T,把當前點雲對齊到參考(前一幀 / 地圖)(T ∈ SE(3),就是 SO(3) 旋轉再加一個平移,合起來描述一次完整的剛體移動)——但點多、無天然柵格、對初值更敏感。三條主流路線:
2.1 ICP(Iterative Closest Point)— 最樸素也最脆弱
兩步交替迭代到收斂:① 資料關聯:對 source 每個點,在 target 找最近點當對應(這步最貴,要 k-d tree 加速);② 求解變換:在當前對應下求 T 最小化殘差平方和,套回去再回到①。
殘差有兩種度量,是 ICP 的關鍵分支:
- point-to-point:殘差 = 對應兩點的歐氏距離。有閉式解(SVD),但收斂慢。
- point-to-plane:殘差 = source 點到 target 點切平面的距離(沿法向量投影)。更符合「LiDAR 量到的是表面」的物理——只罰垂直於表面的誤差、允許點沿平面滑動,收斂更快更準。3D LiDAR SLAM 幾乎都用它或其變形。
弱點(為什麼 ICP 不夠):需要好初值(最近點假設兩雲已大致對齊,初值差就配錯);代價函數非凸、易陷局部最小(長走廊、對稱場景尤其嚴重);每輪對數萬點做最近鄰搜尋、慢。
2.2 NDT(Normal Distributions Transform)— 用機率分布取代逐點對應
反轉 ICP 的思路:把參考點雲切成體素,每個體素內的點擬合成一個高斯(均值 + 協方差),整張參考圖變成連續的機率密度函數。配準時把當前點雲套上候選變換,最大化這些點落在參考 PDF 上的似然。好處:不需顯式最近鄰(點直接查落在哪個體素的高斯)、代價函數較平滑、對初值較不敏感。Autoware 的定位就常用 NDT。
2.3 特徵法:LOAM 的「邊緣點 + 平面點」— 為什麼又快又穩
LOAM(Zhang & Singh, 2014)的洞見:不必用全部點,只挑幾何上資訊量高、可重複辨識的少數特徵點。方法是對每個點算局部平滑度(曲率) c——白話講就是「這個點周圍的鄰居有多不平」:c 大 → 周圍幾何彎折劇烈 → 邊緣 / 角點(柱子、牆角);c 小 → 周圍平坦 → 平面點(地面、牆面)。
配準時對應地用兩種殘差:邊緣點算到參考邊緣線的距離(point-to-line),平面點算到參考平面的距離(point-to-plane)。快——特徵點只佔原始點雲一小部分,優化點數降一兩個量級;穩——邊緣 / 平面是場景中幾何明確、跨幀可重複觀測的結構,比「隨便挑最近點」可靠得多。
來源:Besl & McKay 1992 DOI;Generalized-ICP(Segal 2009)PDF;NDT(Biber & Straßer 2003)Wikipedia;LOAM RSS 2014 CMU RI / 期刊版 Springer DOI。
3. 前端里程計 + 後端優化:兩段式架構
和 2D graph-SLAM 同構,但 3D 把前端進一步拆出里程計層。
3.1 LiDAR Odometry:scan-to-scan vs scan-to-map
- scan-to-scan:當前幀配準到上一幀。快,但誤差直接累積、漂移快。
- scan-to-map:當前幀配準到已累積的局部地圖。約束強、更準、漂移慢,但配準更貴。
LOAM 的經典設計用兩個執行緒:高頻(~10Hz)scan-to-scan 出即時里程計、低頻(~1Hz)scan-to-map 精修並維護地圖——「快但糙」餵「慢但準」,這就是名字 odometry + mapping 的由來。
3.2 為什麼要融 IMU(LIO,LiDAR-Inertial Odometry)
純 LiDAR 里程計在三種情境會崩,IMU 正好補上:
- 去畸變(de-skew):IMU 高頻(數百~上千 Hz)積分出一幀掃描期間的連續運動,把幀內每個點補償回同一時刻 → 消除運動畸變。這是 LIO 最基本的價值。
- 快速運動 / 劇烈旋轉:LiDAR 幀率低,兩幀間高速轉動會讓配準初值極差;IMU 提供高頻運動先驗當初值,把配準從「猜」變「微調」。
- 退化場景:長走廊、隧道、空曠平面——幾何約束在某些方向消失(沿走廊滑動,點雲長一樣),IMU 的慣性約束在這些方向頂住,避免崩解。
緊耦合 vs 鬆耦合:鬆耦合是 LiDAR、IMU 各算一個位姿、事後用 EKF 融合,簡單但 LiDAR 退化時它輸出已經錯了;緊耦合把 LiDAR 的原始殘差和 IMU 的原始量測放進同一個狀態估計聯合求解,退化方向上 IMU 直接補進去,更穩更準。現代主流 LIO 都是緊耦合。
4. 代表方法演進(每代只記「比上一代多解了什麼」)
這節是名詞與方法的對照表,初讀可以跳過,選型或讀論文遇到不認得的縮寫時再回來查。
- LOAM(2014):奠基。邊緣+平面特徵、point-to-line/plane 配準、odometry+mapping 雙執行緒。低漂移、嵌入式可跑,長期居 KITTI 里程計榜首,後續幾乎都從它分支。
- LeGO-LOAM(2018):針對水平裝設的地面車最佳化。加地面分割、點雲分群去噪、兩步優化(先用地面點解
z/roll/pitch、再用邊緣點解x/y/yaw)、內建 loop closure(GTSAM)。更輕量。 - LIO-SAM(2020):升級成緊耦合 LiDAR-慣性,建在 factor graph(GTSAM/iSAM2)上。用 IMU 預積分(預先把兩個關鍵幀之間的幾百筆 IMU 量測壓成「一個等效約束」,優化時不必每筆重算、省很多時間)去畸變並提供初值;LiDAR 里程計、IMU、GPS、loop closure 因子統一進一張圖,用關鍵幀滑動視窗而非全域配準,即時性大增。
- FAST-LIO(2021):緊耦合 iterated EKF(iEKF)。EKF(擴展卡爾曼濾波)是「預測 + 量測修正」反覆估狀態的標準濾波器(數學底見高斯篇);
iterated指每次更新內部再多迭代幾輪、對非線性更準。最大貢獻是新的卡爾曼增益公式,把運算量從「隨量測點數」降成「隨狀態維度」——LiDAR 一幀上千個量測點,原本卡爾曼增益要對「上千」那麼大的矩陣求逆,改寫後只需對狀態維度(十幾維)求逆,機載電腦上一次更新 <25ms。 - FAST-LIO2(2022):① 直接用原始點配 scan-to-map(不抽特徵、不丟資訊,對各種 LiDAR 更通用);② ikd-tree(增量式 k-d tree,支援動態插入/刪除/再平衡),邊建圖邊維護最近鄰結構,達 100Hz+。
- Faster-LIO(2022):把 ikd-tree 換成 iVox(增量稀疏體素),用雜湊體素 + 近似 k-NN 換速度,固態 LiDAR 可達 1000–2000Hz。
來源:LeGO-LOAM IROS 2018 GitHub;LIO-SAM IROS 2020 arXiv 2007.00258 / GitHub;FAST-LIO arXiv 2010.08196;FAST-LIO2 arXiv 2107.06829 / GitHub;Faster-LIO GitHub。
5. 迴路閉合 + 後端優化:消除累積漂移
2D 篇已講透 pose graph 全圖優化「按信心(資訊矩陣)加權攤回」的原理——3D 完全沿用同一套加權最小二乘,只是節點變 6-DoF、邊的協方差是 6×6。這裡補 3D 特有的兩件事。
後端優化器(2D/3D 共用):把 pose graph(節點=歷史關鍵幀位姿,邊=里程計 + loop closure 約束)解成非線性最小二乘。三大現成函式庫:GTSAM(因子圖 + iSAM2 增量求解,最適合線上 SLAM,LIO-SAM 用它)、g2o(老牌通用圖優化)、Ceres(Google 通用最小二乘,Cartographer 用它)。一句話對照:前端配準解「這一兩幀怎麼對齊」的局部問題;後端 pose graph解「整條軌跡幾百個位姿怎麼全局一致」,loop closure 是撐起後端的那條跨時約束。
3D 場景描述子(loop closure 怎麼認出舊地方):3D 點雲大、視角變化大,需要緊湊的全域描述子快速檢索候選回環。Scan Context(2018)把一幀點雲以感測器為中心,按方位角(繞一圈分幾格)× 徑向距離(離中心多遠分幾格)切成棋盤,每格取落在裡面的最高點高度,壓成一張小小的 2D 矩陣當「指紋」。巧妙處:機器人原地轉一個角度,等於這張矩陣的「整列循環平移」(欄不變、列繞圈)——所以比對兩張指紋時,把列對齊回去就能認出「同一個地方、只是面朝不同方向」,對視角旋轉魯棒且檢索快,是 3D loop closure 的事實標準之一。
來源:GTSAM、g2o(Kümmerle ICRA 2011)GitHub、Ceres;Scan Context IROS 2018 GitHub。
6. 點雲地圖表示:幾種存法與取捨
| 表示 | 是什麼 | 適用 |
|---|---|---|
| 原始點雲 | 直接堆所有點 | 最完整無失真,但記憶體無上限、無 free/unknown 概念;多作中間產物 |
| 體素(voxel) | 空間切等大立方格、存代表值 | 降採樣、查詢快;Faster-LIO 的 iVox 屬此(FAST-LIO2 則用 ikd-tree,屬 k-d tree 家族、非體素) |
| Octree / OctoMap | 八叉樹,自適應解析度、機率式記 occupied/free/unknown | 空曠區用大格省空間、明確區分「沒障礙」與「沒看過」;導航避障的標準 3D 地圖 |
| NDT map | 每體素存高斯 | 緊湊,可直接用於 NDT 定位 |
| mesh / surfel | 三角網格 / 面元 | 視覺化、碰撞、模擬 |
選用直覺:即時里程計內部用 voxel / ikd-tree / iVox(配準需快速最近鄰);導航避障層用 OctoMap(要 free/unknown 區分);定位用 NDT map 或原始點雲;展示 / 模擬用 mesh。
來源:OctoMap(Hornung 2013)Springer DOI / octomap.github.io。
7. 2D SLAM vs 3D SLAM:差在哪、各自適合什麼
| 面向 | 2D SLAM | 3D SLAM |
|---|---|---|
| 位姿維度 | 3-DoF (x, y, θ) |
6-DoF,旋轉在 SO(3) 流形 |
| 感測器 | 單線 2D LiDAR(一個平面) | 多線 / 固態 3D LiDAR |
| 一幀點數 | 數百 | 數萬~數十萬 |
| 地圖 | 2D 占據柵格 | 點雲 / voxel / OctoMap / NDT / mesh |
| 配準 | scan matching(2D) | ICP / NDT / 特徵法 + 常融 IMU(LIO) |
| 運算量 | 低,嵌入式輕鬆即時 | 高 1~2 量級,逼出 ikd-tree / iVox / iEKF |
| 運動畸變 | 低速多忽略 | 必須去畸變(IMU de-skew) |
| 退化場景 | 長走廊也退化,但只在平面內 | 走廊 / 隧道 / 空曠;退化方向多,更依賴 IMU |
| 適用場域 | 室內平地:餐廳、倉庫、辦公室、醫院 | 室外 / 起伏:自駕、礦山、無人機、林地、多樓層 |
| 成本 | 2D LiDAR 便宜 | 3D LiDAR 較貴(固態化後快速下降) |
選型直覺:平地、室內、預算敏感、只需平面避障 → 2D SLAM 夠用且更省(送餐機器人主場景);有高度起伏、戶外、需 6-DoF 位姿或 3D 障礙(懸空物、坡道、樓層)→ 3D LiDAR SLAM。兩者框架同源,差在維度,以及為了在 3D 維度爆炸下還能即時所做的全部工程創新。
8. ROS2 上實際能跑的 3D SLAM 套件(2025–2026)
這節是套件與工具的盤點,初讀可以跳過,真的要選型時再回來。
前面講的是方法家族(ICP / NDT / LOAM / LIO);實務上你要的是「ROS2 裝得起來、跑得動」的套件。先點一個最常踩的坑:很多知名 LIO 的官方 repo 主分支其實是 ROS1,ROS2 放在獨立 branch(FAST-LIO 的 ROS2、DLIO 的 feature/ros2、LIO-SAM 的 ros2)——不是 apt install 一行就有的正式 release,得自己 clone 那條 branch 來 build。反而真正「原生 first-class、ROS Index / apt 直接裝」的是另一批:KISS-ICP、MOLA、RTAB-Map、GLIM。
ROS2 支援狀態與 distro 會變動,以下以 2026-06 查證的各 repo / ROS Index 為準;凡來源未明確處標「待查證」,實際採用前請點開該 repo 的 ROS2 branch / ROS Index 頁確認。
| 套件 | 方法家族 | 感測器 | ROS2 支援 | License |
|---|---|---|---|---|
| KISS-ICP | ICP(純里程計、自適應、不調參) | 純 LiDAR | 原生 apt(Humble→Rolling) | MIT |
| MOLA(mola_lidar_odometry) | 模組化 ICP SLAM | 純 LiDAR | 原生(ROS Index,Humble/Jazzy/Kilted/Rolling) | GPLv3 |
| RTAB-Map(rtabmap_ros) | graph SLAM(視覺/LiDAR、appearance loop) | LiDAR + RGB-D / 視覺 | 原生(ROS Index,Humble/Jazzy/Kilted) | BSD |
| GLIM | LIO + 全域因子圖(GPU 可選) | LiDAR + IMU | 原生(glim_ros2,有官方 Docker) |
MIT |
| Cartographer(cartographer_ros) | 2D/3D submap + 分支定界 loop | LiDAR(+IMU) | 原生但僅有限維護,新案斟酌 | Apache-2.0 |
| FAST-LIO2 | LIO(iEKF 直接法) | LiDAR + IMU | 官方 ROS2 branch + 活躍社群移植 |
GPL-2.0 |
| DLIO | LIO(連續時間運動補償) | LiDAR + IMU | 官方 feature/ros2 branch |
MIT |
| LIO-SAM | LIO(LOAM 特徵 + 因子圖 + loop + GPS) | LiDAR + 9 軸 IMU | 官方 ros2 branch(測過 Foxy/Galactic/Humble) |
BSD |
| Point-LIO | LIO(逐點高頻寬、抗劇震) | LiDAR + IMU | 官方 ROS1;社群 point_lio_ros2 |
GPL(版本待查證) |
| KISS-SLAM | 純 LiDAR(KISS-ICP + loop + g2o) | 純 LiDAR | 官方無;社群 wrapper(2025 新出) | MIT |
| Open3D-SLAM | 點雲 graph SLAM | 純 LiDAR | 過渡中(已有 Jazzy release) | BSD-3 |
| hdl_graph_slam | NDT graph SLAM(經典) | 純 LiDAR(+GPS/IMU) | 官方 ROS1;作者建議改用 GLIM | 待查證 |
怎麼選
室內送餐 / 搬運 AMR(輕量、即時,迴路閉合非首要)
- KISS-ICP:純 LiDAR、不調參、原生 apt,最省事的里程計起點,搭 Nav2 既有定位 / 建圖。
- FAST-LIO2 / DLIO:有 IMU 且要更穩的姿態估計就選這兩個輕量緊耦合 LIO(記得是 ROS2 branch、要自己 build;FAST-LIO2 社群移植成熟好上手)。
- RTAB-Map:AMR 已配 RGB-D / 相機的話,一套搞定建圖 + 定位 + 迴路,文件最齊、Nav2 範例多,最適合產品化。
- 送餐多為已知環境 + 重複路線,實務常是「先建圖一次(LIO-SAM / RTAB-Map / Cartographer)→ 之後純定位」。
戶外 / 手持測繪(大場景、要全域一致與迴路閉合)
- GLIM:大場景全域因子圖、GPU 可加速,精度與一致性佳、原生 ROS2;手持 / 背包測繪首選之一(它正是 hdl_graph_slam 的現代後繼)。
- LIO-SAM:LiDAR + 9 軸 IMU(+GPS)、含 loop closure 與 GTSAM 後端,戶外車載成熟、案例多。
- MOLA:ROS2 distro 覆蓋最完整、釋出最正規,在意長期維護與正式 apt 安裝選它(注意 GPLv3)。
License 提醒:商用產品要留意 GPL 系(FAST-LIO2 / Point-LIO 為 GPL-2.0、MOLA 為 GPLv3)對衍生散布的義務;MIT / BSD(KISS-ICP、DLIO、GLIM、RTAB-Map、Open3D-SLAM)對商用最寬鬆。
對照前面:KISS-ICP/MOLA 走 §2.1 ICP、hdl_graph_slam 走 §2.2 NDT、FAST-LIO/LIO-SAM 走 §2.3 LOAM 特徵 / §4 LIO 演進、GLIM/MOLA/RTAB-Map 是完整的 §5 後端 + loop closure 框架。
來源清單(均經查證)
配準:Besl & McKay 1992 DOI 10.1109/34.121791;Generalized-ICP 2009 PDF;NDT 2003 Wikipedia;LOAM RSS 2014 CMU RI / Springer DOI 10.1007/s10514-016-9548-2。
LIO 演進:LeGO-LOAM IROS 2018 GitHub;LIO-SAM IROS 2020 arXiv 2007.00258 / GitHub;FAST-LIO RA-L 2021 arXiv 2010.08196;FAST-LIO2 T-RO 2022 arXiv 2107.06829 / GitHub / ikd-Tree;Faster-LIO RA-L 2022 GitHub。
後端 / 地圖:Scan Context IROS 2018 GitHub;GTSAM;g2o GitHub;Ceres;OctoMap 2013 DOI 10.1007/s10514-012-9321-0。
ROS2 套件(§8):KISS-ICP、KISS-SLAM、FAST-LIO(ROS2 branch)、Point-LIO、DLIO(feature/ros2)、LIO-SAM(ros2 branch)、GLIM / glim_ros2、hdl_graph_slam、Open3D-SLAM、MOLA(ROS Index)、RTAB-Map(ROS Index)、Cartographer(ROS Index)。
Scan Context 的 repo owner 已改成
gisbi-kim(原irapkaist路徑失效),TixiaoShan/LIO-SAM仍有效——兩者都實際開過確認。待查證:Chen & Medioni point-to-plane 原始論文精確年份(1991 期刊 vs 1992 會議);§8 部分套件的 ROS2 確切 distro / branch 釋出狀態與 license(Point-LIO 的 GPL 版本、iG-LIO/LIORF 的 ROS2 狀態)會隨版本變動,以各 repo 與 ROS Index 即時頁面為準。