Theta* : 基于网格的任意角度寻路

原文地址（科学上网）：https://arxiv.org/ftp/arxiv/papers/1401/1401.3843.pdf

1.简介

在本文中，我们将研究一种机器人技术或视频游戏中使用的路径规划方法，该方法适用于离散为阻隔和非阻隔网格的2D连续地形。我们的目标是，在给定一个起始点和一个目标点的情况下，寻找出一条没有阻隔的较短路径。其中，起始点和目标点都是网格单元角顶点。

A * 算法可以很快地寻找出一条附着在网格边缘上的路径，但是，由于其算法中路径点沿网格点排布的约束，导致A * 算法在寻路过程中可形成的角度往往是被网格形状固定的（如下图）。因此，A * 算法寻找出来的路径往往并不是实际地形下真正的最短路径。

图 1：

A * 的这个缺点，引出了我们今天要探讨的主题，我们称之为“任意角度的路径规划”算法。该算法在寻路过程中不需要将路线依附在网格边上，因此在路线方向上，没有角度的限制（如下图）。

图 2：

本文展示了两种任意角度的路径规划算法，分别是 Basic Theta* 和 Angle-Propagation Theta*。它们都是A * 算法的变体，都通过网格的边来扩展，同时也都不会将路径限制在格子的边缘上。与A * 不同的是，它们并不保证找到的路径一定是最短路径。它们名字中的符号 * 并不表示算法的最优性，只是为了表示它们与A * 算法的相似性。

Basic Theta * 算法更容易理解和实现，也能很快的找到较短的路径。而Angle-Propagation Theta * 在扩展顶点的过程当中传递可见角度范围，以实现在顶点扩展的过程中计算不会随着格子数量增线性增长，而是保持在常量级别。但是它也更复杂，速度更慢，找到的路径也通常稍长一些。

我们将这两种算法都统称为Theta * ,并对它的特性进行了研究。通过实验，我们发现在与A * 同级别的时间消耗下，Theta * 可以找出比基于后期平滑处理的A * 算法和FieldD * 算法（我们所知道的A * 的唯一一种基于网格边缘扩展但不将路径固定在网格边上的版本）都要更短的路径。

最后，我们将Theta * 算法扩展到具有非均匀遍历成本的网格，并在路径长短和耗时之间提供权衡。

2.定义问题和标记说明

我们要研究的问题是基于相邻8个均匀格子的路径规划问题。每个格子用黑白两色表示：黑色表示阻隔，不可通过；白色表示不阻隔，可通过。我们在寻路时使用的是格子的角顶点，而不是格子的中心。我们用S表示所有顶点的集合。我们所研究的路径规划问题也就是寻找出一条从给定顶点s_start到给定顶点s_goal之间的没有阻隔的路径。

当且仅当路径上的每个顶点和他的后继点之间存在视线时，我们称这条路为不阻隔的。当且仅当从顶点s到顶点s‘的直线不会从有阻隔的格子中穿过，也不会从两个有阻隔的格子的共同边上穿过时，我们称顶点s和顶点s’之间存在视线（LOS）， 记作LineOfSight(s, s′)，为了方便起见，我们我们允许视线从两个通过对角线方向相邻的有阻隔的格子的交点穿过。

我们用c(s, s′)表示顶点s到顶点s’间的直线距离。用nghbrsvis(s)表示顶点s相邻的可见顶点的集合，也就是那些与顶点s相邻并且与s有LOS存在的顶点。

3.现有的地形离散化方式

在路径规划中，需要将连续的地形进行离散化操作。通过网格离散化地图被广泛应用在机器人技术和视频游戏中。这种方式具有几点很可取的特性：

