機(jī)器人研究的問題包含許許多多的領(lǐng)域，我們常見的幾個研究的問題包括：建圖(Mapping)、定位(Localization)和路徑規(guī)劃（Path Planning），如果機(jī)器人帶有機(jī)械臂，那么運動規(guī)劃（Motion Planning）也是重要的一個環(huán)節(jié)。而同步定位與建圖（SLAM）問題位于定位和建圖
的交集部分。
SLAM需要機(jī)器人在未知的環(huán)境中逐步建立起地圖，然后根據(jù)地區(qū)確定自身位置，從而進(jìn)一步定位。
ROS中SLAM的一些功能包，也就是一些常用的SLAM算法，例如Gmapping、Karto、Hector、Cartographer等算法。我們不會去關(guān)注算法背后的數(shù)學(xué)原理，而是更注重工程實現(xiàn)上的方法，告訴你SLAM算法包是如何工作的，怎樣快速的搭建起SLAM算法

地圖

ROS中的地圖很好理解，就是一張普通的灰度圖像，通常為pgm格式。這張圖像上的黑色像素表示障礙物，白色像素表示可行區(qū)域，灰色是未探索的區(qū)域

在SLAM建圖的過程中，你可以在RViz里看到一張地圖被逐漸建立起來的過程，類似于一塊塊拼圖被拼接成一張完整的地圖。這張地圖對于我們定位、路徑規(guī)劃都是不可缺少的信息。事實上，地圖在ROS中是以Topic的形式維護(hù)和呈現(xiàn)的，這個Topic名稱就叫做 /map ，它的消息類型是nav_msgs/OccupancyGrid

鎖存

由于 /map 中實際上存儲的是一張圖片，為了減少不必要的開銷，這個Topic往往采用鎖存（latched）的方式來發(fā)布。什么是鎖存？其實就是：地圖如果沒有更新，就維持著上次發(fā)布的內(nèi)容不變，此時如果有新的訂閱者訂閱消息，這時只會收到一個 /map 的消息，也就是上次發(fā)布的消息；只有地圖更新了（比如SLAM又建出來新的地圖），這時 /map 才會發(fā)布新的內(nèi)容。 鎖存器的作用就是，將發(fā)布者最后一次發(fā)布的消息保存下來，然后把它自動發(fā)送給后來的訂閱者。這種方式非常適合變動較慢、相對固定的數(shù)據(jù)（例如地圖），然后只發(fā)布一次，相比于同樣的消息不定的發(fā)布，鎖存的方式既可以減少通信中對帶寬的占用，也可以減少消息資源維護(hù)的開銷。

nav_msgs/OccupancyGrid

然后我們來看一下地圖的OccupancyGrid類型是如何定義的，你可以通過 rosmsg shownav_msgs/OccupancyGrid 來查看消息，或者直接 rosed nav_msgs OccupancyGrid.msg 來查看srv文件。

std_msgs/Header header #消息的報頭
uint32 seq
time stamp
string frame_id #地圖消息綁定在TF的哪個frame上，一般為map
nav_msgs/MapMetaData info #地圖相關(guān)信息
time map_load_time #加載時間
float32 resolution #分辨率 單位：m/pixel
uint32 width #寬 單位：pixel
uint32 height #高 單位：pixel
geometry_msgs/Pose origin #原點
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
int8[] data #地圖具體信息

這個srv文件定義了/map話題的數(shù)據(jù)結(jié)構(gòu)，包含了三個主要的部分:header, info和data。
header是消息的報頭，保存了序號、時間戳、frame等通用信息，info是地圖的配置信息，它反映了地圖的屬性，data是真正存儲這張地圖數(shù)據(jù)的部分，它是一個可變長數(shù)組， int8 后面加了 [] ，你可以理解為一個類似于vector的容器，它存儲的內(nèi)容有width*height個int8型的數(shù)據(jù),也就是這張地圖上每個像素。

Gmapping

Gmapping SLAM軟件包

