引言：

無人平臺在大范圍環境中 實現自 主定位與導航的能力 需求日 益嚴苛， 其中 基于激光雷達的同步定位和繪圖 技術(SLAM) 是主流的研究方案。 在這項工作中， 本文系統概述了 3D 激光雷達SLAM算法框架和關鍵模塊， 分析闡述了近年來的研究熱點問題和未來發展趨勢， 梳理了3D激光雷達 SLAM 算法性能的評估標準， 并據此選取目前較為成熟的具有代表性的6種開源 3D 激光雷達SLAM 算法在機器人操作系統(ROS) 中進行了測試評估， 基于 KITTI 基準數據集， 從 KITTI 官方精度標準、SLAM算法精度指標、算法耗時和處理幀率3方面進行了橫向比較， 結果表明， 所選6種算法中 LIO-SAM 算法性能綜合表現突出， 其在 00 序列數據集的測試中， 絕對軌跡誤差(ATE) 和相對位姿誤差(RPE) 的 RMSE 數據分別為 1. 303 和 0. 028， 算法處理的幀率(fps) 為 28. 6， 最后依據 CiteSpace 分析討論了 3D 激光雷達 SLAM 技術的應用趨勢。

隨著移動機器人與無人平臺在各類應用場景的不斷進階與探索， 對移動載體定位能力的要求也越來越高。日益升級的智能駕駛應用， 尤其是更先進的無人駕駛， 為保證安全， 需求定位精度達到厘米級， 現有的定位輸入源主要是基于全球定位系統 ( global positioning system，GPS) 等衛星定位手段， 而傳統的 GPS 定位導航精度只有5 ～10 m， 當前提高定位精度的方案大體上分為兩類。

一類是對衛星信號定位輸入源進行升級， 如升級為更高精度的實時差分定位(real time kinematic， RTK) ， 或者將進行輔助推算導航定位的低成本微機電系統 ( micro electromechanical system， MEMS) 慣導設備升級為更高精度級別的慣導， 雖然此類方式可以在大多數的使用場景下提高定位精度， 但是在 GPS 信號容易丟失的場景下，如停車場、交錯高架路以及室內環境等， 仍存在缺陷且成本較高， 而慣導的推算定位本身就存在初始化和累計誤差問題;

另一類是研究學者基于第一類缺陷提出的引入新的實現自主定位源的方法， 如使用激光雷達、毫米波雷達、攝像頭等感知傳感器， 因此基于各類傳感器的自主定位方案開始受到科研人員的廣泛關注， 尤其是基于激光雷達和深度相機的自主定位， 近年來已成為熱點方案， 本文主要闡述基于多線激光雷達的自主定位方案。

同步定位與繪圖 ( simultaneous localization and mapping，SLAM) 作為一種在未知環境中進行姿態估計與定位的技術， 廣泛應用于移動機器人和無人駕駛等領域。

SLAM 相關概念最早由 Cheeseman 等于 1986 年在 IEEE 機器人與自動化會議被提出， 該技術通過傳感器采集信息，生成無人平臺所探索環境的地圖并對其進行定位， 實現無人平臺的自主移動[1]。從該技術的定義可知， 該技術由地圖構建和定位兩部分組成。地圖構建是把通過傳感器采集的序貫激光雷達點云或視覺特征從各幀局部坐標系投影至全局坐標系， 之后完成地圖拼接和定位， 即獲取移動載體在所建地圖中的位置和姿態信息。

一方面獲取傳感器采集的各幀數據對應的局部坐標系的位置和姿態是構建地圖的關鍵， 即建圖包含了定位問題; 另一方面構建準確的地圖又是精確定位的前提， 因此， 定位與建圖兩者是高度耦合的，可作為一個問題尋找解決方案。在線實時的SLAM 技術作為自主定位領域的核心正在被廣泛研究。

目前 2D 激光 SLAM 技術發展已較為成熟， 既可用于民用服務如掃地機器人， 也可用 于工業現場， 如 KUKA Navigation Solution [2] 。基于激光點云的 3D SLAM 技術利用三維激光傳感器(一般是多線激光雷達， 也有少部分是用自制的單線激光雷達組合) 獲取三維空間點數據， 之后再通過相鄰點云幀之間的掃描匹配進行位姿估計， 并建立完整的點云地圖， 與 2D 激光 SLAM 具有相通的技術原理。

三維激光雷達通過光學測距可以直接采集到海量具有角度和距離精準信息的點， 這些點的合集被稱為點云，可以反映出真實環境中的幾何信息。由于其建圖直觀，測距精度極高且不容易受到光照變化和視角 變化的影響， 是室外大型環境的地圖構建應用中必不可少的傳感器。

得益于DARPA(美國國防部先進研究項目 局地面挑戰賽)[3-4]的推動， 美國 Velodyne 公司的多線激光雷達開始用于無人汽車 SLAM [5] ， 移動機器人的定位與建圖問題由室內逐漸轉為室外， 地圖構建也由二維拓展到三維，范圍成倍擴大， 但多線雷達成本較高。隨著多線激光雷達的量產化和普及化以及嵌入式處理器功耗降低、 計算能力的增強， 基于多線激光雷達的 SLAM 技術正在快速發展。

