科研筆記(二) 一篇文章帶你走進SLAM的前半生

Hello，everyone！
我是CS科研GO，一個熱衷於科研和軟開的胖子！
論文題目：Simultaneous Localization and Mapping: Part I
論文作者：Hugh Durrant-Whyte，Tim Bailey；工作單位：悉尼大學，澳大利亞；
發表期刊：IEEE ROBOTICS & AUTOMATION MAGAZINE（SCI二區）。

話不多說，先來張SLAM效果圖

一、SLAM的早期發展史

1.1 SLAM問題起源

SLAM問題起源於1986年舉行的IEEE Robotics and Automation Conference (San Francisco, California)。會議結束後，參會的研究人員做了許多相關的理論工作。

1.2 理論發展受阻

1990年，Smith等人的研究表明，對於定位和地圖構建問題的完整解決方案需要估算機器人(載具)姿態和每個地標位置組成的聯合狀態，並在每次觀察地標之後進行更新。這將要求估算函數採用複雜的狀態向量(按地圖中維護的地標數量的順序)，計算規模爲地標數量的平方。此外，這項工作沒有考慮地圖映射的收斂性或穩定性，這意味着問題的計算複雜度很高，而且地圖估計的誤差不會收斂。

1.3 理論突破

1997年，Univ. Oxford博士生M. Csorba給出了SLAM問題的收斂性理論和初始結果。

1.4 理論改進

在97年的理論工作基礎上，研究人員關注改進計算能效性，解決數據關聯和迴環問題。1999年International Symposium on Robotics Research (ISRR)首次舉辦SLAM研討會，S. Thrun等人將基於Kalman濾波器的SLAM方法與概率定位和映射方法進行了一定程度的融合。

1.5 研究熱點

2002年，ICRA的SLAM研討會吸引了150位興趣廣泛的研究人員；由H. Christiansen同年舉辦的SLAM暑期學校吸引了來自世界各地的重要研究人員和50名博士生，在此領域的建設上取得了巨大影響。

二、SLAM理論及演變過程

SLAM是一個過程，移動機器人可以通過該過程構建環境地圖，並同時使用該地圖推斷其位置。在SLAM中，無需任何先驗位置知識即可在線估算機器人的軌跡和所有地標的位置。

2.1 預備知識

圖1描述一個基本的SLAM過程。在時刻$t$，定義以下變量：

• $x_k$: 描述機器人（載具）位置和方向的狀態向量；
• $u_k$: 控制向量，用於描述機器人從$t-1$時刻到$t$時刻的變化；
• $m_i$: 描述第$i$個地標位置的向量，假定地標位置不隨時間發生改變；
• $z_{ik}$: 在$k$時刻，機器人對第$i$個地標位置的觀測值；若此時觀測到多個地標，或無需關注的地標，則用$z_k$表示。

此外，定義以下集合：

• $X_{0: k}=\left\{x_{0}, x_{1}, \ldots x_{k}\right\}=\left\{X_{0: k-1}, x_{k}\right\}$: 表示機器人的歷史位置；
• $U_{0: k}=\left\{u_{1}, u_{2} \ldots u_{k}\right\}=\left\{U_{0: k-1}, u_{k}\right\}$: 表示控制輸入的歷史數據；
• $m=\left\{m_{1}, m_{2} \dots m_{n}\right\}$: 表示所有地標的集合；
• $Z_{0: k}=\left\{z_{1}, z_{2}, \ldots z_{k}\right\}=\left\{Z_{0: k-1}, z_{k}\right\}$: 表示所有地標觀測的集合。

2.2 SLAM問題的概率分佈

$P\left(x_{k}, m | Z_{0: k}, U_{0: k}, x_{0}\right)$

表達式描述了，當給定$k$時刻的地標觀測值，控制輸入值和機器人初始狀態時，此時地標位置和機器人狀態的聯合後驗概率密度。通常，SLAM問題可以遞歸求解。從$k−1$時刻的分佈$P\left(x_{k-1}, m | Z_{0: k-1}, U_{0: k-1}, x_{0}\right)$的估計值開始，根據$u_k$和$z_k$，使用貝葉斯定理計算$P\left(x_{k}, m | Z_{0: k}, U_{0: k}, x_{0}\right)$。該計算要求定義狀態轉換(運動)模型和觀測模型，分別描述控制輸入和觀測的影響。

• 運動模型：$P\left(x_{k} | x_{k-1}, u_{k}\right)$，狀態轉換被假定爲一個馬爾科夫過程，在這個過程中，下一個狀態$x_k$只依賴於緊鄰前一個狀態$x_{k-1}$,和應用的控制$u_k$，並且獨立於觀測值和地圖。

• 觀測模型：$P\left(z_{k} | x_{k}, m\right)$，描述機器人位置和地標位置已知時，觀測$z_k$的概率。

SLAM算法可分爲以下兩步：

上述公式提供了一個遞歸過程，用於計算聯合後驗分佈$P\left(x_{k}, m | Z_{0: k}, U_{0: k}, x_{0}\right)$。此遞歸是運動模型$P\left(x_{k} | x_{k-1}, u_{k}\right)$和觀測模型$P\left(z_{k} | x_{k}, m\right)$的函數。

