CN110673648A - 一种防止机间碰撞的固定翼无人机编队的控制方法 - Google Patents

Abstract

本发明涉及一种防止机间碰撞的固定翼无人机编队的控制方法，采用一致性理论作为基础，同时结合斥力场、对位算法等，实现防止机间碰撞的固定翼无人机编队任务。具体包括以下步骤：1、获得无人机的位置信息和所需形成队形的相对位置信息；2、通过对位算法求解每架无人机在所需形成新队形中的位置；3、以一致性理论为基础，引入斥力场，设计无人机编队的速度指令信号；4、建立六自由度固定翼无人机模型并设计无人机编队控制律。本发明以六自由度固定翼无人机模型为基础设计编队控制律，相比现有大部分质点模型编队算法更加贴近工程实际；同时设计了斥力场、对位算法，从而有效解决了编队中的机间碰撞问题。

Description

一种防止机间碰撞的固定翼无人机编队的控制方法

技术领域

本发明属于无人机编队控制技术领域，具体的说，涉及固定翼无人机防止机间碰撞的编队控制方法。

背景技术

自从无人机在1991年的海湾战争中得到成功运用以来，经过几十年的发展，无人机技术已相对成熟，并在各个领域中发挥了其独特的作用。尽管如此，单架无人机执行任务时仍存在问题。为了尽可能地发挥单架无人机的作用，实现多无人机协同编队飞行的控制、决策和管理，从而提高无人机完成任务的效率，拓宽无人机使用范围，达到安全、高可靠性地执行各种任务的目的，无人机编队方案应运而生。由于编队控制理论在多智能体领域的应用逐步成熟和其在军事、民用领域的广大应用前景，无人机的编队控制也成为了近年来的热点。在编队控制发展迅猛的今天，科研人员进行了许多基于多智能体的编队控制理论研究，取得了长足的进步。然而，现有大量的编队研究均是基于简单二阶系统或可以进行定点悬停的旋翼飞行器，而对于可以实际应用在固定翼飞行器编队控制的算法研究上，仍有很大空间，还有很多问题仍需解决。

一致性理论作为编队算法中比较成熟的算法，有其独特的优势。它能通过无人机之间的信息传递，将自己和其他无人机的相对位置进行融合解算，最终达到形成无人机编队的目的。近年来，科研人员对一致性理论的研究方向有考虑时间延迟问题、有限时间收敛问题等，取得很多成绩。然而，一致性理论仍存在一些不足之处，比如单纯运用一致性理论不能避免碰撞问题、对于固定翼飞行器编队应用困难问题等。因此，防止机间碰撞的固定翼无人机编队控制的研究具有重要的现实意义。

发明内容

要解决的技术问题

为了解决现有基于多智能体编队算法和六自由度固定翼无人机控制难结合的问题，本发明提出一种防止机间碰撞的固定翼无人机编队的控制方法。

技术方案

一种防止机间碰撞的固定翼无人机编队的控制方法，其特征在于步骤如下：

步骤1：通过GPS或者其他手段，获得各架无人机的三维位置信息；设计编队队形，获得队形中各个位置相对于虚拟领航者的相对距离信息；

步骤2：通过对位算法和步骤1中获得的数据，解算出每架无人机所对应的新队形中的位置：

2a：以所有无人机的几何中心为原点，X、Y、Z轴分别以

倍翼展为轴向最小区间，将空间网格化，并对每一个网格进行三维坐标编号，三维网格坐标均是整数；

2b：将各个无人机在地面系下的坐标映射到网格空间中，获得各个无人机对应三维网格坐标系下的坐标：

(X_i,Y_i,Z_i)→(X_wi,Y_wi,Z_wi)i＝1,2,...,n (1)

其中(X_i,Y_i,Z_i)表示第i架无人机在地面坐标系下的坐标，(X_wi,Y_wi,Z_wi)表示第i架无人机在网格坐标系下对应的坐标；

2c：将新队形中的各个位置相对于虚拟领航者的期望相对距离向量转换到网格坐标系下：

(dX_j,dY_j,dZ_j)→(dX_wj,dY_wj,dZ_wj)j＝1,2,...,n (2)

其中(dX_j,dY_j,dZ_j)表示新队形中某一位置j，在地面坐标系下与虚拟领航者的期望相对距离，(dX_wj,dY_wj,dZ_wj)表示新队形中某一位置j，在网格坐标系下与虚拟领航者的期望相对距离；

