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

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

Info

Publication number
CN113406975B
CN113406975B CN202110952222.3A CN202110952222A CN113406975B CN 113406975 B CN113406975 B CN 113406975B CN 202110952222 A CN202110952222 A CN 202110952222A CN 113406975 B CN113406975 B CN 113406975B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
cluster
formation
navigation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110952222.3A
Other languages
English (en)
Other versions
CN113406975A (zh
Inventor
孙永斌
贺威
宫乐
何修宇
邹尧
付强
王靖元
李擎
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Science and Technology Beijing USTB
Original Assignee
University of Science and Technology Beijing USTB
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Science and Technology Beijing USTB filed Critical University of Science and Technology Beijing USTB
Priority to CN202110952222.3A priority Critical patent/CN113406975B/zh
Publication of CN113406975A publication Critical patent/CN113406975A/zh
Application granted granted Critical
Publication of CN113406975B publication Critical patent/CN113406975B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及无人机自主导航与协同控制技术领域。提出了一种仿生智能多无人机集群自主编队导航控制方法及装置，包括：通过多无人机底层控制模块进行无人机控制分配及电机动态模拟仿真；通过多无人机相对位置导航模块进行无人机检测以及干扰区滤除，并进行无人机位姿估计；通过多无人机集群自主编队控制模块确定进行仿鹳雁群集群编队；通过多无人机相对位置控制模块无人机目标位置差，并进行飞行控制。通过构建基于仿生智能的多无人机集群自主编队方法及装置，提高了多无人机系统近距相对导航的可靠性和精确性；解决了现有多无人机集群编队近距视觉相对导航位姿精确测量与分布式编队的不足，提高了多无人机集群自主编队水平。

Description

一种仿生智能多无人机集群自主编队导航控制方法及装置
技术领域
本发明涉及多无人机自主导航与协同控制技术领域，特别是指一种仿生智能多无人机集群自主编队导航控制的方法及装置。
背景技术
针对强干扰、高动态的集群作战环境，多无人机集群自主编队需要实时、复杂的机间信息交互，对单机通信带宽要求较高，同时可能面临机间局部通信无法建立、GPS精度低等问题，此外多无人机系统应满足分布式集群编队需求，因此设计合理高效的多无人机集群自主编队导航与控制方法至关重要。
目前，多无人机集群编队过程中常用的相对导航技术包括惯性导航、差分GPS(Global Positioning System, GPS)等，其中惯性导航存在随着时间不断累加误差的缺陷；GPS技术虽然使用起来非常方便，但是由于其依靠卫星来定位，精度和抗干扰能力有待加强，且针对多无人机系统需要实时获取相邻无人机的位置、姿态等信息，对机间通信要求较高。由于复杂作战环境对电磁信号的限制、GPS信号精度与实时性的不足，故传统导航方法无法实现不同无人机间的实时、高精度相对位置获取，同时多无人机集群编队任务需要设计分布式的编队控制，以提高多无人机系统的自愈性、自主性，增强集群作战效能。
发明内容
本发明实施例提供了一种仿生智能多无人机集群自主编队导航控制方法及装置。针对现有多无人机集群编队导航与控制方法在近距相对导航与分布式编队控制上可行性、实时性以及精确性等方面的不足，并且考虑低频低精度GPS与视觉相对导航共用、无需建立复杂的机间通信等具体问题，基于仿生智能设计了多无人机集群自主编队导航控制装置及其方法。技术方案如下：
一方面，提供了一种仿生智能多无人机集群自主编队导航控制方法，该方法包括：
S1：通过多无人机底层控制模块，建立四旋翼无人机六自由度非线性模型，并进行参数初始化；进行无人机控制分配及电机动态模拟仿真；更新多无人机状态量信息，计算仿真时间
Figure 217828DEST_PATH_IMAGE001
，其中，
Figure 407501DEST_PATH_IMAGE002
Figure 900799DEST_PATH_IMAGE003
为仿真步长;
S2：通过多无人机相对位置导航模块，获取多无人机集群自主编队的导航图像，进行候选无人机检测，并进行杂点滤除，得到感兴趣区域ROI区，通过设定无人机的先验标志点，对ROI区进行干扰区域滤除，得到剩余ROI区；以及通过判断剩余ROI区中是否存在先验标志点，判断是否检测到无人机；
S3：相对位置导航模块对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队；
S4：通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导-跟随关系及编队目标位置；
S5：通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制；
S6：通过多无人机底层控制模块，对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否结束多无人机集群自主编队；若
Figure 263648DEST_PATH_IMAGE004
大于最大仿真时间
Figure 768578DEST_PATH_IMAGE005
，则结束仿真并绘制多无人机集群自主编队过程数据曲线，否则重复执行S1-S6。
可选地，建立四旋翼无人机六自由度非线性模型，并进行参数初始化，包括：
Figure 253786DEST_PATH_IMAGE007
其中，
Figure 844168DEST_PATH_IMAGE008
分别为无人机惯性系下
Figure 10707DEST_PATH_IMAGE009
三个方向的位置与速度、滚转角、俯仰角、偏航角以及机体轴三个方向的旋转角速度；
Figure 593365DEST_PATH_IMAGE010
为相应变量的一阶微分；
Figure 124840DEST_PATH_IMAGE011
为无人机螺旋桨产生的总拉力；
Figure 327151DEST_PATH_IMAGE012
为无人机质量，
Figure 31802DEST_PATH_IMAGE013
为重力加速度；
Figure 511325DEST_PATH_IMAGE014
分别为三轴转动惯量；
Figure 338336DEST_PATH_IMAGE015
为陀螺力矩；
Figure 231205DEST_PATH_IMAGE016
为螺旋桨转速；
Figure 411651DEST_PATH_IMAGE017
为电机转子和螺旋桨绕转轴的总转动惯量；
Figure 401473DEST_PATH_IMAGE018
为无人机螺旋桨在机体轴上产生的力矩。
可选地，通过多无人机相对位置导航模块，获取多无人机集群自主编队的导航图像，进行候选无人机检测，并进行杂点滤除，得到感兴趣区域ROI区，通过设定无人机的先验标志点，对ROI区进行干扰区域滤除，得到剩余ROI区；，包括：
S21：利用仿猛禽视觉颜色双拮抗机制,构建多无人机集群自主编队的标志区及标志点检测方法，提取多无人机集群编队近距导航图像中不同波段信号信息，获取多无人机集群自主编队的导航图像；
通过在长波、中波、短波通路中分别保持红色、绿色和蓝色成分灰度值对比度并削弱其它颜色成分灰度值对比度来构建仿猛禽视觉颜色双拮抗机制模型，通过公式（4）来检测多无人机集群编队标志区中红色、绿色、蓝色信息：
Figure 337067DEST_PATH_IMAGE019
其中，
Figure 389337DEST_PATH_IMAGE020
分别为长波、中波、短波以及紫外波段四个通路对图像的输出响应；
Figure 498107DEST_PATH_IMAGE021
分别为输入可见光导航图像中三个颜色通道的灰度值；
Figure 283048DEST_PATH_IMAGE022
Figure 327228DEST_PATH_IMAGE023
分别为长波、中波、短波阈值；
Figure 725848DEST_PATH_IMAGE024
表示取
Figure 575992DEST_PATH_IMAGE025
与
Figure 884614DEST_PATH_IMAGE026
的最大值；
Figure 224328DEST_PATH_IMAGE027
表示取
Figure 579086DEST_PATH_IMAGE025
与
Figure 905025DEST_PATH_IMAGE026
的最小值；
将RGB颜色空间信息转换到HSV颜色空间后，通过公式（5）检测无人机集群编队标志区的紫色、青色标志点：
Figure 661629DEST_PATH_IMAGE028
其中，H、S、V分别为HSV空间的三个分量；
Figure 906665DEST_PATH_IMAGE029
分别为
Figure 420823DEST_PATH_IMAGE030
三个分量的最大值和最小值；
S22：计算仿猛禽视觉双拮抗模型长波通道输出，并对导航图像进行二值化处理，得到二值图像；
S23：对二值图像进行孔洞填充、滤除杂点，得到剩余ROI区；所述剩余ROI区为集群编队候选无人机。
可选地，通过判断剩余ROI区中是否存在先验标志点，判断是否检测到无人机，包括：
通过多无人机相对位置导航模块进行干扰区域滤除后，判断是否存在剩余ROI区，若不存在剩余ROI区，则确定无有效无人机；若存在剩余ROI区，则进一步判断剩余ROI区内是否存在先验标志点；若剩余ROI区内没有先验标志点，确定无有效无人机；若存在先验标志点，则确定检测到有效无人机。
可选地，对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队，包括：
S31：若检测到无人机，则通过仿猛禽颜色检测和HSV方法，提取剩余ROI区内的先验标志点，计算检测到的单个无人机连通域内的先验标志点种类及像素坐标，进而判断剩余ROI区是否只有一种颜色的标志点；
S32：若只有一种颜色的标志点，则为非重叠无人机，直接进行轮廓分块圆拟合，选取最优拟合结果，并进行位姿估计；
若非一种颜色的标志点，则为重叠无人机，需确定重合无人机标志区的分割线，进行多机分块圆拟合，选取最优拟合结果，通过位姿估计得到相机系下的相对位置关系；计算各颜色标志点的像素坐标中心
Figure 612770DEST_PATH_IMAGE031
与
Figure 17688DEST_PATH_IMAGE032
，以及各颜色标志点平均像素点数
Figure 574571DEST_PATH_IMAGE033
与
Figure 638342DEST_PATH_IMAGE034
，进而根据如下公式（8）计算
Figure 430718DEST_PATH_IMAGE035
连线上重合标志球的分割点
Figure 834017DEST_PATH_IMAGE036
Figure 624119DEST_PATH_IMAGE037
其中，
Figure 971924DEST_PATH_IMAGE038
为连线
Figure 708935DEST_PATH_IMAGE039
的分割比例系数；
Figure 29058DEST_PATH_IMAGE040
为面积比例因子;得到重合标志球分割点
Figure 786799DEST_PATH_IMAGE041
后，然后计算与连线
Figure 762845DEST_PATH_IMAGE042
垂直且过
Figure 100286DEST_PATH_IMAGE043
点的分割线
Figure 340162DEST_PATH_IMAGE044
以及斜率
Figure 409749DEST_PATH_IMAGE045
其中斜率
Figure 669829DEST_PATH_IMAGE046
为：
Figure 607698DEST_PATH_IMAGE047
计算重合标志球分割线各点像素坐标，设置导航图像分辨率为
Figure 574517DEST_PATH_IMAGE048
Figure 142901DEST_PATH_IMAGE049
分别为导航图像两个方向的像素坐标最大值，根据分割线斜率
Figure 952594DEST_PATH_IMAGE046
，计算分割线所有像素点坐标点集
Figure 303941DEST_PATH_IMAGE050
Figure 718742DEST_PATH_IMAGE052
其中，
Figure 254766DEST_PATH_IMAGE053
取1到
Figure 161542DEST_PATH_IMAGE054
，并分别计算对应的
Figure 644476DEST_PATH_IMAGE055
；
S33：通过位姿估计/相对位置关系为多无人机集群自主编队系统提供导引信息，进行多无人机进行仿鹳雁群集群编队。
可选地，通过仿猛禽颜色检测和HSV方法，提取剩余ROI区内的先验标志点，包括：
将二值图像与导航图像进行相与操作，得到只包含ROI区的彩色图像，进而在ROI彩色区内通过仿猛禽颜色检测和HSV方法，提取蓝色、绿色、紫色、青色先验标志点。
可选地，通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导-跟随关系及编队目标位置，包括：
S41：设置多无人机期望的初始目标高度
Figure 707591DEST_PATH_IMAGE056
，将每个无人机控制为高度一致；
S42：每个无人机通过地面站每1s获取一次低频低精度的无人机GPS位置，并综合所有无人机的前向位置信息，判断待选定无人机是否位于所有无人机的最前方；
S43：若待选定无人机位于最前方，则确定待选定无人机为总领导无人机；若待选定无人机位于非最前方且没有检测到其它无人机，则待选定无人机为跟随无人机，需要寻找领导无人机，并使所述领导无人机出现在视觉导航视场内；
S44：若检测到其它无人机，则多个跟随无人机的集群编队计算检测到无人机的位置距离，根据位置距离确定领导-跟随关系，进而得到跟随左侧或右侧集群的编队目标位置；
经过仿猛禽视觉相对导航获取检测到无人机的相对位置
Figure 289882DEST_PATH_IMAGE057
，根据如下公式（11）计算仿鹳雁群集群编队位置距离集
Figure 11850DEST_PATH_IMAGE058
Figure 829634DEST_PATH_IMAGE059
其中，
Figure 625551DEST_PATH_IMAGE060
为侧向比例因子，
Figure 503378DEST_PATH_IMAGE061
的取值范围与检测到无人机数量一致；当前无人机跟随检测到的距离最近的相对领导无人机，将距离集
Figure 978221DEST_PATH_IMAGE062
中的元素按从小到大的顺序排列，并选取距离最小的无人机作为领导无人机，即确定领导-跟随关系，记与领导无人机的相对位置为
Figure 802958DEST_PATH_IMAGE063
。
可选地，通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制，包括：
S51：计算定直平飞位置、编队目标位置、寻找领导者目标位置与无人机当前位置的差值；
将仿猛禽视觉导航输出的领导无人机的相对位置
Figure 718961DEST_PATH_IMAGE064
与期望相对位置
Figure 970951DEST_PATH_IMAGE065
做差，得到目标位置的差
Figure 464249DEST_PATH_IMAGE066
Figure 95606DEST_PATH_IMAGE067
S52：计算每个无人机间的相对距离，当相对距离过小时，无人机以一定的速度降低高度，避免碰撞；
S53：根据高度方向目标位置差，得到
Figure 194012DEST_PATH_IMAGE068
方向的无人机底层控制速度指令；
根据以下公式（13）构建多无人机高度相对位置控制：
Figure 554587DEST_PATH_IMAGE070
其中，
Figure 472864DEST_PATH_IMAGE071
为侧向速度指令比例系数；
S54：根据侧向目标位置差，得到
Figure 904982DEST_PATH_IMAGE072
方向的无人机底层控制速度指令；
根据以下公式（14）构建多无人机侧向相对位置控制：
Figure 451370DEST_PATH_IMAGE073
其中，
Figure 310742DEST_PATH_IMAGE074
为侧向速度指令比例系数；
S55：根据是否为总领导无人机或未检测其它无人机的跟随者，确定基本飞行速度；根据前向目标位置差，计算得到x方向的无人机底层控制速度指令；
根据以下公式（15）构建多无人机前向相对位置控制：
Figure 716315DEST_PATH_IMAGE075
其中，
Figure 480353DEST_PATH_IMAGE076
为前向速度指令比例系数；
将相对位置控制得到的速度控制指令
Figure 287772DEST_PATH_IMAGE077
输入内环多无人机底层控制。
一方面，提供一种仿生智能多无人机集群自主编队导航控制装置，该装置应用于上述任意一项的方法，包括：
多无人机底层控制模块，用于建立四旋翼无人机六自由度非线性模型，并进行参数初始化；进行无人机控制分配及电机动态模拟仿真；更新多无人机状态量信息，计算仿真时间
Figure 318045DEST_PATH_IMAGE078
，其中，
Figure 476494DEST_PATH_IMAGE079
Figure 922519DEST_PATH_IMAGE080
为仿真步长;
对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否结束多无人机集群自主编队；若
Figure 584444DEST_PATH_IMAGE078
大于最大仿真时间
Figure 582356DEST_PATH_IMAGE081
，则结束仿真并绘制多无人机集群自主编队过程数据曲线；
多无人机相对位置导航模块，用于获取多无人机集群编队导航图像，进行仿猛禽视觉候选无人机检测，并进行杂点滤除得到感兴趣区域ROI区，通过先验标志点对ROI区进行干扰区域滤除，得到剩余ROI区；判断干扰区域滤除后，是否存在剩余ROI区，以及通过判断剩余ROI区中是否存在先验标志点，判断是否检测到无人机；对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队；
多无人机集群自主编队控制模块，用于对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导-跟随关系及编队目标位置；
多无人机相对位置控制模块，用于计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制。
可选地，多无人机底层控制模块，包括与多无人机数量一一对应的电机动态监测子模块以及控制分配子模块；所述控制分配子模块包括对无人机的横侧向速度控制以及纵向速度控制。
本发明实施例提供的技术方案带来的有益效果至少包括：
上述方案中，一、本发明提供了完整多无人机集群自主编队视觉导航与控制系统框架及其工作流程，系统框架以及流程合理、高效，能够满足多无人机集群自主编队导航与控制性能指标；
二、本发明模仿猛禽颜色视觉机制构建仿猛禽颜色双拮抗检测方法，提高了无人机标志球及标志点的检测效率及准确性；
三、本发明给出了两种相对导航的组合方法，解决了标志球部分遮挡、单一无人机、重合无人机等情况下的近距视觉相对导航问题且视觉导航精度较高，能够实现集群编队全过程的视觉导航，提高了仿猛禽视觉导航的适应性；
四、本发明基于仿猛禽视觉导航设计了仿鹳雁群迁徙行为的集群自主编队控制方法，能够实现多无人机的分布式编队，提高了集群编队方法的自主性与自愈性。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的整体步骤图；
图2是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的干扰区滤除步骤图；
图3是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的标志球与标志点示意图；
图4是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的标志球分块拟合示意图；
图5是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的仿鹳雁群集群编队步骤图；
图6是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的标志球重合分割示意图；
图7是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的外接矩形顶点匹配示意图；
图8是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的领导-跟随关系确定步骤图；
图9是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的无人机集群方位控制步骤图；
图10是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的自主编队场景图；
图11a-11c是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制方法的实时位置数据曲线；
图12是本发明实施例提供的一种仿生智能多无人机集群自主编队导航控制装置的系统框架图。
具体实施方式
为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。
本发明实施例提供了一种仿生智能多无人机集群自主编队导航控制方法，如图1所示，为本发明的仿生智能多无人机集群自主编队导航控制方法整体步骤图，包括：
S1：通过多无人机底层控制模块，建立四旋翼无人机六自由度非线性模型，并进行参数初始化；进行无人机控制分配及电机动态模拟仿真；更新多无人机状态量信息，计算仿真时间
Figure 634626DEST_PATH_IMAGE082
，其中，
Figure 946659DEST_PATH_IMAGE083
Figure 259828DEST_PATH_IMAGE084
为仿真步长;
S2：通过多无人机相对位置导航模块，获取多无人机集群自主编队的导航图像，进行候选无人机检测，并进行杂点滤除，得到ROI区 (Region of Interest，感兴趣区域)，通过设定无人机的先验标志点，对ROI区进行干扰区域滤除，得到剩余ROI区；以及通过判断剩余ROI区中是否存在先验标志点，判断是否检测到无人机；
S3：相对位置导航模块对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队；
S4：通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导-跟随关系及编队目标位置；
S5：通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制；
S6：通过多无人机底层控制模块，对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否结束多无人机集群自主编队；若
Figure 304008DEST_PATH_IMAGE085
大于最大仿真时间
Figure 174399DEST_PATH_IMAGE086
，则结束仿真并绘制多无人机集群自主编队过程数据曲线，否则重复执行S1-S6。
在本实施例中，本发明提出的多无人机集群自主编队协同作战方案是弥补单一无人机感知、机动、决策等能力不足，提高多无人机系统鲁棒性、自主性、智能性，增强系统任务执行能力及作战效能的有效途径。通过构建多无人机集群自主编队方法及装置，提高了多无人机系统近距相对导航的可靠性和精确性；并且本发明的设计适用于多无人机的分布式编队控制方法，从而提高多无人机集群自主编队近距视觉导航与编队控制的精度，促进多无人机集群作战的顺利实现，提高多无人机系统的自主能力水平。
本发明采用视觉相对导航技术通过提取导航图像的目标特征点并进行位姿测量，从而为多无人机集群自主编队提供导引信息。相对于其他导航系统，视觉传感器具有体积小、重量轻、低功耗等特点，同时视觉导航系统的工作波段远离当前电磁对抗的频率范围，抗电磁干扰性能强，近距测量多无人机位姿信息精度较高。此外，相对传统编队方法，仿生编队具有编队形式灵活多变、环境适应与自学习能力强、对抗作战重构性能好等优势。鉴于视觉相对导航与仿生编队既能满足多无人机集群自主编队过程中的导航精度、分布式编队要求，又能处理机间通信无法建立及GPS精度低等特殊情况，本发明提出了一种基于仿生智能的多无人机集群自主编队导航控制系统及其方法，以解决现有多无人机集群编队近距视觉相对导航位姿精确测量与分布式编队鲁棒性和适用性方面的不足，有效提高多无人机系统集群自主编队控制水平。
建立四旋翼无人机六自由度非线性模型，并进行参数初始化，包括：
Figure 821281DEST_PATH_IMAGE087
其中，
Figure 395482DEST_PATH_IMAGE088
分别为无人机惯性系下
Figure 672880DEST_PATH_IMAGE089
三个方向的位置与速度、滚转角、俯仰角、偏航角以及机体轴三个方向的旋转角速度；
Figure 89954DEST_PATH_IMAGE090
为相应变量的一阶微分；
Figure 150314DEST_PATH_IMAGE091
为无人机螺旋桨产生的总拉力；
Figure 641339DEST_PATH_IMAGE092
为无人机质量，
Figure 151954DEST_PATH_IMAGE093
为重力加速度；
Figure 666112DEST_PATH_IMAGE094
分别为三轴转动惯量；
Figure 858059DEST_PATH_IMAGE095
为陀螺力矩；
Figure 265907DEST_PATH_IMAGE096
为螺旋桨转速；
Figure 822790DEST_PATH_IMAGE097
为电机转子和螺旋桨绕转轴的总转动惯量；
Figure 703806DEST_PATH_IMAGE098
为无人机螺旋桨在机体轴上产生的力矩。
在本实施例中，公式（2）的四旋翼无人机六自由度非线性模型无需推导直接给出：给定参数无人机质量
Figure 699444DEST_PATH_IMAGE099
，转动惯量
Figure 837165DEST_PATH_IMAGE100
、
Figure 689583DEST_PATH_IMAGE101
，机体中心和任一电机的距离
Figure 240650DEST_PATH_IMAGE102
，电机拉力系数
Figure 446503DEST_PATH_IMAGE103
，电机转矩系数
Figure 828943DEST_PATH_IMAGE104
Figure 55525DEST_PATH_IMAGE105
，重力加速度
Figure 765992DEST_PATH_IMAGE106
。其中，
Figure 900170DEST_PATH_IMAGE107
为陀螺力矩，其计算公式如下：
Figure 12483DEST_PATH_IMAGE108
Figure 144387DEST_PATH_IMAGE109
为螺旋桨转速；
Figure 735293DEST_PATH_IMAGE110
为电机转子和螺旋桨绕转轴的总转动惯量；
Figure 282949DEST_PATH_IMAGE098
计算公式为：
Figure 577664DEST_PATH_IMAGE111
其中，
Figure 208365DEST_PATH_IMAGE112
为机体中心和任一电机的距离，
Figure 955741DEST_PATH_IMAGE113
分别无量纲的电机拉力系数和转矩系数。
本实施例中，还需要针对步骤S1建立的四旋翼无人机六自由度模型，设计纵向速度PID控制器：
Figure 900564DEST_PATH_IMAGE114
其中，
Figure 721889DEST_PATH_IMAGE115
为期望拉力；
Figure 257913DEST_PATH_IMAGE116
分别为纵向速度控制比例、积分、微分系数；
Figure 430268DEST_PATH_IMAGE117
为纵向速度指令值；
Figure 647623DEST_PATH_IMAGE118
为纵向速度误差。
设计横侧向速度PID控制器，具体如下：
Figure 713668DEST_PATH_IMAGE119
Figure 295959DEST_PATH_IMAGE120
Figure 77314DEST_PATH_IMAGE121
其中，
Figure 770464DEST_PATH_IMAGE122
分别为滚转角与俯仰角的期望值；
Figure 628699DEST_PATH_IMAGE123
分别为前向速度控制比例、积分、微分系数；
Figure 506525DEST_PATH_IMAGE124
分别为侧向速度控制比例、积分、微分系数；
Figure 653472DEST_PATH_IMAGE125
分别为前向、侧向速度指令值；
Figure 274946DEST_PATH_IMAGE126
分别为前向、侧向速度误差；
Figure 253267DEST_PATH_IMAGE127
为转换矩阵。
进而设计内环姿态角PID控制器，具体如下：
Figure 911781DEST_PATH_IMAGE128
Figure 405079DEST_PATH_IMAGE129
Figure 705611DEST_PATH_IMAGE130
Figure 600754DEST_PATH_IMAGE131
其中，
Figure 430170DEST_PATH_IMAGE132
分别为机体轴三个方向的旋转角速度期望值；
Figure 410765DEST_PATH_IMAGE133
分别为三轴旋转角速度比例系数；
Figure 580233DEST_PATH_IMAGE134
分别为滚转角、俯仰角、偏航角误差；
Figure 939671DEST_PATH_IMAGE135
分别为三轴期望力矩；
Figure 330201DEST_PATH_IMAGE136
分别为三轴力矩比例系数；
Figure 673457DEST_PATH_IMAGE137
分别为三轴力矩积分系数；
Figure 440425DEST_PATH_IMAGE138
分别为三轴力矩微分系数；
Figure 247844DEST_PATH_IMAGE139
分别为三轴旋转角速度误差，
Figure 684641DEST_PATH_IMAGE140
分别为相应变量的一阶微分。
利用控制器得到的期望拉力
Figure 374249DEST_PATH_IMAGE141
与期望三轴力矩
Figure 554694DEST_PATH_IMAGE142
，根据四旋翼无人机控制效率矩阵
Figure 278937DEST_PATH_IMAGE143
，进行控制分配：
Figure 886636DEST_PATH_IMAGE144
其中，
Figure 266801DEST_PATH_IMAGE145
分别为期望电机转速。
四旋翼实际转速电机动态，如下式所示：
Figure 109993DEST_PATH_IMAGE146
其中，
Figure 829687DEST_PATH_IMAGE147
为电机动态响应时间，
Figure 729991DEST_PATH_IMAGE148
为复常量。
利用得到的电机实际转速
Figure 597453DEST_PATH_IMAGE149
，根据公式(2)的无人机模型更新无人机各个状态量。
初始化无人机数量、初始位置、定直平飞速度等参数，更新多无人机状态量信息，计算当前的仿真时间
Figure 385280DEST_PATH_IMAGE150
（
Figure 21798DEST_PATH_IMAGE151
为仿真步长）。
多无人机底层控制相关参数选择为：
Figure 361513DEST_PATH_IMAGE152
、
Figure 450691DEST_PATH_IMAGE154
Figure 776630DEST_PATH_IMAGE156
Figure 64392DEST_PATH_IMAGE158
如图2所示，为本发明的干扰区滤除步骤图。通过多无人机相对位置导航模块，获取多无人机集群自主编队的导航图像，进行候选无人机检测，并进行杂点滤除，得到感兴趣区域ROI区，通过设定无人机的先验标志点，对ROI区进行干扰区域滤除，得到剩余ROI区；包括：
S21：利用仿猛禽视觉颜色双拮抗机制,构建多无人机集群自主编队的标志区及标志点检测方法，提取多无人机集群编队近距导航图像中不同波段信号信息，获取多无人机集群自主编队的导航图像；
通过在长波、中波、短波通路中分别保持红色、绿色和蓝色成分灰度值对比度并削弱其它颜色成分灰度值对比度来构建仿猛禽视觉颜色双拮抗机制模型，通过公式（3）来检测多无人机集群编队标志区中红色、绿色、蓝色信息：
Figure 450374DEST_PATH_IMAGE159
其中，
Figure 89166DEST_PATH_IMAGE160
分别为长波、中波、短波以及紫外波段四个通路对图像的输出响应；
Figure 281113DEST_PATH_IMAGE161
分别为输入可见光导航图像中三个颜色通道的灰度值；
Figure 298747DEST_PATH_IMAGE162
Figure 248773DEST_PATH_IMAGE163
分别为长波、中波、短波阈值；
Figure 312544DEST_PATH_IMAGE164
表示取
Figure 980286DEST_PATH_IMAGE165
与
Figure 977061DEST_PATH_IMAGE166
的最大值；
Figure 970425DEST_PATH_IMAGE167
表示取
Figure 318229DEST_PATH_IMAGE165
与
Figure 524083DEST_PATH_IMAGE166
的最小值；
将RGB颜色空间信息转换到HSV颜色空间后，通过公式（4）检测无人机集群编队标志区的紫色、青色标志点：
Figure 844206DEST_PATH_IMAGE169
其中，H、S、V分别为HSV空间的三个分量；
Figure 133105DEST_PATH_IMAGE170
分别为
Figure 905889DEST_PATH_IMAGE171
三个分量的最大值和最小值；
S22：计算仿猛禽视觉颜色双拮抗模型长波通道输出，并对导航图像进行二值化处理，得到二值图像；
S23：对二值图像进行孔洞填充、滤除杂点，得到剩余ROI区；所述剩余ROI区为集群编队候选无人机。
在本实施例中，无人机集群编队标志区设计为红色圆球并在表面分别添加蓝色、绿色、紫色、青色标志点，标志点在球最大半径的水平面每60°添加一个，如图3所示。
利用仿猛禽视觉颜色双拮抗机制构建集群编队标志区及标志点检测方法，提取多无人机集群编队近距导航图像中不同波段信号信息。长波通路主要吸收彩色(Red-Green-Blue, RGB)图像中的红色信号，并对其它颜色进行抑制，RGB图像中R通道的灰度值占R、G、B三通道灰度值之和的比例越大，长波通路的响应越大；同样地，中波通路、短波通路分别吸收彩色图像中的绿色与蓝色信号，并对其它颜色信号进行抑制；紫外通路吸收图像中的亮度信号。通过在长波、中波、短波通路中分别保持红色、绿色和蓝色成分灰度值对比度并削弱其它颜色成分灰度值对比度来构建仿猛禽视觉颜色双拮抗机制模型，通过公式（3）来检测多无人机集群编队标志区中红色、绿色、蓝色信息。
根据公式（4）进行颜色空间转换，在HSV空间内根据集群编队标志点颜色类型（紫色或青色）及深浅等先验信息对H和S两个通道的阈值进行设置，可实现对集群编队标志点的有效分割。
在本实施例中，在得到二值化图像后，需要对二值图像进行孔洞填充、滤除杂点：
如图4所示，将ROI区域边缘分成四部分，对每一部分采用最小二乘法进行圆拟合，定义圆拟合度
Figure 181012DEST_PATH_IMAGE172
：
Figure 621221DEST_PATH_IMAGE173
其中，
Figure 812512DEST_PATH_IMAGE174
表示二值图像中的第
Figure 338171DEST_PATH_IMAGE174
个候选无人机，
Figure 151407DEST_PATH_IMAGE175
表示第
Figure 39597DEST_PATH_IMAGE174
个候选无人机边缘的第
Figure 14506DEST_PATH_IMAGE175
部分
Figure 824199DEST_PATH_IMAGE176
表示第
Figure 441125DEST_PATH_IMAGE175
部分拟合像素点数；
Figure 590347DEST_PATH_IMAGE177
表示第
Figure 126371DEST_PATH_IMAGE175
部分的
Figure 298726DEST_PATH_IMAGE178
个像素点在拟合圆上的像素点数，其计算公式如下：
Figure 312818DEST_PATH_IMAGE180
其中，
Figure 254229DEST_PATH_IMAGE181
为第
Figure 901767DEST_PATH_IMAGE175
部分的拟合像素点；
Figure 420473DEST_PATH_IMAGE182
为拟合圆的中心像素坐标；
Figure 379202DEST_PATH_IMAGE183
为拟合圆的半径；
Figure 299753DEST_PATH_IMAGE184
为判断拟合结果的阈值。从候选无人机的边缘拟合结果中选取最优拟合作为最终用于位姿估计的拟合圆，以降低圆拟合误差，解决标志区部分遮挡的问题。此外，对于候选无人机干扰区，选取圆拟合度的区分阈值
Figure 380842DEST_PATH_IMAGE185
，进行干扰区域滤除，若
Figure 262210DEST_PATH_IMAGE186
，能够说明拟合像素点基本不在拟合圆上，干扰区域为非圆，需要滤除以降低环境干扰影响。
本实施例中，其它仿真参数设置为：
Figure 618105DEST_PATH_IMAGE187
、紫色分割色度阈值范围130~155、紫色分割饱和度阈值范围111~255、青色分割色度阈值范围80~95、青色分割饱和度阈值范围111~255。
通过判断剩余ROI区中是否存在先验标志点，判断是否检测到无人机，包括：
通过多无人机相对位置导航模块进行干扰区域滤除后，判断是否存在剩余ROI区，若不存在剩余ROI区，则确定无有效无人机；若存在剩余ROI区，则进一步判断剩余ROI区内是否存在先验标志点；若剩余ROI区内没有先验标志点，确定无有效无人机；若存在先验标志点，则确定检测到有效无人机。
在本实施例中，若存在剩余ROI区，将二值化图像与原图像进行相与操作得到只包含ROI区的彩色图像，进而在ROI彩色区内通过仿猛禽颜色检测和HSV方法，提取蓝色、绿色、紫色、青色先验标志点，如果ROI彩色区内没有先验标志点，则确定无有效无人机，则进行多无人机高度控制操作；若检测到无人机，通过仿猛禽颜色检测和HSV方法，可提取无人机ROI彩色区内蓝色、绿色、紫色、青色先验标志点，并统计检测到的单个无人机连通域内的标志点种类及像素坐标，进而判断无人机ROI彩色区是否只有一种颜色的标志点。
本实施例中，通过模仿猛禽颜色视觉机制，构建仿猛禽颜色双拮抗检测方法，提高了无人机标志球及标志点的检测效率及准确性。
如图5所示，为本发明的仿鹳雁群集群编队步骤图。对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队，包括：
S31：若检测到无人机，则通过仿猛禽颜色检测和HSV方法，提取剩余ROI区内的先验标志点，计算检测到的单个无人机连通域内的先验标志点种类及像素坐标，进而判断剩余ROI区是否只有一种颜色的标志点；
S32：若只有一种颜色的标志点，则为非重叠无人机，直接进行轮廓分块圆拟合，选取最优拟合结果，并进行位姿估计；
若非一种颜色的标志点，则为重叠无人机，需确定重合无人机标志球的分割线，进行多机分块圆拟合，选取最优拟合结果，通过位姿估计得到相机系下的相对位置关系；计算各颜色标志点的像素坐标中心
Figure 534109DEST_PATH_IMAGE188
与
Figure 582836DEST_PATH_IMAGE189
，以及各颜色标志点平均像素点数
Figure 951500DEST_PATH_IMAGE190
与
Figure 314349DEST_PATH_IMAGE191
，进而根据如下公式（8）计算
Figure 943913DEST_PATH_IMAGE192
连线上重合标志球的分割点
Figure 38908DEST_PATH_IMAGE193
：
Figure 305589DEST_PATH_IMAGE194
其中，
Figure 878653DEST_PATH_IMAGE195
为连线
Figure 628303DEST_PATH_IMAGE196
的分割比例系数；
Figure 222096DEST_PATH_IMAGE197
为面积比例因子;得到重合标志球分割点
Figure 299773DEST_PATH_IMAGE198
后，然后计算与连线
Figure 332320DEST_PATH_IMAGE199
垂直且过
Figure 811843DEST_PATH_IMAGE200
点的分割线
Figure 576537DEST_PATH_IMAGE201
以及斜率
Figure 266144DEST_PATH_IMAGE202
，如图6所示；其中斜率
Figure 446590DEST_PATH_IMAGE202
为：
Figure 170832DEST_PATH_IMAGE203
计算重合标志球分割线各点像素坐标，设置导航图像分辨率为
Figure 778531DEST_PATH_IMAGE204
Figure 158697DEST_PATH_IMAGE205
分别为导航图像两个方向的像素坐标最大值，根据分割线斜率
Figure 4817DEST_PATH_IMAGE202
，计算分割线所有像素点坐标点集
Figure 724512DEST_PATH_IMAGE206
：
Figure 627746DEST_PATH_IMAGE208
其中，
Figure 167311DEST_PATH_IMAGE209
取1到
Figure 17456DEST_PATH_IMAGE210
，，并分别计算对应的
Figure 450711DEST_PATH_IMAGE211
；
S33：通过位姿估计/相对位置关系为多无人机集群自主编队系统提供导引信息，进行多无人机进行仿鹳雁群集群编队。
在本实施例中，若检测到无人机且ROI区只有一种标志点，根据步骤S2中的轮廓分块圆拟合结果，选择最优拟合圆，进而可得最优拟合圆的外接矩形顶点集合
Figure 665792DEST_PATH_IMAGE212
分别对应外接矩形四个顶点如图7所示，根据集群编队标志球实际尺寸的外接矩形对四个顶点像素点进行顺序匹配。
对匹配正确的所有标志点，利用
Figure 551708DEST_PATH_IMAGE213
算法进行精确位姿测量，得到相机坐标系下的无人机位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到与前方无人机在地面惯性系下的位置差
Figure 877647DEST_PATH_IMAGE214
，然后利用仿猛禽相对导航得到的相对位置差进行后续的仿鹳雁群集群编队。
在本实施例中，设置导航图像分辨率为
Figure 368672DEST_PATH_IMAGE215
。
本实施例中，给出了两种相对导航的组合方法，解决了标志球部分遮挡、单一无人机、重合无人机等情况下的近距视觉相对导航问题且视觉导航精度较高，能够实现集群编队全过程的视觉导航，提高了仿猛禽视觉相对导航的适应性。
如图8所示，为本发明的领导-跟随关系确定步骤图。通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导-跟随关系及编队目标位置，包括：
S41：设置多无人机期望的初始目标高度
Figure 613708DEST_PATH_IMAGE216
，将每个无人机控制为高度一致；
S42：每个无人机通过地面站每1s获取一次低频低精度的无人机GPS位置，并综合所有无人机的前向位置信息，判断待选定无人机是否位于所有无人机的最前方；
S43：若待选定无人机位于最前方，则确定待选定无人机为总领导无人机；若待选定无人机位于非最前方且没有检测到其它无人机，则待选定无人机为跟随无人机，需要寻找领导无人机，并使所述领导无人机出现在视觉导航视场内；
S44：若检测到其它无人机，则多个跟随无人机的集群编队计算检测到无人机的位置距离，根据位置距离确定领导-跟随关系，进而得到跟随左侧或右侧集群的编队目标位置；
经过仿猛禽视觉相对导航获取检测到无人机的相对位置
Figure 127866DEST_PATH_IMAGE217
，根据如下公式（11）计算仿鹳雁群集群编队位置距离集
Figure 382130DEST_PATH_IMAGE218
：
Figure 724731DEST_PATH_IMAGE219
其中，
Figure 281614DEST_PATH_IMAGE220
为侧向比例因子，在本实施例中，
Figure 407702DEST_PATH_IMAGE221
的取值范围与检测到无人机数量一致；当前无人机跟随检测到的距离最近的相对领导无人机，将距离集
Figure 809865DEST_PATH_IMAGE222
中的元素按从小到大的顺序排列，并选取距离最小的无人机作为领导无人机，即确定领导-跟随关系，记与领导无人机的相对位置为
Figure 72219DEST_PATH_IMAGE223
。
在本实施例中，为降低前方无人机超出仿猛禽视觉相对导航视场的情况，设置多无人机期望的初始目标高度
Figure 127899DEST_PATH_IMAGE224
，使所有无人机先通过高度控制实现高度一致。每个无人机通过地面站每1s获取一次低频低精度的各个无人机GPS位置，并综合所有无人机的前向位置信息，比较判断当前无人机是否位于所有无人机的最前方。定义前向位置控制标志位
Figure 351070DEST_PATH_IMAGE225
及基础飞行速度
Figure 947137DEST_PATH_IMAGE226
，若当前无人机位于最前方，当前无人机为总领导无人机，以
Figure 267260DEST_PATH_IMAGE227
的速度做定直平飞，并令
Figure 900366DEST_PATH_IMAGE228
；若当前无人机位于非最前方且没有检测到其它无人机，当前无人机需要寻找领导无人机使其出现在视觉导航视场内，无人机保持前向速度
Figure 735467DEST_PATH_IMAGE229
的同时，先通过侧向位置控制向中心位置靠拢，并令
Figure 10591DEST_PATH_IMAGE228
，过程中如果检测到其它无人机，则进行计算仿鹳雁群集群编队位置距离，并确定领导-跟随关系及编队目标位置，如果依旧没有检测到其它无人机，当前无人机以
Figure 981958DEST_PATH_IMAGE230
的速度加速飞行，并令
Figure 51545DEST_PATH_IMAGE228
，直至寻找到领导无人机或成为新的领导无人机。
在本实施例中，若当前无人机跟随检测到的距离最近的领导无人机，将距离集
Figure 314555DEST_PATH_IMAGE231
中的元素按从小到大的顺序排列，并选取距离最小的无人机作为领导无人机即确定领导-跟随关系，记与领导无人机的相对位置为
Figure 252424DEST_PATH_IMAGE232
。然后定义期望相对位置为
Figure 484822DEST_PATH_IMAGE233
，根据领导与跟随无人机的相对位置关系确定集群编队目标位置，若
Figure 584365DEST_PATH_IMAGE234
，当前无人机跟随领导者的左侧跟随位置
Figure 269424DEST_PATH_IMAGE235
；若
Figure 948667DEST_PATH_IMAGE236
，当前无人机跟随领导者的右侧跟随位置
Figure 894626DEST_PATH_IMAGE237
。
本实施例中，基于猛禽视觉导航设计了仿鹳雁群迁徙行为的集群自主编队控制方法，能够实现多无人机的分布式编队，提高了集群编队方法的自主性与自愈性。
如图9所示，为本发明的无人机集群方位控制步骤图。通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制，包括：
S51：计算定直平飞位置、编队目标位置、寻找领导者目标位置与无人机当前位置的差值；
将仿猛禽视觉相对导航输出的领导无人机的相对位置
Figure 40437DEST_PATH_IMAGE238
与期望相对位置
Figure 337426DEST_PATH_IMAGE239
做差，得到目标位置的差
Figure 492464DEST_PATH_IMAGE240
：
Figure 292930DEST_PATH_IMAGE241
S52：计算每个无人机间的相对距离，当相对距离过小时，无人机以一定的速度降低高度，避免碰撞；
S53：根据高度方向目标位置差，得到z方向的无人机底层控制速度指令；
根据以下公式（13）构建多无人机高度相对位置控制：
Figure 203117DEST_PATH_IMAGE242
其中，
Figure 597189DEST_PATH_IMAGE243
为侧向速度指令比例系数；
S54：根据侧向目标位置差，得到
Figure 677622DEST_PATH_IMAGE244
方向的无人机底层控制速度指令；
根据以下公式（14）构建多无人机侧向相对位置控制：
Figure 473540DEST_PATH_IMAGE245
其中，
Figure 351366DEST_PATH_IMAGE246
为侧向速度指令比例系数；
S55：根据是否为总领导无人机或未检测其它无人机的跟随者，确定基本飞行速度；根据前向目标位置差，计算得到
Figure 967155DEST_PATH_IMAGE247
方向的无人机底层控制速度指令；
根据以下公式（15）构建多无人机前向相对位置控制：
Figure 791891DEST_PATH_IMAGE248
其中，
Figure 566949DEST_PATH_IMAGE249
为前向速度指令比例系数；
将相对位置控制得到的速度控制指令
Figure 756622DEST_PATH_IMAGE250
输入内环多无人机底层控制，在本实施例中，
Figure 249920DEST_PATH_IMAGE251
。
本实施例中，在构建无人机集群的高度、侧向以及前向相对位置控制后，还需要进行多无人机集群编队的避碰设计。
综合利用低频低精度GPS信息与仿猛禽视觉相对导航相对位置数据，当相邻无人机间的距离小于
Figure 284873DEST_PATH_IMAGE252
时，给定高度方向速度
Figure 117699DEST_PATH_IMAGE253
，无人机降低高度以实现不同个体间的避碰。
在本实施例中，当相邻无人机间的距离小于
Figure 337328DEST_PATH_IMAGE254
时，给定高度方向速度
Figure 193289DEST_PATH_IMAGE255
，无人机降低高度以实现不同个体间的避碰。最终编队场景如图10所示。
最后，判断无人机集群编队程序是否结束：根据公式（1）若
Figure 156565DEST_PATH_IMAGE256
大于最大仿真时间
Figure 781582DEST_PATH_IMAGE257
，则结束仿真并绘制多无人机集群自主编队过程数据曲线;若
Figure 378304DEST_PATH_IMAGE256
小于最大仿真时间
Figure 580615DEST_PATH_IMAGE257
，则重复执行步骤S1-S6。
在本实施例中，若
Figure 957370DEST_PATH_IMAGE256
大于最大仿真时间
Figure 827106DEST_PATH_IMAGE258
，则结束仿真并绘制多无人机集群自主编队过程数据曲线，数据曲线如图11a-11c所示。
本发明还提供一种多无人机集群自主编队导航控制装置，该装置应用于如上述方法，包括：
多无人机底层控制模块，用于建立四旋翼无人机六自由度非线性模型，并对无人机数量、初始位置、仿真时间以及平飞速度的参数进行初始化；并进行无人机控制分配及电机动态模拟仿真；更新多无人机状态量信息，计算仿真时间
Figure 591799DEST_PATH_IMAGE256
，其中，
Figure 156773DEST_PATH_IMAGE259
Figure 727432DEST_PATH_IMAGE260
为仿真步长;
对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否结束多无人机集群自主编队；若
Figure 389357DEST_PATH_IMAGE261
大于最大仿真时间
Figure 324952DEST_PATH_IMAGE262
，则结束仿真并绘制多无人机集群自主编队过程数据曲线；
多无人机相对位置导航模块，用于获取多无人机集群编队导航图像，进行仿猛禽视觉的候选无人机检测，并进行杂点滤除得到感兴趣区域ROI区，通过先验标志点对ROI区进行干扰区域滤除，得到剩余ROI区；判断干扰区域滤除后，是否存在剩余ROI区，以及通过判断剩余ROI区中是否存在先验标志点，判断是否检测到无人机；对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队；
多无人机集群自主编队控制模块，用于对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导-跟随关系及编队目标位置；
多无人机相对位置控制模块，用于计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制。
多无人机底层控制模块，包括与多无人机数量一一对应的电机动态监测子模块以及控制分配子模块；控制分配子模块包括对无人机的横侧向速度控制以及纵向速度控制。
本发明的一种仿生智能多无人机集群自主编队导航控制装置，旨在通过设计一种基于仿生智能的多无人机集群编队导航控制系统，提高复杂情况下多无人机系统集群编队视觉导航与协同控制的精度，促进多无人机自主集群作战的成功实现。
本实施例中，如图12所示，为本发明的装置框图。多无人机相对位置导航模块，包括仿猛禽视觉无人机检测、是否检测到无人机与重合情况判断、单一无人机相对导航、重合无人机相对导航及坐标系转换。仿猛禽视觉无人机检测，获取集群编队视觉导航图像，进行仿猛禽视觉的候选无人机检测，并以形状特征与编制点特征滤除干扰区域；是否检测到无人机与重合情况判断，根据滤除干扰后的导航图像判断是否检测到无人机，分别对应集群自主编队控制跟随无人机两种情况，并且检测到无人机时，需判断检测到的单个标志区内是否只有一种颜色标志点，以区分非重合无人机相对导航与重合无人机相对导航两种情况；若只有一种颜色标志点，非重合无人机相对导航对标志区轮廓分块进行圆拟合，选取最优拟合结果，并进行位姿估计；若不是只有一种颜色标志点，重合无人机相对导航先确定重合标志区分割线，再进行多机分块圆拟合，选取最优拟合结果，然后通过位姿估计得到相机系下的相对位置关系；坐标系转换将相机系下的无人机位置先转换到机体系再转换到惯性系，为多无人机集群自主编队系统提供导引信息。
多无人机集群自主编队控制模块，包括确定总领导无人机、总领导无人机自主运动、跟随无人机寻找领导者、仿鹳雁群的跟随无人机集群编队。确定总领导无人机，获取地面站低频多无人机GPS位置，根据多无人机相对位置关系，确定当前无人机是否为总领导者；若当前无人机为总领导无人机，该无人机到达一致高度后，以
Figure 501855DEST_PATH_IMAGE263
的速度做定直平飞；若当前无人机不是总领导无人机，在到达一致高度后，如果未检测到其它无人机，该无人机先向中心位置靠拢，以提高检测到其它无人机的几率，若到达中心位置附近且仍未检测到其他无人机，以
Figure 485992DEST_PATH_IMAGE264
Figure 530653DEST_PATH_IMAGE265
的速度快速飞行直至检测到其它无人机或称为总领导者；如果检测到其它无人机，仿鹳雁群的跟随无人机集群编队先计算检测到无人机编队位置距离计算，再根据距离规则确定领导-跟随关系，进而得到跟随左侧或右侧集群编队目标位置。
多无人机相对位置控制模块，包括计算目标位置差、避碰控制、高度控制、侧向位置控制、前向位置控制。计算目标位置差，具体计算定直平飞位置、编队目标位置、寻找领导者目标位置与无人机当前位置的差值；避碰控制，计算不同无人机间的相对距离，当相对距离过小时，无人机以一定的速度降低高度，避免碰撞；高度控制，根据高度方向目标位置差，得到
Figure 433887DEST_PATH_IMAGE266
方向的无人机底层控制速度指令
Figure 707873DEST_PATH_IMAGE267
；侧向位置控制，根据侧向目标位置差，得到
Figure 558017DEST_PATH_IMAGE268
方向的无人机底层控制速度指令
Figure 256852DEST_PATH_IMAGE269
；前向位置控制，一种情况根据是否为总领导无人机或未检测其它无人机的跟随者，确定基本飞行速度，另一种情况根据前向目标位置差，计算得到
Figure 799829DEST_PATH_IMAGE270
方向的无人机底层控制速度指令
Figure 154587DEST_PATH_IMAGE271
。
多无人机底层控制模块，包括纵向速度控制、横侧向速度控制、姿态角控制、控制分配、电机动态。纵向速度控制，根据
Figure 480526DEST_PATH_IMAGE272
方向速度误差，设计纵向速度PID控制器，解算得到期望的四旋翼无人机总拉力
Figure 237129DEST_PATH_IMAGE273
；横侧向速度控制，根据
Figure 747745DEST_PATH_IMAGE274
方向速度误差，分别设计横侧向速度PID控制器，阶段得到四旋翼无人机姿态角指令；姿态角控制，计算外环姿态角指令值与实际值的误差，设计姿态角PID控制器，解算得到无人机三轴期望力矩
Figure 996324DEST_PATH_IMAGE275
；控制分配，根据四旋翼控制效率矩阵，计算得到四旋翼各个电机的期望转速；电机动态，考虑电机的动态特性，解算得到电机的实际转速，以实时更新无人机的所有状态量信息。
本发明提供了完整多无人机集群自主编队视觉导航与控制系统框架及其工作流程，系统框架以及流程合理、高效，能够满足多无人机集群自主编队导航与控制性能指标；本发明通过模仿猛禽颜色视觉机制，构建仿猛禽颜色双拮抗检测方法，提高了无人机标志球及标志点的检测效率及准确性；本发明给出了两种相对导航的组合方法，解决了标志球部分遮挡、单一无人机、重合无人机等情况下的近距视觉相对导航问题且视觉导航精度较高，能够实现集群编队全过程的视觉导航，提高了仿猛禽视觉相对导航的适应性；本发明基于仿猛禽视觉导航设计了仿鹳雁群迁徙行为的集群自主编队控制方法，能够实现多无人机的分布式编队，提高了集群编队方法的自主性与自愈性。
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成，也可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器，磁盘或光盘等。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种仿生智能多无人机集群自主编队导航控制方法，其特征在于，所述方法包括：
S1：通过多无人机底层控制模块，建立四旋翼无人机六自由度非线性模型，并进行参数初始化；进行无人机控制分配及电机动态模拟仿真；更新多无人机状态量信息，计算仿真时间
Figure 509335DEST_PATH_IMAGE001
，其中，
Figure 59134DEST_PATH_IMAGE002
Figure 564065DEST_PATH_IMAGE003
为仿真步长；
S2：通过多无人机相对位置导航模块，获取多无人机集群自主编队的导航图像，进行候选无人机检测，并进行杂点滤除，得到感兴趣区域ROI区，通过设定无人机的先验标志点，对ROI区进行干扰区域滤除，得到剩余ROI区；以及通过判断剩余ROI区中是否存在先验标志点，判断是否检测到无人机；
利用仿猛禽视觉颜色双拮抗机制,构建多无人机集群自主编队的标志球及标志点检测方法，提取多无人机集群编队近距导航图像中不同波段信号信息，获取多无人机集群自主编队的导航图像；
通过在长波、中波、短波通路中分别保持红色、绿色和蓝色成分灰度值对比度并削弱其它颜色成分灰度值对比度来构建仿猛禽视觉颜色双拮抗机制模型，通过公式（2）来检测多无人机集群编队标志区中红色、绿色、蓝色信息：
Figure 924639DEST_PATH_IMAGE004
其中，
Figure 515020DEST_PATH_IMAGE005
分别为长波、中波、短波以及紫外波段四个通路对图像的输出响 应；
Figure 353663DEST_PATH_IMAGE006
分别为输入可见光导航图像中三个颜色通道的灰度值；
Figure 994991DEST_PATH_IMAGE007
，
Figure 260888DEST_PATH_IMAGE008
分 别为长波、中波、短波阈值；
Figure 604144DEST_PATH_IMAGE009
表示取
Figure 246478DEST_PATH_IMAGE010
与
Figure 726001DEST_PATH_IMAGE011
的最大值；
Figure 412066DEST_PATH_IMAGE012
表示取
Figure 977040DEST_PATH_IMAGE010
与
Figure 423064DEST_PATH_IMAGE011
的最小值；
将RGB颜色空间信息转换到HSV颜色空间后，通过公式（3）检测无人机集群编队标志区的紫色、青色标志点：
Figure 288252DEST_PATH_IMAGE013
其中，
Figure 895951DEST_PATH_IMAGE014
分别为
Figure 948221DEST_PATH_IMAGE015
空间的三个分量；
Figure 694808DEST_PATH_IMAGE016
分别为
Figure 414503DEST_PATH_IMAGE017
三 个分量的最大值和最小值；计算仿猛禽视觉双拮抗模型长波通道输出，并对导航图像进行 二值化处理，得到二值图像；
对二值图像进行孔洞填充、滤除杂点，得到剩余ROI区；所述剩余ROI区为集群编队候选无人机；
将ROI区域边缘分成四部分，对每一部分采用最小二乘法进行圆拟合，定义圆拟合度
Figure 458682DEST_PATH_IMAGE018
：
Figure 732669DEST_PATH_IMAGE019
其中，
Figure 520496DEST_PATH_IMAGE020
表示二值图像中的第
Figure 94697DEST_PATH_IMAGE020
个候选无人机，
Figure 559045DEST_PATH_IMAGE021
表示第
Figure 585907DEST_PATH_IMAGE020
个候选无人机边缘的第
Figure 911846DEST_PATH_IMAGE021
部分，
Figure 340553DEST_PATH_IMAGE022
；
Figure 726535DEST_PATH_IMAGE023
表示第
Figure 240693DEST_PATH_IMAGE021
部分拟合像素点数；
Figure 855477DEST_PATH_IMAGE024
表示第
Figure 404270DEST_PATH_IMAGE021
部分的
Figure 961153DEST_PATH_IMAGE025
个像素点在拟合圆上的像素点数，
Figure 697028DEST_PATH_IMAGE024
通过公式（4）计算：
Figure 630349DEST_PATH_IMAGE027
其中，
Figure 17337DEST_PATH_IMAGE028
为第
Figure 745121DEST_PATH_IMAGE021
部分的拟合像素点；
Figure 233871DEST_PATH_IMAGE029
为拟合圆的中心像素坐标；
Figure 970883DEST_PATH_IMAGE030
为拟合圆的半径；
Figure 963110DEST_PATH_IMAGE031
为判断拟合结果的阈值；从候选无人机的边缘拟合结果中选取最优拟合作为最终用于位姿估计的拟合圆；对于候选无人机干扰区，选取圆拟合度的区分阈值
Figure 861796DEST_PATH_IMAGE032
，进行干扰区域滤除，若
Figure 322995DEST_PATH_IMAGE033
，则拟合像素点不在拟合圆上，干扰区域为非圆；
S3：相对位置导航模块对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；匹配正确的所有标志点，利用
Figure 598119DEST_PATH_IMAGE034
算法进行精确位姿测量，得到相机坐标系下的无人机位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到与前方无人机在地面惯性系下的位置差
Figure 444852DEST_PATH_IMAGE035
，利用仿猛禽相对导航得到的相对位置差进行后续的仿鹳雁群集群编队；
S4：通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导-跟随关系及编队目标位置；
S5：通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制；
S6：通过多无人机底层控制模块，对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否结束多无人机集群自主编队；若
Figure 780018DEST_PATH_IMAGE036
大于最大仿真时间
Figure 977782DEST_PATH_IMAGE037
，则结束仿真并绘制多无人机集群自主编队过程数据曲线，否则重复执行S1-S6。
2.根据权利要求1所述的仿生智能多无人机集群自主编队导航控制方法，其特征在于，所述建立四旋翼无人机六自由度非线性模型，并进行参数初始化，包括：
Figure 56596DEST_PATH_IMAGE038
其中，
Figure 7103DEST_PATH_IMAGE039
分别为无人机惯性系下
Figure 513171DEST_PATH_IMAGE040
三 个方向的位置与速度、滚转角、俯仰角、偏航角以及机体轴三个方向的旋转角速度，
Figure 932651DEST_PATH_IMAGE041
为相应变量的一阶微分；
Figure 549577DEST_PATH_IMAGE042
为无人机螺旋桨产生的总拉 力；
Figure 636482DEST_PATH_IMAGE043
为无人机质量，
Figure 47872DEST_PATH_IMAGE044
为重力加速度；
Figure 970959DEST_PATH_IMAGE045
分别为三轴转动惯量；
Figure 391577DEST_PATH_IMAGE046
为陀螺力矩；
Figure 67409DEST_PATH_IMAGE047
为无人机螺旋桨在机体轴上 产生的力矩。
3.根据权利要求1所述的仿生智能多无人机集群自主编队导航控制方法，其特征在于，所述通过判断剩余ROI区中是否存在先验标志点，判断是否检测到无人机，包括：
通过多无人机相对位置导航模块进行干扰区域滤除后，判断是否存在剩余ROI区，若不存在剩余ROI区，则确定无有效无人机；若存在剩余ROI区，则进一步判断剩余ROI区内是否存在先验标志点；若剩余ROI区内没有先验标志点，确定无有效无人机；若存在先验标志点，则确定检测到有效无人机。
4.根据权利要求3所述的仿生智能多无人机集群自主编队导航控制方法，其特征在于，所述对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹配，进行无人机的位姿估计；通过位姿估计的结果得到多无人机的相对位置，对多无人机进行仿鹳雁群集群编队，包括：
S31：若检测到无人机，则通过仿猛禽颜色检测和HSV方法，提取剩余ROI区内的先验标志点，计算检测到的单个无人机连通域内的先验标志点种类及像素坐标，进而判断剩余ROI区是否只有一种颜色的标志点；
S32：若只有一种颜色的标志点，则为非重叠无人机，直接进行轮廓分块圆拟合，选取最优拟合结果，并进行位姿估计；
若非一种颜色的标志点，则为重叠无人机，需确定重合无人机标志球的分割线，进行多 机分块圆拟合，选取最优拟合结果，通过位姿估计得到相机系下的相对位置关系；计算各颜 色标志点的像素坐标中心
Figure 915279DEST_PATH_IMAGE048
与
Figure 309351DEST_PATH_IMAGE049
，以及各颜色标志点平均像 素点数
Figure 268080DEST_PATH_IMAGE050
与
Figure 578844DEST_PATH_IMAGE051
，进而根据如下公式（7）计算
Figure 332037DEST_PATH_IMAGE052
连线上重合标志球的分割点
Figure 213405DEST_PATH_IMAGE053
：
Figure 975825DEST_PATH_IMAGE054
其中，
Figure 891828DEST_PATH_IMAGE055
为连线
Figure 815922DEST_PATH_IMAGE056
的分割比例系数；
Figure 935319DEST_PATH_IMAGE057
为面积比例因子;得到重合标志球分 割点
Figure 970271DEST_PATH_IMAGE058
后，然后计算与连线
Figure 6360DEST_PATH_IMAGE056
垂直且过
Figure 366934DEST_PATH_IMAGE058
点的分 割线
Figure 957315DEST_PATH_IMAGE059
以及斜率
Figure 61538DEST_PATH_IMAGE060
，其中斜率
Figure 201401DEST_PATH_IMAGE060
为：
Figure 467297DEST_PATH_IMAGE061
计算重合标志球分割线各点像素坐标，设置导航图像分辨率
Figure 810554DEST_PATH_IMAGE062
Figure 187308DEST_PATH_IMAGE063
分别为导航图像两个方向的像素坐标最大值，根据分割线斜率
Figure 932410DEST_PATH_IMAGE064
，计算分割线所有像素点坐标点集
Figure 634787DEST_PATH_IMAGE065
：
Figure 950493DEST_PATH_IMAGE066
其中，
Figure 396518DEST_PATH_IMAGE067
取1到
Figure 996127DEST_PATH_IMAGE068
，并分别计算对应的
Figure 869405DEST_PATH_IMAGE069
；
S33：通过位姿估计/相对位置关系为多无人机集群自主编队系统提供导引信息，进行多无人机进行仿鹳雁群集群编队。
5.根据权利要求4所述的仿生智能多无人机集群自主编队导航控制方法，其特征在于，所述通过仿猛禽颜色检测和HSV方法，提取剩余ROI区内的先验标志点，包括：
将二值图像与导航图像进行相与操作，得到只包含ROI区的彩色图像，进而在ROI彩色区内通过仿猛禽颜色检测和HSV方法，提取蓝色、绿色、紫色、青色先验标志点。
6.根据权利要求1所述的仿生智能多无人机集群自主编队导航控制方法，其特征在于，所述通过多无人机集群自主编队控制模块，对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导-跟随关系及编队目标位置，包括：
S41：设置多无人机期望的初始目标高度
Figure 921674DEST_PATH_IMAGE070
，将每个无人机控制为高度一致；
S42：每个无人机通过地面站每1s获取一次低频低精度的无人机GPS位置，并综合所有无人机的前向位置信息，判断待选定无人机是否位于所有无人机的最前方；
S43：若待选定无人机位于最前方，则确定待选定无人机为总领导无人机；若待选定无人机位于非最前方且没有检测到其它无人机，则待选定无人机为跟随无人机，需要寻找领导无人机，并使所述领导无人机出现在视觉导航视场内；
S44：若检测到其它无人机，则多个跟随无人机的集群编队计算检测到无人机的位置距离，根据位置距离确定领导-跟随关系，进而得到跟随左侧或右侧集群的编队目标位置；
经过仿猛禽视觉相对导航获取检测到无人机的相对位置
Figure 171390DEST_PATH_IMAGE071
，根据如下公 式（10）计算仿鹳雁群集群编队位置距离集
Figure 609194DEST_PATH_IMAGE072
Figure 653373DEST_PATH_IMAGE073
其中，
Figure 458518DEST_PATH_IMAGE074
为侧向比例因子，
Figure 980766DEST_PATH_IMAGE075
的取值范围与检测到无人机数量一致；当前无人机跟随检 测到的距离最近的相对领导无人机，将距离集
Figure 554967DEST_PATH_IMAGE076
中的元素按从小到大的顺序排列，并选取 距离最小的无人机作为领导无人机，即确定领导-跟随关系，记与领导无人机的相对位置为
Figure 770048DEST_PATH_IMAGE077
。
7.根据权利要求6所述的仿生智能多无人机集群自主编队导航控制方法，其特征在于，所述通过多无人机相对位置控制模块，计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制，包括：
S51：计算定直平飞位置、编队目标位置、寻找领导者目标位置与无人机当前位置的差值；
将仿猛禽视觉导航输出的领导无人机的相对位置
Figure 547642DEST_PATH_IMAGE078
与期望相对位置
Figure 873581DEST_PATH_IMAGE079
做差，得到目标位置的差
Figure 302288DEST_PATH_IMAGE080
Figure 688270DEST_PATH_IMAGE081
S52：计算每个无人机间的相对距离，当相对距离过小时，无人机以一定的速度降低高度，避免碰撞；
S53：根据高度方向目标位置差，得到z方向的无人机底层控制速度指令；
根据以下公式（12）构建多无人机高度相对位置控制：
Figure 202428DEST_PATH_IMAGE082
其中，
Figure 332058DEST_PATH_IMAGE083
为高度方向速度指令比例系数；
S54：根据侧向目标位置差，得到y方向的无人机底层控制速度指令；
根据以下公式（13）构建多无人机侧向相对位置控制：
Figure 864540DEST_PATH_IMAGE084
其中，
Figure 421423DEST_PATH_IMAGE085
为侧向速度指令比例系数；
S55：根据是否为总领导无人机或未检测其它无人机的跟随者，确定基本飞行速度；根 据前向目标位置差，计算得到
Figure 422877DEST_PATH_IMAGE086
方向的无人机底层控制速度指令；
根据以下公式（14）构建多无人机前向相对位置控制：
Figure 90619DEST_PATH_IMAGE087
其中，
Figure 493918DEST_PATH_IMAGE088
为前向速度指令比例系数；
Figure 221703DEST_PATH_IMAGE089
表示前向位置控制标志位；
Figure 195606DEST_PATH_IMAGE090
表示基础飞行速度；
将相对位置控制得到的速度控制指令
Figure 667039DEST_PATH_IMAGE091
输入内环多无人机底层控制。
8.一种仿生智能多无人机集群自主编队导航控制装置，其特征在于，所述装置应用于如权利要求1-7任意一项所述的方法，包括：
多无人机底层控制模块，用于建立四旋翼无人机六自由度非线性模型，并对无人机数 量、初始位置、仿真时间以及平飞速度的参数进行初始化；并进行无人机控制分配及电机动 态模拟仿真；更新多无人机状态量信息，计算仿真时间
Figure 659265DEST_PATH_IMAGE001
，其中，
Figure 823531DEST_PATH_IMAGE002
Figure 799577DEST_PATH_IMAGE003
为仿真步长;
对无人机的所有状态量信息进行实时更新并根据公式（1）得到的仿真时间，判断是否 结束多无人机集群自主编队；若
Figure 792809DEST_PATH_IMAGE001
大于最大仿真时间
Figure 170701DEST_PATH_IMAGE092
，则结束仿真并绘制多无人机集群 自主编队过程数据曲线；
多无人机相对位置导航模块，用于获取多无人机集群编队导航图像，进行仿猛禽视觉候选无人机检测，并进行杂点滤除得到感兴趣区域ROI区，通过先验标志点对ROI区进行干扰区域滤除，得到剩余ROI区；
判断干扰区域滤除后，是否存在剩余ROI区，以及通过判断剩余ROI区中是否存在先验标志点，判断是否检测到无人机；利用仿猛禽视觉颜色双拮抗机制,构建多无人机集群自主编队的标志球及标志点检测方法，提取多无人机集群编队近距导航图像中不同波段信号信息，获取多无人机集群自主编队的导航图像；
通过在长波、中波、短波通路中分别保持红色、绿色和蓝色成分灰度值对比度并削弱其它颜色成分灰度值对比度来构建仿猛禽视觉颜色双拮抗机制模型，通过公式（2）来检测多无人机集群编队标志区中红色、绿色、蓝色信息：
Figure 974709DEST_PATH_IMAGE093
其中，
Figure 438052DEST_PATH_IMAGE094
分别为长波、中波、短波以及紫外波段四个通路对图像的输出响 应；
Figure 985708DEST_PATH_IMAGE095
分别为输入可见光导航图像中三个颜色通道的灰度值；
Figure 218106DEST_PATH_IMAGE096
Figure 197608DEST_PATH_IMAGE097
分别 为长波、中波、短波阈值；
Figure 882667DEST_PATH_IMAGE098
表示取
Figure 499593DEST_PATH_IMAGE099
与
Figure 586498DEST_PATH_IMAGE100
的最大值；
Figure 997888DEST_PATH_IMAGE101
表示取
Figure 170243DEST_PATH_IMAGE099
与
Figure 574548DEST_PATH_IMAGE100
的最小值；
将RGB颜色空间信息转换到HSV颜色空间后，通过公式（3）检测无人机集群编队标志区的紫色、青色标志点：
Figure 515960DEST_PATH_IMAGE102
其中，H、S、V分别为HSV空间的三个分量；
Figure 363830DEST_PATH_IMAGE103
分别为
Figure 757902DEST_PATH_IMAGE104
三个分量的最 大值和最小值；
计算仿猛禽视觉双拮抗模型长波通道输出，并对导航图像进行二值化处理，得到二值图像；
对二值图像进行孔洞填充、滤除杂点，得到剩余ROI区；所述剩余ROI区为集群编队候选无人机；
将ROI区域边缘分成四部分，对每一部分采用最小二乘法进行圆拟合，定义圆拟合度
Figure 716631DEST_PATH_IMAGE105
：
Figure 512549DEST_PATH_IMAGE106
其中，
Figure 282053DEST_PATH_IMAGE107
表示二值图像中的第
Figure 163421DEST_PATH_IMAGE107
个候选无人机，
Figure 925841DEST_PATH_IMAGE108
表示第
Figure 841844DEST_PATH_IMAGE107
个候选无人机边缘的第
Figure 500358DEST_PATH_IMAGE108
部 分，；表示第
Figure 454911DEST_PATH_IMAGE108
部分拟合像素点数；
Figure 549906DEST_PATH_IMAGE109
表示第
Figure 405866DEST_PATH_IMAGE108
部分的
Figure 510089DEST_PATH_IMAGE110
个像素 点在拟合圆上的像素点数，
Figure 135105DEST_PATH_IMAGE109
通过公式（4）计算：
Figure 151734DEST_PATH_IMAGE111
其中，
Figure 494990DEST_PATH_IMAGE112
为第
Figure 137324DEST_PATH_IMAGE108
部分的拟合像素点；
Figure 882426DEST_PATH_IMAGE113
为拟合圆的中心 像素坐标；
Figure 584803DEST_PATH_IMAGE114
为拟合圆的半径；
Figure 415356DEST_PATH_IMAGE115
为判断拟合结果的阈值；从候选无人机的边缘拟合结果 中选取最优拟合作为最终用于位姿估计的拟合圆；对于候选无人机干扰区，选取圆拟合度 的区分阈值
Figure 845069DEST_PATH_IMAGE116
，进行干扰区域滤除，若
Figure 444678DEST_PATH_IMAGE117
，则拟合像素点不在拟合圆上，干扰区域为 非圆；
对检测到的无人机进行轮廓分块圆拟合，并对最优拟合圆的外接矩形顶点进行顺序匹 配，进行无人机的位姿估计；匹配正确的所有标志点，利用
Figure 317956DEST_PATH_IMAGE118
算法进行精确位姿测量， 得到相机坐标系下的无人机位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转 换，得到与前方无人机在地面惯性系下的位置差
Figure 370225DEST_PATH_IMAGE119
，利用仿猛禽相对导 航得到的相对位置差进行后续的仿鹳雁群集群编队；
多无人机集群自主编队控制模块，用于对仿鹳雁群集群编队后的多无人机进行是否存在总领导无人机的判断；计算多无人机的仿鹳雁群集群编队位置距离，确定领导-跟随关系及编队目标位置；
多无人机相对位置控制模块，用于计算仿鹳雁群集群编队目标位置差，并对多无人机集群进行避碰控制、高度控制、侧向位置控制、前向位置控制。
9.根据权利要求8所述的仿生智能多无人机集群自主编队导航控制装置，其特征在于，所述多无人机底层控制模块，包括与多无人机数量一一对应的电机动态监测子模块以及控制分配子模块；所述控制分配子模块包括对无人机的横侧向速度控制以及纵向速度控制。
CN202110952222.3A 2021-08-19 2021-08-19 一种仿生智能多无人机集群自主编队导航控制方法及装置 Active CN113406975B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110952222.3A CN113406975B (zh) 2021-08-19 2021-08-19 一种仿生智能多无人机集群自主编队导航控制方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110952222.3A CN113406975B (zh) 2021-08-19 2021-08-19 一种仿生智能多无人机集群自主编队导航控制方法及装置

Publications (2)

Publication Number Publication Date
CN113406975A CN113406975A (zh) 2021-09-17
CN113406975B true CN113406975B (zh) 2021-11-30

Family

ID=77688828

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110952222.3A Active CN113406975B (zh) 2021-08-19 2021-08-19 一种仿生智能多无人机集群自主编队导航控制方法及装置

Country Status (1)

Country Link
CN (1) CN113406975B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114020029B (zh) * 2021-11-09 2022-06-10 深圳大漠大智控技术有限公司 一种针对集群的航拍航线自动生成方法、装置及相关组件
CN114384929B (zh) * 2021-12-02 2023-09-12 上海航天控制技术研究所 一种基于偏差优化启发式算法的无人集群编队控制方法
CN115016455A (zh) * 2022-04-24 2022-09-06 福建（泉州）哈工大工程技术研究院 一种机器人集群定位运动方法及系统
CN116359836B (zh) * 2023-05-31 2023-08-15 成都金支点科技有限公司 一种基于超分辨率测向的无人机目标跟踪方法和系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104656663A (zh) * 2015-02-15 2015-05-27 西北工业大学 一种基于视觉的无人机编队感知与规避方法
CN107424156A (zh) * 2017-06-28 2017-12-01 北京航空航天大学 基于仿仓鸮眼视觉注意的无人机自主编队精准测量方法
CN107703966A (zh) * 2017-07-06 2018-02-16 北京航空航天大学 一种基于雁群自组织飞行的无人机自主编队控制方法
CN109813311A (zh) * 2019-03-18 2019-05-28 南京航空航天大学 一种无人机编队协同导航方法
CN110096073A (zh) * 2019-04-18 2019-08-06 北京航空航天大学 仿信鸽智能行为的超大规模无人机集群控制系统及方法
CN111258231A (zh) * 2020-01-20 2020-06-09 北京航空航天大学 仿猛禽视觉导航的自主空中加油对接半物理系统及其方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108268053B (zh) * 2018-01-11 2020-01-17 北京航空航天大学 仿候鸟进化雪堆博弈的无人机自主集群编队轮换控制方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104656663A (zh) * 2015-02-15 2015-05-27 西北工业大学 一种基于视觉的无人机编队感知与规避方法
CN107424156A (zh) * 2017-06-28 2017-12-01 北京航空航天大学 基于仿仓鸮眼视觉注意的无人机自主编队精准测量方法
CN107703966A (zh) * 2017-07-06 2018-02-16 北京航空航天大学 一种基于雁群自组织飞行的无人机自主编队控制方法
CN109813311A (zh) * 2019-03-18 2019-05-28 南京航空航天大学 一种无人机编队协同导航方法
CN110096073A (zh) * 2019-04-18 2019-08-06 北京航空航天大学 仿信鸽智能行为的超大规模无人机集群控制系统及方法
CN111258231A (zh) * 2020-01-20 2020-06-09 北京航空航天大学 仿猛禽视觉导航的自主空中加油对接半物理系统及其方法

Also Published As

Publication number Publication date
CN113406975A (zh) 2021-09-17

Similar Documents

Publication Publication Date Title
CN113406975B (zh) 一种仿生智能多无人机集群自主编队导航控制方法及装置
US11237572B2 (en) Collision avoidance system, depth imaging system, vehicle, map generator and methods thereof
Martínez et al. On-board and ground visual pose estimation techniques for UAV control
CN111258231B (zh) 仿猛禽视觉导航的自主空中加油对接半物理系统及其方法
Roelofsen et al. Reciprocal collision avoidance for quadrotors using on-board visual detection
Eynard et al. UAV altitude estimation by mixed stereoscopic vision
CN102538782B (zh) 一种基于计算机视觉的直升机着陆引导方法
CN109460046B (zh) 一种无人机自然地标识别与自主着陆方法
CN107063261A (zh) 用于无人机精准着陆的多特征信息地标检测方法
CN105549614A (zh) 无人机目标跟踪方法
CN113012219A (zh) 信息处理装置、信息处理方法及计算机可读介质
CN116202489A (zh) 输电线路巡检机与杆塔协同定位方法及系统、存储介质
CN111812978B (zh) 一种多无人机协作slam方法与系统
CN114815871A (zh) 一种基于视觉的垂直起降无人机动平台自主降落方法
CN114689030A (zh) 一种基于机载视觉的无人机辅助定位方法及系统
Chiu et al. Vision-only automatic flight control for small UAVs
CN115272458A (zh) 用于固定翼无人机着陆阶段的视觉定位方法
CN109764864A (zh) 一种基于颜色识别的室内无人机位姿获取方法及系统
CN112987765A (zh) 一种仿猛禽注意力分配的无人机/艇精准自主起降方法
CN105930766A (zh) 无人机
JP7547045B2 (ja) 情報処理装置、情報処理方法およびプログラム
CN110618696B (zh) 一种陆空一体化测绘无人机
CN117270565A (zh) 一种基于视觉的机载自主感知及飞行系统
CN116185049A (zh) 基于视觉引导的无人直升机自主着舰方法
US20230073120A1 (en) Method for Controlling an Unmanned Aerial Vehicle for an Inspection Flight to Inspect an Object and Inspection Unmanned Aerial Vehicle

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant