基于自適應(yīng)粒子群優(yōu)化算法的無人機(jī)三維航跡規(guī)劃

王 磊，趙紅超，王書湖，盧仁偉

（1.海軍航空大學(xué)，山東煙臺264001；2.煙臺南山學(xué)院，山東龍口265713）

近年來，無人機(jī)技術(shù)取得了極大進(jìn)展，各種類型的無人機(jī)紛紛研制成功，性能逐步提高，并已成功應(yīng)用于許多軍用領(lǐng)域和民用領(lǐng)域。無人機(jī)技術(shù)已經(jīng)成為世界各國軍事部門、研究機(jī)構(gòu)和高等院校的一個研究熱點。無人機(jī)航跡規(guī)劃是無人機(jī)技術(shù)領(lǐng)域中的一項重要課題，得到了國內(nèi)外研究者的極大關(guān)注[1]。航跡規(guī)劃就是在綜合考慮無人機(jī)到達(dá)時間、能耗、威脅以及動力學(xué)約束條件等要素的前提下，為無人機(jī)規(guī)劃出最優(yōu)或者滿意的飛行航跡，以保證圓滿地完成飛行任務(wù)[2]。固定翼無人機(jī)的任務(wù)空間是三維環(huán)境空間，跨越的空間范圍很大，環(huán)境復(fù)雜，而且無人機(jī)自身約束條件較多。因此，航跡規(guī)劃算法需要在計算復(fù)雜度、搜索時間和航跡代價之間做好折中處理，以滿足實際任務(wù)要求。航跡規(guī)劃的組成：描述規(guī)劃環(huán)境空間，選擇航跡表示形式，分析約束條件，確定代價函數(shù)，選取航跡搜索算法和航跡平滑。其中，選取航跡搜索算法是解決航跡規(guī)劃問題的核心部分。

航跡規(guī)劃算法有很多種，每種算法都有其自身的優(yōu)缺點和適用范圍，按照規(guī)劃決策可以分為傳統(tǒng)經(jīng)典算法和現(xiàn)代智能算法2大類。常用于無人機(jī)航跡規(guī)劃的傳統(tǒng)經(jīng)典算法包括：Dijkstra 算法[3]、模擬退火算法[4]、Dubins 曲線法[5]、人工勢場法[6]、快速擴(kuò)展隨機(jī)樹及其改進(jìn)型算法[7]等?，F(xiàn)代智能算法包括：A*算法[8]、遺傳算法（GA）[9]、蟻群優(yōu)化（ACO）算法[10]、粒子群優(yōu)化（PSO）算法[11]、鴿群算法[12]等。相比于傳統(tǒng)經(jīng)典算法，現(xiàn)代智能算法的應(yīng)用更為廣泛。其中，粒子群優(yōu)化算法具有2 個顯著特點：一是沒有“優(yōu)勝劣汰”的機(jī)制，所有的粒子在迭代過程中始終作為種群的成員保留；二是沒有交叉、變異等進(jìn)化算子，每個粒子通過追隨當(dāng)前搜索到的最優(yōu)值尋找全局最優(yōu)值。粒子群優(yōu)化算法的優(yōu)點是具有較強(qiáng)的魯棒性，對種群大小敏感性不高，設(shè)計參數(shù)少，前期收斂速度快；缺點是后期收斂速度慢，容易早熟，陷入局部最優(yōu)解。

為了克服PSO算法的缺點，近年來一些研究者進(jìn)行了探索，提出了幾種改進(jìn)方法。文獻(xiàn)[13]在PSO 算法中引入自適應(yīng)靈敏度決策算子克服了PSO 算法易陷入局部最優(yōu)和后期收斂速度慢的問題，獲得了質(zhì)量較好的航跡。文獻(xiàn)[14]在PSO算法中引入空間精選投票機(jī)制找出空間中每一個航跡點的最優(yōu)位置，克服了PSO 算法易早熟，陷入局部最優(yōu)的問題。文獻(xiàn)[15]研究了障礙物密集環(huán)境中的無人機(jī)航跡規(guī)劃問題，在粒子群優(yōu)化算法中加入了人工勢場，克服局部最優(yōu)解問題，并提高了算法的收斂速度。文獻(xiàn)[16]采用模擬退火突跳概率策略，對PSO算法的粒子位置更新方式進(jìn)行了改進(jìn)，能夠增大全局搜索能力，降低航跡規(guī)劃時間。文獻(xiàn)[17]采用PSO算法進(jìn)行無人機(jī)三維局部動態(tài)航跡規(guī)劃，采用分層隨機(jī)初始化與粒子偷懶流放策略，改進(jìn)PSO 算法以加快收斂效率。文獻(xiàn)[18]研究了無人艇航線規(guī)劃問題，采用量子粒子群優(yōu)化算法，并結(jié)合人工勢場法思想，解決了PSO算法局部最優(yōu)解問題，收斂速度快。文獻(xiàn)[19]設(shè)計了分段式慣性權(quán)重調(diào)整公式改進(jìn)PSO 算法，在保證算法搜索速度的同時，提高了航跡規(guī)劃解的精度。

