LIO論文閱讀：Tightly Coupled 3D Lidar Inertial Odometry and Mapping

論文地址與代碼地址

論文：arxiv
代碼：GitHub

摘要

Ego-motion 在移動機器人應用中很重要。傳感器融合可以補償單傳感器缺陷。這篇文章主要介紹了一種LiDAR-IMU 融合的方法，通過聯合優化IMU和LiDAR的測量數據，可以做到在LiDAR退化的情況下也沒有明顯漂移。
另外，本文提出了一種rotation-constrained refinement 算法（LIO-mapping），用於配準LiDAR位姿和全局地圖。
實驗結果顯示，所提出的方法可以做到以IMU的速率估計傳感器的位置，即使在快速移動和缺少特徵情況下也能工作。

I. INTRODUCTION

LiDAR優點：在機器人領域廣泛應用，頻率10Hz，水平的 field of view(FOV)是360 $\degree$。受光照影響小，高精度和高可靠性。

LiDAR的缺點：豎直方向上分辨率太小，太過於稀疏，因此能提供的特徵也很少，讓特徵跟蹤變得棘手。
安裝在機器人上的LiDAR由於機器人的運動，會使測量的數據變差。
另外，在一個狹窄的走廊環境中，只能得到部分天花板，牆和底板的點雲。特徵過少導致跟蹤的丟失。
還有一個問題：更新頻率太低。

在本文中，提出IMU-LiDAR緊耦合的位姿估計算法來解決上述問題。
使用了 fix-lag smoothing 和 原有位姿邊緣化的方法達到實時和一致性的估計，然後再進行 rotation-constrained refinement。

主要工作如下：

• LiDAR-IMU緊耦合算法提供高頻的，實時的，準確的位姿估計。
• rotational constrained refinement 方法優化最終的位姿並且產生點雲地圖，提高一致性和魯棒性的估計。
• 算法在室內外環境都進行了測試，效果比松耦合算法優異。
• 算法開源，業界第一個LiDAR-IMU緊耦合算法。

本文總體結構：
II 介紹相關工作；
III 介紹標記與註記；
IV 介紹里程計算法；
V 介紹 refinement （配準）算法；
VI 和 VII 介紹了實現與測試結果；
VIII 結論。

II. RELATED WORKS

松耦合算法：將LiDAR和IMU的狀態估計分別單獨考慮。
[1] 中的雷達里程計算法帶有IMU的輔助信息。使用加速度信息時會假設它是 zero velocity 的。它將 IMU 和雷達的數據分離，並且將IMU信息作爲整個系統的先驗，因此在優化中，它沒有最大化 IMU 信息的利用。
[2] 中使用EKF來融合2D 情況下的 LiDAR 和 IMU 的信息，但它不能處理3D或者更復雜的環境。
[3] 講了一種模塊化的IMU融合方式，通過 EKF 進行融合。這種松耦合的方式雖然計算效率很高，但是和緊耦合的相比準確度不夠，因爲它只把里程計作爲一個黑盒，卻沒有通過IMU來更新它。
[4]採用了緊耦合的方式。
[5] 是 Soloviev 等人提出的。針對2D平面運動估計，他們提出了一種方法：抽取和匹配2D平面的線特徵。通過 IMU 的預測方位補償雷達信息。過程中應用了 卡爾曼濾波器。
[6] Hemann 等人提出了以誤差狀態的卡爾曼濾波器形式，緊耦合IMU 傳播誤差和 LiDAR heightmap。這種方式在環境地圖已知時，沒有GPS信息也能長時間工作，但沒有地圖無法工作。
[7] 和 [8] 未處理數據和 IMU 的預測數據直接用於計算殘差和進行優化。這種方式沒有牽涉轉化和狀態估計，在系統快速運動時，系統會不可用。

受到其他 VI 方式啓發，如[10] ,[11] ，我們基於預積分和緊耦合的方式設計了整個系統。使用未處理的IMU數據和LiDAR數據進行狀態優化。
整個系統可以在雷達退化或者運動十分快速時也能工作得很好。
據我們所知，我們的系統是少數幾個適合複雜3D環境的。

