《Efficient grid-based spatial representations for robot navigation in dynamic environments》

关键概念

  • DM: 这是距离地图地图（Distance Map）的缩写，它是一种在计算机图形学和计算机视觉中常用的数据结构。距离地图是一个二维数组，其中每个单元格包含到最近的障碍物的距离。距离地图可以用来加速碰撞检测、路径规划和机器人定位等问题的解决。在本文中，我们将距离地图用于表示机器人周围的空间，以便在动态环境中更新机器人的配置空间。

  • GVD: 这是广义Voronoi图（Generalized Voronoi Diagram）的缩写。在计算几何中，广义Voronoi图是一种将空间划分为几个区域的方法，每个区域都包含一个给定集合中的一个对象，任何在该区域内的点都比其他区域的对象更接近该区域的对象。

  • C-Space: 这是配置空间（Configuration Space）的缩写，它是一种在机器人运动规划中常用的概念。配置空间是所有可能的机器人位置的集合。在配置空间中，机器人被视为一个点，而障碍物被扩展为机器人在其内部的所有位置。这使得路径规划问题可以被简化为在无障碍的配置空间中找到从起点到终点的路径。

1.介绍

许多机器人导航方法依赖于占用网格图来编码机器人周围区域的障碍物。这些地图可以从传感器数据中学习，它们非常适合解决路径规划、碰撞避免或定位等问题，而且它们可以轻松地更新以反映环境中的变化。

在过去，已经开发出了几种可以从网格地图派生出来的基于网格的空间表示法，例如，距离地图、Voronoi图和配置空间地图。这些表示法是许多不同机器人应用的重要构建块，因为它们可以用来加速解决上述问题的算法。本文提出了增量更新算法，以便在动态环境中在线使用这些表示法。我们还将我们的方法应用于更新非圆形机器人的配置空间中的距离地图和Voronoi图，例如，加速这些类型的平台的路径规划或碰撞避免。本文扩展了我们在这些主题上的先前工作1: 2，并包括了在同时定位和映射（SLAM）的背景下对3D距离地图和距离地图和Voronoi图的增量更新的额外实验。

广义Voronoi图（GVD）被定义为自由空间中的一组点，这些点到最近的两个障碍物的距离相同3。它是Voronoi图的离散形式，已经被广泛应用于各种领域4。在机器人学的背景下，Voronoi图是一种解决导航任务的流行的单元分解方法。它们作为路径规划的路线图的应用是一种吸引人的技术，因为它们在不同的图上的路径对应于相对于障碍物的拓扑不同的路线，这在某种意义上是“稀疏的”。这大大减少了搜索问题，例如可以用来生成n-best路径，为用户提供路线选择5。此外，沿着Voronoi图的边缘移动可以确保在通过障碍物时获得最大可能的清晰度。当Voronoi图被离散化并存储为地图时，由于邻近Voronoi线之间的错误连接，它们可能会失去其稀疏性质。我们计算GVDs的方法通过额外的条件克服了这个问题，确保了生成的GVDs的稀疏性。

距离地图（DM）中的单元格编码了到相应占用地图中被占用的最近单元格的距离。由于单元格查找只需要常数时间，因此DM提供了有效的碰撞检查手段，用于计算路径规划的遍历成本，以及使用可能性字段进行机器人定位6。由于这种变换的计算是在不考虑机器人形状的情况下进行的，因此直接应用普通DM仅限于对机器人足迹的圆形近似。

在变化的环境中，预计算的GVDs、DMs和c-space地图需要定期更新，以始终反映相应占用地图的当前状态。这些变化可能是由移动的人或车辆、在映射过程中新探索的区域，或者在SLAM中闭合循环后修正地图所引起的。

在这篇论文中，我们提出了计算和更新这些表示法的有效方法。由于我们的算法以增量方式执行所有更新，即，只重新计算受到变化影响的部分，因此即使在大地图或超过两个维度的情况下，也可以在线应用。与以前的方法相比，我们的方法需要较少的计算工作，易于实现，并且在室内和室外环境中都能工作。

此外，我们将DMs和GVDs与c-space碰撞地图结合起来，并提出了距离变换的c-space地图和c-space Voronoi图。这些可以用于非圆形机器人的有效碰撞检查和路径规划。通过我们在这篇论文中描述的算法，这些表示法也可以以增量方式更新。

在第2节中讨论了相关工作后，我们在第3节中描述了brushfire算法。它可以用来计算静态DMs，它是我们在第4节和第5节提出的动态DM和GVD算法的重要基础。第6节描述了我们的动态c-space碰撞地图，然后是第7节的c-space DM和c-space GVD。我们的实验在第8节中呈现，然后我们在第9节中结束我们的论文。

2.相关工作

在过去，已经提出了许多不同的方法来计算DMs、GVDs和c-space碰撞地图。为了在动态环境中在线应用它们，已经花费了大量的精力来开发更有效的算法。然而，与我们的方法不同，大多数这些方法并没有利用增量更新的潜力。本节的其余部分将介绍不同空间表示法的相关工作，并讨论我们方法的贡献。

2.1.距离地图

已经提出了许多不同的方法来计算静态二维DMs，例如，线性图像遍历7，维度分解8，以及使用brushfire算法的距离传播9。我们在第3节中回顾了brushfire算法。对于其他方法的比较性回顾，请参考Fabbri等人的调查10。

每当网格地图中的一个单元格新被占用或空出时，相应的DM都需要更新以反映这种变化。一个简单的方法是重新计算所有改变的单元格周围 \(d̂\) 内的补丁的距离，其中 \(d̂\) 是环境中最小障碍物距离的上界。然而，这种方法通常更新的单元格远远超过必要的数量，例如，如果 \(d̂\) 由于大的开放空间或改变的单元格覆盖了广泛的区域而高。此外，如果变化影响了几个单元格的占用情况，有效地确定最小的更新区域并不是一件简单的事情。

