基于2D激光雷達的SLAM算法研究綜述

沈斯杰，田 昕，魏國亮，袁千賀

(1.上海理工大學 光電信息與計算機工程學院，上海 200093；2.上海理工大學 理學院，上海 200093)

0 引 言

隨著人口老齡化日益嚴重和用人成本的上升，移動機器人在生活中出現的頻率越來越高，如高端酒店中的迎賓機器人、疫情期間醫院中的消毒機器人和搶險救災的救援機器人等。在移動機器人眾多應用場景中，周圍環境不是一成不變的，有時候甚至是未知的。作為移動機器人在未知環境下實現自主導航的關鍵技術，SLAM因此顯得尤為重要。

SLAM可以分為兩大類：基于相機的視覺SLAM和基于激光雷達的SLAM。視覺SLAM對光的依賴度高，在暗處或無紋理區域運行效果較差，而激光SLAM可以在較差的光照條件下運行，并且配合其他傳感器可以直接生成用于機器人導航的環境地圖。

激光SLAM按適用空間可以分為2D激光SLAM和3D激光SLAM。2D激光SLAM是在二維平面內使用的SLAM方法，通過激光傳感器獲取機器人三個自由度的位姿信息及機器人所處的二維地圖。同理，3D激光SLAM是在三維空間中使用的SLAM方法，它可以得到六個自由度的位姿信息及三維地圖。由于2D激光雷達在成本方面要低于3D激光雷達，并且在室內環境下，2D激光SLAM的建圖精度滿足使用需求，所以在移動機器人領域得到了廣泛的應用。因此，該文重點闡述了激光雷達的測距原理和2D激光SLAM的系統框架，并對主流的2D激光SLAM算法進行分析和比較。最后展望未來激光SLAM的發展方向。

1 激光雷達的測距方法

激光雷達作為移動機器人感知周圍環境信息的主要工具之一，在機器人避障中起著十分重要的作用。目前，激光雷達主流的測距方法可以分為三角法與飛行時間法(time of flight)。

1.1 三角法

激光以一定的角度射向被測物體，反射光以一定的角度反射回光源旁的接收器，因為激光器和接收器間隔一段距離，所以不同距離的被測物體在接收器上的成像會在不同的位置。通過三角公式就可以計算出被測物體的距離，三角測距原理如圖1所示。

圖1 三角測距原理

1.2 飛行時間法

激光器發射一個激光脈沖，通過激光雷達中的計時器記錄下發射的時間，反射光由接收器接收，通過計時器得到接收的時間。飛行時間通過接收時間與發射時間相減得到。由于光速已知，因此就可以計算出被測物體的距離，飛行時間測距原理如圖2所示。

圖2 飛行時間測距原理

從測量距離、測量精度、抗強光能力和成本四個方面對三角法與飛行時間法進行比較，如表1所示。

表1 三角法與飛行時間法比較

2 2D激光SLAM的系統框架

2D激光SLAM的系統框架主要可以分為：前端掃描匹配、后端優化、回環檢測和地圖構建。其中，回環檢測不是必須存在的環節。前端掃描匹配是通過讀取相鄰兩幀激光傳感器掃描信息之間的關系來估計自身的位姿。后端優化是用來減少掃描匹配后產生的累積誤差，并利用地圖構建模塊生成環境地圖信息。回環檢測則是通過檢測當前位置的估計值和歷史位置的估計值是否相同，來消除累積誤差，從而減少地圖漂移的現象。

2.1 前端掃描匹配

前端掃描匹配是實現同時定位與建圖的基礎。它可以分為：基于點的掃描匹配、基于特征的掃描匹配、基于數學特性的掃描匹配、基于相關性的掃描匹配和基于優化方法的掃描匹配。

2.1.1 基于點的掃描匹配

基于點的掃描匹配可以認為是對前后兩幀激光點集進行匹配。其中，迭代最近點(ICP)的變種算法——PLICP(point-to-line ICP)，由于其原理簡單、易于實現被廣泛使用。PLICP與經典ICP相比，通過將點與點之間的距離改變成點到線之間的距離，從而加快求取閉合解的速度以及提高匹配精度。

2.1.2 基于特征的掃描匹配

