CN116301041B - 固定翼从机追随领机的编队飞行控制方法、装置和系统 - Google Patents

Abstract

本发明公开了一种固定翼从机追随领机的编队飞行控制方法、装置和系统，属于多无人机编队控制技术领域，通过实时获取每个从机的本机领机对应的飞行状态信息；根据每个从机及领机各自对应的飞行状态信息将预设的本机在领机机体系下的期望编队误差转换为在本机北东地NED坐标系的相对位置误差，进而计算每个从机的加速度控制指令；利用加速度控制指令计算期望俯仰角指令和期望滚转角指令，以控制每个从机对领机进行飞行状态跟踪。本发明结合单个固定翼无人机的飞行特点以及飞行坐标系之间转换的理论公式，将每个从机位置误差统一到同一坐标系，进而计算加速度控制指令，最终控制每个从机对领机进行飞行状态跟踪，能够提升编队飞行的控制精度。

Description

固定翼从机追随领机的编队飞行控制方法、装置和系统

技术领域

本发明属于多无人机编队控制技术领域，更具体地，涉及一种固定翼从机追随领机的编队飞行控制方法、装置和系统。

背景技术

现代战争以及现代无人机应用中，单无人机系统已经不能满足需求。因此多无人机系统在军民领域越来越受到人们的欢迎。在军用方面，多无人机系统能够在很大程度上提高攻击力度以及侦察范围；在民用方面，多无人机系统能够用于火灾检测，农林作业等方面。

相比于四旋翼无人机，固定翼无人机具有运动速度快，载重大，航程长等特点，在执行任务方面有明显的优势，但此类无人机具备复杂的耦合性很强的非线性动力学模型，其控制与飞行状态和气动环境紧密相关，为了避免失速，固定翼无人机常具有最小飞行速度以及有限的转弯半径，这对协同编队提出了很高的要求。

为了能够实现多架固定翼无人机的协同飞行，执行复杂的作战任务或满足民用需求，需要一个适合于固定翼无人机的软硬件系统。目前很难找到可靠性较高、通信稳定、任务部署和算法验证便捷的多固定翼无人机协同飞行系统，且以往的协同系统多数是用于地面载具，对运动控制的准确度要求不高，不能满足无人机协同飞行的需求，不能够帮助无人机完成协同作战任务。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种固定翼从机追随领机的编队飞行控制方法、装置和系统，其目的在于，结合单个固定翼无人机的飞行特点以及飞行坐标系之间转换的理论公式将多个无人机的位置误差统一到同一坐标系，以计算每个从机的加速度控制指令，最终在多个从机的位置误差坐标系统一的基础上控制每个从机对所述领机进行飞行状态跟踪，完成每个所述从机追随领机编队飞行任务，能够显著提升编队协同飞行的控制精度，由此解决现有多无人机协同编队控制准确性低的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种固定翼从机追随领机的编队飞行控制方法，包括：

S1：实时获取每个所述从机及所述领机各自对应的飞行状态信息；

S2：根据每个所述从机及所述领机各自对应的飞行状态信息，将每个所述从机在领机机体系下预设的期望编队误差转换为在本机北东地NED坐标系的相对位置误差；

S3：根据每个所述从机在本机NED坐标系的位置误差和当前位置速度状态，计算每个所述从机的加速度控制指令；利用每个所述从机的加速度控制指令，计算每个所述从机的期望俯仰角指令和期望滚转角指令；

S4：利用每个所述从机的期望俯仰角指令和期望滚转角指令，控制每个所述从机对所述领机进行飞行状态跟踪，从而实现每个所述从机追随所述领机的编队飞行控制。

在其中一个实施例中，所述从机的飞行状态信息包括当前偏航角；所述领机的飞行状态信息包括当前位置和当前偏航角；所述S2包括：

S21：根据所述领机的当前偏航角将每个所述从机在领机机体系下预设的期望编队误差转化为所述领机NED坐标系的编队误差；根据所述领机的当前位置中的经纬度，将每个所述从机在领机NED坐标系的编队误差转化为本机期望绝对位置中的经纬度；

S22：根据每个所述从机对应的本机期望绝对位置中的经纬度与领机当前位置中的经纬度的差值以及所述从机的当前偏航角，计算每个所述从机在本机航迹坐标系的相对位置误差；

S23：将每个所述从机在本机航迹坐标系的相对位置误差转化为每个所述从机在本机NED坐标系的相对位置误差。

