CN113625758A - 一种四旋翼无人机群位姿双环抗干扰编队控制方法 - Google Patents

Abstract

本发明公开了一种四旋翼无人机群位姿双环抗干扰编队控制方法，该方法该方法基于的四旋翼无人机集群由N架四旋翼无人机构成，该方法包括无人机群位置环的抗干扰编队控制和单机姿态环的姿态控制两部分，抗干扰控制基于自抗扰控制原理，采用扩张状态观测器估计并补偿外部扰动。本发明能够将位置环的控制输入转化为姿态环的期望姿态角，以姿态环控制驱动位置环编队控制的方式，实现对四旋翼无人机群的位姿双环抗干扰编队控制。

Description

一种四旋翼无人机群位姿双环抗干扰编队控制方法

技术领域

本发明属于无人机集群编队控制技术领域，具体涉及一种四旋翼无人机群位姿双环抗干扰编队控制方法。

背景技术

无人机群编队控制是协同控制领域的前沿热点问题，在军用和民用领域均有广泛应用和发展前景，在军事领域，无人机群可以分布式搭载多种传感器，为重要军事目标和高危要害区域提供无人巡逻值守和情报侦察。在民用领域，无人机群编队可实现农业植保、勘探测绘等多种场景应用，并且可在发生地质灾害、大型火灾或危险品泄露等事故和险情时进行抢险救助和现场监控。因此，研究无人机群编队控制技术与方法具有重要意义。

四旋翼无人机作为无人机的一种重要类型，凭借其成本低、灵活性强等优势，已在工程领域得到广泛应用。四旋翼无人机由姿态控制驱动位置控制，存在明显的位姿耦合，仅采用位置环编队控制策略难以保证对四旋翼无人机群的有效控制。需要从位置环和姿态环两方面入手，研究针对无人机群位置环的编队控制策略和针对无人机单机姿态环的姿态控制策略，并将位置环编队控制转化为姿态环的姿态控制，从而以姿态控制驱动位置控制，使得四旋翼无人机群形成并保持期望的编队队形。

在实际应用中，由于复杂环境影响，四旋翼无人机群可能受到外部扰动影响，可能导致控制器抖振、编队状态失稳和系统故障等问题，导致编队控制难以顺利实现。因此，需要在编队控制的同时考虑编队控制器的抗干扰性能。目前常见的抗干扰方法包括鲁棒控制、滑模控制和自适应控制等，这些控制策略依赖于精确的数学模型，实际应用较为困难。采用自抗扰控制策略可以在不依赖精确数学模型的情况下对扰动进行估计与补偿，达到较好的扰动抑制效果。然而，无人机群位置环自抗扰编队控制策略仍研究较少，且难以实现位姿双环抗干扰控制，根据目前国内外已有的公开成果来看，尚未见到关于四旋翼无人机群位姿双环抗干扰编队控制方法方面的研究。

发明内容

本发明提供了一种四旋翼无人机群位姿双环抗干扰编队控制方法，旨在解决如下技术问题：如何通过单机姿态控制驱动无人机群位置环编队控制，以位姿双环抗干扰的方式使四旋翼无人机群在实现编队控制的同时具备对抗外部干扰的能力。

本发明采用的技术方案是：

一种四旋翼无人机群位姿双环抗干扰编队控制方法，以单机姿态控制驱动无人机群位置编队，使四旋翼无人机群形成并保持期望的编队队形，并根据自抗扰控制方式实现位置环抗干扰编队控制，具体包括以下步骤：

步骤1：构建无人机群位置环动力学模型和无人机单机姿态环动力学模型；

步骤2：然后根据编队指令设计位置环抗干扰编队控制器；

步骤3：进而依据位置环抗干扰编队控制器的输入和无人机单机姿态环动力学模型，将位置控制转化为期望的姿态角，设计出单机姿态控制器；

步骤4：以单机姿态环控制器驱动机群位置环编队控制过程的方式实现位姿双环抗干扰编队控制。

