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

地圖

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

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

鎖存

由于 /map 中實(shí)際上存儲的是一張圖片掖棉，為了減少不必要的開銷，這個(gè)Topic往往采用鎖存（latched）的方式來發(fā)布膀估。什么是鎖存幔亥？其實(shí)就是：地圖如果沒有更新，就維持著上次發(fā)布的內(nèi)容不變察纯，此時(shí)如果有新的訂閱者訂閱消息帕棉，這時(shí)只會收到一個(gè) /map 的消息，也就是上次發(fā)布的消息饼记；只有地圖更新了（比如SLAM又建出來新的地圖）香伴，這時(shí) /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 #消息的報(bào)頭
uint32 seq
time stamp
string frame_id #地圖消息綁定在TF的哪個(gè)frame上礁遵，一般為map
nav_msgs/MapMetaData info #地圖相關(guān)信息
time map_load_time #加載時(shí)間
float32 resolution #分辨率 單位：m/pixel
uint32 width #寬 單位：pixel
uint32 height #高 單位：pixel
geometry_msgs/Pose origin #原點(diǎn)
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
int8[] data #地圖具體信息

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

Gmapping

Gmapping SLAM軟件包

Gmapping算法是目前基于激光雷達(dá)和里程計(jì)方案里面比較可靠和成熟的一個(gè)算法，它基于粒子濾波眼姐，采用RBPF的方法效果穩(wěn)定诅迷，許多基于ROS的機(jī)器人都跑的是gmapping_slam。這個(gè)軟件包位于ros-perception組織中的slam_gmapping倉庫中众旗。 其中的 slam_gmapping 是一個(gè)metapackage罢杉，它依賴了 gmapping ，而算法具體實(shí)現(xiàn)都在 gmapping 軟件包中逝钥，該軟件包中的 slam_gmapping 程序就是我們在ROS中運(yùn)行的SLAM節(jié)點(diǎn)屑那。如果你感興趣，可以閱讀一下 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上運(yùn)行的方法很簡單

rosrun gmapping slam_gmapping

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

Gmapping SLAM計(jì)算圖

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

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

輸入

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

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

輸出

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

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

里程計(jì)誤差及修正

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

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

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

服務(wù)

slam_gmapping 也提供了一個(gè)服務(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)"/> <!--里程計(jì)坐標(biāo)系-->
<param name="map_update_interval" value="1.0"/> <!--更新時(shí)間(s)，每多久更新一次地圖备埃，不是頻
率-->
<param name="maxUrange" value="20.0"/> <!--激光雷達(dá)最大可用距離姓惑，在此之外的數(shù)據(jù)截?cái)嗖挥?->
<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ù)按脚，而不需要里程計(jì)數(shù)據(jù)于毙。
這種算法比較適合手持式的激光雷達(dá)，并且對激光雷達(dá)的掃描頻率有一定要求辅搬。
Hector算法的效果不如Gmapping唯沮、Karto脖旱，因?yàn)樗鼉H用到激光雷達(dá)信息。這樣建圖與定位的依據(jù)就不如多傳感器結(jié)合的效果好介蛉。但Hector適合手持移動或者本身就沒有里程計(jì)的機(jī)器人使用萌庆。