深度解析：多傳感器融合SLAM技術(shù)全景剖析

本文剖析了卡爾曼濾波系列及滑動窗口優(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、相機(jī)和激光雷達(dá)等其他傳感器獲取的，后驗值是融合了先驗信息與觀測數(shù)據(jù)之后得到的結(jié)果，它代表了根據(jù)所有已知信息計算出的最優(yōu)定位輸出。可以表達(dá)為：

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

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

卡爾曼濾波算法流程

●?擴(kuò)展卡爾曼濾波（EKF）

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

擴(kuò)展卡爾曼濾波算法流程

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

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

迭代卡爾曼濾波算法流程

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

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

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

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

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

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

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-機(jī)械雷達(dá)，SL-固態(tài)雷達(dá)，RC-RGB-D相機(jī)。（4）融合策略：FGO-因子圖優(yōu)化，BA-光束調(diào)整法，SWO-滑窗優(yōu)化，PGO-姿態(tài)圖有優(yōu)化。

2.1 視覺和IMU的融合

●?基于濾波的方法

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

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

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

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

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

2.2 雷達(dá)與IMU的融合

●?松耦合方法

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

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

●?緊耦合方法

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

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

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

2.3 雷達(dá)與視覺融合

●?松耦合算法

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

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

●?緊耦合算法

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

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

2.4 雷達(dá)，視覺與IMU融合

●?松耦合算法

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

●?緊耦合算法

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

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

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

03 ? 未來研究方向

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

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

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

審核編輯：黃飛

?