（2-2）A*算法：A*算法的核心思想_a*算法的原理与思想-CSDN博客

本文链接：https://blog.csdn.net/asd343442/article/details/140218248

2.2 A*算法的核心思想

A*算法的核心思想是结合实际代价和启发式估计，以高效地搜索图形中的最优路径。通过在评估函数中权衡实际代价和启发式估计，A*算法能够在保证找到最优路径的同时，尽可能减小搜索的时间和空间开销。这使得A*算法成为解决路径规划问题的一种高效而灵活的算法。

2.2.1 A*算法的原理和实现步骤

A*算法的独特之处在于使用启发式估计来引导搜索，从而减少搜索空间，提高效率。A*算法的原理如下所示。

启发式搜索：A*算法是一种启发式搜索算法，利用启发式估计（heuristic estimation）来引导搜索过程。启发式函数h(n)用于估计从当前节点n到目标节点的代价。
综合实际代价和估计代价：A算法综合考虑两个代价：实际代价g(n)，表示从起点到当前节点的实际路径代价，和估计代价h(n)，表示从当前节点到目标节点的启发式估计路径代价。A使用评估函数f(n) = g(n) + h(n) 来确定搜索的优先级。
优先级队列：A*使用一个开放列表（Open List）来存储待考察的节点，按照评估函数f(n)值的优先级排列。在每一步中，选择开放列表中具有最小f(n)值的节点进行探索。
避免重复搜索：通过维护一个关闭列表（Closed List）来避免对已经考察过的节点进行重复搜索，确保算法不会陷入无限循环。

A*算法的实现步骤如下所示。

（1）初始化：将起点加入开放列表（Open List），将关闭列表（Closed List）置为空。

（2）循环直到找到最优路径或开放列表为空：

从开放列表中选择具有最小评估函数f(n)值的节点作为当前节点，将当前节点移到关闭列表。
如果当前节点是目标节点，则路径已找到，进行路径追踪。否则，探索当前节点的邻近节点。

（3）探索邻近节点的，对于当前节点的每个邻近节点：

如果邻近节点在关闭列表中，忽略它。
如果邻近节点不在开放列表中，将其加入开放列表，计算g(n)、h(n)和f(n)值，并设置当前节点为其父节点。
如果邻近节点已经在开放列表中，检查通过当前节点到达邻近节点的路径是否更优，如果是则更新邻近节点的g(n)值和父节点。

（4）路径追踪：当目标节点加入关闭列表，从目标节点沿着父节点一直追溯到起点，构建最短路径。

在实际编程过程中，需要注意以下几点：

数据结构的选择：使用合适的数据结构来表示节点、开放列表和关闭列表，以便高效地进行插入、删除和查找操作。
启发式函数的设计：根据具体问题设计合适的启发式函数，以提高搜索效率。
边界条件的处理：考虑起点和目标点是否可通行，以及处理可能的边界情况。

请看下面的实例，功能是在城市街道网格上规划车辆的路径。在这个例子中，假设网格代表城市街道，其中0表示道路，1表示障碍物（如建筑物）。将节点类扩展以包含其他与自动驾驶相关的信息，如速度限制和交通信号。用户输入起点和终点的坐标，然后使用A*算法计算最短路径并在控制台输出路径信息。

实例2-1：使用A*算法计算最短路径（codes/2/easy.py）

实例文件easy.py的具体实现代码如下所示。

import heapq
import numpy as np
import matplotlib.pyplot as plt

class Node:
    def __init__(self, x, y, parent=None, g=0, h=0, speed=1):
        self.x = x
        self.y = y
        self.parent = parent
        self.g = g  					# 到达该节点的成本
        self.h = h  					# 到达目标的启发式成本
        self.speed = speed  			# 该节点的速度限制

    def f(self):
        return self.g + self.h
    
    def __lt__(self, other):
        return self.f() < other.f()

def heuristic(node, goal):
    return abs(node.x - goal.x) + abs(node.y - goal.y)

