基于CPSO的UAV編隊(duì)集結(jié)路徑規(guī)劃

邵壯, 周洲, 王彥雄, 祝小平

(1.西北工業(yè)大學(xué) 無人機(jī)特種技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室, 陜西 西安 710065;2.西北工業(yè)大學(xué) 航空學(xué)院,陜西 西安 710072;3.西北工業(yè)大學(xué) 無人機(jī)研究所,陜西 西安 710065)

基于CPSO的UAV編隊(duì)集結(jié)路徑規(guī)劃

邵壯1, 周洲2, 王彥雄1, 祝小平3

(1.西北工業(yè)大學(xué) 無人機(jī)特種技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室, 陜西 西安 710065;2.西北工業(yè)大學(xué) 航空學(xué)院,陜西 西安 710072;3.西北工業(yè)大學(xué) 無人機(jī)研究所,陜西 西安 710065)

針對(duì)多無人機(jī)編隊(duì)集結(jié)路徑規(guī)劃問題,提出了具有合作機(jī)制的分布式協(xié)同粒子群(CPSO)算法。為了滿足無人機(jī)運(yùn)動(dòng)學(xué)約束,采用曲率連續(xù)的PH曲線作為備選路徑。基于協(xié)同進(jìn)化思想提出CPSO算法，為每架無人機(jī)規(guī)劃出一條滿足機(jī)間協(xié)同約束的最優(yōu)安全可飛行路徑。仿真結(jié)果表明,規(guī)劃得到的多條路徑能夠滿足無人機(jī)運(yùn)動(dòng)學(xué)約束、安全性及無人機(jī)之間的協(xié)同性要求;相比于協(xié)同進(jìn)化遺傳算法,CPSO算法搜索成功率更高,穩(wěn)定性更好。

無人機(jī); 路徑規(guī)劃; 編隊(duì)集結(jié); PH曲線; 協(xié)同粒子群優(yōu)化

0 引言

如何為多無人機(jī)(UAV)規(guī)劃出能夠同時(shí)到達(dá)編隊(duì)集結(jié)點(diǎn)的安全、可飛行路徑,已成為多UAV協(xié)同編隊(duì)的關(guān)鍵技術(shù)之一[1]。多UAV編隊(duì)集結(jié)路徑規(guī)劃是一個(gè)典型的多機(jī)協(xié)同同時(shí)到達(dá)問題。Manathara等[2]提出了基于估計(jì)到達(dá)集結(jié)點(diǎn)時(shí)間的一致性算法,采用的Dubins路徑具有曲率不連續(xù)性,不便于跟蹤實(shí)現(xiàn)。李杰等[3]提出了基于微分幾何與李群的無人機(jī)編隊(duì)會(huì)合的方法,直接以路徑的曲率和撓率作為控制變量,但工程實(shí)現(xiàn)困難。文獻(xiàn)[4-5]針對(duì)多架UAV同時(shí)到達(dá)多個(gè)目標(biāo)問題,首先采用協(xié)同進(jìn)化遺傳算法為各UAV分別生成多條候選路徑;然后通過求解協(xié)同函數(shù)和協(xié)同變量,來確定每架UAV同時(shí)到達(dá)各自目標(biāo)點(diǎn)的飛行路徑和飛行速度。其中Voronoi圖法生成的分段直線路徑需要進(jìn)行后期平滑處理。

當(dāng)前UAV路徑規(guī)劃中,采用的路徑主要有Dubins路徑[2]、Clothoid回旋路徑、B樣條曲線路徑[4]和PH曲線路徑[6]。本文為由不同起點(diǎn)出發(fā)的一組UAV分別規(guī)劃出一條能夠同時(shí)抵達(dá)編隊(duì)集結(jié)點(diǎn),并生成期望構(gòu)型的安全、可飛行路徑。由于PH曲線具有曲率連續(xù)、曲線平滑及曲線長(zhǎng)度和曲率均有有理表達(dá)式的特點(diǎn),因此采用PH曲線作為UAV備選路徑。提出具有合作機(jī)制的分布式協(xié)同粒子群(Cooperative Particle Swarm Optimization,CPSO)算法對(duì)PH路徑參數(shù)進(jìn)行協(xié)同優(yōu)化,使得各路徑滿足安全性約束、UAV運(yùn)動(dòng)學(xué)約束、同時(shí)達(dá)到約束及互碰避免約束,且路徑的整體性能指標(biāo)趨于最優(yōu)。