Gmapping算法是目前基于激光雷達(dá)和里程計方案里面比較可靠和成熟的一個算法，它基于粒子濾波，采用RBPF的方法效果穩(wěn)定，許多基于ROS的機(jī)器人都跑的是gmapping_slam。這個軟件包位于ros-perception組織中的slam_gmapping倉庫中。 其中的 slam_gmapping 是一個metapackage，它依賴了 gmapping ，而算法具體實現(xiàn)都在 gmapping 軟件包中，該軟件包中的 slam_gmapping 程序就是我們在ROS中運行的SLAM節(jié)點。如果你感興趣，可以閱讀一下 gmapping 的源代碼。
如果你的ROS安裝的是desktop-full版本，應(yīng)該默認(rèn)會帶gmapping。你可以用以下命令來檢測
gmapping是否安裝

apt-cache search ros-$ROS_DISTRO-gmapping

如果提示沒有，可以直接用apt安裝

sudo apt-get install ros-$ROS_DISTRO-gmapping

gmapping在ROS上運行的方法很簡單

rosrun gmapping slam_gmapping

但由于gmapping算法中需要設(shè)置的參數(shù)很多，這種啟動單個節(jié)點的效率很低。所以往往我們會把gmapping的啟動寫到launch文件中，同時把gmapping需要的一些參數(shù)也提前設(shè)置好，寫進(jìn)launch文件或yaml文件。 具體可參考教學(xué)軟包中的 slam_sim_demo中的 gmapping_demo.launch 和robot_gmapping.launch.xml文件。

Gmapping SLAM計算圖

gmapping的作用是根據(jù)激光雷達(dá)和里程計（Odometry）的信息，對環(huán)境地圖進(jìn)行構(gòu)建，并且對自身狀態(tài)進(jìn)行估計。因此它得輸入應(yīng)當(dāng)包括激光雷達(dá)和里程計的數(shù)據(jù)，而輸出應(yīng)當(dāng)有自身位置和地圖。 下面我們從計算圖（消息的流向）的角度來看看gmapping算法的實際運行中的結(jié)構(gòu)：

位于中心的是我們運行的 slam_gmapping 節(jié)點，這個節(jié)點負(fù)責(zé)整個gmapping SLAM的工作。它的輸入需要有兩個：

輸入

• /tf以及 /tf_static： 坐標(biāo)變換，類型為第一代的tf/tfMessage或第二代的 tf2_msgs/TFMessage 其中一定得提供的有兩個tf，一個是base_frame 與laser_frame之間的tf，即機(jī)器人底盤和激光雷達(dá)之間的變換；一個是base_frame與 odom_frame之間的tf，即底盤和里程計原點之間的坐標(biāo)變換。odom_frame可以理解為里程計原點所在的坐標(biāo)系
• /scan:激光雷達(dá)數(shù)據(jù)，類型為sensor_msgs/LaserScan

/scan很好理解，Gmapping SLAM所必須的激光雷達(dá)數(shù)據(jù)，而 /tf是一個比較容易忽視的細(xì)節(jié)。盡管 /tf 這個Topic聽起來很簡單，但它維護(hù)了整個ROS三維世界里的轉(zhuǎn)換關(guān)系，而 slam_gmapping 要從中讀取的數(shù)據(jù)是 base_frame 與 laser_frame之間的tf,只有這樣才能夠把周圍障礙物變換到機(jī)器人坐標(biāo)系下，更重要的是 base_frame與 odom_frame 之間的tf，這個tf反映了里程計（電機(jī)的光電碼盤、視覺里程計、IMU）的監(jiān)測數(shù)據(jù)，也就是機(jī)器人里程計測得走了多少距離，它會把這段變換發(fā)布到odom_frame 和laser_frame 之間。
因此 slam_gmapping 會從 /tf 中獲得機(jī)器人里程計的數(shù)據(jù)

輸出

• /tf ： 主要是輸出 map_frame 和 odom_frame 之間的變換
• /slam_gmapping/entropy ： std_msgs/Float64 類型，反映了機(jī)器人位姿估計的分散程度
• /map ： slam_gmapping 建立的地圖
• /map_metadata ： 地圖的相關(guān)信息

輸出的 /tf里又一個很重要的信息，就是 map_frame和 odom_frame 之間的變換，這其實就是對機(jī)器人的定位。通過連通 map_frame 和 odom_frame ，這樣map_frame與 base_frame 甚至與 laser_frame都連通了。這樣便實現(xiàn)了機(jī)器人在地圖上的定位。
同時，輸出的Topic里還有 /map na，在上一節(jié)我們介紹了地圖的類型，在SLAM場景中，地圖是作為SLAM的結(jié)果被不斷地更新和發(fā)布。

