Python中使用K最短路径算法解决路径规划问题

import heapq

def k_shortest_paths(graph, start, end, k):
    # Dijkstra 算法求得最短路径
    shortest_paths = dijkstra_shortest_paths(graph, start, end)
    if not shortest_paths:
        return []

    # 最短路径的长度
    shortest_path_length = shortest_paths[end][0]

    # 用优先队列来保存备选路径
    pq = [(shortest_path_length, shortest_paths[end])]

    # 用于存储最终的k条最短路径
    k_shortest_paths = []

    while len(k_shortest_paths) < k and pq:
        # 弹出当前最短路径备选列表中的最短路径
        path_length, path = heapq.heappop(pq)
        current_node = path[-1]

        # 对所有还未在路径中出现的相邻节点，生成新的备选路径，并加入优先队列中
        for neighbor, weight in graph[current_node].items():
            if neighbor not in path:
                new_path = list(path)
                new_path.append(neighbor)
                new_path_length = path_length + weight
                heapq.heappush(pq, (new_path_length, new_path))

        # 如果当前路径的最后一个节点是终点，则将其保存到k_shortest_paths中
        if current_node == end:
            k_shortest_paths.append(path)

    return k_shortest_paths

def dijkstra_shortest_paths(graph, start, end):
    # 初始化距离和前驱节点
    distances = {node: float('inf') for node in graph}
    distances[start] = 0
    previous_nodes = {node: None for node in graph}

    # 用优先队列来保存未确定最短距离的节点
    pq = [(0, start)]

    while pq:
        # 取出距离起点最近的节点
        current_distance, current_node = heapq.heappop(pq)

        # 如果当前节点已经最短路径已知，则跳过
        if current_distance > distances[current_node]:
            continue

        # 遍历当前节点所有的相邻节点，并更新它们的距离和前驱节点
        for neighbor, weight in graph[current_node].items():
            distance = current_distance + weight
            if distance < distances[neighbor]:
                distances[neighbor] = distance
                previous_nodes[neighbor] = current_node
                heapq.heappush(pq, (distance, neighbor))

    # 如果终点的前驱节点为None，则说明从起点到终点不存在可达路径
    if previous_nodes[end] is None:
        return {}

    # 回溯生成最短路径
    shortest_path = []
    current_node = end
    while current_node != start:
        shortest_path.insert(0, current_node)
        current_node = previous_nodes[current_node]
    shortest_path.insert(0, start)

    return {end: (distances[end], shortest_path)}

# 构造一个无向有权图
graph = {
    'A': {'B': 6, 'C': 1},
    'B': {'A': 6, 'C': 2, 'D': 2},
    'C': {'A': 1, 'B': 2, 'D': 1},
    'D': {'B': 2, 'C': 1, 'E': 5},
    'E': {'D': 5}
}

# 求得从A到E的3条最短路径
k_shortest = k_shortest_paths(graph, 'A', 'E', 3)

# 输出结果
for i, path in enumerate(k_shortest):
    print(f"{i}: {' -> '.join(path)}")

0: A -> C -> D -> E
1: A -> C -> B -> D -> E
2: A -> C -> B -> E