齊乃明,孫小雷,2,董 程,姚蔚然

(1.哈爾濱工業(yè)大學(xué) 航天學(xué)院, 150001 哈爾濱；2.上海微小衛(wèi)星工程中心, 201203 上海；3.航天二院283廠, 100854 北京)

?

航跡預(yù)測(cè)的多無人機(jī)任務(wù)規(guī)劃方法

齊乃明1,孫小雷1,2,董 程3,姚蔚然1

(1.哈爾濱工業(yè)大學(xué) 航天學(xué)院, 150001 哈爾濱；2.上海微小衛(wèi)星工程中心, 201203 上海；3.航天二院283廠, 100854 北京)

為提高無人機(jī)自主控制性能，實(shí)現(xiàn)任務(wù)分配與航跡規(guī)劃整體架構(gòu)，提出一種基于航跡預(yù)測(cè)的多無人機(jī)任務(wù)規(guī)劃方法.首先，將禁飛區(qū)考慮為更接近真實(shí)場(chǎng)景的多邊形模型；然后，使用改進(jìn)A*航跡預(yù)測(cè)算法生成任意兩個(gè)航跡點(diǎn)間障礙規(guī)避后的最短路徑,利用該路徑近似航跡航程作為任務(wù)分配過程的輸入信息，建立目標(biāo)函數(shù)，采用改進(jìn)PSO算法求取最優(yōu)結(jié)果；最后，使用B樣條曲線平滑分配后的路徑組合，生成無人機(jī)可飛行航跡.仿真結(jié)果表明, 該方法能夠以較高的計(jì)算速度和精度生成近似最優(yōu)的任務(wù)分配結(jié)果和滿足飛行約束的平滑航跡.

任務(wù)規(guī)劃；航跡規(guī)劃；無人機(jī)；改進(jìn)A*算法；PSO算法

自主控制是無人機(jī)(unmanned aerial vehicle, UAV)實(shí)現(xiàn)多機(jī)合作、執(zhí)行復(fù)雜任務(wù)的關(guān)鍵技術(shù)[1].在未來的戰(zhàn)場(chǎng)環(huán)境，如廣域搜索、偵查監(jiān)視、防空壓制和目標(biāo)摧毀等應(yīng)用任務(wù)中[2-3]，對(duì)多無人機(jī)協(xié)同自主控制能力要求越來越高,其中任務(wù)分配和航跡規(guī)劃是無人機(jī)實(shí)現(xiàn)自主控制的重要階段[4].

任務(wù)分配面臨非確定性多項(xiàng)式(nondeterministic polynomial, NP-complete)難題，計(jì)算量隨無人機(jī)及任務(wù)目標(biāo)的數(shù)量呈指數(shù)增長[5].目前多無人機(jī)協(xié)同任務(wù)分配算法主要以智能優(yōu)化算法為核心[6]，包括進(jìn)化算法(evolution algorithm, EA)、遺傳算法(genetic algorithm, GA)和粒子群算法(particle swarm optimization, PSO)等.任務(wù)分配的目標(biāo)函數(shù)以任務(wù)點(diǎn)的航程為輸入.但目前的研究中，一般假設(shè)航程已知或用直線距離來近似，具有一定局限性.國內(nèi)外對(duì)航跡優(yōu)化提出了很多算法[7-8]，主要包括：勢(shì)場(chǎng)法(potential field)、基于圖論算法、智能優(yōu)化算法和最優(yōu)控制法等.智能算法一般以犧牲最優(yōu)性來換取算法的計(jì)算速度.如A*搜索算法[9-10]被廣泛應(yīng)用到航跡規(guī)劃中，但傳統(tǒng)算法將搜索環(huán)境建立為網(wǎng)格化模型，大部分網(wǎng)格即使不含有效信息依然會(huì)被搜索，存在計(jì)算冗余.

本文提出一種改進(jìn)A*航跡預(yù)測(cè)算法計(jì)算無人機(jī)與任務(wù)點(diǎn)間的最短路徑，近似最優(yōu)航程，作為任務(wù)分配目標(biāo)函數(shù)的輸入信息.同時(shí)取代圓形和橢圓形障礙模型的假設(shè)，采用更接近真實(shí)任務(wù)場(chǎng)景中的多邊形障礙模型[11].將PSO的粒子結(jié)構(gòu)修改為無人機(jī)與任務(wù)的匹配關(guān)系，最優(yōu)的粒子元素即對(duì)應(yīng)著分配結(jié)果，增強(qiáng)了算法的通用性.最后，考慮無人機(jī)運(yùn)動(dòng)約束，使用控制點(diǎn)優(yōu)化的B樣條方法，對(duì)分配的路徑組合進(jìn)行平滑規(guī)劃，生成可飛行最優(yōu)航跡.

