激光雷達(dá)SLAM算法綜述

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

陸軍工程大學(xué) 通信工程學(xué)院，南京 210000

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

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

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

經(jīng)過調(diào)研，近年來關(guān)于SLAM 的綜述文章逐漸增多，這是因?yàn)樽詣?dòng)駕駛、智能制造、機(jī)器人等人工智能產(chǎn)品正高速發(fā)展，但其中大部分是關(guān)于視覺SLAM 的文章[2-4]，而關(guān)于激光SLAM的文章則較少[5-7]。本文將系統(tǒng)地對激光SLAM進(jìn)行綜述，對相關(guān)的激光SLAM算法進(jìn)行分析總結(jié)。

1 SLAM系統(tǒng)框架

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

1.1 前端里程計(jì)

1.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)系測量得到的。點(diǎn)云畸變會(huì)導(dǎo)致較大的定位誤差，使系統(tǒng)不穩(wěn)定。常見的點(diǎn)云運(yùn)動(dòng)畸變?nèi)コ椒ㄓ屑児烙?jì)法和傳感器輔助法。迭代最近點(diǎn)（iterative closest point，ICP）[8]是一種經(jīng)典的純估計(jì)方法，由給定的兩個(gè)點(diǎn)云集合通過最小二乘法迭代求解位姿變換：

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

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

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

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

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

基于深度學(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)行。關(guān)于其他的激光掃描匹配方法，文獻(xiàn)[23]中有詳細(xì)介紹，表1對上述點(diǎn)云配準(zhǔn)方案進(jìn)行歸納總結(jié)。

表1 點(diǎn)云配準(zhǔn)方案歸納Table 1 Summarization of point cloud registration methods

1.2 后端優(yōu)化

1.2.1 基于濾波

濾波器優(yōu)化算法主要是基于貝葉斯估計(jì)，通過先驗(yàn)概率、傳感器測量和狀態(tài)轉(zhuǎn)移模型來計(jì)算后驗(yàn)概率。

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

表2 狀態(tài)估計(jì)中的濾波器Table 2 Filters in state estimation

值得一提的是，在當(dāng)前使用濾波器的SLAM 算法中，相比于傳統(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)秀的性能。

1.2.2 基于圖優(yōu)化

濾波算法缺點(diǎn)是無法校正累計(jì)誤差，在噪聲大、回環(huán)多的場景下長時(shí)間運(yùn)行會(huì)產(chǎn)生嚴(yán)重的誤差漂移。圖優(yōu)化（graph optimization）利用圖論的方法對系統(tǒng)進(jìn)行建模求解，是當(dāng)前大多數(shù)SLAM 算法所采用的優(yōu)化方案，被廣大研究者所喜愛。因子圖優(yōu)化（factor graph optimization）[24]是一種特殊的圖優(yōu)化方法，針對概率模型應(yīng)用優(yōu)化；在圖優(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)。

當(dāng)前常用的開源非線性優(yōu)化算法庫有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++語言編寫的，利用這些開源算法庫，可以使開發(fā)者專注于系統(tǒng)圖模型的構(gòu)建而非具體的數(shù)學(xué)計(jì)算過程，達(dá)到簡便、高效開發(fā)的目的。文獻(xiàn)[29]對最常用的四種優(yōu)化方法庫G2o、Ceres-Solver、GTSAM和SE-Sync進(jìn)行實(shí)驗(yàn)評估，結(jié)論歸納見表3。

表3 四種非線性優(yōu)化庫Table 3 Four nonlinear optimization libraries

1.3 回環(huán)檢測

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

1.4 地圖構(gòu)建

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

圖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 問題分解為兩個(gè)相互獨(dú)立的子問題：機(jī)器人位姿估計(jì)和地圖坐標(biāo)點(diǎn)的后驗(yàn)概率分布估計(jì)；其中，機(jī)器人位姿估計(jì)使用粒子濾波器實(shí)現(xiàn)，地圖坐標(biāo)后驗(yàn)概率分布則通過擴(kuò)展卡爾曼濾波估計(jì)。Fast-SLAM由于采用粒子濾波技術(shù)，可以處理非線性系統(tǒng)。

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

