ROS从入门到精通6-1：图文详解代价地图costmap原理与配置

1 地图服务

ROS提供处理地图数据的功能包map_server，其中包含两个关键节点：

• map_saver：用于将栅格地图序列化持久存储到磁盘
• map_server：反序列化读取磁盘的栅格地图并以服务的方式开放给ROS系统

1.1 地图保存节点

地图保存节点map_saver通过订阅话题/map来获取地图数据nav_msgs/OccupancyGrid

在建图结束后(或其他任意方式向/map话题发布了地图数据)，可以通过.launch文件或命令行运行地图保存节点

• .launch形式

  <launch>
      <arg name="filename" value="$(find map_lab)/maps/map" />
      <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
  launch>
  

  保存到该功能包下目录/maps中，地图名为map

• shell形式

  rosrun map_server map_saver -f ~/map
  

  保存到目录~/下，地图名为map

保存地图后会产生两个文件：

1. .pgm：地图图片文件，可使用图片查看程序打开或用图片编辑软件编辑

2. .yaml：地图数据描述文件

   给出一个实例：

   # image：对应地图资源路径，可以是绝对路径或相对路径
   # resolution： 图片分片率(单位: m/像素)
   # origin：定义地图原点为左下像素，该项设置了地图位姿(x,y,theta)分别为位置和偏航角(逆时针为正)
   # negate：是否应该颠倒自由栅格/占用栅格的语义
   # occupied_thresh: 占用阈值，占用概率大于此阈值的像素被视为完全占用
   # free_thresh：自由阈值，占用概率小于此阈值的像素被视为完全自由
   
   image: map.pgm
   resolution: 0.050000
   origin: [-10.000000, -10.000000, 0.000000]
   negate: 0
   occupied_thresh: 0.65
   free_thresh: 0.196
   

   所谓占用概率是指$p=(255-x)/255$，即该点像素的归一化数值
   地图原点则是地图相对地图坐标系的偏移位姿
   

1.2 地图服务节点

地图服务节点map_server通过发布话题/map_metadata与/map来开放地图数据

# /map_metadata (nav_msgs / MapMetaData)
time map_load_time
float32 resolution #地图分辨率
uint32 width #地图宽度
uint32 height #地图高度
geometry_msgs/Pose origin #地图位姿数据
  geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

# /map (nav_msgs / OccupancyGrid)
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
#--- 地图元数据
nav_msgs/MapMetaData info
	...(如上)
#--- 地图内容数据，数组长度 = width * height
int8[] data

可以通过.launch文件的形式将磁盘的地图数据加载到内存，并发布到指定话题，同时开启Rviz可视化地图。在可视化中，map_server用颜色的深浅来表示地图的占用情况——完全自由(0)、中间情况(0-100)、完全占用(100)、未知(-1)。

<launch>
    
    <arg name="map" default="map1.yaml" />
    
    <node name="map_server" pkg="map_server" type="map_server" args="$(find map_lab)/maps/$(arg map)"/>
    
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find map_lab)/rviz/map_server_test.rviz"/>
launch>

1.3 地图编辑

有时建立的地图会存在细微的瑕疵，主要是：地图边缘不完全封闭、地图中存在噪点等。为此，需要对ROS机器人建立的场景地图进行处理。本文采用Ubuntu地图编辑器GIMP，该软件可在Ubuntu应用商店中下载。

下面给出一个实例，如图为存在噪点的地图，现通过GIMP画笔工具降噪。

修复后的地图如下所示，地图中部的噪声已被去除。

2 代价地图

2.1 基本概念

机器人导航必须依赖于地图，第1节演示的由SLAM构建的地图为静态地图，在导航中一般不可以直接使用，因为导航过程中障碍信息是可变的，因此地图信息需要时时更新。

代价地图就是ROS定义的用于动态导航的地图数据结构，其在静态地图基础上添加了一些辅助信息，主要分为以下图层：

• 静态地图层(Static Map Layer)：通常是由SLAM建立的静态地图
• 障碍地图层(Obstacle Map Layer)：用于动态记录传感器感知到的障碍信息
• 膨胀层(Inflation Layer)：在以上两层地图基础上进行障碍膨胀，主要目的是防止机器人靠近障碍物边缘时，因惯性、不规则形体等原因与障碍产生碰撞，因此需要让机器人充分远离障碍物
• 其他图层(Other Layers)：可以通过插件形式自定义代价地图层，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。关于插件开发请参考ROS从入门到精通(十三) ROS插件库与开发

2.2 代价定义

