?一、論文

研究領域：激光雷達慣性測距框架
論文：FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter
IEEE Robotics and Automation Letters, 2021
香港大學火星實驗室
論文鏈接
論文github

二、論文概要

2.1 主要思路

2.2 具體實現

2.3 實驗設計

三、論文全文

FAST-LIO：一個快速、魯棒的緊耦合迭代卡爾曼濾波器LiDAR慣性里程計包

摘要

本文提出了一種計算效率高且魯棒的激光雷達慣性測距框架。我們融合LiDAR特征點與IMU數據使用緊耦合迭代擴展卡爾曼濾波器，以允許在發生退化的快速運動，嘈雜或混亂的環境中進行魯棒導航。為了降低大量測量的存在下的計算負荷，我們提出了一個新的公式來計算卡爾曼增益。新公式的計算量取決于狀態維而不是測量維。所提出的方法及其實現在各種室內和室外環境中進行測試。在所有測試中，我們的方法實時產生可靠的導航結果：它在四旋翼機載計算機上運行，在一次掃描中融合了1，200多個有效特征點，并在25 ms內完成iEKF步驟的所有迭代。我們的代碼在Github 開源。

提出了一個新的公式來計算卡爾曼增益。新公式的計算量取決于狀態維而不是測量維。
所提出的方法及其實現在各種室內和室外環境中進行測試

INTRODUCTION

同時定位和地圖繪制（SLAM）是移動的機器人（如無人機（UAV））的基本前提。視覺（慣性）里程計（VO），例如立體式VO [1]和單目鏡VO [2，3]，由于其重量輕且成本低，通常用于移動的機器人。雖然提供豐富的RGB信息，視覺解決方案缺乏直接的深度測量，并且需要大量的計算資源來重建3D環境以進行軌跡規劃。此外，它們對光照條件非常敏感。光探測和測距（LiDAR）傳感器可以克服所有這些困難，但對于小型移動的機器人來說成本太高（而且體積太大）。

固態LiDAR最近成為LiDAR發展的主要趨勢，例如基于微機電系統（MEMS）掃描[4]和旋轉棱鏡[5]的那些。這些LiDAR具有非常高的成本效益（在類似于全局快門相機的成本范圍內），重量輕（可以由小型無人機攜帶），并且具有高性能（產生主動和直接的遠程和高精度的3D測量）。這些特征使得此類LiDAR對于UAV（尤其是工業UAV）是可行的，其需要獲取環境的準確3D地圖（例如，空中測繪）或可在具有嚴重照明變化的雜亂環境中操作（例如，災后搜索和檢查）。

盡管潛力巨大，但固態LiDAR給SLAM帶來了新的挑戰：

1）LiDAR測量中的特征點通常是幾何結構（例如，邊緣和平面）。當UAV在沒有強特征的雜亂環境中操作時，基于LiDAR的解決方案容易退化。當LiDAR具有較小的FoV時，這個問題更加明顯。

?2)由于沿著掃描方向的高分辨率，LiDAR掃描通常包含許多特征點（例如，幾千）。雖然這些特征點不足以在退化的情況下可靠地確定姿態，但是將如此大量的特征點緊密地融合到IMU測量需要UAV機載計算機負擔不起的巨大計算資源。

3)由于LiDAR使用幾個激光器/接收器對順序地采樣點，因此掃描中的激光點總是在不同時間采樣，導致運動失真，這將顯著降低掃描配準[6]。UAV螺旋槳和馬達的恒定旋轉也將顯著的噪聲引入IMU測量。

為了使激光雷達導航可行的小型移動的機器人，如無人機，我們提出了FAST-LIO，計算效率高，魯棒的激光雷達慣性測距包。具體而言，我們的貢獻如下：

1)為了科普快速運動，嘈雜或混亂的環境，退化發生，我們采用了一個緊耦合迭代卡爾曼濾波器融合LiDAR特征點與IMU測量。我們提出了一個正式的反向傳播過程來補償運動失真;

2)為了降低大量的LiDAR特征點所造成的計算負荷，我們提出了一個新的公式計算卡爾曼增益，并證明其等價于傳統的卡爾曼增益公式。新公式的計算復雜度取決于狀態維度而不是測量維度。

3)我們實現我們的配方到一個快速和強大的LiDAR慣性里程計軟件包。該系統能夠在小型四旋翼機載計算機上運行。

4)我們在各種室內和室外環境中進行實驗，并進行實際的無人機飛行測試（圖1）。1）驗證系統在快速運動或強烈振動噪聲存在時的魯棒性。

其余文件組織如下：在第節中。

第二部分，討論了相關的研究工作。

第三部分，我們在第節中給予了完整的系統管道和每個關鍵組件的詳細信息。

第四，實驗

第五部分是結論

RELATED WORKS

關于LiDAR SLAM的現有工作是廣泛的。在這里，我們將我們的評論限制在最相關的作品上：僅LiDAR測距和映射，松耦合和緊耦合LiDAR慣性融合方法。

