A * 算法介绍

封面《花咲ワークスプリング！》
因为某些原因需要接触 A * 算法，所以记录一下

A* 搜索算法

A * 搜索算法是一种带有启发式算法的路径规划算法，由 Stanford 研究院的 Peter Hart, Nils Nilsson 以及 Bertram Raphael 于 1968 年提出，现在仍在使用。结合了最佳优先算法 (Best first Search) 和 Dijkstra 算法的优点，被广泛的应用于游戏等领域。

最佳优先算法 (Best first Search)

通常深度优先算法和广度优先算法都是将任何相邻节点视为下一个需要遍历的节点直接遍历，都是盲目的探索路径而没有考虑遍历成本，此处的成本是指人为定义的成本函数，后续会提到。而最佳优先算法的思想是采用一种评价函数来判断哪个节点更优先进行探索。

其伪代码如下所示

Best-First-Search(Graph g, Node start)
    1) Create an empty PriorityQueue
       PriorityQueue pq;
    2) Insert "start" in pq.
       pq.insert(start)
    3) Until PriorityQueue is empty
          u = PriorityQueue.DeleteMin
          If u is the goal
             Exit
          Else
             Foreach neighbor v of u
                If v "Unvisited"
                    Mark v "Visited"                    
                    pq.insert(v)
             Mark u "Examined"                    
End procedure

下面将用代码对其进行展示，先构造一个迷宫，其中绿色表示其迷宫的起点，红色表示终点，黑色表示障碍物，白色表示可通行的路径。

%matplotlib inline
import matplotlib.pyplot as plt
import time
from functools import wraps
import heapq

def timer(func):
    @wraps(func)
    def wrapper(*args, **kwargs):
        start_time = time.time() 
        result = func(*args, **kwargs)  
        end_time = time.time()  
        cost = end_time - start_time  
        print(f"{func.__name__}耗时:{cost:.4f}s")
        return result  # 返回被装饰函数的执行结果
    return wrapper


class Node:
    '''
    节点
    '''
    def __init__(self,x:int,y:int,walkable:bool=True):
        self.x = x
        self.y = y
        self.walkable = walkable
        self.visited = False
        self.closed = False
        self.parent = None
        self.g = 0
        self.h = 0
    
    def f(self):
        return self.g + self.h
    
    def __lt__(self,other):
        return self.f() < other.f()

class Grid:
    '''
    地图
    '''
    def __init__(self,height:int,width:int,walls:list[list[int]]=None):
        self.height = height
        self.width = width
        self.walls = walls
        self.start_x = height//2
        self.start_y = 0
        self.end_x = height//2
        self.end_y = width-1

        
        self.reset()

    def is_valid(self,x:int,y:int)->bool:
        return x>=0 and x<self.height and y>=0 and y<self.width
    def is_walkable(self,x:int,y:int)->bool:
        return self.is_valid(x,y) and self.nodes[x][y].walkable
    
    def set_walkable(self,x:int,y:int,walkable:bool):
        self.nodes[x][y].walkable = walkable

    def get_neighbors(self,x:int,y:int)->list:
        neighbors = []
        for dx in [-1,0,1]:
            for dy in [-1,0,1]:
                if dx==0 and dy==0:
                    continue
                if self.is_walkable(x+dx,y+dy):
                    neighbors.append((x+dx,y+dy))
        return neighbors

    def is_start(self,x:int,y:int)->bool:
        return x==self.start_x and y==self.start_y
    
    def is_end(self,x:int,y:int)->bool:
        return x==self.end_x and y==self.end_y


    def get_start(self)->tuple:
        return self.start_x,self.start_y
    
    def get_end(self)->tuple:
        return self.end_x,self.end_y

    def get_end_node(self) -> Node:
        return self.nodes[self.end_x][self.end_y]

    def is_visited(self,x:int,y:int)->bool:
        return self.is_valid(x,y) and self.nodes[x][y].visited
    
    def is_closed(self,x:int,y:int)->bool:
        return self.is_valid(x,y) and self.nodes[x][y].closed
    

    def reset(self):
        self.nodes = [[Node(x,y) for y in range(self.width)] for x in range(self.height)]
        if self.walls is not None:
            for wall in self.walls:
                x,y = wall
                self.nodes[x][y].walkable = False

class PriorityQueue:
    def __init__(self):
        self.elements: list[tuple[float, Node]] = []
    
    def empty(self) -> bool:
        return not self.elements
    
    def put(self, item: Node, priority: float):
        heapq.heappush(self.elements, (priority, item))
    
    def get(self) -> Node:
        return heapq.heappop(self.elements)[1]
            

