無人機任務(wù)分配與航跡規(guī)劃協(xié)同控制方法

孫小雷，齊乃明，董 程，姚蔚然

（1.哈爾濱工業(yè)大學(xué)航天學(xué)院，黑龍江哈爾濱150001；2.北京航天新風(fēng)機械設(shè)備有限責(zé)任公司，北京100854）

無人機任務(wù)分配與航跡規(guī)劃協(xié)同控制方法

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

（1.哈爾濱工業(yè)大學(xué)航天學(xué)院，黑龍江哈爾濱150001；2.北京航天新風(fēng)機械設(shè)備有限責(zé)任公司，北京100854）

針對無人機協(xié)同控制問題，提出一種多無人機任務(wù)分配與航跡規(guī)劃的整體控制架構(gòu)。將威脅和障礙區(qū)域考慮為合理的多邊形模型，使用改進(jìn)的A*算法規(guī)劃出兩個航跡點之間的最短路徑。并利用該路徑航程作為任務(wù)分配過程全局目標(biāo)函數(shù)的輸入，采用與協(xié)同系統(tǒng)相匹配的粒子結(jié)構(gòu)進(jìn)行改進(jìn)粒子群優(yōu)化（particle swarm optimization，PSO）任務(wù)分配迭代尋優(yōu)。根據(jù)分配結(jié)果并考慮無人機性能約束，基于B-spline法平滑路徑組合，生成飛行航跡。仿真結(jié)果表明，算法在保證計算速度和收斂性能的同時，能夠產(chǎn)生合理的任務(wù)分配結(jié)果和無人機的可飛行航跡。

無人機；任務(wù)分配；航跡規(guī)劃；改進(jìn)A*算法；改進(jìn)粒子群優(yōu)化

0 引 言

無人機在民用領(lǐng)域和軍事戰(zhàn)略中起著越來越重要的作用，能夠完成包括火災(zāi)監(jiān)測、搜索救援、偵查追蹤和執(zhí)行殲滅等多種任務(wù)［1-3］。執(zhí)行任務(wù)的復(fù)雜性以及環(huán)境的不確定性，要求無人機系統(tǒng)具有更高的決策能力和自主性。

而任務(wù)分配和航跡規(guī)劃是多無人機實現(xiàn)協(xié)同自主控制的關(guān)鍵技術(shù)［4］。由于任務(wù)分配與航跡規(guī)劃屬于非確定性多項式（nondeterministic polynomial，NP）問題［56］，窮舉法或最優(yōu)控制法求解過程將耗費大量的計算代價，甚至難以實現(xiàn)。所以該領(lǐng)域在近_些年的研究中主要以智能算法為核心［7-9］，求取近似最優(yōu)結(jié)果，主要包括：模擬退火（simulated annealing，SA）算法、進(jìn)化算法（evolution algorithm，EA）、遺傳算法（genetic algorithm，GA）和粒子群優(yōu)化（particle swarm optimization，PSO）算法等。任務(wù)分配算法應(yīng)以航跡航程建立全局目標(biāo)函數(shù)，目前的研究過程中一般假設(shè)航程已知，或用直線距離近似航程，使得算法應(yīng)用具有很大的局限性。

故本文提出一種無人機任務(wù)分配與航跡規(guī)劃整體控制架構(gòu)，首先采用改進(jìn)A*算法并考慮多邊形障礙區(qū)域［10］計算無人機與任務(wù)點間的最短路徑；基于該最短路徑航程，使用改進(jìn)PSO算法計算任務(wù)分配方案；最后利用B-spline法離散化無人機承擔(dān)任務(wù)的組合路徑，生成可飛行航跡。

1 協(xié)同控制架構(gòu)

多無人機協(xié)同控制架構(gòu)包括任務(wù)規(guī)劃系統(tǒng)與航跡規(guī)劃系統(tǒng)，如圖1所示。任務(wù)規(guī)劃系統(tǒng)基于場景中無人機與任務(wù)信息改進(jìn)PSO粒子結(jié)構(gòu)，以航跡規(guī)劃系統(tǒng)計算的最短路徑航程建立全局目標(biāo)函數(shù)的自變量信息，計算任務(wù)分配結(jié)果，求得每架無人機所承擔(dān)的任務(wù)組合及執(zhí)行順序。

圖1 任務(wù)與航跡規(guī)劃協(xié)同控制架構(gòu)

航跡規(guī)劃系統(tǒng)包括以改進(jìn)A*算法為核心的路徑規(guī)劃算法及基于B-spline法的航跡平滑算法兩部分。路徑規(guī)劃采用改進(jìn)A*算法計算無人機與任務(wù)點間的最短路徑，將直接可到達(dá)多邊形頂點作為改進(jìn)A*算法的備選節(jié)點，提高算法計算效率；航跡平滑算法以B-spline法為核心，考慮無人機自身性能約束對每架無人機的路徑段組合進(jìn)行平滑計算，得出無人機可飛行航跡。

