SLAM

机器人研究的问题包含许许多多的领域，我们常见的几个研究的问题包括：建图(Mapping)、定位(Localization)和路径规划（Path Planning），如果机器人带有机械臂，那么运动规划（Motion Planning）也是重要的一个环节。而同步定位与建图（SLAM）问题位于定位和建图 的交集部分。 SLAM需要机器人在未知的环境中逐步建立起地图，然后根据地区确定自身位置，从而进一步定位。 ROS中SLAM的一些功能包，也就是一些常用的SLAM算法，例如Gmapping、Karto、Hector、Cartographer等算法。我们不会去关注算法背后的数学原理，而是更注重工程实现上的方法，告诉你SLAM算法包是如何工作的，怎样快速的搭建起SLAM算法

地图

ROS中的地图很好理解，就是一张普通的灰度图像，通常为pgm格式。这张图像上的黑色像素表示障碍物，白色像素表示可行区域，灰色是未探索的区域

在SLAM建图的过程中，你可以在RViz里看到一张地图被逐渐建立起来的过程，类似于一块块拼图被拼接成一张完整的地图。这张地图对于我们定位、路径规划都是不可缺少的信息。事实上，地图在ROS中是以Topic的形式维护和呈现的，这个Topic名称就叫做 /map ，它的消息类型是nav_msgs/OccupancyGrid

锁存

由于 /map 中实际上存储的是一张图片，为了减少不必要的开销，这个Topic往往采用锁存（latched）的方式来发布。什么是锁存？其实就是：地图如果没有更新，就维持着上次发布的内容不变，此时如果有新的订阅者订阅消息，这时只会收到一个 /map 的消息，也就是上次发布的消息；只有地图更新了（比如SLAM又建出来新的地图），这时 /map 才会发布新的内容。 锁存器的作用就是，将发布者最后一次发布的消息保存下来，然后把它自动发送给后来的订阅者。这种方式非常适合变动较慢、相对固定的数据（例如地图），然后只发布一次，相比于同样的消息不定的发布，锁存的方式既可以减少通信中对带宽的占用，也可以减少消息资源维护的开销。

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 #地图相关信息
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话题的数据结构，包含了三个主要的部分:header, info和data。 header是消息的报头，保存了序号、时间戳、frame等通用信息，info是地图的配置信息，它反映了地图的属性，data是真正存储这张地图数据的部分，它是一个可变长数组， int8 后面加了 [] ，你可以理解为一个类似于vector的容器，它存储的内容有width*height个int8型的数据,也就是这张地图上每个像素。

Gmapping

Gmapping SLAM软件包

Gmapping算法是目前基于激光雷达和里程计方案里面比较可靠和成熟的一个算法，它基于粒子滤波，采用RBPF的方法效果稳定，许多基于ROS的机器人都跑的是gmapping_slam。这个软件包位于ros-perception组织中的slam_gmapping仓库中。 其中的 slam_gmapping 是一个metapackage，它依赖了 gmapping ，而算法具体实现都在 gmapping 软件包中，该软件包中的 slam_gmapping 程序就是我们在ROS中运行的SLAM节点。如果你感兴趣，可以阅读一下 gmapping 的源代码。 如果你的ROS安装的是desktop-full版本，应该默认会带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算法中需要设置的参数很多，这种启动单个节点的效率很低。所以往往我们会把gmapping的启动写到launch文件中，同时把gmapping需要的一些参数也提前设置好，写进launch文件或yaml文件。 具体可参考教学软包中的 slam_sim_demo中的 gmapping_demo.launch 和robot_gmapping.launch.xml文件。

Gmapping SLAM计算图

gmapping的作用是根据激光雷达和里程计（Odometry）的信息，对环境地图进行构建，并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据，而输出应当有自身位置和地图。 下面我们从计算图（消息的流向）的角度来看看gmapping算法的实际运行中的结构：

位于中心的是我们运行的 slam_gmapping 节点，这个节点负责整个gmapping SLAM的工作。它的输入需要有两个：

输入

• /tf以及 /tf_static： 坐标变换，类型为第一代的tf/tfMessage或第二代的 tf2_msgs/TFMessage 其中一定得提供的有两个tf，一个是base_frame 与laser_frame之间的tf，即机器人底盘和激光雷达之间的变换；一个是base_frame与 odom_frame之间的tf，即底盘和里程计原点之间的坐标变换。odom_frame可以理解为里程计原点所在的坐标系
• /scan:激光雷达数据，类型为sensor_msgs/LaserScan

