用單臺Livox Horizon (含內置IMU) 實現高魯棒性的激光-慣性里程計，可在各類極端場景下魯棒運行，并達到高精度的定位和建圖效果。(城區擁堵、高速公路、幽暗隧道)

注：該系統主要面向大型室外環境中的汽車平臺設計。用戶可以使用 Livox Horizon 或 HAP LiDAR 輕松運行該系統。在室內環境中系統同樣表現穩定，我們目前也支持面向機器人設計的 Mid-360 雷達。

1.系統框架

該系統由兩個 ROS 節點組成：ScanRegistration 和 PoseEstimation。

ScanRegistration 節點中的類 “LidarFeatureExtractor” 從原始點云中提取角點特征、平面特征和不規則特征。

PoseEstimation 節點中，主線程用于估計傳感器位姿，而類 “Estimator” 中的另一個線程則利用 “MapManager” 類構建和管理特征地圖。

2.?配置文件

在啟動文件中包含以下參數：

IMU_Mode：用于選擇 IMU 信息融合策略，有三種模式可選：

• 0：不使用 IMU 信息，純激光里程計，使用勻速模型進行運動畸變補償；

• 1：使用 IMU 預積分進行運動畸變補償；

• 2：將 IMU 與激光信息進行緊耦合融合。

Extrinsic_Tlb：激光雷達與 IMU 之間的外參，使用 SE3 形式表示。如果使用的是外部 IMU，則需要自行完成傳感器標定，并將該參數設置為您系統中的外參值。

Used_Line：HAP和Horizon等效為6線，MID360等效為4線。

Use_seg：選擇動態物體濾除的分割方式，有兩種模式可選：

• 0：不使用分割方法，如果數據中幾乎沒有動態物體，可以選擇該模式；

• 1：使用分割方法來剔除動態物體。

3. ScanRegistration

系統從節點 “ScanRegistration” 開始運行，該節點負責提取特征點。在特征提取之前，會先從原始點云中剔除動態物體，因為在城市場景中通常存在大量動態物體，這些動態物體會影響系統的魯棒性和精度。為了過濾動態物體，采用了一種快速的點云分割方法。首先應用歐幾里得聚類將點云劃分為多個聚類，然后將原始點云分為地面點、背景點和前景點。前景點被視為動態物體，從特征提取過程中排除。通過動態物體濾除模塊，系統在動態場景中獲得了較高的魯棒性。

使用歐式聚類方法將原始點云聚類為地面點（藍色），背景點（綠色），和前景點（黃色），如下圖所示。其中前景點(黃色)被視為動態物體，并在特征提取過程中去除。去除之后，系統在動態場景中可保持很高的魯棒性，即便激光雷達的大部分視場角被動態物體遮擋，也可以準確地定位和建圖。

注：具體實現在函數AbvGndSeg()的標記背景點SegBG()和SegObjects()中，這部分代碼沒有使用PCL提供的相關聚類方法（在DoSeg()函數最開始的降采樣也是自己實現的，這部分內容以后可以借鑒）。還有一個是每個點地面在KD樹找到相鄰點后，對鄰近點進行PCA分析，計算這些點的特征值和特征向量，求平均得到地面的法向量 CalGndPos_cor()。

在開闊場景中，通常只能提取較少的特征點，容易導致某些自由度上的退化。為了解決這個問題，設計了一套特征提取流程，使得特征點在空間中分布更廣泛且更加均勻。分布廣泛且均勻的特征點可以在六個自由度上提供更多約束，有助于消除退化問題。此外，在特征稀疏的場景中，一些不規則點也能提供有用的信息，因此將不規則點也作為一個類別提取，用于點云配準。

根據局部幾何屬性，特征點被分為三類：角點特征、平面特征和不規則特征。我們首先在每條掃描線上提取曲率較大且孤立的點作為角點特征，然后使用主成分分析（PCA）對其余點進行分類，劃分為平面特征和不規則特征，如下圖所示。針對不同距離的點，設置不同的閾值，以盡可能保證特征點在空間中的分布均勻。

4.?PoseEstimation

在 “PoseEstimation” 節點中，系統首先通過 IMU 預積分或勻速模型對點云的運動畸變進行補償。隨后進入 IMU 初始化模塊。如果初始化成功，系統將切換至 LIO（激光慣性里程計）模式；否則，將以 LO（激光里程計）模式運行，并進行 IMU 狀態初始化。

在 LO 模式下，系統采用幀到模型（frame-to-model）的點云配準方法來估計傳感器位姿。受 ORB-SLAM3 啟發，系統引入了一種最大后驗（MAP）估計方法，用于聯合初始化 IMU 的偏置、速度和重力方向。該方法不依賴繁瑣的初始化流程，支持任意運動軌跡下的初始化，同時考慮了傳感器的不確定性，從而在最大后驗概率意義下獲得最優解，具備高效、魯棒和高精度的特點。

完成初始化后，系統將運行一個基于滑動窗口的緊耦合傳感器融合模塊，估計滑動窗口內的 IMU 位姿、偏置和速度。同時，另一個線程并行地構建和維護全局地圖。

參考：覽沃 LIO-Livox 算法開源 一個魯棒的激光-慣性里程計