机器人建图算法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给出了算法的伪代码，首先给出几个定义：

o b s t s obst_s obsts​ ： 与网格s距离最近的障碍物
d i s t s dist_s dists​ ： s与距离最近的障碍物的距离值
t o R a i s e s toRaise_s toRaises​ ： s是否要获得并传播raise状态
i n s e r t ( o p e n , n , d i s t n ) insert(open, n, dist_n) insert(open,n,distn​) ： 将n与 d i s t n dist_n distn​插入open表中
C l e a r C e l l ( s ) ClearCell(s) ClearCell(s) ： o b s t s = c l e a r e d , d i s t s = ∞ obst_s=cleared, dist_s=\infty obsts​=cleared,dists​=∞
i s O c c ( s ) isOcc(s) isOcc(s) ： o b s t s = = s ∣ ∣ d i s t s = = 0 , T r u e obst_s==s ||dist_s==0, True obsts​==s∣∣dists​==0,True
p o p ( o p e n ) pop(open) pop(open) ： 返回open表中距离最小的s

然后解释几个函数的用途
SetObstacle(s)：在s上放置一个新的新障碍物（也就是观测到s的occupancy变成了1），将s放入open表中

RemoveObstacle(s)：把s上的障碍物去除，将s放入open表中

UpdateDistanceMap()：open表不为空时，取出一个距离最小的s，如果s具有raise状态，那就传播raise。否则如果s是个障碍物，那就传播lower。

raise(s)：对于s附近8邻域每个grid n，如果n最近的障碍不是cleared（这里是说location）并且不在raise状态，那么把n放入open表中，并且如果n最近的障碍没有被占据（这里是说location的occupancy），那么先将grid n clear并获得raise状态再放入open表。最后取消s的raise状态。

lower(s)：对于s附近8邻域每个grid n，如果n不在raise状态，就计算n与距离s最近的障碍物的距离d，如果d小于之前n与距离n最近障碍物的距离，那么更新 o b s t n = o b s t s obst_n=obst_s obstn​=obsts​， d i s t n = d dist_n=d distn​=d并将n放入open表中。

算法流程

下面解释伪代码和示意图给出的算法流程：

1. 通过occupancy map初始化地图，记录每个grid的obst和dist。
2. 在grid s加入或删除障碍物时都会将s放入open表中优先处理，加入障碍物时s的dist变0，obst变s，在删除障碍物时s的dist变无穷，obst被clear，并获得raise状态。
3. 优先更新加入或删除障碍的grid，加入grid进行lower，删除grid先raise再lower。
4. 所谓lower，就是对于那些不在raise中的相邻grid n，如果更新时和父传播点的障碍物的距离比与更新前自身的障碍物距离小，那就更新n的障碍物，并且把这个状态传给子传播点（也就是说，如果父节点的最近障碍物改变了，那么子节点的最近障碍物也很有可能会改变）。
5. 所谓raise，就是说如果父节点处于raise状态，子节点的最近障碍物被删除了，那么子节点就要rest自己的dist和obst并传播raise状态。

总结

一句话概括：raise决定删除障碍物时影响grid的范围，lower决定更新后的grid ESDF值。