基于貪心策略下混合蟻群算法的無人機航跡規劃

曹建秋,徐 鵬,張廣言

(重慶交通大學 信息科學與工程學院，重慶 400074)

0 引 言

智能飛行器已經廣泛用于軍事偵察[1]，災害處理[2]，物資配送[3]等領域。復雜環境下航跡快速規劃是智能飛行器控制的一個重要課題。航跡規劃指飛行器根據任務需要，在復雜環境中選擇滿足約束的最優路徑，要求該路徑的綜合代價最小。

目前航跡規劃的算法主要分為經典算法和群體智能算法兩大類。經典算法包括A*算法，柵格法[4-6,14]等。但隨著問題復雜度增加，被求解問題往往可以通過規約證明屬于NP難問題，此時經典算法的計算復雜度呈指數增長。經典算法也存在自身的局限，如A*算法在實際問題中難以選擇恰當的評價函數，也進一步限制了經典算法的應用場景。隨著蟻群算法，粒子群算法等[7-13]智能算法的發展，這些情況得到了改善。

智能算法中，初始值多采用隨機生成，這降低了算法的收斂速度，算法運行時間較長。為了得到更優解的同時加速算法收斂，實際應用中將經典算法與智能算法融合形成融合算法，能取得較好的效果。文獻[7]融合了簡化A*算法與模擬退火算法；文獻[10]通過使用A*算法取代蟻群算法的信息素隨機初始化，增加了蟻群算法的穩定性；文獻[11]在稀疏A*全局優化搜索的基礎之上融入生物啟發神經動力學模型等。融合算法不僅提高了單一算法的可解釋性，并且較原始算法有一定性能提升。

目前航跡規劃問題的研究難點在于路徑規劃時需要考慮飛行器的性能，如最小轉彎半徑，飛行中產生的誤差等。為處理飛行器航跡規劃中的飛行誤差，筆者重點研究了校正點約束下的多目標航跡規劃問題，建立了校正點約束的航跡規劃模型，提出一種貪心策略下的混合蟻群算法進行求解。該算法基于貪心策略對校正點進行剪枝操作，提升了經典A*算法，粒子群算法，蟻群算法的效果。

1 航跡約束模型

1.1 校正點建模

部分飛行器由于系統結構限制或工作環境復雜，定位系統無法對自身進行精準定位。因此，航跡空間中存在一些可用于誤差校正的校正點，當飛行器到達校正點時，飛行器能根據該位置的誤差校正類型進行校正，如水平校正點只用于校正水平誤差。飛行器定位誤差包括垂直和水平誤差，誤差會隨著飛行積累，且只在一定范圍內才可以校正。若垂直、水平誤差都能得到及時校正，則飛行器可以按照預定航線飛行，通過若干個校正點進行誤差校正后最終到達目的地。

已知航跡空間T內存在兩類校正點，分別為水平校正點X,垂直校正點Y。對于飛行器路徑L上的校正點{xi},{yi}有：

{xi|xi∈X},{yi|yi∈Y}∈L

(1)

?xi∈X,Sc<α1,Ss<α2

(2)

?yi∈Y,Sc<β1,Ss<β2

(3)

式中：Ss為飛行器當前累積的水平誤差；Sc為飛行器當前累計的垂直誤差。

飛行器受性能限制存在兩組誤差校正上限，分別為(α1,α2)與(β1,β2)。當飛行器的垂直誤差不大于α1，水平誤差不大于α2時才能進行垂直誤差校正。垂直誤差不大于β1，水平誤差不大于β2時才能進行水平誤差校正。

1.2 轉彎半徑約束建模

在考慮航跡時，考慮到飛行器在轉彎時受到結構和控制系統的限制，無法完成即時轉彎，存在最小轉向半徑RMIN，對飛行器的轉彎半徑R有：

R≥RMIN

(4)

需要在規劃完成后所得航跡進行光滑處理。

1.3 綜合代價函數

飛行器航跡規劃需滿足兩點優化目標：①航跡長度盡可能小；②經過校正點次數盡可能少。式(5)表示經過的校正點之和最少，式(6)表示經過的校正點距離之和最少。

(5)

(6)

式中：di表示L中第i段路徑的長度。

