CN102830622A - 一种四旋翼飞行器自抗扰自动飞行控制方法 - Google Patents

Abstract

本发明涉及一种利用自抗扰控制技术完成四旋翼无人飞行器自主飞行的方法，属于无人飞行器自动控制领域。将目标值过渡过程安排后的输出x_1d及其微分

分别与扩张状态观测器输出做差，再对两个差值进行非线性变换，得到非线性反馈控制律u₀；对于三个姿态角和纵向位移自抗扰控制器，再与扩张状态观测器的反馈做差，得到的输出作为扩张状态观测器与四旋翼系统相应通道的输入；对于前向和侧向位移自抗扰控制器，直接将u₀作为扩张状态观测器与四旋翼系统相应通道的输入，四旋翼系统相应通道响应后，将实际值反馈给扩张状态观测器，从而形成闭环的自抗扰控制器；本方法抗干扰能力强，有效的解决了四旋翼系统建模困难、飞行过程中环境多样性、干扰频繁的难题。

Description

一种四旋翼飞行器自抗扰自动飞行控制方法

技术领域

本发明涉及一种利用自抗扰控制技术完成四旋翼无人飞行器自主飞行的方法，属于无人飞行器自动控制领域。

背景技术

四旋翼无人机具有固定翼无人机难以比拟的优点：能够适应各种环境；具备自主起飞和着陆能力，高度智能化；能以各种姿态飞行，如空中悬停、前飞、侧飞和倒飞等。这些优点使四旋翼飞行器拥有重要的军用和民用价值，其特别适合在近地面环境(如室内和丛林等)中飞行，在大气监测、搜寻营救、航拍监控、资源勘探、电力线路监测、森林防火等恶劣环境中具有广泛的应用前景；在军用方面，既能执行各种非杀伤性任务，又能执行各种软硬杀伤性任务，包括侦察、监视、目标截获、诱饵、攻击等，大有可为。与此同时，它还是火星探测无人飞行器的重要研究方向之一。

无人机的自主飞行控制系统是无人机系统的核心，其性能好坏很大程度取决于飞行控制律的设计。实际控制问题中应用最广的控制算法为PID（ProportionIntegration Differentiation比例积分微分）控制器，其优点在于算法简单，而不足之处是当对象存在非线性、时变、耦合及参数和结构不确定等情况时，其控制效果不理想。对于许多现代控制理论，虽然一直处于蓬勃发展的状态，但由于其对被控对象模型精确度的要求过高而被许多领域拒之门外。ADRC（AutoDisturbance Rejection Controlller自抗扰控制技术）继承了PID的优点，克服了其缺点。本发明将自抗扰控制技术应用到四旋翼的飞行控制中，在完成飞行任务的同时保证良好的飞行品质。

发明内容

本发明的目的是为解决现有四旋翼自主飞行控制效果不佳的问题，提出一种自抗扰飞行控制方法，以提高四旋翼无人机的飞行品质。

四旋翼飞行器自抗扰自主飞行控制方法，具体设计包括如下步骤：

步骤1：建立四旋翼系统的数学模型。

利用力学定理定律建立以四旋翼系统的三自由度位置前向位移x、侧向位移y、纵向位移z和三自由度姿态偏航角ψ、俯仰角θ、滚转角φ为输出变量的6个二次微分方程：

$\stackrel{\·\·}{x}=\frac{(F_r+F_l+F_f+F_b)}{m}(\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ})$

$\stackrel{\·\·}{y}=\frac{(F_r+F_l+F_f+F_b)}{m}(\cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ})$

$\stackrel{\·\·}{z}=\frac{(F_r+F_l+F_f+F_b)}{m}\cos \mathrm{\φ}\cos \mathrm{\θ}-g+g_z$

$\stackrel{\·\·}{\mathrm{\φ}}=(F_r-F_l)\frac{l}{J_x}+g_{\mathrm{\φ}}$

$\stackrel{\·\·}{\mathrm{\θ}}=(F_f-F_b)\frac{l}{J_y}+g_{\mathrm{\θ}}$

$\stackrel{\·\·}{\mathrm{\ψ}}=(T_r+T_l+T_f+T_b)\frac{1}{J_z}+g_{\mathrm{\ψ}}$

