【路径规划】Dijkstra算法机器人编队路径规划【含Matlab源码 1884期】

✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭：行百里者，半于九十。

更多Matlab仿真内容点击👇
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（Matlab）
语音处理（Matlab）
信号处理（Matlab）
车间调度（Matlab）

⛄一、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列表中挑选离原始点最近的点，这样导致规划效率不高，在排序过程消耗很多不必要的工作时间。

⛄二、部分源代码

clc;
clear all;
close all;
cmap = [1 1 1; …
0 0 0; …
1 0 0; …
0 0 1; …
0 1 0; …
1 1 0; …
0.5 0.5 0.5];

colormap(cmap);
%% Define a small map
map_size = 100;
map = false(map_size);
%% Read Picture For Goal Position
picture=imread(‘1.png’); %Read a picture
GoalPosition = ReadPictureForSketch(picture, map_size);
%Problem Defination
nVar = 2; %Number of Variables
VarSize = [1, nVar]; %Matrix Size of Decision Variables
nPop = length(find(GoalPosition==1)); %Population Size(Robot Number)
%Initialization

%The Particle Template
empty_particle.Position = [];
empty_particle.NextPosition = [];
empty_particle.Velocity = [];
empty_particle.Cost = [];
empty_particle.Goal = [];
empty_particle.Best.Position = [];
empty_particle.Best.Cost = [];
empty_particle.Best.Index = [];

%Create Population Array
particle = repmat(empty_particle, nPop, 1);

%Initialize Global Best
GlobalBest.Cost = inf;

%%Initialize Position and Goal
[goal_location_x_array goal_location_y_array] = find(GoalPosition==1);
for i = 1:nPop
[start_x start_y] = ind2sub(size(map), i+0.5*size(map, 1)*size(map, 1));
particle(i).Position = [start_y start_x];
particle(i).Goal = [goal_location_x_array(i) goal_location_y_array(i)];
end

%initial the map to show
map_plot = zeros(map_size);
map_plot(~map) = 1; % Mark free cells
map_plot(map) = 2; % Mark obstacle cells

%%

while(1)
for i = 1:nPop
neighbours = GetSurroundingPositions(particle(i).Position);
MinCost = inf;
particle(i).NextPosition =zeros(VarSize);
%Get the cloest point to goal
for j = 1:size(neighbours, 1)
current_x = neighbours(j, 1);
current_y = neighbours(j, 2);
if (current_x >0 && current_x <= size(map, 1) && current_y >0 && current_y <= size(map, 1)) %the point in the field
if (map_plot(current_x, current_y) ~=2) %the point is not collision
DistanceToGoal = abs(particle(i).Goal(1) - current_x) + abs(particle(i).Goal(2) - current_y);
if(DistanceToGoal < MinCost) %the point is closer than others
particle(i).NextPosition(1) = current_x;
particle(i).NextPosition(2) = current_y;
MinCost = DistanceToGoal;
end
end
end
end
end
GoalExchangeArray = zeros(1, nPop);
for i = 1:nPop
for j = i:nPop
if(GoalExchangeArray(i) || GoalExchangeArray(j))
break
end
GoalExchange = false;
if (isempty(setdiff(particle(i).NextPosition, particle(j).Position)) && isempty(setdiff(particle(j).NextPosition, particle(i).Position)))
GoalExchange = true;
end

% if (isempty(setdiff(particle(i).NextPosition, particle(j).Position)) && isempty(setdiff(particle(j).NextPosition, particle(j).Position)))
% GoalExchange = true;
% end
%
% if (isempty(setdiff(particle(j).NextPosition, particle(i).Position)) && isempty(setdiff(particle(i).NextPosition, particle(i).Position)))
% GoalExchange = true;
% end
if(GoalExchange && (mod(randperm(100,1), 10) > 5))
TempGoal = particle(i).Goal; %exchange the goal
particle(i).Goal = particle(j).Goal;
particle(j).Goal =TempGoal;
particle(i).NextPosition = particle(i).Position; %keep the particel static
particle(j).NextPosition = particle(j).Position;
GoalExchangeArray(i) = 1;
GoalExchangeArray(j) = 1;
end
end
end
%check if the NextPosition will colllision with other particles or not
for i= 1:nPop
if(map_plot(particle(i).NextPosition(1), particle(i).NextPosition(2)) ~=1)
particle(i).NextPosition = particle(i).Position;
end
%Update the map_plot
map_plot(particle(i).Position(1), particle(i).Position(2)) = 1;
particle(i).Position = particle(i).NextPosition;
map_plot(particle(i).Position(1), particle(i).Position(2)) = 4;
end
%change the goal according to the position of the particle randomly
if (mod(randperm(100,1), 11) > 3)
goal_assigned = GoalAssignment(reshape([particle(1:nPop).Position], 2, nPop)‘, reshape([particle(1:nPop).Goal], 2, nPop)’, map_size);
for i=1:nPop
particle(i).Goal(😃 = goal_assigned(i, 😃;
end
end
%Display
ShowImage(map_plot);
%check the task is finished or not

end

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]陈智康,刘佳,王丹丹,张运喜.改进Dijkstra机器人路径规划算法研究[J].天津职业技术师范大学学报. 2020,30(03)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合