关于代价地图代价的定义，摘录官方说明：如图所示，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值，灰度越高代价越大

• 致命障碍：栅格值254，此时障碍物与机器人中心重叠，必然发生碰撞
• 内切障碍：栅格值253，此时障碍物处于机器人的内切圆内，必然发生碰撞
• 外切障碍：栅格值[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞
• 非自由空间：栅格值(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞
• 自由区域：栅格值0，此处机器人可以自由通过
• 未知区域：栅格值255，未探明是否有障碍物

看一个实例：其中紫色区域为致命障碍；浅蓝色区域为内切障碍；红色区域为外切障碍与非自由障碍空间，颜色越深代价越高；蓝色为自由空间。

3 Costmap配置

打开move_base.launch可以看到代价地图的配置参数说明

<rosparam file="$(find costmap_lab)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
<rosparam file="$(find costmap_lab)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
<rosparam file="$(find costmap_lab)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find costmap_lab)/param/global_costmap_params.yaml" command="load" />

主要涉及三个配置文件：

• costmap_common_params.yaml：局部代价地图与全局代价地图都共同需要的通用代价地图配置参数
• local_costmap_params.yaml：局部代价地图配置参数
• global_costmap_params.yaml：全局代价地图配置参数

接下来一一解释。

3.1 通用配置

# costmap_common_params.yaml
obstacle_range: 3.0
raytrace_range: 3.5

footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]]
#robot_radius: 0.17

inflation_radius: 1.0
cost_scaling_factor: 3.0

map_type: costmap
observation_sources: scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

• obstacle_range：设置机器人检测障碍物的最大范围，只有进入该范围内才把该障碍物当作影响路径规划和移动的障碍物

• raytrace_range：由于机器人是动态运动的，因此可能上个时刻机器人所处位置附近的障碍物在下次检测代价时已经不存在，该参数设置了代价地图的更新范围

• 机器人形状参数：

  • robot_radius：对于圆形机器人，需要该参数设置机器人半径，单位是米
  • footprint：对于其他形状机器人，需要该参数设置机器人凸点坐标，可参考
    

• inflation_radius：膨胀半径，膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。如果机器人经常撞到障碍物就需要增大该值，若经常无法通过狭窄地方就减小该值，代价膨胀公式：

  exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (INSCRIBED_INFLATED_OBSTACLE - 1)

  其中，distance_from_obstacle - inscribed_radius是机器人到实际障碍物与内切圆半径之差(且该差值小于膨胀半径)，INSCRIBED_INFLATED_OBSTACLE设定为254

• cost_scaling_factor：膨胀过程代价比例系数，增大比例因子会降低代价

  

• map_type：地图类型，设定为代价地图

• observation_sources：设置导航中所使用的传感器，例如激光雷达、碰撞传感器、超声波传感器等。每个传感器需要进行配置：

  • sensor_frame：传感器坐标系名称
  • data_type：传感器数据类型
  • topic：传感器发布的话题名
  • marking：是否可使用该传感器来标记障碍物
  • clearing：是否可使用该传感器来清除障碍物标记

3.2 局部配置

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5  

  static_map: false  
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05

• global_frame：在局部代价地图中的全局坐标系，一般设置为里程计odom

• robot_base_frame:机器人基坐标系通过global_frame和robot_base_frame就可以计算两个坐标系间的变换，得知机器人在局部代价地图位置坐标

• update_frequency：局部代价地图的更新频率

• publish_frequency：局部代价地图的发布频率

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

• static_map：配置是否使用map_server提供的静态地图，一般局部地图需要检测是否在机器人附近有新增的动态障碍物，故设置为false

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

  • width：滚动窗口宽度，单位是米
  • height：滚动窗口的高度，单位是米

  

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

3.3 全局配置

global_costmap:
  global_frame: map
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5

  static_map: true

• global_frame：全局代价地图所处的坐标系，一般为map
• robot_base_frame：机器人基坐标系。通过global_frame和robot_base_frame就可以计算两个坐标系间的变换，得知机器人在全局代价地图中的位置坐标
• update_frequency：全局代价地图更新频率，一般全局代价地图更新频率小于局部代价
• publish_frequency：全局代价地图发布频率
• transform_tolerance：坐标系间转换可以忍受的最大延时
• static_map：配置是否使用map_server提供的静态地图，一般全局地图是静态的，需要设置为true

---

更多精彩专栏：

• 《ROS从入门到精通》
• 《机器人原理与技术》
• 《机器学习强基计划》
• 《计算机视觉教程》
• …

源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系