從算法角度看 SLAM（第 2 部分）

作者： Aswin S Babu

正如我們?cè)赱第 1 部分]中所討論的，SLAM 是指在無(wú)地圖區(qū)域中估計(jì)機(jī)器人車輛的位置，同時(shí)逐步繪制該區(qū)域地圖的過程。根據(jù)使用的主要技術(shù)，SLAM 算法可分為三種，分別是基于濾波器的 SLAM、基于圖形的 SLAM 和基于深度學(xué)習(xí)的 SLAM。

基于濾波器的 SLAM 將 SLAM 視為狀態(tài)估計(jì)問題。在這種 SLAM 中，通常使用擴(kuò)展卡爾曼濾波器 (EKF) 或無(wú)跡卡爾曼濾波器 (UKF) 等概率濾波器，來遞歸估計(jì)機(jī)器人的狀態(tài)并根據(jù)傳感器測(cè)量值更新地圖。該濾波器根據(jù)機(jī)器人的運(yùn)動(dòng)模型預(yù)測(cè)機(jī)器人的下一個(gè)狀態(tài)，然后使用傳感器測(cè)量值來修正該預(yù)測(cè)。

與基于濾波器的 SLAM 相比，基于圖形的 SLAM 將問題視為圖形優(yōu)化問題。在這種 SLAM 中，SLAM 問題被表述成一個(gè)圖形，其中節(jié)點(diǎn)表示機(jī)器人姿態(tài)或環(huán)境中的地標(biāo)，邊緣表示它們之間的測(cè)量值或約束。基于圖形的 SLAM 的目標(biāo)是優(yōu)化機(jī)器人的姿態(tài)和地標(biāo)的位置，以便盡可能準(zhǔn)確地滿足測(cè)量的約束條件，例如地標(biāo)之間的距離、機(jī)器人姿態(tài)之間的相對(duì)姿態(tài)。

基于深度學(xué)習(xí)的 SLAM 方法則利用神經(jīng)網(wǎng)絡(luò)直接從傳感器數(shù)據(jù)中學(xué)習(xí)環(huán)境表征，而不依賴手工創(chuàng)建的特征或模型。這些方法可以學(xué)習(xí)傳感器測(cè)量值和機(jī)器人姿態(tài)或地圖之間的復(fù)雜映射，從而實(shí)現(xiàn)端到端的 SLAM 解決方案。

SLAM 的地圖繪制和定位這兩項(xiàng)核心功能彼此之間相互依存，其中機(jī)器人根據(jù)傳感器數(shù)據(jù)不斷更新其地圖并相應(yīng)地調(diào)整位置估計(jì)。作為一種模塊化工具，SLAM 及其概念允許將替換和更改規(guī)劃在內(nèi)。因此，很多時(shí)候，人們會(huì)同時(shí)開發(fā)和使用幾種算法，這使得將 SLAM 作為單一算法進(jìn)行概括和解釋變得有些麻煩。因此，理解 SLAM 的最佳方式是聚焦一種具體的 SLAM 實(shí)現(xiàn)。既然如此，我們就詳細(xì)討論一下基于濾波器的視覺 SLAM (vSLAM)。

視覺 SLAM

顧名思義，vSLAM 使用視覺傳感器（相機(jī)）作為其主要傳感器。此外，它可能還配備編碼器、慣性測(cè)量裝置 (IMU) 和其他傳感器。圖 1 顯示了這種實(shí)現(xiàn)的通用框圖。

圖 1：基于特征的 SLAM 過程的簡(jiǎn)化通用框圖。（復(fù)制自 kudan.io）

相機(jī)測(cè)量

相機(jī)捕捉機(jī)器人周圍環(huán)境的圖像，包括地標(biāo)、邊緣和紋理等特征。然而，由于大多數(shù)相機(jī)鏡頭都會(huì)產(chǎn)生一定程度的失真，因此需要對(duì)捕捉的圖像進(jìn)行失真校正。使用的相機(jī)可以是立體相機(jī)、單目相機(jī)或[帶有飛行時(shí)間 (ToF) 深度傳感器的 RGB-D 相機(jī)]。立體相機(jī)和 RGB-D 相機(jī)的優(yōu)點(diǎn)是可以輕松獲取深度信息。但單目相機(jī)存在尺度不確定性問題。也就是說，單目 SLAM 無(wú)法僅從特征對(duì)應(yīng)關(guān)系來識(shí)別平移運(yùn)動(dòng)的長(zhǎng)度（比例因子）。不過，有一些方法可以緩解這種情況，但不在本文的討論范圍內(nèi)。