III. NOTATIONS AND PRELIMINARIES

A. Notations

雷達每一線的測量記爲 $scan \ C$。
包含所有 $scan$ 的一次測量記爲 $sweep \ S$。16線雷達在一次sweep中包含16個scan。

變換矩陣$\bold{T}_{b}^{a} \in SE(3)$,表示把點 $\bold{x^b} \in \mathbb{R^3}$ 從座標系 $F_b$ 變換到座標系 $F_a$；
$\bold{\bar{T}}_{b}^{a}$表示IMU預測的變換；
$\bold{R}_b^a \in SO(3)$ 和 $\bold{p}_b^a$ 分別是$\bold{T}_{b}^{a}$的旋轉和平移部分；
四元素$\bold{q}_b^a$對應於$\bold{R}_b^a$；

$\bold{\hat{a}}_k$和$\bold{\hat{\omega}}_k$表示在時刻 $k$ 時，IMU的加速度和角速度測量；

在座標系 $F_a$下抽取的特徵表示爲 $\bold{F}_a$， $\bold{F}_a$可以通過 $\bold{F}_a^b$變換到座標系 ${F}_b$。

A. IMU Dynamics

1）狀態
$F_{B_i}$ 和 $F_{L_i}$ 分別是IMU和雷達的座標系；
雷達在離散時刻 $i$ 時接收 $sweep\ S_i$；

我們要進行估計的是IMU在世界座標系下 $F^W$ 的狀態 $\bold{X}_{B_i}^W$以及雷達和IMU之間的外參 $\bold{T}_B^L$，具體如下：
$\bold{X}_{B_i}^W=[ {\bold{p}_{B_i}^W}^T \ {\bold{v}_{B_i}^W}^T \ {\bold{q}_{a_i}^W}^T \ {\bold{b}_{g_i}^W}^T ]^T, \\ \bold{T}_B^L=[{\bold{p}_{B}^L}^T \ {\bold{q}_{B}^L}^T] \tag{1}$
上面的公式中：
${\bold{p}_{B_i}^W}$ 是當前body frame相對於世界座標系的位置；
${\bold{v}_{B_i}^W}$ 是當前body frame相對於世界座標系的速度；
${\bold{q}_{B_i}^W}$ 是當前body frame相對於世界座標系的姿態；
$\bold{b}_a$是IMU的加速度偏置；
$\bold{b}_g$是IMU的角速度偏置；

2）動態模型
輸入：IMU的加速度和角速度；
輸出：${\bold{X}_{B_k}^W}$，將${\bold{X}_{B_i}^W}$ 更新到${\bold{X}_{B_j}^W}$ ；
更新方程如下：

方程內：
$\Delta t$是兩次IMU連續測量的時間間隔；
我們積分的時間區間是兩次雷達掃描之間的時間；
$\bold{g}^W$是世界座標系下的重力；

簡寫：
$(\cdot)_i \dot{=}(\cdot)^W_{B_i}$,
$\Delta t_{ij}=\Sigma ^{j-1}_{k=i}{\Delta t}$ ,
$\prod_{k=i}^{j-i}$是四元素的連乘；

3）預積分
在時刻 $i$ 到時刻 $j$ 之間，body frame 的運動可以通過預積分得到：
$z_j^i=\{\Delta \bold{p}_{ij},\Delta \bold{v}_{ij},\Delta \bold{q}_{ij}\}$
$z^i_j$具有協方差$\bold{C}^{B_i}_{B_j}$
這部分參考補充材料。

IV. TIGHTLY COUPLED LIDAR-IMU ODOMETRY

受到[7],[1],[3]的啓發，他們將里程計和建圖兩部分分開。
本文也分成兩個部分：
Sec. IV介紹緊耦合的lidar-IMU里程計，在局部窗口中優化所有的狀態；
Sec. V介紹rotation constrained refinement 過程，這是一個全局建圖過程。

A. Lidar-IMU Odometry Overview
lidar-IMU里程計系統整體框圖：

