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

作者：

Jun Zhu, Hongyi Li,Tao Zhang, Qing Su

01 ? 狀態(tài)估計算法簡介

1.1 卡爾曼濾波（KF）

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

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

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

卡爾曼濾波算法流程

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

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

擴展卡爾曼濾波算法流程

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

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

迭代卡爾曼濾波算法流程

●?誤差狀態(tài)卡爾曼濾波（ESKF）

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

誤差狀態(tài)卡爾曼濾波算法流程

1.2 滑動窗口優(yōu)化

滑動窗口優(yōu)化就是優(yōu)化滑動窗口內(nèi)的所有狀態(tài)量，對于一個有個狀態(tài)量的窗口，其最優(yōu)解可以通過優(yōu)化以下殘差來獲得：

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

02 ? 多傳感器融合算法

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

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

2.1 視覺和IMU的融合

●?基于濾波的方法

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

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

●?基于優(yōu)化的方法

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

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

2.2 雷達與IMU的融合

●?松耦合方法

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

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

●?緊耦合方法

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

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

此外還有LIO-SAM，LILI-OM，F(xiàn)AST-LIO系列算法。

2.3 雷達與視覺融合

●?松耦合算法

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

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

●?緊耦合算法

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

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

2.4 雷達，視覺與IMU融合

●?松耦合算法

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

●?緊耦合算法

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

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

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

03 ? 未來研究方向

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

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

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

審核編輯：黃飛

?