全局路径规划之Dijkstra算法

Dijkstra是一种基于贪心策略的最短路径算法，其实质是广度优先搜索，是一种发散式的搜索，所以空间复杂度和时间复杂度都比较高。Dijkstra算法只记录到起点的代价。

该算法一般用于全局路径规划，其流程如下图所示（图片截自B站艾若机器人Joe的课程视频）：

如下图所示，利用Dijkstra算法寻找从节点1到节点5的最短路径。

根据Dijkstra算法的思路，open list用于存放当前还没有找到最短路径的节点（从起点开始）、closed list用于存放已经找到最短路径的节点，具体的搜索过程如下表所示，表中的灰色表示从open list移动至closed list。

python代码参考了如下博文：https://blog.csdn.net/feriman/article/details/113619939?spm=1001.2014.3001.5501，为了方便观察搜索过程，将每一步的open list和closed list打印出来，具体如下：

class Dijkstra:
    def __init__(self, graph, start, goal):
        self.graph = graph      
        self.start = start      
        self.goal = goal        

        self.open_list = {}     
        self.closed_list = {}   

        self.open_list[start] = 0.0 

        self.parent = {start: None}     
        self.min_dis = None            

    def shortest_path(self):

        while True:
            if self.open_list is None:
                print('搜索失败， 结束！')
                break
            distance, min_node = min(zip(self.open_list.values(), self.open_list.keys()))      
            self.open_list.pop(min_node)                                                       
            
            print('open list:{}'.format(self.open_list))          

            self.closed_list[min_node] = distance                  
            print('closed list:{}'.format(self.closed_list))
            print('min node:{}'.format(min_node))
            print('--------------------------------------------------------------------------')
            if min_node == self.goal:                              
                self.min_dis = distance
                shortest_path = [self.goal]                        
                father_node = self.parent[self.goal]
                while father_node != self.start:
                    shortest_path.append(father_node)
                    father_node = self.parent[father_node]
                shortest_path.append(self.start)
                print(shortest_path[::-1])                        
                print('最短路径的长度为：{}'.format(self.min_dis))
                print('找到最短路径， 结束！')
                return shortest_path[::-1], self.min_dis			

            for node in self.graph[min_node].keys():             
                if node not in self.closed_list.keys():  
                    if node in self.open_list.keys():             
                        if self.graph[min_node][node] + distance < self.open_list[node]:
                            self.open_list[node] = distance + self.graph[min_node][node]    
                            self.parent[node] = min_node         
                    else:                                          
                        self.open_list[node] = distance + self.graph[min_node][node]            
                        self.parent[node] = min_node             


if __name__ == '__main__':
    g = {'1': {'2': 2, '4': 1},
         '2': {'4': 3, '5': 11},
         '3': {'1': 4, '6': 5},
         '4': {'3': 2, '6': 8, '7': 4},
         '5': {'7': 6},
         '7': {'6': 1},
         '6': {'6': 0}
         }
    start = '1'
    goal = '5'
    dijk = Dijkstra(g, start, goal)
    dijk.shortest_path()

代码执行结果如下所示，可以看见代码的执行结果与之前给出的搜索结果一致。