2.3 SLAM概率分佈討論

爲方便討論，將$P\left(x_{k}, m | Z_{0: k}, U_{0: k}, x_{0}\right)$簡寫爲$P\left(x_{k}, m | z_k\right)$。由於觀測模型$P\left(z_{k} | x_k,m\right)$表明地標觀測值和機器人和地標之間存在依賴關係，因此$P\left(x_{k}, m | z_{k}\right) \neq P\left(x_{k} | z_{k}\right) P\left(m | z_{k}\right)$。

回顧圖1，地標的估計值和真實值之間存在誤差，然而，這些誤差是高度相關的。更重要的是，隨着越來越多的觀測，地標間估計值的相關性單調遞增。實際上，這意味着無論機器人如何運動，對地標的相對位置的認知總是在提高(收斂)，而不是發散。

三、SLAM解決方案

3.1 EKF-SLAM

• 運動模型：$x_{k}=f\left(x_{k-1}, u_{k}\right)+w_{k}$，其中，$f(.)$指非線性的系統狀態轉換函數，$w_k$指運動過程中的預測誤差，服從$N(0,Q)$的高斯白噪聲。
• 觀測模型：$z_{k}=h\left(x_{k}, m\right)+v_{k}$，其中，$h(.)$是非線性測量模型，$v_k$代表觀測誤差，服從$N(0,R)$的高斯白噪聲。

其中，$∇f$是$f(.)$的雅可比矩陣。

其中，$∇h$是$h(.)$的雅可比矩陣。

EKF-SLAM的缺點

1. 算法的時間複雜度爲$n^2$，$n$代表錨點的數量。
2. EKF假定機器人的運動和觀察存在高斯噪聲。在這種情況下，傳感器中的不確定性必須相對較小，因爲EKF中的線性過程可能會引入無法忍受的噪聲。
3. EKF代表高斯後驗模型，它是一種單峯分佈，僅對處理後驗圍繞真實狀態且不確定性很小的問題時纔有效。

3.2 FastSLAM

FastSLAM以遞歸蒙特卡羅採樣或粒子濾波爲基礎，首次直接表示了非線性過程模型和非高斯姿態分佈。FastSLAM的基本結構是一個R-B狀態(求最優解過程)，軌跡由加權樣本表示，並通過解析計算得到地圖。算法基本思想是通過粒子濾波對運動狀態進行遞歸估計，通過EKF對地圖映射狀態進行遞歸估計。

對於SLAM, R-B粒子濾波的一般形式如下。

假設在$t-1$時刻，聯合狀態(機器人位置姿態，觀測地標位置姿態)可以用$\left\{w_{k-1}^{(i)}, X_{0: k-1}^{(i)}, P\left(m | X_{0: k-1}^{(i)}, Z_{0: k-1}\right)\right\}_{i}^{N}$表示，則有：

1. 對於每個粒子，以特定的粒子歷史爲條件，計算提出的分佈，並從中提取樣本
   $\mathbf{x}_{k}^{(i)} \sim \pi\left(\mathbf{x}_{k} | \mathbf{X}_{0: k-1}^{(i)}, \mathbf{Z}_{0: k}, \mathbf{u}_{k}\right)$

2. 根據重要的函數對樣本進行加權，下面方程的分子項分別爲觀測模型和運動模型
   
   

3. 如果有必要，執行重採樣。(什麼時候重採樣是一個未解決的問題。有些方案會對每個時刻重採樣，有些則在固定時間間隔後取樣，還有一些則在權重方差超過閾值時取樣)重採樣是通過從集合$\left\{X_{0: k}^{(i)}\right\}_{i}^{N}$中選擇粒子，並替換它們，包括它們的相關映射來完成的，選擇的概率與$w_{k}^{(i)}$成正比。選定的粒子給予均勻的權值， $w_{k}^{(i)}=1 / \mathrm{N}$。

4. 對於每個粒子，用已知姿態的簡單映射操作對觀測到的地標進行EKF更新。

與EKF-SLAM相比，FastSLAM具有許多優勢。

1. FastSLAM可以在時間上以錨的數量爲對數實現，與普通的EKF-SLAM相比具有計算優勢。
2. FastSLAM提供了顯著的數據關聯魯棒性，因爲它可以保持多個數據關聯的後驗，而不僅僅是基於EKF-SLAM中包含的增量最大似然數據關聯，在任何時間點僅跟蹤單個數據關聯。
3. FastSLAM還可以處理非線性模型，因爲粒子濾波器可以處理非線性機器人運動模型，並且可以保證在某些假設下收斂。

四、SLAM實現

截至2006年，SLAM領域的研究人員編寫了演示SLAM的軟件，分別在MATLAB、C++和Java中實現；此外，他們還提供了一些來自真實環境中傳感器數據集，這是評估各種SLAM算法的寶貴資源。具體內容見下表。

內容靠得住，關注不迷路。