| by YoungTimes | No comments

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

自动驾驶路径规划-Graph-Based的BFS最短路径规划》中提到我们可以将地图抽象为Graph的数据结构,然后利用Graph的广度优先遍历算法(Breadth-First Search, BFS)解决无权重的High-Level的地图级别的规划。但是实际应用场景中,地图中各个路径所代表的Graph的边的权重都是不同的,比如距离长的Edge权重就应该比较低;交通拥堵的Edge权重就应该低等等。对于有权重的Graph如何进行最短路径规划呢,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的最短路径。

图2.1

首先我们需要定义一个数据结构来记录起点(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算法要快。

Dijkstra和A*算法演示

参考链接

1、运动规划-简介篇

2、Dijkstra’s Shortest Path Algorithm | Graph Theory

相关文章

未知环境下的Lidar概率占位栅格图(Occupancy Grid Map) Python代码实现
自动驾驶Mapping-占位栅格图(Occupancy Grid Map)
无人驾驶路径规划技术-三次样条插值(Cubic Spline Interpolation)曲线及Python代码实现
自动驾驶运动规划(Motion Planning)
自动驾驶定位算法(十五)基于多传感器融合的状态估计(muti-Sensors Fusion)
自动驾驶中的车辆运动学模型
自动驾驶路径规划器-Lattice Planner详解
自动驾驶定位算法(十三)-粒子滤波(Particle Filter)
自动驾驶硬件系统(十二)-激光雷达(Lidar)测量模型
自动驾驶硬件系统(十一)-Global Navigation Satellite Systems (GNSS)
自动驾驶定位算法(九)-直方图滤波(Histogram Filter)定位

发表评论