【路徑規(guī)劃】基于粒子群的無人機(jī)三維路徑規(guī)劃含障礙matlab源碼
粒子群算法
1.1 研究背景
粒子群算法的發(fā)展過程。粒子群優(yōu)化算法（Partical Swarm Optimization PSO），粒子群中的每一個(gè)粒子都代表一個(gè)問題的可能解，通過粒子個(gè)體的簡單行為，群體內(nèi)的信息交互實(shí)現(xiàn)問題求解的智能性。由于PSO操作簡單、收斂速度快，因此在函數(shù)優(yōu)化、 圖像處理、大地測量等眾多領(lǐng)域都得到了廣泛的應(yīng)用。 隨著應(yīng)用范圍的擴(kuò)大，PSO算法存在早熟收斂、維數(shù)災(zāi)難、易于陷入局部極值等問題需要解決，主要有以下幾種發(fā)展方向。
（1）調(diào)整PSO的參數(shù)來平衡算法的全局探測和局部開采能力。如Shi和Eberhart對PSO算法的速度項(xiàng)引入了慣性權(quán)重，并依據(jù)迭代進(jìn)程及粒子飛行情況對慣性權(quán)重進(jìn)行線性（或非線性）的動(dòng)態(tài)調(diào)整，以平衡搜索的全局性和收斂速度。2009年張瑋等在對標(biāo)準(zhǔn)粒子群算法位置期望及方差進(jìn)行穩(wěn)定性分析的基礎(chǔ)上，研究了加速因子對位置期望及方差的影響，得出了一組較好的加速因子取值。
（2）設(shè)計(jì)不同類型的拓?fù)浣Y(jié)構(gòu)，改變粒子學(xué)習(xí)模式，從而提高種群的多樣性，Kennedy等人研究了不同的拓?fù)浣Y(jié)構(gòu)對SPSO性能的影響。針對SPSO存在易早熟收斂，尋優(yōu)精度不高的缺點(diǎn)，于2003年提出了一種更為明晰的粒子群算法的形式：骨干粒子群算法（Bare Bones PSO，BBPSO）。
（3）將PSO和其他優(yōu)化算法（或策略）相結(jié)合，形成混合PSO算法。如曾毅等將模式搜索算法嵌入到PSO算法中，實(shí)現(xiàn)了模式搜索算法的局部搜索能力與PSO算法的全局尋優(yōu)能力的優(yōu)勢互補(bǔ)。
（4）采用小生境技術(shù)。小生境是模擬生態(tài)平衡的一種仿生技術(shù)，適用于多峰函數(shù)和多目標(biāo)函數(shù)的優(yōu)化問題。例如，在PSO算法中，通過構(gòu)造小生境拓?fù)?，將種群分成若干個(gè)子種群，動(dòng)態(tài)地形成相對獨(dú)立的搜索空間，實(shí)現(xiàn)對多個(gè)極值區(qū)域的同步搜索，從而可以避免算法在求解多峰函數(shù)優(yōu)化問題時(shí)出現(xiàn)早熟收斂現(xiàn)象。 Parsopoulos提出一種基于“分而治之”思想的多種群PSO算法，其核心思想是將高維的目標(biāo)函數(shù)分解成多個(gè)低維函數(shù)，然后每個(gè)低維的子函數(shù)由一個(gè)子粒子群進(jìn)行優(yōu)化，該算法對高維問題的求解提供了一個(gè)較好的思路。
不同的發(fā)展方向代表不同的應(yīng)用領(lǐng)域，有的需要不斷進(jìn)行全局探測，有的需要提高尋優(yōu)精度，有的需要全局搜索和局部搜索相互之間的平衡，還有的需要對高維問題進(jìn)行求解。這些方向沒有誰好誰壞的可比性，只有針對不同領(lǐng)域的不同問題求解時(shí)選擇最合適的方法的區(qū)別。
1.2 相關(guān)模型和思想
粒子群算法（ Particle Swarm Optimization, PSO）最早是由Eberhart和Kennedy于1995年提出，它的基本概念源于對鳥群覓食行為的研究。設(shè)想這樣一個(gè)場景：一群鳥在隨機(jī)搜尋食物，在這個(gè)區(qū)域里只有一塊食物，所有的鳥都不知道食物在哪里，但是它們知道當(dāng)前的位置離食物還有多遠(yuǎn)。最簡單有效的策略？尋找鳥群中離食物最近的個(gè)體來進(jìn)行搜素。PSO算法就從這種生物種群行為特性中得到啟發(fā)并用于求解優(yōu)化問題。
用一種粒子來模擬上述的鳥類個(gè)體，每個(gè)粒子可視為N維搜索空間中的一個(gè)搜索個(gè)體，粒子的當(dāng)前位置即為對應(yīng)優(yōu)化問題的一個(gè)候選解，粒子的飛行過程即為該個(gè)體的搜索過程．粒子的飛行速度可根據(jù)粒子歷史最優(yōu)位置和種群歷史最優(yōu)位置進(jìn)行動(dòng)態(tài)調(diào)整．粒子僅具有兩個(gè)屬性：速度和位置，速度代表移動(dòng)的快慢，位置代表移動(dòng)的方向。每個(gè)粒子單獨(dú)搜尋的最優(yōu)解叫做個(gè)體極值，粒子群中最優(yōu)的個(gè)體極值作為當(dāng)前全局最優(yōu)解。不斷迭代，更新速度和位置。最終得到滿足終止條件的最優(yōu)解。
算法流程如下：
1、初始化
首先，我們設(shè)置最大迭代次數(shù)，目標(biāo)函數(shù)的自變量個(gè)數(shù)，粒子的最大速度，位置信息為整個(gè)搜索空間，我們在速度區(qū)間和搜索空間上隨機(jī)初始化速度和位置，設(shè)置粒子群規(guī)模為M，每個(gè)粒子隨機(jī)初始化一個(gè)飛翔速度。
2、 個(gè)體極值與全局最優(yōu)解
定義適應(yīng)度函數(shù)，個(gè)體極值為每個(gè)粒子找到的最優(yōu)解，從這些最優(yōu)解找到一個(gè)全局值，叫做本次全局最優(yōu)解。與歷史全局最優(yōu)比較，進(jìn)行更新。
3、 更新速度和位置的公式
4、 終止條件
（1）達(dá)到設(shè)定迭代次數(shù)；（2）代數(shù)之間的差值滿足最小界限
以上就是最基本的一個(gè)標(biāo)準(zhǔn)PSO算法流程。和其它群智能算法一樣，PSO算法在優(yōu)化過程中，種群的多樣性和算法的收斂速度之間始終存在著矛盾．對標(biāo)準(zhǔn)PSO算法的改進(jìn)，無論是參數(shù)的選取、小生境技術(shù)的采用或是其他技術(shù)與PSO的融合，其目的都是希望在加強(qiáng)算法局部搜索能力的同時(shí)，保持種群的多樣性，防止算法在快速收斂的同時(shí)出現(xiàn)早熟收斂。
%% ?PSO 三維路徑規(guī)劃
clc,clear , close all
feature jit off
%% 模型基本參數(shù)
% 載入地形 ?矩陣
filename = 'TestData1.xlsx' ;
model.x_data ?= xlsread( ?filename ?, 'Xi') ;
model.y_data = xlsread(filename, 'Yi') ;
model.z_data ?= xlsread( filename , 'Zi') ;
model.x_grid = ?model.x_data(1,:) ;
model.y_grid =model.y_data(:, 1) ;
model.xs = ?10 ?; ?%起點(diǎn) ? 相關(guān)信息
model.ys = 90 ?;
model.zs ?= ? interp2( ?model.x_data , ?model.y_data, ? model.z_data ? , ?...
? ?model.xs , ? ? model.ys ? ,'linear' ) ; ?% ?高度為插值得到
model.xt ?= ?150 ; % 終點(diǎn) 相關(guān)信息
model.yt ?= 40 ;
model.zt = interp2( ?model.x_data , ?model.y_data, model.z_data ? , ?...
? ?model.xt ?, ? ?model.yt ?, ?'linear'); ?% ?高度為插值得到
model.n= ? 5 ?; ?% ?粗略導(dǎo)航點(diǎn)設(shè)置
model.nn= ?80 ; ?% ?插值法獲得的導(dǎo)航點(diǎn)總數(shù)
model.Safeh = 0.01 ; ?% ?與障礙物的最低飛行高度 ?
% 導(dǎo)航點(diǎn) ? 邊界值
model.xmin= ?min( ?model.x_data( ?: ?) ) ;
model.xmax= max ( ?model.x_data( ?: ?) ) ;
model.ymin= min( ?model.y_data( ?: ?) ) ;
model.ymax= max( ?model.y_data( ?: ?) ) ;
model.zmin= min( ?model.z_data( ?: ?) ) ;
model.zmax =model.zmin + (1+ 0.1)*( max( model.z_data(:) )-model.zmin ) ;
% 模型的其他參數(shù)
model.nVar ?= ?3*model.n ; % 編碼長度
model.pf = 10^7 ; % 懲罰系數(shù)
% ?障礙物 位置坐標(biāo)及半徑
model.Barrier = ?[10,60 , 8 ;
?
? ?100, 40, ?15 ] ;
model.Num_Barrier ?= ?size(model.Barrier , 1 ); % ?障礙物的數(shù)目
model.weight1 = 0.5; % 權(quán)重1 飛行線路長度權(quán)重
model.weight2 = 0.3; % 權(quán)重2 ?飛行高度相關(guān)權(quán)重
model.weight3 = 0.2; % 權(quán)重3 ?Jsmooth ?指標(biāo)權(quán)重
%% ?算法參數(shù)設(shè)置
param.MaxIt = ?400; ? ? ? ? ?% ? ?迭代次數(shù)
param.nPop= 20; ? ? ? ? ? % ?種群數(shù)目
param.w=1; ? ? ? ? ? ? ? ?% ? 權(quán)重
param.wdamp=0.99; ? ? ? ? % ? ?退化率
param.c1=1.5; ? ? ? ? ? ? % Personal Learning Coefficient
param.c2=2 ; ? ? ? ? ? ? % Global Learning Coefficient
% 局部搜索部分參數(shù)
param.MaxSubIt= 0; ? ?% Maximum Number of Sub-iterations ( 內(nèi)循環(huán)迭代次數(shù) ?)
param.T = ?25; ?%
param.alpha1=0.99; ? ? % Temp. Reduction Rate
param.ShowIteration = 50; % 每過 ?多少次迭代顯示一次圖
%% ?運(yùn)行算法
CostFunction=@(x) MyCost(x,model); ? ?% ? 設(shè)置目標(biāo)函數(shù)
[ GlobalBest ?, BestCost ] = ?pso( param ? , model , CostFunction ) ;
QQ1575304183
參考文獻(xiàn)
O. Wilson, E.,?Sociobiology: The New Synthesis.?1976.
J Matari’c, M. and A. Brooks,?Interaction and Intelligent Behavior.?1999.
Trelea, I.C.,?The particle swarm optimization algorithm: convergence
analysis and parameter selection.?Information Processing Letters, 2003.
85(6): p. 317-325.Kennedy, J. and R. Eberhart.?Particle swarm optimization. in?Proceedings
of ICNN’95 - International Conference on Neural Networks. 1995.Zhan, Z., et al.?Adaptive control of acceleration coefficients for particle
swarm optimization based on clustering analysis. in?2007 IEEE Congress on
Evolutionary Computation. 2007.
