齊 驥，王宇鵬，鐘 志

(1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，哈爾濱 150001;2.哈爾濱工程大學(xué) 信息與通信工程學(xué)院，哈爾濱 150001)

?

無(wú)人機(jī)多階段航跡預(yù)測(cè)協(xié)同任務(wù)規(guī)劃

齊 驥1，王宇鵬1，鐘 志2

(1.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，哈爾濱 150001;2.哈爾濱工程大學(xué) 信息與通信工程學(xué)院，哈爾濱 150001)

針對(duì)多無(wú)人機(jī)(Unmanned Aerial Vehicles, UAVs)協(xié)同控制問(wèn)題，提出了一種UAVs多階段航跡預(yù)測(cè)分布式任務(wù)規(guī)劃方法；定義從一次任務(wù)分配開(kāi)始到其中一項(xiàng)任務(wù)完成為一個(gè)任務(wù)周期；在每個(gè)規(guī)劃周期，首先，各UAV使用A*算法快速預(yù)測(cè)到所有任務(wù)目標(biāo)的路徑，提供至任務(wù)分配；然后，采用聚類算法修改目標(biāo)價(jià)值向量，協(xié)商分配結(jié)果，并實(shí)時(shí)計(jì)算探測(cè)范圍內(nèi)的最短路徑；最后，采用三次B樣條曲線平滑所分配的最短路徑，在線規(guī)劃出滿足飛行約束的飛行航跡；通過(guò)仿真實(shí)驗(yàn)對(duì)算法的有效性進(jìn)行了驗(yàn)證，結(jié)果表明，提出的算法能夠?qū)崟r(shí)獲得近似最優(yōu)的任務(wù)分配結(jié)果并規(guī)劃出可飛行航跡，并有效處理突發(fā)任務(wù)。

任務(wù)規(guī)劃；多無(wú)人機(jī)；任務(wù)分配；航跡規(guī)劃

0 引言

多無(wú)人機(jī)(unmanned aerial vehicles, UAVs)協(xié)同自主控制中，航跡規(guī)劃與任務(wù)分配，一般作為任務(wù)規(guī)劃的兩個(gè)獨(dú)立的層次開(kāi)展研究。在公開(kāi)文獻(xiàn)中，學(xué)者們先后提出了最優(yōu)控制法、圖論法、人工勢(shì)場(chǎng)法和人工智能等多種航跡規(guī)劃方法[1]。為滿足飛行約束，航跡規(guī)劃進(jìn)一步與航跡平滑的組合用于生成可飛行航跡[2]。

另一方面，任務(wù)分配技術(shù)經(jīng)歷了從以最優(yōu)化方法或啟發(fā)式方法為核心的集中式算法，到基于協(xié)商或市場(chǎng)機(jī)制等分布式算法[3]的發(fā)展。為實(shí)現(xiàn)全局最優(yōu)分配，這些算法在規(guī)劃開(kāi)始后一次性計(jì)算出所有任務(wù)的分配結(jié)果，計(jì)算量過(guò)于集中，同時(shí)，在出現(xiàn)突發(fā)任務(wù)等應(yīng)用場(chǎng)景變化時(shí)，需對(duì)整個(gè)任務(wù)分配進(jìn)行重新計(jì)算。此外，任務(wù)分配的全局目標(biāo)函數(shù)以任務(wù)航程為核心變量[4]，所以聯(lián)合任務(wù)規(guī)劃的航跡規(guī)劃和任務(wù)分配兩個(gè)層次進(jìn)行整體研究具有更重要的理論和現(xiàn)實(shí)意義。

針對(duì)上述問(wèn)題，相關(guān)學(xué)者進(jìn)行了任務(wù)規(guī)劃整體研究工作[5]，但一般只考慮了任務(wù)規(guī)劃的某些方面，完整的研究應(yīng)包括：分布式任務(wù)分配、考慮障礙規(guī)避的實(shí)時(shí)航跡規(guī)劃和包含飛行約束的航跡平滑3個(gè)部分[6]。針對(duì)前人研究的局限性，本文針對(duì)物理特性一致的UAVs協(xié)同任務(wù)分配問(wèn)題，提出了一種多階段航跡預(yù)測(cè)(Multi-Stage Path Prediction, MSPP)分布式任務(wù)規(guī)劃方法(Decentralized Mission Planning System, D-MPS)。

