机器人基于ROS的move_base导航功能的部署（代价地图）_movebase 中 tracking pid

预备知识

move_base的基本框架

• ROS中用于导航避障的功能包为move_base，代码链接为 https://github.com/ros-planning/navigation.git。
• 关于导航与避障的总体配置可以参考： http://wiki.ros.org/navigation/Tutorials/RobotSetup
• move_base的核心代码为 nav_core，代码链接为 https://wiki.ros.org/nav_core， 利用nav_core::BaseGlobalPlanner接口可以指定全局路径规划器， 利用nav_core::BaseLocalPlanner 接口可以指定局部路径规划器。在move_base的节点中， 维护着两张代价地图， 一张是全局代价地图， 一张是局部代价地图，以此来完成需要的导航任务。
• move_base的总体框架图如下所示：
  白色节点为move_base提供的， 灰色节点为可选的， 比如map_server， 这样我们就可以实现在未知环境进行建图， 蓝色节点为必须指定的节点。
• move_base 导航栈中可以在不需要构建的地图情况下便可进行相应的导航操作。
  
• move_base功能包中默认的自恢复行为过程如下：
  
  首先，用户指定区域的障碍物地图将会被清除， 其次， 如果可行的话， 机器人将会旋转来清除外部空间，如果这也失败的话， 机器人将会积极地旋转运动来清除方形区域外的障碍物区域， 如果这都失败的话， 机器人将会通知move_base放弃该导航任务。自恢复行为可以通过 recovery_behaviors参数来进行配置， 或者使用recovery_behavior_enabled参数来开启和关闭自恢复行为。
• move_base功能包的配置参数，包含如下：