/scan很好理解，Gmapping SLAM所必须的激光雷达数据，而 /tf是一个比较容易忽视的细节。尽管 /tf 这个Topic听起来很简单，但它维护了整个ROS三维世界里的转换关系，而 slam_gmapping 要从中读取的数据是 base_frame 与 laser_frame之间的tf,只有这样才能够把周围障碍物变换到机器人坐标系下，更重要的是 base_frame与 odom_frame 之间的tf，这个tf反映了里程计（电机的光电码盘、视觉里程计、IMU）的监测数据，也就是机器人里程计测得走了多少距离，它会把这段变换发布到odom_frame 和laser_frame 之间。 因此 slam_gmapping 会从 /tf 中获得机器人里程计的数据

输出

• /tf ： 主要是输出 map_frame 和 odom_frame 之间的变换
• /slam_gmapping/entropy ： std_msgs/Float64 类型，反映了机器人位姿估计的分散程度
• /map ： slam_gmapping 建立的地图
• /map_metadata ： 地图的相关信息

输出的 /tf里又一个很重要的信息，就是 map_frame和 odom_frame 之间的变换，这其实就是对机器人的定位。通过连通 map_frame 和 odom_frame ，这样map_frame与 base_frame 甚至与 laser_frame都连通了。这样便实现了机器人在地图上的定位。 同时，输出的Topic里还有 /map na，在上一节我们介绍了地图的类型，在SLAM场景中，地图是作为SLAM的结果被不断地更新和发布。

里程计误差及修正

目前ROS中常用的里程计广义上包括车轮上的光电码盘、惯性导航元件（IMU）、视觉里程计，你可以只用其中的一个作为odom，也可以选择多个进行数据融合，融合结果作为odom。通常来说，实际ROS项目中的里程计会发布两个Topic：

• /odom : 类型为 nav_msgs/Odometry ，反映里程计估测的机器人位置、方向、线速度、角速度信息。
• /tf : 主要是输出 odom_frame 和 base_frame 之间的tf。这段tf反映了机器人的位置和方向变换，数值与 /odom 中的相同。

由于以上三种里程计都是对机器人的位姿进行估计，存在着累计误差，因此当运动时间较长时， odom_frame 和 base_frame 之间变换的真实值与估计值的误差会越来越大.你可能会想，能否用激光雷达数据来修正 odom_frame 和 base_frame 的tf。事实上gmapping不是这么做的，里程计估计的是多少， odom_frame 和base_frame 的tf就显示多少，永远不会去修正这段tf。gmapping的做法是把里程计误差的修正发布到 map_frame 和 odom_frame 之间的tf上，也就是把误差补偿在了地图坐标系和里程计原点坐标系之间。通过这种方式来修正定位 这样 map_frame 和 base_frame ，甚至和 laser_frame 之间就连通了，实现了机器人在地图上的定位。

服务

slam_gmapping 也提供了一个服务： /dynamic_map ： 其srv类型为nav_msgs/GetMap，用于获取当前的地图 该srv定义如下： nav_msgs/GetMap.srv

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

可见该服务的请求为空，即不需要传入参数，它会直接反馈当前地图。

参数

slam_gmapping 需要的参数很多，这里以 slam_sim_demo 教学包中的 gmapping_demo 的参数为例，注释了一些比较重要的参数，具体请查看 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)"/> <!--底盘坐标系-->
<param name="odom_frame" value="$(arg odom_frame)"/> <!--里程计坐标系-->
<param name="map_update_interval" value="1.0"/> <!--更新时间(s)，每多久更新一次地图，不是频
率-->
<param name="maxUrange" value="20.0"/> <!--激光雷达最大可用距离，在此之外的数据截断不用-->
<param name="maxRange" value="25.0"/> <!--激光雷达最大距离-->
<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只需要激光雷达数据，而不需要里程计数据。 这种算法比较适合手持式的激光雷达，并且对激光雷达的扫描频率有一定要求。 Hector算法的效果不如Gmapping、Karto，因为它仅用到激光雷达信息。这样建图与定位的依据就不如多传感器结合的效果好。但Hector适合手持移动或者本身就没有里程计的机器人使用。