【路径规划】二维Dijkstra启发式改进算法

我们使用了A*算法的启发式（曼哈顿距离）来改进Dijkstra算法的性能。当我们将邻居节点添加到优先队列时，我们使用了distance + heuristic作为优先级，这样我们可以更快地找到通往目标节点的路径。

import heapq
import numpy as np


def heuristic(a, b):
    (x1, y1) = a
    (x2, y2) = b
    return abs(x1 - x2) + abs(y1 - y2)  # 使用曼哈顿距离作为启发式


def dijkstra_with_a_star_heuristic(graph, start, end):
    # 初始化距离字典，将所有节点设置为无穷大，除了起点
    distances = {position: float('infinity') for position in np.ndindex(graph.shape)}
    distances[start] = 0

    # 优先队列，存储待检查的节点和它们的距离
    pq = [(0, start)]

    while pq:
        # 弹出当前最小距离的节点
        current_distance, current_position = heapq.heappop(pq)

        # 如果已经找到更短的路径到当前节点，则跳过
        if current_distance > distances[current_position]:
            continue

        # 如果到达目标节点，返回路径（这里未实现路径重构）
        if current_position == end:
            return distances[current_position]  # 这里只返回距离，路径重构需要额外工作

        # 遍历当前节点的邻居
        for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
            x2, y2 = current_position[0] + dx, current_position[1] + dy

            # 检查邻居是否在网格内且不是障碍物
            if 0 <= x2 < graph.shape[0] and 0 <= y2 < graph.shape[1] and graph[x2, y2] != 1:
                # 计算到达邻居节点的距离
                distance = current_distance + 1

                # 使用启发式来改进Dijkstra的选择
                priority = distance + heuristic((x2, y2), end)

                # 如果找到更短的路径到邻居节点，则更新距离并添加到优先队列中
                if priority < distances[(x2, y2)]:
                    distances[(x2, y2)] = priority
                    heapq.heappush(pq, (priority, (x2, y2)))

    # 如果没有找到路径到目标节点
    return None
if __name__ == '__main__':


    # 示例用法
    graph = np.array([
        [0, 0, 0, 1, 0],
        [1, 1, 0, 1, 0],
        [0, 0, 0, 0, 0],
        [0, 1, 1, 1, 1],
        [0, 0, 0, 0, 0]
    ])
    start = (0, 0)
    end = (4, 4)

    distance = dijkstra_with_a_star_heuristic(graph, start, end)
    print(f"The shortest distance from {start} to {end} is: {distance}")