Kalra等人提出了一种动态brushfire算法，该算法通过从新占用或空出的单元格开始传播波前，增量更新DMs和GVDs11。虽然他们的方法基于Stentz的增量路径规划算法D*12，但这里提出的算法直接源于brushfire算法，并且由于单元格访问次数大大减少，对于同样的任务需要的计算时间大大减少。

Kalra等人的波前积累8连通网格步骤来近似障碍物距离13。这将真实的欧几里得距离高估了最多8.0%14，这对于机器人来说意味着碰撞风险或过于保守的移动。Scherer等人采用并修正了Kalra的算法作为他们的DM更新方法15。他们传播障碍物位置而不是网格步数来确定欧几里得距离，这将绝对过估误差降低到0.09像素单位的上限以下16。根据Cuisenaire和Macq，这种传播误差可能发生的最短距离是13像素17，这产生了最大的相对误差0.69%。此外，通过传播障碍物引用，我们的表示法可以提供最近的障碍物的位置，而不仅仅是到它的距离，这对于碰撞避免方法可能很有吸引力。在最近的一篇论文中，Scherer等人基于我们原始的DM更新方法，并将其与他们的地图滚动方法相结合18。

这篇论文通过添加限制传播距离的可能性，将我们在19中提出的DMs扩展到3D，以保持在大的开放空间和户外的在线可行性，如Scherer等人所提出的20。

此外，我们描述了如何进一步减少访问的邻居单元格的数量，这提高了3D DMs的效率，我们还提供了额外的实验。

2.2.Voronoi图

传统的Voronoi算法计算分隔连续空间中表示的单个障碍点或线段的参数线或曲线。有一些方法可以更新这样的Voronoi图，例如，对于在探索过程中新发现的障碍21 22，移动输入点23，或已插入或删除的点24。然而，分析方法对于网格地图的使用并不实用，因为它们会在所有成对的占用单元格之间附加Voronoi线，即使对于较大的障碍物和墙壁也是如此。

存在几种逐步构建Voronoi图的方法，例如25 26。然而，它们中的大多数不适合动态环境或使用SLAM的增量映射，因为它们不支持清除先前占用的地图单元格，这是处理SLAM中由于闭环引起的动态环境和地图修正所必需的。

Tao等人提出了线拟合来解决SLAM应用中的这个问题27。他们的算法可以在探索过程中逐步构建分析Voronoi图，这包括障碍物的添加，但不包括它们的移除。此外，还必须更新线拟合，这可能会导致Voronoi图的突然变化。

Kalra等人提出的更新GVDs的方法直接操作网格地图，但引入了唯一分配给连接的障碍单元格的障碍标识符28。如果两个相邻的单元格根据他们的标识符有不同的最近障碍物，那么这两个单元格都会被添加到GVD。然而，这个条件生成了两个单元格宽的线，违反了GVD的稀疏性属性。此外，它不在凹形障碍物复合体的内部生成Voronoi线，如图2（左）所示。这破坏了GVD的连通性，对于路径规划是有问题的，特别是在室内环境中。此外，由于Kalra等人使用8连通步距离作为距离地图，Voronoi线也遵循这个度量，从而只是近似GVD。

在这篇论文中，我们描述了我们的基于条件的方法，用于增量更新GVDs，这是Lau等人首次提出的29。它考虑实际的欧几里得距离，并使用一个新的标准来确定一个单元格是否是GVD的一部分，而不需要像Kalra等人的方法那样需要障碍标识符30。此外，我们的方法正确处理了室内环境，并生成了可以用于计算拓扑不同路径的n-best的细Voronoi线，如图2所示。此外，它受益于我们上述的动态brushfire算法的加速。

图2

2.3.配置空间地图

配置空间（c-space）地图编码了给定的机器人姿态是否会与环境发生碰撞。对三维对象进行有效碰撞检查的算法仍然是一个活跃的研究领域。作为运动规划和计算机图形学之间的重叠区域，大多数方法用多边形网格表示环境和障碍物。例如，Tang等人最近提出了一个连接碰撞查询算法，该算法检测在给定状态之间移动的三角形网格的碰撞31。因此，它可以用于基于采样的路径规划。为了在线可行性，Pan和Manocha使用多核GPU进行碰撞查询32。然而，每次碰撞检查的成本取决于用于表示被测试对象的多边形的数量。

对于碰撞避免系统，Schlegel提出预先计算圆弧轨迹的碰撞距离作为相对障碍物位置和曲率的函数33。因此，运动学分析是离线完成的，碰撞距离可以通过每个障碍物的一次查找获得。相反，预先计算c-space表示进一步减少了在线碰撞检查的努力，只需要一次查找。自从Lozano-Perez发表了关于静态多面障碍物c-space规划的开创性论文以来34，许多方法被提出来减少计算c-space障碍物的成本，例如Wise和Bowyer的调查35。例如，Linan和Zhenmin提出了一种方法，用于增量增长多个机器人的多边形c-space障碍物，但没有考虑其他的变化，比如正在进行的探索36。由于这个问题的相关性，特别是在动态环境中，研究人员仍在努力提高效率37。

将机器人环境的网格地图与其足迹的图像进行卷积，可以得到一个离散的c-space地图。为了反映之前未知或移动障碍物的当前状态，这些地图需要定期更新。Kavraki提出使用快速傅里叶变换（FFT）来降低卷积的计算成本38，Therón等人添加了并行化以进一步加速39。后来，同一组提出了一种多分辨率方法，以减少大工作空间中的内存和计算负载40。为了加速自动驾驶汽车的路径规划，Ziegler和Stiller将车辆的形状分解为圆盘41。

作为变化环境的第一个动态方法，Wu等人提出预先计算每个可能被占用的单元格在操作器工作空间中的碰撞机器人姿态42：对给定的占用单元格集合的碰撞姿态取并集，可以得到c-space碰撞地图，无需进一步重新计算。然而，对于移动机器人，操作区域的大小可能使数据库存储或在线计算并集变得不可行。相反，我们更新c-space碰撞地图的方法是真正的增量式：它在离线阶段执行常规的地图卷积，并在在线应用中只更新受环境变化影响的单元格43。