前人對應(yīng)用于無人機(jī)航跡規(guī)劃的粒子群優(yōu)化算法開展了一些研究，但仍存在一些問題須要進(jìn)一步深入研究。小型固定翼無人機(jī)自身約束條件較多而且嚴(yán)苛，未來其執(zhí)行任務(wù)的空間環(huán)境將更復(fù)雜、更危險，須要結(jié)合無人機(jī)自身約束條件和實際工程中任務(wù)環(huán)境的復(fù)雜狀況，對無人機(jī)航跡規(guī)劃算法進(jìn)一步深入探索。受制造成本等經(jīng)濟(jì)因素限制，機(jī)載計算機(jī)的解算能力有限，因而要求航跡規(guī)劃算法盡量簡單高效，具有計算量小、精度高、容錯率大等優(yōu)點。本文提出了一種自適應(yīng)粒子群優(yōu)化算法，只需要自適應(yīng)地調(diào)節(jié)慣性權(quán)重和學(xué)習(xí)因子，不需要額外加入其他算法，就能夠克服局部最優(yōu)問題和后期收斂速度慢的問題，滿足了簡單高效的要求，為固定翼無人機(jī)在復(fù)雜三維環(huán)境空間中執(zhí)行任務(wù)提供了可靠保障。

1 無人機(jī)航跡規(guī)劃問題

1.1 三維航跡規(guī)劃環(huán)境建模

環(huán)境建模就是將環(huán)境中的各種物理信息轉(zhuǎn)換為計算機(jī)算法能處理的數(shù)字模型，這是規(guī)劃無人機(jī)飛行航跡的前提和基礎(chǔ)。本文以無人機(jī)在山區(qū)環(huán)境中執(zhí)行自然災(zāi)害勘察任務(wù)為例進(jìn)行研究。山區(qū)環(huán)境存在山峰、高壓電線、樹木、鳥群和風(fēng)切變等威脅。以東北天坐標(biāo)系作為慣性參考坐標(biāo)系，采用圓錐體近似模擬高聳的山峰，山區(qū)地形高度可用如下算法模擬[6]：

式（1）中：z0為當(dāng)?shù)鼗鶞?zhǔn)地形高度；hi為第i 個山峰的高度峰值；I 為山峰的數(shù)目；x0,i、y0,i分別為第i 個山峰峰頂在水平面內(nèi)的橫、縱坐標(biāo)；xs,i和ys,i分別為第i個山峰的橫向坡度和縱向坡度。

各個山峰是飛行環(huán)境中的地形威脅。山峰附近區(qū)域通常存在風(fēng)切變等極端氣象，這些區(qū)域也是飛行威脅區(qū)，用橢球體近似模擬，設(shè)第i 個山峰附近橢球威脅區(qū)的中心坐標(biāo)為(xc,i,yc,i,zc,i)，半軸長為(rx,i,ry,i,rz,i)。對于高壓電線和樹木等威脅，無人機(jī)通過自身最小飛行高度約束來規(guī)避，即無人機(jī)的最小飛行高度應(yīng)比高壓電線和樹木的最大高度高出一定的距離。飛行的鳥群對無人機(jī)來說是突發(fā)的動態(tài)威脅，飛鳥或鳥群的運動信息比較復(fù)雜，本文暫不研究此類突發(fā)狀況。

1.2 無人機(jī)自身約束條件

固定翼無人機(jī)不具備旋翼式無人機(jī)的垂直起降、空中懸停、飛行靈活等性能，因而飛行中自身約束條件要比旋翼式無人機(jī)嚴(yán)格得多。固定翼無人機(jī)比旋翼式無人機(jī)飛行速度快、航程遠(yuǎn)、能耗低，更適合到距離遠(yuǎn)、范圍大的空間環(huán)境中去執(zhí)行任務(wù)。主要考慮以下幾項約束條件。

1）最大航跡距離。設(shè)無人機(jī)最大航程為Lmax，規(guī)劃航跡包括K 段航跡段，第k 段航跡段的長度為Lk，k=1,2,…,K ，則最大航跡距離約束表示為：

