costmap代价地图学习

为什么要有costmap

ROS里的地图的概念，地图就是/map这个topic，它也是一张图片，一个像素代表了实际的一块面积，用灰度值来表示障碍物存在的可能性。然而在实际的导航任务中，光有一张地图是不够的，机器人需要能动态的把障碍物加入，或者清楚已经不存在的障碍物，有些时候还要在地图上标出危险区域，为路径规划提供更有用的信息。

因为导航的需要，所以出现了代价地图。你可以将代价地图理解为，在/map之上新加的另外几层地图，不仅包含了原始地图信息，还加入了其他辅助信息。

代价地图有一下特点： １．首先，代价地图有两张，一张是local_costmap，一张是global_costmap，分别用于局部路径规划器和全局路径规划器，而这两个costmap都默认并且只能选择costmap_2d作为插件。 ２. 无论是local_costmap还是global_costmap，都可以配置他们的Layer，可以选择多个层次。costmap的Layer包括以下几种：

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

可以同时选择多个Layer并存。

配置costmap_common_params.yaml

与9.2节中move_base插件的配置类似，costmap配置也同样用yaml 来保存，其本质是维护在参数服务器上。由于costmap通常分为local和global的coastmap，我们习惯把两个代价地图分开。

robot_radius: 0.2
obstacle_layer:
  enabled: true
  combination_method: 1
  track_unknown_space: true
  obstacle_range: 2.5
  raytrace_range: 3.0
  observation_sources: laser_scan_sensor
  laser_scan_sensor: {
    sensor_frame: /robot0_laser_0,
    data_type: LaserScan,
    topic: /robot0/laser_0,
    marking: true,
    clearing: true
  }
inflation_layer:
  enabled: true
  cost_scaling_factor: 5.0
  inflation_radius: 0.36
static_layer:
  enabled: true

下面来依次解释下各参数的意义，：
robot_radius：设置机器人的半径，单位是米。由于在stdr中机器人是圆形的，所以可以直接设置该参数。如果你的机器人不是圆形的那就需要使用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:设置导航中所使用的传感器，这里可以用逗号形式来区分开很多个传感器，例如激光雷达，碰撞传感器，超声波传感器等，我们这里只设置了激光雷达;

laser_scan_sensor:添加的激光雷达传感器

sensor_frame:激光雷达传感器的坐标系名称;

data_type:激光雷达数据类型;

topic:该激光雷达发布的话题名;

marking:是否可以使用该传感器来标记障碍物;

clearing:是否可以使用该传感器来清除障碍物标记为自由空间;

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_frame:在全局代价地图中的全局坐标系;
#  robot_base_frame:机器人的基坐标系;
global_costmap:
  global_frame: /map
  robot_base_frame: /robot0
  update_frequency: 0.5
  static_map: true
  rolling_window: false
  transform_tolerance: 1.0
  plugins:
    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

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

global_frame:全局代价地图需要在哪个坐标系下运行;

robot_base_frame:在全局代价地图中机器人本体的基坐标系，就是机器人上的根坐标系。通过global_frame和robot_base_frame就可以计算两个坐标系之间的变换，得知机器人在全局坐标系中的坐标了。

update_frequency:全局代价地图更新频率，一般全局代价地图更新频率设置的比较小;

static_map:配置是否使用map_server提供的地图来初始化，一般全局地图都是静态的，需要设置为true;

rolling_window:是否在机器人移动过程中需要滚动窗口，始终保持机器人在当前窗口中心位置;

transform_tolerance:坐标系间的转换可以忍受的最大延时;

plugins:在global_costmap中使用下面三个插件来融合三个不同图层，分别是static_layer、obstacle_layer和inflation_layer，合成一个master_layer来进行全局路径规划。

配置local_costmap_params.yaml

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

#FileName: local_costmap_params.yaml
#  本地代价地图需要配置的参数，各参数意义如下:
#  global_frame:在本地代价地图中的全局坐标系;
#  robot_base_frame:机器人本体的基坐标系;
local_costmap:
  global_frame: /map_static
  robot_base_frame: /robot0
  update_frequency: 5.0
  publish_frequency: 3.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.05
  transform_tolerance: 0.5
  plugins:
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

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

global_frame:在局部代价地图中的全局坐标系，一般需要设置为odom_frame，但是由于stdr没有这个坐标系，我就拿/map_static来代替了;

robot_base_frame:机器人本体的基坐标系;

update_frequency:局部代价地图的更新频率;

publish_frequency:局部代价地图的发布频率;

static_map:局部代价地图一般不设置为静态地图，因为需要检测是否在机器人附近有新增的动态障碍物;

rolling_window:使用滚动窗口，始终保持机器人在当前局部地图的中心位置;

width:滚动窗口的宽度，单位是米;

height:滚动窗口的高度，单位是米;

resolution:地图的分辨率，该分辨率可以从加载的地图相对应的配置文件中获取到;

transform_tolerance:局部代价地图中的坐标系之间转换的最大可忍受延时;

plugins:在局部代价地图中，不需要静态地图层，因为我们使用滚动窗口来不断的扫描障碍物，所以就需要融合两层地图（inflation_layer和obstacle_layer）即可，融合后的地图用于进行局部路径规划;