激光即時定位與建圖算法綜述

劉志成，王華龍，馬興錄

(青島科技大學 信息科學技術學院，山東 青島 266061)

0 引言

激光即時定位與建圖(SLAM，simultaneous localization and mapping)是機器人領域中的關鍵技術，它可以利用激光雷達掃描環境并提取特征點，實現機器人的自主定位和地圖構建。近年來，隨著機器人技術和激光雷達技術的不斷發展，激光SLAM也得到了廣泛的研究和應用。

目前，國內外對于激光SLAM的研究主要集中在以下幾個方面：算法優化，硬件平臺，應用場景等[1]。在算法優化方面，研究人員不斷探索和改進各種激光SLAM算法，如基于優化的SLAM算法、基于點云配準的SLAM算法、基于學習的SLAM算法等，以提高算法的精度和魯棒性，并降低計算復雜度和硬件要求。在硬件平臺方面，研究人員致力于開發更加高效、精準和穩定的激光雷達，以及與之配套的機器人平臺。在應用場景方面，研究人員不斷探索和拓展激光SLAM在各種實際應用場景的應用，如室內導航、無人駕駛、智能物流等。

1 國外激光SLAM算法研究現狀與應用

隨著人工智能的發展，目前國外在激光SLAM方面的主要研究現狀有算法優化、多傳感器融合以及深度學習與SLAM等。激光SLAM主要應用在自動駕駛、機器人、AR/VR以及無人飛行器等方面。也有大量國外學者針對相關激光SLAM算法的缺點和不足進行了大量研究并且發表了很多優秀文章，例如，對于基于濾波器的SLAM算法，具有局限性，如計算復雜度高、粒子數目選擇、粒子退化等問題，在文獻[2-3]中通過對粒子濾波算法改進，有效地解決了粒子退化，提高了精度。文獻[4-5]中提出深度學習的濾波器算法，對非線性噪聲進行建模，提高了算法的精度和魯棒性。對于基于圖優化的SLAM算法，存在一些缺點有計算復雜性、實時性、局部最優解等，在文獻[6-9]中主要舉例UKF-SLAM算法，通過改進算法，有效地解決了計算復雜度高、粒子退化問題，并且對處理大規模環境時具有較好的效果 。文獻[10-11]舉例了Gmapping算法，通過對激光雷達數據中的特征點進行提取和匹配，減少了計算量和存儲空間，提高了算法的效率和存儲空間。對于基于配準的SLAM算法，缺點有對環境要求較高，在未知環境或者動態環境可能存在一定的局限性等，文獻[12-17]主要針對算法對環境不穩定和噪聲干擾等問題，提出了一系列方法，有效地降低噪聲和誤差的影響，使得地圖更加精確和魯棒。對于基于學習的SLAM算法，具有數據依賴性、過擬合以及解釋性差等缺點，文獻[18-21]主要采用了分類聚類算法對點云數據進行處理，減少點云數據的噪聲和不完整性，通過選取最有的聚類中心，提高了聚合器的效率和準確性。文獻[22-26]中研究學者分別提出了不同的深度強化學習的SLAM算法，有效地提高了算法的性能和效率，解決了過擬合等問題。

2 激光SLAM算法綜述

總的來說，激光SLAM的算法可以分為傳統的基于濾波器和基于圖優化的算法，以及近年來興起的基于點云配準和基于學習的算法。如圖1所示分類圖。

圖1 激光SLAM算法分類圖

2.1 基于濾波器的算法

基于濾波器的SLAM算法主要利用不同類型的濾波器對機器人的軌跡和地圖進行估計和優化?；跒V波器的算法一般有卡爾曼濾波器SLAM算法和粒子濾波器SLAM算法。

2.1.1 卡爾曼濾波器

卡爾曼濾波器是一種經典的線性高斯模型的狀態估計濾波器，由美國工程師R.E. Kalman于1960年提出[27]。該濾波器主要用于對系統狀態的估計和預測，其基本原理是通過對系統狀態和觀測數據進行加權平均，獲得更準確的狀態估計結果。在SLAM中的應用主要是對機器人的位姿和地圖進行估計和優化?；诳柭鼮V波器的SLAM算法包括EKF-SLAM和UKF-SLAM等，它們的基本原理都遵循卡爾曼濾波器的原理機制，即狀態的估計和預測，從而獲得更準確的位姿和地圖估計結果[28]。下面分別介紹。

基于擴展卡爾曼濾波器的SLAM算法(EKF-SLAM)有兩個階段：預測和更新。其算法如表1所示。