优选的，在步骤1中，四旋翼无人机群由N架四旋翼无人机组成，其中第i架四旋翼无人机的位置环动力学模型建模如下：

其中，i＝1,2,…,N，x_i(t)、v_i(t)、u_i(t)和q_i(t)均为三维向量，代表无人机i的位置、速度、控制量和干扰项，α_x和α_v表示无人机位置和速度的阻尼。

优选的，在步骤1中，每架四旋翼无人机的姿态环动力学模型由无人机的平动方程和转动方程联合确定，根据四旋翼无人机模型为对称刚体且质心与几何中心重合的特性，每架四旋翼无人机的姿态环动力学模型均可建模如下：

其中，{a_x，a_y，a_z}表示四旋翼无人机机体沿地理坐标系三个坐标轴方向的加速度，{I_x，I_y，I_z}为四旋翼无人机在机体坐标系下绕三个坐标轴旋转的转动惯量， {ω_x，ω_y，ω_z}是四旋翼无人机在机体坐标系下绕三个坐标轴旋转的转动角速度， {ω₁，ω₂，ω₃，ω₄}表示四个旋翼的转速，ω_sum＝-ω₁+ω₂-ω₃+ω₄，I_r是螺旋桨自身的转动惯量，l是四旋翼无人机质心到旋翼旋转轴之间的距离，b为旋翼的升力系数， d为旋翼的阻力系数，g为重力加速度，m为四旋翼无人机质量，{φ，θ，ψ}是姿态角，分别为俯仰角、横滚角和偏航角，

优选的，在步骤2中，编队指令由分段连续可微分的向量

表示，根据编队指令将位置环抗干扰编队控制器描述为：

其中，K表示编队控制增益矩阵，Γ_i表示无人机i的所有相邻无人机形成的集合，

符号

为克罗内克积，z_i(t)是抗干扰补偿项。

优选的，在步骤3中，位姿双环抗干扰编队控制方式是将位置环抗干扰编队控制器转化为对每架四旋翼无人机姿态环的控制输入，从而在姿态环中调节无人机姿态和推力的大小以实现编队控制，在实际操作中，四旋翼无人机的俯仰角和横滚角通常在0度附近变化，从而有sinθ≈θ，cosθ≈1，sinφ≈φ，cosφ≈1，结合姿态环动力学模型，可将无人机i的期望姿态角表示为：

其中u_ip＝u_i4/m；

无人机姿态环控制器由期望姿态角和姿态环动力学模型联合确定，姿态环控制器表示为：

其中，k_φ1，k_φ2，k_θ1，k_θ2，k_ψ1，k_ψ2为控制增益，φ_i，θ_i，ψ_i为无人机当前姿态角，φ_i ^R，θ_i ^R，ψ_i ^R为所需给定的期望偏航角。

优选的，在步骤4中，位姿双环抗干扰编队控制方法，具体包括以下步骤：

步骤401：无人机群位置环动力学模型参数选取，按照四旋翼无人机动力学特性和实际应用情况设置阻尼参数α_x和α_v；

步骤402：队形指令设置，根据实际要求确定期望的编队队形，设置编队位置和编队速度的队形指令；

步骤403：队形指令检验，按照抗干扰编队可达判据检验队形指令是否满足要求，若满足判据要求，则继续执行步骤404；否则，队形指令不能被无人机群执行，返回至步骤402重新设置队形指令；

步骤404：编队控制增益设计，利用线性矩阵不等式求解对称正定矩阵，并根据该矩阵设计位置环抗干扰编队控制器的编队控制增益K；

步骤405：抗干扰补偿控制，根据自抗扰控制原理，设计扩张状态观测器确定位置环抗干扰编队控制器的抗干扰补偿项z_i(t)；

步骤406：位姿双环转化控制，将位置环抗干扰编队控制器确定的控制输入代入姿态环动力学模型中，确定无人机的期望姿态角，从而将无人机群位置环抗干扰编队控制转化为无人机单机姿态环控制；