1 問題描述與路徑模型

1.1 問題描述

考慮N架UAV協(xié)同編隊(duì)集結(jié)問題。假設(shè)已知第i架UAV的起點(diǎn)位置和航向(簡(jiǎn)稱位姿)psi=(xsi,ysi,χsi)以及編隊(duì)集結(jié)點(diǎn)處的位姿pf=(xf,yf,χf),且集結(jié)點(diǎn)的期望編隊(duì)構(gòu)型由固聯(lián)于集結(jié)點(diǎn)的編隊(duì)坐標(biāo)系Ofxfyf上的一組相對(duì)位置坐標(biāo){(xfi,yfi)|i=1,…,N}定義,則該UAV的目標(biāo)位姿pdi=(xdi,ydi,χdi)可由pf和期望編隊(duì)構(gòu)型求得。因此N架UAV協(xié)同編隊(duì)集結(jié)路徑規(guī)劃問題描述為:

(1)

為了使各UAV同時(shí)抵達(dá)集結(jié)點(diǎn)時(shí)能夠以相同的速度進(jìn)入編隊(duì)保持階段,這里假設(shè)集結(jié)過程中各UAV均以相同的速度勻速飛行,且集結(jié)環(huán)境中僅存在少量禁飛區(qū)和障礙物;因此UAV間同時(shí)達(dá)到約束,即等價(jià)于各路徑長(zhǎng)度相等或近似相等。

1.2PH曲線路徑模型

PH曲線是由Farouki等[7]提出的一種具有有理特性的參數(shù)化多項(xiàng)式曲線,其定義如下:

(2)

式中:pk為曲線控制頂點(diǎn);n為曲線階數(shù)。5次PH曲線是適合于無人機(jī)路徑規(guī)劃的最低階PH曲線[4],因此本文采用5次PH曲線進(jìn)行路徑規(guī)劃。

(3)

根據(jù)微分幾何知識(shí),曲線的平滑特性由曲率κ(q)描述。對(duì)于UAV,路徑曲率與UAV的側(cè)向加速度成正比,因此UAV的曲線路徑需滿足其自身性能決定的最大曲率約束(即UAV的運(yùn)動(dòng)學(xué)約束)。曲線上任一點(diǎn)處的曲率求解公式為[4]:

(4)

為定量描述PH曲線的平滑程度,定義曲線的彎曲能量E為曲線曲率沿曲線弧長(zhǎng)的積分[4],即:

(5)

2 CPSO協(xié)同路徑規(guī)劃算法

目前,協(xié)同進(jìn)化遺傳算法(Cooperative Co-evolutionary Genetic Algorithms, CCGA)已經(jīng)用于UAV的協(xié)同路徑規(guī)劃研究[8],然而CCGA算法存在搜索效率低、容易早熟收斂的缺點(diǎn);因此,本文基于協(xié)同進(jìn)化思想,提出了CPSO算法,實(shí)現(xiàn)UAV編隊(duì)集結(jié)路徑的協(xié)同優(yōu)化。

2.1 標(biāo)準(zhǔn)粒子群算法(PSO)

設(shè)D維搜索空間中有n個(gè)粒子,第i個(gè)粒子的位置和飛行速度分別用向量Xi=(xi1,…,xiD)和Vi=(vi1,…,viD)表示。把粒子的位置帶入定義的適應(yīng)度函數(shù)可求得粒子對(duì)應(yīng)的適應(yīng)度值。設(shè)粒子i所經(jīng)歷過的最好位置記為粒子i的個(gè)體極值pbesti=(pi1,…,piD),整個(gè)種群所有粒子經(jīng)歷過的最好位置記為群體極值gbest=(g1,…,gD)。每個(gè)粒子根據(jù)如下公式進(jìn)行速度和位置更新:

(6)

2.2CPSO算法

