【三維路徑規劃】基于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表示第i段航跡長度, 該項代表距離代價，Hi表示第i段航跡的平均海拔高度, 該項代表高度代價，Ti為第i段航跡的威脅指數, 該項代表威脅代價，k1、k2、k3分別是距離代價、高度代價和威脅代價的權重值, 權重的選取與飛行任務要求相關，

2 基本粒子群演算法
粒子群演算法初始化為一群數量為N的隨機粒子 (隨機解) , 在D維空間中通過重復迭代、更新自身的位置以搜索適應度值最優解，粒子的位置代表被優化問題在搜索空間中的潛在解，在每次迭代中, 粒子通過跟蹤2個“極值”來更新自己的速度和位置:一個是粒子自身目前所找到的最優解, 即個體極值;另一個是整個粒子群目前找到的最優解, 即全域極值，粒子i (i=1, 2, …, N) 在第j (j=1, 2, …, D) 維的速度vij和位置xij按如下格式更新:

式中, ω為非負數, 稱為慣性權值 (慣性因子) , 描述了粒子對之前速度的“繼承”, 即體現出粒子的“慣性”;c1和c2為非負常數, 稱為學習因子 (加速因子) , 體現了粒子的社會性, 即粒子向全域最優粒子學習的特性;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

四、運行結果

五、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)