【路径规划】基于粒子群的三维无人机路径规划matlab源码
作者：互联网
1 简介
2 部分代码
%
clc;
clear;
close all;
%% Problem Definition
model=CreateModel();
model.n=8; % number of Handle Points ???????
CostFunction=@(x) MyCost(x,model); % Cost Function ????,@(x)????? ??????x
nVar=model.n; % Number of Decision Variables ?????
VarSize=[1 nVar]; % Size of Decision Variables Matrix ?????????
%%X??Y???????
VarMin.x=model.xmin; % Lower Bound of Variables
VarMax.x=model.xmax; % Upper Bound of Variables
VarMin.y=model.ymin; % Lower Bound of Variables
VarMax.y=model.ymax; % Upper Bound of Variables
VarMin.z=model.zmin;
VarMax.z=model.zmax;
%% PSO??????
%% PSO Parameters
MaxIt=100; % Maximum Number of Iterations ??????
nPop=50; % Population Size (Swarm Size) ?????
% w=1; % Inertia Weight ????
% wdamp=0.98; % Inertia Weight Damping Ratio ???????
wmax=0.9;
wmin=0.4;
w=wmax;
Lam = 20;
c1=1.5; % Personal Learning Coefficient ??????
c2=1.5; % Global Learning Coefficient ??????
% % Constriction Coefficient
% phi1=2.05;
% phi2=2.05;
% phi=phi1+phi2;
% chi=2/(phi-2+sqrt(phi^2-4*phi));
% w=chi; % Inertia Weight
% wdamp=1; % Inertia Weight Damping Ratio
% c1=chi*phi1; % Personal Learning Coefficient
% c2=chi*phi2; % Global Learning Coefficient
%%X??Y???????
alpha=0.1;
VelMax.x=alpha*(VarMax.x-VarMin.x); % Maximum Velocity
VelMin.x=-VelMax.x; % Minimum Velocity
VelMax.y=alpha*(VarMax.y-VarMin.y); % Maximum Velocity
VelMin.y=-VelMax.y; % Minimum Velocity
VelMax.z=alpha*(VarMax.z-VarMin.z);
VelMin.z=-VelMax.z;
%% Initialization ???
% Create Empty Particle Structure ?????,??????????????????????????????????
empty_particle.Position=[];
empty_particle.Velocity=[];
empty_particle.Cost=[];
empty_particle.Sol=[];%solution 瑙?
empty_particle.Best.Position=[];
empty_particle.Best.Cost=[];
empty_particle.Best.Sol=[];
% Initialize Global Best ?????????,????
GlobalBest.Cost=inf;
% Create Particles Matrix ???????
particle=repmat(empty_particle,nPop,1); %???????????? empty_particle ? nPop?1 ??
%%
%[empty_particle
%.
%.
%.
%empty_particle]
%%
% Initialization Loop ????????
for i=1:nPop
% Initialize Position ????????
if i > 1
particle(i).Position=CreateRandomSolution(model);
else
% Straight line from source to destination ???????????
xx = linspace(model.xs, model.xt, model.n+2);
yy = linspace(model.ys, model.yt, model.n+2);
zz = linspace(model.zs, model.zt, model.n+2);
particle(i).Position.x = xx(2:end-1);
particle(i).Position.y = yy(2:end-1);
particle(i).Position.z = zz(2:end-1);
end
% Initialize Velocity ????????
particle(i).Velocity.x=zeros(VarSize);
particle(i).Velocity.y=zeros(VarSize);
particle(i).Velocity.z=zeros(VarSize);
% Evaluation ?????????
[particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position);
% Update Personal Best ????????
particle(i).Best.Position=particle(i).Position;
particle(i).Best.Cost=particle(i).Cost;
particle(i).Best.Sol=particle(i).Sol;
% Update Global Best ????????
if particle(i).Best.Cost<GlobalBest.Cost
GlobalBest=particle(i).Best;
end
end
% Array to Hold Best Cost Values at Each Iteration ??????????????
BestCost=zeros(MaxIt,1);
%% PSO Main Loop PSO??????
for it=1:MaxIt
for i=1:nPop
% x Part x???
% Update Velocity ???? rand(VarSize)????VarSize*VarSize???
particle(i).Velocity.x = w*particle(i).Velocity.x ...
+ c1*rand(VarSize).*(particle(i).Best.Position.x-particle(i).Position.x) ...
+ c2*rand(VarSize).*(GlobalBest.Position.x-particle(i).Position.x);
% Update Velocity Bounds ??????,?????,????????????
particle(i).Velocity.x = max(particle(i).Velocity.x,VelMin.x);
particle(i).Velocity.x = min(particle(i).Velocity.x,VelMax.x);
% Update Position ????
particle(i).Position.x = particle(i).Position.x + particle(i).Velocity.x;
% Velocity Mirroring ????
OutOfTheRange=(particle(i).Position.x<VarMin.x | particle(i).Position.x>VarMax.x);
particle(i).Velocity.x(OutOfTheRange)=-particle(i).Velocity.x(OutOfTheRange);
% Update Position Bounds ??????
particle(i).Position.x = max(particle(i).Position.x,VarMin.x);
particle(i).Position.x = min(particle(i).Position.x,VarMax.x);
% y Part Y???
% Update Velocity ????
particle(i).Velocity.y = w*particle(i).Velocity.y ...
+ c1*rand(VarSize).*(particle(i).Best.Position.y-particle(i).Position.y) ...
+ c2*rand(VarSize).*(GlobalBest.Position.y-particle(i).Position.y);
% Update Velocity Bounds ??????
particle(i).Velocity.y = max(particle(i).Velocity.y,VelMin.y);
particle(i).Velocity.y = min(particle(i).Velocity.y,VelMax.y);
% Update Position ????
particle(i).Position.y = particle(i).Position.y + particle(i).Velocity.y;
% Velocity Mirroring ????
OutOfTheRange=(particle(i).Position.y<VarMin.y | particle(i).Position.y>VarMax.y);
particle(i).Velocity.y(OutOfTheRange)=-particle(i).Velocity.y(OutOfTheRange);
% Update Position Bounds ??????
particle(i).Position.y = max(particle(i).Position.y,VarMin.y);
particle(i).Position.y = min(particle(i).Position.y,VarMax.y);
% z Part
% Update Velocity ???? rand(VarSize)????VarSize*VarSize???
particle(i).Velocity.z = w*particle(i).Velocity.z ...
+ c1*rand(VarSize).*(particle(i).Best.Position.z-particle(i).Position.z) ...
+ c2*rand(VarSize).*(GlobalBest.Position.z-particle(i).Position.z);
% Update Velocity Bounds ??????,?????,????????????
particle(i).Velocity.z = max(particle(i).Velocity.z,VelMin.z);
particle(i).Velocity.z = min(particle(i).Velocity.z,VelMax.z);
% Update Position ????
particle(i).Position.z = particle(i).Position.z + particle(i).Velocity.z;
% Velocity Mirroring ????
OutOfTheRange=(particle(i).Position.z<VarMin.z | particle(i).Position.z>VarMax.z);
particle(i).Velocity.z(OutOfTheRange)=-particle(i).Velocity.z(OutOfTheRange);
% Update Position Bounds ??????
particle(i).Position.z = max(particle(i).Position.z,VarMin.z);
particle(i).Position.z = min(particle(i).Position.z,VarMax.z);
% Evaluation ????
[particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position);
% Update Personal Best ????????
if particle(i).Cost<particle(i).Best.Cost
particle(i).Best.Position=particle(i).Position;
particle(i).Best.Cost=particle(i).Cost;
particle(i).Best.Sol=particle(i).Sol;
% Update Global Best ????????
if particle(i).Best.Cost<GlobalBest.Cost
GlobalBest=particle(i).Best;
end
end
%CM
cm = tan(pi*(rand(1)-0.5));
particlenew(i).Position.x=particle(i).Position.x+cm*(particle(i).Position.x-GlobalBest.Position.x);
particlenew(i).Position.y=particle(i).Position.y+cm*(particle(i).Position.y-GlobalBest.Position.y);
particlenew(i).Position.z=particle(i).Position.z+cm*(particle(i).Position.z-GlobalBest.Position.z);
[particlenew(i).Cost, particlenew(i).Sol]=CostFunction(particlenew(i).Position);
if particlenew(i).Cost < particle(i).Cost
particle(i).Position = particlenew(i).Position
end
end
% Update Best Cost Ever Found ???????????
BestCost(it)=GlobalBest.Cost;
% Inertia Weight Damping ???????
% w=w*wdamp;
w=(wmax-wmin)*exp(-(((MaxIt-it)/MaxIt)*Lam)^2)+wmin;
% Show Iteration Information ??????
if GlobalBest.Sol.IsFeasible %???????
Flag=' *';
else
Flag=[', Violation = ' num2str(GlobalBest.Sol.Violation)]; %Violation ?? ;num2str?????????,???????fprintf?disp??????
end
disp(['Iteration ' num2str(it) ': Best Cost = ' num2str(BestCost(it)) Flag]);
% Plot Solution
figure(1); %??????????
PlotSolution(GlobalBest.Sol,model);
pause(0.1);
end
%% Results ??
figure;
%plot(BestCost,'LineWidth',2);
plot(BestCost,'r--','LineWidth',2);
hold on;
%plot(BestCost2,'r--','linewidth',2)
xlabel('Iteration');
ylabel('Best Cost');
grid on; %grid????????????,grid on?????,grid off?????
3 仿真结果
4 参考文献
-
O. Wilson, E., Sociobiology: The New Synthesis. 1976.
-
J Matari’c, M. and A. Brooks, Interaction and Intelligent Behavior. 1999.
-
Trelea, I.C., The particle swarm optimization algorithm: convergence analysis and parameter selection. Information Processing Letters, 2003. 85(6): p. 317-325.
-
Kennedy, J. and R. Eberhart. Particle swarm optimization. in Proceedings of ICNN’95 - International Conference on Neural Networks. 1995.
-
Zhan, Z., et al. Adaptive control of acceleration coefficients for particle swarm optimization based on clustering analysis. in 2007 IEEE Congress on Evolutionary Computation. 2007.
标签：particle,路径,Update,源码,matlab,Velocity,Position,model,Best 来源： https://blog.csdn.net/qq_59747472/article/details/120681072