技术领域
[0001] 本
发明涉及一种微小型无人
飞行器编队控制，属于飞行控制技术领域，具体涉及一种多无人机的动态编队控制方法。
背景技术
[0002] 当前已经有多达三十多个国家投入大量人
力和财力从事无人机的研究和生产。经过二十年的发展，该项技术已经比较成熟，在军民各个领域发挥着作用，尽管如此，单架无人机在遂行任务时存在着一些问题，例如单架无人机可能受到
传感器的数量限制，不能从多
角度全方位的对目标区域进行观察，面临大面积搜索任务时，不能有效的
覆盖整个搜索区域；如果执行的是救援任务，单架无人机在
载荷方面受到限制，往往影响整个救援的效能，带来更大损失，另外，一旦单架无人机出现故障，必须立即中断任务返回，可能会延误救援时机。
[0003] 针对单架无人机的缺点，近些年提出了编队飞行控制的概念并取得了一定得研究成果主要包括：队形设计，
气动耦合、队形动态调整、航迹协调规划、移动自组织网络以及编队飞行控制方法等。编队控制方法通常包括基于行为方式的编队控制、“长机-僚机”方式的编队控制和虚拟结构方式编队控制。
[0004] 基于行为方式的编队控制：在多无人机近距编队飞行过程中，机群中每一架无人机对其传感器输入信息的行为相应分成以下四种情况：碰撞避免、障碍物回避、目标获取和队形保持。基于行为方式的编队控制方法的最大特点是借助于行为响应控制的平均权重来确定编队中每一架无人机该采用哪一种行为响应方式，但该方法的队形刚性不够，往往为松散编队。
[0005] “长机-僚机”方式的编队控制：这种编队策略的特点是基于预设的编队结构，通过对长机的航向速度、航向角和高度
跟踪，来调整僚机，达到保持编队队形的目的。由于这种控制结构会受到很大的干扰影响，因此针对其特点，采用了鲁棒控制方法、极值搜索控制方法、自适应控制方法和变结构控制方法等多种技术，这种编队的缺点是队形刚性太强，威胁回避能力不足。
[0006] 虚拟结构方式编队控制：虚拟结构方式一般采取虚拟长机的方法协调其他飞机，这种方式可以避免“长机-僚机”方式的干扰问题，但是合成虚拟长机
位置和向编队各无人机传输其位置，需要以高通信
质量和高计算能力为代价，另外虚拟结构的
节点位置固定，避障功能往往很差。
[0007] 除了上述的三种编队控制方法以外，无人机的编队控制方法还包括一些其他方式的控制，如MPC模型预测控制策略方法、基于
模糊逻辑和神经网络技术的编队控制与避障策略方法，这两种方法控制结构比较复杂，在模糊逻辑和神经网络权值选取上需要做较多次试验才能确定，占用时间较多，且结果不具有普适性。
发明内容
[0008] 本发明的目的是解决便于工程化的微小型
无人飞行器编队控制问题，提出一种多无人机的动态编队控制方法，针对传统的虚拟结构方式编队控制对通信质量要求较高的缺点，引入了基于行为的编队控制方法，降低了对编队无线数据链更新率的要求，增强了无人机群编队的避障能力；针对传统基于行为方式的编队控制编队刚性保持不好的缺点，引入了虚拟结构作为参考，通过两种编队方法的融合，可以发挥各自方法的优点：在保持队形相对稳定的前提下，增强微小型无人机在未知环境下规避障碍物和威胁的能力，对于无人机群协同低空突防有一定借鉴意义的。且本发明提出的一种多无人机的动态编队控制方法，尤其适用于威胁不确定环境下，以往无人机编队遂行侦查或搜救任务的环境一般是山地或平原，这时的障碍物一般是尺度比较大的自然物(例如山峰)，这样的障碍物易被数字地图提取，往往在执行编队飞行任务前已经装载在飞控计算机的数字地图中。而对于城市的侦查和搜救任务，地面情况较为复杂，为保证侦查图像
精度要求编队飞行高度低，飞行高度较低的无人机易与地面人工障碍物(例如楼群、高压线塔和电线杆)发生碰撞，而人工障碍物与自然物有很大区别，人工障碍物尺度较小，不易在数字地图提取，易引发编队飞行更大的
风险。
[0009] 一种多无人机的动态编队控制方法，其特征在于：包括以下几个步骤：
[0010] 步骤一：队形保持方法；
[0012] 建立地面坐标系XOY，其中X轴代表东向位置，Y轴代表北向位置，ML和MF分别表示编队长机和僚机，ΨL和ΨF分别表示长机和僚机的航迹偏角，MV表示虚拟结构设定的僚机，简称为虚拟僚机，L和a分别表示虚拟僚机对长机期望的距离和观测角，Ll和al分别表示实际僚机对长机的距离和观测角，存在的通信网络延迟为Δt，MVH和MFH分别表示虚拟僚机和实际僚机在Δt移动后的位置，MFH表示长机MF在通信网络延迟Δt内飞行的剖面距离，MVH表示虚拟僚机MV在通信网络延迟Δt内飞行的剖面距离；
[0013] (2)建立各个关系公式；
[0014] 设定长机和僚机在通信周期内移动距离均为l，根据长机、僚机和虚拟僚机的几何关系建立长机ML和MVH的位置关系公式：
[0015]
[0016] 其中，xMVH、yMVH、xML、yML分别表示考虑通信延迟的虚拟僚机的东向位置、北向位置、长机的东向位置、北向位置，L表示虚拟僚机对长机期望的距离，ΨL表示长机的航迹偏角，a表示虚拟僚机对长机期望的观测角，l表示长机僚机在通信周期内移动距离；
[0017] 建立僚机MF和MFH的位置关系公式：
[0018]
[0019] 其中，xMFH、yMFH、xMF、yMF分别表示考虑通信延迟的实际僚机的东向位置、北向位置、实际僚机的东向位置、北向位置，ΨF表示僚机的航迹偏角；公式(2)减公式(1)可以建立MVH和MFH的跟踪误差关系公式：
[0020]
[0021] 其中，ex、ey分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差；对MVH和MFH的跟踪误差关系公式(3)进行求导，得到：
[0022]
[0023] 其中，VL、VF分别表示长机和僚机的速度，ωL、ωF分别表长机和僚机的
偏航角速度， 分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差的导数，l表示长机僚机在通信周期内移动距离，L表示僚机对虚拟长机期望的距离，ΨL表示长机的航迹偏角，a表示虚拟长机期望的观测角，ΨF僚机的航迹偏角；
[0024] 将地面坐标系的ex、ey投影到僚机的速度坐标系：
[0025]
[0026] 其中，ex、ey分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差， 分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差在僚机速度坐标系下的投影；
[0027] 对变换坐标后的公式(5)进行求导，得到：
[0028]
[0029] 其中 分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差的导数， 分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差在僚机速度坐标系下的投影的导数；
[0030] 联立公式(3)、公式(4)和公式(6)得到：
[0031]
[0032] 当僚机的偏航角速度和速度指令为公式(8)所示时，公式(7)可以化简为公式(9)：
[0033]
[0034] 其中，k1，k2表示实际编队过程中待调整的控制律系数；
[0035]
[0036] 考虑李雅普诺夫函数：
[0037]
[0038] 公式(9)代入公式(10)可得
[0039]
[0040] 由李雅普诺夫定律，对于给定的偏差 采用公式(8)的速度和角速度指令，编队队形偏差收敛为0，使得误差表达式 收敛为0；
[0041] 最终的编队过载指令 和编队速度指令 为：
[0042]
[0043] 步骤二：避障方法；
[0044] 地面障碍物在空间中服从均匀分布，M为无人机当前位置，V为无人机速度矢量，r为障碍物的威胁范围，R为无人机距离障碍物的距离，为保证无人机不进入威胁区，无人机可以沿左侧航迹MA飞行也可以沿右侧航迹MB飞行，设距无人机最近的威胁源为圆心O，理想威胁回避航迹为与圆O外切且与速度矢量V相切的两圆Ol、Or的圆弧MA和MB，定义切点A、B为威胁回避导航点；
[0045]
转弯半径应用解三角形的方法求取，在ΔOOlM中应用余弦定理得：
[0046]
[0047] 得出无人机威胁回避的转弯半径：
[0048]
[0049]
[0051]
[0052] 其中ar、al分别表示无人机沿圆弧MB、MA进行避障所需要的侧向加速度；
[0053] 最终的避障过载指令
[0054]
[0055] 最终避障速度指令选择与长机速度对齐指令
[0056]
[0057] 其中VL表示长机的速度；
[0058] 步骤三：基于行为的编队过程；
[0059] (1)行为分解过程：将任务行为分解为三个互相平行的子行为：异常情况处理，队形保持方法和威胁回避方法，同时为每个子行为赋予相应的重要性，用权值表示；队形保持方法中无人机通过无线通信网络获得长机的信息，具体包括长机速度VL、偏航角速度ωL和航迹偏角ΨL；无人机通过自身本机导航设备获得导航信息，具体包括速度VF、角速度ωF和偏角ΨF，按照步骤一公式(8)和公式(12)生成编队过载指令 和编队速度指令 避障方法中无人机通过
光学传感器获取障碍物的信息包括：无人机距离障碍物的距离R，速度矢量与无人机与障碍物连线的夹角为b，按照步骤二公式(17)和公式(18)生成避障过载指令 和避障速度指令 异常情况处理是指飞行器在执行飞行任务时，遇到通信链路中断等无法维持编队的原因，无人机飞控计算机进入异常处理状态，异常处理要保证无人机按照单机最优的策略执行任务或飞离搜索区域到达
指定位置准备降落，异常处理在执行任务中优先级最高，无人机按照单机最优的策略执行任务需要的过载指令和速度指令分别为[0060] (2)控制实现过程指无人机根据运动模型和当前运动状态，加权计算无人机实际的控制指令，进而输入执行机构：无人机的偏航通道产生方向
舵偏，从而实现对无人飞行器编队的运动控制；
[0061] 根据步骤一和步骤二中的队形保持方法和避障方法，各个子行为的过载指令和速度指令获得后按照公式(17)进行加权：
[0062]
[0063]
[0064] 其中wn0+wn1+wn2＝1.0，wV0+wV1+wV2＝1.0
[0065] 其中，wn0表示异常处理时过载指令权值、wV0表示异常处理时速度指令权值、wn1表示队形保持时过载指令权值、wV1表示队形保持时速度指令权值、wn2表示避障时过载指令权值、wV2表示避障时速度指令权值；
[0066] 1)符合异常处理的条件：wn0＝wV0＝1.0，wn1＝wV1＝wn2＝wV2＝0.0，这时队形保持和避障方法已经不再有效，无人机采取单机飞行的方式执行任务，或者沿指定导航点进入降落阶段；
[0067] 2)未符合异常处理条件时，队形保持方法与避障方法有效，按照光学传感器是否发现障碍物进行权值计算：
[0068]
[0069] 其中，wn2，wV2的取值取决于地面障碍物尺度与障碍物威胁范围的比值，wn1＝1.0-wn2，wV1＝1.0-wV2。
[0070] 本发明的优点在于：
[0071] (1)针对传统的虚拟结构方式编队控制对通信质量要求较高的缺点，引入了基于行为的编队控制方法，降低了对编队无线数据链更新率的要求，增强了无人机群编队的避障能力；
[0072] (2)针对传统基于行为方式的编队控制编队刚性保持不好的缺点，引入了虚拟结构作为参考，通过两种编队方法的融合，可以发挥各自方法的优点：在保持队形相对稳定的前提下，增强微小型无人机在未知环境下规避障碍物和威胁的能力，对于无人机群协同低空突防有一定借鉴意义的；
[0073] (3)针对实际微小型无人机提出的编队策略，采用的数学模型也是实际微小型无人机动力学与
运动学模型，不仅对
算法进行了验证，还具有很强的实际工程意义。
[0074] (4)为了验证编队方法与威胁回避策略，开发了实际仿真群体行为的
软件，该软件具有用户友好、便于操作与演示的优点，对于编队仿真软件的开发与国产化有一定的借鉴意义。
附图说明
[0075] 图1：本发明中长机、僚机和虚拟僚机在地面坐标系中位置关系图；
[0076] 图2：本发明中未采用队形保持方法无人机群初始位置及障碍物分布俯视图；
[0077] 图3：本发明中接入队形保持方法无人机群编队控制结果图；
[0078] 图4：本发明中无人机与地面障碍物位置关系图；
[0079] 图5：本发明中接入避障方法进行威胁回避的结果图；
[0080] 图6：本发明中基于行为的编队过程的流程示意图；
[0081] 图7：本发明中威胁回避之后重新编队的结果图；
[0082] 图8：本发明中实现上述仿真结果所利用的演示软件的示意图。
具体实施方式
[0083] 下面将结合附图对本发明作进一步的详细说明。
[0084] 本发明提出的一种多无人机的动态编队控制方法，包括以下几个步骤：
[0085] 步骤一：队形保持方法；
[0086] (1)建立地面坐标系XOY；
[0087] 建立地面坐标系XOY，如图1所示，其中X轴代表东向位置，Y轴代表北向位置。ML和MF分别代表编队长机和僚机，ΨL和ΨF分别代表长机和僚机的航迹偏角，MV代表虚拟结构设定的僚机，简称为虚拟僚机，L和a分别代表虚拟僚机对长机期望的距离和观测角，Ll和al分别代表实际僚机对长机的距离和观测角，存在的通信网络延迟为Δt，MVH和MFH分别表示虚拟僚机和实际僚机在Δt移动后的位置，所以无人机编队问题等同于MFH对MVH的跟踪问题，也即考虑网络延迟后的实际僚机对虚拟僚机的跟踪问题，其中MFH表示长机MF在通信网络延迟Δt内飞行的剖面距离，MVH表示虚拟僚机MV在通信网络延迟Δt内飞行的剖面距离。要求所使用无人机纵向能够处于稳定的定高飞行状态，无人机采用方向舵进行侧滑转弯，所以主升力面面积和飞行速度在飞行过程变化不大，耦合引起的飞行器高度损失能够用升降舵进行很好的补偿。
[0088] (2)建立各个关系公式；
[0089] 由于编队的长机僚机纵向均处于定高状态，采用的转弯方式均为侧滑转弯，故长机僚机速度偏差不大，一般偏差不超过10m时，所以设定长机和僚机在通信周期内移动距离均为l，根据图1中长机、僚机和虚拟僚机的几何关系建立长机ML和MVH的位置关系公式：
[0090]
[0091] 其中，xMVH、yMVH、xML、yML分别表示考虑通信延迟的虚拟僚机的东向位置、北向位置、长机的东向位置、北向位置。L表示虚拟僚机对长机期望的距离，ΨL表示长机的航迹偏角，a表示虚拟僚机对长机期望的观测角，l表示长机僚机在通信周期内移动距离。
[0092] 根据图1中长机、僚机和虚拟僚机的几何关系建立僚机MF和MFH的位置关系公式：
[0093]
[0094] 其中，xMFH、yMFH、xMF、yMF分别表示考虑通信延迟的实际僚机的东向位置、北向位置、实际僚机的东向位置、北向位置，ΨF表示僚机的航迹偏角。公式(2)减公式(1)可以建立MVH和MFH的跟踪误差关系公式：
[0095]
[0096] 其中，ex、ey分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差。
[0097] 对MVH和MFH的跟踪误差关系公式(3)进行求导，得到：
[0098]
[0099] 其中，VL、VF分别表示长机和僚机的速度，ωL、ωF分别表长机和僚机的偏航角速度， 分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差的导数。l表示长机僚机在通信周期内移动距离，L表示僚机对虚拟长机期望的距离，ΨL表示长机的航迹偏角，a表示虚拟长机期望的观测角，ΨF僚机的航迹偏角。
[0100] 将地面坐标系的ex、ey投影到僚机的速度坐标系，这个坐标系的定义为僚机的速度方向为X轴，与X垂直顺
时针选转90度为Y轴。
[0101]
[0102] 其中，ex、ey分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差， 分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差在僚机速度坐标系下的投影。
[0103] 对变换坐标后的公式(5)进行求导，得到：
[0104]
[0105] 其中 分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差的导数， 分别表示考虑通信延迟的虚拟僚机和实际僚机的东向位置、北向位置偏差在僚机速度坐标系下的投影的导数。
[0106] 联立公式(3)、公式(4)和公式(6)，得到：
[0107]
[0108] 由公式(7)可以看出，当僚机的偏航角速度和速度指令为公式(8)所示时，公式(7)可以化简为公式(9)
[0109]
[0110] 其中，k1，k2表示实际编队过程中待调整的控制律系数，实际编队中取值量级与ωF近似的正数。
[0111]
[0112] 考虑李雅普诺夫函数：
[0113]
[0114] 公式(9)代入公式(10)可得
[0115]
[0116] 由李雅普诺夫定律，对于给定的偏差 采用公式(8)的速度和角速度指令，编队队形偏差能够收敛为0。使得误差表达式 收敛为0。
[0117] 最终的编队过载指令 和编队速度指令
[0118]
[0119] 应用本发明中队形保持方法进行编队，未采用队形保持方法无人机群初始位置及障碍物分布俯视图如图2所示，编队结果俯视图如图3所示，其中0、1、2、3、4代表无人机，0号代表长机，1-4号代表实际无人机，实线的椭圆圈代表有一定威胁的区域，虚线椭圆圈代表无人机探测范围，从图中可以看出，应用本发明步骤一提出的队形保持方法，能够完成微小型无人机的编队任务。
[0120] 步骤二：避障方法；
[0121] 无人机在编队飞行的过程中，往往存在地面上的障碍物，例如对地观测搜索时的地面高压电线和楼群，如果这些障碍物在执行任务之前是已知的，并且在无人机编队飞行过程中障碍物不移动或移动的位置变化不大，那么在执行任务之前可以靠离线航路规划来回避地面已知障碍物。实际上侦查区域的障碍物一般是未知的，所以要求无人机编队对于未知或突然出现的障碍物有一定的回避能力。
[0122] 设地面障碍物在空间中服从均匀分布，如图4所示，M为无人机当前位置，V为无人机速度矢量，r为障碍物的威胁范围，R为无人机距离障碍物的距离。为保证无人机不进入威胁区，无人机可以沿左侧航迹MA飞行也可以沿右侧航迹MB飞行，设距无人机最近的威胁源为圆心O，理想威胁回避航迹为与圆O外切且与速度矢量V相切的两圆Ol、Or的圆弧MA和MB，定义切点A、B为威胁回避导航点。
[0123] 转弯半径应用解三角形的方法求取，在ΔOOlM中应用余弦定理得：
[0124]
[0125] 进一步得出无人机威胁回避的转弯半径：
[0126]
[0127]
[0128] 相应的侧向加速度的表达式为：
[0129]
[0130] 其中ar、al分别表示无人机沿圆弧MB、MA进行避障所需要的侧向加速度。
[0131] 最终的避障过载指令
[0132]
[0133] 在障碍物区域不是很大的前提下，最终避障速度指令选择与长机速度对齐指令[0134]
[0135] 其中VL表示长机的速度。
[0136] 采用步骤二中的避障方法进行威胁回避的结果如图5所示，其中实线长曲线表示无人机飞行轨迹，实线的椭圆圈代表有一定威胁的区域，虚线椭圆圈代表无人机探测范围。1-4号无人机可以采用步骤二中的避障方法进行威胁回避策，避开可能存在的地面威胁，由于编队的权值不完全为零，所以在回避威胁的情况下，基本的编队位置也能进一步维持，为威胁回避后的重新编队做好准备。
[0137] 步骤三：基于行为的编队过程；
[0138] 步骤一和步骤二给出了微小型无人机的编队方法和避障方法，实际编队飞行过程就是上述两种情况的
叠加，为了获得大面积对地观测往往要求飞行器进行编队飞行，当飞行器遇到前期航路规划未标明的障碍物时往往要求飞行器有临时躲避威胁的功能，采用基于行为的编队过程，将步骤一和步骤二的过程结合起来，具体分为两个步骤，如图6所示，分别为行为分解与控制实现。
[0139] (1)在行为分解过程中，无人机根据当前的环境信息和任务性质，将任务行为分解为三个互相平行的子行为：异常情况处理，队形保持方法和威胁回避方法，同时为每个子行为赋予相应的重要性，用权值表示。队形保持方法参考步骤一，无人机通过无线通信网络获得长机的信息，具体包括长机速度VL、偏航角速度ωL和航迹偏角ΨL；无人机通过自身本机导航设备获得导航信息，具体包括速度VF、角速度ωF和偏角ΨF，按照步骤一公式(8)和公式(12)生成编队过载指令 和编队速度指令 避障方法参考步骤二，无人机通过光学传感器获取障碍物的信息包括：无人机距离障碍物的距离R，速度矢量与无人机与障碍物连线的夹角为b，按照步骤二公式(17)和公式(18)生成避障过载指令 和避障速度指令异常情况处理是指飞行器在执行飞行任务时，遇到通信链路中断等无法维持编队的原因，无人机飞控计算机进入异常处理状态，异常处理要保证无人机按照单机最优的策略执行任务或飞离搜索区域到达指
定位置准备降落，异常处理在执行任务中优先级最高，无人机按照单机最优的策略执行任务需要的过载指令和速度指令分别为 例如沿直线飞行。
[0140] (2)控制实现过程指无人机根据运动模型和当前运动状态，加权计算无人机实际的控制指令，进而输入执行机构，无人机的偏航通道产生方向舵偏，从而实现对无人飞行器编队的运动控制。
[0141] 根据步骤一和步骤二中的队形保持方法和避障方法，各个子行为的过载指令和速度指令求出以后可以按照下式进行加权：
[0142]
[0143]
[0144] 其中wn0+wn1+wn2＝1.0，wV0+wV1+wV2＝1.0
[0145] 其中，wn0表示异常处理时过载指令权值、wV0表示异常处理时速度指令权值、wn1表示队形保持时过载指令权值、wV1表示队形保持时速度指令权值、wn2表示避障时过载指令权值、wV2表示避障时速度指令权值。
[0146] 1)符合异常处理的条件：wn0＝wV0＝1.0，wn1＝wV1＝wn2＝wV2＝0.0，这时队形保持和避障方法已经不再有效，无人机采取单机飞行的方式执行任务，或者沿指定导航点进入降落阶段；
[0147] 2)未符合异常处理条件时，队形保持方法与避障方法有效，按照光学传感器是否发现障碍物进行权值计算：
[0148]
[0149] 其中，wn1 wV1 wn2 wV2可根据实际情况适当调整，wn2、wV2的取值主要取决于地面障碍物尺度与障碍物威胁范围的比值，比值大说明障碍物尺度比较大，这时wn2，wV2取值要加大，防止与障碍物发生碰撞，否则wn2，wV2取值减小，wn1＝1.0-wn2，wV1＝1.0-wV2增大，增强*无人机的编队能力。wn2优选取值为wn2＝0.3，wn1＝0.7。本发明的生成指令n 为过载指令，故对于无法安装测量气流角的传感器的无人机有一定借鉴意义。
[0150] 如图7所示，在无人机探测不到威胁的情况下，重新恢复编队队形，进行编队飞行，其中实线长曲线表示无人机飞行轨迹，实线的椭圆圈代表有一定威胁的区域，虚线椭圆圈代表无人机探测范围。1-4号无人机可以采用步骤三中的基于行为的编队方法进行队形保持与威胁回避，在避开可能存在的地面威胁的前提下保持队形的刚性。图8为实现上述仿真结果所利用的演示软件的示意图，国内目前现有的群体编队仿真软件往往不便于演示，而该软件具有用户友好、便于操作与演示的优点，其软件架构对于编队仿真软件的开发与国产化有一定的借鉴意义。
