基于改進Theta*算法的直升機三維航路規(guī)劃

任天祥, 賀建良， 鄒 杰

(中國航空工業(yè)集團公司洛陽電光設(shè)備研究所，河南 洛陽 471000)

0 引言

直升機在執(zhí)行作戰(zhàn)任務(wù)前需要規(guī)劃好飛行路徑，在低空飛行條件下考慮敵方威脅和地形回避等問題，規(guī)劃一條從當(dāng)前位置抵達目標(biāo)位置的飛行航路，以提高直升機的生存概率和作戰(zhàn)效率。航路規(guī)劃是一個在復(fù)雜多約束條件下實現(xiàn)規(guī)劃的過程，在遺傳算法[1]、粒子群算法[2]、人工勢場法[3]、傳統(tǒng)A*算法[4]等多種智能算法和相關(guān)改進算法中，傳統(tǒng)A*算法在三維航路規(guī)劃問題上實現(xiàn)了良好應(yīng)用。

Theta*算法[5]在路徑搜索過程中結(jié)合傳統(tǒng)A*算法和基于可視圖A*算法[6]兩者的優(yōu)點，路徑代價更小且減少了路徑轉(zhuǎn)向次數(shù)，航路更為平滑，但搜索速度相對變慢。本文在Basic Theta*算法的基礎(chǔ)上采用變權(quán)重評估函數(shù)和減少無效節(jié)點計算量的方法改進了直升機三維空間航路規(guī)劃算法，進一步減小規(guī)劃航路的代價，提升搜索效率和路徑規(guī)劃效果。

1 威脅環(huán)境建模

直升機的三維空間全局航路規(guī)劃問題可以轉(zhuǎn)化為找到飛行經(jīng)過的航路點集{s,x1,x2,…,xn,d}，針對直升機低空飛行的特點，航路點以及航路點間的連線需要盡量避免規(guī)劃空間中的威脅，包括地形威脅，主要是山峰和建筑物，還有局部渦流等惡劣氣象威脅，雷達探測威脅，禁飛區(qū)威脅，其他威脅情況可以按照上述威脅模型簡化。

1.1 地形威脅

地形威脅主要是對規(guī)劃區(qū)域中的山體進行描述，單個山峰可以模擬表示為[7]

z(p,q)=hi*e-[(p-ai)/ki]2-[(q-bi)/ki]2

(1)

式中：(p,q)表示山峰表面某點在水平面的投影坐標(biāo)；z為該點對應(yīng)的高度值；hi為控制山峰高度的參數(shù)；(ai,bi)表示山峰中心的投影坐標(biāo)；ki控制山峰的地形坡度。

圖1為模擬山峰示意圖。

圖1 模擬山峰示意圖Fig.1 Sketch map of simulated mountain

單座山峰對直升機的威脅值表示為

(2)

式中：Rm為直升機當(dāng)前高度山體截面的最大半徑；dm為直升機到該截面中心的距離。直升機與山體表面距離越近，危險越大；距離小于4 m表示直升機即將與山體相撞，威脅值為無窮大?？紤]到直升機的飛行特性，設(shè)置直升機距離地形表面的最小高度為10 m，低于此高度直升機飛行過程有撞地風(fēng)險。

1.2 惡劣氣象威脅

惡劣氣象威脅區(qū)域近似表示為圓柱體，對直升機的威脅值表示為

(3)

式中：de為直升機到圓形中心的距離；Re為該圓形的半徑。惡劣氣象威脅在直升機高度的水平截面是一個圓形,在威脅圓內(nèi)，直升機距離圓心越近，危險越大。

1.3 雷達探測威脅

雷達探測威脅區(qū)域表示為球體，對直升機的威脅值表示為

(4)

式中：dr為直升機到威脅球心的距離；Rd為威脅球體的半徑。

1.4 禁飛區(qū)威脅

直升機進行航路規(guī)劃時需要避開規(guī)定的禁飛區(qū)域，飛行航線不能落入禁飛區(qū)中。禁飛區(qū)對第i段航路的威脅值可以表示為

(5)

式中：ri為第i段航路路徑；ZNF為禁飛區(qū)空間，兩個區(qū)域若有交點，則威脅值為無窮大。

2 改進的Theta*算法

2.1 Basic Theta*算法