由于基于多線激光雷達的三維 SLAM 方法在幀間匹配方面有著更豐富的匹配方式及更好的魯棒性， 并可以同圖像信息、物理模型等信息進行融合[6]使定位精度得到提升， 因而具有巨大的發展潛力。關于 SLAM 的相關工作前人已有總結， 尤其是基于視覺的 SLAM 綜述出現較多， 但基于激光的 SLAM 總結工作比較少， 且主要是基于 2D 激光， 涉及 3D 激光雷達SLAM 算法的介紹不多。在本文中， 我們將系統的介紹和分析三維激光 SLAM 技術， 并對選取的代表性算法進行性能比較及客觀評價。這項工作為我們之后對三維激光 SLAM 技術進行更深入的研究做下了鋪墊， 也希望本文能夠為其他對 3D 激光雷達 SLAM 技術感興趣的研究人員提供些許幫助。

1、3D 激光雷達 SLAM 方案

依賴激光雷達建立地圖的激光 SLAM 方案按求解方式可以分為基于濾波器和基于圖優化兩類， 基于濾波器的方法源于貝葉斯估計理論， 是早期解決 SLAM 問題的方法， 在室內或小范圍場景應用中具有不錯的效果， 但由于只考慮移動載體的當前位姿狀態和當前環境觀測信息， 且不具有回環檢測能力， 存在線性化以及更新效率低等問題[7-8]， 在程序運行中還會隨著場景的增大占用成倍增加的計算資源， 這使得它在室外大型場景中的表現效果比較差， 現階段基于濾波器的激光 SLAM 方案主要應用在二維室內小范圍場景。

基于圖優化的 SLAM 方案考慮了移動載體歷程中全部的位姿狀態和環境觀測信息， 用節點和邊形成的圖來表示一系列的移動機器人位姿和約束， 建立和維護關聯數據，可獨立出前端實現并行計算， 是一種更為高效和普適的優化方法。

相較于早期基于濾波器的 SLAM 方法， 通常可以得出全局一致性更好的地圖， 且隨著求解方法的不斷發展，在相同計算量的前提下， 圖優化 SLAM 的求解速度也已經超過濾波器方法， 是目前 SLAM 領域內的主流方法，也是三維激光 SLAM 采取的主要方案，Hauke 等[9]研究了圖優化方法為什么較濾波器方法能取得更優的效果。

Lu 等[10]于上世紀九十年代首次提出基于圖優化的2D SLAM 算法， 他們用帶約束的位姿網絡實現了數據關聯， 具備圖優化的原型。Gutmann 等[11]于 1999 年正式提出了圖優化框架， 該框架與目 前主流的圖優化框架大致相同， 具備前端掃描匹配、 全局優化以及閉環檢測模塊，但由于當時技術發展和認知局限， 沒有認識到系統的稀疏性， 并未實現實時 SLAM。之后， 國內外諸多研究學者也不斷探索， 為圖優化 SLAM 方法的發展做出了 巨大貢獻， 圖優化 SLAM 方案框架的各個模塊也在逐步完善。

基于二維激光的 SLAM 算法相對成熟， Santos 等[12]對 5 種 具有代表性的 基于單線 激光 雷 達的 2D 激光SLAM 算法進行了測試評估和總結， 結果表明 Gmapping和 KartoSLAM 算法在定位和建圖的準確性和效率上要更加優越。之后 Google 開源的 Cartographer 算法[13]

采用目前主流的基于圖優化的激光 SLAM 算法框架， 提出了分支定界的方法解決子地圖的構建以及與全局地圖的匹配問題， 實現了閉環檢測和較好效果的全局優化， 是目前較為先進和成熟的二維激光 SLAM 技術的代表。

二維激光雷達在確定高度的水平面上通過測量旋轉掃描的激光信號與其回波的時間差、 相位差確定環境中目標的距離和角度， 并依據這兩類數據在水平面上建立二維極坐標系來表示所感知的環境信息， 可視為一種單線程的三維激光雷達。

相比于只能感知環境中單個平面信息、適用于室內幾何結構實現小型區域地圖構建的二維激光雷達， 三維激光雷達可以進一步獲取高程上更豐富的信息， 對于室外大型場景也有更好的感知效果。

激光雷達根據線數可分為單線、4 /8 線以及 16 /32 /64 /128線 3 類， 隨著線束的增多， 激光雷達能夠感知環境的信息更豐富， 所得的數據量也相應更大， 設備的成本也成倍增加， 因此基于激光的 SLAM 算法需要在線束上有所考量，要達到更好的實時性就需要處理減少每幀的輸入數據量， 而較少的初始數據量因為線束稀疏又不能很好的反映環境信息。

目前的3D 激光雷達 SLAM 算法研究多基于16 /32 /64 線激光雷達， 而面向無人駕駛的應用則追求更高精度的 128 線， 如圖 1 所示。3D 激光雷達 SLAM 技術基于多線激光雷達， 沿用并發展了 基于圖優化的 SLAM算法框架， 并將其應用于無人駕駛等領域解決大型場景的定位與建圖問題。

1. 1 圖優化方法

圖優化 SLAM 的研究基礎是基于圖論， 圖(graph) 是一種數據結構， 由頂點(vertex) 與連接頂點的邊(edge) 組成表示為 G(V， E) ， 其中 G 表示圖， 頂點的集合表示為 V，邊的集合表示為 E， 其思想是用頂點表示事物， 而連接不同頂點之間的邊則用 于表示事物之間 的關系， 如果在圖 G 中存在一個頂點上連接兩個以上的邊， 則稱該圖為超圖， 在 SLAM 中研究的就是根據已有的觀測數據實現超圖的構建以及優化的過程。

