Dijkstra，A*，DWA，TEB

最新推荐文章于 2025-05-01 14:35:41 发布

原创

最新推荐文章于 2025-05-01 14:35:41 发布 · 3.1k 阅读

28 ·

CC 4.0 BY-SA版权

文章标签：

#算法

本文介绍了Dijkstra算法和A*算法在寻找最短路径中的应用，以及DWA算法在实时避障中的策略。Dijkstra算法适用于计算图的单源最短路径，而A*算法通过引入启发式信息提高搜索效率。DWA算法则考虑了目标导向、障碍物距离和速度优化来指导小车行驶。此外，还提及了TEB算法，它通过路径形变和多目标优化实现灵活避障及路径跟随。

Dijkstra

Dijkstra算法是典型的单元最短路径算法，用于计算赋权有向图或无向图的单源最短路径问题

Dijkstra算法的思想是，将带权有向图或无向图中的顶点分为两类，一类是已经确定最短路径的顶点集合S，开始时只有源点s，另一类是未确定最短路径的点集合U，然后从集合U中找出最短路径长度的顶点添加到S中，并更新其他顶点的最短路径。这样源点s到S中的所有顶点的最短路径距离都不大于源点s到U中顶点的最短路径距离，保证了S中顶点都是已经求出了最短的路径长度。

def dijkstra_search(graph, start, end):
	frontier = PriorityQueue()
	frontier.put(start, 0)
	cost_so_far = {
   
   }
	cost_so_far[star] = 0
	come_from = {
   
   }
	come_from[start] = None

	while not frontier.empty():
		current