張殿富，劉 福，楊 俊

（1.武警工程大學(xué)，西安710086；2.空軍工程大學(xué)，西安710038）

無人機（UAV）的航跡規(guī)劃是指在特定的約束條件下，尋找滿足無人機機動性能以及作戰(zhàn)環(huán)境要求的從出發(fā)點到目標(biāo)點的最優(yōu)飛行航路[1]。目前，用于無人機航跡規(guī)劃的尋優(yōu)算法有很多，按照航跡搜索算法的不同大致分為2 類[2]。一類是確定型搜索算法，如A*算法[3]、D*算法[4]和動態(tài)規(guī)劃法等。這類算法的特點是搜索精度比較高，但復(fù)雜環(huán)境下容易陷入局部最優(yōu)；另一類為隨機型搜索算法，如遺傳算法、蟻群算法和粒子群算法[5-7]等。

粒子群算法作為隨機型搜索算法的一種不僅具有計算并行性、搜索全局性等特點，相比其他隨機型搜索算法，它還有概念簡單、容易實現(xiàn)、運算較快的特點，比較適合于復(fù)雜環(huán)境下的快速求解問題[8-9]。但基本粒子群算法在迭代后期尋優(yōu)速度會隨著迭代次數(shù)的增加而顯著變慢，且所得路徑精度不高[10]。為進一步提高其收斂速度，解決收斂精度不高的問題，本文在基本粒子群算法的基礎(chǔ)上引入了多級優(yōu)化思想，即在粒子群算法后期搜索效率顯著下降、算法還未達到完全收斂時退出粒子群迭代過程，轉(zhuǎn)而切換到多級優(yōu)化算法上。利用多級優(yōu)化算法較強的局部尋優(yōu)能力完成所得粗略解的最優(yōu)化處理。由于多級優(yōu)化算法本質(zhì)上屬于確定型搜索算法，具有局部快速收斂特性，因而將它與粒子群算法結(jié)合將使得整個航跡生成過程既能滿足全局最優(yōu)，又能滿足快速收斂的要求。

1 環(huán)境建模

無人機航跡規(guī)劃的要求是得到的航跡能夠有效避開敵方雷達探測和敵方威脅的攻擊，要求避開可能影響飛行的險要地形、惡劣氣候和人工障礙等不利因素[11]，并且在此基礎(chǔ)上使得航程盡量小，同時也需滿足一系列飛機機動性能限制條件。即提供給無人機的航跡要安全、快捷、可飛。

將飛行空間用二維空間表示，假設(shè)無人機的飛行高度和速度恒定，任務(wù)限制條件為偏航角不能超過30°，在航跡規(guī)劃前假設(shè)各種威脅在地圖上的范圍已知。由于傳統(tǒng)的威脅建模方法存在難以處理不規(guī)則形狀威脅場、需要對威脅進行分類處理等缺點，故采取網(wǎng)格建模法[12]來對所有威脅進行歸一化處理。

設(shè)規(guī)劃區(qū)域為一個300 km×200 km 的長方形區(qū)域，任務(wù)起點終點分別為P、Q。對該區(qū)域進行網(wǎng)格化，由此生成一個有m×n個網(wǎng)點的平面陣A。以左下角的點為基準(zhǔn)點，則任一網(wǎng)點的邏輯位置坐標(biāo)可表示為(i,j)，其對應(yīng)的實際坐標(biāo)為(xb,yb)，有

式中，GP為網(wǎng)格邊長所代表的實際長度，單位為km。

在網(wǎng)點平面陣上描出威脅區(qū)域，將那些落在威脅區(qū)域內(nèi)的網(wǎng)點定為威脅點，以元素1表示；區(qū)域外的點為安全點，以元素0表示，由此生成一個與平面陣A存在映射關(guān)系的二值矩陣M。其對應(yīng)關(guān)系為平面陣中邏輯位置為(i,j)的網(wǎng)點在M陣中所對應(yīng)的元素為M(j,i)。

由于所有區(qū)域的位置信息和威脅信息都以平面陣A和威脅點陣M的形式表示，因而該建模方法能對任意形狀的威脅區(qū)域進行建模，如圖1所示，且無需對威脅的種類加以區(qū)分。這樣，后續(xù)的優(yōu)化算法只需對網(wǎng)格化后的邏輯平面陣A以及抽象出來的二值威脅陣M進行操作，從而大大減小了后續(xù)算法的工作量。