2d：将无人机与队形位置进行排序对位；首先按照X_wi的大小将无人机从1到n编号，如果无人机的X_wi相等，再按照Y_wi的大小排列，以此类推，优先级按照X_wi→Y_wi→Z_wi；对n架无人机进行排序编号，每架无人机获得自己的编号S_i i∈1,2,...n；然后按照同样的方法对新队形的n个位置进行排序，优先级需与无人机优先级相同，新队形中每个位置获得自己的编号S_wi i∈1,2,...n；则拥有相同编号的无人机与队形位置匹配，即该无人机飞往匹配的队形位置；

步骤3：以一致性做为理论基础，同时引入本发明设计的斥力场信号，设计出无人机编队的速度指令信号：

其中

表示第i架无人机和第j架无人机之间的斥力，

表示第i架无人机受到的斥力指令信号，m是无人机质量，

是一个控制参数，

为两架无人机的距离，kq是正参数，用来调节曲线弯曲度，bc表示斥力场范围半径与翼展的比值，b表示翼展长度；

则无人机编队的速度指令信号为：

V_Di＝ΔV_ci+V_di (4)

其中

是由一致性协议所求出的速度指令，

是一致性协议与斥力场结合所获得的最终的速度指令信号；

步骤4：得到步骤3设计的速度指令后，以固定翼无人机的六自由度模型为基础，设计无人机的非线性控制器：

其中T_ci是发动机推力输入，m是无人机质量，D是阻力，γ是航迹倾斜角，α_ci是期望攻角，

是期望无人机速度的导数，K_Si，λ_Si∈(0,1)是控制参数，x_1i＝[θ_i φ_i ψ_i]^T，x_2i＝[q_i p_i r_i]^T，x_1Di＝[θ_Di φ_Di ψ_Di]^T，e_oi＝x_1i-x_1Di，δ_cοi是舵偏输入；

p,q,r是机体坐标系下无人机转动角速度的三轴分量，φ,ψ,θ分别表示无人机的滚转角、偏航角、俯仰角，

c₅＝I_z-I_x/I_y，c₆＝I_xz/I_y，c₇＝1/I_y，

I_xz、I_x、I_y、I_z表示无人机对应的转动惯量，L,M,N表示无人机机体三轴所受力矩，δ_οi＝[δ_ei δ_ai δ_ri]^T表示三个舵偏角。

有益效果

本发明提出的一种防止机间碰撞的固定翼无人机编队控制方法，提出了一种对位算法和与斥力场结合的一致性理论算法，有效的解决了普通的一致性理论协议，在编队和队形变换的过程中，无法避免机间碰撞的问题。

附图说明

图1为无人机编队队形位置随机分配与对位算法对比示意图；

图2为地面坐标系转换为网格坐标系示意图；

图3为斥力大小与相对距离关系图；

图4为双机斥力场简要示意图；

图5为实例通信拓扑结构图；

图6为实例无人机编队轨迹图；

图7为一种防止机间碰撞的固定翼无人机编队的控制方法流程图。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

如下步骤：

步骤一：首先通过GPS或者其他手段，获得各架无人机的三维位置信息；设计编队队形，获得队形中各个位置相对于虚拟领航者的相对距离信息。

步骤二：通过本发明所设计的对位算法和步骤一中获得的数据，解算出每架无人机对应的新队形的位置。随机分配与对位算法的对比图如图1所示。其主要分为以下步骤：

步骤1)以所有无人机的几何中心为原点，X、Y、Z轴分别以

倍翼展为轴向最小区间，将空间网格化，并对每一个网格进行三维坐标编号，这样选取网格长度可以保证每个网格坐标在同一时刻有且仅有一架无人机与之对应。网格编号均为整数，具体如图2.所示。

步骤2)将各个无人机在地面系下的坐标映射到网格空间中，获得得各个无人机对应三维网格坐标系下的坐标：

(X_i,Y_i,Z_i)→(X_wi,Y_wi,Z_wi)i＝1,2,...,n (1)

其中(X_i,Y_i,Z_i)表示第i架无人机在地面坐标系下的坐标，(X_wi,Y_wi,Z_wi)表示第i架无人机在网格坐标系下对应的坐标。

步骤3)将新队形中各个位置相对于虚拟领航者的期望相对距离转换到网格坐标系下。

