一.SLAM介紹

即使定位和建圖問題（simultaneous localization and mapping），一般簡稱為SLAM， 也稱作（Concurrent Mapping and Localization, CML）。當(dāng)機器人沒有環(huán)境地圖或者不清楚自身的位置時就會產(chǎn)生SLAM問題。機器人所具有的信息就只有歷次測量值$z_{1:t}$和控制量$u_{1:t}$。 在SLAM問題中，機器人構(gòu)建地圖的同時確定自生相對于地圖的位置。SLAM問題遠比我們目前討論過的問題困難的多。SLAM 問題比到目前為止討論過的機器人領(lǐng)域的所有其他問題都要難解決得多。該問題比定位問題更困難，是因為地圖是未知的，并且不得不沿著路徑估計該地圖。該問題也比給定位姿構(gòu)建地圖要困難，因為位姿是未知的，并且不得不沿路徑估計姿態(tài)。

從概率學(xué)的角度來看，SLAM問題有兩種主要的形式，它們具有同等重要的地位。其一稱為在線SLAM問題online SLAM problem，它估計的是瞬時位姿以及地圖的后驗概率:
$$p(x_t,m|z_{1:t},u_{1:t})\text{\hspace{1em}(1.1)}$$
其中$x_t$是時刻t的位姿，m則是地圖，而$z_{1:t}$和$u_{1:t}$分別是歷次測量值和控制量。因為只包含了時刻t 的變最估計問題，該問題被稱為在線SLAM 問題。在線SLAM 問題的許多算法是增量的。在這些算法中，過去的測量值和控制量一旦被處理即被丟棄。

第二種SLAM問題成為完全SLAM問題full SLAM problem。在完全SLAM中，我們嘗試計算整個路徑$x_{1:t}$以及地圖的后驗概率，而不僅僅是當(dāng)前的位置$x_t$，如圖10.2所示。 $$p(x_{1:t},m|z_{1:t},u_{1:t})\text{\hspace{1em}(1.2)}$$ 在線和完全SLAM問題之間的細微差異在于，他們使用了不同類型的算法。特別的，在線SLAM問題是對完全SLAM問題的歷史位姿的積分 $$\begin{gathered} p(x_t,m|z_{1:t},u_{1:t}) \\ =\int \int ?\int p(x_{1:t},m|z_{1:t},u_{1:t})d{x}_{1}d{x}_2?d{x}_{t?1}\text{\hspace{1em}(1.3)} \end{gathered}$$

上圖為在線SLAM 問題的圖示模型（在線SLAM 的目標(biāo)是估計機器人當(dāng)前位姿和地圖的后驗）

上圖為全SLAM 問題的圖示模型（計算關(guān)千整個機器人路徑和地圖的聯(lián)合后驗）

SLAM 問題的第二個關(guān)鍵特點，與估計問題的本質(zhì)有關(guān)。SLAM 問題擁有連
續(xù)和離散的要素。連續(xù)的估計問題涉及地圖中物體的定位和機器人自身位姿變量。在基于特征的表示方式中，物體可能是地標(biāo)，或者是由測距傳感器探測到的某些部分。離散特性與一致性有關(guān)：當(dāng)物體被檢測到時， SLAM 算法推理該物體與之前被檢測到的物體之間的聯(lián)系。該推理過程是離散的：該物體要么與之前探測到的物體是同一個，要么不是。

有時，明確一致性變量是非常有用的，在線SLAM 后驗由下式給定：
$$p(x_t,m,c_t|z_{1:t},u_{1:t})\text{\hspace{1em}(1.4)}$$

全SLAM 后驗由下式給定：
$$p(x_{1:t},m,c_{1:t}|z_{1:t},u_{1:t})\text{\hspace{1em}(1.5)}$$

在線后驗由全后驗對過去機器人位姿積分和對所有過去一致性求和得到：
$$\begin{gathered} p(x_t,m,c_t|z_{1:t},u_{1:t}) \\ =\int \int ?\int \sum _{c_1}\sum _{c_2}?\sum _{c_{t?1}}p(x_{1:t},m,c_{1:t}|z_{1:t},u_{1:t})d{x}_{1}d{x}_2?d{x}_{t?1}\text{\hspace{1em}(1.6)} \end{gathered}$$

在兩個SLAM 問題版本（即在線SLAM 和全SLAM) 中，估計全后驗的
式(1. 4) 或式(1. 5) 是SLAM 問題的黃金定律。全后驗捕捉所有關(guān)千地圖
和位姿或路徑的已知內(nèi)容。
在實際應(yīng)用中，計算全后驗常是不可實現(xiàn)的。其原因有兩個：①連續(xù)參數(shù)空
間的高維數(shù)；②離散一致性變量的數(shù)量巨大。許多先進的SLAM 算法用數(shù)以萬計或更多的特征來構(gòu)建地圖。即使在已知一致性的情況下，只是這些地圖的后驗就包含了$10^5$甚至更多維的概率分布。這與定位問題形成了鮮明的對比，定位問題的后驗是在三維連續(xù)空間上的估計。而且，在多數(shù)應(yīng)用中，一致性是未知的。一致性變量$c_{1:t}$的可能分配的數(shù)量隨時間呈指數(shù)增長。因此，可用的能處理一致性問題的SLAM 算法必須依賴近似。