步骤407：姿态环控制参数设置，根据无人机动力学特性和实际应用要求，设置姿态环控制器的控制增益和期望偏航角，至此完成位姿双环控制器全部参数设置；

步骤408：位姿双环抗干扰编队控制效果验证，用位置环抗干扰编队控制器和姿态环控制器对四旋翼无人机群实施编队控制，验证四旋翼无人机编队效果。

优选的，在步骤403中，采用如下抗干扰编队可达判据：

其中，σ是任意给定的正常数，t^*表示完成时间；上述抗干扰编队可达判据描述了编队位置队形指令f_ix(t)和编队速度队形指令f_iv(t)之间应当满足的关系。

优选的，在步骤404中，编队控制增益设计为

其中δ＞1/(2γ)，γ为拓扑图拉普拉斯矩阵的最小非零特征值，正定矩阵P满足下列不等式：

其中，B₁＝[1，0]^T，B₂＝[0，1]^T，ε为正常数。

优选的，在步骤405中，扩张状态观测器描述为：

其中，i＝1，2，…，N，z_i0(t)为观测器变量，β_i1＝2θ_ω和

为带宽，θ_ω为观测器频率。

优选的，在步骤406中，四旋翼无人机的期望姿态角由如下四旋翼无人机的姿态环控制输入转化而得：

通过步骤406的操作，以期望姿态角为连接，建立四旋翼无人机群位置环抗干扰编队控制到无人机单机姿态环控制的转化关系，从而以位姿双环联合抗干扰的方式实现对四旋翼无人机群的编队控制。

本发明的有益效果：

本发明可以将四旋翼无人机群位置环的抗干扰编队控制转化为姿态环的单机姿态控制，其核心是将位置环控制输入转化为姿态环的期望姿态角，通过姿态环驱动位置环的方式实现编队控制。该方法的有益效果是可以直接实施于存在明显位姿耦合的四旋翼无人机群中，实用性较强。

本发明根据自抗扰控制原理，设计扩张状态观测器估计并补偿外部扰动对无人机群编队控制的影响，并将扰动补偿加入位置环抗干扰编队控制器中，使得位置环编队控制具备抗干扰能力，从而实现位姿双环抗干扰编队控制。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1所示为本发明的四旋翼无人机机体坐标系和地理坐标系的示意图；(a) 机机体坐标系；(b)地理坐标系。

图2所示为四旋翼无人机群位姿双环控制示意图。

图3所示为位姿双环抗干扰编队控制算法流程图。

图4所示为四旋翼无人机通信拓扑图。

图5所示为三架无人机正三角形螺旋上升编队轨迹图。

图6所示为一号无人机实际姿态角对期望姿态角的跟踪曲线图。

图7所示为二号无人机实际姿态角对期望姿态角的跟踪曲线图。

图8所示为三号无人机实际姿态角对期望姿态角的跟踪曲线图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例具体提供了一种四旋翼无人机群位姿双环抗干扰编队控制方法，以单机姿态控制驱动无人机群位置编队，使四旋翼无人机群形成并保持期望的编队队形，并根据自抗扰控制方式实现位置环抗干扰编队控制，具体包括以下步骤：

步骤1：构建无人机群位置环动力学模型和无人机单机姿态环动力学模型；

步骤2：然后根据编队指令设计位置环抗干扰编队控制器；

步骤3：进而依据位置环抗干扰编队控制器的输入和无人机单机姿态环动力学模型，将位置控制转化为期望的姿态角，设计出单机姿态控制器；

步骤4：以单机姿态环控制器驱动机群位置环编队控制过程的方式实现位姿双环抗干扰编队控制。

无人机群动力学模型和位姿双环控制器

对于由N架四旋翼无人机组成的无人机群，其动力学模型包括机群位置环动力学模型和单机姿态环动力学模型两部分，其中，机群位置环动力学模型建模如下：

