視覺SLAM十四講：第2講 初識SLAM

第2講：初識SLAM

2.1 引言

定位和建圖是感知的“內外之分”：前者任務是明白自身的狀態（即位置）；後者任務是瞭解外在的環境（即地圖）。

傳感器：

1. 攜帶於機器人本體上的，例如機器人的輪式編碼器、相機、激光、慣性測量單元（inertial measurement unit，IMU）等；

2. 安裝於環境之中，例如前導軌、二維碼標誌等等。

攜帶於機器人本體上的傳感器測到的通常都是一些間接的物理量而不是直接的位置數據，只能通過一些間接的手段，從這些數據推算自己的位置。它沒有對環境提出任何要求，使得這種定位方案可適用於未知環境。

安裝於環境中的傳感設備，通常能夠直接測量到機器人的位置信息，簡單有效地解決定位問題。然而，由於它們必須在環境中設置，在一定程度上限制了機器人的使用範圍。

視覺SLAM傳感器——相機：按照相機的工作方式，相機分爲三個大類，單目（monocular）、雙目（stereo）和深度相機（RGB-D）

單目相機：只使用一個攝像頭進行SLAM的做法稱爲單目SLAM（monocular SLAM）。

照片本質上是拍照時的場景（scene），在相機的成像平面上留下的一個投影，以二維的形式反映了三維的世界。這個過程丟掉了場景的一個維度：即深度（或距離）。在單目相機中，無法通過單個圖片來計算場景中物體離我們的距離（遠近）。在單張圖像裏，無法確定一個物體的真實大小。它可能是一個很大但很遠的物體，也可能是一個很近但很小的物體。由於近大遠小的原因，它們可能在圖像中變成同樣大小的樣子。

單目SLAM必須移動相機才能估計運動（motion），及估計場景中物體的遠近和大小（結構，structure）。

1. 運動信息推測：如果相機往右移動，則圖像裏的東西就會往左邊移動；

2. 結構信息推測：近處的物體移動快，遠處的物體則運動緩慢。當相機移動時，這些物體在圖像上的運動，形成了視差。通過視差，能定量地判斷物體的遠近。

單目SLAM估計的軌跡、地圖與真實的軌跡、地圖相差一個因子，即尺度（scale）。由於單目SLAM無法僅憑圖像確定這個真實尺度，所以又稱爲尺度不確定性。

雙目相機和深度相機：通過某種手段測量物體離相機的距離，通過單個圖像恢復場景的三維結構，消除尺度不確定性。

雙目相機（stereo）：由兩個單目相機組成，兩相機之間的距離（基線，baseline）已知，通過基線可以估計每個像素的空間位置。

1. 通過雙目相機估計每個像素點的深度需要大量的計算並且結果不太可靠；

2. 雙目相機測量深度的範圍與基線有關，基線距離越大，能夠測量的距離越遠；

3. 雙目相機的距離估計是比較左右眼的圖像獲得的，不依賴其他傳感設備，室內、室外均可使用；

4. 雙目或多目相機的缺點是配置與標定均較爲複雜，其深度量程和精度受雙目的基線與分辨率限制，而且視差的計算非常消耗計算資源，需要使用GPU和FPGA設備加速，才能實時輸出整張圖像的距離信息。

深度相機（RGB-D相機）：通過紅外結構光或time-of-flight（ToF）原理，通過主動向物體發射光並接收返回的光，測出物體離相機的距離。距離是由物理測量得到，因此節省了大量計算。現階段，多數RGB-D相機存在測量範圍窄、噪聲大、視野小、易受日光干擾、無法測量透射材質等諸多問題。深度相機主要用於室內SLAM，室外則較難應用。

2.2 經典視覺SLAM框架

視覺SLAM流程：

1. 傳感器信息讀取，主要爲相機圖像信息的讀取和預處理。如果在機器人中，還可能有碼盤、慣性傳感器等信息的讀取和同步；

2. 視覺里程計（visual odometry，VO），視覺里程計任務是估算相鄰圖像間相機的運動，以及局部地圖的樣子。VO又稱爲前端（front end）；