在其中一个实施例中，所述S2包括：

S21：利用公式将每个所述从机在领机机体系下预设的期望编队误差/>转化为每个所述从机在所述领机NED坐标系的编队误差利用公式/>将/>转化为本机期望绝对位置中的经纬度/>

S22：利用公式根据与领机当前位置中的经纬度(λ^f,φ^f)的差值，计算每个所述从机在本机航迹坐标系的相对位置误差(Δx_kf,Δy_kf)；

S23：利用公式将(Δx_kf,Δy_kf)转化每个所述从机在为本机NED坐标系的相对位置误差(Δx_Nf,Δy_Ef)；

其中，ψ_l为所述领机的偏航角；所述领机的经纬度为(λ^l,φ^l)，R_earth为地球半径；ψ_f为所述从机的偏航角。

在其中一个实施例中，所述S3包括：

S31：利用公式计算每个所述从机对应的加速度控制指令u_i(t)；

S32：利用所述加速度控制指令u_i(t)计算每个所述从机对应期望俯仰角指令θ_cmd和期望滚转角指令γ_cmd；

其中，k_i(t)表示从机i控制参数估计值，由从机i的控制精度参数确定，T表示转置，x_i(t)表示从机i在所述本机NED坐标系下的位置速度状态，p_i表示从机i的当前位置，v_i表示从机i的当前速度，x₀(t)为所述领机在所述领机NED坐标系下的位置速度状态，/>p₀表示所述领机的当前位置，v₀表示所述领机的当前速度，u₀(t)为所述领机的加速度控制指令，Δ_i＝[Δx_Nf,Δy_Nf,0,0]^T为所述领机与从机i之间在本机NED坐标系下的期望状态误差。

在其中一个实施例中，所述从机i的控制参数估计值k_i(t)采用如下方式设置：

所述领机与从机i之间的期望位置误差表示为：Δ_i＝[δ_i,p δ_i,v]^T；

其中，δ_i,p＝[Δx_Nf,Δy_Nf]^T表示从机i与所述领机之间的期望位置误差，δ_i,v表示从机i和位于队形中间的所述领机之间的期望速度误差δ_i,v＝v_i-v₀＝[0,0]^T，η表示给定矩阵，且是对称正定矩阵；P是正定对称矩阵，B_m表示从机i的状态空间模型中的控制矩阵，I₂表示二阶单位矩阵，e_i(t)表示从机i的跟踪误差，e_i(t)＝x_i(t)-Δ_i。

在其中一个实施例中，所述加速度控制指令u_i(t)包括x轴控制指令a_x和y轴控制指令a_y，表示为u_i(t)＝[a_x,a_y]^T；所述S32包括：

利用所述x轴控制指令a_x计算期望速度指令V_cmd；

基于PID控制方法和所述期望速度指令V_cmd计算所述期望俯仰角指令θ_cmd和所述期望滚转角指令γ_cmd，以使本机根据所述期望俯仰角指令θ_cmd和所述期望滚转角指令γ_cmd对所述领机进行飞行状态跟踪；

其中，γ_cmd＝arctan(a_y/g)， 表示控制参数，limit表示限幅函数，用于保证积分项在/>之间，g表示重力加速度。

在其中一个实施例中，所述S4包括：

将所述期望俯仰角指令θ_cmd和期望滚转角指令γ_cmd输入如下公式计算每个所述从机的升降舵δ_e和副翼舵的数字舵偏δ_a控制每个所述从机对所述领机进行飞行状态跟踪；

其中，表示俯仰角和滚转角的PID控制参数，k_q＝15/V²表示动压修正项，V为每个所述从机的当前速度，θ表示每个所述从机的当前俯仰角，γ表示每个所述从机的当前滚转角，ω_θ,ω_γ分别为每个所述从机的当前俯仰角和滚转角速率。

按照本发明的另一方面，提供了一种固定翼从机追随领机的编队飞行控制装置，包括：

协同控制环模块，用于实时获取每个所述从机及所述领机各自对应的飞行状态信息；根据每个所述从机及所述领机各自对应的飞行状态信息，将每个所述从机在领机机体系下预设的期望编队误差转换为在本机北东地NED坐标系的相对位置误差；根据每个所述从机在本机NED坐标系的位置误差和当前位置速度状态，计算每个所述从机的加速度控制指令；利用每个所述从机的加速度控制指令，计算每个所述从机的期望俯仰角指令和期望滚转角指令；

