【路径规划】基于matlab Dijkstra算法多机器人路径规划【含Matlab源码 1883期】
【摘要】
一、Dijkstra算法机器人编队路径规划简介
1 传统Dijkstra算法步骤 Dijkstra算法是一种经典的求解最短路径的算法，用于计算一个节点去往其他各个不相关节点的最小移动代价。本思想是把在图...
一、Dijkstra算法机器人编队路径规划简介
1 传统Dijkstra算法步骤
Dijkstra算法是一种经典的求解最短路径的算法，用于计算一个节点去往其他各个不相关节点的最小移动代价。本思想是把在图中无论如何出现的所有节点分离为2组，第1组包含被准确认定是最短路径的节点，第2组存放待检查的不确定的节点。根据最小移动代价逐渐增大的顺序，逐个将第2组需要检查的节点加入到第1组中，一直到从起始点出发可以到达的所有节点都包含在第1组中。Dijkstra算法运行的主要特征是以机器人抽象的出发点为中心向外层层延伸，直到延伸到整个区域的末端为止。Dijkstra算法一般能够在最终得到最优的路径，但是这个路径偶尔会出现冗余拐点，由于其遍历节点多，所以有时算法的效率不高。Dijkstra算法流程如图2所示。
分别创建2个表，为：START与FINISH。START表中存放环境中已经生成存在但是未经计算的节点，FINISH表中存放所有经过计算考察的节点。
(1）观测寻找在环境路网中离起始源点近而且是没有被计算过的点，把满足调节的点放在START数组中等待检查。
(2）从START表中找出距离起始点最小的点，然后把该点放进FINISH表中。
(3）遍历去寻找考察当前此点的子节点，计算出起始源点到这些子节点的距离值，将值存入数组中用来排序，将子节点放在START表中。
(4）重复（2）和（3），直到把START表中的点全部清空，或者找到真正要去的目标点。
2 Dijkstra算法的思想
(1）将环境可视图中出现的节点或是顶点分别加入2个组，其中一组存放经过求解验证为最短路径所经过的顶点，该集合用FINISH表示，刚开始时START表中只有1个起始点，在后面的运算过程中每得到1条最短路径，就把相关节点加入到集合FINISH中。如果全部的顶点都加入到FINISH后，此算法运算完成。另一组存放未经过考察且还未被认定为最短路径的顶点，用START表示。
(2）在将顶点逐渐加入到FINISH表的过程中，在运行算法期间总保持从出发点到FINISH表中各个点的最短路径的长度不超过出发点到START表中任何顶点的最短路径长度。
典型的Dijkstra算法在寻找最优路径的准则上较为单一，以某一个点为中心向外层层扩展就类似于画圆，随着搜索半径的逐渐增大，最后总能找到想要的目标点，但同时最优路径会出现一些无关的冗余点，这样在一定程度上给实际的移动机器人造成寻找路径的困难。如果能够对经典算法的计算准则进行优化，达到减少路径中拐点的目的，就可以减少一大部分机器人寻路所耗费的时间。在大面积环境中使用Dijkstra算法时，由于每次都要从START列表中挑选离原始点最近的点，这样导致规划效率不高，在排序过程消耗很多不必要的工作时间。
二、部分源代码
%define a map
close all ; clear all ;
map = false(14) ;
map(3,3:4) = true ; map(3,7:8) = true ; map(3,11:12) = true ;
map(4,3:4) = true ; map(4,7:8) = true ; map(4,11:12) = true ;
map(7,3:4) = true ; map(7,7:8) = true ; map(7,11:12) = true ;
map(8,3:4) = true ; map(8,7:8) = true ; map(8,11:12) = true ;
map(11,3:4) = true ; map(11,7:8) = true ; map(11,11:12) = true ;
map(12,3:4) = true ; map(12,7:8) = true ; map(12,11:12) = true ;
%define the weight of node
weight = [0 1 0 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 0 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 0 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 0 0; 0 0 0 1;%第一列
0 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0 ;1 0 1 0 ; 1 0 0 1; %第二列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第三列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第四列
0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 0 0 1;%第五列
0 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1;%第六列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第七列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第八列
0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 0 0 1;%第九列
0 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 1;%第十列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第十一列
0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1;%第十二列
0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 1 0 1; 0 1 0 0; 0 1 0 0; 0 1 1 0; 0 0 0 1;%第十三列
0 0 1 0; 1 0 0 0; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 0; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 0; 1 0 0 0; 1 0 0 0; 1 0 1 0; 1 0 0 0;%第十四列
] ;
%start point array and destination point array
start_coords_array = [6,2; 1,1; 5,9; 9,14; 5,8; 12,10; 14,7; 3,14; 1,9; 14,10 ];
dest_coords_mark_array =[9 3 2 8 3 1 3 4 5 3];
FLAG = zeros(1, 10);
[map_color, route, dest_node] = multi_path_generation( start_coords_array, dest_coords_mark_array, map, weight);
%初始化机器人开始的位置并且显示出来
for j = 1:10
robot_color = 16+j;
map_color(route(j, 1)) = robot_color;
end
image(1.5, 1.5, map_color);
grid on;
axis image;
pause(0.5);
for i = 1:100
for j = 1:10
robot_color = 16+j;
if (route(j, i+1)) %如果未达到终点
[exist_robot, collision_robot_index, is_dest_node] = detect_robot_state(map_color, dest_coords_mark_array, route, [j, i], dest_node);
if (~exist_robot) %如果路径可行，则直接移动到下一个路径点
map_color(route(j, i+1)) = robot_color;
map_color(route(j, i)) = 1;
else %否则路径上存在机器人
if (is_dest_node) %如果机器人在终点，则进行“绕桌”行为
%障碍物机器人绕桌
[route, dest_node] = desk_surround(map, route, collision_robot_index, j, i, dest_node, dest_coords_mark_array);
%当前机器人等待
[nrows, ncols] = size(route);
route(j, 1:ncols+1) = add_path_point(route(j, :), route(j, i),i+1);
map_color(route(j, i+1)) = robot_color;
else %如果障碍物不在终点，当前机器人等待即可
%当前机器人等待
[nrows, ncols] = size(route);
route(j, 1:ncols+1) = add_path_point(route(j, :), route(j, i),i+1);
map_color(route(j, i+1)) = robot_color;
end
end
else %如果已经达到终点,原地等待，并且添加一个FLAG
[nrows, ncols] = size(route);
route(j, 1:ncols+1) = add_path_point(route(j, :), route(j, i),i+1);
map_color(route(j, i+1)) = robot_color;
FLAG(j) = 1;
end
%显示过程
image(1.5, 1.5, map_color);
grid on;
axis image;
pause(0.01);
%终止条件
if FLAG == ones(1, 10)
disp(route');
success = true;
[row, col] = size(route);
for n = 1:col
if route(:, n) ==zeros(10,1)
break;
end
for m = 1:row
if route(m,n)
route_final(m, n)= route(m, n);
else
route_final(m, n) = route_final(m, n-1);
end
end
end
disp(route_final);
route_final = route_final';
fid = fopen('path_point.txt', 'wt');
[row, col] = size(route_final);
disp(row);
for m = 1:row
for n = 1:col
if route_final(m, n)
fprintf(fid, '%g\t', route_final(m,n));
end
end
fprintf(fid, '\n');
end
return;
end
[nrows, ncols] = size(route);
if (ncols > 500)
success = false;
disp('FAILED..........................!');
return;
end
end
end
% cmap = [1 1 1; ...
% 0 0 0; ...
% 1 0 0; ...
% 0 0 1; ...
% 0 1 0; ...
% 1 1 0; ...
% 0.9 0.9 0.9; %路径1
% 0.8 0.8 0.8; %路径2
% 0.7 0.7 0.7; %路径3
% 0.6 0.6 0.6; %路径4
% 0.5 0.5 0.5; %路径5
% 0.4 0.4 0.4; %路径6
% 0.3 0.3 0.3; %路径7
% 0.2 0.2 0.2; %路径8
% 0.1 0.1 0.1; %路径9
% 0.05 0.05 0.05; %路径10
% 1 0.75 0.8; %机器人1
% 0.86 0.08 0.24; %机器人2
% 0.5 0 0.5; %机器人3
% 0.25 0.4 0.86; %机器人4
% 0.44 0.5 0.56; %机器人5
% 0 1 1; %机器人6
% 1 0.84 0; %机器人7
% 1 0.55 0; %机器人8
% 0.74 0.56 0.56; %机器人9
% 0.12 0.56 1]; %机器人10
% colormap(cmap);
% for i = 1:9
% dest_coords = dest_mark_2_dest_coords(i);
% for j = 1: length(dest_coords)
% map(dest_coords(j,1) , dest_coords(j,2)) = i+1;
%
% end
%
% end
% image(1.5, 1.5, map);
% grid on;
% axis image;
% disp(map)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
三、运行结果
四、matlab版本及参考文献
1 matlab版本
2014a
2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]陈智康,刘佳,王丹丹,张运喜.改进Dijkstra机器人路径规划算法研究[J].天津职业技术师范大学学报. 2020,30(03)
3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除
文章来源: qq912100926.blog.csdn.net，作者：海神之光，版权归原作者所有，如需转载，请联系作者。
原文链接：qq912100926.blog.csdn.net/article/details/125288079
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）