表1 擴展卡爾曼濾波器算法步驟

表1顯示了擴展卡爾曼濾波器的預測和更新步驟。在預測步驟中，我們首先使用狀態空間來估計狀態Xt。然后，我們使用之前在時間t-1處的協方差矩陣P獲得時間t處的狀態協方差矩陣P。在初始時刻我們沒有協方差矩陣，需要自己初始化它。EKF-SLAM算法的主要優點是能夠處理非線性和非高斯問題，具有較高的估計精度和實時性。缺點是非線性噪聲問題：EKF-SLAM算法假設傳感器噪聲為線性高斯噪聲，但實際上很多傳感器噪聲是非線性的，這會導致EKF-SLAM算法的精度下降；粒子退化問題：EKF-SLAM算法使用的是單個最優的估計值，因此在處理大規模環境時，容易出現粒子退化問題，影響算法的效率和精度。

基于無跡卡爾曼濾波器的SLAM算法(UKF-SLAM)主要思想是通過無跡變換將非線性函數變換成一個高斯分布，從而實現對非線性問題的處理。具體來說，UKF-SLAM算法將機器人的軌跡和地圖表示為一個聯合高斯分布，通過無跡變換對非線性函數進行線性化，從而實現對機器人軌跡和地圖的估計和優化。具體來說，UKF-SLAM算法包括以下步驟。

1)狀態預測：

① 通過機器人的運動模型和控制輸入，計算機器人的預測狀態。

② 通過無跡變換，將預測狀態映射到高斯分布上。

③ 計算高斯分布的均值和協方差矩陣，作為機器人的預測狀態

2)狀態更新：

① 通過機器人的觀測數據，計算機器人當前的觀測狀態。

② 通過無跡變換，將觀測狀態映射到高斯分布上。

③ 計算機器人位姿和地圖的聯合高斯分布的均值和協方差矩陣，作為機器人的最終狀態。

3)地圖優化：

① 使用非線性優化方法對地圖進行優化，以進一步提高估計精度。

② 通過無跡變換，將優化后的地圖狀態映射到高斯分布上。

③ 計算機器人位姿和地圖的聯合高斯分布的均值和協方差矩陣，作為機器人的最終狀態和地圖狀態。

UKF-SLAM算法是一種使用無跡卡爾曼濾波器(UKF)進行SLAM的方法，相比于EKF-SLAM，UKF-SLAM具有更好的非線性建模能力和更少的線性化誤差。然而，UKF-SLAM仍然存在一些缺點，如計算復雜度高、粒子退化等[6]。

2.1.2 粒子濾波器

粒子濾波SLAM(PF-SLAM)是一種基于蒙特卡羅方法的非參數濾波器，最初由Dieter Fox等人提出，發表于2003年的ICRA會議上，用于同時估計機器人的軌跡和環境地圖[29]。其主要思想是使用一組狀態粒子來估計機器人的位姿和環境地圖，在PF-SLAM中，每個狀態粒子表示機器人的一個假設位姿和地圖，它們的權重表示該假設與真實狀態的匹配程度。在預測和重采樣過程中，每個粒子的權重被用來決定其在新粒子集合中的數量，從而使更有可能的粒子數量增加，不太可能的粒子數量減少。這樣，通過多次迭代，可以逐漸減小估計誤差，實現對機器人軌跡和環境地圖的估計。如圖2是粒子濾波算法流程圖。常見的基于粒子濾波的SLAM算法有FastSLAM算法、Gmapping算法等。

圖2 粒子濾波算法流程圖

其中FastSLAM算法使用了分解式濾波的思想，將地圖分解為一系列特征，每個特征由一個粒子來描述，同時對于每個特征，都估計了其對應的位姿，從而實現對機器人軌跡和地圖的同時估計，大大降低了計算復雜度。

FastSLAM算法的基本流程如下：

1)初始化粒子集合和地圖：隨機生成一組粒子，每個粒子都表示機器人的位姿和地圖的狀態；

2)預測粒子狀態：根據運動模型對每個粒子進行預測，得到機器人的位姿；

3)更新粒子權重：根據觀測數據，計算每個粒子的權重，表示該粒子對應的狀態與觀測數據的匹配程度；

4)重采樣：根據粒子的權重對粒子進行重采樣，使得權重大的粒子被選中的概率更高；

5)更新地圖：對每個粒子的地圖進行更新，包括添加新的特征和更新已有特征的狀態。

