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

1. SLAM 為何重要

早在多年前，人們就已開始對(duì) SLAM 開展技術(shù)研究。如今，隨著計(jì)算機(jī)處理速度顯著提升，且相機(jī)和激光測(cè)距儀等低成本傳感器大為普及，SLAM 更是在越來(lái)越多的領(lǐng)域投入實(shí)際應(yīng)用。

SLAM 為何重要？要回答這個(gè)問(wèn)題，我們可以看看以下幾個(gè)例子，了解它有哪些好處和應(yīng)用。

SLAM 示例

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

SLAM 給掃地機(jī)器人帶來(lái)的好處

SLAM 還可用于許多其他應(yīng)用場(chǎng)景中，例如讓一隊(duì)移動(dòng)機(jī)器人在倉(cāng)庫(kù)中移動(dòng)并整理貨架，讓自動(dòng)駕駛汽車停泊到空車位，或者讓無(wú)人機(jī)在未知環(huán)境中完成送貨。MATLAB 和 Simulink 提供了 SLAM 算法、函數(shù)和分析工具來(lái)開發(fā)各種應(yīng)用。您可以在實(shí)現(xiàn)同步定位與地圖構(gòu)建的同時(shí)，完成傳感器融合、目標(biāo)跟蹤、路徑規(guī)劃和路徑跟隨等其他任務(wù)。

2. SLAM 工作原理

大致說(shuō)來(lái)，實(shí)現(xiàn) SLAM 需要兩類技術(shù)。一類技術(shù)是傳感器信號(hào)處理（包括前端處理），這類技術(shù)在很大程度上取決于所用的傳感器。另一類技術(shù)是位姿圖優(yōu)化（包括后端處理），這類技術(shù)與傳感器無(wú)關(guān)。

SLAM 處理流程

為了進(jìn)一步了解前端處理技術(shù)，我們不妨先來(lái)了解一下兩種不同的 SLAM 方法——視覺 SLAM 和激光雷達(dá) SLAM。

視覺 SLAM

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

視覺 SLAM 所需的相機(jī)價(jià)格相對(duì)低廉，因此實(shí)現(xiàn)成本較低。此外，相機(jī)可以提供大量信息，因此還可以用來(lái)檢測(cè)路標(biāo)（即之前測(cè)量過(guò)的位置）。路標(biāo)檢測(cè)還可以與基于圖的優(yōu)化結(jié)合使用，這有助于靈活實(shí)現(xiàn) SLAM。

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

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

運(yùn)動(dòng)重建。

RGB-D SLAM 點(diǎn)云配準(zhǔn)

激光雷達(dá) SLAM

光探測(cè)與測(cè)距（激光雷達(dá)）方法主要使用激光傳感器（或距離傳感器）。

對(duì)比相機(jī)、ToF 和其他傳感器，激光可以使精確度大大提高，常用于自動(dòng)駕駛汽車和無(wú)人機(jī)等高速移動(dòng)運(yùn)載設(shè)備的相關(guān)應(yīng)用。激光傳感器的輸出值一般是二維 （x， y） 或三維 （x， y， z） 點(diǎn)云數(shù)據(jù)。激光傳感器點(diǎn)云提供了高精確度距離測(cè)度數(shù)據(jù)，特別適用于 SLAM 建圖。一般來(lái)說(shuō)，首先通過(guò)點(diǎn)云匹配來(lái)連續(xù)估計(jì)移動(dòng)。然后，使用計(jì)算得出的移動(dòng)數(shù)據(jù)（移動(dòng)距離）進(jìn)行車輛定位。對(duì)于激光點(diǎn)云匹配，會(huì)使用迭代最近點(diǎn) （ICP） 和正態(tài)分布變換 （NDT） 等配準(zhǔn)算法。二維或三維點(diǎn)云地圖可以用柵格地圖或體素地圖表示。

但就密度而言，點(diǎn)云不及圖像精細(xì)，因此并不總能提供充足的特征來(lái)進(jìn)行匹配。例如，在障礙物較少的地方，將難以進(jìn)行點(diǎn)云匹配，因此可能導(dǎo)致跟丟車輛。此外，點(diǎn)云匹配通常需要高處理能力，因此必須優(yōu)化流程來(lái)提高速度。鑒于存在這些挑戰(zhàn)，自動(dòng)駕駛汽車定位可能需要融合輪式測(cè)距、全球?qū)Ш叫l(wèi)星系統(tǒng) （GNSS） 和 IMU 數(shù)據(jù)等其他測(cè)量結(jié)果。倉(cāng)儲(chǔ)機(jī)器人等應(yīng)用場(chǎng)景通常采用二維激光雷達(dá) SLAM，而三維激光雷達(dá)點(diǎn)云 SLAM 則可用于無(wú)人機(jī)和自動(dòng)駕駛。

