固定翼无人机编队协同控制算法及避撞研究

代飞扬，周绍磊，肖支才，祁亚辉，刘玄冰

(海军航空大学，山东 烟台 264000)

1 引言

单架无人机(unmanned aerial vehicle,UAV)由于其续航时间短、活动半径小及抗干扰能力差等固有缺点，在执行大型复杂任务时效率较差、成功率低[1]。相对而言，多无人机编队能解决执行大型复杂任务存在的时间、空间和任务层面上的冲突，从而大大提高任务效率[2]。协同控制技术是多无人机作战系统实现战斗力融合发展的基础，得到了广泛的重视和发展，近年来涌现出丰富的研究成果。编队控制作为协同控制的基础性研究之一，也取得了长足的发展[3]。

现在编队控制的主要方法有leader-follower法[4]、虚拟结构法[5]、人工势场法[6]、基于行为法、预测控制法[7]及基于一致性的编队控制法[8]等方法。leader-follower法具有控制简单、较为直观以及便于误差分析的优势。近年来，leader-follower法因其优点越发成为编队控制的基础性方法，研究人员针对leader-follower法和其他控制方法的糅合应用进行了大量研究。如文献[4]利用滑膜控制理论设计了一种分布式控制律，实现了非完整机器人编队的领导跟随控制，保证了机器人方向的有界性。但是leader-follower法具有基础性缺陷，作为leader的无人机一旦不能正常运行，将导致整个系统的失控，因此极少有leader-follower法的单独应用，一般以和其他控制方法复合应用为主。基于行为法设计的控制器是一系列基本动作组成的合集，一般包括队形生成、队形保持、避障、聚集等，基于行为法能灵活地改变编队队形，针对不同场景适应力较强，同时可扩展性强，编队可以进一步扩大，但是行为控制的模型复杂，难以用数学方法分析，同时对系统的稳定性分析难度较大，这是基于行为法的缺点所在。基于一致性法就是通过图论中的图来表示编队个体之间的相互关系，通过图论和控制理论对编队控制问题进行分析研究；基于一致法应用的关键步骤为通过邻接矩阵、Laplacian矩阵等进行状态变换或矩阵分解，将要研究的问题转化为一致性问题，再选择合适的一致性理论对问题进行处理，在编队控制的实际应用中也有着广泛应用[9,10]，该方法是下一阶段编队发展的重点方向。

根据多无人机编队的内部通信方式，一般将编队控制分为集中式和分布式[11]。集中式控制是在编队中选取一个或多个leader，通过leader对编队中的每个成员发送指令，从而实现整个编队的控制。分布式控制通过编队中相邻个体之间的信息交互实现控制目标，既能减少编队的通信压力，又能具有极强的容错性和适应性，因此分布式控制逐渐成为编队控制的热门控制策略[12]，如文献[13]提出了一种大规模固定翼无人机集群分组分层编队控制的方案，将无人机分为2层，分别提出不同的控制律，实现了百架规模的无人机集群在控制约束和风扰条件下的编队控制，为大规模编队的实际作战应用提供了一种实用解决方案。

相对旋翼无人机，固定翼无人机有着速度快、载重大、作战范围大等特点，对于大型复杂任务的成功执行有着较大优势[14]。固定翼无人机是典型的具有非完整性约束的非线性运动体，且存在最大/最小空速以及最大航向角速率等限制，其编队控制问题更具有挑战性[15]。

本文以采用leader-follower法的固定翼无人机编队为研究对象，在编队拓扑网络中引入分布式结构，改善编队通信拓扑结构。根据长机和僚机不同的任务要求设计不同的编队控制器，并对僚机编队控制器进行优化，在实际飞行中长机主要追踪期望轨迹，僚机跟踪长机的指定状态以达成初步一致，并在此基础上采用附加限幅PID方法实现精确编队，该方法针对小型固定翼无人机，具有易于实现、可扩展性强等优点；并根据僚机速度大小和飞行路径的变化，动态调整控制器中的期望距离参数，以实现更精准、平滑的控制；依靠拓扑网络中的分布式结构实现相邻僚机的信息交互，以此实时比对僚机之间的相对距离，确定控制状态，适时切换常态控制方法和临时避撞控制方法来实现无人机编队的避撞控制。最后，针对不同的控制情景，通过数据仿真进行模拟实验，对控制算法和避撞策略的可行性和有效性进行验证。

