【路径规划】基于粒子群的三维无人机路径规划matlab源码

2021-10-10 00:01:40 阅读：195 来源： 互联网

标签：particle 路径 Update 源码 matlab Velocity Position model Best

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

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

【路径规划】基于粒子群的三维无人机路径规划matlab源码

1 简介

2 部分代码

3 仿真结果

4 参考文献