本文剖析了卡爾曼濾波系列及滑動窗口優化方法，并系統回顧了視覺-慣性、LiDAR-慣性、視覺-LiDAR與LiDAR-視覺-慣性，四類SLAM融合方案及開源算法。

作者：

Jun Zhu, Hongyi Li,Tao Zhang, Qing Su

01 ? 狀態估計算法簡介

1.1 卡爾曼濾波（KF）

SLAM中，先驗值通常從一系列傳感器獲得，比如慣性測量單元（IMU）和編碼器，而觀測值則是通過GPS、相機和激光雷達等其他傳感器獲取的，后驗值是融合了先驗信息與觀測數據之后得到的結果，它代表了根據所有已知信息計算出的最優定位輸出。可以表達為：

其中，表示第k時刻機器人的狀態向量，即位置、姿態等信息，是初始狀態向量，代表從第一次到第k次的輸入向量，是從初始狀態到第k時刻的所有觀測向量。

卡爾曼濾波器（KF）在解決機器人狀態估計問題上是一種常用的方法，它被公認為貝葉斯濾波研究技術中的佼佼者，但其僅適用于線性高斯系統。

卡爾曼濾波算法流程

●?擴展卡爾曼濾波（EKF）

EKF是卡爾曼濾波器的一個重要變體，特別適用于處理非線性系統。

擴展卡爾曼濾波算法流程

●?迭代卡爾曼濾波（IEKF）

在處理非線性問題時，IEKF通過多次迭代改進線性化點以減小誤差。它反復計算并更新卡爾曼增益及狀態估計，直到達到收斂或變化極小為止，雖增加計算量，但能有效彌補了標準EKF方法的不足。

迭代卡爾曼濾波算法流程

●?誤差狀態卡爾曼濾波（ESKF）

ESKF是一種針對非線性問題優化設計的狀態估計算法。與標準的EKF相比，ESKF通過將真實狀態和名義狀態分離，將誤差作為狀態變量進行估計，從而簡化了非線性系統的處理。

誤差狀態卡爾曼濾波算法流程

1.2 滑動窗口優化

滑動窗口優化就是優化滑動窗口內的所有狀態量，對于一個有個狀態量的窗口，其最優解可以通過優化以下殘差來獲得：

其中是由IMU與積分得來的殘差項，是視覺或者雷達的殘差項，和是對應的方差，是IMU的量測集，是視覺或者雷達的量測集。是由上一個滑窗邊緣化后留下的先驗殘差。

02 ? 多傳感器融合算法

多傳感器融合可分為四個類別：視覺+IMU，LiDAR+IMU，視覺+LiDAR和視覺+LiDAR+IMU，每個分類中SOTA的算法如下表。

目前SOTA的多傳感器融合算法 編譯注：（1）類型：FB-濾波，OB-優化，LC-松耦合，TC-緊耦合。（2）回環方式：FAST和ORB-特征點，DBoW-詞袋模型。(3)傳感器類型：MC-單目，ML-機械雷達，SL-固態雷達，RC-RGB-D相機。（4）融合策略：FGO-因子圖優化，BA-光束調整法，SWO-滑窗優化，PGO-姿態圖有優化。

2.1 視覺和IMU的融合

●?基于濾波的方法

MSCKF：采用了一種無結構化的方法來處理視覺信息，在優化過程中考慮了最近一窗口內的所有相機姿態以及相關的特征點觀測。它允許系統在實時運行的同時，利用已知的靜態特征點實現對六自由度機器人的位姿進行高效估計，而不必將所有的歷史特征點都包含在濾波器的狀態向量中，從而避免了計算復雜度隨著特征點數量增加而呈二次增長的問題。

MSCKF 2.0：由于在線性化測量模型時使用的是不斷更新的估計值，這會導致線性化系統的可觀測性質與實際非線性系統不匹配。改進版的MSCKF 2.0算法，在計算雅可比矩陣時使用首次可用的每種狀態估計值，以確保系統的適當可觀測性。

●?基于優化的方法

OKVIS：利用了IMU預積分技術將IMU測量整合到相對運動約束中，以此避免因狀態估計不斷變化而重復計算IMU傳播的繁瑣過程。此外，該系統引入了關鍵幀的概念，僅對最近的關鍵幀進行詳細的特征點跟蹤和優化，從而提高了系統的實時性和效率。

VINS-mono：在單目視覺場景中，初始化是顯著挑戰，因為需要加速度激勵才能觀測到尺度信息，意味著單目VINS無法從靜止初始狀態直接啟動。此外，還需要考慮攝像頭-IMU外參數校準以及數據處理等問題。這些問題在VINS-mono算法中得到了有效解決，該算法包括五個主要部分：預處理、初始化、基于非線性優化的VIO、環路閉合以及全局位姿圖優化。

2.2 雷達與IMU的融合

●?松耦合方法

LOAM：LOAM是一個經典的三維LiDAR SLAM方法，其結構包含特征提取、里程計計算和地圖構建三個主要模塊。在LOAM中，通過比較當前掃描周期內提取的邊緣點和平面點與前一掃描周期中的對應關系來更新姿態變換。然而，在高速運動場景下，LOAM的準確性會顯著下降，此時可以通過整合IMU測量數據來改善，IMU提供的高頻率運動信息有助于補償動態環境下的運動失真，從而大大提高系統的準確性和魯棒性。