(dX_j,dY_j,dZ_j)→(dX_wj,dY_wj,dZ_wj)j＝1,2,...,n (2)

其中(dX_j,dY_j,dZ_j)表示新队形中某一位置j，在地面坐标系下与虚拟领航者的期望相对距离，(dX_wj,dY_wj,dZ_wj)表示新队形中某一位置j，在网格坐标系下与虚拟领航者的期望相对距离。

步骤4)将无人机与队形位置进行排序对位。首先按照X_wi的大小将无人机从1到n编号，如果无人机的X_wi相等，再按照Y_wi的大小排列，以此类推，(优先级按照X_wi→Y_wi→Z_wi，可根据情况调整优先级)对n架无人机进行排序编号，每架无人机获得自己的编号S_i i∈1,2,...n。然后按照同样的方法对新队形的n个位置进行排序，(优先级需与无人机优先级相同)，新队形中每个位置获得自己的编号S_wi i∈1,2,...n。则拥有相同编号的无人机与队形位置匹配，即该无人机飞往匹配的队形位置。

步骤三：以一致性理论做为基础，同时引入本发明设计的斥力场信号，设计出无人机编队的速度指令信号。

在说明一致性协议之前，必须介绍图论的相关知识。图是由顶点集合和顶点间的二元关系集合(即边的集合)组成的数据结构，通常记为G＝(V，E)，其中顶点集合V(G)和边的集合E(G)表示为V(G)＝{v₁，v₂，…，v_n}，E(G)＝{(v_i，v_j)∶v_i，v_j∈V(G)}。在图G中，如果节点i和节点j有信息交换，则存在边(v_i，v_j)；若节点v_i与节点v_j之间存在边(v_i，v_j)，则称节点v_i和节点v_j是邻接的。若对于图G中的每一对节点，都存在一条路径连接两点，则称该图为连通图。邻接矩阵用代数矩阵的方式来描述节点之间的位置关系，记邻接矩阵

其中

根据队形设计要求，得到相对距离矩阵

其中

表示D_l中的第i列元素，它的意义是第i架无人机与领航者需要保持的期望距离；[x_ij y_ijh_ij]^T＝D_li-D_lj，它的意义是第i架无人机与第j架无人机需要保持的期望距离。然后根据一阶一致性协议，可以获得速度指令信号：

其中k_Xi，k_Yi，k_Hi为控制参数，并且符合如下规律：和虚拟领航者有联系的无人机k_Xi，k_Yi，k_Hi>0，无联系的无人机k_Xi，k_Yi，k_Hi＝0；a_ij是邻接矩阵中的元素；x_i，x_j分别是第i、j架无人机x轴坐标；y_i，y_j分别是第i、j架无人机的y轴坐标；h_i，h_j分别是第i、j架无人机的h轴坐标；x_L，y_L，h_L是虚拟领航者的x轴，y轴，h轴坐标；V_xdi，V_ydi，V_hdi分别为第i架无人机的一致性速度指令信号。

由于在不同队形的相互变换的过程中，两个队形的几何构造不相同，即使已经采用了对位算法，仍会出现小概率的机间碰撞的现象，因此机间斥力算法应运而生。假设无人机速度存在一个最大值V_max，则每架无人机的斥力场函数设计如下：

其中

表示第i架无人机和第j架无人机之间的斥力，当两架无人机越接近时，斥力越大，如图3.所示。

表示第i架无人机受到的总斥力，m是无人机质量，

是一个控制参数

为两架无人机的距离，kq是正参数，用来调节斥力曲线弯曲度，bc表示斥力场范围半径与翼展的比值，b表示翼展长度。图4.是斥力场信号的简要示意图。

将斥力场指令加入到一阶一致性协议所解算出来的速度指令信号V_di＝[V_xdi V_ydiV_hdi]^T中，最终获得带斥力场的一致性速度指令信号V_Di。

V_Di＝ΔV_ci+V_di (5)

将斥力场与一致性协议融合，每个无人机便存在了自己的防碰撞领域，进而有效避免了机间碰撞。

步骤四：得到步骤三设计出的速度指令后，以固定翼无人机的六自由度模型为基础，设计无人机的非线性控制器。

固定翼无人机的六自由度模型选取为：

其中u,v,w是机体坐标系下无人机速度的三轴分量，p,q,r是机体坐标系下无人机转动角速度的三轴分量，F_x,F_y,F_z表示地面坐标系下，无人机三轴的受力，φ,ψ,θ分别表示无人机的滚转角、偏航角、俯仰角，x,y,h表示无人机的三维坐标，m表示无人机质量。