假設移動載體的位姿節點用 μ = { μ 1， μ2 ， …， μ n } 表示， 將環境中的地標表示為 S = {S 1 ， S 2 ， …， S n } ， 則移動平臺的位姿和地標可以用圖 2 表示。

如果某時刻 k， 移動載體在位置通過激光傳感器進行掃描觀測得到數據， 則傳感器的觀測方程為:

由于系統參數和傳感器觀測存在誤差， 使得上式不可能精確相等， 因此誤差便存在，如果把:

式(2) 作為目 標函數， 把作為變量進行優化， 便可以求解得到移動載體位姿的估計值，， 從而計算估計出平臺移動的軌跡。具體到 SLAM 問題中， 頂點表示為激光雷達的位姿以及特征點的位姿， 而邊表示觀測方程， 觀測方程的表達形式有多種， 可定義為移動平臺不同位姿之間的約束， 也可以定義為移動平臺在某位置觀測得到的某空間點坐標表達式。

一般為位姿之間由里程計(odometry) 或者匹配(registration) 計算出的轉換矩陣， 這樣對移動平臺位姿的求解過程就轉化為求解圖中的優化問題。圖優化 SLAM 的模型表示形式也可以從彈簧能量模型[14-15]的視角來解釋， 如圖 3 所示， 在 SLAM 中是對位姿的最大似然估計， 彈簧模型中則是對應系統的最小能量狀態， 而二者的本質問題都可以轉換為非線性最小二乘問題。

1.2 圖優化 SLAM 方案框架

基于圖優化的 SLAM 方案可以分為掃描匹配、 閉環檢測、后端優化、點云地圖存儲表示 4 個部分。掃描匹配利用激光雷達、慣性測量單元(IMU) 及編碼器等傳感器數據進行掃描匹配， 利用相鄰幀之間的關系估計當前幀的運動姿態， 得到短時間內的相對位姿和地圖， 考慮的是局部數據關聯問題;

由于長時間的增量式掃描匹配會不可避免地造成誤差累積， 而閉環檢測可以通過比較當前幀與歷史關鍵幀來優化位姿， 檢查匹配確立節點間的約束關系， 減少全局地圖的漂移誤差， 考慮的是全局數據關聯; 如果是從基于圖優化的表示形式來看， 掃描匹配和閉環檢測都是為了根據觀測信息建立圖的節點以及節點間的約束， 即完成圖的構建。研究學者們將兩者一起劃分為圖優化 SLAM 的前端部分[14-15]。

由于系統參數誤差、觀測噪聲以及匹配誤差的存在，通過前端模塊所構建的位姿圖一致性較差， 且通常情況下構建圖的邊與邊的約束存在“沖突”。若用表示幀間匹配的相對變換矩陣， 則 T 0 ， T 1 ， T 2 ， …， T n 構成一個閉環， 在理想情況下應當滿足 T 0 T 1 T 2 …T n = I， 其中 I 表示單位矩陣， 但實際工程中通過前端得到的相對變換矩陣一般是達不到此結果的。與前端部分不同， 圖優化部分是對前端構建的圖信息進行非線性優化， 取得盡量滿足所有約束關系的最優解， 最后輸出姿態估計結果和全局點云地圖， 這一部分也被稱為 SLAM 后端， 與 SLAM 前端共同組成整個圖優化 SLAM 框架。

1.2 .1 掃描匹配

對于前端掃描匹配， 代表性的三維點云匹配算法大體可分為兩類: 基于匹配的方法和基于特征的方法。基于匹配的方法根據算法建立的目 標評價函數可以分為基于距離判斷和基于概率模型判斷兩種， 基于概率模型判斷的方法主要是正態分布變換( normalized distribution transform， NDT) 算法[16]， 基于距離判斷的方法主要是 ICP算法及其變種算法， 適合 3D 激光雷達 SLAM 的算法包括PP-ICP [17] 、NICP [18] 、 IMLS-ICP [19] 等， 其中代表性的算法是廣義迭代最近點 ( generalized iterative closest point，GICP)[20]， 算法原理是將 ICP 算法和 PL-ICP 算法結合到概率框架模型上進行點云配準， 提升了算法的適用性和精確度。

基于匹配的算法一般通過直接使用掃描點來實現準確估計， 需要使用大量的點進行穩定配準， 雖然匹配精度較高但通常計算效率不高。目前最新的能夠快速準確的實現三維激光點云匹配配準的算法是一種體素化的廣義迭代最近點算法[21]， 該方法通過體素化擴展了GICP 方法， 避免了高代價的最近鄰搜索， 同時保持了 算法的精度， 結合了類 ICP 和 NDT 算法的優勢。

基于特征的方法通過從掃描點云中提取特征點來提高計算效率， 包括使用角點和平面點特征的 LOAM [22] 以及使用面元特征的三維柵格匹配算法多分辨率柵格地圖(multi-resolution surfel map)[20]等， 也有許多專注于雷達點云特征描述進行點云配準的研究[23-27]， 如快速點特征直方圖 ( fast point feature histograms， FPFH )[23]、 VFH(viewpoint feature histogram)[24]等， 這種方法能夠改善計算成本提高實時性從而得到廣泛研究。一個經典的基于特征的點云配準算法通常包括關鍵點檢測、 特征描述符提取、真實匹配、異常值剔除和轉換估計幾個步驟。