1.4 航跡規劃問題貪心策略

1.4.1 問題的規約

對于給定滿足式(1)～(4)的圖G(V,E)，V=X,Y。E為滿足條件的邊(Vi,Vj)的集合。對圖G進行規約：①令式(2)，式(3)中的α1,α2,β1,β2>M。M為航跡空間T內航跡誤差的最大值；②刪除校正點，使航跡空間的校正點類型為同一類校正點(水平或垂直)；③令飛行器航跡起點A與終點B重合；④令RMIN=0。

此時圖G(V,E)轉化為求解經典的旅行商問題(traveling salesman problem，TSP)。由于a-d的規約均可在多項式時間內完成，TSP又屬于經典的NP-完全(non-deterministic polynomial complete problem，NPC)問題。根據NP完全理論，圖G(V,E)問題屬于NPC問題，即圖G(V,E)問題僅能在多項式時間內驗證，不能在多項式時間內求解。

1.4.2 貪心策略

對NPC問題，使用貪心思想進行求解。貪心思想指在對問題求解時，只關注當前條件下局部最優解。由于圖G(V,E)的求解復雜度較高，筆者提出一種貪心策略對問題進行簡化，在實驗中取得了較好的效果。該策略核心思想為：基于貪心思想對校正點進行剪枝操作，具體的實現主要分為兩步：

步驟1選擇當前矯正點。設飛行器路徑L上的第i點為水平校正點，此時Sc=k,Ss=0；

步驟2選擇下一矯正點。若第i+1點為水平校正點，則下一校正點的選擇范圍為：min(β1-Sc,β2-Ss)，即min(β1-k,β2)。若第i+1點為垂直校正點，則下一校正點的選擇范圍為：min{α1-k,α2}。

貪心策略要求每一步能盡量選擇更多的點，因此第i+1點的選擇范圍如式(7)：

max{min(β1-k,β2),min(α1-k,α2)}

(7)

第i點為垂直校正點時處理方式相同。

實際數據中，水平校正點的對水平誤差校正能力強，垂直校正點的對垂直誤差的校正能力強，若連續經過同一類型校正點，易出現另一類型的誤差積累而無法進行校正的情況。此外，根據貪心策略，需要盡量選擇更多的可行點。因此在規劃路徑時，應優先選擇與當前矯正點類型不同的矯正點。

2 航跡規劃算法

2.1 簡化的貪心A*算法

A*算法是靜態路網中求解最短路的高效方法之一。作為一種啟發式算法，A*算法既具有明顯的速度優勢又在理論上保證收斂到全局最優解，其代價函數如式(8)：

f(n)=g(n)+h(n)

(8)

式中：f(n)為至當前節點n的代價總和；g(n)為航跡起點到該節點的實際代價；h(n)為該節點到航跡終點的預估代價。

通常情況下，啟發函數中的代價使用兩點的歐氏距離來表示。

傳統的A*算法屬于二維柵格法，在搜索下一節點過程中，需使用鄰域定義確定搜索范圍。在非柵格場景中，若直接將A*算法推廣至三維空間，代價函數往往不能精確刻畫實際航跡代價，使問題求解變得困難。為使其應用于圖G(V,E)表示的三維航跡規劃問題，對A*算法的中的下一節點的搜索范圍重新定義，如圖1。

圖1 備選校正點Fig. 1 Alternative correction points

算法在確定下一節點時，從當前節點出發，根據最大搜索半徑對下一節點進行篩選作為備選節點。由于圖G(V,E)屬于NPC問題，為降低時間復雜度，對A*算法進行簡化：①根據文獻[7]，將下一節點定為當前備選節點中式(8)代價最小的節點；②根據貪心策略由式(7)決定備選點的最大搜索半徑，并對備選點進行剪枝操作，最終經貪心策略剪枝后結果如圖2，此時縮小了矯正點的選擇范圍。

圖2 貪心策略后的備選校正點Fig. 2 Alternative correction points under greedy strategy

得到圖2備選矯正點后，可以使用A*算法快速求解圖G(V,E)表示的三維航跡規劃問題得到初始路徑。

簡化后的貪心A*算法如下：