1 整體控制架構(gòu)與模型

1.1 整體控制架構(gòu)

整體控制架構(gòu)主要包含航跡預(yù)測(cè)、任務(wù)分配和航跡規(guī)劃3個(gè)控制層，如圖1所示.

1)航跡預(yù)測(cè)基于多邊形障礙模型，利用改進(jìn)A*算法求取障礙規(guī)避后到任務(wù)點(diǎn)的最短路徑.

2)任務(wù)分配控制層，以最短路徑的航程為變量，參考無人機(jī)與任務(wù)的匹配關(guān)系建立PSO的粒子結(jié)構(gòu).求得每架無人機(jī)的任務(wù)組合，即得出每架無人機(jī)需要航行的路徑組合.

3)航跡規(guī)劃層在無人機(jī)的飛行參數(shù)及約束下，通過B樣條法平滑最短路徑，得出無人機(jī)可飛行航跡.

圖1 任務(wù)規(guī)劃整體控制架構(gòu)

1.2 無人機(jī)運(yùn)動(dòng)模型

航跡規(guī)劃一般忽略空氣動(dòng)力及姿態(tài)機(jī)動(dòng)等因素，只考慮無人機(jī)的質(zhì)心運(yùn)動(dòng)學(xué)方程.設(shè)任務(wù)場(chǎng)景包含N架無人機(jī)，I = {1,…, N}為其索引集合，則二維笛卡爾坐標(biāo)系下無人機(jī)i的運(yùn)動(dòng)學(xué)模型為[12-14]

式中：Xi= [xi, yi,φi]T, 其中iI為無人機(jī)i的狀態(tài)向量；(xi, yi)為坐標(biāo)；V為飛行速度；φi為航向角；uiφ為對(duì)應(yīng)的控制量.

應(yīng)滿足的控制量約束條件為

得出最小轉(zhuǎn)彎半徑為

航向角約束為

1.3 任務(wù)分配模型

假設(shè)任務(wù)控制場(chǎng)景中有M個(gè)任務(wù)，如圖2所示.任務(wù)分配的目標(biāo)為最大化全局目標(biāo)函數(shù)，實(shí)現(xiàn)任務(wù)到無人機(jī)的無沖突分配.即假設(shè)每個(gè)任務(wù)只分配給一架無人機(jī)，每架無人機(jī)可執(zhí)行多個(gè)任務(wù).任務(wù)分配問題的全局目標(biāo)函數(shù)為[15]

式中：Rij為任務(wù)j對(duì)于無人機(jī)i的分配價(jià)值，為航程相關(guān)的函數(shù)；sij= 1表示無人機(jī)i分配到任務(wù)j，否則為0；J = {1,…, M}為任務(wù)索引集合.

圖2 任務(wù)分配場(chǎng)景示意

2 改進(jìn)A*航跡預(yù)測(cè)算法

在執(zhí)行航跡規(guī)劃與任務(wù)分配前，需建立任務(wù)場(chǎng)景障礙區(qū)域數(shù)學(xué)模型.本文提出以下假設(shè)：

1)只考慮無人機(jī)的質(zhì)心運(yùn)動(dòng)方程；

2)將禁飛區(qū)考慮為多邊形，其坐標(biāo)可以表示為多邊形的各頂點(diǎn)坐標(biāo)序列，并假定不隨時(shí)間變化；

3)一些情況下可以將多個(gè)障礙物或威脅建模為一個(gè)多邊形.

無人機(jī)與任務(wù)點(diǎn)間最短路徑可以定義為：連接兩點(diǎn)且不穿過多邊形威脅區(qū)域的最短折線，即為起點(diǎn)、終點(diǎn)及若干多邊形區(qū)域頂點(diǎn)的連線.改進(jìn)A*算法路徑點(diǎn)d的距離函數(shù)為

(1)

