# 【三维路径规划】基于matlab球面矢量粒子群算法无人机三维路径规划【含Matlab源码 1682期】

1 无人机航迹规划模型
1.1 航迹表示方法

1.2 航迹代价函数

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=100;          % Maximum Number of Iterations

nPop=100;           % 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

``````

## 五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]巫茜,罗金彪,顾晓群,曾青.基于改进PSO的无人机三维航迹规划优化算法[J].兵器装备工程学报. 2021,42(08)
[4]方群,徐青.基于改进粒子群算法的无人机三维航迹规划[J].西北工业大学学报. 2017,35(01)

THE END