地图 路径规划中离线的路径算法 代码示例

以下是一个使用Dijkstra算法进行离线路径规划的示例代码：

import heapq

def dijkstra(graph, start, end):
    # 初始化距离字典，用于存储每个节点到起始节点的最短路径长度
    distance = {node: float('inf') for node in graph}
    distance[start] = 0

    # 初始化优先队列，用于存储待处理的节点和对应的距离
    queue = [(0, start)]

    while queue:
        # 取出队列中距离最小的节点
        current_distance, current_node = heapq.heappop(queue)

        # 如果当前节点已经处理过，则跳过
        if current_distance > distance[current_node]:
            continue

        # 遍历当前节点的邻居节点
        for neighbor, weight in graph[current_node].items():
            # 计算邻居节点的新距离
            new_distance = distance[current_node] + weight

            # 如果新距离比当前距离小，则更新距离字典，并将邻居节点加入优先队列
            if new_distance < distance[neighbor]:
                distance[neighbor] = new_distance
                heapq.heappush(queue, (new_distance, neighbor))

    # 返回起始节点到目标节点的最短路径长度
    return distance[end]

# 创建示例地图
graph = {
    'A': {'B': 5, 'C': 2},
    'B': {'A': 5, 'C': 1, 'D': 3},
    'C': {'A': 2, 'B': 1, 'D': 6},
    'D': {'B': 3, 'C': 6}
}

# 执行路径规划
start_node = 'A'
end_node = 'D'
shortest_distance = dijkstra(graph, start_node, end_node)

print(f"The shortest distance from {start_node} to {end_node} is {shortest_distance}")

在上面的代码中，我们使用字典来表示地图，其中每个节点都是一个键，其值是一个字典，其中包含与该节点相邻的节点及其对应的距离。例如，graph['A']返回一个字典{'B': 5, 'C': 2}，表示节点'A'与节点'B'的距离为5，与节点'C'的距离为2。

在dijkstra函数中，我们使用了优先队列来存储待处理的节点和对应的距离。每次从队列中取出距离最小的节点，并遍历其邻居节点，计算新的距离并更新距离字典和优先队列。最终，函数返回起始节点到目标节点的最短路径长度。

在示例中，我们创建了一个简单的地图，然后使用Dijkstra算法计算从节点'A'到节点'D'的最短路径长度。输出结果为The shortest distance from A to D is 8，表示起始节点'A'到目标节点'D'的最短路径长度为8