式中:g(d)為從起點(diǎn)到當(dāng)前路徑點(diǎn)d的真實(shí)距離，h(d)為路徑點(diǎn)d到目標(biāo)點(diǎn)的估計(jì)距離.圖3給出一種應(yīng)用場(chǎng)景中改進(jìn)A*算法的搜索過程.對(duì)于圖3中搜索階段，{B,B1,B2,B3}為起點(diǎn)A的直接搜索子節(jié)點(diǎn)，根據(jù)式(1)計(jì)算的距離為minf(d) =AB+BC+λCE，其中:g(d) =AB+BC，h(d) =λCE,λ為權(quán)重系數(shù).

圖3 改進(jìn)A*搜索過程示意

改進(jìn)A*算法將當(dāng)前位置可直接到達(dá)的禁飛區(qū)頂點(diǎn)作為備選路徑點(diǎn)，以減小了標(biāo)準(zhǔn)A*算法的搜索次數(shù).該算法中只考慮最短路徑，待航跡平滑階段再考慮飛行約束條件.圖4給出了航跡預(yù)測(cè)仿真結(jié)果，多邊形區(qū)域表示禁飛區(qū)，虛線表示改進(jìn)A*算法的計(jì)算結(jié)果，無人機(jī)在規(guī)避了障礙之后，可規(guī)劃出到任務(wù)的最短路徑.

圖4 預(yù)測(cè)航跡求取結(jié)果

3 基于最短路徑及改進(jìn)PSO的任務(wù)分配

3.1 任務(wù)獎(jiǎng)賞函數(shù)

任務(wù)j對(duì)于無人機(jī)i的價(jià)值Rij為獎(jiǎng)勵(lì)函數(shù)與懲罰函數(shù)之和.獎(jiǎng)勵(lì)函數(shù)Rewardj應(yīng)具備任務(wù)時(shí)效性和時(shí)間窗約束，懲罰函數(shù)Costij為燃料相關(guān)函數(shù)與執(zhí)行時(shí)間成正比：

其中

式中：t為任務(wù)執(zhí)行時(shí)間；Gj為任務(wù)j的靜態(tài)得分值；εj為時(shí)間折扣常數(shù)；μi為相關(guān)權(quán)重常數(shù).Θij= ηiζijξj為約束因子，其中:ηi為燃料約束;ζij為任務(wù)匹配約束;ξj為任務(wù)有效時(shí)間窗約束.

3.2 改進(jìn)PSO任務(wù)分配

考慮到任務(wù)場(chǎng)景中包含N架無人機(jī)與M個(gè)任務(wù)，為提高計(jì)算效率，將PSO算法的搜索空間建模為NM維.該方式的優(yōu)點(diǎn)為，粒子中每個(gè)元素直接對(duì)應(yīng)著無人機(jī)與任務(wù)間的分配關(guān)系，其通用性有利于算法的擴(kuò)展.種群規(guī)模為Q，粒子n的位置向量為

當(dāng)前階段最佳粒子位置為pn，之前所有階段種群的最優(yōu)位置為pg，則在第k次迭代更新過程中，改進(jìn)PSO算法的更新公式為：

(2)

(3)

式中：i∈I,j∈J;ω為慣性常數(shù);c1、c2分別為加速度系數(shù)，r1、r2分別為[0, 1]上均勻分布的隨機(jī)數(shù).

算法初始化種群參數(shù)后，循環(huán)更新最優(yōu)值及所有粒子位置和速度.每次迭代中，計(jì)算相應(yīng)粒子的適應(yīng)度值，若大于當(dāng)前階段最優(yōu)值pn，則用該值更新pn.根據(jù)已計(jì)算的所有階段更新全局最優(yōu)位置pg.利用式(2)和式(3)更新粒子位置和速度.所有粒子都進(jìn)行循環(huán)計(jì)算，直至滿足終止條件.

4 基于B樣條的航跡規(guī)劃方法

無人機(jī)的飛行航跡應(yīng)平滑連續(xù)且滿足飛行約束.B樣條法可以控制曲線導(dǎo)數(shù)的連續(xù)性，添加約束后可以保證無人機(jī)控制參數(shù)及約束的要求[16].3次B樣條曲線的兩階導(dǎo)數(shù)連續(xù)的特性，使其適用于航跡平滑過程.B樣條曲線方程定義為

式中：Ph(h= 0,…,Nh)為控制點(diǎn)；Bh,K(τ)為K階基函數(shù)，即由節(jié)點(diǎn)向量，非遞減參數(shù)序列T:τ0≤τ1≤τNh+K確定的K階多項(xiàng)式樣條函數(shù).

