一种分布式的旋翼无人机协同控制方法、装置和系统

1.本发明属于无人机编队控制技术领域，更具体地，涉及一种分布式的旋翼无人机协同控制方法、装置和系统。


背景技术：

2.多无人机系统在现代战争以及现代无人机应用中受到了广泛的关注。在军用方面，多无人机系统能够进行地形探测，协同侦察等；在民用方面，多无人机系统能够用于电线巡检，无人机快递等。
3.相对于固定翼无人机，小型旋翼无人机具有隐蔽性强，安装与操作便捷，飞行可靠性高等特点，更适合于侦察类及监测类等对速度有一定限制的任务。为了提高任务完成的效率，需要使用多架旋翼无人机进行任务飞行，因此需要设计一个适合于室外环境工作的分布式的小型旋翼无人机的软硬件系统。集群系统硬件系统的轻量化、稳定性以及可扩展性还有软件系统的便捷性以及可移植性都是需要重点考虑的内容。
4.现有技术中很难找到多机通信稳定性高、单机飞行可靠性强、集群任务部署和集群算法验证便捷的小型旋翼无人机协同飞行系统。已有的协同系统，对数据传输的实时性和运动控制的准确度要求不高，并且属于集中式的协同控制架构，不能满足无人机分布式协同飞行的需求，不能够帮助无人机完成协同作战任务。


技术实现要素：