1.2 .2 閉環檢測

閉環檢測基于全局數據關聯， 是實現魯棒 SLAM 的核心步驟， 通過識別是否到達歷史場景促使地圖閉環的能力， 能夠校正累積誤差， 從而產生全局一致性的映射地圖。但相應的， 錯誤閉環的結果則會嚴重影響后端優化的準確度， 甚至會直接導致最終地圖的效果不佳。閉環檢測的難點主要體現在:

1 ) 感知歧義。例如在長廊、 隧道、 樓梯等結構化十分相似的場景， 會加劇判斷難度。

2) 由于激光傳感器本身的稀疏性造成的觀測數據魯棒性和區分性受限問題， 即如何建立易于處理的環境有效表征方式。

3) 數據規模會隨著運行時間增加而導致需要判斷的幀數據不斷增長， 會降低建圖的實時性。

基于激光的場景識別致力于尋求一種有效而簡明的位置描述符， 目 前已有的 閉 環檢測 技術有基于 MonteCarlo 的節點搜索算法[28]， 也可依據 GPS 輔助法進行輔助閉環判斷; 有基于描述子的回環檢測算法， 通過提取局部或全局場景描述子進行場景識別， 局部描述子代表算法有 FPFH [23] : 利用 局部表面法向量計算局部描述子，Bosse 等[29]提出一種基于 Gasalt3D 描述符的概率投票方法， 由幾何信息和強度信息組成局部描述符 ISHOT [30] ;

全局描述子代表算法有將關鍵點對的相對幾何位置編碼成直方圖的 GLAROT 方法[31]， 將激光掃描投影到全局描述符的掃描上下文 Scan Context 方法[32]等， BoW(bag of words)[33]一種基于詞袋模型的場景識別算法， 還有 FAB-Map(fast appearance based mapping)[34]和 DBoW2 [35] 等方法， 但 這 些 方 法 起 初 是 被 用 于 視 覺 SLAM， 如 ORB SLAM [36] 和 LDSO [37] 。

1.2 .3后端圖優化

后端優化是將各幀雷達的位姿和幀間運動約束綜合起來達到整體優化的一個過程， 可以消除局部累計誤差，在大尺度的建圖中， 一般需要具備一個“監管者”來時刻協調之前的軌跡， 這便是 SLAM 的后端優化。

綜述[38-39]對優化方法已有系統的詳細介紹， 基于圖優化 SLAM 的后端優化方法可概括分為 4 類: 基于最小二乘法的優化方法、基于松弛迭代的優化方法、基于隨機梯度下降的優化方法以及基于流形迭代方法。目前基于圖優化的開源優化庫有 iSAM(incremental smoothing and mapping)[40]、GTSAM ( georgia tech smoothing and mapping)[41]、 G2O( general graph optimization )[42]、 Ceres [43] 、 BA ( bundle adjustment)[44]等， 借助于這些優化庫可節省后端迭代求解優化值的時間。

1.2 .4 地圖表示

通過 3D 激光雷達 SLAM 算法最容易得出的是點云地圖， 如圖 4(a) ， 它可以直觀描述周圍環境， 具備基本的環境展示功能， 但因點云數量龐大， 一般需要通過體素濾波進行一定的降采樣才能正常顯示。

此外由于單純的點云無法表示障礙物信息且不具備特征， 所以無法直接用于導航和避障以及基于特征的定位， 但可用于實現基于點云配準的定位研究。在點云地圖的基礎上進一步處理， 根據使用目的， 將從點云地圖中提取出的特征聚在一起可以構成使用特征進行定位的稀疏特征地圖， 如圖 4(c) 所示， 還可以構成用于導航避障和路徑規劃的占據網格地圖以及壓縮性能更好的八叉樹地圖， 如圖 4(b)所示。

占據網格地圖常采取柵格或者體素的方式， 在最小化柵格或者體素中用概率或者 0 ～ 1 的形式表示被占據的狀態， CSM [45]中構建了二維占據柵格地圖，OctoMap [46] 中提出了將點云地圖轉變為三維占據地圖的方法， 八叉樹地圖是一種特殊的占據柵格地圖， 該結構中占據概率相同的柵格可進行合并， 從而降低存儲地圖的空間。

近年來語義地圖的出現與發展也提高了機器對周圍環境感的內 容理解， 如圖 4(c) 所示， 語義地圖聯合幾何和內容兩個層次的感知， 幫助提高 SLAM 建圖和定位的精度， 也能更好的提升機器智能化理解環境的能力。

2、研究熱點及發展趨勢

激光雷達點云具有局部稀疏性、數據量大和動態對象會引起噪聲的特征， 這也成為了基于激光點云的SLAM 研究中的難點。與圖像匹配問題相比， 點云的稀疏性使得從源點云和目標點云中找到兩個精確匹配點通常是不可行的， 而且激光掃描儀從不同角度觀察到的同一物體的外觀差異很大， 這就增加了 特征提取的難度。

其次由于激光掃描儀每秒會產生數百萬個點， 需要高效的算法和強大的計算單元， 對于算法的實時性有著較高的要求。最后動態對象點云會引起噪聲干擾， 對其進行處理的方法是否得當對于能否實現理想的高精度估計也是至關重要的。此外， 場景的不斷變化被認為是解決 3D激光雷達 SLAM 問題的最大挑戰。

