基于自適應粒子群優化算法的無人機三維航跡規劃

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

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

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

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

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

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

1 無人機航跡規劃問題

1.1 三維航跡規劃環境建模

環境建模就是將環境中的各種物理信息轉換為計算機算法能處理的數字模型，這是規劃無人機飛行航跡的前提和基礎。本文以無人機在山區環境中執行自然災害勘察任務為例進行研究。山區環境存在山峰、高壓電線、樹木、鳥群和風切變等威脅。以東北天坐標系作為慣性參考坐標系，采用圓錐體近似模擬高聳的山峰，山區地形高度可用如下算法模擬[6]：

式（1）中：z0為當地基準地形高度；hi為第i 個山峰的高度峰值；I 為山峰的數目；x0,i、y0,i分別為第i 個山峰峰頂在水平面內的橫、縱坐標；xs,i和ys,i分別為第i個山峰的橫向坡度和縱向坡度。

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

1.2 無人機自身約束條件

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

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

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

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

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

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

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

2 自適應粒子群優化算法設計

2.1 算法基本原理

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

為克服此缺陷，本文在粒子群優化算法中加入自適應調節算法，調節慣性權重和2個學習因子，從而提出了一種自適應粒子群優化（APSO）算法。

式（8）、（9）中：w 為慣性權重；c1和c2為學習因子；r1和r2為[0,1]范圍內的隨機數。在傳統粒子群優化算法中參數w、c1和c2都取常數。

分析這些參數的作用，提出相應的改進策略。w反映了粒子維持自己先前運動速度的趨勢，取較大值能夠加快搜索速度，但是影響搜索精度。c1反映了粒子的認知能力，取較大值能夠提高個體搜索速度，增強全局搜索能力，但是影響局部搜索能力和搜索精度。c2反映了粒子的社會能力，取較大值能夠增強局部搜索能力，加快收斂速度，但是容易早熟，陷入局部最優解。基于上述分析，動態調整參數的數值則能夠實現搜索速度與搜索精度、全局搜索與局部搜索的協調，克服局部最優解。

自適應調節算法設計如下：

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

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

2.2 適應度函數設計

適應度函數就是無人機航跡規劃的優化函數，它是衡量一個粒子優劣的指標函數。結合前面分析的各種環境威脅、無人機自身約束條件等限制，適應度函數中應包含航跡長度、環境威脅和約束條件等指標，表達式如下：

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

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

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

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

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

風切變區域也是飛行威脅區，用橢球體近似模擬，無人機航跡也要遠離威脅區。威脅區的數目為I ，第i 個威脅區的中心坐標為(xc,i,yc,i,zc,i)。適應度函數為：

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

概括起來，環境威脅適應度函數為：

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

其他約束條件的適應度函數也是同理，若不滿足約束條件則適應度值為C0，若滿足約束條件則適應度值為0。

2.3 航跡規劃流程

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

圖1 航跡規劃流程圖Fig.1 Flowchart of path planning

3 仿真分析

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

設山區基準地形高度為z0=0.05 km，并設置8 座山峰和8個風切變橢球區，山峰參數見表1，風切變橢球區參數見表2。

表1 山峰參數Tab.1 Parameters of the peaks

表2 風切變橢球區參數Tab.2 Parameters of the wind shear ellipsoid areas

然后，利用式（1）計算出x ∈[0,100] 、y ∈[0,100]范圍內的山區地形高度z1。設置無人機的約束條件分別為：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。為了降低航跡規劃的復雜度，提高規劃效率，對規劃航跡高度采用直接設置方式，即，根據山區地形高度和無人機最大/小飛行高度約束，設置無人機的實時航跡高度高于其下方地形高度0.2 km，滿足高度約束。

采用APSO算法進行航跡規劃實際上是對全部航跡點的x 和y 坐標值進行迭代尋優。

應用仿真試湊法進行反復嘗試，確定算法參數如下：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。

為了與傳統PSO 算法進行對比，也采用了傳統PSO算法進行無人機航跡規劃仿真實驗。

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

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

給出最后一次的仿真結果，基于APSO 算法的最優航跡及其適應度值收斂曲線如圖2 所示，基于傳統PSO算法的最優航跡及其適應度值收斂曲線如圖3所示。

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

4 結束語

本文提出了一種自適應粒子群優化算法，用來解決固定翼無人機在三維環境空間中的航跡規劃問題。以無人機飛行到山區環境執行自然災害勘察任務為例，開展了研究工作。考慮了山區地形環境威脅和無人機自身約束條件。在此基礎上，設計了自適應粒子群優化算法的適應度函數和航跡規劃算法流程。為了檢驗APSO 算法的有效性，分別采用APSO算法和傳統PSO算法，開展了無人機三維航跡規劃仿真實驗。仿真結果對比表明，所提出的APSO 算法比傳統PSO 算法具有更高的全局搜索能力和搜索精度。本文研究主要考慮了環境中的靜態威脅，下一步將研究無人機規避突發動態威脅的航跡規劃方法。