2）最小慣性距離。最小慣性距離是指無人機(jī)突然變換飛行方向時，由于慣性作用需要按原方向飛行的最短距離。設(shè)最小慣性距離為Lmin，則最小慣性距離約束表示為：

3）最大水平轉(zhuǎn)彎角。受本身硬件性能的限制，無人機(jī)在水平方向轉(zhuǎn)彎時，轉(zhuǎn)彎角度無法超過最大水平轉(zhuǎn)彎角Δψmax，設(shè)第k 段航跡段相比于第k-1 段航跡段的水平轉(zhuǎn)彎角為Δψk，則水平轉(zhuǎn)彎角約束為：

4）最大高低角。高低角是指無人機(jī)向上爬升或向下俯沖的角度，由于性能限制，無人機(jī)飛行中的高低角不能超過最大高低角θmax，設(shè)第k 段航跡段與水平面的夾角為θk，則最大高低角約束為：

5）最大/小飛行高度。無人機(jī)執(zhí)行勘察任務(wù)需要在低空飛行，設(shè)無人機(jī)離地面的最大高度為Δhmax，一條航跡由D 個航跡點組成，第d（d=1,2,…,D）個航跡點的海拔高度為zd，航跡點下面的地形高度為z1（由式（1）解算），則最大飛行高度約束表示為：

另外，無人機(jī)的飛行高度還要求比高壓電線和樹木的最大高度高出一定的安全距離。設(shè)高壓電線和樹木的最大高度為ht，安全距離為hsafe，則最小飛行高度約束表示為：

2 自適應(yīng)粒子群優(yōu)化算法設(shè)計

2.1 算法基本原理

粒子群優(yōu)化算法是無人機(jī)航跡規(guī)劃常采用的一種方法，其主要缺陷是：后期收斂速度慢，易陷入局部最優(yōu)解。

為克服此缺陷，本文在粒子群優(yōu)化算法中加入自適應(yīng)調(diào)節(jié)算法，調(diào)節(jié)慣性權(quán)重和2個學(xué)習(xí)因子，從而提出了一種自適應(yīng)粒子群優(yōu)化（APSO）算法。

式（8）、（9）中：w 為慣性權(quán)重；c1和c2為學(xué)習(xí)因子；r1和r2為[0,1]范圍內(nèi)的隨機(jī)數(shù)。在傳統(tǒng)粒子群優(yōu)化算法中參數(shù)w、c1和c2都取常數(shù)。

分析這些參數(shù)的作用，提出相應(yīng)的改進(jìn)策略。w反映了粒子維持自己先前運動速度的趨勢，取較大值能夠加快搜索速度，但是影響搜索精度。c1反映了粒子的認(rèn)知能力，取較大值能夠提高個體搜索速度，增強(qiáng)全局搜索能力，但是影響局部搜索能力和搜索精度。c2反映了粒子的社會能力，取較大值能夠增強(qiáng)局部搜索能力，加快收斂速度，但是容易早熟，陷入局部最優(yōu)解?；谏鲜龇治?，動態(tài)調(diào)整參數(shù)的數(shù)值則能夠?qū)崿F(xiàn)搜索速度與搜索精度、全局搜索與局部搜索的協(xié)調(diào)，克服局部最優(yōu)解。

自適應(yīng)調(diào)節(jié)算法設(shè)計如下：

式（10）～（12）中：wmax和wmin分別為w 的最大值和最小值；c1max和c1min分別為c1的最大值和最小值；c2max和c2min分別為c2的最大值和最小值；t 為當(dāng)前迭代次數(shù)；tmax為最大迭代次數(shù)。

此改進(jìn)策略的原理是：在迭代前期w 和c1的取值較大、而c2的取值較?。焕脀 和c1較大加快搜索速度、增強(qiáng)全局搜索能力，對于搜索精度較差的問題，在迭代后期再彌補(bǔ)；利用c2較小來避免早熟陷入局部最優(yōu)解。在迭代后期w 和c1的取值較小、而c2的取值較大；后期粒子群基本上運動到了最優(yōu)解的附近區(qū)域，通過減慢搜索速度來提高搜索精度，降低全局搜索能力而提高局部搜索能力，從而加快收斂到最優(yōu)解的速度；實現(xiàn)了搜索速度與搜索精度、全局搜索與局部搜索的協(xié)調(diào)配合。

2.2 適應(yīng)度函數(shù)設(shè)計

適應(yīng)度函數(shù)就是無人機(jī)航跡規(guī)劃的優(yōu)化函數(shù)，它是衡量一個粒子優(yōu)劣的指標(biāo)函數(shù)。結(jié)合前面分析的各種環(huán)境威脅、無人機(jī)自身約束條件等限制，適應(yīng)度函數(shù)中應(yīng)包含航跡長度、環(huán)境威脅和約束條件等指標(biāo)，表達(dá)式如下：

