同步定位與地圖構建 （SLAM） 是自動駕駛汽車所用的一種技術，您不僅可以用它構建地圖，還可同時在該地圖上定位您的車輛。SLAM 算法讓汽車能夠構建未知環境的地圖。工程師們使用地圖信息執行路徑規劃和避障等任務。

1. SLAM 為何重要

早在多年前，人們就已開始對 SLAM 開展技術研究。如今，隨著計算機處理速度顯著提升，且相機和激光測距儀等低成本傳感器大為普及，SLAM 更是在越來越多的領域投入實際應用。

SLAM 為何重要？要回答這個問題，我們可以看看以下幾個例子，了解它有哪些好處和應用。

SLAM 示例

假設有一個家用掃地機器人。沒有 SLAM，它只會在房間里隨機移動，無法打掃整個地面空間。此外，這種方法會消耗更多功率，因此電池會更快耗盡。相反，采用 SLAM 的機器人可以使用滾輪轉數等信息以及來自相機和其他成像傳感器的數據，確定所需的移動量。這稱為定位。機器人還可以同步使用相機和其他傳感器創建其周圍障礙物的地圖，避免同一區域清潔兩次。這稱為建圖。

SLAM 給掃地機器人帶來的好處

SLAM 還可用于許多其他應用場景中，例如讓一隊移動機器人在倉庫中移動并整理貨架，讓自動駕駛汽車停泊到空車位，或者讓無人機在未知環境中完成送貨。MATLAB 和 Simulink 提供了 SLAM 算法、函數和分析工具來開發各種應用。您可以在實現同步定位與地圖構建的同時，完成傳感器融合、目標跟蹤、路徑規劃和路徑跟隨等其他任務。

2. SLAM 工作原理

大致說來，實現 SLAM 需要兩類技術。一類技術是傳感器信號處理（包括前端處理），這類技術在很大程度上取決于所用的傳感器。另一類技術是位姿圖優化（包括后端處理），這類技術與傳感器無關。

SLAM 處理流程

為了進一步了解前端處理技術，我們不妨先來了解一下兩種不同的 SLAM 方法——視覺 SLAM 和激光雷達 SLAM。

視覺 SLAM

顧名思義，視覺 SLAM（又稱 vSLAM）使用從相機和其他圖像傳感器采集的圖像。視覺 SLAM 可以使用普通相機（廣角、魚眼和球形相機）、復眼相機（立體相機和多相機）和 RGB-D 相機（深度相機和 ToF 相機）。

視覺 SLAM 所需的相機價格相對低廉，因此實現成本較低。此外，相機可以提供大量信息，因此還可以用來檢測路標（即之前測量過的位置）。路標檢測還可以與基于圖的優化結合使用，這有助于靈活實現 SLAM。

使用單個相機作為唯一傳感器的 vSLAM 稱為單目 SLAM，此時難以定義深度。這個問題可以通過以下方式解決：檢測待定位圖像中的 AR 標記、棋盤格或其他已知目標，或者將相機信息與其他傳感器信息融合，例如測量速度和方向等物理量的慣性測量單元 （IMU） 信息。vSLAM 相關的技術包括運動重建 （SfM）、視覺測距和捆綁調整。

視覺 SLAM 算法可以大致分為兩類。稀疏方法：匹配圖像的特征點并使用 PTAM 和 ORB-SLAM 等算法。稠密方法：使用圖像的總體亮度以及 DTAM、LSD-SLAM、DSO 和 SVO 等算法。

運動重建。

RGB-D SLAM 點云配準

激光雷達 SLAM

光探測與測距（激光雷達）方法主要使用激光傳感器（或距離傳感器）。

對比相機、ToF 和其他傳感器，激光可以使精確度大大提高，常用于自動駕駛汽車和無人機等高速移動運載設備的相關應用。激光傳感器的輸出值一般是二維 （x， y） 或三維 （x， y， z） 點云數據。激光傳感器點云提供了高精確度距離測度數據，特別適用于 SLAM 建圖。一般來說，首先通過點云匹配來連續估計移動。然后，使用計算得出的移動數據（移動距離）進行車輛定位。對于激光點云匹配，會使用迭代最近點 （ICP） 和正態分布變換 （NDT） 等配準算法。二維或三維點云地圖可以用柵格地圖或體素地圖表示。

但就密度而言，點云不及圖像精細，因此并不總能提供充足的特征來進行匹配。例如，在障礙物較少的地方，將難以進行點云匹配，因此可能導致跟丟車輛。此外，點云匹配通常需要高處理能力，因此必須優化流程來提高速度。鑒于存在這些挑戰，自動駕駛汽車定位可能需要融合輪式測距、全球導航衛星系統 （GNSS） 和 IMU 數據等其他測量結果。倉儲機器人等應用場景通常采用二維激光雷達 SLAM，而三維激光雷達點云 SLAM 則可用于無人機和自動駕駛。

二維激光雷達 SLAM

三維激光雷達 SLAM

SLAM 面臨的常見挑戰

