基于多級(jí)優(yōu)化的粒子群算法在航跡規(guī)劃中的應(yīng)用

張殿富，劉 福，楊 俊

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

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

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

1 環(huán)境建模

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

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

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

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

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

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

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

2 航跡規(guī)劃算法

2.1 PSO基本原理

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

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

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

2.2 PSO算法實(shí)現(xiàn)

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

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

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

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

①檢查每個(gè)元素對(duì)應(yīng)的航路點(diǎn)是否為安全點(diǎn)；

②檢查每兩個(gè)有效航路點(diǎn)是否安全可連。

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

圖2 相鄰航路點(diǎn)連線有效性檢驗(yàn)圖

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

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

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

具體的算法步驟如下：

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

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

③更新每個(gè)微粒的速度和位置；

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

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

⑥再次對(duì)微粒進(jìn)行有效性檢驗(yàn)，剔除無效粒子，并生成新粒子；

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

算法流程圖如圖3所示。

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

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

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

圖4 粒子群收斂效果圖

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

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

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

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

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

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

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

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

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

3 仿真及分析

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

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

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

4 結(jié)束語

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

[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] 王國師，李強(qiáng)，吳長飛，等.基于改進(jìn)粒子群算法的單預(yù)警機(jī)動(dòng)態(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]王新增，慈林林，李俊山，等.基于改進(jìn)粒子群優(yōu)化算法的無人機(jī)實(shí)時(shí)航跡規(guī)劃[J].微電子學(xué)與計(jì)算機(jī)，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é)報(bào)，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.