姿态控制环模块，用于利用每个所述从机的期望俯仰角指令和期望滚转角指令，控制本机对所述领机进行飞行状态跟踪，从而实现每个所述从机追随领机的编队飞行控制。

按照本发明的另一方面，提供了一种固定翼从机追随领机的编队飞行控制系统，包括存储器和控制器，包括：

所述存储器存储有计算机程序；

所述控制器执行所述计算机程序时实现所述的方法的步骤。

按照本发明的另一方面，提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的步骤。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

(1)本发明提供了一种固定翼从机追随领机的编队飞行控制方法，结合单个固定翼无人机的飞行特点以及飞行坐标系之间转换的理论公式，将每个所述从机在领机机体系下预设的期望编队误差转换为在本机北东地NED坐标系的相对位置误差，能够将多个无人机的位置误差统一到同一坐标系，在此基础上计算每个所述从机的加速度控制指令，并将其转化为姿态控制指令，最终高精度的控制多从机跟踪领机的编队飞行控制。本发明通过将多个无人机的位置误差统一到同一坐标系，能够显著提升协同编队飞行的控制精度，由此解决现有多无人机协同编队控制准确性低的技术问题。

(2)本发明根据每个从机和领机各自对应的飞行状态信息，将每个从机在领机机体系下预设的期望编队误差转化为领机NED坐标系的编队误差进而转化为本机期望绝对位置中的经纬度；根据每个从机对应的本机期望绝对位置中的经纬度与领机当前位置中的经纬度的差值，计算每个从机在本机航迹坐标系的相对位置误差，最终转化为每个从机在本机NED坐标系的相对位置误差。相对于现有的直接用北东地坐标系误差，本申请中从机对应的相对位置误差携带更丰富的飞行状态信息，且每个从机的的位置误差统一到同一坐标系，能够提高编队飞行的控制精度。

(3)本发明提供了一种固定翼从机追随领机的编队飞行控制系统，能够实现多个从机与主机的误差坐标系统一，显著提升了协同编队飞行的控制精度，此系统能够快速对协同飞行控制算法进行有效准确的验证。

附图说明

图1为本发明一实施例提供的固定翼从机追随领机的编队飞行控制方法系统的流程图。

图2为本发明一实施例提供的固定翼从机追随领机的编队飞行控制方法的系统架构图。

图3为本发明一实施例提供的机体坐标系与北东地坐标系转换图。

图4为本发明一实施例提供的北东地坐标系与地心地固坐标系转换图。

图5为本发明一实施例提供的多固定翼无人机控制节点连接图。

图6为本发明一实施例提供的多固定翼无人机软硬件参照图。

图7为本发明一实施例提供的多固定翼无人机硬件设备连接图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明一种固定翼从机追随领机的编队飞行控制方法，其对应的系统框图如图2左侧部分所示，可以在Ubuntu系统上进行搭载。此主要分为两个控制器模块(协同控制环模块和姿态控制环模块)，协同控制环模块对应基于ROS的上层编队控制器，姿态控制环模块对应基于PX4的底层姿态控制器。上层编队控制器可以通过MAVROS包从底层姿态控制器中获取预设格式的飞行状态信息，如MAVLINK格式的传感器数据，包括无人机姿态信息，位置信息，飞行速度信息等。领机执行特定的飞行任务，每一架从机都通过ROS的订阅/发布机制从领机订阅领机的状态信息，无人机在上层ROS系统中实现编队控制器以及任务发布后，通过MAVROS功能包向底层的姿态控制环发送期望姿态和油门指令。姿态控制环基于PID控制方法向舵机和电机发送指令。

多固定翼无人机协同软件系统可以先给每架无人机上传飞行任务，通过地面终端键入飞行指令，包括有起飞段、编队段、降落段，各阶段计算得到相应的姿态控制指令，发送到PX4内环进行PID姿态跟踪。起飞段包括无人机的一键式起飞，需要提前设定好起飞任务。无人机起飞以后，如果不发送编队指令，无人机就会按照设定的任务进行飞行，依次飞过给定的任务航点。可在编队段指令给定后，每个从机执行编队控制系统的控制流程，使无人机之间能够形成提前设计的队形。降落段指令给定后，无人机将会通过降落伞或自主降落的方式进行降落。

需要说明的是，可以采用一个控制系统执行本发明的固定翼从机追随领机的编队飞行控制方法，实现对每个从机追踪领机的编队飞行控制。