基于特征的掃描匹配方法，其思想就是從激光掃描信息中提取點、線、面等元素進行匹配。Zhao等人從掃描數據中提取隱函數作為特征，在具有閉合圖形的場合下，具有優異的效果。針對傳統的平面掃描匹配方法缺乏足夠的邊緣和角點特征，Zhao等人提出基于二次曲線特征的匹配方法。

2.1.3 基于數學特性的掃描匹配

此類算法的核心是通過各種數學特性來描述幀與幀之間的位姿變化。正態分布變換(NDT)是基于數學特性的匹配算法中最為典型的。該算法將當前幀掃描到的2D平面離散為柵格，根據每個柵格上點的分布，計算出該柵格的概率密度函數。通過牛頓的優化方法求解出概率密度之和最大的變換參數，從而達到最優匹配結果。

2.1.4 基于相關性的掃描匹配

相關性掃描匹配由Olson提出，首先構造似然場模型，然后在指定的搜索空間內進行搜索，計算每個位姿的得分，最后根據位姿得分，計算本輪位姿匹配的方差。由于該方法使用暴力搜索，因此需要較長的匹配時間。不過，該方法可以結合分枝定界策略加速匹配。

2.1.5 基于優化方法的掃描匹配

將掃描匹配問題轉換為非線性最小二乘問題，通過梯度下降的方式來求解，因此需要選擇合適的初始位置，否則容易陷入局部極值。

目前，在2D激光SLAM中，相關性掃描匹配結合梯度優化是最主流的前端掃描匹配方法。

2.2 后端優化

SLAM中的后端優化環節主要是減少掃描匹配所產生的累積誤差。目前，可以分為兩大類：基于貝葉斯濾波的后端優化方法和基于圖優化的后端優化方法。

2.2.1 基于貝葉斯濾波器的后端優化

基于貝葉斯濾波的SLAM方法都是在馬爾可夫假設的前提下進行的，即當前時刻機器人的狀態只與當前時刻的測量值和上一時刻的狀態相關，與其他時刻的無關。根據后驗概率表示方法的不同，可以分為卡爾曼濾波和粒子濾波。濾波類方法最大的缺點就是無法建立大尺度地圖，因為一旦當前時刻的估計出現偏差，偏差就無法消除，從而導致所建立的地圖有錯位的情況。

2.2.2 基于圖優化的后端優化

基于圖優化的SLAM方法則沒有馬爾可夫假設，當前時刻機器人的狀態與之前所有的測量值都相關，圖優化的系統框架如圖3所示。所謂圖優化，就是將圖論的思想引入到SLAM的問題中。它將機器人的位姿表示成一個節點，相鄰節點之間的弧表示空間約束關系，以此構成的圖就是位姿圖。通過調整位姿圖中的節點以最大程度滿足空間約束關系，從而獲得機器人的位姿信息和地圖。

圖3 圖優化的系統框架

2.3 回環檢測

回環檢測是指機器人通過傳感器測量的數據檢測到自己又回到之前來過的位置。這里可以借助控制科學中經典的反饋控制理論，來理解閉環檢測在SLAM中的作用，消除累積誤差，從而改善建圖效果。

2.3.1 幀與幀(scan-to-scan)的回環檢測

Scan-to-scan是利用Olson所提出的相關性掃描匹配，將兩幀激光雷達數據通過相對平移和旋轉角度進行匹配，從而達到回環檢測的目的。然而，因為單幀激光數據包含的信息量太少，容易出現匹配失敗的情況。

2.3.2 幀與子圖(scan-to-map)的回環檢測

將當前幀的激光雷達數據與子圖進行匹配，該子圖由一段時間內連續的激光數據幀構成。其中最具代表性的是于2016年google提出的cartographer算法，通過scan-to-map的回環檢測方法，減少連續多幀激光雷達數據之間的冗余性，提高回環的效率。

2.3.3 子圖與子圖(map-to-map)的回環檢測

Map-to-map就是將當前連續時間內的激光數據幀構建成子圖，與之前生成的子圖進行匹配，從而改善單幀激光數據所含信息量少的問題。文國成所提出的map-to-map的回環檢測方法，使用定位篩選和壓縮表對回環候選集進行優化，在構建大尺度地圖時，可以加快回環速度，降低回環出錯的可能性。