~base_global_planner (string, default: "navfn/NavfnROS" For 1.1+ series)
#用于指定全局路径规划的插件名，默认值为 "NavfnROS"
~base_local_planner (string, default: "base_local_planner/TrajectoryPlannerROS" For 1.1+ series)
用于指定局部路径规划的插件名， 默认值为 "TrajectoryPlannerROS"
~recovery_behaviors (list, default: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
用于指定机器人在无法避障情况下的恢复行为， 其整体流程参考上图。
~/local_costmap/circumscribed_radius
用于指定代价地图的机器人外切半径
~controller_frequency (double, default: 20.0)
发送速度控制命令的频率。
~planner_patience (double, default: 5.0)
规划器尝试查找有效行进路径的最大容忍时长。
~controller_patience (double, default: 15.0)
如果没有收到有效的控制的命令，那么在进行空间清除的操作前，机器人会会等待的最长时间秒数。
~conservative_reset_dist (double, default: 3.0)
距离机器人的距离，以米为单位，当试图清除地图中的空间时，超过该距离的障碍物将从成本地图中清除. 注意： 该参数只在默认自恢复行为被使用的时候有效。
~recovery_behavior_enabled (bool, default: true)
是否使能机器人的自恢复行为。
~clearing_rotation_allowed (bool, default: true)
机器人是否允许通过原地旋转来进行清除地图的动作。 注意： 该参数只在默认自恢复行为被使用的时候有效。
~shutdown_costmaps (bool, default: false)
当move_base的节点未启动的情况下， 机器人是否能够关闭节点的代价地图。
~oscillation_timeout (double, default: 0.0)
在执行自恢复行为之前允许振荡运动多长时间（秒）。 0.0的值对应着无限长的超时时间在在最新的navigation 1.3.1中。
~oscillation_distance (double, default: 0.5)
来回运动在多大距离以上不会被认为是振荡运动。 超出这个距离将会重置oscillation_timeout的值在最新的navigation 1.3.1中。
~planner_frequency (double, default: 0.0)
全局路径规划的频率值， 如果这个值被设置为0.0， 那么全局规划器将会在只收到新的目标的情况下运行或者局部规划器报告改路径已经存在阻塞， 发布于最新的版本navigation 1.6.0中。
~max_planning_retries (int32_t, default: -1)
在执行自恢复行为之前， 允许尝试多少次路径规划， -1.0的值表示允许无穷次的尝试。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

• move_base通过action_lib与用户进行联动开发， 后续将会使用其进行导航点相关的动作。
• move_base的配置主要包含两部分， 一部分是代价地图的配置， 一部分是路径规划的配置。

代价地图的配置

• 2d代价地图在机器人导航的时候， 需要配置生成全局的地图，也需要配置局部的代价地图， 通过movs_base功能包中costmap节点可以创建两张costmap地图文件， 每一张地图都有其独立的命名空间 local_costmap and global_costmap。
• 关于2d代价地图的详细介绍可以参考: http://wiki.ros.org/costmap_2d，局部和全局代价地图都要配置对应的参数特性， 其常见的参数如下所示：

# 插件配置
# 如果不指定， 则使用默认插件行为，如果指定代价地图的插件，则需要配置插件中的所需的每个参数，
# 一般使用默认插件行为的话， 只需配置infaltion_radius等参数， inflation_radius参数其实是inflation_layer中的一个配置参数。
~<name>/plugins  (sequence, default: pre-Hydro behavior) 
#坐标系配置
~<name>/global_frame (string, default: "/map")  #指定全局坐标系， 一般为map
~<name>/transform_tolerance (double, default: 0.2) #指定坐标变换的最长时间容忍度
# 频率配置
~<name>/update_frequency (double, default: 5.0)
~<name>/publish_frequency (double, default: 0.0)
# 地图管理
~<name>/footprint (geometry_msgs/Polygon) #设置机器人的投影尺寸
~<name>/static_map  # 设置true的时候， 表示从map话题中初始化障碍物层，需要通过map_server发布话题， 一般用在全局代价地图中。
~<name>/rolling_window (bool, default: false)   #如果static_map设置为true的话， 该值必须设置为false
~<name>/always_send_full_costmap (bool, default: false) #设置为false表示表示只更新变换的部分，设置为true表示总是更新整张地图。
~<name>/width (int, default: 10) # 地图的宽度
~<name>/height (int, default: 10)# 地图的高度
~<name>/resolution (double, default: 0.05) #地图的精度
~<name>/origin_x (double, default: 0.0) #地图的原点
~<name>/origin_y (double, default: 0.0)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

• 创建global_costmap_params.yaml文件，用于设置全局代价地图

global_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 5.0
  static_map: true  #是否通过map_server的地图初始化代价地图。
1
2
3
4
5

• 创建local_costmap_params.yaml文件，用于设置局部代价地图

local_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05
1
2
3
4
5
6
7
8
9
10

• 常见的代价地图中的共同的配置，即Common Configuration (local_costmap) & (global_costmap)如下， 创建文件为costmap_common_params.yaml，这个文件包含着global_costmap和local_costmap相同的配置，我们可以借此避免重复设置编写， 填入以下内容：

obstacle_range: 2.5  #表示将会被更新到代价地图的最远障碍物距离。
raytrace_range: 3.0  #表示将会跟踪空闲区域的传感器距离范围。
footprint: [[x0, y0], [x1, y1], ... [xn, yn]]   #设置机器人的投影尺寸大小，此时机器人中心为(0.0, 0.0)，投影尺寸设置的点坐标支持逆时针设置，也支持顺时针设置。
#robot_radius: ir_of_robot   #如果机器人是圆形的话，通过设置机器人的半径来设置机器人的投影尺寸。
inflation_radius: 0.55  #设置障碍物的膨胀半径，也就是可容忍的安全距离，这是inflation_layer包的所包含的一个参数

observation_sources: laser_scan_sensor point_cloud_sensor #设置障碍物的观测源， 每个观测源的具体定义如下所示。
# 激光传感器的配置如下示例：
laser_scan_sensor: {sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking: true, clearing: true}
#点云数据的配置如下示例：
point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}
1
2
3
4
5
6
7
8
9
10
11

• 以上配置是对代价地图比较常规的配置，当我们需要引入更多的层的时候， 这样的配置就远远不够了， 后续还要引入障碍层插件来进行相关配置， 其每个层都有自己相关的参数，要一个个配置起来， 如下所示：

