# 路径规划算法之 A* 与 D* Lite 原理详解

# 问题描述

如何在一个网格地图中找到两点之间的最短路径

# 基础算法介绍

如果要在一个网格地图中找到两点之间的最短路径，很容易想到的广度优先算法（Breadth First）、最佳优先算法和 Dijkstra 算法。

# 广度优先搜索

广度优先搜索算法如其名称所示以广度做为优先级进行搜索。

从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点。

这种算法就像洪水（Flood fill）一样向外扩张，算法的过程如下图所示：

广度优先算法的优点是一定可以找到两点间的最优路径，但是代价就是需要搜索的点非常多，速度会比较慢。

# 最佳优先算法

在一些情况下，如果我们可以预先计算出每个节点到终点的距离，则我们可以利用这个信息更快的到达终点。

最佳优先算法和广度优先算法不同，它需要使用一个优先队列，用每个节点到终点的距离作为优先级每次始终选取到终点移动代价最小（离终点最近）的节点作为下一个遍历的节点，直到到达终点。这种算法称之为最佳优先（Best First）算法。和广度优先相比，最佳优先所需要搜索的点要少很多，可以大大加快路径的搜索速度，如下图所示：

但最佳优先算法的缺点就是，当起点和终点有障碍物时，可能最佳优先算法找到的路径并不是最佳的路径，下图描述了这种情况：

# Dijkstra 算法

Dijkstra 算法是由计算机科学家Edsger W. Dijkstra 在 1956 年提出的

Dijkstra 算法用来寻找图形中节点之间的最短路径。

考虑这样一种场景，在一些情况下，图形中相邻节点之间的移动代价并不相等。例如，游戏中的一幅图，既有平地也有山脉，那么游戏中的角色在平地和山脉中移动的速度通常是不相等的。

在 Dijkstra 算法中，需要计算每一个节点距离起点的总移动代价。同时，还需要一个优先队列结构。对于所有待遍历的节点，放入优先队列中会按照代价进行排序。

在算法运行的过程中，每次都从优先队列中选出代价最小的作为下一个遍历的节点。直到到达终点为止。

下面对比了不考虑节点移动代价差异的广度优先搜索与考虑移动代价的 Dijkstra 算法的运算结果：

当图形为网格图，并且每个节点之间的移动代价是相等的，那么 Dijkstra 算法将和广度优先算法变得一样。

# A* 算法

A* 算法最初发表于 1968 年，由 Stanford 研究院的 Peter Hart, Nils Nilsson 以及 Bertram Raphael 发表。它可以被认为是 Dijkstra 算法的扩展。

由于借助启发函数的引导，A*算法通常拥有更好的性能。

A* 算法通过下面这个函数来计算每个节点的优先级。 $$ f(n) = g(n) + h(n) $$ 其中：

• f(n) 是节点 n 的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。
• g(n) 是节点 n 距离起点的实际代价。
• h(n) 是启发函数，是节点 n 到终点的估计值
  • 在极端情况下，启发函数始终为 0，则将由 g(n)g(n)决定节点的优先级，此时算法就退化成了 Dijkstra 算法。
  • 如果 h(n)始终小于等于节点 n 到终点的代价，则 A*算法保证一定能够找到最短路径。但是当 h(n)的值越小，算法将遍历越多的节点，也就导致算法越慢。
  • 如果 h(n)完全等于节点 n 到终点的代价，则 A*算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。
  • 如果 h(n)的值比节点 n 到终点的代价要大，则 A*算法不能保证找到最短路径，不过此时会很快。
  • 在另外一个极端情况下，如果 h(n)相较于 g(n)大很多，则此时只有 h(n)产生效果，这也就变成了最佳优先搜索。

由上面这些信息我们可以知道，通过调节启发函数我们可以控制算法的速度和精确度。因为在一些情况，我们可能未必需要最短路径，而是希望能够尽快找到一个路径即可。这也是 A*算法比较灵活的地方。

对于网格形式的图，有以下这些启发函数可以使用：

• 如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离（Manhattan distance）。
• 如果图形中允许朝八个方向移动，则可以使用对角距离。
• 如果图形中允许朝任何方向移动，则可以使用欧几里得距离（Euclidean distance）。

A* 算法还需要使用两个集合来表示待遍历的节点，与已经遍历过的节点。

• OpenList：可到达的节点
• CloseList：已到达的节点

A* 算法具体的运行过程为：每次从优先队列中选取 f(n)值最小（优先级最高）的节点作为下一个待遍历的节点，如果该节点是目标节点，则直接返回，算法结束。如果不是，则遍历其邻居节点，对所有不在 CloseList 中的、在网格范围内的、非障碍物的节点，计算其中 F 值、G 值和 H 值，添加到优先队列（OpenList）中和 CloseList 中。

A* 算法 Java 实现如下图所示：

# A* 算法变种

