一、文章概述

1.1 摘 要

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

1.2問題導向

SLAM最早是由Smith和Cheeseman于1986提出，發展至今已有30多年。激光雷達可 以直接測量距離，對環境的感知更加準確，可以獲取物 體的空間位置和形狀信息，構建高精地圖進行精確定 位，對長時間運行的SLAM系統也更加可靠和穩定。激 光SLAM被廣泛應用于室內導航、三維重建和自動駕駛。本文將系統地對激光SLAM進行綜述，對相關的激光SLAM算法進 行分析總結。

二、SLAM系統框架

傳感器數據采集處理、前端里程計、后端優化、回環檢測和地圖構建。

2.1前端里程計

2.1.1點云去畸變

激光SLAM中的點云畸變是指點云的運動畸變：激光雷達在掃描過程中，載體機器人是在不斷運動的，這就會導致同一幀中的點云數據，是由在不同位置下的激光雷達坐標系測量得到的。常見的點云運動畸變去除方法有純估計法和傳感器輔助法。

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

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

2.1.2點云配準

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 算法中，用于提高配準的準確度和魯棒性

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

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

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

基于數學特征NDT

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

基于極大團的配 準假設生成方法MAC：實現了 無需樣本、無需訓練、同時兼顧精度和效率的三維配準重建效果。

基于學習的方法

基于深度學習的點云配準方法通過數據驅動方式學習，能獲得比人工設計更精準的模型，但其模型的前 期訓練和模型部署需要消耗大量的GPU資源，暫時無 法實現在CPU上的實時計算運行。

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

2.SpinNet：由一個空間點轉換器和一個基于3D卷積神經網絡的特征提取器組成,能夠提取點云表面特征,具有旋轉不變性

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

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

2.2 后端優化

2.2.1基于濾波

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

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

2.2.1基于圖優化

圖優化（graph optimization）利用圖論的方法對系統進行建模求解。因子圖優化（factor graph optimization）在圖優化的基礎上將圖劃分為變量節點和因子節點。因子圖用變量節點表示問題的狀態變量，用 因子節點連接它們并表示約束條件，利用迭代最小化算 法，在因子圖上反復更新變量節點和因子節點的估計值，直至達到收斂狀態。源非線性優化算法庫有

Ceres-Solver
G2O
iSAM
GTSAM
SE-Sync
SE

2.3 回環檢測

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

Scan Context++
Scan Context
FPFH

2.4 地圖構建

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

三、激光SLAM方案

3.1 2D激光SLAM

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

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

Karto：第一個基于圖優化的開源SLAM算法

Hector-SLAM：創新性地采用高斯牛頓法直接進行幀-圖匹配（scan-to-map）優化，構建柵格地圖。缺少閉環檢測，沒有修正能力

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

## 3.2 3D激光SLAM

LOAM：根據曲率大小將特征點分類為邊緣特征點和平面特征點，缺少后端優化和回環檢 測模塊。

A-LOAM：使用Eigen 矩陣庫（https：//eigen.tuxfamily.org）和Ceres-Solver非線性優化 庫代替原本復雜的數學推導

LeGO-LOAM：引入關鍵幀概念，使用關鍵幀及其局部范圍內的數據幀組成 loop-submap。基于地面點的優化更加魯棒精確，計算量更小

Loam_livox：針對Livox新型固態激光雷達掃描特性，對有效點篩選和特征提取部分進行優化

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

CP-ICP：是一種彈性的帶回環的激光雷達里程計法，考慮了幀內數據的連續性和幀間數據的非連續性。

Intensity-SLAM：利用強度信息構建強度地圖，協助特征點提取和位姿估計

提出一種基于強度信息的純激光雷達 SLAM算法，算法的關鍵在于利用點云的強度信息生成一張強度圖像，后續的特征提取和配準、回環檢測與位 姿圖估計都是基于強度圖像進行的

3.3 多傳感器融合SLAM

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

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

松耦合存在兩個優化過程，它將激光雷達數據和IMU數據分別進行參數估計，再使用估計出的參數進行融合

緊耦合表現出更好的魯棒性和準確性，它使用激光雷達數 據和 IMU數據共同構建參數向量，再進行優化和估計。

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

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

R2LIVE：使用誤差狀態迭代卡爾曼濾波融合三個傳感器的測量值進行狀態估計，并通過因子圖進一步優化提高整體精度，但其視覺系統采用特征點法，時間開銷大且在非結構化環境下易失效。

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

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

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

FAST-LIO2：為了使算法能自然適應不同掃描模式的激光雷達和實現更 快的計算，省略了耗時的特征提 取模塊，采用直接法將所有點云數據統一處理。

Faster-LIO：在FAST-LIO2的基礎上使用一種增量式稀疏體素（iVox）數據結構代替ikd-tree，可以有效降低點云配準的時間

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

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

3.4 基于深度學習的3D激光SLAM

深度學習與激光雷達SLAM的結合主要應用于系統中的幾個模塊，如點云的特征提取和配準、回環檢測、構建語義.

特征提取：FCGF、SpinNet
配準方法：Lepard、 REGTR
回環：OverlapNet、Overlap-Transformer

SUMA++：基于激光雷達的語義 SLAM，使用 RangeNet++[70]對點云進行語義分割，根據語義信息剔除動態目標，再將語義信息融入系統，與幾何信息一起建 立約束，從而提高定位和建圖精度。

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

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

提出一種自監督的激光里程計，在投影的強度圖像上使用SuperPoint提取特征點，使用傳統ICP方法估算位姿后用于在線訓練網絡

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

四、數據集與評價指標

4.1 數據集

KITTI：包含視覺圖像、激 光雷達點云、IMU、GPS等數據

SemanticKITTI：在KITTI數據集的基礎上對點云數據進行標注，分類為28類語義，實現全場景分割

ApolloScape：數據集使用了高精度地圖采集車，是目前行業內環境最復雜、標注最精準、數據量最大的三維 自動駕駛公開數據集

NTU-VIRAL：搭載激光雷達、相機、IMU和超帶寬填補了SLAM領域無人機數據集的空白

TUM-RGBD

EuRoC

Oxford RobotCar

USVInland

4.2評價指標

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

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

五、總結展望

5.1 結合深度學習

深度學習已經在計算機視覺領域取得了巨大成功，在SLAM中也有巨大的潛力。深度學習可以用于改善SLAM中的關鍵技術，比如特征提取、匹配、姿態估計等方面。例如，使用深度學習進行語義分割，可以提取更加語義豐富的地圖信息；使用深度學習進行圖像去噪或者對抗擾動，可以提高SLAM系統在復雜環境下的魯棒性。

5.2 多傳感器融合

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

5.3 分布式多智能體協同作業

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

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

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