2 基于改進(jìn)PSO的任務(wù)分配算法

2.1 任務(wù)描述

假設(shè)無人機協(xié)同控制應(yīng)用場景包含N架無人機和M個待執(zhí)行任務(wù)，則任務(wù)分配矩陣sij為

式中，i∈I，j∈J；I＝｛1，2，…，N｝與J＝｛1，2，…，M｝分別為無人機與任務(wù)索引集合。

任務(wù)分配的目標(biāo)是最大化全局目標(biāo)函數(shù)，實現(xiàn)任務(wù)到無人機的無沖突分配，無沖突指每個任務(wù)只能由一架無人機來執(zhí)行，但每架無人機可根據(jù)自身能力執(zhí)行多個任務(wù)。

根據(jù)上述信息，全局目標(biāo)函數(shù)［11］建立為

式中，Rij為無人機i分配到任務(wù)j后的獎賞函數(shù)，一般與對應(yīng)的航跡航程相關(guān)。

2.2 任務(wù)獎賞函數(shù)

無人機i分配到任務(wù)j后所得的得分值Rij為任務(wù)獎勵函數(shù)與懲罰函數(shù)之和［12］。獎勵函數(shù)Rewj具備任務(wù)時效性，懲罰函數(shù)Costij為燃料相關(guān)函數(shù)

式中，t為執(zhí)行時間；Gj為任務(wù)j的靜態(tài)得分值；εj和μi為相關(guān)權(quán)重常數(shù)。Θij＝ηi ijξj為約束因數(shù)，其中ηi為燃料約束，表示剩余燃料可飛行航程Dr（i）應(yīng)大于航跡航程Di；ζij為任務(wù)匹配約束；ξj為任務(wù)有效期約束，任務(wù)j開始時間pj與結(jié)束時間qj，滿足如下關(guān)系：

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

設(shè)PSO算法的種群規(guī)模為Q，根據(jù)任務(wù)分配場景信息，考慮到N架無人機執(zhí)行M個任務(wù)，PSO的粒子位置向量dn構(gòu)建為

設(shè)最佳粒子位置為pn，種群最優(yōu)位置為pg，第k次粒子更新公式為

式中，ω為慣性常數(shù)；c1，c2為加速度系數(shù)；r1，r2為［0，1］上均勻分布的隨機數(shù)。

下面給出改進(jìn)PSO算法步驟。外循環(huán)（第2～12行）不斷更新直至滿足算法終止條件，其中內(nèi)循環(huán)（第3～11行）為每個粒子更新過程。

3 航跡規(guī)劃系統(tǒng)

3.1 無人機質(zhì)心運動方程

在執(zhí)行任務(wù)中無人機短時間內(nèi)一般保持同一高度飛行，其二維質(zhì)心運動方程［13］為

式中，（xi，yi）為無人機i的二維坐標(biāo)；V表示飛行速度；i為航向角；ui為對應(yīng)的控制量，最小轉(zhuǎn)彎半徑rmin＝V／ui?？刂屏颗c航向角約束為

3.2 基于改進(jìn)A*算法的路徑規(guī)劃算法

最短路徑可以定義為，連接無人機與任務(wù)點且不穿過多邊形威脅區(qū)域的最短組合折線長度。為簡化問題，做如下假設(shè)：

（1）只考慮無人機的質(zhì)心運動方程；

（2）障礙物及威脅可用多邊形近似；

（3）無人機具備防碰撞能力及控制算法。

A*算法為經(jīng)典啟發(fā)式算法［1415］，計算當(dāng)前位置可到達(dá)路徑點距離，選擇最小距離路徑點列入備選路徑點集合。路徑點d的距離函數(shù)為

式中，g（d）表示起點到當(dāng)前路徑點d的真實距離；h（d）表示路徑點d到目標(biāo)點的估計距離。

取代了標(biāo)準(zhǔn)A*算法的搜索區(qū)域，本文提出的改進(jìn)A*算法用當(dāng)前位置可直接到達(dá)的多邊形威脅區(qū)頂點作為備選路徑點，以減少了算法的計算次數(shù)，該階段只考慮最短路徑，航跡平滑階段將加入安全距離及無人機自身性能等約束。如圖2所示，扇形O1OO2區(qū)域為無人機已估算范圍，P1、P2與P3為障礙區(qū)域，初始路徑點O的下一階段備選路徑點為｛A，A1，A2，A3｝，路徑點A的備選路徑點為｛B，B1｝，以此類推。

圖2 改進(jìn)A*算法備選路徑點選擇

