肖自兵, 袁冬莉, 屈耀紅
(西北工業(yè)大學(xué) 自動化學(xué)院, 陜西 西安 710129)
基于A*定長搜索算法的多無人機(jī)協(xié)同航跡規(guī)劃
肖自兵, 袁冬莉, 屈耀紅
(西北工業(yè)大學(xué) 自動化學(xué)院, 陜西 西安 710129)
基于多無人機(jī)同時作業(yè)情況下的航跡規(guī)劃問題,提出了一種A*定長航跡搜索算法。該算法通過選擇代價值最接近給定值的節(jié)點(diǎn)作為最佳節(jié)點(diǎn),得到定長規(guī)劃航跡,接著進(jìn)一步通過限定最佳節(jié)點(diǎn)的選擇范圍,改善了航跡的可飛性。仿真結(jié)果表明,利用該算法規(guī)劃的定長航跡長度誤差可以控制在1.4%以內(nèi),協(xié)同航跡長度誤差可以控制在0.8%以內(nèi),能夠滿足多無人機(jī)同時到達(dá)的一般要求。
無人機(jī); A*算法; 協(xié)同航跡規(guī)劃; 航跡代價
多無人機(jī)協(xié)同作業(yè)在軍事領(lǐng)域有著重要的意義,近年來成為國內(nèi)外研究的熱點(diǎn)。多無人機(jī)的協(xié)同航跡規(guī)劃是一個復(fù)雜的問題,包含了無人機(jī)物理會合約束、作業(yè)環(huán)境約束和其他操作要求約束[1]。
選擇算法是協(xié)同航跡規(guī)劃的重要一環(huán),適宜的航跡搜索算法在沒有人工干預(yù)的條件下,能夠使無人機(jī)有效地規(guī)避障礙[2]。常用的航跡搜索算法有Dijkstra算法、Bellman Ford算法、Floyd-Warshall算法和A*算法等[3]。A*算法通過構(gòu)造啟發(fā)函數(shù)提高了搜索效率,成為靜態(tài)路網(wǎng)中求解最短路徑(或航跡)的有效方法[4]。然而A*算法多應(yīng)用于單機(jī)航跡規(guī)劃,不能用于多機(jī)協(xié)同航跡規(guī)劃。文獻(xiàn)[5-7]對A*算法進(jìn)行了改進(jìn),這些改進(jìn)措施雖然優(yōu)化了規(guī)劃結(jié)果或提高了搜索效率,卻仍然局限于單機(jī)航跡規(guī)劃應(yīng)用,不能實(shí)現(xiàn)多機(jī)協(xié)同航跡規(guī)劃。
本文提出了一種A*定長航跡搜索算法。用該算法可以實(shí)現(xiàn)定長航跡規(guī)劃,進(jìn)而實(shí)現(xiàn)多無人機(jī)的協(xié)同航跡規(guī)劃。
1.1 問題描述
假設(shè)N架無人機(jī)在時刻t0分別處于不同位置S1,S2,…,SN,要求在同一時刻t1到達(dá)N個不同目標(biāo)點(diǎn)D1,D2,…,DN,實(shí)施打擊且代價最小。飛經(jīng)區(qū)域存在雷達(dá)和導(dǎo)彈陣地威脅,設(shè)各無人機(jī)勻速飛行且航速相同。
1.2 協(xié)同航跡規(guī)劃原理
多無人機(jī)協(xié)同航跡規(guī)劃總體結(jié)構(gòu)[8]如圖1所示。
圖1 多無人機(jī)協(xié)同航跡規(guī)劃總體結(jié)構(gòu)圖
整個工作過程大致是:協(xié)同管理層根據(jù)戰(zhàn)場環(huán)境、無人機(jī)速度的變化范圍以及路徑規(guī)劃層傳來的路徑長度,確定出編隊(duì)的協(xié)同時間t,并把t和各無人機(jī)的相應(yīng)路徑編號送到路徑規(guī)劃器;路徑規(guī)劃器把協(xié)同管理層確定的t和相應(yīng)的路徑航路點(diǎn)序列送到軌跡控制層;軌跡控制層根據(jù)航路點(diǎn)序列確定可飛軌跡以及相應(yīng)的控制向量,并把求得的可飛航跡的高度、速度和航向送到無人機(jī)自動駕駛伺服系統(tǒng)去執(zhí)行,操縱無人機(jī)按規(guī)劃出來的航跡飛行。
一類協(xié)同航跡規(guī)劃問題為多機(jī)同時到達(dá)問題。為了使各無人機(jī)能夠同時到達(dá)目標(biāo),通常采用兩種方式:一種是通過調(diào)節(jié)無人機(jī)的飛行速度使航程較大的無人機(jī)采取較大的速度,而航程較小的無人機(jī)采取較小的速度;另一種是對航路進(jìn)行修正,通過增加較短航路的長度使每架無人機(jī)的航程大致相等。
1.3 航跡代價
當(dāng)飛行區(qū)域存在地面雷達(dá)或地空導(dǎo)彈威脅時,一種代價計(jì)算方法[9]為:
J=kJt+(1-k)Jf
(1)
式中,J為航跡總代價;k為威脅權(quán)重系數(shù),由規(guī)劃者根據(jù)實(shí)際情況給出;Jt為雷達(dá)或?qū)椡{代價;Jf為燃油代價。
為使問題簡化,本文只考慮航程代價(與燃油代價成正比),而使無人機(jī)完全繞開地面雷達(dá)和地空導(dǎo)彈的威脅,即威脅代價為0(對應(yīng)上式中k=0)。
A*算法是一種經(jīng)典的啟發(fā)式搜索算法,其代價函數(shù)為:
f(n)=g(n)+h(n)
(2)
式中,f(n)為節(jié)點(diǎn)n從初始點(diǎn)到目標(biāo)點(diǎn)的估計(jì)代價,稱為代價函數(shù);g(n)為在狀態(tài)空間中從初始節(jié)點(diǎn)到n節(jié)點(diǎn)的實(shí)際代價(即已走路徑代價);h(n)為從n到目標(biāo)節(jié)點(diǎn)最佳路徑的估計(jì)代價,稱為啟發(fā)函數(shù)。
3.1 A*定長搜索算法與定長航跡規(guī)劃
A*算法的搜索策略是:選取Open表中f值(代價)最小的節(jié)點(diǎn)作為最佳節(jié)點(diǎn)加以擴(kuò)展,得到代價最小的航跡。如果重新定義最佳節(jié)點(diǎn),應(yīng)能規(guī)劃出不同的航跡。
修改A*算法如下:選取Open表中f值(航程)最接近于給定長度f0的節(jié)點(diǎn)作為最佳節(jié)點(diǎn)加以擴(kuò)展。如此當(dāng)能規(guī)劃出長度為f0的定長航跡,從而為多無人機(jī)協(xié)同航跡規(guī)劃奠定了基礎(chǔ)。改進(jìn)后的A*算法由于采用了定長搜索策略,稱其為A*定長搜索算法,將分別在下面介紹的兩種情形下應(yīng)用。
情形1:設(shè)無人機(jī)飛經(jīng)區(qū)域100 km×100 km范圍內(nèi)共40個威脅點(diǎn)(雷達(dá)陣地和導(dǎo)彈陣地),且設(shè)任意兩個威脅點(diǎn)的間距大于無人機(jī)安全距離的兩倍,無人機(jī)沿這些威脅點(diǎn)生成的Voronoi圖邊線飛行。
情形2:在400 km×400 km的區(qū)域內(nèi)分布著6個威脅區(qū)域(雷達(dá)陣地和導(dǎo)彈陣地),地圖比例尺取為1∶10。地圖柵格劃分及節(jié)點(diǎn)擴(kuò)展方式如下:用間距為10 km的水平線和豎直線將該大小為400 km×400 km的正方形柵格化,使得每個網(wǎng)格均為10 km×10 km的正方形,所有網(wǎng)格的頂點(diǎn)均作為可擴(kuò)展的節(jié)點(diǎn)。除邊界上的點(diǎn)外,每個節(jié)點(diǎn)都按8個方向進(jìn)行擴(kuò)展以到達(dá)相鄰的8個節(jié)點(diǎn),節(jié)點(diǎn)擴(kuò)展方式如圖2所示。
圖2 節(jié)點(diǎn)擴(kuò)展示意圖
設(shè)情形1下無人機(jī)的起始位置S的坐標(biāo)為(10,10) km,起始時刻t0為8∶00,所要到達(dá)的目標(biāo)點(diǎn)D的坐標(biāo)為(40,85) km,要求的到達(dá)時刻t1為8∶15,且設(shè)無人機(jī)的航速為v=200 m/s。
設(shè)情形2下無人機(jī)的起始位置S的坐標(biāo)為(10, 30) km,起始時刻t0為8∶00,所要到達(dá)的目標(biāo)點(diǎn)D的坐標(biāo)為(360,300) km,要求的到達(dá)時刻t1為8∶50,且設(shè)無人機(jī)的航速為v=200 m/s。
情形1中因?yàn)椴捎玫氖荲oronoi圖,最佳節(jié)點(diǎn)只能是Voronoi節(jié)點(diǎn),而無人機(jī)的起始點(diǎn)和目標(biāo)點(diǎn)不一定與Voronoi節(jié)點(diǎn)重合。為此,直線連接起點(diǎn)S(xS,yS)和最近的Voronoi節(jié)點(diǎn)A(xA,yA)并計(jì)算長度:
(3)
直線連接終點(diǎn)D(xD,yD)和最近的Voronoi節(jié)點(diǎn)B(xB,yB)并計(jì)算長度:
(4)
則中間應(yīng)規(guī)劃航跡長度為dc=f0-c1-c2,接下來只要進(jìn)行起始點(diǎn)為A、目標(biāo)點(diǎn)為B、給定長為dc的定長航跡規(guī)劃就可以了。
情形2中為使航跡在長度上更精確,采用圖2所示的節(jié)點(diǎn)擴(kuò)展方式,這就決定了航跡上可能出現(xiàn)的角度分別為45°,90°,135°,180°,而45°和90°的角將影響航跡的可飛性。以往處理的辦法是對所生成的航跡進(jìn)行平滑處理,這就將航跡規(guī)劃工作人為地分為了兩個部分,且平滑工作較為繁雜。這里通過對算法進(jìn)行進(jìn)一步的改進(jìn)來改善航跡的可飛性。
航跡上出現(xiàn)直角甚至銳角的原因是最佳節(jié)點(diǎn)的選擇范圍過大,為此,對算法進(jìn)行進(jìn)一步改進(jìn):從最新添加進(jìn)Open表的節(jié)點(diǎn)(也即上一循環(huán)中既不屬于Open表也不屬于Closed表的擴(kuò)展節(jié)點(diǎn))中選擇最佳節(jié)點(diǎn),這從根本上杜絕了銳角的產(chǎn)生,且能降低直角發(fā)生率,具體分析如下。
圖3為航跡角度控制示意圖。圖中,符號“○”表示節(jié)點(diǎn)。
圖3 航跡角度控制示意圖
搜索過程如下:
(1)選擇A點(diǎn)作為最佳節(jié)點(diǎn),擴(kuò)展A點(diǎn),產(chǎn)生后繼節(jié)點(diǎn)D,B,E,將D,B,E點(diǎn)加入Open表。
(2)從最新添加進(jìn)Open表的節(jié)點(diǎn)D,B,E中選擇最佳節(jié)點(diǎn)。假設(shè)為B,擴(kuò)展B點(diǎn),產(chǎn)生后繼節(jié)點(diǎn)F,C,G,H,I,E,A,D,將F,C,G,H,I添加進(jìn)Open表(E,A,D已在Open表,不需再添加)。
(3)從最新添加進(jìn)Open表的節(jié)點(diǎn)F,C,G,H,I中選擇最佳節(jié)點(diǎn)。如果選F,則∠ABF=90°;如果選C,則∠ABC=135°;如果選G,則∠ABG=180°;如果選H,則∠ABH=135°;如果選I,則∠ABI=90°。由于避免了選擇E和D作為最佳節(jié)點(diǎn),當(dāng)前航跡不會出現(xiàn)銳角(45°)。假設(shè)最佳節(jié)點(diǎn)選為C,擴(kuò)展C點(diǎn),產(chǎn)生后繼節(jié)點(diǎn)J,K,L,G,H,B,D,F,將J,K,L添加進(jìn)Open表(G,H,B,D,F已在Open表,不需再添加)。
(4)從最新添加進(jìn)Open表的節(jié)點(diǎn)J,K,L中選擇最佳節(jié)點(diǎn)。如果選J,則∠BCJ=135°;如果選K,則∠BCK=180°;如果選L,則∠BCL=135°。由于避免了選擇H和D作為最佳節(jié)點(diǎn),當(dāng)前航跡不會出現(xiàn)銳角(45°),又由于避免了選擇G和F作為最佳節(jié)點(diǎn),當(dāng)前航跡不會出現(xiàn)直角。
上述分析表明:針對算法的改進(jìn)措施可以有效地改善航跡的可飛性。但這樣會導(dǎo)致一個問題:由于最佳節(jié)點(diǎn)的選取范圍縮小了,導(dǎo)致不一定能搜索到目標(biāo)節(jié)點(diǎn),可能會與之“擦肩而過”,以致造成程序“死循環(huán)”。為避免這種情況的發(fā)生,需要修改程序終止的條件,將“搜索到的最佳節(jié)點(diǎn)為目標(biāo)節(jié)點(diǎn)”這一條件改為“搜索到的最佳節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)較為接近”。
情形1中不能通過這樣進(jìn)一步的算法改進(jìn)來改善航跡可飛性,其原因是Voronoi圖中每個節(jié)點(diǎn)只有3個擴(kuò)展方向,下一個最佳節(jié)點(diǎn)只能從這3個點(diǎn)中選取,如果再縮小選擇范圍,就可能導(dǎo)致找不到最佳節(jié)點(diǎn)而使搜索無法繼續(xù)。
3.2 多機(jī)協(xié)同航跡規(guī)劃
在用改進(jìn)的A*算法實(shí)現(xiàn)定長航跡規(guī)劃后,可進(jìn)一步實(shí)現(xiàn)多無人機(jī)協(xié)同航跡規(guī)劃。以3架無人機(jī)為例,其策略為:根據(jù)起始點(diǎn)、目標(biāo)點(diǎn)以及途中要規(guī)避的威脅,3架無人機(jī)先各自進(jìn)行最短航跡規(guī)劃,然后從中選取長度最大的一條作為參照,以其長度作為給定值,對其他兩架無人機(jī)進(jìn)行定長航跡規(guī)劃。這樣就實(shí)現(xiàn)了3機(jī)同時出發(fā)、同時到達(dá)且用時最短的協(xié)同航跡規(guī)劃。
設(shè)在上述情形1下,第1架無人機(jī)的起始點(diǎn)S1坐標(biāo)為(10, 10) km,目標(biāo)點(diǎn)D1坐標(biāo)為(70, 70) km;第2架無人機(jī)的起始點(diǎn)S2坐標(biāo)為(5, 40) km,目標(biāo)點(diǎn)D2坐標(biāo)為(70, 95) km;第3架無人機(jī)的起始點(diǎn)S3坐標(biāo)為(30, 10) km,目標(biāo)點(diǎn)D3坐標(biāo)為(90, 70) km。
設(shè)在上述情形2下,第1架無人機(jī)的起始點(diǎn)S1坐標(biāo)為(10, 10) km,目標(biāo)點(diǎn)D1坐標(biāo)為(270, 270) km;第2架無人機(jī)的起始點(diǎn)S2坐標(biāo)為(10, 110) km,目標(biāo)點(diǎn)D2坐標(biāo)為(290, 360) km;第3架無人機(jī)的起始點(diǎn)S3坐標(biāo)為(70, 10) km,目標(biāo)點(diǎn)D3坐標(biāo)為(340, 280) km。
在MATLAB環(huán)境下,進(jìn)行上述單機(jī)定長航跡規(guī)劃和3機(jī)協(xié)同航跡規(guī)劃數(shù)字仿真。
啟發(fā)函數(shù)取為當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)直線距離的1.4倍。1.4為啟發(fā)系數(shù),決定了啟發(fā)函數(shù)h(n)的大小。
計(jì)算可得情形1下無人機(jī)應(yīng)飛航跡的給定長度為f0=v(t1-t0)=180 km,情形2下無人機(jī)應(yīng)飛航跡的給定長度為f0=v(t1-t0)=600 km。
圖4為第一次改進(jìn)的A*算法在情形1下的定長航跡規(guī)劃結(jié)果,航跡長度為179.309 6 km,與理論應(yīng)飛航程180 km非常接近,相對誤差為0.383 6%。
圖4 情形1下定長規(guī)劃結(jié)果
圖5為第一次改進(jìn)的A*算法在情形2下的定長航跡規(guī)劃結(jié)果,航跡長度為592.132 km,與理論應(yīng)飛航程600 km較為接近,相對誤差為1.311 3%。但同時看到圖中航跡出現(xiàn)了較多的直角,有些地方甚至出現(xiàn)了銳角,這使得航跡的可飛性受到很大影響。
圖5 情形2下定長規(guī)劃結(jié)果
圖6為進(jìn)一步改進(jìn)的A*算法在情形2下的定長航跡規(guī)劃結(jié)果,航跡長度為599.411 km,與理論應(yīng)飛航程600 km非常接近,相對誤差為0.098 2%。相對于圖5而言,盡管圖6中航跡仍然包含少許直角,其可飛性還是得到了很大改善,基本滿足飛行要求。
圖6 改善可飛性的定長航跡
圖7為在情形1下3機(jī)協(xié)同航跡規(guī)劃的仿真結(jié)果,3條航跡(從上到下)的長度分別為97.506 4,98.235 9,97.591 3 km,較為接近(相對于中間的參考航跡,上面航跡的誤差為0.742 6%,下面航跡的誤差為0.656 2%),滿足3機(jī)同時出發(fā)、同時到達(dá)的協(xié)同航跡規(guī)劃要求。
圖7 情形1下的協(xié)同航跡
圖8為在情形2下3機(jī)協(xié)同航跡規(guī)劃的結(jié)果,3條航跡(從上到下)的長度分別為403.553,405.269,405.269 km,較為接近(相對于下面的參考航跡,上面航跡的誤差為0.423 4%,中間航跡的誤差為0),滿足3機(jī)同時出發(fā)、同時到達(dá)的協(xié)同航跡規(guī)劃要求。
圖8 情形2下的協(xié)同航跡
本文通過對A*算法進(jìn)行改進(jìn),提出了一種A*定長航跡搜索算法,用該算法規(guī)劃出了定長航跡,并以此為基礎(chǔ)實(shí)現(xiàn)了多機(jī)協(xié)同航跡規(guī)劃,仿真結(jié)果證明了算法的有效性。本文研究的多機(jī)協(xié)同航跡規(guī)劃問題為靜態(tài)路網(wǎng)中的各無人機(jī)同時到達(dá)問題,對于其他協(xié)同航跡規(guī)劃問題,如動態(tài)規(guī)劃情形,還有待今后進(jìn)一步探討。所做工作對今后無人機(jī)航跡規(guī)劃有一定的借鑒意義。
[1] Shanmugavel M,Tsourdos A,White B,et al.Co-operative path planning of multiple UAVs using Dubins paths with clothoid arcs[J].Control Engineering Practice,2010,18(9):1084-1092.
[2] Oliver M,Natalie F,Christian A,et al.Adaptive path planning for VTOL-UAVs[J].IEEE Aerospace and Electronic Systems Magazine,2009,24(7):36-41.
[3] Sathyaraj B M,Jain L C,Finn A,et al.Multiple UAVs path planning algorithms:a comparative study[J].Fuzzy Optimization and Decision Making,2008,7(3):257-267.
[4] Seo W J,Ok S H,Ahn J H,et al.An efficient hardware architecture of the A-star algorithm for the shortest path search engine[C]//2009 Fifth International Joint Conference on INC,IMS and IDC.Piscataway:IEEE Computer Society,2009:1499-1502.
[5] Li Ji,Sun Xiuxia.Route planning’s method for unmanned aerial vehicles based on improved A-star algorithm [J].Binggong Xuebao,2008,29(7):788-792.
[6] 孟中杰,黃攀峰,閆杰.基于改進(jìn)稀疏A*算法的高超聲速飛行器航跡規(guī)劃技術(shù)[J].西北工業(yè)大學(xué)學(xué)報(bào),2010,28(2):182-186.
[7] Dai Zhiquan,Guan Yong,Guan Ran.Dynamic adjustment A*rounting algorithm[C]//CICC-ITOE 2010 International Conference on Innovative Computing and Communication and 2010 Asia-Pacific Conference on Information Technology and Ocean Engineering. Piscataway:IEEE Computer Society,2010:316-318.
[8] 高曉光,符小衛(wèi),宋紹梅. 多UCAV航跡規(guī)劃研究[J].系統(tǒng)工程理論與實(shí)踐,2004,24(5):140-143.
[9] Meng Bobo,Gao Xiaoguang,Wang Yunhui.Multi-mission path re-planning for multiple unmanned aerial vehicles based on unexpected events[C]//2009 International Conference on Intelligent Human-Machine Systems and Cybernetics.Piscataway:IEEE Computer Society,2009:423-426.
Multi-UAVscooperativepathplanningbasedonA*fixedlengthsearchalgorithm
XIAO Zi-bing, YUAN Dong-li, QU Yao-hong
(College of Automation, NWPU, Xi’an 710129, China)
An improved A*algorithm for fixed length path searching was proposed based on the path planning problems of multiple unmanned aerial vehicles(UAVs) operating simultaneously. A path with fixed length was obtained by choosing nodes with costs closest to given value as best nodes in the algorithm. Then, the path was smoothed by limiting the range of the best nodes choosing from in the algorithm. Simulation results show that length error of the fixed length path obtained from the algorithm can be controlled within 1.4%, and length error of collaborative paths is less than 0.8%. It basically meets the requirements of multi-UAVs arriving at the same time.
unmanned aerial vehicle(UAV); A*algorithm; cooperative path planning; path costs
2011-04-18;
2011-09-04
國家自然科學(xué)基金資助(60974146)
肖自兵(1984-),男,四川簡陽人,碩士,主要研究方向?yàn)楝F(xiàn)代控制理論及應(yīng)用;
袁冬莉(1966-),女,陜西乾縣人,副教授,博士,研究方向?yàn)橄冗M(jìn)控制理論及應(yīng)用、導(dǎo)航制導(dǎo)與控制、計(jì)算機(jī)控制與智能控制、飛行控制與仿真等。
V279; V249.122
A
1002-0853(2012)01-0092-05
(編輯:姚妙慧)