CN107608367B - 多变量干扰补偿四旋翼无人机轨迹与姿态协同控制方法 - Google Patents

Abstract

本发明涉及无人飞行器控制技术领域，为提出一种基于多变量干扰补偿的有限时间轨迹姿态协同控制算法，突破传统四旋翼无人机控制方法在控制精度、响应速度及抗干扰方面的不足，解决四旋翼无人机在综合干扰影响下轨迹与姿态的高精度快速稳定跟踪控制问题，达到提高四旋翼无人机控制性能，增强***安全性和可靠性的目的。多变量干扰补偿四旋翼无人机轨迹与姿态协同控制方法，具体步骤如下：第一部分，轨迹控制器‑干扰补偿器设计；第二部分，姿态解算；第三部分，姿态控制器‑干扰补偿器设计。本发明主要应用于无人飞行器控制场合。

Description

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

技术领域

本发明涉及无人飞行器控制技术领域，尤其涉及一种四旋翼无人机的高精度快速轨迹与姿态协同控制领域。具体讲,涉及多变量干扰补偿四旋翼无人机轨迹与姿态协同控制方法。

背景技术

四旋翼无人机在机械结构上具有轴对称性，四个旋翼均匀的分布在十字架结构的四个端点上，该类型无人机的动力由各个旋翼产生的升力提供，仅需改变四个旋翼的旋转速度，即可实现无人机的翻滚、俯仰、偏航等动作。四旋翼无人机具有体积小、重量轻、隐蔽性好、适合多平台、多空间使用，可以在地面、军舰上灵活垂直起降等优点。其简单的结构、超强的机动性、独特的飞行方式，在军事和民事领域展现出巨大的应用价值，已经被广泛应用于军事侦察、险区探测、目标捕获、战场管理、火力支援、电子干扰及通信中继等领域。

然而，四旋翼无人机是一个集多变量、不确定、非线性、快时变、强耦合、静不稳定及欠驱动为一体的复杂被控对象，其飞行控制技术一直是航空领域研究的重点和难点。目前，基于经典理论的传统控制方法，如PID控制算法，因其结构简单，易于实现，不依赖于***数学模型，在飞行控制领域被广泛使用。然而，基于传统理论的控制方法在进行四旋翼无人机控制器设计时，存在的不足主要体现在以下几个方面：(1)基于传统控制策略进行飞控***设计时，通常将多输入多输出的无人机模型，简化为多个单输入单输出的模型，然后针对每个通道进行控制器设计，这种人为强行解耦的设计策略在无人机进行快速机动飞行时，容易导致失稳；(2)基于Lyapunov理论设计的传统飞行控制***，只能实现对给定参考指令的渐近稳定跟踪控制，限制了无人机的极限飞行能力，现有理论表明基于有限时间的飞行控制***，在确保***稳定的同时，能够全面提升***的跟踪精度、收敛速度和鲁棒性；(3)基于传统控制方法设计的控制器，其参数调整严重依赖于***模型不确定和外界干扰，在实际飞行控制时，由于未知干扰的存在，飞行控制性能往往难以获得期望的控制效果，基于该策略设计的控制器参数调整费时费力。

针对上述局限，本发明在多变量的控制框架内，首次提出了针对四旋翼无人机的多变量干扰补偿有限时间轨迹姿态协同控制方法，基于该方法设计的飞控***，能对***模型参数不确定、未建模动态及外界风干扰等综合干扰进行在线估计与补偿，显著提高四旋翼无人机应对复杂飞行环境的能力；此外，基于该方法设计的控制器能够实现四旋翼无人机轨迹与姿态的有限时间协同，确保飞行控制***具有更高的控制精度、更快的收敛速度和更强的鲁棒性，有效增强***的控制性能；更重要的是基于本发明给出的控制器参数选取仅取决于***的标称模型，与***模型不确定和外界综合干扰无关，大大简化了控制***的参数调试。

本发明涉及一种四旋翼无人机飞行控制技术领域。具体来说，首先提出了不同于以往传统控制方式的基于多变量干扰补偿的四旋翼无人机轨迹姿态协同控制算法，随后通过Simulink仿真与传统PID方法进行了对比分析，验证了本发明提出算法的有效性。

发明内容

为克服现有技术的不足，本发明旨在提出一种应用于四旋翼无人机的高精度快速轨迹姿态协同控制算法。具体而言，考虑无人机复杂飞行环境下安全稳定可靠飞行的迫切需求，在综合考虑无人机模型参数不确定、未建模动态及外界干扰影响的情形下，提出一种基于多变量干扰补偿的有限时间轨迹姿态协同控制算法，突破传统四旋翼无人机控制方法在控制精度、响应速度及抗干扰方面的不足，解决四旋翼无人机在综合干扰影响下轨迹与姿态的高精度快速稳定跟踪控制问题，达到提高四旋翼无人机控制性能，增强***安全性和可靠性的目的。本发明采用的技术方案是，多变量干扰补偿四旋翼无人机轨迹与姿态协同控制方法，具体步骤如下：

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

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

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

