重温数据结构与算法之A star 算法

前言

A*(A-Star)算法是一种静态路网中求解最短路径有效的直接搜索方法，也是解决许多搜索问题的有效算法。

A*算法属于启发式搜索算法，它结合了最佳优先(Best-First)搜索和Dijkstra算法的优点，能够快速地在图中找到一条从起点到终点的最短路径。自从1968年由 Peter Hart, Nils Nilsson 和 Bertram Raphael 提出以来，A* 算法已经成为了许多领域中最常用的路径规划算法之一。

• 最佳优先搜索通过使用启发函数来评估每个节点的优先级，从而快速地找到目标节点。
• Dijkstra算法则通过记录每个节点到起点的距离来保证找到的路径是最短的。

A* 算法结合了这两种方法，它使用一个启发函数来评估每个节点到目标节点的距离$h(n)$，并将这个值与该节点到起点的距离$g(n)$相加，从而得到一个总估计值$f(n)=g(n)+h(n)$。A*算法总是选择具有最小总估计值的节点进行扩展，因此能够快速地找到一条从起点到终点的最短路径。

此外，在游戏开发、人工智能和运筹学等领域中都有着广泛应用。

一、原理

1.1 网格距离

根据网格的拓扑结构，可选用以下三种距离，假设A与B两点横纵坐标距离分别为x 和 y：

1. 曼哈顿距离，只允许4个方向移动，AB的距离为：$x+y$

2. 对角距离，允许8方向移动，AB的距离为：$x+y+(\sqrt{2}-2)\ast min(x,y)$

3. 欧几里得距离，允许任意方向移动，AB的距离为：$\sqrt{x^2+y^2}$

下面默认曼哈顿距离

1.2 宽度优先搜索

宽度优先搜索(Breadth First Search，简称 BFS)，在我之前一篇博客中就有介绍，这是最基本的求最短路径的算法。BFS 相当于暴力搜索，这个算法在网格中搜索有2个较大的缺陷：

1. 格子之间的移动花费是均等的，实际应用中不同格子之间移动代价会不一样。
2. 它是均等的向四周搜索，即使你肉眼看到目标就在右边，但也会均匀向四周扩撒。

当碰到障碍时

1.3 Dijkstra 算法

Dijkstra 算法是由荷兰计算机科学家狄克斯特拉 于1959年提出的算法。它是从一个顶点到其余各顶点的最短路径算法，解决的是有权图中最短路径问题。Dijkstra 算法主要特点是从起始点开始，采用贪心算法的策略，每次遍历到始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止。

例如在游戏《文明》中，走平地或沙漠，花费1个移动点，但是穿越森林或山丘可能需要花费5个移动点。

1.4 最佳优先搜索

最佳优先搜索算法是一种启发式搜索算法，它使用一个启发函数，会优先选择离终点距离更近的节点。

下图表示当网格间权重都相等时，Dijkstra 算法会退化为宽度优先搜索，搜索时间更长，搜索格子更多

1.5 A*算法

上述最佳优先搜索并不一定是最短的，例如在下面场景下选择的就不是最短路径

这时候结合Dijkstra 算法 和最佳优先搜索算法的A* 就上场了，它结合了这两者的优点，它选择两个值相加（已走距离+预估距离）作为节点的优先级。

二、代码实现

2.1 伪码

frontier = PriorityQueue()
frontier.put(start, 0)
came_from = dict()
came_from[start] = None

while not frontier.empty():
   current = frontier.get()

   if current == goal:
      break
   
   for next in graph.neighbors(current):
      if next not in came_from:
         priority = heuristic(goal, next)
         frontier.put(next, priority)
         came_from[next] = current

2.2 python 实现

import numpy as np
import heapq
from collections import namedtuple
import matplotlib.pyplot as plt
from matplotlib.font_manager import FontProperties 
font_set = FontProperties(fname=r"c:\\windows\\fonts\\simsun.ttc", size=15)#导入宋体字体文件

# 15*15网格，0表示可以通过的点，而1表示障碍物
grid = np.array([[0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,0, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,0, 0, 0],
                 [0, 0, 1, 1, 1, 1,1, 1, 1, 1, 1, 1,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,1, 0, 0],
                 [0, 0, 1, 1, 1, 1,1, 1, 1, 1, 1, 1,1, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,0, 0, 0],
                 [0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0,0, 0, 0]])

Node = namedtuple('Node', ['x', 'y'])
dir = [Node(0, 1), Node(0, -1), Node(1, 0), Node(-1, 0)]
# dir = [Node(0, 1), Node(0, -1), Node(1, 0), Node(-1, 0),
#        Node(1, 1), Node(-1, -1), Node(1, -1), Node(-1, 1) ]
start = Node(12, 0)
end = Node(2, 14)