2.3.4 特殊的回環檢測

Olson提出一種基于多分辨率掃描匹配的方法，將當前激光數據幀和多分辨率地圖進行匹配。該方法即使在位置不確定的場合下，也能通過枚舉法找到最佳匹配。Yin等人提出一種基于孿生神經網絡的激光雷達點云半手工表示學習方法，在此基礎上，進一步建立KD樹，加速回環檢測。

目前，在2D激光SLAM 領域中，使用最多的回環檢測方法是幀與子圖之間的匹配，利用分枝定界法加速回環并且通過延時決策(lazy decision)降低回環出錯的可能性。

2.4 地圖構建

SLAM所構建的地圖是移動機器人實現自主導航與定位的前提與基礎。地圖可以分為：尺度地圖、拓撲地圖和語義地圖。在2D激光SLAM中，主要針對尺度地圖中的柵格地圖和特征地圖進行研究。

2.4.1 柵格地圖

柵格地圖是尺度地圖中最為常用的一種。柵格地圖是指將環境空間劃分成一個個大小相等的柵格單元，每個柵格單元中含有被障礙物占據的概率值。如果柵格被占用，則概率值越接近于1。當柵格中不含物體時，則概率值越接近于0。若不確定柵格中是否有物體，則概率值等于0.5。柵格地圖具有高準確性并且能夠充分反映出環境的結構特征，因此柵格地圖可以直接用于移動機器人的自主導航與定位。

2.4.2 特征地圖

特征地圖又稱為幾何地圖，一般由環境信息中提取到的點、線或圓弧等幾何特征構成。當構建小場景地圖時，特征地圖占用的資源較少且建圖精度尚可；當構建環境較大的地圖時，由于特征地圖無法有效表現出真實環境中的詳細信息，其建圖精度會大打折扣，因此不適用于大場景建圖。

3 主流的2D激光SLAM算法

主流的2D激光SLAM可以分為：基于貝葉斯濾波、基于圖優化和基于高斯牛頓優化的激光SLAM。

3.1 基于貝葉斯濾波的激光SLAM

擴展卡爾曼濾波(EKF)被用來解決SLAM問題最早可追溯到1987年，通過聯合狀態向量來同時表示機器人的位姿和路標所處的位置。雖然EKF SLAM開創了將概率統計運用于SLAM上的先河，但是此方法存在著一些問題，如所占用的計算資源較大、算法穩定性較差和所建立的特征地圖無法直接用于機器人的自主導航等。

基于粒子濾波器的激光SLAM方法對上述問題進行改善。2003年，Montemerlo等人提出Fast SLAM1.0算法，將SLAM問題分解成機器人位姿的估計和位置路標的估計，分別使用粒子濾波器和卡爾曼濾波器求解。但是粒子濾波器存在粒子退化的問題，為此Fast SLAM2.0算法提出一種求取重要性函數的方法。為了繼續優化Fast SLAM，Gmapping于2007年被提出，它通過改進提議分布和選擇性重采樣，進一步改善粒子退化的情況，建圖效果如圖4所示。

圖4 Gmapping算法

由于每個粒子都攜帶機器人的軌跡和對應的環境地圖，所以占用的計算資源較大。2010年提出的Core SLAM，是一種只有200行代碼的輕量化SLAM方法，因此性能損耗較小。

不管是基于卡爾曼濾波器還是粒子濾波器的SLAM算法，由于缺少回環檢測，因此在構建大尺度地圖時無法取得理想的效果。

3.2 基于圖優化的激光SLAM

圖優化的思想在SLAM中的使用最早可追溯到1997年，其通過最小二乘法來解決前端匹配所產生的累積誤差。由于沒有考慮到矩陣稀疏性，當時采用矩陣求逆的方法來求解，只能離線使用，無法滿足實時構建地圖的需求。1999年，Gutmann等人提出與當前相似的圖優化框架，但仍忽視矩陣稀疏性。千禧年以后，隨著研究者們的深入探索，SLAM中的矩陣稀疏性得以發現，使得高效求解成為可能。正是站在巨人的肩膀上，在2010年，Konolige等人提出Karto SLAM算法。這是首個基于圖優化的開源算法，利用高度優化和非迭代平方根法分解從而進行稀疏化解耦求解，建圖效果如圖5所示。但是該算法依然有不足之處，在回環檢測部分，需要預先構建局部子圖。