利用激光點云解決 SLAM 中三維地圖的構建問題，最早是在 2014 年由卡耐基梅隆大學的 Zhang 博士等提出的， 即著名的 LOAM(lidar odometry and mapping)[22]算法， 該算法以新穎的方法提取激光點云線面特征減少計算量， 并創造性的將運動估計問題分成兩個獨立算法來共同完成， 一個算法執行高頻率的里程計但是低精度的運動估計， 另一個算法運行頻率較低的匹配建圖但返回高精確的運動估計， 最終將兩個數據融合成高頻率高精度的運動估計， 很好的權衡了精度和效率， 實時性高， 唯一的不足點在于缺少回環檢測。

之后 LeGO-LOAM [47] 在LOAM 的基礎上衍生出新的框架， 在特征提取上提升輕量化和地面優化， 并增加了回環檢測， 整個框架與目前成熟的圖優化3D激光雷達SLAM 方案框架吻合， 構建的地圖更加完善。hdl_gragh_slam [48] 也是標準的圖優化 3D激光雷達 SLAM 框架， 新穎點在于后端優化過程中融合了 GPS、IMU 和路面約束信息， 可以更好的構建全局一致性地圖。隨著多線激光雷達的問世， 激光 SLAM 的研究學者們在三維激光 SLAM 領域開拓進展， 主要的研究熱點與趨勢也聚焦在圖優化 3D 激光雷達 SLAM 框架的關鍵模塊上。

2. 1 激光慣性里程計

激光里程計(LiDAR odometry， LO) 僅基于點云配準算法獲取當前幀的全局位姿， 但由于 Lidar 本身會受稀疏性和運動擾動影響而導致感知準確度降低， 以及在一些Lidar 退化情況下其感知點數量會嚴重減少， 這都會使激光里程計的精度受到較大程度影響。研究學者們通過融合 IMU 的數據來保證和提升激光里程計的精度。一種類別是松耦合方法， 該方法分別考慮 Lidar 和 IMU 的估計， 以損失信息為代價來換取較小的計算負載。

例如在LOAM 中依賴于 IMU 解算朝向作為輔助但假設勻速運動， 將 IMU 作為整個系統的先驗， 但它不能利用 IMU 測量用于進一步優化。

另一種類別是緊耦合方法， 通過 IMU 測量提供狀態預測， 同時測量更新被用于修正預測的狀態， 領域內也稱之為 激 光 慣 性 里 程 計 ( lidar inertial odometry， LIO ) ，LIO_mapping [49] 初次提出了 緊耦合的 Lidar-IMU 融合方法，通過聯合優化 IMU 和 Lidar 的測量數據， 即使在 Lidar退化的情況下也沒有明顯漂移， 相較于松耦合的方法， 緊耦合的方法會使精度大幅提升。

Liom 提出一種新的緊耦合激光慣性里程計和建圖方法來達到低漂移和魯棒的位姿估計，激光慣性里程計模塊采用誤差狀態卡爾曼濾波器(ESKF) 來實現激光掃 描儀和 IMU 的 傳感器融合[50]。

MC2SLAM 提出一種新的緊耦合方法， 把點云畸變補償和點云匹配統一到一個優化任務中，并在后端使用 IMU 預積分進行位姿優化， 精度被進一步提高

[51]。LIO-SAM 作為LeGO-SLAM 的作者，在圖優化框架的基礎上實現緊耦合激光 - 慣性里程計， 采用的是因子圖而非濾波方式進行優化[52]。

激光里程計模塊作為整個3D 激光雷達 SLAM 方案的基礎模塊， 可直接進行位姿估計與構建地圖， 在理想情況下該模塊的輸出結果可直接導出使用， 其精度直接影響最后的結果，因此找到一種高效通用的方法實現激光和慣導數據的緊耦合并提升精度仍是未來的研究重點。

2. 2 場景識別

場景識別是 SLAM 的關鍵任務之一。場景識別具有兩個功能， 一是用于歷史場景的識別， 二是用于相對于存在地圖的位姿估計即重定位。Dube 等[53]首次提出基于分割的 3D 點云場景識別方法 SegMatch， 把傳統基于局部特征和全局特征進行場景識別的方法進行了 折中，能夠實現實時、 可靠的檢測閉環。Segmap 方法[54]采用與 Segmatch 相同的基于分割的思想， 提出 了 一種基于深度學習的 3D 點云分割描述子， 提升了 定位性能且能夠提取語義信息。

SUMA 方法[55]使用 Surfel 地圖 高效地生成投影數據關聯并實現閉環檢測， 此前 Surfel 地圖曾被用在 RGBD-SLAM 中， SUMA 將其首次應用于激光SLAM 中， 該方法只基于激光點云就可以建立大尺度環境下的全局一致性地圖。

DELIGHT 僅依靠激光雷達實現全局定位， 采用 雷達獲取的反射強度信息而非常規的距離信息進行場景識別， 算法分為兩個階段， 包含一個基于密度描述子的 先驗估計和 一個基于幾何的驗證， 從而解決了“機器人喚醒”問題[56]。

ISC 方法[57]是基于激光雷達點云的全局回環檢測算法， 利用點云的幾何信息和密度信息構建新型全局描述子 ISC。場景識別的基本能力 是能夠有效地描述地圖 信息， 因此地圖描述的高效性和魯棒性至關重要， 能夠增加機器人理解環境的能力， 幾何信息與語義信息結合的描述形式將是未來的一個可能趨勢。

