代价地图(三) 代价值和机器人轮廓的关系

costmap中，机器人需要避开区域包括3类：

静态地图中的障碍物，适合描述整个静态场景的静止障碍物信息，如墙面、固定不动的桌柜等，对应StaticLayer，一旦在costmap中加入static_map,即使地图中的障碍物后来移走了，该区域在costmap中依然存在，机器人仍然会避开该区域。
传感器观察到的障碍物，适合描述运动的障碍物，如行人、小车、移动位置的桌椅等。通过传感器激光实时扫描到的障碍物ObstacleLayer。这一块障碍物信息是不断变化的，对于运动小车的实时避障是很有帮助的。
自定义障碍物，禁止行驶区域

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

定义了5个具体的值衡量机器人的状态

上图共分为五个部分:（下面的红色框图是机器人的轮廓，旁边的黑框是上图的映射位置）

Lethal（致命的）:机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。枚举值254
Inscribed（内切）：网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。枚举值253
Possibly circumscribed（外切）：网格的外切圆与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
Freespace（自由空间）：没有障碍物的空间。枚举值0
Unknown（未知）：未知的空间。枚举值255

代价地图采用网格的形式，分为三种状态：占用（有障碍物）、无用（空闲的）、未知。每个网格的值如下：

NO_INFORMATION = 255;
LETHAL_OBSTACLE = 254;
INSCRIBED_INFLATED_OBSTACLE = 253;
FREE_SPACE = 0;

注意252和127没有定义

cost_lethal 代价值254，机器人中心在一个cell里，这肯定发生碰撞
cost_inscribed 代价值253，机器人中心所在的cell与障碍物的距离小于机器人的内切圆半径，还是肯定碰撞，cell的代价值大于等于inscribed cost
cost_possibly_circumscribed 使用机器人的外接圆半径作为 cutoff distance. 机器人中心所在cell到障碍物距离小于机器人外切圆半径，但是大于内切圆半径。如果机器人中心所在的cell大于等于这个值，是否碰撞要取决于机器人的朝向

possibly这个词表示不一定真是一个obstacle cell，而是用户自定义一个代价值的情况。比如，如果用户想让机器人应对避开一段特定的区域，应当向那段区域的代价地图插入自定义的代价值，而不考虑任何障碍物。

尽管128也在上图中，但真实的代价值受内切圆半径和外接圆半径的影响，对于源码在InflationLayer::computeCost:

/** @brief  Given a distance, compute a cost.
 * @param  distance The distance from an obstacle in cells
 * @return A cost value for the distance */
inline unsigned char computeCost(double distance) const
{
  unsigned char cost = 0;
  if (distance == 0)
    cost = LETHAL_OBSTACLE;
  else if (distance * resolution_ <= inscribed_radius_)
    cost = INSCRIBED_INFLATED_OBSTACLE;
  else
  {
    // make sure cost falls off by Euclidean distance
    double euclidean_distance = distance * resolution_;
    double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
    cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
  }
  return cost;
}

障碍层膨胀层

Obstacle Map Layer：障碍层，用于动态的记录传感器感知到的障碍物信息。

Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的撞上障碍物。膨胀是一个从占用cell代价值从内切障碍过渡到FREE的过程，距离cell越远代价越小，膨胀层会把障碍物代价膨胀直到该半径为止。

到实际障碍物距离dist， $内切半径 < dist < 内切半径+膨胀半径$ . 在内切圆半径到膨胀半径之间的所有cell，可以使用如下公式来计算膨胀代价：

$cost = 253\ e^{-cost\_scale\_factor*(dist - inscribed\_radius)}$

如果cost_scaling_factor默认10，膨胀半径默认0.55，那么cost最小约为1

cost_scaling_factor: 膨胀过程中应用到代价值的比例因子，默认10。从上面公式可以看出，增大比例因子会降低代价。10对应的代价值降低速度很快

内切圆半径0.255，不同代价系数对应的代价曲线

inflation_radius:膨胀半径，默认0.55. 膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。如果太小，车就会贴着障碍走，或者说容易撞到障碍物就需要增大该值，若经常无法通过狭窄地方就减小该值。

增大膨胀半径，cost_scaling_factor=0.7
cost_scaling_factor=65
cost_scaling_factor=1
cost_scaling_factor=25
默认cost_scaling_factor=10

减小cost_scaling_factor，使代价值增大，rviz里的膨胀层变为粉色。如果这个参数很大很大，代价值趋近于0，也就是FREE。如果很小，代价值趋近于253，相当于放大了内切障碍值。cost_scaling_factor越小，膨胀层越趋近粉色，反之趋近蓝色。

查看话题/move_base/local_costmap/costmap_updates，会发现一些数值变大

全局代价地图一般膨胀较多，使机器人转弯顺利。 但是局部代价可以不用膨胀层，这是因为TEB已有障碍物膨胀功能。增大cost_scaling_factor和减小膨胀半径可以使机器人容易进窄通道，但一般是调整TEB，这两个参数不修改。

添加了静态层、障碍层、膨胀层的代价地图是这样的：

如果代价地图不添加膨胀层，结果是这样：

但是加不加障碍层，看上去没有变化， 目前无法在rviz里直接可视化每一层，只能添加或去除后观察

障碍层 膨胀层

障碍层膨胀层