2. 实现基于规则的路径规划器

任务目标

实现一个基于A*算法的路径规划器，在二维栅格地图上搜索从起点到终点的最短可行路径，并输出路径点序列。地图中障碍物为静态已知，路径允许四方向移动（上、下、左、右）。

实现方案

使用Python实现A*算法核心逻辑。地图以二维列表表示，0为可通行，1为障碍物。启发函数采用曼哈顿距离（四方向移动下保证最优性）。通过优先队列（heapq）维护待扩展节点，使用字典记录父节点以回溯路径。最终输出从起点到终点的坐标点列表。

完整代码

import heapq

def astar_path(grid, start, goal):
    """
    基于A*算法的路径规划器
    :param grid: 二维列表，0=可通行，1=障碍物
    :param start: 起点坐标 (row, col)
    :param goal: 终点坐标 (row, col)
    :return: 路径点列表 [(row, col), ...]，无路径时返回空列表
    """
    rows, cols = len(grid), len(grid[0])
    
    # 检查起点和终点合法性
    if grid[start[0]][start[1]] == 1 or grid[goal[0]][goal[1]] == 1:
        return []
    
    # 四方向移动
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
    
    def heuristic(a, b):
        """曼哈顿距离启发函数"""
        return abs(a[0] - b[0]) + abs(a[1] - b[1])
    
    # open_list: (f_score, g_score, current_node)
    open_list = []
    heapq.heappush(open_list, (heuristic(start, goal), 0, start))
    
    # 记录信息
    came_from = {}          # 父节点回溯
    g_score = {start: 0}    # 起点到当前节点的实际代价
    closed_set = set()      # 已扩展节点
    
    while open_list:
        _, current_g, current = heapq.heappop(open_list)
        
        # 到达终点，回溯路径
        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 dr, dc in directions:
            nr, nc = current[0] + dr, current[1] + dc
            neighbor = (nr, nc)
            
            # 边界与障碍物检查
            if nr < 0 or nr >= rows or nc < 0 or nc >= cols:
                continue
            if grid[nr][nc] == 1:
                continue
            if neighbor in closed_set:
                continue
            
            tentative_g = current_g + 1
            
            if neighbor not in g_score or tentative_g < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f_score = tentative_g + heuristic(neighbor, goal)
                heapq.heappush(open_list, (f_score, tentative_g, neighbor))
    
    return []  # 无可行路径

if __name__ == "__main__":
    # 测试地图：0=空地，1=障碍物
    test_grid = [
        [0, 0, 0, 0, 1],
        [1, 1, 0, 1, 0],
        [0, 0, 0, 1, 0],
        [0, 1, 0, 0, 0],
        [0, 0, 0, 1, 0]
    ]
    start = (0, 0)
    goal = (4, 4)
    
    result = astar_path(test_grid, start, goal)
    print("规划路径:", result)
    print("路径长度:", len(result) - 1 if result else "无路径")

运行与验证

运行方式：将代码保存为 astar_planner.py，在终端执行 python astar_planner.py。

预期输出：基于给定测试地图，算法应输出一条从(0,0)到(4,4)的可行路径。由于地图中存在障碍物，路径会绕行，例如：[(0,0), (0,1), (0,2), (1,2), (2,2), (2,1), (2,0), (3,0), (4,0), (4,1), (4,2), (4,3), (4,4)]，路径长度为12步。

验证方法：

1. 检查输出路径中每个相邻点坐标差值之和为1（四方向移动合法）；
2. 检查路径中所有点对应地图值均为0（未穿越障碍物）；
3. 检查路径首尾分别为起点和终点；
4. 可修改test_grid中的障碍物布局或起点终点，验证算法在不同地图下的表现；将起点或终点设为障碍物位置，应返回空列表。

关键说明

本实现采用曼哈顿距离作为启发函数，在四方向移动模型中保证找到最短路径。若需八方向移动，只需扩展directions为8个方向并将启发函数改为切比雪夫距离或欧几里得距离。代码中heapq的元组排序机制确保每次优先扩展f值最小的节点，closed_set避免重复扩展，保证算法在有限地图上必然终止。