一种仿生智能多无人机集群自主编队导航控制方法及装置

1.本发明涉及多无人机自主导航与协同控制技术领域，特别是指一种仿生智能多无人机集群自主编队导航控制的方法及装置。


背景技术：

2.针对强干扰、高动态的集群作战环境，多无人机集群自主编队需要实时、复杂的机间信息交互，对单机通信带宽要求较高，同时可能面临机间局部通信无法建立、gps精度低等问题，此外多无人机系统应满足分布式集群编队需求，因此设计合理高效的多无人机集群自主编队导航与控制方法至关重要。
3.目前，多无人机集群编队过程中常用的相对导航技术包括惯性导航、差分gps(global positioning system, gps)等，其中惯性导航存在随着时间不断累加误差的缺陷；gps技术虽然使用起来非常方便，但是由于其依靠卫星来定位，精度和抗干扰能力有待加强，且针对多无人机系统需要实时获取相邻无人机的位置、姿态等信息，对机间通信要求较高。由于复杂作战环境对电磁信号的限制、gps信号精度与实时性的不足，故传统导航方法无法实现不同无人机间的实时、高精度相对位置获取，同时多无人机集群编队任务需要设计分布式的编队控制，以提高多无人机系统的自愈性、自主性，增强集群作战效能。


技术实现要素：

