自动驾驶路径规划-Dijkstra算法

图片来源:http://www.csie.ntnu.edu.tw/~u91029/Circuit.html

1、什么是Dijkstra算法

Dijkstra算法是一种有权图(Graph)的单源最短路径求解算法，给定一个起点，使用Dijkstra算法可以得到起点到其它所有节点的最短路径。Dijkstra算法要求图(Graph)中所有边的权重都为非负值，只有保证了这个条件才能该算法的适用性和正确性。

2、Dijkstra算法Overview

假设有权图(Graph)的如下，起点(Starting Node)为0，我们一步步看看如何使用Diskstra算法计算起点(Starting Node)到达所有其它Node的最短路径。

首先我们需要定义一个数据结构来记录起点(Starting Node)到其它所有Node的最短距离，并将所有最短距离的值初始化为正的无穷大，表示各个节点(Node)均不可达。

dist = [infinity, infinity, ..., infinity]

然后，需要定义一个(index, distance)的Pair对象来存储起点(Starting Node)到index Node的当前最短距离，其中index为顶点的索引，distance为index Node到Starting Node的最短距离。在开始进行路径搜索前，所有Node对应的最短距离都初始化为正的无穷大。

在图的路径搜索过程中，我们需要先搜索距离最近的节点，所以我们需要使用优先级队列(Priority Queue)或者小顶堆(Min Heap)的数据结构来存储(index, distance)的Pair，保证每次取出的待访问Node都是距离起点(Starting Node)最近的Node。

准备好数据结构之后，开始算法执行过程：

1、首先将起点(Starting Node)=(0, 0)放入优先级队列(Priority Queue)，表明我们从index=0的节点开始查找，该节点距离起点(Starting Node)的distance = 0；

此时优先级队列(Priority Queue)的内容如下:

2、从优先级队列(Priority Queue)中取出distance最小的Node(此时index=0的Node的distance最小)，遍历它的所有Neighbor节点。

首先遍历index=1的Node，该Node未被访问过，所以直接更新起点(Starting Node)到index = 1的Node的最短距离为4，并且放入到优先级队列中(Priority Queue)中；

然后遍历index=4的Node，该Node未被访问过，所以直接更新Starting Node到index=2的最短距离为1，并且放入到优先级队列中(Priority Queue)中；

此时优先级队列(Priority Queue)的内容如下:

3、从优先级队列(Priority Queue)中取出distance最小的Node(此时index=2的Node的distance最小)，遍历它的所有Neighbor节点。

首先遍历index=1的Node，由于index=1的Node已经被访问过，此时需要比较当前的距离是否小于已有的距离。起点(Starting Node)到index =1的distance为1 + 2 =3，小于已有的起点(Starting Node)直接到index=1的Node的distance = 4，因此更新起点(Starting Node)到index=1的distance为3。

然后遍历index=3的Node，该Node未被访问过，所以直接更新其到起点(Starting Node)的distance为1+ 5 = 6。

此时优先级队列(Priority Queue)的内容如下:

4、从优先级队列(Priority Queue)中取出距离最小的Node(此时index=1的Node的distance最小)，遍历它的所有Neighbor节点。

遍历index=3的Node，起点(Starting Node)到index=3的Node距离为3 + 1 = 4，小于原有起点(Starting Node)经由index=2的到达index=3的Node的距离6，所以从更新起点(Starting Node)到index=3的Node的最短距离为4。

此时优先级队列(Priority Queue)的内容如下:

5、从优先级队列(Priority Queue)中取出距离最小的Node(此时index=3的Node的distance最小)，遍历它的所有Neighbor节点。

遍历到index=4的Node，该Node尚未被遍历过，起点(Starting Node)到index=4的Node最短距离为4 + 3 = 7;。

此时优先级队列(Priority Queue)的内容如下:

这就是Dijkstra算法的完整执行过程，至此我们得到了从起点(Starting Node)到所有其它Node的最短距离。

3、Dijkstra算法实现路径查找

因为我们的目标是搜索从起点到目的地的最短路径，而Dijkstra算法提供了从起点(Starting Node)到其它所有节点的最短路径，所以我们在路径查找中对Dijkstra算法做了剪枝处理。

def extractPath(self, u, pred):
    path = []
    k = u
    path.append(k)
    while k in pred:
        path.append(pred[k])
        k = pred[k]
        
    path.reverse()
   
    return path
    
def findShortestPathInWeightGraph(self, start, end): 
    # Mark all the vertices as not visited
    closed = set()

    pq = queue.PriorityQueue()
    pred = {}
    min_dist = {}
    pq.put((0,start))
    min_dist["0"] = 0
        

    while pq.qsize() != 0:
        dist, u = pq.get()
        closed.add(u)

        if u == end:
            path = self.extractPath(u, pred)
            return path

        for to_node, to_weight in self.__graph_dict[u].items():
            if to_node in closed:
                continue;
            newdist = dist + to_weight
            if to_node in min_dist:
                if newdist < min_dist[to_node] :
                   min_dist[to_node] = newdist
                   pred[to_node] = u
            else:
                min_dist[to_node] = newdist
                pred[to_node] = u
                pq.put((newdist, to_node))

构造如图2.1的Graph，测试代码如下：

g = { "0" : {"1":4, "2":1},
      "2" : {"1":2, "3":5},
      "1" : {"3":1},
      "3" : {"4":3}
    }

graph = Graph(g)

print('The path from vertex "0" to vertex "3":')
path = graph.findShortestPathInWeightGraph("0", "3")

print(path)

路径检索结果如下:

The path from vertex "0" to vertex "3":
['0', '2', '1', '3']

作为运动规划领域最著名的算法之一，Dijkstra算法可以解决带权重有向图的最短路径规划问题，在实际的路径规划中也有大规模的实际应用。但是Dijkstra算法的搜索没有方向性，会有大量冗余的搜索操作。我们可以给Dijkstra加上一些启发性的信息，引导搜索算法快速的搜索到目标，这就是A*算法。

由于加入引导信息，A*算法在大多数情况下会比Dijkstra算法要快。

参考链接

1、运动规划-简介篇

2、Dijkstra's Shortest Path Algorithm | Graph Theory