B樣條的基函數(shù)更新方程如下：

其中，節(jié)點(diǎn)方程為

A*算法計(jì)算的預(yù)測(cè)航跡包括3種情況，如圖5所示.以情況1為例給出B樣條控制點(diǎn)的選擇過程.為滿足安全距離約束Rs，以其為半徑，做頂點(diǎn)B的威脅圓.從始點(diǎn)A與終點(diǎn)C分別做圓B切線交于B′，為修改后控制點(diǎn).根據(jù)B樣條曲線特性，為使曲線經(jīng)過特定控制點(diǎn)，需在該點(diǎn)基礎(chǔ)上再擴(kuò)展與之共線的兩點(diǎn).在始點(diǎn)和終點(diǎn)處使擴(kuò)展點(diǎn)與上述切線共線，頂點(diǎn)處使擴(kuò)展點(diǎn)連線垂直于BB′.最終選取的控制點(diǎn)序列為{A1,A,A2,B1,B′,B2,C1,C,C2}.

圖5 B樣條控制點(diǎn)選取

5 結(jié)果與分析

在執(zhí)行遠(yuǎn)距離任務(wù)時(shí)，無人機(jī)飛行高度相對(duì)變化較小，本文采用二維仿真驗(yàn)證算法性能.仿真環(huán)境為：Intel Pentium Dual-Core CPU E5800 3.2 GHz，4 G內(nèi)存的DELL計(jì)算機(jī).仿真場(chǎng)景中隨機(jī)給出4架無人機(jī)初始位置和12個(gè)需要執(zhí)行的任務(wù)位置.無人機(jī)以200 m/s的恒速飛行，控制量約束為6 (°)/s，對(duì)應(yīng)的最小轉(zhuǎn)彎半徑為2 km，障礙規(guī)避安全距離為3 km.改進(jìn)PSO的平均計(jì)算時(shí)間為1.44 s，仿真結(jié)果如圖6所示.以無人機(jī)1為例，根據(jù)預(yù)測(cè)航跡計(jì)算無人機(jī)執(zhí)行任務(wù)的獎(jiǎng)賞函數(shù)，進(jìn)行任務(wù)分配，結(jié)果為無人機(jī)1有序執(zhí)行任務(wù)6、9、11.然后基于路徑組合，規(guī)劃B樣條控制點(diǎn)，并考慮安全距離和最小轉(zhuǎn)彎半徑，規(guī)劃出無人機(jī)1的飛行航跡.在仿真的航跡曲線中可以看出，選擇的控制點(diǎn)能夠規(guī)劃出避障后的平滑航跡.圖7為場(chǎng)景中相應(yīng)無人機(jī)的控制量仿真結(jié)果，對(duì)應(yīng)的曲線滿足控制約束條件，并相對(duì)平滑連續(xù)，控制規(guī)律較易實(shí)現(xiàn).

6 結(jié) 論

1)建立了一種任務(wù)分配與航跡規(guī)劃整體架構(gòu)，用于多無人機(jī)協(xié)同控制中的任務(wù)規(guī)劃問題.采用改進(jìn)A*航跡預(yù)測(cè)算法得出兩航跡點(diǎn)間的最短路徑，作為任務(wù)分配目標(biāo)函數(shù)的核心變量.

圖6 協(xié)同控制系統(tǒng)仿真結(jié)果

圖7 仿真場(chǎng)景中無人機(jī)控制量曲線

2) 任務(wù)與無人機(jī)的匹配信息建立PSO粒子結(jié)構(gòu)，取得最優(yōu)分配結(jié)果后，考慮無人機(jī)性能約束，基于B樣條法處理每架無人機(jī)的最短路徑，生成平滑航跡.仿真表明提出的算法能夠產(chǎn)生近似最優(yōu)的任務(wù)分配結(jié)果和平滑的飛行航跡.

[1] 陳宗基, 魏金鐘, 王英勛, 等. 無人機(jī)自主控制等級(jí)及其系統(tǒng)結(jié)構(gòu)研究[J]. 航空學(xué)報(bào), 2011, 32(6):1075-1083.[2] KALYANAM K, CHANDLER P, PACHTER M, et al. Optimization of perimeter patrol operations using unmanned aerial vehicles[J]. Journal of Guidance, Control, and Dynamics, 2012, 35(2): 434-441.