4.本发明实施例提供了一种仿生智能多无人机集群自主编队导航控制方法及装置。针对现有多无人机集群编队导航与控制方法在近距相对导航与分布式编队控制上可行性、实时性以及精确性等方面的不足，并且考虑低频低精度gps与视觉相对导航共用、无需建立复杂的机间通信等具体问题，基于仿生智能设计了多无人机集群自主编队导航控制装置及其方法。技术方案如下：一方面，提供了一种仿生智能多无人机集群自主编队导航控制方法，该方法包括：s1：通过多无人机底层控制模块，建立四旋翼无人机六自由度非线性模型，并进行参数初始化；进行无人机控制分配及电机动态模拟仿真；更新多无人机状态量信息，计算仿真时间，其中，，其中，为仿真步长;s2：通过多无人机相对位置导航模块，获取多无人机集群自主编队的导航图像，进行候选无人机检测，并进行杂点滤除，得到感兴趣区域roi区，通过设定无人机的先验标志点，对roi区进行干扰区域滤除，得到剩余roi区；以及通过判断剩余roi区中是否存在先验标志点，判断是否检测到无人机；s3：相对位置导航模块对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人
机的相对位置，对多无人机进行仿鹳雁群集群编队；s4：通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导
‑
跟随关系及编队目标位置；s5：通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制；s6：通过多无人机底层控制模块，对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否结束多无人机集群自主编队；若大于最大仿真时间，则结束仿真并绘制多无人机集群自主编队过程数据曲线，否则重复执行s1
‑
s6。
5.可选地，建立四旋翼无人机六自由度非线性模型，并进行参数初始化，包括：其中， 分别为无人机惯性系下
三个方向的位置与速度、滚转角、俯仰角、偏航角以及机体轴三个方向的旋转角速度；为相应变量的一阶微分；为无人机螺旋桨产生的总拉力；为无人机质量，为重力加速度；分别为三轴转动惯量； 为陀螺力矩；为螺旋桨转速；为电机转子和螺旋桨绕转轴的总转动惯量；为无人机螺旋桨在机体轴上产生的力矩。
6.可选地，通过多无人机相对位置导航模块，获取多无人机集群自主编队的导航图像，进行候选无人机检测，并进行杂点滤除，得到感兴趣区域roi区，通过设定无人机的先验标志点，对roi区进行干扰区域滤除，得到剩余roi区；，包括：s21：利用仿猛禽视觉颜色双拮抗机制,构建多无人机集群自主编队的标志区及标志点检测方法，提取多无人机集群编队近距导航图像中不同波段信号信息，获取多无人机集群自主编队的导航图像；通过在长波、中波、短波通路中分别保持红色、绿色和蓝色成分灰度值对比度并削弱其它颜色成分灰度值对比度来构建仿猛禽视觉颜色双拮抗机制模型，通过公式（4）来检测多无人机集群编队标志区中红色、绿色、蓝色信息：其中，分别为长波、中波、短波以及紫外波段四个通路对图像的输出响应；分别为输入可见光导航图像中三个颜色通道的灰度值；分别为输入可见光导航图像中三个颜色通道的灰度值；分别为长波、中波、短波阈值；表示取与的最大值；表示取与的最小值；将rgb颜色空间信息转换到hsv颜色空间后，通过公式（5）检测无人机集群编队标志区的紫色、青色标志点：
其中，h、s、v分别为hsv空间的三个分量；分别为三个分量的最大值和最小值；s22：计算仿猛禽视觉双拮抗模型长波通道输出，并对导航图像进行二值化处理，得到二值图像；s23：对二值图像进行孔洞填充、滤除杂点，得到剩余roi区；所述剩余roi区为集群编队候选无人机。
7.可选地，通过判断剩余roi区中是否存在先验标志点，判断是否检测到无人机，包括：通过多无人机相对位置导航模块进行干扰区域滤除后，判断是否存在剩余roi区，若不存在剩余roi区，则确定无有效无人机；若存在剩余roi区，则进一步判断剩余roi区内是否存在先验标志点；若剩余roi区内没有先验标志点，确定无有效无人机；若存在先验标志点，则确定检测到有效无人机。
8.可选地，对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队，包括：s31：若检测到无人机，则通过仿猛禽颜色检测和hsv方法，提取剩余roi区内的先验标志点，计算检测到的单个无人机连通域内的先验标志点种类及像素坐标，进而判断剩余roi区是否只有一种颜色的标志点；s32：若只有一种颜色的标志点，则为非重叠无人机，直接进行轮廓分块圆拟合，选取最优拟合结果，并进行位姿估计；若非一种颜色的标志点，则为重叠无人机，需确定重合无人机标志区的分割线，进行多机分块圆拟合，选取最优拟合结果，通过位姿估计得到相机系下的相对位置关系；计算各颜色标志点的像素坐标中心与，以及各颜色标志点平均像素点数与，进而根据如下公式（8）计算连线上重合标志球的分割点
其中，为连线的分割比例系数；为面积比例因子;得到重合标志球分割点后，然后计算与连线垂直且过点的分割线以及斜率其中斜率为：计算重合标志球分割线各点像素坐标，设置导航图像分辨率为计算重合标志球分割线各点像素坐标，设置导航图像分辨率为分别为导航图像两个方向的像素坐标最大值，根据分割线斜率，计算分割线所有像素点坐标点集，计算分割线所有像素点坐标点集其中，取1到，并分别计算对应的；s33：通过位姿估计/相对位置关系为多无人机集群自主编队系统提供导引信息，进行多无人机进行仿鹳雁群集群编队。
9.可选地，通过仿猛禽颜色检测和hsv方法，提取剩余roi区内的先验标志点，包括：将二值图像与导航图像进行相与操作，得到只包含roi区的彩色图像，进而在roi彩色区内通过仿猛禽颜色检测和hsv方法，提取蓝色、绿色、紫色、青色先验标志点。
10.可选地，通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导
‑
跟随关系及编队目标位置，包括：s41：设置多无人机期望的初始目标高度，将每个无人机控制为高度一致；s42：每个无人机通过地面站每1s获取一次低频低精度的无人机gps位置，并综合所有无人机的前向位置信息，判断待选定无人机是否位于所有无人机的最前方；s43：若待选定无人机位于最前方，则确定待选定无人机为总领导无人机；若待选定无人机位于非最前方且没有检测到其它无人机，则待选定无人机为跟随无人机，需要寻找领导无人机，并使所述领导无人机出现在视觉导航视场内；s44：若检测到其它无人机，则多个跟随无人机的集群编队计算检测到无人机的位
置距离，根据位置距离确定领导
‑
跟随关系，进而得到跟随左侧或右侧集群的编队目标位置；经过仿猛禽视觉相对导航获取检测到无人机的相对位置，根据如下公式（11）计算仿鹳雁群集群编队位置距离集如下公式（11）计算仿鹳雁群集群编队位置距离集其中，为侧向比例因子，的取值范围与检测到无人机数量一致；当前无人机跟随检测到的距离最近的相对领导无人机，将距离集中的元素按从小到大的顺序排列，并选取距离最小的无人机作为领导无人机，即确定领导
‑
跟随关系，记与领导无人机的相对位置为。
11.可选地，通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制，包括：s51：计算定直平飞位置、编队目标位置、寻找领导者目标位置与无人机当前位置的差值；将仿猛禽视觉导航输出的领导无人机的相对位置与期望相对位置做差，得到目标位置的差做差，得到目标位置的差s52：计算每个无人机间的相对距离，当相对距离过小时，无人机以一定的速度降低高度，避免碰撞；s53：根据高度方向目标位置差，得到方向的无人机底层控制速度指令；根据以下公式（13）构建多无人机高度相对位置控制：其中，为侧向速度指令比例系数；s54：根据侧向目标位置差，得到方向的无人机底层控制速度指令；根据以下公式（14）构建多无人机侧向相对位置控制：其中，为侧向速度指令比例系数；s55：根据是否为总领导无人机或未检测其它无人机的跟随者，确定基本飞行速度；根据前向目标位置差，计算得到x方向的无人机底层控制速度指令；
根据以下公式（15）构建多无人机前向相对位置控制：其中，为前向速度指令比例系数；将相对位置控制得到的速度控制指令输入内环多无人机底层控制。
12.一方面，提供一种仿生智能多无人机集群自主编队导航控制装置，该装置应用于上述任意一项的方法，包括：多无人机底层控制模块，用于建立四旋翼无人机六自由度非线性模型，并进行参数初始化；进行无人机控制分配及电机动态模拟仿真；更新多无人机状态量信息，计算仿真时间，其中，，其中，为仿真步长;对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否结束多无人机集群自主编队；若大于最大仿真时间，则结束仿真并绘制多无人机集群自主编队过程数据曲线；多无人机相对位置导航模块，用于获取多无人机集群编队导航图像，进行仿猛禽视觉候选无人机检测，并进行杂点滤除得到感兴趣区域roi区，通过先验标志点对roi区进行干扰区域滤除，得到剩余roi区；判断干扰区域滤除后，是否存在剩余roi区，以及通过判断剩余roi区中是否存在先验标志点，判断是否检测到无人机；对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队；多无人机集群自主编队控制模块，用于对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导
‑
跟随关系及编队目标位置；多无人机相对位置控制模块，用于计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制。
13.可选地，多无人机底层控制模块，包括与多无人机数量一一对应的电机动态监测子模块以及控制分配子模块；所述控制分配子模块包括对无人机的横侧向速度控制以及纵向速度控制。
14.本发明实施例提供的技术方案带来的有益效果至少包括：上述方案中，一、本发明提供了完整多无人机集群自主编队视觉导航与控制系统框架及其工作流程，系统框架以及流程合理、高效，能够满足多无人机集群自主编队导航与控制性能指标；二、本发明模仿猛禽颜色视觉机制构建仿猛禽颜色双拮抗检测方法，提高了无人机标志球及标志点的检测效率及准确性；三、本发明给出了两种相对导航的组合方法，解决了标志球部分遮挡、单一无人机、重合无人机等情况下的近距视觉相对导航问题且视觉导航精度较高，能够实现集群编
队全过程的视觉导航，提高了仿猛禽视觉导航的适应性；四、本发明基于仿猛禽视觉导航设计了仿鹳雁群迁徙行为的集群自主编队控制方法，能够实现多无人机的分布式编队，提高了集群编队方法的自主性与自愈性。
附图说明
15.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
16.图1是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的整体步骤图；图2是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的干扰区滤除步骤图；图3是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的标志球与标志点示意图；图4是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的标志球分块拟合示意图；图5是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的仿鹳雁群集群编队步骤图；图6是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的标志球重合分割示意图；图7是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的外接矩形顶点匹配示意图；图8是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的领导
‑
跟随关系确定步骤图；图9是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的无人机集群方位控制步骤图；图10是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的自主编队场景图；图11a
‑
11c是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的实时位置数据曲线；图12是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制装置的系统框架图。
具体实施方式
17.为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。
18.本发明实施例提供了一种仿生智能多无人机集群自主编队导航控制方法，如图1所示，为本发明的仿生智能多无人机集群自主编队导航控制方法整体步骤图，包括：
s1：通过多无人机底层控制模块，建立四旋翼无人机六自由度非线性模型，并进行参数初始化；进行无人机控制分配及电机动态模拟仿真；更新多无人机状态量信息，计算仿真时间，其中，，其中，为仿真步长;s2：通过多无人机相对位置导航模块，获取多无人机集群自主编队的导航图像，进行候选无人机检测，并进行杂点滤除，得到roi区 (region of interest，感兴趣区域)，通过设定无人机的先验标志点，对roi区进行干扰区域滤除，得到剩余roi区；以及通过判断剩余roi区中是否存在先验标志点，判断是否检测到无人机；s3：相对位置导航模块对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队；s4：通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导
‑
跟随关系及编队目标位置；s5：通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制；s6：通过多无人机底层控制模块，对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否结束多无人机集群自主编队；若大于最大仿真时间，则结束仿真并绘制多无人机集群自主编队过程数据曲线，否则重复执行s1
‑
s6。
19.在本实施例中，本发明提出的多无人机集群自主编队协同作战方案是弥补单一无人机感知、机动、决策等能力不足，提高多无人机系统鲁棒性、自主性、智能性，增强系统任务执行能力及作战效能的有效途径。通过构建多无人机集群自主编队方法及装置，提高了多无人机系统近距相对导航的可靠性和精确性；并且本发明的设计适用于多无人机的分布式编队控制方法，从而提高多无人机集群自主编队近距视觉导航与编队控制的精度，促进多无人机集群作战的顺利实现，提高多无人机系统的自主能力水平。
20.本发明采用视觉相对导航技术通过提取导航图像的目标特征点并进行位姿测量，从而为多无人机集群自主编队提供导引信息。相对于其他导航系统，视觉传感器具有体积小、重量轻、低功耗等特点，同时视觉导航系统的工作波段远离当前电磁对抗的频率范围，抗电磁干扰性能强，近距测量多无人机位姿信息精度较高。此外，相对传统编队方法，仿生编队具有编队形式灵活多变、环境适应与自学习能力强、对抗作战重构性能好等优势。鉴于视觉相对导航与仿生编队既能满足多无人机集群自主编队过程中的导航精度、分布式编队要求，又能处理机间通信无法建立及gps精度低等特殊情况，本发明提出了一种基于仿生智能的多无人机集群自主编队导航控制系统及其方法，以解决现有多无人机集群编队近距视觉相对导航位姿精确测量与分布式编队鲁棒性和适用性方面的不足，有效提高多无人机系统集群自主编队控制水平。
21.建立四旋翼无人机六自由度非线性模型，并进行参数初始化，包括：
其中，分别为无人机惯性系下三个方向的位置与速度、滚转角、俯仰角、偏航角以及机体轴三个方向的旋转角速度；为相应变量的一阶微分；为无人机螺旋桨产生的总拉力；为无人机质量，为重力加速度；分别为三轴转动惯量；为陀螺力矩；为螺旋桨转速；为电机转子和螺旋桨绕转轴的总转动惯量；为无人机螺旋桨在机体轴上产生的力矩。
22.在本实施例中，公式（2）的四旋翼无人机六自由度非线性模型无需推导直接给出：给定参数无人机质量，转动惯量、，机体中心和任一电机的距离
，电机拉力系数，电机转矩系数，电机转矩系数，重力加速度。其中，为陀螺力矩，其计算公式如下：为陀螺力矩，其计算公式如下：为螺旋桨转速；为电机转子和螺旋桨绕转轴的总转动惯量；计算公式为：其中，为机体中心和任一电机的距离，分别无量纲的电机拉力系数和转矩系数。
23.本实施例中，还需要针对步骤s1建立的四旋翼无人机六自由度模型，设计纵向速度pid控制器：其中，为期望拉力；分别为纵向速度控制比例、积分、微分系数；为纵向速度指令值；为纵向速度误差。
24.设计横侧向速度pid控制器，具体如下：
其中，分别为滚转角与俯仰角的期望值；分别为前向速度控制比例、积分、微分系数；分别为侧向速度控制比例、积分、微分系数；分别为前向、侧向速度指令值；分别为前向、侧向速度误差；为转换矩阵。
25.进而设计内环姿态角pid控制器，具体如下：进而设计内环姿态角pid控制器，具体如下：进而设计内环姿态角pid控制器，具体如下：进而设计内环姿态角pid控制器，具体如下：其中，分别为机体轴三个方向的旋转角速度期望值；分别为三轴旋转角速度比例系数；分别为滚转角、俯仰角、偏航角误差；分别为三轴期望力矩；分别为三轴力矩比例系数；分别为三轴力矩积分系数；分别为三轴力矩微分系数；分别为三轴旋转角速度误差，分别为相应变量的一阶微分。
26.利用控制器得到的期望拉力与期望三轴力矩，根据四旋翼无人机控制效率矩阵，进行控制分配：其中，分别为期望电机转速。
27.四旋翼实际转速电机动态，如下式所示：其中，为电机动态响应时间，为复常量。
28.利用得到的电机实际转速，根据公式(2)的无人机模型更新无人机各个状态量。
29.初始化无人机数量、初始位置、定直平飞速度等参数，更新多无人机状态量信息，计算当前的仿真时间（为仿真步长）。
30.多无人机底层控制相关参数选择为：、、、如图2所示，为本发明的干扰区滤除步骤图。通过多无人机相对位置导航模块，获取多无人机集群自主编队的导航图像，进行候选无人机检测，并进行杂点滤除，得到感兴趣区域roi区，通过设定无人机的先验标志点，对roi区进行干扰区域滤除，得到剩余roi区；包括：s21：利用仿猛禽视觉颜色双拮抗机制,构建多无人机集群自主编队的标志区及标志点检测方法，提取多无人机集群编队近距导航图像中不同波段信号信息，获取多无人机集群自主编队的导航图像；通过在长波、中波、短波通路中分别保持红色、绿色和蓝色成分灰度值对比度并削弱其它颜色成分灰度值对比度来构建仿猛禽视觉颜色双拮抗机制模型，通过公式（3）来检测多无人机集群编队标志区中红色、绿色、蓝色信息：
其中，分别为长波、中波、短波以及紫外波段四个通路对图像的输出响应；分别为输入可见光导航图像中三个颜色通道的灰度值；分别为输入可见光导航图像中三个颜色通道的灰度值；分别为长波、中波、短波阈值；表示取与的最大值；表示取与的最小值；将rgb颜色空间信息转换到hsv颜色空间后，通过公式（4）检测无人机集群编队标志区的紫色、青色标志点：其中，h、s、v分别为hsv空间的三个分量；分别为三个分量的最大值和最小值；s22：计算仿猛禽视觉颜色双拮抗模型长波通道输出，并对导航图像进行二值化处理，得到二值图像；s23：对二值图像进行孔洞填充、滤除杂点，得到剩余roi区；所述剩余roi区为集群编队候选无人机。
31.在本实施例中，无人机集群编队标志区设计为红色圆球并在表面分别添加蓝色、绿色、紫色、青色标志点，标志点在球最大半径的水平面每60
°
添加一个，如图3所示。
32.利用仿猛禽视觉颜色双拮抗机制构建集群编队标志区及标志点检测方法，提取多无人机集群编队近距导航图像中不同波段信号信息。长波通路主要吸收彩色(red
‑
green
‑
blue, rgb)图像中的红色信号，并对其它颜色进行抑制，rgb图像中r通道的灰度值占r、g、b三通道灰度值之和的比例越大，长波通路的响应越大；同样地，中波通路、短波通路分别吸收彩色图像中的绿色与蓝色信号，并对其它颜色信号进行抑制；紫外通路吸收图像中的亮度信号。通过在长波、中波、短波通路中分别保持红色、绿色和蓝色成分灰度值对比度并削
弱其它颜色成分灰度值对比度来构建仿猛禽视觉颜色双拮抗机制模型，通过公式（3）来检测多无人机集群编队标志区中红色、绿色、蓝色信息。
33.根据公式（4）进行颜色空间转换，在hsv空间内根据集群编队标志点颜色类型（紫色或青色）及深浅等先验信息对h和s两个通道的阈值进行设置，可实现对集群编队标志点的有效分割。
34.在本实施例中，在得到二值化图像后，需要对二值图像进行孔洞填充、滤除杂点：如图4所示，将roi区域边缘分成四部分，对每一部分采用最小二乘法进行圆拟合，定义圆拟合度：其中，表示二值图像中的第个候选无人机，表示第个候选无人机边缘的第部分表示第部分拟合像素点数；表示第部分的个像素点在拟合圆上的像素点数，其计算公式如下：其中，为第部分的拟合像素点；为拟合圆的中心像素坐标；为拟合圆的半径；为判断拟合结果的阈值。从候选无人机的边缘拟合结果中选取最优拟合作为最终用于位姿估计的拟合圆，以降低圆拟合误差，解决标志区部分遮挡的问题。此外，对于候选无人机干扰区，选取圆拟合度的区分阈值，进行干扰区域滤除，若，能够说明拟合像素点基本不在拟合圆上，干扰区域为非圆，需要滤除以降低环境干扰影响。
35.本实施例中，其它仿真参数设置为：、紫色分割色度阈值范围130~155、紫色分割饱和度阈值范围111~255、青色分割色度阈值范围80~95、青色分割饱和度阈值范围111~255。
36.通过判断剩余roi区中是否存在先验标志点，判断是否检测到无人机，包括：通过多无人机相对位置导航模块进行干扰区域滤除后，判断是否存在剩余roi区，若不存在剩余roi区，则确定无有效无人机；若存在剩余roi区，则进一步判断剩余roi区内是否存在先验标志点；若剩余roi区内没有先验标志点，确定无有效无人机；若存在先验标志点，则确定检测到有效无人机。
37.在本实施例中，若存在剩余roi区，将二值化图像与原图像进行相与操作得到只包
含roi区的彩色图像，进而在roi彩色区内通过仿猛禽颜色检测和hsv方法，提取蓝色、绿色、紫色、青色先验标志点，如果roi彩色区内没有先验标志点，则确定无有效无人机，则进行多无人机高度控制操作；若检测到无人机，通过仿猛禽颜色检测和hsv方法，可提取无人机roi彩色区内蓝色、绿色、紫色、青色先验标志点，并统计检测到的单个无人机连通域内的标志点种类及像素坐标，进而判断无人机roi彩色区是否只有一种颜色的标志点。
38.本实施例中，通过模仿猛禽颜色视觉机制，构建仿猛禽颜色双拮抗检测方法，提高了无人机标志球及标志点的检测效率及准确性。
39.如图5所示，为本发明的仿鹳雁群集群编队步骤图。对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队，包括：s31：若检测到无人机，则通过仿猛禽颜色检测和hsv方法，提取剩余roi区内的先验标志点，计算检测到的单个无人机连通域内的先验标志点种类及像素坐标，进而判断剩余roi区是否只有一种颜色的标志点；s32：若只有一种颜色的标志点，则为非重叠无人机，直接进行轮廓分块圆拟合，选取最优拟合结果，并进行位姿估计；若非一种颜色的标志点，则为重叠无人机，需确定重合无人机标志球的分割线，进行多机分块圆拟合，选取最优拟合结果，通过位姿估计得到相机系下的相对位置关系；计算各颜色标志点的像素坐标中心与，以及各颜色标志点平均像素点数与，进而根据如下公式（8）计算连线上重合标志球的分割点：其中，为连线的分割比例系数；为面积比例因子;得到重合标志球分割点后，然后计算与连线垂直且过点的分割线以及斜率，如图6所示；其中斜率为：计算重合标志球分割线各点像素坐标，设置导航图像分辨率为计算重合标志球分割线各点像素坐标，设置导航图像分辨率为分别为导航图像两个方向的像素坐标最大值，根据分割线斜率，计算分割线所有像素点坐标点集：
其中，取1到，，并分别计算对应的；s33：通过位姿估计/相对位置关系为多无人机集群自主编队系统提供导引信息，进行多无人机进行仿鹳雁群集群编队。
40.在本实施例中，若检测到无人机且roi区只有一种标志点，根据步骤s2中的轮廓分块圆拟合结果，选择最优拟合圆，进而可得最优拟合圆的外接矩形顶点集合分别对应外接矩形四个顶点如图7所示，根据集群编队标志球实际尺寸的外接矩形对四个顶点像素点进行顺序匹配。
41.对匹配正确的所有标志点，利用算法进行精确位姿测量，得到相机坐标系下的无人机位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到与前方无人机在地面惯性系下的位置差，然后利用仿猛禽相对导航得到的相对位置差进行后续的仿鹳雁群集群编队。
42.在本实施例中，设置导航图像分辨率为。
43.本实施例中，给出了两种相对导航的组合方法，解决了标志球部分遮挡、单一无人机、重合无人机等情况下的近距视觉相对导航问题且视觉导航精度较高，能够实现集群编队全过程的视觉导航，提高了仿猛禽视觉相对导航的适应性。
44.如图8所示，为本发明的领导
‑
跟随关系确定步骤图。通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导
‑
跟随关系及编队目标位置，包括：s41：设置多无人机期望的初始目标高度，将每个无人机控制为高度一致；s42：每个无人机通过地面站每1s获取一次低频低精度的无人机gps位置，并综合所有无人机的前向位置信息，判断待选定无人机是否位于所有无人机的最前方；s43：若待选定无人机位于最前方，则确定待选定无人机为总领导无人机；若待选定无人机位于非最前方且没有检测到其它无人机，则待选定无人机为跟随无人机，需要寻找领导无人机，并使所述领导无人机出现在视觉导航视场内；s44：若检测到其它无人机，则多个跟随无人机的集群编队计算检测到无人机的位置距离，根据位置距离确定领导
‑
跟随关系，进而得到跟随左侧或右侧集群的编队目标位置；经过仿猛禽视觉相对导航获取检测到无人机的相对位置，根据如下公式（11）计算仿鹳雁群集群编队位置距离集：其中，为侧向比例因子，在本实施例中，的取值范围与检测到无人机数量一致；当前无人机跟随检测到的距离最近的相对领导无人机，将距离集中的元素按
从小到大的顺序排列，并选取距离最小的无人机作为领导无人机，即确定领导
‑
跟随关系，记与领导无人机的相对位置为。
45.在本实施例中，为降低前方无人机超出仿猛禽视觉相对导航视场的情况，设置多无人机期望的初始目标高度，使所有无人机先通过高度控制实现高度一致。每个无人机通过地面站每1s获取一次低频低精度的各个无人机gps位置，并综合所有无人机的前向位置信息，比较判断当前无人机是否位于所有无人机的最前方。定义前向位置控制标志位及基础飞行速度，若当前无人机位于最前方，当前无人机为总领导无人机，以的速度做定直平飞，并令；若当前无人机位于非最前方且没有检测到其它无人机，当前无人机需要寻找领导无人机使其出现在视觉导航视场内，无人机保持前向速度的同时，先通过侧向位置控制向中心位置靠拢，并令，过程中如果检测到其它无人机，则进行计算仿鹳雁群集群编队位置距离，并确定领导
‑
跟随关系及编队目标位置，如果依旧没有检测到其它无人机，当前无人机以的速度加速飞行，并令，直至寻找到领导无人机或成为新的领导无人机。
46.在本实施例中，若当前无人机跟随检测到的距离最近的领导无人机，将距离集中的元素按从小到大的顺序排列，并选取距离最小的无人机作为领导无人机即确定领导
‑
跟随关系，记与领导无人机的相对位置为。然后定义期望相对位置为，根据领导与跟随无人机的相对位置关系确定集群编队目标位置，若，当前无人机跟随领导者的左侧跟随位置；若，当前无人机跟随领导者的右侧跟随位置。
47.本实施例中，基于猛禽视觉导航设计了仿鹳雁群迁徙行为的集群自主编队控制方法，能够实现多无人机的分布式编队，提高了集群编队方法的自主性与自愈性。
48.如图9所示，为本发明的无人机集群方位控制步骤图。通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制，包括：s51：计算定直平飞位置、编队目标位置、寻找领导者目标位置与无人机当前位置的差值；将仿猛禽视觉相对导航输出的领导无人机的相对位置与期望相对位置做差，得到目标位置的差：s52：计算每个无人机间的相对距离，当相对距离过小时，无人机以一定的速度降
低高度，避免碰撞；s53：根据高度方向目标位置差，得到z方向的无人机底层控制速度指令；根据以下公式（13）构建多无人机高度相对位置控制：其中，为侧向速度指令比例系数；s54：根据侧向目标位置差，得到方向的无人机底层控制速度指令；根据以下公式（14）构建多无人机侧向相对位置控制：其中，为侧向速度指令比例系数；s55：根据是否为总领导无人机或未检测其它无人机的跟随者，确定基本飞行速度；根据前向目标位置差，计算得到方向的无人机底层控制速度指令；根据以下公式（15）构建多无人机前向相对位置控制：其中，为前向速度指令比例系数；将相对位置控制得到的速度控制指令输入内环多无人机底层控制，在本实施例中，。
49.本实施例中，在构建无人机集群的高度、侧向以及前向相对位置控制后，还需要进行多无人机集群编队的避碰设计。
50.综合利用低频低精度gps信息与仿猛禽视觉相对导航相对位置数据，当相邻无人机间的距离小于时，给定高度方向速度，无人机降低高度以实现不同个体间的避碰。
51.在本实施例中，当相邻无人机间的距离小于时，给定高度方向速度，无人机降低高度以实现不同个体间的避碰。最终编队场景如图10所示。
52.最后，判断无人机集群编队程序是否结束：根据公式（1）若大于最大仿真时间，则结束仿真并绘制多无人机集群自主编队过程数据曲线;若小于最大仿真时间，则重复执行步骤s1
‑
s6。
53.在本实施例中，若大于最大仿真时间，则结束仿真并绘制多无人机集群自主编队过程数据曲线，数据曲线如图11a
‑
11c所示。
54.本发明还提供一种多无人机集群自主编队导航控制装置，该装置应用于如上述方法，包括：多无人机底层控制模块，用于建立四旋翼无人机六自由度非线性模型，并对无人机数量、初始位置、仿真时间以及平飞速度的参数进行初始化；并进行无人机控制分配及电机动态模拟仿真；更新多无人机状态量信息，计算仿真时间，其中，
为仿真步长;对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否结束多无人机集群自主编队；若大于最大仿真时间，则结束仿真并绘制多无人机集群自主编队过程数据曲线；多无人机相对位置导航模块，用于获取多无人机集群编队导航图像，进行仿猛禽视觉的候选无人机检测，并进行杂点滤除得到感兴趣区域roi区，通过先验标志点对roi区进行干扰区域滤除，得到剩余roi区；判断干扰区域滤除后，是否存在剩余roi区，以及通过判断剩余roi区中是否存在先验标志点，判断是否检测到无人机；对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队；多无人机集群自主编队控制模块，用于对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导
‑
跟随关系及编队目标位置；多无人机相对位置控制模块，用于计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制。
55.多无人机底层控制模块，包括与多无人机数量一一对应的电机动态监测子模块以及控制分配子模块；控制分配子模块包括对无人机的横侧向速度控制以及纵向速度控制。
56.本发明的一种仿生智能多无人机集群自主编队导航控制装置，旨在通过设计一种基于仿生智能的多无人机集群编队导航控制系统，提高复杂情况下多无人机系统集群编队视觉导航与协同控制的精度，促进多无人机自主集群作战的成功实现。
57.本实施例中，如图12所示，为本发明的装置框图。多无人机相对位置导航模块，包括仿猛禽视觉无人机检测、是否检测到无人机与重合情况判断、单一无人机相对导航、重合无人机相对导航及坐标系转换。仿猛禽视觉无人机检测，获取集群编队视觉导航图像，进行仿猛禽视觉的候选无人机检测，并以形状特征与编制点特征滤除干扰区域；是否检测到无人机与重合情况判断，根据滤除干扰后的导航图像判断是否检测到无人机，分别对应集群自主编队控制跟随无人机两种情况，并且检测到无人机时，需判断检测到的单个标志区内是否只有一种颜色标志点，以区分非重合无人机相对导航与重合无人机相对导航两种情况；若只有一种颜色标志点，非重合无人机相对导航对标志区轮廓分块进行圆拟合，选取最优拟合结果，并进行位姿估计；若不是只有一种颜色标志点，重合无人机相对导航先确定重合标志区分割线，再进行多机分块圆拟合，选取最优拟合结果，然后通过位姿估计得到相机系下的相对位置关系；坐标系转换将相机系下的无人机位置先转换到机体系再转换到惯性系，为多无人机集群自主编队系统提供导引信息。
58.多无人机集群自主编队控制模块，包括确定总领导无人机、总领导无人机自主运动、跟随无人机寻找领导者、仿鹳雁群的跟随无人机集群编队。确定总领导无人机，获取地面站低频多无人机gps位置，根据多无人机相对位置关系，确定当前无人机是否为总领导者；若当前无人机为总领导无人机，该无人机到达一致高度后，以的速度做定直平飞；若
当前无人机不是总领导无人机，在到达一致高度后，如果未检测到其它无人机，该无人机先向中心位置靠拢，以提高检测到其它无人机的几率，若到达中心位置附近且仍未检测到其他无人机，以他无人机，以的速度快速飞行直至检测到其它无人机或称为总领导者；如果检测到其它无人机，仿鹳雁群的跟随无人机集群编队先计算检测到无人机编队位置距离计算，再根据距离规则确定领导
‑
跟随关系，进而得到跟随左侧或右侧集群编队目标位置。
59.多无人机相对位置控制模块，包括计算目标位置差、避碰控制、高度控制、侧向位置控制、前向位置控制。计算目标位置差，具体计算定直平飞位置、编队目标位置、寻找领导者目标位置与无人机当前位置的差值；避碰控制，计算不同无人机间的相对距离，当相对距离过小时，无人机以一定的速度降低高度，避免碰撞；高度控制，根据高度方向目标位置差，得到方向的无人机底层控制速度指令；侧向位置控制，根据侧向目标位置差，得到方向的无人机底层控制速度指令；前向位置控制，一种情况根据是否为总领导无人机或未检测其它无人机的跟随者，确定基本飞行速度，另一种情况根据前向目标位置差，计算得到方向的无人机底层控制速度指令。
60.多无人机底层控制模块，包括纵向速度控制、横侧向速度控制、姿态角控制、控制分配、电机动态。纵向速度控制，根据方向速度误差，设计纵向速度pid控制器，解算得到期望的四旋翼无人机总拉力；横侧向速度控制，根据方向速度误差，分别设计横侧向速度pid控制器，阶段得到四旋翼无人机姿态角指令；姿态角控制，计算外环姿态角指令值与实际值的误差，设计姿态角pid控制器，解算得到无人机三轴期望力矩；控制分配，根据四旋翼控制效率矩阵，计算得到四旋翼各个电机的期望转速；电机动态，考虑电机的动态特性，解算得到电机的实际转速，以实时更新无人机的所有状态量信息。
61.本发明提供了完整多无人机集群自主编队视觉导航与控制系统框架及其工作流程，系统框架以及流程合理、高效，能够满足多无人机集群自主编队导航与控制性能指标；本发明通过模仿猛禽颜色视觉机制，构建仿猛禽颜色双拮抗检测方法，提高了无人机标志球及标志点的检测效率及准确性；本发明给出了两种相对导航的组合方法，解决了标志球部分遮挡、单一无人机、重合无人机等情况下的近距视觉相对导航问题且视觉导航精度较高，能够实现集群编队全过程的视觉导航，提高了仿猛禽视觉相对导航的适应性；本发明基于仿猛禽视觉导航设计了仿鹳雁群迁徙行为的集群自主编队控制方法，能够实现多无人机的分布式编队，提高了集群编队方法的自主性与自愈性。
62.本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成，也可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器，磁盘或光盘等。
63.以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。