一种多无人机协同编队机间防碰撞控制方法与流程

1.本技术涉及无人机技术领域，尤其涉及无人机编队控制领域，更具体的说涉及一种多无人机协同编队机间防碰撞控制方法。

背景技术：

2.随着无人机技术的发展，多无人机协同编队成为各无人机研发强国和科研工作者的研究热点。多无人机协同编队具有诸多好处：首先，采用多机协同侦察、联合作战的模式，可以在一定程度上提高单机单次飞行的任务执行成功率；其次，在目标打击、突击突防方面，编队飞行可以提高单次飞行完成任务的效率；最后，多无人机协同编队还能够实现“蜂”、“编组”、“忠诚僚机”和诱饵等多种应用场景。
3.而多无人机在以楔形队、梯形队、横队、纵队和v形队等诸多设计队形进行协同编队飞行时，为了更好地发挥出协同编队的价值，往往机间间距要求很小，一般在十几米至百米范围内。因此，多无人机协同编队机间防碰撞控制不仅关乎队形保持的效果，更关乎飞行安全，尤其是对于高速编队飞行，机间防碰撞控制对飞行安全的影响愈加显著。
4.目前，业内专家学者提出了许多的多无人机协同编队机间防碰撞控制方法，例如，基于行为法、虚拟结构法、图论法、基于一致性方法、模型预测控制、模糊逻辑、神经网络技术、信息感知视觉传感器控制等，专家学者对这些方法进行了很多探索研究与试验，并得到了不少可喜的成果。然而，这些方法基本都存在以下不足：首先利用的算法较为复杂，甚至还结合了智能算法、仿生学等内容，不易于工程实现；其次，机间传输数据量大，对编队飞行过程中机间信息交互的可靠性和信息吞吐容量要求较高；最后，还有的算法需要依托于复杂的传感器系统才能实现编队控制，提高了无人机系统的复杂度和设备成本；另外，算法复杂使得编队控制与单机控制耦合严重，不便于飞行控制律设计和机载软件开发，当需要将算法推广至新构型飞机时，需要对飞行控制律和机载软件进行大量改动，增加了设计风险，并且也增长了设计周期。
5.例如公开号为cn111580542a，公开日为2020年08月25日，发明名称为“动态无人机编队控制方法、装置及存储介质”的发明专利，以及公开号为cn108958289a，公开日为2018年12月07日，发明名称为“基于相对速度障碍的集无人机避碰方法”的发明专利。
6.上述两个现有技术中，虽然也涉及pid控制，但是与本技术的方案存在本质上的区别。首先两个文件分别是针对多旋翼和四旋翼无人机的编队控制，而本技术提出的方案是针对固定翼无人机的编队控制；其次，第一个对比文件中无人机的期望位置由地面站发送，且x、y、z三个方向都是通过计算实际位置与期望位置的偏差的比例、微分、积分三项之和得到期望速度，而本技术中僚机的期望位置由长机发送，僚机跟踪的是实时变化的长机后方位置，并不是地面站发送的设置位置，且长机将自身的速度目标值发送给僚机作为僚机速度目标值的基准，体现了僚机对长机速度目标的跟踪；再者，第一个对比文件中x、y、z三个方向的外环控制都有速度控制环，而本技术的方案则直接通过姿态角实现对侧向位置、高向位置的控制，控制响应更加迅速；第二个对比文件则需要通过复杂的避碰算法优化求解
期望速度，而本技术通过前向位置误差的pid控制，并以长机的速度目标值作为基准，即可得到僚机的速度目标值，从而实现避碰控制，并且第二个对比文件中的编队中，各无人机都承担防碰撞责任，都协同参与防碰撞控制；而本技术提出的方案中长机作为领航者角负责对预订航线进行跟踪，编队的防撞控制则完全由僚机担负，编队防撞控制实现更为简单，僚机控制器兼具了跟踪和防碰撞功能。

技术实现要素：

