激光雷達SLAM算法綜述

劉銘哲，徐光輝，唐 堂，錢曉健，耿 明

陸軍工程大學 通信工程學院，南京 210000

即時定位與地圖構建技術可以描述為無人裝備在未知環境中，通過本體攜帶的傳感器進行數據采集，進行位姿估計和定位，構建增量式地圖，從而實現自主定位和導航。SLAM 最早是由Smith 和Cheeseman[1]于1986提出，發展至今已有30 多年。根據使用的傳感器類別不同，當前主流的SLAM 系統主要分為激光SLAM、視覺SLAM以及各類傳感器輔助激光/視覺的多傳感器融合SLAM技術。視覺SLAM以相機為基礎傳感器，其獲取信息豐富、輕量、低成本的特點受到研究者的青睞。視覺SLAM系統需要處理大量圖像數據，且由于相機對光線較為敏感和無法直接測距的缺點，導致視覺SLAM在光照條件不理想的情況下無法正常工作、精確度不高且系統的實時性難以保證。以激光雷達為主要傳感器的激光SLAM，可以很好地解決上述問題。激光雷達可以直接測量距離，對環境的感知更加準確，可以獲取物體的空間位置和形狀信息，構建高精地圖進行精確定位，對長時間運行的SLAM系統也更加可靠和穩定。激光SLAM被廣泛應用于室內導航、三維重建和自動駕駛等領域，是人工智能時代的研究熱點。

激光SLAM 主要分為2D 激光SLAM 和3D 激光SLAM。激光雷達根據激光線數，可以分為單線激光雷達和多線激光雷達兩種類型。單線激光雷達也稱為一線掃描激光雷達，是2D激光SLAM的主要傳感器，通過單個探測器從一個角度掃描環境，只能獲取一個平面的點云數據，無法獲取三維物體的高度信息，因此應用場景有限，主要應用于室內地面環境，掃地機器人是2D激光SLAM的一個典型應用。多線激光雷達應用于3D激光SLAM，采用多個探測器同時從不同的角度掃描環境，可以獲取含有三維物體坐標信息的點云數據。多線激光雷達根據探測器數目，即線數的不同，又可分為16、32、64、128等不同型號；隨著線數增加，獲取點云數據的速度越快、數據量更豐富，建立的地圖精度越高。多線激光雷達較為昂貴，主要應用于自動駕駛等需要進行高精度環境感知和地圖構建的領域。固態激光雷達是一種新型的激光雷達傳感器，與傳統的機械掃描式激光雷達不同，它是一種窄視野（field of view，FoV）的激光雷達，通過光學相位陣列或光電子掃描來實現寬視角的測量。固態激光雷達具有低成本、小尺寸、高可靠性、高穩定性、高速的優點，被認為是未來激光雷達的發展趨勢之一。圖1是Livox Mid40掃描軌跡圖。

圖1 固態激光雷達Livox Mid40掃描軌跡（顏色代表采樣時間）Fig.1 Solid-state LiDAR Livox Mid40 scan trajectory (colors represent sampling time)

經過調研，近年來關于SLAM 的綜述文章逐漸增多，這是因為自動駕駛、智能制造、機器人等人工智能產品正高速發展，但其中大部分是關于視覺SLAM 的文章[2-4]，而關于激光SLAM的文章則較少[5-7]。本文將系統地對激光SLAM進行綜述，對相關的激光SLAM算法進行分析總結。

1 SLAM系統框架

SLAM系統框架（圖2）大致分為五個部分：傳感器數據采集處理、前端里程計、后端優化、回環檢測和地圖構建。激光雷達及其他輔助傳感器采集數據，交由前端里程計處理分析，快速估算出相鄰激光雷達數據幀之間的位姿變換，此時計算出的位姿含有累計誤差，不夠準確；后端優化負責全局軌跡優化，得出精確位姿，構建全局一致性地圖。在此過程中，回環檢測一直在執行，它用于識別經過的場景，實現閉環，消除累計誤差。本章主要針對后四部分進行闡述，對數據采集部分不做過多介紹。

1.1 前端里程計

1.1.1 點云去畸變

激光SLAM中的點云畸變是指點云的運動畸變：激光雷達在掃描過程中，載體機器人是在不斷運動的，這就會導致同一幀中的點云數據，是由在不同位置下的激光雷達坐標系測量得到的。點云畸變會導致較大的定位誤差，使系統不穩定。常見的點云運動畸變去除方法有純估計法和傳感器輔助法。迭代最近點（iterative closest point，ICP）[8]是一種經典的純估計方法，由給定的兩個點云集合通過最小二乘法迭代求解位姿變換：