对于使用圆形机器人的路径规划，2D Voronoi图是吸引人的路线图，因为它们用少量的单元格覆盖了地图中所有拓扑不同的路径。然而，对于矩形机器人，2D Voronoi规划失去了其完整性属性，这需要在Voronoi图导致碰撞的狭窄区域修复路径，例如，通过使用Foskey等人提出的快速探索随机树（RRTs）44。在这篇论文中，我们将动态距离地图和Voronoi图与我们新的可增量更新的c-space碰撞地图结合起来。这样，我们就可以克服上述问题，并在非圆形机器人的配置空间中进行完整的Voronoi规划。由于能够进行增量更新，所得到的系统适合在动态环境中在线应用。

虽然我们使用A*规划作为一个示例应用，但我们的方法可以与其他规划器结合，例如，D* Lite 45，或者带有Voronoi偏置采样的RRTs 46 47。

3. Brushfire算法

Verwer等人描述的Brushfire算法使用类似于Dijkstra的算法的最短路径搜索计算静态距离地图，该算法有多个源48。通过使用一个优先队列，该队列按照单元格到其最近障碍物的距离排序扩展，传播以障碍物的位置为起点在波前中扩散，如图3所示。

图3

\[ \begin{aligned} \hline \\ &\large{\textbf{算法1}：Brushfire算法}&\qquad\qquad\qquad\qquad\qquad \\ \hline \\ &\textbf{computeDistanceMap()} \\ &1:\quad\textbf{for all } s \textbf{ do} \\ &2:\quad\quad\textbf{if } M(s)=1 \textbf{ then} \\ &3:\quad\quad\quad D(s)\leftarrow 0 \\ &4:\quad\quad\quad obst(s)\leftarrow s \\ &5:\quad\quad\quad \text{insert}(open,s,0) \\ &6:\quad\quad\textbf{else } D(s) \leftarrow \infty \\ &7:\quad\textbf{while } open \neq ∅ \textbf{ do} \\ &8:\quad\quad s \leftarrow \text{pop}(open) \\ &9:\quad\quad \text{lower}(s) \\ &10:\quad \textbf{return }D \\ &\textbf{lower}(s) \\ &11:\quad\textbf{for all } n \in \text{Adj}_8(s) \textbf{ do} \\ &12:\quad\quad d \leftarrow \| obst(s)-n \| \\ &13:\quad\quad \textbf{if } d < D(n) \textbf{ then} \\ &14:\quad\quad\quad D(n) \leftarrow d \\ &15:\quad\quad\quad obst(n) \leftarrow obst(s) \\ &16:\quad\quad\quad \text{insert}(open,n,d) \\ \hline \\ \end{aligned} \]

只要这个队列包含单元格，算法就会反复调用函数pop(open)，它返回具有最低入队距离的单元格s，并从队列中移除它（第8行）。然后，它更新s的8连通邻域Adj8(s)中的单元格：如果邻居n到s的最近障碍物的距离d（由obst(s)指定）小于当前值D(n)（第13行），那么n的距离值和最近障碍物就会用s的障碍物更新（第14-15行）。此外，每个更新的邻居单元格n都会用其新的距离值插入到优先队列中，以继续传播（第16行）。因此，每个障碍物都会引发一个传播波前，该波前以圆形扩展，并更新访问过的单元格到障碍物的欧几里得距离的距离值。由于这个更新只能减少与单元格关联的距离值，我们称这种传播为“降低”波前。

如果一个波前不能降低访问过的单元格的任何邻居的距离，那么就不会再有更多的单元格入队。当所有的波前都以这种方式停止后，优先队列就为空了，算法返回距离地图。

优先队列的实现可以用 \(O(1)\) 的复杂性来添加和删除元素，如第4.2节所述。然后，Brushfire算法与简单的图像传递算法在同一复杂性类中，即对于 \(n\times n\) 的输入地图，复杂性为 \(O(n^2)\) 。

4.动态2D欧几里得距离地图

在二进制占用网格地图 \(M\) 中，物体的移动、插入或删除会导致单个单元格从自由（0）翻转到占用（1）状态，或者反过来。本节介绍了一种使用Brushfire方法的动态变体来更新欧几里得距离地图以反映这种变化的方法。在注册了一组新占用和新释放的单元格后，算法以增量方式执行更新，即，利用其先前的结果。如第2节所讨论的，我们的方法直接源自第3节中介绍的Brushfire算法，而不是Kalra等人的方法，后者源自D* Lite 49。

图4的例子显示了在移除一个障碍物并插入另一个障碍物后，如何更新图3中的DM。帧（A）显示了初始状态，这等同于图3的最终状态（D）。

图4

在执行更新时，新占用的单元格（蓝色轮廓）启动“降低”波前（B），这些波前类似于算法的静态变体，更新受影响单元格的最近障碍物距离。这些波前传播到一个不同的障碍物更近的地方（C）。此外，“提升”波前从新释放的单元格（红色轮廓）开始，并清除所有最近障碍物是被删除的那个的单元格的距离条目（B）。当它们在具有不同最近障碍物的单元格处停止时，它们启动新的“降低”波前，根据剩余的障碍物重新计算清除的单元格的距离（C）。

“提升”和“降低”波前都通过将处理过的单元格的相邻单元格入队到同一个优先队列来传播自己。由于队列按距离对其元素进行排序，因此“提升”和“降低”波前的处理是交织在一起的。在所有波前都停止后，队列为空，更新完成（D）。

4.1.动态brushfire算法解释

动态Brushfire算法的伪代码在算法2中给出。