7.为了解决上述现有技术中存在的问题和不足，本技术提出了一种多无人机协同编队机间防碰撞控制方法，本方法采用pid控制结构，飞行控制律算法简单，易于设计人员掌握，机载软件开发也十分便利，能够快速向新构型飞机进行设计移植，并且编队飞行过程中机间交互信息量少，长机仅需向僚机发送经纬高、水平地速、长机速度目标值等几个状态量，对机间通信信道容量要求低，机间信息交互可靠性高；由于不需要额外增加视觉感知等传感器，利用单机飞行原本所配备的传感器即可实现编队控制，因此，还降低了无人机系统的复杂度和机载设备成本。
8.为了实现上述发明目的，本技术的技术方案具体如下：
9.一种多无人机协同编队机间防碰撞控制方法，包括如下步骤：
10.确定长机与僚机之间的间距，包括长机与僚机之间的前向距离、侧向距离以及高向距离；
11.设计多无人机协同编队前向机间防碰撞控制器、侧向机间防碰撞控制器以及高向机间防碰撞控制器；
12.前向机间防碰撞控制器输出僚机发动机油门指令，控制僚机发动机油门；侧向机间防碰撞控制器输出僚机副翼舵指令，控制僚机副翼；高向机间防碰撞控制器输出僚机升降舵指令，控制僚机升降舵；这三个控制器共同组成机间防碰撞控制器，最终实现多无人机协同编队机间防碰撞控制。
13.进一步地，所述确定长机与僚机之间的间距，包括：
14.步骤s11.建立长机牵连坐标系；
15.步骤s12.计算僚机重心在长机牵连坐标系中的坐标值；
16.步骤s13.建立长机航迹坐标系；
17.步骤s14.将步骤s12得到的僚机重心坐标值转换至长机航迹坐标系中；
18.步骤s15.计算长机与僚机的前向距离；
19.步骤s16.计算长机与僚机的侧向距离；
20.步骤s17.计算长机与僚机的高向距离。
21.进一步地，所述设计多无人机协同编队前向机间防碰撞控制器，包括：
22.步骤s21.设计僚机前向距离控制回路，该回路的输出量为僚机表速目标值；
23.步骤s22.基于步骤s21得到的僚机表速目标值，设计僚机表速控制回路，该回路的输出量为僚机表速微分目标值；
24.步骤s23.基于步骤s22得到的僚机表速微分目标值，设计僚机表速微分控制回路，该回路的输出量为僚机发动机油门指令。
25.进一步地，所述设计多无人机协同编队侧向机间防碰撞控制器，包括：
26.步骤s31.设计僚机侧向距离控制回路，该回路的输出量为僚机滚转角指令；
27.步骤s32.基于步骤s31得到的僚机滚转角指令，设计僚机滚转角控制回路，该回路的输出量为僚机滚转角速率指令；
28.步骤s33.基于步骤s32得到的僚机滚转角速率指令，设计僚机滚转角速率内环控制回路，该回路的输出量为僚机副翼舵指令。
29.进一步地，所述设计多无人机协同编队高向机间防碰撞控制器，包括：
30.步骤s41.设计僚机高向距离控制回路，该回路的输出量为僚机俯仰角指令；
31.步骤s42.基于步骤s41得到的僚机俯仰角指令，设计僚机俯仰角控制回路，该回路的输出量为僚机俯仰角速率指令；
32.步骤s43.基于步骤s42得到的僚机俯仰角速率指令，设计僚机俯仰角速率内环控制回路，该回路的输出量为僚机升降舵指令。
33.进一步地，所述步骤s11中，建立长机牵连坐标系，包括：
34.以长机重心o为坐标原点，地理北向为坐标轴on轴的正方向，地理东向为坐标轴oe轴的正方向，og轴垂直于one平面且指向地心为正方向。
35.进一步地，所述步骤s12中，僚机重心在长机牵连坐标系中的坐标值如下
[0036][0037]
其中，(n,e,g)为僚机重心在长机牵连坐标系中的坐标值，r为地球赤道平均半径，e为地球扁率，hw为僚机高度，h
l
为长机高度，lonw为僚机经度，latw为僚机纬度，lon
l
为长机经度，lat
l
为长机纬度。
[0038]
进一步地，所述步骤s13中，建立长机航迹坐标系，包括：
[0039]
以长机重心o为坐标原点，长机水平地速的矢量方向为ox轴正方向，oy轴在水平面内垂直于ox轴且指向长机右侧为正，oz轴垂直于oxy平面且指向下为正。
[0040]
进一步地，所述步骤s14中，僚机在长机牵连坐标系中的坐标值转换至长机航迹坐标系中，其数学表达式如下
[0041][0042]
其中，(x,y,z)为僚机重心在长机航迹坐标系中的坐标值，ψ为长机航迹偏角。
[0043]
进一步地，所述步骤s15中，长机与僚机的前向距离为长机重心与僚机重心的距离在长机航迹坐标系中ox轴上的投影。
[0044]
进一步地，所述步骤s16中，长机与僚机的侧向距离为长机重心与僚机重心的距离在长机航迹坐标系中oy轴上的投影。
[0045]
进一步地，所述步骤s17中，长机与僚机的高向距离为长机重心与僚机重心的距离在长机航迹坐标系中oz轴上的投影。
[0046]
进一步地，所述步骤s21中，设计僚机前向距离控制回路，该回路的输出量为僚机
表速目标值，包括：
[0047]
僚机前向距离控制回路以长机表速目标值作为基准控制目标，采用前向距离偏差的比例控制加积分控制结构，并以前向距离的微分作为阻尼项，数学表达式如下
[0048][0049]
其中，为僚机表速目标值，为长机表速目标值，x为长机与僚机的前向距离，xg为前向距离控制目标，为前向距离的微分值，为前向距离比例项控制参数，为前向距离积分项控制参数，为前向距离阻尼项控制参数。
[0050]
进一步地，所述步骤s22中，僚机表速控制回路采用表速偏差比例控制，其数学表达式如下
[0051][0052]
其中，为僚机表速微分目标值，为僚机表速，为步骤s21中得到的僚机表速目标值，为表速偏差比例项控制参数。
[0053]
进一步地，所述步骤s23中，僚机表速微分控制回路采用表速微分偏差的积分项加表速微分阻尼项的控制结构，其数学表达式如下
[0054][0055]
其中，δ
p
为僚机发动机油门指令，为僚机表速微分，为步骤s22中得到的僚机表速微分目标值，为表速微分阻尼项控制参数，为表速微分控制积分项控制参数。
[0056]
进一步地，所述步骤s31中，僚机侧向距离控制回路采用侧向距离偏差的比例控制加积分控制结构，并以侧向距离的微分作为阻尼项，数学表达式如下
[0057][0058]
其中，φ
wg
为僚机滚转角指令，y为长机与僚机间的侧向距离，yg为侧向距离控制目标，为侧向距离的微分值，为侧向距离比例项控制参数，为侧向距离积分项控制参数，为侧向距离阻尼项控制参数。
[0059]
进一步地，所述步骤s32中，僚机滚转角控制回路采用滚转角偏差比例控制，数学表达式如下
[0060][0061]
其中，p
wg
为僚机滚转角速率指令，φw为僚机滚转角，φ
wg
为步骤s31中得到的僚机滚转角指令，为滚转角偏差比例项控制参数。
[0062]
进一步地，所述步骤s33中，僚机滚转角速率内环控制回路采用滚转角速率偏差的
积分项加滚转角速率阻尼项的控制结构，数学表达式如下
[0063][0064]
其中，δa为僚机副翼舵指令，pw为僚机滚转角速率，p
wg
为步骤s32中得到的僚机滚转角速率指令，为滚转角速率阻尼项控制参数，为滚转角速率偏差积分项控制参数。
[0065]
进一步地，所述步骤s41中，僚机高向距离控制回路以僚机配平俯仰角作为基准控制目标，采用高向距离偏差的比例控制加积分控制结构，并以高向距离的微分作为阻尼项，数学表达式如下
[0066][0067]
其中，θ
wg
为僚机俯仰角指令，θ
ref
为僚机配平俯仰角，z为长机与僚机的高向距离，zg为高向距离控制目标，为高向距离的微分值，为高向距离比例项控制参数，为高向距离积分项控制参数，为高向距离阻尼项控制参数。
[0068]
进一步地，所述步骤s42中，僚机俯仰角控制回路采用俯仰角偏差比例控制，数学表达式如下
[0069][0070]
其中，q
wg
为僚机俯仰角速率指令，θw为僚机俯仰角，θ
wg
为步骤s41中得到的僚机俯仰角指令，为俯仰角偏差比例项控制参数。
[0071]
进一步地，所述步骤s43中，僚机俯仰角速率内环控制回路采用俯仰角速率偏差的积分项加俯仰角速率阻尼项的控制结构，数学表达式如下
[0072][0073]
其中，δe为僚机升降舵指令，qw为僚机俯仰角速率，q
wg
为步骤s42中得到的僚机俯仰角速率指令，为俯仰角速率阻尼项控制参数，为俯仰角速率偏差积分项控制参数。
[0074]
本技术的有益效果：
[0075]
(1)本技术公开的机间防碰撞方法采用pid控制结构，相较于现有技术，飞行控制律算法简单，易于设计人员掌握，机载软件开发也十分便利，能够快速向新构型飞机进行设计移植，对新构型飞机原有设计和机载软件改动小，缩短了设计周期，降低了设计风险，工程应用价值也更高。
[0076]
(2)本技术公开的机间防碰撞方法，相较于现有技术，编队飞行过程中机间交互信息量少，长机仅需向僚机发送经纬高、水平地速、长机速度目标值等几个状态量，因此，对机间通信信道容量要求低，机间信息交互可靠性更高。
[0077]
(3)本技术公开的机间防碰撞方法，在实现编队飞行的基础上，并不需要额外增加视觉感知等传感器，利用单机飞行所具有的传感器即可实现编队控制，降低了无人机系统的复杂度和机载设备成本。
附图说明
[0078]
本技术的前述和下文具体描述在结合以下附图阅读时变得更清楚，附图中：
[0079]
图1为本技术方法流程图；
[0080]
图2为本技术长机与僚机的前向间距、侧向间距以及高向距离示意图；
[0081]
图3为本技术前向机间防碰撞控制器结构示意图；
[0082]
图4为本技术侧向机间防碰撞控制器结构示意图；
[0083]
图5为本技术高向机间防碰撞控制器结构示意图；
[0084]
图6为本技术机间防碰撞控制器结构示意图。
具体实施方式
[0085]
为了使本领域的技术人员更好地理解本技术中的技术方案，下面将通过几个具体的实施例来进一步说明实现本技术发明目的的技术方案，需要说明的是，本技术要求保护的技术方案包括但不限于以下实施例。基于本技术中的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应当属于本技术保护的范围。
[0086]
多无人机在以楔形队、梯形队、横队、纵队和v形队等诸多设计队形进行协同编队飞行时，为了更好地发挥出协同编队的价值，往往机间间距要求很小，一般在十几米至百米范围内。因此，多无人机协同编队机间防碰撞控制不仅关乎队形保持的效果，更关乎飞行安全，尤其是对于高速编队飞行，机间防碰撞控制对飞行安全的影响愈加显著。
[0087]
目前，业内专家学者提出了许多的多无人机协同编队机间防碰撞控制方法，例如，基于行为法、虚拟结构法、图论法、基于一致性方法、模型预测控制、模糊逻辑、神经网络技术、信息感知视觉传感器控制等。但是，这些方法基本都存在算法复杂、机间传输数据量大、使得编队控制与单机控制耦合严重，不便于飞行控制律设计和机载软件开发等问题，这些问题毫无无疑增加了无人机系统的成本以及设计周期。
[0088]
基于此，本技术的实施例提出了一种基于pid控制结构的多无人机协同编队机间防碰撞控制方法，本方法利用长机对预先规划的航线进行跟踪，然后僚机通过控制其与长机间的前向间距、侧向间距、高向距离实现对长机的跟踪及机间防碰撞。其中，前向间距控制通过前向机间防碰撞控制器实现；侧向间距控制通过侧向机间防碰撞控制器实现；高向间距控制通过高向机间防碰撞控制器实现。本技术相较于现有技术，飞行控制律算法简单，易于设计人员掌握，机载软件开发也十分便利，能够快速向新构型飞机进行设计移植，对新构型飞机原有设计和机载软件改动小，缩短了设计周期，降低了设计风险，工程应用价值也更高。
[0089]
为了更好的对本技术的技术方案进行理解，本实施例首先对以下专业术语做出解释说明。
[0090]
长机，飞行术语，是指编队飞行中的带队飞机。
[0091]
僚机，指空中编队中跟随长机飞行的飞机，可接收并执行长机发送的指令。
[0092]
长机和僚机构成了空战中的战术编队，通常长机在前，僚机在后。
[0093]
本实施例提出了一种多无人机协同编队机间防碰撞控制方法，参照说明书附图1，该方法具体如下。
[0094]
步骤s1.确定长机与各个僚机之间的前向距离、侧向距离以及高向距离。
[0095]
所述前向间距、侧向间距以及高向距离如图2所示，并通过如下的步骤计算得到。
[0096]
步骤s11.建立长机牵连坐标系，具体方法为：
[0097]
以长机重心o为坐标原点，地理北向为坐标轴on轴的正方向，地理东向为坐标轴oe轴的正方向，og轴垂直于one平面且指向地心为正方向，依此建立长机牵连坐标系。
[0098]
步骤s12.根据以下数学表达式，计算僚机重心在长机牵连坐标系中的坐标值：
[0099][0100]
其中，(n,e,g)为僚机重心在长机牵连坐标系中的坐标值，r为地球赤道平均半径，e为地球扁率，hw为僚机高度，h
l
为长机高度，lonw为僚机经度，latw为僚机纬度，lon
l
为长机经度，lat
l
为长机纬度。
[0101]
步骤s13.建立长机航迹坐标系，具体方法为：
[0102]
以长机重心o为坐标原点，长机水平地速的矢量方向为ox轴正方向，oy轴在水平面内垂直于ox轴且指向长机右侧为正，oz轴垂直于oxy平面且指向下为正，依此建立长机航迹坐标系。
[0103]
步骤s14.根据以下数学表达式，将步骤s12得到的僚机重心坐标值转换至长机航迹坐标系中：
[0104][0105]
其中，(x,y,z)为僚机重心在长机航迹坐标系中的坐标值，ψ为长机航迹偏角。
[0106]
本实施例中，需要说明的是，长机航迹偏角也即长机航迹坐标系的ox轴正方向与长机牵连坐标系的on轴正方向之间的夹角，
[0107]
本实施例中，还需要说明的是，规定若on轴向地理东向旋转一个0
°
～180
°
的角度后能与ox轴重合，则长机航迹偏角符号为正，否则为负，且长机航迹偏角范围为-180
°
～180
°
。
[0108]
步骤s15.计算长机与僚机的前向距离。
[0109]
在本实施例中，需要说明的是，长机与僚机的前向距离为长机重心与僚机重心的距离在长机航迹坐标系中ox轴上的投影，也即是步骤s14中计算出的x。
[0110]
步骤s16.计算长机与僚机的侧向距离。
[0111]
在本实施例中，需要说明的是，长机与僚机的侧向距离为长机重心与僚机重心的距离在长机航迹坐标系中oy轴上的投影，也即步骤s14中计算出的y。
[0112]
步骤s17.计算长机与僚机的高向距离。
[0113]
在本实施例中，需要说明的是，长机与僚机的高向距离为长机重心与僚机重心的距离在长机航迹坐标系中oz轴上的投影，也即步骤s14中计算出的z。
[0114]
在本实施例中，需要说明的是，高向距离指的是长机与僚机之间的高度差。
[0115]
步骤s2.设计多无人机协同编队的前向机间防碰撞控制器、侧向机间防碰撞控制
器以及高向机间防碰撞控制器。
[0116]
所述前向机间防碰撞控制器结构如图3所示，该控制器包括僚机前向距离控制回路、僚机表速控制回路和僚机表速微分控制回路，具体如下：
[0117]
1、僚机前向距离控制回路
[0118]
僚机前向距离控制回路以长机表速目标值作为基准控制目标，采用前向距离偏差的比例控制加积分控制结构，并以前向距离的微分作为阻尼项，该回路的输出量为僚机表速目标值，具体数学表达式如下
[0119][0120]
其中，为僚机表速目标值，为长机表速目标值，x为长机与僚机的前向距离，xg为前向距离控制目标，为前向距离的微分值，为前向距离比例项控制参数，为前向距离积分项控制参数，为前向距离阻尼项控制参数。
[0121]
在本实施例中，需要说明的是，以及这三个控制参数可基于飞机数字模型或飞行试验进行调参得到。
[0122]
2、僚机表速控制回路
[0123]
僚机表速控制回路是基于僚机前向距离控制回路得到的僚机表速目标值进行设计的，该回路的输出量为僚机表速微分目标值。僚机表速控制回路采用表速偏差比例控制，其数学表达式如下
[0124][0125]
其中，为僚机表速微分目标值，为僚机表速，为僚机表速目标值，为表速偏差比例项控制参数。
[0126]
在本实施例中，需要说明的是，可基于飞机数字模型或飞行试验进行调参得到。
[0127]
3、僚机表速微分控制回路
[0128]
僚机表速微分控制回路是基于僚机表速控制回路得到的僚机表速微分目标值进行设计的，该回路的输出量为僚机发动机油门指令。僚机表速微分控制回路采用表速微分偏差的积分项加表速微分阻尼项的控制结构，其数学表达式如下
[0129][0130]
其中，δ
p
为僚机发动机油门指令，为僚机表速微分，为僚机表速微分目标值，为表速微分阻尼项控制参数，为表速微分控制积分项控制参数。
[0131]
在本实施例中，需要说明的是，和这两个控制参数可基于飞机数字模型或飞行试验进行调参得到。
[0132]
所述侧向机间防碰撞控制器结构如图4所示，该控制器包括僚机侧向距离控制回路、僚机滚转角控制回路和僚机滚转角速率内环控制回路，具体如下：
[0133]
1、僚机侧向距离控制回路
[0134]
僚机侧向距离控制回路采用侧向距离偏差的比例控制加积分控制结构，并以侧向距离的微分作为阻尼项，该回路的输出量为僚机滚转角指令，具体的数学表达式如下
[0135][0136]
其中，φ
wg
为僚机滚转角指令，y为长机与僚机间的侧向距离，yg为侧向距离控制目标，为侧向距离的微分值，为侧向距离比例项控制参数，为侧向距离积分项控制参数，为侧向距离阻尼项控制参数。
[0137]
在本实施例中，需要说明的是，以及这三个控制参数可基于飞机数字模型或飞行试验进行调参得到。
[0138]
2、僚机滚转角控制回路
[0139]
僚机滚转角控制回路是基于僚机侧向距离控制回路得到的僚机滚转角指令进行设计的，该回路的输出量为僚机滚转角速率指令。僚机滚转角控制回路采用滚转角偏差比例控制，具体的数学表达式如下
[0140][0141]
其中，p
wg
为僚机滚转角速率指令，φw为僚机滚转角，φ
wg
为僚机滚转角指令，为滚转角偏差比例项控制参数。
[0142]
在本实施例中，需要说明的是，控制参数可基于飞机数字模型或飞行试验进行调参得到。
[0143]
3、僚机滚转角速率内环控制回路
[0144]
本回路是基于僚机滚转角控制回路输出的僚机滚转角速率指进行设计的，该回路的输出量为僚机副翼舵指令。僚机滚转角速率内环控制回路采用滚转角速率偏差的积分项加滚转角速率阻尼项的控制结构，其数学表达式如下
[0145][0146]
其中，δa为僚机副翼舵指令，pw为僚机滚转角速率，p
wg
为僚机滚转角速率指令，为滚转角速率阻尼项控制参数，为滚转角速率偏差积分项控制参数。
[0147]
在本实施例中，需要说明的是，和这两个控制参数可基于飞机数字模型或飞行试验进行调参得到。
[0148]
所述高向机间防碰撞控制器结构如图5所示，该控制器包括僚机高向距离控制回路、僚机俯仰角控制回路和僚机俯仰角速率内环控制回路，具体如下：
[0149]
1、僚机高向距离控制回路
[0150]
僚机高向距离控制回路以僚机配平俯仰角作为基准控制目标，采用高向距离偏差的比例控制加积分控制结构，并以高向距离的微分作为阻尼项，该回路的输出量为僚机俯仰角指令，具体的数学表达式如下
[0151][0152]
其中，θ
wg
为僚机俯仰角指令，θ
ref
为僚机配平俯仰角，z为长机与僚机的高向距离，zg为高向距离控制目标，为高向距离的微分值，为高向距离比例项控制参数，为高向距离积分项控制参数，为高向距离阻尼项控制参数。
[0153]
在本实施例中，需要说明的是，和这三个控制参数可基于飞机数字模型或飞行试验进行调参得到。
[0154]
2、僚机俯仰角控制回路
[0155]
本回路是基于僚机高向距离控制回路输出的僚机俯仰角指令进行设计的，该回路的输出量为僚机俯仰角速率指令。僚机俯仰角控制回路采用俯仰角偏差比例控制，其数学表达式如下
[0156][0157]
其中，q
wg
为僚机俯仰角速率指令，θw为僚机俯仰角，θ
wg
为僚机俯仰角指令，为俯仰角偏差比例项控制参数。
[0158]
在本实施例中，需要说明的是，控制参数可基于飞机数字模型或飞行试验进行调参得到。
[0159]
3、僚机俯仰角速率内环控制回路
[0160]
本回路是基于僚机俯仰角控制回路输出的僚机俯仰角速率指令进行设计的，该回路的输出量为僚机升降舵指令。僚机俯仰角速率内环控制回路采用俯仰角速率偏差的积分项加俯仰角速率阻尼项的控制结构，其数学表达式如下
[0161][0162]
其中，δe为僚机升降舵指令，qw为僚机俯仰角速率，q
wg
为僚机俯仰角速率指令，为俯仰角速率阻尼项控制参数，为俯仰角速率偏差积分项控制参数。
[0163]
在本实施例中，需要说明的是，和这两个控制参数可基于飞机数字模型或飞行试验进行调参得到。
[0164]
步骤s3.上述前向机间防碰撞控制器、侧向机间防碰撞控制器以及高向机间防碰撞控制器这三个控制器共同组成如图6所示的无人机机间防碰撞控制器，最终利用该机间防碰撞控制器实现多无人机协同编队机间防碰撞控制；其中，前向机间防碰撞控制器输出僚机发动机油门指令，用于控制僚机发动机油门；侧向机间防碰撞控制器输出僚机副翼舵指令，用于控制僚机副翼；高向机间防碰撞控制器输出僚机升降舵指令，用于控制僚机升降舵。
[0165]
在本技术的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本技术和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本技术保护
范围的阻碍。
[0166]
在本技术的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“安装”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本技术中的具体含义。
[0167]
以上所述，仅是本技术的较佳实施例，并非对本技术做任何形式上的阻碍，凡是依据本技术的技术实质对以上实施例所作的任何简单修改、等同变化，均落入本技术的保护范围之内。