式（13）中：FL表示航跡長度代價；FT表示環(huán)境威脅代價；FS表示約束條件代價；wL、wT和wS分別表示3項代價的權(quán)重，wL+wT+wS=1。

對于無人機(jī)三維航跡規(guī)劃問題，一個粒子代表了一條航跡，航跡由一組三維航跡點組成。粒子的位置向量中有D 個元素，即一條航跡由D 個航跡點組成，由K（K=D-1）段航跡段組成，則航跡長度適應(yīng)度函數(shù)為：

環(huán)境威脅主要包括山峰威脅和風(fēng)切變區(qū)域威脅。飛行環(huán)境中的山峰位置及其附近的風(fēng)切變區(qū)域都是已知的。高壓電線和樹木等威脅通過約束無人機(jī)的最小飛行高度來規(guī)避；由高度約束式（7）可知，無人機(jī)飛行高度須高于包括山峰在內(nèi)的地形高度。此處考慮的山峰威脅是要求無人機(jī)航跡在某一高度的水平面內(nèi)也要遠(yuǎn)離山峰以提高飛行安全性；避免僅依靠增大飛行高度來規(guī)避山峰威脅，無人機(jī)飛的過高則不利于勘察工作。

設(shè)第n 條航跡上的第d（d=1,2,…,D）個航跡點的位置坐標(biāo)為snd=(xnd,ynd,znd)。山峰威脅適應(yīng)度函數(shù)為：

式（15）、（16）中：fT1表示山峰威脅代價常數(shù)因子；R1i表示在水平面內(nèi)第n 條航跡上的所有航跡點到第i 個山峰中心點的最小距離。

風(fēng)切變區(qū)域也是飛行威脅區(qū)，用橢球體近似模擬，無人機(jī)航跡也要遠(yuǎn)離威脅區(qū)。威脅區(qū)的數(shù)目為I ，第i 個威脅區(qū)的中心坐標(biāo)為(xc,i,yc,i,zc,i)。適應(yīng)度函數(shù)為：

式（17）、（18）中：fT2表示風(fēng)切變區(qū)域威脅代價常數(shù)因子；R2i表示第n 條航跡上的所有航跡點到第i 個威脅區(qū)中心的最小距離。

概括起來，環(huán)境威脅適應(yīng)度函數(shù)為：

建立約束條件的代價函數(shù)。為了簡便起見，采用懲罰常數(shù)因子來描述代價函數(shù)[9]，設(shè)懲罰常數(shù)因子為C0，不妨取較大的正數(shù)。最大航跡距離的適應(yīng)度函數(shù)為：

其他約束條件的適應(yīng)度函數(shù)也是同理，若不滿足約束條件則適應(yīng)度值為C0，若滿足約束條件則適應(yīng)度值為0。

2.3 航跡規(guī)劃流程

采用APSO 算法開展無人機(jī)三維航跡規(guī)劃，通過粒子群的迭代搜索尋優(yōu)，最終找到的全局極值就是規(guī)劃出的最優(yōu)參考航跡。所規(guī)劃出的航跡是從起始點經(jīng)由K 段航跡段聯(lián)通到目標(biāo)點的一條路徑。航跡規(guī)劃的流程如圖1所示。

圖1 航跡規(guī)劃流程圖Fig.1 Flowchart of path planning

3 仿真分析

采用Matlab R2010b 軟件編程構(gòu)建無人機(jī)三維航跡規(guī)劃仿真模型，進(jìn)行仿真實驗檢驗APSO 算法的有效性。假設(shè)山區(qū)環(huán)境為100 km×100 km×4 km 的區(qū)域，無人機(jī)的起始點S 坐標(biāo)為(0,0,0.06)，第1 個任務(wù)點T1坐標(biāo)為(43,81,1.1) ，第2 個任務(wù)點T2坐標(biāo)為(80,23,1.2)。無人機(jī)從起始點飛行到T1點上空執(zhí)行自然災(zāi)害（如山體滑坡、泥石流等）勘察任務(wù)，然后再飛行到T2點上空勘察，最后返回起始點。本文重點研究航跡規(guī)劃問題。為了簡便起見，將上述各段航跡分別進(jìn)行規(guī)劃，即，第1段規(guī)劃航跡為：S →T1；第2段規(guī)劃航跡為：T1→T2；第3段規(guī)劃航跡為：T2→S。無人機(jī)在T1點上空盤旋一周執(zhí)行勘察任務(wù)，并完成轉(zhuǎn)彎，然后飛行到T2點上空勘察；盤旋勘察工作這里不進(jìn)行研究。