Karto[41]是第一個(gè)基于圖優(yōu)化的開源SLAM算法，并且認(rèn)識到系統(tǒng)的稀疏性。前端部分，當(dāng)前數(shù)據(jù)幀與相鄰數(shù)據(jù)幀組成局部地圖，采用相關(guān)性掃描匹配（correlative scan matching，CSM）[42]方法讓數(shù)據(jù)幀與局部地圖匹配（scan-to-map）計(jì)算位姿；后端部分采用基于圖優(yōu)化的方法，進(jìn)行全局優(yōu)化；閉環(huán)檢測使用候選數(shù)據(jù)幀構(gòu)建局部地圖，采用scan-to-map方式進(jìn)行匹配，檢測閉環(huán)。

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

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

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

2.2 3D激光SLAM

2.2.1 純激光里程計(jì)

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

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

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

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

2.2.2 多傳感器融合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ì)。緊耦合也是目前多傳感器融合的研究熱點(diǎn)。

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

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

圖6 激光視覺慣性緊耦合系統(tǒng)架構(gòu)Fig.6 Lidar-visual-inertial tightly coupled system architecture

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

目前為止的激光雷達(dá)SLAM算法處理點(diǎn)云的方式，都是根據(jù)激光雷達(dá)的掃描模式，將一定時(shí)間內(nèi)掃描的點(diǎn)云封裝組幀，以點(diǎn)云數(shù)據(jù)幀為單位進(jìn)行后續(xù)處理。這種組幀的過程不可避免地會(huì)使點(diǎn)云產(chǎn)生運(yùn)動(dòng)畸變。雖然使用IMU 測量補(bǔ)償運(yùn)動(dòng)畸變是一種有效的方法，但它也受限于IMU 測量頻率、IMU 測量噪聲和偏置估計(jì)。He 等[65]提出一種新穎的激光慣性里程計(jì)算法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)闆]有人工組幀的過程，所以從根本上消除了運(yùn)動(dòng)失真。Point-LIO 也具有一些缺陷，以單個(gè)點(diǎn)云為處理單位難以建立點(diǎn)與點(diǎn)之間的關(guān)系，一些離群點(diǎn)的加入會(huì)影響算法精度，并且無法實(shí)時(shí)利用點(diǎn)云的語義信息。

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

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

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

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

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

3 數(shù)據(jù)集與評價(jià)指標(biāo)

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

在公開數(shù)據(jù)集上運(yùn)行SLAM 算法，觀察運(yùn)行結(jié)果，是判斷算法是否可行的直觀方式。使用公開數(shù)據(jù)集，可以暫時(shí)避免各種繁瑣的工作如傳感器的標(biāo)定、時(shí)間同步、數(shù)據(jù)打標(biāo)簽等，節(jié)省了數(shù)據(jù)準(zhǔn)備工作所需的時(shí)間。當(dāng)然，一個(gè)算法是否真的魯棒，還需要借助硬件平臺(tái)，在真實(shí)環(huán)境條件下進(jìn)行測試，因?yàn)槊總€(gè)算法的運(yùn)行場景都是不一樣的，數(shù)據(jù)集只是提供仿真條件。

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

3.2 評價(jià)指標(biāo)

SLAM算法的精度指標(biāo)主要有以下兩種：

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

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

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

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

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

4 研究展望

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

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

4.2 多傳感器融合

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

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

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

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

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

5 總結(jié)

SLAM技術(shù)是自主移動(dòng)機(jī)器人實(shí)現(xiàn)路徑規(guī)劃、避障和導(dǎo)航的基礎(chǔ)。隨著激光雷達(dá)技術(shù)向高精度、輕量化、低成本的發(fā)展，激光SLAM 也取得了豐富的研究成果：從最開始的室內(nèi)單線激光雷達(dá)實(shí)現(xiàn)定位，建立二維柵格地圖，到多線激光雷達(dá)和固態(tài)激光雷達(dá)室外大場景下的定位和高精三維地圖和語義地圖建立。本文從SLAM的系統(tǒng)框架出發(fā)，對激光SLAM系統(tǒng)的各個(gè)模塊做系統(tǒng)的介紹，再對優(yōu)秀的開源方法進(jìn)行闡述和總結(jié)，最后對激光SLAM的發(fā)展方向做出展望，希望對有興趣研究激光SLAM的研究者能夠起到幫助作用。