其中m为四旋翼系统的质量，l为螺旋桨中心到机体中心的距离，g为重力加速度，F_f，F_b,F_l，F_r分别是前后左右四个螺旋桨旋转产生的向上的升力。T_f,T_b,T_l，T_r分别是前后左右四个螺旋桨旋转产生的扭矩。g_φ,g_θ,g_ψ,g_z分别是φ、θ、ψ和z微分方程中的耦合效应、非线性特性等未建模动态以及外部干扰的干扰总和。J_x,J_y,J_z分别为四旋翼机体绕滚转轴、俯仰轴、偏航轴旋转的转动惯量。

将以上所得的每个二次微分方程用一个二阶子系统等效表示，具体实施如下：将位移或姿态作为二阶系统的一个状态变量x₁，位移或姿态的变化率作为二阶系统的另一个状态变量x₂，分别将6个二次微分方程转换成6组如下所示的严格反馈形式的状态方程：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=f(x_1,x_2,w)+\mathrm{bu}\end{array}\right.$

其中w表示系统通道中的外干扰，b为控制输入系数，u为状态方程中的控制输入或等效控制输入。

在φ姿态状态方程，f(x₁,x₂,w)=g_φ，u=F_r-F_l。

在θ姿态状态方程，f(x₁,x₂,w)=g_θ，

u=F_f-F_b。

在ψ姿态状态方程，f(x₁,x₂,w)=g_ψ，

u=T_r+T_l+T_f+T_b。

在z状态方程中，f(x₁,x₂,w)=g_z，

u=F_r+F_l+F_f+F_b。

x状态方程根据力学及几何关系直接求解而得，f(x₁,x₂,w)=0，

u=cosφsinθcosψ+sinφsinψ。

y状态方程根据力学及几何关系直接求解而得，f(x₁,x₂,w)=0，

u=cosφsinθsinψ-sinφcosψ。

步骤2：根据步骤1得到的数学模型，分别建立基于前向位移x、侧向位移y、纵向位移z、偏航角ψ、俯仰角θ、滚转角φ的六个自抗扰控制器。

所述自抗扰控制器的设计方法为：

步骤2.1，对四旋翼系统的目标位置(x_d，y_d,z_d)以及目标航向角ψ_d进行过渡过程安排。

所述过渡过程安排，是考虑四旋翼系统中螺旋桨的物理实现能力，根据目标位置(x_d ，y_d,z_d)以及目标航向角ψ_d，安排一个满足单调性的过渡过程，输出参考信号x_1d及其微分

本发明中采用正弦函数实现递增或递减过渡来代替阶跃指令的跳变过程。

步骤2.2，设计非线性反馈控制律。

分别对误差比例项e_P、微分项e_D进行非线性变换，得到非线性反馈控制律u₀：

u₀=β₀fal(e_P,α₀,δ)+β₁fal(e_D,α₁,δ)

其中β₀为影响响应幅值的参数、β₁为影响超调量的参数。β₀越大响应幅值越大，β₁越大抑制超调的能力越强。控制过程中，将β₁从小往大调节，抑制超调。fal(·)表示非线性函数，其具体形式为：