1 任務(wù)規(guī)劃問(wèn)題描述及系統(tǒng)建模

1.1 基本定義及假設(shè)

規(guī)定每個(gè)任務(wù)只由一架UAV執(zhí)行，設(shè)UAVs應(yīng)用于遠(yuǎn)程任務(wù)，高度變化對(duì)航程影響可線性近似。考慮N個(gè)UAV完成M個(gè)任務(wù)的任務(wù)規(guī)劃問(wèn)題，目的是：1) 實(shí)現(xiàn)任務(wù)分配，即UAVs與任務(wù)目標(biāo)間的最優(yōu)匹配；2) 規(guī)劃出可飛行航跡。為了描述問(wèn)題方便，本文中采用如下定義和假設(shè)：

定義1 (任務(wù)規(guī)劃周期，Mission Planning Period)：從一次任務(wù)分配開(kāi)始到其中一項(xiàng)任務(wù)被完成定義為一個(gè)任務(wù)周期.

假設(shè)1 (質(zhì)心運(yùn)動(dòng)假設(shè))：只考慮UAVs質(zhì)心運(yùn)動(dòng)，忽略空氣運(yùn)動(dòng)的影響；

假設(shè)2 (等速假設(shè))：UAVs以相同的恒速飛行，控制量只改變其飛行方向；

假設(shè)3 (禁飛區(qū)假設(shè))：采用凸多邊形建模禁飛區(qū)[6]，模型間距離足夠，即UAV具有足夠的空間生成所需航跡。

1.2 UAV運(yùn)動(dòng)學(xué)方程

圖1 UAVs航跡傾角和航向角定義