特征提取

使用相機(jī)傳感器捕捉圖像后，我們需要通過找出特定幀的特征來唯一地識(shí)別該幀，以供將來參考。在這種情況下，特征是唯一且可以一致識(shí)別的像素的集合?；蛘呶覀兛梢哉f像素是圖像中的獨(dú)特點(diǎn)，不會(huì)因旋轉(zhuǎn)、縮放和失真而發(fā)生變化，因此即使在圖像處理之后也很容易重新識(shí)別它們。考慮到我們使用立體相機(jī)作為主要傳感器，我們應(yīng)該能夠看到相機(jī)捕捉的立體圖像之間的重疊特征。然后，可以使用這些相同的特征來估計(jì)與傳感器之間的距離。然而，如前所述，在此之前，我們需要識(shí)別立體圖像對(duì)上的共同特征。這一過程由特征檢測(cè)器和匹配器來完成。一些常見特征檢測(cè)器的示例包括尺度不變特征轉(zhuǎn)換 (SIFT)、定向 FAST 和旋轉(zhuǎn) BRIEF (ORB) 以及良好特征跟蹤 (GFTT)。圖 2 顯示了使用一些熱門特征檢測(cè)器識(shí)別的特征。一旦識(shí)別出特征，就會(huì)使用相同的特征檢測(cè)器對(duì)其進(jìn)行描述。此過程有助于將來輕松地重新識(shí)別這些特征。

圖 2：使用 a) GFTT b) SIFT 在 XRP 機(jī)器人圖像上檢測(cè)到的關(guān)鍵點(diǎn)。（來源：SparkFun Electronics）

找出關(guān)鍵點(diǎn)之后，我們通過匹配來建立這些點(diǎn)之間的對(duì)應(yīng)關(guān)系。一些可使用的特征匹配算法包括暴力匹配器或快速最近鄰搜索庫(kù) (FLANN)。圖 3 顯示了匹配算法作用機(jī)制的視覺表示。圖中所示直線連接匹配之處，由于我們使用了鏡像，理想情況下，如果系統(tǒng)是完美的，我們應(yīng)該只得到水平（平行）直線。但遺憾的是，特征匹配算法并不完美，因此會(huì)導(dǎo)致錯(cuò)誤匹配，其中一些用斜線表示。這就是為什么我們需要隨機(jī)抽樣一致性 (RANSAC) 這樣的異常值剔除工具。

圖 3：使用 FLANN 在兩幅對(duì)稱圖像上表示特征匹配的直線。（來源：SparkFun Electronics）

RANSAC

使用像 RANSAC 這樣的算法可濾除這些不正確的匹配，確保僅使用正確值（正確的匹配）作進(jìn)一步處理。RANSAC 的工作方式是使用所提供數(shù)據(jù)的隨機(jī)子集構(gòu)建模型。也就是說，我們將一些隨機(jī)點(diǎn)視為內(nèi)群值（正常值），并嘗試根據(jù)這些選定的點(diǎn)匹配所有剩余的點(diǎn)。然后，我們會(huì)評(píng)估模型與整個(gè)數(shù)據(jù)集的匹配程度。這個(gè)過程不斷重復(fù)，直到找到一個(gè)由成本函數(shù)確定的模型，該模型能夠準(zhǔn)確描述數(shù)據(jù)且誤差最小。

特征和數(shù)據(jù)關(guān)聯(lián)

在此步驟中，我們采用檢測(cè)到的特征及其在空間中的估計(jì)位置來創(chuàng)建這些特征的地圖。隨著在后續(xù)幀繼續(xù)進(jìn)行該過程，系統(tǒng)會(huì)將新特征與地圖的已知元素關(guān)聯(lián)，并丟棄不確定的特征。

當(dāng)在后續(xù)幀中跟蹤相機(jī)運(yùn)動(dòng)時(shí)，可以根據(jù)已知特征以及它們預(yù)計(jì)隨運(yùn)動(dòng)變化的方式來進(jìn)行預(yù)測(cè)。不過，計(jì)算資源和時(shí)間約束（尤其是在實(shí)時(shí)應(yīng)用中）給 SLAM 帶來了限制。隨著系統(tǒng)收集更多的特征測(cè)量值并更新位置/姿態(tài)，環(huán)境表示的約束和優(yōu)化變得至關(guān)重要。