由前述分析可知,N架UAV協(xié)同集結(jié)路徑規(guī)劃問題等價(jià)于為每架UAV協(xié)同尋找一組(m0,m1),使得生成的各PH路徑滿足各種約束,同時(shí)所有路徑的總體性能指標(biāo)趨于最優(yōu)或次優(yōu)。將N架UAV的可行解看作N個(gè)粒子群,各子種群內(nèi)的粒子按標(biāo)準(zhǔn)PSO算法獨(dú)立地進(jìn)行速度和位置的更新及適應(yīng)度計(jì)算等操作。在進(jìn)行UAV間協(xié)同評(píng)價(jià)時(shí),各子種群分別選擇代表個(gè)體進(jìn)行交換形成合作團(tuán)體,并對(duì)各子種群粒子進(jìn)行適應(yīng)度協(xié)同修正及個(gè)體極值和群體極值更新,使得各子種群分別向滿足團(tuán)體協(xié)同性下的最優(yōu)(或次優(yōu))位置靠近。為增強(qiáng)搜索效率,算法采用了精英保留策略。算法流程如圖1所示。

圖1 CPSO算法流程Fig.1 Flow Chart of CPSO Algorithm

2.2.1 子種群初始化

由上述分析可知,任意子種群i中的粒子zij=(mij,0,mij,1)代表第i個(gè)UAV的一條候選PH路徑j(luò),其飛行速度記為Vij=(Vij,0,Vij,1),其中i=1,2,…,N；j=1,2,…,M(M為子種群規(guī)模,假設(shè)各子種群規(guī)模相同)。根據(jù)給定的取值空間[Zmin,Zmax]和[-Vmax,Vmax],隨機(jī)初始化各子種群粒子的位置zij及飛行速度Vij,并采用下面介紹的適應(yīng)度函數(shù)和適應(yīng)度協(xié)同修正方法計(jì)算所有粒子的適應(yīng)度,同時(shí)初始化各子種群的個(gè)體極值pbestij和群體極值gbesti。

2.2.2 粒子速度和位置更新

(7)

2.2.3 粒子適應(yīng)度計(jì)算

為了使生成的PH路徑安全可飛行,且具有較短的長(zhǎng)度和較好的平滑性,這里定義第i個(gè)UAV(即子種群i)的適應(yīng)度函數(shù)如下:

(8)

式中:w1為權(quán)值;Ji,fuel為路徑燃油代價(jià),近似等于路徑長(zhǎng)度代價(jià);Ji,curve為路徑彎曲代價(jià),代表了路徑的平滑特性,由路徑彎曲能量來描述;Ji,obsc,Ji,nofly,Ji,unflyble分別為UAV避開障礙物、禁飛區(qū)以及滿足UAV最大曲率約束的懲罰代價(jià)。

為便于判斷,將PH路徑離散為Np個(gè)點(diǎn),當(dāng)全部離散點(diǎn)均不會(huì)與障礙物發(fā)生碰撞或穿過禁飛區(qū)時(shí),則近似認(rèn)為該路徑能夠避開障礙物或禁飛區(qū)。此時(shí)令Ji,obsc=0或Ji,nofly=0;否則令Ji,obsc或Ji,nofly為一個(gè)很大的正數(shù)。同樣地,若路徑上全部離散點(diǎn)處的曲率均不大于UAV的最大曲率約束κmax,即認(rèn)為該路徑滿足UAV的運(yùn)動(dòng)學(xué)約束,則令Ji,unflyble=0,否則為一個(gè)很大的正數(shù)。

2.2.4 粒子適應(yīng)度協(xié)同修正

為實(shí)現(xiàn)各UAV間的空間和時(shí)間協(xié)同,各子種群完成全部個(gè)體適應(yīng)度計(jì)算后,需分別提供一個(gè)代表個(gè)體(這里選取為各子種群的群體極值),并與其他子種群組成N個(gè)合作團(tuán)體。生成合作團(tuán)體后,需對(duì)各子種群的全部粒子進(jìn)行適應(yīng)度協(xié)同修正,具體方法為:對(duì)于合作團(tuán)體i,判斷其當(dāng)前子種群i的每個(gè)個(gè)體是否會(huì)與其他子種群的代表個(gè)體發(fā)生碰撞,若會(huì)碰撞則給予一個(gè)很大的碰撞罰值,用以保證UAV間的空間協(xié)同。同時(shí)在全部N個(gè)代表個(gè)體中找出曲線長(zhǎng)度最大的代表個(gè)體p,并令其曲線長(zhǎng)度為參考長(zhǎng)度Lref。若p不屬于當(dāng)前子種群i,則對(duì)當(dāng)前子種群i的每個(gè)個(gè)體均增加一個(gè)時(shí)間協(xié)同懲罰項(xiàng),用以保證各UAV的長(zhǎng)度趨于相等。且當(dāng)前個(gè)體曲線長(zhǎng)度越接近于參考長(zhǎng)度,罰值越小,反之罰值越大。協(xié)同修正后第i個(gè)UAV的適應(yīng)度函數(shù)為:

