基于即時修復(fù)式稀疏A*算法的動態(tài)航跡規(guī)劃

王生印,龍 騰,王 祝,蔡祺生

(1.北京理工大學(xué)宇航學(xué)院,北京 100081;2.飛行器動力學(xué)與控制教育部重點(diǎn)實(shí)驗(yàn)室,北京 100081)

0 引 言

在復(fù)雜多變的真實(shí)環(huán)境中,往往存在著突發(fā)、移動威脅和機(jī)動目標(biāo),無人機(jī)必須對環(huán)境變化做出快速反應(yīng),在規(guī)定時間范圍內(nèi)即時規(guī)劃出新的可行航跡以保證無人機(jī)飛行安全,同時盡可能提高航跡最優(yōu)性以減小無人機(jī)能量消耗和提高任務(wù)效能。

目前,無人機(jī)航跡規(guī)劃方法主要包括智能優(yōu)化算法[1-5]和圖搜索算法[6-9]等。其中,智能優(yōu)化算法由于其計(jì)算量隨問題維數(shù)呈指數(shù)增長,難以在復(fù)雜環(huán)境下規(guī)劃出可行航跡[10]。而以稀疏A*搜索(sparse A*search,SAS)算法[11-14]為代表的啟發(fā)式圖搜索算法因其效率高和魯棒性強(qiáng)等優(yōu)點(diǎn),在工程上應(yīng)用最為廣泛。但SAS算法的規(guī)劃時間會隨著規(guī)劃環(huán)境復(fù)雜程度的增大而增長,并容易陷入局部搜索,難以滿足動態(tài)航跡規(guī)劃的快速性需求。快速擴(kuò)展隨機(jī)樹算法[15-18]通過引入隨機(jī)采樣能夠有效避免算法陷入局部搜索的缺陷,快速找到一條可行航跡,但算法的隨機(jī)性導(dǎo)致規(guī)劃結(jié)果穩(wěn)定性差,而且最優(yōu)性難以滿足實(shí)際應(yīng)用需求。

針對復(fù)雜動態(tài)環(huán)境中傳統(tǒng)航跡規(guī)劃算法難以穩(wěn)定地在規(guī)定時間內(nèi)生成較優(yōu)航跡的問題,國內(nèi)外學(xué)者開展了基于即時架構(gòu)的搜索算法研究[19-28]。即時搜索算法族可以在規(guī)劃開始后的任意時刻輸出當(dāng)前搜索到的最優(yōu)規(guī)劃結(jié)果[29],并且隨著計(jì)算時間的增加通過循環(huán)規(guī)劃的方式使航跡質(zhì)量不斷提升,實(shí)現(xiàn)了規(guī)劃時效性和結(jié)果最優(yōu)性之間的合理權(quán)衡,能夠有效處理動態(tài)航跡規(guī)劃問題。

即時搜索架構(gòu)主要包括權(quán)重式、窗口式和修復(fù)式[19-20,22]。文獻(xiàn)[20]提出了加權(quán)式即時A*算法,該算法能夠在較大的啟發(fā)權(quán)重下快速找到初始解,并通過后續(xù)不斷重復(fù)擴(kuò)展具有局部不一致性的節(jié)點(diǎn)以尋找更優(yōu)解。文獻(xiàn)[19]提出了窗口式即時A*算法,通過設(shè)置窗口大小從而限制擴(kuò)展節(jié)點(diǎn)的數(shù)目,使得只有在窗口內(nèi)的節(jié)點(diǎn)才能加入擴(kuò)展;當(dāng)初始窗口為0時,該方法退化為深度優(yōu)先搜索算法,在復(fù)雜環(huán)境下可能錯失可行解,并且隨著窗口的增大后續(xù)的搜索效率也逐漸降低。文獻(xiàn)[22]提出了即時修復(fù)式A*(anytime repairing A*,ARA*)算法,通過逐次減小啟發(fā)項(xiàng)權(quán)重多次執(zhí)行加權(quán)A*算法,以獲得更優(yōu)的規(guī)劃結(jié)果;ARA*算法能夠在每一次迭代過程中利用以往的擴(kuò)展信息提高后續(xù)搜索效率。

