机器人建图算法2.1从栅格占据地图到ESDF地图

前言

最基础的地图是占据栅格地图Occupancy map，每个格子标明了该位置是否被物体占据。然而对于规划和避障而言，地图中的占据信息是不够的，还需要障碍距离、方向等信息。TSDF和ESDF地图弥补了这个缺陷。

IROS 2010: Improved updating of Euclidean distance maps and Voronoi diagrams 这篇论文提供了一种从Occupancy Map更新ESDF地图的方法。

论文解读

这篇文章实际上要解决通过occupancy map生成ESDF和voronoi graph两个问题，但这里只解读与ESDF相关的内容。

文章Part.Ⅲ 描述了生成ESDF的算法流程，伪代码如下：

并且给出了一个示意图：

示意图说明

ABCD四幅图中网格grids的深浅表明了与最近障碍物之间的距离（其实就是ESDF），纯黑色的grid说明该位置是障碍物。

• A图是ESDF的原始状态
• B图中，A图左方的障碍物被去除，A图右方放置了一个新的障碍物。新障碍物附近的grids获得lower状态并向外传播，旧障碍物附近的grids获得raise状态并向外传播。
• C图中，新障碍物附近稍远的少量grids继续传播lower状态，而多数grids不再传播lower状态。旧障碍物附近稍远的grids获得了lower状态，并向旧障碍物方向传播。
• D图中，新障碍物附近稍远的grids不再传播lower状态。旧障碍物向内传播lower状态达到中心后也停止了传播。

raise和lower的意思是向上，也就是（最近障碍物）距离增加和减少。显而易见，从每个grid的ESDF是通过距离的增减状态传播进行更新的。

将这个示意图与伪代码放在一起看能够加速算法理解。

伪代码说明

Algorithm 1给出了算法的伪代码，首先给出几个定义：

$obs{t}_s$ ： 与网格s距离最近的障碍物
$dis{t}_s$ ： s与距离最近的障碍物的距离值
$toRais{e}_{}$