圖3描述了改進(jìn)A*算法最優(yōu)路徑計算過程。在已估算范圍內(nèi)，考慮障礙規(guī)避規(guī)劃出無人機與任務(wù)間的最短折線路徑g（d），即OA＋AB。在估算范圍外，用無人機與任務(wù)的直線距離，即線段BD的加權(quán)來近似障礙規(guī)避后的實際路徑BC＋CD，得到估計航程h（d），從而得到改進(jìn)A*算法的距離函數(shù)f（d）。

圖3 改進(jìn)A*算法最優(yōu)路徑求取過程

改進(jìn)A*最優(yōu)路徑算法可減少不必要的計算量，具體計算流程如下：——

圖4給出一種應(yīng)用場景的仿真結(jié)果，多邊形為障礙外形。虛線段即為該算法求取的最優(yōu)路徑，可以看出在未考慮安全距離等因素下，改進(jìn)A*算法可有效規(guī)劃出一條無人機與任務(wù)間的最短路徑。

圖4 最優(yōu)路徑求取結(jié)果

3.3 基于B-spline的航跡平滑算法

航跡平滑階段除無人機自身性能約束外，還需考慮飛行安全距離等約束，可通過在B-spline法中添加控制點的方式來實現(xiàn)。特別是在多邊形威脅區(qū)域頂點處，在二維空間中可通過設(shè)置兩個控制點能夠有效規(guī)避威脅并生成平滑飛行航跡。二維B-spline離散化曲線方程［16］定義為

式中，（Xh，Yh）（h＝0，…，Nb）為求得的控制點；Bh，K（τ）為K階基函數(shù)，更新方程如下：

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

4 仿真實驗分析

仿真實驗中隨機給出4個無人機及7個任務(wù)位置，并設(shè)置任務(wù)獎賞函數(shù)。以Core E5800 3.2 GHz CPU，4 G內(nèi)存的Dell PC為仿真實驗環(huán)境。假設(shè)無人機恒速飛行，速度為200 m／s，最小轉(zhuǎn)彎半徑2 km，控制量約束umax＝6°／s，障礙規(guī)避安全距離4 km。

圖5給出該條件下的仿真實驗結(jié)果，其中多邊形為障礙區(qū)域。以無人機1為例，虛線段為航跡平滑前使用改進(jìn)A*算法產(chǎn)生的最短路徑。改進(jìn)PSO算法任務(wù)分配后無人機1承擔(dān)任務(wù)1和任務(wù)3，并給出路徑組合，實線為采用B-spline法生成的可飛行航跡?？梢钥闯鰹榱擞行б?guī)避威脅，航跡附近每個多邊形威脅區(qū)的頂點處設(shè)置兩個控制點，并得到了較理想的航跡規(guī)劃結(jié)果。

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

圖6為任務(wù)場景中控制量隨時間的變化規(guī)律?？梢钥闯雒考軣o人機的控制規(guī)律不僅與仿真場景中的對應(yīng)曲線相一致，并且控制曲線連續(xù)平滑，控制規(guī)律較易實現(xiàn)。

圖6 仿真場景中無人機控制量

5 結(jié) 論

本文提出一種無人機協(xié)同控制系統(tǒng)架構(gòu)，包括任務(wù)規(guī)劃及航跡規(guī)劃子系統(tǒng)。航跡規(guī)劃系統(tǒng)使用改進(jìn)A*算法規(guī)劃出最短路徑，提供任務(wù)規(guī)劃系統(tǒng)的控制輸入，使其完成任務(wù)分配過程。任務(wù)規(guī)劃系統(tǒng)基于無人機應(yīng)用場景相關(guān)的改進(jìn)PSO算法，求解任務(wù)分配結(jié)果，反饋至航跡規(guī)劃系統(tǒng)，利用B-spline法生成無人機可飛行航跡。仿真結(jié)果驗證了算法的有效性，并給出了仿真場景中每架無人機的控制量。

［1］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.

［2］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.

［3］Bethke B，Valenti M，How J P.UAV task assignment［J］.IEEE Robotics and Automation Magazine，2008，15（1）：39- 44.

［4］Chen Z J，Wei J Z，Wang Y X，et al.UAV autonomous control levels and system structure［J］.Acta Aeronautica et Astronautica Sinica，2011，32（6）：1075- 1083.（陳宗基，魏金鐘，王英勛，等.無人機自主控制等級及其系統(tǒng)結(jié)構(gòu)研究［J］.航空學(xué)報，2011，32（6）：1075- 1083.）

［5］Shen L C，Chen J，Wang N.Overview of air vehicle mission planning techniques［J］.Acta Aeronautica et Astronautica Sinica，2014，35（3）：593- 606.（沈林成，陳璟，王楠.飛行器任務(wù)規(guī)劃技術(shù)綜述［J］.航空學(xué)報，2014，35（3）：593- 606.）

［6］Bertuccelli L F.Robust decision-making with model uncertaintyin aerospace systems［D］.Cambridge：Massachusetts Institute of Technology，2008.