文獻(xiàn)[25]對修復(fù)式、權(quán)重式和重啟式3種即時搜索架構(gòu)進(jìn)行了分析和對比,結(jié)果表明即時修復(fù)式構(gòu)架能夠更快地收斂到最優(yōu)解,并且針對不同的搜索算例具有更強(qiáng)的魯棒性。但是該架構(gòu)需要依賴很小的啟發(fā)項(xiàng)權(quán)重才能獲得最優(yōu)解[13],該特點(diǎn)降低了算法節(jié)點(diǎn)擴(kuò)展的效率。此外,現(xiàn)有的即時A*算法主要針對機(jī)器人和機(jī)械臂規(guī)劃問題[21,23],無法直接應(yīng)用于無人機(jī)航跡規(guī)劃。

本文基于無人機(jī)動態(tài)航跡規(guī)劃的需求,將即時修復(fù)式構(gòu)架與SAS相結(jié)合,并針對即時修復(fù)式構(gòu)架中低啟發(fā)項(xiàng)偏好缺陷和算法效率對規(guī)劃區(qū)域和障礙尺度強(qiáng)烈敏感的問題進(jìn)行改進(jìn),引入雙排序準(zhǔn)則、存儲空間約束及變步長策略,提出了即時修復(fù)式稀疏A*搜索(anytime repairing SAS,AR-SAS)算法。靜態(tài)環(huán)境下蒙特卡羅仿真結(jié)果表明AR-SAS算法生成可行航跡與最優(yōu)航跡的時間都小于分層SAS算法[12,14]和標(biāo)準(zhǔn)SAS算法[26]。同時,動態(tài)環(huán)境仿真結(jié)果表明,AR-SAS算法能夠快速規(guī)劃出較優(yōu)的可行航跡,具有較高的規(guī)劃效率和較強(qiáng)的魯棒性,滿足無人機(jī)動態(tài)航跡規(guī)劃的需求。

1 航跡規(guī)劃問題建模

無人機(jī)航跡規(guī)劃需綜合考慮地形、氣象、障礙等環(huán)境因素以及平臺自身的飛行性能約束,為無人機(jī)制定出從初始位置到目標(biāo)位置的最優(yōu)飛行路徑[27,30]。

記無人機(jī)三維空間內(nèi)位置為(x,y,z),其中x、y表示東向和北向位置坐標(biāo),z表示海拔高度。無人機(jī)航跡通過一組節(jié)點(diǎn)序列{Sstart,S1,…,Sn-1,G}來表示,其中Sstart為起始點(diǎn),G為目標(biāo)點(diǎn),S1,S2,…,Sn-1為中間待規(guī)劃的航跡點(diǎn)。

在無人機(jī)航跡規(guī)劃過程中需要考慮無人機(jī)機(jī)動能力、地形、威脅等約束,具體如下。

(1) 最大轉(zhuǎn)彎角約束:受無人機(jī)機(jī)動性能的約束,規(guī)劃的航跡需要避免過大的轉(zhuǎn)彎角,以保證航跡合理可行。假設(shè)最大轉(zhuǎn)彎角為θmax,則最大轉(zhuǎn)彎角約束可表示為

θi≤θmax

(1)

式中,θi表示第i航跡點(diǎn)處的轉(zhuǎn)彎角，i=1,2,…,n。

(2) 最大爬升/俯沖角約束:受無人機(jī)推力、重力等因素的影響,無人機(jī)的爬升/俯沖能力存在一定的極限值。假設(shè)無人機(jī)最大爬升/俯沖角為Φmax,則最大爬升/俯沖角約束表示為

Φi≤Φmax

(2)

式中,Φi表示第i航跡點(diǎn)處的爬升/俯沖角。

(3) 最小航段長度約束:受無人機(jī)機(jī)動性能限制,無人機(jī)每次改變航向前必須保證一個最短的直線飛行距離。假設(shè)無人機(jī)最短直飛距離為lmin,則最小航段長度約束表示為

li≥lmin

(3)

式中,li表示第i段航跡的長度,其表達(dá)式為

(4)

(4) 最小相對高度約束:為了保證無人機(jī)的飛行安全,飛行過程中無人機(jī)需與地面保持一定的相對安全距離。假設(shè)允許的最小相對高度為hmin,則最小相對高度約束可表示為

z-hT≥hmin

(5)

式中,hT表示無人機(jī)正下方的地形海拔高度。

(5) 禁飛區(qū)約束:假設(shè)環(huán)境中有M個威脅(文中考慮圓柱威脅),為保證飛行安全,要求無人機(jī)在任意時刻都位于威脅區(qū)域外部,即?m∈{1,2,…,M},有

