针对无人机编队飞行控制问题，Leader-Follower法是最经典的一种编队控制方法，最早见于移动机器人编队控制的文献[1]中，随后在无人机编队控制中得到了广泛的应用[2-4]。该方法实现起来比较简单，但编队稳定性不够而且误差会逐级放大。虚拟结构法引入刚体运动的概念[5-7]，将刚体中的各个顶点作为每架无人机的位置。文献[8-10]采用模型预测控制方法对编队进行控制，具有很好的适应性，可用于动态变化的编队控制问题，但是模型复杂、计算量较大。除此之外，人工势场法[11]和基于行为法[12]也经常用于编队控制中，但控制精度较差。
本文在Leader-Follower法的基础上提出了一种基于诱导航线的编队飞行控制方法，并考虑飞行过程中出现的突发障碍以及编队内飞机的碰撞问题[13-14]。
1 问题描述 1.1 无人机运动模型 将无人机视为质点，其在三维平面内的运动模型为
 | (1) |
式中：x、y和z分别代表无人机的经度、纬度和高度; v为飞机空速; φ和γ分别为无人机的航迹偏航角和俯仰角;
使用质点模型的同时一般应该加入适当的约束条件，来对模型进行限制：
 | (2) |
式中：vmin和vmax分别为飞机最小和最大速度限制；amin和amax分别为飞机最小和最大加速度限制；a为飞机加速度; ω为飞机偏航角速度; ωmax为飞机最大偏航角速度。
1.2 编队队形 编队队形给出了长机以及各僚机在编队飞行时需要保持的相对位置关系，包括前向和侧向距离，具体表示在原点与长机固联方位角等于长机偏航角的导航坐标系中，如图 1所示。
 |
| 图 1 编队坐标系 Fig. 1 Coordinate system of formation |
| 图选项 |
1.3 系统框图 每架无人机的飞行控制系统框图如图 2所示。
 |
| 图 2 系统框图 Fig. 2 System block diagram |
| 图选项 |
整个系统框图分为3层，分别为决策层、侦测感知层和飞行控制层。无人机通过无线通信获得编队信息以及编队中其他飞机信息，通过感知系统获得环境信息进一步获得是否有障碍信息。根据获得的信息进行编队控制、避障控制以及避撞控制，得出相应控制量，将得出的控制量传到飞行控制系统也就是自动驾驶仪，然后通过飞行控制系统完成对无人机的控制。
2 基于诱导航线的编队飞行控制 形成编队后，长机以期望的速度按照给定的航线飞行，僚机根据相对长机的位置进行编队保持控制，具体是根据当前相对于期望位置的误差和长机的飞行状态生成诱导航线，并跟踪该航线，同时根据长机的速度以及自身相对期望位置的纵向误差调节飞机速度。
2.1 诱导航线生成原理 当僚机跟踪长机的时候采用诱导航线控制方式，根据当前僚机相对期望位置的误差生成一条诱导航线，然后利用航线跟踪方法控制僚机。该控制方式根据位置误差的大小切换不同的航线生成方法，切换航线控制方法的原理就是：当远离期望位置的时候朝期望位置飞行，当处于期望位置附近时，沿经过僚机期望位置并与长机速度方向平行的诱导航线飞行。
航线切换由僚机相对期望位置的误差大小确定，切换范围采用继电器开关线形式，切换范围的大小根据飞机转弯能力设定，如图 3所示。
 |
| 图 3 航线切换范围 Fig. 3 Range of route switching |
| 图选项 |
较大椭圆的外边界表示从平行诱导航线切换到朝向期望位置诱导航线的切换线，在平行诱导的状态下一旦僚机从较大椭圆内穿出就进行切换。
较小椭圆的外边界表示从朝向期望位置诱导航线切换到平行诱导航线的切换线，在朝向期望位置诱导的状态下一旦僚机从外部穿入较小椭圆就切换。
朝向期望位置诱导航线的生成方法是：在每一个控制周期，以僚机当前位置为起点、以僚机期望位置为终点生成诱导航线，如图 4所示。
 |