其中，R為所求的旋轉矩陣，t為所求的平移向量，它們通過不斷迭代求解得到。事實上ICP 算法并沒有考慮點云的運動畸變，而是通過兩組點云集合直接估計位姿變換。VICP（velocity updating ICP，VICP）[9]是ICP 算法的變種，考慮了激光的運動畸變。VICP 算法假設在一幀激光雷達數據中機器人是勻速運動，對ICP估計的位姿采用線性插值法對點云作運動補償，將補償后的數據再次放入ICP 中求解，形成一個迭代閉環求解最優值。事實上，純估計方法在本質上未能解決點云的運動畸變問題，而是使用一些技巧和方法盡可能減小點云運動畸變帶來的影響。要較好地解決點云運動畸變所帶來的影響，引入外部傳感器進行輔助是當前的最優方案。傳感器輔助法主要是利用高頻率傳感器如慣性測量單元（inertial measurement unit，IMU）極高測量頻率的特點，直接測量角速度和線速度，得出每個時刻的位姿，對點云作運動補償。

1.1.2 點云配準

前端里程計的另一個重要功能是進行點云配準，計算相鄰數據幀之間位姿。采用的配準算法主要是ICP及其變種算法、基于數學特征NDT（normalized distribution transform，NDT）類方法和基于學習的方法。PP-ICP（point-to-plane ICP）[10]基于ICP 算法，將原來計算點與點之間的距離改成計算的是點到面之間的距離，相對于ICP的一階收斂速度，PP-ICP是二階的，收斂速度更快，適用于3D 激光SLAM；PL-ICP（point-to-line ICP）[11]與PP-ICP類似，通過求取點線之間的最小距離使算法快速收斂，提高匹配精度，適用于2D和3D激光SLAM；NICP（normal ICP）[12]將點云表面法向量信息加入到了ICP算法中，用于提高配準的準確度和魯棒性；IMLS-ICP（implicit moving least square ICP）[13]采用隱式函數來表示點云表面，并且通過最小化MLS逼近和隱式函數之間的距離來進行點云配準，能有效處理非剛性物體的配準問題；廣義迭代最近點法（generalized ICP，GICP）[14]通過引入一個全局參考框架來增強ICP算法的全局優化能力，解決了ICP算法運行緩慢、容易陷入局部最優解等問題；VGICP（voxelized GICP）[15]是一種擴展的廣義迭代最近點法，通過將點云轉換為一組小體素網格，提出一種多點分布聚合方法估計體素分布，使匹配更加高效。

NDT[16]是一種基于數學特性的點云配準算法，利用高斯分布對離散點云數據進行建模，將點云轉化為一個點分布函數，進而計算點云之間的相對位姿關系，具有速度快，穩定性好，精度高的優點；Magnusson 等[17]將NDT算法從2D推廣到3D場景，提出P2D-NDT（point-todistribution NDT）。文獻[18]首次提出基于極大團的配準假設生成方法MAC（maximal cliques，MAC），實現了無需樣本、無需訓練、同時兼顧精度和效率的三維配準重建效果。

傳統的點云配準方法盡管在某些場景取得了不錯的效果，但依然沒有一種能夠適應全場景的點云配準方案。由于深度學習領域的快速發展，基于深度學習的點云配準的方案成為研究者們新的研究方向。FCGF[19]提出一種全卷積幾何特征，擺脫了先前SOTA方案需要低級特征輸入的限制，但在對具有不同旋轉分布的點云配準時效果不好；SpinNet[20]由一個空間點轉換器和一個基于3D 卷積神經網絡的特征提取器組成，能夠提取點云表面特征，具有旋轉不變性。Li等[21]提出了一種利用三維位置知識，可用于剛體和可變性場景下的點云配準算法Lepard；算法引入Transformer 注意力機制，通過結合RANSAC和ICP完成點云配準；局限性在于Lepard是一種粗匹配方法，精細度有待提高，在數據低重疊情況下具有挑戰性。Yew 等[22]使用注意力機制取代顯性特征匹配和RANSAC，提出一個包含自注意和交叉注意的transformer 網絡架構REGTR，直接預測點云剛性變換，解決了點云降采樣帶來的分辨率問題，算法在3DMatch數據上取得了最先進的性能。

基于深度學習的點云配準方法通過數據驅動方式學習，能獲得比人工設計更精準的模型，但其模型的前期訓練和模型部署需要消耗大量的GPU 資源，暫時無法實現在CPU上的實時計算運行。關于其他的激光掃描匹配方法，文獻[23]中有詳細介紹，表1對上述點云配準方案進行歸納總結。

表1 點云配準方案歸納Table 1 Summarization of point cloud registration methods

1.2 后端優化

1.2.1 基于濾波

濾波器優化算法主要是基于貝葉斯估計，通過先驗概率、傳感器測量和狀態轉移模型來計算后驗概率。

在狀態估計中，常使用的濾波器算法有卡爾曼濾波（Kalman filter，KF）、擴展卡爾曼濾波（extended Kalman filter，EKF）、無跡卡爾曼濾波（unscented Kalman filter，UKF）、狀態誤差卡爾曼濾波（error-state Kalman filter，ESKF）、粒子濾波（particle filter，PF），表2 對這幾種濾波器做了總結。