圖5 Karto算法

一般的圖優化方法都需要初始假設，即起始點經過多次迭代后得到局部代價函數的最小值。然而在2011年，Carlone等人提出一種近似線性化的圖優化SLAM方法。2016年，Google團隊開源了Cartographer算法，通過相關性掃描匹配結合梯度優化完成前端匹配，利用分枝定界法加速回環檢測以及結合延時決策(lazy decision)降低回環出錯的可能性，建圖效果如圖6所示。2020年，Li等人利用高斯過程(Gaussian process，GP)回歸，提出了一種基于區域化GP地圖重建算法的新型地圖表示方法。該方法可以采用簡潔的數學方法完成位姿估計和地圖更新。

圖6 Cartographer算法

3.3 基于高斯牛頓優化的激光SLAM

2011年，Kohlbrecher等人提出Hector SLAM算法，該算法無后端優化部分且不需要借助額外的傳感器，所以需要高更新頻率且測量噪聲小的激光雷達。Hector SLAM將當前幀的激光雷達數據與子圖進行匹配，通過高斯牛頓法進行位姿優化。為了避免陷入局部最優解，求得全局最優解，Hector SLAM采取多分辨率地圖的策略。此算法在旋轉過快時，建圖易發生漂移，建圖效果如圖7所示。

圖7 Hector算法

目前，主流的激光SLAM算法的優缺點分析如表2所示。

表2 激光SLAM算法的優缺點分析

4 激光SLAM的發展趨勢

激光SLAM的系統框架已經成熟，但是仍有很多工程應用問題有待解決，目前的發展趨勢有以下三種：

(1)多傳感器融合。

在復雜環境下，多傳感器融合是一種必然的趨勢。李陸君等人提出將二維激光雷達與深度相機融合，從而提高傳感器的抗干擾能力、探測范圍和建圖精度。陳志鍵等人針對室內定位中UWB存在非視距的影響以及激光雷達有誤差累積問題，將UWB絕對定位技術和激光雷達相對定位技術相融合，提高定位準確度。

(2)與深度學習的結合。

近些年，深度學習得到蓬勃的發展和廣泛的關注度。深度學習在特征提取和特征學習方面性能突出，不少研究者在SLAM中引入深度學習改善前端匹配與回環檢測的性能。Wang等人將深度學習用于激光里程計系統，提高預測位姿的魯棒性與準確度。鄒勤提出一種基于深度學習的激光SLAM回環檢測方法，將SLAM回環檢測的問題轉化為SLAM數據樣本的檢索問題。

(3)優化激光SLAM算法。

為了推動移動機器人在日常生活中的普及，需要進一步提升SLAM算法的穩定性和實時性，降低硬件成本。劉哲銘通過速度估計補償法和里程計插值模型去除激光雷達中的運動畸變，提升前端掃描匹配的精確度。Zhang等人提出一種新穎的序列數據處理流程，該方法有效減少了建圖漂移現象，提升了系統的魯棒性。Pang等人通過對激光掃描數據進行分割，提取出特殊的邊緣或角點特征，使其在低成本的設備下可以保持較高的建圖精度。

5 結束語

文中主要對2D激光SLAM的系統框架和發展現狀進行了概述，總結分析了主流2D激光SLAM算法，不難發現，基于圖優化的激光SLAM將是未來的主流。若要進一步提高SLAM算法的實時性、魯棒性和準確性，結合多傳感器融合與深度學習技術符合當下的發展趨勢。

移動機器人是人工智能的重要載體，而激光SLAM是機器人學中的關鍵技術，是實現機器人自主探索的基石。現如今激光SLAM技術的建圖精度和硬件成本仍呈正比關系，相信在未來，隨著激光SLAM技術的深入發展和硬件成本的降低，會出現低成本高精度的激光SLAM技術。到那時，越來越多的移動機器人會出現在人們的視野中，便利你我的生活。