‖s(t)-pm‖2≥rm,?t∈[t0,tf]

(6)

式中,pm=[xm,ym]T為第m個威脅的位置;rm為對應(yīng)威脅的半徑。

2 AR-SAS航跡規(guī)劃算法

本節(jié)將即時修復(fù)式構(gòu)架與SAS算法相結(jié)合,并引入雙排序準(zhǔn)則、存儲空間約束及變步長策略,提出了AR-SAS動態(tài)航跡規(guī)劃算法。下面針對AR-SAS算法流程及其中應(yīng)用的改進(jìn)策略分別展開描述。

2.1 AR-SAS算法流程

AR-SAS算法通過將SAS算法[26]嵌入到即時修復(fù)式架構(gòu)中,實(shí)現(xiàn)兩者優(yōu)勢的結(jié)合,利用將航跡約束加入節(jié)點(diǎn)擴(kuò)展過程的策略以縮小搜索空間、加快算法收斂速度和保證航跡結(jié)果可行性,同時基于即時修復(fù)式架構(gòu)實(shí)現(xiàn)極短時間內(nèi)可行航跡的生成以及后續(xù)對現(xiàn)有航跡的不斷優(yōu)化。此外,通過引入雙排序準(zhǔn)則、存儲空間約束以及變步長策略,克服了即時修復(fù)式構(gòu)架中低啟發(fā)項(xiàng)偏好缺陷以及算法效率對規(guī)劃區(qū)域和障礙尺度敏感的問題,提升了算法的魯棒性。

AR-SAS算法偽代碼如表1所示,其中調(diào)用的定制加權(quán)SAS算法偽代碼如表2所示。

AR-SAS算法的詳細(xì)步驟如下：

步驟1(2-3行):節(jié)點(diǎn)鏈表初始化,并將目標(biāo)點(diǎn)真實(shí)代價(即初始最優(yōu)航跡的長度)設(shè)為無窮大;

步驟2(4行):判斷規(guī)劃是否超出時限或啟發(fā)項(xiàng)權(quán)重系數(shù)ε1是否小于1,若是則算法退出并輸出當(dāng)前最優(yōu)航跡,否則執(zhí)行步驟3;

步驟3(5行):根據(jù)輸入的起終點(diǎn)、當(dāng)前啟發(fā)項(xiàng)權(quán)重系數(shù)以及擴(kuò)展步長使用定制加權(quán)SAS算法進(jìn)行單次可行航跡規(guī)劃;

步驟4(6-7行):由CLOSED表節(jié)點(diǎn)回溯獲得當(dāng)前規(guī)劃航跡并保存;

步驟5(8-12行):根據(jù)規(guī)則減小啟發(fā)項(xiàng)權(quán)重ε1、ε2和擴(kuò)展步長;

步驟6(13-14行):將INCONS表中節(jié)點(diǎn)移入OPEN表中并根據(jù)新的啟發(fā)項(xiàng)權(quán)重更新節(jié)點(diǎn)代價,返回步驟2。

表1 AR-SAS算法偽代碼Table 1 Pseudo code of AR-SAS

表2 定制加權(quán)SAS算法偽代碼Table 2 Pseudo code of customized weighted SAS

針對AR-SAS算法架構(gòu)定制的加權(quán)SAS算法詳細(xì)步驟如下:

步驟1(1行):判斷OPEN表中所有節(jié)點(diǎn)代價key(s,1)中最小值是否小于目標(biāo)節(jié)點(diǎn)真實(shí)代價(即當(dāng)前最優(yōu)航跡的長度),若是則進(jìn)行步驟2,否則退出;

步驟2(2-5行):判斷當(dāng)前擴(kuò)展節(jié)點(diǎn)是否滿足g(s)

步驟3(6-7行):將該節(jié)點(diǎn)移入CLOSED表作為當(dāng)前擴(kuò)展節(jié)點(diǎn)并擴(kuò)展該節(jié)點(diǎn);

步驟4(8-16行):根據(jù)各擴(kuò)展子節(jié)點(diǎn)的信息將各個子節(jié)點(diǎn)放入對應(yīng)的節(jié)點(diǎn)鏈表中;

