自動駕駛02：點云預處理——02（運動補償篇LIO-SAM）

當激光雷達（LiDAR）在運動中采集點云時，每個點的時間戳不同，而車輛在移動，導致點云在不同時間點的坐標與實際情況不符，這種現象稱為運動畸變（Motion Distortion）。為了得到無畸變的點云，需要進行運動畸變補償。

LIO-SAM（IMU + LiDAR）：https://github.com/TixiaoShan/LIO-SAM

https://ieeexplore.ieee.org/document/9341176

LIO-SAM 是一個基于因子圖優化的激光雷達-慣性導航里程計（LiDAR-Inertial Odometry, LIO）的開源 SLAM 框架。它結合了激光雷達點云與IMU 數據，通過優化算法（GTSAM）進行高精度定位和建圖，特別適用于動態環境和運動畸變補償。

LIO-SAM 主要用于：

點云去畸變補償（消除 LiDAR 在運動中的扭曲）
實時激光 SLAM（局部里程計 + 全局地圖優化）
自動駕駛、無人機、機器人導航等場景

研究背景

LOAM是目前為止激光里程計（LO）領域最經典最廣泛使用的方法。但是它存在一個問題，就是它直接存儲全局體素地圖而不是局部地圖，從而很難執行回環檢測以修正漂移，或者組合GPS（提供絕對位置）等測量進行位姿修正。并且體素地圖的使用效率會隨時間降低。為了克服該問題，作者只獨立地存儲每個關鍵幀的特征，而不是在位姿估計完成后就將特征加入到全局地圖中。

另一方面，IMU和LiDAR的聯合位姿估計已經被廣泛研究，大致分為兩類。第一類是松耦合的方法，例如LOAM和LeGO-LOAM中使用IMU去除LiDAR點云的運動畸變，以及[8]-[12]使用EKF整合LiDAR和IMU的測量。第二類是緊耦合的方法，例如R-LINS[15]，使用誤差狀態卡爾曼濾波器迭代地修正機器人的位姿估計，再比如LIOM [16]聯合優化LiDAR和IMU測量。但是LIOM一次性處理所有測量，因此不能實時運行。而本文也屬于緊耦合的激光-慣性里程計方法，只是采用了因子圖優化而不是濾波的方法。

LOAM 主要基于激光點云特征匹配來估計位姿，每一幀都會根據上一幀的位姿進行增量更新。但這種方法會導致：

短期誤差累積：每一幀匹配的誤差會逐步積累，導致位姿逐步偏移。
長時間運行后誤差顯著：如果機器人或無人車長時間運行，沒有全局優化，就可能出現位置漂移，使得建圖產生錯位。

回環檢測：當機器人/無人車回到某個曾經到達的地方，通過匹配當前點云和歷史點云，可以發現當前估計的位姿和真實位姿的偏差。

1. LIO-SAM 運動畸變補償原理

LiDAR 由于逐點掃描，每個點的采集時間不同，因此在車輛運動時，同一幀點云可能包含不同時間的位置信息，導致點云扭曲（Motion Distortion）。

圖2展示了LIO-SAM的因子圖。圖中主要包含四種因子。第一種是IMU預積分因子（橙色），由兩個相鄰關鍵幀之間的IMU測量積分得到。第二種是激光里程計因子（綠色），由每個關鍵幀和之前n個關鍵幀之間的幀圖匹配結果得到。第三種是GPS因子（黃色），由每個關鍵幀的GPS測量得到。第四種是回環因子（黑色），由每個關鍵幀和候選回環關鍵幀的時序相鄰的2m+1個關鍵幀之間的幀圖匹配得到。

因子圖是什么？

因子圖是一種概率圖模型，它用于優化機器人在不同時間步的位姿（Position & Orientation），使其符合傳感器觀測數據的約束。在LIO-SAM中，因子圖的優化由 iSAM2（增量平滑和映射）算法完成。

因子圖的本質：

變量（節點）：表示待優化的機器人關鍵幀位姿（位置、方向）。

約束（因子）：連接不同變量的觀測數據，提供優化條件。

IMU預積分因子（橙色）

數據來源：IMU傳感器（慣性測量單元）。

作用：利用IMU數據，估計兩個相鄰關鍵幀之間的位移和旋轉。

原理：

由于IMU可以高頻率提供加速度和角速度信息，因此可以通過 數值積分 來推算位姿變化。