LION：LION算法設計了用于處理低計算負荷的輕量化結構，特別關注在無法使用視覺傳感器時如何依賴LiDAR和IMU融合實現準確且魯棒的定位。通過結合LiDAR觀測到的點云特征以及IMU提供的連續高頻測量信息，尤其是在快速運動情況下，能夠有效補償因單純依靠LiDAR帶來的運動畸變問題，從而提升整體導航系統的精度和穩定性。

●?緊耦合方法

LIOM：LIOM借鑒了視覺-慣性融合技術中的思想，它采用滑動窗口的方式處理連續的LiDAR掃描幀，并對每個幀的姿態以及后續姿態進行聯合優化。為了處理動態環境下的快速運動問題，LIOM引入了IMU預積分技術，即預先整合IMU測量數據以校正由于快速運動導致的點云失真，從而提升定位精度和穩定性。

LINS：LINS采用了迭代擴展卡爾曼濾波器的形式來進一步優化估計過程。這種迭代形式使得系統能夠在每次迭代中更接近真實狀態，減少線性化帶來的誤差。同時，LINS還考慮了機器人中心坐標系下算法的表述方式，以適應不同應用場景的需求。

此外還有LIO-SAM，LILI-OM，FAST-LIO系列算法。

2.3 雷達與視覺融合

●?松耦合算法

DEMO：DEMO首先通過估計出的相機姿態將LiDAR點云轉換到與相機坐標系相對應的空間中，并生成一個深度圖。在該過程中，相機前方點云中新產生的點被添加到地圖構建中。為了高效處理和匹配這些點，地圖中的點采用了球面坐標系統表示，并存儲在一個基于兩個角度坐標的2D k-d樹結構中。對于每個從圖像中提取的特征點，可以通過投影到由k-d樹中該特征點最近鄰三點構成的平面片上，獲得相應的深度值。

LIMO：LIMO算法結合了單目相機和激光雷達（LiDAR）的數據，采用深度學習來識別并剔除環境中移動物體的特征，這樣可以避免這些不可靠的特征對估計機器人自身運動狀態的影響。該方法能夠有效改善SLAM（同時定位與建圖）過程中因誤匹配動態對象而導致的軌跡漂移問題，從而提高視覺和LiDAR數據融合后的定位準確度。

●?緊耦合算法

V-LOAM：利用了相機采集頻率遠高于LiDAR的特點，通過增強后的視覺里程計獲取可觀察尺度信息來校正LiDAR點云的畸變。同時，針對視覺里程計在一次掃描內線性運動產生的漂移進行了建模，改進了去畸變過程的性能。接著，對校正后的點云進行匹配并注冊到當前構建的地圖中，進一步優化估計出的機器人姿態。

TVL-SLAM：TVL-SLAM利用高精度的時間戳同步保證了視覺幀和LiDAR掃描在同一時刻對應，進而實現基于因子圖優化的緊耦合狀態估計。這樣可以有效消除由于傳感器獨立工作而帶來的誤差累積，并且通過同時考慮視覺特征和LiDAR幾何特征，提升了對動態物體、光照變化以及無紋理區域的適應能力。

2.4 雷達，視覺與IMU融合

●?松耦合算法

VIL-SLAM：利用立體相機作為視覺傳感器，在某些退化場景（例如長隧道）中表現優異，能夠彌補純LiDAR系統在此類環境中容易失效的問題。算法采用緊耦合的方式整合立體匹配和IMU測量數據，通過固定滯后平滑濾波技術，輸出了基于IMU速率和相機速率的VIO位姿估計，這些估計結果用于去除運動畸變并執行LiDAR點云到地圖的配準操作。

●?緊耦合算法

LIC-Fusion 2.0：首先利用IMU測量預測運動狀態，然后通過視覺-慣性里程計進一步細化估計，最后使用LiDAR掃描到地圖匹配來微調估計結果。還引入了一種新穎的平面特征跟蹤算法至LIC-Fusion 2.0版本中，該算法從經由IMU測量去畸變后的LiDAR點云中提取平面點，并在一個滑動窗口中采用包含異常點剔除準則的數據關聯策略，從而高效且魯棒地處理LiDAR數據。

Super odometry：采用了以IMU為中心的數據處理管道，其包括三個主要組成部分：IMU里程計、視覺-慣性里程計以及LiDAR-慣性里程計。IMU偏差受到視覺-慣性和LiDAR-慣性里程計提供的位姿先驗約束，同時，LiDAR-慣性里程計接收來自IMU里程計的運動預測。為實現實時高效率運行，該系統應用了動態八叉樹結構。這種設計背后的洞察是，只要其他傳感器能很好地約束IMU的偏置漂移，那么基于IMU的估計就能保持相當高的準確性，因為IMU產生的數據雖然帶有噪聲但很少出現離群值。

此外還有Super odometry，LVI-SAM和R3LIVE等算法。

03 ? 未來研究方向

●?通用高效傳感器融合策略：當前先進的融合框架算法通常針對特定平臺設計，這導致它們難以在具有相似傳感器的其他平臺上部署。為了實現通用性和高效性，亟需研發一種靈活多變且高效的融合框架，使其能夠適應不同平臺的需求和配置。

● 深度學習在SLAM中的應用：利用深度學習進行特征提取、噪聲抑制、動態物體識別和位姿估計等方面的改進。

● 分布式協作方式：多個機器人配備不同類型的傳感器來共同執行相同的SLAM（同時定位與建圖）任務，這樣可以顯著減輕單個機器人的工作負擔。

審核編輯：黃飛

?