步驟5(17-18行):判斷OPEN表節(jié)點(diǎn)數(shù)量是否超出存儲界限,若是刪除key(s,2)較大的節(jié)點(diǎn)直至滿足存儲界限約束,返回步驟1。

AR-SAS算法中代價函數(shù)計(jì)算、啟發(fā)項(xiàng)權(quán)重和鏈表更新、節(jié)點(diǎn)擴(kuò)展及收斂條件判斷的具體方法分別如下所述。

(1) 代價函數(shù)計(jì)算

AR-SAS算法在節(jié)點(diǎn)擴(kuò)展過程中代價函數(shù)的一般形式可表示為

f(s)=g(s)+ε×h(s)

(7)

式中,g(s) 表示從起始點(diǎn)到當(dāng)前節(jié)點(diǎn)s的真實(shí)代價;h(s) 表示當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)代價;ε為啟發(fā)項(xiàng)權(quán)重系數(shù)(ε≥ 1)。

為了快速得到可行航跡,初始啟發(fā)項(xiàng)權(quán)重一般較大,導(dǎo)致算法不再具有可接納性[31]且不滿足一致性[32]條件。因此在迭代搜索路徑的過程中,AR-SAS同時保存3個節(jié)點(diǎn)鏈表,即OPEN表、CLOSED表和INCONS表。OPEN表保存未擴(kuò)展節(jié)點(diǎn),CLOSED表保存已擴(kuò)展節(jié)點(diǎn),INCONS表保存具有非一致性的節(jié)點(diǎn)。

當(dāng)AR-SAS搜索重復(fù)擴(kuò)展至CLOSED表中某一節(jié)點(diǎn),并使該節(jié)點(diǎn)的g值下降時,則更新該節(jié)點(diǎn)代價值,并將該節(jié)點(diǎn)放入INCONS表中。INCONS表中的節(jié)點(diǎn)不會被立即擴(kuò)展,而是會在下一次迭代搜索過程與OPEN表合并,并重新排序進(jìn)行擴(kuò)展。

(2) 啟發(fā)項(xiàng)權(quán)重和鏈表更新

在完成單次航跡規(guī)劃后,若規(guī)劃時間還沒超出限度,則進(jìn)行啟發(fā)項(xiàng)權(quán)重和節(jié)點(diǎn)鏈表更新,并以此進(jìn)行下一次的加權(quán)SAS規(guī)劃。

AR-SAS算法中啟發(fā)項(xiàng)權(quán)重的更新采用線性縮減方式,表示為

ε=ε-Δε

(8)

式中,Δε參考文獻(xiàn)[22]的經(jīng)驗(yàn)值取為0.1。

鏈表的更新包括:將CLOSED表置空;將INCONS表中節(jié)點(diǎn)移入OPEN表,并將INCONS表置空;將OPEN表中的節(jié)點(diǎn)代價值按新的啟發(fā)項(xiàng)權(quán)重進(jìn)行重新計(jì)算。

(3) 節(jié)點(diǎn)擴(kuò)展

為了有效縮減節(jié)點(diǎn)搜索空間,縮短規(guī)劃時間,AR-SAS算法將最小航跡長度、最大轉(zhuǎn)彎角和最大爬升/下滑角等飛行器動力學(xué)約束以及飛行高度、禁飛區(qū)和地形限制等環(huán)境約束條件結(jié)合到節(jié)點(diǎn)擴(kuò)展過程中,若節(jié)點(diǎn)不滿足飛行器動力學(xué)約束或與環(huán)境約束沖突,則該節(jié)點(diǎn)被視為無效節(jié)點(diǎn),不再擴(kuò)展。

(4) 收斂條件

標(biāo)準(zhǔn)SAS算法只執(zhí)行單次搜索,其搜索退出條件為目標(biāo)點(diǎn)被擴(kuò)展,而在AR-SAS算法中,由于每次執(zhí)行加權(quán)SAS算法時都會根據(jù)新的啟發(fā)項(xiàng)權(quán)重對OPEN表節(jié)點(diǎn)進(jìn)行代價更新,因此每次執(zhí)行加權(quán)SAS算法的退出條件為

f(sgoal)

(9)

式中,sgoal表示目標(biāo)節(jié)點(diǎn);f(sgoal)表示當(dāng)前路徑代價;mins∈OPEN(f(s))表示OPEN表中所有節(jié)點(diǎn)代價的最小值。單次加權(quán)SAS算法搜索過程中,若OPEN表中所有節(jié)點(diǎn)的代價值均大于當(dāng)前路徑代價,則退出該次路徑搜索。