表2 狀態估計中的濾波器Table 2 Filters in state estimation

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

1.2.2 基于圖優化

濾波算法缺點是無法校正累計誤差，在噪聲大、回環多的場景下長時間運行會產生嚴重的誤差漂移。圖優化（graph optimization）利用圖論的方法對系統進行建模求解，是當前大多數SLAM 算法所采用的優化方案，被廣大研究者所喜愛。因子圖優化（factor graph optimization）[24]是一種特殊的圖優化方法，針對概率模型應用優化；在圖優化的基礎上將圖劃分為變量節點和因子節點。因子圖用變量節點表示問題的狀態變量，用因子節點連接它們并表示約束條件，利用迭代最小化算法，在因子圖上反復更新變量節點和因子節點的估計值，直至達到收斂狀態。

當前常用的開源非線性優化算法庫有Ceres-Solver（http：//ceres-solver.org）、G2o（general graph optimization）[25]、iSAM（incremental smoothing and mapping）[26]、GTSAM（Georgia tech smoothing and mapping）[27]、SE-Sync（synchronization over special Euclidean group SE（n））[28]，它們都是基于C++語言編寫的，利用這些開源算法庫，可以使開發者專注于系統圖模型的構建而非具體的數學計算過程，達到簡便、高效開發的目的。文獻[29]對最常用的四種優化方法庫G2o、Ceres-Solver、GTSAM和SE-Sync進行實驗評估，結論歸納見表3。

表3 四種非線性優化庫Table 3 Four nonlinear optimization libraries

1.3 回環檢測

回環檢測模塊用于消除累計誤差，提高定位和地圖精度，對在大環境、回環多、長時間下運行的SLAM系統至關重要?；邳c云掃描匹配的方法也可以作用于回環檢測部分，通過檢測兩個激光雷達數據幀中點云數據之間的相似性來判斷它們是否來自同一位置。由于一幀點云數據量較大，含有的信息復雜不夠直觀，所以多采用的是基于特征點提取的方法，匹配特征點以提高回環檢測的準確率和魯棒性。Kim 等[30]提出基于掃描上下文的方法Scan Context，這是一種非直方圖的全局描述符，它分割每一幀點云為若干區域，選取具有代表性的點保存到二維數組中，通過kd-tree[31]查找歷史Scan Context 描述子進行相似度計算，檢測是否回環。Scan Context算法的優點在于它具有較低的計算復雜度和較高的魯棒性，它能夠處理不同視角、噪聲和局部遮擋等現實世界中的挑戰，并在大規模環境中實現高效的回環檢測。Scan Context++[32]是Scan Context 的改進版，在Scan Context的旋轉不變性基礎上，增加了平移不變性，并且提升了速度；快速點特征直方圖（fast point feature histograms，FPFH）[33]在PFH（point feature histograms）[34]的基礎上改進而來，利用局部范圍內點的表面法向量計算局部描述子，具有旋轉不變性和尺度不變性，計算速度快。基于深度學習的回環檢測首先應用于視覺SLAM 領域，因為相機可以采集到豐富的語義信息；近年來，有不少學者將語義信息融入激光雷達回環檢測模塊中，實現魯棒的效果，關于這部分內容，將在后面激光雷達SLAM結合深度學習的章節（2.2.3小節）進行介紹。

1.4 地圖構建

SLAM 中的地圖隨著位姿和坐標點的優化而增量式生成，實際上就是坐標點的集合。圖3展示了3D激光SLAM常用的三種地圖表達形式。對于2D激光SLAM來說，一般使用的是柵格地圖：將真實環境劃分為一個個網格，用概率0～1表示該網格在實際環境中被物體占據的概率；3D 激光SLAM 構建的是3D 點云地圖，使用體素濾波器[35]進行降采樣。Ruan等[36]在SLAMesh中提出一種基于高斯過程重構和頂點連接的網格劃分策略，實現網格地圖的快速重建；八叉樹地圖（OctoMap）[37]是一種基于八叉樹數據結構（Octree）的三維空間地圖表示方法，通過將三維空間遞歸分解為8個相等的子單元來高效地處理和存儲大規模環境信息；語義地圖通過語義分割將環境中的物體進行識別分類，使機器人能更好地感知環境，這也是SLAM 建圖模塊的發展方向。文獻[38]提出將建圖過程分為地圖初始化和移動掃描兩個過程，通過創建靜態的初始地圖來提升移動掃描過程中定位與建圖的精度，但該方法只適用于室內靜態環境。

圖3 地圖形式Fig.3 Map forms

2 激光SLAM方案

2.1 2D激光SLAM

