A*算法详细解读——附伪代码，python代码实现

A*算法通过下面这个函数来计算每个节点的优先级。

其中：
f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。
g(n) 是节点n距离起点的代价。
h(n)是节点n距离终点的预计代价，这也就是A算法的启发函数。关于启发函数我们在下面详细讲解。
A算法在运算过程中，每次从优先队列中选取f(n)值最小（优先级最高）的节点作为下一个待遍历的节点。

另外，A*算法使用两个集合来表示待遍历的节点，与已经遍历过的节点，这通常称之为open_set和close_set。

完整的A*算法描述如下：

初始化open_set和close_set；
* 将起点加入open_set中，并设置优先级为0（优先级最高）；
* 如果open_set不为空，则从open_set中选取优先级最高的节点n：
    * 如果节点n为终点，则：
        * 从终点开始逐步追踪parent节点，一直达到起点；
        * 返回找到的结果路径，算法结束；
    * 如果节点n不是终点，则：
        * 将节点n从open_set中删除，并加入close_set中；
        * 遍历节点n所有的邻近节点：
            * 如果邻近节点m在close_set中，则：
                * 跳过，选取下一个邻近节点
            * 如果邻近节点m也不在open_set中，则：
                * 设置节点m的parent为节点n
                * 计算节点m的优先级
                * 将节点m加入open_set中

启发函数
上面已经提到，启发函数会影响A*算法的行为。

在极端情况下，当启发函数h(n)始终为0，则将由g(n)决定节点的优先级，此时算法就退化成了Dijkstra算法。

如果h(n)始终小于等于节点n到终点的代价，则A算法保证一定能够找到最短路径。但是当h(n)的值越小，算法将遍历越多的节点，也就导致算法越慢。
如果h(n)完全等于节点n到终点的代价，则A算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。

如果h(n)的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径，不过此时会很快。

在另外一个极端情况下，如果h()n相较于g(n)大很多，则此时只有h(n)产生效果，这也就变成了最佳优先搜索。

由上面这些信息我们可以知道，通过调节启发函数我们可以控制算法的速度和精确度。因为在一些情况，我们可能未必需要最短路径，而是希望能够尽快找到一个路径即可。这也是A*算法比较灵活的地方。

关于距离
曼哈顿距离

如果图形中只允许朝上下左右四个方向移动，则启发函数可以使用曼哈顿距离，它的计算方法如下图所示：

计算曼哈顿距离的函数如下，这里的D是指两个相邻节点之间的移动代价，通常是一个固定的常数。

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * (dx + dy)

对角距离

如果图形中允许斜着朝邻近的节点移动，则启发函数可以使用对角距离。它的计算方法如下：

计算对角距离的函数如下，这里的D2指的是两个斜着相邻节点之间的移动代价。如果所有节点都正方形，则其值就是

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

欧几里得距离

如果图形中允许朝任意方向移动，则可以使用欧几里得距离。

欧几里得距离是指两个节点之间的直线距离，因此其计算方法也是我们比较熟悉的：

其函数表示如下：

function heuristic(node) =
    dx = abs(node.x - goal.x)
    dy = abs(node.y - goal.y)
    return D * sqrt(dx * dx + dy * dy)

python代码实现

1.设置一个存储结点属性的类，包括结点，结点的f_score,结点g_score,结点的h_score

如下：

2.代价函数的定义:

用两个点之间的距离来作为h(x)

def A_star(start, goal, a_map):

    closedset = set([]) 
       

    firstItem = HeapItem(start,goal, a_map)
    firstItem.set_scores(0.0)
    


    scoreHeap = [firstItem]    #openset
    openDict = {start:firstItem}   #保存openset结点的的一些信息，便于查询



    came_from = {}
    
    while any(scoreHeap): # is not empty
      #heapq.heappop弹出值最小的点
        x  = heapq.heappop(scoreHeap)


         #如果找到目标节点 回溯路径
        if x.node == goal:
            return reconstruct_path(came_from,goal)


        del openDict[x.node]
        closedset.add(x.node)

         #对每个相邻节点进行操作
        for node_y in a_map.neighbor_nodes(x.node):
            if node_y in closedset:
                continue

            #预估 node_y的g值
            tentative_g_score = x.g_score + a_map.dist_between(x.node, node_y)
            
            tentative_is_better = False
            tentative_is_new = False

            y = openDict.get(node_y,None)
            
            if y == None:
                y = HeapItem(node_y, goal, a_map)
                openDict[node_y] = y
                tentative_is_new = True
            elif tentative_g_score < y.g_score:
#如果不是new，只是better的话，也就是之前我们遍历了在openset里面，这时候它就有值存储在HeapItem里面，然后判断由其他的父节点走这条路径过来值会不会更小
                tentative_is_better = True
                
            if tentative_is_new or tentative_is_better:
                came_from[node_y] = x.node
                y.set_scores(tentative_g_score)
                
                if tentative_is_new:
                    heapq.heappush(scoreHeap, y)   #y加入列表，并进行堆排序
                else:
                    heapq.heapify(scoreHeap)  #将其堆化排序
         
    return []

用scoreHeap来作为openset

其中neighbor_nodes(x.node)的函数（遍历上下左右):

回溯函数：