Algorithm:簡化貪心A?算法Input: α1,α2,β1,β2起點A,終點B,棧S,棧L,當前節點TEMP,航跡空間TOutput: LWhile(不能到達終點B)1)If(TEMP==NULL)

2) TEMP =A3) L.Push(TEMP)4) 隨機選擇下一節點類型;5)Else根據公式(7)確定下一節點類型6)根據公式(2),(3)確定備選點范圍7)If (備選點存在)使用公式(8)確定備選范圍內各點代價,將各備選點按代價從大至小排序后壓入棧S.Push()8)將棧S中代價最小的彈出TEMP=S.Pop(),其余壓棧L.Push(TEMP)9)Else TEMP = L.Pop()10)輸出:L

2.2 稀疏A*初始化的貪心蟻群算法

蟻群算法是基于螞蟻之間簡單交互的高效智能算法。在優化無人救生船航行路徑[8]，解決無人機三維的航跡規劃問題[9]，以及在旅行商問題求解[3]上，均獲得了較好的結果。

使用蟻群算法求解航跡問題時，將航跡空間定義為所有螞蟻的可行解空間。通過不斷更新路徑上的信息素濃度，由于正反饋作用，路徑不斷優化，最終得到滿足規劃要求的航跡。

當每次路徑搜索完成時，需要更新航跡空間的信息素濃度分布。通過信息素的信息交互方式是蟻群算法的獨特優勢。信息素更新如式(9)、式(10)：

τi,j=(1-ρ)×φi,j+Δγi,j

(9)

(10)

由于蟻群算法初始搜索策略的隨機性，使算法在起始點會搜索許多無效路徑，既影響了算法的航跡規劃質量，又增加了系統運行時間。同時經典蟻群算法信息素均按預設常數更新，未考慮蟻群整體質量。筆者針對蟻群算法做出改進：①使用A*算法生成的初始航跡替換蟻群算法中的隨機初始化方法；② 螞蟻選擇下一節點時，使用式(7)對校正點剪枝后，再使用輪盤賭法進行選擇；③借鑒粒子群中的適應度概念，定義一個自適應系數對信息素更新進行修改，此時，式(10)變為式(11)：

(11)

式中：Fk定義第k只螞蟻航跡距離終點距離的倒數；s為啟發值常數，取值范圍為[0,1]。

此外，對式(5)的目標，在保證式(6)路徑存在的情況下，通過降低初始化路徑節點數求得。

稀疏A*初始化的貪心蟻群算法定義如下：

Algorithm: 稀疏A?初始化的貪心蟻群算法Input:Input: α1,α2,β1,β2起點A,終點B,棧L,螞蟻數量m,迭代次數n,航跡空間TOutput: min(L′)1)使用簡化貪心A?算法得到初始航跡L2)For i=1:m t = random(length(L));3)將L中t至length(L)的航跡點隨機生成4)For j=2:n For i=1:m For k=1: length(L)5)按公式(7)確定校正類型與備選點范圍6)從備選點中找到所有滿足公式(2),(3)的校正點7)根據信息素濃度,按照經典輪盤賭法選擇校正點8)If(航跡能到達終點B) 記錄航跡至L′9)按照公式(9)(11)更新航跡空間的信息素濃度10)輸出:L′中公式(6)最小的路徑

3 三維空間的Dubins曲線平滑算法

在作業中，考慮到飛行器航跡需要滿足對最小轉彎半徑的約束，需對航跡進行光滑處理。目前對航跡的平滑處理已經成為航跡規劃研究中的必備步驟。如：文獻[10]由于B樣條曲線具有參數表達簡單，局部修改性，凸包性等優點，使用三次B樣條曲線對規劃軌跡進行優化；文獻[15]針對航帶間轉彎軌跡進行了分類研究；文獻[5]使用對規劃航跡進行了優化，此外圓切法等方法也被用于不規則航跡的光滑。

但目前的方法都存在一定缺陷：B樣條方法需要對整條航跡進行規劃，不能滿足航跡最短的要求；圓切法規劃的航跡會使路徑繞過校正點；經典Dubins曲線僅用于對二維平面曲線進行光滑。