采用UAVn,n(N，表示第n架UAV，其中索引集合N= {1,…,N}。UAVn相對(duì)于地面坐標(biāo)系Oxyz的航跡角θn和航向角φn的定義如圖1所示，其運(yùn)動(dòng)學(xué)[7]方程為：

(1)

式中，(xn, yn, zn)為三維坐標(biāo)；V為飛行速度；uθ和uφ分別為航跡傾角和航向角對(duì)應(yīng)的控制量，如。記Sn= [xn, yn, zn, θn, φn]T為UAVn的狀態(tài)向量，Un= [uθn, uφn]T為對(duì)應(yīng)的控制量。

1.3 約束條件

記第m個(gè)任務(wù)為T(mén)askm, m (M, M = {1,…, M}為目標(biāo)索引集合。假設(shè)UAVn分配到Taskm，則在初始時(shí)刻為t0，對(duì)應(yīng)UAVn的初始狀態(tài)向量為：

(2)

在控制量Un,n∈N的作用下，UAVn終端約束為T(mén)askm的位置(Xm, Ym, Zm)。

UAVn航跡傾角、航向角及相應(yīng)的控制量應(yīng)滿足的機(jī)動(dòng)性能約束為：

(3)

式中,uθmax為最大航跡傾角控制量；uφmax為最大航向角控制量；對(duì)應(yīng)的最小轉(zhuǎn)彎半徑為：

(4)

1.4 任務(wù)分配模型

考慮目標(biāo)價(jià)值時(shí)效性，Taskm對(duì)于UAVn的價(jià)值函數(shù)由獎(jiǎng)勵(lì)函數(shù)Gm和懲罰函數(shù)Jnm組成：

(5)

式中,gm為T(mén)askm靜態(tài)價(jià)值；εm為價(jià)值耗損系數(shù)；ωn為燃料消耗系數(shù)；性能約束Θnm= 1表示UAVn能夠執(zhí)行Taskm，否則為0。

任務(wù)分配的全局目標(biāo)為：

(6)

式中，當(dāng)hnm= 1表示UAVn獲得Taskm，否則為0。

2 多階段航跡預(yù)測(cè)任務(wù)規(guī)劃算法

如圖2所示，D-MPS系統(tǒng)包括MSPP和任務(wù)分配兩個(gè)部分。而MSPP由3個(gè)階段組成：

圖2 任務(wù)規(guī)劃系統(tǒng)整體架構(gòu)

階段1：UAVs將探測(cè)范圍內(nèi)的障礙多邊形頂點(diǎn)實(shí)時(shí)添加至搜索空間，采用A*算法快速計(jì)算全部任務(wù)的估計(jì)路徑，生成價(jià)值向量。

階段2：利用聚類算法修改價(jià)值向量完成任務(wù)分配；UAV仍采用A*算法，實(shí)時(shí)更新探測(cè)范圍內(nèi)的最短路徑；突發(fā)任務(wù)添加至下一規(guī)劃周期。

階段3：采用三次B樣條曲線同步平滑上述最短路徑，規(guī)劃出滿足飛行約束的平滑航跡。

2.1 路徑估計(jì)

為快速計(jì)算至所有目標(biāo)的估計(jì)航程，采用UAV當(dāng)前位置臨近區(qū)域內(nèi)(或探測(cè)范圍)的禁飛區(qū)多邊形頂點(diǎn)建立A*算法的搜索空間。如圖3所示，該時(shí)刻搜索空間為{B, B1, B2, B3, C, C1}。與利用單元分解等傳統(tǒng)建模方法相比搜索空間維度大大減小。A*算法的性能指標(biāo)函數(shù)為：

minF(d)=min[L(d)+H(d)]

(7)

式中，d為搜索點(diǎn)位置，L(d)為探測(cè)范圍內(nèi)實(shí)際路徑；H(d)為至目標(biāo)的啟發(fā)式路徑。圖3所示的預(yù)測(cè)航跡結(jié)果為連接{A,B,C,T}的路徑線段組合。

圖3 A*算法路徑估計(jì)過(guò)程

2.2 路徑規(guī)劃

路徑規(guī)劃需根據(jù)任務(wù)分配結(jié)果展開(kāi)。UAVs基于估計(jì)路徑計(jì)算全部任務(wù)的價(jià)值向量，采用聚類算法對(duì)價(jià)值向量進(jìn)行修改。在聚類算法中，距離最近的兩個(gè)任務(wù)歸為一類，重復(fù)該過(guò)程，至類間距離達(dá)到給定上限。對(duì)于UAVn，若Taski與Taskj組成一類，則Taski的價(jià)值Rni修改為：

(8)

式中，γ為權(quán)重系數(shù)，pij為類間距離。

每架UAV首先選擇自身任務(wù)列表中價(jià)值最高的任務(wù)，UAVn選擇的任務(wù)為：

(9)

設(shè)競(jìng)爭(zhēng)同一任務(wù)Taskk*的UAVs集合記為Μk*，價(jià)值最高的UAVn*獲得相應(yīng)任務(wù)，即：

(10)

重復(fù)上述過(guò)程，至消除所有分配沖突。

各UAV根據(jù)分配結(jié)果利用A*算法計(jì)算探測(cè)范圍內(nèi)的最短路徑，并隨飛行過(guò)程實(shí)時(shí)更新。

2.3 路徑平滑

m次B樣條曲線方程定義[7]為：

(11)

式中,0 ≤s≤ 1,Pi= (Xi,Yi,Zi),i= 0,…,b為選擇的控制點(diǎn)；m階基函數(shù)Bi,m(s)為：

(12)

三次B樣條每段曲線由相鄰4個(gè)控制點(diǎn)決定，在連接點(diǎn)處兩階導(dǎo)數(shù)連續(xù)，通過(guò)添加和修正控制點(diǎn)使B樣條曲線滿足UAVs機(jī)動(dòng)性能約束和避障安全距離約束。圖4(a)所示為控制點(diǎn)的選取。首先根據(jù)最短路徑，將UAV位置A、目標(biāo)位置T及轉(zhuǎn)向點(diǎn)B確定為備選控制點(diǎn)。避障安全距離為Rs，威脅圓B以Rs為半徑，其切線AB'與B'T交于點(diǎn)B'，則{A,B',T}為滿足安全距離約束的控制點(diǎn)。為使曲線經(jīng)過(guò)控制點(diǎn)A，增加與其共線距離接近的輔助控制點(diǎn){A1,A2}，同理{B1,B2}、{C1,C2}分別為擴(kuò)展輔助控制點(diǎn)。線段A1A2與AB'共線、線段C1C2與B'C共線，線段B1B2垂直于BB'。則滿足安全距離約束的控制點(diǎn)序列為{A1,A,A2,B1,B',B2,C1,C,C2}。

