参考链接：

【路径规划】全局路径规划算法——Dijkstra算法（含python实现 | c++实现）-CSDN博客 

 算法图解：

代码

def dijkstra(matrix, source):"""迪杰斯特拉算法实现Args:matrix (_type_): 用邻接矩阵表示带权图source (_type_): 起点Returns:_type_: 最短路径的节点集合，最短路径的节点的最短距离，每个节点到起点的最短路径"""INF = float('inf')n = len(matrix)m = len(matrix[0])assert n == m, "Error, please examine matrix dim"assert source < n, "Error, start point should be in the range!"S = [source]        # 已找到最短路径的节点集合   S：可以缩写为"SP"，代表"Shortest Path"（最短路径）。U = [v for v in range(n) if v not in S]  # 记录还未确定最短路径的节点集合  U：可以缩写为"UNP"，代表"Unprocessed Nodes"（未处理的节点）。distance = [INF] * n          # source到已找到最短路径的节点的最短距离distance[source] = 0  # 起点到自己的距离path_optimal = [[]]*n           # source到其他节点的最短路径path_optimal[source] = [source]while len(S) < n:   # 当已找到最短路径的节点小于n时min_value = INFcol = -1row = -1for s in S:     # 以已找到最短路径的节点所在行为搜索对象for u in U:   # 从U中搜索尚未记录的节点if matrix[s][u] + distance[s] < min_value:  # 找出最小值# 在某行找到最小值要加上source到该行的最短路径min_value = matrix[s][u] + distance[s]row = s         # 记录所在行列col = uif col == -1 or row == -1:  # 若没找出最小值且节点还未找完，说明图中存在不连通的节点breakS.append(col)  # 在S中添加已找到的节点U.remove(col)  # 从U中移除已找到的节点distance[col] = min_value # source到该节点的最短距离即为min_valuepath_optimal[col] = path_optimal[row][:]    # 复制source到已找到节点的上一节点的路径path_optimal[col].append(col)       # 再其后添加已找到节点即为source到该节点的最短路径return S, distance, path_optimaldef main():INF = float('inf')# 使用邻接矩阵存储图# A B C D E F Gmatrix = [[0, 12, INF, INF, INF, 16, 14],[12, 0, 10, INF, INF, 7, INF],[INF, 10, 0, 3, 5, 6, INF],[INF, INF, 3, 0, 4, INF, INF],[INF, INF, 5, 4, 0, 2, 8],[16, 7, 6, INF, 2, 0, 9],[14, INF, INF, INF, 8, 9, 0]]S, distance, path_optimal = dijkstra(matrix, 3)print('S:')print(S)print('distance:')print(distance)print('path_optimal:')for p in path_optimal:print(p)if __name__ == '__main__':main()