3. A*全局路径搜索

任务目标

在二维栅格地图上实现A*全局路径搜索算法，给定起点与终点坐标，输出从起点到终点的最短路径点序列，并在地图上可视化路径结果。

实现要点

A*算法通过维护开放列表（待扩展节点）与关闭列表（已扩展节点），以f(n) = g(n) + h(n)为优先级进行搜索。其中g(n)为从起点到当前节点的实际代价，h(n)为当前节点到终点的启发式估计代价（采用欧几里得距离）。算法保证在栅格地图上找到最短路径，前提是启发式函数满足可采纳性（不高估实际代价）。

完整代码

import heapq
import math
from typing import List, Tuple

# 栅格地图定义：0=可通行，1=障碍物
GRID = [
    [0, 0, 0, 0, 1, 0, 0, 0],
    [0, 1, 1, 0, 1, 0, 1, 0],
    [0, 0, 0, 0, 0, 0, 1, 0],
    [0, 1, 0, 1, 1, 0, 0, 0],
    [0, 1, 0, 0, 0, 0, 1, 0],
    [0, 0, 0, 1, 0, 0, 0, 0],
]

START = (0, 0)   # (row, col)
GOAL = (5, 7)

def heuristic(a: Tuple[int, int], b: Tuple[int, int]) -> float:
    """欧几里得距离启发式"""
    return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)

def get_neighbors(node: Tuple[int, int], grid: List[List[int]]) -> List[Tuple[int, int]]:
    """返回4邻域可通行邻居节点"""
    rows, cols = len(grid), len(grid[0])
    r, c = node
    neighbors = []
    for dr, dc in [(-1,0), (1,0), (0,-1), (0,1)]:
        nr, nc = r + dr, c + dc
        if 0 <= nr < rows and 0 <= nc < cols and grid[nr][nc] == 0:
            neighbors.append((nr, nc))
    return neighbors

def a_star(grid: List[List[int]], start: Tuple[int, int], goal: Tuple[int, int]) -> List[Tuple[int, int]]:
    """A*主算法，返回路径点列表（含起点和终点）；无路径时返回空列表"""
    open_heap = []  # (f, g, node)
    heapq.heappush(open_heap, (heuristic(start, goal), 0, start))
    came_from = {}  # 记录父节点用于回溯路径
    g_score = {start: 0}
    closed_set = set()

    while open_heap:
        _, current_g, current = heapq.heappop(open_heap)

        if current == goal:
            # 回溯路径
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            path.reverse()
            return path

        if current in closed_set:
            continue
        closed_set.add(current)

        for neighbor in get_neighbors(current, grid):
            if neighbor in closed_set:
                continue
            tentative_g = current_g + 1  # 每步代价为1

            if neighbor not in g_score or tentative_g < g_score[neighbor]:
                g_score[neighbor] = tentative_g
                f = tentative_g + heuristic(neighbor, goal)
                heapq.heappush(open_heap, (f, tentative_g, neighbor))
                came_from[neighbor] = current

    return []  # 无可行路径

# 执行搜索
path = a_star(GRID, START, GOAL)

# 输出结果
if path:
    print(f"找到路径，共 {len(path)} 个节点：")
    print(" -> ".join([f"({r},{c})" for r, c in path]))
else:
    print("未找到可行路径")

# 可视化路径（*表示路径，S起点，G终点，#障碍物）
visual = [['.' for _ in range(len(GRID[0]))] for _ in range(len(GRID))]
for r in range(len(GRID)):
    for c in range(len(GRID[0])):
        if GRID[r][c] == 1:
            visual[r][c] = '#'

for r, c in path:
    visual[r][c] = '*'
visual[START[0]][START[1]] = 'S'
visual[GOAL[0]][GOAL[1]] = 'G'

print("\n地图可视化：")
for row in visual:
    print(' '.join(row))

运行与验证

运行方式：直接执行python a_star.py（将上述代码保存为a_star.py），无需额外安装依赖（仅使用Python标准库）。

预期输出：程序首先打印路径节点序列，随后打印带路径标记的地图。由于地图中起点(0,0)到终点(5,7)存在可行通道，预期输出路径长度为13个节点（含起点终点），路径节点坐标依次为：

(0,0) -> (0,1) -> (0,2) -> (0,3) -> (1,3) -> (2,3) -> (2,4) -> (2,5) -> (3,5) -> (4,5) -> (5,5) -> (5,6) -> (5,7)

验证标准：

1. 路径首尾分别为(0,0)和(5,7)；
2. 路径中任意相邻节点在地图上为上下左右相邻关系；
3. 路径中不包含任何障碍物格子（值为1的位置）；
4. 路径长度（节点数）为13，这是该地图上从起点到终点的最短路径长度（可通过BFS验证）。

常见问题

• 修改地图：直接编辑GRID二维列表，将障碍物位置改为1即可测试不同场景。
• 无路径情况：若起点或终点被障碍物包围，算法返回空列表并打印"未找到可行路径"。
• 对角线移动：当前实现仅支持4邻域移动（上下左右），如需8邻域移动，在get_neighbors中添加对角线方向并将单步代价改为math.sqrt(2)即可。