二維激光雷達(dá) SLAM

三維激光雷達(dá) SLAM

SLAM 面臨的常見挑戰(zhàn)

雖然 SLAM 已在某些場(chǎng)景下投入實(shí)際應(yīng)用，但是仍面臨諸多技術(shù)挑戰(zhàn)，因此難以得到更為廣泛的應(yīng)用。不過(guò)，每項(xiàng)挑戰(zhàn)都可以憑借特定的對(duì)策加以克服。

1） 定位誤差累積，導(dǎo)致與實(shí)際值產(chǎn)生偏差

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

對(duì)策之一是記住之前到過(guò)的某處的某些特征，將其作為路標(biāo)，從而最小化定位誤差。構(gòu)建位姿圖有助于修正誤差。將誤差最小化問(wèn)題視為優(yōu)化問(wèn)題進(jìn)行求解，以生成更準(zhǔn)確的地圖數(shù)據(jù)。這種優(yōu)化在視覺 SLAM 中稱為捆綁調(diào)整。

構(gòu)建位姿圖并最小化誤差的示例

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

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

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

定位失敗時(shí)，一種恢復(fù)對(duì)策是記住之前經(jīng)過(guò)的某個(gè)位置的關(guān)鍵幀，將其作為路標(biāo)。搜索路標(biāo)時(shí)，會(huì)以特定方法進(jìn)行特征提取以便高速掃描。有些方法基于圖像特征，例如特征袋 （BoF） 和視覺詞袋 （BoVW）。近年來(lái)，人們也使用深度學(xué)習(xí)來(lái)比較特征距離。

3） 圖像處理、點(diǎn)云處理和優(yōu)化帶來(lái)高計(jì)算成本

在車輛硬件上實(shí)現(xiàn) SLAM 時(shí)，計(jì)算成本是個(gè)問(wèn)題。計(jì)算通常在處理能力有限的緊湊型低功耗嵌入式微處理器上執(zhí)行。為了實(shí)現(xiàn)準(zhǔn)確定位，必須高頻率執(zhí)行圖像處理和點(diǎn)云匹配。此外，閉環(huán)等優(yōu)化計(jì)算都是高成本計(jì)算流程。此處的挑戰(zhàn)在于如何在嵌入式微處理器上執(zhí)行這種高成本處理。

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

3. 使用 MATLAB 實(shí)現(xiàn) SLAM

MATLAB 可以幫助您在目標(biāo)系統(tǒng)上實(shí)現(xiàn) SLAM 應(yīng)用，并可幫助您應(yīng)對(duì)各種已知的 SLAM 技術(shù)挑戰(zhàn)。

SLAM 前端的傳感器信號(hào)和圖像處理

使用 Lidar Toolbox 和 Navigation Toolbox 進(jìn)行二維/三維激光雷達(dá)處理和掃描匹配

三維點(diǎn)云處理和點(diǎn)云配準(zhǔn)

使用特征袋和視覺詞袋進(jìn)行閉環(huán)檢測(cè)

使用深度學(xué)習(xí)進(jìn)行目標(biāo)檢測(cè)和語(yǔ)義分割

使用 Automated Driving Toolbox 生成包含三維激光雷達(dá)點(diǎn)云的地圖

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

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

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

基于節(jié)點(diǎn)和邊約束優(yōu)化位姿圖

使用 Computer Vision Toolbox 進(jìn)行捆綁調(diào)整

使用 SLAM 地圖生成器生成占據(jù)柵格

從 MATLAB 工作區(qū)或 rosbag 文件導(dǎo)入二維激光雷達(dá)數(shù)據(jù)并創(chuàng)建占據(jù)柵格

尋找并修改閉環(huán)，然后將地圖導(dǎo)出為占據(jù)柵格以用于路徑規(guī)劃

使用來(lái)自 SLAM 算法的輸出地圖進(jìn)行路徑規(guī)劃和控制

使用 Navigation Toolbox 實(shí)現(xiàn) RRT 或 Hybrid A* 等路徑規(guī)劃算法

發(fā)送控制指令以跟隨規(guī)劃路徑并避開障礙

使用 Parallel Computing Toolbox 并行運(yùn)行計(jì)算密集型流程（例如圖像處理相關(guān)的流程），以加快流程處理速度

使用 ROS Toolbox 從 MATLAB 和 Simulink 部署獨(dú)立 ROS 節(jié)點(diǎn)并與支持 ROS 的機(jī)器人通信

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

審核編輯 ：李倩