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

YoungTimes

发布于 2022-04-28 16:00:54

1.7K2

发布于 2022-04-28 16:00:54

经典的Dijkstra算法是一种Graph Based的单源最短路径规划算法，可以解决带权重有向图的最短路径规划问题。双向Dijkstra算法是对经典Dijkstra算法的一种优化方法，其主要思想就是从起点和终点同时开始搜索，从而达到提升算法执行效率的效果。实际效果表明，在大部分情况下，双向Dijkstra算法均优于经典的Dijkstra算法。

先直观的感受一下，Dijkstra算法和双向Dijkstra算法的区别。

来源：https://www.youtube.com/watch?time_continue=44&v=8Jjdp6f7oaE&feature=emb_logo

1、双向Dijstra算法思想

双向Dijstra算法的思想是：

分别从路径搜索的起点s和路径搜索的终点t同时执行前向的Dijkstra算法和后向的Dijkstra算法。
当前向Dijkstra算法和后向Dijkstra算法相遇时，搜索结束。假设二者相遇时的节点为u，起点s到u节点的Dijkstra路径为D(s, u)，终点t到u节点的Dijkstra路径为D(t, u)，则起点s到终点t的最短路径为: D(s, u) + D(u, t)。

2、双向Dijstra算法代码

def findShortestPath_BiDijkstra(self, start, end): 
    closed = set()

    forward_pq = queue.PriorityQueue()
    backward_pq = queue.PriorityQueue()
    forward_pred = {}
    backward_pred = {}
    forward_min_dist = {}
    forward_min_dist[start] = 0
    backward_min_dist = {}
    backward_min_dist[end] = 0
    forward_pq.put((0,start))
    backward_pq.put((0, end))
        
    shortest_path = []
        
    path_find = False
    meet_node = None

    while forward_pq.qsize() != 0 or backward_pq.qsize() != 0:
        if forward_pq.qsize() != 0:
            dist, u = forward_pq.get()
 
            if u in closed:
                path_find = True
                meet_node = u
                break
                
            closed.add(u)
                
            if u not in self.__graph_dict:
                continue
                    
                
            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 forward_min_dist:
                    if newdist &lt; forward_min_dist[to_node] :
                        forward_min_dist[to_node] = newdist
                        forward_pred[to_node] = u
                            
                else:
                    forward_min_dist[to_node] = newdist
                    forward_pred[to_node] = u
                    forward_pq.put((newdist, to_node))
                        
                        
        if backward_pq.qsize() != 0:
            dist, u = backward_pq.get()
                
            if u in closed:
                path_find = True
                meet_node = u
                break
                    
            closed.add(u)
                
            if u not in self.__reverse_graph_dict:
                continue
                
            for to_node, to_weight in self.__reverse_graph_dict[u].items():
                if to_node in closed:
                    continue;
                        
                newdist = dist + to_weight
                if to_node in backward_min_dist:
                    if newdist &lt; backward_min_dist[to_node] :
                        backward_min_dist[to_node] = newdist
                        backward_pred[to_node] = u
                else:
                    backward_min_dist[to_node] = newdist
                    backward_pred[to_node] = u
                    backward_pq.put((newdist, to_node))
                        
    if path_find:
        path = self.extractPath(meet_node, forward_pred)
        shortest_path.extend(path)
        path = self.extractPath(meet_node, backward_pred)
        path.reverse()
        shortest_path.extend(path[1:])
            
    return shortest_path

代码效果测试：

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.findShortestPath_Dijkstra("0", "3")
print(path)

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

算法的输入结果如下，可以看到二者的规划结果是相同的。

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

3、双向Dijstra在美国道路网上的路径规划效果

来源：https://www.youtube.com/watch?v=1oVuQsxkhY0&feature=emb_logo

推荐一个在线动态交互的路径规划网站，用户可自行构造障碍物和不同的路线规划算法，比较算法的效果:

路径规划算法测试对比,来源:http://qiao.github.io/PathFinding.js/visual/

本文参与腾讯云自媒体同步曝光计划，分享自微信公众号。

原始发表：2020-02-29，如有侵权请联系 cloudcommunity@tencent.com 删除

编程算法

https

网络安全

腾讯云测试服务

本文分享自半杯茶的小酒杯微信公众号，前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体同步曝光计划，欢迎热爱写作的你一起参与！

登录后参与评论

0 条评论

热度