在机器人路径规划之动态窗口法文中，介绍了一种局部路径规划方法——动态窗口法，本文将介绍一种全局路径规划方法——Dijkstra算法(狄克斯特拉算法)。Dijkstra算法是从一个顶点到其余各顶点的最短路径算法，解决的是有向图中最短路径问题。
其基本原理是：每次新扩展一个距离最短的点，更新与其相邻的点的距离。
以下图为例，计算左上角节点到右下角节点的最短路径，箭头上的数值表示两个节点间的距离
首先扩展第一个节点，计算其余节点与第一个节点的距离，如下图所示，用橙色标出已经扩展的节点，未扩展的节点仍用绿色标出，其中圆中的数值表示与第一个节点的距离， ∞ \infty ∞表示该节点没有直接到达此时已扩展节点的路径。
从未扩展的节点（绿色部分）中选择距离最小的节点进行拓展，并更新其余节点到第一个节点的距离，如下图
重复进行上面的步骤，直到所有节点都已扩展。
最后标出左上角节点到右下角节点的最短路径
参考：https://wiki.mbalib.com/wiki/Dijkstra%E7%AE%97%E6%B3%95
假定有如下的地图：
黑色边框为障碍物，要求找到左下角x到右上角x的最短距离，而且每次只能向周围（上、下、左、右、左上、左下、右上、右下）八个点移动
接下来使用Dijkstra算法来解决这个问题，先看最终效果：
为了解决这个问题，我们还需要定义一个节点类，包括自身位置、与开始位置的最短距离，以及在最短路径中前一个节点的索引
class Node:
def __init__(self, x, y, cost, pind):
self.x = x # 自身位置的x坐标
self.y = y # 自身位置的y坐标
self.cost = cost # 与开始位置的最短距离
self.pind = pind # 在最短路径中位于当前节点的前一个节点的索引
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
sx = 10.0 # 开始位置的x坐标
sy = 10.0 # 开始位置的y坐标
gx = 50.0 # 目标位置的x坐标
gy = 50.0 # 目标位置的y坐标
grid_size = 1.0 # 网格大小
robot_size = 1.0 # 机器人大小
ox = [] # 障碍物的x坐标列表
oy = [] # 障碍物的y坐标列表
# 向地图中添加障碍物
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
for i in range(15):
ox.append(5.0 + i)
oy.append(20)
for i in range(15):
ox.append(40.0 + i)
oy.append(40)
# 画出障碍物、开始位置、目标位置
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
nstart = Node(round(sx / grid_size), round(sy / grid_size), 0.0, -1)
ngoal = Node(round(gx / grid_size), round(gy / grid_size), 0.0, -1)
ox = [iox / grid_size for iox in ox]
oy = [ioy / grid_size for ioy in oy]
# 生成障碍物地图
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, grid_size, robot_size)
# 节点的可能移动情况
# x方向位移，y方向位移，移动距离
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)]]
初始化完成后，就是算法的循环过程：每次扩展一个距离最短的点，并更新与其相邻的点的距离
# 已扩展的节点，未扩展的节点
openset, closedset = dict(), dict()
# 将开始位置加入已扩展节点字典
openset[calc_index(nstart, xw, minx, miny)] = nstart
while True:
# 找到未扩展节点中距离最小的节点
c_id = min(openset, key=lambda o: openset[o].cost)
current = openset[c_id]
print("current", current)
# 显示当前扩展的节点
plt.plot(current.x * grid_size, current.y * grid_size, "xc")
if len(closedset.keys()) % 10 == 0: # 显示10个节点后暂停一下
plt.pause(0.001)
# pass
# 判断当前扩展的节点是不是目标节点
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break # 如果是则退出循环
# 将当前扩展的节点从未扩展节点字典中剔除
del openset[c_id]
# 将当前扩展的节点添加到已扩展节点字典
closedset[c_id] = current
# 循环将当前扩展节点周围的节点加入到未扩展节点字典
for i, _ in enumerate(motion):
node = Node(current.x + motion[i][0], current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
# 判断该节点是否在地图外或者处于障碍物上
if not verify_node(node, obmap, minx, miny, maxx, maxy):
# 是则进入下一轮循环
continue
# 如果该节点已经被扩展了，则进入下一轮循环
if n_id in closedset:
continue
# 如果该节点已经在未扩展节点字典中，则更新它到开始位置的最短距离
if n_id in openset:
if openset[n_id].cost > node.cost:
openset[n_id].cost = node.cost
openset[n_id].pind = c_id
# 否则加入到未扩展节点字典中
else:
openset[n_id] = node
# 计算最终路径
rx, ry = [ngoal.x * grid_size], [ngoal.y * grid_size]
pind = ngoal.pind
# 从最终节点依次向前递推得到最短路径
while pind != -1:
n = closedset[pind]
rx.append(n.x * grid_size)
ry.append(n.y * grid_size)
pind = n.pind
plt.plot(rx, ry, "-r")
plt.show()
# 判断节点是否在地图外或者处于障碍物上
def verify_node(node, obmap, minx, miny, maxx, maxy):
if obmap[node.x][node.y]:
return False
if node.x < minx:
return False
elif node.y < miny:
return False
elif node.x > maxx:
return False
elif node.y > maxy:
return False
return True
def calc_obstacle_map(ox, oy, grid_size, vr):
# 四舍五入取整
minx = round(min(ox))
miny = round(min(oy))
maxx = round(max(ox))
maxy = round(max(oy))
xwidth = round(maxx - minx)
ywidth = round(maxy - miny)
# 障碍物地图生成
# 有障碍物为True，否则为False
obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
for ix in range(xwidth):
x = ix + minx
for iy in range(ywidth):
y = iy + miny
for iox, ioy in zip(ox, oy): # zip：将对象打包为一个个元组
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
if d <= vr / grid_size:
obmap[ix][iy] = True
break
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
def calc_index(node, xwidth, xmin, ymin):
# 返回节点在地图中的索引：y*xw+x
return (node.y - ymin) * xwidth + (node.x - xmin)
程序参考：https://github.com/AtsushiSakai/PythonRobotics