costmap_2d详解1：costmap设计理论

Costmap 代价地图设计理论

    使用gmapping 构建的地图为全局静态地图，要实现导航避障功能，单靠这一张地图是不够安全的，例如在导航过程中突然出现的障碍物(动态障碍物)，因此需要对这张地图进行各种加工修饰，使导航避障更安全，而这功能在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等开源插件。

图 4-15 costmap图层

图costmap的分层（左图默认分层结构，右图为人为分层结构示例）

costmap由多个层共同组成，将不同功能放置在不同层中。如图8-2（左）所示，Static_Layer（静态地图层，即原始地图）是第一层；Obstacle_Layer（障碍物层，即原始地图中没有的动态障碍物）是第二层；Inflation_Layer（膨胀层，即将前述所有层所有障碍物都膨胀一定大小）是第三层。这三层组合成了master map（最终的costmap），供给路线规划模块使用。其他层可以通过pluginlib在costmap中实现和使用

图 4-16 costmap障碍物膨胀图

二维costmap地图示例如图上所示。红色单元表示代价地图中的障碍物，蓝色单元表示由机器人内切半径来计算膨胀的障碍物，红色多边形表示机器人的垂直投影（footprint），浅灰色代表已知且未被占用的区域，深灰色代表未知区域。为了避免机器人与障碍碰撞，机器人的垂直投影（footprint）不能与红色单元相交，机器人的中心不能穿过蓝色单元

Costmap 地图又叫占用栅格图，即将整张地图分成多个小栅格(默认每个小栅格是边长为0.05m的正方型)，每个栅格又分为三种情况: Occupied被占用（有障碍）、Free自由区域（无障碍）、Unknown Space未知区域,划分规则如下图所示：

图 4-17 costmap 代价计算说明图

其中规划行走路径时，被占用和未知区域是不允许行走的，而Free自由区域中的每个小栅格，会根据离障碍物的远近，计算出一个代价值，最终用于路径规划，计算公式如下

exp(-1.0 * cost_scaling_factor * (distance_from_obstacle – inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE – 1)

 

cost_scaling_factor 是膨胀的比例系数，默认配置为10.0, 越大表明指数曲线下降越快

代价地图中到实际障碍物距离在内切圆半径到膨胀半径之间的所有cell可以使用如下公式来计算膨胀代价