由于目前算法均不能滿足三維空間無人機下需經過規劃點且軌跡最短的要求。因此，筆者將Dubins曲線算法推廣至三維空間，該算法為降低運算復雜度，將曲線的求解過程投影到二維進行求解后再映射回三維計算航跡長度。算法的主要步驟為：①坐標點變換；②圓心計算；③切點計算；④坐標點逆變換；⑤得到光滑后航跡。

以給定的三個矯正點A′(x′1,y′1,z′1)，B′(x′2,y′2,z′2)，C′(x′3,y′3,z′3)為例說明算法的詳細實現過程。

3.1 坐標點變換

對坐標系進行變換，將A′,B′,C′ 三個點所在平面作為X′O′Y′平面，如圖3。將圖3(a)所示的三維問題轉化為圖3(b)中在X′O′Y′平面中的二維問題。

圖3 坐標變換Fig. 3 Coordinate transformation

首先進行坐標點變換。構造向量A′B′，B′C′分別為k1,k2。

(12)

進行施密特正交化：

(13)

對結果進行單位化后得到η1,η2：

(14)

η3=(ηi,ηj,ηk)

(15)

(16)

此時對于坐標系中任意一向量P有:

P(2)=P(3)M

(17)

式中：P(2)為以新坐標系為基的向量；P(3)為以原坐標系為基的向量。

將A′,B′,C′分別代入式(17)進行坐標變換，截取坐標前兩個維度后得到：A(x1,y1)，B(x2,y2)，C(x3,y3)。

3.2 圓心計算

根據以上步驟，得到了二維平面上的三個坐標A,B,C。此時需要在二維平面上使用Dubins曲線進行路徑的平滑，如圖4。

圖4 平面Dubins曲線Fig. 4 Dubins curve in plane

直線LAB的兩點式為：

(18)

設圓心O坐標為(x0,y0)，將式(18)化為一般式：

(y2-y1)x-(x2-x1)y-(y2-y1)x1-(x2-x1)y1=0

(19)

已知圓心O到LAB的最短距離為R，由點到直線距離公式有：

(20)

又有OB·AB=0，則：

(x2-x1)(x2-x0)-(y2-y1)(y2-y0)=0

(21)

聯立式(20)、式(21)得到兩個解，圓心O為兩個解中距離C(x3,y3)點較近的坐標。

3.3 切點計算

此時已經求得圓心，根據圓心進一步對切點進行計算。

由圖4可知，過C(x3,y3)點的直線LCT的直線為：

kx-y-kx3+y3=0

(22)

設切點T坐標為(xT,yT)，切點T到圓心的最短距離為半徑R。即：

(23)

聯立公式(22)、式(23)并求解，得到斜率不同的兩個切點T1,T2。最終切點T坐標取T1,T2中∠BOTi(i=1,2)中角度較小的Ti。

3.4 坐標點逆變換

按照對切點T進行逆變換P(2)M-1得到T′(x′T,y′T,z′T)，此時LA′B′,LT′C′,B′T′,即為光滑后的航跡。

4 仿真實驗驗證

4.1 實驗設計

設計仿真實驗驗證筆者算法的有效性，：

飛行器起點為S點，終點為E點。在飛行區域內分布有613個校正點(編號0至612)，用于飛行器誤差校正，且校正點在空間的坐標已知。校正點類型包括垂直校正點與水平校正點。校正點分布位置依賴于地形，無統一規律。飛行器每飛行1 m，垂直誤差和水平誤差將各增加δ個專用單位，下簡稱單位。到達終點時，垂直誤差和水平誤差均應小于θ個單位。為簡化問題，設當垂直和水平誤差均小于θ單位時，飛行器仍能按照規劃路徑飛行。

在出發地A點，飛行器的垂直和水平誤差均為0。飛行器在垂直誤差校正可將其垂直誤差校正為0，水平誤差保持不變。在水平誤差校正點也只校正水平誤差為0。矯正機制設置如1.1節。假設飛行器的最小轉彎半徑為R。

給定參數如下(數據來自2019年全國研究生數學建模競賽F題)：

α1=25，α2=15，β1=20，β2=25，

θ=30，δ=0.001，R=200

4.2 實驗對比

實驗環境：Intel Core i5-7300HQ，GTX 1050Ti顯卡，16G內存+128SSD，算法使用MATLAB R2014b編寫。

