Costmap2d 膨胀算法图解

costmap2d 的基本概念和结构可以参考这篇 costmap2d详解 文章，写得非常详细。

代价地图

Fig.1 costmap 代价地图的官方说明图

图片左下角红色多边形是机器的 footprint，上方是“障碍物距离-代价”函数。

障碍物所在栅格的代价是 254。footprint 内蓝色圆圈是机器的内切半径圆，与障碍物在这个内切半径距离内对应的代价是 253，内切值。footprint 外面更大的蓝色圆圈是机器的外接半径圆，与障碍物在这个外接半径距离对应的代价是 128，外接值。

准备

生成距离表和代价值表

距离表，坐标轴以栅格为单位：

X 轴表示两个栅格坐标的 x 的差绝对值，Y 轴表示两个栅格坐标的 y 的差绝对值。由于计算地图上两个坐标之间的距离是相对的，所以任意坐标之间的距离都可以作差取绝对值转换到缓存表中。

e.g. [2,2] 和 [5,5] 作差取绝对值得到 [3,3] 直接查表得到对应的距离 。

代价表，坐标轴以栅格为单位：

X 轴表示栅格到障碍物坐标的 x 的差绝对值，Y 轴表示栅格到障碍物坐标的 y 的差绝对值。表格保存了到障碍物距离对应的地图代价。

比如 [0,0] 对应的距离为 0，即障碍物本身，所以栅格代价是 254；再比如 [1,0] 对应的距离为 1，假设该距离是机器 footprint 的内切半径，那么栅格代价是 253；[2,2] 对应的距离是，假设该距离超出了机器的内切半径，通过公式计算得到代价 235。

e.g.

下图中右上角白色栅格距离左方障碍物栅格 x 轴相差 3栅格，y 轴相差 2 个栅格，查表 [3,2] 得到白色栅格代价为 210

Fig.2 距离表，代价表查值

更新边界范围

更新的边界范围需要增加一个膨胀半径大小。因为在原范围外的障碍物更新可能会影响到边界内。

Fig.3 黑框表示膨胀层需要更新的范围

Fig.3 图表示更新范围（黑框）内无法正确计算出膨胀代价。把黑框外附近的障碍物膨胀信息忽视掉了。

Fig.4 绿框表示临时扩大的更新范围

Fig.4 图示，当原更新范围（黑框）外附近出现了新障碍物，那么该障碍物的膨胀会影响到黑框内。如果临时扩大更新范围一个膨胀半径大小，达到绿框的大小，就可以获取到新障碍物信息，黑框内的膨胀代价才能正确计算。

更新地图代价

迭代流程图

Fig.5 膨胀迭代流程图

需要知道膨胀栅格列表一开始只有障碍物的信息，在第一次迭代后，就有了与障碍物距离为一个栅格的膨胀栅格，迭代第二次就有了与障碍物距离为两个栅格的膨胀栅格......

而膨胀栅格列表使用 std::map<障碍物距离，膨胀栅格队列> 的数据结构存储。map 容器默认会把元素按<键>的大小，从小到大排列，而膨胀也正好是从距离从近到远迭代。膨胀每迭代一次，正好给 map 插入了一系列 <障碍物距离> 更大的栅格元素！

迭代过程

Fig.6 获取更新范围内障碍物栅格(紫)

Fig.7 第一次膨胀由障碍物(紫)扩散

Fig.8 第二次膨胀由内切栅格(青)扩散

Fig.9 第三次膨胀由膨胀栅格(红)扩散

Fig.10 第四次膨胀由栅格(洋红)扩散