1.FAST-LIO

FAST-LIO?是一個計算效率高、魯棒性強的激光-慣性里程計系統。該系統通過緊耦合的迭代擴展卡爾曼濾波器（IEKF）將激光雷達特征點與IMU數據進行融合，使其在快速運動、噪聲較大或環境復雜、存在退化的情況下仍能實現穩定的導航。

1.1問題

1）LiDAR測量中提取的特征點通常是環境中的幾何結構（如邊緣和平面）。當無人機在特征不明顯的復雜環境中運行時，LiDAR方案容易出現退化問題。該問題在視場角（FoV）較小的LiDAR中尤為明顯； （特征稀缺的環境中或使用小FoV的LiDAR時，性能容易退化）

2）由于在掃描方向上具有高分辨率，一幀LiDAR掃描通常包含大量特征點（例如幾千個）。盡管這些特征點在退化場景中無法穩定估計位姿，但若將其與IMU測量進行緊耦合融合，將導致巨大的計算負擔，而這對于無人機的機載計算機來說是不可承受的；（嵌入式平臺對算力有要求）

3）由于LiDAR使用少數激光發射/接收對逐點掃描，因此每個激光點的采樣時間不同，這種時間差造成的運動畸變會顯著降低點云配準的準確性。此外，無人機螺旋槳和電機的持續旋轉還會對IMU測量引入顯著噪聲。（點云去畸變以及外部因素影響IMU測量）

1.2解決方法

為使LiDAR導航適用于小型移動機器人（如無人機），提出了FAST-LIO——一個計算高效且魯棒的LiDAR-IMU里程計算法。主要貢獻如下：

1）為應對高速運動、有噪聲或復雜環境中發生退化的情況，我們采用緊耦合的迭代卡爾曼濾波器，將LiDAR特征點與IMU測量數據融合。我們提出了正式的后向傳播方法來補償運動畸變；

2）為減少大量LiDAR特征點帶來的計算負擔，我們提出了一種新的卡爾曼增益計算公式，并證明其與傳統公式等價。新公式的計算復雜度取決于狀態維度，而不是測量維度；

3）我們將這些公式實現為一個快速且魯棒的LiDAR-IMU里程計算軟件包，系統可以運行在小型四旋翼的機載計算機上；

4）我們在多種室內和室外環境下，以及實際無人機飛行測試中，驗證了系統在快速運動或強烈振動噪聲條件下的魯棒性。

1.3系統框架

• LiDAR原始數據首先輸入到特征提取模塊，提取出平面特征和邊緣特征。
• 提取出的特征與IMU測量數據一同輸入到狀態估計模塊中，以10Hz或50Hz的頻率進行狀態估計。IMU數據前向傳播（粗略地位姿估計），后向傳播（運動補償去畸變）
• 估計得到的位姿用于將當前幀的特征點配準到全局坐標系中，并與當前已構建的特征點地圖進行融合。更新后的地圖將用于下一步中新點云的配準操作。

1.4系統描述? ? ? ??

關于流行的解釋

流形(Manifold?)是局部具有歐幾里得空間性質的空間，是歐幾里得空間中的曲線、曲面等概念的推廣。參考文章中關于二維圓和三維球的例子解釋的很好，二維空間中的圓就是一個一維流形，三維空間中的球面是一個二維流形。

參考：https://www.cnblogs.com/icmzn/p/11082509.html

IMU運動模型

本質上是對P、V、Q的求導

離散模型

需要估計的狀態量是個18維的量，包括位置、速度、姿態、偏置和加速度g?.

數據預處理

由于原始LiDAR點是以非常高的頻率（例如200kHz）采樣的，因此通常無法在每接收到一個新點時立即處理它們。更實際的方法是將這些點積累一段時間，然后一次性處理它們。

在FAST-LIO中，最小的積累間隔設置為20毫秒，從而實現高達50Hz的全狀態估計（即里程計輸出）和地圖更新。這種積累后的點集被稱為一個掃描（scan），并且處理該掃描的時間為 tk。從原始點云中，我們提取具有較高局部平滑性的平面點和具有較低局部平滑性的邊緣點。

1.5狀態估計

使用迭代擴展卡爾曼濾波估計狀態量X

前向傳播?Forward Propagation

在 FAST-LIO 系統中，一旦接收到 IMU 輸入，就會執行一次前向傳播，用于預測系統當前的狀態（積分得到粗略地位姿估計）。

后向傳播與運動補償?Backward Propagation and Motion Compensation

激光雷達一幀數據由很多點組成，這些點顯然不是同一時間測量得到的，所以需要補償時間差帶來的運動誤差。把一幀不同采樣時間的點轉換到同一時刻（反向傳播去運動畸變）

殘差計算和迭代狀態更新

計算lidar里程計的殘差，和LOAM、LIO-SAM一樣，計算點線、點面之間的距離作為殘差。

利用迭代卡爾曼濾波估計位姿。

算法流程

1.6地圖更新?MapUpdate

根據求解的狀態量 X 把激光雷達點投影到世界坐標系的過程.

1.7初始化

為了獲得系統狀態（例如重力向量 Gg、偏差和噪聲協方差）的良好初始估計，從而加速狀態估計，需要進行初始化。在 FAST-LIO 中，初始化非常簡單：保持 LiDAR 靜止幾秒鐘（本文中的所有實驗為 2 秒），然后將收集的數據用于初始化 IMU 偏差和重力向量。如果 LiDAR 支持非重復掃描（例如，Livox AVIA），保持靜止還允許 LiDAR 捕捉一個初始的高分辨率地圖，這對后續的導航非常有利。

?

2.代碼相關

注：FAST-LIO 的代碼是6e1fa94之前的commit的，之后的都是FAST-LIO2

有大佬對原始的FAST-LIO進行修改，S-FAST_LIO

• 利用Sophus庫定義狀態變量，替代了FAST-LIO中復雜的IKFOM
• 重力分量直接利用Vector3d定義，因此可以省去2維流形的計算
• 優化了代碼結構，刪除了冗余的代碼
• 代碼中添加了詳細的中文注釋
• 添加了對于 Robosense LiDAR 的支持

而且新增了重定位功能

roslaunch sfast_lio mapping_rs_relocalization.launch
rosbag play YOUR_DOWNLOADED.bag