其中，i＝1,2,…,N，x_i(t)、v_i(t)、u_i(t)和q_i(t)均为三维向量，代表无人机i的位置、速度、控制量和干扰项，α_x和α_v表示无人机位置和速度的阻尼。无人机群的通信连接关系被描述为拓扑图，无人机i表示为图的第i个节点，节点之间的通信连接表示为图的边，通信作用权重表示为边权重w_ij，拓扑图的拉普拉斯矩阵为L。

每架四旋翼无人机的姿态环动力学模型由无人机的平动方程和转动方程联合确定，根据四旋翼无人机模型为对称刚体且质心与几何中心重合的特性，每架四旋翼无人机的姿态环动力学模型均可建模如下：

其中，{a_x，a_y，a_z}表示四旋翼无人机机体沿地理坐标系三个坐标轴方向的加速度，{I_x，I_y，I_z}为四旋翼无人机在机体坐标系下绕三个坐标轴旋转的转动惯量， {ω_x，ω_y，ω_z}是四旋翼无人机在机体坐标系下绕三个坐标轴旋转的转动角速度， {ω₁，ω₂，ω₃，ω₄}表示四个旋翼的转速，ω_sum＝-ω₁+ω₂-ω₃+ω₄，I_r是螺旋桨自身的转动惯量，l是四旋翼无人机质心到旋翼旋转轴之间的距离，b为旋翼的升力系数， d为旋翼的阻力系数，g为重力加速度，m为四旋翼无人机质量，{φ，θ，ψ}是姿态角，分别为俯仰角、横滚角和偏航角，

编队指令由分段连续可微分的向量

表示，根据编队指令将位置环抗干扰编队控制器描述为：

其中，K表示编队控制增益矩阵，Γ_i表示无人机i的所有相邻无人机形成的集合，

符号

为克罗内克积，z_i(t)是抗干扰补偿项。

根据上述无人机群位置环和单机姿态环动力学模型，将位置环抗干扰编队控制器(3)转化为对每架无人机姿态环的控制输入，从而在姿态环中调节无人机姿态和推力的大小以实现编队控制。在实际操作中，无人机的俯仰角和横滚角通常在0度附近变化，从而有sinθ≈θ，cosθ≈1，sinφ≈φ，cosφ≈1，结合姿态环动力学模型，无人机的姿态环控制输入描述为：

根据上述姿态环控制输入表达式，结合姿态环动力学模型(2)，可将无人机 i的期望姿态角表示为：

其中u_ip＝u_i4/m。

根据期望姿态角和姿态环动力学模型，构建如下姿态环控制器：

其中，k_φ1，k_φ2，k_θ1，k_θ2，k_ψ1，k_ψ2为控制增益，φ_i，θ_i，ψ_i为无人机当前姿态角，φ_i ^R，θ_i ^R，ψ_i ^R为所需给定的期望偏航角。

说明1：本发明的姿态环动力学模型需要用到四旋翼无人机机体坐标系和地理坐标系。其中，机体坐标系的三个坐标轴{x_b,y_b,z_b}的正方向设为机身的右、前、上三个方向；地理坐标系依托地球表平面建立，其坐标轴{x_e,y_e,z_e}的正方向分别指向东、北、天三个方向。机体坐标系和地理坐标系的示意图如图1所示。无人机的电机编号绕z_b轴逆时针旋转依次记为1、2、3、4，其中y_b轴正方向所在的电机编号为1，x_b轴正方向所在的电机编号为4。电机1、3和电机2、4转向相反，控制四个电机的转速即可控制四旋翼无人机的俯仰、横滚和偏航运动。