3. 後端優化（optimization），後端接受不同時刻視覺里程計測量的相機位姿，以及迴環檢測的信息，對其優化，得到全局一致的軌跡和地圖。由於接在 VO之後，又稱爲後端（back end）。

4. 迴環檢測（loop closing），測判斷機器人是否曾經到達過先前的位置。如果檢測到迴環，它會把信息提供給後端進行處理。

5. 建圖（mapping），根據估計的軌跡，建立與任務要求對應的地圖。

2.2.1 視覺里程計

**視覺里程計（VO）**通過相鄰幀圖像估計相機運動，並恢復場景的空間結構。由於VO只計算相鄰時刻間的運動，而與歷史信息無聯，因此VO是一種短時記憶機制。

VO是視覺SLAM的關鍵問題之一。通過喫飯VO相鄰幀圖像間相機運動的估計，並將相鄰時刻的運動“串聯”，可以構成機器人的運動軌跡，從而解決了定位問題；同時，根據每時刻相機的位置，可以計算出各像素對應的空間點位置，繼而得到地圖。然而僅通過VO估計軌跡，將出現累計漂移（accumulating drift）（由於VO只估計相鄰圖像間的運動，而每次估計都存在誤差，先前時刻的誤差將會傳遞到下一時刻，因此，經過一段時間後，估計的軌跡將不再準確。）

後端優化和迴環檢測用於解決漂移（drift）問題，其中，迴環檢測負責把“機器人回到原始位置”的事情檢測出來；而後端優化則根據該信息，校正整個軌跡的形狀。

2.2.2 後端優化

後端優化主要處理SLAM過程中的噪聲，從帶有噪聲的數據中，估計整個系統的狀態，以及這個狀態估計的不確定性有多大，即最大後驗概率估計（maximum a posteriori，MAP）。

在SLAM框架中，前端負責爲後端提供待優化的數據，以及這些數據的初始值；後端負責整體的優化過程，它處理的只有數據，無需關心這些數據的來源。在視覺SLAM中，前端和計算機視覺相關，如圖像特徵提取與匹配等；後端主要是濾波與非線性優化算法。

早期的SLAM問題是一個狀態估計問題，即後端優化。SLAM問題的本質爲：對運動主體自身和周圍環境空間不確定性的估計。爲解決SLAM，需要狀態估計理論描述定位和建圖的不確定性，然後採用濾波器或非線性優化，估計狀態的均值和不確定性（方差）。

2.2.3 迴環檢測

迴環檢測又稱閉環檢測（loop closure detection），主要解決位置估計隨時間漂移的問題。假設機器人經過一段時間運動後回到了原點，迴環檢測通過識別原點計算位置估計值的漂移。迴環檢測的關鍵在於使機器人具有識別曾到達過的場景的能力。視覺迴環檢測通常基於圖像間相似性實現，其本質是一種計算圖像數據相似性的算法。

迴環檢測與“定位”和“建圖”密切相關，事實上，地圖存在的主要意義，是爲了讓機器人知曉自己到達過的地方。

檢測到迴環後，將A與B是同一個點的信息輸入後端優化算法。後端根據這些信息，可以把軌跡和地圖調整到符合迴環檢測的結果。如果能充分且正確的迴環檢測，就可以消除累積誤差，得到全局一致的軌跡和地圖。

2.2.4 建圖

**建圖（mapping）**是指構建地圖的過程。地圖是對環境的描述，構建過程視具體應用而定。

地圖爲度量地圖與拓撲地圖兩種。

度量地圖（metric map）：精確表示地圖中物體的位置關係，通常分爲**稀疏（sparse）與稠密（dense）**兩類：稀疏地圖對地圖中的物體進行抽象，選擇一部分具有代表意義的物體，稱爲路標（landmark），並忽略非路標部分，稀疏地圖是由路標組成的地圖；稠密地圖着重於建模所有物體。稠密地圖通常按照某種分辨率，由多個基本單元組成，二維地圖的基本單元爲網格（grid），三維地圖的基本單元爲體素（voxel）。一般基本單元包含佔據、空閒、未知三種狀態，以表達該單元內是否包含物體。當查詢某個空間位置時，稠密地圖能夠給出該位置是否可以通過的信息。