2.2 AR-SAS算法策略

針對即時修復(fù)式構(gòu)架中低啟發(fā)項(xiàng)偏好缺陷和算法效率對規(guī)劃區(qū)域和障礙尺度強(qiáng)烈敏感的問題,本節(jié)提出了雙排序準(zhǔn)則、存儲空間約束及變步長策略,以進(jìn)一步提升AR-SAS算法的魯棒性和搜索效率,具體描述如下。

(1) 雙排序準(zhǔn)則

當(dāng)AR-SAS算法完成一次搜索后,將INCONS表中節(jié)點(diǎn)移入OPEN表中,OPEN表將依據(jù)新的啟發(fā)項(xiàng)權(quán)重系數(shù)更新表中節(jié)點(diǎn)信息。由于啟發(fā)項(xiàng)權(quán)重的影響,距離目標(biāo)節(jié)點(diǎn)sgoal附近的節(jié)點(diǎn)明顯具有更低的代價值,在算法對OPEN表排序時這些節(jié)點(diǎn)也將優(yōu)先被擴(kuò)展,而起始點(diǎn)附近的未擴(kuò)展節(jié)點(diǎn)只有在啟發(fā)項(xiàng)權(quán)重較小時才會被考慮擴(kuò)展。因此當(dāng)最優(yōu)航跡與當(dāng)前找到的次優(yōu)航跡在遠(yuǎn)離目標(biāo)點(diǎn)的位置發(fā)生分叉,則啟發(fā)項(xiàng)權(quán)重需要縮減至一個較小的值時算法才能找到當(dāng)前存在的最優(yōu)航跡,這一缺陷降低了AR-SAS算法收斂至最優(yōu)航跡的速度。搜索過程中,前半部分的路徑選擇在很大程度上影響最終的結(jié)果,并且較低的啟發(fā)項(xiàng)權(quán)重意味著大量的節(jié)點(diǎn)擴(kuò)展,導(dǎo)致搜索效率的下降。

為了解決這一問題,加快算法尋找最優(yōu)解的速度,可以在搜索的不同階段采用不同的啟發(fā)項(xiàng)權(quán)重。因此,本文提出一種雙排序準(zhǔn)則,在節(jié)點(diǎn)擴(kuò)展過程中保存兩個代價信息,記為key(s,1)和key(s,2),其表達(dá)式為

(10)

式中,ε1、ε2為啟發(fā)項(xiàng)權(quán)重系數(shù),并且ε1>ε2。

在航跡搜索的前半部分,即當(dāng)前節(jié)點(diǎn)滿足g(s)

(2) 存儲空間約束

為了搜索到當(dāng)前存在的最優(yōu)解,啟發(fā)項(xiàng)權(quán)重需要下降到一個較小的值,這會增加OPEN表內(nèi)的節(jié)點(diǎn)數(shù)量,導(dǎo)致OPEN表中節(jié)點(diǎn)代價更新消耗大量的計(jì)算時間。實(shí)際上,許多待擴(kuò)展節(jié)點(diǎn)無法導(dǎo)向一個更優(yōu)的航跡結(jié)果,因此在搜索過程中可以刪除OPEN表中部分無用節(jié)點(diǎn)以提高航跡規(guī)劃的效率。

同時,對于節(jié)點(diǎn)代價f(s)=g(s)+ε×h(s),若其啟發(fā)項(xiàng)權(quán)重系數(shù)越小,則該代價越接近經(jīng)過該節(jié)點(diǎn)的最優(yōu)路徑的實(shí)際代價。因此為了避免錯誤刪除可能位于最優(yōu)路徑上的節(jié)點(diǎn),根據(jù)起終點(diǎn)歐式距離和擴(kuò)展步長比例設(shè)置OPEN表節(jié)點(diǎn)存儲界限。當(dāng)OPEN表中的節(jié)點(diǎn)數(shù)超出存儲界限,則刪除key(s,2)較大的節(jié)點(diǎn),以保證OPEN表中的節(jié)點(diǎn)數(shù)目在約束范圍之內(nèi)。例如將OPEN表節(jié)點(diǎn)數(shù)量限制為n,一旦航跡迭代搜索過程中節(jié)點(diǎn)擴(kuò)展數(shù)量超過存儲限制n,則按照key(s,2)對節(jié)點(diǎn)代價進(jìn)行排序,并移除其中代價值較大的節(jié)點(diǎn),以滿足存儲空間約束,提升搜索效率。

