ROS理论与实践(以移动机器人为例)连载(九) ——机器人自主导航

本章功能包链接: https://pan.baidu.com/s/1a6hUN4S3kdHzB6UplAtkrQ 密码: ri5s

可以扩展阅读的资料链接↓

Fetch Robotics 仓储机器人：https://fetchrobotics.com/products-technology/cloud-robotics-platform-for-warehouse-automation/.

---

1. ROS中的导航框架

Created with Raphaël 2.2.0 Goal AMCL Path Planner：全局规划（最短时间/最短路程） move_base：本地规划（实时避障） /cmd_vel+/odom Base Controller Motor Speeds

基于move_base的导航框架 wiki

sudo apt-get install ros-melodic-navigation

• global_planner：全局规划，目的是规划出最优路径；
• local_planner：本地规划，目的是根据周围障碍物避障对路径进行修改；
• global_costmap：全局代价地图；
• local_costmap：本地代价地图；
• recovery_behaviors：恢复机制，“撞了南墙能回头”；
• map_server：用来提供地图；
• amcl：输入是激光雷达信息，是一个定位功能包；
• odometry source：里程计信息，通过编码器微分得到的当前位置信息。

可以参见Setup and Configuration of the Navigation Stack on a Robot。

2. 导航框架中的关键功能包

1）move_base

• 全局路径规划（global planner）
  • 全局最优路径规划；
  • $Dijkstra$（最优距离）或$A\ast$算法（时间较短）。
• 本地实时规划（local planner）
  • 规划机器人每个周期内的线速度、角速度，使之尽量符合全局最优路径；
  • 实时避障；
  • $Trajectory$ $Rollout$ 和 $Dynamic$ $Window$ $Approaches$算法；
  • 搜索躲避和行进的多条路径，综合各评价标准选取最优路径。

这里需要引入一种新的ROS通信机制——$Action$。
在之前的通信那章已经给出了详细的讲解 跳转，这里做一个概要。

①Action 工作机制

描述图片：

action通讯方式采用服务器/客户端的方式，通过“action protocol（协议）”进行通讯：

客户端可以给服务器发送目标指令和终止指令，而服务器会反馈当前状态和结果：

• goal： 任务目标
• cancel： 取消当前任务
• status： 通知client当前的状态
• feedback： 周期性反馈任务执行的监控数据
• result： 向client发送任务的执行结果（只在执行结束后发送一次）

②move_base功能包中的话题和服务

③配置move_base节点

<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />

    <rosparam file="$(find mbot_navigation)/config/mbot/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mbot_navigation)/config/mbot/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find mbot_navigation)/config/mbot/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/mbot/global_costmap_params.yaml" command="load" />

    <rosparam file="$(find mbot_navigation)/config/mbot/move_base_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/mbot/dwa_local_planner_params.yaml" command="load" />
  node>
  
launch>

这些 $.yaml$ 文件都是参数文件，具体的参数可以下载功能包在mbot_navigation/config/mbot下找到，根据需求进行修改。

注意：取决于大家的模型宽度，本地costmap可能会对机器人通过狭小区域造成影响，那么需要修改下面的inflation_radius称作膨胀区半径，在costmap_common_params.yaml中

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[0.175, 0.175], [0.175, -0.175], [-0.175, -0.175], [-0.175, 0.175]]
#footprint_inflation: 0.01
#robot_radius: 0.175
inflation_radius: 1.0
cost_scaling_factor: 3.0
map_type: costmap
#transform_tolerance: 1
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true}

2）amcl

• 蒙特卡罗定位方法
• 二维环境定位
• 针对已有地图使用粒子滤波器跟踪一个机器人的姿态

• 里程计定位： 只通过里程计的数据来处理 /base 和 /odom 之间的TF转换；
• amcl定位： 可以估算机器人在地图坐标系 /map 下的位姿信息，提供 /base、/odom、/map之间的TF变换。

①amcl功能包中的话题和服务

②配置amcl节点

<launch>
    <arg name="scan_topic" default="scan"/>
    <arg name="initial_pose_x" default="0.0"/>
    <arg name="initial_pose_y" default="0.0"/>
    <arg name="initial_pose_a" default="0.0"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="min_particles"             value="500"/>
        <param name="max_particles"             value="3000"/>
        <param name="kld_err"                   value="0.02"/>
        <param name="update_min_d"              value="0.20"/>
        <param name="update_min_a"              value="0.20"/>
        <param name="resample_interval"         value="1"/>
        <param name="transform_tolerance"       value="0.5"/>
        <param name="recovery_alpha_slow"       value="0.00"/>
        <param name="recovery_alpha_fast"       value="0.00"/>
        <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
        <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
        <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
        <param name="gui_publish_rate"          value="50.0"/>

        <remap from="scan"                      to="$(arg scan_topic)"/>
        <param name="laser_max_range"           value="3.5"/>
        <param name="laser_max_beams"           value="180"/>
        <param name="laser_z_hit"               value="0.5"/>
        <param name="laser_z_short"             value="0.05"/>
        <param name="laser_z_max"               value="0.05"/>
        <param name="laser_z_rand"              value="0.5"/>
        <param name="laser_sigma_hit"           value="0.2"/>
        <param name="laser_lambda_short"        value="0.1"/>
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="laser_model_type"          value="likelihood_field"/>

        <param name="odom_model_type"           value="diff"/>
        <param name="odom_alpha1"               value="0.1"/>
        <param name="odom_alpha2"               value="0.1"/>
        <param name="odom_alpha3"               value="0.1"/>
        <param name="odom_alpha4"               value="0.1"/>
        <param name="odom_frame_id"             value="odom"/>
        <param name="base_frame_id"             value="base_footprint"/>
    node>
