一、簡介

該論《Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping》是日本先進工業科學技術研究所（AIST）的Koide等人于2022年在IEEE國際機器人與自動化會議（ICRA）上發表的一篇論文。該研究提出了一種基于全局匹配代價最小化和LiDAR-IMU緊耦合的實時三維建圖框架，旨在提高在特征稀缺或動態環境中的定位與建圖精度。
文由 Koide 等人在 2022 年發表，提出了一種全局一致且緊耦合的三維 LiDAR-IMU 建圖方法。其核心目標是提升在特征稀缺或動態環境下的三維定位與建圖精度，并保持實時性。與傳統基于位姿圖優化的方法不同，本文方法直接最小化全局匹配代價，并在前端和后端緊耦合 LiDAR 與 IMU 數據，實現全局一致性和高魯棒性。

---

二、文章貢獻（創新點）

1. 全局匹配代價最小化：不依賴傳統位姿圖優化，直接優化全局匹配代價以保證地圖全局一致性。
2. LiDAR-IMU 緊耦合：前端和后端均融合 IMU 數據，增強系統在特征稀缺環境下的魯棒性和精度。
3. GPU 加速體素化 GICP 匹配：利用 GPU 提升匹配代價計算速度，實現實時建圖。
4. 適應特征稀缺環境：可在缺乏明顯幾何特征的環境中仍保持高精度定位和地圖生成。

---

三、詳細內容

1. 算法背景

• 傳統方法問題：基于位姿圖優化的 LiDAR-SLAM 在特征豐富環境中表現良好，但在特征稀缺或動態環境中容易漂移。
• LiDAR-IMU 融合：通過將 IMU 數據引入前端與后端優化，提供額外約束，減少漂移，提高精度。
• 全局一致性需求：單純局部優化容易累積誤差，影響整體地圖質量。

---

2. 需解決的關鍵問題

1. 特征稀缺環境下定位漂移：在空曠或重復結構場景中，LiDAR 匹配不穩定。
2. 高效融合 LiDAR 與 IMU 數據：前端與后端均需充分利用 IMU 信息，同時保證計算效率。
3. 實時性與計算效率：全局優化和匹配代價計算需高效以支持實時運行。

---

3. 算法流程

1. 預處理

   • 點云去畸變、濾波、體素化。

2. 前端估計

   • 關鍵幀選擇 + 固定滯后平滑（Fixed-Lag Smoother）
   • GPU 加速的體素化 GICP 匹配代價因子
   • IMU 預積分因子約束

3. 局部建圖

   • 構建局部因子圖
   • 優化局部匹配代價 + IMU 約束

4. 全局建圖

   • 構建全局因子圖
   • 最小化全局匹配代價，保證全局一致性

---

4. 具體實現步驟

圖 1 展示了提出的整體框架。整個系統由一個預處理模塊和三個估計模塊組成：里程計估計、本地建圖和全局建圖。這些模塊都基于緊耦合的 LiDAR-IMU 融合。里程計估計模塊（即前端）負責魯棒地追蹤傳感器的運動，并提供最新狀態的初始估計。隨后，本地建圖模塊會進一步優化這些狀態，并將若干局部幀整合成一個子圖。全局建圖模塊則對這些子圖的位姿進行整體優化，以確保地圖全局一致性。整個流程通過多線程并行運行，提高效率。

圖1

用 $x\_t$ 來表示需要估計的傳感器狀態：

$$x_t=[T_t,v_t,b_t{]}^T,$$