| 图 4 朝向诱导航线 Fig. 4 Directing guidance route |
| 图选项 |
平行诱导航线的生成方法是：在每一个控制周期，以僚机期望位置为起点，将过该点平行于长机航向的射线作为诱导航线，如图 5所示。
 |
| 图 5 平行诱导航线 Fig. 5 Parallel guidance route |
| 图选项 |
僚机跟踪诱导航线的控制方法与跟踪普通航线的方法完全相同。
2.2 诱导航线生成 下面为诱导航线上2个点的计算公式，这里以僚机1为例，僚机1的实际位置为(x1, y1, h1)，速度和偏航角为(v1, φ1)，长机的实际位置为(xL, yL, hL)，速度和偏航角为(vL, φL)。
 | (3) |
 | (4) |
式中：δk为固定的距离；参数dk决定诱导航线位置；he和ve分别为僚机1的期望高度和期望速度；xe和ye为僚机1的期望位置：
 | (5) |
其中：DF和DW分别为僚机在编队中相对于长机的前向距离和侧向距离。
诱导航线产生过程如图 6所示。由式(3)可知，诱导航线的位置主要由参数dk决定，dk为一个从0到1的变量，根据飞机的滚转能力设定。当dk=0时，以僚机实际位置为基准、以长机飞行方向为航向产生诱导航线；当dk=1时，以僚机期望位置为基准、以长机飞行方向为航向产生诱导航线。在飞行过程中不断改变dk，使诱导航线逐渐接近僚机的期望位置，僚机通过跟随诱导航线逐渐从实际位置到期望位置移动，最终使得无人机到达期望的位置。
 |
| 图 6 诱导航线产生过程 Fig. 6 Production process of guidance route |
| 图选项 |
2.3 僚机诱导航线跟踪 僚机跟踪诱导航线时，主要涉及3个关键变量：期望速度、航向以及侧偏。
期望速度与长僚机在诱导航线上的投影距离(d)、长机速度分量(vLd)和长僚机速度分量(vLg)差有关。
 | (6) |
式中：k1和k2为比例系数，与飞机的调速能力有关。
长僚机在诱导航线上的投影距离：
 | (7) |
长机速度分量：
 | (8) |
长僚机速度分量差：
 | (9) |
式中：
 | (10) |
其中：de为僚机实际位置与期望位置的距离；d0为切换范围。可见，长僚机在诱导航线上的投影距离、长机速度分量和长僚机速度分量差越大，则期望速度就越大。
当前航向与期望航向的夹角：
 | (11) |
侧偏：僚机当前点到诱导航线的有向距离。
2.4 编队避障 无人机编队在飞行过程中面对突发威胁时，要及时反应，在保证顺利躲避障碍的前提下，尽量减小整个编队的机动动作。
避障流程图如图 7所示。
 |
| 图 7 避障算法流程图 Fig. 7 Flowchart of obstacle avoidance algorithm |
| 图选项 |
无人机检测到障碍物后，根据当前的相对位置以及障碍物的位置计算无人机恰好绕过障碍的距离，同时考虑改变位置后与其他无人机是否有碰撞冲突；如果有，计算当前位置再绕过其他无人机的距离，此时还要考虑尽量减小整个编队机动动作。将2次计算的距离作用到无人机原来编队位置上，从而改变编队队形顺利通过障碍。
为了便于说明算法流程，现举例说明。当飞机在躲避障碍时，与其他飞机存在碰撞可能时，如图 8所示。
 |
| 图 8 编队避障 Fig. 8 Formation of obstacle avoidance |
| 图选项 |
当d1 < d2时，飞机从左边避障，但会与3号飞机碰撞，会造成编队的扰动，因此需要从右边避障。如果d1 << d2，4号无人机从右边要飞一个很远的距离，此时需要从左边避障，在改变侧向距离的同时改变前向距离，防止与3号碰撞。
各飞机改变后的相对位置为
 | (12) |
式中：df和de分别为僚机编队中的前向和横向距离。
2.5 编队算法的自动驾驶仪控制 飞机的航向控制，根据飞机距航线的侧偏距离和当前航向差：
 | (13) |
