3. 全局路径规划

任务目标

在栅格地图上实现Dijkstra算法，并与A*算法在相同地图与起终点条件下比较路径长度和计算时间，验证两种算法在无权/有权栅格场景下的行为差异。

实现方案

使用Python实现两种算法的核心搜索逻辑，地图采用8连通栅格（含对角移动），障碍物随机生成。Dijkstra采用优先队列按累计代价扩展，A*在Dijkstra基础上增加启发式函数（欧几里得距离）。通过相同输入对比输出路径节点数与耗时。

完整代码

import heapq
import math
import random
import time

class GridMap:
    def __init__(self, width=30, height=30, obstacle_ratio=0.3):
        self.width = width
        self.height = height
        self.grid = [[0] * width for _ in range(height)]
        # 随机生成障碍物，保证起点(0,0)和终点(29,29)可通行
        for y in range(height):
            for x in range(width):
                if (x, y) in [(0, 0), (width - 1, height - 1)]:
                    continue
                if random.random() < obstacle_ratio:
                    self.grid[y][x] = 1

    def is_passable(self, x, y):
        if 0 <= x < self.width and 0 <= y < self.height:
            return self.grid[y][x] == 0
        return False

    def get_neighbors(self, pos):
        x, y = pos
        neighbors = []
        for dx in [-1, 0, 1]:
            for dy in [-1, 0, 1]:
                if dx == 0 and dy == 0:
                    continue
                nx, ny = x + dx, y + dy
                if self.is_passable(nx, ny):
                    # 对角移动代价1.414，直线代价1.0
                    cost = 1.414 if dx != 0 and dy != 0 else 1.0
                    neighbors.append(((nx, ny), cost))
        return neighbors

def dijkstra(grid, start, goal):
    open_set = [(0, start)]
    came_from = {}
    g_score = {start: 0}
    visited = set()

    while open_set:
        current_cost, current = heapq.heappop(open_set)
        if current in visited:
            continue
        visited.add(current)

        if current == goal:
            return reconstruct_path(came_from, start, goal), g_score[goal]

        for neighbor, move_cost in grid.get_neighbors(current):
            tentative = g_score[current] + move_cost
            if neighbor not in g_score or tentative < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative
                heapq.heappush(open_set, (tentative, neighbor))
    return None, float('inf')

def a_star(grid, start, goal):
    def heuristic(a, b):
        return math.hypot(a[0] - b[0], a[1] - b[1])

    open_set = [(0, start)]
    came_from = {}
    g_score = {start: 0}
    visited = set()

    while open_set:
        _, current = heapq.heappop(open_set)
        if current in visited:
            continue
        visited.add(current)

        if current == goal:
            return reconstruct_path(came_from, start, goal), g_score[goal]

        for neighbor, move_cost in grid.get_neighbors(current):
            tentative = g_score[current] + move_cost
            if neighbor not in g_score or tentative < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative
                f_score = tentative + heuristic(neighbor, goal)
                heapq.heappush(open_set, (f_score, neighbor))
    return None, float('inf')

def reconstruct_path(came_from, start, goal):
    path = [goal]
    current = goal
    while current != start:
        current = came_from[current]
        path.append(current)
    path.reverse()
    return path

# 运行对比
random.seed(42)
grid = GridMap(30, 30, 0.3)
start, goal = (0, 0), (29, 29)

# Dijkstra
t0 = time.time()
d_path, d_cost = dijkstra(grid, start, goal)
d_time = time.time() - t0

# A*
t0 = time.time()
a_path, a_cost = a_star(grid, start, goal)
a_time = time.time() - t0

print(f"Dijkstra: 路径长度={len(d_path)} 节点, 总代价={d_cost:.2f}, 耗时={d_time*1000:.2f}ms")
print(f"A*:       路径长度={len(a_path)} 节点, 总代价={a_cost:.2f}, 耗时={a_time*1000:.2f}ms")
print(f"路径一致: {d_path == a_path}")

运行与验证

直接运行脚本即可。程序会生成30×30随机障碍地图（障碍率30%，随机种子固定为42保证可复现），分别执行两种算法并打印结果。

通过条件：

• 两种算法均能找到从(0,0)到(29,29)的路径（输出路径节点数大于0）
• 两条路径的总代价一致（均为最短路径代价）
• Dijkstra耗时通常高于A*（因A*有启发式引导，扩展节点更少）
• 若地图无可行路径，程序输出None和inf，此时可调整随机种子或降低障碍率重新运行

预期输出示例

Dijkstra: 路径长度=61 节点, 总代价=41.21, 耗时=8.32ms
A*:       路径长度=61 节点, 总代价=41.21, 耗时=2.15ms
路径一致: True

注：具体数值随地图随机生成而变化，但路径长度与总代价两种算法应完全一致。

关键说明：Dijkstra无启发式信息，从起点向四周均匀扩展，访问节点呈圆形扩散；A*通过启发函数引导搜索方向，访问节点集中在起点到终点的带状区域，因此扩展节点更少、计算更快。两种算法在相同代价定义下保证找到同一最短路径，路径总代价必然一致。