\[ \begin{aligned} \hline \\ &\large{\textbf{算法2}：更新欧几里得距离地图的伪代码}&\qquad\qquad\qquad\qquad\qquad \\ \hline \\ &\textbf{setObstacle(s)} \\ &17:\quad obst(s) \leftarrow s \\ &18:\quad D(s) \leftarrow 0 \\ &19:\quad\text{insert}(open,s,0) \\ &\textbf{removeObstacle(s)} \\ &20:\quad\text{clearCell}(s) \\ &21:\quad toRaise(s) \leftarrow \text{true} \\ &22:\quad\text{insert}(open,s,0) \\ &\textbf{updateDistanceMap()} \\ &23:\quad\textbf{while } open \neq ∅ \textbf{ do} \\ &24:\quad\quad s \leftarrow \text{pop}(open) \\ &25:\quad\quad \textbf{if } toRaise(s) \textbf{ then} \\ &26:\quad\quad\quad \text{raise}(s) \\ &27:\quad\quad \textbf{else if } \text{isOcc}(obst(s)) \textbf{ then} \\ &28:\quad\quad\quad voro(s) \leftarrow \text{false} \\ &29:\quad\quad\quad \text{lower}(s) \\ &30:\quad \textbf{return }D \\ &\textbf{raise(s)} \\ &31:\quad\textbf{for all } n \in \text{Adj}_8(s) \textbf{ do} \\ &32:\quad\quad \textbf{if}(obst(n)\neq \text{cleared}∧¬ toRaise ( n )) \textbf{ then} \\ &33:\quad\quad\quad \text{insert}(open,n,D(n)) \\ &34:\quad\quad\quad \textbf{if } ¬ \text{isOcc} ( obst ( n )) \textbf{ then} \\ &35:\quad\quad\quad\quad \text{clearCell}(n) \\ &36:\quad\quad\quad\quad toRaise(n) \leftarrow \text{true} \\ &37:\quad toRaise(s) \leftarrow \text{false} \\ &\textbf{lower(s)} \\ &38:\quad\textbf{for all } n \in \text{Adj}_8(s) \textbf{ do} \\ &39:\quad\quad \textbf{if} ¬toRaise(n) \textbf{ then} \\ &40:\quad\quad\quad d \leftarrow \| obst(s)-n \| \\ &41:\quad\quad\quad \textbf{if } d < D(n) \textbf{ then} \\ &42:\quad\quad\quad\quad D(n) \leftarrow d \\ &43:\quad\quad\quad\quad obst(n) \leftarrow obst(s) \\ &44:\quad\quad\quad\quad \text{insert}(open,n,d) \\ &45:\quad\quad\quad \textbf{else }\text{checkVoro}(s,n) \\ \hline \\ \end{aligned} \]

该算法以给定的距离地图 \(D(s)\) 和对应的占用网格地图 \(M(s)\) 的障碍物参考地图 \(obst(s)\) 为初始值。如果根据 \(M(s)\) ，单元格 \(s\) 被占用，那么它的距离为 \(D(s)=0\) ，并将自己作为最近的障碍物位置，即， \(obst(s)=s\) 。否则， \(D(s)\) 是到最近占用单元格的距离值，其位置存储在 \(obst(s)\) 中。通过调用函数 \(\text{setObstacle}(s)\) 或 \(\text{removeObstacle}(s)\) 来注册单元格 \(s\) 的占用情况的变化，该函数更新s并将其插入到优先队列中。从而，函数 \(\text{clearCell}(s)\) 将 \(s\) 重置为 \(D(s)=\infty和obst(s)=\text{cleared}.^3\) 一个额外的标志 \(\text{toRaise}\) 用于确保在波前中的单元格的正确处理，特别是在提升和降低波前相遇的地方。它指示每个单元格是否需要用提升波前（true）或不需要（false）处理其邻居。

在使用 \(\text{setObstacle}(s)\) 和 \(\text{removeObstacle}(s)\) 注册一组更改后，优先队列open将填充更新的单元格。调用函数 \(\text{updateDistanceMap}()\) 通过将更改传播到所有受影响的单元格来执行更新。当优先队列不为空时，它会反复检索下一个未处理的单元格 \(s\) （第23-24行）。如果 \(s\) 仍然需要传播一个提升波前，那么将调用函数 \(\text{raise}(s)\) （第25-26行）。如果不是这种情况，并且如果 \(s\) 有一个有效的最近障碍物，那么将调用函数 \(\text{lower}(s)\) 来传播降低波前（第27-29行）。函数 \(\text{isOcc}(s)\) 通过检查 \(obst(s)=s\) 来测试单元格 \(s\) 是否被占用。

函数 \(\text{raise}(s)\) 处理 \(s\) 的8连通邻域 \(\text{Adj}_8(s)\) 中的每个单元格 \(n\) ，该单元格尚未被提升并且仍然引用最近的障碍物 \(obst(n)\) （第31-32行）。

单元格 \(n\) 以其旧的距离值插入到优先队列中（第33行）。如果 \(obst(n)\) 引用的单元格不再被占用，那么 \(n\) 被清除并标记为传播提升波前（第34-36行）。否则，提升波前在 \(n\) 处停止，保持n不变，并传播一个降低波前，如图4(C)所示。处理完邻居后， \(s\) 的提升更新完成，\(toRaise(s)\) 设置为false（第37行）。

函数 \(\text{lower}(s)\) 考虑s的8连通邻域 \(\text{Adj}_8(s)\) 中的每个单元格 \(n\) 。如果单元格n没有被标记为提升波前的一部分（第38-39行），那么它将按照算法的静态版本进行更新：将 \(n\) 到 \(s\) 的最近障碍物的欧几里得距离与 \(n\) 的当前最近障碍物距离进行比较（第40-41行）。如果它更小，那么n的距离和最近障碍物的值将被更新，以反映 \(obst(s)\) 现在也是 \(n\) 的最近障碍物。此外， \(n\) 被插入到优先队列中，以传播降低波前（第42-44行）。为了避免在提升波前与降低波前重叠的地方产生多余的提升波前，第41行的条件可以扩展为也覆盖引用已删除障碍物的等距离单元格。有了这个修改，这行读作

\[ \textbf{if }d\lt D(n) \vee (d=D(n) \wedge \neg \text{isOcc}(obst(n))) \textbf{ then} \]

