激光雷達(dá)在SLAM算法中的應(yīng)用綜述

一、文章概述

1.1 摘 要

即時定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）是自主移動機器人和自動駕駛的關(guān)鍵 技術(shù)之一，而激光雷達(dá)則是支撐SLAM算法運行的重要傳感器?；诩す饫走_(dá)的SLAM算法，對激光雷達(dá)SLAM總體框架進(jìn)行介紹，詳細(xì)闡述前端里程計、后端優(yōu)化、回環(huán)檢測、地圖構(gòu)建模塊的作用并總結(jié)所使用的算法；按由2D到 3D，單傳感器到多傳感器融合的順序，對經(jīng)典的具有代表性的開源算法進(jìn)行描述和梳理歸納；介紹常用的開源數(shù)據(jù)集，以及精度評價指標(biāo)和測評工具；從深度學(xué)習(xí)、多傳感器融合、多機協(xié)同和魯棒性研究四個維度對激光雷達(dá)SLAM 技術(shù)的發(fā)展趨勢進(jìn)行展望。

1.2問題導(dǎo)向

SLAM最早是由Smith和Cheeseman于1986提出，發(fā)展至今已有30多年。激光雷達(dá)可 以直接測量距離，對環(huán)境的感知更加準(zhǔn)確，可以獲取物 體的空間位置和形狀信息，構(gòu)建高精地圖進(jìn)行精確定 位，對長時間運行的SLAM系統(tǒng)也更加可靠和穩(wěn)定。激 光SLAM被廣泛應(yīng)用于室內(nèi)導(dǎo)航、三維重建和自動駕駛。本文將系統(tǒng)地對激光SLAM進(jìn)行綜述，對相關(guān)的激光SLAM算法進(jìn) 行分析總結(jié)。

二、SLAM系統(tǒng)框架

傳感器數(shù)據(jù)采集處理、前端里程計、后端優(yōu)化、回環(huán)檢測和地圖構(gòu)建。

2.1前端里程計

2.1.1點云去畸變

激光SLAM中的點云畸變是指點云的運動畸變：激光雷達(dá)在掃描過程中，載體機器人是在不斷運動的，這就會導(dǎo)致同一幀中的點云數(shù)據(jù)，是由在不同位置下的激光雷達(dá)坐標(biāo)系測量得到的。常見的點云運動畸變?nèi)コ椒ㄓ屑児烙嫹ê蛡鞲衅鬏o助法。

純估計法
VICP（velocity updating ICP，VICP）[9]是ICP算法的變種，考慮了激光的運動畸變。VICP算法假設(shè)在 一幀激光雷達(dá)數(shù)據(jù)中機器人是勻速運動，對ICP估計的 位姿采用線性插值法對點云作運動補償，將補償后的數(shù) 據(jù)再次放入 ICP中求解，形成一個迭代閉環(huán)求解最優(yōu)值。

傳感器輔助法
傳感器輔助法主要是利用高頻率傳感器如慣性測量單元（inertial measurement unit，IMU）極高測量頻率的特點，直接測量角速度和線速度，得出每個時刻的位 姿，對點云作運動補償

2.1.2點云配準(zhǔn)

ICP
1.PP-ICP （point-to-plane ICP）原來計算點與點之間的距離改成計算的是點到面之間的距離，相對于 ICP的一階收斂速度，PP-ICP是二階的，收斂速度更快。

2.PL-ICP（point-to-line ICP）與PP-ICP類似，通過求取點線之間的最小距離使算法快速 收斂，提高匹配精度

3.NICP（normal ICP）將點云表面法向量信息加入到了 ICP 算法中，用于提高配準(zhǔn)的準(zhǔn)確度和魯棒性

4.IMLS-ICP （implicit moving least square ICP）采用隱式函數(shù)來表示點云表面，并且通過最小化MLS逼近和隱式函數(shù)之間的距離來進(jìn)行點云配準(zhǔn)，能有效處理非剛性物體的配準(zhǔn)問題

5.廣義迭代最近點法（generalized ICP，GICP）引入一個全局參考框架來增強ICP算法的全局優(yōu)化能力，解決了ICP算法運行緩慢、容易陷入局部最優(yōu)解等問題

