[发明专利]主动变形四旋翼飞行器自抗扰飞行控制方法

权利要求书

1.主动变形四旋翼飞行器自抗扰飞行控制方法，其特征在于，包括以下步骤：

S1、根据主动变形四旋翼飞行器的两种变形方式，对主动变形四旋翼飞行器进行结构设计；

S2、根据步骤S1中所设计的主动变形四旋翼飞行器结构，推导动态变形时机体重心位置、惯量矩阵参数的变化；具体为：

主动变形四旋翼飞行器机体重心与机体系坐标原点的偏移量r_CoG∈R³表示如下：

其中，m_body表示机体的质量，m_arm表示机臂的质量，m_mot表示旋翼的质量，m_rot表示电机的质量，m_elo表示伸缩舵机的质量，r_body表示机体系坐标原点到机体的矢径，r_arm,i表示机体系坐标原点到ith机臂的矢径，r_mot,i表示机体系坐标原点到ith旋翼的矢径，r_rot,i表示机体系坐标原点到ith电机的矢径，r_elo,i表示机体系坐标原点到ith伸缩舵机的矢径，ith表示第i个，i＝1...4；

对于主动变形四旋翼飞行器机体惯量矩阵I_B，各部分惯量矩阵计算式如下：

其中，I_body表示机体的惯量矩阵，I_mot表示旋翼的惯量矩阵，I_rot表示电机的惯量矩阵，表示第i个机臂的惯量矩阵，表示第i个机臂上的伸缩舵机的惯量矩阵，l_body、h_body分别为包含旋转舵机在内的机体的长宽和高；l_arm,i、w_arm、h_arm分别为机臂的长、宽、高；r_mot、h_mot分别为旋翼的半径和高；r_rot、h_rot分别为电机的半径和高；l_elo、w_elo、h_elo分别为伸缩舵机的长、宽、高；电机、旋翼、机臂和伸缩舵机在主动变形四旋翼飞行器进行折叠变形时，它们的惯量矩阵也必须进行重新计算，引入旋转矩阵对它们进行重新表示：

其中，I_arm,i表示ith机臂的惯量矩阵，I_elo,i表示ith伸缩舵机的惯量矩阵，R_z(δ_i)表示旋转矩阵，R_z(δ_i)^T表示旋转矩阵的转置；

其中，旋转矩阵表达如下：

其中，表示cosδ_i，表示sinδ_i；

惯量矩阵I_B计算式表示为：

S3、根据步骤S1、S2推导出主动变形四旋翼飞行器的动力学模型和控制分配形式；具体为：

根据牛顿-欧拉方程，得到主动变形四旋翼飞行器的平移动力学模型为：

上式中m表示机体总质量，即m＝m_body+4m_arm+4m_mot+4m_rot+4m_elo，F^E表示主动变形四旋翼飞行器系统受力总和，通过下式计算：

其中，n_i是第i旋翼的转速，k_f0是旋翼的升力系数，G^E表示重力，F_t^E表示旋翼产生的升力，表示风扰力，D^E表示空气阻力，^ER_B表示从机体系旋转到地球固连坐标系的旋转矩阵，T_i表示机体系下第i个旋翼产生的升力，c表示空气阻力系数，S_air表示迎风面积，V_air表示飞行器与空气的相对速度，即表示风速，V^E表示飞行器速度；

主动变形四旋翼飞行器的转动动力学模型为：

其中，I_B是主动变形四旋翼飞行器的惯量矩阵，Ω^B表示机体系下三轴欧拉角速度，表示机体系下三轴欧拉角加速度，M^B表示系统所受力矩，通过下式计算：

其中，为旋翼升力所产生力矩，为旋翼旋转反扭矩，为系统陀螺效应项，为风扰力矩，为重力矩，L_i代表第i个旋翼的升力作用点在机体系下的坐标矢量，J_P为陀螺力矩系数，^BR_E为地球固连坐标系旋转到机体系的旋转矩阵，k_m为反扭矩系数，为第i个旋翼产生的风扰力；

综合上述平移动力学模型和转动动力学模型，可得主动变形四旋翼飞行器的动力学模型：

其中，表示地球固连坐标系下三轴加速度，和分别代表旋翼产生的力和力矩；主动变形四旋翼飞行器系统输入力，输入力矩，旋翼转速描述为：

或

其中，表示旋翼产生的驱动力，表示旋翼产生的驱动力矩，n_i²表示第i个旋翼的转速平方，i＝1...4；C为控制效率矩阵，C^-1为控制分配矩阵，δ_i表示第i个机臂绕机体系z_B轴旋转的角度，i＝1...4，和分别为sin(α_i)和cos(α_i)，l_i表示第i个机臂长度，i＝1...4，k_f表示升力系数，k_m表示旋翼电机的反扭矩系数；

