机器人路径规划Dijkstra算法代码

以下是使用Dijkstra算法实现机器人路径规划的代码示例：

import heapq

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

    # 创建优先队列并将起始节点加入队列
    queue = [(0, start)]

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

        # 如果当前节点已经被访问过，则跳过
        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
                predecessors[neighbor] = current_node

                # 将邻居节点加入优先队列
                heapq.heappush(queue, (distance, neighbor))

    # 构建最短路径
    path = []
    current_node = end
    while current_node is not None:
        path.insert(0, current_node)
        current_node = predecessors[current_node]

    return path

# 示例图形表示
graph = {
    'A': {'B': 5, 'C': 1},
    'B': {'A': 5, 'C': 2, 'D': 1},
    'C': {'A': 1, 'B': 2, 'D': 4, 'E': 8},
    'D': {'B': 1, 'C': 4, 'E': 3, 'F': 6},
    'E': {'C': 8, 'D': 3},
    'F': {'D': 6}
}

start_node = 'A'
end_node = 'F'

# 调用Dijkstra算法求解最短路径
shortest_path = dijkstra(graph, start_node, end_node)
print("最短路径:", shortest_path)