但單獨使用IMU會導致 漂移（drift），因此需要與其他傳感器（如Lidar）進行融合校正。

激光里程計因子（綠色）

數據來源：激光雷達點云的匹配結果（幀間匹配）。

作用：通過點云匹配計算關鍵幀之間的位姿變換，提高里程計的精度。

原理：

LIO-SAM在每個關鍵幀與前 n 個關鍵幀之間進行點云匹配，計算相對位姿變化。

這個因子提供了短時間尺度的幾何約束，用于修正IMU的漂移。

GPS因子（黃色）

數據來源：GPS 傳感器。

作用：提供全局定位信息，防止長期漂移。

原理：

GPS測量的是絕對坐標（經緯度或UTM坐標），可以為因子圖提供全球位置信息。

由于GPS可能存在噪聲（如城市環境中的多路徑效應），LIO-SAM并不直接采用GPS的數值，而是將其作為軟約束，用于優化位姿。

回環因子（黑色）

數據來源：基于激光雷達點云的回環檢測（Loop Closure）。

作用：當機器人經過曾經到達過的地方，檢測并校正累積漂移，提高長期建圖精度。

原理：

通過掃描匹配（Scan Matching），檢測當前關鍵幀是否與歷史關鍵幀匹配。

如果匹配成功，則添加一個回環因子，提供全局幾何約束，用于修正誤差。

LIO-SAM使用了 2m+1 個關鍵幀進行匹配，以確保匹配穩定性。

因子圖優化（iSAM2）

LIO-SAM使用 iSAM2（增量平滑和映射）算法進行因子圖優化。

iSAM2的特點：

通過增量更新的方式，避免每次重新計算整個因子圖，提高計算效率。

只在新增關鍵幀時進行局部優化，保證了實時性。

結合 IMU、Lidar、GPS、回環檢測 的多傳感器信息，提高位姿估計的精度和魯棒性。

2.? 核心思路及步驟

LIO-SAM 通過IMU 預積分（IMU Pre-integration），將 IMU 計算出的運動軌跡與 LiDAR 點云的時間戳對齊，從而補償 LiDAR 運動導致的畸變。核心思路如下：

步驟 1：獲取 LiDAR 和 IMU 數據

LiDAR 點云幀（Scan）：包含多個點，每個點有自己的時間戳。
IMU 數據（慣性測量）：
- 加速度計（Acceleration）
- 陀螺儀（Gyroscope）
- 角速度 & 線速度
時間同步：確保 IMU 與 LiDAR 的時間戳對齊（通常用硬件同步，或軟件插值處理）。

步驟 2：IMU 預積分（Pre-integration）

IMU 在兩個時刻之間的測量值（加速度 & 角速度）可以計算增量運動軌跡，即：

旋轉增量 $R_{i+1} = R_i \cdot \text{exp}(\omega \Delta t)$
位置增量 $p_{i+1} = p_i + v_i \Delta t + \frac{1}{2} a_i \Delta t^2$
速度增量 $v_{i+1} = v_i + a_i \Delta t$

LIO-SAM 通過 IMU 預積分，在 LiDAR 點云的采集時間范圍內，估計出每個時間點的位姿 $T_w(t)$ ，用于運動畸變補償。

步驟 3：將點云變換到統一坐標系

設 $p_i$ 是 LiDAR 采集的某個點
設 $T_w(t_i)$ 是該點采集時刻的世界坐標系位姿
目標是將所有點轉換到相同時間（通常是當前幀起始時間 $t_0$ ）：

其中：
- $p_i^{(w)}$ 是補償后的世界坐標
- $p_i^{(l)}$ 是 LiDAR 采集時的局部坐標
- $T_w(t_0)$ 是掃描開始時刻的位姿
- $T_w(t_i)$ 是該點采集時刻的位姿

步驟 4：因子圖優化（Factor Graph Optimization）

LIO-SAM 采用 GTSAM（Factor Graph） 進行位姿優化：

通過 IMU 計算短時間位姿變換（高頻）
通過 LiDAR 進行回環檢測（低頻）
結合 GPS，優化全局地圖

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/bicheng/75337.shtml
繁體地址，請注明出處：http://hk.pswp.cn/bicheng/75337.shtml
英文地址，請注明出處：http://en.pswp.cn/bicheng/75337.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！