從演算法的角度探討 SLAM (第 2 篇)

如同第 1 篇所討論的，SLAM 的功能是估算機器人車輛在未知區域的位置，同時逐步為該區域製作地圖。SLAM 演算法可分為三種主要技術，分別是濾波器型 SLAM、圖形式 SLAM 和深度學習式 SLAM。

濾波器式 SLAM 會將 SLAM 視為狀態估算問題。在此通常會採用延伸型卡爾曼濾波器 (EKF) 或無損型卡爾曼濾波器 (UKF) 等機率濾波器，來遞迴估算機器人的狀態，並根據感測器量測值來更新地圖。濾波器會根據其運動模型來預測機器人的下一個狀態，接著使用感測器量測值修正此預測結果。

相較於濾波器式 SLAM，圖型式 SLAM 會將問題當作圖形最佳化問題來處理。SLAM 問題會製成圖形，圖形中以節點代表環境中機器人的姿態或地標，並以邊緣代表姿態或地標之間的量測值或約束條件。圖型式 SLAM 的目標是將機器人的姿態和地標的位置最佳化，進而盡可能準確地符合測量的約束條件，例如地標間距、機器人姿態之間的相對姿態等等。

深度學習式 SLAM 方法則是運用神經網路，以感測器資料直接學習環境的呈現樣貌，無需仰賴手工繪製的特徵點或模型。這些方法能學習感測器量測值與機器人姿態或地圖之間的複雜對應情況，進而達到端對端的 SLAM 解決方案。

SLAM、建圖和定位的核心功能彼此交織，並利用機器人根據感測器資料不斷更新其地圖，且依此調整位置估算結果。SLAM 屬於模組化工具，SLAM 及其概念可允許管線中進行替換與變更，因此往往會開發並同時併用數種演算法，要以單一演算法的方式來歸納並說明 SLAM 並不容易。如此一來，要瞭解 SLAM 的最佳方式，就是專注在單一特定的 SLAM 實作。既然如此，我們就詳細探討濾波器式的視覺 SLAM (vSLAM)。

視覺 SLAM

顧名思義，vSLAM 使用視覺感測器 (即攝影機) 當作主要感測器。此外，可能也同時具備編碼器、慣性量測單元 (IMU) 及其他感測器。圖 1 展示了實作的一般方塊圖。

圖 1：SLAM 流程特徵的簡化版一般方塊圖。(依 kudan.io 資料重製)

攝影機量測

攝影機會擷取機器人周遭環境的影像，包括地標、邊緣和紋理等特徵點。但由於多數攝影機鏡頭都會造成一定程度的失真，因此需要對擷取的影像進行失真修正。可使用的攝影機包括立體、單眼或 RGB-D 攝影機，且搭配飛時測距 (ToF) 深度感測器。立體攝影機和 RGB-D 攝影機的優點在於能輕鬆取得深度資訊。不過，單眼攝影機很難避免標度模糊的問題。也就是說，單眼 SLAM 無法單靠特徵點的對應關係來辨別平移的長度 (標度因子)。幸好有方法可緩解此問題，但並不在本文討論的範圍內。

特徵點擷取

使用攝影機感測器擷取影像後，需利用獨特的方式來識別訊框，找出特定訊框的特徵點，以供日後參考。這類特徵點屬於像素集合，當中每個像素都是唯一的，且能達到一致的識別結果。也可以說這些特徵是影像中的獨特點，不會因為旋轉、縮放和失真而產生變化，因此即使經過影像處理，也很容易再次識別。由於我們使用立體攝影機當作主要感測器，因此應該能看見攝影機擷取的立體影像之間有重疊的特徵點。就可利用這些相同的特徵點來估算與感測器間的距離。但在此之前，如前文所述，我們必須先識別立體影像對組上的共同特徵點。這個步驟會用特徵點偵測演算法和匹配演算法進行。一些常見的特徵點偵測演算法包括尺度不變特徵點轉換 (SIFT)、定向 FAST 和旋轉 BRIEF (ORB) 以及易追蹤的優質特徵點 (GFTT)。圖 2 顯示使用某些常用特徵點偵測器識別的特徵點。識別特徵點後，會使用相同的特徵點偵測器賦予描述。這個流程有助於日後再次輕鬆識別這些特徵點。

圖 2：使用 a) GFTT b) SIFT 在 XRP 機器人影像上偵測到的關鍵點。(來源：SparkFun Electronics)

