2. 全局路径规划

任务目标

在二维栅格地图上实现A*寻路算法，给定起点与终点坐标，输出从起点到终点的最短路径点序列（含起点与终点）。地图中障碍物以栅格值标记，路径规划需绕开障碍物。

实现方案

采用Python实现，使用优先队列（heapq）管理开放集，曼哈顿距离作为启发函数（四方向移动）。地图以二维列表表示，0为可通行，1为障碍。核心逻辑包括：节点代价记录、邻居扩展、路径回溯。代码自包含，不依赖第三方库。

关键完整代码

import heapq

def astar(grid, start, goal):
    """A*算法主函数
    grid: 二维列表，0可通行，1障碍
    start: (row, col) 起点
    goal: (row, col) 终点
    返回: 路径点列表[(r,c),...]；无路径返回None
    """
    rows, cols = len(grid), len(grid[0])
    # 合法性检查
    if not (0 <= start[0] < rows and 0 <= start[1] < cols) or \
       not (0 <= goal[0] < rows and 0 <= goal[1] < cols):
        return None
    if grid[start[0]][start[1]] == 1 or grid[goal[0]][goal[1]] == 1:
        return None

    # 四方向移动
    dirs = [(-1,0),(1,0),(0,-1),(0,1)]

    # g_cost: 起点到该节点的实际代价
    g_cost = {start: 0}
    # parent: 记录路径回溯
    parent = {start: None}

    def heuristic(a, b):
        """曼哈顿距离"""
        return abs(a[0]-b[0]) + abs(a[1]-b[1])

    # 优先队列元素: (f_cost, g_cost, node)
    open_heap = [(heuristic(start, goal), 0, start)]
    closed_set = set()

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

        if current == goal:
            # 回溯路径
            path = []
            node = current
            while node is not None:
                path.append(node)
                node = parent[node]
            path.reverse()
            return path

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

        for dr, dc in dirs:
            nr, nc = current[0]+dr, current[1]+dc
            if 0 <= nr < rows and 0 <= nc < cols and grid[nr][nc] == 0:
                neighbor = (nr, nc)
                tentative_g = current_g + 1
                if neighbor not in g_cost or tentative_g < g_cost[neighbor]:
                    g_cost[neighbor] = tentative_g
                    parent[neighbor] = current
                    f_cost = tentative_g + heuristic(neighbor, goal)
                    heapq.heappush(open_heap, (f_cost, tentative_g, neighbor))

    return None  # 无可行路径

# ========== 最小验证示例 ==========
if __name__ == "__main__":
    # 5x5栅格地图，1为障碍
    grid_map = [
        [0, 0, 0, 0, 0],
        [0, 1, 1, 1, 0],
        [0, 0, 0, 1, 0],
        [0, 1, 0, 0, 0],
        [0, 0, 0, 0, 0]
    ]
    start = (0, 0)
    goal = (4, 4)

    result = astar(grid_map, start, goal)
    if result:
        print("路径点序列:")
        for p in result:
            print(p)
    else:
        print("未找到可行路径")

运行与验证

运行方式：将上述代码保存为 astar_demo.py，在命令行执行 python astar_demo.py。

预期输出：由于地图中存在障碍，A*会绕开障碍物，输出路径点序列。根据代码逻辑可唯一推出以下路径（从起点到终点）：

(0, 0) -> (1, 0) -> (2, 0) -> (2, 1) -> (2, 2) -> (3, 2) -> (3, 3) -> (3, 4) -> (4, 4)

验证方法：运行程序后，观察输出是否包含上述9个路径点且顺序一致。可手动修改 grid_map 中的障碍位置或起点终点坐标，验证算法能正确绕障或返回 None（当起点或终点被障碍包围时）。

常见问题

• 路径非最短：检查启发函数是否满足一致性（曼哈顿距离在四方向移动下满足），若使用欧氏距离可能导致次优路径。
• 死循环或内存溢出：确认 closed_set 正确跳过已访问节点，且邻居坐标边界检查完整。
• 起点等于终点：代码会直接返回包含单个点的路径，符合预期。