SLAM Demystify

Posted on | In

What and Why SLAM

SLAM (Simultaneous Location And Mapping) 聽起來既熟悉（SLAM Dunk）但又陌生。爲什麽”同時”、”位置“、”地圖“要放在一起？

SLAM 一般都是和 robot 機器人連結在一起，但必須是移動的機器人，掃地機器人就是標準的例子。

  1. 便宜的掃地機器人基本是做隨機直綫運動，遇到障礙物 (隨機) 轉一個角度，和二維布朗運動很像，如下圖左。

  • 好處是算法非常簡單。機器人完全不需要内建地圖 (map) 和現在的位置 (location)。

  • 壞處是要很久才能清掃大部分的區域，而且無法保證何時可以完全清理。非常浪費時間和資源。大多情況是無法接受。

  1. 另一種完全相反的例子是掃地機器人已經有内建的地圖 (map)，知道那些區域需要清理。光有地圖不夠，還需要知道目前的位置 (location) 對應在地圖的坐標。例如使用 GPS 獲得絕對坐標（假設在戶外可以接受 GPS 訊號），或者其他方式獲得相對坐標 (e.g. indoor positioning system using beacon or infrared)。一般是由 outside-in 方式獲得。掃地機器人藉助内建的 (global) map 加上自己的坐標，就可以規劃掃地的路徑。如下圖右。

  • 好處是可以完全計算多久可以清理所有區域，甚至即時清理的比例。也可以 optimize 算法規劃最有效率的路徑。這類算法也不複雜。

  • 壞處是 (1) 需要事先内建地圖 (global map); (2) 需要 real-time outside-in 的位置 (location).

  1. 是否有不需要内建地圖，也不用 outside-in 位置 (e.g. GPS) 的方式，完全憑藉機器人自身的 sensor 獲得地圖 (map) 和位置 (location) 的方法？答案就是 SLAM 算法。有了 SLAM, 就可以做到類似下圖右的清理路徑。當然 SLAM 這個地圖是邊走邊建出來的，和 method 2 可以根據内建 (global) 地圖事先規劃路徑還是不同。其實這和大腦的工作方式類似。
    • 好處是機器人不需要事先内建地圖，這個地圖是邊動邊學出來。也不需要 outside-in 獲得坐標。在大多數的情況下 (簡單的凸多邊形區域) 仍然可以得到最佳或接近最佳的路徑規劃。
    • 壞處是 (1) 機器人需要 depth sensor (e.g. cameras, Lidar) 或是 motion + obstacle detection sensor (e.g. ultra sonic) (2) 需要相對複雜的算法，也就是 SLAM.

image-20220325205710980

How SLAM Work

廣義而言，SLAM 包含 front-end 和 back-end。

Front-end 包含 sensor signal processing 例如 motion estimation, depth estimation, location estimation, etc. 很大程度上取決於所使用的 sensor。 前端相當於VO (Visual Odometry)，研究幀與幀之間變換關系。首先提取每幀圖像特征點，利用相鄰幀圖像，進行特征點匹配，然後利用 RANSAC (RANdom SAample Consensus) 去除大噪聲，然後進行匹配，得到一個 pose 信息（位置和姿態），同時可以利用IMU（Inertial measurement unit慣性測量單元）提供的姿態信息進行濾波融合。

image-20220326225052858

Back-end 是與 sensor 無關的處理。後端則主要是對前端出結果進行優化，利用濾波理論 EKF (Extended Kalman Filter)、UKF (Unscented Kalman Filter)、PF (Particle Filter)、或者優化理論 TORO、G2O 進行樹或者圖優化 (Graph optimization)。最終得到最優的位姿估計。

一些常見 SLAM 的 backend.

Filter theory backend: EKF SLAM; BASALT SLAM (EKF)

Graph Optimization backend: ORB SLAM (G2O)

後端這邊難點比較多，涉及到的數學知識也比較多，總的來說大家已經慢慢拋棄傳統的濾波理論走向圖優化去了。

因為基於濾波的理論，濾波器穩度增長太快，這對於需要頻繁求逆的EKF，PF壓力很大。而基於圖的SLAM，通常以keyframe（關鍵幀）為基礎，建立多個節點和節點之間的相對變換關系，比如仿射變換矩陣，並不斷地進行關鍵節點的維護，保證圖的容量，在保證精度的同時，降低了計算量。

我們進一步探討 front-end 處理模組，包含兩類常見 visual SLAM 和 lidar SLAM。

Visual SLAM (vSLAM)

顧名思義，視覺 SLAM（或 vSLAM）使用從相機和其他圖像傳感器獲取的圖像。視覺 SLAM 可以使用簡單的相機（廣角、魚眼和球面相機）、複眼相機（立體和多相機）和 RGB-D 相機（深度和 ToF 相機）。

vSLAM 可以用相對便宜的相機以低成本實現。此外，由於相機提供了大量信息，它們可用於 landmark detection（先前測量的位置）。Landmark detection 還可以與基於 graph optimization 相結合，在實現 SLAM 具有更大的靈活性。

單目 (monocular) SLAM 是當 vSLAM 使用單個 camera 作為唯一傳感器時，這使得深度的計算具有挑戰性。這可以通過檢測圖像中的 AR 標記、棋盤格、或其他已知物體進行定位，或者通過將相機信息與另一個傳感器（例如慣性測量單元 (IMU)）融合來解決。慣性測量單元 (IMU) 可以測量速度和方向等物理量。與 vSLAM 相關的技術包括 structure from motion (SfM)、visual odometry, and bundle adjustment.

vSLAM 算法可以大致分為兩類。稀疏 (sparse) 方法匹配圖像的特徵點，並使用 PTAM 和 ORB-SLAM 等算法。密集 (dense) 方法使用圖像的整體亮度，並使用 DTAM、LSD-SLAM、DSO 和 SVO 等算法。