实施例1

本发明一种固定翼从机追随领机的编队飞行控制方法，应用于每个从机，可以分为四个阶段，状态获取与更新、误差坐标变换、编队算法求解控制量和飞行控制。如图2右侧部分所示，具体包括：

S1：实时获取每个从机及领机各自对应的飞行状态信息；

S2：根据每个从机及领机各自对应的飞行状态信息，将每个从机在领机机体系下预设的期望编队误差转换为本机北东地(North East Down，NED)坐标系的相对位置误差；

S3：根据每个从机在本机NED坐标系的位置误差和当前位置速度状态，计算每个从机的加速度控制指令；利用每个从机的加速度控制指令，计算每个从机的期望俯仰角指令和期望滚转角指令；

S4：利用每个从机的期望俯仰角指令和期望滚转角指令，控制每个从机对领机进行飞行状态跟踪，从而实现每个从机追随领机的编队飞行控制。

具体的，在误差计算开始前，需要更新当前时刻的领机的飞行状态信息和每个从机的飞行状态信息，以便计算实时误差。领机的飞行状态信息包括：当前偏航角ψ_l、当前位置中的经纬度(λ^l,φ^l)，NED坐标系下的位置速度信息每个从机的飞行状态信息包括：当前速度V、当前俯仰角速率ω_θ，当前滚转角速率ω_γ，当前位置中的经纬度(λ^l,φ^l)，期望位置中的经度和纬度/>NED坐标系下的位置速度信息/>

实施例2

控制目标可以是让无人机群形成设计好的编队队形，因此可以基于领机的机体系设计编队队形，步骤S2包括：将每个从机的期望位置与实际位置之间的误差统一到同一个坐标系中。为了提高位置坐标的准确性，将领机与每个从机的位置误差都统一到绝对坐标系即地心地固坐标系。步骤S2可以包括：

S21：根据领机的当前偏航角将每个从机在领机机体系下预设的期望编队误差转化为领机NED坐标系的编队误差；根据领机的当前位置中的经纬度，将每个从机在领机NED坐标系的编队误差转化为本机期望绝对位置中的经纬度；

S22：根据每个从机对应的本机期望绝对位置中的经纬度与领机当前位置中的经纬度的差值以及从机的当前偏航角，计算每个从机在本机航迹坐标系的相对位置误差；

S23：将每个从机在本机航迹坐标系的相对位置误差转化为每个从机在本机NED坐标系的相对位置误差。

实施例3

为了便于进行控制器的设计，还需将每个从机的绝对坐标系下的位置误差转换为NED坐标系下的位置误差以及航迹坐标系的位置误差。详细过程如下：

1)针对每个从机，定义本机在领机机体坐标系下的期望位置，形成期望的编队队形。假设本机在领机的机体系下的坐标即领机机体系下的编队误差为

2)获取每个从机的在领机NED坐标系下的编队误差领机机体坐标系与其NED坐标系之间的转换关系如图3所示，相对应的转换公式为：图4为北东地坐标系与地心地固坐标系转换图。

3)将领机NED坐标系下的编队误差转化为本机期望绝对位置即期望经纬度/>NED坐标系下NE方向的位置误差可以等效为地心地固坐标系下的一段弧长，因此能够完成北东地坐标与期望经纬度之间的转换。领机的经纬度为(λ^l,φ^l)，那么上述转换公式为：/>其中，R_earth为地球半径。

4)将本机期望位置与实际位置(λ^f,φ^f)的绝对位置误差转换为本机航迹系相对位置误差(Δx_kf,Δy_kf)及本机NED坐标系的相对位置误差(Δx_Nf,Δy_Ef)。转换公式为：

实施例4

在步骤S2获得了领机和每个从机之间的位置误差之后，步骤S3需要根据位置误差计算得到期望的俯仰角滚转角指令θ_cmd,γ_cmd。固定翼无人机通常在巡航段进行编队控制，因此编队控制器主要基于巡航段与每个从机在本机航迹坐标系x和y方向的位置误差进行设计得到期望的加速度，经过相应转化得到期望的姿态角。

