算法背景
Dijkstra算法是一种用于在加权图中找到单个源点到所有其他顶点的最短路径的算法。这种算法由荷兰计算机科学家艾兹格·戴克斯特拉（Edsger W. Dijkstra）在1956年提出。它适用于有向图和无向图，并且图中的边权重必须是非负数。
基本原理
如下图所示，找到一条从v1（节点1）到v6（节点1）的最短路径，从起点开始逐步扩展，每一步为一个节点寻找到最短的路径。
主要是运行如下这个循环：
While True：
step1：从未访问的节点选择距离最小的节点收录（贪心思想）。
step2：收录节点后遍历该节点的邻接节点，并更新到起点的距离。
【注】为什么被收录的节点已经找到距离起点最短的路径？
（反证法）假设要收录上述的v4，但是有一条g（1->2->4）< g（1->4），这样就可以推出 g（1->2）< g（1->4），这样搞根据贪心思想需要收录的是v2，与假设矛盾！
算法流程
open list表示存放从当前节点到节点已经更新过的路径；
closed list表示截至目前到起点最短路径的节点进行收录。
那么针对下面这个图的存储流程应如下：
open list：1(0) #从起点开始，1(0)表示节点1到起点的距离为0
closed list：
open list：
closed list：1(0)
open list：2(2) 4(1) # 遍历收录节点的邻接节点
closed list：1(0)
open list：2(2)
closed list：1(0) 4(1)
open list：2(2) 3(3) 6(9) 7(5)
closed list：1(0) 4(1)
open list：3(3) 6(9) 7(5)
closed list：1(0) 4(1) 2(2)
open list：3(3) 6(9) 7(5) 5(13) #由于4号节点已经被收录已经是最优的，无需更新
closed list：1(0) 4(1) 2(2)
open list：6(9) 7(5) 5(13)
closed list：1(0) 4(1) 2(2) 3(3) #此处在遍历4号节点时，已经找到6号节点了，但是再遍历3号节点时距离为8比之前的9要短，所以更新一下
open list：6(8) 7(5) 5(13)
closed list：1(0) 4(1) 2(2) 3(3)
open list：6(8) 5(13)
closed list：1(0) 4(1) 2(2) 3(3) 7(5)
open list：6(6) 5(13)
closed list：1(0) 4(1) 2(2) 3(3) 7(5)
open list： 5(13)
closed list：1(0) 4(1) 2(2) 3(3) 7(5) 6(6)
获取遍历路径的方式：如下图，在6号节点中记录他来自7号父节点，再依次往前推，直至找到起点所设置的-1，停止，最终就能得到1->4->7->6路径。
栅格地图与节点编写方式：
机器人的运动方式：
案例实操
对起点[-5，-5],终点 [50, 50]在边长范围为[-10，60]的正方形进行路径规划，其中设置了两个墙体作为障碍物。
【实验代码】
"""
Grid based Dijkstra planning
author: Atsushi Sakai(@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import math
show_animation = True
class Dijkstra:
def __init__(self, ox, oy, resolution, robot_radius):
"""
Initialize map for planning
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
resolution: grid resolution [m]
rr: robot radius[m]
"""
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 #resolution就是传进来的栅格大小
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 # index of grid
self.y = y # 栅格坐标index of grid
self.cost = cost # 路径即代价值g(n)
self.parent_index = parent_index # 记录父节点寻找路径index of previous Node
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(
self.cost) + "," + str(self.parent_index)
def planning(self, sx, sy, gx, gy):
"""
dijkstra path search
input:
s_x: start x position [m]
s_y: start y position [m]
gx: goal x position [m]
gx: goal x position [m]
output:
rx: x position list of the final path
ry: y position list of the final path
"""
#设置起点与终点，前两个需要转换，路径代价先设为0，父节点没有的
start_node = self.Node(self.calc_xy_index(sx, self.min_x),
self.calc_xy_index(sy, self.min_y), 0.0, -1) #calc_xy_index从位置转换为下标round((position - minp) / self.resolution)
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 - value存储
open_set[self.calc_index(start_node)] = start_node#calc_index为栅格编号
while 1:#文档中的流程图
c_id = min(open_set, key=lambda o: open_set[o].cost) # 取cost最小的节点记为c_id
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.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
# 从open_set中删掉
del open_set[c_id]
# 添加至 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)#根据当前节点的xy加上运动方式的xy，当前的路径加上移动的代价求邻接节点
n_id = self.calc_index(node)#当前节点key的顺序，用来找路径
if n_id in closed_set:#判断当前节点key是否已经收录到closed_set，如若是，continue进入下一个循，若没有收录需要对值进行计算收录
continue
if not self.verify_node(node):#判断这个邻接节点是否超出地图范围或者是障碍物，如若是，continue进入下一个循
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:
#如果已经在open_set里面比较值大小进行，若小更新
open_set[n_id] = node
rx, ry = self.calc_final_path(goal_node, closed_set)#计算最后的路径
return rx, ry
def calc_final_path(self, goal_node, closed_set):
# generate final course
rx, ry = [self.calc_position(goal_node.x, self.min_x)], [
self.calc_position(goal_node.y, self.min_y)]#代码中的rx, ry两个列表存储路径，为了画图方便，存贮具体的数值而非下标
parent_index = goal_node.parent_index#从中带你的父节点开始
while parent_index != -1:
n = closed_set[parent_index]
rx.append(self.calc_position(n.x, self.min_x))
ry.append(self.calc_position(n.y, self.min_y))#从父节点的索引就得到他来自那一个节点放入rx，ry列表中
parent_index = n.parent_index#再把这个n节点的取出来循环到起点初始化的-1
return rx, ry
def calc_position(self, index, minp):
pos = index * self.resolution + minp#将下标与栅格大小相乘加上最小值的偏移量
return pos
def calc_xy_index(self, position, minp):
return round((position - minp) / self.resolution)
def calc_index(self, node):
return node.y * self.x_width + node.x #y乘x轴个数再加上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
if self.obstacle_map[node.x][node.y]:
return False
return True
def calc_obstacle_map(self, ox, oy):
''' 第1步：构建栅格地图 '''
self.min_x = round(min(ox))
self.min_y = round(min(oy))
self.max_x = round(max(ox))
self.max_y = round(max(oy))#地图的边界值，角落上的点
print("min_x:", self.min_x)
print("min_y:", self.min_y)
print("max_x:", self.max_x)
print("max_y:", self.max_y)
self.x_width = round((self.max_x - self.min_x) / self.resolution)
self.y_width = round((self.max_y - self.min_y) / self.resolution)#计算x轴与y轴方向具体的栅格个数x_width y_width，（最大-最小）/栅格大小
print("x_width:", self.x_width)
print("y_width:", self.y_width)
# obstacle map generation
# 初始化地图
self.obstacle_map = [[False for _ in range(self.y_width)]
for _ in range(self.x_width)]#地图采用二维向量表示，在代码中用两层的列表表示，false表示还没设置障碍物
# 设置障碍物
for ix in range(self.x_width):#前两层for循环就是用来遍历栅格的
x = self.calc_position(ix, self.min_x)#通过下标来计算位置
for iy in range(self.y_width):
y = self.calc_position(iy, self.min_y)
for iox, ioy in zip(ox, oy):#第三层for循环用来遍历障碍物
d = math.hypot(iox - x, ioy - y)#先是计算障碍物到栅格的距离
if d <= self.robot_radius:#膨胀障碍物构建 Configuration Space 构型空间的过程，如果距离比机器人的半径要小，机器人不能通行，把这一栅格设置为true
self.obstacle_map[ix][iy] = True
break
@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 main():
# 设置起点与终点start and goal position
sx = -5.0 # [m]
sy = -5.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 2.0 # 设置栅格大小 [m]
robot_radius = 1.0 # 设置机器人半径 [m]
# 设置障碍物位置，图中所有黑点，前四个for循环设置围成的四堵墙，后面的两个foe循环设置中间的两个，set obstacle positions
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.0 - i)
if show_animation: #画上面这些图 pragma: no cover
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)
rx, ry = dijkstra.planning(sx, sy, gx, gy)
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.pause(0.01)
plt.show()
if __name__ == '__main__':
main()
【运行结果】