• 网格是一种很简单的数据结构，并且很容易在路径规划算法中使用。
• 通过在地图上放置网格，并标记那些被部分或全部阻隔的格子，就可以很简单地将地图进行离散化处理。
• 网格提供了一个连续地形上可通行区域的全景图。当路径规划被用在一个动态的环境中并且必须与导航者进行交互的时候，这个特点是很有用的。例如，如果游戏中的一个角色在寻路路径中碰到了一堵墙，它可以通过网格迅速查找到左侧和右侧的格子是否有阻隔，从而来确定左转还是右转。
• 除了是否可通行，网格还可以在格子中存储其他的信息，比如金矿的数量，以用作具体的地形显示信息。
• 由于网格是一种可随机访问的数据结构，因而存储在格子中的信息可以在需要时被快速访问。
• 通过增加格子的密度，可以很容易地提升寻路或导航路线的精确度。

下面列出了一些其他的离散化方式，方便起见，这里假设地形中的障碍物都是多边形。

• 沃罗诺伊图(AurenHammer，1991)通过偏离阻塞多边形的路径来离散地形。 由此产生的路径可能比真正的最短路径长得多。
• Mitchell和Papadimitriou（1991）将地形划分为具有线性和双曲边的区域，使得可以在O(m^5/3)的时间和空间复杂度下寻找到真正的最短路径（其中m是阻塞多边形的角数）。 因此，路径规划的运行时间可以在阻塞多边形的角数中超线性地增长。
• Framed Quadtrees(Yahja，Stentz，Singh，&Brumitt，1998)递归地将地形细分为四个大小相等的单元，直到所有单元完全阻塞、完全通畅或足够小。 由此产生的路径可能会有不必要的转向（转向发生在自由空间而不是阻塞多边形的角顶点）。
• 概率路线图(Kavraki，Svestka，Latombe，&Overmars，1996)或快速探索随机树(LaValle&Kuffner，2001)随机放置顶点（除了开始和目标顶点），并将有视线的顶点相连。 顶点的随机位置需要仔细调整，因为它影响路径规划的运行时、找到路径的可能性和路径的长度。
• 可视图(Lee，1978；Lozano-P´Erez&Wesley，1979)使用每个阻塞多边形的角作为顶点（除了开始和目标顶点）。如果两个顶点之间有视线，便将两个顶点连线，这种方法可以找到真正的最短路径。但由于在这种方法中，边的数量随顶点数的增加呈指数级增长，因此当顶点数量增多时，时间消耗也迅速增加。

4.现有的路径规划算法

在本节中，我们将介绍几种现有的路径规划算法，所有这些算法都是A*算法的变体。A * 算法是当前机器人技术领域和视频游戏领域内非常流行的路径规划算法。在A * 算法中，对于每个顶点s，有三个很重要的值：

• G值：g(s)，表示从起点到当前s点的长度（或消耗）。
• H值：h(s)，表示从当前s点到目标点的估算长度（或消耗）。A* 通过公式 f(s) = g(s) + h(s) 计算得到F值，也就是通过s点的整条路径的估算距离（或消耗）。
• 父节点：parent(s)，用于在寻路结束后提取最终路径。

A*中还有两个重要的全局数据结构：

• open list ：包含了A * 在扩展过程中待处理的顶点的优先队列。
• close list ：包含了A * 在扩展过程中已经处理过的点的集合，用于确保每个点只会被路过一次。

A * 算法的具体步骤这里不再赘述。

4.1 基于网格的A *

可以通过网格的顶点和边来进行A * 算法的计算，由此产生的路径会被人为地强制地附着在网格的边上（图1）。因此，基于网格的A * 得出的路径并不是真正的最短路径，它们要么偏离最短路径相当距离，要么有更多不必要的转向，于是便出现了针对A * 的平滑后处理方案。

4.2 经过平滑后处理的A * （A * PS）

通过后处理对A * 寻找的路径进行平滑处理，我们称之为 A * PS。

A * PS同样也是基于网格的，但是在其寻找到路径之后，通过一定的后处理算法，对其找到的路径进行平滑处理，这会一定程度缩短路径，但也会增加整个寻路过程的耗时。
下图展示了一种简单的后处理算法：

图 4：