def generate_wall(height:int,width:int):
    result = []
    start_x = height//8
    start_y = width//8
    end_x = height//8*7
    end_y = width//8*7
    for i in range(start_y,end_y+1):
        result.append([start_x,i])
        result.append([end_x,i])
    for i in range(start_x,end_x+1):
        result.append([i,end_y])
    return result
        
def backtrace_path(end:Node)->list[list[int]]:
    path = []
    while end is not None:
        path.append((end.x,end.y))
        end = end.parent
    return path

def draw_grid(grid:Grid,path:list[tuple[int,int]]=None):
    height = grid.height
    width = grid.width
    
    fig,ax = plt.subplots(figsize=(8,8))
    #ax.set_xticks([])  # 隐藏 x 轴刻度
    #ax.set_yticks([])  # 隐藏 y 轴刻度
    ax.set_xlim(-0.5, width - 0.5)  # 设置坐标轴范围
    ax.set_ylim(-0.5, height - 0.5)

    # 绘制网格
    for x in range(height):
        for y in range(width):
            if grid.is_start(x,y):
                ax.add_patch(plt.Rectangle((x - 0.5, y - 0.5), 1, 1, color="green"))
            elif grid.is_end(x,y):
                ax.add_patch(plt.Rectangle((x - 0.5, y - 0.5), 1, 1, color="red"))
            elif grid.is_closed(x,y):
                ax.add_patch(plt.Rectangle((x - 0.5, y - 0.5), 1, 1, color="#afeeee"))
            elif grid.is_visited(x,y):
                ax.add_patch(plt.Rectangle((x - 0.5, y - 0.5), 1, 1, color="#98fb98"))
            elif grid.is_walkable(x,y):
                ax.add_patch(plt.Rectangle((x - 0.5, y - 0.5), 1, 1, color="white"))  # 墙壁
            else:
                ax.add_patch(plt.Rectangle((x - 0.5, y - 0.5), 1, 1, color="black"))  # 通道

    # 绘制网格线
    for x in range(width + 1):
        ax.plot([x - 0.5, x - 0.5], [-0.5, height - 0.5], color="gray", linewidth=0.5)
    for y in range(height + 1):
        ax.plot([-0.5, width - 0.5], [y - 0.5, y - 0.5], color="gray", linewidth=0.5)

    # 绘制路径
    if path:
        path_x, path_y = zip(*path)  # 将路径坐标拆分为 x 和 y
        ax.plot(path_x, path_y, color="blue", linewidth=2, marker="o", markersize=6)

    plt.show()




g = Grid(20,20,generate_wall(20,20))
draw_grid(g)

最佳优先算法其执行结果如下，浅蓝色和浅绿色为其进行搜索过节点，蓝色为算法求得的最短路线。该算法的优点是可以快速找到一条路径，但是这条路径不一定是最优路径，因为它只是根据评价函数来判断哪个节点更优先进行探索，而没有考虑到整体的路径成本。

@timer
def best_first_search(grid:Grid,start_x,start_y,end_x,end_y):
    pq = PriorityQueue()
    start:Node = grid.nodes[start_x][start_y]
    pq.put(start,0)
    start.visited = True
    while not pq.empty():
        node = pq.get()
        node.closed = True
        if grid.is_end(node.x,node.y):
            return backtrace_path(grid.get_end_node())
        for x,y in grid.get_neighbors(node.x,node.y):
            neighbor = grid.nodes[x][y]

            if neighbor.closed:
                continue

            if not neighbor.visited:
                neighbor.visited = True
                neighbor.parent = node
                h = abs(x-end_x)+abs(y-end_y) # 启发函数
                neighbor.h = h
                pq.put(neighbor,h)

    return []

grid:Grid = Grid(20,20,generate_wall(20,20))
start_x,start_y = grid.get_start()
end_x,end_y = grid.get_end()
path = best_first_search(grid,start_x,start_y,end_x,end_y)
draw_grid(grid,path)
    

best_first_search耗时:0.0009s

Dijkstra 算法

Dijkstra 算法是一种贪心算法，用于计算图中的最短路径。它的基本思想是从起点开始，逐步扩大搜索范围，直到找到终点为止。Dijkstra 算法是一种动态规划算法。其先找到相邻节点的最短路径，再基于该路径继续寻找其他节点的最短路径，其伪代码如下所示。

