基于CPSO的UAV編隊集結路徑規劃

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

(1.西北工業大學 無人機特種技術國家重點實驗室, 陜西 西安 710065;2.西北工業大學 航空學院,陜西 西安 710072;3.西北工業大學 無人機研究所,陜西 西安 710065)

基于CPSO的UAV編隊集結路徑規劃

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

(1.西北工業大學 無人機特種技術國家重點實驗室, 陜西 西安 710065;2.西北工業大學 航空學院,陜西 西安 710072;3.西北工業大學 無人機研究所,陜西 西安 710065)

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

無人機; 路徑規劃; 編隊集結; PH曲線; 協同粒子群優化

0 引言

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

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

1 問題描述與路徑模型

1.1 問題描述

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

(1)

為了使各UAV同時抵達集結點時能夠以相同的速度進入編隊保持階段,這里假設集結過程中各UAV均以相同的速度勻速飛行,且集結環境中僅存在少量禁飛區和障礙物;因此UAV間同時達到約束,即等價于各路徑長度相等或近似相等。

1.2PH曲線路徑模型

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

(2)

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

(3)

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

(4)

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

(5)

2 CPSO協同路徑規劃算法

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

2.1 標準粒子群算法(PSO)

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

(6)

2.2CPSO算法

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

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

2.2.1 子種群初始化

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

2.2.2 粒子速度和位置更新

(7)

2.2.3 粒子適應度計算

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

(8)

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

為便于判斷,將PH路徑離散為Np個點,當全部離散點均不會與障礙物發生碰撞或穿過禁飛區時,則近似認為該路徑能夠避開障礙物或禁飛區。此時令Ji,obsc=0或Ji,nofly=0;否則令Ji,obsc或Ji,nofly為一個很大的正數。同樣地,若路徑上全部離散點處的曲率均不大于UAV的最大曲率約束κmax,即認為該路徑滿足UAV的運動學約束,則令Ji,unflyble=0,否則為一個很大的正數。

2.2.4 粒子適應度協同修正

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

(9)

其中:

式中:Ji,collision和Ji,dtime分別為UAV碰撞懲罰項和時間協同懲罰項;Li為某一個體的曲線長度。

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

(10)

3 仿真驗證及結果分析

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

表1 UAV起點和集結點位姿及期望編隊構型

仿真結果如圖2和圖3所示。圖2為3架UAV集結路徑曲線,圖中■為UAV起點;●為編隊集結點;▲為各UAV完成集結時的期望隊形位置。圖3為3條路徑的曲率隨路徑長度的變化曲線。

圖2 UAV協同編隊集結路徑Fig.2 Cooperative formation rendezvous paths of UAVs

圖3 路徑曲率Fig.3 Path curvatures

可以看出,兩種算法規劃得到的3條路徑均能避開障礙物和禁飛區,路徑曲率連續變化且均未超過UAV的最大曲率約束,即均能滿足UAV的運動學約束。表2為不同算法得到的3條路徑長度及相互之間的最短分離距離。

表2 路徑長度和最短分離距離

由表2可知,CPSO算法得到的3條路徑長度最大相差僅為2.8 m,幾乎可以忽略不計。表明3架UAV以相同速度勻速飛行時能夠同時到達各自目標點，形成期望編隊構型,且任意兩條路徑間的最短分離距離均大于兩架UAV的安全半徑之和,即任意兩架UAV均不會發生碰撞。對于CCGA算法,規劃得到的3條路徑長度最大相差100 m,且最大長度值偏大,說明3架UAV以相同速度勻速飛行時不能實現同時到達各自目標位置。

表3 平均長度和標準差

4 結束語

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

[1] 樊瓊劍,楊忠,方挺,等.多無人機協同編隊飛行控制的研究現狀[J].航空學報,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].國防科技大學學報,2013,35(6):157-164.

[4] 王懌.攻擊型無人機協同路徑規劃研究[D].西安:西北工業大學,2014.

[5] 韓昕鋒,葉文,陳海生,等.多UCAV協同航路規劃算法[J].海軍航空工程學院學報,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個案例分析[M].北京:北京航空航天大學出版社,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;

時間:2016-11-10 09:12

“十二五”國防預研項目資助(41101060101)

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

V249.1

A

1002-0853(2017)01-0061-05