A星寻路算法详解

A星寻路算法

前言

A星寻路算法是静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法，它可以应对包括复杂地形，各种尺度的障碍物以及不同地形的路径规划问题。掌握A星寻路算法能够提高路径规划效率，应对各种复杂情况，并在实际应用中发挥重要作用。

算法原理

A星算法的核心公式为：F = G + H。算法正是利用这个公式的值来计算最佳路径。

其中 F 表示当前点的总估价，G 表示从起始点，沿着产生的路径，移动到指定网格的实际代价，H 表示从当前网格点到终点的预计代价。公式中的 G 是确定的，而 H 是不确定的。

启发函数

H 代价的大小取决于计算 H 代价的函数，又被称为启发函数(Heuristic Function)。常用的启发函数包括曼哈顿距离和欧几里得距离。

曼哈顿距离

曼哈顿距离，是指在一个坐标系中，从一个点到另一个点沿着网格线（水平或垂直）的距离。曼哈顿距离只允许朝上下左右四个方向移动。示意图如下：

图中从 A 点运动到 B 点有三条路径，三条路径计算出来的总路径都是相等的，这个长度就是曼哈顿距离，可以用如下公式表示曼哈顿距离：

D = |x1 - x2| + |y1 - y2|

欧几里得距离

欧几里得距离，是指在n维空间中，两点之间的直线距离。它是由古希腊数学家欧几里得所提出的。在二维空间中，欧几里得距离可以通过勾股定理得到，即两点之间的距离等于它们在 x 轴上的距离的平方加上它们在 y 轴上的距离的平方，再取平方根。下图为一个二维空间中 A 点到 B 点的欧几里得距离示意图：

距离计算公式为：

D = \sqrt{(x1 - x2)^2 + (y1 - y2)^2}

算法原理

A星算法的实现步骤如下：

初始化: 设置起点和终点，定义两个队列 openList 和 closeList，openList 中存储待探索网格点，closeList 中存储已经探索过的网格点。初始化起点的G值和H值为 0，并将起点加入到 openList 中，如果有障碍物网格点，需要将障碍物网格点加入 closeList 中。
进入循环: 重复下面的步骤直到找到终点或 openList 为空。
- 选择估值最小单元格: 从 openList 中找到F估值最小的网格 (F = G + H) 作为当前节点，并将其移出 openList，加入到 closeList 中。
- 找到当前网格周围的节点: 根据当前网格点，找到其相邻的所有可行节点(不包括障碍物点)，计算它们的 G 值、H 值和 F 值，对每个相邻节点进行以下操作：
  - a. 如果节点不可达或已在 closedList 中，则忽略该节点。
  - b. 如果节点不在 openList 中，则将其加入 openList，并计算该节点的 G 值、H 值和设置父节点。
  - c. 如果节点已在 openList 中，并且经过当前节点到达该节点的 G 值更小，则更新该节点的 G 值和父节点指针。
- 判断终点: 每次加入节点到 openList 时，都需要判断该节点是否为终点，如果是，则说明已经找到了最短路径。
- 循环结束: 当 openList 为空时，表示没有找到终点，搜索结束。
- 构建最短路径: 从终点开始按照父节点指针逆向回溯，直至回溯到起点，即可得到最短路径。

举个例子

假设我们有如下一个网格地图，其中黑色方格表示障碍物，白色方格为可通行区域，绿色方格为起点，红色方格为终点。

我们规定，从起点出发，可以沿着网格线或者网格的对角线方向移动，每次沿着网格线朝上、下、左、右方向运动一格，距离记为10，朝着网格对角线方向运动一格，距离记为 $\sqrt{2}$ ×10≈14(这里为了简化计算过程，舍弃小数点，进行取整)。

第一步：

从 openList 中取出一个网格(就是开始节点，因为此时 openList 中只有一个开始节点)，计算其周围相邻的8个网格的 G 值、H 值和 F 值，这 8 个网格的父节点指向起始节点。其中，起点上下左右四个网格的 G 值为 10，对角线四个网格的 G 值为 14，8 个网格的 F 值采用曼哈顿方法进行计算，也就是待计算网格到终点的水平距离*10 + 待计算网格到终点的垂直距离*10，将已经探索过的起点加入 closeList 中，将起点周围 8 个节点都加到 openList 中。计算结果如下图所示：

需要注意的是，在计算 H 值的时候，如果遇到障碍物，H 值不需要考虑障碍物的影响。

第二步：

对 openList 中的节点按照 F 值的大小进行排序，并从排序后的 openList 中选取 F 值最小的一个网格，作为下一个要探索的点，此时 F = 44 的网格点（记为F44）被选中。以 F44 为中心，找到其周围的 8 个网格点，其中右边的三个网格点属于障碍物节点，已经存在于 closeList 中，直接跳过，左上方的网格点是起点，也在 closeList 中，跳过，F44上方和左边两个点在 openList 中，根据上一节介绍的A星算法的原理，需要判断经过当前节点的路径所得到的 G 值是否更小，如果更小则更新它们的 G 值、F 值还有父节点，否则保持不变。计算的方法就是取它父节点的 G 值，然后根据它相对父节点是水平垂直方向还是对角线方向，分别增加 10 和 14。

我们先看 F44 正上方的点，此时父节点为 F44，G 值为 14，方向是垂直方向，所以再加上 10，得到新的 G 值为 14 + 10 = 24，大于原来的 G 值 10，因此不做更新。

F44 左边的点，其到 F44 的方向是水平方向的，因此新的 G 值为父节点的 G 值加上 10，为 14 + 10 = 24，同样大于原来的 G 值 10，因此也不做更新。

F44 正下方和左下方的点，不在 openList 中，因此，需要将它们加入到 openList 中，并更新它们的 G 值、F 值和父节点。

计算结束后，将 F44 加入 closeList 中。

计算结果如下图所示：

第三步

从 openList(图中黄色网格区域) 中选取 F 值最小的网格点，作为当前要探索的点，此时 openList 中 F 值最小值为 50，但是找到了两个 F = 50 的网格点，这时要如何处理呢?

对算法而言，无论先选择哪个网格点先进行探索，最终的结果都是一样的，这里我们先选取起始点右边的 F=50 网格点进行探索。

以 F=50 的网格点为中心，找到其周围的 8 个网格点，其中右边三个网格点是障碍物，已经在 closeList 中，忽略，F50 正下方的点是我们之前已经探索过的点，忽略，F50 左边的点起点，也忽略，剩余的三个网格点都在 openList 中，根据上面介绍的方法，判断他们的 G 值是否更小，如果更小则更新它们的 G 值、F 值和父节点，否则保持不变。

已经探索过的网格点标记为蓝色，计算结果如下图所示：

第四步

从 openList 中选取 F 值最小的网格点进行探索，此时发现还是 F = 50 为最小值。以 F = 50 的网格点为中心，找到其周围的 8 个网格点，按照上面的方法对网格点进行更新，需要注意的是，此时 F50 正下方的网格点的 G 值为其父节点的 G 值，加上其到 F50 的距离，G = 10 + 10 = 20，小于之前的 G 值 28，因此需要更新 G 值、F 值和父节点。

计算结果如下图所示：