Fast-SLAM[39]是一種基于粒子濾波器的SLAM 算法，由Thrun 等于2002 年提出；Fast-SLAM 提出RBPF（Rao-Blackwellised particle filter）算法，將SLAM 問題分解為兩個相互獨立的子問題：機器人位姿估計和地圖坐標點的后驗概率分布估計；其中，機器人位姿估計使用粒子濾波器實現，地圖坐標后驗概率分布則通過擴展卡爾曼濾波估計。Fast-SLAM由于采用粒子濾波技術，可以處理非線性系統。

Fast-SLAM在大環境下或者在里程計誤差較大時需要更多的粒子才能得到較好的估計，這可能會造成內存爆炸，而粒子的重采樣會導致粒子耗散。GMapping[40]在Fast-SLAM的基礎上進行改進：通過降低粒子數量的方法大幅度緩解內存爆炸；采用選擇性重采樣方法，對粒子進行重要性權重排序，對權值低的粒子進行重采樣，解決RBPF算法中粒子耗散問題。GMapping算法在室內環境下實時性好，精度高，是早期乃至現在最常用的SLAM算法之一。GMapping是基于濾波的算法且無回環檢測，長時間運行存在較大累計誤差，無法在室外構建大環境地圖。

Karto[41]是第一個基于圖優化的開源SLAM算法，并且認識到系統的稀疏性。前端部分，當前數據幀與相鄰數據幀組成局部地圖，采用相關性掃描匹配（correlative scan matching，CSM）[42]方法讓數據幀與局部地圖匹配（scan-to-map）計算位姿；后端部分采用基于圖優化的方法，進行全局優化；閉環檢測使用候選數據幀構建局部地圖，采用scan-to-map方式進行匹配，檢測閉環。

Hector-SLAM[43]不需要里程計信息，創新性地采用高斯牛頓法直接進行幀-圖匹配（scan-to-map）優化，構建柵格地圖。但由于缺少回環檢測，其對地圖沒有修正能力，一旦地圖出錯將影響后續的匹配和建圖效果；scan-to-map 計算量較大，為保證獲取較好的建圖效果，需要機器人以較低的速度移動。

Google在Karto的基礎上進行改進，于2016提出并開源Cartographer[44]算法，加入傳感器同步、位姿外推器、激光數據預處理功能；引入子圖（submap）概念，使用CSM 和梯度優化相結合的方法使幀與子圖進行匹配，提升匹配精度和效率；后端基于圖優化算法，將當前激光幀和之前建立的所有子圖都加入閉環檢測模塊，采用分支定界法（branch and bound）[45]提升搜索速度。Cartographer是目前精度最高、實時性、魯棒性最好、二次開發最為便捷的開源SLAM 算法之一。表4 對2D 激光SLAM算法進行歸納。

表4 2D激光SLAM方案Table 4 2D LiDAR SLAM

2.2 3D激光SLAM

2.2.1 純激光里程計

Zhang 等[46]于2014 年提出的LOAM（lidar odometry and mapping），是經典且具有代表性的算法，后續許多激光SLAM 算法都借鑒其思想。圖4 是LOAM 算法框架，它采用特征點法，利用局部范圍內的點計算點的曲率，根據曲率大小將特征點分類為邊緣特征點和平面特征點；算法對激光雷達做勻速運動假設，使用線性插值法進行運動補償。香港科技大學Qin 根據LOAM 論文復現代碼，開源A-LOAM 算法，使用Eigen 矩陣庫（https：//eigen.tuxfamily.org）和Ceres-Solver 非線性優化庫代替原本復雜的數學推導，代碼簡潔易懂，適合初學者學習。LOAM 算法的缺點是缺少后端優化和回環檢測模塊，在大規模場景和回環較多場景下會產生漂移，精度會大幅度降低。2018年，Shan等[47]在LOAM的基礎上加入回環檢測功能，提出并開源LeGO-LOAM，引入關鍵幀概念，使用關鍵幀及其局部范圍內的數據幀組成loop-submap，這樣可以極大地減少計算量，過濾冗余數據，讓當前數據幀與loop-submap進行匹配，達到回環檢測的目的。LeGO-LOAM是一個輕量級的SLAM算法，比LOAM 更加魯棒精確，計算量更小，但它也有局限性，相比于LOAM，LeGO-LOAM 提取特征的策略是將三維點云投影到二維圖像上，以此來分離地面點和非地面點，去除噪聲，這就使得算法對地面環境要求苛刻。作者后期在算法中以松耦合的方式融合了IMU 傳感器，但相比于其他緊耦合算法，松耦合的提升有限。

圖4 LOAM算法框架Fig.4 Algorithm framework for LOAM