$\mathrm{fal}(e,a,\mathrm{\δ})=\left\{\begin{array}{cc}\frac{e}{{\mathrm{\δ}}^{1-a}}& \left|e\right|\≤\mathrm{\δ}\\ {\left|e\right|}^a\mathrm{sign}\left(e\right)& \left|e\right|\≥\mathrm{\δ}\end{array}\right.$

为了避免高频颤振现象的出现，将幂函数|e|^asign(e)原点附近部分替换为线性函数

δ为线性段的区间长度。本发明实施实例中δ选取为控制系统采样周期0.001秒。

而fal(·)中重要参数α的取值分如下四种情况：

为了取得好的控制效果，本控制方法选取0<α<1。

步骤2.3，设计扩张状态观测器。

针对步骤1所得的每个二阶状态方程，设计其扩张状态观测器：

$\left\{\begin{array}{cc}e=z_1-y_0& \\ \mathrm{fe}=\mathrm{fal}(e,0.5,\mathrm{\&delta;})& {\mathrm{fe}}_1=\mathrm{fal}(e,0.25,\mathrm{\&delta;})\\ {\stackrel{\&CenterDot;}{z}}_1=z_2-{\mathrm{\&beta;}}_{2}e& \\ {\stackrel{\&CenterDot;}{z}}_2=z_3+\mathrm{bu}-{\mathrm{\&beta;}}_3\mathrm{fe}& \\ {\stackrel{\&CenterDot;}{z}}_3=-{\mathrm{\&beta;}}_4{\mathrm{fe}}_1& \end{array}\right.$

其中Z₁、Z₂分别为两个状态变量x₁、x₂的估计值，Z₃为系统通道中干扰总和的估计值，y₀为系统中相应通道的输出。β₂、β₃、β₄为观测器调节参数，为提高跟踪精度，在实际控制中根据使得估计误差达到最小的原则，对β₂、β₃、β₄的取值进行优化调整。

步骤2.4，设计自抗扰控制律。

三个姿态角和纵向位移Z自抗扰控制器的自抗扰控制律包含两部分，一部分是非线性反馈控制律，一部分是系统通道中干扰总和的估计值z₃的前馈补偿，其具体形式为

u=u₀-z₃/b

前向和侧向位移自抗扰控制器中不考虑z₃的前馈补偿，自抗扰控制律为如下形式：

u=u₀

步骤2.5，形成自抗扰控制器。

将目标值经步骤2.1过渡过程安排后的输出x_1d及其微分

分别与步骤2.3得到的扩张状态观测器输出Z₁、Z₂做差，得到e_P和e_D；对二者进行非线性变换，得到非线性反馈控制律u₀。对于三个姿态角和纵向位移Z自抗扰控制器，再与扩张状态观测器的反馈做差，得到的输出作为扩张状态观测器与四旋翼系统相应控制通道的输入；对于前向和侧向位移自抗扰控制器，直接将u₀作为扩张状态观测器与四旋翼系统相应控制通道的输入，四旋翼系统相应通道响应后，将实际值反馈给扩张状态观测器，从而形成闭环的自抗扰控制器。

所述前向位移x自抗扰控制器中,e_P为过渡安排后的目标位置与四旋翼系统实际前向位移估计值（相应的扩张状态观测器的z₁）的差值；e_D为过渡安排后的目标位置的微分与四旋翼系统实际前向速度估计值z₂的差值；

所述侧向位移y自抗扰控制器中,e_P为过渡安排后的目标位置与四旋翼系统实际侧向位移估计值（相应的扩张状态观测器的z₁）的差值；e_D为过渡安排后的目标位置的微分与四旋翼系统实际侧向速度估计值的差值；

所述纵向位移z自抗扰控制器中,e_P为过渡安排后的目标位置与四旋翼系统实际纵向位移估计值（相应的扩张状态观测器的z₁）的差值；e_D为过渡安排后的目标位置的微分与四旋翼系统实际纵向速度估计值z₂的差值；

所述偏航角ψ自抗扰控制器中,e_P为过渡安排后的目标偏航角与四旋翼系统实际偏航角估计值（相应的扩张状态观测器的z₁）的差值；e_D为过渡安排后的目标偏航角的微分与四旋翼系统实际偏航角变化率估计值z₂的差值；

所述俯仰角θ自抗扰控制器中,e_P为由x和y位置通道的自抗扰控制律解算出的目标俯仰角与相应的扩张状态观测器的z₁（实际俯仰角的估计值）的差值；e_D为目标俯仰角的微分与俯仰角变化率估计值z₂的差值；

所述滚转角φ自抗扰控制器中,e_P为由x和y位置通道的自抗扰控制律解算出的目标滚转角与相应的扩张状态观测器的z₁（实际滚转角的估计值）的差值；e_D为目标滚转角的微分与滚转角变化率估计值z₂的差值。

步骤3：将六个自抗扰控制器关联起来，形成完整的自主飞行控制律。具体方法为：

1）对于前向位移、侧向位移、纵向位移以及三个姿态角的自抗扰控制器生成的系统通道控制律输出分别为u_x,u_y,u_z,u_φ,u_θ,u_ψ，由步骤1可知

u_x=cosφsinθcosψ+sinφsinψ

u_y=cosφsinθsinψ-sinφcosψ

将四旋翼系统的目标滚转角φ_R和目标俯仰角θ_R代入前向位移、侧向位移的通道控制律中，得到

φ_R=asin(u_x sinψ-u_y cosψ)

${\mathrm{\&theta;}}_R=a\sin \left(\frac{u_x\cos \mathrm{\&psi;}+u_y\sin \mathrm{\&psi;}}{\cos {\mathrm{\&phi;}}_R}\right)$

将所得的φ_R和θ_R分别作为φ和θ姿态自抗扰控制器的参考输入，对四旋翼系统的姿态进行控制，从而实现对前向位移、侧向位移的调整。

2）由步骤1可知

u_z＝F_r+F_l+F_f+F_b

u_φ＝F_r-F_l

u_θ＝F_f-F_b

u_ψ=T_r+T_l+T_f+T_b

而四个升力F_f,F_b,F_l,F_r,以及四个扭矩T_f,T_b,T_l，T_r的表达式如下所示

F_f＝K_fU₁    T_f=-K_tU₁

F_b＝K_fU₂    T_b=-K_tU₂

F_l＝K_fU₃    T_l＝K_tU₃

F_r＝K_fU₄    T_r＝K_tU₄

上式中T_f、T_b取负是因为前后两个电机需要逆时针安装，而左右两个电机是顺时针安装。

同时K_f和K_t分别是通过辨识算法估计出的升力和力矩系数；U₁,U₂,U₃,U₄分别为四旋翼系统的四个螺旋桨驱动电机的输入，由以上三组方程解出；当采用PWM驱动电机时，U₁,U₂,U₃,U₄为PWM脉宽值，当采用电压驱动电机时，U₁,U₂,U₃,U₄为电压值。

将U₁,U₂,U₃,U₄作为四旋翼系统的控制输入，使系统实时调整飞行状态，到达目标位置(x_d，y_d,z_d)，并实现目标偏航角ψ_d，使系统以良好的飞行品质完成飞行任务。

有益效果

本发明设计的自抗扰飞行控制方法对系统数学模型建立精确度要求不高，并且抗干扰能力强，其采用扩张状态观测器对未建模动态及外部干扰进行估计、实现前馈动态补偿，有效的解决了四旋翼系统建模困难、飞行过程中环境多样性、干扰频繁的难题。不仅适用于四旋翼系统，在对本发明方法的公式作适当推广、变形的情况下，本发明方法还适用于其他无人机系统。

附图说明

图1为现有技术的四旋翼的动力学特性；

图2为本发明的控制系统原理框图；

图3为本发明的自抗扰控制器的结构示意图；

图4为实施例1的控制系统硬件系统结构图；

图5为实施例1的目标位移与实际系统位移曲线的对比图；

图6为实施例1的目标航向和高度与实际系统的航向和高度曲线对比图；

图7为实施例1的梳状航线飞行任务仿真实验的航向和高度图；

图8为实施例1的梳状航线飞行任务仿真实验的飞行航线图；

图9为实施例2的四旋翼悬浮控制系统的闭环构成；

图10为实施例2的正弦信号跟踪图；

图11为实施例2的带安排过渡过程的方波信号跟踪图。

具体实施方案

为了更好的说明本发明的目的和优点，下面结合附图和实施例对技术方案作进一步说明。

根据实际的飞行任务，四旋翼无人飞行器需要完成相应的机动，包括巡航和悬停。巡航涉及到达目标高度、目标位置，以及沿设定航线飞行等。悬停要求控制系统实现姿态的增稳。

将自抗扰控制技术应用到四旋翼飞行控制中，首先考虑四旋翼系统的基本结构和动力学特性。如图1所示，四旋翼系统中前向电机与后向电机的转速差会驱动四旋翼系统绕俯仰轴做俯仰的姿态运动；左向电机和右向电机的转速差会驱动四旋翼系统绕滚转轴做滚转的姿态运动；前后电机反转、左右电机正转，当这两对电机产生的扭矩不平衡时，四旋翼系统会有偏航运动。简而言之，整个系统是通过调节四个螺旋桨的驱动电机的输入，改变四个螺旋桨的转速快慢，以此改变系统所受力和力矩，从而调节系统的姿态。而四旋翼系统所受到的合外力为四个螺旋桨旋转产生的垂直机体的升力和重力，当四旋翼系统水平飞行时，升力和重力协同调节飞行高度，但当系统有俯仰或是滚转运动时，升力的分力会产生一个前向或是侧向的加速度，从而有前向或是侧向位移。概括而言，四旋翼系统通过改变四个螺旋桨的转速，改变系统所受力和力矩，从而调节系统的线运动和角运动。换言之，四旋翼系统中，控制量为四个螺旋桨的驱动输入，而被控量为四旋翼机体的三自由度姿态以及三自由度线位移。

自抗扰飞行控制律在四旋翼系统上实现的基本原理如图2所示：根据四旋翼的动力学特性，前向和侧向线运动是通过机体的俯仰和滚转两种角运动来实现，而四旋翼机体的角运动通过四旋翼的四个螺旋桨的转速变化来实现。由此将四旋翼动力学模型划分为姿态/位置内外环子系统的结构，因此控制律也是按内外环分层结构设计。控制律的设计基本原理如下：设定目标位置(x_d，y_d,z_d)以及目标航向角ψ_d，位置环中x，y的控制设计中，根据目标的x_d，y_d，通过位移x，y两个通道的自抗扰控制器得出期望的滚转角、俯仰角值φ_R,θ_R，作为姿态控制环的目标值，姿态环再根据目标的姿态角值φ_R,θ_R,ψ_d，通过三个姿态角自抗扰控制器，得出三个姿态自抗扰控制律，z位移控制中，根据目标z_d，通过z位移通道自抗扰控制器，得出纵向位移控制律，综合纵向位移控制律以及三个姿态控制律，解算出四旋翼系统四个螺旋桨的控制输入U₁,U₂,U₃,U₄，从而构成整个闭环的控制系统，使四旋翼无人机实时调整飞行状态，到达目标位置(x_d，y_d,z_d)，并实现目标偏航角ψ_d，最终控制四旋翼系统以良好的飞行品质完成飞行任务。

具体而言，位置环控制律设计包括三自由度位置前向位移、侧向位移、纵向位移x，y,z的控制律设计，即三个控制通道分别用自抗扰控制器进行设计。

姿态环控制律设计包括三自由度姿态偏航角、俯仰角、滚转角ψ,θ,φ的控制律设计。

每个自抗扰控制器的结构如图3所示，其包含三个部分：安排过渡过程，扩张状态观测器以及非线性反馈控制。具体设计方法如发明内容中步骤2.1至2.5所述，整个自主飞行控制律的设计如发明内容中步骤3所述。

实例1：

根据本发明方法设计的四旋翼飞行控制系统硬件部分如图4所示，包括主控单元，传感器模块，无线通信模块和电机控制模块。四旋翼飞行器作为整个控制系统的载体，主要包括四轴机架，两对电机和电调以及自驾仪控制器。

主控单元：采用AVR单片机1作为自驾仪控制器的主控单元。整个系统的数据处理以及自抗扰控制律都是以此为基础搭建的。

传感器模块：传感器模块主要实现姿态采集，位置信息采集以及气压高度和磁航向的测量。采用三轴陀螺13测量飞机当前的三轴角速度值，三轴加速计14测量飞机当前三轴角加速度值。由于惯性测量单元都采用模拟芯片，需要通过模数转换器15进行信号的模数转换，数模转换后的值通过SPI总线与AVR单片机1进行通信，最后通过姿态解算得到飞机的当前姿态信息，作为自抗扰控制器的当前姿态值的反馈；GPS模块9用于测量飞机当前的位置信息，并通过串口使用GPS固定协议将数据传递给AVR单片机1；磁力计10作为本系统的电子罗盘，用于测量飞机当前的航向角，可以弥补采用姿态解算得到的航向角不够精确的不足；气压计11用于测量气压高度值，其精度好于GPS高度；空速计12提供飞机的空速值。

无线通信模块：本系统采用Zigbee或者电台6进行自驾仪与PC机的无线通信工作，通过PC机观察飞行控制效果。

电机控制模块：遥控器4的信号通过接收机5的前五个通道（分别为滚转通道、俯仰通道、油门通道、偏航通道和模式切换）传递给PPM解码器2，PPM解码器将输入的PWM值通过PPM（脉冲位置调制）模式发给AVR单片机1。复选器3通过模式切换的功能，确定选择输出遥控器4的输入PWM值（即手控模式）还是输出主控器1给出的控制量（即自驾模式）。电子调速器7的电源线、信号线与地线与电机相连，为电机供电并将复选器3选择后的控制量输出该给电机。这样就完成了电机的控制工作。

实例1中硬件系统搭建如下：

AVR单片机采用ATmega2560。控制器完成的主要工作包括数据采集，数据处理，算法实现以及电机控制。

数据采集主要包括三轴角速度，三轴角加速度以及位置信息等。陀螺仪采用IDG500（双轴X和Y）和ISZ500（单轴Z），加速计采用ADXL335。当前位置信息通过GPS确定经纬度，结合GPS和气压传感器确定当前高度值。GPS采用u-Blox的LEA-5H，气压传感器采用BMP085。

数据处理主要是根据陀螺仪以及加速度计给出的三轴角速度和角加速度值解算出当前四旋翼飞行器的当前姿态，包括滚转角和俯仰角。这里采用的姿态解算的方法是方向余弦矩阵算法。由于该算法在解算偏航角的时候，需要根据GPS进行补偿修正，其在无速度或者速度较小的时候会出现严重漂移的情况，所以本实例的偏航角选择通过电子罗盘测量所得。电子罗盘选用HMC5833。数据处理的另一个需要完成的任务是给出四旋翼无人机机的目标航向角和目标位置信息。其中目标位置信息即为目标航点的经纬度及高度值（经纬度与坐标值之间可进行转换），目标航向角则根据如下公式计算：目标航向角=无人机当前位置和目标航点连线与正北向的夹角-无人机朝向与正北向的夹角。

算法实现部分则是根据数据处理所得到的一些数据信息进行飞行器的控制任务。飞行器的姿态信息由惯性测量单元给出，高度和位置信息分别由气压传感器和GPS给出。通过自抗扰控制器给出对四个电机的控制量。

电机选择无刷直流电机，通过PWM控制其转速以达到对各桨产生的力和力矩大小进行控制。

为了验证本发明中提出的姿态和位置内外环分层结构控制律设计方案，进行离线仿真实验，仿真实验结果曲线如图5、图6所示，由仿真曲线可知，控制系统实现了无超调、无稳差的参考信号渐进跟踪，从而验证了方案的可行性和有效性。

进一步进行梳状航线飞行任务的仿真实验，仿真实验过程中的基本设置如下：设定所有飞行状态量初始都为零，目标飞行高度为10m，控制目标是使四旋翼无人机保持高度并完成梳状航线飞行任务。仿真实验结果如图7、图8所示，由仿真图形可知，四旋翼无人机基本按所设定梳状航线飞行，从而验证了本发明提出的自抗扰飞行控制方法实现四旋翼无人机精确、高品质的飞行控制的可行性和有效性。

实例2：

在Quanser公司研发的四旋翼悬浮系统上应用自抗扰控制技术进行姿态调节验证。整个系统闭环结构框图9所示：姿态角由悬浮系统1上的编码器测量，反馈到数据采集/处理板卡2上，通过数据采集/处理板卡2运行的控制程序4（给出的是控制程序的框架示意图）得出控制信号，经过功放3放大，然后驱动悬浮系统1上的四个螺旋桨，改变其转速，以调节姿态。姿态调节曲线如图10、图11所示，通过和当前四旋翼控制仿真中常用的两种非线性控制方法反步法和滑膜控制进行了控制效果对比，实验结果显示自抗扰控制器拥有更好的信号跟踪和稳定效果，自抗扰闭环控制系统中的振荡和稳差都较小。

Claims (8)

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

步骤1：建立四旋翼系统的数学模型；

利用力学定理定律建立以四旋翼系统的三自由度位置前向位移x、侧向位移y、纵向位移z和三自由度姿态偏航角ψ、俯仰角θ、滚转角φ为输出变量的6个二次微分方程：

其中m为四旋翼系统的质量，l为螺旋桨中心到机体中心的距离，g为重力加速度，F_f，F_b,F_l，F_r分别是前后左右四个螺旋桨旋转产生的向上的升力；T_f,T_b,T_l，T_r分别是前后左右四个螺旋桨旋转产生的扭矩；g_φ,g_θ,g_ψ,g_z分别是φ、θ、ψ和z微分方程中的未建模动态以及外部干扰的干扰总和；J_x,J_y,J_z分别为四旋翼机体绕滚转轴、俯仰轴、偏航轴旋转的转动惯量；

将每个二次微分方程用一个二阶子系统等效表示，具体实施如下：将位移或姿态作为二阶系统的一个状态变量x₁，位移或姿态的变化率作为二阶系统的另一个状态变量x₂，分别将6个二次微分方程转换成6组严格反馈形式的状态方程：

其中w表示系统通道中的外干扰，b为控制输入系数，u为状态方程中的控制输入或等效控制输入； 

在φ姿态状态方程，f(x₁,x₂,w)=g_φ， 

u=F_r-F_l；

在θ姿态状态方程，f(x₁,x₂,w)=g_θ， 

u=F_f-F_b；

在ψ姿态状态方程，f(x₁,x₂,w)=g_ψ， u=T_r+T_l+T_f+T_b；

在z状态方程中，f(x₁,x₂,w)=g_z， 

u=F_r+F_l+F_f+F_b；

x状态方程根据力学及几何关系直接求解而得，f(x₁,x₂,w)=0， 

u=cosφsinθcosψ+sinφsinψ；

y状态方程根据力学及几何关系直接求解而得，f(x₁,x₂,w)=0， u=cosφsinθsinψ-sinφcosψ；

步骤2：根据步骤1得到的数学模型，分别建立基于前向位移x、侧向位移y、纵向位移z、偏航角ψ、俯仰角θ、滚转角φ的六个自抗扰控制器；

所述自抗扰控制器的设计方法为：

步骤2.1，对四旋翼系统的目标位置(x_d，y_d,z_d)以及目标航向角ψ_d进行过渡过程安排，输出参考信号x_1d及其微分 

步骤2.2，设计非线性反馈控制律；

分别对误差比例项e_P、微分项e_D进行非线性变换，得到非线性反馈控制律u₀：

u₀=β₀fal(e_P,α₀,δ)+β₁fal(e_D,α₁,δ)

其中β₀为影响响应幅值的参数、β₁为影响超调量的参数；fal(·)表示非线性函数，其具体形式为：

δ为线性段的区间长度；0<α<1；

步骤2.3，设计扩张状态观测器；

针对步骤1所得的每个二阶状态方程，设计其扩张状态观测器： 

其中Z₁、Z₂分别为两个状态变量x₁、x₂的估计值，Z₃为系统通道中干扰总和的估计值，y₀为系统中相应通道的输出；β₂、β₃、β₄为观测器调节参数；

步骤2.4，设计自抗扰控制律；

三个姿态角和纵向位移Z自抗扰控制器的自抗扰控制律包含两部分，一部分是非线性反馈控制律，一部分是系统通道中干扰总和的估计值z₃的前馈补偿，其具体形式为

u=u₀-z₃/b

前向和侧向位移自抗扰控制器中不考虑z₃的前馈补偿，自抗扰控制律为如下形式：

u=u₀

步骤2.5，形成六个自抗扰控制器：前向位移x自抗扰控制器、侧向位移y自抗扰控制器、纵向位移z自抗扰控制器、偏航角ψ自抗扰控制器、俯仰角θ自抗扰控制器和滚转角φ自抗扰控制器；

将目标值经步骤2.1过渡过程安排后的输出x_1d及其微分 

分别与步骤2.3得到的扩张状态观测器输出Z₁、Z₂做差，得到e_P和e_D；对二者进行非线性变换，得到非线性反馈控制律u₀；对于三个姿态角和纵向位移Z自抗扰控制器，再与扩张状态观测器的反馈做差，得到的输出作为扩张状态观测器和四旋翼系统相应控制通道的输入；对于前向和侧向位移自抗扰控制器，直接将u₀作为扩张状态观测器和四旋翼系统相应控制通道的输入，四旋翼系统相应通道响应后，将实际值反馈给扩张状态观测器，从而形成闭环的自抗扰控制器；

步骤3：将六个自抗扰控制器关联起来，形成完整的自主飞行控制律；具体方法为：

1）对于前向位移、侧向位移、纵向位移以及三个姿态角的自抗扰控制器生成的系统通道控制律输出分别为u_x,u_y,u_z,u_φ,u_θ,u_ψ，由步骤1可知 

u_x=cosφsinθcosψ+sinφsinψ

u_y=cosφsinθsinψ-sinφcosψ

将四旋翼系统的目标滚转角φ_R和目标俯仰角θ_R分别代入前向位移、侧向位移的通道控制律中，得到

φ_R=asin(u_x sinψ-u_y cosψ)

将所得的φ_R和θ_R分别作为φ和θ姿态自抗扰控制器的参考输入，对四旋翼系统的姿态进行控制，从而实现对前向位移、侧向位移的调整；

2）由步骤1可知

u_z＝F_r+F_l+F_f+F_b

u_φ＝F_r-F_l

u_θ＝F_f-F_b

u_ψ=T_r+T_l+T_f+T_b

而四个升力F_f,F_b,F_l,F_r,以及四个扭矩T_f，T_b,T_l，T_r的表达式如下所示

F_f＝K_fU_l    T_f=-K_tU₁

F_b＝K_fU₂    T_b=-K_tU₂

F_l＝K_fU₃    T_l＝K_tU₃

F_r＝K_fU₄    T_r＝K_tU₄

同时K_f和K_t分别是通过辨识算法估计出的升力和力矩系数；U₁,U₂,U₃,U₄分别为四旋翼系统的四个螺旋桨驱动电机的输入，由以上三组方程解出；

将U₁,U₂,U₃,U₄作为四旋翼系统的控制输入，使系统实时调整飞行状态，到达目标位置(x_d，y_d,z_d)，并实现目标偏航角ψ_d，从而使系统以良好的飞行品质完成飞行任务。

2.根据权利要求1所述的一种四旋翼飞行器自抗扰自动飞行控制方法，其特征在于：所述过渡过程安排，是根据目标位置(x_d，y_d,z_d)以及目标航向角ψ_d，安排一个满足单调性的过渡过程，输出参考信号x_1d及其微分 本发明中采用正弦函数实现递增或递减过渡来代替阶跃指令的跳变过程。 

3.根据权利要求1所述的一种四旋翼飞行器自抗扰自动飞行控制方法，其特征在于：β₀越大响应幅值越大，β₁越大抑制超调的能力越强；控制过程中，将β₁从小往大调节，抑制超调。

4.根据权利要求1所述的一种四旋翼飞行器自抗扰自动飞行控制方法，其特征在于：作为一个实施例，δ选取为控制系统采样时间。

5.根据权利要求1所述的一种四旋翼飞行器自抗扰自动飞行控制方法，其特征在于：前向位移x自抗扰控制器中,e_P为过渡安排后的目标位置与四旋翼系统实际前向位移估计值的差值；e_D为过渡安排后的目标位置的微分与四旋翼系统实际前向速度估计值z₂的差值；

侧向位移y自抗扰控制器中,e_P为过渡安排后的目标位置与四旋翼系统实际侧向位移估计值的差值；e_D为过渡安排后的目标位置的微分与四旋翼系统实际侧向速度估计值的差值；

纵向位移z自抗扰控制器中,e_P为过渡安排后的目标位置与四旋翼系统实际纵向位移估计值的差值；e_D为过渡安排后的目标位置的微分与四旋翼系统实际纵向速度估计值z₂的差值；

偏航角ψ自抗扰控制器中,e_P为过渡安排后的目标偏航角与四旋翼系统实际偏航角估计值的差值；e_D为过渡安排后的目标偏航角的微分与四旋翼系统实际偏航角变化率估计值z₂的差值；

俯仰角θ自抗扰控制器中,e_P为由x和y位置通道的自抗扰控制律解算出的目标俯仰角与实际俯仰角估计值的差值；e_D为目标俯仰角的微分与俯仰角变化率估计值z₂的差值；

滚转角φ自抗扰控制器中,e_P为由x和y位置通道的自抗扰控制律解算出的目标滚转角与实际滚转角的估计值的差值；e_D为目标滚转角的微分与滚转角变化率估计值z₂的差值。

6.根据权利要求1所述的一种四旋翼飞行器自抗扰自动飞行控制方法，其特征在于：由于四旋翼的前后两个电机逆时针安装、左右两个电机顺时针安装，因此T_f、T_b取负，T_l，T_r取正。

7.根据权利要求1所述的一种四旋翼飞行器自抗扰自动飞行控制方法，其特征在于：当采用PWM驱动电机时，U₁,U₂,U₃,U₄为PWM脉宽值，当采用电压驱动电机时，U₁,U₂,U₃,U₄为电压值。 

8.根据权利要求1所述的一种四旋翼飞行器自抗扰自动飞行控制方法，其特征在于：不仅适用于四旋翼系统，对本发明方法的公式作相应变形后，还适用于其他无人机系统。 

Images