圆形障碍物路径寻找

# 圆形障碍物寻路 源代码：https://redblobgames.github.io/circular-obstacle-pathfinding/ 在 GitHub 上查看（https://github.com/redblobgames/circular-obstacle-pathfinding） ### 绕圆形障碍物寻路 2017年3月 ## 穿越森林 A* 寻路算法是一种快速生成最优路径的强大方法。通常人们用网格地图来演示 A*，但 A* 不仅仅适用于网格算法！它可以适用于任何图。我们可以用 A* 在这个圆形障碍物的世界中找到一条路径。同一个算法如何解决这两个问题？我们先回顾一下 A* 的工作原理。 ## A* 算法 A* 算法从起点到终点找到*最优路径*，沿途避开障碍物。它通过逐步扩展一组*部分路径*来实现。每条部分路径是从起点到前往目标途中某个中间点的一系列步骤。随着 A* 的推进，部分路径越来越接近目标点。算法一旦找到一条完整的路径，并且能证明它比任何剩余的可能路径更优，就终止。 在算法的每一步，A* 会评估这组部分路径，并通过扩展集合中最有希望的路径来生成一些新路径。为此，A* 将部分路径保存在一个优先级队列中，按*估计长度*排序——路径到目前为止的实际测量长度，加上到目标剩余距离的猜测。这个猜测必须是*低估*；也就是说，猜测可以小于实际距离，但不能大于。在大多数寻路问题中，一个好的低估是从部分路径末端到目标的几何直线距离。从部分路径末端到目标的最优路径可能比这个直线距离长，但不可能更短。 当 A* 开始时，优先级队列中只有一条部分路径：起点。算法通过反复从优先级队列中移除最有希望的路径（即估计长度最小的路径）来工作。如果这条路径的末端是目标点，则算法完成——优先级队列确保没有其他路径可能更优。否则，从从队列中移除的部分路径的末端开始，A* 通过向所有可能方向迈出一步来生成一些新路径。它将这些新路径放回优先级队列，并重新开始这个过程。 ## 图 A* 在*图*上工作：一个由*边*连接的*节点*集合。在基于网格的世界中，每个节点代表一个网格位置，每条边代表连接到北、南、东、西的相邻位置。在 A* 运行于圆形障碍物森林之前，我们需要将其转换为图。 穿过森林的所有路径都由交替的直线段和弧段组成。这些就是路径图中的边。这些边的端点成为节点。通过图的一条路径是一系列由边连接的节点：线段和弧段都作为图中的边。我们把线段称为*滑行边*，因为路径利用它们在障碍物之间滑行；把弧段称为*贴边*，因为它们在路径中的作用是贴合障碍物的侧面。 接下来我们将探索一种简单的方法，将障碍物森林转化为图：生成所有可能的滑行边和贴边。这被称为*切线可见性图*。 ### 生成滑行边 一对圆形之间的滑行边是刚好与两个圆相切的线段；这些线段被称为*公切线*，每对圆有四个。在圆之间交叉的公切线是*内公切线*，而沿着外部的是*外公切线*。 #### 内公切线 历史上，内公切线对于计算跨过两个不同尺寸滑轮的皮带长度很重要，因此构造内公切线的问题被称为*皮带问题*。要找到内公切线，计算下图中的角度 \\(\\theta\\)。 重叠的圆没有公切线 结果表明，对于圆心在点 \\(A\\) 和 \\(B\\)、半径分别为 \\(r_A\\) 和 \\(r_B\\)、圆心距离为 \\(d\\) 的圆： \\[\\theta = \\arccos\\{\\{r_A+r_B\\}\\over{d}\\}\\] 一旦知道 \\(\\theta\\)，就很容易找到点 \\(C\\)、\\(D\\)、\\(E\\) 和 \\(F\\)。 #### 外公切线 构造外公切线——*滑轮问题*——使用类似的技术。 小圆完全被大圆包含 对于外公切线，我们可以这样求 \\(\\theta\\)： \\[\\theta = \\arccos\\{\\{\\lvert r_A - r_B \\rvert\\} \\over d\\}\\] 圆 A 和 B 谁大并不重要，但如图所示，\\(\\theta\\) 出现在 A 朝向 B 的一侧，而在 B 远离 A 的一侧。 #### 视线 综合起来，两个圆之间的内公切线和外公切线构成了它们之间的滑行边。但如果第三个圆阻挡了一条或多条滑行边呢？ 圆阻挡视线 A 和 B 彼此可见 如果滑行边被另一个圆阻挡，我们需要丢弃这条边。为了检测这种情况，我们使用简单的*点到直线距离*计算。如果从滑行边到障碍物中心的距离小于障碍物的半径，那么障碍物就阻挡了滑行边，我们应该丢弃这条边。 要计算点 \\(C\\) 到线段 \\(\\overline{AB}\\) 的距离，使用以下方法（http://paulbourke.net/geometry/pointlineplane/）：首先计算 \\(u\\)，即沿着线段 \\(\\overline{AB}\\) 的垂直升交点到达点 \\(C\\) 的距离比例： \\[ u = \\frac{(C - A) \\cdot (B-A)}{(B-A)\\cdot(B-A)} \\] 然后在 \\(\\overline{AB}\\) 上计算位置 \\(E\\)： \\[ E = A + \\mathrm{clamp}(u, 0, 1) \\times (B - A) \\] 01u={{Math.round(100*(C.x-A.x)/(B.x-A.x))/100}}dr 从 \\(C\\) 到线段 \\(\\overline{AB}\\) 的距离 \\(d\\) 就是从 \\(C\\) 到 \\(E\\) 的距离： \\[d = \\|E - C\\|\\] 由于 \\(d < r\\)，圆阻挡了从 \\(A\\) 到 \\(B\\) 的视线，这条边应被丢弃。 \\(d \\ge r\\)，从 \\(A\\) 到 \\(B\\) 有视线，这条边应保留。 试着移动圆来看看 \\(d \\ge r\\) 和 \\(d < r\\) 的情况。 ### 生成贴边 图中的节点将滑行边连接到贴边。我们在前面小节中生成了滑行边。要生成贴边，我们从滑行边的端点开始，绕圆行进，并在另一条滑行边的端点处终止。 要找到某个圆的一组贴边，首先找到所有与该圆接触的滑行边。然后，在圆上所有滑行边端点之间创建贴边。 ## 整合在一起 给定滑行边、贴边和节点的生成，以及被阻挡的滑行边的剔除，我们可以构建一个图，并使用 A* 算法进行寻路。 ## 改进 我们讨论的图生成过程对于解释算法已经足够，但有许多地方可以改进。这些改进使算法使用更少的 CPU 和内存，并允许处理更多情况。让我们看几个。 ### 接触的障碍物 也许你注意到了——到目前为止给出的示例中，没有圆形障碍物重叠甚至接触。允许圆接触会使寻路问题稍微难一点，但并不多。 #### 公切线 回想一下，内公切线可以用这个公式找到： \\[\\theta = \\arccos\\{\\{r_A+r_B\\}\\over{d}\\}\\] 外公切线则用这个： \\[\\theta = \\arccos\\{\\{\\lvert r_A - r_B \\rvert\\} \\over d\\}\\] 当两个圆接触或重叠时，它们之间没有内公切线。此时 \\(\\{r_A+r_B\\}\\over d\\) 大于 1。由于反余弦函数在其定义域 \\([-1, 1]\\) 之外无定义，因此在执行反余弦之前检查圆是否重叠非常重要。同样，如果一个圆完全包含另一个圆，那么它们之间没有外公切线。此时 \\(\\{r_A - r_B\\} \\over d\\) 超出范围 \\([-1, 1]\\)，没有反余弦。 圆不重叠：同时有外公切线和内公切线 圆重叠：只有外公切线 小圆被大圆包含：没有公切线 #### 滑行边视线 当允许障碍物接触或重叠时，计算滑行边视线会出现新情况。回顾一下计算 \\(u\\)（https://redblobgames.github.io/circular-obstacle-pathfinding/#surfing-line-of-sight），即沿着滑行边的距离比例，在该点垂直于边的线到达该点。当不允许圆接触时，\\(u\\) 的值超出 \\([0,1]\\) 意味着该圆无法接触边，因为要接触就必须碰到边的某个端点。这是不可能的，因为边的端点已经与其他圆相切（因而接触）。然而，如果允许圆重叠，那么超出 \\([0,1]\\) 的 \\(u\\) 值可能会阻挡沿着边的视线。这对应于圆位于滑行边末端之外，但覆盖或接触了一个端点的情况。为了从数学上捕捉这种情况，我们将 \\(u\\) *钳制*到范围 \\([0,1]\\)： \\[E = A + clamp(u, 0, 1) \\times (B - A)\\] #### 贴边视线 当允许障碍物接触或重叠时，贴边可能像滑行边一样被障碍物阻挡。考虑下图中贴边的情况。如果另一个障碍物接触到贴边，则它被阻挡，应丢弃。 贴边被阻挡 贴边有效 要确定贴边是否被另一个障碍物阻挡，使用以下方法（http://paulbourke.net/geometry/circlesphere/）来确定两个圆相交的点。对于圆心在点 \\(A\\) 和 \\(B\\)、半径分别为 \\(r_A\\) 和 \\(r_B\\) 的圆，其中 \\(d\\) 是 \\(A\\) 和 \\(B\\) 之间的距离，首先需要检查几种情况。如果圆没有接触（即 \\(d > r_A + r_B\\)），或者一个圆在另一个圆内部（\\(d < |r_A - r_B|\\)），或者圆心重合（\\(d = 0\\) 且 \\(r_A = r_B\\)），那么这些圆不会干扰彼此的贴边。如果这些情况都不成立，则两个圆相交于两点——如果圆相切，则这两点重合。考虑连接这两个交点的*根轴*；它垂直于连接 \\(A\\) 和 \\(B\\) 的线，交于某点 \\(C\\)。我们可以计算从 \\(A\\) 到 \\(C\\) 的距离 \\(a\\) 如下： \\[a = \\frac{r_A^2 - r_B^2 + d^2}{2d}\\] 找到 \\(a\\) 后，可以求出角度 \\(\\theta\\)： \\[\\theta = \\arccos \\frac{a}{r_A}\\] 如果 \\(\\theta\\) 为零，则圆在 \\(C\\) 点相切。否则有两个交点，对应正负 \\(\\theta\\)。 大圆包含小圆 圆不重叠 接下来，确定这两个交点是否落在贴边的起点和终点之间。如果是，则障碍物阻挡了贴边，我们应丢弃这条边。注意，我们不必担心贴边完全被障碍物包含的情况，因为滑行边的视线剔除已经会丢弃那条边。 在修改了公切线计算以及滑行边和贴边的视线检查之后，其他一切都能正常工作。 ### 可变角色半径（闵可夫斯基扩展） 当中空的圆形物体在圆形障碍物世界中移动时，我们可以做一些简化问题的观察。首先，通过注意到将一个半径为 \\(r\\) 的圆移动穿过圆形障碍物森林，等价于将一个点穿过同一森林，但每个障碍物的半径增加了 \\(r\\)，这会使问题更简单。这是*闵可夫斯基加法*的一个极其简单的应用。如果角色半径大于零，我们只需在开始前增大障碍物的大小。 ### 延迟边生成 一般来说，对于包含 \\(n\\) 个障碍物的森林，图中有 \\(O(n^2)\\) 条滑行边，但由于每条边都必须检查与 \\(n\\) 个障碍物的视线，因此生成图的总时间为 \\(O(n^3)\\)。此外，滑行边对可以引出贴边，并且每条贴边也必须针对每个障碍物检查视线。然而，由于 A* 算法非常高效，通常只需查看这个大图的一小部分就能生成最优路径。我们可以通过在 A* 算法执行过程中动态生成图的小部分，而不是预先完成所有工作，来节省时间。如果 A* 很快找到路径，我们就只生成图的一小部分。 我们通过将边生成移到 `neighbors()` 函数中来实现这一点。有几种情况。在算法开始时，我们需要起点的邻居。这些是从起点到每个障碍物的左右边缘的滑行边。下一种情况是 A* 刚刚沿着滑行边到达障碍物 \\(C\\) 边缘上的点 \\(p\\)；`neighbors()` 需要返回从 \\(p\\) 出发的贴边。为此，计算 \\(C\\) 与每个其他障碍物之间的公切线，丢弃任何没有视线的公切线，从而确定离开该障碍物的滑行边。然后找到所有将 \\(p\\) 连接到这些滑行边的贴边，丢弃被其他障碍物阻挡的贴边。返回所有这些贴边，并将滑行边保存起来，以便在后续的 `neighbors()` 调用中返回。最后一种情况是 A* 沿着障碍物 \\(C\\) 的贴边行进后，需要通过滑行边离开 \\(C\\)。由于上一步计算并保存了所有滑行边，因此可以直接查找并返回正确的边集。 ### 剔除带尖点的贴边 贴边连接接触同一圆形的滑行边，但事实证明，许多这样的贴边不能用于任何最优路径。我们可以通过消除它们来加速算法。通过障碍物森林的最优路径总是由交替的滑行边和贴边组成。假设我们从节点 \\(A\\) 进入，并尝试决定如何退出：从 \\(A\\) 进入意味着我们沿*顺时针* \\(\\circlearrowright\\) 方向行进。我们必须通过一个让我们保持顺时针 \\(\\circlearrowright\\) 的节点退出，因此只能通过节点 \\(B\\) 或 \\(D\\) 退出。通过 \\(C\\) 退出会在路径中产生一个*尖点* \\(\\curlywedge\\)，这永远不会是最优的。我们想要过滤掉这些带尖点的边。 首先注意，A* 已经将每条无向边 \\(P \\longleftrightarrow Q\\) 视为两条有向边 \\(P \\longrightarrow Q\\) 和 \\(Q \\longrightarrow P\\)。我们可以利用这一点，给边和节点加上方向标记。 1. 节点 \\(P\\) 变为带方向的节点：顺时针 \\(P\\circlearrowright\\) 或逆时针 \\(P\\circlearrowleft\\)。 2. 无向滑行边 \\(P \\longleftrightarrow Q\\) 变为两条有向边：\\(P \\longrightarrow Q\\) 和 \\(Q \\longrightarrow P\\)。 3. 来自 \\(P\\) 的无向贴边变为两条边：\\(P\\circlearrowright \\longrightarrow Q\\) 和 \\(P\\circlearrowleft \\longrightarrow Q\\)，具体取决于环绕圆的方向。 在构建图时，我们可以检查边 \\(P\\circlearrowright \\longrightarrow Q\\) 是否形成尖点。如果从 \\(P\\) 进入的方向（即到达 \\(P\\) 的边）也是顺时针，那么离开 \\(P\\) 的边必须保持顺时针。如果离开的边方向与进入方向相反，则产生一个尖点。通过仅保留不产生尖点的边，我们显著减少了图中的边数，从而加快了寻路速度。 ### 处理大量障碍物 当障碍物数量很大时（例如数百个），生成所有成对公切线的 \\(O(n^2)\\) 复杂度可能变得昂贵。我们可以通过空间分区技术（如四叉树或网格）来优化。仅在可能影响路径的障碍物对之间生成边，例如那些彼此接近的障碍物。此外，可以启发式地只考虑位于起点和终点之间方向上的障碍物。这些优化可以使算法在复杂场景中保持实用性。 ## 结论 通过将圆形障碍物环境转化为切线可见性图，我们可以使用标准的 A* 算法高效地进行路径规划。本文介绍的方法提供了基础实现，并讨论了使其更健壮和高效的多种改进。无论是游戏中的角色导航还是机器人路径规划，这种基于图的方法都是一种灵活而强大的工具。 ## 进一步阅读 - Amit Patel 的 A* 教程：http://theory.stanford.edu/~amitp/GameProgramming/ - Paul Bourke 的点到线段距离：http://paulbourke.net/geometry/pointlineplane/ - Paul Bourke 的圆相交：http://paulbourke.net/geometry/circlesphere/ - 切线可见性图的维基百科：https://en.wikipedia.org/wiki/Visibility_graph - 闵可夫斯基加法的维基百科：https://en.wikipedia.org/wiki/Minkowski_addition