Loam_livox[48]由香港大學Lin提出，針對Livox新型固態激光雷達掃描特性，對有效點篩選和特征提取部分進行優化，對后續其他基于固態激光雷達的SLAM方案具有重要參考意義。NASA 提出一種基于稠密點云快速定位的直接激光里程計[49]，將關鍵幀和相關聯點云儲存在字典中，節約幀圖匹配的計算資源和降低基于關鍵幀構成的子圖所含有的重復信息；同時提出一種定制的迭代最近點解算器NanoGICP，用于輕量級點云配準，具有跨對象數據共享功能；大量實驗證明，該算法能夠在高速條件下實時處理稠密點云數據，提高定位精度。使用勻速模型校正點云畸變的做法雖然在大多數駕駛場景下表現不錯，但無法適應方向和速度快速變換的場景，CP-ICP[50]是一種彈性的帶回環的激光雷達里程計算法，考慮了幀內數據的連續性和幀間數據的非連續性，以實現對高頻劇烈運動更魯棒的目的；但其回環檢測部分約束過多，限制了傳感器的運動，在某些應用場景具有局限性。

傳統的激光雷達SLAM 方法主要關注幾何信息和時間信息，而往往會忽略激光雷達反射的強度信息。Wang等認為一個魯棒的激光雷達SLAM系統應該考慮強度信息，提出強度信息輔助的Intensity-SLAM[51]，利用強度信息構建強度地圖，協助特征點提取和位姿估計；在回環模塊使用基于強度信息的Scan Context描述符，對旋轉具有魯棒性。Li 等[52]同樣利用強度信息增強激光雷達在退化環境下的可靠性，為了避免邊緣強度特征點退化為平面點，采用基于圖像處理的線型圖管理模塊來剔除邊緣強度點異常值，實現退化環境下的魯棒性運行。文獻[53]提出一種基于強度信息的純激光雷達SLAM算法，算法的關鍵在于利用點云的強度信息生成一張強度圖像，后續的特征提取和配準、回環檢測與位姿圖估計都是基于強度圖像進行的。

2.2.2 多傳感器融合SLAM

相較于純激光雷達方案，使用激光雷達與IMU 進行信息融合，利用IMU高頻率輸出運動信息，校正點云運動畸變，提供一個良好的初值可以使算法避免陷入局部最小值，提高算法精度和減少計算量。激光雷達與IMU融合可以分為緊耦合和松耦合；松耦合存在兩個優化過程，它將激光雷達數據和IMU 數據分別進行參數估計，再使用估計出的參數進行融合；相較于松耦合，緊耦合表現出更好的魯棒性和準確性，它使用激光雷達數據和IMU 數據共同構建參數向量，再進行優化和估計。緊耦合也是目前多傳感器融合的研究熱點。

LIO-SAM[54]算法是經典的基于平滑和建圖（smoothing and mapping）的緊耦合激光-慣性里程計。算法里程計部分延續LOAM的思想，提取特征點，并使用IMU數據校正點云畸變，提供數據幀之間位姿變換的初始值；后端采用因子圖優化架構（圖5），消除累計誤差，進行全局優化。由于LIO-SAM 算法是基于特征點法，且激光雷達傳感器本身具有局限性，導致算法在退化場景和非結構場景下會缺乏有效觀測而失效。要解決激光雷達在退化場景無法正常工作的問題，可以考慮加入相機傳感器，相機可以采集到豐富的語義信息，彌補激光雷達的缺陷。由激光雷達、IMU、相機組成的多傳感器融合是當前最受歡迎的組合，可以應對絕大部分場景；它通常由激光-慣性里程計（lidar-inertial odometry，LIO）和視覺慣性里程計（visual-inertial odometry，VIO）通過IMU緊密結合，達到緊耦合目的（圖6[55]）。LIO-SAM作者在LIO-SAM中加入相機傳感器，提出激光-視覺-慣性緊耦合的系統LVI-SAM[56]。LVI-SAM由激光-慣性系統（lidarinertial system，LIS）和視覺-慣性系統（visual-inertial system，VIS）組成；LIS 為VIS 提供準確的深度信息，提高VIS 精度；反過來LIS 利用VIS 的初步位姿估計進行掃描匹配。算法的優點在于VIS或LIS其中一個發生故障，LVI-SAM也能正常工作，這使得算法在紋理較少和缺少特征的環境下具有較好的魯棒性。同樣的激光-慣性-視覺里程計還有香港大學Lin等[57]提出的R2LIVE，使用誤差狀態迭代卡爾曼濾波融合三個傳感器的測量值進行狀態估計，并通過因子圖進一步優化提高整體精度，但其視覺系統采用特征點法，時間開銷大且在非結構化環境下易失效。為了提高算法的實時性和精度，作者在改進版R3LIVE[58]中對視覺系統進行重構，使用光流法代替原本的特征點法進行幀間跟蹤，通過最小化幀到地圖的光度誤差融合視覺數據，并為地圖渲染RGB 顏色。實驗證明，R3LIVE 實現目前最先進的多傳感器融合算法精度和魯棒性。R3LIVE++[59]在R3LIVE基礎上，考慮了相機光度校準和對相機曝光時間的在線估計，進一步提高了定位和建圖的精度。

圖5 LIO-SAM算法因子圖框架Fig.5 Factor graph framework of LIO-SAM