?A. LiDAR Odometry and Mapping

Besl等人[6]提出了一種用于掃描配準的迭代最近點（ICP）方法，該方法為LiDAR測距法奠定了基礎。ICP對于密集3D掃描表現良好。然而，對于稀疏點云的激光雷達測量，ICP所需的精確點匹配很少存在。為了科普這個問題，Segal et al.[7]提出了一種基于點到平面距離的廣義ICP。然后Zhang et al.[8]將這種ICP方法與點到邊緣距離相結合，并開發了LiDAR測距和映射（LOAM）框架。此后，開發了許多LOAM變體，例如LeGO-LOAM [9]和LOAM-Livox [10]。雖然這些方法適用于結構化環境和大FoV的LiDAR，但它們非常容易受到無特征環境或小FoV LiDAR的影響[10]。

FOV雷達是指視場角（Field of View）雷達，是一種用于探測目標的雷達系統。視場角是指雷達波束所能夠覆蓋到的區域大小，通常以角度來衡量。FOV雷達可以通過改變天線的位置和方向來控制其視場角，從而實現對特定區域的探測。

B. Loosely-coupled LiDAR-Inertial Odometry

IMU測量通常用于減輕無特征環境中的LiDAR退化問題。松散耦合的LiDAR-慣性測距（LIO）方法通常分別處理LiDAR和IMU測量結果，并在稍后融合它們的結果。例如，IMU輔助LOAM [8]將從IMU測量積分的姿態作為LiDAR掃描配準的初始估計。Zhen等人[11]使用誤差狀態EKF融合IMU測量結果和LiDAR測量結果的高斯粒子濾波器輸出。Balazadegan等人[12]添加IMU重力模型以估計6-DOF自我運動，以輔助LiDAR掃描配準。Zuo等人[13]使用多狀態約束卡爾曼濾波器（MSCKF）將掃描配準結果與IMU和視覺測量融合。松耦合方法的常見過程是通過配準新的掃描來獲得姿態測量，然后將姿態測量與IMU測量融合。掃描配準和數據融合之間的分離減少了計算負荷。然而，它忽略了系統的其他狀態（例如，速度）和新掃描的姿態。此外，在無特征環境的情況下，掃描配準可能在某些方向上退化，并且在后期階段導致不可靠的融合。

C. Tightly-coupled LiDAR-Inertial Odometry

與松耦合方法不同，緊耦合LiDAR-慣性測距方法通常將LiDAR的原始特征點（而不是掃描配準結果）與IMU數據融合。緊密耦合LIO有兩種主要方法：基于優化和基于過濾器。日內瓦等人[14]使用具有IMU預積分約束[15]和平面約束的圖形優化。最近，Ye et al.[17]提出了LIOM包，它使用類似的圖形優化，但基于邊緣和平面特征。對于基于濾波器的方法，Bry [18]使用高斯粒子濾波器（GPF）來融合IMU和平面2D LiDAR的數據。這種方法也被用于Boston Dynamics Atlas人形機器人。由于粒子濾波器的計算復雜度隨著特征點的數量和系統維度的增加而快速增長，因此卡爾曼濾波器及其變體通常更受歡迎，例如擴展卡爾曼濾波器[19]，無跡卡爾曼濾波器[20]和迭代卡爾曼濾波器[21]。

我們的方法屬于緊密耦合方法。我們采用類似于[21]的迭代擴展卡爾曼濾波器來減輕線性化誤差。卡爾曼濾波器（及其變體）的時間復雜度為O (m^2)，其中m是測量維度[22]，當處理大量LiDAR測量時，這可能導致非常高的計算負載。樸素的下采樣將減少測量的數量，但以信息丟失為代價。[21]通過提取和擬合類似于[9]的地平面，減少了測量次數。然而，這不適用于地平面不總是存在的空中應用。

緊密耦合方法
采用類似于[21]的迭代擴展卡爾曼濾波器來減輕線性化誤差
卡爾曼濾波器（及其變體）的時間復雜度為O (m^2)，其中m是測量維度

METHODOLOGY

框架概述

本文將使用表I所示的符號。我們的工作流程概述如圖2.

LiDAR輸入被饋送到特征提取模塊中以獲得平面和邊緣特征。然后，提取的特征和IMU測量值被饋送到我們的狀態估計模塊中，以10Hz?50Hz進行狀態估計。然后，估計的姿勢將特征點配準到全局幀，并將它們與迄今為止構建的特征點圖合并。更新后的地圖最終用于在下一步驟中配準另外的新點。

1、LiDAR輸入被送到特征提取模塊中以獲得平面和邊緣特征;

lidar input --> 點積累 --> 特征提取 --> planar and edge?

2、提取的特征和IMU測量值被送到狀態估計模塊中，以10Hz?50Hz進行狀態估計;

3、估計的姿勢將特征點配準到全局幀，并將它們與迄今為止構建的特征點圖合并；

4、更新后的地圖最終用于在下一步驟中配準另外的新點。

System Description