具体包括：S31：利用公式计算每个从机的加速度控制指令u_i(t)；S32：利用加速度控制指令u_i(t)计算期望俯仰角指令θ_cmd和期望滚转角指令γ_cmd，以控制每个从机对领机进行飞行状态跟踪；其中，k_1i(t)表示从机i控制参数估计值k₁，T表示转置，x_i(t)为从机i位置速度状态，x_i(t)为从机i在本机NED坐标系下的位置速度状态，/>x₀(t)为领机在领机NED坐标系下的位置速度状态，u₀(t)为领机的加速度控制指令，Δ_i＝[Δx_Nf,Δy_Nf,0,0]^T为领机与从机i之间在本机NED坐标系下的期望状态误差。

实施例5

定义第i架固定翼无人机的状态空间模型为如下形式：其中，A、B分别表示从机i状态空间模型中的系统矩阵和控制矩阵；/>I₂表示二阶单位矩阵，x_i表示无人机的位置速度状态，u_i表示无人机轴向加速度。设定同构无人机的标称虚拟领机的状态空间模型如下：/>其中，x₀分别表示领机。

定义相邻无人机之间的期望位置误差为：δ_i,p＝[x_i,y_i]^T-[x₀,y₀]^T；

将上述位置误差扩展为状态误差：

δ_i,p＝p_i-p₀，δ_i,v＝δ_0,v＝[0,0,0]^T；

假设对于系统矩阵有如下条件满足：其中A_m∈R^4×4,B_m∈R^4×2。且存在已知的矩阵η∈R^2×2，使得/>可以设计编队控制律如下：/>x_i(t)表示第i个无人机在xy平面的位置速度，Δ_i＝[Δx_Nf,Δy_Nf,0,0]^T，x₀(t)表示领机xy平面的位置速度；u₀(t)表示领机加速度；其中，参数的自适应更新律为：/>其中，P＝P^T＞0，且满足/>在上述控制律的作用下，无人机能够根据编队位置误差(Δx_Nf,Δy_Ef)计算得到控制指令u_i(t)＝[a_x,a_y]^T。

实施例6

步骤S32具体包括：将计算得到的x轴控制指令a_x积分得到期望速度指令然后根据PID控制方法获得期望的俯仰角根据y轴控制指令a_y生成期望的滚转角指令γ_cmd＝arctan(a_y/g)。其中，/>表示控制参数，limit表示限幅函数，保证积分项在/>之间，g表示重力加速度。

实施例7

步骤S4包括：上层位置编队控制器完成编队任务后，PX4能够接收MAVROS传过来的期望姿态角并对其实现快速准确跟踪，设计控制律如下：

其中δ_e,δ_a分别表示升降舵和滚转舵的数字舵偏，表示俯仰角和滚转角的PID控制参数，k_q＝15/V²表示动压修正项，V为固定翼无人机的实际速度，θ,γ分别表示俯仰角和滚转角。ω_θ,ω_γ分别为每个从机的当前俯仰角和滚转角速率。

实施例8

本发明提供了一种固定翼从机追随领机的编队飞行控制装置，包括：

协同控制环模块，用于实时获取每个从机及领机各自对应的飞行状态信息；根据每个从机及领机各自对应的飞行状态信息，将每个从机在领机机体系下预设的期望编队误差转换为在本机北东地NED坐标系的相对位置误差；根据每个从机在本机NED坐标系的位置误差，计算每个从机的加速度控制指令；利用每个从机的加速度控制指令，计算每个从机的期望俯仰角指令和期望滚转角指令；

姿态控制环模块，用于利用每个从机的期望俯仰角指令和期望滚转角指令，控制每个从机对领机进行飞行状态跟踪，从而实现每个从机追随领机的编队飞行控制。

本发明提供了一种固定翼从机追随领机的编队飞行控制装置的ROS节点分为操作节点switch_node、执行节点fw_main_node、打包节点msg_states_node三类。操作节点负责发布指令，控制每架无人机在起飞，任务，编队，降落等飞行阶段之间的切换。不仅能够单独对某一架飞机进行控制，也能够对所有飞机进行同时控制。执行节点在收到操作节点的指令后，会执行对应的飞行阶段，包括起飞段子程序、编队段子程序、降落段子程序等，每段子程序都能够计算得到当前阶段的制导指令，如标志位指令、期望姿态角指令等。打包节点从执行节点收到指令，并将此信息进行打包，通过MAVROS发送给PX4。PX4在接收到指令后会执行相应的任务。各节点之间的连接图如图5所示。

实施例9

本发明提供了一种固定翼从机追随领机的编队飞行控制系统，包括存储器、上层位置编队控制器和底层姿态跟踪控制器，包括：