def astar(start, goal, grid, speeds):
    open_list = []
    closed_set = set()

    heapq.heappush(open_list, start)
    
    while open_list:
        current_node = heapq.heappop(open_list)

        if current_node.x == goal.x and current_node.y == goal.y:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return path[::-1]
        
        closed_set.add((current_node.x, current_node.y))

        for dx, dy in [(1, 0), (-1, 0), (0, 1), (0, -1)]:
            new_x, new_y = current_node.x + dx, current_node.y + dy

            if 0 <= new_x < len(grid) and 0 <= new_y < len(grid[0]) and grid[new_x][new_y] == 0 and (new_x, new_y) not in closed_set:
                speed = speeds[new_x][new_y]
                new_node = Node(new_x, new_y, current_node, current_node.g + (1 / speed), heuristic(Node(new_x, new_y), goal), speed)
                heapq.heappush(open_list, new_node)

    return None

def visualize_path(grid, path):
    grid = np.array(grid)
    for x, y in path:
        grid[x][y] = 2
    plt.imshow(grid, cmap='viridis', interpolation='nearest')
    plt.title('Shortest Path')
    plt.show()

def main():
    # Define the grid: 0 = road, 1 = obstacle
    grid = [
        [0, 0, 0, 0, 0, 0],
        [0, 1, 1, 1, 1, 0],
        [0, 0, 0, 0, 0, 0],
        [0, 1, 1, 1, 1, 0],
        [0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0]
    ]

    # Define the speeds: speed of travel through each cell
    speeds = [
        [1, 1, 1, 1, 1, 1],
        [1, 0, 0, 0, 0, 1],
        [1, 1, 1, 1, 1, 1],
        [1, 0, 0, 0, 0, 1],
        [1, 1, 1, 1, 1, 1],
        [1, 1, 1, 1, 1, 1]
    ]

    start_x, start_y = map(int, input("Enter start coordinates (x y): ").split())
    goal_x, goal_y = map(int, input("Enter goal coordinates (x y): ").split())

    start_node = Node(start_x, start_y)
    goal_node = Node(goal_x, goal_y)

    path = astar(start_node, goal_node, grid, speeds)
    if path:
        print("Shortest Path:", path)
        visualize_path(grid, path)
    else:
        print("No path found.")

if __name__ == "__main__":
    main()

上述代码的实现流程如下所示：

（1）首先，定义了一个节点类 (Node)，每个节点包含坐标、父节点、实际代价、启发式代价和速度限制；

（2）然后，定义启发式函数 (heuristic)，使用曼哈顿距离计算节点到目标节点的估计代价，并实现A算法 (astar)，通过优先队列和集合管理节点的处理，考虑速度限制计算实际代价，在网格中找到从起点到目标的最短路径.

（3）最后，定义路径可视化函数 (visualize_path)，将找到的路径在网格上显示出来，并在主程序中初始化网格和速度限制，获取用户输入，运行A算法，并展示结果。执行后会打印输出下面的结果，并绘制如图2-8所示的路径可视化图。

Enter start coordinates (x y): 1 1
Enter goal coordinates (x y): 4 4
Shortest Path: [(1, 1), (2, 1), (2, 2), (2, 3), (2, 4), (2, 5), (3, 5), (4, 5), (4, 4)]

图2-8 路径可视化图

2.2.2 选择启发式函数（估算函数）

选择适当的启发式函数对A*算法的性能至关重要，启发式函数（heuristic function）用于估计从当前节点到目标节点的代价，以指导搜索过程。合理的启发式函数应该在尽量短的时间内提供较为准确的路径代价估计。下面列出了选择启发式函数时的一些常见策略。

（1）曼哈顿距离（Manhattan Distance）

适用于方格网格图形。
计算当前节点到目标节点的水平和垂直距离之和。
在城市街区网格中常用，但不适用于对角线移动。

（2）欧几里得距离（Euclidean Distance）

适用于连续空间的图形。
计算当前节点到目标节点的直线距离。
可能导致在方格网格上过于乐观的估计。

（3）对角线距离

考虑对角线移动，是曼哈顿距离和欧几里得距离的一种折中。
计算水平、垂直和对角线距离的权衡值。

（4）最大方向距离

在方格网格中，计算水平和垂直距离中较大的一个。
避免了对角线移动的过于乐观估计。

（5）自定义启发式函数

根据问题的特性设计特定的启发式函数。
可以利用问题的领域知识来提高估计的准确性。

请看下面的例子，实现了一个城市街道网格，其中每个位置有速度限制，通过使用优先队列和启发式函数（曼哈顿距离）遍历邻居节点并选择代价最小的路径。

实例2-2：使用优先队列和曼哈顿距离寻找最小的路径（codes/2/qi.py）

实例文件qi.py的具体实现代码如下所示。

