[发明专利]一种基于双干扰观测器四旋翼无人机分层抗干扰方法

权利要求书

1.一种基于双干扰观测器四旋翼无人机分层抗干扰方法，其特征在于：包括以下步骤：

第一步，根据牛顿第二定律和拉格朗日-欧拉方程，建立四旋翼无人机六自由度动力学模型为：

其中，矢量q＝[φ,θ,Ψ]^T代表三个旋转变量，φ,θ,Ψ分别代表滚转角度、俯仰角度、偏航角度，为q的一阶导数和二阶导数；p＝[x,y,z]^T表示三个平移变量，x,y,z分别表示四旋翼无人机在惯性系中x-y-z三轴坐标系的位置坐标分别表示x,y,z的二阶导数；g和m分别表示无人机的重力加速度和质量，用来表示动力学模型中的重力；F为四旋翼无人机四个升降舵的总升力，f₁,f₂,f₃,f₄分别为无人机四个升降舵各自提供的升力；无人机所受外部力包括干扰外力d_p控制外力矩τ_q，其中，d_p＝[d_px,d_py,d_pz]^T为干扰外力在x-y-z三轴惯性坐标系中的分量，τ_q＝[τ_qφ,τ_qθ,τ_qψ]表示控制力矩在φ-θ-Ψ旋转坐标系下的分量，满足下式：

其中，w和1为旋翼的宽度和长度，K_c表示扭矩系数；

根据牛顿-欧拉法得到旋翼的旋转动力学方程如下：

其中表示离心力、哥式力等非线性力，M_q(q)为惯性阵；

建立如下四旋翼无人机数学模型：

其中M_q^-1(q)为惯性阵的逆矩阵；

第二步，针对质心运动控制回路，设计第一个非线性干扰观测器，估计质心运动干扰外力；

对第一步所建立的六自由度无人机模型，针对作用在质心的干扰力，设计如下第一个非线性干扰观测器：

首先将第一步中建立的位置系统状态方程改写为矩阵形式：

其中，M_q＝diag[m,m,m]^T为对角惯性阵的转置，C_p＝diag[0,0,-mg]^T为对角非线性力矩阵的转置，τ_p为升力向量：

定义位置误差方程：

e_p＝p_d-p

其中p_d∈R³，p∈R³为输入期望轨迹及实测轨迹，分别为输入期望速度及实测速度，e_p∈R³,分别表示位置误差和速度误差值；

针对平移控制回路，设计第一个非线性干扰观测器：

其中是d_p的估计值，z_p和分别是辅助变量及其导数，为辅助方程，满足：

是增益矩阵，设计如下：

L_p＝M_q^-1λ_m

其中，λ_m为设计参数；

第三步，针对姿态控制回路，设计第二个非线性干扰观测器，估计干扰力矩；

定义姿态误差方程：

e_q＝q_d-q

其中,q_d∈R³，q∈R³为输入期望姿态角及实测姿态角，分别为输入期望姿态角速度及实测姿态角速度，e_q∈R³，分别表示姿态角误差和姿态角速度误差值；

针对姿态控制回路，设计第二个非线性干扰观测器：

其中是d_q的估计值，z_q和分别是辅助变量及其导数，为辅助方程，满足：

是增益矩阵，设计如下：

L_q＝M_q^-1λ_n

其中，λ_n为设计参数；

第四步，利用第二、三步中干扰观测器的估计值，分别设计质心运动控制回路和姿态控制回路的控制律；

其中，K_pm,K_Im,K_Dm∈R³分别为需要设计的控制增益，u_p为质心运动控制器输出；

针对姿态控制回路设计PD控制律如下：

其中，K_qn,K_Dn∈R³分别为需要设计的控制增益，u_q为姿态控制器输出。