说明2：本发明的四旋翼无人机水平方向的位置运动由姿态角驱动，高度方向的运动由旋翼的升力直接驱动。因此，无人机群的位置环控制由姿态控制驱动，在实际飞行中，无人机群主要考虑位置环的协同编队，对姿态的协同要求不高，因此，在无人机群建模时，将每架无人机视为一个二阶系统，即经典的位置-速度-加速度构成的运动系统。在单机建模时，则要考虑姿态环的控制。无人机群位置环和姿态环的双环控制示意图如图2所示。

位姿双环抗干扰编队控制算法

如图3所示，位姿双环抗干扰编队控制算法，包括如下步骤：

步骤401：无人机群位置环动力学模型参数选取，按照四旋翼无人机动力学特性和实际应用情况设置阻尼参数α_x和α_v；

步骤402：队形指令设置，根据实际要求确定期望的编队队形，设置编队位置和编队速度的队形指令；

步骤403：队形指令检验，按照如下抗干扰编队可达判据检验队形指令是否满足要求：

其中，σ是任意给定的正常数，t^*表示完成时间；上述抗干扰编队可达判据描述了编队位置队形指令f_ix(t)和编队速度队形指令f_iv(t)之间应当满足的关系；若满足判据要求，则继续执行步骤404；否则，队形指令不能被无人机群执行，返回至步骤402重新设置队形指令；

步骤404：编队控制增益设计，利用线性矩阵不等式

求解对称正定矩阵P，其中， B₁＝[1，0]^T，B₂＝[0，1]^T，ε为正常数；并根据该矩阵设计位置环抗干扰编队控制器的编队控制增益

其中δ＞1/(2γ)，γ为拓扑图拉普拉斯矩阵的最小非零特征值；

步骤405：抗干扰补偿控制，根据自抗扰控制原理，设计扩张状态观测器确定位置环抗干扰编队控制器的抗干扰补偿项z_i(t)；

其中，i＝1，2，…，N，z_i0(t)为观测器变量，β_i1＝2θ_ω和

为带宽，θ_ω为观测器频率。

步骤406：位姿双环转化控制，将位置环抗干扰编队控制器确定的控制输入代入姿态环动力学模型中，确定无人机的期望姿态角，从而将无人机群位置环抗干扰编队控制转化为无人机单机姿态环控制；

步骤407：姿态环控制参数设置，根据无人机动力学特性和实际应用要求，设置姿态环控制器的控制增益和期望偏航角，至此完成位姿双环控制器全部参数设置；

步骤408：位姿双环抗干扰编队控制效果验证，用位置环抗干扰编队控制器和姿态环控制器对四旋翼无人机群实施编队控制，验证四旋翼无人机编队效果。

实施例

通过MATLAB数值仿真的方式验证四旋翼无人机群位姿双环抗干扰编队控制方法的有效性。

编队控制为三架无人机组成正三角形编队，整个编队呈螺旋上升队形。四旋翼无人机质量为m＝1.976kg，四旋翼无人机在机体系下绕三个坐标轴旋转的转动惯量分别为I_x＝I_y＝0.01253，I_z＝0.02359，螺旋桨自身的转动惯量I_r＝0.02359，单位为kg·m²。四旋翼无人机的升力系数为1.026×10^-5N/(rad/s)²，阻力系数为 1.313×10^-5N·m/(rad/s)²，质心到旋翼旋转轴之间的距离为0.225m。四个旋翼的初始转速设为{ω₁，ω₂，ω₃，ω₄}＝{0，0，0，0}rad/s。无人机位置环动力学模型中的干扰项设置为：

位置和速度阻尼为α_x＝-0.01和α_v＝0，扩张状态观测器带宽参数设置为β_i1＝40和β_i2＝400。编队指令在水平方向为正三角形，三架无人机分别位于三角形的三个顶点，并绕中心旋转，旋转半径为r＝2m。无人机的通信关系拓扑图可描述为一个0-1权重有向图，如图4所示。通过线性矩阵不等式求解得到位置环抗干扰编队控制器的编队控制增益为