import heapq
import matplotlib.pyplot as plt
import numpy as np

class Node:
    def __init__(self, x, y, parent=None, g=0, h=0, speed=1):
        self.x = x
        self.y = y
        self.parent = parent
        self.g = g  # cost to reach this node
        self.h = h  # heuristic cost to reach goal
        self.speed = speed  # speed limit at this node

    def f(self):
        return self.g + self.h

    def __lt__(self, other):
        return self.f() < other.f()

def heuristic(node, goal):
    return abs(node.x - goal.x) + abs(node.y - goal.y)

def astar(start, goal, grid, speeds):
    open_list = []
    closed_set = set()

    heapq.heappush(open_list, start)
    
    while open_list:
        current_node = heapq.heappop(open_list)

        if current_node.x == goal.x and current_node.y == goal.y:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return path[::-1], True
        
        closed_set.add((current_node.x, current_node.y))

        for dx, dy in [(1, 0), (-1, 0), (0, 1), (0, -1)]:
            new_x, new_y = current_node.x + dx, current_node.y + dy

            if 0 <= new_x < len(grid) and 0 <= new_y < len(grid[0]) and grid[new_x][new_y] == 0 and (new_x, new_y) not in closed_set:
                speed = speeds[new_x][new_y]
                new_node = Node(new_x, new_y, current_node, current_node.g + (1 / speed), heuristic(Node(new_x, new_y), goal), speed)
                heapq.heappush(open_list, new_node)

    return [], False

def visualize_path(grid, path, start, goal):
    grid = np.array(grid)
    for x, y in path:
        grid[x][y] = 2
    grid[start[0]][start[1]] = 3
    grid[goal[0]][goal[1]] = 4
    
    plt.imshow(grid, cmap=plt.cm.get_cmap('viridis', 5), interpolation='nearest')
    plt.colorbar(ticks=[0, 1, 2, 3, 4], format=plt.FuncFormatter(lambda val, loc: ['Road', 'Obstacle', 'Path', 'Start', 'Goal'][int(val)]))
    plt.title('Shortest Path')
    plt.show()

def main():
    grid = [
        [0, 0, 1, 0, 0, 0],
        [0, 0, 1, 0, 0, 0],
        [0, 0, 0, 0, 1, 0],
        [0, 0, 1, 1, 1, 0],
        [0, 0, 0, 0, 0, 0]
    ]

    speeds = [
        [1, 1, 0, 1, 1, 1],
        [1, 1, 0, 1, 1, 1],
        [1, 1, 1, 1, 0, 1],
        [1, 1, 0, 0, 0, 1],
        [1, 1, 1, 1, 1, 1]
    ]

    start_x, start_y = 0, 0
    goal_x, goal_y = 4, 5

    start_node = Node(start_x, start_y)
    goal_node = Node(goal_x, goal_y)

    path, found = astar(start_node, goal_node, grid, speeds)
    if found:
        print("Shortest Path:", path)
        visualize_path(grid, path, (start_x, start_y), (goal_x, goal_y))
    else:
        print("No path found.")

if __name__ == "__main__":
    main()

上述代码的实现流程如下所示：

（1）首先，定义了一个节点类（Node），每个节点包含坐标、父节点、实际代价、启发式代价和速度限制。

（2）然后，定义启发式函数（heuristic），使用曼哈顿距离计算节点到目标节点的估计代价，并实现A算法（astar），通过优先队列和集合管理节点的处理，考虑速度限制计算实际代价，在网格中找到从起点到目标的最短路径。

（3）定义路径可视化函数（visualize_path），将路径、起点和终点分别标记不同的颜色，并在图例中说明，确保可视化效果清晰明了。

（4）主在程序中初始化网格和速度限制，获取用户输入，运行A算法，并展示结果。执行后会打印输出如下找到的最短路径，并绘制如图2-8所示的路径可视化图。

Shortest Path: [(0, 0), (1, 0), (2, 0), (3, 0), (3, 1), (4, 1), (4, 2), (4, 3), (4, 4), (4, 5)]

图2-8 路径可视化图

注意：在选择启发式函数时，需要权衡准确性和计算效率。过于简单的启发式函数可能导致算法收敛速度慢，而过于复杂的函数可能使算法变得过于计算密集。在实践应用中，不同问题可能需要尝试不同的启发式函数，以找到最适合问题特性的估计方法。