#robot_radius: 0.20  # distance a circular robot should be clear of the obstacle (kobuki: 0.18)
footprint: [[x0, y0], [x1, y1], ... [xn, yn]]  # if the robot is not circular
map_type: voxel #costmap , 指定代价地图显示的类型。
obstacle_range: 2.5
raytrace_range: 3.0
plugins:
     - {name: static_layer,            type: "costmap_2d::StaticLayer"}
     - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

obstacle_layer: #http://wiki.ros.org/costmap_2d/hydro/obstacles
  enabled:              true
  max_obstacle_height:  0.6
  origin_z:             0.0
  z_resolution:         0.2
  z_voxels:             2
  unknown_threshold:    15
  mark_threshold:       0
  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
  publish_voxel_map: false
  observation_sources:  scan bump
  scan:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    min_obstacle_height: 0.25
    max_obstacle_height: 0.35
  bump:
    data_type: PointCloud2
    topic: mobile_base/sensors/bumper_pointcloud
    marking: true
    clearing: false
    min_obstacle_height: 0.0
    max_obstacle_height: 0.15
  # for debugging only, let's you see the entire voxel grid

#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer: #http://wiki.ros.org/costmap_2d/hydro/inflation
  enabled:              true
  cost_scaling_factor:  5.0  	# exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1),  所以当值取越小的时候，代价会越高。
  								# exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.5  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:  # http://wiki.ros.org/costmap_2d/hydro/staticmap
  enabled:              true
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

除了以上的层， 我们还可以配置 超声波的层参考 http://wiki.ros.org/social_navigation_layers 以及人流的层http://wiki.ros.org/social_navigation_layers。

• 整个代价地图的计算参考下图，非常的清晰明了。
  
• 总结一下， 首先机器人会通过static_map生成类似下图的黄色点的障碍物地图， 然后通过obstacles_layer实时将传感器的状态以黄色点的显示形式更新到障碍物层中， 再通过膨胀层膨胀出安全的半径距离，也就是inflation_radius， 注意这里的膨胀半径在rviz中会通过两部分不同的颜色显示，包括青蓝色以及紫色，其中青蓝色表示机器人footprint的外界半径， 即上图中的的值为 circumscribed radius， 最外层的紫色的值为 inflation_radisu - circumscribed_radius。
  

配置obstacles layer

• 在使用obstacle_layer插件生成障碍物层的时候，需要通过map_type参数来指定障碍地图的显示类型为 voxel或者obstacles， 障碍物的显示支持二维以及三维的数据显示， 其将会订阅激光点云数据以及point点云数据来生成相应的地图， 可以参考链接：http://wiki.ros.org/costmap_2d/hydro/obstacles。
• 地图跟踪显示的插件包含了ObstacleCostmapPlugin以及VoxelCostmapPlugin， 前者是二维的， 后者是三维的。使用时，可以通过以下配置来指定某个层的显示的插件类型，

plugins:
   - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
#  - {name: obstacle_layer,          type: "costmap_2d::ObstacleLayer"} 
1
2
3

• 当配置了obstacles_layer为costmap_2d::VoxelLayer数据类型时， 并且设置了publish_voxel_map=true，此时voxel_grid (costmap_2d/VoxelGrid)类型的三维地图数据将会被发布，但是目前该话题无法在RVIZ中直接显示，rviz没有开发相应的显示插件，需要借助以下命令来转换一下voxel_grid的数据, 显示效果如下：

  	rosrun costmap_2d costmap_2d_markers voxel_grid:=<your_voxel_grid_topic> visualization_marker:=		<your_visualization_marker_topic>
  1

  

配置static layer

一般配置为enabled即可， 具体请参考 http://wiki.ros.org/costmap_2d/hydro/staticmap, 该层由map_server发布的map话题来生成。

配置infalation_layer

enabeled， 表示使能该层，并加入到代价地图中。
~/inflation_radius (double, default: 0.55) ， 配置障碍物的膨胀半径，
~/cost_scaling_factor (double, default: 10.0)，配置机器人的代价地图的比例因子
如下图， 黄色点表示障碍物， 青蓝色表示机器人footprint的外接半径， 紫色加青蓝色表示机器人的膨胀层的膨胀半径。