圖1 不同威脅區(qū)域的點陣表示

2 航跡規(guī)劃算法

2.1 PSO基本原理

粒子群優(yōu)化（PSO）算法是一種基于群體智能的演化算法，由于其具有較好的全局尋優(yōu)功能、可以并行計算、算法相對簡單等一系列優(yōu)點，因而在很多領(lǐng)域得到了廣泛的應(yīng)用[13]。

假設(shè)在D維搜索空間中，有N個粒子組成的群體，第i個粒子在D維空間中的位置表示為向量Xi=(xi1,xi2,…,xiD) ，每個粒子的飛行速度為Vi=(vi1,vi2,…,viD)，第i個粒子經(jīng)歷過的最好位置為Pi=(pi1,pi2,…,piD)。整個群體中，所有粒子經(jīng)歷過的最好位置為Gb=(g1,g2,…,gD)，PSO 算法在每一代對第i個粒子在第d(1 ≤d≤D)維上，根據(jù)下面的公式來更新自己的速度和位置：

式（2）、（3）中：i=1,2,…,N（N是該群體中粒子的總數(shù)）；d=1,2,…,D（D為每個粒子的維數(shù)）；vid(k)代表第k次迭代粒子i飛行速度矢量的第d維分量；xid(k)代表第k次迭代粒子i、位置矢量的第d維分量；pid代表粒子i個體最好位置Pi的第d維分量；gd代表群體最好位置Gb的第d維分量；C1、C2為學(xué)習(xí)因子（常數(shù)）；R1、R2為[0，1]上的隨機數(shù)；w為慣性權(quán)重。每一維粒子的速度均被限制在最大速度Vmax之內(nèi)。如果每一維速度更新后的值超過用戶設(shè)定的上限Vmax，則速度就會被限定為Vmax。

2.2 PSO算法實現(xiàn)

1）性能指標(biāo)。粒子群算法中的每一個粒子都是一個解，也就是一條路徑。粒子群算法的目的是從眾多的解中尋找出一個最優(yōu)解從而找到一條最優(yōu)路徑。每個粒子的適應(yīng)度函數(shù)就是粒子所代表的路徑長度。顯然適應(yīng)度函數(shù)值越小，航跡最優(yōu)。

式（4）中：J為航跡的總長；L(ai,ai+1)分別為第i和第i+1個航路點之間的直線距離。

2）粒子編碼。進行粒子群算法前，首先要對每個粒子進行編碼。具體方法為將規(guī)劃起點到目標(biāo)點在x方向上n等分，設(shè)每個航路點只能在n+1條等分線上選取，于是所有航路點的橫坐標(biāo)就已確定，只需存儲每個航路點的縱坐標(biāo)。將一條航跡中所有航路點的縱坐標(biāo)用一個一維數(shù)組表示，將其視為一個粒子，在初始化生成一定數(shù)量的粒子后通過反復(fù)的迭代優(yōu)化運算就可從這些粒子中篩選出一個最優(yōu)粒子即可得到最優(yōu)路徑。

3）有效性檢驗。不管是在初始化還是每一步迭代完畢，都需要對現(xiàn)有的粒子進行有效性檢驗，即檢查其代表的航跡是否滿足相關(guān)約束條件。具體的檢驗指標(biāo)為：

①檢查每個元素對應(yīng)的航路點是否為安全點；

②檢查每兩個有效航路點是否安全可連。

對于第1項只需查詢航路點在環(huán)境矩陣中對應(yīng)元素值。為0 則航路點有效，為1 則無效。對于第二項采取“天梯法”檢驗。如圖2，設(shè)A、H點為2 相鄰航路點，以這2點為對角可確定一個類似于天梯的矩陣，從CD行至EF行，一旦有一行同時有2 個點為威脅點，則視為從A點到H點的路徑無效。否則為有效。

圖2 相鄰航路點連線有效性檢驗圖

