你不能改变过去，但你可以改变未来。
—《狮子王》
🏰代码及环境配置：请参考 环境配置和代码运行!
DFS算法无法找到最优解，BFS算法仅仅适用于无权图或者权重相同的图，而Dijkstra算法适用于带有非负权重的有向图和无向图。其由荷兰计算机科学家Edsger W. Dijkstra在1956年提出，是一种用于在图中找到单源最短路径的算法，解决了从图中一个顶点到其他所有顶点的最短路径问题，此外，如果知道了源节点到目标节点的最短路径，算法也可以提前终止。
4.4.1 数据结构-优先队列
优先队列（priority queue）是一种抽象的数据类型，其与普通队列（queue）相似但也有所不同，其特性如下：
优先队列中的每个元素都有相对应的优先级。 在进行移除元素操作时，高优先级的元素将优先被移除。 如果两个元素的优先级相同，则按照先进先出（FIFO）的原则进行排序。
其特性表明优先级队列只支持可比较的元素，这意味着元素要么按升序排列，要么按降序排列。
升序优先队列: 小值拥有更高的优先级
降序优先队列: 大值拥有更高的优先级
4.4.2 基本思想
Dijkstra算法的基本思想是从一个源节点开始，逐步向外探索，直至找到从源节点到所有其他节点（或者目标节点）的最短路径。相比较于BFS，其维护了每个节点到源节点的最小累计cost：，每次从队列中取的是cost最小的节点，而非先进入队列的节点。
其算法步骤如下：
初始化： 创建一个数组 g[]，其中g[i]表示从源节点start到节点i的最小cost，初始时，源节点的cost为0。创建一个布尔数组 visited[]来跟踪每个节点是否已被访问过，初始时，所有顶点都未访问创建一个优先队列 open_list，用于根据cost选择下一个要处理的节点。优先队列中的每个元素是一个包含顶点和cost的配对，初始时将源节点start和其cost(0)加入优先队列。循环处理优先队列中的节点： 计算邻接节点 next的cost。若邻接节点 next不在优先队列中，则将邻接节点next和其对应的cost加入优先队列，并在数组g[]中设置邻接节点next的cost，最后将临接节点next的父节点设为v。若邻接节点 next在优先队列中，则在数组g[]和优先队列open_list中更新节点next的相关信息，最后将临接节点next的父节点设为v。从优先队列中取出cost最小的节点 v，并将节点v标记为已访问，若节点v就是目标节点，则提前返回。遍历节点 v中所有未被访问过的邻接节点，对于每一个邻接节点next：继续处理优先队列中的顶点，直到队列为空或所有顶点都被访问。 从目标节点 goal开始，通过父节点向回追溯，直到起始节点，最终得到一条路径。
其伪代码如下：
Algorithm Dijkstra(G, start, goal):
let **open_list** be a priority_queue
**open_list.**push(**start,** 0)
g[start] = 0
while **open_list** is not empty do
v = **open_list**.pop()
mark v as visted
if v is the **goal**:
return v
for **all unvisited neighbours** next of v in Graph G do
next_cost = g[v] + cost(v, next)
if next is not in **open_list**:
g[next] = next_cost
**open_list**.push(next, next_cost)
next.parent = v
else:
if g[next] > next_cost:
g[next] = next_cost
**open_list**.**update_priority**(next, next_cost)
next.parent = v
4.4.3 Dijkstra算法的图解
以从下面的无向图中寻找出节点0 到其他所有节点的最短路径为例，其中两个节点之间的权重表示他们之间的距离（或者cost），并初始话两个表：visited nodes info和unvisited nodes info。
所有节点的初始cost如下：
源节点到自身的cost为0。 源节点到其它节点的cost还没有确定，暂时先标记为无穷大。
从源节点 0开始，将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新源节点0的邻近节点（节点1和节点2）的信息。节点 1：节点1最短路径为0 —> 1，cost为2，父节点为0。节点 2：节点2最短路径为0 —> 2，cost为6，父节点为0。
查看未访问列表，节点 1的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新节点1的邻近节点（节点3）的信息。节点 3：节点3的最短路径为0 —> 1 —> 3，cost为7，父节点为1。
查看未访问列表，节点 2的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新节点2的邻近节点（节点3）的信息。节点 3：此时，节点3有两条可选路径：0 —> 1 —> 3和0 —> 2 —> 3，前者的cost是7，后者的cost是14，因此选择前者，即节点3的最短路径为0 —> 1 —> 3，cost为7，父节点为1。
查看未访问列表，节点 3的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新节点3的邻近节点（节点4和节点5）的信息。节点 4：节点4的最短路径为0 —> 1 —> 3 —> 4，cost为17，父节点为3。节点 5：节点5的最短路径为0 —> 1 —> 3 —> 5，cost为22，父节点为3。
查看未访问列表，节点 4的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新节点4的邻近节点（节点5和节点6）的信息。节点 5：此时节点5有两条可选路径：0 —> 1 —> 3 —> 5和0 —> 1 —> 3 —> 4 —> 5，前者的cost是22，后者的cost是23，因此选择前者，即节点5的最短路径为0 —> 1 —> 3 —> 5，父节点为3。节点 6：节点6的最短路径为0 —> 1 —> 3 —> 4 —> 6，cost为19，父节点为4。
查看未访问列表，节点 6的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除。同时在未访问列表中更新节点6的邻近节点（节点5）的信息。0 —> 1 —> 3 —> 5，cost为22。0 —> 1 —> 3 —> 4 —> 5，cost为23。0 —> 1 —> 3 —> 4 —> 6 —> 5,cost为25。节点 5：此时节点5共有3条可选路径（如下），因此最短路径为0 —> 1 —> 3 —> 5，cost为22，父节点为3。
查看未访问列表，节点 5的cost最小，因此将其加入到已访问列表中，并从未访问列表中删除。此时未访问列表中没有节点，因此搜索结束。此时，我们可以给定目标节点，并通过回溯父节点的方式找到从源节点到目标节点的最短路径。例如我们另节点6是目标节点，可以得到从源节点到目标节点的最短路径为：0 —> 1 —> 3 —> 4 —> 6
4.4.4 Dijkstra算法特点
优点：
（1）总能找到从单个源点到图中所有其他顶点的最短路径，前提是所有的边权重都是非负的。
（2）原理简单，应用广泛，且满足最优性原则。
缺点：
（1）搜索时没有引入目标节点的信息，导致搜索效率较低。
（2）不适用于包含负权重的图。
4.4.c Dijkstra代码解析
本节提供了Dijkstra算法的代码测试：
python3 tests/search_based_planning/dijkstra_test.py
4.4.c.1 构图的代码实现
基于图搜的运动规划中最重要的一步是构图，构建的图比较简单，主要包含map border和obstacles，读者也可根据需求修改构图方式。
def construct_env_info():
border_x = []
border_y = []
ox = []
oy = []
# Map border.
for i in range(-10, 60):
border_x.append(i)
border_y.append(-10.0)
for i in range(-10, 60):
border_x.append(60.0)
border_y.append(i)
for i in range(-10, 61):
border_x.append(i)
border_y.append(60.0)
for i in range(-10, 61):
border_x.append(-10.0)
border_y.append(i)
# Obstacle 1.
for i in range(40, 55, 1):
for j in range(5, 15, 1):
ox.append(i)
oy.append(j)
# Obstacle 2.
for i in range(40):
for j in range(20, 25, 1):
ox.append(j)
oy.append(i)
# Obstacle 3.
for i in range(30):
for j in range(40, 45, 1):
ox.append(j)
oy.append(58.0 - i)
# Obstacle 4.
for i in range(0, 20, 1):
for j in range(35, 40, 1):
ox.append(i)
oy.append(j)
return border_x, border_y, ox, oy
4.4.c.2 Dijkstra的代码实现
在Dijkstra算法中，首先我们定义了节点Node ，这是图的基础元素。其中（x, y）表示节点的位置，cost表示从源节点到当前节点的cost，parent 表示当前节点的父节点。
class Node:
def __init__(self, x, y, cost, parent_index, parent):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.parent_index = parent_index
self.parent = parent
下面是Dijkstra的核心算法，基于环境信息，起始点位置，目标点位置搜到最优路径，其中：
sx:起始点的x坐标的值
sy:起始点的y坐标的值
gx:目标点的x坐标的值
gy:目标点的y坐标的值
最终返回一条路径：rx, ry
def planning(self, sx, sy, gx, gy):
start_node = self.Node(
self.calc_xy_index(sx, self.min_x),
self.calc_xy_index(sy, self.min_y),
0.0,
-1,
)
goal_node = self.Node(
self.calc_xy_index(gx, self.min_x),
self.calc_xy_index(gy, self.min_y),
0.0,
-1,
)
open_set, closed_set = dict(), dict()
open_set[self.calc_index(start_node)] = start_node
while True:
c_id = min(open_set, key=lambda o: open_set[o].cost)
current = open_set[c_id]
# show graph
if show_animation: # pragma: no cover
plt.plot(
self.calc_position(current.x, self.min_x),
self.calc_position(current.y, self.min_y),
"xc",
)
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
"key_release_event",
lambda event: [exit(0) if event.key == "escape" else None],
)
if len(closed_set.keys()) % 10 == 0:
plt.savefig(gif_creator.get_image_path())
plt.pause(0.001)
if current.x == goal_node.x and current.y == goal_node.y:
print("Find goal")
goal_node.parent_index = current.parent_index
goal_node.cost = current.cost
break
# Remove the item from the open set
del open_set[c_id]
# Add it to the closed set
closed_set[c_id] = current
# expand search grid based on motion model
for move_x, move_y, move_cost in self.motion:
node = self.Node(
current.x + move_x,
current.y + move_y,
current.cost + move_cost,
c_id,
)
n_id = self.calc_index(node)
if n_id in closed_set:
continue
if not self.verify_node(node):
continue
if n_id not in open_set:
open_set[n_id] = node # Discover a new node
else:
if open_set[n_id].cost >= node.cost:
# This path is the best until now. record it!
open_set[n_id] = node
rx, ry = self.calc_final_path(goal_node, closed_set)
return rx, ry
4.4.c.3 Dijkstra的代码测试
在main 函数中设置起始点，目标点，grid的分辨率和机器人的半径，在创建grid map之后，并运行Dijkstra算法，即可找到最优路径。如下图所示，红色路径即为最终Dijkstra搜出来的最优路径。
def main():
# start and goal position
start_x = 10.0 # [m]
start_y = 10.0 # [m]
goal_x = 50.0 # [m]
goal_y = 0.0 # [m]
grid_size = 2.0 # [m]
robot_radius = 1.0 # [m]
# construct environment info.
border_x, border_y, ox, oy = construct_env_info()
ox.extend(border_x)
oy.extend(border_y)
dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
rx, ry = dijkstra.planning(start_x, start_y, goal_x, goal_y)
4.4.5 参考
https://en.wikipedia.org/wiki/Dijkstra's_algorithm#
https://www.javatpoint.com/ds-priority-queue
https://www.freecodecamp.org/chinese/news/dijkstras-shortest-path-algorithm-visual-introduction/
推荐阅读:
自动驾驶小白说官网: https://www.helloxiaobai.cn