雖然 SLAM 已在某些場景下投入實際應用，但是仍面臨諸多技術挑戰，因此難以得到更為廣泛的應用。不過，每項挑戰都可以憑借特定的對策加以克服。

1） 定位誤差累積，導致與實際值產生偏差

SLAM 會估計連續移動，并容許一定的誤差。但是誤差會隨著時間累積，導致與實際值產生明顯偏差。誤差還會導致地圖數據瓦解或失真，讓后續搜索變得困難。我們來看一個繞正方形通道行駛的例子。隨著誤差累積，機器人的起點和終點對不上了。這稱為閉環問題。這類位姿估計誤差不可避免。我們必須設法檢測到閉環，并確定如何修正或抵消累積的誤差。

對策之一是記住之前到過的某處的某些特征，將其作為路標，從而最小化定位誤差。構建位姿圖有助于修正誤差。將誤差最小化問題視為優化問題進行求解，以生成更準確的地圖數據。這種優化在視覺 SLAM 中稱為捆綁調整。

構建位姿圖并最小化誤差的示例

2） 定位失敗，地圖上的位置丟失

圖像和點云建圖不考慮機器人的移動特征。在某些情況下，這種方法會生成不連續的位置估計。例如，可能會有計算結果顯示，以 1 米/秒速度移動的機器人突然向前“瞬移”了 10 米。避免這種定位失敗的辦法有兩種：一是使用恢復算法；二是將運動模型與多個傳感器融合，以基于傳感器數據計算。

有多種方法可以實現運動模型的傳感器融合。一種常見方法是使用卡爾曼濾波進行定位。由于大部分差速驅動機器人和四輪車輛一般都使用非線性運動模型，因此通常會使用擴展卡爾曼濾波器和粒子濾波器（蒙特卡羅定位）。某些情況下，也可以使用無跡卡爾曼濾波器等更加靈活的貝葉斯濾波器。一些常用傳感器是慣性測量裝置，例如慣性測量單元 （IMU）、航姿參考系統 （AHRS）、慣性導航系統 （INS）、加速度計傳感器、陀螺儀傳感器和磁力傳感器。安裝到車輛的輪式編碼器通常用于測距。

定位失敗時，一種恢復對策是記住之前經過的某個位置的關鍵幀，將其作為路標。搜索路標時，會以特定方法進行特征提取以便高速掃描。有些方法基于圖像特征，例如特征袋 （BoF） 和視覺詞袋 （BoVW）。近年來，人們也使用深度學習來比較特征距離。

3） 圖像處理、點云處理和優化帶來高計算成本

在車輛硬件上實現 SLAM 時，計算成本是個問題。計算通常在處理能力有限的緊湊型低功耗嵌入式微處理器上執行。為了實現準確定位，必須高頻率執行圖像處理和點云匹配。此外，閉環等優化計算都是高成本計算流程。此處的挑戰在于如何在嵌入式微處理器上執行這種高成本處理。

對策之一是并行運行多個不同流程。例如，用于匹配流程前處理的特征提取就相對適合并行運行。使用多核 CPU 進行處理時，單指令多數據 （SIMD） 計算和嵌入式 GPU 在某些情況下可以進一步提升速度。而且，由于位姿圖優化可以在相對長的周期里執行，降低其優先級并以規律間隔執行也能提高性能。

3. 使用 MATLAB 實現 SLAM

MATLAB 可以幫助您在目標系統上實現 SLAM 應用，并可幫助您應對各種已知的 SLAM 技術挑戰。

SLAM 前端的傳感器信號和圖像處理

使用 Lidar Toolbox 和 Navigation Toolbox 進行二維/三維激光雷達處理和掃描匹配

三維點云處理和點云配準

使用特征袋和視覺詞袋進行閉環檢測

使用深度學習進行目標檢測和語義分割

使用 Automated Driving Toolbox 生成包含三維激光雷達點云的地圖

使用 Sensor Fusion and Tracking Toolbox 將傳感器融合用于定位和多目標跟蹤

SLAM 后端的二維/三維位姿圖

使用 Navigation Toolbox 生成二維/三維位姿圖

基于節點和邊約束優化位姿圖

使用 Computer Vision Toolbox 進行捆綁調整

使用 SLAM 地圖生成器生成占據柵格

從 MATLAB 工作區或 rosbag 文件導入二維激光雷達數據并創建占據柵格

尋找并修改閉環，然后將地圖導出為占據柵格以用于路徑規劃

使用來自 SLAM 算法的輸出地圖進行路徑規劃和控制

使用 Navigation Toolbox 實現 RRT 或 Hybrid A* 等路徑規劃算法

發送控制指令以跟隨規劃路徑并避開障礙

使用 Parallel Computing Toolbox 并行運行計算密集型流程（例如圖像處理相關的流程），以加快流程處理速度

使用 ROS Toolbox 從 MATLAB 和 Simulink 部署獨立 ROS 節點并與支持 ROS 的機器人通信

使用 MATLAB Coder 和 GPU Coder 將在 MATLAB 和 Simulink 中開發的圖像處理和導航算法部署到嵌入式微處理器

審核編輯 ：李倩