function Dijkstra(Graph, source):
   // Initialize distances to all nodes as infinity, and to the source node as 0.


    distances = map(all nodes -> infinity)


    distances = 0


   // Initialize an empty set of visited nodes and a priority queue to keep track of the nodes to visit.
   visited = empty set
   queue = new PriorityQueue()
   queue.enqueue(source, 0)


   // Loop until all nodes have been visited.
   while queue is not empty:
       // Dequeue the node with the smallest distance from the priority queue.
       current = queue.dequeue()


       // If the node has already been visited, skip it.
       if current in visited:
           continue


       // Mark the node as visited.
       visited.add(current)


       // Check all neighboring nodes to see if their distances need to be updated.
       for neighbor in Graph.neighbors(current):
           // Calculate the tentative distance to the neighbor through the current node.
           tentative_distance = distances[current] + Graph.distance(current, neighbor)


           // If the tentative distance is smaller than the current distance to the neighbor, update the distance.
           if tentative_distance < distances[neighbor]:
               distances[neighbor] = tentative_distance


               // Enqueue the neighbor with its new distance to be considered for visitation in the future.
               queue.enqueue(neighbor, distances[neighbor])


   // Return the calculated distances from the source to all other nodes in the graph.
   return distances

下图为 Dijkstra 算法的执行结果，浅蓝色和浅绿色为其进行搜索过节点，蓝色为算法求得的最短路线。Dijkstra 算法的优点是可以找到最短路径，但是可以看到其遍历的节点相较于最佳优先算法要多很多，因为它是根据起点到每个节点的距离来进行搜索的，而不是根据评价函数来判断哪个节点更优先进行探索。

import math

@timer
def dijkstra(grid:Grid,start_x,start_y,end_x,end_y):
    pq = PriorityQueue()
    start:Node = grid.nodes[start_x][start_y]
    pq.put(start,0)
    start.visited = True
    while not pq.empty():
        node = pq.get()
        node.closed = True
        if grid.is_end(node.x,node.y):
            return backtrace_path(grid.get_end_node())
        for x,y in grid.get_neighbors(node.x,node.y):
            neighbor = grid.nodes[x][y]
            if neighbor.closed:
                continue

            ng = node.g + math.sqrt((x-node.x)**2+(y-node.y)**2)

            if (not neighbor.visited) or ng < neighbor.g:
                neighbor.g = ng
                neighbor.parent = node
                neighbor.visited = True
                pq.put(neighbor,ng)

    return []


grid.reset()
start_x,start_y = grid.get_start()
end_x,end_y = grid.get_end()
path = dijkstra(grid,start_x,start_y,end_x,end_y)
draw_grid(grid,path)

dijkstra耗时:0.0026s

A * 算法

从前面的最佳优先算法和 Dijkstra 算法可以看出，最佳优先算法需要遍历的节点数较少，但是无法保证可以找到最优路径，而 Dijkstra 算法可以找到最优路径，但是需要遍历的节点数较多。A * 算法是一种结合了最佳优先算法和 Dijkstra 算法的优点的路径规划算法。其基本思想是综合考虑起点到当前节点的距离和当前节点到终点的距离，通过评价函数来判断哪个节点更优先进行探索。其伪代码如下所示。

// A* Search Algorithm
1.  Initialize the open list
2.  Initialize the closed list
    put the starting node on the open 
    list (you can leave its f at zero)
3.  while the open list is not empty
    a) find the node with the least f on 
       the open list, call it "q"
    b) pop q off the open list
  
    c) generate q's 8 successors and set their 
       parents to q
   
    d) for each successor
        i) if successor is the goal, stop search
        
        ii) else, compute both g and h for successor
          successor.g = q.g + distance between 
                              successor and q
          successor.h = distance from goal to 
          successor (This can be done using many 
          ways, we will discuss three heuristics- 
          Manhattan, Diagonal and Euclidean 
          Heuristics)
          
          successor.f = successor.g + successor.h
        iii) if a node with the same position as 
            successor is in the OPEN list which has a 
           lower f than successor, skip this successor
        iV) if a node with the same position as 
            successor  is in the CLOSED list which has
            a lower f than successor, skip this successor
            otherwise, add  the node to the open list
     end (for loop)
  
    e) push q on the closed list
    end (while loop)

A* 算法采用了一种启发式评价函数来判断哪个节点更优先进行探索，其评价函数是 f (n) = g (n) + h (n)，其中 g (n) 是从起点到当前节点的距离，可以视为历史代价，h (n) 是启发函数算得的从当前节点到终点的距离，可以视为未来预期代价。当 g (n) 为 0 时 A* 算法就退化为了最佳优先算法，当 h (n)=0 时，A* 算法就退化为了 Dijkstra 算法。A* 算法的优点是可以找到最优路径，同时遍历的节点数也相对较少。下图为 A* 算法的执行结果，浅蓝色和浅绿色为其进行搜索过节点，蓝色为算法求得的最短路线。可以看到 A* 的算法效果与 dijkstra 算法相似，但是遍历的节点数要少很多。

$ f(n) = g(n) + h(n) $

