路径规划算法(1) - Dijsktra路径算法 python实现及解析

• 迪克斯特拉(Dijkstras)算法，适用于加权图(weighted graph)，找出的是总权重最小的路径。

• Dijsktra算法包含四个步骤：

  1. 找出当前离起点最近的节点；
  2. 对于该节点的邻居，检查是否有前往它们的更短路径，如果有，就更新其开销，并更新邻居的父节点；
  3. 重复这个过程，直到对所有节点都这样做了；
  4. 计算最终路径(路线和开销)。

code

#!/bin/python
# -*- coding:utf-8 -*-

def dijkstra(graph, startIndex, path, cost, max):
    """
    求解各节点最短路径，获取path，和cost数组，
    path[i] 表示vi节点的前继节点索引，一直追溯到起点。
    cost[i] 表示vi节点的花费
    """
    lenth = len(graph)
    v = [0] * lenth
    # 初始化 path，cost，V, 获得start节点同相邻节点的 权重情况
    for i in range(lenth):
        if i == startIndex:
            v[startIndex] = 1
        else:
            cost[i] = graph[startIndex][i] # 获得了第一行的链接权重
            path[i] = (startIndex if (cost[i] < max) else -1)
    print v, cost, path

    for i in range(1, lenth):
        minCost = max
        curNode = -1
        # for 获取最小权值的节点
        for w in range(lenth):
            if v[w] == 0 and cost[w] < minCost:
                minCost = cost[w]
                curNode = w
                print("curNode: ", curNode)
        # 不可通行的节点，跳出循环
        if curNode == -1: break 
        v[curNode] = 1
        print "v: ",v
        # 更新其他节点的权值（距离）和路径
        for w in range(lenth):
            # v[w] == 0 当前节点尚未遍历, 
            # cost 总是维护了一个 从start 到其他点的最小距离,所以 Q(a=>c) 可与 Q(a=>b) + Q(b=>c) 比较
            if v[w] == 0 and (graph[curNode][w] + cost[curNode] < cost[w]):
                print "======> 更新 w: ", w , "  graph[curNode][w]:", graph[curNode][w], \
                      " cost[curNode]:", cost[curNode], "  || cost[w]:" , cost[w]
                cost[w] = graph[curNode][w] + cost[curNode] # 更新权值
                path[w] = curNode # 更新路径
    return path, cost

def show_graph(graph):
    for idx in graph:
        print idx
if __name__ == '__main__':
    max = 2147483647
    graph = [
        [max, max, 10, max, 30, 100],
        [max, max, 5, max, max, max],
        [max, max, max, 50, max, max],
        [max, max, max, max, max, 10],
        [max, max, max, 20, max, 60],
        [max, max, max, max, max, max],
        ]
    show_graph(graph)
    path = [0] * 6
    cost = [0] * 6
    path, cost = dijkstra(graph, 0, path, cost, max)
    print "path :", path
    print "cost :", cost

总结: 
1. cost 总是维护了一个 从start 到其他点的最小距离,

参考:
https://blog.csdn.net/weixin_40512497/article/details/82773192
https://blog.csdn.net/yucicheung/article/details/79674334