在前一节中,采用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>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<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>
<!-- Load the URDF into the ROS Parameter Server -->
<arg name="model" default="$(find nav_sim)/urdf/myrobot.xacro" />
<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
<!-- Run a python script to send a service call the gazebo_ros to spawn a URDF robot -->
<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
<?xml version="1.0"?>
<launch>
<!-- Run the map server -->
<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
参考文献
- http://wiki.ros.org/
- ROS by Example vol1 indigo
- Learnning ROS for Robotics Programming second edition