(9)

其中:

式中:Ji,collision和Ji,dtime分別為UAV碰撞懲罰項(xiàng)和時(shí)間協(xié)同懲罰項(xiàng);Li為某一個(gè)體的曲線長(zhǎng)度。

下面分析UAV間不發(fā)生碰撞的條件?？紤]第i和第j個(gè)UAV的PH路徑ri(q)和rj(q),設(shè)UAV安全半徑分別為Ri和Rj。為簡(jiǎn)單起見,將PH路徑均勻離散為NPH個(gè)等分點(diǎn)。由于假設(shè)各UAV勻速飛行且同時(shí)到達(dá)各自目標(biāo)位置,因此任意時(shí)刻下的兩UAV間的最短分離距離近似等于ri(q)和rj(q)的所有相同等分點(diǎn)間距的最小值。PH曲線長(zhǎng)度可由式(3)求得。因此已知PH曲線長(zhǎng)度時(shí),可由Newton-Raphson迭代法反解得到任一等分點(diǎn)處的曲線參數(shù)q,再由式(3)可得該等分點(diǎn)的位置坐標(biāo)。假設(shè)已求得ri(q)和rj(q)的任一相同等分點(diǎn)k的坐標(biāo)分別為(xi,k,yi,k)和(xj,k,yj,k),k=1,…,NPH,則兩UAV不發(fā)生碰撞的條件可表示為:

(10)

3 仿真驗(yàn)證及結(jié)果分析

為驗(yàn)證本文算法的有效性,以3架UAV從不同基地出發(fā)集結(jié)生成V型編隊(duì)的路徑規(guī)劃為例進(jìn)行仿真試驗(yàn)。環(huán)境中障礙物和禁飛區(qū)分別用圓形和矩形近似,且位置和大小事先已知;各UAV的起點(diǎn)位姿和集結(jié)點(diǎn)位姿以及期望編隊(duì)構(gòu)型如表1所示。各UAV的安全半徑均為0.1 km,最大曲率約束κmax=2 km-1,子種群規(guī)模均為M=20,最大進(jìn)化代數(shù)T=50,w1=0.5,Np=NPH=50。

表1 UAV起點(diǎn)和集結(jié)點(diǎn)位姿及期望編隊(duì)構(gòu)型

仿真結(jié)果如圖2和圖3所示。圖2為3架UAV集結(jié)路徑曲線,圖中■為UAV起點(diǎn);●為編隊(duì)集結(jié)點(diǎn);▲為各UAV完成集結(jié)時(shí)的期望隊(duì)形位置。圖3為3條路徑的曲率隨路徑長(zhǎng)度的變化曲線。

圖2 UAV協(xié)同編隊(duì)集結(jié)路徑Fig.2 Cooperative formation rendezvous paths of UAVs

圖3 路徑曲率Fig.3 Path curvatures

可以看出,兩種算法規(guī)劃得到的3條路徑均能避開障礙物和禁飛區(qū),路徑曲率連續(xù)變化且均未超過UAV的最大曲率約束,即均能滿足UAV的運(yùn)動(dòng)學(xué)約束。表2為不同算法得到的3條路徑長(zhǎng)度及相互之間的最短分離距離。

表2 路徑長(zhǎng)度和最短分離距離

由表2可知,CPSO算法得到的3條路徑長(zhǎng)度最大相差僅為2.8 m,幾乎可以忽略不計(jì)。表明3架UAV以相同速度勻速飛行時(shí)能夠同時(shí)到達(dá)各自目標(biāo)點(diǎn)，形成期望編隊(duì)構(gòu)型,且任意兩條路徑間的最短分離距離均大于兩架UAV的安全半徑之和,即任意兩架UAV均不會(huì)發(fā)生碰撞。對(duì)于CCGA算法,規(guī)劃得到的3條路徑長(zhǎng)度最大相差100 m,且最大長(zhǎng)度值偏大,說明3架UAV以相同速度勻速飛行時(shí)不能實(shí)現(xiàn)同時(shí)到達(dá)各自目標(biāo)位置。