輸入是 lidar raw inputs $S_j$ 和 IMU raw inputs $I_{i,j}$
步驟：
1）在 $S_j$ 接收到之前，IMU狀態通過公式-2進行迭代;
2）同時預積分得到 $\Delta \bold{p}_{ij},\Delta \bold{v}_{ij},\Delta \bold{q}_{ij}$；
3）接收到 $S_j$ 後，進行校正，然後得到 $\bar{S}_j$;
4）進行特徵抽取，給數據降維，得到最重要的特徵點 $\bold{F}_{L_j}$;
5）上一次雷達特徵點 $\bold{F}_{L_{o,i}}$ 根據上一次的對應優化狀態 $\bold{T}^W_{b_{o,i}}$ 和 $\bold{T}^L_B$，得到一個 local map $\bold{M}^{L_{p}}_{L_o,i{}}$;
6）根據對 $\bold{F}_j$時的 lidar pose 的預測，得到 lidar 的性對測量 $\bold{m}_{L_{p+1,j}}$;
7）聯合優化，根據雷達得到的兩幀之間的相對變換和IMU的預積分進行最大後驗估計，估計的結果再去更新1）中 IMU 狀態，避免 drift。

B. De-skewing and Feature Extraction
這一部分對應上面總體框圖的步驟3，步驟4。

