【路径规划】(2) A* 算法求解最短路，附python完整代码_a*算法python代码

大家好，今天和各位分享一下机器人路径规划中非常经典的 A* 算法，感兴趣的点个关注，文末有 python 代码，那我么开始吧。

---

1. 算法介绍

A* 算法是 1968 年 P.E.Hart[1]等人所提出的在全局地图环境中所有已知情形下求解最短路径问题的方法，由于其简洁高效，容易实施等优点而受到人们的广泛关注。但是，A*算法在实际应用过程中也暴露出其严重弊端，例如：在搜索空间较大的环境下，增加了算法的执行时间，从而大大降低了路线规划效果；复杂环境下，由于地图精度不高，从而减小了获取最佳路径的可能性；在搜索过程中存在相同 F 值无法得到最佳路线等问题。

而面对上述问题，不同的研发人员也以不同的方法以对其做出了改善，并获得了不少研究成果。

Korf 等人[2]在 A*算法的基础上构建了迭代改进的 IDA* 算法，在计算过程中根据迭代次数得到最优解，解决了A*算法容易陷入盲目搜索的不足；Tran HAM[3]等人在 A*算法基础上构建了一种基于视觉的路径规划和导航算法，然后对搜索的路径进行优化。Szczerba[4]等人提出了稀疏 A*算法，在传统 A*算法基础上添加相应的约束条件，根据不同的约束条件修改 A*算法的搜索空间，提升了路线规划效果，大大增加了获取最佳路径的可能性。

参考文献：

[1] Hart P E, Nilsson N J, Raphael B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107. 

[2] Korf,  Richard  E.  Depth-first  iterative-deepening:  an  optimal  admissible  tree  search[J]. Artificial Intelligence, 1985, 27(1):97-109. 

[3] Tran H  A M, Ngo H Q  T,  Nguyen  T  P,  et  al.  Implementation  of  vision-based  autonomous mobile platform to control by A* algorithm[C]//2018 2nd International Conference on Recent Advances in Signal Processing, Telecommunications & omputing (SigTelCom). IEEE, 2018: 39-44.  

[4] Szczerba R J, Galkowski P, Glicktein I S, et al. Robust algorithm for real-time route planning[J]. IEEE Transactions on Aerospace & Electronic Systems, 2000, 36(3):869-878.   

---

2. 算法原理

经典 A* 算法主要用在静态且周围环境已知的情况下，是建立在 Dijkstra 和BFS 基础上的启发式遍历搜索算法，在路径规划时不仅要考虑自身与最近节点位置的距离（Dijkstra 实现），还需要考虑自身位置与目标点的距离（BFS 实现）。与传统的遍历搜索算法相比，弥补了搜索过程中的贪心机制，从而很好地完成路径规划的任务。

2.1 基本原理

A* 算法以起点为中心开始进行路径规划，并在路径规划过程中扩展周围的邻近节点。通过 A*算法公式对起始点到当前位置和当前位置到目标点的代价值进行计算。在所有的节点中选取代价值最小的节点作为当前最优节点，以当前最优节点继续搜索，直到搜索到目标点，最后生成一条从起始点到目标点的路径。

A* 算法本身遵循 821 定律，同时在搜索过程中将节点的状态定义为未检查、待检查、已检查三种准备状态；8 是指以当前定位点周围的 8 个邻域的代价值作为计算目标，2 是指两个参数列表，一个是 open list 列表，存放待检查的节点；另外一个是 close list 列表，存放已检查完成不再需要关注的节点，1 是指一个计算公式，如下。 

其中，f(n) 是指当前节点 n 的全局代价值，g(n) 是指从起始点到当前节点 n 的实际代价值，h(n) 是指当前节点 n 到终点的预测值。

---

2.2 案例解析

A* 算法整体搜索过程如下图所示，图中绿色格子为寻径起始点，红色格子为寻径终止点，蓝色代表障碍物。从起始点开始，将其放到 open list 列表中，同时让其作为当前节点。通过当前节点查找与其相邻的 8 个邻域节点，如图中带有绿色线条的方格。8 个邻域节点方格中，箭头指向的方向为当前相邻节点 n 的父节点，用于代价值的计算以及最终路径的回溯。 

以起始点作为当前节点，通过 A* 算法代价公式计算相邻节点的 f 值，并将起始节点放入 close list 列表中。起始点周围每个方格节点的三个夹角处由数字标识，其中左下角表示的是从起始点到当前节点 n 的真实代价值 g(n)，如果起始点到当前节点 n 是在横向上往前/后移动了一个节点，或者在竖直方向上往上/下移动了一个节点，普遍设定 g(n) 移动一个节点的代价为 10；如果起始点到当前节点 n 是经过对角方向的移动，普遍设定 g(n) 的移动代价值为 14。右下角表示的是从当前节点 n 到终点的预估值 h(n)，通过启发函数进行计算。左上角表示的是全局代价值 f(n)，将以上计算出的 g(n)与 h(n)相加可得到对应的 f(n)。 

基于以上计算，进一步判断当前节点的 8 个邻域节点是否在 open list 列表中，如果某个邻域节点未在 open list 列表中，则将带有 f  值的邻节点添加到 open list 列表中；如果某个邻域节点已经存放在 open  list 表中，则以当前方格为父节点检查经由当前父节点到达那个方格是否具有更小的 g 值，如果没有不做任何操作，如果存在更小的 g 值，把那个方格的父节点设置为当前方格，然后重新计算那个方格的 g 值和 f 值。判断完成之后，获取 open list 列表中最小代价值节点。从上图中可以看出起始点周围相邻节点的全局代价值 f 根据从上到下、从左向右的顺序分别为 124、110、104、110、90、104、90、84，从以上数字中选取 f 值最小的 84 的点作为当前节点。 

循环上述搜索步骤，直至搜索到右侧的红色目标点，将目标点添加到 close list 列表中。根据close list 列表中的各个节点按照父节点的指向逆序输出，从而得到搜索路径。如下图所示，带有黄色边框的格子节点为最终搜索到的最佳路径。 

---

2.3 启发函数

在路径规划时，通常需要寻找一些与搜索环境相关的特征信息，在计算过程中，将这些特征信息称为启发函数。启发函数被用来估计从当前节点到目标节点生成的代价值。常用的启发函数主要包括曼哈顿距离函数、欧氏距离函数、对角距离函数三种。 

假设起点的坐标为 (