A* 算法有不少的变种，主要有如下算法：

• ARA * ：ARA* - Anytime A* with Provable Bounds on Sub-Optimality

• D* ：D* 是 Dynamic A* 的简写，其算法和 A*类似，不同的是，其代价的计算在算法运行过程中可能会发生变化。

  • Project “Fast Replanning （Incremental Heuristic Search）”
  • Real-Time Replanning in Dynamic and Unknown Environments

• Field D* ： Field D* 扩展了 D* 和 D* Lite，是一种基于插值（ interpolation-based ）的规划算法，它使用线性插值来有效地生成低成本路径，从而消除不必要的转向。

  在给定线性插值假设的情况下，路径是最优的，并且在实践中非常有效。该算法目前被各种现场机器人系统使用。

  关于 Field D* 的详细内容可以看下面这篇论文：

  • Field D*: An Interpolation-based Path Planner and Replanner

# D* Lite 算法

D* Lite 算法是一种增量启发式搜素算法，由 Sven Koeing 和 Maxim Likhachev 于 2004 年提出，是基于 LPA* 和 Dynamic SWSF-FP 的一种算法。D* Lite 算法可以适用于地图未知、环境随时会发生变化的情况，在遇到新增加的障碍物时，可以利用先前搜索所获得的信息，而不需要完全重新规划路径。

D* Lite 的启发函数与 A* 类似，同样有一个启发函数，不过因为 D* Lite 是从终点向起点搜索，所以对应的启发函数 h(n) 也变成了节点 n 到起点的估计值。

D* Lite 中几个概念的定义：

• g(n)：当前节点到终点的实际代价

• h(n)：当前节点到起点的估计值

• rhs（right-hand side)：公式如下

  

  一个点的 rhs 值是它的父代节点中 g 值加上这两点之间的代价中的最小值，相当于一个点从父代节点到达这个点的最小代价。其实在算法的大部分过程中，g 值和 rhs 值是相等的。

• 两个 key 值：在 A* 算法中，通过 f(n) 的大小来判断一个点的优先级，而在 D* Lite 中，需要通过两个 key 值来判断一个点的优先级，key 值越小优先级越高，先判断第一个 key 值，如果第一个 key 值相等再判断第二个 key 值。公式如下：

  

  其中 km 的定义为，算法初始化时会先将 km 设置为 0，之后当机器人有检测到地图的变化时，km 需要加上上一个起点与当前位置的启发距离，并且把当前所在的点设置为新的起点，即更新起点的位置。

  如果在机器人还没有移动的时候 km 就等于 0，这时算法其实就相当于一个反向从终点往起点方向搜索的 A* 算法了。

  当机器人检测到障碍的变化时会再一次规划路径，这时候的实际起点应该是机器人当前的位置，起点发生了变化，每个点的 h 值也会相应变化，key 值也发生了变化。如果不引入这个参数的话，就需要把优先队列中的全部节点都重新计算一遍 key 值，增加了计算量。引入之后就可以一定程度上保证 key 值的一致性，减少计算量。

• 第二 key 值就是 g 值和 rhs 值中的最小值，它的意义在于当两个点的第一个 key 值相等的时候，算法会优先选择距离终点近的点。

• 局部一致性：D* Lite 算法中还有一个很重要的概念就是局部一致性，通过一个点的局部一致性来判断当前点是否需要计算。其定义如下：当一个点的 g 值等于 rhs 值时称这个点为局部一致的点，否则称这个点为局部不一致。其中局部不一致的情况还可细分成为局部过一致和局部欠一致：当一个点的 g 值大于 rhs 值时，这个点为局部过一致，通常是有障碍物删除时或者算法第一次搜索路径时；当一个点的 g 值小于 rhs 值时，这个点为局部欠一致，通常是检测到了新增的障碍物。

D* Lite 算法的步骤：

• 将当前点设置为起点
• 将优先队列设置为空队列，将所有节点的 g 值和 rhs 值设置为无穷，将终点的 rhs 值设为 0，并且计算它的 key 值加入到优先队列中。
• 调用 ComputeShortestPath()开始计算它的最短路径
• 移动到子代中 g 值加上这两个点之间代价中最小的点。
• 如果检测到了障碍的变化，根据上一个起点和当前点的启发值，修改 k_m 的值，并将当前节点设置为新的起点。
• 对所有两个点之间的代价发生变化的，更新这两个点之间的代价，如果两个点之间的代价变小，说明有障碍物删除，更新它的 rhs 值，如果代价变大了，说明新增了一个障碍物，需要通过它的子代来更新 rhs 值。
• 更新受影响的节点。
• 计算最短路径。

D* Lite 算法 Java 代码实现还未完成

python 版代码参考：https://github.com/avgaydashenko/d_star

# 参考文献

[1]路径规划之 A*算法

[2]路径规划之 D* Lite 算法详解及实现