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

无人机航迹规划是指在综合考虑无人机飞行油耗、威胁、飞行区域和自身物理条件限制的前提下， 为飞机在飞行区域内从初始点到目标点规划最佳或满意的飞行航迹， 其本质是多约束的目标优化。航迹规划算法是航迹规划的核心。相关研究在国内外相继进行， 提出了许多航迹规划算法, 如模拟退火算法、人工势场法、遗传算法、蚁群算法等。但由于无人机面临的规划空间极其复杂，规划约束多，模糊性大， 搜索算法存在寻优能力差、计算量大、效率低等问题， 航迹规划的最优性和实时性有待进一步提高。

粒子群优化算法 (particle swarm optimization, PSO)是Kennedy和Eberhart群体智能仿生算法于1995年提出， 在解决一些典型的函数优化问题时， 能取得较好的优化效果。

1 无人机航迹规划模型 1.1 航迹表示方法 一般地, 无人机航迹规划的空间可以表示为三维坐标系下所有点的集合{ (x, y, z) |xmin≤x≤xmax, ymin≤y≤ymax, zmin≤z≤zmax}, 其中x, y该节点在飞行水平面下的坐标， 这一点的经纬度也可以表示， z是高程数据或海拔高度。航迹规划的目的是在空间中获得无人机的飞行轨迹， 生成的航迹可以表示为三维空间的一系列点{PS, P1, P2, …, Pn-2, PG}, 直线段连接相邻航迹点。

1.2 航迹成本函数 在航迹规划中， 经常使用适当简化的航迹代价计算公式 式中, s表示航迹段数， Li表示第一段航迹的长度， 代表距离成本。Hi表示第一段航迹的平均海拔高度， 代表高成本。Ti是第一段航迹的威胁指数， 代表威胁代价。k1、k2、k三是距离成本、高成本和威胁成本的权重值， 权重的选择与飞行任务的要求有关。

2 基本粒子群算法 粒子群算法初始化为数量为N的随机粒子 (随机解) , 在D维空间中，通过重复迭代和更新自己的位置，搜索适应性值的最佳解。在搜索空间中，粒子的位置代表了题的潜在解决方案。在每次迭代中， 粒子通过跟踪两个极值来更新自己的速度和位置：一是粒子本身目前发现的最佳解， 也就是个体极值；另一种是目前整个粒子群找到的最优解， 即全局极值。粒子i (i=1, 2, …, N) 在第j (j=1, 2, …, D) 维的速度vij和位置xij按如下格式更新:

式中, ω为非负数, 称为惯性权值 (惯性因子) , 描述了粒子对以前速度的继承， 即反映粒子的惯性；c1和c二是非负常数， 叫学习因素 (加速因子) , 体现粒子的社会性， 即粒子向全球最优粒子学习的特性；r1和r2为 (0, 1) 随机数之间；pi= (pi1, pi2, …, pi D) 表示粒子i的个体极值所在位置;pg= (pg1, pg2, …, pg D) 表示所有粒子的整体极值。

第一项速度更新公式， 反映粒子当前速度的影响， 每个粒子按照惯性权值的比例沿自身速度方向搜索， 平衡全局， 第二项反映了个体最优值对粒子速度的影响， 即粒子本身的记忆和理解， 使粒子具有全局搜索能力。第三项反映了群体对个体的影响， 也就是说，群体之间的信息共享起着加速收敛的作用。

 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

1 matlab版本 2014a