Dijkstra移动机器人路径规划代码

下面是Dijkstra算法的网格移动机器人路径规划的Python实现代码：

import heapq

def dijkstra(start, end, grid):
    # 将起点加入堆中
    heap = [(0, start)]
    # 初始化距离字典和路径字典
    distances = {start: 0}
    path = {start: None}
    # 遍历堆
    while heap:
        # 取出堆中距离最小的节点
        (cost, current) = heapq.heappop(heap)
        # 判断是否到达终点
        if current == end:
            break
        # 遍历当前节点的邻居节点
        for neighbor in grid.neighbors(current):
            # 计算到达邻居节点的代价
            new_cost = distances[current] + grid.cost(current, neighbor)
            # 判断是否已经遍历过该节点，或者代价更大
            if neighbor not in distances or new_cost < distances[neighbor]:
                # 更新距离和路径
                distances[neighbor] = new_cost
                path[neighbor] = current
                # 将邻居节点加入堆中
                heapq.heappush(heap, (new_cost, neighbor))
    # 生成路径
    node = end
    result = [node]
    while path[node] is not None:
        result.append(path[node])
        node = path[node]
    result.reverse()
    return result

其中，`start`和`end`分别表示起点和终点的坐标，`grid`是一个网格类，包含了邻居节点和节点代价的计算。在实际应用中，需要根据具体情况修改`grid`类的实现。`dijkstra`函数返回从起点到终点的最短路径。