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

作者：

Jun Zhu, Hongyi Li,Tao Zhang, Qing Su

01 ? 狀態(tài)估計(jì)算法簡(jiǎn)介

1.1 卡爾曼濾波（KF）

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

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

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

卡爾曼濾波算法流程

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

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

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

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

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

迭代卡爾曼濾波算法流程

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

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

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

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

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

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

02 ? 多傳感器融合算法

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

目前SOTA的多傳感器融合算法 編譯注：（1）類型：FB-濾波，OB-優(yōu)化，LC-松耦合，TC-緊耦合。（2）回環(huán)方式：FAST和ORB-特征點(diǎn)，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：采用了一種無(wú)結(jié)構(gòu)化的方法來(lái)處理視覺信息，在優(yōu)化過(guò)程中考慮了最近一窗口內(nèi)的所有相機(jī)姿態(tài)以及相關(guān)的特征點(diǎn)觀測(cè)。它允許系統(tǒng)在實(shí)時(shí)運(yùn)行的同時(shí)，利用已知的靜態(tài)特征點(diǎn)實(shí)現(xiàn)對(duì)六自由度機(jī)器人的位姿進(jìn)行高效估計(jì)，而不必將所有的歷史特征點(diǎn)都包含在濾波器的狀態(tài)向量中，從而避免了計(jì)算復(fù)雜度隨著特征點(diǎn)數(shù)量增加而呈二次增長(zhǎng)的問題。

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

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

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

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

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

●?松耦合方法

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

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

●?緊耦合方法

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

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

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

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

●?松耦合算法

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

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

●?緊耦合算法

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

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

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

●?松耦合算法

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

●?緊耦合算法

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

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

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

03 ? 未來(lái)研究方向

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

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

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

審核編輯：黃飛

?