式中：kd和kφ为控制参数。
飞机的速度采用PI控制：
 | (14) |
式中：ev为期望速度与当前速度的偏差；Kp为比例系数；T为时间。
飞机的高度控制为
 | (15) |
式中：kh为比例系数；He为期望高度；H为高度。
限制条件为
 | (16) |
3 编队防撞算法 无人机在编队飞行过程中最为重要的就是防止飞机之间的碰撞，针对这个问题，本文根据无人机的空间位置设计了基于碰撞优先级的防撞算法，算法流程图如图 9所示。
 |
| 图 9 防撞算法流程图 Fig. 9 Flowchart of collision avoidance algorithm |
| 图选项 |
防撞算法作用于整个编队飞行过程中，每架无人机都实时获得其他无人机信息，并预测它们可能的碰撞点，根据当前位置与碰撞点位置设计防撞算法，在避撞的同时还要考虑优先级问题。
3.1 防撞算法设计 避撞时根据飞机在空间中的速度向量以及相对位置(见图 10)，分别从高度和航向两方面进行躲避[15]。其中：
 |
| 图 10 飞机之间的几何位置 Fig. 10 Geometric position of aircrafts |
| 图选项 |
 | (17) |
式中：vi和vj为2架无人机的速度向量；eij为方向向量。
根据飞机在碰撞点的垂直距离以及到达碰撞点的时间设计避免碰撞的控制量：
 | (18) |
式中：dij为2架无人机的高度差；ti和tj为到达碰撞点的时间；ki、kij和kde为控制参数，与飞机的转向以及爬升能力有关。
可以看出随着两机的接近，时间逐渐变短，控制量随之增大，将控制量分别叠加到方向以及高度控制量上：
 | (19) |
式中：
3.2 基于模糊逻辑的避撞优先级设计 当2架无人机有碰撞冲突时，双方都进行避让会造成较大的机动代价，给整个编队带来较大的扰动。因此在避让时要考虑避让优先级问题，高优先级的无人机可以不用避让，低优先级的无人机进行避让。无人机的避撞优先级由无人机的机动潜力、受损程度以及无人机的重要程度决定。
3.2.1 模糊集和隶属度函数的设计 根据无人机受损程度将受损度划分为10个等级，即论域为[1, 10]，在此论域的基础上定义5个模糊集，分别为受损严重、较重、一般、较轻、轻微。5个模糊集的隶属函数设计如图 11所示。图中实线和虚线分别代表了不同模糊子集的隶属度函数，模糊子集在对应区间的隶属度为1，与相邻区间的隶属度为一条斜线，底端与相邻区间中点连接，顶端与两区间交点连接。这样定义好处是论域上任意一点只隶属于一个模糊子集。
 |
| 图 11 受损隶属度函数 Fig. 11 Damaged membership function |
| 图选项 |
根据无人机的飞行状况以及避撞方向对无人机的机动潜力进行判断，所谓机动潜力指的是在无人机当前运动的基础上做避撞机动的能力。
例如当无人机当前正向右转弯时，这时检测到威胁在无人机的右方需要向左躲避，在右转弯的基础上向左转执行起来较困难，机动潜力较差。如果在右转的基础上向右躲避，执行起来比较容易，机动潜力较强，具体如图 12所示。在图中侧滑角为β1，无人机向右转，威胁在左方。
 |
| 图 12 无人机机动方向 Fig. 12 UAV maneuvering direction |
| 图选项 |
根据侧滑角的大小以及威胁位置建立无人机机动潜力的模糊集。论域为无人机侧滑角的范围[βmin，βmax]，在论域的基础上定义5个模糊集，分别为很差、差、一般、强、很强。隶属度函数设计与之前类似，如图 13所示。
 |