[3] SHAFERMAN V, SHIMA T. Unmanned aerial vehicles cooperative tracking of moving ground target in urban environments[J]. Journal of Guidance, Control, and Dynamics, 2008, 31(5): 1360-1371.

[4] BETHKE B, VALENTI M, HOW J P. UAV task assignment-an experimental demonstration with integrated health monitoring[J]. IEEE Robotics and Automation Magazine, 2008, 15(1): 39-44.

[5] 沈林成, 陳璟, 王楠. 飛行器任務(wù)規(guī)劃技術(shù)綜述[J]. 航空學(xué)報(bào), 2014, 35(3): 593-606.

[6] 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.

[7] MCLAIN T W, BEARD R W. Coordination variables, coordination functions, and cooperative timing missions[J]. Journal of Guidance, Control, and Dynamics, 2005, 28(1): 150-161.

[8] DAVIS J D, CHAKRAVORTY S. Motion planning under uncertainty: application to an unmanned helicopter[J]. Journal of Guidance, Control, and Dynamics, 2007, 30(5): 1268-1276.

[9] RUSSELL S J, NORVIG P. Artificial intelligence: a modern approach[M]. 3rd ed. New Jersey: Prentice Hall, 2009: 97-104.

[10]賈慶軒, 陳鋼, 孫漢旭, 等. 基于A*算法的空間機(jī)械臂避障路徑規(guī)劃[J]. 機(jī)械工程學(xué)報(bào), 2010, 46(13): 109-115.

[11]KOYUNCU E, URE N K, INALHAN G. Integration of path/maneuver planning in complex environments for agile maneuvering UCAVs[J]. Journal of Intelligent and Robotic Systems, 2010, 57(1/2/3/4): 143-170.

[12]DE F L, GUGLIERI G, QUAGLIOTTI F B. A novel approach for trajectory tracking of UAVs[J]. Aircraft Engineering and Aerospace Technology, 2014, 86(3):198-206.[13]SUJIT P B, HUDZIETZ B P, SARIPALLI S. Route planning for angle constrained terrain mapping using an unmanned aerial vehicle[J]. Journal of Intelligent and Robotic Systems, 2013, 69(1/2/3/4): 273-283.

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

[15]沈林成, 牛軼峰, 朱華勇. 多無人機(jī)自主協(xié)同控制理論與方法[M]. 北京: 國防工業(yè)出版社, 2013: 82-85.

[16]NIKOLOS I K, VALAVANIS K P, TSOURVELOUDIS N C, et al. Evolutionary algorithm based offline/online path planner for UAV navigation[J]. IEEE Transactions on Systems Man and Cybernetics Part B, 2003, 33 (6): 898-912.

(編輯 張 紅)

Mission planning based on path prediction for multiple UAVs

QI Naiming1, SUN Xiaolei1,2, DONG Cheng3, YAO Weiran1

(1.School of Astronautics, Harbin Institute of Technology, 150001 Harbin, China; 2.Shanghai Engineering Center for Microsatellites, 201203 Shanghai, China; 3.The Second Academy of CASIC 283 Factory, 100854 Beijing, China)

In order to improve the autonomous ability of unmanned aerial vehicles (UAVs) and achieve the integral framework of task assignment and path planning, a mission planning system based on path prediction algorithm for multiple UAVs is presented. To model obstacles more accurately, the forbidden areas are defined as polygons. Then, the optimal path segment avoiding all obstacles between two waypoints is computed by using improved A* path prediction algorithm. According to this path segment, the task assignment is determined by improved particle swarm optimization (PSO)algorithm. Finally, the B-spline method is adopted to smooth the flight path, which consists of the sequential path segments. Numerical results demonstrate that the proposed method can achieve the near-optimal task assignment and best flight routes with effectiveness of computation speed and precision.

mission planning; path planning; unmanned aerial vehicles (UAVs); improved A* algorithm; particle swarm optimization (PSO)

10.11918/j.issn.0367-6234.2016.04.005

2015-03-10.

國家自然科學(xué)基金(61171189)；上海航天科技創(chuàng)新基金(SAST201312).

齊乃明(1962—)，男，教授，博士生導(dǎo)師.

孫小雷，yuyancool@163.com.

V19

A

0367-6234(2016)04-0032-05