6.VGICP（voxelized GICP）是一種擴展的廣義迭代最近點法，通過將點云轉(zhuǎn)換為一組小體素網(wǎng)格，提出一種多點分布聚合方法估計體素分布，使匹配更加高效。

基于數(shù)學(xué)特征NDT

P2D-NDT（point-todistribution NDT）：NDT算法從2D推廣到3D場景

基于極大團的配 準(zhǔn)假設(shè)生成方法MAC：實現(xiàn)了 無需樣本、無需訓(xùn)練、同時兼顧精度和效率的三維配準(zhǔn)重建效果。

基于學(xué)習(xí)的方法

基于深度學(xué)習(xí)的點云配準(zhǔn)方法通過數(shù)據(jù)驅(qū)動方式學(xué)習(xí)，能獲得比人工設(shè)計更精準(zhǔn)的模型，但其模型的前 期訓(xùn)練和模型部署需要消耗大量的GPU資源，暫時無 法實現(xiàn)在CPU上的實時計算運行。

1.FCGF提出一種全卷積幾何特征，擺脫了先前SOTA方案需要低級特征輸入的限制，但在對具有不同旋轉(zhuǎn)分布的點云配準(zhǔn)時效果不好；

2.SpinNet：由一個空間點轉(zhuǎn)換器和一個基于3D卷積神經(jīng)網(wǎng)絡(luò)的特征提取器組成,能夠提取點云表面特征,具有旋轉(zhuǎn)不變性

3.Lepard：算法引入Transformer注意力機制，通過結(jié)合 RANSAC和ICP完成點云配準(zhǔn)

4.REGTR：使用注意力機制取代顯性特征匹配和RANSAC，提出一個包含自注意和交叉注意的 transformer網(wǎng)絡(luò)架構(gòu)

2.2 后端優(yōu)化

2.2.1基于濾波

相比于傳統(tǒng)的EKF，研究學(xué)者們往往更傾向于使用誤差狀態(tài)卡爾曼濾波器ESKF，相比于其他卡爾曼濾波算法，ESKF估計的是狀態(tài)的誤差量，維度較小且總是在 原點附近，離奇異點較遠(yuǎn)，不會由于離工作點太遠(yuǎn)而導(dǎo)致線性化近似不夠的問題，在現(xiàn)有的開源算法中已展現(xiàn)其優(yōu)秀的性能。

濾波算法缺點是無法校正累計誤差，在噪聲大、回環(huán)多的場景下長時間運行會產(chǎn)生嚴(yán)重的誤差漂移。

2.2.1基于圖優(yōu)化

圖優(yōu)化（graph optimization）利用圖論的方法對系統(tǒng)進(jìn)行建模求解。因子圖優(yōu)化（factor graph optimization）在圖優(yōu)化的基礎(chǔ)上將圖劃分為變量節(jié)點和因子節(jié)點。因子圖用變量節(jié)點表示問題的狀態(tài)變量，用 因子節(jié)點連接它們并表示約束條件，利用迭代最小化算 法，在因子圖上反復(fù)更新變量節(jié)點和因子節(jié)點的估計值，直至達(dá)到收斂狀態(tài)。源非線性優(yōu)化算法庫有

Ceres-Solver
G2O
iSAM
GTSAM
SE-Sync
SE

2.3 回環(huán)檢測

回環(huán)檢測模塊用于消除累計誤差，提高定位和地圖精度，對在大環(huán)境、回環(huán)多、長時間下運行的SLAM系統(tǒng)至關(guān)重要。

Scan Context++
Scan Context
FPFH

2.4 地圖構(gòu)建

2D地圖用柵格
3D地圖形式如下

三、激光SLAM方案

3.1 2D激光SLAM

Fast-SLAM：采用粒子濾波技術(shù)， 可以處理非線性系統(tǒng)。大環(huán)境下或者在里程計誤差較大時需要更多的粒子才能得到較好的估計，這可能會造成內(nèi)存 爆炸，而粒子的重采樣會導(dǎo)致粒子耗散。