| 图 13 威胁隶属度函数 Fig. 13 Threat membership function |
| 图选项 |
同理将优先级等级设为10个等级，即论域为[1, 10]，在此论域的基础上定义5个模糊集，分别为很高、高、一般、低、很低，隶属度与之前设计类似，不再赘述。
3.2.2 模糊推理 模糊逻辑的控制规则形式为“IF-THEN”型，由于知识的前提条件是2条，属于复合条件，控制规则集如表 1所示。
表 1 控制规则集 Table 1 Set of control rule
| 受损度 | 机动潜力 | ||||
| 很强 | 强 | 一般 | 差 | 很差 | |
| 轻微 | 很高 | 很高 | 高 | 一般 | 低 |
| 较轻 | 很高 | 高 | 高 | 低 | 低 |
| 一般 | 高 | 高 | 一般 | 很低 | 很低 |
| 较重 | 一般 | 低 | 很低 | 很低 | 很低 |
| 严重 | 低 | 低 | 很低 | 很低 | 很低 |
表选项
根据无人机受损程度以及当前机动潜力，利用表 1中的模糊规则得到无人机的避撞优先级。
当具有碰撞冲突的2架无人机的避撞优先级相等时，要根据无人机的战术重要程度进行再判断，例如长机重要程度要高于僚机，同等优先级的僚机要躲避长机。战术重要程度一般在任务开始前就要根据情况设定好，便于后续的应用。
4 仿真结果与分析 为了体现算法的有效性，以下所有仿真都采用了F15飞机模型。状态不同的4架无人机(UAV1~UAV4)由分散状态形成编队状态，并在形成编队的同时做转弯机动，以横一队形飞行，无人机之间的间距为300m，飞行速度为170m/s，编队飞行轨迹如图 14所示，编队飞行状态如图 15所示。
 |
| 图 14 编队转弯飞行轨迹 Fig. 14 Flight path of formation turning |
| 图选项 |
 |
| 图 15 编队转弯飞行状态 Fig. 15 Flight condition of formation turning |
| 图选项 |
可以看出，4架无人机的速度、高度以及偏航角在编队算法的作用下迅速收敛并保持稳定，以横一队形编队飞行。
在飞行过程中遇到的不是简单的障碍，而是一个狭长的山谷，不能容纳2架无人机并行飞过，此时在防撞算法的作用下，编队飞行轨迹如图 16所示，编队飞行状态如图 17所示。
 |
| 图 16 编队避障飞行轨迹 Fig. 16 Flight path of formation when avoiding obstacle |
| 图选项 |
 |
| 图 17 编队避障飞行状态 Fig. 17 Flight condition of formation when avoiding obstacle |
| 图选项 |
从图 16和图 17中可以看出，横一编队为了躲避障碍，通过速度以及航向的调节依次绕过障碍，形成一个最利于通过障碍的纵一队形，并且各飞机之间保持一个相对安全距离，安全绕过障碍后又重新形成横一队形。
在上述算例的飞行过程中，由于编队要进行机动和躲避障碍使得无人机之间具有碰撞的危险，以UAV3为例，在一次机动过程中与UAV2有碰撞冲突，根据模糊逻辑规则推理出UAV2的优先级要高，因此UAV3要进行主动避让。
为了体现避撞算法的有效性，给出未加碰撞消除和增加碰撞消除时的最近邻机距离和控制量变化，飞行状况如图 18和图 19所示。
 |
| 图 18 UAV3最近邻机距离 Fig. 18 Distance between UAV3 and the nearest neighbor |
| 图选项 |
 |
