【路径规划】基于matlab Dijkstra算法多机器人路径规划【含Matlab源码 1883期】

一、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