(3) 變步長策略

為保證飛行安全,標(biāo)準(zhǔn)SAS的擴(kuò)展步長必須大于最小航長約束lmin,具體數(shù)值根據(jù)實(shí)際需求設(shè)定。擴(kuò)展步長取值過大會導(dǎo)致航跡質(zhì)量較差,取值過小則影響求解效率,且容易陷入局部搜索。為了加快初始航跡規(guī)劃速度并保證算法退出時航跡的最優(yōu)性,采用變步長的策略進(jìn)行迭代規(guī)劃,即初始搜索時使用較大的擴(kuò)展步長,在獲得初始航跡后,逐次減小擴(kuò)展步長進(jìn)行迭代搜索以獲得最優(yōu)性更好的航跡。另外,SAS算法對OPEN表節(jié)點(diǎn)的代價計(jì)算與排序會消耗大量的計(jì)算時間,使用較大的擴(kuò)展步長能有效減少OPEN表中的節(jié)點(diǎn)數(shù)目,提高初始規(guī)劃效率,并且避免算法在地形或威脅復(fù)雜區(qū)域的擴(kuò)展陷入局部搜索。例如在特定環(huán)境下,綜合考慮威脅信息和無人機(jī)飛行性能,確定最大擴(kuò)展步長lmax和最小擴(kuò)展步長lmin,為提升可行航跡規(guī)劃速度,使用lmax作為初始擴(kuò)展步長,并隨著航跡迭代過程的進(jìn)行,線性減小擴(kuò)展步長,以提升航跡規(guī)劃質(zhì)量,當(dāng)步長減小到lmin時則不再減小。

3 仿真對比分析

本節(jié)首先在三維靜態(tài)環(huán)境下分別使用分層SAS[12-13]、標(biāo)準(zhǔn)SAS[26]和AR-SAS 3種算法進(jìn)行蒙特卡羅仿真試驗(yàn),從多個方面對比各算法的計(jì)算時效性和最優(yōu)性。然后在三維動態(tài)環(huán)境下,使用AR-SAS算法進(jìn)行動態(tài)航跡規(guī)劃仿真試驗(yàn),以驗(yàn)證算法在動態(tài)環(huán)境和有限規(guī)劃時間條件下的航跡規(guī)劃能力。

3.1 三維靜態(tài)仿真試驗(yàn)

三維靜態(tài)環(huán)境下將敵方威脅建模為半徑4～5 km不等的圓柱,任務(wù)規(guī)劃區(qū)域?yàn)?0 km×60 km的方形區(qū)域,地形數(shù)據(jù)為某山脈高程地形,并假設(shè)飛行器最小轉(zhuǎn)彎半徑為1.5 km,最大飛行高度為1 000 m,最小離地高度為50 m。

算法設(shè)置方面,初始啟發(fā)項(xiàng)權(quán)重ε1=3、ε2=1.2,迭代縮減值Δε=0.1,初始擴(kuò)展步長取為最大威脅半徑,即l0=5 000 m,迭代縮減步長Δl=500 m,最小擴(kuò)展步長為無人機(jī)最小轉(zhuǎn)彎半徑,即lmin=1 500 m,OPEN表節(jié)點(diǎn)存儲容量300,規(guī)劃退出時間15 s。

為了對比AR-SAS算法同分層SAS和標(biāo)準(zhǔn)SAS兩種算法的規(guī)劃性能,在地圖中的左上角和右下角10 km2范圍內(nèi)隨機(jī)設(shè)置100組規(guī)劃起始點(diǎn)和目標(biāo)點(diǎn),分別使用AR-SAS算法、分層SAS算法和標(biāo)準(zhǔn)SAS算法進(jìn)行蒙特卡羅仿真試驗(yàn),并統(tǒng)計(jì)各算法的可行航跡、最優(yōu)航跡規(guī)劃時間及航跡代價。由于規(guī)劃起點(diǎn)與終點(diǎn)隨機(jī)選擇,不同試驗(yàn)中航跡絕對代價值變化非常大,為了便于比較,本文將規(guī)劃航跡長度除以起點(diǎn)到目標(biāo)的歐式距離定義為航跡相對代價,并將其作為各算法最優(yōu)性的評價指標(biāo)。