表3 平均長(zhǎng)度和標(biāo)準(zhǔn)差

4 結(jié)束語

針對(duì)無人機(jī)編隊(duì)集結(jié)路徑規(guī)劃問題,采用5次PH曲線作為UAV備選路徑，并提出具有合作機(jī)制的分布式協(xié)同粒子群算法,對(duì)各UAV的PH路徑參數(shù)進(jìn)行協(xié)同優(yōu)化,得到的各路徑能夠滿足UAV間的協(xié)同性約束和可飛行性約束。接下來可進(jìn)一步研究三維復(fù)雜環(huán)境下多UAV編隊(duì)集結(jié)路徑規(guī)劃問題。

[1] 樊瓊劍,楊忠,方挺,等.多無人機(jī)協(xié)同編隊(duì)飛行控制的研究現(xiàn)狀[J].航空學(xué)報(bào),2009,30(4):683-691.

[2] Manathara J G,Ghose D.Rendezvous of multiple UAVs with collision avoidance using consensus[J].Journal of Aerospace Engineering,2012,25(4):480-489.

[3] 李杰,彭雙春,安宏雷,等.基于微分幾何與李群的無人機(jī)編隊(duì)會(huì)合方法[J].國(guó)防科技大學(xué)學(xué)報(bào),2013,35(6):157-164.

[4] 王懌.攻擊型無人機(jī)協(xié)同路徑規(guī)劃研究[D].西安:西北工業(yè)大學(xué),2014.

[5] 韓昕鋒,葉文,陳海生,等.多UCAV協(xié)同航路規(guī)劃算法[J].海軍航空工程學(xué)院學(xué)報(bào),2010,25(5):535-541.

[6] Neto A A,Macharet D G,Campos M F M.On the generation of trajectories for multiple UAVs in environments with obstacles[J].Journal of Intelligent and Robotic Systems,2010,57(1):123-141.

[7] Farouki R T,Sakkalis T.Pythagorean hodographs[J].IBM Journal of Research and Development,1990,34(5):736-752.

[8] Shorakaei H,Vahdani M,Gholami B I A.Optimal cooperative path planning of unmanned aerial vehicles by a parallel genetic algorithm[J].Robotica,2014,34(4):823-836.

[9] 史峰,王輝,郁磊,等.Matlab智能算法30個(gè)案例分析[M].北京:北京航空航天大學(xué)出版社,2011:129-136.

(編輯：李怡)

Formation rendezvous path planning for multi-UAVs based on CPSO

SHAO Zhuang1, ZHOU Zhou2, WANG Yan-xiong1, ZHU Xiao-ping3

(1.National Key Laboratory of Special and Technology on UAV, NWPU, Xi’an 710065, China;2.School of Aeronautics, NWPU, Xi’an 710072, China;3.UAV Research Institute, NWPU, Xi’an 710065, China)

To solve the problem of UAVs formation rendezvous path planning, a distributed cooperative particle swarm optimization (CPSO) algorithm was proposed. In view of the kinematic constraints of UAVs, PH curve was used because of its curvature continuity. Inspired by co-evolutionary theory, CPSO was proposed to plan a flyable safe path for each UAV, while meeting kinematic constraints of UAVs and the cooperation constraints. Simulations results show that the paths planned with CPSO could meet the kinematic constraints of UAVs, safety constraints, and the cooperation requirements between UAVs. Compared with the co-evolutionary genetic algorithms, the proposed CPSO has a higher success rate and better stability for searching.

UAV; path planning; formation rendezvous; PH curves; cooperative particle swarm optimization

2016-07-06;

2016-10-26;

時(shí)間:2016-11-10 09:12

“十二五”國(guó)防預(yù)研項(xiàng)目資助(41101060101)

邵壯(1987-)，男，安徽蚌埠人，博士研究生，從事多無人機(jī)編隊(duì)控制、多機(jī)協(xié)同路徑規(guī)劃研究。

V249.1

A

1002-0853(2017)01-0061-05