GMapping：通過降低粒子數(shù)量的 方法大幅度緩解內(nèi)存爆炸；采用選擇性重采樣方法，對粒子進(jìn)行重要性權(quán)重排序，對權(quán)值低的粒子進(jìn)行重采樣，解決RBPF算法中粒子耗散問題。基于濾波的算法且無 回環(huán)檢測，長時間運行存在較大累計誤差，無法在室外 構(gòu)建大環(huán)境地圖。

Karto：第一個基于圖優(yōu)化的開源SLAM算法

Hector-SLAM：創(chuàng)新性地采用高斯牛頓法直接進(jìn)行幀-圖匹配（scan-to-map）優(yōu)化，構(gòu)建柵格地圖。缺少閉環(huán)檢測，沒有修正能力

Cartographer：是目前精度最高、實時性、魯棒性最好、二次開發(fā)最為便捷的開源SLAM算法之一。引入子圖（submap）概念，使用 CSM和梯度優(yōu)化相結(jié)合的方法使幀與子圖進(jìn)行匹配。后端基于圖優(yōu)化算法，將當(dāng)前激 光幀和之前建立的所有子圖都加入閉環(huán)檢測模塊，采用分支定界法提升搜索速度。

## 3.2 3D激光SLAM

LOAM：根據(jù)曲率大小將特征點分類為邊緣特征點和平面特征點，缺少后端優(yōu)化和回環(huán)檢 測模塊。

A-LOAM：使用Eigen 矩陣庫（https：//eigen.tuxfamily.org）和Ceres-Solver非線性優(yōu)化 庫代替原本復(fù)雜的數(shù)學(xué)推導(dǎo)

LeGO-LOAM：引入關(guān)鍵幀概念，使用關(guān)鍵幀及其局部范圍內(nèi)的數(shù)據(jù)幀組成 loop-submap?；诘孛纥c的優(yōu)化更加魯棒精確，計算量更小

Loam_livox：針對Livox新型固態(tài)激光雷達(dá)掃描特性，對有效點篩選和特征提取部分進(jìn)行優(yōu)化

NASA提出一種基于稠密點云快速定位的直接激光里程計將關(guān)鍵幀和相關(guān)聯(lián)點云儲存在字典中，節(jié)約幀圖匹配的計算資源和降低基于關(guān)鍵 幀構(gòu)成的子圖所含有的重復(fù)信息

CP-ICP：是一種彈性的帶回環(huán)的激光雷達(dá)里程計法，考慮了幀內(nèi)數(shù)據(jù)的連續(xù)性和幀間數(shù)據(jù)的非連續(xù)性。

Intensity-SLAM：利用強度信息構(gòu)建強度地圖，協(xié)助特征點提取和位姿估計

提出一種基于強度信息的純激光雷達(dá) SLAM算法，算法的關(guān)鍵在于利用點云的強度信息生成一張強度圖像，后續(xù)的特征提取和配準(zhǔn)、回環(huán)檢測與位 姿圖估計都是基于強度圖像進(jìn)行的

3.3 多傳感器融合SLAM

相較于純激光雷達(dá)方案，使用激光雷達(dá)與IMU進(jìn)行信息融合，利用IMU高頻率輸出運動信息，校正點云運動畸變，提供一個良好的初值可以使算法避免陷入局部最小值，提高算法精度和減少計算量。

激光雷達(dá)與IMU融合可以分為緊耦合和松耦合

松耦合存在兩個優(yōu)化過程，它將激光雷達(dá)數(shù)據(jù)和IMU數(shù)據(jù)分別進(jìn)行參數(shù)估計，再使用估計出的參數(shù)進(jìn)行融合

緊耦合表現(xiàn)出更好的魯棒性和準(zhǔn)確性，它使用激光雷達(dá)數(shù) 據(jù)和 IMU數(shù)據(jù)共同構(gòu)建參數(shù)向量，再進(jìn)行優(yōu)化和估計。

LIO-SAM：提取特征點，并使用IMU數(shù)據(jù) 校正點云畸變，提供數(shù)據(jù)幀之間位姿變換的初始值；后端采用因子圖優(yōu)化架構(gòu)，消除累計誤差，進(jìn)行全局優(yōu)化。