FastSLAM算法的優點是具有良好的魯棒性和實時性，適用于多種不同的機器人和環境。由于采用了分解式濾波的思想，將地圖分解為一系列特征，極大地降低了計算復雜度，同時還可以處理非線性運動模型和非線性觀測模型。FastSLAM算法的缺點是對于大規模地圖的處理效果不如基于圖優化的SLAM算法，同時在處理復雜環境時容易出現粒子退化的問題，即大部分粒子的權重趨于零，導致估計精度下降。

Gmapping算法原理是使用柵格地圖來表示環境，并利用粒子濾波器來進行位姿估計和地圖構建。具體來說，Gmapping算法通過將環境劃分為一系列小的柵格單元，每個格柵單元一共有3種狀態：占有、空閑、未知。如圖3所示為格柵示意圖，在格柵中黑色表示占有，白色表示空閑，灰色表示未知。該算法通過激光雷達對周圍環境進行掃描，對檢測范圍內的網格根據占有概率將其判定為占有或空閑，通過機器人的移動對未知網格進行探索，并使用激光雷達數據進行障礙物檢測和柵格地圖更新直到地圖建立完成。同時，算法使用粒子濾波器對機器人的位姿進行估計，并根據估計的位姿和激光雷達數據來更新柵格地圖。最終，通過多次迭代，Gmapping算法可以得到高精度的柵格地圖和機器人的位姿估計。

圖3 格柵示意圖

Gmapping算法的優點包括可以處理復雜的環境，如室內環境、城市街道等?？梢詷嫿ǜ呔鹊臇鸥竦貓D，對機器人的定位能力提供了較好的支持。對激光雷達數據的噪聲和誤差具有較強的魯棒性。Gmapping算法的缺點包括需要進行大量的計算，特別是在處理大規模環境時，計算復雜度會急劇增加。依賴于激光雷達，對于沒有激光雷達的機器人來說不適用。

2.2 基于圖優化的算法

基于圖優化的算法是一種常用于SLAM問題的方法，最早出現在20世紀80年代，主要用于解決機器人路徑規劃和SLAM等問題[30-31]。其主要思想是將機器人在環境中的運動軌跡和環境的拓撲結構建模成一個圖，然后通過優化圖中的節點和邊來最小化誤差，得到機器人的位姿估計和環境的地圖。圖優化SLAM算法主要包括前端和后端兩個過程，其算法框架如圖4所示。前端負責數據關聯和閉環檢測，數據關聯主要來處理局部數據關系，解決連續數據幀間的匹配以及相關姿態估計的問題。閉環檢測主要針對全局數據關系，通過傳感器所獲得數據判斷機器人當前位姿與之前已訪問區域位姿之間的匹配及相對位姿估計問題。通過上述兩個過程完成位姿圖的創建，即圖優化SLAM的前端。由于傳感器與觀測噪聲和掃描匹配自身誤差的緣故會導致前端獲得的位姿圖有偏差，因此需要后端圖優化部分對位姿圖進行修正[32]。后端處理不直接對傳感器的觀測數據進行處理，而是僅對前端創建的位姿圖進行優化，得到位姿的最大似然估計即最優的位姿序列。常見的基于圖優化的SLAM算法有最小二乘法SLAM，因子圖優化算法，非線性優化算法等。

圖4 基于圖優化方法的SLAM算法框架

在基于圖優化SLAM算法中，機器人的位姿序列以及位姿間的約束關系可以轉化成如圖5所示。

圖5 機器人位姿轉化為圖的表示方法

在圖5中x=(xi，xj，...)T表示機器人的位姿序列，它是一個向量，在圖中表示點集，其中xi和xj分別代表i節點和j節點的位姿；zij表示i節點和j節點的相對位姿，即不同時刻下的關聯信息；Ωij代表節點i和節點j之間的信息矩陣；fij(x)為測量函數，表示在理想情況下的測量值；eij(x)代表向量誤差函數，即測量值與預測值的差。

假設誤差函數eij(x)服從高斯分布，則目標函數可表示為如下形式：

Fij(x)∝(fij(x)-zij)Ωij(fij(x)-zij)

(2)

Fij(x)∝eij(x)TΩijeij(x)

(3)

由上面的表達式可知，只需找到節點x*使得目標函數Fij(x)的值最小即可獲得最優解，即

x*=argminF(x)

(4)

2.3 基于配準的算法