設(shè)山區(qū)基準(zhǔn)地形高度為z0=0.05 km，并設(shè)置8 座山峰和8個風(fēng)切變橢球區(qū)，山峰參數(shù)見表1，風(fēng)切變橢球區(qū)參數(shù)見表2。

表1 山峰參數(shù)Tab.1 Parameters of the peaks

表2 風(fēng)切變橢球區(qū)參數(shù)Tab.2 Parameters of the wind shear ellipsoid areas

然后，利用式（1）計算出x ∈[0,100] 、y ∈[0,100]范圍內(nèi)的山區(qū)地形高度z1。設(shè)置無人機(jī)的約束條件分別為：Lmax=500 km ，Lmin=1.0 km ，Δψmax=1 rad ，θmax=0.8 rad ， Δhmax=0.5 km ， ht=0.02 km ，hsafe=0.05 km。為了降低航跡規(guī)劃的復(fù)雜度，提高規(guī)劃效率，對規(guī)劃航跡高度采用直接設(shè)置方式，即，根據(jù)山區(qū)地形高度和無人機(jī)最大/小飛行高度約束，設(shè)置無人機(jī)的實時航跡高度高于其下方地形高度0.2 km，滿足高度約束。

采用APSO算法進(jìn)行航跡規(guī)劃實際上是對全部航跡點的x 和y 坐標(biāo)值進(jìn)行迭代尋優(yōu)。

應(yīng)用仿真試湊法進(jìn)行反復(fù)嘗試，確定算法參數(shù)如下：N=300，D=25，wmax=0.8，wmin=0.2，c1max=3.0，c1min=1.0，c2max=3.0，c2min=1.0，tmax=300，wL=0.4，wT=0.3，wS=0.3，fT1=300，fT2=300，C0=900。

為了與傳統(tǒng)PSO 算法進(jìn)行對比，也采用了傳統(tǒng)PSO算法進(jìn)行無人機(jī)航跡規(guī)劃仿真實驗。

在傳統(tǒng)PSO算法中,w、c1和c2都為常數(shù)，其取值偏大或者偏小都會影響航跡規(guī)劃效果，通常是根據(jù)設(shè)計經(jīng)驗和仿真試湊法進(jìn)行選取。不失一般性，選取其值為前面APSO 算法的最大值和最小值之間的平均值，即w=0.5，c1=2.0，c2=2.0，其他參數(shù)與APSO算法相同。仿真實驗共進(jìn)行了50次，統(tǒng)計出各段規(guī)劃航跡的平均適應(yīng)度值，2種算法的適應(yīng)度值對比如表3所示。由表3可以看出，對于3段規(guī)劃航跡，本文提出的APSO 算法比傳統(tǒng)PSO 算法均獲得了更小的適應(yīng)度值，表明本文算法具有更高的全局搜索能力和搜索精度。

表3 2種算法的適應(yīng)度值對比Tab.3 Contrast of fitness values of two algorithms

給出最后一次的仿真結(jié)果，基于APSO 算法的最優(yōu)航跡及其適應(yīng)度值收斂曲線如圖2 所示，基于傳統(tǒng)PSO算法的最優(yōu)航跡及其適應(yīng)度值收斂曲線如圖3所示。

圖3 基于PSO算法的無人機(jī)航跡規(guī)劃Fig.3 UAV path planning based on the PSO algorithm

4 結(jié)束語

本文提出了一種自適應(yīng)粒子群優(yōu)化算法，用來解決固定翼無人機(jī)在三維環(huán)境空間中的航跡規(guī)劃問題。以無人機(jī)飛行到山區(qū)環(huán)境執(zhí)行自然災(zāi)害勘察任務(wù)為例，開展了研究工作?？紤]了山區(qū)地形環(huán)境威脅和無人機(jī)自身約束條件。在此基礎(chǔ)上，設(shè)計了自適應(yīng)粒子群優(yōu)化算法的適應(yīng)度函數(shù)和航跡規(guī)劃算法流程。為了檢驗APSO 算法的有效性，分別采用APSO算法和傳統(tǒng)PSO算法，開展了無人機(jī)三維航跡規(guī)劃仿真實驗。仿真結(jié)果對比表明，所提出的APSO 算法比傳統(tǒng)PSO 算法具有更高的全局搜索能力和搜索精度。本文研究主要考慮了環(huán)境中的靜態(tài)威脅，下一步將研究無人機(jī)規(guī)避突發(fā)動態(tài)威脅的航跡規(guī)劃方法。