ROS——SLAM与Navigation

第九章 SLAM

本章简介

机器人研究的问题包含许许多多的领域，我们常见的几个研究的问题包括：建图(Mapping)、定位(Localization)和路径规划（Path Planning），如果机器人带有机械臂，那么运动规划（Motion Planning）也是重要的一个环节。而同步定位与建图（SLAM）问题位于定位和建图的交集部分。

SLAM需要机器人在未知的环境中逐步建立起地图，然后根据地区确定自身位置，从而进一步定位。

这一章我们来看ROS中SLAM的一些功能包，也就是一些常用的SLAM算法，例如Gmapping、Karto、Hector、Cartographer等算法。这一章我们不会去关注算法背后的数学原理，而是更注重工程实现上的方法，告诉你SLAM算法包是如何工作的，怎样快速的搭建起SLAM算法。

9.1 地图

9.1.1 直观印象

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

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

9.1.2 锁存

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

9.1.3 nav_msgs/OccupancyGrid

然后我们来看一下地图的OccupancyGrid类型是如何定义的，你可以通过rosmsg show nav_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型的数据,也就是这张地图上每个像素。

9.2 Gmapping

9.2.1 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文件。

9.2.2 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，在上一节我们介绍了地图的类型，在SLAM场景中，地图是作为SLAM的结果被不断地更新和发布。

9.2.3 里程计误差及修正

目前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之间就连通了，实现了机器人在地图上的定位。

• /odom

9.2.4 服务

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

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

9.2.5 参数

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>

演示截图

gmapping算法演示效果图如下：

9.3 Karto

9.3.1 Karto SLAM计算图

Karto SLAM和Gmapping SLAM在工作方式上非常类似，如下图所示

输入的Topic同样是/tf和/scan，其中/tf里要连通odom_frame与base_frame，还有laser_frame。这里和Gmapping完全一样。

唯一不同的地方是输出，slam_karto的输出少相比slam_gmapping了一个位姿估计的分散程度.

9.3.2 服务

与Gmapping相同，提供/dynamic_map服务

9.3.3 参数

这里以ROS-Academy-for-Beginners中的karto_slam为例，选取了它的参数文件slam_sim_demo/param/karto_params.yaml，关键位置做了注释：

# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.2 
minimum_travel_heading: 0.174                  #in radians
scan_buffer_size: 70
scan_buffer_maximum_scan_distance: 20.0
link_match_minimum_response_fine: 0.8
link_scan_maximum_distance: 10.0
loop_search_maximum_distance: 4.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 0.4     # gets squared later
loop_match_minimum_response_coarse: 0.8
loop_match_minimum_response_fine: 0.8

# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.3
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.03

# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03

# Scan Matcher Parameters
distance_variance_penalty: 0.3              # gets squared later
angle_variance_penalty: 0.349                # in degrees (gets converted to radians then squared)
fine_search_angle_offset: 0.00349               # in degrees (gets converted to radians)
coarse_search_angle_offset: 0.349            # in degrees (gets converted to radians)
coarse_angle_resolution: 0.0349                # in degrees (gets converted to radians)
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: false

演示截图