LVI-SAM：由激光-慣性系統(tǒng)和視覺-慣性系統(tǒng)組成。LIS為VIS提供準(zhǔn)確的深度信息，提 高VIS精度；反過來LIS利用VIS的初步位姿估計進(jìn)行掃描匹配。

R2LIVE：使用誤差狀態(tài)迭代卡爾曼濾波融合三個傳感器的測量值進(jìn)行狀態(tài)估計，并通過因子圖進(jìn)一步優(yōu)化提高整體精度，但其視覺系統(tǒng)采用特征點法，時間開銷大且在非結(jié)構(gòu)化環(huán)境下易失效。

R3LIVE：使用光流法代替原本的特征點法進(jìn)行幀間跟蹤，通過最小化幀到 地圖的光度誤差融合視覺數(shù)據(jù)，并為地圖渲染RGB顏色。

R3LIVE++：在R3LIVE++基礎(chǔ)上，考慮了相機光度校準(zhǔn)和對相機曝光時間的在線估計，進(jìn)一步提高了定位和建圖的精度。

FAST-LIO：使用激光雷達(dá)與 IMU 緊耦合的誤差狀態(tài)迭代卡爾曼濾波算法。提 出一個新的計算卡爾曼增益的公式，使計算量不再依賴于測量維數(shù)，而是依賴于狀態(tài)維數(shù)，極大地減少了計算量。

FAST-LIO2：為了使算法能自然適應(yīng)不同掃描模式的激光雷達(dá)和實現(xiàn)更 快的計算，省略了耗時的特征提 取模塊，采用直接法將所有點云數(shù)據(jù)統(tǒng)一處理。

Faster-LIO：在FAST-LIO2的基礎(chǔ)上使用一種增量式稀疏體素（iVox）數(shù)據(jù)結(jié)構(gòu)代替ikd-tree，可以有效降低點云配準(zhǔn)的時間

FAST-LIVO：在FAST-LIO2的LIO系統(tǒng)基礎(chǔ)上使用相機傳感器加入VIO子系統(tǒng)，在測量層面實現(xiàn)兩個傳感器的耦合，在激光雷達(dá)退化場景和光線變化劇烈的場景下依舊能可靠運行。

Point-LIO：以單個點云為數(shù)據(jù)處理單位，更新每個點的狀態(tài)， 具有極高的里程計輸出頻率和帶寬，在自身運動存在嚴(yán)重振動和高角速度高線速度的情況下也能魯棒運行，從根本上解決了運動失真。

3.4 基于深度學(xué)習(xí)的3D激光SLAM

深度學(xué)習(xí)與激光雷達(dá)SLAM的結(jié)合主要應(yīng)用于系統(tǒng)中的幾個模塊，如點云的特征提取和配準(zhǔn)、回環(huán)檢測、構(gòu)建語義.

特征提?。篎CGF、SpinNet
配準(zhǔn)方法：Lepard、 REGTR
回環(huán)：OverlapNet、Overlap-Transformer

SUMA++：基于激光雷達(dá)的語義 SLAM，使用 RangeNet++[70]對點云進(jìn)行語義分割，根據(jù)語義信息剔除動態(tài)目標(biāo)，再將語義信息融入系統(tǒng)，與幾何信息一起建 立約束，從而提高定位和建圖精度。

SA-LOAM：提出一種語義輔助的 ICP算法，并在閉環(huán)檢測模塊中集成基于語義圖的位置識別方法，適合在大規(guī)模場景中構(gòu)建全局一致性語義地圖。

ASL-SLAM：提出一種基于活動語義的回環(huán)檢測方法，通過IMU傳感器對Z軸角速度和加速度的分析來檢測轉(zhuǎn)彎和通過減速帶兩種語義。

提出一種自監(jiān)督的激光里程計，在投影的強度圖像上使用SuperPoint提取特征點，使用傳統(tǒng)ICP方法估算位姿后用于在線訓(xùn)練網(wǎng)絡(luò)

EfficientLO-Net：是第一個完全端到端的高效3D激光雷達(dá)里程計框架，提出了基于投影感知的三維點云表示 方法和特征學(xué)習(xí)方法