找出關鍵點後，我們會對其進行匹配，進而建立關鍵點之間的對應關係。可用的特徵點匹配演算法包括暴力匹配演算法 (Brute Force Matcher) 或近似最近鄰快速開源庫 (FLANN)。圖 3 以視覺方式呈現出匹配演算法的實際運作。圖中的線條將匹配項相連；由於我們使用鏡像，若系統本身完美，則只會出現水平線 (平行線)。但遺憾的是，特徵點匹配演算法並非完美無缺，因此會產生錯誤匹配項，其中某些會以斜線表示。因此，我們需要使用隨機取樣一致性 (RANSAC) 演算法等異常值排除工具。

圖 3：使用 FLANN 演算法以線條指出兩個反轉影像上的特徵點匹配項。(來源：SparkFun Electronics)

RANSAC

RANSAC 等演算法可用來剔除這些不正確的匹配項，確保僅使用合格項 (正確的匹配項) 進行進一步處理。RANSAC 的運作方式是從提供的資料中隨機取子集建立模型。也就是說，我們會將幾個隨機點視為合格項，並嘗試依據這些選定點對所有剩餘的點進行匹配。接著會評估模型與整個資料集的匹配程度。此流程會重複執行，直到找到一個可準確描述資料且誤差最小的模型為止 (由成本函數來決定)。

特徵點與資料關聯

此步驟會使用偵測到的特徵點及其估算的空間位置，建立這些特徵點的地圖。在後續訊框持續進行此流程下，系統會將新的特徵點與已知的地圖元素建立關聯，並捨棄不確定的特徵點。

在後續訊框中追蹤攝影機運動時，就可根據已知的特徵點來進行預測，並可瞭解特徵點預期隨著運動而產生的變化。不過，運算資源和時間限制 (尤其是在即時應用中) 會對 SLAM 施加限制。隨著系統收集越多特徵點量測值，並逐漸對位置／姿態進行更新，就必須對環境的呈現進行約束和最佳化。

位置、姿態與地圖的更新

卡爾曼濾波器

SLAM 程序往下進行時會累積雜訊，並在攝影機擷取的影像和相關的運動之間形成不確定性。卡爾曼濾波器能不斷產生預測、執行更新，並根據觀察到的量測值來微調模型，進而降低雜訊影響並減少不同量測值之間的不確定性。這有助於建立線性系統模型。在 SLAM 的實際實作上，我們使用延伸卡爾曼濾波器 (EKF)，其採用非線性系統，會根據平均值將預測和量測值線性化。EKF 能執行感測器融合 (例如攝影機、IMU)，將眾多感測器的資料進行整合，以便提高狀態和地圖估算值的準確性。資料來源的融合有助於達到更可靠的 SLAM 結果。在以 EKF 為基礎的 SLAM 中，其狀態向量包含地圖中機器人的姿態 (位置和方向) 及地標的位置。

關鍵訊框的選擇

處理所有擷取的影像需要大量運算，因此在擷取的影像中挑選關鍵訊框，就可減少運算量。但我們選擇的是能良好呈現環境的訊框，並只用這些訊框進行運算。此作法同樣也是準確度和效率之間的權衡之計。

透過迴路閉合和重新定位來修正誤差

在建立環境模型時會逐漸累積量測誤差並發生感測器漂移，進而影響產生的地圖。使用迴路閉合能對此問題達到一定的緩解。當系統發現再次到訪某個已建圖的區域時，就會發生迴路閉合。SLAM 系統會對照同一區域過去建立的地圖來重新校準目前的地圖，進而修正累積的誤差。

重新定位

當系統忘記本身的位置和方向 (姿態) 時，就需要重新定位。現在，我們需要利用目前可觀察到的特徵點重新估算姿態。一旦系統成功將現有特徵點與可用的地圖互相匹配，SLAM 流程就可照常進行。

結論

SLAM 流程可估算機器人車輛的位置，同時對未知區域建立地圖。SLAM 技術包括濾波器式、圖型式以及深度學習式 (利用神經網路) 的方法。視覺 SLAM 會利用攝影機來擷取影像、擷取特徵點、進行匹配，並剔除不正確的匹配項。系統的建圖方式是將新的特徵點與已知的元素建立關聯、使用卡爾曼濾波器更新機器人的位置和姿態、挑選關鍵訊框，並透過迴路閉合和重新定位來修正誤差。

參考資料

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