全局路径规划——A*搜索算法

参考：https://blog.csdn.net/HuangChen666/article/details/125487491
https://zhuanlan.zhihu.com/p/54510444
https://www.redblobgames.com/pathfinding/a-star/introduction.html

视频：https://www.bilibili.com/video/BV1bv411y79P/?spm_id_from=333.337.search-card.all.click&vd_source=07c19be7e433a8cd75d9808899f4c9d9

A*算法流程

f(n)是节点n的总代价。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。

g(n) 是节点n距离起点的代价。

h(n)是节点n距离终点的预计代价，这也就是A*算法的启发函数。

A*算法在运算过程中，每次从优先队列中选取f(n)值最小（优先级最高）的节点作为下一个待遍历的节点。

待遍历的节点open_set（用优先队列），已经遍历过的节点close_set。

* 1初始化open_set和close_set；
* 2将起点加入open_set中，并设置优先级为0（优先级最高）；
* 3如果open_set不为空，则从open_set中选取优先级最高的节点n：
*    如果节点n为终点，则：
*         从终点开始逐步追踪parent节点，一直达到起点；
*         返回找到的结果路径，算法结束；
*    如果节点n不是终点，则：
*         将节点n从open_set中删除，并加入close_set中；
*         遍历节点n所有的邻近节点：
*              如果邻近节点m在close_set中，则：
*                   跳过，选取下一个邻近节点
*              如果邻近节点m在open_set中，则：
*                   比较原代价g与现代价g0：
*                       若g<=g0：跳过，选取下一个邻近节点
*                       若g>g0：将该点父节点改为n，并将代价更新为g0
*              如果邻近节点m不在close_set和open_set中，则：
*                   设置节点m的parent为节点n
*                   计算节点m的优先级g
*                   将节点m加入open_set中

代码：https://github.com/zhm-real/PathPlanning
https://zhuanlan.zhihu.com/p/54510444