其中，即是控制效率矩阵；

其中，s_(·)表示sin(·)，c_(·)表示cos(·)；

S4、根据步骤S3中所建立的主动变形四旋翼飞行器的动力学模型，设计位姿自抗扰飞行控制器；具体为：

将主动变形四旋翼飞行器系统动力学模型中的6个状态即三轴位置和三轴角度，视作6个通道，通过自抗扰控制ADRC将系统各通道之间耦合与变形引起的参数摄动当作内部扰动处理，采用扩张状态观测器ESO估计并补偿系统内外扰动实现各通道状态解耦，并且引入虚拟控制量实现系统控制解耦，从而将系统描述形式从MIMO转换为六个SISO系统的组合；表示如下：

其中：s_i(·)为不确定项，和分别是系统所受外部扰动和变形产生的扰动；表示x方向上位置、速度、加速度；表示y方向上位置、速度、加速度；表示z方向上位置、速度、加速度；表示滚转角、滚转角速度、滚转角加速度；表示俯仰角、俯仰角速度、俯仰角加速度；表示偏航角、偏航角速度、偏航角加速度；(p,q,r)表示机体三轴角速度；(b₁,b₂,b₃)是大小在附近的可调参数，(b₄,b₅,b₆)是大小分别在附近的可调参数，I_xx、I_yy、I_zz分别是主动变形四旋翼飞行器x、y、z轴转动惯量值，(u₁,u₂,u₃,u₄,u₅,u₆)是引入的虚拟控制量；

控制律采用内外环策略，内环为姿态控制，外环为位置控制，引入控制量(U₁,U₂,U₃,U₄)分别代表总升力期望值和绕机体系三轴的转动力矩期望值；位置实际值(x,y,z)和位置期望值(x^d,y^d,z^d)作为外环位置ADRC控制律的输入，输出滚转角和俯仰角期望值(φ^d,θ^d)以及U₁姿态实际值(φ,θ,ψ)和姿态期望值(φ^d,θ^d,ψ^d)作为内环姿态ADRC控制律输入，输出为(U₂,U₃,U₄)；最后(U₁,U₂,U₃,U₄)经过控制分配得到电机期望转速其中，虚拟控制量(u₁,u₂,u₃,u₄,u₅,u₆)与(U₁,φ^d,θ^d)和(U₂,U₃,U₄)之间的转换关系式分别为：

式中，s_φ表示sinφ，c_φ表示cosφ，c_θ表示cosθ，t_θ表示tanθ，s_ψ表示sinψ，c_ψ表示cosψ，表示cosφ^d；

俯仰角θ的姿态ADRC控制律设计过程为：

1)设计跟踪微分器TD，以给定信号期望俯仰角θ^d作为参考输入安排过渡过程：

其中，e是实际俯仰角与期望俯仰角的差值，v₁是跟踪微分器TD从初值到θ^d的过渡量，v₂是v₁导数值，参数(r′,h)分别为快速因子和滤波因子；

2)设计扩张状态观测器ESO，以系统输出俯仰角θ和控制输入u₅用于实时观测系统状态和所受扰动：

其中，(z₁,z₂)跟踪z₃估计总扰动s₅，(β₀₁,β₀₂,β₀₃)是一组可调参数；

3)设计非线性误差反馈律NLSEF，计算u₀并和扰动补偿组合计算出控制量u₅：

其中，参数(r′,h,c′)为可调参数，最速跟踪控制综合函数fhan(x₁,x₂,r′,h)和非线性函数fal(e,α,δ)表达式如下：

其中，δ0，φ和ψ通道控制律设计与θ通道相同；

位置ADRC控制律设计过程中，位置三通道的TD、ESO和NLSEF的设计都与θ通道所设计的相同，唯一不同点在于z通道NLSEF采用的是如下形式：

其中，(k₁,k₂)为可调参数。

2.根据权利要求1所述的主动变形四旋翼飞行器自抗扰飞行控制方法，其特征在于，步骤S1中两种变形方式为：

(a)机臂伸缩，即通过伸缩舵机使机臂进行伸缩变形，以改变机臂长度；

(b)机臂折叠，通过旋转舵机使机臂绕机体系z_B轴进行折叠变形，即改变δ_i，其中，δ_i表示第i个机臂绕机体系z_B轴旋转的角度，i＝1...4；其四个机臂单独或组合进行变形。