2 编队模型建立

在leader-follower结构的固定翼无人机编队中，长机跟踪预设轨迹飞行，僚机根据长机位置以及队形要求获得目标位置，通过跟踪目标位置实现对长机的跟踪控制，最终达到对长机的跟踪控制从而实现编队飞行。下面针对编队实际需要分别建立数学模型和网络拓扑结构。

2.1 数学模型建立

首先对固定翼无人机的动力学特性和运动学特性进行分析，以建立合适的数学模型进行算法分析。固定翼无人机的空间运动可以分为质心运动和绕质心的转动2种，需要6个自由度；选用型号的固定翼无人机的操纵机构包括副翼、方向舵、升降舵以及螺旋桨发电机，并配有飞控系统。用无人机3个线坐标和3个角坐标来表示无人机的空间运动，包括无人机质心的位移(线运动：飞行速度的增减运动，升降运动和侧移运动)和绕质心的转动(角运动：俯仰角运动、偏航角运动和滚转角运动)。

惯性坐标系采用北东地(NED)坐标系。以长机航向方向为X轴正向，X轴右向(俯视视角)为Y轴正向，Z轴在无人机的对称平面上，与X轴垂直指向机身下方，以此建立机体坐标系。将无人机的质心运动直接在机体坐标系进行投影，质心运动则从惯性坐标系转换到机体坐标系，从而得出以下方程[16, 17]。

状态变量：

XT=[u,v,w,φ,θ,φ,p,q,r,x,y,z]

(1)

控制变量：

UT=[δT,δe,δa,δr]

(2)

式(1)～(2)中：u、v、w分别为机体坐标系下沿纵轴、横轴和竖轴的速度分量；φ、θ、φ分别为滚转角、俯仰角、偏航角；p、q、r分别为滚转、俯仰、偏航角速度；x、y、z分别为惯性坐标系下的位置分量；对无人机的控制输入而言；δT为固定翼无人机气门大小的控制量；δe、δa、δr分别为升降舵偏角、副翼偏角和方向舵偏角。

固定翼无人机的动力学特性如式(3)、式(4)所示，主要在控制器的内环控制考虑。

(3)

(4)

运动学特性如式(5)、式(6)所示，主要在控制器的外环进行控制。

(5)

(6)

针对本文具体情况对相关模型进行简化。

由飞行动力学可知，当无人机在空中保持水平飞行或其高度变化比较平缓时，即θ≈0。在此条件下，无人机的垂直受力情况如图1所示，FL为固定翼无人机在飞行过程中产生的升力。

根据图1所示，此时侧向加速度as和滚转角φ的关系为：

mgtanφ=mas

(7)

图1 无人机的垂直受力情况示意图

本文在以下假设条件下进行相关设计：

1) 无人机的高度基本保持稳定，即θ≈0，q≈0，w=0。

在此条件下，由式(6)、式(7)可以得出：

(8)

(9)

在飞行过程中，固定翼无人机因其固有特性，上述参数都必须在一定范围内变化，以满足机动性和飞行性能的限制要求。约束条件如下：

(10)

以4架固定翼无人机构成的菱形编队为研究对象，期望编队队形和编队内各无人机坐标如图2所示。

图2 无人机编队模型坐标示意图

2.2 编队网络拓扑结构

无人机编队在飞行中，使用通信网络拓扑中的通信链路实现信息交互，从而实现飞行任务。在飞行过程中，长机向僚机发送经纬度、高度、速度、航向等信息，但并不获取僚机信息，长机只需维持自身按预期航线飞行。僚机一方面获取长机的位置和姿态信息，通过长机和僚机的相对距离确定自身期望位置；另一方面通过获取相邻僚机的位置信息以避免碰撞。本文在编队内采用的拓扑结构为强连通有向图，具体如图3所示。该拓扑结构的好处在于僚机之间可以通过僚机间的通信链路实现信息交互，克服了通过长机间接获取僚机位置信息导致的传递迭代误差，避免不必要的误差修正过程。

图3 无人机编队拓扑结构示意图

3 编队控制律设计

编队控制可以解耦为内环控制和外环控制，内环控制确定姿态，外环控制则驱动无人机到达期望的编队位置。本文在控制律设计时主要针对外环控制，内环控制只涉及到具体指令的补偿改善。

选取菱形编队中1号机作为编队的长机，其余无人机作为僚机，分别对长机和僚机设计控制律。