圖4 B樣條控制點(diǎn)選取與曲率近似

初選控制點(diǎn)確定后，需驗(yàn)證其B樣條曲線是否滿足轉(zhuǎn)彎半徑約束。如圖4(b)所示，連續(xù)3個(gè)控制點(diǎn)A、B、C所組成的三角形中，ai為邊AC的長(zhǎng)度，li為該邊上的高，則由該三角形確定的該段曲線的極限曲率半徑ri可近似[7]表達(dá)為：

(13)

曲線上點(diǎn)Zi為控制點(diǎn)B對(duì)應(yīng)的極限曲率點(diǎn)。根據(jù)這種近似方式可以快速驗(yàn)證所選控制點(diǎn)是否滿足轉(zhuǎn)彎半徑約束ri≥rmin。在不滿足最小轉(zhuǎn)彎半徑約束時(shí)，采用“能量最優(yōu)法”[8]調(diào)整輔助控制點(diǎn)使其滿足約束要求。

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

仿真實(shí)驗(yàn)中給出5架UAVs及13個(gè)任務(wù)的仿真場(chǎng)景。以CoreE5800 3.2GHzCPU，4G內(nèi)存的DellPC為仿真實(shí)驗(yàn)環(huán)境。設(shè)飛行速度為200m/s，對(duì)應(yīng)轉(zhuǎn)彎半徑為1.91km；探測(cè)范圍為25km，避障安全距離為3km；航跡傾角和航向角取值范圍分別為[-25°, 25°]和[-180°, 180°]，對(duì)應(yīng)的控制約束分別為3°/s和6°/s。

圖5 三維仿真結(jié)果

為了驗(yàn)證任務(wù)分配方法的有效性，隨機(jī)選取UAVs與任務(wù)位置，重復(fù)多次仿真實(shí)驗(yàn)。本文采用可得到全局最優(yōu)解的集中式任務(wù)分配結(jié)果作為仿真對(duì)比。以任務(wù)完成后的全局目標(biāo)函數(shù)收益值作為評(píng)價(jià)標(biāo)準(zhǔn)，仿真結(jié)果如表1所示。其中實(shí)驗(yàn)1、4、5、6、7中，D-MPS均得到與集中式任務(wù)分配一致的最優(yōu)結(jié)果，實(shí)驗(yàn)2、3、8也得到了較好的近似最優(yōu)解。

表1 不同任務(wù)場(chǎng)景實(shí)驗(yàn)結(jié)果

表2 實(shí)驗(yàn)1任務(wù)分配結(jié)果

表3 仿真初值

表2給出實(shí)驗(yàn)1的分配結(jié)果，表3為對(duì)應(yīng)的UAVs初始狀態(tài)。在相同的仿真場(chǎng)景下，任務(wù)規(guī)劃開(kāi)始后50 s在位置(20, 30, 10)和(120, 65, 10)分別出現(xiàn)突發(fā)任務(wù)T1和T2，三維仿真結(jié)果如圖5所示。對(duì)比后可以看出，為實(shí)現(xiàn)全局最優(yōu)，UAVs在后續(xù)階段分配進(jìn)行了調(diào)整。UAV3將執(zhí)行的任務(wù)由{Task12, Task2}調(diào)整為{Task12, T1}，Task2在第二階段的任務(wù)規(guī)劃中分配至UAV2。T2被分配至UAV5，其任務(wù)執(zhí)行順序由{Task8, Task6, Task4}調(diào)整為{Task8, T2, Task6, Task4}。

4 結(jié)論

