内容列表
2.1 Dijkstra路径规划原理
杰斯特拉算法(Dijkstra)是由荷兰计算机科学家狄克斯特拉于1959年提出的，因此又叫狄克斯特拉算法。是从一个顶点到其余各顶点的最短路径算法，解决的是有权图中最短路径问题。迪杰斯特拉算法主要特点是从起始点开始，采用贪心算法的策略，每次遍历到始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止。
理论参考连接
2.2 Dijkstra算法图论示例
2.3 Dijkstra路径规划算法MATLAB代码
2.3.1 Matlab实现示例
MATLAB实现的路径规划动图如图：
2.3.2 主代码：Dijkstra_main.m
主代码，各部见注释，对其中关键数据的解释如图：
%% 定义一些基础数据，包括地图XY长宽，起点，终点，障碍物位置等
clc;clear;close all;
rows = 38; cols = 63; % 地图行列尺寸
startSub = [35,2]; % 起点行列位置
goalSub = [1,20]; % 终点行列位置
load('obsSub.mat'); % 加载地图数据，为障碍物的行列坐标值，n*2格式
dy_SearchArea =[]; % 存放所有遍历节点，用于动态绘制搜索区域
searchHead = 100;searchEnd = searchHead; % 动态绘制搜索区域的速度
% 当使用随机障碍物时，生成随机障碍物的行列值
% randr = randi([1,Rows],100,1);randc = randi([1,Cols],100,1);
% ObsSub = [randr randc];
%% 定义栅格地图，并初始化，此部分以后内容，均不用修改-------------------------------------------------
% 初始化地图数值为1，代表全空白区域
field = ones(rows, cols);
% 起点、终点、障碍物区域的权值
% Tips:对于单个点，Filed(x,y) = 2是可以的，但是多个点需要转成索引数组才能
field(startSub(1),startSub(2)) = 4;
field(goalSub(1),goalSub(2)) = 5;
% 后续路径规划算法，对于点的位置，均用索引值进行表示（输入变量只需要一个就行）
obsR = obsSub(:,1);obsC = obsSub(:,2);
obsIndex = sub2ind([rows,cols],obsR,obsC);
startIndex = sub2ind([rows,cols],startSub(1),startSub(2));
goalIndex = sub2ind([rows,cols],goalSub(1),goalSub(2));
field(obsIndex) = 2;
%% 绘制栅格地图-----------------------------------------------------------------------------------------
% 定义函数，列数，以及障碍物坐标
cmap = [1 1 1; ... % 1-白色-空地
0 0 0; ... % 2-黑色-静态障碍
1 0 0; ... % 3-红色-动态障碍
1 1 0;... % 4-黄色-起始点
1 0 1;... % 5-品红-目标点
0 1 0; ... % 6-绿色-到目标点的规划路径
0 1 1]; % 7-青色-动态规划的路径
% 绘制地图，安装数值不同显示不同颜色
colormap(cmap);
image(1.5,1.5,field);
% 设置栅格属性
grid on;hold on;
set(gca,'gridline','-','gridcolor','k','linewidth',0.5,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
set(gca, 'XAxisLocation','top')
axis image;
%% Dijkstra算法******************************************************************************************
% S/U的第一列表示栅格节点线性索引编号
% 对于S，第二列表示从起点到本节点Node已求得的最小距离；
% 对于U，第二列表示从起点到本节点Node暂时求得的最小距离
% U是n*2的向量，第一列表示地图某位置，第二列表示起点到该点的距离
U(:,1) = (1: rows*cols)'; % 将行向量转置成列向量
U(:,2) = inf; % 初始起点到各个点距离都是无穷inf
S = [startIndex, 0]; % 将起点放入S集合，起点到起点距离为0
U(startIndex,:) = []; % 在U集合中删除起点的信息
% ======================dijkstra第一段，更新起点的邻节点及代价，目的使得某些变量不为空，进入循环
% 函数DJ_NextNodes主要获得目标点附近8个点的距离值
nextNodes = Dijkstra_NextNodes(rows, cols, startIndex, field);
% 起点周围8个点更新在U中的权值，即距离值
for i = 1:8
next = nextNodes(i,1);
% 判断该子节点是否存在,然后将子节点权值更新到U
if ~isinf(next)
idx = find(U(:,1) == next); % 找到子节点在U数组的位置来修改值
U(idx,2) = nextNodes(i,2); % 更新子节点在U中的权值
end
end
% path为n*2向量，第一列存放地图上点的索引位置，第二列存放父节点到子节点的可达路径
for i = 1:rows*cols
% 存放点的索引值
path{i,1} = i;
end
% 更新起点到周围8个点的路径
% Tips:起点到任意有效点的路径都有，但是是否为最短路径，通过U集合比较来判断
% Tips:程序在这一步，并没有进行最短路径的比较
for i = 1:8
next = nextNodes(i,1);
if ~isinf(nextNodes(i,2))
% 起点到周围点的路径
path{next,2} = [startIndex,nextNodes(i,1)];
end
end
% =======================dijkstra第二段，各种数据不为空之后，进行循环遍历
while ~isempty(U)
% ************求最短路径在U集合找出当前最小距离值的节点，并移除该节点至S集合中
[dist_min, idx] = min(U(:,2)); % 在U集合中找出此时最短距离的节点和距离值
node = U(idx, 1); % 获得最短距离的节点索引值
S(end+1,:) = [node, dist_min]; % 最短距离的节点索引和距离添加到S集合
U(idx,:) = []; % 将其移除出U
% ===========在循环中绘制动态搜索的区域，即所有被判断过的Node组成的集合
if find(node==obsIndex)
elseif node == goalIndex
else
dy_SearchArea(end+1) = node; % 动态搜索区域不包括起点终点和障碍物点
field(dy_SearchArea) = 7; % 搜索区域为青色
if mod(searchHead,searchEnd) == 0 % 控制动图绘制速度
image(1.5,1.5,field); % 联合drawnow函数绘制动态效果
drawnow;
end
end
searchHead = searchHead + 1; % 更新刷新速度
% 从获得的新节点Node出发（相当于Node = startpos），进行循环判断求最短路径
nextNodes = Dijkstra_NextNodes(rows, cols, node, field);
% 依次遍历Node（父节点）的邻近节点（子节点），判断子节点是否在U集合中，更新子节点的距离值
for i = 1:8
next = nextNodes(i,1);
% 需要判断的子节点,子节点不为空，而且不在S集合里（已选择点的集合）
if ~isinf(next) && ~ismember(next, S(:,1))
idx_U = find(next == U(:,1)); % 找出U集合中节点childNode的索引值
cost = nextNodes(i,2); % 获取子节点距离值
% 判断是否更新，此部分见图解，理解
% Tips:首先U(:2)初始化默认为inf无穷大，需要一步步的判断更新距离值
if dist_min + cost < U(idx_U, 2)
U(idx_U, 2) = dist_min + cost;
path{next, 2} = [path{node, 2}, next]; % 更新最优路径
end
end
end
end
% =======================dijkstra第三段：找出目标最优路径
optPath = path{goalIndex,2}; % dijkstra算法，目标点在path的第二列上存放到目标点的最短路径集合
% Tips:从此中能看到，在未到达目标点钱，路径规划的最短路径都非正确的
% Tips:因此想要绘制动态路径曲线这种方法是不行的
% Tips:只能绘制动态的搜索区域（判断U集合的第二列距离值是否非inf）
%% 绘制最优路径的折线图***********************************************************************************
% 由于绘图函数plot针对的窗口是x,y，它们与row，col是的先后顺序是相反的,即X对应col，y对应row
image(1.5,1.5,field); % 将动画速度未显示完的搜索区域全部显示
field(dy_SearchArea) = 1; % 最后将搜索区域的值改回为空白值
[plotr,plotc] = ind2sub([rows,cols],optPath);
plot(plotc+0.5,plotr+0.5,'LineWidth',2.5);
2.3.3 函数代码：Dijkstra_NextNodes.m
function nextNodes = Dijkstra_NextNodes(rows,cols,nodeIndex,field)
% DJ_NEIGHBORNODES 获取当前点（父节点）的索引值和地图信息Field来进行下一个节点（子节点）的信息更新
% 更新的信息包括子节点的索引值和父节点到子节点的距离值
%% 将父节点装换成行列式位置信息，方便进行上下左右移动，并对存放子节点的数据进行初始化
[row,col] = ind2sub([rows,cols],nodeIndex); % 父节点装换成行列式位置信息
nextNodes = inf(8,2); % 子节点的信息列表
movePos = [-1,1;0,1;1,1;-1,0;1,0;-1,-1;0,-1;1,-1]; % 父节点移动方向列表
%% 更新父节点朝着各个方向移动到子节点的距离
for i = 1:8
% 父节点有效移动范围，不能越界
if 0 < row+movePos(i,1) && row+movePos(i,1) <= rows && 0 < col+movePos(i,2) && col+movePos(i,2) <= cols
subscript = [row+movePos(i,1),col+movePos(i,2)];
index = sub2ind([rows,cols], subscript(1),subscript(2));
nextNodes(i,1) = index;
% 子节点不为障碍物时，通过计算二范数公式计算距离
if field(subscript(1), subscript(2)) ~= 2 % 子节点不为障碍物
cost = norm(subscript - [row,col]); % 二范数计算距离
nextNodes(i,2) = cost; % 更新信息表
end
end
end
end
2.4 Dijkstra路径规划算法Python代码
可以参照PythonRobotics：（https://github.com/redglassli/PythonRobotics#a-algorithm）
本栏参代码逻辑和MATLAB是一致的
2.4.1 Python实现dijkstra示例
采用3 移动机器人路径规划（1- 栅格地图绘制）中第1.4.3热力图绘制的静态最短路径图像
采用3 移动机器人路径规划（1- 栅格地图绘制）中1.4.5的plot方法绘制的动态搜索和最短路径如下图
2.4.2 辅助函数PathPlanning.py
见3 移动机器人路径规划（1- 栅格地图绘制）的第1.4.2是一样的
2.4.3 Dijkstra路径规划Python代码
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
import seaborn as sns
from matplotlib import colors
import copy
import PathPlanning
'''
Dijkstra_NextNode(field,node)找寻周围节点路径函数，比MATLAB省略部分多余输入
'''
def Dijkstra_NextNode(field,node):
rows = len(field)
cols = len(field[0])
movepos = [[-1, 1], [0, 1], [1, 1], [-1, 0],
[1, 0], [-1, -1], [0, -1], [1, -1]]
nextnodes = [[np.inf,np.inf],[np.inf,np.inf],[np.inf,np.inf],[np.inf,np.inf],
[np.inf,np.inf],[np.inf,np.inf],[np.inf,np.inf],[np.inf,np.inf]]
# 将父节点装换成行列式位置信息，方便进行上下左右移动，并对存放子节点的数据进行初始化
nodeSub = PathPlanning.ind2sub([rows,cols],node)
## 更新父节点朝着各个方向移动到子节点的距离
for i in range(8):
r = nodeSub[0] + movepos[i][0]
c = nodeSub[1] + movepos[i][1]
# 父节点有效移动范围，不能越界
if -1 < r and r < rows and -1 < c and c < cols:
nextnodes[i][0] = PathPlanning.sub2ind([rows,cols],r,c)
# 子节点不为障碍物时，计算距离
if field[r, c] != 2:
dist = np.sqrt(movepos[i][0]*movepos[i][0]+movepos[i][1]*movepos[i][1])
nextnodes[i][1] = dist
return nextnodes
'''
# 初始化地图---------------------------------------------------------------------
'''
rows = 5
cols = 6
startSub = [2,1]
goalSub = [2,5]
obsSub = [[1,3],[2,3],[3,3]]
# 栅格地图属性
field = np.ones((rows, cols))
field[startSub[0], startSub[1]] = 4
field[goalSub[0], goalSub[1]] = 5
for i in range(len(obsSub)):
field[obsSub[i][0], obsSub[i][1]] = 2
# 数据转换
startIndex = PathPlanning.sub2ind([rows,cols],startSub[0], startSub[1])
goalIndex = PathPlanning.sub2ind([rows,cols],goalSub[0], goalSub[1])
'''
## Dijkstra算法******************************************************************************************
# S/U的第一列表示栅格节点线性索引编号
# 对于S，第二列表示从起点到本节点Node已求得的最小距离；
# 对于U，第二列表示从起点到本节点Node暂时求得的最小距离
'''
# U_pos存放索引点
# U_dist存放起点到该点的距离
U_pos = []
U_dist = []
for i in range(rows*cols):
U_pos.append(i)
U_dist.append(np.inf)
# 将起点放入S集合，起点到起点距离为0
S_pos = [startIndex]
S_dist = [0]
# 在U集合中删除起点的信息
idx = U_pos.index(startIndex)
U_pos.pop(idx)
U_dist.pop(idx)
# path为n维向量，默认0->rows*cols个，按照索引的一致性，存放点到点的路线
path = []
for i in range(rows*cols):
# append([数组])
path.append([-1])
# ======================dijkstra第一段，更新起点的邻节点及代价，目的使得某些变量不为空，进入循环
# 函数DJ_NextNodes主要获得目标点附近8个点的距离值
nextNodes = Dijkstra_NextNode(field,startIndex)
# 起点周围8个点更新在U中的权值，即距离值
for i in range(8):
nextnode = nextNodes[i][0]
# 判断该子节点是否存在,然后将子节点权值更新到U
if nextnode != np.inf:
idx = U_pos.index(nextnode)
U_dist[idx] = nextNodes[i][1]
# 更新起点到周围8个点的路径
# Tips:起点到任意有效点的路径都有，但是是否为最短路径，通过U集合比较来判断
# Tips:程序在这一步，并没有进行最短路径的比较
if nextNodes[i][1] != np.inf:
path[nextnode] = copy.deepcopy([startIndex,nextNodes[i][0]])
# searhx = []
# searhy = []
# # 新建画布指定大小
# fig = plt.figure(figsize=(4,3))
# # 新建子图
# ax = fig.add_subplot(111)
# startXY = PathPlanning.sub2xy([rows,cols],startSub[0],startSub[1])
# goalXY = PathPlanning.sub2xy([rows,cols],goalSub[0],goalSub[1])
# obsX = []
# obsY = []
# for i in range(len(obsSub)):
# obsxy = PathPlanning.sub2xy([rows,cols],obsSub[i][0],obsSub[i][1])
# obsX.append(obsxy[0])
# obsY.append(obsxy[1])
# =======================dijkstra第二段，各种数据不为空之后，进行循环遍历
while len(U_pos) > 0:
# ************求最短路径在U集合找出当前最小距离值的节点，并移除该节点至S集合中
idx = U_dist.index(min(U_dist))
dist_min = U_dist[idx]
node = U_pos[idx]
S_pos.append(node)
S_dist.append(dist_min)
U_pos.pop(idx)
U_dist.pop(idx)
# # 绘图1
# nodesub = PathPlanning.ind2sub([rows,cols],node)
# if field[nodesub[0]][nodesub[1]] == 1:
# nodexy = PathPlanning.sub2xy([rows,cols],nodesub[0],nodesub[1])
# searhx.append(nodexy[0])
# searhy.append(nodexy[1])
# 从获得的新节点Node出发（相当于Node = startpos），进行循环判断求最短路径
nextNodes = Dijkstra_NextNode(field,node)
# 依次遍历Node（父节点）的邻近节点（子节点），判断子节点是否在U集合中，更新子节点的距离值
for i in range(8):
nextnode = nextNodes[i][0]
if nextnode != np.inf:
if nextnode not in S_pos:
idx_u = U_pos.index(nextnode)
cost = nextNodes[i][1]
# 判断是否更新，此部分见图解，理解
# Tips:首先U(:2)初始化默认为inf无穷大，需要一步步的判断更新距离值
if dist_min+cost < U_dist[idx_u]:
U_dist[idx_u] = dist_min+cost
path[nextnode] = copy.deepcopy(path[node])
path[nextnode].append(nextnode)
# # 绘图2
# plt.plot(startXY[0],startXY[1],'r+')
# plt.plot(goalXY[0],goalXY[1],'b+')
# plt.plot(obsX,obsY,'sk')
# plt.plot(searhx,searhy,'sr')
# ax.set_xlim([-1,cols])
# ax.set_ylim([-1,rows])
# ax.set_xticks(np.arange(cols))
# ax.set_yticks(np.arange(rows))
# plt.pause(0.05)
# plt.cla()
opt_pathIndex = path[goalIndex]
optpathsub = []
for i in range(len(opt_pathIndex)):
optpathsub.append(PathPlanning.ind2sub([rows,cols],opt_pathIndex[i]))
optx = []
opty = []
for i in range(0,len(optpathsub),1):
field[optpathsub[i][0]][optpathsub[i][1]] = 6
optxy = PathPlanning.sub2xy([rows,cols],optpathsub[i][0],optpathsub[i][1])
optx.append(optxy[0])
opty.append(optxy[1])
field[startSub[0], startSub[1]] = 4
field[goalSub[0], goalSub[1]] = 5
# plt.plot(startXY[0],startXY[1],'r+')
# plt.plot(goalXY[0],goalXY[1],'b+')
# plt.plot(obsX,obsY,'sk')
# plt.plot(searhx,searhy,'sr')
# plt.plot(optx,opty,'b')
# ax.set_xlim([-1,cols])
# ax.set_ylim([-1,rows])
# ax.set_xticks(np.arange(cols))
# ax.set_yticks(np.arange(rows))
# plt.pause(5)
# PathPlanning.DrawHeatMap(field)
# plt.show()
2.5 0积分资源下载
代码直接复制即可运行
评论（2）
您还未登录，请登录后发表或查看评论