第28行和第45行是在更新距离地图过程中实时增量更新Voronoi图的钩子（参见第5节）。如果只需要距离地图，那么可以省略这些行。

4.2.实现细节

上述距离地图算法计算并比较存储在 \(D(s)\) 中的实值欧几里得距离。如Scherer等人50和其他人之前所做的，我们在实践中使用整数平方距离，这节省了平方根的计算费用。由于平方根函数对于正输入的严格单调性，这并不改变算法的行为。

我们的算法中的一个核心数据结构是排序的优先队列open。这样的队列通常使用二叉树上的搜索来实现，这使得插入操作的复杂度为 \(O(log n)\) ，其中 \(n\) 是入队元素的数量。由于单元格的处理是按距离排序的，并且单元格只将其直接邻居入队，所以优先队列中的许多元素具有相同的距离值。我们通过使用Cuisenaire和Macq51提出的分桶技术来实现队列，从而利用了这一点。它将具有相同距离的单元格汇集在未排序的列表中，并跟踪下一个非空容器。从而将插入成本从 \(O(log n)\) 降低到 \(O(1)\) 。

为了实现具有唯一条目和可增加优先级的优先队列，我们实际上在元素更新时就插入它们，并为每个单元格 \(s\) 携带一个Boolean标志 \(toProcess\) 。它由 \(\text{insert}(open, s, d)\) 设置为true，并由 \(\text{pop}(open)\) 恢复为false。函数 \(\text{pop}(open)\) 迭代地出队元素，直到它到达一个满足 \(toProcess(s)=\text{true}\) 的 \(s\) ，从而丢弃重复的条目。

4.3.扩展到更高维度

5.动态2D Voronoi图

在连续空间中，如果一个点到其最近的两个障碍物的距离相同，那么它就是Voronoi图的一部分。对于离散的GVDs，这个条件不能直接应用于离散化的单元格坐标。相反，GVD是包含其关联区域中的连续Voronoi线的单元格的集合。此外，将占用单元格隐式地分组为障碍物起着重要的作用：将每个占用单元格视为一个单独的障碍物会导致GVD变得混乱，因为每对相邻的占用单元格之间都会插入一条线。相反，将所有连接的占用单元格视为一个单独的障碍物会导致在室内环境中缺少Voronoi线，如图2（左）所示。

在连续空间中，Voronoi图由无限薄的线和曲线组成。由于GVDs是在离散化的网格上表示的，因此可能会出现错误连接的形式的人为错误。首先，通过相邻单元格的一对附近的Voronoi线成为连接，从而在图中创建错误的圆和互连。其次，位于连续空间中两个离散单元格位置之间的单一Voronoi线在GVD中造成双线。在这两种情况下，GVD都失去了Voronoi图的稀疏性属性，即，GVD中的路径不再对应于与障碍物有关的拓扑不同的路线。当使用8连通的网格模型时，通过视觉检查，GVD看起来更薄。然而，额外的连接通常在相邻的单元格中创建额外的路径变化。因此，8连通的GVDs经常违反稀疏性条件，而4连通的则不会（参见图5的一个例子）。

图5

我们提出了一组条件，用于生成完全连接的GVDs，同时没有相邻的Voronoi线触及彼此，如图2（右）所示。尽管我们关注的是4连通的GVDs，但我们的方法也可以生成8连通的。一个额外的修剪步骤处理了由于离散化而产生的人为错误，即双线和错误的连接，从而确保了GVD的稀疏性。该算法直接与我们更新距离地图的方法集成，易于实现。

我们通过一个二进制地图 \(voro(s)\) 来表示GVD，它为每个单元格 \(s\) 指定它是否是GVD的一部分（ \(voro(s)=\text{true}\) ）或者不是（ \(voro(s)=\text{false}\) ）。GVD的更新直接与第4节中算法2给出的DMs的更新集成：较低的波前从GVD中移除所有访问过的单元格（第28行），并可能添加它们停止的单元格。如果由单元格 \(s\) 传播的较低波前找到一个相邻的单元格 \(n\) ，其距离不能通过采用 \(obst(s)\) 作为最近的障碍物来降低，那么这两个单元格都是GVD的候选者（第45行），并且可能在根据算法4检查我们的额外条件 \(\text{checkVoro}(s, n)\) 后被添加。这个函数测试候选邻居单元格 \(s\) 和 \(n\) 中至少有一个不相邻于其最近的障碍物（第50行）。此外，邻居 \(n\) 必须有一个有效的最近障碍物，该障碍物与 \(s\) 的最近障碍物不同且不相邻。如果满足这些条件，一个Voronoi线将通过这些单元格的中心，距离下一个单元格有足够的距离，这两个单元格都是离散GVD的候选者。

为了避免双线，该函数只添加单元格 \(c∈\{s, n\}\) ，它在较小程度上违反了连续的Voronoi条件，即，当从自己最近的障碍物切换到竞争邻居的障碍物时，距离增加最小的那个。如果两者的增加相同，那么两个单元格都会被插入（第51-52行）。为了获得8连通的GVDs，这些行中的“≤”被替换为对角邻居 \(s\) 和 \(n\) 的“<”，因为在增加相等的情况下，不需要插入任何单元格。

5.1.裁剪

如前所述，Voronoi图上的不同路径对应于环境中拓扑上不同的路线。为了在网格地图上的GVDs保持这个属性，我们希望Voronoi线条尽可能地细，即只有一个像素宽。然而，Kalra等人之前关于动态GVDs的工作经常生成两个或三个像素宽的Voronoi线。我们的可选修剪步骤会侵蚀那些在连续的Voronoi线会恰好穿过两个单元格的地方出现的2像素宽的Voronoi线。因此，所有新的Voronoi单元格都被插入到一个优先队列中，并由修剪阶段进行处理。

图6中显示的图像操作符模式在中心单元格 \(s\) 为其一个或多个相邻单元格提供连通性时匹配。左侧显示了确保4连通性所需的两种模式，右侧的三种模式对应于8连通性。在任何模式中，“1”匹配 \(voro(s) = \text{true}\) ，而“0”代表 \(voro(s) = \text{false}\) ，空字段被忽略。如箭头所示，同一模式应用于所有独特的90°旋转。

