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

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

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

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

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

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

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

0 引言

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

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

1 問題描述與路徑模型

1.1 問題描述

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

(1)

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

1.2PH曲線路徑模型

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

(2)

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

(3)

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

(4)

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

(5)

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

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

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

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

(6)

2.2CPSO算法

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

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

2.2.1 子種群初始化

由上述分析可知,任意子種群i中的粒子zij=(mij,0,mij,1)代表第i個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],隨機初始化各子種群粒子的位置zij及飛行速度Vij,并采用下面介紹的適應(yīng)度函數(shù)和適應(yīng)度協(xié)同修正方法計算所有粒子的適應(yīng)度,同時初始化各子種群的個體極值pbestij和群體極值gbesti。

2.2.2 粒子速度和位置更新

(7)

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

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

(8)

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

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

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

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

(9)

其中:

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

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

(10)

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

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

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

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

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

圖3 路徑曲率Fig.3 Path curvatures

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

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

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

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

4 結(jié)束語

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

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

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

[5] 韓昕鋒,葉文,陳海生,等.多UCAV協(xié)同航路規(guī)劃算法[J].海軍航空工程學(xué)院學(xué)報,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].北京:北京航空航天大學(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;

時間:2016-11-10 09:12

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

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

V249.1

A

1002-0853(2017)01-0061-05