航迹预测的多无人机任务规划方法

齐乃明,孙小雷,2,董 程,姚蔚然

(1.哈尔滨工业大学 航天学院, 150001 哈尔滨；2.上海微小卫星工程中心, 201203 上海；3.航天二院283厂, 100854 北京)



航迹预测的多无人机任务规划方法

齐乃明1,孙小雷1,2,董 程3,姚蔚然1

(1.哈尔滨工业大学 航天学院, 150001 哈尔滨；2.上海微小卫星工程中心, 201203 上海；3.航天二院283厂, 100854 北京)

为提高无人机自主控制性能，实现任务分配与航迹规划整体架构，提出一种基于航迹预测的多无人机任务规划方法.首先，将禁飞区考虑为更接近真实场景的多边形模型；然后，使用改进A*航迹预测算法生成任意两个航迹点间障碍规避后的最短路径,利用该路径近似航迹航程作为任务分配过程的输入信息，建立目标函数，采用改进PSO算法求取最优结果；最后，使用B样条曲线平滑分配后的路径组合，生成无人机可飞行航迹.仿真结果表明, 该方法能够以较高的计算速度和精度生成近似最优的任务分配结果和满足飞行约束的平滑航迹.

任务规划；航迹规划；无人机；改进A*算法；PSO算法

自主控制是无人机(unmanned aerial vehicle, UAV)实现多机合作、执行复杂任务的关键技术[1].在未来的战场环境，如广域搜索、侦查监视、防空压制和目标摧毁等应用任务中[2-3]，对多无人机协同自主控制能力要求越来越高,其中任务分配和航迹规划是无人机实现自主控制的重要阶段[4].

任务分配面临非确定性多项式(nondeterministic polynomial, NP-complete)难题，计算量随无人机及任务目标的数量呈指数增长[5].目前多无人机协同任务分配算法主要以智能优化算法为核心[6]，包括进化算法(evolution algorithm, EA)、遗传算法(genetic algorithm, GA)和粒子群算法(particle swarm optimization, PSO)等.任务分配的目标函数以任务点的航程为输入.但目前的研究中，一般假设航程已知或用直线距离来近似，具有一定局限性.国内外对航迹优化提出了很多算法[7-8]，主要包括：势场法(potential field)、基于图论算法、智能优化算法和最优控制法等.智能算法一般以牺牲最优性来换取算法的计算速度.如A*搜索算法[9-10]被广泛应用到航迹规划中，但传统算法将搜索环境建立为网格化模型，大部分网格即使不含有效信息依然会被搜索，存在计算冗余.

本文提出一种改进A*航迹预测算法计算无人机与任务点间的最短路径，近似最优航程，作为任务分配目标函数的输入信息.同时取代圆形和椭圆形障碍模型的假设，采用更接近真实任务场景中的多边形障碍模型[11].将PSO的粒子结构修改为无人机与任务的匹配关系，最优的粒子元素即对应着分配结果，增强了算法的通用性.最后，考虑无人机运动约束，使用控制点优化的B样条方法，对分配的路径组合进行平滑规划，生成可飞行最优航迹.

1 整体控制架构与模型

1.1 整体控制架构

整体控制架构主要包含航迹预测、任务分配和航迹规划3个控制层，如图1所示.

1)航迹预测基于多边形障碍模型，利用改进A*算法求取障碍规避后到任务点的最短路径.

2)任务分配控制层，以最短路径的航程为变量，参考无人机与任务的匹配关系建立PSO的粒子结构.求得每架无人机的任务组合，即得出每架无人机需要航行的路径组合.

3)航迹规划层在无人机的飞行参数及约束下，通过B样条法平滑最短路径，得出无人机可飞行航迹.

图1 任务规划整体控制架构

1.2 无人机运动模型

航迹规划一般忽略空气动力及姿态机动等因素，只考虑无人机的质心运动学方程.设任务场景包含N架无人机，I = {1,…, N}为其索引集合，则二维笛卡尔坐标系下无人机i的运动学模型为[12-14]

式中：Xi= [xi, yi,φi]T, 其中iI为无人机i的状态向量；(xi, yi)为坐标；V为飞行速度；φi为航向角；uiφ为对应的控制量.

应满足的控制量约束条件为

得出最小转弯半径为

航向角约束为

1.3 任务分配模型

假设任务控制场景中有M个任务，如图2所示.任务分配的目标为最大化全局目标函数，实现任务到无人机的无冲突分配.即假设每个任务只分配给一架无人机，每架无人机可执行多个任务.任务分配问题的全局目标函数为[15]

式中：Rij为任务j对于无人机i的分配价值，为航程相关的函数；sij= 1表示无人机i分配到任务j，否则为0；J = {1,…, M}为任务索引集合.

图2 任务分配场景示意

2 改进A*航迹预测算法

在执行航迹规划与任务分配前，需建立任务场景障碍区域数学模型.本文提出以下假设：

1)只考虑无人机的质心运动方程；

2)将禁飞区考虑为多边形，其坐标可以表示为多边形的各顶点坐标序列，并假定不随时间变化；

3)一些情况下可以将多个障碍物或威胁建模为一个多边形.

无人机与任务点间最短路径可以定义为：连接两点且不穿过多边形威胁区域的最短折线，即为起点、终点及若干多边形区域顶点的连线.改进A*算法路径点d的距离函数为

(1)

式中:g(d)为从起点到当前路径点d的真实距离，h(d)为路径点d到目标点的估计距离.图3给出一种应用场景中改进A*算法的搜索过程.对于图3中搜索阶段，{B,B1,B2,B3}为起点A的直接搜索子节点，根据式(1)计算的距离为minf(d) =AB+BC+λCE，其中:g(d) =AB+BC，h(d) =λCE,λ为权重系数.