位置、姿態(tài)和地圖更新

卡爾曼濾波器

隨著 SLAM 過程的深入，會(huì)不斷積累噪聲，并在相機(jī)捕捉的圖像與其相關(guān)運(yùn)動(dòng)之間產(chǎn)生不確定性。通過根據(jù)觀察到的測(cè)量值不斷生成預(yù)測(cè)、更新并微調(diào)模型，卡爾曼濾波器可以減少不同測(cè)量值之間的噪聲和不確定性的影響。這有助于創(chuàng)建線性系統(tǒng)模型。在 SLAM 的實(shí)際實(shí)現(xiàn)中，我們使用擴(kuò)展卡爾曼濾波器 (EKF)，其采用非線性系統(tǒng)，并圍繞平均值對(duì)預(yù)測(cè)和測(cè)量值進(jìn)行線性化處理。EKF 可以通過執(zhí)行傳感器融合（例如，相機(jī)、IMU）整合來自多個(gè)傳感器的數(shù)據(jù)，以提高狀態(tài)和地圖估計(jì)的準(zhǔn)確性。這種數(shù)據(jù)源的融合有助于獲得更可靠的 SLAM 結(jié)果?；?EKF 的 SLAM 中的狀態(tài)向量包括機(jī)器人的姿態(tài)（位置和方向）和地圖中地標(biāo)的位置。

關(guān)鍵幀選擇

在捕捉的圖像中選擇關(guān)鍵幀，可減少處理全部捕捉圖像所需的大量計(jì)算。我們選擇的是可以很好地表示環(huán)境的幀，并僅將它們用于計(jì)算。這種方法又是一次準(zhǔn)確性和效率之間的權(quán)衡。

通過閉環(huán)和重定位來糾正誤差

隨著構(gòu)建環(huán)境模型的過程不斷推進(jìn)，測(cè)量誤差和傳感器漂移會(huì)逐漸積累，從而影響生成的地圖。閉環(huán)可以在一定程度上緩解這種情況。當(dāng)系統(tǒng)發(fā)現(xiàn)自己正在重新訪問已經(jīng)構(gòu)建地圖的區(qū)域時(shí)，就會(huì)發(fā)生閉環(huán)。通過重新對(duì)齊當(dāng)前地圖與先前建立的同一區(qū)域的地圖，SLAM 系統(tǒng)可以糾正累積的誤差。

重定位

當(dāng)系統(tǒng)不知道其位置和方向 (POSE) 時(shí)，需要重定位。此刻，我們需要利用當(dāng)前可觀察到的特征重新估計(jì)姿態(tài)。一旦系統(tǒng)成功將當(dāng)前獲得的特征與可用地圖匹配，我們就可以正常繼續(xù) SLAM 過程。

結(jié)語(yǔ)

SLAM 是一邊估計(jì)機(jī)器人車輛位置一邊繪制未知區(qū)域地圖的過程。SLAM 技術(shù)包括基于濾波器、基于圖形和基于深度學(xué)習(xí)（使用神經(jīng)網(wǎng)絡(luò)）的方法。視覺 SLAM 使用相機(jī)捕捉圖像、提取特征、匹配特征并濾除不正確的匹配點(diǎn)。該系統(tǒng)通過將新特征與已知元素關(guān)聯(lián)、使用卡爾曼濾波器更新機(jī)器人的位置和姿態(tài)、選擇關(guān)鍵幀，并通過閉環(huán)和重定位糾正誤差來創(chuàng)建地圖。

參考資料

1. [Remote Sensing | Free Full-Text | SLAM Overview: From Single Sensor to Heterogeneous Fusion (mdpi.com)]
2. [Understanding how V-SLAM (Visual SLAM) works | Kudan global]
3. [Feature-based visual simultaneous localization and mapping: a survey | Discover Applied Sciences (springer.com)]
4. [Introduction to Visual SLAM: Chapter 1 —Introduction to SLAM | by Daniel Casado | Medium]
5. [An Introduction to Key Algorithms Used in SLAM - Technical Articles (control.com)]
6. [What Is SLAM (Simultaneous Localization and Mapping) – MATLAB & Simulink - MATLAB & Simulink (mathworks.com)]
7. [A survey of state-of-the-art on visual SLAM - ScienceDirect]

審核編輯 黃宇