ROS学习（17）机器人SLAM功能包——gmapping和hector_slam

---

前言

  本篇主要介绍常用的slam功能包——hector_slam，在这之前，先简单了解下gmapping。前面文章中所有涉及slam的demo，全部基于gmapping+激光雷达。

一、gmapping功能包

1、阐述

  gmapping功能包是最为常用和成熟的slam功能包，其集成了Rao-Blackwellized粒子滤波算法。
  功能包的总体框架如下：

  该功能包订阅机器人的深度信息、IMU信息和里程计信息，同时完成一些必要参数配置，即可创建并输出基于概率的二维格栅地图。
  gmapping功能包基于openslam社区的开源slam算法。

2、安装

  kinetic版本的ROS源中已经集成了其二进制安装文件，执行如下命令进行安装：

sudo apt-get install ros-kinetic-gmapping

若之前已经安装过，这一步可跳过。

3、编写gmapping.launch文件

  之前的学习中，只是启动了gmapping节点，节点参数使用的是默认参数，实现了slam功能。但是实际应用中，这些参数需要我们自己进行优化，以提升slam效果，如下：

<launch>

    <arg name="scan_topic" default="scan" />

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        <param name="odom_frame" value="odom"/>
        <param name="map_update_interval" value="5.0"/>
        <!-- Set maxUrange < actual maximum range of the Laser -->
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <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="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="-1.0"/>
        <param name="ymin" value="-1.0"/>
        <param name="xmax" value="1.0"/>
        <param name="ymax" value="1.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)"/>
        <param name="base_link" value="base_footprint"/>
    </node>

</launch>

需要注意两个参数的设置：
  1）里程计坐标系，odom_frame参数需要和机器人本身的里程计坐标系一致。
  2）激光雷达话题名，gmapping节点订阅的激光雷达话题名是/scan，如果与机器人发布的激光雷达话题名不一致，需要使用进行重映射。

4、效果

二、hector_slam功能包

1、阐述

  hector_slam的核心节点是hector_mapping，订阅/scan话题发布的激光雷达数据。与gmapping相同的是，其也会发布map话题。与gmapping不同的是，其还会发布slam_out_pose和poseupdate话题，提供当前估计的机器人位姿。
  hector功能包使用高斯牛顿方法，不需要里程计数据，只根据激光信息进行地图构建，该算法对雷达帧率要求较高，一般要求高于20HZ。若雷达帧率较低，就要调低机器人运行速度小于0.3m/s，如果不降低机器人行驶速度，建图效果会非常差，甚至无法完成建图。
  功能包的总框架如下：

2、安装

kinetic版本的ROS源中已经集成了其二进制安装文件，执行如下命令：

sudo apt-get install ros-kinetic-hector-slam

3、创建功能包

  在catkin_ws工作空间下，创建名为mbot_slam_hector的功能包，建图环境仍然为我家户型图，将需要的文件复制到该功能包下。

4、编写hector.launch文件

如gmapping一样，这里不再使用hector_slam建图算法的默认参数，而是尝试自己修改建图算法参数，从而优化建图效果，也方便进一步的深入学习。

<launch>
  
   <node pkg = "hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
        <!-- Frame names -->
        <param name="pub_map_odom_transform" value="true"/>
        <param name="map_frame" value="map" />
        <param name="base_frame" value="base_footprint" />
        <param name="odom_frame" value="base_footprint" />

        <!-- Tf use -->
        <param name="use_tf_scan_transformation" value="true"/>
        <param name="use_tf_pose_start_estimate" value="false"/>

        <!-- Map size / start point -->
        <param name="map_resolution" value="0.05"/>
        <param name="map_size" value="2048"/>
        <param name="map_start_x" value="0.5"/>
        <param name="map_start_y" value="0.5" />
        <param name="laser_z_min_value" value = "-1.0" />
        <param name="laser_z_max_value" value = "1.0" />
        <param name="map_multi_res_levels" value="2" />

        <param name="map_pub_period" value="2" />
        <param name="laser_min_dist" value="0.4" />
        <param name="laser_max_dist" value="5.5" />
        <param name="output_timing" value="false" />
        <param name="pub_map_scanmatch_transform" value="true" />

        <!-- Map update parameters -->
        <param name="update_factor_free" value="0.4"/>
        <param name="update_factor_occupied" value="0.7" />    
        <param name="map_update_distance_thresh" value="0.2"/>
        <param name="map_update_angle_thresh" value="0.06" />

        <!-- Advertising config --> 
        <param name="advertise_map_service" value="true"/>
        <param name="scan_subscriber_queue_size" value="5"/>
        <param name="scan_topic" value="/scan"/>

        <!--<param name="base_link" value="base_footprint"/>-->
    </node>


    <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0 0 0.0 /map /odom 10" />
    <!--<node pkg="tf" type="static_transform_publisher" name="odom_to_base_footprint" args="0.0 0.0 0.0 0 0 0.0 /odom /base_footprint 10" />
    <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0.0 0.0 0.0 0 0 0.0 /base_footprint /base_link 10" />-->
    <!--<node pkg="tf" type="static_transform_publisher" name="laser_broadcaster" args="0 0 0 0 0 0 /base_link /lidar_link 10" />-->

    <node pkg="tf" type="static_transform_publisher" name="map_scanmatcher_broadcaster" args="0 0 0 0 0 0 map scanmatcher_frame 5"/>

</launch>

值得注意的一点是，因为hector_slam不需要里程计信息，所以将base_frame、odom_frame都设为base_link。

5、运行

运行如下命令：

roslaunch mbot_slam_hector robot_gazebo_navigation.launch

6、效果

  在遥控机器人四处走动时，需要注意机器人的速度不可以过快，否则就会发生打滑现象，导致创建的地图出现偏差。这是因为该算法不需要订阅里程计信息，而是直接使用激光估计里程计信息。
  如果角速度旋转太快，会出现上图中所示的问题，即SearchDir angle change too large。一般出现该错误的原因就是雷达出现了剧烈摇晃或数据的不稳，要不然就是在一些无法定位的场景中，例如在一个走廊中。所以hector适合在那种地形比较复杂的环境中使用。
  所以还是建议，如果有里程计信息的情况下，大家还是使用gmapping算法，在没有里程计的情况下使用hector算法。
  我们降低机器人的线速度和角速度后，效果得到很大改善，如下：