［7］Ni T Q，Wang J D，Liu Y A.Application of particle swarm algorithm in route planning of UAV［J］.Systems Engineering and Electronics，2011，33（4）：806- 810.（倪天權(quán)，王建東，劉以安.交叉粒群算法在無人機航路規(guī)劃中的應(yīng)用［J］.系統(tǒng)工程與電子技術(shù)，2011，33（4）：806- 810.）

［8］Mc Lain 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.

［9］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，2014，74（1／2）：499- 511.

［10］Koyuncu E，Ure N K，Inalhan G.Integration of path／maneuver planning in complex environments for agile maneuvering UCAVs［J］.Journal of Intelligent&Robotic Systems，2010，57（1／4）：143- 170.

［11］Shen L C，Niu Y F，Zhu H Y.Theories and methods of autonomous cooperative control for multiple UAVs［M］.Beijing：National Defense Industry Press，2013：82- 85.（沈林成，牛軼峰，朱華勇.多無人機自主協(xié)同控制理論與方法［M］.北京：國防工業(yè)出版社，2013：82- 85.）

［12］Bakker T，Klenke R H.Dynamic multi-task allocation for collaborative unmanned aircraft systems［C］∥Proc.of the 52nd Aerospace Sciences Meeting，2014：1- 10.

［13］Wan M，Dai Z，Chu W K.Turning path planning for UAV in region coverage with scanline［J］.Systems Engineering and Electronics，2014，36（9）：1750- 1754.（萬明，代忠，褚文奎.無人機掃描線區(qū)域覆蓋中的轉(zhuǎn)彎航跡規(guī)劃［J］.系統(tǒng)工程與電子技術(shù)，2014，36（9）：1750- 1754.）

［14］Chandler P R，Rasmussen S，Pachter M.UAV cooperative path planning［C］∥Proc.of the AIAA Guidance，Navigation，and Control Conference and Exhibit，2000：1- 11.

［15］Jia Q X，Chen G，Sun H X，et al.Path planning for space manipulator to avoid obstacle based on A*algorithm［J］.Journal of Mechanical Engineering，2010，46（13）：109- 115.（賈慶軒，陳鋼，孫漢旭，等.基于A*算法的空間機械臂避障路徑規(guī)劃［J］.機械工程學(xué)報，2010，46（13）：109- 115.）

［16］Nikolos I K，Valavanis K P，Tsourveloudis N C，et al.Evolutionary algorithm based offline／online path planner for UAV navigation［J］.IEEE Trans.on Systems Man and Cybernetics Part B，2003，33（6）：898- 912.

Cooperative control algorithm of task assignment and path planning for multiple UAVs

SUN Xiao-lei1，QI Nai-ming1，DONG Cheng2，YAO Wei-ran1
（1.School of Astronautics，Harbin Institute of Technology，Harbin 150001，China；2.Beijing Aerospace Xinfeng Machinery Equipment Limited Liability Company，Beijing 100854，China）

An integrating framework of task assignment and path planning for multiple unmanned aerial vehicles（UAVs）is presented.To avoid the obstacles area which is represented as polygon，the shortest path segment between UAVs and task can be found by the improved A*algorithm.According to this segment distance，the global objective function of task allocation is modeled.The assignment process is determined by improved particle swarm optimization（PSO），which particle structure matches the cooperative system.The B-spline method is adopted to smooth the flight path，which consists of path segments of the assignment.Numerical results demonstrate that the proposed method can achieve the optimal task assignment solution and best flight routes.

unmanned aerial vehicle；task assignment；path planning；improved A*algorithm；improved particle swarm optimization（PSO）

V 19

A

10.3969／j.issn.1001-506X.2015.12.17

孫小雷（198-5- ），男，博士研究生，主要研究方向為飛行器協(xié)同任務(wù)分配與航跡規(guī)劃方法。

E-mail：yuyancool＠163.com

齊乃明（1962-- ），男，教授，博士，主要研究方向為飛行器機電一體化。

E-mail：qnmok2000＠163.com

董 程（198-3- ），男，副主任工藝師，主要研究方向為飛行器機電一體化。

E-mail：dongo2001＠163.com

姚蔚然（199-0- ），男，碩士研究生，主要研究方向為飛行器協(xié)同智能控制與優(yōu)化算法。

E-mail：yaowr1990＠163.com

1001-506X（2015）12-2772-05

2014- 12- 03；

2015- 07- 23；網(wǎng)絡(luò)優(yōu)先出版日期：2015- 08- 31。

網(wǎng)絡(luò)優(yōu)先出版地址：http：∥www.cnki.net／kcms／detail／11.2422.TN.20150831.1752.006.html

國家自然科學(xué)基金（61171189）；上海航天科技創(chuàng)新基金（SAST201312）資助課題