假设基于网格的A * 算法寻找到一条路径（S₀， S₁， … S_c），其中S₀ = S_start ，S_c = S_goal， A * PS将路径中的第一个点S₀ 作为当前顶点，然后检查它的后继节点S₁ 的后继节点S₂，如果S₀ 和 S₂ 之间存在LOS，则将S₁ 从路径当中删除，从而缩短路径。之后重复这个过程，如果某个节点和当前节点间不存在LOS，则将当前节点前推。重复以上步骤，直至到达最后一点。

在实验中，我们使用两点间的直线距离（h(s) = c(s, s_goal)）作为H值。

A * PS通常可以产生比 A * 更短的路径，但是它并不能保证获取到真实情况下的最短路径（如下图）。

图 5：

假设 A * 找到的最终路径是上图中的蓝色虚线路径，这是网格中众多最短路径中的一条。然后通过A * PS对其进行平滑后处理，最终得到上图中蓝色实线路径，而这条路径并不是真实情况中的最短路径。上图中，红色虚线路径才是真实情况中的最短路径。A * PS之所以不能保证得到真实的最短路径，是因为它只是对基于A * 寻找的路径进行平滑处理，而A * 在寻找的过程中，只会找出多条最短路径当中的其中一条，并不会考虑其他路径。这是一个先找出所有点，再进行平滑处理的过程。而有时候 ，需要交叉进行查找和平滑处理，才能获取到真正的最短路径。事实上，Theta * 与 A * PS 是很相似的，但是 Theta * 采取了查找和平滑处理交叉进行的方式。

4.3 Field D * (FD *)

关于D * 系列的算法可以参考其他博文。

4.4 基于可视图的A *

另外一种方式是基于可视图的A * 算法，在实验中我们通过使用网格来表示可视图。这种方式也可以找到真实的最短路径（如下图）。

图 6：

相比于基于网格的A * 、A * PS 或者FD * ,基于可视图的A * 只会在障碍物的转角处进行转向，而不会产生各种不必要的转向，从而得到最短的路径。但是另一方面，基于可视图的A * 在执行效率上可能会相对较低。这是因为，对于可视图来说，信息是沿着障碍物的边缘传播，而不是沿网格的边缘传播，每增加一个新的障碍物，都会增加新的转角与当前所有转角之间的视线，因此，可视图中视线增加的数量与边缘增加是平方级别的，而其他算法中只考虑网格的边缘，因而是线性增长的关系。

当然也可以通过一些手段对算法进行优化，例如不要在A * 开始寻路之前先构建可视图的视线关系，而是在路径每次扩展到一个新的顶点时再去计算该顶点与已存在顶点的视线关系，这会降低大量不必要的计算。但是总体来讲，这种方式依然达不到其他A * 方式的效率。

5. Basic Theta *

在本节中，我们将会介绍Theta * 算法。这是一种任意角度下进行路径规划的A * 版本，它依然沿着网格边缘进行扩展，但并不会将路径约束在网格边缘上。

Theta * 算法结合了基于网格的A * 和基于可视图的A * 两者的优点，既能够保证计算量与格子的增加呈线性关系，又能保证只在转角处进行转向。通过这种方式寻找出来的最终路径只比真实情况下的最短路径稍长，当然，速度也比单纯使用基于网格的A * 稍慢。

Theta * 与 A * 关键的不同点在于，在 A * 中，一个顶点的父节点，只能是与它相邻的顶点，而在Theta * 中，一个顶点的父节点可以是任意的顶点。

下面我们先介绍一种简单版本的Theta * ，称之为 Basic Theta * 。

图 7：

5.1 Basic Theta * 的实现

Basic Theta * 的实现很简单，与A * 算法的唯一不同在于，当扩展一个新的顶点时，A * 只会考虑一条路径，而Basic Theta * 会考虑两条路径（如下图）。

图 8：

上图中，我们要从B₃点（s）扩展到C₃ 点（s’），其中，B₃点的父节点是A₄点（s_start），此时，Basic Theta * 会考虑如下两条路径：