只有當(dāng)粒子的每個元素檢驗均有效后，才能視該粒子為有效粒子。在每次迭代后總有一部分有效粒子會變成無效粒子，如果每次都重新生成新的粒子勢必要影響算法的收斂時間和精度，在此可給粒子一次“補救”的機會，即先將粒子對應(yīng)的無效元素替換為該粒子歷史最好位置對應(yīng)的元素，再對其進行有效性檢驗，若依舊無效則重新生成粒子[13]。

4）迭代更新。每次迭代系統(tǒng)都將把粒子當(dāng)前適應(yīng)度同歷史最優(yōu)適應(yīng)函數(shù)值以及全局最優(yōu)適應(yīng)函數(shù)值進行對比，若滿足適應(yīng)函數(shù)值更小，則將對應(yīng)的粒子進行更新替換。

給定粒子群的迭代次數(shù)，則迭代完畢后的全局最優(yōu)粒子所代表的即為近似最優(yōu)航跡。

具體的算法步驟如下：

①初始化種群中各微粒的位置和速度；

②評價每個微粒的適應(yīng)度，將當(dāng)前各微粒的位置和適應(yīng)值存儲在Pi中，將所有Pi中適應(yīng)值最優(yōu)的個體的位置和適應(yīng)值儲存在Gb中；

③更新每個微粒的速度和位置；

④對微粒進行有效性檢驗，對無效粒子的適應(yīng)值替換為歷史最好位置；

⑤比較當(dāng)前所有Pi和Gb的值，更新Gb；

⑥再次對微粒進行有效性檢驗，剔除無效粒子，并生成新粒子；

⑦迭代次數(shù)達到設(shè)定值，輸出結(jié)果，否則返回③繼續(xù)搜索。

算法流程圖如圖3所示。

圖3 基本粒子群算法流程圖

2.3 多級優(yōu)化思想引入

盡管PSO 算法相對其他智能算法（如遺傳算法）在算法運行速度上有一定提高，但仍有進一步提升的空間。由于純粒子群算法適應(yīng)度函數(shù)下降曲線與迭代次數(shù)類似于負(fù)指數(shù)函數(shù)，迭代初期適應(yīng)度函數(shù)下降得快，隨著迭代次數(shù)的增加適應(yīng)函數(shù)值下降得會越來越慢，如圖4 所示。而評價一個算法的效能不僅要看收斂精度也要看收斂時間。因此，可認(rèn)為當(dāng)粒子群適應(yīng)函數(shù)下降到一定值時其尋優(yōu)效能達到最大，超過此點再往后繼續(xù)迭代則尋優(yōu)效率降低，時間上“不經(jīng)濟”。

圖4 粒子群收斂效果圖

從信息論的角度講，粒子群的尋優(yōu)過程是將一些雜亂無章無利用價值的粒子逐步變?yōu)橛行?、有利用價值粒子的過程，當(dāng)粒子適應(yīng)函數(shù)降到一定程度后就可認(rèn)為該粒子已具有利用價值，且包含有潛在的最優(yōu)路徑信息。此時，便可退出粒子群迭代過程，轉(zhuǎn)而采用其他算法對當(dāng)前所得的結(jié)果路徑進行2次、3次甚至4次優(yōu)化，直到當(dāng)前航跡真正達到全局最優(yōu)。由于不需要大量循環(huán)迭代，故每級優(yōu)化運算的用時很短。將多個不同優(yōu)化算子的單級優(yōu)化運算以類似于流水線加工方式組合在一起，即為多級優(yōu)化思想。

2.4 多級優(yōu)化算法實現(xiàn)

1）一級優(yōu)化。由于粒子群算法所得的當(dāng)前路徑適應(yīng)函數(shù)過大且路徑毛刺較多。因此，一級優(yōu)化的主要目的為降航程。根據(jù)2 點之間直線最短，故可用由若干條線段組成的折線對其實施替換。替換原則為不改變路徑的有效性，且線段數(shù)目盡量少。

圖5 一次優(yōu)化效果圖

