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

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

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

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

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

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

針對(duì)無人機(jī)協(xié)同控制問題，提出一種多無人機(jī)任務(wù)分配與航跡規(guī)劃的整體控制架構(gòu)。將威脅和障礙區(qū)域考慮為合理的多邊形模型，使用改進(jìn)的A*算法規(guī)劃出兩個(gè)航跡點(diǎn)之間的最短路徑。并利用該路徑航程作為任務(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é)果并考慮無人機(jī)性能約束，基于B-spline法平滑路徑組合，生成飛行航跡。仿真結(jié)果表明，算法在保證計(jì)算速度和收斂性能的同時(shí)，能夠產(chǎn)生合理的任務(wù)分配結(jié)果和無人機(jī)的可飛行航跡。

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

0 引 言

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

而任務(wù)分配和航跡規(guī)劃是多無人機(jī)實(shí)現(xiàn)協(xié)同自主控制的關(guān)鍵技術(shù)［4］。由于任務(wù)分配與航跡規(guī)劃屬于非確定性多項(xiàng)式（nondeterministic polynomial，NP）問題［56］，窮舉法或最優(yōu)控制法求解過程將耗費(fèi)大量的計(jì)算代價(jià)，甚至難以實(shí)現(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)用具有很大的局限性。

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

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

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

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

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

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

2.1 任務(wù)描述

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

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

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

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

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

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

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

式中，t為執(zhí)行時(shí)間；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開始時(shí)間pj與結(jié)束時(shí)間qj，滿足如下關(guān)系：

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

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

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

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

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

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

3.1 無人機(jī)質(zhì)心運(yùn)動(dòng)方程

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 仿真實(shí)驗(yàn)分析

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

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

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

圖6為任務(wù)場(chǎng)景中控制量隨時(shí)間的變化規(guī)律。可以看出每架無人機(jī)的控制規(guī)律不僅與仿真場(chǎng)景中的對(duì)應(yīng)曲線相一致，并且控制曲線連續(xù)平滑，控制規(guī)律較易實(shí)現(xiàn)。

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

5 結(jié) 論

本文提出一種無人機(jī)協(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)基于無人機(jī)應(yīng)用場(chǎng)景相關(guān)的改進(jìn)PSO算法，求解任務(wù)分配結(jié)果，反饋至航跡規(guī)劃系統(tǒng)，利用B-spline法生成無人機(jī)可飛行航跡。仿真結(jié)果驗(yàn)證了算法的有效性，并給出了仿真場(chǎng)景中每架無人機(jī)的控制量。

［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.（陳宗基，魏金鐘，王英勛，等.無人機(jī)自主控制等級(jí)及其系統(tǒng)結(jié)構(gòu)研究［J］.航空學(xué)報(bào)，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é)報(bào)，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)，王建東，劉以安.交叉粒群算法在無人機(jī)航路規(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.（沈林成，牛軼峰，朱華勇.多無人機(jī)自主協(xié)同控制理論與方法［M］.北京：國(guó)防工業(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.（萬明，代忠，褚文奎.無人機(jī)掃描線區(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*算法的空間機(jī)械臂避障路徑規(guī)劃［J］.機(jī)械工程學(xué)報(bào)，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- ），男，博士研究生，主要研究方向?yàn)轱w行器協(xié)同任務(wù)分配與航跡規(guī)劃方法。

E-mail：yuyancool＠163.com

齊乃明（1962-- ），男，教授，博士，主要研究方向?yàn)轱w行器機(jī)電一體化。

E-mail：qnmok2000＠163.com

董 程（198-3- ），男，副主任工藝師，主要研究方向?yàn)轱w行器機(jī)電一體化。

E-mail：dongo2001＠163.com

姚蔚然（199-0- ），男，碩士研究生，主要研究方向?yàn)轱w行器協(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

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