图5描述了三架无人机在位姿双环抗干扰编队控制算法作用下进行正三角形螺旋上升飞行的轨迹，三架无人机的初始状态由正方形表示，终止状态由五角星表示，飞行轨迹由不同线型的曲线分别表示。由图5可以看出，在干扰条件下，三架无人机能够形成正三角形编队队形并螺旋上升到达终点位置。

图6-图8分别展示了30秒内一号到三号无人机实际姿态角对期望姿态角的跟踪曲线，图中虚线表示期望姿态角的曲线，实线表示实际姿态角的曲线。从图 6-图8可以看出，三架无人机的俯仰角、横滚角和偏航角都对期望值有较好的跟踪效果，验证了姿态环控制的有效性。从上述仿真结果可以看出，在扰动条件下，通过采用位姿双环抗干扰编队控制算法，能够实现对三架无人机位置环和姿态环的有效控制，保证三架无人机形成正三角形编队队形并实现螺旋上升编队飞行。

本发明将位置环控制输入转化为姿态环的期望姿态角，通过姿态环驱动位置环的方式实现编队控制。该方法的有益效果是可以直接实施于存在明显位姿耦合的四旋翼无人机群中，实用性较强。

以上所述，仅用以说明本发明的技术方案而非限制，本领域普通技术人员对本发明的技术方案所做的其它修改或者等同替换，只要不脱离本发明技术方案的精神和范围，均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，以单机姿态控制驱动无人机群位置编队，使四旋翼无人机群形成并保持期望的编队队形，并根据自抗扰控制方式实现位置环抗干扰编队控制，具体包括以下步骤：

步骤1：构建无人机群位置环动力学模型和无人机单机姿态环动力学模型；

步骤2：然后根据编队指令设计位置环抗干扰编队控制器；

步骤3：进而依据位置环抗干扰编队控制器的输入和无人机单机姿态环动力学模型，将位置控制转化为期望的姿态角，设计出单机姿态控制器；

步骤4：以单机姿态环控制器驱动机群位置环编队控制过程的方式实现位姿双环抗干扰编队控制。

2.根据权利要求1所述的一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，在步骤1中，四旋翼无人机群由N架四旋翼无人机组成，其中第i架四旋翼无人机的位置环动力学模型建模如下：

其中，i＝1,2,…,N，x_i(t)、v_i(t)、u_i(t)和q_i(t)均为三维向量，代表无人机i的位置、速度、控制量和干扰项，α_x和α_v表示无人机位置和速度的阻尼。

3.根据权利要求2所述的一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，在步骤1中，每架四旋翼无人机的姿态环动力学模型由无人机的平动方程和转动方程联合确定，根据四旋翼无人机模型为对称刚体且质心与几何中心重合的特性，每架四旋翼无人机的姿态环动力学模型均可建模如下：

其中，{a_x，a_y，a_z}表示四旋翼无人机机体沿地理坐标系三个坐标轴方向的加速度，{I_x，I_y，I_z}为四旋翼无人机在机体坐标系下绕三个坐标轴旋转的转动惯量，{ω_x，ω_y，ω_z}是四旋翼无人机在机体坐标系下绕三个坐标轴旋转的转动角速度，{ω₁，ω₂，ω₃，ω₄}表示四个旋翼的转速，ω_sum＝-ω₁+ω₂-ω₃+ω₄，I_r是螺旋桨自身的转动惯量，l是四旋翼无人机质心到旋翼旋转轴之间的距离，b为旋翼的升力系数，d为旋翼的阻力系数，g为重力加速度，m为四旋翼无人机质量，{φ，θ，ψ}是姿态角，分别为俯仰角、横滚角和偏航角，

4.根据权利要求3所述的一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，在步骤2中，编队指令由分段连续可微分的向量

表示，根据编队指令将位置环抗干扰编队控制器描述为：

其中，K表示编队控制增益矩阵，Γ_i表示无人机i的所有相邻无人机形成的集合，

符号

为克罗内克积，z_i(t)是抗干扰补偿项。

