SLAM：同時定位和建圖（Simultaneous Localization and Mapping）。

希望機器人從未知環境的未知地點出發，在運動過程中通過重復觀測到的地圖特征（比如，墻角，柱子等）定位自身位置和姿態，再根據自身位置增量式的構建地圖，從而達到同時定位和地圖構建的目的。

所用傳感器：激光雷達，相機（單目，雙目，RGBD，事件）

SLAM框架：
傳感器信息提取—前端視覺里程計—后端優化—建圖
傳感器信息提取—回環檢測—后端優化（這兩條線路之間形成一個閉環）

傳感器信息提取：SLAM中傳感器信息的提取和預處理，往往是多種傳感器信息的融合。

前端視覺里程計（VO）：通過估算相鄰圖像間的關系來推導相機的運動，以及局部地圖的樣子。

后端優化：接受不同時刻視覺里程計測量的相機位姿以及回環檢測的信息，并對他們進行優化（往往是非線性優化），進而得到全局地圖和軌跡。

回環檢測：用于判斷機器人是否到達先前預定的位置，用于解決位置估計隨時間漂移的問題。

建圖：根據估計的軌跡，建立相應要求的地圖（柵格地圖，拓撲地圖，點云地圖，網格地圖）。

綜上，定位與建圖便完成了。

SLAM數學表達
此處分為4個部分進行闡述：運動方程及李代數優化，觀測方程，非線性優化。

**1. 運動方程：**從K-1時刻運動到K時刻，位姿X變化的數學表示。
可以表述為一個方程：X_k = F(X_(k-1), U_k, W_k)
其中U_k表示傳感器輸入，W_k表示過程中的噪聲，F往往表示非線性函數。

為了進一步從數學角度來理解空間變換，需要了解一下旋轉矩陣、變換矩陣。

1.1 旋轉矩陣
我們用一個3X1的矩陣來表示物體的位置，3X3的矩陣來表示物體的姿態。
物體在空間里的運動可以分解為平移加旋轉，平移操作只需要坐標的簡單相加，旋轉則涉及到坐標系的變換，下面對旋轉矩陣進行分析：
設某個單位正交基(e1，e2，e3) 經過一次旋轉，變成了(e1, e2, e3)。那么，對于同一個向量a（該向量并沒有隨著坐標系的旋轉而發生運動），它在兩個坐標系下的坐標為[a1, a2, a3]T 和[a1, a2, a3]。根據坐標的定義，有：

上面等式左右同乘以[a1, a2, a3`]T，變成：

我們把R稱為旋轉矩陣，它描述了旋轉前后同一個向量的坐標變換關系。通過旋轉矩陣，我們便可以求解物體的旋轉變換了。

1.2 變換矩陣
有了旋轉矩陣和平移矩陣，便可以描述一個物體的空間變換了，不過還有一個遺憾，便是操作的復雜性，因為目前變換前后的坐標不是一個線性關系，而是兩次變換（平移和旋轉）。為了使其變為一次線性操作，我們提出了變換矩陣的概念：
將平移矩陣和旋轉矩陣按照如下規律合在一起，便稱之為變換矩陣T：

，易知，后式成立：

式中[a, i]T為齊次坐標（四維向量）。

如此，變換前后的坐標便成了線性關系。

但是，這樣的表示還不夠，因為旋轉矩陣一共有9個量，但是它只描述了3個自由度的旋轉，這便產生了冗余。同理，變換矩陣有16個量，但只描述了6個自由度。于是，我們就尋找更加緊湊的表達。

書中給出了很多種表達，如旋轉向量、歐拉角和四元數等，但是由于前兩種三維向量描述方式都帶有奇異性（所謂奇異性，即在某些情況下這種描述方式不管用），所以常用的還是四元數，此處僅介紹一下四元數：

四元數是一種類似于復數的代數，由四個部分組成，即是緊湊的，也沒有奇異性。
一個四元數q 擁有一個實部和三個虛部。q = q0 + q1i + q2j + q3k;
其中i; j; k 為四元數的三個虛部。這三個虛部滿足關系式：

假設某個旋轉是繞單位向量n = [nx; ny; nz]T 進行了角度為 theta 的旋轉，那么這個旋轉的四元數形式為：

具體表示：假設一個空間三維點p = [x; y; z]，以及一個由軸角n; theta 指定的旋轉。三維點p 經過旋轉之后變成為p′。如果使用矩陣描述，那么有p′ = Rp。如果用四元數描述旋轉:
首先，把三維空間點用一個虛四元數來描述: p = [0; x; y; z]
用四元數q 表示這個旋轉：

旋轉后的點p′ 即可表示為這樣的乘積：

至此，我們已經可以用四元數和一個平移矩陣來表示物體的空間運動了。

2. 運動方程的李代數優化

目前，我們已經知道了三維世界中剛體運動的表示方式，但是在SLAM 中，除了表示之
外，我們還要對它們進行估計和優化。因為在 SLAM 中位姿是未知的，而我們需要解決什
么樣的相機位姿最符合當前觀測數據這樣的問題。

一種典型的方式是把它構建成一個優化問題，求解最優的R; t，使得誤差最小化。

但是，由于旋轉矩陣自身是帶有約束的（正交且行列式為1）。所以它們作為優化變量時，會引入額外的約束，使優化問題變得困難。于是我們提出了通過李群——李代數間的轉換關系，把位姿估計變成無約束的優化問題，簡化求解方式。

李群：下節再講，好累啊，感覺在做無用功···

3. 觀測方程：在X_k位置上觀測到路標點Y_i時，產生的觀測數據Z_(k,j).
Z_(k,j) = H(Y_i, X_k, V_(k,j))
其中V_(k,j)表示此時的噪聲

總結

以上是生活随笔為你收集整理的视觉SLAM十四讲：运动方程的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。