設(shè)一條航跡由航路點(a1,a2,…,aN)構(gòu)成，定義線段l(ai,aj), (j＞i)為航路點ai與航路點aj的連線，若沒穿過威脅區(qū)則為有效，穿過則為無效。從起點a1開始，檢查連線段l(a1,a2)的有效性，若有效則檢查連線段l(a1,a3)的有效性，依次類推直到l(a1,ai)無效為止，將此航路點的前一點即ai-1存儲起來，連接(a1,ai-1)并將ai-1作為新的直線段起點再次運用此法進行直線段檢驗，當(dāng)線段終點抵達航路終點時優(yōu)化過程結(jié)束。將存儲的新的航路點連接起來即為一條由多條線段組成的折線，所以該方法又稱折線優(yōu)化法。具體操作中為了得到更好的優(yōu)化效果往往以所得折線路徑為輸入路徑再進行一次折線優(yōu)化。

2）二級優(yōu)化。一級優(yōu)化后，航跡長度雖有大幅減小，但航跡偏角過大。為了使航跡變得平滑，需要對超過一定角度的偏角進行“切割”。處理原則為在不影響航跡有效性的前提下，將由2 條線段組成的拐角用3條線段進行替換，且3條線段中的第2條線段盡量長，其處理效果如圖6所示。

圖6 航跡偏角優(yōu)化效果圖

具體方法為將一級優(yōu)化后的路徑作為輸入路徑，計算每個拐角的偏角度數(shù)。若大于某個值，則按上圖的方法進行三段式平滑。若拐角∠ai-1aiai+1大于設(shè)定閾值θ，則先檢驗線段l(ai-1,ai+1)的有效性。若l(ai-1,ai+1)有效，則繼續(xù)檢驗線段l(ai-2,ai+2)，直到線段l(ai-t,ai+t)無效為止。將此線段的上級線段l(ai-t+1,ai+t-1)繪出來，將航路點ai-t+1,ai+t-1存儲起來。若l(ai-1,ai+1)無效，則該拐角航路點需保留。由三角形邊角理論可知，φ1＜φ，φ2＜φ，l＜l1+l2，即每個較大的偏角都被2 個較小的偏角所取代，每個偏角附近的航跡都被新的更短的航跡所取代，因而保證了新生成的航跡更平滑、路徑更短。具體操作中，為得到更好地平滑優(yōu)化效果，往往以平滑一次后的路徑，作為輸入路徑再以該算法進行若干次的自迭代平滑優(yōu)化。

3）三級優(yōu)化。對于二次優(yōu)化后的路徑已經(jīng)很接近于全局最優(yōu)解，為了充分利用安全區(qū)域，進一步對航跡長度進行壓縮以及對航線進行平滑，可將二次優(yōu)化后的路徑再進行復(fù)合優(yōu)化。即相繼進行一次折線優(yōu)化、一次平滑優(yōu)化。這樣所得的路徑就完全滿足全局最優(yōu)，所得航跡為最終航跡。

3 仿真及分析

在MATLAB 7.8 環(huán)境下對算法進行仿真，在300 km×200 km 的地形區(qū)域內(nèi)，設(shè)置8 個威脅區(qū)域。網(wǎng)點間隔GP=4 ，粒子群規(guī)模NUM=8 ，粒子維數(shù)N=300/GP+1=76，迭代次數(shù)Ndmax=20，慣性權(quán)重w=0.9，學(xué)習(xí)因子C1=C2=2，粒子元素最大移動速度Vmax=1×GP，航跡偏角閾值θ=30°。為了便于與基本粒子群算法形成對比，分別用基本粒子群算法及“PSO+多級優(yōu)化”算法對同一區(qū)域進行規(guī)劃，所得結(jié)果如圖7所示，二者的適應(yīng)度函數(shù)變化曲線如圖8所示。

圖7 不同算法的航跡效果圖

由圖7、8 可知，采用基本粒子群算法及“PSO+多級優(yōu)化”算法生成的航路都能較好的避開威脅抵達終點。二者用時分別為t1=14 s，t2=2.5 s，最終的指標(biāo)函數(shù)值分別為J1=361，J2=318。對比人工估計的最優(yōu)指標(biāo)函數(shù)318至326這一區(qū)間，顯然后者的用時更短，指標(biāo)更優(yōu)。因此，不論是從算法的用時，還是精度方面，都說明了后者生成路徑的效率要優(yōu)于前者。

4 結(jié)束語

