路径规划——A*算法

路径规划——A*算法

算法原理

为了解决Dijkstra算法效率低的问题，A Star 算法作为一种启发式算法被提出。该算法在广度优先的基础上加入了一个估价函数。如BFS和常规方法如Dijsktra算法结合在一起的算法，有点不同的是，类似BFS的启发式经常给出一个近似解而不是保证最佳解，A Star是路径搜索中最受欢迎的选择，它相当灵活，并且能用于多种多样的情理之中，A Star 潜在的搜索一个很大的区域，A Star算法能用于搜索最短路径，A Star 能用启发式函数引发它自己，在简单的情况中，它和BFS一样快。
BFS算法：Best First Search
Dijkstra算法：Dijkstra

1968年，启发式方法提出了A* ，其中包括将已知区域划分为单个单元，并计算每个单元达到目标的总成本。给定从起点到当前分析像元的总距离以及从分析像元到目标的距离，总路径成本使用以下公式计算：
$$\begin{gathered} F(n)=G(n)+H(n),(1) \\ F(n)是总路径开销，也是节点n的综合优先级。每次选择下一个要访问的节点时， \\ 总是选取综合优先级最高的节点，也就是总开销最小的节点； \\ G(n)是从起点到达指定单元的成本和； \\ H(n)是从指定单元到达目标的预估成本，也是A^{\ast }算法的启发函数。 \end{gathered}$$

• start：路径规划的起始点
• goal：路径规划的终点
• g_score：从当前点(current_node)到出发点(start)的移动代价
• h_score：不考虑障碍物，从当前点(current_node)到目标点的估计移动代价
• f_score：f_score=g_score+h_score
• openlist：寻找过程中的待检索节点列表
• closelist：不需要再次检索的节点列表
• came_Form：存储父子节点关系的列表，用于回溯最优路径，非必须，也可以通过节点指针实现

A*算法的核心就是上式，在运算过程中，每次从优先队列中选取F(n)值最小的节点作为下一个待访问的节点。

优点：利用启发式函数，搜索范围小，提高了搜索效率；如果最优路径存在，那么一定能找到最优路径。
缺点：A* 算法不适用于动态环境；不适用于高维空间，计算量大；目标点不可时会造成大量性能消耗。

算法流程：
1.从起点start开始，将start放入open优先队列中，open中存储的是将要遍历的节点，列表closed为空，closed是用来存放已经遍历的节点；
2.遍历start，将start从open中移除并添加到closed中，寻找start的邻居节点，至于最多可以找到多少邻居节点取决于算法规划，这里假设只允许节点向上下左右4个方向寻找那便是最多可以找到4个可通行的邻居节点，并计算F(n),将可通行的邻居节点放入open中；
3.从open中选择F(n)值最小的节点node，将node节点从open中移除并添加到closed中，寻找node的可通行的邻居节点并计算F(n)；
4.如果邻居节点neighbor已经在closed中了，则跳过，不进行其他处理，
5.如果邻居节点neighbor不在open中，则将其添加到open中，并且将node设置为neighbor的父节点；
6.如果邻居节点neighbor已经在open中了，则计算新的路径从start到neighbor的代价即G(n)值：
如果新的G(n)值比之前存储的值更小则更新，并且将neighbor的父节点改为节点node，并计算F(n)，之后存储到open中会根据F(n)值进行排序，
如果新的G(n)值比之前存储的值更大，则说明新的路径代价更高便不会选取这个节点也就不需要做出更新等处理；
7.再次从第3步开始执行，如此循环，直到在第3步中找出的节点node是目标节点goal时结束循环，也就成功找到了路径；或者在执行第3步时open为空，也要结束搜索过程了，此时说明无法找到合适路径。

算法实现

"""
    Filename: a_star.py
    Description: Plan path using A* Algorithm
    Author: Benxiaogu:https://github.com/Benxiaogu
    Date: 2024-08-05
"""
import heapq
import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation


class AStar:
    def __init__(self,grid,start,goal,board_size):
        self.grid = grid
        self.start = self.Node(start,0,0