def distance(node1, node2):
    x = abs(node1.x - node2.x)
    y = abs(node1.y - node2.y)
    return x + y
    # return x + y + (2**0.5 - 2) * min(x, y)


def far_cost(node1, node2):
    return distance(node1, node2)


def a_star(grid, start, end):
    if grid[start.x][start.y] == 1 or grid[end.x][end.y] == 1:
        print("起/终点在障碍物上")
        return []
    rows, cols = grid.shape
    # open_set 是一个优先队列，用于存储待扩展的节点。每次从 open_set 中取出代价最小的节点进行扩展。
    open_set = []
    # cost 是一个字典，用于存储从起点到每个节点的代价。
    cost = {}
    cost[start] = 0
    # path 是一个字典，存储最终路径每个节点的前一个节点
    path = {}
    path[start] = None
    heapq.heappush(open_set, (0, start))

    while (open_set):
        current = heapq.heappop(open_set)[1]
        if (current == end):
            break
        for p in dir:
            newpx = current.x + p.x
            newpy = current.y + p.y
            if newpx < 0 or newpx >= rows or newpy < 0 or newpy >= cols or grid[newpx][newpy] == 1:
                continue
            newNode = Node(newpx, newpy)
            newCost = cost[current] + far_cost(current, newNode)
            
            if not newNode in cost or newCost < cost[newNode]:
                cost[newNode] = newCost
                priority = newCost + distance(newNode, end)
                heapq.heappush(open_set, (priority, newNode))
                path[newNode] = current
    if end not in path:
        print("未能到达结束点")
        return []
    shortest_path = []
    current = end
    while current != None:
        shortest_path.append(current)
        current = path[current]
    shortest_path.reverse()
    return shortest_path
nodes = a_star(grid, start ,end)
print(nodes)

使用上述代码会打印出路径所经过的节点，如下所示，但这样不太很直观查看路径，需要可视化数据

[Node(x=12, y=0), Node(x=11, y=0), Node(x=10, y=0), Node(x=9, y=0), Node(x=8, y=0), Node(x=7, y=0), Node(x=6, y=0), Node(x=5, y=0), Node(x=4, y=0), Node(x=3, y=0), Node(x=2, y=0), Node(x=2, y=1), Node(x=1, y=1), Node(x=1, y=2), Node(x=1, y=3), Node(x=1, y=4), Node(x=1, y=5), Node(x=1, y=6), Node(x=1, y=7), Node(x=1, y=8), Node(x=1, y=9), Node(x=1, y=10), Node(x=1, y=11), Node(x=1, y=12), Node(x=1, y=13), Node(x=1, y=14), Node(x=2, y=14)]

2.3 可视化

可以使用matplotlib对网格数据进行可视化

def draw(nodes):
    fig, ax = plt.subplots()
    im = ax.imshow(grid)
    rows, cols = grid.shape
  
    ax.set_xticks(np.arange(rows))
    ax.set_yticks(np.arange(cols))
 
    plt.setp(ax.get_xticklabels(), rotation_mode="anchor")

    for n in nodes:
        text = ax.text(n.y, n.x, '2', ha="center", va="center", color="blue")
    # bgColor = ['black', 'gray']
    for i in range(rows):
        for j in range(cols):
            if Node(i, j) not in nodes:
                text = ax.text(j, i, grid[i][j],
                               ha="center", va="center", color="white")#, backgroundcolor=bgColor[grid[i][j]])
    ax.set_title('A*算法', fontproperties = font_set)
    fig.tight_layout()
    plt.show()
nodes = a_star(grid, start ,end)
draw(nodes)

上述是4方向曼哈顿距离，可简单修改代码实现八方向对角距离

三、优缺点分析

3.1 优点

1. 相对于需要将所有节点展开搜寻的算法，A* 算法最大的优点就是引入了启发信息作为向目标点移动的决策辅助，所以不再需要遍历整个地图，降低了计算复杂度，减少了时间损耗少。

2. A *算法得到的是最优解是可以被理论证明的。如果得到的不是最优解，那么就是实现的时候出了什么问题，或者是没有完全按照 A * 的定义去实现

3.2 缺点

1. 实时性差，随着节点数的增多，算法搜索效率降低

2. 在不存在解的情况下（比如在寻路问题中，根本不存在一条通达的路），采用 A* 算法求解，势必会穷举所有的可能。

3. A*算法需要使用一个启发函数来估算未知节点的成本。如果启发函数不准确，可能会导致找到的路径不是最短的。

参考

1. Introduction to the A* Algorithm
2. A*算法详解 一看就会 手把手推导 完整代码注释
3. Python A*算法的简单实现
4. A*算法
5. A* 算法之误区
6. 自动驾驶路径规划：A*（Astar）算法