具体实现过程如下：

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

位置子***

姿态子***

其中，ξ＝[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表示无人机的质量；Θ＝[φ,θ,ψ]表示无人机的姿态角向量，其中φ表示俯仰角，θ表示滚转角，ψ表示偏航角；W＝[ω_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(i＝1,2,3,4)使得多项式s⁴+λ₄s³+λ₃s²+λ₂s+λ₁为Hurwitz多项式，干扰补偿器的增益k₁和k₂满足

其中L_o代表位置子***综合干扰综

的上界，控制器参数ρ_i(i＝1,...,4)通过式(9)计算

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

其中τ'_fi(i＝1,2,3)表示虚拟控制向量τ'_f的第i个元素，s_φ＝sinφ,s_θ＝sinθ,

c_φ＝cosφ,cos_θ＝cosθ,

φ_ref和θ_ref为通过姿态解算获得的期望姿态角指令；

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

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

定义姿态跟踪误差

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

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

其中

为姿态子***的干扰补偿器，用以实现对姿态子***综合干扰的实时在线估计。式(16)中，λ₅和λ₆为任意的正常值，干扰补偿器增益k₃和k₄满足

其中L_i表示姿态子***综合干扰△'的上界，控制器参数ρ₅＝ρ/(2-ρ),ρ₆＝ρ，其中ρ∈(0,1)。四旋翼无人机实际控制转矩可通过(15)计算为：τ＝IW^-1τ'。

验证有效性具体步骤，将四旋翼无人机的轨迹姿态协同控制***在Matlab/Simulink中进行集成设计，并进行仿真实验，仿真实现过程如下：

(1)参数设置

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％的惯性参数不确定。

本发明的特点及有益效果是：

将本发明提出的算法与基于传统PID控制算法进行对比，两种控制方法获得的仿真结果如图2-图8所示。其中，图2-图5是利用本发明提出的基于多变量干扰补偿的四旋翼无人机轨迹姿态协同控制结果，图6-图7是基于传统PID获得结果，图8是两种控制算法在轨迹姿态协同控制精度和收敛速度上的对比曲线。

基于本发明提出的算法在对期望姿态指令进行跟踪时，最大跟踪误差约为10^-6弧度，跟踪精度明显优于PID控制算法。

附图说明：

附图1基于多变量干扰补偿的四旋翼无人机轨迹与姿态协同控制***结构图。

附图2干扰补偿-轨迹姿态协同跟踪曲线。

附图3干扰补偿-控制输入变化曲线。

附图4干扰补偿-位置环(外环)干扰估计曲线。

附图5干扰补偿-姿态环(内环)干扰估计曲线。

附图6传统PID-轨迹姿态协同跟踪曲线。

附图7传统PID-控制输入变化曲线。

附图8干扰补偿-传统PID轨迹姿态协同跟踪误差对比。

具体实施方式

本发明提出的基于多变量干扰补偿的四旋翼无人机轨迹姿态协同控制算法总体技术方案如图1所示，整个***主要包括三部分：轨迹控制器-干扰补偿器设计、姿态解算和姿态控制器-干扰补偿器设计，具体技术方案如下：

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

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

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

最后为了验证本发明提出算法的有效性，搭建四旋翼无人机轨迹姿态协同控制的MATLAB/Simulink仿真***，并将仿真结果与传统PID控制方法进行对比，验证本发明提出的算法在跟踪精度和收敛速度上的优势。

本发明以基于齐次性的有限时间控制理论为主要研究手段，提出一种基于多变量干扰补偿的四旋翼无人机轨迹与姿态协同控制算法，具体实现过程如下。

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

位置子***

姿态子***

其中，ξ＝[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表示无人机的质量；Θ＝[φ,θ,ψ]表示无人机的姿态，其中φ表示俯仰角，θ表示滚转角，ψ表示偏航角；W＝[ω_x,ω_y,ω_z]^T表示姿态角速度；I＝diag[I_x,I_y,I_z]表示飞行器惯性矩阵；τ＝[τ₁,τ₂,τ₃]^T表示控制转矩；△₁和△₂分别表示位置子***和姿态子***所受到的外界干扰；矩阵W定义如下

正交转动矩阵R定义如下

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

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

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

其中x_1ref＝[x_ref,y_ref,z_ref]表示无人机的参考轨迹，

表示x_1ref的i次微分信号，则基于轨迹跟踪误差的增广动态***表述为

针对四旋翼无人机轨迹误差增广***(22)，设计控制律如(23)所示

其中

式(23)中的

相当于干扰补偿器，用于实现对式(22)中的综合干扰

的实时在线估计，式(23)中，选择参数λ_i(i＝1,2,3,4)，使得多项式s⁴+λ₄s³+λ₃s²+λ₂s+λ₁为Hurwitz多项式，干扰补偿器的增益k₁和k₂满足

其中L_o代表位置子***综合干扰

的上界，控制器参数ρ_i(i＝1,...,4)通过式(25)计算

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

其中τ'_fi(i＝1,2,3)表示虚拟控制向量τ'_f的第i个元素，s_φ＝sinφ,s_θ＝sinθ,

c_φ＝cosφ,cos_θ＝cosθ,

φ_ref和θ_ref为通过姿态解算获得的期望姿态角指令。

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

x₅＝Θ,x₆＝WΩ (29)

定义姿态跟踪误差

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

针对四旋翼无人机的姿态误差动态***(31)，设计控制转矩如(32)所示

其中

为姿态子***的干扰补偿器，用以实现对姿态子***综合干扰的实时在线估计。式(32)中，λ₅和λ₆为任意的正常值，干扰补偿器增益k₃和k₄满足

其中L_i表示姿态子***综合干扰△'的上界，控制器参数ρ₅＝ρ/(2-ρ),ρ₆＝ρ，其中ρ∈(0,1)。四旋翼无人机实际控制转矩可通过(31)计算为：τ＝IW^-1τ'。

为了验证本发明提出的基于多变量干扰补偿的四旋翼无人机轨迹姿态协同控制算法的有效性，首先将四旋翼无人机的轨迹姿态协同控制***在Matlab/Simulink中进行集成设计，并进行了仿真实验,主要仿真过程如下：

(1)参数设置

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％的惯性参数不确定。

(2)结果分析

在上述给定的条件下，将本发明提出的算法与基于传统PID控制算法进行对比，两种控制方法获得的仿真结果如图2-图8所示。其中，图2-图5是利用本发明提出的基于多变量干扰补偿的四旋翼无人机轨迹姿态协同控制结果，图6-图7是基于传统PID获得结果，图8是两种控制算法在轨迹姿态协同控制精度和收敛速度上的对比曲线。

多变量干扰补偿轨迹姿态协同控制结果分析：图2表明，在提出的四旋翼无人机轨迹姿态协同控制框架(如图1)内，利用本发明提出的基于多变量干扰补偿的轨迹与姿态协同控制算法，能很好的实现对给定参考轨迹和期望姿态协同跟踪控制。图3给出了相应的用于实现飞行轨迹姿态协同控制飞行所需的控制转矩和控制总升力，从中可以发现基于本发明算法获得控制曲线变化平缓，易于工程实现。此外，本算法在有限时间内确保实现对四旋翼无人机轨迹姿态协同控制的同时，能够对无人机所受到的外界干扰及模型不确定等综合干扰进行在线观测，图4和图5分别给出了位置环和姿态环干扰补偿器对四旋翼无人机位置子***和姿态子***所受到综合干扰估计的变化曲线，从中可以看出，本发明给出的算法能够很好的实现对***所受到综合干扰的实时在线观测。

多变量干扰补偿轨与传统PID轨迹姿态协同控制结果分析：图6和图7分别给出了传统PID控制下的轨迹姿态协同跟踪曲线及响应的控制曲线，从中可以看出基于PID的控制策略，虽然能较好的实现对给定轨迹的跟踪，但对期望姿态指令的跟踪效果欠佳。进一步，图8给出了两种算法在四旋翼无人机轨迹姿态协同跟踪控制精度上的对比曲线，从曲线中可以看出基于PID的协同控制策略在x,y,z方向上,对给定参考轨迹x_ref,y_ref和z_ref的最大稳态跟踪误差分别为0.02m,0.027m和0.021m；从图8的放大图中可以发现，基于本课题提出的多变量干扰补偿的协同控制算法在x,y,z方向上，对给定参考轨迹的最大稳态跟踪误差的数量级在10^-5m左右，明显优于PID协同控制算法对给定参考轨迹的跟踪精度。此外，图8中给出了两种协同控制算法姿态跟踪误差曲线的对比，仿真结果表明，PID对滚转、俯仰和偏航三个方向上的最大姿态跟踪误差分别为1.7*10^-3弧度，8*10^-3弧度和*10^-4弧度，而基于本发明提出的算法在对期望姿态指令进行跟踪时，最大跟踪误差约为10^-6弧度，跟踪精度明显优于PID控制算法。

Claims (3)

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％的惯性参数不确定。

Images