本文以網(wǎng)格法和二值威脅陣為環(huán)境建模形式，將粒子群算法引入到航跡規(guī)劃中，并針對基本粒子群后期出現(xiàn)收斂效率低這一情況，提出了多級優(yōu)化思想。從而加速了航路的最優(yōu)化，彌補了基本粒子群的不足。該算法把粒子群算法全局尋優(yōu)的特點與多級優(yōu)化局部最優(yōu)的特點有機結(jié)合，提高了算法的速度和收斂的精度。仿真表明，在人工模擬的復(fù)雜環(huán)境下，該合成算法能快速有效地根據(jù)戰(zhàn)場環(huán)境信息規(guī)劃出全局最優(yōu)的路徑，有較好的戰(zhàn)場適應(yīng)性和工程實踐性。

[1] FU Y G，DING M Y，ZHOU C P，et al.Path planning for UAV based on quantum-behaved particle swarm optimization[C]//Proceedings of SPIE International Symposium on Multispectral Image Processing and Pattern Recognition.Yichang：SPIE，2009（74970B）：74971-74977.

[2] 王維平，劉娟.無人飛行器航跡規(guī)劃方法綜述[J].飛行力學(xué)，2010，28（2）：6-15.

WANG WEIPING，LIU JUAN. Introduction to unmanned air vehicle route planning methods[J]. Flight Dynamics，2010，28（2）：6-15.（in Chinese）

[3] MELCHIOR P，ORSONI B，LAVIALLE O，et al.Consideration of obstacle danger level in path planning using A*and fast-marching optimization comparative study[J].Signal Processing，2003，83（11）：2387-2396.

[4] CHAKRABORTY J，KONAR A，JAIN L C，et al. Cooperative multi-robot path planning using differential evolution[J]. Journal of Intelligent and Fuzzy Systems，2009，20（1）：13-27.

[5] TAN HAO，LI YUFENG. AC-PSO algorithm for UAV mission planning[J]. Transactions of Nanjing University of Aeronautics and Astronautics，2005，22（3）：264-270．

[6] LI NING，ZOU TONG，SUN DEBAO.Particle swarm optimization for vehicle routing problem with time windows[J]. Systems Engineering Theory and Practice，2004，22（4）：130-135.

[7] CLERC M，KENNEDY J.The particle swarm：explosion，stability，and convergence in a multi-dimensional complex space[J]. IEEE Transactions on Evolutionary Computation，2002，6（l）：58-73．

[8] 王國師，李強，吳長飛，等.基于改進粒子群算法的單預(yù)警機動態(tài)航跡規(guī)劃[J].火力與指揮控制，2012，37（6）：25-30.

WANG GUOSHI，LI QIANG，WU CHANGFEI，et al.Modeling and simulation of dynamic route planning for the early warning airborne based on PSO[J]. Fire Control and Command Control，2012，37（6）：25-30.（in Chinese）

[9] LI XIAODONG. Adaptively choosing neighbourhood bests using species in a particle swarm optimizer formultimodal function optimization[J].Lecture Notes in Computer Science，2004，3102：105-116.

[10]王新增，慈林林，李俊山，等.基于改進粒子群優(yōu)化算法的無人機實時航跡規(guī)劃[J].微電子學(xué)與計算機，2011，28（4）：87-90.

WANG XINZENG，CI LINLIN，LI JUNSHAN，et al. Real-time route planning for uav based on improved PSO algorithm[J].Microelectronics and Computer，2011，28（4）：87-90.（in Chinese）

[11]EBERHART R C，SHI Y H.Particle swarm optimization：development，applications and resources[C]//Proceedings of the 2001 Congress on Evolutionary Computation.New York：IEEE，2001：81-86.

[12]傅陽光，周成平，丁明躍.基于混合量子粒子群優(yōu)化算法的三維航跡規(guī)劃[J]. 宇航學(xué)報，2010，31（12）：2657-2664.

FU YANGGUANG，ZHOU CHENGPING，DING MINGYUE. 3-D path planning based on hybrid quantum-behaved particle swarm optimization[J]. Journal of Astronautics，2010，31（12）：2657-2664.（in Chinese）

[13] LI XIAODONG. Niching without niching parameters：particle swarm optimization using a ring topology[J].IEEE Transactions on Evolutionary Computation，2010，14（1）：150-169.