（4-2） 轨迹规划算法和优化：A*算法_a*算法实战

4.2  A*算法

A*算法（A星算法）是一种常用于图搜索和路径规划的启发式搜索算法，用于找到从起点到目标点的最短路径。该算法综合了最短路径搜索和贪婪搜索的优点，在现实中具有较高的效率和广泛的应用。

4.2.1  A*算法介绍

A*算法的基本思想是通过估计从起点到目标点的总代价（通常记为f(n)）来选择每一步的移动，该总代价由如下两个部分组成：

1. 实际已经花费的代价（g(n)）：从起点到当前节点n的实际路径代价。
2. 启发式估计的剩余代价（h(n)）：从当前节点n到目标节点的估计路径代价。这是A算法的关键之一，启发式函数要足够接近实际代价，但又要高效计算。如果启发式函数等于0，A算法就相当于Dijkstra算法。

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

（1）初始化：将起点加入开放列表（open list），并将其f值设为g值和h值的和。

（2）循环：从开放列表中选择具有最小f值的节点作为当前节点。如果当前节点是目标节点，算法结束；否则，将当前节点从开放列表中移入封闭列表（closed list），并考虑其邻居节点。

（3）邻居节点处理：对于每个邻居节点，如果它不在开放列表中，计算并设置其g值、h值和f值，并将其加入开放列表。如果邻居节点已经在开放列表中，检查新的g值是否更小，如果是，更新g值，并重新计算f值。

（4）终止条件：当目标节点被加入封闭列表，或者开放列表为空（表示无法到达目标），算法终止。

A*算法的优点在于它能够在有向图或网络中找到最短路径，并且通过合适的启发式函数，它可以在实际应用中取得较好的性能。

4.2.2  A*算法实战

下面是一个简单的使用A*算法进行轨迹规划的例子，模拟了自动驾驶场景。在例子中使用了一个简化的2D网格地图，其中包含起点、目标点和一些障碍物。使用A*算法在这个网格地图上规划路径，起点是(0, 0)，目标点是(4, 4)，并设置了一些障碍物。在实际应用中，可以根据实际需求修改地图和起点、目标点来进行测试。

实例4-1：使用A*算法进行自动驾驶的轨迹规划（源码路径：codes\4\gui\ax.py）

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

import heapq
 
class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.g = float('inf')  # 到达当前节点的实际代价
        self.h = 0  # 启发式估计的剩余代价
        self.f = 0  # 总代价
        self.parent = None  # 用于记录路径的上一节点
 
    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(grid, start, goal):
    open_list = []
    closed_set = set()
 
    start_node = Node(start[0], start[1])
    goal_node = Node(goal[0], goal[1])
 
    start_node.g = 0
    start_node.h = heuristic(start_node, goal_node)
    start_node.f = start_node.g + start_node.h
 
    heapq.heappush(open_list, start_node)
 
    while open_list:
        current_node = heapq.heappop(open_list)
 
        if current_node.x == goal_node.x and current_node.y == goal_node.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 i, j in [(0, 1), (0, -1), (1, 0), (-1, 0)]:
            neighbor = Node(current_node.x + i, current_node.y + j)
 
            if (
                neighbor.x < 0
                or neighbor.x >= len(grid)
                or neighbor.y < 0
                or neighbor.y >= len(grid[0])
                or grid[neighbor.x][neighbor.y] == 1
                or (neighbor.x, neighbor.y) in closed_set
            ):
                continue
 
            neighbor.g = current_node.g + 1
            neighbor.h = heuristic(neighbor, goal_node)
            neighbor.f = neighbor.g + neighbor.h
            neighbor.parent = current_node
 
            if neighbor not in open_list:
                heapq.heappush(open_list, neighbor)
 
    return None  # 无法到达目标点
 
# 示例地图，0表示可通行，1表示障碍物
grid = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 0, 0],
    [0, 1, 0, 0, 0],
    [0, 0, 0, 1, 0],
    [0, 0, 0, 1, 0],
]
 
start_point = (0, 0)
goal_point = (4, 4)
 
path = astar(grid, start_point, goal_point)
 
if path:
    print("找到路径:", path)
else:
    print("无法找到路径")

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

1. 首先，定义类Node表示地图中的每个节点。每个节点包含了其坐标 (x, y)，实际代价 g、启发式估计的剩余代价 h 和总代价 f，以及记录路径的上一节点 parent。
2. 接着，定义了启发式函数 heuristic，用于估算两个节点之间的启发式估计值。在这里，我们使用的具体估算方法是曼哈顿距离，即计算节点在横向和纵向上的距离之和。曼哈顿距离是一种在网格状结构中常用的距离度量方式，也被称为 L1 范数。
3. 然后，实现了 A* 算法的实现函数 astar。使用优先队列 open_list 存储待处理的节点，并使用集合 closed_set 存储已经处理过的节点。通过实例化起点节点和目标节点，并初始化起点的 g、h 和 f 值，将起点节点加入 open_list 中。
4. 接下来，算法进入主循环。在每一次循环中，从 open_list 中选择具有最小 f 值的节点作为当前节点。如果当前节点是目标节点，算法结束，构建并返回路径。否则，将当前节点加入 closed_set，并处理其邻居节点。
5. 在邻居节点处理阶段，算法检查邻居节点是否越界、是否为障碍物、是否已经在 closed_set 中。如果通过检查，计算邻居节点的 g、h 和 f 值，并设置其 parent 为当前节点。如果邻居节点不在 open_list 中，将其加入；如果已经在 open_list 中，检查新的 g 值是否更小，如果是，更新 g 值。
6. 最后，算法终止的条件是目标节点被加入 closed_set，或者 open_list 为空（表示无法到达目标）。如果终止条件满足，算法返回构建好的路径；否则，返回 None，表示无法找到路径。

执行后会输出下面的结果，这说明成功找到了从起点到目标点的路径。这个路径是从 (0, 0) 到 (4, 4) 的有效路径，并避开了障碍物。

找到路径: [(0, 0), (0, 1), (0, 2), (0, 3), (0, 4), (1, 4), (2, 4), (3, 4), (4, 4)]