5.根据权利要求4所述的一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，在步骤3中，位姿双环抗干扰编队控制方式是将位置环抗干扰编队控制器转化为对每架四旋翼无人机姿态环的控制输入，从而在姿态环中调节无人机姿态和推力的大小以实现编队控制，在实际操作中，四旋翼无人机的俯仰角和横滚角通常在0度附近变化，从而有sinθ≈θ，cosθ≈1，sinφ≈φ，cosφ≈1，结合姿态环动力学模型，可将无人机i的期望姿态角表示为：

其中u_ip＝u_i4/m；

无人机姿态环控制器由期望姿态角和姿态环动力学模型联合确定，姿态环控制器表示为：

其中，k_φ1，k_φ2，k_θ1，k_θ2，k_ψ1，k_ψ2为控制增益，φ_i，θ_i，ψ_i为无人机当前姿态角，φ_i ^R，θ_i ^R，ψ_i ^R为所需给定的期望偏航角。

6.根据权利要求5所述的一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，在步骤4中，位姿双环抗干扰编队控制方法，具体包括以下步骤：

步骤401：无人机群位置环动力学模型参数选取，按照四旋翼无人机动力学特性和实际应用情况设置阻尼参数α_x和α_v；

步骤402：队形指令设置，根据实际要求确定期望的编队队形，设置编队位置和编队速度的队形指令；

步骤403：队形指令检验，按照抗干扰编队可达判据检验队形指令是否满足要求，若满足判据要求，则继续执行步骤404；否则，队形指令不能被无人机群执行，返回至步骤402重新设置队形指令；

步骤404：编队控制增益设计，利用线性矩阵不等式求解对称正定矩阵，并根据该矩阵设计位置环抗干扰编队控制器的编队控制增益K；

步骤405：抗干扰补偿控制，根据自抗扰控制原理，设计扩张状态观测器确定位置环抗干扰编队控制器的抗干扰补偿项z_i(t)；

步骤406：位姿双环转化控制，将位置环抗干扰编队控制器确定的控制输入代入姿态环动力学模型中，确定无人机的期望姿态角，从而将无人机群位置环抗干扰编队控制转化为无人机单机姿态环控制；

步骤407：姿态环控制参数设置，根据无人机动力学特性和实际应用要求，设置姿态环控制器的控制增益和期望偏航角，至此完成位姿双环控制器全部参数设置；

步骤408：位姿双环抗干扰编队控制效果验证，用位置环抗干扰编队控制器和姿态环控制器对四旋翼无人机群实施编队控制，验证四旋翼无人机编队效果。

7.根据权利要求6所述的一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，在步骤403中，采用如下抗干扰编队可达判据：

其中，σ是任意给定的正常数，t^*表示完成时间；上述抗干扰编队可达判据描述了编队位置队形指令f_ix(t)和编队速度队形指令f_iv(t)之间应当满足的关系。

8.根据权利要求6所述的一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，在步骤404中，编队控制增益设计为

其中δ＞1/(2γ)，γ为拓扑图拉普拉斯矩阵的最小非零特征值，正定矩阵P满足下列不等式：

其中，B₁＝[1，0]^T，B₂＝[0，1]^T，ε为正常数。

9.根据权利要求6所述的一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，在步骤405中，扩张状态观测器描述为：

其中，i＝1，2，…，N，z_i0(t)为观测器变量，β_i1＝2θ_ω和

为带宽，θ_ω为观测器频率。

10.根据权利要求6所述的一种四旋翼无人机群位姿双环抗干扰编队控制方法，其特征在于，在步骤406中，四旋翼无人机的期望姿态角由如下四旋翼无人机的姿态环控制输入转化而得：

通过步骤406的操作，以期望姿态角为连接，建立四旋翼无人机群位置环抗干扰编队控制到无人机单机姿态环控制的转化关系，从而以位姿双环联合抗干扰的方式实现对四旋翼无人机群的编队控制。

Images