【路径规划】(1) Dijkstra 算法求解最短路，附python完整代码
好久不见，我又回来了，这段时间把路径规划的一系列算法整理一下，感兴趣的点个关注。今天介绍一下机器人路径规划算法中最基础的 Dijkstra 算法，文末有 python 完整代码，那我们开始吧。
1. 算法介绍
1959 年，荷兰计算机科学家 ·EdsgerWybe·Dijkstra 发表了论文《 A note on two problems in connexion with graphs 》，提出了 Dijkstra 算法。发展至今日，Dijkstra 算法成为了解决带权图最短路径问题的经典算法之一，现在常常被用于网络内部路由问题的求解或者作为其它的复杂图论算法的子算法辅助进行计算。
近年来，Dijkstra 算法在许多领域得到广泛应用，比如：物流中心分层选址[1]、电网故障行波定位[2]、电能路由策略[3]。众多学者作了研究，比如：徐洋洋等人提出将Dijkstra 算法应用于交通阻塞路径规划中，生成的最佳路径有效避开拥堵路段[4]；吴红波等人提出 Dijkstra 算法和 GIS 网络分析的有效集成不但可以实现城市车辆行驶路线优化决策，而且 Dijkstra 算法优化能减少节点访问次数和时间复杂度[5]；王芝麟等人提出使用最小二叉堆作为 Dijkstra 最短路径算法的辅助数据结构，有效降低算法的运算次数并提高运算效率[6]。
参考文献：
[1] 靳国伟,何世伟,黎浩东,何必胜,殷玮川.Harmony Search-Dijkstra 混合算法在铁路物流中心分层选址中的应用[J].北京交通大学学报,2016,40(04):45-52.
[2] 李泽文,唐平,曾祥君,肖仁平,赵廷.基于 Dijkstra 算法的电网故障行波定位方法[J].电力系统自动化,2018,42(18):162-168.
[3] 江渝,叶泓炜,张青松,王克,徐志鹏,杨睿.能源互联网中基于 Dijkstra 算法的分布式电能路由策略的实现[J].电网技术,2017,41(07).
[4] 吴红波,王英杰,杨肖肖.基于 Dijkstra 算法优化的城市交通路径分析[J].北京交通大学学报,2019,43(04):116-121+130.
[5] 王芝麟,乔新辉,马旭,严研.一种基于二叉堆的Dijkstra 最短路径优化方法[J].工程数学学报,2021,38(05):709-720.
2. 算法原理
Dijkstra 算法是典型的单源最短路径计算算法，用于解决源点到其它所有点之间的最短路径计算的问题。它采用了贪心的思想搜索全局，求取最优解，搜索过程是以起点为圆心，向周围以同心圆的方式进行无序扩张搜索，搜索完全部节点后算法才终止。经典 Dijkstra 算法的搜索过程如下图所示，最外层的实线圆代表所有待搜索的点的集合。
而下图展示了 Dijkstra 算法的一般运算流程，为了更直观的描述运算过程，下面以图论的方法来描述 Dijkstra 算法：设 G=(V,E) 是一个带权有向图。其中 V 表示图中所有顶点的集合，E 表示图中每条边的长度权值。
将顶点集合 V 分为两组，第一组为已找到最短路径的顶点集合，用 Close 表示，初始 Close 集合中只包含有源节点，每求得一条最短路径, 就将对应的中间结点加入到集合 Close 中；
第二组为其余未确定最短路径的顶点集合，用 OPEN 表示。按最短路径的递增次序依次把第二组中的顶点加入 Close 中。
此外，每个顶点都对应着一个距离，Close 中的顶点的距离就是从 v 到此顶点的最短路径长度，OPEN 中的顶点的距离是从 v 到此顶点只包括 S 中的顶点为中间顶点的当前路径的最短长度。其中节点到自身的距离视为 0。
算法步骤如下：
Step1：初始时，生成集合 Close={v}，集合 OPEN={其余顶点}，集合 Close 和 OPEN 互补；
Step2：从集合 OPEN 中选取一个距离 v 最小的顶点 k，把 k 加入集合 Close 中（该距离就是 v 到 k 的最短路径），记录节点 v 为节点 k 的父节点；
Step3：以 k 为新考虑的中间点，修改 OPEN 集合中各顶点的距离：若从源点 v 到顶点 u 的距离比原来距离短，则修改顶点 u 的距离值，修改后的距离值为顶点 k 的距离加上边上的权，同时修改节点 k 的父节点；
Step4：重复 Step2 和 Step3 直到所有顶点都包含在集合 Close 中；
Step5：根据目标节点的父节点反向进行迭代，输出最短路径。
接下来将以下图所示的井下巷道的局部点网图为例，使用 Dijkstra 算法进行最优路径规划并列表进行算法的运算步骤说明，图中距离单位均为千米。
A 点为矿井副井口，即逃生终点，H 点为井下被困人员的当前位置，即逃生起点，搜索过程从 H 点开始，逐渐向外开始搜索，一直到搜索完所有节点才停止。
初始时，Close 表中仅包含起点 H，OPEN 表中包含有其余所有的节点。从 F 点开始进行搜索。下表中列出了基于 Dijkstra 算法的最短疏散路径求解过程。
从求解过程中可以看到，不管需要求取最短路径的是哪两个点，Dijkstra 算法总会求出从源节点到图 G 中所有顶点的最短路径。反映到算法的计算过程，就是将集合 S 从仅含有源节点的一个集合逐步变成为全集，U 集合变为空集。求取完成之后，再根据父节点进行推演得出所需求解的最短路径。
3. 代码实现
算法优点：鉴于 Dijkstra 算法的全局遍历性，其计算结果准确性非常高，Dijkstra 算法可以避开局部最优陷阱，100%的求解出最优路径。
算法缺点：但是正由于其要求遍历所有节点，在路径节点比较多的时候，计算速度会大大降低。由于 Dijkstra 算法使用了两次循环，所以它的时间复杂度为 
，其中 n 为图中的顶点个数。在顶点数较多的情况下，算法的运算效率将受到影响，
3.1 伪代码
算法的一般求解流程用伪代码表示如下：
3.2 python 代码
实现效果图如下，左图为搜索过程，右图为最终路径
老样子每句都有注释，有问题可以在评论区留言
import math
import matplotlib.pyplot as plt
min_set = 10
show_animation = True # 绘图
# 创建一个类
class Dijkstra:
# 初始化
def __init__(self, ox, oy, resolution, robot_radius):
# 属性分配
self.min_x = None
self.min_y = None
self.max_x = None
self.max_y = None
self.x_width = None
self.y_width = None
self.obstacle_map = None
self.resolution = resolution # 网格大小(m)
self.robot_radius = robot_radius #
self.calc_obstacle_map(ox, oy) # 绘制栅格地图
self.motion = self.get_motion_model() # 机器人运动方式
# 构建节点，每个网格代表一个节点
class Node:
def __init__(self, x, y, cost, parent_index):
self.x = x # 网格索引
self.y = y
self.cost = cost # 路径值
self.parent_index = parent_index # 该网格的父节点
def __str__(self):
return str(self.x) + ',' + str(self.y) + ',' + str(self.cost) + ',' + str(self.parent_index)
# 寻找最优路径，网格起始坐标(sx,sy)，终点坐标（gx,gy）
def planning(self, sx, sy, gx, gy):
# 节点初始化
# 将已知的起点和终点坐标形式转化为节点类型，0代表路径权重，-1代表无父节点
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()
# 先将起点入库，获取每个网格对应的key
open_set[self.calc_index(start_node)] = start_node
# 循环
while 1:
# 获取外库中损失最小的节点索引c_id, 获取该节点
c_id = min(open_set, key=lambda o: open_set[o].cost)
current = open_set[c_id] # 从字典中取出该节点
# 绘图
if show_animation:
# 网格索引转换为真实坐标
plt.plot(self.calc_position(current.x, self.min_x),
self.calc_position(current.y, self.min_y), 'xc')
plt.pause(0.001)
# 判断是否是终点，如果选出来的损失最小的点是终点
if current.x == goal_node.x and current.y == goal_node.y:
# 更新终点的父节点
goal_node.cost = current.cost
# 更新终点的损失
goal_node.parent_index = current.parent_index
break
# 在外库中删除该最小损失点，把它入库
del open_set[c_id]
closed_set[c_id] = current
# 遍历邻接节点
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)
# 获取该邻居节点的key
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
# 节点在外库中时
else:
# 如果该点到起点的距离，要小于外库中该点的距离，就更新外库中的该点信息，更改路径
if node.cost <= open_set[n_id].cost:
open_set[n_id] = node
# 找到终点
rx, ry = self.calc_final_path(goal_node, closed_set)
return rx, ry
# 机器人行走的方式，每次能向周围移动8个网格移动
@staticmethod
def get_motion_model():
# [dx, dy, cost]
motion = [[1,0,1], # 右
[0,1,1], # 上
[-1,0,1], # 左
[0,-1,1], # 下
[-1,-1,math.sqrt(2)], # 左下
[-1,1,math.sqrt(2)], # 左上
[1,-1,math.sqrt(2)], # 右下
[1,1,math.sqrt(2)]] # 右上
return motion
# 绘制栅格地图
def calc_obstacle_map(self, ox, oy):
# 地图边界坐标
self.min_x = round(min(ox)) # 四舍五入取整
self.min_y = round(min(oy))
self.max_x = round(max(ox))
self.max_y = round(max(oy))
# 地图的x和y方向的栅格个数，长度/每个网格的长度=网格个数
self.x_width = round((self.max_x-self.min_x)/self.resolution) # x方向网格个数
self.y_width = round((self.max_y-self.min_y)/self.resolution) # y方向网格个数
# 初始化地图，二维列表，每个网格的值为False
self.obstacle_map = [[False for _ in range(self.y_width)]
for _ in range(self.x_width)]
# 设置障碍物
for ix in range(self.x_width): # 遍历x方向的网格 [0:x_width]
x = self.calc_position(ix, self.min_x) # 根据网格索引计算x坐标位置
for iy in range(self.y_width): # 遍历y方向的网格 [0:y_width]
y = self.calc_position(iy, self.min_y) # 根据网格索引计算y坐标位置
# 遍历障碍物坐标(实际坐标)
for iox, ioy in zip(ox, oy):
# 计算障碍物和网格点之间的距离
d = math.hypot(iox-x, ioy-y)
# 膨胀障碍物，如果障碍物和网格之间的距离小，机器人无法通行，对障碍物膨胀
if d <= self.robot_radius:
# 将障碍物所在网格设置为True
self.obstacle_map[ix][iy] = True
break # 每个障碍物膨胀一次就足够了
# 根据网格编号计算实际坐标
def calc_position(self, index, minp):
# minp代表起点坐标，左下x或左下y
pos = minp + index * self.resolution # 网格点左下左下坐标
return pos
# 位置坐标转为网格坐标
def calc_xy_index(self, position, minp):
# (目标位置坐标-起点坐标)/一个网格的长度==>目标位置的网格索引
return round((position-minp) / self.resolution)
# 给每个网格编号，得到每个网格的key
def calc_index(self, node):
# 从左到右增大，从下到上增大
return node.y * self.x_width + node.x
# 邻居节点是否超出范围
def verify_node(self, node):
# 根据网格坐标计算实际坐标
px = self.calc_position(node.x, self.min_x)
py = self.calc_position(node.y, self.min_y)
# 判断是否超出边界
if px < self.min_x:
return False
if py < self.min_y:
return False
if px >= self.max_x:
return False
if py >= self.max_y:
return False
# 节点是否在障碍物上，障碍物标记为True
if self.obstacle_map[node.x][node.y]:
return False
# 没超过就返回True
return True
# 计算路径, parent属性记录每个节点的父节点
def calc_final_path(self, goal_node, closed_set):
# 先存放终点坐标（真实坐标）
rx = [self.calc_position(goal_node.x, self.min_x)]
ry = [self.calc_position(goal_node.y, self.min_y)]
# 获取终点的父节点索引
parent_index = goal_node.parent_index
# 起点的父节点==-1
while parent_index != -1:
n = closed_set[parent_index] # 在入库中选择父节点
rx.append(self.calc_position(n.x, self.min_x)) # 节点的x坐标
ry.append(self.calc_position(n.y, self.min_y)) # 节点的y坐标
parent_index = n.parent_index # 节点的父节点索引
return rx, ry
def main():
# 设置起点和终点
sx = -5.0
sy = -5.0
gx = 50.0
gy = 50.0
# 网格大小
grid_size = 2.0
# 机器人半径
robot_radius = 1.0
# 设置障碍物位置
ox, oy = [], []
for i in range(-10,60): ox.append(i); oy.append(-10.0) # 下边界
for i in range(-10,60): ox.append(60.0); oy.append(i) # 右边界
for i in range(-10,61): ox.append(i); oy.append(60.0) # 上边界
for i in range(-10,61): ox.append(-10.0); oy.append(i) # 左边界
for i in range(-10,40): ox.append(20.0); oy.append(i) # 左围栏
for i in range(0,40): ox.append(40.0); oy.append(60-i) # 右围栏
# 绘图
if show_animation:
plt.plot(ox, oy, '.k') # 障碍物黑色
plt.plot(sx, sy, 'og') # 起点绿色
plt.plot(gx, gy, 'xb') # 终点蓝色
plt.grid(True)
plt.axis('equal') # 坐标轴刻度间距等长
# 实例化，传入障碍物，网格大小
dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
# 求解路径，返回路径的 x 坐标和 y 坐标列表
rx, ry = dijkstra.planning(sx, sy, gx, gy)
# 绘制路径经过的网格
if show_animation:
plt.plot(rx, ry, '-r')
plt.pause(0.01)
plt.show()
if __name__ == '__main__':
main()