技术特征：

1.一种多无人机协同编队机间防碰撞控制方法，其特征在于，包括如下步骤：确定长机与僚机之间的间距，包括长机与僚机之间的前向距离、侧向距离以及高向距离；设计多无人机协同编队前向机间防碰撞控制器、侧向机间防碰撞控制器以及高向机间防碰撞控制器；前向机间防碰撞控制器输出僚机发动机油门指令，控制僚机发动机油门；侧向机间防碰撞控制器输出僚机副翼舵指令，控制僚机副翼；高向机间防碰撞控制器输出僚机升降舵指令，控制僚机升降舵；这三个控制器共同组成机间防碰撞控制器，最终实现多无人机协同编队机间防碰撞控制。2.根据权利要求1所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述确定长机与僚机之间的间距，包括：步骤s11.建立长机牵连坐标系；以长机重心o为坐标原点，地理北向为坐标轴on轴的正方向，地理东向为坐标轴oe轴的正方向，og轴垂直于one平面且指向地心为正方向；步骤s12.计算僚机重心在长机牵连坐标系中的坐标值；僚机重心在长机牵连坐标系中的坐标值如下其中，(n,e,g)为僚机重心在长机牵连坐标系中的坐标值，r为地球赤道平均半径，e为地球扁率，h
w
为僚机高度，h
l
为长机高度，lon
w
为僚机经度，lat
w
为僚机纬度，lon
l
为长机经度，lat
l
为长机纬度；步骤s13.建立长机航迹坐标系；以长机重心o为坐标原点，长机水平地速的矢量方向为ox轴正方向，oy轴在水平面内垂直于ox轴且指向长机右侧为正，oz轴垂直于oxy平面且指向下为正；步骤s14.将步骤s12得到的僚机重心坐标值转换至长机航迹坐标系中；僚机在长机牵连坐标系中的坐标值转换至长机航迹坐标系中，其数学表达式如下其中，(x,y,z)为僚机重心在长机航迹坐标系中的坐标值，ψ为长机航迹偏角；步骤s15.计算长机与僚机的前向距离；长机与僚机的前向距离为长机重心与僚机重心的距离在长机航迹坐标系中ox轴上的投影；步骤s16.计算长机与僚机的侧向距离；长机与僚机的侧向距离为长机重心与僚机重心的距离在长机航迹坐标系中oy轴上的投影；
步骤s17.计算长机与僚机的高向距离；长机与僚机的高向距离为长机重心与僚机重心的距离在长机航迹坐标系中oz轴上的投影。3.根据权利要求1所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述设计多无人机协同编队前向机间防碰撞控制器，包括：步骤s21.设计僚机前向距离控制回路，该回路的输出量为僚机表速目标值；步骤s22.基于步骤s21得到的僚机表速目标值，设计僚机表速控制回路，该回路的输出量为僚机表速微分目标值；步骤s23.基于步骤s22得到的僚机表速微分目标值，设计僚机表速微分控制回路，该回路的输出量为僚机发动机油门指令。4.根据权利要求1所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述设计多无人机协同编队侧向机间防碰撞控制器，包括：步骤s31.设计僚机侧向距离控制回路，该回路的输出量为僚机滚转角指令；步骤s32.基于步骤s31得到的僚机滚转角指令，设计僚机滚转角控制回路，该回路的输出量为僚机滚转角速率指令；步骤s33.基于步骤s32得到的僚机滚转角速率指令，设计僚机滚转角速率内环控制回路，该回路的输出量为僚机副翼舵指令。5.根据权利要求1所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述设计多无人机协同编队高向机间防碰撞控制器，包括：步骤s41.设计僚机高向距离控制回路，该回路的输出量为僚机俯仰角指令；步骤s42.基于步骤s41得到的僚机俯仰角指令，设计僚机俯仰角控制回路，该回路的输出量为僚机俯仰角速率指令；步骤s43.基于步骤s42得到的僚机俯仰角速率指令，设计僚机俯仰角速率内环控制回路，该回路的输出量为僚机升降舵指令。6.根据权利要求3所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述步骤s21中，设计僚机前向距离控制回路，该回路的输出量为僚机表速目标值，包括：僚机前向距离控制回路以长机表速目标值作为基准控制目标，采用前向距离偏差的比例控制加积分控制结构，并以前向距离的微分作为阻尼项，数学表达式如下其中，为僚机表速目标值，为长机表速目标值，x为长机与僚机的前向距离，x
g
为前向距离控制目标，为前向距离的微分值，为前向距离比例项控制参数，为前向距离积分项控制参数，为前向距离阻尼项控制参数。7.根据权利要求3所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述步骤s22中，僚机表速控制回路采用表速偏差比例控制，其数学表达式如下其中，为僚机表速微分目标值，为僚机表速，为步骤s21中得到的僚机表速目
标值，为表速偏差比例项控制参数。8.根据权利要求3所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述步骤s23中，僚机表速微分控制回路采用表速微分偏差的积分项加表速微分阻尼项的控制结构，其数学表达式如下其中，δ
p
为僚机发动机油门指令，为僚机表速微分，为步骤s22中得到的僚机表速微分目标值，为表速微分阻尼项控制参数，为表速微分控制积分项控制参数。9.根据权利要求4所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述步骤s31中，僚机侧向距离控制回路采用侧向距离偏差的比例控制加积分控制结构，并以侧向距离的微分作为阻尼项，数学表达式如下其中，φ
wg
为僚机滚转角指令，y为长机与僚机间的侧向距离，y
g
为侧向距离控制目标，为侧向距离的微分值，为侧向距离比例项控制参数，为侧向距离积分项控制参数，为侧向距离阻尼项控制参数。10.根据权利要求4所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述步骤s32中，僚机滚转角控制回路采用滚转角偏差比例控制，数学表达式如下其中，p
wg
为僚机滚转角速率指令，φ
w
为僚机滚转角，φ
wg
为步骤s31中得到的僚机滚转角指令，为滚转角偏差比例项控制参数。11.根据权利要求4所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述步骤s33中，僚机滚转角速率内环控制回路采用滚转角速率偏差的积分项加滚转角速率阻尼项的控制结构，数学表达式如下其中，δ
a
为僚机副翼舵指令，p
w
为僚机滚转角速率，p
wg
为步骤s32中得到的僚机滚转角速率指令，为滚转角速率阻尼项控制参数，为滚转角速率偏差积分项控制参数。12.根据权利要求5所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述步骤s41中，僚机高向距离控制回路以僚机配平俯仰角作为基准控制目标，采用高向距离偏差的比例控制加积分控制结构，并以高向距离的微分作为阻尼项，数学表达式如下其中，θ
wg
为僚机俯仰角指令，θ
ref
为僚机配平俯仰角，z为长机与僚机的高向距离，z
g
为高向距离控制目标，为高向距离的微分值，为高向距离比例项控制参数，为高向
距离积分项控制参数，为高向距离阻尼项控制参数。13.根据权利要求5所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述步骤s42中，僚机俯仰角控制回路采用俯仰角偏差比例控制，数学表达式如下其中，q
wg
为僚机俯仰角速率指令，θ
w
为僚机俯仰角，θ
wg
为步骤s41中得到的僚机俯仰角指令，为俯仰角偏差比例项控制参数。14.根据权利要求5所述的一种多无人机协同编队机间防碰撞控制方法，其特征在于，所述步骤s43中，僚机俯仰角速率内环控制回路采用俯仰角速率偏差的积分项加俯仰角速率阻尼项的控制结构，数学表达式如下其中，δ
e
为僚机升降舵指令，q
w
为僚机俯仰角速率，q
wg
为步骤s42中得到的僚机俯仰角速率指令，为俯仰角速率阻尼项控制参数，为俯仰角速率偏差积分项控制参数。

技术总结

本申请涉及无人机技术领域，公开了一种多无人机协同编队机间防碰撞控制方法，本方法首先确定长机与僚机之间的前向距离、侧向距离和高向距离，然后设计多无人机协同编队的前向机间防碰撞控制器、侧向机间防碰撞控制器和高向机间防碰撞控制器，这三个控制器共同组成机间防碰撞控制器，最终利用该机间防碰撞控制器实现多无人机协同编队机间防碰撞控制。本申请的防碰撞控制器采用PID控制结构，飞行控制律算法简单，并且编队飞行过程中机间交互信息量少，对机间通信信道容量要求低，信息交互可靠性高，同时由于不需要额外增加视觉感知等传感器，利用单机飞行原本所配备的传感器即可实现编队控制，因此，还降低了无人机系统的复杂度和机载设备的成本。和机载设备的成本。和机载设备的成本。