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

Posted 2021-05-18 Matlab走起

1 研究背景

粒子群算法的发展过程。粒子群优化算法（Partical Swarm Optimization PSO），粒子群中的每一个粒子都代表一个问题的可能解，通过粒子个体的简单行为，群体内的信息交互实现问题求解的智能性。由于PSO操作简单、收敛速度快，因此在函数优化、 图像处理、大地测量等众多领域都得到了广泛的应用。 随着应用范围的扩大，PSO算法存在早熟收敛、维数灾难、易于陷入局部极值等问题需要解决，主要有以下几种发展方向。

（1）调整PSO的参数来平衡算法的全局探测和局部开采能力。如Shi和Eberhart对PSO算法的速度项引入了惯性权重，并依据迭代进程及粒子飞行情况对惯性权重进行线性（或非线性）的动态调整，以平衡搜索的全局性和收敛速度。2009年张玮等在对标准粒子群算法位置期望及方差进行稳定性分析的基础上，研究了加速因子对位置期望及方差的影响，得出了一组较好的加速因子取值。

（2）设计不同类型的拓扑结构，改变粒子学习模式，从而提高种群的多样性，Kennedy等人研究了不同的拓扑结构对SPSO性能的影响。针对SPSO存在易早熟收敛，寻优精度不高的缺点，于2003年提出了一种更为明晰的粒子群算法的形式：骨干粒子群算法（Bare Bones PSO，BBPSO）。

（3）将PSO和其他优化算法（或策略）相结合，形成混合PSO算法。如曾毅等将模式搜索算法嵌入到PSO算法中，实现了模式搜索算法的局部搜索能力与PSO算法的全局寻优能力的优势互补。

（4）采用小生境技术。小生境是模拟生态平衡的一种仿生技术，适用于多峰函数和多目标函数的优化问题。例如，在PSO算法中，通过构造小生境拓扑，将种群分成若干个子种群，动态地形成相对独立的搜索空间，实现对多个极值区域的同步搜索，从而可以避免算法在求解多峰函数优化问题时出现早熟收敛现象。 Parsopoulos提出一种基于“分而治之”思想的多种群PSO算法，其核心思想是将高维的目标函数分解成多个低维函数，然后每个低维的子函数由一个子粒子群进行优化，该算法对高维问题的求解提供了一个较好的思路。

不同的发展方向代表不同的应用领域，有的需要不断进行全局探测，有的需要提高寻优精度，有的需要全局搜索和局部搜索相互之间的平衡，还有的需要对高维问题进行求解。这些方向没有谁好谁坏的可比性，只有针对不同领域的不同问题求解时选择最合适的方法的区别。

2 相关模型和思想

粒子群算法（ Particle Swarm Optimization, PSO）最早是由Eberhart和Kennedy于1995年提出，它的基本概念源于对鸟群觅食行为的研究。设想这样一个场景：一群鸟在随机搜寻食物，在这个区域里只有一块食物，所有的鸟都不知道食物在哪里，但是它们知道当前的位置离食物还有多远。最简单有效的策略？寻找鸟群中离食物最近的个体来进行搜素。PSO算法就从这种生物种群行为特性中得到启发并用于求解优化问题。

用一种粒子来模拟上述的鸟类个体，每个粒子可视为N维搜索空间中的一个搜索个体，粒子的当前位置即为对应优化问题的一个候选解，粒子的飞行过程即为该个体的搜索过程．粒子的飞行速度可根据粒子历史最优位置和种群历史最优位置进行动态调整．粒子仅具有两个属性：速度和位置，速度代表移动的快慢，位置代表移动的方向。每个粒子单独搜寻的最优解叫做个体极值，粒子群中最优的个体极值作为当前全局最优解。不断迭代，更新速度和位置。最终得到满足终止条件的最优解。

算法流程如下：

1、初始化

首先，我们设置最大迭代次数，目标函数的自变量个数，粒子的最大速度，位置信息为整个搜索空间，我们在速度区间和搜索空间上随机初始化速度和位置，设置粒子群规模为M，每个粒子随机初始化一个飞翔速度。

2、 个体极值与全局最优解

定义适应度函数，个体极值为每个粒子找到的最优解，从这些最优解找到一个全局值，叫做本次全局最优解。与历史全局最优比较，进行更新。

3、 更新速度和位置的公式

 

 

4、 终止条件

（1）达到设定迭代次数；（2）代数之间的差值满足最小界限

 

以上就是最基本的一个标准PSO算法流程。和其它群智能算法一样，PSO算法在优化过程中，种群的多样性和算法的收敛速度之间始终存在着矛盾．对标准PSO算法的改进，无论是参数的选取、小生境技术的采用或是其他技术与PSO的融合，其目的都是希望在加强算法局部搜索能力的同时，保持种群的多样性，防止算法在快速收敛的同时出现早熟收敛。

 
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?????
 

完整代码或者代写添加QQ1575304183