[发明专利]多变量干扰补偿四旋翼无人机轨迹与姿态协同控制方法

权利要求书

1.一种多变量干扰补偿四旋翼无人机轨迹与姿态协同控制方法，其特征是，具体步骤如下：

第一部分，轨迹控制器-干扰补偿器设计：基于齐次性理论针对外环位置子系统设计具有有限时间收敛特性的多变量轨迹控制器和干扰补偿器，其中干扰补偿器用于完成对位置子系统外界干扰的在线观测，并将干扰估计值实时传送给轨迹控制器，轨迹控制器确保在有限时间内实现对参考轨迹的高精度快速跟踪，并提供给姿态解算模块可行的虚拟控制指令；

第二部分，姿态解算：基于四旋翼无人机自身的飞行特性，构建虚拟控制指令与期望姿态角指令的解析关系，确保后续的内环轨迹控制器在通过实现对期望姿态跟踪的同时，达到间接跟踪给定参考轨迹的目的；

第三部分，姿态控制器-干扰补偿器设计：基于齐次性理论针对内环姿态子系统设计具有有限时间收敛特性的多变量姿态控制器和干扰补偿器，其中干扰补偿器用于完成对姿态子系统外界干扰的在线观测，并将干扰估计值实时传送给姿态控制器，姿态控制器确保在有限时间内实现对期望姿态的高精度快速跟踪。

2.如权利要求1所述的多变量干扰补偿四旋翼无人机轨迹与姿态协同控制方法，其特征是，具体实现过程如下：

第一步，基于多时间尺度的四旋翼无人机模型分析：依据无人机的多时间尺度特性，将六自由度的无人机模型划分为位置子系统和姿态子系统：

位置子系统

姿态子系统

其中，ξ＝[x，y，z]^T∈R³表示无人机的位置；v＝[v_x,v_y,v_z]^T表示无人机在坐标轴x，y，z方向的速度；g表示重力加速度；e_z＝[0，0，1]^T；τ_f表示无人机总升力；m表示无人机的质量；Θ＝[φ，θ，ψ]表示无人机的姿态角向量，其中φ表示俯仰角，θ表示滚转角，ψ表示偏航角；

Ω＝[ω_x,ω_y,ω_z]^T表示姿态角速度向量；I＝diag[I_x,I_y,I_z]表示飞行器惯性矩阵；τ＝[τ₁,τ₂，τ₃]^T表示控制转矩；△₁和△₂分别表示位置子系统和姿态子系统所受到的外界干扰；矩阵W定义如下

正交转动矩阵R定义如下

第二步，四旋翼无人机轨迹控制器-干扰补偿器设计：针对位置子系统(1)，引入虚拟控制变量

为保证控制信号τ_f连续，对位置子系统(1)进行扩展，定义如下的增广变量：

定义四旋翼无人机轨迹跟踪误差

其中x_1ref＝[x_ref，y_ref，z_ref]表示无人机的参考轨迹，表示x_1ref的i次微分信号，则基于轨迹跟踪误差的增广动态系统表述为：

针对四旋翼无人机轨迹误差增广系统(6)，设计控制律如(7)所示

其中式(7)中的相当于干扰补偿器，用于实现对式(6)中的综合干扰的实时在线估计，式(7)中，选择参数λ_i，使得多项式s⁴+λ₄s³+λ₃s²+λ₂s+λ₁为Hurwitz多项式，干扰补偿器的增益k₁和k₂满足：

其中L_o代表位置子系统综合干扰的上界，控制器参数ρ_i，通过式(9)计算：

第三步，四旋翼无人机姿态解算：通过姿态解算获得四旋翼无人机飞行轨迹与飞行姿态之间的解析关系，同时得到四旋翼无人机的总升力，具体计算过程如下：

其中τ'_fi，i＝1，2，3表示虚拟控制向量τ'_f的第i个元素，s_φ＝sinφ，s_θ＝sinθ，c_φ＝cosφ，c_θ＝cosθ，φ_ref和θ_ref为通过姿态解算获得的期望姿态角指令；

第四步，四旋翼无人机姿态控制器-干扰补偿器设计：针对姿态子系统(2)，引入中间变量

x₅＝Θ，x₆＝WΩ (13)

定义姿态跟踪误差

其中Θ_ref＝[φ_ref，θ_ref，ψ_ref]表示无人机的期望姿态，则基于姿态跟踪误差的动态系统表述为：

针对四旋翼无人机的姿态误差动态系统(15)，设计控制转矩如(16)所示

其中为姿态子系统的干扰补偿器，用以实现对姿态子系统综合干扰的实时在线估计，式(16)中，λ₅和λ₆为任意的正常值，干扰补偿器增益k₃和k₄满足k₄2L_i，其中L_i表示姿态子系统综合干扰△'的上界，控制器参数ρ₅＝ρ/(2-ρ)，ρ₆＝ρ，其中ρ∈(0，1)，四旋翼无人机实际控制转矩可通过(15)计算为：τ＝IW^-1τ'。

3.如权利要求1所述的多变量干扰补偿四旋翼无人机轨迹与姿态协同控制方法，其特征是，验证有效性具体步骤，将四旋翼无人机的轨迹姿态协同控制系统在Matlab/Simulink中进行集成设计，并进行仿真实验，仿真实现过程如下：

1)四旋翼无人机参考轨迹：p_ref(t)＝p_r0+a_3pt³+a_4pt⁴+a_5pt⁵，其中p_ref(t)表示p∈{x，y,z}方向上给定的参考轨迹，p_r0代表初始位置，系数a_3p，a_4p，a_5p由公式

计算得到，其中p_f和t_f分别表示终端位置及响应的时刻，仿真中取t_f＝20，x，y，z方向的终端位置x_f＝20,y_f＝5,z_f＝10，航向参考轨迹ψ_ref＝0；

2)四旋翼无人机物理参数：飞行器质量m＝0.625kg，惯性参数I_x＝2.3×10^-3kgm²，I_y＝2.4×10^-3kgm²，I_z＝2.6×10^-3kgm²；

3)协同控制器参数设置：轨迹控制器-干扰补偿器参数λ₁＝6，λ₂＝15，λ₃＝15，λ₄＝6ρ＝0.75，L_o＝10；姿态控制器-干扰补偿器参数λ₅＝15，λ₆＝20ρ＝0.7，L_i＝10；

仿真测试验证过程中，采样时间设置为定步长1毫秒，位置环外界干扰用时变函数△₁＝50(1+cos(t)+sin(t))diag(I_x,I_y，I_z)进行模拟，姿态环外界干扰△₂＝0.5(1+cos(t)+sin(t))，此外，仿真中假定模型存在20％的惯性参数不确定。