3.1 长机控制律设计

长机以匀速按照预期航线进行飞行，并实时将信息传递给编队中的僚机，因此长机只需保证能平滑地跟踪预期航线即可。

根据需要设计长机控制律，长机能控制律的基本原理就是在目标航线上找到一个与无人机相距距离L1的目标点，利用该目标点牵引无人机飞行。具体方法是在目标航线一侧作一段圆弧与无人机速度向量相切，圆弧与目标航线的交点为目标点，如图4所示[19]。假设目标航路点与长机的距离为L1，圆弧半径为R1，长机与目标航路点的方位角为ψ。

图4 长机跟踪模型目标点示意图

根据几何关系可得：

(11)

将式(11)代入向心加速度公式可得[19]：

(12)

由式(5)可知，横向加速度需要获得长机航速v1与向量L1之间的夹角ψ1，计算如下：

(13)

通过文献[7]可以对长机控制律的稳定性进行验证。

3.2 僚机控制律设计

对于僚机而言，其目标位置与实际位置之间的关系是通过长机的机体坐标系确定的横向相对距离和纵向相对距离来描述。机体坐标系和惯性坐标的关系如图5所示。

图5 僚机与长机位置关系示意图

两者坐标的转换关系(i=2、3、4)为：

(14)

从式(14)得，僚机与长机的实时相对距离为lXi=|Xi|；lYi=|Yi|(=2、3、4)。

根据长机坐标和编队任务需要，可得到长机与僚机的期望相对距离LXi、LYi。僚机的实时相对距离和期望相对距离的误差ΔXi、ΔYi为：

(15)

每架僚机与长机的期望相对距离LXi、LYi固定，通过期望相对距离LXi、LYi和长机期望点P1实时确定僚机目标点Pi，从而实时引导僚机跟踪长机飞行，形成期望编队队形，僚机目标点示意图如图6。

Li值是控制僚机跟踪控制的关键参数，是确定僚机目标位置的距离参数。与长机不同，僚机的Li值不是一个指定的参数，而是根据实时生成的僚机目标点位置Pi和僚机的实时位置机计算所得。

由于僚机没有固定的目标点，其飞行指令直接根据长机的飞行状态来确定，长机与僚机的联系最为紧密，且僚机的指令能根据长机的位置姿态变化而立刻做相应变化，因此该方法的编队效果较好。

图6 僚机目标点示意图

当僚机的期望位置与实际位置不一致时，通过控制僚机的飞行速度和横向加速度是无人机以平滑轨迹向无人机向目标点运动。给定僚机的速度为：

vi=v1-k1ΔXi

(16)

式(16)中，k1为比例因子，一般为常数。

僚机对长机的跟踪控制实质上是对期望位置的跟踪，与长机控制模式不同之处在于僚机是对动态期望位置的跟踪，而长机是对于固定航线的跟踪控制，因此取僚机的横向加速度取为：

(17)

3.3 避撞策略

为了避免编队中的无人机在飞行过程中相互碰撞，对僚机的控制方法增加避撞策略。僚机通过拓扑结构可知其余僚机的位置，将其余僚机所在区域整体简化为以僚机为圆心的圆形威胁区域[20]，通过避免飞过其余僚机所在的圆形威胁区域实现避撞。

由于长机与僚机的理想相对距离为固定值，因此可以通过长机的位置推算出僚机的理想位置。僚机的动态理想位置构成理想航线与威胁区域相交时，僚机进入避撞控制；当理想位置不与威胁区域相交时，退出避撞控制。根据编队中长机和僚机的位置分别确定理想位置和避撞威胁区域，由于编队中无人机位置是动态变化的，因此避撞控制的具体参数也在实时变化。这正是与固定航线避障策略的区别所在。避撞策略的基本流程如图7所示。

图7 僚机避撞策略的基本流程框图

当该僚机的期望位置开始与其余僚机所在的圆形威胁区域相接时，开始采取临时避撞控制方法对僚机进行控制，避撞飞行示意如图8所示。避撞结束后，僚机采取常规方法对长机进行跟踪控制。

图8 僚机避撞飞行示意图

Rij为僚机之间的距离，计算公式为：

rj为其余僚机的威胁区域半径，夹角γi和ηi计算公式为：

此时僚机速度vi取固定值v0，Li值取适宜的基础值L0。横向加速度ai为：

(18)