• 路线1： s_start → s → s’ ，此时 g(s’) = g(s) + c(s, s’)，parent(s’) = s。这也是A * 考虑的路径，上图中用红色虚线表示。
• 路径2： s_start → s’, 此时g(s’) = g(s_start) + c(s_start, s’) = g(parent(s)) + c(parent(s), s’)，parent(s’) = parent(s)。这是Basic Theta * 比 A * 额外考虑的路径，在上图中用蓝色实线表示。

根据三角形性质，两边和一定大于第三边，可知路径2的距离一定比路径1更短。

另外，需要检查两点之间是否有LOS，上图中s’ 和 s_start之间存在LOS，因此可以选择路径2。有时二者之间并不存在LOS，就只能放弃这条路径（如下图）。

图 9：

下图表示了从A₄寻路到C₁点的完整过程：

图 10：

5.2 性质

原文里有一堆的推论和定理，来证明Basic Theta * 的正确性和优越性，有兴趣的可以自己看原文。

6. Angle-Propagation Theta * ( AP Theta * )

由于计算两点间是否存在视线的算法是与格子数量呈线性关系的，因此Basic Theta * 每扩展一个顶点时需要的时间也是随格子增长而线性增加的。本节我们将介绍 Angle-Propagation Theta * ( AP Theta * )算法。AP Theta * 算法与 Basic Theta * 算法最大的区别在于，AP Theta * 在扩展的过程中计算顶点的角度范围，并通过角度范围来直接判断新的顶点与父节点之间是否存在LOS，因此，AP Theta * 在扩展顶点时的消耗不再与格子数线性相关，而是变成常量级。

试想，如果在某个顶点A处存在一个点光源，并且光线不能穿过阻隔的格子，那么所有处于阴影中的格子，都表示光不能到达，也就是与A点无LOS的，而完全被光照亮的格子是与A点有LOS的。这些区域都存在与从A点发出的两条射线的夹角之内（如下图）。AP Theta * 每次在在从一个顶点向外扩展的时候，都会计算它的可见角度范围，并根据新点的角度是否在这个范围内来判断LOS是否存在。由于计算角度的时间几乎是常量的，因而使得可以在常量时间内对LOS是否存在进行检查。

图 11：

6.1 角度的定义

在AP Theta * 算法中，每个顶点s都含有两个表示条件的变量，分别是下角度区域 lb(s) 和上角度区域 ub(s)，合在一起记做 [ lb(s), ub(s) ]。这其中代表的是从s点的父节点parent(s)发出的射线的角度，其中从parent(s)到s点的射线的角度为0。对于s的邻接点，如果从parent(s)到该点的射线角度在 [ lb(s), ub(s) ] 之内，则该点到parent(s)之间一定存在LOS（如下图）。

图 12：

上图中，点s（C₃）的父节点为s_start (A₄)，角度范围为[-18, 27]，图中用红色区域表示。因此，s的所有位于红色区域内的邻接点，都与s_start之间存在LOS（比如C₄），而s所有在红色区域以外的邻接点，都与s_start之间不存在LOS（比如B₂）。

如下我们给出角度范围的更正式的定义。

用 Θ(s, p, s′) ∈ [ -90, 90] 表示射线 ps 和射线 ps’ 之间的夹角。如果射线 ps 在射线 ps’ 的顺时针方向，则角度取正值，反之，如果射线 ps 在射线 ps’ 的逆时针反向，则角度取负值。由上可得，正好穿过s点的射线的角度为0。对于顶点s的邻接点s’，如果存在 lb(s) ≤ Θ(s, parent(s), s′) ≤ ub(s) ，则s’和 parent(s) 之间一定存在LOS。

6.2 计算角度范围

当AP Theta * 星对一个顶点进行扩展的时候，会先将它的角度范围设置为[−∞,∞]，然后通过检查其他限制条件，对角度范围进行限制。当然，如果是起始点，由于起始点没有父节点，没有必要考虑LOS的问题，就不需要限制了。

