1 简介

【路径规划】基于粒子群遗传求解多无人机三维路径规划matlab源码_无人机路径规划

【路径规划】基于粒子群遗传求解多无人机三维路径规划matlab源码_无人机路径规划_02

【路径规划】基于粒子群遗传求解多无人机三维路径规划matlab源码_无人机路径规划_03

【路径规划】基于粒子群遗传求解多无人机三维路径规划matlab源码_无人机路径规划_04

2 部分代码

clc;
  • 1.
clear;
  • 1.
close all;
  • 1.
model =CreateModel();
  • 1.
tic;
  • 1.
plotmap(model);
  • 1.
global_chromosome =Muti_Uav_Ga(model);
  • 1.
toc;
  • 1.
function [ cost,sol,costs ] = FitnessFunction( chromosome,model )
  • 1.
%UNTITLED2 Summary of this function goes here改进：用局部极值来进行交叉操作
  • 1.
%   Detailed explanation goes here
  • 1.
for uav=1:model.UAV
  • 1.
x= zeros(1,model.dim);
  • 1.
y= zeros(1,model.dim);
  • 1.
z = zeros(1,model.dim);
  • 1.
%取第uav个航路的坐标
  • 1.
for i=1:model.dim
  • 1.
x(i) = chromosome.pos(i,1,uav);
  • 1.
y(i) = chromosome.pos(i,2,uav);
  • 1.
z(i) = chromosome.pos(i,3,uav);
  • 1.
end
  • 1.
sx = model.sx(uav);
  • 1.
sy = model.sy(uav);
  • 1.
sz = model.sz(uav);
  • 1.
ex = model.endp(1);
  • 1.
ey =model.endp(2);
  • 1.
ez=model.endp(3);
  • 1.
xobs = model.xobs;
  • 1.
yobs = model.yobs;
  • 1.
zobs = model.zobs;
  • 1.
robs = model.robs;
  • 1.
XS=[sx x ex];
  • 1.
YS=[sy y ey];
  • 1.
ZS=[sz z ez];
  • 1.
k =numel(XS);
  • 1.
TP =linspace(0,1,k);
  • 1.
tt =linspace(0,1,50);
  • 1.
xx =[];
  • 1.
yy =[];
  • 1.
zz=[];
  • 1.
for i=1:k-1
  • 1.
%每一段向量分成10个点
  • 1.
x_r = linspace(XS(i),XS(i+1),10);
  • 1.
y_r= linspace(YS(i),YS(i+1),10);
  • 1.
z_r =linspace(ZS(i),ZS(i+1),10);
  • 1.
xx = [xx,x_r];
  • 1.
yy = [yy,y_r];
  • 1.
zz =[zz ,z_r];
  • 1.
end
  • 1.
%calc L
  • 1.
dx =diff(xx);
  • 1.
dy =diff(yy);
  • 1.
dz = diff(zz);
  • 1.
Length = sum(sqrt(dx.^2+dy.^2+dz.^2));
  • 1.
nobs = numel(xobs);
  • 1.
violation=0;
  • 1.
for i=1:nobs
  • 1.
d = sqrt( (xx-xobs(i)).^2+(yy-yobs(i)).^2 );
  • 1.
v = max(1-d/robs(i),0);
  • 1.
violation = violation + mean(v);
  • 1.
end
  • 1.
sol(uav).TP=TP;
  • 1.
sol(uav).XS =XS;
  • 1.
sol(uav).YS=YS;
  • 1.
sol(uav).ZS=ZS;
  • 1.
sol(uav).tt=tt;
  • 1.
sol(uav).xx=xx;
  • 1.
sol(uav).yy=yy;
  • 1.
sol(uav).zz=zz;
  • 1.
sol(uav).dx=dx;
  • 1.
sol(uav).dy=dy;
  • 1.
sol(uav).dz=dz;
  • 1.
sol(uav).Length=Length;
  • 1.
sol(uav).violation=violation;
  • 1.
sol(uav).IsFeasible=(violation==0);
  • 1.
%计算协调适应值
  • 1.
% 3、飞行高度限制
  • 1.
high=0;
  • 1.
for k=1:numel(XS)
  • 1.
x=XS(k);
  • 1.
y=YS(k);
  • 1.
h=terrain(x,y);
  • 1.
if ZS(k)<=(h+10)  %限制飞行最低高度
  • 1.
high=high+10000;
  • 1.
elseif ZS(k)>375   %限制飞行最高高度
  • 1.
high=high+10000;
  • 1.
else
  • 1.
high=high+abs(ZS(k)-287); %计算与理想高度差距和
  • 1.
end
  • 1.
end
  • 1.
%z
  • 1.
w1 =0.03;
  • 1.
w2=0.3;
  • 1.
w3=0.1;
  • 1.
w4=0.2;
  • 1.
%markov evaluatea
  • 1.
%获取所有维度的坐标
  • 1.
r_xx=[];r_yy=[];r_zz=[];
  • 1.
for i=2:numel(XS)-2
  • 1.
%每一段向量分成10个点
  • 1.
r_x = linspace(XS(i),XS(i+1),3);
  • 1.
r_y= linspace(YS(i),YS(i+1),3);
  • 1.
r_z =linspace(ZS(i),ZS(i+1),3);
  • 1.
r_xx = [r_xx,r_x];
  • 1.
r_yy = [r_yy,r_y];
  • 1.
r_zz =[r_zz ,r_z];
  • 1.
end
  • 1.
Allpos = [r_xx',r_yy',r_zz'];
  • 1.
[stateProbabilityProcess, expectedCostProcess]=MarkovEvaluate(Allpos,model);
  • 1.
sol(uav).MarkovState = stateProbabilityProcess;
  • 1.
sol(uav).MarkovCost = expectedCostProcess;
  • 1.
sol(uav).costs=[w1*Length,w3*high,w4*mean(expectedCostProcess)*1000];
  • 1.
single_cost(uav)= w1*Length +w3*high+w4*mean(expectedCostProcess)*1000;
  • 1.
sol(uav).cost =single_cost(uav);
  • 1.
end
  • 1.
cost =sum(single_cost);
  • 1.
costs=single_cost;
  • 1.
end
  • 1.

点击并拖拽以移动

3 仿真结果

【路径规划】基于粒子群遗传求解多无人机三维路径规划matlab源码_无人机路径规划_06点击并拖拽以移动

4 参考文献

[1]江冰, 郭彭. 基于粒子群算法的三维无人机路径规划方法及规划系统:. 

【路径规划】基于粒子群遗传求解多无人机三维路径规划matlab源码_无人机路径规划_08

​