我们使用了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}")
  • 1.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 7.
  • 8.
  • 9.
  • 10.
  • 11.
  • 12.
  • 13.
  • 14.
  • 15.
  • 16.
  • 17.
  • 18.
  • 19.
  • 20.
  • 21.
  • 22.
  • 23.
  • 24.
  • 25.
  • 26.
  • 27.
  • 28.
  • 29.
  • 30.
  • 31.
  • 32.
  • 33.
  • 34.
  • 35.
  • 36.
  • 37.
  • 38.
  • 39.
  • 40.
  • 41.
  • 42.
  • 43.
  • 44.
  • 45.
  • 46.
  • 47.
  • 48.
  • 49.
  • 50.
  • 51.
  • 52.
  • 53.
  • 54.
  • 55.
  • 56.
  • 57.
  • 58.
  • 59.
  • 60.
  • 61.
  • 62.
  • 63.
  • 64.
  • 65.