對於定位任務，可採用稀疏路標地圖，而對於導航任務時，通常需要稠密的地圖。由於稠密地圖需要存儲每個格點的狀態，因此佔用大量的存儲空間，而且多數情況下地圖的許多細節部分是無用的，此外，大規模度量地圖會出現一致性問題。

**拓撲地圖（topological map）**強調地圖元素之間的關係。拓撲地圖是一個圖（graph），由節點和邊組成，只考慮節點間的連通性。它放鬆了地圖對精確位置的需要，去掉地圖的細節，是一種緊湊表達方式。然而，拓撲地圖不擅長表達具有複雜結構的地圖。

2.3 SLAM問題的數學表述

假設地圖由$N$個路標（$\mathbf{y}_{1}, \mathbf{y}_{2}, \dots, \mathbf{y}_{N}$）組成的，在每個時刻$t_{k}$（$i = 1, 2, \dots, K$），機器人自身位置記爲$\mathbf{x}_{k}$，同時傳感器會觀測到一部分路標點，則“攜帶傳感器的機器人在環境中運動”可由如下兩件事情描述：

1. 運動：考慮從$t_{k - 1}$時刻到$t_{k}$時刻，機器人的位置$\mathbf{x}$如何變化；

2. 觀測：假設機器人在$t_{k}$時刻，於$\mathbf{x}_{k}$處探測到了某個路標$\mathbf{y}_{j}$，這件事情如何用數學語言來描述。

運動方程：描述運動的數學模型

$\mathbf{x}_{k} = f(\mathbf{x}_{k − 1}, \mathbf{u}_{k}, \mathbf{w}_{k}) \tag {2-1}$

其中，$\mathbf{u}_{k}$表示運動傳感器的讀數（輸入），$\mathbf{w}_{k}$表示噪聲。

觀測方程：描述機器人在位置$\mathbf{x}_{k}$處，觀測到某個路標點$\mathbf{y}_{j}$，產生一個觀測數據$\mathbf{z}_{k, j}$。

$\mathbf{z}_{k, j} = h(\mathbf{y}_{j}, \mathbf{x}_{k}, \mathbf{v}_{k, j}) \tag {2-2}$

其中，$\mathbf{v}_{k, j}$表示觀測噪聲。

針對不同的傳感器，這兩個方程的參數化形式不同。這兩個基本方程共同描述了SLAM基本問題：已知運動觀測的讀數$\mathbf{u}$ 、傳感器的讀數$\mathbf{z}$時，如何求解定位問題（估計$\mathbf{x}$）和建圖問題（估計$\mathbf{y}$）

$\begin{cases} \mathbf{x}_{k} & = & f(\mathbf{x}_{k − 1}, \mathbf{u}_{k}, \mathbf{w}_{k}) \\ \mathbf{z}_{k, j} & = & h(\mathbf{y}_{j}, \mathbf{x}_{k}, \mathbf{v}_{k, j}) \end{cases}$

SLAM問題可以建模成一個狀態估計問題：通過帶有噪聲的測量數據，估計內部的、隱藏着的狀態變量。狀態估計問題的求解與兩個方程的具體形式，以及噪聲分佈有關。按照運動和觀測方程是否爲線性，噪聲是否服從高斯分佈，狀態估計問題可分爲線性、非線性和高斯、非高斯系統，其中線性高斯系統（linear gaussian，LG）最簡單，它的無偏的最優估計可以由卡爾曼濾波器（Kalman filter，KF）給出。對於複雜非線性非高斯系統（non-linear non-Gaussian，NLNG），可使用擴展卡爾曼濾波器（extended Kalman filter，EKF）和非線性優化兩大類方法求解。

早期實時視覺SLAM系統是基於EKF開發的。隨後，爲克服EKF的缺點（線性化誤差、噪聲高斯分佈假設），人們開始使用粒子濾波器（particle filter）和非線性優化方法。當今主流視覺SLAM使用以圖優化（graph optimization）爲代表的優化技術進行狀態估計。

三維空間的運動由三個軸構成，所以機器人的運動要由三個軸上的平移，以及繞着三個軸的旋轉來描述，稱爲六自由度位姿（位置和姿態）。