c₅＝I_z-I_x/I_y,c₆＝I_xz/I_y,c₇＝1/I_y,

I_xz、I_x、I_y、I_z表示无人机对应的转动惯量。L,M,N表示无人机机体三轴所受力矩，其求解公式如下：

其中δ_a,δ_e,δ_r为三个控制舵面，f_L,f_M,f_N是气动力矩求解中与舵面无关的数值。

由于本发明中无人机采用协调转弯的模式进行转弯，所以侧滑角β基本保持为0，忽略侧力，可以得到如下无人机运动学的简化模型：

其中表示发动机推力，V表示无人机速度，D,L表示阻力和升力；γ,χ,μ表示航迹俯仰角，航迹偏航角，航迹滚转角；α表示攻角。

通过下列公式，可以将无人机速度指令信号转换为该无人机的V_Di,γ_Di,χ_Di指令信号:

其中[V_xDi V_yDi V_hDi]分别为第i架无人机的速度指令信号。设计无人机的

指令信号为：

其中

为控制参数。将(16)带入(14)，不难得到:

此时我们已经得到γ_ci，χ_ci，μ_ci，α_ci，并且根据上述假设β_i＝0。则通过角度转换关系可以得到姿态角指令:

θ_Di＝arcsin(sinα_ci cosγ_ci cosμ_ci+cosα_ci sinγ_ci)

将模型(12)中的姿态模型部分改写为下述模型公式：

其中：

其中x_1i＝[θ_i φ_i ψ_i]^T,x_2i＝[q_i p_i r_i]^T,δ_οi＝[δ_ei δ_ai δ_ri]^T，x_1Di＝[θ_Di φ_Di ψ_Di]^T。得到上述模型后，开始设计无人机滑模姿态控制器，可以定义姿态误差为：

e_oi＝x_1i-x_1Di (17)

设计滑模面为:

控制输入设计为:

其中K_Si>0，λ_Si∈(0,1)为控制参数。

下面证明控制系统的稳定性，选取利亚普诺夫函数为：

对其求导不难得到:

由此可证，本发明所设计的控制器的稳定性。

本实例采用四架固定翼无人机进行编队和队形变换，无人机的气动参数选取于某型固定翼无人机的实际数据。所发明的一种防止机间碰撞的固定翼无人机编队的控制方法具体实施步骤如下：

步骤一：首先通过GPS或者其他手段，获得各架无人机的地面坐标系下的三维位置信息；设计编队队形，获得队形中各个位置相对于虚拟领航者的期望相对距离信息。

本实例各无人机的初始位置设置为：

表1.各无人机地面坐标系下的初始位置

本实例编队各无人机相对于虚拟领航者的期望距离设计为：

表2.队形中各无人机相对于虚拟领航者在地面坐标系下的期望距离

步骤二：通过本发明所设计的对位算法和步骤一中获得的数据，解算出每架无人机对应的新队形的位置。

其主要分为以下步骤：

步骤1)以所有无人机的几何中心为原点，X、Y、Z轴分别以

倍翼展为轴向最小区间，将空间网格化。本实例中选取无人机的翼展为2米，所以网格的边长为：

米。

步骤2)将各个无人机在地面系下的坐标映射到网格空间中，获得得各个无人机对应三维网格坐标系下的坐标。

根据本实例上述的无人机初始位置表和网格坐标系，可以得到无人机初始位置在网格坐标系下的坐标。

表3.各无人机网格坐标系下的初始位置

步骤3)将新队形中各个位置相对于虚拟领航者的期望相对距离转换到网格坐标系下。根据本实例上述各无人机相对于虚拟领航者的相对距离表，可以得到其在网格坐标系下的相对距离：

表4.队形中各无人机相对于虚拟领航者在网格坐标系下的期望距离

步骤4)将无人机与队形位置进行排序对位。首先按照X_wi的大小将无人机从1到n编号，如果无人机的X_wi相等，再按照Y_wi的大小排列，以此类推，(优先级按照X_wi→Y_wi→Z_wi，可根据情况调整优先级)对n架无人机进行排序编号，我们可以得到四架无人机的序号为：

表5.各无人机排序编号

然后按照同样的方法对新队形的n个位置进行排序，(优先级需与无人机优先级相同)，新队形中每个位置获得自己的编号：

