基于ROS的机器人建图与导航仿真全过程

引言

       之前一直想写一篇关于ROS机器人建图与导航仿真全过程的教程，终于有时间来做这个事啦，本人也拿过吉林省高校机器人大赛——ROS竞速组的冠军，第十六届全国智能车比赛——讯飞餐厅组线上赛二等奖，我想这个教程对接下来的一些参赛者多多少少也会有一些贡献。当然我觉得你已经会ROS的一些基本操作了，本文章只是简单扼要的介绍这个过程，其中细节部分难免可能不会太详细还请见谅，当人后续也会有更多这方面的文章，也会传授一些ROS机器人建图与导航方面的经验，当然我也在学习的过程，难免一些不足之处，话不多说啦，让我先把这个全过程的思维导图放在下面。

文章所用的代码已经开源：https://gitee.com/xiaolong_ROS/Map_construction-Navigation_simulation.git

一、环境

1.机器人建模

       我个人习惯把机器人本体放在单独的一个package下，让我们先看看里面的所有东西。

       可以看到这个机器人的URDF模型还有它所拥有的传感器，相机、惯性测量单元、激光雷达。
       我们对URDF文件进行检查，check_urdf命令会解析URDF文件，并且显示解析过程中发现的错误，如果一切正常，就会显示如下信息：

       当然我们也可以在rviz中查看这个模型：

       也可以看看机器人的TF关系：

2.运动控制器配置

       接下来我们主要在gazebo环境下操作，我们再创建一个单独的package，同样我们先看看完整的内容：

       我们在config下可以看到一个racecar_control.yaml文件：

racecar:

  left_rear_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: left_rear_axle
    pid: {p: 1000.0, i: 0.00, d: 0.0}


  right_rear_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: right_rear_axle 
    pid: {p: 1000.0, i: 0.00, d: 0.0}


  left_front_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: left_front_axle
    pid: {p: 1000.0, i: 0.00, d: 0.0}


  right_front_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: right_front_axle
    pid: {p: 1000.0, i: 0.00, d: 0.0}
    
  left_steering_hinge_position_controller:
    type: effort_controllers/JointPositionController
    joint: left_steering_joint
    pid: {p: 10000.0, i: 0.1, d: 500.0}
  
  right_steering_hinge_position_controller:
    type: effort_controllers/JointPositionController
    joint: right_steering_joint
    pid: {p: 10000.0, i: 0.1, d: 500.0}


  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

       这个文件便定义了机器人所有的运动控制器以及参数，我们通过launch文件添加以下内容便可以加载这些控制器：

3.world创建

       world的创建方法有很多了，你可以自己画一个世界，也可以导入，这里可以给大家安利另外一个仿真神器：Webots，你会发现不一样的东西。
       给大家看看比赛的官方赛道吧，使用gazebo racecar_runway_original.world 打开：

4.launch文件启动并测试

       我们先通过roslaunch racecar_gazebo racecar.launch 来打开小车所在的仿真环境：

       我们可以看到racecar_gazebo/scripts下有一个XL_keyboard_remote.py的，我们可以通过rosrun racecar_gazebo XL_keyboard_remote.py运行它，然后我们的机器人就可以前后左右移动并且转向啦（注意运行之后弹出来的窗口需要鼠标点击一下再控制机器人）。

二、建图

       建图的话我们以Gmapping算法功能包为例子进行地图构建，当然可以用其它的算法，比如：hector，cartographer等。

1.参数配置

       首先我们创建一个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"/>
        
        <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>

       然后我们需要创建一个gmapping_demo.launch用来打开gazebo，rviz等并建图：

<launch>

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

    <include file="$(find racecar_gazebo)/launch/racecar.launch"/>
  
    
    <node name="XL_keyboard_remote" pkg="racecar_gazebo" type="XL_keyboard_remote.py" output="screen">
    node>

    
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find racecar_gazebo)/rviz/gmapping.rviz"/>

launch>

       代码第一块就是建图参数的配置，第二块就是打开之前的gazebo环境等，第三块是打开键盘控制，第四块是打开一个已经配置好的rviz（配置的方法很简单的，就是添加一些东西）。

2.launch文件启动并建图

       之所以创建一个gmapping_demo.launch是想直接启动一个launch就可以开始建图，我们直接在终端输入roslaunch racecar_gazebo racecar.launch就可以开始建图啦：

       建图过程需要有耐心，最好速度不要太快，当然不同的算法适应性也不太一样，我们可以看看建图效果还是可以的：

       最后建成的地图我们需要及时保存。保存的地图一共有两个文件，map.pgm和map.yaml。

       让我们看一下建好的地图效果还是蛮不错的：

三、导航

       我们先来看一下导航的launch启动文件，第4行就是启动之前的launch文件；第6～8行是加载配置好的rviz；第10行是加载地图；注意第13行，我们加载了一个amcl.xml文件，这个是我们配置的定位方法参数。自主定位即机器人在任意状态下都可以推算出自己在地图中所处的位置，ROS为开发者提供了一种自适应(或kld采样)的蒙特卡罗定位方法(amcl)，这是一种概率统计方法，针对已有地图使用粒子滤波器跟踪一个机器人的姿态；第16～32行是导航需要的配置文件；第34行是一个导航脚本（自定义的一个ROS节点），初学者可以不用深究其内容。

1.代价地图的配置

       导航功能包使用两种代价地图存储周围环境中的障碍信息：一种用于全局路径规划(global_costmap)，一种用于本地路径规划和实时避障(local_costmap)。
       两种代价地图需要使用一些共用的或独立的配置文件：通用配置文件、全局规划配置文件和本地规划配置文件。config/navigation下这三个文件分别与之对应。

       代价地图用来存储周围环境的障碍信息，其中需要声明地图关注的机器人传感器消息，以便于地图信息的更新。
       针对两种代价地图通用的配置选项，创建名为costmap_common_params.yaml的配置文件。全局规划配置文件用于存储配置全局代价地图的参数，命名为global_costmap_params.yaml，本地规划配置文件用来存储本地代价地图的配置参数，命名为local_costmap_params.yaml。

2.本地规划器的配置

       比赛一般都需要实时避障的，我们导航所用的地图都是加上锥桶的，当然我们在建图的时候是不允许扫描锥桶的信息的，所以我们需要配置本地规划器，我们通过gazebo racecar_runway.world 打开环境如下：

       常用的本地规划算法有TEB算法和DWA算法，这篇教程主要用的TEB算法，本地规划器当然也是用的TEB算法的参数，具体可以查看最后一个配置文件teb_local_planner_params.yaml。

3.launch文件启动并导航

       我们通过roslaunch racecar_gazebo racecar_navigation.launch来开始导航前所有准备工作：

       通过rviz上2D Nav Goal来给机器人发布导航终点信息，可以看出机器人已经开始实时扫描信息并规划路径向着终点出发啦：

       我们可以通过rosrun rqt_tf_tree rqt_tf_tree来查看导航过程中的TF树：

       也可以通过rosrun rqt_graph rqt_graph来查看导航过程中的各节点：

本文内容参考：
GitHub上的racecar_ZJ的项目
《ROS机器人开发实践》

如有错误或者不足之处，欢迎大家留言指正！