launch>

可以参考http://wiki.ros.org/amcl了解更多参数的信息。

3. 机器人自主导航案例——导航仿真

1）无动态避障效果

$ roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
$ roslaunch mbot_navigation nav_cloister_demo.launch

使用这两条命令打开gazebo和rviz。

rviz图中周围的红色点是雷达的仿真信息；绿色点是amcl粒子算法中的粒子，最密集的地方就是机器人所处的位置；蓝色区域是膨胀区，相当于机器人运动的安全区域，机器人远离这个区域；紫红区域是本地代价地图，用来得到本地最优 线速度、角速度。
两个工具：左边这个用来更改机器人在仿真环境中的位置和朝向方向；右边这个用来确定导航目标点和朝向方向，确定后机器人会立即开始导航。

RVIZ中的几种Plan。参考： https://blog.csdn.net/luohuiwu/article/details/93859257

nav_cloister_demo.launch：

<launch>

    
    <arg name="map" default="cloister_gmapping.yaml" />

    
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mbot_navigation)/maps/$(arg map)"/>

    
    <include file="$(find mbot_navigation)/launch/move_base.launch"/>

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

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

launch>

cloister_gmapping.yaml是上节在建图时使用rosrun map_server map_saver -f cloister_gmapping保存的（同时保存了图片）

image: cloister_gmapping.pgm
resolution: 0.050000
origin: [-15.400000, -12.200000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

后面分别启动了move_base和amcl节点。

2）动态避障

可以在规划路径时，在gazebo仿真中添加动态障碍物，机器人自动导航会规划出绕过该障碍物的最优路径。

3）通过程序控制自动导航，而不是gui

下面这个python的程序和cpp的程序都粘贴在这里了↓详细的注解会以注释的形式写在程序里。

①Python

#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import roslib;
import rospy  
import actionlib  
from actionlib_msgs.msg import *  
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  

# 节点初始化 
rospy.init_node('move_test', anonymous=True)  
  
# 订阅move_base服务器的消息  
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
# 这里创建了一个Action的针对动作的客户端订阅move_base的信息，实现MoveBaseAction

rospy.loginfo("Waiting for move_base action server...")  

# 等待连接服务器，5s等待时间限制 
while move_base.wait_for_server(rospy.Duration(5.0)) == 0:
    rospy.loginfo("Connected to move base server")  
# 与服务器做连接，成功的话打印日志

# 设定目标点  
target = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))  
# 对目标点进行封装，'map'表示这是地图里的一个点，以及朝向用四元数表示
goal = MoveBaseGoal()  
goal.target_pose.pose = target  
goal.target_pose.header.frame_id = 'map'  
goal.target_pose.header.stamp = rospy.Time.now()  

rospy.loginfo("Going to: " + str(target))  

# 向目标进发  
move_base.send_goal(goal)  

# 五分钟时间限制  
finished_within_time = move_base.wait_for_result(rospy.Duration(300))   
# 等待五分钟结果

# 查看是否成功到达  
if not finished_within_time:  
    move_base.cancel_goal()  
    rospy.loginfo("Timed out achieving goal")  
else:  
    state = move_base.get_state()  
    if state == GoalStatus.SUCCEEDED:  
        rospy.loginfo("Goal succeeded!")
    else:  
      rospy.loginfo("Goal failed！ ")  

②CPP

#include 
#include 
#include 
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "move_test");
 
  //订阅move_base服务器的消息  
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base("move_base", true);
 
  //等待连接服务器，5s等待时间限制 
  while(!move_base.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for move_base action server...");
  }
 
  ROS_INFO("Connected to move base server");

  //设定目标点
  move_base_msgs::MoveBaseGoal goal;
  goal.target_pose.header.frame_id = "map";
  goal.target_pose.header.stamp = ros::Time::now();
 
  goal.target_pose.pose.position.x = -5.543;
  goal.target_pose.pose.position.y = -4.779;

  goal.target_pose.pose.orientation.z = 0.645;
  goal.target_pose.pose.orientation.w = 0.764;
 
  ROS_INFO("Sending goal");
  move_base.sendGoal(goal);
 
  move_base.waitForResult();
 
  if(move_base.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Goal succeeded!");
  else
    ROS_INFO("Goal failed");
 
  return 0;

}

效果图超过了5M就不展示了。

功能包中还有一个随机点规划导航的Python文件，这里就不做展示了，感兴趣的可以在文章开头的网盘进行下载。

4）move_base + gmapping

机器人导航同时进行建图，根据更新图和障碍物修改导航路径。

实现文件是这样的↓

<launch>

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

    
    <include file="$(find mbot_navigation)/launch/move_base.launch" />

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

launch>

这里很清楚打开了 gmapping SLAM节点和 move_base 导航节点。