编队的避撞飞行类似于对于圆弧形航迹的跟踪控制，但是不同之处在于是对动态区域的跟踪控制，而不是对于静态圆弧形轨迹的跟踪。僚机避撞分析如图9所示。

图9 僚机避撞分析示意图

在如图9所示的情景下，可以假设：

η1≈0,η2≈0,η3≫0

(19)

并在此时的条件下，可知ψi=η，η2为当前航速方向和僚机j产生的圆形路线的切线之间产生的夹角。

角度之间的关系式为：

(20)

将式(19)代入到式(12)中，可得：

(21)

定义

(22)

又因为η2的极小特性可知：

(23)

(24)

在式(19)条件下，可得：

(25)

因此可得：

(26)

在式(12)、式(19)和η=η1+η2+η3下，可得：

(27)

(28)

将式(26)和式(28)代入式(12)可得：

(29)

4 仿真验证

由于固定翼无人机的运动学特性限制，取φmin=-30°，φmax=30°，vmin= 20 m/s，vmax=75 m/s。

由于在实际飞行中，存在一些难以消除的实际误差，如无人机无法保持水平飞行、传感器误差或模型不准确等，因此在算法应用时需要对编队中各无人机的横向加速度指令进行一定的误差补偿，具体为：

(30)

式(30)中，a1_last、ai_last分别为长机和僚机上一时刻的加速度指令。又由式(7)可得：

(31)

由于在实际飞行中俯仰角难于完全消除，俯仰角存在时俯仰角会偏小，因此对俯仰角进行补偿，即：

(32)

式(32)中，k4∈[1.0,2.0)。

下面对编队的队形保持进行可行性实验。

初始条件参数如表1所示。

表1 初始条件参数

此时编队中各无人机(UAV1～UAV4)的初始位置分别为(0,80)、(30,130)、(-20,130)(10,180)，初始位置的编队队形即为期望编队队形，编队中僚机的初始速度与长机相同，均以45 m/s。

首先令长机跟踪一个边长为1 000 m的正方形轨迹(如图10中H0所示)，编队中各无人机的飞行轨迹如图10所示。

图10 编队飞行轨迹示意图

由图10可知，无人机编队在直线飞行中能实现良好的编队保持，在协调转弯过程中编队个体间的相对距离会产生一定误差，编队队形会变差，但在恢复直线飞行后会快速消除误差，恢复理想队形。

然后,对避撞策略的有效性和可行性进行验证。取v1=v0=40 m/s，威胁区域半径r2=r3=r4=30 m， UAV1～UAV4各自初始坐标为(220,260)、(180,60)、(80,20)、(30,20)，初始航向角为φ1=45°，φ2=30°，φ3=φ4=60°。此时模拟的情景是编队中UAV2位置误差较大，在跟踪控制过程中如不进行避撞控制,会与UAV3产生碰撞，具体情况如图11、图12所示。

图11 编队飞行轨迹曲线

图12 编队滚转角变化曲线

图11中，1、2、3时刻分别为开始避撞控制、结束避撞控制、飞行结束时刻的各无人机位置，分别在开始和结束避撞控制时刻以UAV2位置为圆心做圆，并同时显示了UAV3不进行了避撞控制的飞行路径和避撞控制的飞行轨迹，证明了避撞控制的必要性。由图11、图12可以看出，UAV3通过避撞控制，避开了以UAV2为圆心的威胁区域，实现了避撞控制的目的，从而证明通过对编队控制算法增加避撞策略可以有效地预防编队飞行的碰撞风险，并且在控制过程中滚转角符合限幅要求，证明了本文方法的可行性。

5 结论

1) 针对长机和僚机分别提出的编队控制律基本完成跟踪预定航线和跟踪动态目标的任务，编队在直线飞行中能实现良好的编队队形保持，在协调转弯中队形不能较好保持，但是在恢复直线飞行后能快速消除编队误差，基本实现了编队协同控制。

2) 在编队通信网络引入了分布式结构，减少了编队内通信压力，提高了编队内相邻无人机的定位精度，并通过增加避撞策略解决了编队内无人机容易互相碰撞的问题，实现了编队飞行过程中对威胁邻机的实时规避，增强了编队控制算法的实用性、可行性性和有效性,后续研究可以通过增加事件触发机制减小通信压力。

3) 控制算法内具体参数的选择及其精确度至关重要，编队控制算法的精确度和可靠性还需进一步提升。