靜態(tài)環(huán)境下,3種規(guī)劃算法的仿真結(jié)果對比如圖1所示。

圖1 三維靜態(tài)環(huán)境各算法航跡規(guī)劃結(jié)果對比Fig.1 Path planning results of different planning algorithms in 3D static environments

圖1(a)所示為AR-SAS算法生成的航跡結(jié)果。由圖1(b)可知,AR-SAS算法在可行航跡規(guī)劃的時效性上具有明顯的優(yōu)勢,在1 s的時間內(nèi)即可規(guī)劃出航跡;分層SAS算法的航跡平均求解時間為2.57 s;標(biāo)準(zhǔn)SAS算法的航跡平均求解時間為12.11 s,并且在100次仿真試驗(yàn)中有5次未能在15 s內(nèi)規(guī)劃出可行航跡。圖1(c)所示為最優(yōu)航跡相對代價對比,AR-SAS算法規(guī)劃出的航跡代價與分層SAS算法接近,并優(yōu)于標(biāo)準(zhǔn)SAS算法。圖1(d)為某一次仿真試驗(yàn)中航跡代價隨規(guī)劃時間的變化曲線(在算法未規(guī)劃出可行航跡之前代價為無窮大),可以看出分層SAS和標(biāo)準(zhǔn)SAS算法均是通過一次規(guī)劃得到可行較優(yōu)的航跡,并且標(biāo)準(zhǔn)SAS算法耗時相對較長;而AR-SAS算法在很短時間內(nèi)即可規(guī)劃出可行航跡,并能夠繼續(xù)進(jìn)行優(yōu)化,在達(dá)到規(guī)劃時限時得到了與標(biāo)準(zhǔn)SAS和分層SAS算法最優(yōu)性相當(dāng)?shù)目尚泻桔E。

表3給出了100次試驗(yàn)中3種算法得到初始可行航跡和最優(yōu)航跡時OPEN表和CLOSED表中節(jié)點(diǎn)數(shù)量的平均值。由于分層SAS算法和標(biāo)準(zhǔn)SAS算法只進(jìn)行單次航跡規(guī)劃,因此它們找到可行航跡和最優(yōu)航跡時,節(jié)點(diǎn)鏈表中的節(jié)點(diǎn)數(shù)不變。

表3 不同算法的節(jié)點(diǎn)擴(kuò)展數(shù)量對比Table 3 Comparison of expanding numbers of different algorithms

由表3可知,在完成可行航跡求解時,AR-SAS算法的OPEN表和CLOSED表的節(jié)點(diǎn)數(shù)量明顯小于其他兩種算法,大大縮減了規(guī)劃可行航跡時對OPEN表節(jié)點(diǎn)的排序和代價計(jì)算時間,提高了可行航跡的求解速度。

3.2 三維動態(tài)仿真試驗(yàn)

為了驗(yàn)證AR-SAS算法在動態(tài)環(huán)境下即時航跡規(guī)劃的性能,在三維戰(zhàn)場環(huán)境中設(shè)置移動威脅和運(yùn)動目標(biāo),并假設(shè)移動威脅與運(yùn)動目標(biāo)均以15 m/s的速度作勻速運(yùn)動,其中目標(biāo)運(yùn)動方向隨機(jī),移動威脅沿與x軸成夾角45°方向作直線折返移動。

戰(zhàn)場環(huán)境示意如圖2所示,圖2中藍(lán)色五角星為起點(diǎn)坐標(biāo)(3 000 m,51 000 m),紅色星形為運(yùn)動目標(biāo)初始坐標(biāo)(58 000 m,8 000 m)。

圖2 動態(tài)環(huán)境示意圖Fig.2 Diagram of dynamic environment

環(huán)境中的威脅信息如表4所示。

表4 威脅信息表Table 4 Information of threats

考慮到威脅和目標(biāo)的連續(xù)機(jī)動過程,采用滾動規(guī)劃策略,每間隔10 s調(diào)用航跡規(guī)劃算法完成當(dāng)前點(diǎn)到運(yùn)動目標(biāo)點(diǎn)的動態(tài)航跡重規(guī)劃。由于動態(tài)規(guī)劃的實(shí)時性要求,算法規(guī)劃時間限定為0.2 s,其余設(shè)置與第3.1節(jié)相同。