A*算法是一種常用的啟發(fā)式路徑搜索算法，通過不斷評估規(guī)劃空間中搜索節(jié)點的代價來確定最優(yōu)路徑。Basic Theta*算法是A*算法的一種變體，與A*算法的估價函數(shù)均可表示為

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

(6)

式中：g(n)表示從起始節(jié)點s到當(dāng)前節(jié)點n的航路代價；h(n)表示從當(dāng)前節(jié)點n到目標(biāo)節(jié)點d的航路代價。A*算法中一個節(jié)點的父節(jié)點一定與該節(jié)點相鄰，而Basic Theta*算法沒有此限制。記節(jié)點n的父節(jié)點為p(n)，與節(jié)點n相鄰的節(jié)點集合為N(n)，節(jié)點n′屬于N(n)。Basic Theta*算法判斷p(n)和每個節(jié)點n′之間是否可視(即是否存在障礙物)，并據(jù)此選擇代價更小的路徑。Basic Theta*算法的搜索流程如下。

1) 初始化open表和close表，將起始節(jié)點s插入到open表中。

2) 若open表為空，搜索失敗，退出程序。

3) 從open表中取出路徑代價f(n)最小的節(jié)點插入到close表中作為當(dāng)前節(jié)點n。

4) 若當(dāng)前節(jié)點n是目標(biāo)節(jié)點d，則通過節(jié)點n的父節(jié)點p(n)回溯到起點，即找到路徑節(jié)點集合，算法搜索過程結(jié)束；若節(jié)點n不是目標(biāo)節(jié)點d，則執(zhí)行5)。

5) 對節(jié)點n的相鄰節(jié)點集合N(n)中的每個節(jié)點n′執(zhí)行如下操作：若節(jié)點n′在close表中，則忽略節(jié)點n′；否則執(zhí)行6)。

6) 計算節(jié)點n′的代價并檢查節(jié)點p(n)與節(jié)點n′的可視性。

① 若p(n)和n′兩節(jié)點之間可視，那么飛行航路s→…→p(n)→n→n′的路徑代價記為g(n′)，飛行航路s→…→p(n)→n′的路徑代價記為g(p(n))+c(p(n),n′)，其中，c(p(n),n′)表示p(n)→n′的路徑代價。若g(p(n))+c(p(n),n′)

② 若p(n)和n′兩節(jié)點之間不可視，并且航路s→…→n→n′的路徑代價g(n)+c(n,n′)小于航路s→…→n的路徑代價g(n′)，那么更新g(n′)=g(n)+c(n,n′)，p(n′)=n。若n′此前已在open表中則更新n′，否則將n′ 插入到open表中。

7) 根據(jù)代價值對open表中節(jié)點排序，然后轉(zhuǎn)到2)。

2.2 改進的Basic Theta*算法

2.2.1 變權(quán)重航路代價評估函數(shù)

Basic Theta*算法的航路代價評估函數(shù)完全繼承了A*算法，選擇變權(quán)重的評估方法改進Theta*算法搜索效果[7]，即

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

(7)

式中，g(n)和h(n)的定義決定了在航路規(guī)劃的不同階段，它們對路徑搜索的影響效果不同。若β較大，算法運行初期搜索效果較好；若α較大,算法運行后期搜索效果較好。調(diào)整g(n)和h(n)的權(quán)重

α(x)=αmin+(αmax-αmin)(1-h(n)/D)

(8)

β=1-α

(9)

式中，D表示起始節(jié)點到目標(biāo)節(jié)點的歐氏距離。算法搜索過程中，α從最小值αmin變化到最大值αmax，提高了路徑搜索的合理性。

直升機執(zhí)行作戰(zhàn)任務(wù)時，既要保證自身能夠避開執(zhí)行任務(wù)區(qū)域的威脅，又要保證飛行航路盡可能短，以縮短飛行時間、減少油耗。因此航路代價函數(shù)設(shè)計為

(10)

式中：Li為第i段航路的長度代價；Ti為地形威脅代價；Ei為惡劣天氣威脅代價；Pi為雷達探測威脅代價；Zi為禁飛區(qū)威脅代價；ω1，ω2，ω3，ω4，ω5分別為對應(yīng)威脅值的權(quán)重。Li的算式為

(11)