@timer
def astar(grid:Grid,start_x,start_y,end_x,end_y,weight=1):
    pq = PriorityQueue()
    start:Node = grid.nodes[start_x][start_y]
    pq.put(start,0)
    start.visited = True
    while not pq.empty():
        node = pq.get()
        node.closed = True
        if grid.is_end(node.x,node.y):
            return backtrace_path(grid.get_end_node())
        
        for x,y in grid.get_neighbors(node.x,node.y):
            neighbor = grid.nodes[x][y]
            if neighbor.closed:
                continue

            ng = node.g + math.sqrt((x-node.x)**2+(y-node.y)**2)

            if (not neighbor.visited) or ng < neighbor.g:
                neighbor.g = ng
                h = math.sqrt((x-end_x)**2+(y-end_y)**2)
                neighbor.h = h * weight
                neighbor.parent = node
                neighbor.visited = True
                pq.put(neighbor,neighbor.f())

    return []


grid.reset()
start_x,start_y = grid.get_start()
end_x,end_y = grid.get_end()
path = astar(grid,start_x,start_y,end_x,end_y,weight=2)
draw_grid(grid,path)

astar耗时:0.0035s

启发函数 (heuristic function)

启发函数告诉 A* 算法去预估到目标点的代价，当启发函数 h (n) 为 0 时，A* 算法退化为了 Dijkstra 算法，当 A* 算法值较小时，A* 算法能够保证找到最短路径，h (n) 越小 A* 算法要遍历的节点越多，速度越慢。当 h (n) 相较于 g (n) 特别大的时候，算法退化为最佳优先算法。当 h (n) 稍大于 g (n) 时，A* 算法无法保证能够找到最短路径，但是运行速度很快。当 h (n) 与 g (n) 量级相接近时，A* 算法呢能够找到最短路径，同时遍历的节点数也相对较少，是最理想的情况。

除了 h (n) 规模意外，启发函数自身也起很重要的作用，当启发函数恰好为最佳路径的距离时，A* 算法会遍历很少的节点。当 h (n) 与 g (n) 完全匹配时，f (n) 的值不会随着路径变化，此时 A* 算法不会偏离最短路径，但是这种情况很理想的情况。下面将介绍一些启发算法

曼哈顿距离 (Manhattan Distance)

方形网格标准启发函数时曼哈顿距离，其计算方式是两点之间的横纵坐标的差的绝对值之和。其公式如下所示，比较适合作为在方形网格上四个方向上的移动的启发函数。

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

对角线距离 (Diagonal Distance)

如果允许在方形网格上进行对角线移动，那么对角线距离是一个更好的启发函数。其计算方式是两点之间的横纵坐标的最大差的绝对值。其公式如下所示。

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)

当 D=1、D2=1 时，对角线距离就是切比雪夫距离。当 D=1、D2=sqrt(2) 时，对角线距离就是 Octile 距离。

欧拉距离 (Euclidean Distance)

如果能够在任意方向上移动，那么欧拉距离是一个更好的启发函数。其计算方式是两点之间的直线距离。其公式如下所示。

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

breaking ties

在 A* 算法中，如果有多个节点的 f (n) 值相等。例如，在地形没有变化的平坦区域上，会有多条具有相同长度的最短路径，A* 算法就会探索所有具有相同 f 值的路径。这种情况称为 ties。

解决这个问题的快速方法是调整 g 或者 h 值，使得 f 值不相等，A* 算法按照值进行排序，这意味着 A* 算法会优先探索具有最小 f 值的节点，不会同时探索那些 f 值相同的节点。一种打破方法是将 h 向上调整（如果向下调整会使得 A* 算法更倾向于寻找靠近起点的节点）。其公式如下所示：

heuristic *= (1.0 + p)

当有障碍物时依然需要探索寻找绕开障碍物的路径，但是越过障碍物后 A* 需要探索的节点数会减少。

此外还可以在 f 值相同的情况下，对 f 进行比较，或者在启发函数中引入确定的随机数

A* 变种

一种变种是加权 A* 算法，通过对启发函数进行甲醛可以让 A* 算法运行更快，其公式如下所示：

f(n) = g(n) + w * h(n)

前面实现的 A* 算法就是加权 A* 算法

另一变种是动态加权 A* 算法，在不同的位置会有不同的加权启发，其公式如下所示：

f(n) = g(n) + w(n) * h(n)

此外还有带宽搜索 (bandwidth)、双向搜索 (Bidirectional Search) 等变种，具体的可以看这篇斯坦福的文章

Variants of graph search

demo

这里推荐几个动态的 A * 等算法 demo，特别是斯坦福的动画做的很好，让人很容易懂

• 斯坦福 很详细易懂
• JS demo 这个 demo 有 IDA*、A*、Dijkstra 等算法实现
• JS demo 这个 demo 含有 A * 算法变体，以及各个步骤的解释

参考资料

• best-first-search-informed-search
• path finding
• Amit’s A* Pages