3D雷達在運動時得到的數據會有失真，解決方法如下：
1.使用和LOAM中類似的方法，假設了雷達的勻速運動模型。
2.然後從IMU狀態中預測出 $\bold{\bar{T}}^{L_j}_{L_{j'}}$。
3.對於每個點 $\bold{x}(t) \in S_j \subset \mathbb{R}^3$,通過 $\bold{\bar{T}}^{L_j}_{L_{j'}}$ 線性插補，進行位姿校正並轉換座標系到 ending pose of the sweep，得到 $\bar{S}_j$。
4.這一 sweep 中的時間戳 $t \in (t_{j'},t_j]$,$t_{j'}$ 是 sweep 的開始，$t_{j}$ 是 sweep 的結束。

使用與LOAM中相同的提取特徵邊和特徵平面的方法提高計算效率。
$\bold{F}_{L_j} \in \bar{S}_j$通過曲率進行選擇，選擇最平的和最陡的。

C. Relative Lidar Measurements
這一部分對應上面總體框圖的步驟5，步驟6。

爲了和IMU預積分相適應，對於 lidar 數據採用 sweep 之間的相對位姿運動。算法如下面框圖所示：

因爲單個 sweep 不夠稠密，計算準確的特徵對應很困難，所以需要先建立一個local map。

local map包含 $N_m$ 個離散的時刻：
$\{o,...,p,...,i\}$
如下圖所示，

$o$ 是第一個lidar sweep；
$p$ 是 pivot lidar sweep；
$i$ 是最後一個 lidar sweep；

local map $\bold{M}^{L_{p}}_{L_o,i{}}$ 採用 $\bold{F}^{L_{p}}_{L_{\gamma}}$ ,$\gamma \in \{o,...,i\}$的數據，並建立在pivot座標系上。
$\bold{F}^{L_{p}}_{L_{\gamma}}$ 經過上一時刻優化得到的 lidar poses $\bold{T}^{L_{p}}_{L_{\gamma}}$ （這裏簡寫了，準確來說是$\bold{\bar{T}}^{L_{p}}_{L_{\gamma}}$ ）。

需要被預測的狀態是： $\{p+1,...,i,j\}$，裏面：
$p+1$是 pivot的下一個時間戳；
$j$ 是窗口中當前的 lidar sweep。

有了local map之後，可以找到 $\bold{M}^{L_{p}}_{L_o,i{}}$ 和原始 $\bold{F}_{L_{\alpha}}$ ,$\alpha \in \{ p+1,...,j\}$ 之間的對應關係。
這種對應關係實質就是相對雷達運動，他們兩者都是相對於他們的 pivot 位姿的運動，pivot 位姿會隨着滑動窗口變化。

在 Sec. IV-B 中抽取的原始特徵是座標系 $F_{L{\alpha}}$ 中最平坦或者是 edge 點。
在實際中，edge對效果提高不明顯，所以我們後面只針對平面點。

使用KNN找到 $\bold{x}^{L_p} \in \bold{F}^{L_{p}}_{L_{\alpha}}$ 的K個最近點 $\pi(\bold{x}^{L_{p}}) \in \bold{M}^{L_{p}}_{L_o,i{}}$；
然後將這些點擬合到座標系 $F_{L_{p}}$ 中的一個平面上去；
平面的係數通過如下方程確定：
$\bold{ \omega }^{T}\bold{x'}+d=0, \bold{x'} \in \pi(\bold{x}^{L{p}})$
$\bold{\omega }$是平面的法向量；
$d$ 是距離座標系 $F_{L_{p}}$ 原點 $o$ 的距離；

對於每個平面特徵點 $\bold{x} \in \bold{F}_{L_{\alpha}}$,$m=[ \bold{x},\omega ,d] \in \bold{m}_{L{\alpha}}$ 是它的相對雷達運動。
每個$m \in \bold{m}_{L_{\alpha}}$ , $\bold{x}$定義在座標系 $F_{L_{\alpha}}$ 中，$d$ 定義在座標系 $F_{L_{p}}$ 中。

D. Lidar Sweep Matching
這一部分對應上面總體框圖的步驟7。

相對位姿提供的是pivot 和後續lidar poses之間的相對運動。
優化過程會優化窗口中的所有位姿，包括第一個位姿 $\bold{T}^W_{L_p}$（也就是說pivot所在的座標系 $F_{L_p}$不是固定的）。
因此在lidar的 代價函數中，每個位姿都會涉及到兩個量： $\bold{T}^W_{L_p}$ 和 $\bold{T}^W_{L_{\alpha}},\alpha \in \{p+1,...,j\}$。
優化 pivot 的 pose 可以減小預積分誤差。

估計的狀態都是IMU的，因此需要通過IMU狀態引入外參來表示 lidar constraints。
pivot 後面的 lidar poses 相對於 pivot 的相對位姿變換可以這樣定義：

有了前面的對應關係，每個lidar 測量$m=[ \bold{x},\omega ,d] \in \bold{m}_{L{\alpha}}$,$\alpha \in \{p+1,...,j\}$ 的殘差都可以表示成點到平面的距離。

E. Optimization
這一部分對應總體框圖的步驟7。

優化狀態的獲得通過 fixed-lag 平滑和邊緣化。
fixed-lag 平滑在滑動窗口內保持 $N_s$ 個IMU 狀態（$\bold{X}^W_{B_p}$ 到 $\bold{X}^W_{B_j}$），圖2所示。
當有新的測量數據進啦，平滑器將加入新的量，將窗口中最舊的量邊緣化。
整個要估計的量是：
$\bold{X}=[\bold{X}^W_{B_p}, ..., \bold{X}^W_{B_j}, \bold{T}^{L}_{B}] \tag{5}$

然後去求下列馬氏距離的代價函數的最小值，獲的 $\bold{X}$ 的最大後驗估計。

上面公式中：
$\bold{r}_{\mathcal{P} }(\bold{X})$ 是邊緣化的先驗項；
$\bold{r_{\mathcal{L}}}(m,\bold{X})$ 是 relative lidar constraints；
$\bold{r_{\mathcal{B}}}(z^{\beta}_{\beta +1},\bold{X})$ 是 residual of the IMU constraints；
上面的非線性最小二乘的代價函數可以通過 Gauss-Newton 法按照 $\bold{H \delta X=-b}$ 的形式求解。LIO通過Ceres Solver 進行求解。

對於每個 lidar 的相對測量，可以從等式-4中獲得 $\bold{r_{\mathcal{L}}}(m,\bold{X})$；
協方差矩陣 $\bold{C}^{m}_{L_{\alpha}}$ 由 lidar accuracy 決定；
IMU constraint $\bold{r_{\mathcal{B}}}(z^{\beta}_{\beta +1},\bold{X})$ 通過IMU的預積分獲得，和[10]類似；
$||\bold{r}_{\mathcal{P} }(\bold{X})||^2=\bold{b}^T_{\mathcal{P} }\bold{H}^+_{\mathcal{P} }\bold{b}_{\mathcal{P} }$ 可以通過舒爾補獲得[文獻12]；

V. REFINEMENT WITH ROTATIONAL CONSTRAINTS

將特徵點註冊到全局地圖座標系 $F_W$ 而不是局部地圖，能使lidar poses 具有一致性。refinement 方法使用相對lidar 測量 $\bold{m}_L$。
對齊最近的lidar 特徵點到global map：

上面的公式中：
$\bold{T}=\bold{T}^W_L$ 是小估計的lidar poses；
相對lidar 測量$m$ 中包含的 $\bold{x}$ 定義在座標系 $F_L$ 中，係數 $\omega,d$ 定義在 $F_W$ 中；
然後用一個相似的Gauss-Newton方法去優化 $\bold{C}_{\mathcal{M}}$;
優化方式：通過殘差 $\bold{C}_{\mathcal{M}}$，$\bold{J_p^C}$, $\bold{J_{\theta}^C}$ , $\theta$ 是對應於四元素 $\bold{q}$ 的誤差狀態。

長時間的誤差累積會使建圖錯誤更大，LIO採取了和文獻[16]類似的方法，以 SE(2)-constraints 優化SE(3)。
這種方法保證了最後的圖總是和重力是對齊的。
圖3說明了這種方法：

z軸方形的姿態有更高的不確定，而roll和pitch更準確，因此姿態的Jacobian對代價函數進行限制（詳細的推導在文獻[12]中）：

上面公式中：
$\breve{(\cdot)}$ 是上一次迭代的狀態；
$\breve \bold{\Omega}_z$ 是姿態相對於座標系 $F_W$ 的信息矩陣的近似值；
$\epsilon_{x}$ , $\epsilon_{y}$通過information ratio 獲得；

然後，在優化步驟中通過$\bold{J_p^C}$, $\bold{J_{\theta_z}^C}$作爲Jacobians，lidar poses 的增量可以表示爲 $\delta\bold{\theta}_z$ 和$\delta\bold{p}$，然後獲得狀態的更新：

VI. IMPLEMENTATION

介紹關於以下四個方面的內容：

• 傳感器配置
• 系統初始化
• 參數
• 室內及室外測試

A. Different Sensor Configurations
lidar和IMU距離很近的時候，算法按照上面介紹的進行就可以；
lidar和IMU距離遠的時候，需要將外參加到 等式-6 (總體的最小二乘代價函數)，如下圖：

B. Initialization
1.粗略的匹配算法使用LOAM的初始化方式；
2.當IMU和 lidar 的測量數據足夠多的時候，可以用這些數據進行初始化；
3.在實驗中，採用文獻[10]的方式進行線性外參初始化；
4.最後再滑動窗口內進行非線性優化。

VII. TESTS AND ANALYSES

進行了室內和室外的實驗，並提供定性和定量的分析。

A. Quantitative Analysis
1）不同環境下的測驗：
LIO 是帶有窗口優化的方法；
LIO-raw 是沒有運動補償的方法；
LIO-no-ex 是沒有在線外參估計的方法；
LIO-mapping 是帶有 rotaional constraints的方法，效果最好，速度在高速時效果更好，低速時漂移多一點。

2）隨時間漂移
LIO-mapping 的效果比LOAM或LOAM+IMU的效果好。

B. Qualitative Results
進行了 KAIST Urban dataset 的實驗，在視頻中展示結果。

C. Running Time Analysis
配置：Intel i7-7700K CPU at 4.20GHz,16GB RAM
總結了室內室外雷達的建圖運行時間。

VIII. CONCLUSION

1.提出了緊耦合的 lidar-IMU 融合方法；
2.LIO需要初始化，但是也更加魯棒，更新頻率也更高。