运行环境MatlabR2022a
注:原理可自行Google,这里只有代码实现。
关于Matlab坐标转换的问题自行Mathworks。
1.Dijkstra_main.m文件
clear;clc;close all;
rows = 20;
cols = 20;
sz = [rows cols];
start_sub = [20 1]; % 起始点
goal_sub = [1 20]; % 终点
%障碍物
%load写法
load obs_sub.mat -ascii % 这里面写障碍物的坐标(自己固定障碍物)
%随机障碍物
%rand_r = randi([1 rows], 100, 1); % 总共是100个障碍物, 100 × 1 列向量, 各值范围为1-rows
%rand_c = randi([1 cols], 100, 1); % 总共是100个障碍物, 100 × 1 列向量, 各值范围为1-cols
%obs_sub = [rand_r rand_c]; % 表示的是每个障碍物的坐标(row, col)
dy_search_area = []; % 存放所有遍历节点,用于动态绘制搜索区域
search_head = 100;
search_end = search_head;
%%初始化地图
%初始点、起始点颜色设置、索引设置
field = ones(sz);
%field(start_sub(1), start_sub(2)) = 4; % 单个点的话可以这样设置,多个点的话需要sub2ind,所以obs是多个需要转换
%field(goal_sub(1), goal_sub(2)) = 5;
start_ind = sub2ind(sz, start_sub(1), start_sub(2));
goal_ind = sub2ind(sz, goal_sub(1), goal_sub(2));
field(start_ind) = 4;
field(goal_ind) = 5;
%障碍物颜色设置、索引设置
obs_r = obs_sub(:, 1);
obs_c = obs_sub(:, 2);
obs_ind = sub2ind(sz, obs_r, obs_c);
field(obs_ind) = 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.2 0.8 0.6]; % 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表,第一列表示格栅的ind编号 第二列表示的是从起点到本节点Node已求得的最小距离
% U表,第一列表示格栅的ind编号 第二列表示的是从起点到本节点Node暂时求得的最小距离
% U表n×2的向量,第一列是位置ind索引,第二列是起点到改点的距离
U(:, 1) = (1:rows*cols)'; % 将格栅图的所有ind坐标存到U的第一列
U(:, 2) = inf; % 初始是起点到各个ind的位置为inf
U(start_ind, :) = []; % 将起始点信息从U表中删除
S = [start_ind 0]; % 将起点放进S表中 起点到起点的距离为0
%更新起点的相邻节点以及代价
next_nodes = Dijkstra_NextNodes(rows, cols, start_ind, field);
%将起点周围的点更新在U中,即在U中的位置
for i = 1:8
next = next_nodes(i, 1); % 将第一个子节点的ind存放在next中
%判断节点是否存在,然后将子节点的代价(距离)更新到U第二列中
if ~isinf(next) % 存在的话isinf(next)会是0因为next是一个确切的值不是inf 加个~的话整体值是1
index = find(U(:, 1) == next); % find找非0的索引即ind值,
U(index, 2) = next_nodes(i, 2); % 找到这个节点在U中的位置之后,对应的第二列的dist_cost做相应的修改
end
end
%path是n×2的向量,第一列存放地图上点的索引位置,第二列存放父节点到子节点的可达路径
for i = 1:rows*cols
path{i, 1} = i; % 把所有的图上的点都存进去path cell数组
end
%更新起点到周围8个点的路径
% 注:起点到任意有效点的路径都有,但是是否为最短路径,通过U集合比较来判断
% 注:程序在这一步,并没有进行最短路径的比较
for i = 1:8
next = next_nodes(i, 1); % 将此子节点存放到next
if ~isinf(next_nodes(i, 2)) % 如果父节点到此子节点的距离不为inf也就是两者之间有距离
% 此时将起始节点start_ind和起始节点start_ind到next这个子节点的欧氏距离存放到对应的子节点ind的path中第二列cell数组中
path{next, 2} = [start_ind, next_nodes(i, 1)];
end
end
%检查上述节点数据不为空之后进行循环遍历
while ~isempty(U) % U非空
% 此时是第一次从起点开始的8个子节点求最小值(min具体用法自行mathworks)最小值对应此次U中的index(1,2,...8)
[dist_min, index] = min(U(:, 2));
node = U(index, 1); % 找到最小值的ind
S(end+1, :) = [node, dist_min]; % 将这个最短距离的节点存和距离到S表中
U(index, :) = []; % 将这个节点从U表中去除
%绘制动态搜索区域即所有node节点,动态区域不包括起点终点和障碍物
if find(node == obs_ind)
elseif node == goal_ind
else
dy_search_area(end+1) = node; % 动态搜索区域
field(dy_search_area) = 7;
if mod(search_head, search_end) == 0 % 控制动图绘制速度
image(1.5, 1.5, field);
drawnow; %更新图窗并处理任何挂起的回调。如果您修改图形对象并且需要在屏幕上立即查看这次更新。
end
end
search_head = search_head + 10000; % 更新刷新速度
%从当前的node节点开始当做其实节点进行遍历,判断子节点是否自U集合中,更新子节点的距离
next_nodes = Dijkstra_NextNodes(rows, cols, node, field);
for i = 1:8
next = next_nodes(i, 1);
%判断子节点是否为空,并且不能在S集合(已选择的节点)里
if ~isinf(next) && ~ismember(next, S(:, 1))
index_U = find(next == U(:, 1));
cost = next_nodes(i, 2);
%判断是否更新,此时的next在U中的距离为inf
if dist_min + cost < U(index_U, 2)
U(index_U, 2) = dist_min + cost;
path{next, 2} = [path{node, 2}, next]; % 更新路径,此时next位置上的第二列就是起点到next的最短路径点的ind
end
end
end
end
%找到最优路径, 我只要终点的,也可以选择其他点
optimal_path = path{goal_ind, 2};
best_path = [20 95 114 172 290 309 364 381];
%绘制最优路径折线图
image(1.5, 1.5, field);
hold on;
field(dy_search_area) = 1;
[plotr, plotc] = ind2sub(sz, optimal_path);
[plot_r, plot_c] = ind2sub(sz, best_path);
plot(plotc + 0.5, plotr + 0.5, '-b', 'LineWidth', 2);
plot(plot_c + 0.5, plot_r + 0.5, '-r', 'LineWidth', 2);
scatter(plotc + 0.5, plotr + 0.5, 30, 'filled', MarkerEdgeColor='b', MarkerFaceColor='red');
%set(gca, 'xticklabel',[], 'yticklabel',[]) % 隐藏坐标轴的数字
%set(gca,'xtick',[], 'ytick',[]) % 隐藏坐标轴的刻度 也可以复合写
2.Dijkstra_NextNodes.m文件
function next_nodes = Dijkstra_NextNodes(rows, cols, parent_node_ind, field)
%首先将parent_node_ind的ind坐标转换为sub坐标
sz = [rows cols];
[row, col] = ind2sub(sz, parent_node_ind); % 先转换为行列表示方便后续8个方向上的移动
next_nodes = inf(8, 2); % 初始父节点下一个节点即8个方向上的子节点为inf, 后续存放更新的信息,大小8×2, 第一列表示子节点的位置,第二列表示父子的距离信息
move_pos = [1, 1; 1, 0; 0, 1; -1, 1; -1, 0; -1, -1; 0, -1; 1, -1]; % 8×2
%更新next_nodes,子节点坐标ind和父子距离
for i = 1:8
%前提是不能超出边界做判断,因为是行列表示,所以行列的坐标肯定要>0
if 0 < row + move_pos(i, 1) && row + move_pos(i, 1) <= rows && 0 < col + move_pos(i, 2) && col + move_pos(i, 2) <= cols
temp = [row + move_pos(i, 1), col + move_pos(i, 2)]; % 临时存放当前移动过去的子节点
ind = sub2ind(sz, temp(1), temp(2)); % 将当前子节点的sub坐标转换为ind坐标
next_nodes(i, 1) = ind; % 将当前子节点ind存放到next_nodes中的第一列
%判断当前子节点是否是障碍物不是的话计算父子的欧式距离
if field(ind) ~= 2 % 不是障碍物 注:如果是障碍物不用管默认为inf
dist_cost = norm([row col] - temp); % 一定得是行列表示法进行欧氏距离计算
next_nodes(i, 2) = dist_cost; % 更新父子节点的距离存放到next_nodes中的第二列
end
end
end
end
3.obs_sub.mat障碍物
写法很多,随机的也行固定的也行,或者不用加载mat文件直接在Dijkstra_NextNodes.m文件这样写obs_sub=[1, 2; 3, 4]等随便在里面添加前提是在地图范围内。
14 20
3 4
15 6
3 8
3 2
13 14
7 9
14 20
15 9
12 13
15 4
5 8
15 4
20 16
18 18
2 8
8 14
8 6
14 11
12 17
16 12
8 7
5 6
2 10
16 9
5 8
8 12
12 15
5 9
13 9
10 3
4 1
16 6
3 7
6 14
5 20
11 19
2 10
9 5
3 16
3 16
16 15
6 15
13 3
20 14
9 10
14 5
16 2
9 17
14 4
3 4
19 14
4 18
6 11
16 15
10 4
16 20
8 11
6 14
1 1
14 17
9 15
10 3
13 11
2 7
7 11
16 8
14 9
3 4
3 6
2 1
1 19
9 14
14 19
15 4
11 19
3 16
13 12
3 9
3 6
2 16
3 5
4 2
4 16
7 14
7 15
5 13
6 9
18 8
15 17
12 7
4 17
5 16
2 18
19 11
15 13
12 20
7 9
4 2
13 18
4.文件结构
最后运行的话在Dijkstra_main.m文件运行。
5.运行结果
OVER