下图伪代码展示了AP Theta * 对可见角度范围进行约束的过程。

图 13：

AP Theta * 对于角度范围的限制需要对以下几步进行检查：

• Case 1 ：如果当前点s是一个阻隔格子的顶点，并且当前阻隔格子的其他每一个顶点都至少满足以下条件之一，则当前点的下角度区域为0：

  • parent(s) = s’
  • Θ(s, parent(s), s′) < 0
  • Θ(s, parent(s), s′
    ) = 0 且 c(parent(s), s′) ≤ c(parent(s), s)

• Case 2 ：如果当前点s是一个阻隔格子的顶点，并且当前阻隔格子的其他每一个顶点都至少满足以下条件之一，则当前点的上角度区域为0：

  • parent(s) = s′
  • Θ(s, parent(s), s′)
  • Θ(s, parent(s), s′
    ) = 0 且 c(parent(s), s′) ≤ c(parent(s), s)

• Case 3 ：对于当前点s的邻接点s’，如果s’满足以下所有条件：

  • s′ ∈ closed
  • parent(s) = parent(s′)
  • s′ ≠ s_start

  此时s的可见角度范围需要进行约束，其约束的范围从几何上讲，可以理解为邻接点s’与s的夹角加上s’自身的下角度区域（或上角度区域）形成的整体区域A 与s的对应区域B， 二者叠加重叠的部分。从数学上讲就是讲lb(s’)或者ub(s’)旋转Θ(s, parent(s), s′)，即 lb(s’) + Θ(s, parent(s), s′)（或者 ub(s’) + Θ(s, parent(s), s′)）。

• Case 4 ：对于当前点s的邻接点s’，如果s’满足以下所有条件：

  • c(parent(s), s′) < c(parent(s), s)
  • parent(s) ≠ s′
  • s′ ∉ closed 或者 parent(s) ≠ parent(s′)

  此时表示AP Theta * 并没有点s’的完整信息，因此无法通过s’对s点的可见角度范围进行精准的约束，只能对s’的影响做最保守的估计，认为s’只是刚好与parent(s)存在LOS，即s’不存在lb(s’) 和 ub(s’)。

在对可见角度范围作了约束之后，在从s点向任意邻接点s’点扩展时，只需要通过检查是否满足lb(s) ≤ Θ(s, parent(s), s′) ≤ ub(s) 即可判断s’是否与parent(s)间存在LOS，而不再需要通过视线算法来求，这是AP Theta * 与 Basic Theta * 的唯一区别。

下图展示了从点A₄到点C₁的完整寻路过程。

图 14：

（a）表示从A₄点开始寻路，将A₄点的可见角度范围设置为 [−∞,∞] 。
（b）中路径扩展到B₃点，此时需要计算B₃点的可见角度范围。首先将其设置为 [−∞,∞] ，然后根据上文Case 1 对阻隔格子A₂ - A₃ - B₃ - B₂ 的检查，将下角度区域设置为0。根据上文Case 4 对非完整信息（未在close list中）的邻接点B₄进行检查，将上角度区域约束为45度。
（c）中路径扩展到点B₂，先将其可见角度范围设置为 [−∞,∞] ，然后根据上文Case 1 对阻隔格子A₂ - A₃ - B₃ - B₂ 的检查，将下角度区域设置为0。没有满足对上角度区域进行约束的条件，因此上角度区域范围不变。
（d）表示假设C₁点不是寻路终点时对该点可见角度范围的计算情况。首先，依然是将其设置为 [−∞,∞] 。然后根据上文Case 3 对邻接点B₂的检查，将下角度区域约束为-27度。根据上文Case 4 对非完整信息的邻接点C₂进行检查，将上角度区域约束为18度。

其他剩下的还有一些属性和实验，以及对Theta * 的扩展，如果感兴趣可以查看原文。