圖6 激光視覺慣性緊耦合系統架構Fig.6 Lidar-visual-inertial tightly coupled system architecture

為了應對快速運動、噪聲和雜波環境條件，Xu等[60]提出一個高計算效率和魯棒性強的緊耦合激光-慣性里程計框架FAST-LIO。FAST-LIO 使用激光雷達與IMU緊耦合的誤差狀態迭代卡爾曼濾波算法，IMU數據前向傳播狀態估計量，反向傳播對點云數據作運動補償；提出一個新的計算卡爾曼增益的公式，使計算量不再依賴于測量維數，而是依賴于狀態維數，極大地減少了計算量。FAST-LIO2[61]在FAST-LIO的基礎上進行改進，為了使算法能自然適應不同掃描模式的激光雷達和實現更快的計算，省略了耗時的特征提取模塊，采用直接法將所有點云數據統一處理，計算點面殘差；使用一種新的數據結構動態kd-Tree——ikd-Tree[62]存儲點云數據，與現有的動態數據結構（octree、R*-tree 和nanoflann k-d tree）相比，它有更好的整體性能。大量實驗證明，FAST-LIO2 相較于其他激光-慣性緊耦合方案具有更高效率，在速度方面有巨大提升，在高速環境下魯棒性更好。但在大范圍場景下，點云數量龐大，ikd-Tree 維護樹結構動態平衡的時間不可忽略，這也決定了FAST-LIO2無法實時實現大規模場景下的建圖任務。Faster-LIO[63]在FAST-LIO2的基礎上使用一種增量式稀疏體素（iVox）數據結構代替ikd-tree，可以有效降低點云配準的時間，也不會大幅度影響里程計的精度表現，能夠達到FAST-LIO2 的1.5～2 倍的速度。Zheng 等[64]提出的激光-慣性-視覺緊耦合算法FASTLIVO在FAST-LIO2的LIO系統基礎上使用相機傳感器加入VIO 子系統，在測量層面實現兩個傳感器的耦合，在激光雷達退化場景和光線變化劇烈的場景下依舊能可靠運行；但在劇烈運動場景中，融合模糊的圖像對里程計精度并沒有幫助，在低速運動場景下更能體現其優勢。FAST-LIVO 可以在Intel 和ARM 處理器上實時運行，并有著顯著的計算余量。

目前為止的激光雷達SLAM算法處理點云的方式，都是根據激光雷達的掃描模式，將一定時間內掃描的點云封裝組幀，以點云數據幀為單位進行后續處理。這種組幀的過程不可避免地會使點云產生運動畸變。雖然使用IMU 測量補償運動畸變是一種有效的方法，但它也受限于IMU 測量頻率、IMU 測量噪聲和偏置估計。He 等[65]提出一種新穎的激光慣性里程計算法Point-LIO，以單個點云為數據處理單位，更新每個點的狀態，具有極高的里程計輸出頻率和帶寬，在自身運動存在嚴重振動和高角速度高線速度的情況下也能魯棒運行；并且因為沒有人工組幀的過程，所以從根本上消除了運動失真。Point-LIO 也具有一些缺陷，以單個點云為處理單位難以建立點與點之間的關系，一些離群點的加入會影響算法精度，并且無法實時利用點云的語義信息。

關鍵幀的生成對SLAM算法的性能有很大影響，選取正確的關鍵幀可以節約計算資源，剔除冗余數據，提高計算速度和精度?，F有的激光雷達SLAM系統如LeGOLOAM，LIO-SAM都采用固定的關鍵幀生成機制，以兩幀之間的距離和時間作為閾值選取關鍵幀。文獻[66]提出一種自適應選擇關鍵幀算法，可以根據周圍環境的變換做出相應的調整，在寬闊特征富足的環境下增大關鍵幀之間的距離和時間，在狹小隨時可能發生突變的環境下加快關鍵幀選取頻率，適用于復雜的地底環境。

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

深度學習與激光雷達SLAM 的結合主要應用于系統中的幾個模塊，如點云的特征提取和配準、回環檢測、構建語義地圖。在點云處理部分主要分為兩種方式：（1）基于學習的特征提取，如FCGF、SpinNet；（2）基于深度學習網絡的端到端的點云配準方法，如Lepard、REGTR。這部分的相關內容在1.1.2 小節已有相關介紹。準確的回環檢測一直是激光雷達SLAM 有待解決的問題，利用深度學習構建合理的網絡模型，通過大量的學習可以使算法提取點云中的關鍵特征信息，提高回環準確率。OverlapNet[67]是為激光雷達SLAM設計的回環檢測算法，它無需兩幀點云數據的相對位姿，而是利用圖像重疊率的方式來檢測回環。算法使用深度圖、強度圖、法向量圖和語義圖作為模型輸入，輸出圖像重疊率和偏航角的預測。但在環境突變的情況下，算法的準確率會降低，雖然所處位置相似，但此時兩幀激光雷達所映射的圖像差別很大，重疊率低。Ma等[68]提出Overlap-Transformer 同樣利用OverlapNet 中圖像重疊率的思想訓練網絡，它僅需要三維點云投影的距離圖像，生成全局描述子，使用Transformer 結構增強全局描述子的特異性，具有偏航角旋轉不變性。