式中，(xi,yi,zi)，(xi+1,yi+1,zi+1)分別是第i段航路起點與終點的橫坐標(biāo)、縱坐標(biāo)和高度坐標(biāo)。

設(shè)航路節(jié)點n的坐標(biāo)為(xc,yc,zc),目標(biāo)節(jié)點d的坐標(biāo)為(xd,yd,zd)，啟發(fā)函數(shù)h(n)表示為

(12)

2.2.2 改進路徑節(jié)點搜索過程

Basic Theta*算法搜索時間較長的主要原因是需要做大量的可視性檢查。有多少點進入open表就要執(zhí)行多少次節(jié)點可視性檢查，若規(guī)劃空間的柵格數(shù)量較多，則用于判斷可視性的計算量較大，進而影響路徑節(jié)點搜索速度。

實際航路規(guī)劃情況中，有很多進入open表的集合N(n)中的節(jié)點n′最終不屬于路徑節(jié)點，這意味著有大量可視性檢查是無效的，因此，從減少檢查次數(shù)這一方面來改進節(jié)點搜索過程。

改進算法假定節(jié)點n的父節(jié)點p(n)與相鄰節(jié)點集合N(n)中的所有節(jié)點n′都具備可視性，即對所有節(jié)點n′執(zhí)行Basic Theta*算法步驟6)中的①操作。改進算法將節(jié)點可視性檢查放在原來的步驟3)和4)之間，如果兩點可視，算法繼續(xù)執(zhí)行；否則，重新計算n′及其父節(jié)點。改進算法中只有真正要擴展的節(jié)點才會實際執(zhí)行可視性檢查，顯著減少了運算量。

3 仿真結(jié)果與分析

3.1 航路規(guī)劃空間定義

基于Matlab R2017A軟件平臺進行算法仿真。設(shè)置直升機的起始節(jié)點坐標(biāo)為(10,10,0.1)，目標(biāo)節(jié)點坐標(biāo)為(95,95,1)，單位均為km。規(guī)劃區(qū)域的山峰、惡劣天氣區(qū)域、禁飛區(qū)以及雷達參數(shù)分別如表1-表4所示。

表1 山峰地形參數(shù)Table 1 Parameters of the mountains

表2 惡劣天氣區(qū)域參數(shù)Table 2 Parameters of the bad-weather area

表3 禁飛區(qū)參數(shù)Table 3 Parameters of the no-fly zones

表4 雷達參數(shù)Table 4 Parameters of the radars

3.2 航路規(guī)劃結(jié)果和分析

圖2為三維航路規(guī)劃路線圖。

圖2 三維航路規(guī)劃路線圖Fig.2 Schematic of 3D path planning results

分別使用A*算法、Theta*算法和改進Theta*算法在相同三維規(guī)劃空間中進行航路規(guī)劃。A*算法和Theta*算法的路徑代價權(quán)重均取α=0.4，β=0.6；改進Theta*算法的路徑代價權(quán)重取αmin=0.4，αmax=0.6。

圖3為二維平面航線示意圖。

圖3 二維平面航線示意圖Fig.3 Schematic of the 2D planned paths

從圖2和圖3可以看出，本文提出的改進Theta*算法與其他算法相比，規(guī)劃航跡更加平滑，能夠有效對任務(wù)區(qū)域中的各種威脅進行回避。

各算法規(guī)劃出的航跡參數(shù)見表5。

表5 航跡參數(shù)表Table 5 Parameters of the paths obtained

通過對比表5中數(shù)據(jù)可以發(fā)現(xiàn)，對航路代價函數(shù)權(quán)重α和β的實時調(diào)整，有效地降低了航路規(guī)劃代價，縮短了規(guī)劃時間。

4 結(jié)束語

本文在Theta*算法基礎(chǔ)上采用實時調(diào)整航路代價估計權(quán)重的航路評價函數(shù)，并通過改進航路搜索過程來減少可視性檢查計算量，提升了規(guī)劃效率和規(guī)劃效果。通過對直升機低空飛行過程中存在的威脅進行分析建模，分別用A*算法、Theta*算法和改進Theta*算法進行航路規(guī)劃。仿真結(jié)果驗證了本文的改進Theta*算法可以有效縮短規(guī)劃時間、減少規(guī)劃步數(shù)、減小航路規(guī)劃代價，是一種有效的三維路徑規(guī)劃算法。