2. 3 后端優化

如何高效的通過后端優化來修正運動位姿和提高構建地圖的精度也是研究關注的熱點之一。C-T SLAM [58]是一種分層、連續時間的 3D 激光雷達 SLAM 算法， 采用高效的局部地圖和分層的優化后端， 允許實時建圖期間優化修正。BALM 將視覺 SLAM 領域中取得極大成功的BA 框架引入激光建圖模塊， 以此降低建圖過程中的累計誤差[59]。

LIO-SAM 的[52]后端優化采用因子圖優化， 融合激光里程計因子、IMU 預積分因子、GPS 因子和回環因子， 得到更魯棒的運動估計。現有的后端優化都是建立在正確的數據關聯基礎上的， 否則會產生錯誤的地圖結果， 這將使得后端優化的使用性受到限制， 因此能夠自動甄別并刪除錯誤的關聯數據， 實現在存有錯誤約束數據的情況下仍能構建正確地圖的魯棒性后端優化方法也是未來的發展趨勢。

2. 4 動態物體

在動態變化的環境中實現準確的定位是一件困難的事情， 大多數激光 SLAM 方法假設環境是靜態的， 因為這些方法依賴激光提供的測量， 它們很難處理物體劇烈運動導致環境幾何退化的問 題。因此如何在高速運動場景中實現魯棒性的位姿估計亦是研究的熱點之一。

SuMa + +[60]基于 SUMA 框架提出， 利用每幀點云中逐點的語義標號構建全局語義地圖， 利用 RangeNet + +可靠濾除場景中的動 態物體， 提高構 建地圖 的 精度。LIO-LiDAR [61] 融合激光慣性里程計(LIO) 和 LiDAR 全局定位模塊到一個位姿圖優化框架中， 利用二者的互補性解決動態場景中長期定位的精準問題。

LIOM 考慮到動態物體在環境地圖構建過程中的幾何退化， 不利于構建大尺度環境， 設計動態物體檢測模塊用于檢測和移除每幀的動態物體擾動產生的點云[50]。但 LIOM 一次性處理所有測量， 因此不能達到較高的實時性。

2. 5 結合深度學習

為了有效地利用所有可用的掃描數據， 基于深度學習的方法為直接從激光雷達數據估計機器人的姿態提供了潛在的解決方案。類似的方法已經成功地應用于相機數據， 并證明了有希望的結果[62]。在使用激光雷達數據進行基于學習的機器人姿態估計領域， 將深度學習用于解決 SLAM 問題也逐漸成為趨勢， 研究人員開始利用深度學習方法對 3D 激光雷達 SLAM 框架中的關鍵模塊進行改進， 如 LO-Net [63] 、 DMLO [64] 、 DeepLO [65]等針對激光里程計任務設計了基于深度學習框架的特征匹配估計;百度團隊提出的 DeepICP [66] 端到端 3D 點云配準深度學習框架， 考慮到動態對象的干擾， 充分利用靜態對象的顯著特點， 從而達到高魯棒性; SUMA + +[60]中也利用RangeNet + + 網絡濾除場景中的動態物體。

在閉環檢測模塊中利用深度神經網絡設計 OverlapNet [67] ， 該方法基于 LiDAR 數據的不同線索搜尋回路閉合， 基于 LiDAR 數據的不同維度信息搜尋回路閉合， 將里程計結果結合范圍、法向量、強度和語義等信息定義重疊率， 對閉環結果進行檢測、修正。

除以上研究熱點外， 點云匹配算法的優化也在不斷創新， 如 Fossel 等[68]提出 一個 LiDAR SLAM 前端， 稱之為 NOctoSLAM。該方案使用點面 ICP進行幀圖位姿匹配， 并使用 octree 替代傳統的 kd-tree 構建地圖， 從而可以快速地尋找數據關聯和計算近似法向量。與傳統的基于kd-tree 地圖的點面 ICP 算法相比， 效率提高了 近兩倍。

文獻[69-70] 考慮到室內較于室外多結構化特征環境的特點， 提出面向室內 環境的激光慣性里程計方法和點云平面特征提取方法。除此之外， 研究人員還考慮到地圖的長期重復定位問題， 如 Egger 等[71]基于 3D 點云特征提出了一種新的點云地圖表示方式 PoseMap， 保證了在動態環境中機器人可靠的長期定位， 新的地圖表示方法允許地圖在線更新擴展。

3、算法評估測試

3. 1 數據集

研究應用 3D 激光雷達 SLAM 的移動載體有無人車、無人船以及無人機， 而目前的激光點云數據集主要是針對自動駕駛場景， 室外場景下的數據收集工作規模大而繁瑣， 涉及各種傳感器間的時間同步、 坐標校準和標定等， 公開數據集節省了算法研究的數據準備工作， 提供的序列和基準框架也有利于算法開發。

目前領域內公開的激光點云數據集有: KITTI 數據集[72-73]，是目前國際上最大的自 動駕駛場景下的評測數據集，也是學術界最常使用的數據集; Waymo 數據集[74]， 自動駕駛公司 Waymo 的數據開放項目 ; PandaSet 數 據集[75]， 采集場景位于舊金山， 用以開發復雜環境和極端天氣下安全可控的自動駕駛技術; Oxford Robotcar 數據集[76]， 由牛津大學機器人實驗室提出 的 公開數據集;