基于配準的SLAM算法基本思想是通過匹配兩幀或多幀激光雷達或相機捕捉到的點云數據，估計每個時刻機器人的位姿并構建地圖[33]。該算法通常分為兩個步驟：前端配準和后端優化。前端配準是指對連續的點云數據進行匹配，以得到機器人的運動軌跡和局部地圖。后端優化則是指對前端得到的軌跡和地圖進行全局優化，以進一步提高位姿估計和地圖質量。常見的基于配準的激光SLAM方法分為基于特征的點云配準算法、基于全局優化的點云配準算法和基于位姿圖優化的點云配準算法。其中主流的SLAM算法就是Hector算法。

Hector-SLAM 主要原理是通過激光雷達數據獲取環境地圖，并同時估計機器人在地圖中的位置和姿態，實現自主導航和環境建圖。Hector整體算法很直接，就是將激光點與已有的地圖“對齊”，即掃描匹配。掃描匹配就是使用當前幀與已經有的地圖數據構建誤差函數，使用高斯牛頓法得到最優解和偏差量。其工作是實現激光點到柵格地圖的轉換，t時刻所有的激光點都能變換到柵格地圖中，也就意味著匹配成功。如表2所示，Hector-SLAM主要包括以下幾個步驟：

表2 Hector算法步驟

通過以上步驟，Hector-SLAM能夠在實時性和精度之間取得良好的平衡。Hector 算法具有以下優點：實時性較高，Hector SLAM算法利用線段作為路標，提高了匹配效率，同時使用EKF進行位姿估計，保證了實時性；精度較高，Hector SLAM算法在路標提取、位姿估計、閉環檢測和地圖優化等方面都有較好的設計，能夠提高地圖的精度和魯棒性。缺點有：對環境要求較高；魯棒性有待提高，算法對于激光雷達數據的噪聲和不確定性較為敏感，需要進一步提高算法的魯棒性。

2.4 基于學習的算法

基于學習的SLAM算法是指利用機器學習的方法，通過對傳感器數據和地圖數據的學習，實現環境建圖和自主導航的技術。這種方法可以從數據中自動提取特征，避免了手動設計特征的困難和不確定性，同時具有較好的泛化能力和魯棒性[34]?；趯W習的SLAM算法主要解決了傳統SLAM算法中存在的以下問題：特征設計問題，傳統SLAM算法需要手動設計特征，這存在一定的主觀性和不確定性，這增加了算法的難度和復雜度?；趯W習的SLAM算法可以從數據中自動學習特征，避免了手動設計特征的困難和不確定性；實時性問題，傳統SLAM算法需要大量的計算和存儲資源，對實時性的要求較高，基于學習的SLAM算法可以通過端到端的學習和高效的計算方法，提高算法的實時性和效率；建圖準確性問題，傳統SLAM算法在環境建圖中容易出現漏洞和誤差，基于學習的SLAM算法可以通過數據的學習和模型的優化，提高建圖的準確性和精度[18]。目前，基于學習的SLAM算法主要包括以下幾種：深度學習SLAM以及強化學習SLAM。

2.4.1 基于深度學習的SLAM算法

基于深度學習的激光SLAM算法主要利用深度學習技術來解決SLAM算法中的以下幾個問題：特征提取，傳統的SLAM算法需要手動設計特征點，但這些特征點的選擇和設計往往具有一定的主觀性和不確定性[35]。而基于深度學習的SLAM算法可以通過卷積神經網絡等對激光數據進行特征提取，自動提取出圖像和激光數據中的特征點、線段、平面等特征[36]；點云配準，傳統的SLAM算法中需要通過點云配準來實現環境建圖和定位，但這通常需要進行復雜的數學計算?；谏疃葘W習的SLAM算法可以通過學習大量數據，利用深度學習模型學習點云之間的相似性和差異性，從而實現點云的配準和匹配；地圖優化，基于深度學習的激光SLAM算法可以通過學習大量數據，利用深度學習模型學習地圖的結構和特征，從而實現地圖的自適應優化，提高地圖的準確性和魯棒性[37]。目前主流的幾個基于深度學習的激光SLAM算法有PointNetVLAD算法、PointNetGPD算法、DeepMapping算法以及LIO-SAM算法等。

2.4.2 基于強化學習的激光SLAM算法

基于強化學習的SLAM算法主要思想是將SLAM問題轉化為一個強化學習問題，并使用強化學習方法進行求解。最早是在2016年提出的，由芬蘭赫爾辛基大學的研究者首次將強化學習應用于SLAM問題中[38]。

該算法將SLAM問題看作一個強化學習問題，其中機器人作為一個智能體，通過與環境的交互來學習最優的行為策略，從而實現同時定位和地圖構建。具體來說，算法使用一個價值函數來評估機器人當前狀態和行動的價值，并通過一系列的行動來最大化累計回報，最終實現優化的定位和地圖構建結果。如表3所示基于強化學習的激光SLAM算法步驟如表3。