表6.队形中各个位置的排序编号

则拥有相同编号的无人机与队形位置匹配，即该无人机飞往匹配的队形位置。由此可以推断出无人机的编队飞行顺序分别为：

表7.各无人机的编队飞行位置顺序信息

步骤三：以一致性理论做为基础，同时引入本发明设计的斥力场信号，设计出无人机编队的速度指令信号。本实例采用通信关系如图5所示。则可以得到本实例中，编队的邻接矩阵为：

并且根据一阶一致性协议，可以得到如下计算公式：

其中k_Xi，k_Yi，k_Hi为控制参数，并且符合如下规律：和虚拟领航者有联系的无人机k_Xi，k_Yi，k_Hi>0，无联系的无人机k_Xi，k_Yi，k_Hi＝0；a_ij是邻接矩阵中的元素；x_i，x_j分别是第i、j架无人机x轴坐标；y_i，y_j分别是第i、j架无人机的y轴坐标；h_i，h_j分别是第i、j架无人机的h轴坐标；x_L，y_L，h_L是虚拟领航者的x轴，y轴，h轴坐标；V_xdi，V_ydi，V_hdi分别为第i架无人机的一致性速度指令信号。

由于在不同队形的相互变换的过程中，两个队形的几何构造不相同，即使已经采用了对位算法，仍会出现小概率的机间碰撞的现象，因此机间斥力算法应运而生。假设无人机速度存在一个最大值V_max，则每架无人机的斥力场函数设计如下：

其中

表示第i架无人机和第j架无人机之间的斥力，

表示第i架无人机受到的总斥力，m是无人机质量，

是一个控制参数

为两架无人机的距离，kq是正参数，用来调节斥力曲线弯曲度，bc表示斥力场范围半径与翼展的比值，b表示翼展长度。

将斥力场指令加入到一阶一致性协议所解算出来的速度指令信号V_di＝[V_xdi V_ydiV_hdi]^T中，最终获得带斥力场的一致性速度指令信号V_Di。

V_Di＝ΔV_ci+V_di (25)

步骤四：得到步骤三设计的速度指令后，以固定翼无人机的六自由度模型为基础，设计无人机的非线性控制器。

固定翼无人机的六自由度模型选取为：

其中u,v,w是机体坐标系下无人机速度的三轴分量，p,q,r是机体坐标系下无人机转动角速度的三轴分量，F_x,F_y,F_z表示地面坐标系下，无人机三轴的受力，φ,ψ,θ分别表示无人机的滚转角、偏航角、俯仰角，x,y,h表示无人机的三维坐标，m表示无人机质量。

c₅＝I_z-I_x/I_y,c₆＝I_xz/I_y,c₇＝1/I_y,

I_xz、I_x、I_y、I_z表示无人机对应的转动惯量。L,M,N表示无人机机体三轴所受力矩，其求解公式如下：

其中δ_a,δ_e,δ_r为三个控制舵面，f_L,f_M,f_N是气动力距的求解中与舵面无关的数值。在本实例中，气动力和力矩的求解基于某型固定翼无人机。

由于本发明中无人机采用协调转弯的模式进行转弯，所以侧滑角β基本保持为0，忽略侧力，可以得到如下无人机运动学的简化模型：

通过下列公式，可以将无人机速度指令信号转换为该无人机的V_Di,γ_Di,χ_Di指令信号:

其中[V_xDi V_yDi V_hDi]分别为第i架无人机的速度指令信号。设计无人机的

指令信号为：

其中

为控制参数。将(15)带入(13)，不难得到:

此时我们已经得到γ_ci，χ_ci，μ_ci，α_ci，并且根据上述假设β_i＝0。则通过角度转换关系可以得到姿态角指令:

θ_Di＝arcsin(sinα_ci cosγ_ci cosμ_ci+cosα_ci sinγ_ci)

将模型(32)中的姿态模型部分改写为下述模型公式：

其中：

其中x_1i＝[θ_i φ_i ψ_i]^T,x_2i＝[q_i p_i r_i]^T,δ_οi＝[δ_ei δ_ai δ_ri]^T，x_1Di＝[θ_Di φ_Di ψ_Di]^T。得到上述模型后，开始设计无人机滑模姿态控制器，可以定义姿态误差为：

e_oi＝x_1i-x_1Di (37)

设计滑模面为:

控制输入设计为:

其中