USVInland 數據集[77]， 仿照 KITTI 數據集模式， 清華大學與西北工業大學的研究人員 聯合歐卡智舶公開的一組內陸水道中 無人船的多傳感器數據集， 也是全球第一個內河場景下包含激光點云數據的無人船數據集。而針對無人機平臺的激光點云公開數據集目 前尚未出現， 學術界研究仍多是自收集數據。

3. 2 算法性能評估標準

近年來學術界有關 3D 激光雷達 SLAM 算法的研究幾乎都是在 KITTI 數據集上進行的[47，51-52，55，57-67]， 該數據集是汽車在具有動態對象和植被的各種道路環境中駕駛期間捕獲的， 例如高速公路、 鄉村道路和城市區域， 行駛速度高達 90 km/h。由車載 Velodyne HDL-64ES2 捕獲的點云已經完成去偏移處理， 此外數據包中也包含有 IMU、GPS 以及圖像數據。

因此， 該數據集被允許對任何由 3D 激光雷達 SLAM 方法獲得的軌跡進行評估， 可以被直接饋送到所提出 的方法中 進行評估比較。對于所提出的算法性能一般基于以下 3 個標準進行性能評估:

1) KITTI 官方標準[72-73]:

KITTI 里程計基準總共包括 22 個序列數據， 官方只提供 11(序列號為 00-10) 個帶有真實軌跡的數據集， 另外 11 (序列號為 11-21) 個沒有真實軌跡的數據集序列用于 KITTI 官方的評估對比。評估方式為計算數據集中相對于真實軌跡不同長度(100 ～800 m) 的平移誤差和旋轉誤差， 并計算平均值。

2) 精度指標: 相 對 位 姿 誤 差 ( relative pose error，RPE)[78]用于描述相隔一定時間 差的兩幀位姿差的精度， 即在用時間戳對齊之后， 每隔一段相同時間分別計算真實位姿和估計位姿的變化量， 然后對兩者做差， 從而獲得相對位姿誤差， 之后可以用均方根誤差 RMSE 統計各段時間的相對位姿誤差從而獲得總體值; 絕對軌跡誤差(absolute trajectory error， ATE)[78]描述的是估計位姿和真實位姿之間的直接差值， 可以非常直觀地反映算法精度和軌跡的全局一致性。

3) 算法耗時和處理幀率: 作為計算效率的指標， 統計了處理 KITTI 里程數據集的序列所花費的時間并計算出幀率， 能夠直觀反映計算負載和實時性能。

除 KITTI 數據集等公開數據集以外， 也可以使用通過多線激光雷達錄制的數據包， 依據精度指標進行激光SLAM 算法的性能評估。

3. 3 算法性能評估實驗

本文選取了 6 種目前開源的3D 激光雷達 SLAM 算法進行測試與評估， 如表 1 所示。我們將其應用于機器人操作系統(robot operating system， ROS) 中， 所有的算法都在基于 KITTI 公開數據集基準的實驗中進行了評估和比較。為了評價所測試算法的性能， 在相同的條件下收集實驗結果， 并進行性能度量。

我們將序列號為 07 的數據輸入選取的各算法， 得到構建的點云地圖。圖 5 所示為其中具有代表性的 5 種算法所構建的點云地圖， SUMA 算法構建的點云地圖效果基本相同， 可以看到， 僅憑地圖構建效果無法判斷算法在性能上的差異， 僅能在圖 5(e) 判斷 Cartographer 算法在建圖結果上稍有遜色， 圖 5(e) 中圓圈 A 代表為同一部分的不同視角， 沒有形成閉環， 因此我們需采用 3. 2 節所述算法性能評估標準進行評估。

1) KITTI 官方標準

表 2 列舉了 KITTI 數據集中序列號分別為 00、05 和07 的數據測評結果， 該表中數據來源于 LITAMIN2 [79] 其作者使用 的實驗設備為 具有 32 GB RAM 的 Intel Corei9-9900K和 NVIDIA GeForce RTX 2080 Ti 的 臺 式 機。

KITTI 官方也提供了專門用于數據平移誤差和旋轉誤差的精度測評工具 KITTI_odometry_evaluation_tool， 其測評效果如圖 6 所示。

2) 精度指標

本文借助于 evo 測評工具進行測試實驗， 主要通過精度指標 RPE 以及 ATE 對算法性能評估。實驗硬件設備為具有 16 GB RAM 的 Intel Core i7-10700 和 NVIDIA GeForce RTX 3070 的 臺 式 機， 系 統 環 境 為 基 于Ubuntu18. 04 操作系統的 ROS 系統。選取了 KITTI 數據集中序列號為 00、05 和 07 的數據進行算法的測試評估。

如圖 7 所示， 對比的是 00 序列數據五種算法最終輸出的軌跡和位姿結果， 其中 KITTI_00_gt 虛線表示的是該序列的軌跡真值。從圖 7(a) 中 y 方向可以較為直觀的得出 hdl_graph_slam 和 LIO-SAM 算法的輸出結果最貼近真實軌跡， 說明該算法較其他算法有更加精確的估計軌跡。

圖 7(b) 中可以看出姿態角每個算法輸出相較于真實值各不相同， 但從俯仰角(pitch) 也可以看出 hdl_graph_slam 和 LIO-SAM 算法的輸出結果最貼近真實值。如圖 8 所示， 通過 evo 工具得到的 LeGO-LOAM 算法輸出軌跡平移誤差的 ATE、 RPE 結果， 圖中的 APE 等同于 ATE。表 3 中統計了除 Cartographer 算法 00 序列 5 種算法平移誤差 ATE、 RPE 的 rmse 數據， 表中數據也映證了 LIO-SAM 算法較于其他算法在軌跡誤差上的優越性。