存储器存储有计算机程序；

上层位置编队控制器执行计算机程序时实现方法的步骤，得到每个从机对应的期望俯仰角指令θ_cmd和期望滚转角指令γ_cmd；

多个无人机的底层姿态跟踪控制器，用于根据期望俯仰角指令θ_cmd和期望滚转角指令γ_cmd进行飞行状态调整。

协同控制软件系统是实现多固定翼无人机协同飞行的基础，多固定翼无人机协同控制硬件系统是软件系统的载体，是实现多固定翼无人机协同飞行的保障。硬件的选型，组网，匹配对于多无人机的协同飞行是十分重要的。

其中，多固定翼无人机协同控制软硬件系统之间的关系如图6所示。上层编队控制器需要一块能够搭载Ubuntu系统的开发板，姿态控制系统需要一块能运行PX4软件的飞控板，同时还需要传感器能够获取无人机的状态，包括姿态信息，位置信息，速度信息等，还需要执行机构能够执行控制指令。图中的左侧部分对应的是硬件系统的选型，上层编队控制器可以采用RK3399，底层姿态控制器选用Pixhawk Mini4飞控板，两者可以通过有线串口进行通信。飞控板上连接有GPS，空速管，陀螺仪、磁力计等传感器，连接有电机舵机等执行器，连接有接收机，数传信号传输设备。地面端可以通过PC机或者遥控器对无人机进行操作。各无人机之间以及无人机与地面站PC之间，可以通过MESH自组网电台通信。

硬件设备之间的连接图如图7所示，领机和每个从机的硬件结构及连接是一致的。这也方便进行代码移植以及领从无人机的设置与切换。系统组网需要所有无人机的机载电台与地面站PC连接的电台需要在一个网段内，使得无人机与地面站彼此之间能够网络互通。ROS系统是一种分布式通信框架，能够帮助程序进程之间更方便的通信。因此在保证网络互通的基础上，就能够进行各个网络设备间的消息传递。

实施例10

本发明提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述步骤。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种固定翼从机追随领机的编队飞行控制方法，其特征在于，包括：

S1：实时获取每个所述从机及所述领机各自对应的飞行状态信息；

S2：根据每个所述从机及所述领机各自对应的飞行状态信息，将每个所述从机在领机机体系下预设的期望编队误差转换为在本机北东地NED坐标系的相对位置误差；

S3：根据每个所述从机在本机北东地NED坐标系的位置误差和当前位置速度状态，计算每个所述从机的加速度控制指令；利用每个所述从机的加速度控制指令，计算每个所述从机的期望俯仰角指令和期望滚转角指令；

S4：利用每个所述从机的期望俯仰角指令和期望滚转角指令，控制每个所述从机对所述领机进行飞行状态跟踪，从而实现每个所述从机追随所述领机的编队飞行控制；

所述从机的飞行状态信息包括当前偏航角；所述领机的飞行状态信息包括当前位置和当前偏航角；所述S2包括：S21：根据所述领机的当前偏航角将每个所述从机在领机机体系下预设的期望编队误差转化为所述领机北东地NED坐标系的编队误差；根据所述领机的当前位置中的经纬度，将每个所述从机在领机北东地NED坐标系的编队误差转化为本机期望绝对位置中的经纬度；S22：根据每个所述从机对应的本机期望绝对位置中的经纬度与领机当前位置中的经纬度的差值以及所述从机的当前偏航角，计算每个所述从机在本机航迹坐标系的相对位置误差；S23：将每个所述从机在本机航迹坐标系的相对位置误差转化为每个所述从机在本机北东地NED坐标系的相对位置误差；

所述S3包括：S31：利用公式计算每个所述从机对应的加速度控制指令u_i(t)；S32：利用所述加速度控制指令u_i(t)计算每个所述从机对应期望俯仰角指令θ_cmd和期望滚转角指令γ_cmd；k_i(t)表示从机i控制参数估计值，由从机i的控制精度参数确定；T表示转置，x_i(t)表示从机i在所述本机北东地NED坐标系下的位置速度状态，/>p_i表示从机i的当前位置，v_i表示从机i的当前速度，x₀(t)为所述领机在所述领机北东地NED坐标系下的位置速度状态，/>p₀表示所述领机的当前位置，v₀表示所述领机的当前速度，u₀(t)为所述领机的加速度控制指令，Δ_i＝[Δx_Nf,Δy_Ef,0,0]^T为所述领机与从机i之间在本机北东地NED坐标系下的期望状态误差；