image-20220326231408650

列舉幾個目前比較有名的 vSLAM 算法：PTAM, MonoSLAM, ORB-SLAM, RGBD-SLAM, RTAB-SLAM, LSD-SLAM。 不同的 vSLAM 算法的主要差別就在 front-end 的特徵提取算法和 backend-end 的優化算法。

  Input Front end (Sparse or Dense) Back end (Filter or Graph)
LSD (large scale direct) Mono NA (direct and sparse) KF
Mono SLAM Mono   KF
BASALT Mono + IMU? FAST (sparse) EKF
PTAM Mono FAST (sparse) BA (graph)
ORB SLAM Mono ORB: Oriented FAST and Rotated BRIEF G2O; BA? (graph)
ORB SLAM2 Stereo/RGB-D ORB  
ORB SLAM2s   optical flow?  
RTAB SLAM RBG-D    

image-20220507080815219

Lidar SLAM

Lidar or ToF 的使用愈來愈廣汎，包含車用、機器人、手機、平板都内建有 Lidar 或 ToF 可以直接得到空間的深度資訊。

Lidar 是一種主要使用 laser sensor（或 distance sensor）的方法。 與 camera、ToF 和其他 sensor 相比，雷射的精度要高得多，並且用於自動駕駛汽車和無人機等高速移動車輛的應用。Lidar 的輸出值一般是 2D (x, y) 或 3D (x, y, z) 點雲數據。 Lidar 點雲提供高精度的距離測量，並且非常有效地用於 SLAM 構建地圖。

通常通過匹配點雲的順序可以估計運動。計算出的運動（行駛距離）可以用於定位車輛。對於 lidar 點雲匹配，使用了例如 ICP 和 NDT 算法等 registration algorithms。 2D 或 3D 點雲圖可以表示為 grid map 或 voxel map。

另一方面，點雲在密度方面不如圖像精細，並且可能無法提供足夠的特徵進行匹配。例如，在障礙物很少的地方，點雲很難對齊，這可能會導致車輛位置丟失。此外點雲匹配通常需要很高的處理能力，因此需要優化處理速度。由於這些挑戰，自動駕駛汽車的定位可能涉及融合其他測量結果，例如車輪里程計、全球導航衛星系統 (GNSS) 和 IMU 數據。對於倉庫機器人等應用，通常使用 2D lidar SLAM，而使用 3D lidar 點雲的 SLAM 可用於無人機和自動駕駛。

image-20220327092804643

SLAM Challenges

儘管 SLAM 用於一些實際應用，但一些技術挑戰阻礙了更通用的採用。 每個挑戰都有對應的方法。

Localization 誤差累積

SLAM 估計 sequential movement，每一個估計都有一些誤差。 誤差會隨著時間累積，導致與實際值有很大的偏差。 它還可能導致地圖數據崩潰或扭曲，導致後續搜索變得困難。我們以在方形通道周圍行駛為例。 隨著誤差的累積，機器人的起點和終點不再匹配。 這稱為閉環問題。 像這樣的 pose estimation errors 是不可避免的。 檢測閉環並確定如何糾正或消除累積的錯誤非常重要。

image-20220327225210977

一種對策是記住以前訪問過的地方的一些特徵作為路標並最小化 location error。構建 pose graphs 可以幫助糾正錯誤。 通過將誤差最小化作為優化問題，可以生成更準確的 map。 這種優化在 vSLAM 稱為 bundle adjustment。

Localization fails and the position on the map is lost

Image 和 point-cloud 映射並不考慮機器人運動的特徵。在某些情況下，這種方法會產生不連續的位置估計。例如，一個計算結果可能會得到以 1 m/s 的速度運動的機器人突然向前跳躍了 10 米。可以通過使用 recovery algorithm 或將運動模型與多個傳感器融合以根據傳感器數據進行計算來防止這種定位失敗。

有幾種方法可以使用帶有傳感器融合的運動模型。一種常見的方法是使用卡爾曼濾波進行定位。由於大多數差動驅動機器人和四輪車輛一般使用非線性運動模型，因此經常使用 extended 卡爾曼濾波器和 particle filters（蒙特卡洛定位）。在某些情況下，也可以使用更靈活的貝葉斯濾波器，例如 unscented 卡爾曼濾波器。一些常用的傳感器包含慣性測量設備，例如 IMU、Attitude and Heading Reference system 或 AHRS、慣性導航系統或 INS、加速度計傳感器、陀螺儀傳感器和磁傳感器。連接到車輛的車輪編碼器通常用於里程計。

當定位失敗時，恢復的對策是記住一個路標作為以前訪問過的地方的關鍵幀。在搜索路標時，會以可以高速掃描的方式應用特徵提取過程。一些基於圖像特徵的方法包括 bag of feature（BoF）和 bag of visual words（BoVW）。最近深度學習被用於比較與特徵的距離。

High computational cost for image processing, point cloud processing, and optimization

在移動硬體上實施 SLAM 時，計算成本是一個問題。計算通常在處理能力有限的 compact and low power 嵌入式微處理器上執行。為了實現準確的定位，必須以高頻執行圖像處理和點雲匹配。此外，諸如閉環之類的優化計算是高計算過程。挑戰在於如何在嵌入式微型計算機上執行這種計算量大的處理。

一種對策是平行運行不同的 processes。特徵提取等過程是匹配過程的預處理，比較適平行化。在某些情況下，使用多核 CPU 進行處理、單指令多數據 (SIMD) 計算和嵌入式 GPU 可以進一步提高速度。此外，由於可以在相對較長的周期內執行 pose graph 優化，因此降低其優先級並定期執行此 process 也可以改善性能。

Reference