【ROS】移动机器人导航仿真（3）——定位（amcl）和路径规划（move_base）

在前一节中，采用gmapping包实现了机器人工作环境的地图构建，这一节将利用amcl包和move_base包实现移动机器人的自主导航。至此已完成整个移动机器人导航仿真的工作，完整代码见：http://download.csdn.net/detail/wangchao7281/9717920

---

amcl

amcl是移动机器人在2D环境中的概率定位系统。它实现了自适应（或KLD采样）蒙特卡罗定位方法，其使用粒子滤波器来针对已知的地图跟踪机器人的位姿。目前情况下，该节点仅能使用激光扫描数据和激光雷达地图来工作。它可以通过修改代码以扩展到其他传感器数据。

amcl接收基于激光的地图，激光扫描和tf变换消息，并输出位姿估计。在启动时，amcl根据提供的参数初始化其粒子滤波器。注意，如果没有设置初始化参数，则根据默认值初始滤波器状态将是以（0,0,0）为中心的中等大小的粒子云。在rviz中可以通过2D Pose Estimate按钮来初始化位姿。

在ROS自带的文件中，rospack find amcl/examples/amcl_diff.launch支持差分驱动机器人平台，而amcl_omni.launch 支持全向移动机器人平台。

在启动launch文件前，可以配置一系列的参数来满足系统的要求。其中比较重要的几个参数如下：
1. 参数min_particles和max_particles设定了算法运行所允许的粒子最小和最大数量。粒子数越多，结果越精确，CPU消耗也越高。
2. 参数laser_model_type用于配置激光雷达的类型。
3. 参数laser_likehood_max_dist用于设置地图中障碍物膨胀的最大距离，使用此参数前提是laser_type_model选择了likehood_field模式。
详见：http://wiki.ros.org/amcl

---

move_base

move_base包提供了一个动作的实现（参见actionlib包），在地图上给定一个目标，move_base将会规划出路径并使机器人避开障碍物从而到达目标。详见：http://wiki.ros.org/move_base
move_base节点将全局导航和局部导航链接在一起以完成其导航任务。全局导航用于建立到地图上最终目标或一个远距离目标的路径，局部导航用于建立到近距离目标和为了临时躲避障碍物的路径，例如机器人四周一个4x4 m的方形窗口。
move_base节点支持任何遵循nav_core包中指定的nav_core :: BaseGlobalPlanner接口的全局规划以及任何遵循nav_core包中指定的nav_core :: BaseLocalPlanner接口的局部规划。move_base节点还维护两个代价地图，分别为全局代价地图和局部代价地图。

在运行move_base节点之前需要四个配置文件，这些文建定义了一系列相关参数，包括越过障碍物的代价、机器人的半径、路劲规划时的需要考虑未来多长的路，机器人的的最大速度等等，四个配置文件分别如下
- base_local_palnner_params.yaml
- costmap_common_params.yaml
- global_costmap_params.yaml
- local_costmap_params.yaml

具体的参数配置信息参见http://wiki.ros.org/base_local_planner、http://wiki.ros.org/costmap_2d
博主在仿真过程中发现对导航效果有较大影响的参数如下：
1.基本局部规划器配置（base_local_palnner_params.yaml）
- max_vel_x：机器人的最大线速度，单位是 m/s，对于室内机器人来说0.5就已经很快了。
- max_rotation_vel：机器人的最大旋转速度，单位是 rad/s，不要把这个值设的过高，不然机器人会错过目标方向。
- yaw_goal_tolerance：至多距离目标方向的偏差，把这个值设的太小可能会导致机器人在目标附近徘徊。
- xy_goal_tolerance：至多距离目标位置的偏差，把这个值设的太小可能会导致机器人在目标附近徘徊。注意：不要把最大误差设置的比地图分辨率还小，否则机器人会一直在目标附近徘徊而永远到不了目标。
- pdist_scale：全局路径规划和到达目的之间的权重。
- gdist_scale：到达目的位置和全局路径规划之间的权重。
2.代价地图共同的参数配置（costmap_common_params.yaml）
- inflation_radius：地图上障碍物的膨胀半径，单位为 m，如果机器人不能很好的通过狭窄的地方，则稍微减小这个值，相反，如果机器人不断的撞到东西，则稍微增大这个值。

---

仿真实现

首先启动机器人模型

<launch>
  
  <arg name="paused" default="true"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>
  <remap from="robot/laser/scan" to="/scan"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find nav_sim)/urdf/wall.world"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
  include>

  
  <arg name="model" default="$(find nav_sim)/urdf/myrobot.xacro" />
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />

  
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model robot -param robot_description -z 0.05"/> 
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_sim)/urdf/navigation.rviz"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>

launch>

然后启动amcl和move_base

<launch>

  
   <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_sim)/maps/map.yaml" output="screen"/>

  <include file="$(find amcl)/examples/amcl_diff.launch" >
  include> 

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="controller_frequency" value="10.0"/>
    <param name="controller_patiente" value="15.0"/>
    <rosparam file="$(find nav_sim)/launch/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find nav_sim)/launch/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find nav_sim)/launch/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find nav_sim)/launch/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find nav_sim)/launch/base_local_planner_params.yaml" command="load" />
  node>

launch>

上图包围机器人的红色部分即为用于机器人定位系统的李子云，点云的分布表示了再定位系统中机器人位姿的不确定性。

点击2D Nav Goal按钮可以设置机器人要达到的目标点，然后move_base会规划处一条绿色的全局路径，和不断规划蓝色的局部路径。粒子点云也随着机器人的移动不确定性逐渐减少，机器人的位姿估计更加准确。

move_base的一项重要功能是当机器人遇到障碍物时，能够对原定路径进行重新规划和计算，我在gazebo环境中临时添加了一个新的物体，机器人检测到新的障碍物并重新规划路径。

在仿真过程中，可以使用rqt_reconfigure在不重新启动仿真的情况下对参数值进行修改，使用以下指令：

$ rosrun rqt_reconfigure rqt_reconfigure

节点关系图：

仿真过程视频：http://v.youku.com/v_show/id_XMTg2NzY4MjY5Ng==.html

---

参考文献

1. http://wiki.ros.org/
2. ROS by Example vol1 indigo
3. Learnning ROS for Robotics Programming second edition