图6

在第一阶段，修剪算法会合并那些由于地图分辨率有限而错误连接的Voronoi线。这是通过使用匹配模式 \(P_3^8\) 来完成的，该模式可以检测到被Voronoi单元格包围的单元格。如果这样的单元格是空闲的并且不是GVD的一部分，那么它在这个时候就会被添加进来。这样可以合并那些由于地图分辨率限制而无法分开的Voronoi线。与接下来的修剪步骤一起，这可以防止错误的连接，并确保生成的GVD是稀疏的。

第二阶段实现了实际的修剪步骤。按照距离的递增顺序，依次从优先队列中弹出排队的单元格。如果这样的单元格在GVD上有多于一个的邻居，并且不需要保持GVD的连通性，那么它可以从GVD中移除，而不影响其拓扑结构。同样，这是通过使用图像操作符模式来测试的：如果在单元格位置没有任何连通性模式匹配，那么该单元格在GVD中就不是必需的。

5.2.Voronoi图上的路径规划

如前所述，GVD是一种对路径规划有吸引力的单元分解方法。这一部分详细介绍了在动态环境中Voronoi规划的重要方面。通常，规划问题的起始和目标位置并不是GVD的一部分。直接的方法是在这两个位置寻找最近的Voronoi单元格，并用直线将它们连接到图52。这在实践中是有问题的，因为起始姿态的微小变化可以显著改变计划的路径，如图7（顶行）所示。相反，开始一个目标驱动的搜索可以轻易地扩展出大部分不在GVD上的空间。

图7

我们的方法由算法5给出，可以克服这些问题。首先，我们在起始和目标位置插入虚拟障碍物（行54-55）。更新距离图和GVD后（使用我们的增量算法，行56），这些位置会被形成“泡泡”状区域的Voronoi线包围，如图7（底行）所示。通过简单的brushfire扩展，我们标记所有在泡泡内到包围的Voronoi线的单元格（行57-58）。现在我们可以开始一个目标导向的搜索，该搜索仅限于标记的单元格或属于GVD的单元格。这样，搜索从起点扩展到Voronoi图，沿着Voronoi线，然后在到达目标泡泡时连接到目标。由于整个路径都是目标导向的图搜索的结果，因此为移动机器人规划的连续路径非常相似，不会突然改变（见图7）。计算出最短路径后，我们通过移除虚拟障碍物并进行另一次更新来撤销对GVD的更改（行63-65）。