| 图 19 碰撞消除控制量 Fig. 19 Controlled quantity of collision elimination |
| 图选项 |
从图 18和图 19可以看出，未加碰撞消除控制量时，最近邻机距离超过安全距离警戒线，具有碰撞危险，增加碰撞消除控制量之后，可以看出随着距离的接近控制量也随之起作用，控制无人机进行躲避，最终使最近距离保持在警戒距离之外。
5 结论 1)建立了基于诱导航线的编队飞行控制方法，可以保证编队以指定队形稳定飞行。
2)根据突发障碍建立规避模型，保证整个编队顺利规避，同时考虑规避过程中无人机之间的碰撞。
3)建立了飞机之间的避碰模型，并基于模糊逻辑进行了避撞优先级设计，在避撞的同时尽量减小编队的扰动。
参考文献
| [1] | WANG P K C. Navigation strategies for multiple autonomous mobile robots moving in formation[J].Journal of Robotic System, 1991, 9(2): 177–195. |
| [2] | 丁衍, 魏晨, 鲍树语. 基于一致性算法的时延多无人机编队分散化控制[J].计算机应用, 2014, 34(S1): 151–155.DING Y, WEI C, BAO S Y. Formation control for multiple UAVs with time delay based on consensus algorithm[J].Journal of Computer Applications, 2014, 34(S1): 151–155.(in Chinese) |
| [3] | WEITZ L A, HURTADO J E, SINCLAIR A J. Decentralized cooperative-control design for multivehicle formations[J].Journal of Guidance, Control, and Dynamics, 2008, 31(4): 970–979.DOI:10.2514/1.33009 |
| [4] | KRISTIANSEN R, LORIA A, CHAILLET A, et al. Spacecraft relative rotation tracking without angular velocity measurements[J].Automatica, 2009, 45(3): 750–756.DOI:10.1016/j.automatica.2008.10.012 |
| [5] | 邵壮, 祝小平, 周洲, 等. 无人机编队机动飞行时的队形保持反馈控制[J].西北工业大学学报, 2015, 33(1): 26–32.SHAO Z, ZHU X P, ZHOU Z, et al. A formation keeping feedback control for formation flight of UAVs[J].Journal of Northwestern Polytechnical University, 2015, 33(1): 26–32.(in Chinese) |
| [6] | REN W, SORENSEN N. Distributed coordination architecture for multi-robot formation control[J].Robotics and Autonomous Systems, 2008, 56(4): 324–333.DOI:http://html.rhhz.net/BJHKHTDXXBZRB/10.1016/j.robot.2007.08.005 |
| [7] | REN W. Decentralization of virtual structure in formation control of multiple vehicle systems via consensus strategies[J].European Journal of Control, 2008, 14(2): 1–11. |
| [8] | 华思亮, 尤优, 张红, 等. 无人机编队的滚动时域控制[J].电光与控制, 2012, 19(3): 1–5.HUA S L, YOU Y, ZHANG H, et al. Receding horizon control of UAV formations[J].Electronics Optics & Control, 2012, 19(3): 1–5.(in Chinese) |
| [9] | FAHIMI F. Non-linear model predictive formation control for groups of autonomous surface vessels[J].International Journal of Control, 2007, 80(8): 1248–1259.DOI:10.1080/00207170701280911 |
| [10] | KEVICAKY T, BORRELLI F, FREGENE K, et al. Decentralized receding horizon control and coordination of autonomous vehicle formations[J].IEEE Transactions on Control System Technologies, 2008, 16(1): 19–33.DOI:10.1109/TCST.2007.903066 |
| [11] | KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots[J].International Journal of Robotics Research, 1986, 5(1): 290–298. |
| [12] | BALCH T, ARKIN R C. Behavior-based formation control for multirobot teams[J].IEEE Transaction on Robotics and Automation, 1998, 14(6): 926–939.DOI:10.1109/70.736776 |
| [13] | 曹建福, 凌志浩, 高冲, 等. 基于群集思想的多智能体编队避障算法研究[J].系统仿真学报, 2014, 26(3): 562–566.CAO J F, LING Z H, GAO C, et al. Obstacle avoidance and formation control for multi-agent based on swarming[J].Journal of System Simulation, 2014, 26(3): 562–566.(in Chinese) |
| [14] | 许敬刚, 王宏伦, 刘畅, 等. 无人机动态避撞区建模方法研究[J].电光与控制, 2014, 21(12): 30–35.XU J G, WANG H L, LIU C, et al. Modeling of dynamic collision avoidance zone for UAVs[J].Electronics Optics & Control, 2014, 21(12): 30–35.(in Chinese) |
| [15] | LALISH E, MORGANSEN K A, TSUKAMAKI T.Formation tracking control using virtual structures and deconfliction[C]//Proceedings of the 45th IEEE Conference on Decision and Control 2006. Piscataway, NJ:IEEE Press, 2006:5699-5705. |