本文提出了一種多階段航跡預(yù)測(cè)算法，應(yīng)用于包含任務(wù)分配和航跡規(guī)劃的分布式實(shí)時(shí)任務(wù)規(guī)劃中。航跡預(yù)測(cè)算法由3個(gè)階段組成：基于A*算法的路徑估計(jì)、任務(wù)分配后的路徑規(guī)劃以及基于三次B樣條的航跡平滑算法。任務(wù)分配過(guò)程采用聚類算法修改任務(wù)價(jià)值函數(shù)。仿真結(jié)果表明算法能夠逼近最優(yōu)分配結(jié)果，并且分布式控制方式可有效處理突發(fā)任務(wù)目標(biāo)，控制量滿足約束，驗(yàn)證了規(guī)劃航跡的可行性。

[1]Luca D F, Giorgio G, Fulvia B Q. A novel approach for trajectory tracking of UAVs[J]. Aircraft Engineering and Aerospace Technology, 2014, 86(3): 198-206.

[2]Sun T, Huo C, Tsai S, et al. Intelligent flight task algorithm for unmanned aerial vehicle[J]. Expert Systems with Applications. 2011, 38(8): 10036-10048.

[3]Ren W, Beard R W, Atkin E M. Information consensus in multivehicle cooperative control[J]. IEEE Control System Magazine, 2007, 27(2): 71-82.

[4]Choi H L, Brunet L, How, J P. Consensus-based decentralized auctions for robust task allocation[J]. IEEE Transactions on Robotics, 2009, 25(4): 912-926.

[5]Sahingoz O K. Generation of Bezier curve-based flyable trajectories for multi-UAV systems with parallel genetic algorithm[J]. Journal of Intelligent and Robotic Systems: Theory & Applications, 2014, 74: 499-511.

[6]Moon S, Oh E, Shim D H. An integral framework of task assignment and path planning for multiple unmanned aerial vehicles in dynamic environments[J]. Journal of Intelligent and Robotic Systems, 2013, 70(1-4): 303-314.

[7]Wang Y, Wang S, Tan M, et al. Real-time dynamic Dubins-helix method for 3-D trajectory smoothing[J]. IEEE Transactions on Control Systems Technology, 2015, 23(2): 730-736.

[8]Abbas A, Nasri A, Maekawa T. Generating B-spline curves with points, normals and curvature constraints: a constructive approach[J]. Visual Computer, 2010, 26: 823-829.

Multi-stage Path Prediction Mission Planning Algorithm for Multiple Unmanned Aerial Vehicles

Qi Ji1, Wang Yupeng1, Zhong Zhi2

(1.College of Automation, Harbin Engineering University, Harbin 150001, China;2.College of Information and Communication Engineering, Harbin Engineering University, Harbin 150001, China)

In this paper, a multi-stage path prediction algorithm of the decentralized mission planning for cooperative UAVs is presented. The planning horizon is defined as the period between the start of task assignment and completion of any task. In every planning horizon, each UAV utilizes the A* algorithm to predict the paths to all tasks and provide the path distances for task assignment. Furthermore, the cluster algorithm is introduced to modify the tasks value vector. The UAVs negotiate the task assignment solution and calculate the shortest path to assigned task in the detection range in real time. Finally, the B-spline curve is addressed to convert the shortest path into flyable smoothing trajectory that subject to the flight constraints. For validation, the scenario of multiple UAVs to perform cooperative missions is considered. Numerical results show that the proposed algorithm can achieve the quasi-optimal assignment solution and generate the flyable trajectory in real time. In addition, the satisfactory performance to accomplish the pop-up tasks is demonstrated.

mission planning; unmanned aerial vehicles; task assignment; path planning

2015-12-08；

2015-12-29。

齊 驥(1995-)，男，黑龍江哈爾濱人，本科，主要從事智能算法與控制方向的研究。

鐘 志(1976-)，男，湖南岳陽(yáng)人，副教授，碩士生導(dǎo)師，主要從事通信、信號(hào)處理方向的研究。

1671-4598(2016)06-0189-03

10.16526/j.cnki.11-4762/tp.2016.06.052

V19

A