第九章 机器人SLAM建图 课后作业

代码包

所有代码已上传至github网站：
github链接: https://github.com/YuemingBi/ros_practice/tree/master

课后作业

1、在Gazebo中构建一个用于建图和导航的虚拟环境，可以使用Building Editor工具创建，也可以使用其他功能包中已有的虚拟环境。
2、将自己构建的机器人模型放置到虚拟环境中，使用gmapping和hector功能包实现SLAM地图构建，并将建立完成的地图保存到功能包的maps文件夹中。

第一题

第一题我使用了两个仿真环境，一个是自己利用Building Editor工具创建的比较简单的仿真环境room.world；另一个是使用中科院软件博物馆仿真环境museum.world。

1、机器人模型

机器人模型参见第六章: https://blog.csdn.net/weixin_42361804/article/details/106015809，这里做了一个简单的修改，去掉了kinect相机，将rplidar激光雷达放在了最上方，由于建立的是二维地图，kinect相机的作用不大，所以一个激光雷达已经足够了。

2、robot_rplidar_nav.launch

<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find robot_gazebo)/worlds/museum.world"/>
    <!--arg name="world_name" value="$(find robot_gazebo)/worlds/room.world"/-->
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(arg world_name)" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find robot_description)/urdf/robot_kinect_rplidar.xacro'" />

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

    <!-- 在gazebo中加载机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-urdf -model robot -param robot_description"/>

</launch>

此launch文件用来加载机器人模型和仿真环境，并发布机器人的关节状态和tf坐标变换，在此文件中可以修改对应的world文件。

3、robot in room

4、robot in museum

第二题

1、使用gmapping算法建图

（1）gmapping_demo.launch

<launch>

    <include file="$(find robot_navigation)/launch/gmapping.launch"/>

    <!-- 启动rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find robot_navigation)/rviz/gmapping.rviz"/>

</launch>

此launch文件嵌套了一个gmapping.launch文件，并启动了rviz。

（2）gmapping.launch

<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)"/>
    </node>
</launch>

此launch文件只运行了一个slam_gmapping节点，并配置了一些参数。

（3）建图

roslaunch robot_gazebo robot_rplidar_nav.launch
roslaunch robot_navigation gmapping_demo.launch
roslaunch robot_teleop robot_teleop.launch

在robot_rplidar_nav.launch可以修改对应的world文件。

room

建图过程：

建好的地图：

可以看到这个地图并不是那么好看，这是由于算法的局限性（电脑性能）和传感器信息的单一性导致的。一种方法可以通过调节参数来改进，比如gmapping.launch文件中的iterations和particles两个参数，其中iterations表示点云配准时迭代的次数，迭代次数越多点云配准得就越精确，particles表示粒子滤波中粒子的数量，粒子的数量越多定位建图越精确，此外还可以修改激光雷达的扫描频率，rplidar.xacro文件中的<update_rate>标签。另一种方法是添加传感器，比如imu或相机进行传感器融合，既可以保证地图的准确性，又可以保证自身的定位，同时对闭环检测也有很大的改进。

museum

建图过程：

建好的地图：

2、使用hector算法建图

（1）hector_demo.launch

<launch>

    <include file="$(find robot_navigation)/launch/hector.launch"/>

    <!-- 启动rviz-->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find robot_navigation)/rviz/gmapping.rviz"/>

</launch>

此launch文件嵌套了一个hector.launch文件，并启动了rviz。

（2）hector.launch

<launch>
    <node pkg="hector_mapping" type="hector_mapping" name="hector_height_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_link" />
    <param name="odom_frame" value="odom" />

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

    <!--mapsize /start point-->
    <param name="map_resolution" value="0.025"/>
    <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="1"/>
    <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 parameter-->
    <param name="update_factor_free" value="0.45"/>
    <param name="update_factor_occupied" value="0.7"/>
    <param name="map_update_distance_thresh" value="0.1"/>
    <param name="map_update_angle_thresh" value="0.05"/>

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

  </node>

  <!-- Move base -->
 <include file="$(find robot_navigation)/launch/include/move_base.launch.xml"/>

</launch>

此launch文件只运行了一个hector_mapping节点，并配置了一些参数。

（3）建图

roslaunch robot_gazebo robot_rplidar_nav.launch
roslaunch robot_navigation hector_demo.launch
roslaunch robot_teleop robot_teleop.launch

在robot_rplidar_nav.launch可以修改对应的world文件。

room

hector算法相比gmapping算法而言，在建图过程中非常容易发生漂移，尤其是在转弯的时候。
建图过程：
转弯时速度太快而发生漂移：

所以使用hector算法建图时机器人的速度一定要非常慢，尤其是在转弯的时候。hector算法也可以通过调节参数来提高地图精度和鲁棒性，例如hector.launch文件中的map_resolution和map_size两个参数，map_size表示地图的大小，建立的地图中的点数为map_size*map_size，map_resolution表示地图中每一小方块代表的尺寸大小。除了提高地图分辨率之外，还可以修改激光雷达的扫描频率，rplidar.xacro文件中的<update_rate>标签。

meseum

建图过程：

建好的地图：