A星寻路算法
前言
A星寻路算法是静态路网中求解最短路径最有效的直接搜索方法,也是解决许多搜索问题的有效算法,它可以应对包括复杂地形,各种尺度的障碍物以及不同地形的路径规划问题。掌握A星寻路算法能够提高路径规划效率,应对各种复杂情况,并在实际应用中发挥重要作用。
算法原理
A星算法的核心公式为:F = G + H
。算法正是利用这个公式的值来计算最佳路径。
其中 F
表示当前点的总估价,G
表示从起始点,沿着产生的路径 ,移动到指定网格的实际代价,H
表示从当前网格点到终点的预计代价。公式中的 G
是确定的,而 H
是不确定的。
启发函数
H
代价的大小取决于计算 H
代价的函数,又被称为启发函数(Heuristic Function) 。常用的启发函数包括曼哈顿距离 和欧几里得距离。
曼哈顿距离
曼哈顿距离,是指在一个坐标系中,从一个点到另一个点沿着网格线(水平或垂直)的距离。曼哈顿距离只允许朝上下左右四个方向移动。示意图如下:
图中从 A
点 运动到 B
点有三条路径,三条路径计算出来的总路径都是相等的,这个长度就是曼哈顿距离,可以用如下公式表示曼哈顿距离:
math
D = |x1 - x2| + |y1 - y2|
欧几里得距离
欧几里得距离,是指在n维空间中,两点之间的直线距离。它是由古希腊数学家欧几里得所提出的。在二维空间中,欧几里得距离可以通过勾股定理得到,即两点之间的距离等于它们在 x 轴上的距离的平方加上它们在 y 轴上的距离的平方,再取平方根。下图为一个二维空间中 A
点到 B
点的欧几里得距离示意图:
距离计算公式为:
math
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
值和父节点指针。
- a. 如果节点不可达或已在
-
判断终点: 每次加入节点到
openList
时,都需要判断该节点是否为终点,如果是,则说明已经找到了最短路径。 -
循环结束: 当
openList
为空时,表示没有找到终点,搜索结束。 -
构建最短路径: 从终点开始按照父节点指针逆向回溯,直至回溯到起点,即可得到最短路径。
-
举个例子
假设我们有如下一个网格地图,其中黑色方格表示障碍物,白色方格为可通行区域,绿色方格为起点,红色方格为终点。
我们规定,从起点出发,可以沿着网格线或者网格的对角线方向移动,每次沿着网格线朝上、下、左、右方向运动一格,距离记为10,朝着网格对角线方向运动一格,距离记为 <math xmlns="http://www.w3.org/1998/Math/MathML"> 2 \sqrt{2} </math>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 值和父节点。
计算结果如下图所示:
第五步
从 openList 中选取 F 值为 64 的网格进行探索,这时候找到了三个网格点 F 值都为 64,随机选取右上角 F=64 的网格点进行探索。被探索过的网格点加入 closeList 中。
计算结果为下图所示:
第六步
经过上一步操作后,openList 中 F 值最小还是为 64,随机选取左下方 F = 64 的网格点进行探索。被探索过的网格点加入 closeList 中。
计算结果为下图所示:
第七步
此时 openList 中F值最小还是为 64,选取下方 F = 64 的网格点继续进行探索。被探索过的网格点加入 closeList 中。
计算结果为下图所示:
第八步
探索 F= 70 的网格点,被探索过的网格点加入 closeList 中。结果为下图所示:
第九步
探索第二个 F = 70 的网格点,被探索过的网格点加入 closeList 中。结果如下图所示:
第十步
探索第三个 F = 70 的网格点,被探索过的网格点加入 closeList 中。结果如下图所示:
第十一步
选取起点右上方 F= 78的网格点进行探索,被探索过的网格点加入 closeList 中。结果如下图所示:
第十二步
选取 F = 72 的网格点进行探索,被探索过的网格点加入 closeList 中。结果如下图所示:
第十三步
选取 F = 66 的网格点进行探索,此时与 F66 相邻的 8 个网格中包含终点,因此到此整个 A星算法的探索过程结束。我们再从终点开始,根据记录的父节点的指针,找到A星算法的最佳路劲。结果如下图所示:
算法总结
A星算法是一种启发式搜索算法,它通过在地图上找到一条从起点到终点的路径来解决一些问题。该算法通过启发式函数来评估每个节点,并选择具有最低 F 值的节点作为下一个要探索的节点。最终,该算法会找到一条最优的路径。