所述S4包括：将所述期望俯仰角指令θ_cmd和期望滚转角指令γ_cmd输入如下公式计算每个所述从机的升降舵δ_e和副翼舵的数字舵偏δ_a，控制每个所述从机对所述领机进行飞行状态跟踪；

limit表示限幅函数，其中，表示俯仰角和滚转角的PID控制参数，k_q＝15/V²表示动压修正项，V为每个所述从机的当前速度，θ表示每个所述从机的当前俯仰角，γ表示每个所述从机的当前滚转角，ω_θ,ω_γ分别为每个所述从机的当前俯仰角和滚转角速率。

2.如权利要求1所述的固定翼从机追随领机的编队飞行控制方法，其特征在于，所述S2包括：

S21：利用公式将每个所述从机在领机机体系下预设的期望编队误差/>转化为每个所述从机在所述领机北东地NED坐标系的编队误差利用公式/>将/>转化为本机期望绝对位置中的经纬度/>

S22：利用公式根据与领机当前位置中的经纬度/>的差值，计算每个所述从机在本机航迹坐标系的相对位置误差(Δx_kf,Δy_kf)；

S23：利用公式将(Δx_kf,Δy_kf)转化为每个所述从机在本机北东地NED坐标系的相对位置误差(Δx_Nf,Δy_Ef)；

其中，ψ_l为所述领机的偏航角；(λ^l,φ^l)为所述领机的实际绝对位置中的经纬度，R_earth为地球半径；ψ_f为所述从机的偏航角。

3.如权利要求1所述的固定翼从机追随领机的编队飞行控制方法，其特征在于，所述从机i的控制参数估计值k_i(t)采用如下方式设置：

所述领机与从机i之间的期望位置误差表示为：Δ_i＝[δ_i,p δ_i,v]^T；

其中，δ_i,p＝[Δx_Nf,Δy_Ef]^T表示从机i与所述领机之间的期望位置误差，δ_i,v表示从机i和位于队形中间的所述领机之间的期望速度误差δ_i,v＝v_i-v₀＝[0,0]^T，η表示给定矩阵，且是对称正定矩阵；P是正定对称矩阵，B_m表示从机i的状态空间模型中的控制矩阵，I₂表示二阶单位矩阵，e_i(t)表示从机i的跟踪误差，e_i(t)＝x_i(t)-Δ_i。

4.如权利要求1所述的固定翼从机追随领机的编队飞行控制方法，其特征在于，所述加速度控制指令u_i(t)包括x轴控制指令a_x和y轴控制指令a_y，表示为u_i(t)＝[a_x,a_y]^T；所述S32包括：

利用所述x轴控制指令a_x计算期望速度指令V_cmd；

基于PID控制方法和所述期望速度指令V_cmd计算所述期望俯仰角指令θ_cmd和所述期望滚转角指令γ_cmd，以使每个所述从机根据所述期望俯仰角指令θ_cmd和所述期望滚转角指令γ_cmd对所述领机进行飞行状态跟踪；

其中，γ_cmd＝arctan(a_y/g)， 表示控制参数，limit用于保证积分项在/>之间，g表示重力加速度。

5.一种固定翼从机追随领机的编队飞行控制装置，其特征在于，用于执行权利要求1至4中任一项所述的方法的步骤包括：

协同控制环模块，用于实时获取每个所述从机及所述领机各自对应的飞行状态信息；根据每个所述从机及所述领机各自对应的飞行状态信息，将每个所述从机在领机机体系下预设的期望编队误差转换为在本机北东地NED坐标系的相对位置误差；根据每个所述从机在本机北东地NED坐标系的位置误差和当前位置速度状态，计算每个所述从机的加速度控制指令；利用每个所述从机的加速度控制指令，计算每个所述从机的期望俯仰角指令和期望滚转角指令；

姿态控制环模块，用于利用每个所述从机的期望俯仰角指令和期望滚转角指令，控制本机对所述领机进行飞行状态跟踪，从而实现每个所述从机追随所述领机的编队飞行控制。

6.一种固定翼从机追随领机的编队飞行控制系统，包括存储器和控制器，其特征在于，

所述存储器存储有计算机程序；

所述控制器执行所述计算机程序时实现权利要求1至4中任一项所述的方法的步骤。

7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至4中任一项所述的方法的步骤。