其中 KaTeX parse error: Undefined control sequence: \[ at position 8: T\_t = \?[?R\_t | t\_t] \i… 表示位姿，$v\_t\in {\mathbb{R}}^3$ 為速度，KaTeX parse error: Undefined control sequence: \[ at position 8: b\_t = \?[?b^a\_t, b^\omeg… 是 IMU 的加速度和角速度偏置。狀態估計依賴 LiDAR 點云 $P\_t$ 以及 IMU 測量（線加速度 $a\_t$ 和角速度 $\omega \_t$）。為了簡化處理， 將 LiDAR 點云統一映射到 IMU 坐標系下。

接下來，將介紹兩個核心因子：LiDAR 匹配代價因子和 IMU 預積分因子，這些因子是框架中因子圖的主要組成部分，然后再詳細說明每個模塊的實現。

---

4.1、 LiDAR 匹配代價因子

匹配代價因子用來約束兩個位姿 $T\_i$ 和 $T\_j$，目標是讓它們對應的點云 $P\_i$ 和 $P\_j$ 盡可能對齊。采用體素化 GICP（Voxelized GICP, VGICP）作為代價函數，它是 GICP 的一種變體，適合 GPU 加速計算。

VGICP 會將每個點 $p\_k\in P\_i$ 用高斯分布 $(\mu \_k,C\_k)$ 來建模，其中協方差 $C\_k$ 由鄰域點計算得到。點云 $P\_j$ 則被離散為體素，并為每個體素計算均值和協方差。匹配代價定義為：

$$e_M(P_i,P_j,T_i,T_j)=\sum _{p_k\in P_i}{e}_{D2D}(p_k,T_i^{?1}{T}_j),$$

$$e_{D2D}(p_k,T_{ij})=d_k^T(C_k^0+T_{ij}{C}_k{T}_{ij}^T{)}^{?1}{d}_k,$$

其中 $d\_k=\mu \_k^0?T\_ij\mu \_k$ 是點間殘差，$p\_k^0=(\mu \_k^0,C\_k^0)$ 來自 $P\_j$ 的體素地圖。

每次優化迭代都會重新計算并線性化 $e_M$，相比傳統的 SE(3) 相對位姿約束，VGICP 提供了更精確的約束信息。

---

4.2、 IMU 預積分因子

IMU 預積分因子能夠高效地將 IMU 測量融入因子圖。給定加速度 $a\_t$ 和角速度 $\omega \_t$，傳感器狀態隨時間演化：

$$R_{t+\Delta t}=R_t\exp (({\omega }_t?b_{\omega }^t?{\eta }_{\omega }^t)\Delta t),$$

$$v_{t+\Delta t}=v_t+g\Delta t+R_t(a_t?b_a^t?{\eta }_a^t)\Delta t,$$

$$t_{t+\Delta t}=t_t+v_t\Delta t+\frac{1}{2}g\Delta t^2+\frac{1}{2}{R}_t(a_t?b_a^t?{\eta }_a^t)\Delta t^2.$$

通過預積分，可以在兩個時間步 $i$ 和 $j$ 之間獲得相對運動約束，從而在缺乏幾何特征或 LiDAR 因子受限的環境中仍能保持穩定估計，并減少四自由度漂移，同時提供重力方向約束。

---

4.3、 預處理

輸入點云先經過體素濾波下采樣。在去畸變過程中，每個體素不僅對點位置取平均，也對時間戳取平均。如果點的時間與體素時間差過大，會將其重新分配到新體素，以避免掃描首尾點被錯誤融合。

接著，為每個點找到 $k$ 個鄰近點，用于協方差估計。假設去畸變過程中點的鄰域關系不會發生大的變化，這些鄰近點可直接用于后續計算。

---

4.4、 里程計估計

里程計估計模塊通過融合 LiDAR 和 IMU 測量，補償快速傳感器運動并穩健地估計傳感器狀態。首先，根據 IMU 動力學預測，將點云變換到 IMU 坐標系，以去除運動畸變。隨后，通過預先計算的鄰近點協方差，為每個點建立不確定性模型。

在去畸變后的點云基礎上，構建因子圖（如圖 2 所示）。為了保持實時性能，采用固定時滯平滑（fixed-lag smoothing）方法，并對舊幀進行邊緣化。借鑒直接稀疏里程計（Direct Sparse Odometry）的思路，引入關鍵幀機制來實現高效且低漂移的軌跡估計。關鍵幀是一組在空間上分布合理、且與最新幀重疊度較高的幀。在最新幀與每個關鍵幀之間添加匹配代價因子，以減小漂移。若關鍵幀已被固定時滯平滑器邊緣化，則其位姿被視為固定，并通過一元匹配代價因子約束最新幀的位姿。

圖2

關鍵幀管理策略如下：

1. 定義兩幀 $P\_i$ 和 $P\_j$ 的重疊率為 $P\_i$ 中落在 $P\_j$ 體素內的點占比 。

2. 新幀到達時，若其與最新關鍵幀重疊率低于閾值（如 90%），則將其加入關鍵幀列表。

3. 為避免冗余關鍵幀：

   • 移除與最新關鍵幀重疊率低于閾值（如 5%）的關鍵幀；

   • 若關鍵幀總數超過 $N\_\text{odom}$（如 20），則移除評分最低的關鍵幀：

     s(i)=o(i,Nodom)∑j∈[1,Nodom?1]?{i}(1?o(i,j)),s(i) = o(i, N_{\text{odom}}) \sum_{j \in [1, N_{\text{odom}}-1] \setminus \{i\}} (1 - o(i, j)), s(i)=o(i,Nodom?)j∈[1,Nodom??1]?{i}∑?(1?o(i,j)),

     其中 $o(i,j)$ 表示第 $i$ 幀與第 $j$ 幀關鍵幀的重疊率。該評分函數旨在保持關鍵幀在空間上的均勻分布，同時保留最新幀附近的關鍵幀。

此外，在最新幀與最近幾幀（如最近三幀）之間建立匹配代價因子，以增強里程計對快速運動的魯棒性。同時，連續幀之間添加 IMU 預積分因子，在特征稀少環境下進一步提升估計穩定性。

---

4.5、 局部建圖

里程計估計中被邊緣化的幀會被送入局部建圖模塊，作為子圖（submap）的初始狀態。局部建圖將若干幀組合為子圖，從而減少全局優化變量數量。

處理流程如下：

1. 使用邊緣化狀態重新去畸變（deskewing）并計算協方差，從而獲得更準確的點云初始估計。
2. 評估新幀與子圖中最新幀的重疊率，若低于閾值（如 90%），則將新幀加入子圖因子圖。
3. 為子圖內每一幀組合添加匹配代價因子，實現全幀兩兩配準。
4. 連續幀間添加 IMU 預積分因子，并為每幀速度與偏置添加邊緣化狀態先驗因子，以增強子圖優化的穩定性。

當子圖幀數達到 $N\_\text{sub}$（如 15）或首尾幀重疊率低于閾值（如 5%）時，通過 Levenberg-Marquardt 優化器 對因子圖進行優化，并將優化結果合并為單個子圖。

---

4.6、 全局建圖

全局建圖模塊用于消除累積漂移，實現全局一致的地圖構建。每對子圖間若重疊率超過閾值（如 5%），則創建匹配代價因子，生成稠密的全局因子圖。這樣，子圖不僅與相鄰子圖對齊，還能與重新訪問的子圖對齊，從而實現閉環校正。

由于子圖創建間隔較大（如 10 秒），僅依賴 IMU 因子無法有效約束子圖間相對位姿，同時會丟失局部建圖的速度與偏差信息。為解決這一問題， 為每個子圖 $x\_i$ 引入兩個“端點”狀態：$x\_i^L$ 和 $x\_i^R$，分別對應子圖首尾幀相對于子圖原點的狀態。

具體做法：

1. 子圖原點 $T\_i$ 定義為中間幀 $T\_N\_\text{sub}/2$ 的位姿。
2. 將每幀狀態 $x\_t$ 表示為相對于子圖原點的狀態。
3. 在 $x\_i$ 與端點 $x\_i^L,x\_i^R$ 之間創建相對狀態因子，以滿足公式 11–13。
4. 在$x\_i^R$ 與 $x\_{i+1}^L$ 之間添加 IMU 因子，以覆蓋較短時間間隔，既約束子圖位姿，又保留局部建圖速度與偏差信息。

每當累積若干新子圖（如每 5 個）時，使用 GTSAM 的 iSAM2 優化器對全局因子圖進行增量優化，持續更新全局一致的地圖。

---

四、優勢總結

1. 全局一致性：通過全局匹配代價最小化保證地圖一致性。
2. 高魯棒性：緊耦合 LiDAR-IMU，可在特征稀缺或動態環境中保持精度。
3. 實時性：GPU 加速匹配代價計算，前端/后端優化高效。
4. 靈活擴展：方法可適應不同傳感器速率、不同點云密度和環境類型。

---

五、參考文獻

1. Koide, K., Yokozuka, M., Oishi, S., & Banno, A. (2022). Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping. IEEE ICRA 2022. PDF
2. Koide, K., Yokozuka, M., Oishi, S., & Banno, A. (2021). Globally Consistent 3D LiDAR Mapping with GPU-accelerated GICP Matching Cost Factors. IEEE Robotics and Automation Letters, 6(3), 5282–5289.
3. Shan, T., Englot, B., Meyers, D., Wang, W., Ratti, C., & Rus, D. (2020). LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping. arXiv:2007.00258.

---

相關文獻