不同算法的路徑規劃對比結果如表1。粒子群算法參數設置為最大迭代次數為100，粒子數量為20。蟻群算法參數為最大迭代次數為100，蟻群數量為20，啟發值常數為0.5，信息素常量為10。筆者算法為：蟻群+貪心+啟發值+ A*初始化。

表1 實驗對比Table 1 Experiment comparison

表1中為消除算法隨機性，將算法運行10次取最優結果作為最終結果實驗結果表明，在校正點的航跡規劃任務中，使用貪心策略的蟻群算法的最優長度為112.013 km，優于粒子群算法與經典A*算法。啟發值融合貪心策略后，能有效降低經典蟻群算法在無效路徑的嘗試，時間從8.11 降至6.11 s，路徑長度為109.798 km。證明啟發值能有效優化算法的運行效率與結果。

4.3 參數影響

考慮到算法主要參數對結果的影響，對蟻群算法的迭代次數，蟻群數量，啟發值進行消融實驗。評價指標為十次算法運行中最優路徑。

參數選擇時，較大的蟻群數量與較低的啟發值常數可以增加有效解數量，而解的數量多說明該組解的魯棒性較強，因而多次迭代后往往能獲得較短的航跡長度。啟發值，迭代次數和蟻群數量三個模型參數對路徑長度的影響程度，如圖5。

圖5 模型參數對比Fig. 5 Comparison of model parameters

由圖5可知，啟發值選擇0.1時，蟻群評價對路徑的改進效果最好，此時路徑長度較短。迭代次數并不總是越多越好，最好的結果在蟻群數量為100時得到。蟻群數量太少削弱了解的多樣性，蟻群的數量多則會增加算法復雜度，但根據實驗，蟻群數量合理增加有助于生成更高效的解。

為研究各個參數相互影響下路徑長度的變化，對比了迭代次數,啟發值與蟻群數量對最終結果的影響，如圖6。由于圖5中，迭代次數和啟發值的波動劇烈且蟻群數量最優長度差距較小，為突出參數間的影響，選擇蟻群數量差距較大的20和100作為對比。結果顯示，蟻群數量固定時，最優解均出現在圖像的右下角，即迭代次數和啟發值取較小值。又圖6可知，隨著蟻群數量的增加，較小的啟發值常數下算法規劃的路徑長度更短。

圖6 模型復合參數對比Fig. 6 Comparison of model composite parameters

綜合考慮圖5、圖6。筆者在蟻群算法時均選擇啟發值常數為0.1 蟻群數量為100 ，最大迭代次數100。

使用Dubins曲線平滑后航跡路徑如圖7。

圖7展示了對起點S點至終點E點的規劃路徑，規劃空間為100 km×100 km×10 km，空間內分布有包括起點0，終點612，共計613個規劃點。實心圓點為垂直校正點，空心圓點為水平校正點。僅使用貪心A*算法便可以獲得較好的規劃結果，貪心蟻群算法規劃的軌跡更加平滑，加入A*初始化后，貪心蟻群算法規劃航跡的前半部分得到了優化，規劃航跡進一步縮短。

圖7 規劃航跡Fig. 7 Planning track

最終規劃航跡節點編號為：0，506，294，91，607，540，250，340，277，612，經過8個校正點，航跡總長度為104.06km。改進后結果較原始結果航跡長度減少了約6%，時間減少了約25%。

6 結 語

通過對基于校正點的航跡規劃研究，提出了一種使用A*算法優化的自適應蟻群算法，該算法融合多種策略對原始蟻群進行優化，不僅增強了算法在解空間上的搜索效率，也提高了最優解的質量。在參數設置上，當蟻群數量較大時，設置較小的啟發值常數能獲得更好的結果。迭代次數需要根據具體實驗進行適當選擇，太小算法難以收斂，太大則會造成算法出現過擬合現象。

對貪心蟻群算法使用A*算法進行初始化，能同時滿足多約束下綜合代價最小的規劃要求。實驗仿真表明：改進后航跡長度減少了約6%，時間減少了約25%。使用A*算法優化的自適應蟻群算法能有效的在復雜環境中針對多目標進行航跡規劃，保證了無人機在極限條件下完成戰術任務的成功率。