二.SLAM的性質(zhì)

• 形式
  該特征與它的兩種形式有關(guān)。在線SLAM問題與地圖一起計算當(dāng)前姿勢的后驗，而完整SLAM問題與地圖一起計算整個路徑的后驗。

• 性質(zhì)
  SLAM問題的第二個關(guān)鍵特征與其性質(zhì)有關(guān)。SLAM問題通常具有連續(xù)和離散的元素。

  • 連續(xù)
    讓我們從SLAM問題的連續(xù)部分開始。
    在SLAM期間，機器人會不斷收集測距信息以估算機器人的姿勢，并不斷感應(yīng)環(huán)境以估算物體或地標(biāo)的位置。因此，機器人的姿勢和物體位置都是SLAM問題的連續(xù)方面。

  • 離散
    現(xiàn)在，轉(zhuǎn)到SLAM問題的第二部分。
    正如之前提到的，機器人不斷地感知環(huán)境以估計對象的位置，這樣做時，SLAM算法必須識別在任何新檢測到的對象與先前檢測到的對象之間是否存在關(guān)系。這有助于機器人了解它之前是否曾經(jīng)在同一位置。在每個時刻，機器人都必須回答以下問題：“我以前來過這里嗎？”。這個問題的答案是二進制-是或否-這就是使對象之間的關(guān)系成為SLAM問題的離散組成部分的原因，即對應(yīng)關(guān)系已知對象之間的這種離散關(guān)系。

SLAM挑戰(zhàn)

在SLAM下，計算由機器人姿態(tài)，地圖和對應(yīng)關(guān)系組成的整個后驗對機器人技術(shù)提出了很大的挑戰(zhàn)，這主要是由于連續(xù)和離散的部分。

連續(xù)
由機器人姿勢和對象位置組成的連續(xù)參數(shù)空間是高維的。在繪制環(huán)境并進行自身定位時，機器人會遇到許多物體，并且必須跟蹤其中的每個物體。因此，變量的數(shù)量將隨時間增加，這使問題具有高度的維度，并且難以計算后驗。

離散
接下來，離散參數(shù)空間由對應(yīng)值組成，并且由于大量的對應(yīng)變量而具有很高的維數(shù)。不僅如此，由于機器人將繼續(xù)感測環(huán)境并將新檢測到的對象與先前檢測到的對象相關(guān)聯(lián)，因此對應(yīng)值隨時間呈指數(shù)增長。即使假設(shè)已知的對應(yīng)值，在地圖上的后驗也仍然是高維的。

因此，SLAM算法在估計后驗時必須依賴于近似，以節(jié)省計算內(nèi)存。

三、FastSLAM

1.FastSLAM簡介

FastSLAM算法解決了全部問題SLAM已知對應(yīng)。

• 估計軌跡： FastSLAM使用粒子過濾器方法估計軌跡的后驗。這將為SLAM提供一個優(yōu)勢，以解決已知姿勢的映射問題。
• 估計地圖： FastSLAM使用低維擴展卡爾曼濾波器來求解使用局部高斯模型建模的地圖的獨立特征。

用Rao-Blackwellized粒子過濾器方法已知用粒子過濾器和高斯表示后驗的慣用方法。

我們已經(jīng)看到，FastSLAM算法可以解決具有已知對應(yīng)關(guān)系的完整SLAM問題。由于FastSLAM使用粒子濾波器方法來解決SLAM問題，因此一些機器人專家認為它是一種能夠解決Full SLAM和Online SLAM問題的強大算法。

FastSLAM估計完整的機器人路徑，因此可以解決Full SLAM問題。
另一方面，FastSLAM中的每個粒子都估計瞬時姿勢，因此FastSLAM也解決了在線SLAM問題。

現(xiàn)在，存在FastSLAM算法的三個不同實例。

• FastSLAM 1.0
  FastSLAM 1.0算法既簡單又易于實現(xiàn)，但是由于粒子濾波器會導(dǎo)致樣本效率低下，因此該算法效率不高。

• FastSLAM 2.0
  FastSLAM 2.0算法通過施加不同的分布來克服FastSLAM 1.0的效率低下問題，這導(dǎo)致了較少的粒子數(shù)量。請記住，FastSLAM 1.0和2.0算法都使用低維擴展卡爾曼濾波器來估計地圖特征上的后驗。

