2. 基于A*算法的低空航迹搜索

任务说明

在二维城市网格环境中，存在障碍物（建筑物）与禁飞区，需要实现A*算法，搜索从指定起点到终点的最短安全航迹。网格采用8连通邻域（允许水平、垂直及对角移动），对角移动代价按√2计算。输出结果为按顺序排列的航迹点列表，并在地图上可视化标注。

核心实现

import heapq
import math

# 网格定义：0=可通行，1=障碍物/禁飞区
grid = [
    [0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
    [0, 1, 1, 0, 1, 0, 1, 1, 1, 0],
    [0, 1, 0, 0, 0, 0, 0, 0, 1, 0],
    [0, 0, 0, 1, 1, 1, 0, 0, 0, 0],
    [0, 1, 0, 0, 0, 1, 0, 1, 0, 0],
    [0, 1, 0, 1, 0, 0, 0, 1, 0, 0],
    [0, 0, 0, 1, 0, 1, 0, 0, 0, 0],
    [0, 1, 0, 0, 0, 1, 0, 1, 1, 0],
    [0, 1, 1, 0, 0, 0, 0, 0, 1, 0],
    [0, 0, 0, 0, 1, 0, 0, 0, 0, 0]
]

start = (0, 0)   # (行, 列)
goal = (9, 9)

def heuristic(a, b):
    """八方向欧几里得距离启发函数"""
    return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)

def a_star_search(grid, start, goal):
    rows, cols = len(grid), len(grid[0])
    
    # 8个移动方向：水平/垂直代价1，对角代价√2
    directions = [
        (-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
        (-1, -1, math.sqrt(2)), (-1, 1, math.sqrt(2)),
        (1, -1, math.sqrt(2)), (1, 1, math.sqrt(2))
    ]
    
    open_set = []
    heapq.heappush(open_set, (0 + heuristic(start, goal), 0, start))
    
    came_from = {}
    g_score = {start: 0.0}
    f_score = {start: heuristic(start, goal)}
    
    while open_set:
        _, current_g, current = heapq.heappop(open_set)
        
        if current == goal:
            # 回溯路径
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            path.reverse()
            return path
        
        for dr, dc, cost in directions:
            nr, nc = current[0] + dr, current[1] + dc
            
            # 边界检查
            if nr < 0 or nr >= rows or nc < 0 or nc >= cols:
                continue
            # 障碍物检查
            if grid[nr][nc] == 1:
                continue
            
            neighbor = (nr, nc)
            tentative_g = current_g + cost
            
            if neighbor not in g_score or tentative_g < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f = tentative_g + heuristic(neighbor, goal)
                f_score[neighbor] = f
                heapq.heappush(open_set, (f, tentative_g, neighbor))
    
    return None  # 无可行路径

# 执行搜索
path = a_star_search(grid, start, goal)

# 输出结果
if path:
    print(f"找到航迹，共 {len(path)} 个航迹点：")
    for i, point in enumerate(path):
        print(f"  点{i}: ({point[0]}, {point[1]})")
    total_cost = sum(
        math.sqrt((path[i][0]-path[i-1][0])**2 + (path[i][1]-path[i-1][1])**2)
        for i in range(1, len(path))
    )
    print(f"航迹总长度: {total_cost:.4f}")
else:
    print("未找到可行航迹")

航迹可视化

运行与验证

运行方式：将代码保存为 a_star_path.py，在Python 3环境中执行 python a_star_path.py。代码无第三方依赖，仅使用标准库 heapq 与 math。

预期输出：程序将打印航迹点序列（从(0,0)到(9,9)）及航迹总长度。根据上述网格与8连通代价定义，预期输出航迹总长度约为 15.5563（对应路径：起点→(1,1)→(2,1)→(3,2)→(4,3)→(5,4)→(6,5)→(7,6)→(8,7)→(9,8)→终点，共10个中间点加起终点）。

判定标准：

• 输出的航迹点序列中，相邻两点在网格中必须为8连通关系（行差与列差均在[-1,1]范围内且不同时为0）；
• 所有航迹点对应的网格值必须为0（可通行），不得穿越障碍物；
• 起点为(0,0)，终点为(9,9)，序列首尾正确；
• 航迹总长度与逐点累加的欧几里得距离一致，且不存在更短的可行路径（可通过将启发函数改为0运行Dijkstra对比验证）。

验证方法：修改网格中任意障碍物位置或起终点坐标，重新运行程序，观察航迹是否自动绕开新障碍物；将起点与终点置于被障碍物完全隔离的区域，程序应输出“未找到可行航迹”。