假設(shè)每次動態(tài)規(guī)劃前,無人機(jī)能夠通過偵察獲取移動威脅和運(yùn)動目標(biāo)的當(dāng)前位置和速度。動態(tài)規(guī)劃過程中考慮當(dāng)前威脅的移動速度和方向,并且在一個規(guī)劃周期內(nèi),移動威脅的速度方向和大小保持不變。因此動態(tài)規(guī)劃輸入的威脅位置可由式得到

(xinput,yinput)Threat=(xi,yi)Threat+VThreat×T

(11)

式中,(xi,yi)Threat表示第i個威脅的當(dāng)前位置;VThreat表示威脅移動速度;T為動態(tài)規(guī)劃周期。

仿真結(jié)果如圖3、圖4及表5所示。其中圖3和圖4分別為AR-SAS算法動態(tài)航跡規(guī)劃過程的三維和二維示意圖。白色三角形表示無人機(jī)當(dāng)前位置,紅色五角星表示運(yùn)動目標(biāo)當(dāng)前位置,紅色米形表示運(yùn)動目標(biāo)初始位置,紅色圓柱表示移動威脅當(dāng)前位置,藍(lán)色圓柱表示移動威脅初始位置。

圖3 AR-SAS動態(tài)航跡規(guī)劃過程(3維)Fig.3 Process of AR-SAS dynamic path planning (3D)

圖4 AR-SAS動態(tài)航跡規(guī)劃過程(2維)Fig.4 Process of AR-SAS dynamic path planning (2D)

表5 動態(tài)環(huán)境下航跡規(guī)劃仿真結(jié)果Table 5 Simulation results of dynamic path planning

由表5可知,使用AR-SAS算法進(jìn)行動態(tài)航跡規(guī)劃時,整個仿真飛行過程持續(xù)701 s,當(dāng)無人機(jī)到達(dá)目標(biāo)點(diǎn)5 km范圍(5 km為無人機(jī)偵察范圍,當(dāng)目標(biāo)進(jìn)入無人機(jī)偵察范圍內(nèi)則轉(zhuǎn)入自動導(dǎo)引階段)內(nèi)不再進(jìn)行規(guī)劃。仿真過程中共調(diào)用65次AR-SAS算法進(jìn)行動態(tài)航跡規(guī)劃,并全部在規(guī)定時限內(nèi)找到可行較優(yōu)航跡,無人機(jī)飛抵運(yùn)動目標(biāo)的過程中成功規(guī)避所有威脅(如圖3～圖4所示)。上述數(shù)值仿真試驗(yàn)結(jié)果表明本文提出的AR-SAS算法能夠滿足動態(tài)環(huán)境下的無人機(jī)航跡規(guī)劃需求,具有較高的規(guī)劃效率和較強(qiáng)的魯棒性。

根據(jù)靜態(tài)環(huán)境和動態(tài)環(huán)境下的仿真結(jié)果與分析可知,相較于傳統(tǒng)的SAS算法及分層SAS算法,本文提出的AR-SAS算法可以在很短的時間內(nèi)規(guī)劃出可行航跡并充分利用規(guī)劃時間持續(xù)優(yōu)化航跡,既能夠在靜態(tài)環(huán)境中以較高的效率搜索得到較優(yōu)的可行航跡,也能適應(yīng)動態(tài)環(huán)境下規(guī)劃時效性與最優(yōu)性的嚴(yán)苛要求,具有較強(qiáng)的魯棒性。

4 結(jié) 論

提出了即時修復(fù)式構(gòu)架與SAS算法相結(jié)合的AR-SAS算法,該算法能夠快速規(guī)劃出可行航跡,并隨規(guī)劃時間的增加不斷提高航跡質(zhì)量。考慮航跡規(guī)劃擴(kuò)展中的運(yùn)動學(xué)約束并針對即時修復(fù)式構(gòu)架中的缺陷,引入雙排序準(zhǔn)則、存儲空間約束及變步長策略,在保證航跡最優(yōu)性的同時,顯著提升了規(guī)劃效率和魯棒性。

仿真測試結(jié)果表明,AR-SAS算法的規(guī)劃效率高于分層SAS和標(biāo)準(zhǔn)SAS算法,能夠即時處理動態(tài)環(huán)境下的航跡規(guī)劃問題,具有較強(qiáng)的工程實(shí)用性。