图3 改进A*搜索过程示意

改进A*算法将当前位置可直接到达的禁飞区顶点作为备选路径点，以减小了标准A*算法的搜索次数.该算法中只考虑最短路径，待航迹平滑阶段再考虑飞行约束条件.图4给出了航迹预测仿真结果，多边形区域表示禁飞区，虚线表示改进A*算法的计算结果，无人机在规避了障碍之后，可规划出到任务的最短路径.

图4 预测航迹求取结果

3 基于最短路径及改进PSO的任务分配

3.1 任务奖赏函数

任务j对于无人机i的价值Rij为奖励函数与惩罚函数之和.奖励函数Rewardj应具备任务时效性和时间窗约束，惩罚函数Costij为燃料相关函数与执行时间成正比：

其中

式中：t为任务执行时间；Gj为任务j的静态得分值；εj为时间折扣常数；μi为相关权重常数.Θij= ηiζijξj为约束因子，其中:ηi为燃料约束;ζij为任务匹配约束;ξj为任务有效时间窗约束.

3.2 改进PSO任务分配

考虑到任务场景中包含N架无人机与M个任务，为提高计算效率，将PSO算法的搜索空间建模为NM维.该方式的优点为，粒子中每个元素直接对应着无人机与任务间的分配关系，其通用性有利于算法的扩展.种群规模为Q，粒子n的位置向量为

当前阶段最佳粒子位置为pn，之前所有阶段种群的最优位置为pg，则在第k次迭代更新过程中，改进PSO算法的更新公式为：

(2)

(3)

式中：i∈I,j∈J;ω为惯性常数;c1、c2分别为加速度系数，r1、r2分别为[0, 1]上均匀分布的随机数.

算法初始化种群参数后，循环更新最优值及所有粒子位置和速度.每次迭代中，计算相应粒子的适应度值，若大于当前阶段最优值pn，则用该值更新pn.根据已计算的所有阶段更新全局最优位置pg.利用式(2)和式(3)更新粒子位置和速度.所有粒子都进行循环计算，直至满足终止条件.

4 基于B样条的航迹规划方法

无人机的飞行航迹应平滑连续且满足飞行约束.B样条法可以控制曲线导数的连续性，添加约束后可以保证无人机控制参数及约束的要求[16].3次B样条曲线的两阶导数连续的特性，使其适用于航迹平滑过程.B样条曲线方程定义为

式中：Ph(h= 0,…,Nh)为控制点；Bh,K(τ)为K阶基函数，即由节点向量，非递减参数序列T:τ0≤τ1≤τNh+K确定的K阶多项式样条函数.

B样条的基函数更新方程如下：

其中，节点方程为

A*算法计算的预测航迹包括3种情况，如图5所示.以情况1为例给出B样条控制点的选择过程.为满足安全距离约束Rs，以其为半径，做顶点B的威胁圆.从始点A与终点C分别做圆B切线交于B′，为修改后控制点.根据B样条曲线特性，为使曲线经过特定控制点，需在该点基础上再扩展与之共线的两点.在始点和终点处使扩展点与上述切线共线，顶点处使扩展点连线垂直于BB′.最终选取的控制点序列为{A1,A,A2,B1,B′,B2,C1,C,C2}.

图5 B样条控制点选取

5 结果与分析

在执行远距离任务时，无人机飞行高度相对变化较小，本文采用二维仿真验证算法性能.仿真环境为：Intel Pentium Dual-Core CPU E5800 3.2 GHz，4 G内存的DELL计算机.仿真场景中随机给出4架无人机初始位置和12个需要执行的任务位置.无人机以200 m/s的恒速飞行，控制量约束为6 (°)/s，对应的最小转弯半径为2 km，障碍规避安全距离为3 km.改进PSO的平均计算时间为1.44 s，仿真结果如图6所示.以无人机1为例，根据预测航迹计算无人机执行任务的奖赏函数，进行任务分配，结果为无人机1有序执行任务6、9、11.然后基于路径组合，规划B样条控制点，并考虑安全距离和最小转弯半径，规划出无人机1的飞行航迹.在仿真的航迹曲线中可以看出，选择的控制点能够规划出避障后的平滑航迹.图7为场景中相应无人机的控制量仿真结果，对应的曲线满足控制约束条件，并相对平滑连续，控制规律较易实现.

6 结 论

1)建立了一种任务分配与航迹规划整体架构，用于多无人机协同控制中的任务规划问题.采用改进A*航迹预测算法得出两航迹点间的最短路径，作为任务分配目标函数的核心变量.

图6 协同控制系统仿真结果

图7 仿真场景中无人机控制量曲线

2) 任务与无人机的匹配信息建立PSO粒子结构，取得最优分配结果后，考虑无人机性能约束，基于B样条法处理每架无人机的最短路径，生成平滑航迹.仿真表明提出的算法能够产生近似最优的任务分配结果和平滑的飞行航迹.

[1] 陈宗基, 魏金钟, 王英勋, 等. 无人机自主控制等级及其系统结构研究[J]. 航空学报, 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] 沈林成, 陈璟, 王楠. 飞行器任务规划技术综述[J]. 航空学报, 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]. 机械工程学报, 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]沈林成, 牛轶峰, 朱华勇. 多无人机自主协同控制理论与方法[M]. 北京: 国防工业出版社, 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.

国家自然科学基金(61171189)；上海航天科技创新基金(SAST201312).

齐乃明(1962—)，男，教授，博士生导师.

孙小雷，yuyancool@163.com.

V19

A

0367-6234(2016)04-0032-05