一、文章概述

1.1 摘 要

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

1.2問題導(dǎo)向

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

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

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

2.1前端里程計(jì)

2.1.1點(diǎn)云去畸變

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

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

傳感器輔助法
傳感器輔助法主要是利用高頻率傳感器如慣性測(cè)量單元（inertial measurement unit，IMU）極高測(cè)量頻率的特點(diǎn)，直接測(cè)量角速度和線速度，得出每個(gè)時(shí)刻的位 姿，對(duì)點(diǎn)云作運(yùn)動(dòng)補(bǔ)償

2.1.2點(diǎn)云配準(zhǔn)

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

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

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

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

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

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

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

P2D-NDT（point-todistribution NDT）：NDT算法從2D推廣到3D場(chǎng)景

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

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

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

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

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

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

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

2.2 后端優(yōu)化

2.2.1基于濾波

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

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

2.2.1基于圖優(yōu)化

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

Ceres-Solver
G2O
iSAM
GTSAM
SE-Sync
SE

2.3 回環(huán)檢測(cè)

回環(huán)檢測(cè)模塊用于消除累計(jì)誤差，提高定位和地圖精度，對(duì)在大環(huán)境、回環(huán)多、長時(shí)間下運(yù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)境下或者在里程計(jì)誤差較大時(shí)需要更多的粒子才能得到較好的估計(jì)，這可能會(huì)造成內(nèi)存 爆炸，而粒子的重采樣會(huì)導(dǎo)致粒子耗散。

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

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

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

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

## 3.2 3D激光SLAM

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

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(diǎn)的優(yōu)化更加魯棒精確，計(jì)算量更小

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

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

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

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

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

3.3 多傳感器融合SLAM

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

四、數(shù)據(jù)集與評(píng)價(jià)指標(biāo)

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

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

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

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

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

TUM-RGBD

EuRoC

Oxford RobotCar

USVInland

4.2評(píng)價(jià)指標(biāo)

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

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

五、總結(jié)展望

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

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

5.2 多傳感器融合

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

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

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

5.4 特殊場(chǎng)景下的魯棒性研究

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