SUMA++[69]是基于激光雷達的語義SLAM，使用RangeNet++[70]對點云進行語義分割，根據語義信息剔除動態目標，再將語義信息融入系統，與幾何信息一起建立約束，從而提高定位和建圖精度。SA-LOAM[71]是基于LOAM的語義輔助激光SLAM，提出一種語義輔助的ICP算法，并在閉環檢測模塊中集成基于語義圖的位置識別方法，適合在大規模場景中構建全局一致性語義地圖。現有的回環檢測方法大多基于圖像或者點云，環境結構或者紋理相似時，這些方法會產生誤差。針對這一問題，ASL-SLAM[72]提出一種基于活動語義的回環檢測方法，通過IMU 傳感器對Z 軸角速度和加速度的分析來檢測轉彎和通過減速帶兩種語義，但在復雜的城市道路，兩種活動語義遠遠不夠，這也使得算法檢測到較少的回環。文獻[73]提出一種自監督的激光里程計，在投影的強度圖像上使用SuperPoint[74]提取特征點，使用傳統ICP 方法估算位姿后用于在線訓練網絡。該算法可以以高于傳感器頻率的頻率輸出里程計，但由于提取的特征點是稀疏的，無法用于構建可導航的地圖。EfficientLO-Net[75]是第一個完全端到端的高效3D 激光雷達里程計框架，提出了基于投影感知的三維點云表示方法和特征學習方法，其性能優于近期所有基于學習的激光雷達里程計，甚至優于基于幾何方法的LOAM。表5歸納了近年來優秀的3D激光SLAM方案。

表5 3D激光雷達SLAM方案歸納Table 5 Summary of 3D LiDAR SLAM method

3 數據集與評價指標

3.1 數據集

在公開數據集上運行SLAM 算法，觀察運行結果，是判斷算法是否可行的直觀方式。使用公開數據集，可以暫時避免各種繁瑣的工作如傳感器的標定、時間同步、數據打標簽等，節省了數據準備工作所需的時間。當然，一個算法是否真的魯棒，還需要借助硬件平臺，在真實環境條件下進行測試，因為每個算法的運行場景都是不一樣的，數據集只是提供仿真條件。

KITTI[77-78]數據集是一個廣泛用于計算機視覺和自動駕駛研究的公開數據集，它由德國卡爾斯魯厄理工學院和日本豐田技術研究所聯合創建，包含視覺圖像、激光雷達點云、IMU、GPS等數據，被廣泛運用于SLAM算法的評估實驗；SemanticKITTI[79]在KITTI數據集的基礎上對點云數據進行標注，分類為28類語義，實現全場景分割，可用于訓練和測試評估點云分割模型；百度公司的ApolloScape[80]數據集使用了高精度地圖采集車，是目前行業內環境最復雜、標注最精準、數據量最大的三維自動駕駛公開數據集；南洋理工大學開源的自主飛行器數據集NTU-VIRAL[81]，搭載激光雷達、相機、IMU 和超帶寬（ultra wide band，UWB），涵蓋室內室外多個場景，填補了SLAM領域無人機數據集的空白。除此之外，開源的數據集有TUM-RGBD[82]、EuRoC[83]、Oxford Robot-Car[84]、USVInland[85]等。

3.2 評價指標

SLAM算法的精度指標主要有以下兩種：

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

公式（2）中，Qi代表第i幀的真實位姿；Pi代表第i幀的估計位姿；單目相機具有尺度不確定性，S是從估計位姿到真實位姿的相似轉換矩陣；Fi代表第i幀絕對位姿誤差值；公式（3）中n是總數，Δ為時間間隔，m為實際參與計算結果個數，其值為m=n-Δ；trans(Fi)代表取絕對位姿誤差的平移部分。

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

公式（4）中，Ei代表第i幀的相對位姿誤差；公式（5）中trans(Ei)表示取相對位姿誤差的平移部分，其余參數與ATE表示一致。

KITTI 官方提供了專用于評測數據集平移誤差和旋轉誤差的工具KITTI_odometry_evaluation_tool；evo（https：//github.com/MichaelGrupp/evo）是一款受到研究者喜愛的開源軌跡評測工具，它可以根據時間戳將軌跡對齊，評測軌跡誤差、ATE、RPE等指標，并且支持KITTI、TUM、EuRoc、ROS bag等多種數據格式。

4 研究展望

4.1 結合深度學習