3) 算法耗時和處理幀率

本文測試實驗對 5 種算法的耗時和處理幀率進行了統計， 表 4 數據為本文實驗統計數據， 硬件設備為16 GB RAM 的 Intel Core i7-10700 和 NVIDIA GeForceRTX3070。

表 4 數據可以看到， Lego-LOAM 算法在耗時和處理幀率上表現最佳， SUMA 算法緊隨其后， LIO-SAM 算法也有著不凡的表現， LOAM 和 hdl_graph_slam 兩種算法表現一般， 而 Cartographer 算法相較其他算法的實時性差距過大， 在實時建圖需求下并非良好的選擇。

本測試實驗中， 除了 Cartographer 算法其他算法均實現了實時性， 甚至可以加速處理。依據表 3 和表 4 的測試數據， Lego-LOAM 雖然優化 LOAM 算法框架實現了計算上的輕量化， 使得算法在計算效率上大幅提升， 但在精度方面卻有所欠缺; 所選算法中 LIO-SAM 算法在精度方面展現出優越性的同時， 實時性也得到了保障， 在所選取的算法中表現優越; Cartographer 算法雖然在室內 2D 建圖上性能較為優越， 但在室外大范圍環境中實現 3D 建圖則捉襟見肘。

4、技術應用趨勢分析

3D 激光雷達 SLAM 技術近年來的蓬勃發展是有目共睹的， 其核心課題是同步定位與地圖創建。我們針對3D 激光雷達 SLAM 技術應用領域， 運用 Citespace 進行科學知識圖譜繪制， 從而研究技術應用趨勢。我們分別從CNKI 學術期刊庫和 WOS 核心合集檢索到文獻的關鍵詞共現分析結果， 圖 9 展示了檢索文獻的關鍵詞聯合突現分析結果， 針對 2000 ～2021 年的論文， 其中 CNKI 檢索主題詞為“3D 激光雷達 SLAM”， WOS 檢索主題詞為“3D Lidar SLAM”， 再對文獻進行領域相關篩選， 最終得到與3D 激光雷達 SLAM 技術應用領域密切相關的 207 條中文文獻和 346 條英文文獻。從共現結果可以看到， 目前應用領域主要包括移動機器人領域和測繪領域。

移動機器人領域: 當下服務型機器人已經成為 3D 激光雷達 SLAM 技術的熱門應用場景， 主要有物流配送機器人，導游機器人，巡檢機器人等。從原理上來說， 無人駕駛汽車也屬于移動機器人，目前無人駕駛領域的裝備競演愈發火熱，隨著城市物聯網和智能系統的完善， 無人駕駛已是大勢所趨。無人船、無人機移動載體能夠規避障礙物、路徑規劃，實現自主智能化也需要 SLAM 技術的支持。針對單個平臺激光雷達建圖周期長、算力需求大的現狀， 研究多車協同建圖方法， 增加平臺間的地圖重用性， 提高建圖效率，實現有效負載均衡是未來發展的趨勢。

測繪領域: 高效實時地獲取 3D 精細化模型， 對目標區域進行快速三維重建是測繪領域的核心課題，3D 激光雷達 SLAM 技術使得實時快速重建成為可能， 現下已成功應用于各種移動測繪平臺， 如移動背包 SLAM、 手持SLAM 掃 描 系 統、 推 車 SLAM、 無 人 機 SLAM、 無 人 船SLAM 等， 應用場景有室內測繪、礦井測繪、林業測量、工地測量， 海洋平臺測繪等。

由于測繪需求的高精度， 多數移動測繪平臺無法分配更多的計算資源用于后端優化構建， 一般采用離線處理或將實時數據上傳到高性能服務器上進行計算， 從而取得高精度重建結果。在保證精度的前提下， 如何改進優化算法提高數據處理速度實現實時性， 如何克服大量相似場景間的誤匹配， 仍是未來探索的方向和難點。

結 論

3D 激光雷達 SLAM 算法在傳統關鍵模塊上仍需實現更好的輕量化、 精確性、 魯棒性以及通用化， 語義地圖和深度學習的融入已成為趨勢， 與其他能實現自 主定位源的傳感器如深度相機、 毫米波雷達等進行多源融合亦是當前研究熱點，3D 激光雷達 SLAM 技術對無人平臺實現自主智能化的發展必將產生深遠影響。

本文依據梳理的算法性能評估標準對開源算法進行了測試實驗， 并初步得出評估結論， 而由于算法的開源性問題， 如 LOAM 算法原項目 未開源， 以及 hdl_graph_slam和 LIO-SAM 算法融入了 GPS 數據， 受實驗條件和開源算法輸入限制， 本文僅利用點云和 IMU 數據進行測試， 實驗本身還具有一定缺陷， 無法對各算法進行更加深入的測試評估， 在未來工作中我們將嘗試融入多源數據， 進行更加全面且深刻的研究。

原文標題：3D 激光雷達 SLAM 算法綜述

文章出處：【微信公眾號：機器視覺智能檢測】歡迎添加關注!文章轉載請注明出處。

審核編輯：湯梓紅