里程計誤差及修正

目前ROS中常用的里程計廣義上包括車輪上的光電碼盤、慣性導(dǎo)航元件（IMU）、視覺里程計，你可以只用其中的一個作為odom，也可以選擇多個進(jìn)行數(shù)據(jù)融合，融合結(jié)果作為odom。通常來說，實際ROS項目中的里程計會發(fā)布兩個Topic：

• /odom : 類型為 nav_msgs/Odometry ，反映里程計估測的機(jī)器人位置、方向、線速度、角速度信息。
• /tf : 主要是輸出 odom_frame 和 base_frame 之間的tf。這段tf反映了機(jī)器人的位置和方向變換，數(shù)值與 /odom 中的相同。

由于以上三種里程計都是對機(jī)器人的位姿進(jìn)行估計，存在著累計誤差，因此當(dāng)運動時間較長時， odom_frame 和 base_frame 之間變換的真實值與估計值的誤差會越來越大.你可能會想，能否用激光雷達(dá)數(shù)據(jù)來修正 odom_frame 和 base_frame 的tf。事實上gmapping不是這么做的，里程計估計的是多少， odom_frame 和base_frame 的tf就顯示多少，永遠(yuǎn)不會去修正這段tf。gmapping的做法是把里程計誤差的修正發(fā)布到 map_frame 和 odom_frame 之間的tf上，也就是把誤差補(bǔ)償在了地圖坐標(biāo)系和里程計原點坐標(biāo)系之間。通過這種方式來修正定位
這樣 map_frame 和 base_frame ，甚至和 laser_frame 之間就連通了，實現(xiàn)了機(jī)器人在地圖上的定位。

服務(wù)

slam_gmapping 也提供了一個服務(wù)：
/dynamic_map ： 其srv類型為nav_msgs/GetMap，用于獲取當(dāng)前的地圖
該srv定義如下： nav_msgs/GetMap.srv

# Get the map as a nav_msgs/OccupancyGrid
---
nav_msgs/OccupancyGrid map

可見該服務(wù)的請求為空，即不需要傳入?yún)?shù)，它會直接反饋當(dāng)前地圖。

參數(shù)

slam_gmapping 需要的參數(shù)很多，這里以 slam_sim_demo 教學(xué)包中的 gmapping_demo 的參數(shù)為例，注釋了一些比較重要的參數(shù)，具體請查看 ROS-Academy-for-Beginners/slam_sim_demo/launch/include/robot_gmapping.launch.xml

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="base_frame" value="$(arg base_frame)"/> <!--底盤坐標(biāo)系-->
<param name="odom_frame" value="$(arg odom_frame)"/> <!--里程計坐標(biāo)系-->
<param name="map_update_interval" value="1.0"/> <!--更新時間(s)，每多久更新一次地圖，不是頻
率-->
<param name="maxUrange" value="20.0"/> <!--激光雷達(dá)最大可用距離，在此之外的數(shù)據(jù)截斷不用-->
<param name="maxRange" value="25.0"/> <!--激光雷達(dá)最大距離-->
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="minimumScore" value="200"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.5"/>
<param name="angularUpdate" value="0.436"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="80"/>
<param name="xmin" value="-25.0"/>
<param name="ymin" value="-25.0"/>
<param name="xmax" value="25.0"/>
<param name="ymax" value="25.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>

Karto

Karto SLAM和Gmapping SLAM在工作方式上非常類似

Hector

Hector SLAM算法不同于前面兩種算法，Hector只需要激光雷達(dá)數(shù)據(jù)，而不需要里程計數(shù)據(jù)。
這種算法比較適合手持式的激光雷達(dá)，并且對激光雷達(dá)的掃描頻率有一定要求。
Hector算法的效果不如Gmapping、Karto，因為它僅用到激光雷達(dá)信息。這樣建圖與定位的依據(jù)就不如多傳感器結(jié)合的效果好。但Hector適合手持移動或者本身就沒有里程計的機(jī)器人使用。