LR FAST-LIO

Abstract

一個高效的LiDAR-inertial 里程計框架. 我們融合了LiDAR特征點和IMU數據, 用緊耦合的迭代EKF.

為了降低大量觀測數量導致的計算負載, 我們用了一種新的方法來計算 Kalman gain. 新的方法的計算量是基於狀態量維度而不是測量維度.

我們提出的方法在很多地方測過了, 在一次掃描中用了多余1200個特征點, 一次iEKF的耗時是25ms.

1. Introduction

1. LiDAR觀測的特征點一般是幾何結構(e.g edge 和 plane). 當UAV在cluttered環境里運行的時候, LiDAR的方案會退化, 特別是小FOV的LiDAR.
2. ...
3. ...

我們提出了FAST-LIO. 貢獻:

1. 為了解決快速移動, 噪聲或者是cluttered環境(退化會發生的時候), 我們采用了緊耦合的 iEKF來融合LiDAR特征點和IMU觀測.
2. 降級計算力, 提出了新方法. 跟傳統的卡爾曼增益是等價的
3. 沒啥子.
4. 我們測試了很多.

2. Related works

A. LiDAR Odometry and Mapping

[7] 提出了ICP方法. 但是對於稀疏的LiDAR觀測來說, ICP需要的准確的點的匹配很少.

為了解決這個問題, [8] 提出了一個 generalized-ICP, 是基於 點-平面距離的.

然后[9] 又把ICP方法和點-edge進行組合, 開發了一個LiDAR odometry and mapping 框架 (LOAM).

后面就有很多LOAM的變種了, 比如 LeGO-LOAM [10] 和 LOAM-Livox[11].

雖然這些方法對於幾何環境, 還有大FoV工作的很好, 但是對於沒有特征的環境很差.

B. Loosely-coupled LiDAR-Inertial Odometry

IMU-aided LOAM [9].

[12] 將IMU觀測和 LiDAR測量的粒子濾波融合, 用error-state EKF.

[13] 加入了IMU-重力 模型來估計6DOF ego-motion 來輔助LiDAR掃描.

[14] 用 MSCKF 來融合掃描結果和IMU以及視覺測量.

還是有點問題, 它忽略了系統其他狀態 (比如 速度) 和新掃描的pose.

C. Tightly-coupled LiDAR-Inertial Odometry

和松耦合不同, 緊耦合 LiDAR-inertial 里程計方法融合了 raw-feature points (而不是 scan registration results) with IMU.

有兩種緊耦合的方法, 優化和濾波.

[15] 用了圖優化和IMU預積分約束[16] 和 LiDAR 特征的平面約束.

[18] 最近日出了LIOM, 用類似的graph 優化, 但是是基於edge 和平面特征的.

[19] 用 Gaussian Partical Filter (GPF) 融合IMU的數據和平面2D LiDAR. 波士頓動力 Atlas 任性機器人就用的這個方法.

因為粒子濾波隨着LiDAR點的算力提升的太快了, EKF [20] , UKF [21] 和 IKF [22]更受偏好.

3. Methodolody

A. Framework Overview

B. System Description

1. Continuous model

假設IMU跟LiDAR是剛體互聯的, kinematic model如下:

1. Discrete model

1. Measurement model

因為lidar點的采樣率很高 (e.g. 200kHz), 一般不會每次收到就處理. 一般采用積累到一定階段然后處理的方法. 這種叫做一個scan.

C. States Estimator

我們用iterated extended Kalman filter.

假設上一次最優狀態估計在時間 \(t_{k-1}\)是 \(\bar{x}_{k-1}\), 協方差是 \(\bar{P}_{k-1}\).

生成一個MAP:

\[\min _{\widetilde{\mathbf{x}}_{k}^{\kappa}}\left(\left\|\widetilde{\mathbf{x}}_{k}^{\kappa}\right\|_{\widehat{\mathbf{P}}_{k}^{-\frac{1}{2}} \mathbf{J}^{T}}+\sum_{j=1}^{m}\left\|\mathbf{z}_{j}^{\kappa}+\mathbf{H}_{j}^{\kappa} \widetilde{\mathbf{x}}_{k}^{\kappa}\right\|_{\mathbf{R}_{j}^{-1}}^{2}\right)\tag{17} \]

(17)的結果是標准的迭代Kalman濾波[22, 26].

\[\begin{aligned} \mathbf{K} &=\mathbf{P} \mathbf{H}^{T}\left(\mathbf{H} \mathbf{P} \mathbf{H}^{T}+\mathbf{R}\right)^{-1} \\ \widetilde{\mathbf{x}}_{k}^{\kappa} &=-\mathbf{K} \mathbf{z}_{k}^{\kappa} \Longrightarrow \widehat{\mathbf{x}}_{k}^{\kappa+1}=\widehat{\mathbf{x}}_{k}^{\kappa} \oplus\left(-\mathbf{K} \mathbf{z}_{k}^{\kappa}\right) \end{aligned} \tag{18} \]

\(\hat{x}^{k+1}_k\) 可以用 \((I_KH)P\) 來計算, 這個是真值狀態 \(x_k\) 的誤差的協方差???.

所以 \(\bar{P}_k = J^{-1}(I-KH)PJ^{-T}\).

如果IKF完全收斂了, \(J=I\). 狀態更新 (19) 和協方差更新 (21) 可以用於下一次掃描.

---

一個經常的問題是(18)里, 需要對於 \(HPH^T+R\) 求逆, 是以測量為維度的. 實際上, LiDAR的個數是很大的, 對於這個矩陣大小來求逆是難以接受的. 如[22, 26], 是只用了少量的測量.

---

我們的靈感來源於(17), 這里面cost function是 over the state, 所以可以以狀態維度來作為計算復雜度. 事實上, 如果能直接解決(17), 就可以獲得(18)一樣的解.

\[\mathbf{K}=\left(\mathbf{H}^{T} \mathbf{R}^{-1} \mathbf{H}+\mathbf{P}^{-1}\right)^{-1} \mathbf{H}^{T} \mathbf{R}^{-1} \tag{23} \]

D. Initialization

靜止初始化, 就可以得到IMU偏置和重力方向.

4. Experiment Result

A. Computational Complexity Experiments