5.针对现有技术的以上缺陷或改进需求，本发明提供了一种分布式的旋翼无人机协同控制方法，其目的在于通过获取各个无人机当前时刻本机及其邻机各自对应的飞行状态信息，进而确定编队误差计算期望控制指令，实现分布式协同控制每架无人机的飞行状态，由此解决现有无人机编队控制中准确率低实时性差的技术问题。
6.为实现上述目的，按照本发明的一个方面，提供了一种分布式的旋翼无人机协同控制方法，包括：
7.s1：获取当前时刻本机及其邻机各自对应的飞行状态信息，所述飞行状态信息包括：北东地坐标系下的位置坐标和速度坐标；
8.s2：利用所述本机及其邻机各自对应的飞行状态信息计算所述本机的实际位置与期望位置之间的三轴相对编队误差，所述三轴相对编队误差满足第一控制律和第二控制律；所述第一控制律确保全部无人机的编队误差低于误差阈值；所述第二控制律用于确保预设位置为编队中心；
9.s3：根据所述本机的三轴相对编队误差获取对应的期望推力、期望滚转角和期望俯仰角；所述三轴相对编队误差包括：位置编队误差和速度编队误差；
10.s4：将所述本机对应的期望推力、期望滚转角和期望俯仰角转化为姿态控制指令，从而控制所述本机在编队中的飞行状态。
11.在其中一个实施例中，所述三轴相对编队误差包括：位置编队误差和
速度编队误差所述s2包括：
12.利用公式和计算所述本机的实际位置与期望位置之间的三轴相对编队误差，以满足第一控制律和第二控制律；
13.其中，i表示本机的序号，(xi,yi,zi)和(v
i,x
,v
i,y
,v
i,z
)分别表示所述本机在北东地坐标系下的位置坐标和速度坐标；(δ
i,x
,δ
i,y
,δ
i,z
)表示期望编队队形，(xd,yd,zd)和(v
d,x
,v
d,y
,v
d,z
)分别表示代表编队中心的虚拟领机的位置坐标和位置坐标，虚拟领机的偏航角为ψd。
14.在其中一个实施例中，所述第一控制律表征随着时间的变化，本机与相邻的第j架无人机之间的位置误差满足期望误差δ
ij,p
，表示为：其中，xi(t),yi(t),zi(t)分别表示t
→
∞时本机在其ned坐标系下的位置，xj(t),yj(t),zj(t)分别表示第j架无人机在其ned坐标系下的位置，t
→
∞表示时间趋向于无穷的时候，δ
ij,p
＝[δ
i,x
,δ
i,y
,δ
i,z
]
t-[δ
j,x
,δ
j,y
,δ
j,z
]
t
；
[0015]
所述第二控制律表征虚拟坐标(xd,yd,zd)在所有无人机位置的编队中心，表示为：n表示无人机总数。
[0016]
在其中一个实施例中，所述s3包括：
[0017]
s31：基于一致性理论和线性二次型调节控制算法设置控制量计算公式：以计算所述本机的三维控制量向量；
[0018]
s32：利用所述本机的三维控制量向量获取对应的期望推力、期望滚转角和期望俯仰角；
[0019]
其中，u＝[u
i,x
,u
i,y
,u
i,z
]
t
表示控制量向量，c,k1,k2表示控制参数，ni表示本机的邻机集合；a
ij
＝1or 0表示本机与第j架无人机能否进行数据传输；bi＝1or 0表示本机能否获知所述虚拟领机的飞行信息，[δ
i,x
,δ
i,y
,δ
i,z
]
t
表示本机的期望位置误差，n表示无人机总数。
[0020]
在其中一个实施例中，所述s32包括：
[0021]
将所述本机的三维控制量向量(u
i,x
,u
i,y
,u
i,z
)输入期望姿态角指令求解公式计算所述本机的期望推力t
i,sp
、期望滚转角φ
i,sp
和俯仰角θ
i,sp
；
[0022]
其中，mi表示本机的质量，g表示重力加速度，ti表示本机螺旋桨产生的推力，φi,θi,ψi分别表示本机的当前滚转角、当前俯仰角和当前偏航角；ψ
i,sp
表示期望偏航角，根据虚拟领机的偏航角ψd设置，ψ
i,sp
＝ψd。
[0023]
在其中一个实施例中，所述s4包括：
[0024]
将所述本机对应的期望推力、期望滚转角和期望俯仰角转化为电机的转速指令w
i,1
,w
i,2
,w
i,3
,w
i,4
；实现所述本机飞行状态调整；
[0025]
其中，c
t
,cm分别表示升力系数和阻力系数，l表示本机的几何中心到电机中心的距离，u
i,1
,u
i,2
,u
i,3
,u
i,4
分别表示无人机的实际控制量；式中的期望俯仰角滚转角以及偏航角指令分别由θ
i,cmd
＝θ
i,sp
,φ
i,cmd
＝φ
i,sp
,ψ
i,cmd
＝ψ
i,sp
,求得；分别表示本机俯仰角控制回路的pid三个控制参数，分别表示本机滚转角控制回路的pid三个控制参数，分别表示本机偏航角控制回路的pid三个控制参数；ω
i,φ
,ω
i,θ
,ω
i,ψ
分别表示滚转角速率、俯仰角速率和偏航角速率；limit表示限幅函数，保证积分项在
±
max之间，分别表示本机在俯仰回路、滚转回路和偏航回路对应积分项的最大值。
[0026]
按照本发明的另一方面，提供了一种分布式的旋翼无人机协同控制装置，包括：
[0027]
协同控制环，用于获取当前时刻本机及其邻机各自对应的飞行状态信息，所述飞行状态信息包括：北东地坐标系下的位置坐标和速度坐标；利用所述本机及其邻机各自对应的飞行状态信息计算所述本机的实际位置与期望位置之间的三轴相对编队误差，所述三轴相对编队误差满足第一控制律和第二控制律；所述第一控制律确保全部无人机的编队误差低于误差阈值；所述第二控制律用于确保预设位置为编队中心；根据所述本机的三轴相对编队误差获取对应的期望推力、期望滚转角和期望俯仰角；
[0028]
姿态控制环，用于将所述本机对应的期望推力、期望滚转角和期望俯仰角转化为
姿态控制指令，从而控制所述本机在编队中的飞行状态。
[0029]
按照本发明的另一方面，提供了一种分布式的旋翼无人机协同控制系统，包括：
[0030]
存储器，用于存储有计算机程序；
[0031]
控制器，用于执行所述计算机程序时实现上述方法的步骤。
[0032]
按照本发明的另一方面，提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述方法的步骤。
[0033]
总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
[0034]
本发明提供一种分布式的旋翼无人机协同控制方法，通过获取当前时刻本机及其邻机各自对应的飞行状态信息，并依据此计算所述本机的实际位置与期望位置之间的三轴相对编队误差，进而计算对应的期望推力、期望滚转角和期望俯仰角，并转化为姿态控制指令对所述本机在编队中的飞行状态进行控制。本发明通过获取无人机群中各无人机以及虚拟领机的飞行状态信息，从而计算得到编队状态偏差，并计算得到期望的姿态控制指令，进而得到实际的控制量及电机转速信息，使得无人机群能够按照设定的队形调整各自的飞行状态。本发明满足无人机分布式协同控制的准确性和实时性的要求。
附图说明
[0035]
图1为本发明一实施例中单个小型旋翼无人机硬件拓扑连接图。
[0036]
图2为本发明一实施例中小型旋翼无人机分布式协同硬件拓扑连接图。
[0037]
图3为本发明一实施例中分布式的旋翼无人机协同控制方法的软件架构示意图。
[0038]
图4为本发明一实施例中底层姿态控制器的结构示意图。
[0039]
图5为本发明一实施例中分布式的旋翼无人机协同控制方法的ros节点连接图。
具体实施方式
[0040]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0041]
如图1所示，小型旋翼无人机协同控制硬件系统主要包括上层分布式协同控制器，底层姿态控制器，电源管理系统与执行机构系统。所有无人机通过板载wifi进行组网后，每架无人机的上层协同控制器能够从底层获取传感器数据，包括gps绝对位置、无人机速度和姿态等信息，从相邻无人机获取邻居无人机位置速度等状态信息，经过上层协同控制器处理后，向底层姿态控制器发送期望姿态与油门信息，底层姿态控制器使用pid控制四个电机的转速，实现稳定的姿态跟踪。电源管理系统负责给各组成单元供电。执行机构系统按照底层控制系统的指令，对四个电机的转速进行控制。除了上述系统外，还包括能够装载所有子系统的qav250小型无人机机架。qav250机架的选择，符合体积小，重量轻的理念，更方便实现小型无人机集群。
[0042]
其中，上层协同控制器采用的是nanopi duo2开发板，内部操作系统环境为ubuntu16.04，其上配置有新型的ros2系统，便于分布式控制的实现，能够通过装载的
mavros功能包实现与pixhawk底层控制板的信息传递。除此之外，此控制板装载有ap6181wifi模块，通过连接路由器，将所有无人机的网络地址配置为同一网段，能够实现多无人机与地面站之间的组网，两者能够保证分布式协同控制器的实现。
[0043]
其中，底层姿态控制器采用体积较小的pixhawk mini4主控板，用于无人机底层姿态跟踪控制。该控制板集成板载传感器：imu、陀螺仪；连接外部传感器gps，能够获得无人机准确的姿态、位置信息，通过pid姿态控制算法，实现无人机稳定的姿态跟踪控制。
[0044]
其中，小型旋翼无人机的电源管理系统分为锂铂电池、免电池电路bec及电源管理模块三部分。电池的供电路线可以分为三部分，第一部分经过电源管理模块给pixhawk飞控板及nano板供电，第二部分经过电源管理模块，连接电调给动力系统电机供电，第三部分直接连接5v的bec模块，输出连接到pixhawk i/o口，平衡电压。
[0045]
其中，小型旋翼无人机的执行机构可以是包括四个无刷电机及两对三叶五寸螺旋桨，通过控制四个电机的转速，实现无人机的起飞和降落，调节无人机的姿态，进而调节无人机的位置和速度。
[0046]
此外，协同硬件拓扑图如图2所示。由于每架无人机都能够通过板载wifi形成无人机组网，因此各无人机之间能够实现相互通信。无人机接收到其他无人机的信息后，经过协同控制器处理，能够得到内环期望信息，最终驱动无刷电机，从而控制无人机。
[0047]
实施例1
[0048]
本发明提供了一种分布式的旋翼无人机协同控制方法，包括：
[0049]
s1：获取当前时刻本机及其邻机各自对应的飞行状态信息，飞行状态信息包括：北东地坐标系下的位置坐标和速度坐标；
[0050]
s2：利用本机及其邻机各自对应的飞行状态信息计算本机的实际位置与期望位置之间的三轴相对编队误差，三轴相对编队误差满足第一控制律和第二控制律；第一控制律确保全部无人机的编队误差低于误差阈值；第二控制律用于确保预设位置为编队中心；
[0051]
s3：根据本机的三轴相对编队误差获取对应的期望推力、期望滚转角和期望俯仰角；三轴相对编队误差包括：位置编队误差和速度编队误差；
[0052]
s4：将本机对应的期望推力、期望滚转角和期望俯仰角转化为姿态控制指令，从而控制本机在编队中的飞行状态。
[0053]
其中，软件层面的操作需要先给每架无人机上传飞行任务，通过地面终端电脑键入飞行指令，包括有一键起飞、协同飞行、返航降落等阶段，各阶段任务计算得到相应的期望姿态指令，发送到px4内环进行pid姿态跟踪，整个流程如图3所示。s1-s3可以由上层协同控制器实现，s4可以由底层姿态控制器实现。上层协同控制器的软件设计可以分为三个阶段，状态获取与更新，编队算法求解控制量，期望姿态角转换。
[0054]
具体的，在误差计算开始前，需要更新当前时刻本机本身状态：北东地坐标系下的相对于起飞位置的位置坐标(xi,yi,zi)及速度坐标(v
i,x
,v
i,y
,v
i,z
)。邻居无人机的位置和速度坐标分别为(xi,yi,zi),(v
i,x
,v
i,y
,v
i,z
),j∈ni，ni表示本机的邻居集合，用于计算实时协同误差。在s1获得了当前无人机和邻居无人机的位置后，需要计算位置和速度误差求解得到控制量，进而转换得到期望姿态角。
[0055]
实施例2
[0056]
误差计算过程具体如下：为了提高协同控制系统的鲁棒性，这里引入虚拟领导者
并设计分布式无人机协同控制方法，实现多架旋翼无人机的分布式控制。
[0057]
设定本机的期望编队队形为(δ
i,x
,δ
i,y
,δ
i,z
)，定义本机与其期望位置的三轴相对编队误差为
[0058][0059]
其中，i表示本机的序号，(xi,yi,zi)和(v
i,x
,v
i,y
,v
i,z
)分别表示本机在北东地坐标系下的位置坐标和速度坐标；(δ
i,x
,δ
i,y
,δ
i,z
)表示期望编队队形，(xd,yd,zd)和(v
d,x
,v
d,y
,v
d,z
)分别表示代表编队中心的虚拟领机的位置坐标和位置坐标，虚拟领机的偏航角为ψd。假设多架旋翼无人机按照虚拟领导者的轨迹(xd,yd,zd)飞行，虚拟领导者的姿态角为(φd,θd,ψd)。
[0060]
实施例3
[0061]
为了使得无人机之间能够完成期望的编队并且使(xd,yd,zd)为队形中心，因此需设计控制律满足如下目标：
[0062][0063][0064]
其中，δ
ij,p
＝[δ
i,x
,δ
i,y
,δ
i,z
]
t-[δ
j,x
,δ
j,y
,δ
j,z
]
t
表示本机与相邻的第j架无人机之间的期望位置误差，n表示无人机的总数。公式(2)表示第一个控制目标，含义为无人机之间的位置误差满足期望值δ
ij,p
；公式(3)表示第二个控制目标，含义为虚拟坐标(xd,yd,zd)在所有无人机位置的中心。
[0065]
实施例4
[0066]
对于四旋翼无人机来说，位置控制系统模型如下所示
[0067][0068]
其中，mi表示本机的质量，g表示重力加速度，ti表示本机螺旋桨产生的推力，φi,θi,ψi分别表示本机的滚转角，俯仰角和偏航角。为了便于设计协同控制器，首先将(4)简化为如下二阶系统模型。
[0069][0070]
其中，(xi,yi,zi)表示本机的位置信息，表示本机的加速度，(u
i,x
,u
i,y
,u
i,z
)分别表示本机三个坐标轴的控制量。
[0071]
将上述系统(4)转化为矩阵形式，能够得到如下公式
[0072][0073]
其中，x＝[xi,yi,zi,v
i,x
,v
i,y
,v
i,z
]
t
表示位置速度状态向量,表示系统矩阵,表示控制矩阵，u＝[u
i,x
,u
i,y
,u
i,z
]
t
表示控制量向量。
[0074]
上层协同控制器的设计需要使无人机能够满足上述控制目标(2)和(3)。因此，基于多智能体一致性理论并结合lqr控制方法，设计如下协同控制器：
[0075][0076]
其中，c,k1,k2为控制参数，ni表示本机的邻居集合。a
ij
＝1or 0表示本机与第j架无人机能否进行数据传输，如果a
ij
＝1表示无人机i和无人机j之间有数据传输，否则表示没有。bi＝1or 0表示无人机i能否准确知道中心位置(xd,yd,zd)的信息，如果bi＝1表示无人机i能够得到(xd,yd,zd)的准确值，否则表示不能。反馈增益选择根据lqr方法进行设计，并且满足k＝[k1,k2]
t
＝r-1bt
p，实对称正定矩阵p满足p＝p
t
＞0，且是如下algebraic riccati equation的解：
[0077]
pa+a
t
p-pbr-1bt
p＝-q
ꢀꢀꢀꢀꢀꢀꢀ
(8)
[0078]
其中，r,q＞0均为权重矩阵。选择合适的参数，能够求解得到合适的k值，以此来保证旋翼无人机完成指定编队，完成任务需求。
[0079]
实施例5
[0080]
根据一致性理论得到虚拟控制量(u
i,x
,u
i,y
,u
i,z
)后，需求解内环的期望姿态角指令。根据公式(4)和(5)可以得到如下转换公式。
[0081][0082]
对公式(9)进行反解，即可得到期望姿态角和推力
[0083][0084]
其中，t
i,sp
,φ
i,sp
,θ
i,sp
分别为本机期望的推力，滚转角和俯仰角。需要注意的是，期望偏航角ψi是直接由虚拟领导者给定的，且与虚拟领导者相同，即ψ
i,sp
＝ψd。
[0085]
实施例6
[0086]
上层协同控制器完成编队任务后，px4能够接收mavros传过来的期望姿态角并对其实现快速准确跟踪，控制器结构如图4所示。
[0087]
acc2att()表示期望加速度与期望姿态角之间的转换公式(10)，并且能够得到实际控制量u
i,4
＝t
i,sp
。期望姿态角θ
i,sp
,φ
i,sp
,ψ
i,sp
经过角度和角速度的pid控制能够其余三个控制量u
i,1
,u
i,2
,u
i,3
。
[0088][0089]
其中，u
i,1
,u
i,2
,u
i,3
,u
i,4
分别表示无人机的实际控制量，期望俯仰角滚转角以及偏航角指令分别由θ
i,cmd
＝θ
i,sp
,φ
i,cmd
＝φ
i,sp
,ψ
i,cmd
＝ψ
i,sp
,求得，分别表示本机俯仰角控制回路的pid三个控制参数，分别表示本机滚转角控制回路的pid三个控制参数，分别表示本机偏航角控制回路的pid三个控制参数。ω
i,φ
,ω
i,θ
,ω
i,ψ
分别表示滚转角速率，俯仰角速率和偏航角速率。limit表示限幅函数，保证积分项在
±
max之间，分别表示本机在俯仰回路，滚转回路，偏航回路的积分项最大值。
[0090]
根据(11)计算得到的实际控制量u
i,1
,u
i,2
,u
i,3
,u
i,4
，结合转换矩阵公式
[0091][0092]
其中，w
i,1
,w
i,2
,w
i,3
,w
i,4
分别为本机四个电机的转速，c
t
,cm表示无人机的升力系数和阻力系数，l表示无人机几何中心到电机中心的距离。从而得到四旋翼无人机四个电机的
转速，实现对无人机的控制。
[0093]
实施例7
[0094]
本发明提供一种分布式的旋翼无人机协同控制装置，包括：
[0095]
协同控制环，用于获取当前时刻本机及其邻机各自对应的飞行状态信息，飞行状态信息包括：北东地坐标系下的位置坐标和速度坐标；利用本机及其邻机各自对应的飞行状态信息计算本机的实际位置与期望位置之间的三轴相对编队误差，三轴相对编队误差满足第一控制律和第二控制律；第一控制律确保全部无人机的编队误差低于误差阈值；第二控制律用于确保预设位置为编队中心；根据本机的三轴相对编队误差获取对应的期望推力、期望滚转角和期望俯仰角；
[0096]
姿态控制环，用于将本机对应的期望推力、期望滚转角和期望俯仰角转化为姿态控制指令，从而控制本机在编队中的飞行状态。
[0097]
如图3所示，小型旋翼无人机分布式协同控制软件系统示。此系统主要分为两个控制环模块，即内置协同控制环的基于ros2的上层协同控制器和内置姿态控制环的基于px4软件系统的底层姿态控制器。上层协同控制器通过装载的mavros功能包从底层姿态控制器中获取mavlink消息格式的传感器数据，包括无人机gps绝对位置信息，欧拉角姿态信息，飞行速度信息等。所有无人机共同执行指定的飞行任务，每一架无人机都通过ros2的discovery机制拿到邻居无人机的状态信息，每架无人机在上层ros2系统中实现协同控制任务以后，通过mavros功能包向底层的姿态内环发送当前无人机期望姿态和推力指令。姿态控制环基于传统pid控制器向四个电机发送指令。
[0098]
其中，ros节点分为操作节点(switch_mc_node)、执行节点(mc_i_main)、打包节点(msg_mc_i_states)三类。操作节点负责发布指令，控制每架无人机在起飞，任务，编队，降落等飞行阶段之间的切换。不仅能够单独对某一架飞机进行控制，也能够对所有飞机进行同时控制。执行节点在收到操作节点的指令后，会执行对应的飞行阶段，包括起飞子程序、协同子程序、降落子程序等，每段子程序都能够计算得到当前阶段的指令，如标志位指令、期望姿态角指令等。打包节点从执行节点收到指令，并将此信息进行打包，通过mavros发送给px4。px4在接收到指令后会执行相应的任务。各节点之间的连接图如图5所示。
[0099]
实施例8
[0100]
本发明提供了一种分布式的旋翼无人机协同控制系统，包括：
[0101]
存储器，用于存储有计算机程序；
[0102]
控制器，用于执行上述计算机程序时实现上述方法的步骤。
[0103]
实施例9
[0104]
按照本发明的另一方面，提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述方法的步骤。
[0105]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种分布式的旋翼无人机协同控制方法，其特征在于，应用于各旋翼无人机，包括：s1：获取当前时刻本机及其邻机各自对应的飞行状态信息，所述飞行状态信息包括：北东地坐标系下的位置坐标和速度坐标；s2：利用所述本机及其邻机各自对应的飞行状态信息计算所述本机的实际位置与期望位置之间的三轴相对编队误差，所述三轴相对编队误差满足第一控制律和第二控制律；所述第一控制律确保全部无人机的编队误差低于误差阈值；所述第二控制律用于确保预设位置为编队中心；s3：根据所述本机的三轴相对编队误差获取对应的期望推力、期望滚转角和期望俯仰角；所述三轴相对编队误差包括：位置编队误差和速度编队误差；s4：将所述本机对应的期望推力、期望滚转角和期望俯仰角转化为姿态控制指令，从而控制所述本机在编队中的飞行状态。2.如权利要求1所述的分布式的旋翼无人机协同控制方法，其特征在于，所述三轴相对编队误差包括：位置编队误差和速度编队误差所述s2包括：利用公式和计算所述本机的实际位置与期望位置之间的三轴相对编队误差，以满足第一控制律和第二控制律；其中，i表示本机的序号，(x
i
,y
i
,z
i
)和(v
i,x
,v
i,y
,v
i,z
)分别表示所述本机在北东地坐标系下的位置坐标和速度坐标；(δ
i,x
,δ
i,y
,δ
i,z
)表示期望编队队形，(x
d
,y
d
,z
d
)和(v
d,x
,v
d,y
,v
d,z
)分别表示代表编队中心的虚拟领机的位置坐标和位置坐标，虚拟领机的偏航角为ψ
d
。3.如权利要求2所述的分布式的旋翼无人机协同控制方法，其特征在于，所述第一控制律表征随着时间的变化，本机与相邻的第j架无人机之间的位置误差满足期望误差δ
ij,p
，表示为：其中，x
i
(t),y
i
(t),z
i
(t)分别表示t
→
∞时本机在其ned坐标系下的位置，x
j
(t),y
j
(t),z
j
(t)分别表示第j架无人机在其ned坐标系下的位置，t
→
∞表示时间趋向于无穷的时候，δ
ij,p
＝[δ
i,x
,δ
i,y
,δ
i,z
]
t-[δ
j,x
,δ
j,y
,δ
j,z
]
t
；所述第二控制律表征虚拟坐标(x
d
,y
d
,z
d
)在所有无人机位置的编队中心，表示为：n表示无人机总数。4.如权利要求2所述的分布式的旋翼无人机协同控制方法，其特征在于，所述s3包括：s31：基于一致性理论和线性二次型调节控制算法设置控制量计算公式：
以计算所述本机的三维控制量向量；s32：利用所述本机的三维控制量向量获取对应的期望推力、期望滚转角和期望俯仰角；其中，u＝[u
i,x
,u
i,y
,u
i,z
]
t
表示控制量向量，c,k1,k2表示控制参数，n
i
表示本机的邻机集合；a
ij
＝1 or 0表示本机与第j架无人机能否进行数据传输；b
i
＝1 or 0表示本机能否获知所述虚拟领机的飞行信息，[δ
i,x
,δ
i,y
,δ
i,z
]
t
表示本机的期望位置误差，n表示无人机总数。5.如权利要求4所述的分布式的旋翼无人机协同控制方法，其特征在于，所述s32包括：将所述本机的三维控制量向量(u
i,x
,u
i,y
,u
i,z
)输入期望姿态角指令求解公式计算所述本机的期望推力t
i,sp
、期望滚转角φ
i,sp
和俯仰角θ
i,sp
；其中，m
i
表示本机的质量，g表示重力加速度，t
i
表示本机螺旋桨产生的推力，φ
i
,θ
i
,ψ
i
分别表示本机的当前滚转角、当前俯仰角和当前偏航角；ψ
i,sp
表示期望偏航角，根据虚拟领机的偏航角ψ
d
设置，ψ
i,sp
＝ψ
d
。6.如权利要求5所述的分布式的旋翼无人机协同控制方法，其特征在于，所述s4包括：将所述本机对应的期望推力、期望滚转角和期望俯仰角转化为电机的转速指令w
i,1
,w
i,2
,w
i,3
,w
i,4
；实现所述本机飞行状态调整；其中，c
t
,c
m
分别表示升力系数和阻力系数，l表示本机的几何中心到电机中心的距离，u
i,1
,u
i,2
,u
i,3
,u
i,4
分别表示无人机的实际控制量；式中的期望俯仰角滚转角以
及偏航角指令分别由θ
i,cmd
＝θ
i,sp
,φ
i,cmd
＝φ
i,sp
,ψ
i,cmd
＝ψ
i,sp
,求得，分别表示本机俯仰角控制回路的pid三个控制参数，分别表示本机滚转角控制回路的pid三个控制参数，分别表示本机偏航角控制回路的pid三个控制参数；ω
i,φ
,ω
i,θ
,ω
i,ψ
分别表示滚转角速率、俯仰角速率和偏航角速率；limit表示限幅函数，保证积分项在
±
max之间，分别表示本机在俯仰回路、滚转回路和偏航回路对应积分项的最大值。7.一种分布式的旋翼无人机协同控制装置，其特征在于，包括：协同控制环，用于获取当前时刻本机及其邻机各自对应的飞行状态信息，所述飞行状态信息包括：北东地坐标系下的位置坐标和速度坐标；利用所述本机及其邻机各自对应的飞行状态信息计算所述本机的实际位置与期望位置之间的三轴相对编队误差，所述三轴相对编队误差满足第一控制律和第二控制律；所述第一控制律确保全部无人机的编队误差低于误差阈值；所述第二控制律用于确保预设位置为编队中心；根据所述本机的三轴相对编队误差获取对应的期望推力、期望滚转角和期望俯仰角；姿态控制环，用于将所述本机对应的期望推力、期望滚转角和期望俯仰角转化为姿态控制指令，从而控制所述本机在编队中的飞行状态。8.一种分布式的旋翼无人机协同控制系统，其特征在于，包括：存储器，用于存储有计算机程序；控制器，用于执行所述计算机程序时实现权利要求1至6中任一项所述的方法的步骤。9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至6中任一项所述的方法的步骤。

技术总结
本发明公开了一种分布式的旋翼无人机协同控制方法、装置和系统，属于无人机编队控制技术领域，所述方法包括：获取当前时刻本机及其邻机各自对应的飞行状态信息；利用本机及其邻机各自对应的飞行状态信息计算本机的实际位置与期望位置之间的三轴相对编队误差；根据本机的三轴相对编队误差获取对应的期望推力、期望滚转角和期望俯仰角；三轴相对编队误差包括：位置编队误差和速度编队误差；将本机对应的期望推力、期望滚转角和期望俯仰角转化为姿态控制指令，从而控制本机在编队中的飞行状态。本发明采集编队中的各无人机及其领机的飞行状态以计算编队偏差并据此生成对应的姿态控制指令，能够及时准确的调整编队飞行状态。能够及时准确的调整编队飞行状态。能够及时准确的调整编队飞行状态。


技术研发人员：刘磊 职永然 曾紫媛 薛辰龙 樊慧津 王博
受保护的技术使用者：华中科技大学
技术研发日：2023.03.15
技术公布日：2023/7/12