基于球向量的粒子群优化（SPSO）算法在无人机路径规划中的实现（Matlab代码实现）

 ‍个人主页：研学社的博客 

欢迎来到本博客❤️❤️

博主优势：博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

本文目录如下：

目录

1 概述

2 运行结果

3 参考文献

4 Matlab代码实现

---

1 概述

本文基于球向量的粒子群优化（SPSO）算法在无人机路径规划中的实现。目前的实施是用于无人机（UAV）的路径规划。但是，可以对其进行修改以应用于其他优化问题。

文献来源：

Manh Duong Phung, Quang Phuc Ha, "Safety-enhanced UAV Path Planning with Spherical Vector-based Particle Swarm Optimization", Journal of Applied soft computing, vol. 107, pp. 107376, 2021.

2 运行结果

 

 

部分代码：

clc;
clear;
close all;

%% Problem Definition

model = CreateModel(); % Create search map and parameters

CostFunction=@(x) MyCost(x,model);    % Cost Function

nVar=model.n;       % Number of Decision Variables = searching dimension of PSO = number of path nodes

VarSize=[1 nVar];   % Size of Decision Variables Matrix

% Lower and upper Bounds of particles (Variables)
VarMin.x=model.xmin;           
VarMax.x=model.xmax;           
VarMin.y=model.ymin;           
VarMax.y=model.ymax;           
VarMin.z=model.zmin;           
VarMax.z=model.zmax;                 

VarMax.r=2*norm(model.start-model.end)/nVar;           
VarMin.r=0;

% Inclination (elevation)
AngleRange = pi/4; % Limit the angle range for better solutions
VarMin.psi=-AngleRange;            
VarMax.psi=AngleRange;          

% Azimuth 
% Determine the angle of vector connecting the start and end points
dirVector = model.end - model.start;
phi0 = atan2(dirVector(2),dirVector(1));
VarMin.phi=phi0 - AngleRange;           
VarMax.phi=phi0 + AngleRange;           

% Lower and upper Bounds of velocity
alpha=0.5;
VelMax.r=alpha*(VarMax.r-VarMin.r);    
VelMin.r=-VelMax.r;                    
VelMax.psi=alpha*(VarMax.psi-VarMin.psi);    
VelMin.psi=-VelMax.psi;                    
VelMax.phi=alpha*(VarMax.phi-VarMin.phi);    
VelMin.phi=-VelMax.phi;                    

%% PSO Parameters

MaxIt=200;          % Maximum Number of Iterations

nPop=500;           % Population Size (Swarm Size)

w=1;                % Inertia Weight
wdamp=0.98;         % Inertia Weight Damping Ratio
c1=1.5;             % Personal Learning Coefficient
c2=1.5;             % Global Learning Coefficient

%% Initialization

% Create Empty Particle Structure
empty_particle.Position=[];
empty_particle.Velocity=[];
empty_particle.Cost=[];
empty_particle.Best.Position=[];
empty_particle.Best.Cost=[];

% Initialize Global Best
GlobalBest.Cost=inf; % Minimization problem

% Create an empty Particles Matrix, each particle is a solution (searching path)
particle=repmat(empty_particle,nPop,1);

% Initialization Loop
isInit = false;
while (~isInit)
        disp("Initialising...");
   for i=1:nPop

        % Initialize Position
        particle(i).Position=CreateRandomSolution(VarSize,VarMin,VarMax);

        % Initialize Velocity
        particle(i).Velocity.r=zeros(VarSize);
        particle(i).Velocity.psi=zeros(VarSize);
        particle(i).Velocity.phi=zeros(VarSize);

        % Evaluation
        particle(i).Cost= CostFunction(SphericalToCart(particle(i).Position,model));

        % Update Personal Best
        particle(i).Best.Position=particle(i).Position;
        particle(i).Best.Cost=particle(i).Cost;

        % Update Global Best
        if particle(i).Best.Cost < GlobalBest.Cost
            GlobalBest=particle(i).Best;
            isInit = true;
        end
    end
end

% Array to Hold Best Cost Values at Each Iteration
BestCost=zeros(MaxIt,1);

%% PSO Main Loop

for it=1:MaxIt

    % Update Best Cost Ever Found
    BestCost(it)=GlobalBest.Cost;

    for i=1:nPop          
        % r Part
        % Update Velocity
        particle(i).Velocity.r = w*particle(i).Velocity.r ...
            + c1*rand(VarSize).*(particle(i).Best.Position.r-particle(i).Position.r) ...
            + c2*rand(VarSize).*(GlobalBest.Position.r-particle(i).Position.r);

        % Update Velocity Bounds
        particle(i).Velocity.r = max(particle(i).Velocity.r,VelMin.r);
        particle(i).Velocity.r = min(particle(i).Velocity.r,VelMax.r);

        % Update Position
        particle(i).Position.r = particle(i).Position.r + particle(i).Velocity.r;

        % Velocity Mirroring
        % If a particle moves out of the range, it will moves backward next
        % time
        OutOfTheRange=(particle(i).Position.rVarMax.r);
        particle(i).Velocity.r(OutOfTheRange)=-particle(i).Velocity.r(OutOfTheRange);

        % Update Position Bounds
        particle(i).Position.r = max(particle(i).Position.r,VarMin.r);
        particle(i).Position.r = min(particle(i).Position.r,VarMax.r);

        % psi Part

        % Update Velocity
        particle(i).Velocity.psi = w*particle(i).Velocity.psi ...
            + c1*rand(VarSize).*(particle(i).Best.Position.psi-particle(i).Position.psi) ...
            + c2*rand(VarSize).*(GlobalBest.Position.psi-particle(i).Position.psi);

        % Update Velocity Bounds
        particle(i).Velocity.psi = max(particle(i).Velocity.psi,VelMin.psi);
        particle(i).Velocity.psi = min(particle(i).Velocity.psi,VelMax.psi);

3 参考文献

部分理论来源于网络，如有侵权请联系删除。

Manh Duong Phung, Quang Phuc Ha, "Safety-enhanced UAV Path Planning with Spherical Vector-based Particle Swarm Optimization", Journal of Applied soft computing, vol. 107, pp. 107376, 2021.Redirecting

4 Matlab代码实现