四、數(shù)據(jù)集與評價指標(biāo)

4.1 數(shù)據(jù)集

KITTI：包含視覺圖像、激 光雷達(dá)點云、IMU、GPS等數(shù)據(jù)

SemanticKITTI：在KITTI數(shù)據(jù)集的基礎(chǔ)上對點云數(shù)據(jù)進(jìn)行標(biāo)注，分類為28類語義，實現(xiàn)全場景分割

ApolloScape：數(shù)據(jù)集使用了高精度地圖采集車，是目前行業(yè)內(nèi)環(huán)境最復(fù)雜、標(biāo)注最精準(zhǔn)、數(shù)據(jù)量最大的三維 自動駕駛公開數(shù)據(jù)集

NTU-VIRAL：搭載激光雷達(dá)、相機、IMU和超帶寬填補了SLAM領(lǐng)域無人機數(shù)據(jù)集的空白

TUM-RGBD

EuRoC

Oxford RobotCar

USVInland

4.2評價指標(biāo)

（1）絕對軌跡誤差（absolute trajectory error，ATE）用于衡量估計的軌跡與真實軌跡之間的誤差，可以非常 直觀地反映算法精度和軌跡全局一致性。首先，將估計 軌跡與真實軌跡進(jìn)行時間戳對齊，再計算每個時間步估 算的位姿矩陣與真實位姿矩陣之間的歐式距離，最后對 所有時間步的相對位姿誤差取均方根誤差（root mean square error，RMSE）得到ATE。

（2）相對位姿誤差（relative pose error，RPE）用于衡量估計的相對位姿變換與真實相對位姿變換之間的誤 差，適合估計系統(tǒng)的漂移。同樣的，RPE第一步也是進(jìn) 行時間戳對齊，接著對每一段時間間隔計算估計值的相 對位姿矩陣和對應(yīng)真實值的相對位姿矩陣，最后使用 RMSE求平均值得到RPE。

五、總結(jié)展望

5.1 結(jié)合深度學(xué)習(xí)

深度學(xué)習(xí)已經(jīng)在計算機視覺領(lǐng)域取得了巨大成功，在SLAM中也有巨大的潛力。深度學(xué)習(xí)可以用于改善SLAM中的關(guān)鍵技術(shù)，比如特征提取、匹配、姿態(tài)估計等方面。例如，使用深度學(xué)習(xí)進(jìn)行語義分割，可以提取更加語義豐富的地圖信息；使用深度學(xué)習(xí)進(jìn)行圖像去噪或者對抗擾動，可以提高SLAM系統(tǒng)在復(fù)雜環(huán)境下的魯棒性。

5.2 多傳感器融合

多傳感器融合是提高SLAM系統(tǒng)精度和魯棒性的關(guān)鍵。將來，SLAM系統(tǒng)可能會結(jié)合更多種類的傳感器，如高精度慣性測量單元（IMU）、全景相機、多頻段激光雷達(dá)等，以獲取更豐富的環(huán)境信息。同時，如何有效地融合這些異構(gòu)傳感器的數(shù)據(jù)，是未來研究的重點之一。

5.3 分布式多智能體協(xié)同作業(yè)

隨著智能體數(shù)量的增加和應(yīng)用場景的復(fù)雜化，分布式多智能體協(xié)同作業(yè)在SLAM中的應(yīng)用也將變得更加重要。未來的SLAM系統(tǒng)可能會由多個智能體組成，它們分布在不同位置，共同完成地圖構(gòu)建和定位任務(wù)。這就需要研究分布式算法和通信機制，以實現(xiàn)多智能體之間的協(xié)同工作。

5.4 特殊場景下的魯棒性研究

SLAM系統(tǒng)在特殊場景下的魯棒性是一個挑戰(zhàn)，比如光照變化大、大規(guī)模動態(tài)物體存在、傳感器故障等情況。未來的研究可能會集中在如何提高SLAM系統(tǒng)在這些特殊場景下的魯棒性，可能通過引入更多的傳感器、優(yōu)化算法或者結(jié)合深度學(xué)習(xí)等方法來解決這些問題。