CN111650952A - 一种基于双干扰观测器四旋翼无人机分层抗干扰方法 - Google Patents

Abstract

本发明涉及一种基于双干扰观测器四旋翼无人机分层抗干扰方法，针对一类受到多源干扰影响的四旋翼无人机，通过设计双干扰观测器，实现对干扰的实时估计与补偿，首先，建立四旋翼无人机六自由度动力学模型；其次，针对质心运动控制回路，设计第一个非线性干扰观测器，估计质心运动干扰外力；然后，针对姿态控制回路，设计第二个非线性干扰观测器，估计干扰力矩；最后，利用两个干扰观测器的估计值，分别设计质心运动控制回路和姿态控制回路的控制律，保证飞行器闭环系统的稳定性。本发明能够提高四旋翼无人机抵抗多源干扰的能力。

Description

一种基于双干扰观测器四旋翼无人机分层抗干扰方法

技术领域

本发明涉及一种基于双干扰观测器四旋翼无人机分层抗干扰方法，主要应用于四旋翼无人机全回路位置、姿态跟踪控制，属于飞行器控制技术领域。

背景技术

四旋翼无人机具有自由悬停、运动性能好、体积小、环境友好等优势，越来越多受到国内外无人机爱好者的关注，近几十年发展势头迅猛，在农业遥感、远程摄影、智能物流等领域得到诸多应用，然而，由于其强非线性，强耦合，欠驱动等特性，且航空环境复杂，在大气中需要面对诸多外援干扰，导致四旋翼无人机的飞行控制系统比较复杂，面对未知的外界干扰需要具备很强的抗干扰能力。针对这种强扰动，常用的抗干扰策略为干扰估计和抑制，现有的应用方法多为自抗扰控制方法，即对每个控制通道利用扩张状态观测器，这种方法在整个控制过程中设计多套控制器增益，需要调节的参数过多，多依赖于工程师的经验，不利于实际方法的推广和应用。

针对四旋翼无人机在面对未知的外界干扰时的控制问题，本发明通过分别在质心运动控制环路和姿态控制环路设计两个非线性干扰观测器给出了很好的解决方案。本发明引入双回路干扰观测器，对复杂多源未知干扰进行估计，反馈给控制系统，并分别在两环路设计控制律，来抑制作用于质心和姿态回路的外界干扰，更具普适应用价值。

发明内容

本发明的技术解决问题是：针对四旋翼无人机在面对未知的外界干扰时的控制问题，本发明通过运用复合分层抗干扰控制结构，分别在质心运动控制环路和姿态控制环路设计两个非线性干扰观测器，对复杂多源未知干扰进行估计，反馈给控制系统，并分别在两环路设计控制律，来抑制作用于四旋翼无人机质心和姿态回路的外界干扰，保证了四旋翼无人机飞行过程中良好的控制性能。

本发明的技术解决方案为一种基于双干扰观测器四旋翼无人机分层抗干扰方法，其实现步骤如下：

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

其中，矢量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和l为旋翼的宽度和长度，K_c表示扭矩系数。

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

其中，

表示离心力、哥式力等非线性力，M_q(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为设计参数。

第四步，针对质心运动回路设计PID控制律如下：

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

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

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

本发明与现有技术相比的优点在于：

(1)本发明能够解决受内外干扰的四旋翼无人机的位置姿态跟踪控制问题，不依赖额外的硬件或积分滤波算法；

(2)与以往四旋翼飞行器姿态控制器设计过程相比，本发明中在位置和姿态控制器设计过程中形成了扰动估计的两个环路，使得控制系统具有更强的鲁棒性，控制效果更好；

(3)与传统的四旋翼无人机控制相比，通过引入具有两个非线性干扰观测器的复合分层抗干扰策略，可以消除外界多源干扰的影响。

附图说明

图1为本发明一种基于双干扰观测器四旋翼无人机分层抗干扰方法的流程框图。

具体实施方式

如图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和l为旋翼的宽度和长度，K_c表示扭矩系数。

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

其中，

表示离心力、哥式力等非线性力，M_q(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为设计参数。

第四步，针对质心运动回路设计PID控制律如下：

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

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

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

本专利中所用到的控制器具体参数选取如下：

K_Pm＝[π/10,π/10,35]，K_Im＝[0,0,4],K_Dm＝[π/6,π/6,14],K_Pn＝[1.5,1.24,0.0395]，K_Dn＝[0.23,0.17,0.0395]；

非线性观测器设计参数选取为：λ_m＝0.4，λ_n＝0.1。

通过Matlab仿真，可以得到双干扰观测器的复合分层抗干扰控制方法，实现四旋翼无人机飞行过程中抗干扰控制，同时保证了较好的控制效果。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (5)

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

第一步，建立四旋翼无人机六自由度动力学模型；

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

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

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

2.根据权利要求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和l为旋翼的宽度和长度，K_c表示扭矩系数；

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

其中，

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

综上，可建立如下四旋翼无人机数学模型：

其中，

为惯性阵的逆矩阵。

3.根据权利要求1中所述的一种基于双干扰观测器四旋翼无人机分层抗干扰方法，其特征在于：所述第二步，对第一步所建立的六自由度无人机模型，针对作用在质心的干扰力，设计如下第一个非线性干扰观测器：

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

其中，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为设计参数。

4.根据权利要求1中所述的一种基于双干扰观测器四旋翼无人机分层抗干扰方法，其特征在于：所述第三步，针对作用在姿态回路的干扰力，设计如下第二个非线性干扰观测器：

定义姿态误差方程：

e_q＝q_d-q

其中,q_d∈R³，q∈R³为输入期望姿态角及实测姿态角，

分别为输入期望姿态角速度及实测姿态角速度，e_q∈R³，

分别表示姿态角误差和姿态角速度误差值；

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

其中，

是d_q的估计值，z_q和

分别是辅助变量及其导数，

为辅助方程，满足：

是增益矩阵，

设计如下：

L_q＝M_q ^-1λ_n

其中，λ_n为设计参数。

5.根据权利要求1中所述的一种基于双干扰观测器四旋翼无人机分层抗干扰方法，其特征在于：所述第四步中，针对质心运动回路设计PID控制律如下：

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

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

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

Images