表3 基于強化學習的激光SLAM算法步驟

3 激光SLAM算法分析與應用

本文對目前主流的激光SLAM算法進行了簡單的歸納和綜述，并且對每一種SLAM算法列舉了其實現機制和原理，以及優缺點和改進等。這四類激光SLAM算法的聯系在于它們都是利用激光雷達數據來實現機器人的自我定位和地圖構建，但是具體實現方式和特點有所不同。四類算法相對于其他算法的改進主要表現在以下方面。

基于濾波器的激光SLAM算法：這類算法使用卡爾曼濾波器、擴展卡爾曼濾波器等濾波器來實現激光SLAM，具有計算速度快、適用于小規模場景等優點。但是，由于濾波器的線性化假設，這類算法對于非線性問題的處理效果不如基于優化的算法。常見的應用場景包括室內環境、小型機器人等。

基于圖優化的激光SLAM算法：這類算法使用非線性優化方法，如圖優化、束優化等，來精確估計機器人的位姿和地圖，能夠處理非線性問題和大規模場景。但是，這類算法計算復雜度較高，處理速度較慢。常見的應用場景包括無人車、大型機器人等。

基于配準的激光SLAM算法：這類算法使用點云配準方法來實現激光SLAM，能夠處理復雜地形和多層次地圖。但是，由于配準算法對初始位姿較為敏感，因此需要先驗信息或者較為準確的初始位姿。常見的應用場景包括室外環境、地下礦井等。

基于學習的激光SLAM算法：這類算法使用深度學習技術來實現激光SLAM，能夠自動學習特征表示，具有較強的泛化能力。但是，由于深度學習技術的數據需求量較大，這類算法需要大量的訓練數據。常見的應用場景包括室內環境、室外環境等。

不同的激光SLAM算法適用于不同的應用場景，需要根據具體場景來綜合考慮選擇哪種算法。

4 激光SLAM算法發展趨勢

本文綜述了移動機器人激光SLAM算法的原理、優缺點、研究現狀和改進。激光SLAM算法是機器人實現自主導航和環境感知的關鍵技術。隨著激光雷達技術和機器學習等技術的不斷發展，激光SLAM算法也在不斷更新和發展。以下是激光SLAM算法未來的發展趨勢和展望：

多傳感器融合：隨著各種傳感器技術的不斷發展和普及，多傳感器融合已經成為激光SLAM算法發展的趨勢之一，并且已經取得了不錯的成績，例如文獻[39-40]，傳感器融合可以提高定位精度和魯棒性。

深度學習技術應用：未來深度學習技術將被更廣泛地應用于激光SLAM算法中，例如文獻[35-37]、文獻[41]，將深度學習用于點云配準、建圖和定位等方面。此外，基于學習的激光SLAM算法已經取得了不俗的成績，未來也將會有更多的基于學習的算法被提出。

實時性和效率：隨著機器人應用場景的不斷擴大，對激光SLAM算法的實時性和效率的要求也越來越高。因此，未來的激光SLAM算法將更加注重算法的實時性和效率，并且會結合分布式計算、GPU加速等技術來提高算法的實時性和效率。

大規模場景處理：傳統的激光SLAM算法在大規模場景中往往面臨著精度不高、計算量大等問題，因此未來的激光SLAM算法將更加注重大規模場景的處理，如分布式SLAM、增量式SLAM等算法。此外，一些基于圖優化的方法、基于機器學習的方法也在被不斷地提出和改進。

自主決策和交互：未來的激光SLAM算法不僅僅是用于機器人自我定位和地圖構建，更會結合機器人的自主決策和交互能力，例如文獻[42]。如基于SLAM的路徑規劃、環境感知等。未來的激光SLAM算法將更加注重機器人與環境的交互和合作，如機器人對環境的理解和預測等。

總之，未來激光SLAM算法將更加注重多傳感器融合、深度學習技術應用、實時性和效率、大規模場景處理以及自主決策和交互等方面，這些發展趨勢將推動激光SLAM算法的不斷更新和發展[43-75]。

5 結束語

激光SLAM算法是移動機器人的關鍵技術，目前的激光SLAM算法具有一定的局限性，仍需要大量工作進行改進，提高算法的性能和實用性等?？傊す釹LAM算法的未來發展充滿了機遇和挑戰，期待著激光SLAM算法在未來的發展中不斷地創新和突破，為機器人應用帶來更加優秀和可靠的解決方案。