ROS机器人学习(七)--ROS中的导航框架

本文最后更新于：2022年5月29日上午

move_base

基于move_base的导航框架

全局路径规划（global planner）

全局最优路径规划
Dijkstra或A*算法

本地实时规划（local planner）

规划机器人每个周期内的线速度、角速度，使之尽量符合全局最优路径。
实时避障
Trajectory Rollout和Dynamic Window Approaches算法
搜索躲避和行进的多条路径，综合评价标准选取最优路径

move_base功能包中的话题和服务

move_base配置

据move_base的的导航框架得知，在进行路径规划时costmap是必不可少的。
在机器人进行路径规划时，我们需要明白规划算法是依靠什么在地图上来计算出来一条路径的。依靠的是gmapping扫描构建的一张环境全局地图，但是仅仅依靠一张原始的全局地图是不行的。因为这张地图是静态的，无法随时来更新地图上的障碍物信息。
在现实环境中，总会有各种无法预料到的新障碍物出现在当前地图中，或者旧的障碍物现在已经从环境地图中被移除掉了，那么我们就需要来随时更新这张地图。同时由于默认的地图是一张黑白灰三色地图，即只会标出障碍物区域、自由移动区域和未被探索区域。机器人在这样的地图中进行路径规划，会导致规划的路径不够安全，因为我们的机器人在移动时需要与障碍物之间保持一定的安全缓冲距离，这样机器人在当前地图中移动时就更安全了。

costmap代价地图

costmap简单来说就是为了在这张地图上进行各种加工，方便我们后面进行路径规划而存在的。那具体该如何实现costmap呢？
在ROS中使用 costmap_2d 这个软件包来实现的，该软件包在原始地图上实现了两张新的地图。一个是local_costmap，另外一个就是global_costmap，根据名字大家就可以知道了，两张costmap一个是为局部路径规划准备的，一个是为全局路径规划准备的。无论是local_costmap还是global_costmap，都可以配置多个图层，包括下面几种：

Static Map Layer：静态地图层，基本上不变的地图层，通常都是SLAM建立完成的静态地图。
Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的撞上障碍物。
Other Layers：你还可以通过插件的形式自己实现costmap，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。

配置costmap相关的参数

新建launch启动文件

在 mbot_navigation 功能包launch目录下新建启动文件，命名为move_base.launch：

<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <rosparam file="$(find mbot_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mbot_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find mbot_navigation)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/base_local_planner_params.yaml" command="load" />
  </node>
  
</launch>

在启动move_base节点时，可以看到首先加载了costmap_common_params.yaml到global_costmap和local_costmap两个命名空间中，因为该配置文件是一个通用的代价地图配置参数，即local_costmap和global_costmap都需要配置的参数。然后下面是local_costmap_params.yaml专门为了局部代价地图配置的参数，global_costmap_params.yaml专门为全局代价地图配置的参数。

配置costmap_common_params.yaml

在config目录下，创建costmap_common_params.yaml文件，配置的参数如下：

robot_radius: 0.16
footprint: [[0.16, 0.16], [0.16, -0.16], [-0.16, -0.16], [-0.16, 0.16]]
footprint_inflation: 0.01

obstacle_layer:
  enabled: true
  combination_method: 1
  track_unknown_space: true    # true needed for disabling global path planning through unknown space
  obstacle_range: 2.5
  raytrace_range: 3.0
  observation_sources: scan
  scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
  max_obstacle_height: 0.6
  min_obstacle_height: 0.0

#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer:
  enabled:              true
  cost_scaling_factor:  2.5  # exponential rate at which the obstacle cost drops off (default:10)
  inflation_radius:     0.15  # max. distance from an obstacle at which costs are incurred for planning paths.


static_layer:
  enabled:              true

下面来依次解释下各参数的意义，方便大家以后来根据需要来自行修改调试：