人工智能是工業4.0 重要的研究內容和發展方向，深度學習與SLAM 相結合可以提升系統運行效率和魯棒性。歸功于深度學習技術在圖像處理領域大放異彩，深度學習在視覺SLAM 領域已有深入研究[86-88]，并已應用于圖像特征匹配、閉環檢測和語義地圖構建模塊；而在激光SLAM 中，由于點云數據缺少紋理信息、所含信息量較少，所以與深度學習的融合不如視覺SLAM那樣成熟，主要應用于點云特征提取和配準、點云語義分割。PointNet[89]是一個優秀的點云分類與分割模型，提取的全局特征能夠很好地完成分類任務，但局部特征提取能力較差；PointNet++[90]提出多層次特征提取結構，提高局部特征提取的能力，彌補了PointNet的缺點。當前深度學習與SLAM 算法結合的困難點在于深度學習需要大量的訓練樣本和GPU 資源，難以部署在嵌入式平臺上實時運行，并且模型的泛化能力不足。

4.2 多傳感器融合

激光雷達能夠提供精確的坐標信息，但缺少紋理信息，且在雨霧環境下會失效；相機能夠提取豐富的環境信息，便于目標識別和語義分析，但對光線要求較為苛刻，且無法提供比肩激光精度的深度信息。無人裝備往往運行在復雜、動態變化的環境中，而單一傳感器作用場景有限，無法很好地應對這種復雜環境。解決復雜環境下SLAM算法的魯棒性問題，多傳感器融合技術是關鍵。毫米波雷達不受雨雪、煙塵的影響，具有更好的穿透力，可以與激光雷達融合，彌補激光雷達的缺陷；IMU本身不依賴外部環境，又能實現高頻率測量，是多傳感器融合必不可少的一部分，當前主流的SLAM算法幾乎都有IMU參與。常見的多傳感器組合方案有激光-慣性里程計（LIO）、視覺-慣性里程計（VIO）、激光-視覺-慣性里程計（LVIO）。LIO 和LVIO 在第二部分已有詳細介紹；VIO 也是當前學術界的研究熱點，著名的開源算法有VINS-Mono[91]、VINS-Fusion[92]、ORB-SLAM3[93]；各類新型傳感器的應用為多傳感器融合領域注入新活力，期待研究者們探索新型傳感器的應用以解決現有問題。

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

多智能體集群控制是當前的研究熱點和發展方向，分布式協同SLAM 意在通過多個智能體在未知環境下協同工作，并通過通信協議交換彼此獲取的地圖、自身位置等信息，達到快速感知環境、精確定位和地圖構建的目的，對某些特殊任務如搜救、戰場環境勘測具有重大意義。分布式通信機制、數據共享與地圖融合、環境閉環檢測和異常處理是多機SLAM 需要解決的問題。分布式協同SLAM需要機器人之間進行頻繁的通信，傳輸地圖數據、位置等信息。然而，通信帶寬限制和延遲問題可能會導致通信瓶頸，影響數據傳輸，導致數據無法實時同步，影響協同建圖的實時性和準確性。如何設計通信機制，解決數據同步和一致性問題是一個挑戰。分布式協同SLAM系統應具備較強的魯棒性和容錯性，能夠應對機器人故障、通信中斷或其他異常情況。如何設計機制來處理這些問題，并保證整體系統的穩定運行，也是一個重要的挑戰。目前比較好的多機SLAM工作有DOOR-SLAM[94]、Kimera-Multi[95]、D2SLAM[96]、Swarm-LIO[97]。

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

研究改進一個SLAM算法，使其在各個場景下都具有較好的魯棒性是一件非常困難的事。事實上大部分智能體都是在較為固定的場景下工作的，特別是工業機器人，如室內服務機器人、水下作業機器人[98]、礦井作業機器人[99]、電網巡檢空中機器人[100]等。針對這些特定場景，對環境特征進行建模分析并結合考慮應用場景的側重點，對算法進行應用優化，這也是SLAM 技術在工業界發展的趨勢。在城市場景下，動態SLAM技術扮演著重要的角色。城市環境中存在著大量的動態物體，例如車輛、行人、自行車以及其他交通參與者。動態SLAM旨在實現對這些動態物體的實時檢測和跟蹤，并在建立地圖的過程中將其與靜態部分分離開來，以確保地圖的準確性和穩定性。如何使SLAM 算法能夠在復雜動態的環境下魯棒運行，是一項非常具有挑戰性的任務。

5 總結

SLAM技術是自主移動機器人實現路徑規劃、避障和導航的基礎。隨著激光雷達技術向高精度、輕量化、低成本的發展，激光SLAM 也取得了豐富的研究成果：從最開始的室內單線激光雷達實現定位，建立二維柵格地圖，到多線激光雷達和固態激光雷達室外大場景下的定位和高精三維地圖和語義地圖建立。本文從SLAM的系統框架出發，對激光SLAM系統的各個模塊做系統的介紹，再對優秀的開源方法進行闡述和總結，最后對激光SLAM的發展方向做出展望，希望對有興趣研究激光SLAM的研究者能夠起到幫助作用。