\[ \begin{aligned} \hline \\ &\large{\textbf{算法5}：\text{“泡泡”技术用于在GVD上进行路径规划}} \\ \hline \\ &\textbf{planPath}(start, goal) \\ &53:\quad \textbf{if } M(start) = 1 \vee M(goal) = 1 \textbf{ then return} \\ &54:\quad \text{setObstacle}(start) &\text{// 创建Voronoi泡泡} \\ &55:\quad \text{setObstacle}(goal) \\ &56:\quad \text{updateDistanceMap()} \\ &57:\quad \text{brushfireMark}(start) &\text{// 标记泡泡可搜索} \\ &58:\quad \text{brushfireMark}(goal) \\ &59:\quad \text{push}(astarqueue, goal) \\ &60:\quad \textbf{while } astarqueue \neq ∅ \textbf{ do} &\text{// 在Voronoi和泡泡的}A^*\text{搜索} \\ &61:\quad\quad s \leftarrow \text{pop}(astarqueue) \\ &62:\quad\quad \textbf{if } s = goal \textbf{ then} \\ &63:\quad\quad\quad \text{removeObstacle}(start) \\ &64:\quad\quad\quad \text{removeObstacle}(goal) \\ &65:\quad\quad\quad \text{updateDistanceMap()} \\ &66:\quad\quad\quad \textbf{return } path\ from\ start\ to \ goal \\ &67:\quad\quad \textbf{for all }n\in \text{Adj}_4(s) \textbf{do} \\ &68:\quad\quad\quad \textbf{if }voro(n)=\text{true} \vee marked(n)=\text{true} \textbf{ then} \\ &69:\quad\quad\quad\quad A^*\ update\ for\ costs\ and \ heuristic\ of\ n \\ &70:\quad\quad\quad\quad \text{push}(astarqueue, n) \\ &\textbf{brushfireMark}(s) \\ &71:\quad\text{push}(unsortedqueue,s) \\ &72:\quad\textbf{while } unsortedqueue \neq ∅ \textbf{ do} \\ &73:\quad\quad s \leftarrow \text{pop}(unsortedqueue) \\ &74:\quad\quad marked(s) \leftarrow \text{true} \\ &75:\quad\quad \textbf{for all }n\in \text{Adj}_4(s) \textbf{do} \\ &76:\quad\quad\quad \textbf{if }M(n)=1\textbf{ then continue} \\ &77:\quad\quad\quad \textbf{if }voro(n)=\text{false}\wedge marked(n)=\text{false} \textbf{ then} \\ &78:\quad\quad\quad\quad \text{push}(unsortedqueue,n) \\ \hline \\ \end{aligned} \]

为了使算法真正增量化，应该将标记存储在哈希映射结构中，而不是在每个帧之后都要清除的二进制网格中。此外，可以使用像D*这样的增量重规划算法，而不是A*。

6.动态c-space碰撞地图

如2.3节所讨论的，配置空间（c-space）网格图为机器人的每个离散配置编码，表示它是否会与环境中的障碍物发生碰撞。在平面上移动的非圆形机器人有一个三维的c-space，因为它们的姿态 \(⟨x，y，θ⟩\) 由它们在地面上的2D位置和它们的方向 \(θ\) 给出。

计算c-space地图通常需要对每个方向的地图和机器人的形状进行卷积。重新计算这些卷积以反映动态环境中的变化，通常在需要在线应用的帧率下是不可行的。本节介绍了一种有效更新c-space碰撞地图的方法，该方法使用图1中显示的障碍物模型。为了清晰起见，我们首先描述我们的算法对于具有悬挂障碍物（右下角）的2.5D表示，然后讨论对其他障碍物模型的适应。

资料

  1. http://www2.informatik.uni-freiburg.de/~lau/dynamicvoronoi/

  1. B. Lau, C. Sprunk, W. Burgard, Improved updating of Euclidean distance maps and Voronoi diagrams, in: IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS, Taipei, Taiwan, 2010, pp. 281–286.↩︎

  2. B. Lau, C. Sprunk, W. Burgard, Incremental updates of configuration space representations for non-circular mobile robots with 2D, 2.5D, or 3D obstacle models, in: European Conference on Mobile Robots, ECMR, Örebro, Sweden, 2011, pp. 49–54.↩︎

  3. H. Choset, J. Burdick, Sensor-based exploration: the hierarchical generalizedVoronoigraph,InternationalJournalofRoboticResearch(IJRR)19(2)(2000)96–125.↩︎

  4. F. Aurenhammer, Voronoi diagrams—a survey of a fundamental geometricdatastructure,ACMComputingSurveys(CSUR)23(3)(1991)345–405.↩︎

  5. C. Mandel, U. Frese, Comparison of wheelchair user interfaces for the paralysed: head-joystick vs. verbal path selection from an offered route-set, in: European Conference on Mobile Robots, ECMR, 2007.↩︎

  6. S. Thrun, A probabilistic on-line mapping algorithm for teams of mobile robots, International Journal of Robotic Research (IJRR) 20 (5) (2001) 335–363.↩︎

  7. G. Borgefors, Distance transformations in digital images, Computer Vision, Graphics, and Image Processing 34 (3) (1986) 344–371.↩︎

  8. R. Fabbri, L. da Fontoura Costa, J.C. Torelli, O.M. Bruno, 2D Euclidean distance transform algorithms: a comparative survey, ACM Computing Surveys 40 (1) (2008) 1–44.↩︎

  9. B.J.H. Verwer, P.W. Verbeek, S.T. Dekker, An efficient uniform cost algorithm applied to distance transforms, IEEE Transactions on Pattern Analysis and Machine Intelligence 11 (4) (1989) 425–429.↩︎

  10. R. Fabbri, L. da Fontoura Costa, J.C. Torelli, O.M. Bruno, 2D Euclidean distance transform algorithms: a comparative survey, ACM Computing Surveys 40 (1) (2008) 1–44.↩︎

  11. N. Kalra, D. Ferguson, A. Stentz, Incremental reconstruction of generalized Voronoi diagrams on grids, Robotics and Autonomous Systems (RAS) 57 (2) (2009) 123–128.↩︎

  12. A. Stentz, Optimal and efficient path planning for partially-known environments, in: IEEE International Conference on Robotics and Automation, ICRA, San Diego, CA, USA, 2004, pp. 3310–3317.↩︎

  13. N. Kalra, D. Ferguson, A. Stentz, Incremental reconstruction of generalized Voronoi diagrams on grids, Robotics and Autonomous Systems (RAS) 57 (2) (2009) 123–128.↩︎

  14. P.-E. Danielsson, Euclidean distance mapping, Computer Graphics and Image Processing 14 (3) (1980) 227–248.↩︎

  15. S. Scherer, D. Ferguson, S. Singh, Efficient c-space and cost function updates in 3D for unmanned aerial vehicles, in: IEEE International Conference on Robotics and Automation, ICRA, Kobe, Japan, 2009, pp. 3860–3865.↩︎

  16. P.-E. Danielsson, Euclidean distance mapping, Computer Graphics and Image Processing 14 (3) (1980) 227–248.↩︎

  17. O. Cuisenaire, B. Macq, Fast Euclidean distance transformation by propagation using multiple neighborhoods, Computer Vision and Image Understanding 76 (2) (1999) 163–172.↩︎

  18. S. Scherer, J. Rehder, S. Achar, H. Cover, A. Chambers, S. Nuske, S. Singh, River mapping from a flying robot: state estimation, river detection, and obstacle mapping, Autonomous Robots 33 (2012) 189–214.↩︎

  19. B. Lau, C. Sprunk, W. Burgard, Improved updating of Euclidean distance maps and Voronoi diagrams, in: IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS, Taipei, Taiwan, 2010, pp. 281–286.↩︎

  20. S. Scherer, D. Ferguson, S. Singh, Efficient c-space and cost function updates in 3D for unmanned aerial vehicles, in: IEEE International Conference on Robotics and Automation, ICRA, Kobe, Japan, 2009, pp. 3860–3865.↩︎

  21. T. Tao, S. Tully, G. Kantor, H. Choset, Incremental construction of the saturated-GVG for multi-hypothesis topological SLAM, in: IEEE International Conference on Robotics and Automation, ICRA, 2011, pp. 3072–3077.↩︎

  22. N. Rao, N. Stoltzfus, S. Iyengar, A ‘retraction’ method for learned navigation in unknown terrains for a circular robot, IEEE Transactions on Robotics and Automation 7 (5) (1991) 699–707.↩︎

  23. C.M. Gold, P.R. Remmele, T. Roos, Voronoi methods in GIS, in: Algorithmic Foundations of Geographic Information Systems, Vol. 1340, Springer, Berlin, Heidelberg, 1997, pp. 21–35.↩︎

  24. I. Lee, M. Gahegan, Interactive analysis using Voronoi diagrams: algorithms to support dynamic update from a generic triangle-based data structure, Transactions in GIS 6 (2) (2002) 89–114.↩︎

  25. L.J. Guibas, D.E. Knuth, M. Sharir, Randomized incremental construction of Delaunay and Voronoi diagrams, Algorithmica 7 (1992) 381–413.↩︎

  26. H. Choset, S. Walker, K. Eiamsa-Ard, J. Burdick, Sensor-based exploration: incremental construction of the hierarchical generalized Voronoi graph, The International Journal of Robotics Research 19 (2000) 126–148.↩︎

  27. T. Tao, S. Tully, G. Kantor, H. Choset, Incremental construction of the saturated-GVG for multi-hypothesis topological SLAM, in: IEEE International Conference on Robotics and Automation, ICRA, 2011, pp. 3072–3077.↩︎

  28. N. Kalra, D. Ferguson, A. Stentz, Incremental reconstruction of generalized Voronoi diagrams on grids, Robotics and Autonomous Systems (RAS) 57 (2) (2009) 123–128.↩︎

  29. B. Lau, C. Sprunk, W. Burgard, Improved updating of Euclidean distance maps and Voronoi diagrams, in: IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS, Taipei, Taiwan, 2010, pp. 281–286.↩︎

  30. N. Kalra, D. Ferguson, A. Stentz, Incremental reconstruction of generalized Voronoi diagrams on grids, Robotics and Autonomous Systems (RAS) 57 (2) (2009) 123–128.↩︎

  31. M. Tang, Y.J. Kim, D. Manocha, CCQ: efficient local planning using connection collision query, in: Algorithmic Foundations of Robotics IX, in: Springer Tracts in Advanced Robotics, STAR, vol. 68, Springer, Berlin, Heidelberg, 2011, pp. 229–247.↩︎

  32. J. Pan, D. Manocha, GPU-based parallel collision detection for real-time motion planning, in: Algorithmic Foundations of Robotics IX, in: Springer Tracts in Advanced Robotics, STAR, vol. 68, Springer, Berlin, Heidelberg, 2011, pp. 211–228.↩︎

  33. C. Schlegel, Fast local obstacle avoidance under kinematic and dynamic constraints for a mobile robot, in: IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS, Victoria, Canada, 1998, pp. 594–599.↩︎

  34. T. Lozano-Perez, Spatial planning: a configuration space approach, IEEE Transactions on Computers C-32 (2) (1983) 108–120.↩︎

  35. K.D. Wise, A. Bowyer, A survey of global configuration-space mapping techniques for a single robot in a static environment, International Journal of Robotic Research (IJRR) 19 (8) (2000) 762–779.↩︎

  36. J. Linan, T. Zhenmin, Building configuration space for multiple UGVs, in: IEEE International Conference on Vehicular Electronics and Safety, 2005, pp. 245–250.↩︎

  37. E. Behar, J.-M. Lien, A new method for mapping the configuration space obstacles of polygons, Tech. Rep. GMU-CS-TR-2011-11, Department of Computer Science, George Mason University, 2010.↩︎

  38. L.E. Kavraki, Computation of configuration-space obstacles using the fast Fourier transform, IEEE Transactions on Robotics and Automation 11 (3) (1995) 408–413.↩︎

  39. R. Therón, V. Moreno, B. Curto, F.J. Blanco, Configuration space of 3D mobile robots: parallel processing, in: 11th International Conference on Advanced Robotics, vols. 1–3, 2003, pp. 210–215.↩︎

  40. F.J. Blanco, V. Moreno, B. Curto, R. Therón, c-space evaluation for mobile robots at large workspaces, in: IEEE International Conference on Robotics and Automation, ICRA, Barcelona, Spain, 2005, pp. 3434–3439.↩︎

  41. J. Ziegler, C. Stiller, Fast collision checking for intelligent vehicle motion planning, in: IEEE Intelligent Vehicles Symposium, San Diego, CA, USA, 2010, pp. 518–522.↩︎

  42. X.J. Wu, J. Tang, K.H. Heng, On the construction of discretized configuration space of manipulators, Robotica 25 (1) (2007) 1–11.↩︎

  43. B. Lau, C. Sprunk, W. Burgard, Incremental updates of configuration space representations for non-circular mobile robots with 2D, 2.5D, or 3D obstacle models, in: European Conference on Mobile Robots, ECMR, Örebro, Sweden, 2011, pp. 49–54.↩︎

  44. M. Foskey, M. Garber, M.C. Lin, D. Manocha, A Voronoi-based hybrid motion planner, in: IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS, Maui, HI, USA, 2001, pp. 55–60.↩︎

  45. S. Koenig, M. Likhachev, D ∗ lite, in: Eighteenth National Conference on Artificial Intelligence, AAAI, 2002, pp. 476–483.↩︎

  46. S. Lindemann, S. LaValle, Incrementally reducing dispersion by increasing Voronoi bias in RRTs, in: IEEE International Conference on Robotics and Automation, ICRA, 2004, pp. 3251–3257.↩︎

  47. L. Zhang, D. Manocha, An efficient retraction-based RRT planner, in: IEEE International Conference on Robotics and Automation, ICRA, Pasadena, 2008, pp. 3743–3750.↩︎

  48. B.J.H. Verwer, P.W. Verbeek, S.T. Dekker, An efficient uniform cost algorithm applied to distance transforms, IEEE Transactions on Pattern Analysis and Machine Intelligence 11 (4) (1989) 425–429.↩︎

  49. N. Kalra, D. Ferguson, A. Stentz, Incremental reconstruction of generalized Voronoi diagrams on grids, Robotics and Autonomous Systems (RAS) 57 (2) (2009) 123–128.↩︎

  50. S. Scherer, D. Ferguson, S. Singh, Efficient c-space and cost function updates in 3D for unmanned aerial vehicles, in: IEEE International Conference on Robotics and Automation, ICRA, Kobe, Japan, 2009, pp. 3860–3865.↩︎

  51. O. Cuisenaire, B. Macq, Fast Euclidean distance transformation by propagation using multiple neighborhoods, Computer Vision and Image Understanding 76 (2) (1999) 163–172.↩︎

  52. R. Geraerts, M. Overmars, A comparative study of probabilistic roadmap planners, in: Algorithmic Foundations of Robotics V, in: Springer Tracts in Advanced Robotics (STAR), vol. 7, Springer, Berlin, Heidelberg, 2004, pp. 43–58.↩︎