robot_radius：设置机器人的半径，单位是米。由于我的机器人是圆形的，所以可以直接设置该参数。如果你的机器人不是圆形的那就需要使用 footprint 这个参数，该参数是一个列表，其中的每一个坐标代表机器人上的一点，设置机器人的中心为[0,0]，根据机器人不同的形状，找到机器人各凸出的坐标点即可，具体可参考下图来设置：
obstacle_layer：配置障碍物图层
- enabled:是否启用该层
- combination_method：只能设置为0或1，用来更新地图上的代价值，一般设置为1;
- track_unknown_space: 如果设置为false，那么地图上代价值就只分为致命碰撞和自由区域两种，如果设置为true，那么就分为致命碰撞，自由区域和未知区域三种。意思是说假如该参数设置为false的话，就意味着地图上的未知区域也会被认为是可以自由移动的区域，这样在进行全局路径规划时，可以把一些未探索的未知区域也来参与到路径规划，如果你需要这样的话就将该参数设置为false。不过一般情况未探索的区域不应该当作可以自由移动的区域，因此一般将该参数设置为true;
- obstacle_range:设置机器人检测障碍物的最大范围，意思是说超过该范围的障碍物，并不进行检测，只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物;
- raytrace_range:在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可自由移动的空间数据。假如设置该值为3米，那么就意味着在3米内的障碍物，本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间
- observation_sources:设置导航中所使用的传感器，这里可以用逗号形式来区分开很多个传感器，例如激光雷达，碰撞传感器，超声波传感器等，我这里只设置了激光雷达;
- data_type:激光雷达数据类型;
- topic:该激光雷达发布的话题名;
- marking:是否可以使用该传感器来标记障碍物;
- clearing:是否可以使用该传感器来清除障碍物标记为自由空间;
- max_obstacle_height:以米为单位插入costmap的任何障碍物的最大高度。此参数应设置为略高于机器人的高度。
- min_obstacle_height:传感器读数的最小高度（以米为单位）视为有效。通常设置为地面高度。
inflation_layer:膨胀层，用于在障碍物外标记一层危险区域，在路径规划时需要避开该危险区域
enabled:是否启用该层;
- cost_scaling_factor:膨胀过程中应用到代价值的比例因子，代价地图中到实际障碍物距离在内切圆半径到膨胀半径之间的所有cell可以使用如下公式来计算膨胀代价：
  
  exp(-1.0 * cost_scaling_factor * (distance_from_obstacle – inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE – 1)
  
  公式中costmap_2d::INSCRIBED_INFLATED_OBSTACLE目前指定为254，
  
  注意：由于在公式中cost_scaling_factor被乘了一个负数，所以增大比例因子反而会降低代价。
- inflation_radius:膨胀半径，膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。
Static_layer:静态地图层，即SLAM中构建的地图层
- enabled:是否启用该地图层;

配置global_costmap_params.yaml

全局代价地图是作为进行全局路径规划时的参考，我们需要在config目录中，创建global_costmap_params.yaml文件，该文件是为全局代价地图配置的参数，具体配置的参数如下：

global_costmap:
   global_frame: map
   robot_base_frame: base_footprint
   update_frequency: 1.0
   publish_frequency: 1.0
   static_map: true
   rolling_window: false
   resolution: 0.05
   transform_tolerance: 1.0
   map_type: costmap

下面我们来详细解释下该全局代价地图配置文件中各参数的意义：

global_frame：全局代价地图需要在哪个坐标系下运行;
robot_base_frame：在全局代价地图中机器人本体的基坐标系，就是机器人上的根坐标系。通过global_frame和robot_base_frame就可以计算两个坐标系之间的变换，得知机器人在全局坐标系中的坐标了。
update_frequency：全局代价地图更新频率，一般全局代价地图更新频率设置的比较小;
static_map：配置是否使用map_server提供的地图来初始化，一般全局地图都是静态的，需要设置为true;
rolling_window：是否在机器人移动过程中需要滚动窗口，始终保持机器人在当前窗口中心位置;
transform_tolerance：坐标系间的转换可以忍受的最大延时;

配置local_costmap_params.yaml

局部代价地图配置参数所建立的地图主要是为局部路径规划所使用，我们可以在config目录下，创建local_costmap_params.yaml文件，完整内容如下：

local_costmap:
   global_frame: odom
   robot_base_frame: base_footprint
   update_frequency: 3.0
   publish_frequency: 1.0
   static_map: false
   rolling_window: true
   width: 6.0
   height: 6.0
   resolution: 0.05
   transform_tolerance: 1.0

下面来详细解释下每个参数的意义：

global_frame：在局部代价地图中的全局坐标系，一般需要设置为odom_frame；
robot_base_frame：机器人本体的基坐标系;
update_frequency：局部代价地图的更新频率;
publish_frequency：局部代价地图的发布频率;
static_map：局部代价地图一般不设置为静态地图，因为需要检测是否在机器人附近有新增的动态障碍物;
rolling_window：使用滚动窗口，始终保持机器人在当前局部地图的中心位置;
width：滚动窗口的宽度，单位是米;
height：滚动窗口的高度，单位是米;
resolution：地图的分辨率，该分辨率可以从加载的地图相对应的配置文件中获取到;
transform_tolerance：局部代价地图中的坐标系之间转换的最大可忍受延时;

具体更多参数含义参考官网：http://wiki.ros.org/move_base/

amcl

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

amcl功能包中的话题和服务

转换

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

amcl参数配置

在 mbot_navigation 功能包launch目录下新建amcl.launch文件：

<launch>
    <arg name="use_map_topic" default="false"/>
    <arg name="scan_topic" default="scan"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="use_map_topic" value="$(arg use_map_topic)"/>
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="60"/>
        <param name="laser_max_range" value="12.0"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="2000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.2"/>
        <param name="odom_alpha4" value="0.2"/>
        <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_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.25"/>
        <param name="update_min_a" value="0.2"/>
        <param name="odom_frame_id" value="odom"/>
        <param name="resample_interval" value="1"/>
        <!-- Increase tolerance because the computer can get quite busy -->
        <param name="transform_tolerance" value="1.0"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>