• 基于網(wǎng)格的FastSLAM
  FastSLAM的第三個實例實際上是對FastSLAM的擴展，稱為基于網(wǎng)格的FastSLAM算法，該算法使FastSLAM適應(yīng)于網(wǎng)格圖。在本課程中，您將學(xué)習(xí)基于網(wǎng)格的FastSLAM。

2.基于網(wǎng)格的FastSLAM技術(shù)

為了使FastSLAM適應(yīng)網(wǎng)格映射，我們需要三種不同的技術(shù)：

1. Sampling Motion- $p(x_t|x_{t?1}^{[k]},u_t)$：給定第k個粒子的先前姿態(tài)，并根據(jù)當(dāng)前控制$u$估算當(dāng)前姿態(tài)。
2. Map Estimation- $p(m_t|z_t,x_t^{[k]},m_{t?1}^{[k]})$：根據(jù)當(dāng)前測量值，當(dāng)前第k個粒子姿態(tài)和先前的第k個粒子圖估計當(dāng)前圖
3. 重要性權(quán)重- $p(z_t|x_t^{[k]},m^{[k]})$：根據(jù)當(dāng)前第k個粒子姿態(tài)和當(dāng)前第k個粒子圖，估計當(dāng)前測量的可能性。

基于網(wǎng)格的FastSLAM算法

3.gmapping ROS包

gmapping提供基于激光的SLAM。這意味著可以向其節(jié)點提供機器人激光測量值和里程計值，并期望它提供環(huán)境的2D占用柵格圖。當(dāng)機器人移動并使用其激光測距儀傳感器收集感官信息時，地圖將更新。

該軟件包包含用于OpenSlam Gmapping的ROS包裝器。gmapping軟件包提供了基于激光的SLAM（同時定位和映射），作為稱為slam_gmapping的ROS節(jié)點。使用slam_gmapping，可以根據(jù)激光和移動機器人收集的數(shù)據(jù)來創(chuàng)建二維占用柵格圖（如建筑物平面圖）。

更多參考此處。

創(chuàng)建catkin_ws

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace
$ cd ..
$ catkin_make

執(zhí)行系統(tǒng)更新/升級

$ apt-get update
$ apt-get upgrade -y

報錯：

W: GPG error: http://packages.ros.org trusty InRelease: The following signatures couldn't be verified because the public key is not available: NO_PUBKEY F42ED6FBAB17C654

解決辦法：

sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys <PUBKEY>

這里的<PUBKEY>就是丟失的public key。例如F42ED6FB17C654。

在src中應(yīng)用turtlebot_gazebo和turtlebot_teleop

$ cd src/
$ git clone https://github.com/turtlebot/turtlebot_simulator
$ git clone https://github.com/turtlebot/turtlebot

安裝軟件包依賴項

$ cd ..
$ source devel/setup.bash
$ rosdep -i install turtlebot_gazebo
$ rosdep -i install turtlebot_teleop

編譯軟件包

$ catkin_make
$ source devel/setup.bash

在Willow Garage環(huán)境中部署Turltebot

$ roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=worlds/willowgarage.world

現(xiàn)在，將gmapping包與機器人對接以執(zhí)行SLAM。

解決辦法：

$ cd ~/
$ hg clone https://bitbucket.org/osrf/gazebo_models

下載完成后講gazebo_models復(fù)制到~/.gazebo文件夾中，重命名為models

Terminal 1
在Willow環(huán)境中啟動Turtlebot

$ cd ~/catkin_ws
$ source devel/setup.bash
$ roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=worlds/willowgarage.world 

Turtlebot should now appear in a Willow Garage environment.

Terminal 2
運行 keyboard teleop node

$ cd ~/catkin_ws
$ source devel/setup.bash
$ roslaunch turtlebot_teleop keyboard_teleop.launch

Don’t move your robot yet!

Terminal 3
運行 slam_gmapping node

$ cd ~/catkin_ws
$ source devel/setup.bash
$ rosrun gmapping slam_gmapping

Terminal 4
運行rviz并訂閱不同的已發(fā)布主題以可視化地圖

$ rosrun rviz rviz

如下編輯rviz配置：

將固定框架更改為map
保持參考系為默認
添加機器人模型
添加相機并選擇/camera/rgb/image_raw主題
添加地圖并選擇/map主題

Terminal 5

$ cd /home/workspace/
$ rosrun map_server map_saver -f myMap

運行map_server將生成map.pgm和map.yaml文件：

但是我發(fā)現(xiàn)我的rviz一直無法獲得地圖。猜測可能是tf信息轉(zhuǎn)換有問題，我使用了他給的整合包無法解決這個問題。

整合包在此處。

總結(jié)

以上是生活随笔為你收集整理的Udacity机器人软件工程师课程笔记（三十五) - SLAM - 基于网格的FastSLAM的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。