K_Si>0，λ_Si∈(0,1)为控制参数。

下面证明控制系统的稳定性，选取利亚普诺夫函数为：

对其求导不难得到:

由此可证，本发明所设计的控制器的稳定性。最终编队仿真结果如图6.所示。在本实例过程中，各个参数分别选取为：

k_Xi＝1，k_Yi＝1，k_Hi＝1，

kq＝2，bc＝2，

K_Si＝2，λ_Si＝0.8。

Claims (1)

1.一种防止机间碰撞的固定翼无人机编队的控制方法，其特征在于步骤如下：

步骤1：通过GPS或者其他手段，获得各架无人机的三维位置信息；设计编队队形，获得队形中各个位置相对于虚拟领航者的相对距离信息；

步骤2：通过对位算法和步骤1中获得的数据，解算出每架无人机所对应的新队形中的位置：

2a：以所有无人机的几何中心为原点，X、Y、Z轴分别以

倍翼展为轴向最小区间，将空间网格化，并对每一个网格进行三维坐标编号，三维网格坐标均是整数；

2b：将各个无人机在地面系下的坐标映射到网格空间中，获得各个无人机对应三维网格坐标系下的坐标：

(X_i,Y_i,Z_i)→(X_wi,Y_wi,Z_wi) i＝1,2,...,n (1)

其中(X_i,Y_i,Z_i)表示第i架无人机在地面坐标系下的坐标，(X_wi,Y_wi,Z_wi)表示第i架无人机在网格坐标系下对应的坐标；

2c：将新队形中的各个位置相对于虚拟领航者的期望相对距离向量转换到网格坐标系下：

(dX_j,dY_j,dZ_j)→(dX_wj,dY_wj,dZ_wj) j＝1,2,...,n (2)

其中(dX_j,dY_j,dZ_j)表示新队形中某一位置j，在地面坐标系下与虚拟领航者的期望相对距离，(dX_wj,dY_wj,dZ_wj)表示新队形中某一位置j，在网格坐标系下与虚拟领航者的期望相对距离；

2d：将无人机与队形位置进行排序对位；首先按照X_wi的大小将无人机从1到n编号，如果无人机的X_wi相等，再按照Y_wi的大小排列，以此类推，优先级按照X_wi→Y_wi→Z_wi；对n架无人机进行排序编号，每架无人机获得自己的编号S_i i∈1,2,...n；然后按照同样的方法对新队形的n个位置进行排序，优先级需与无人机优先级相同，新队形中每个位置获得自己的编号S_wi i∈1,2,...n；则拥有相同编号的无人机与队形位置匹配，即该无人机飞往匹配的队形位置；

步骤3：以一致性做为理论基础，同时引入本发明设计的斥力场信号，设计出无人机编队的速度指令信号：

其中

表示第i架无人机和第j架无人机之间的斥力，

表示第i架无人机受到的斥力指令信号，m是无人机质量，

是一个控制参数，

为两架无人机的距离，kq是正参数，用来调节曲线弯曲度，bc表示斥力场范围半径与翼展的比值，b表示翼展长度；

则无人机编队的速度指令信号为：

V_Di＝ΔV_ci+V_di (4)

其中

是由一致性协议所求出的速度指令，

是一致性协议与斥力场结合所获得的最终的速度指令信号；

步骤4：得到步骤3设计的速度指令后，以固定翼无人机的六自由度模型为基础，设计无人机的非线性控制器：

其中T_ci是发动机推力输入，m是无人机质量，D是阻力，γ是航迹倾斜角，α_ci是期望攻角，

是期望无人机速度的导数，

K_Si，λ_Si∈(0,1)是控制参数，x_1i＝[θ_i φ_i ψ_i]^T，x_2i＝[q_ip_i r_i]^T，x_1Di＝[θ_Di φ_Di ψ_Di]^T，e_oi＝x_1i-x_1Di，

δ_cοi是舵偏输入；

p,q,r是机体坐标系下无人机转动角速度的三轴分量，φ,ψ,θ分别表示无人机的滚转角、偏航角、俯仰角，

c₅＝I_z-I_x/I_y，c₆＝I_xz/I_y，c₇＝1/I_y，

I_xz、I_x、I_y、I_z表示无人机对应的转动惯量，L,M,N表示无人机机体三轴所受力矩，δ_οi＝[δ_ei δ_ai δ_ri]^T表示三个舵偏角。

Images