一种无人直升机自抗扰飞行速度和姿态控制方法与流程

本发明涉及无人机控制领域，具体的说是一种无人直升机自抗扰飞行速度和姿态控制方法。

背景技术：

无人直升机飞行控制系统主要由两部分组成，第一部分为速度和姿态控制系统，主要负责增稳，使无人直升机在外界条件不改变的情况下处于稳定状态，是飞行器能够自主飞行的基础，第二部分为位置控制系统，它的作用是控制飞行器的飞行路径，使其能够按照用户给定的航线或者航点进行飞行。

传统直升机飞控的姿态控制和速度控制一般是多使用经典控制理论作为理论依据，pid控制器作为其控制器来进行控制的。pid是一种无模型控制器，它可以只根据被控变量的误差来进行控制，所以在设计基于pid控制器的控制系统时，可以将被控对象看作一个只有输入、输出的黑盒。正是因为pid控制技术是立足于使用误差来控制误差，不需要复杂的数学建模过程，它才能在实际的工程应用当中占有统治地位。但是，由于pid控制技术是一个较早出现的控制技术，因此它受到研发时代的理论水平、技术水平落后的影响，设计出pid时并没有先进的数字计算机、以及使用数字计算机完成的各种数字信号处理技术，当时的pid控制技术带着许多现在看来“不尽人意的缺陷”进入了控制工程中，出色地完成了大部分的控制任务，从而在控制工程这一领域占有了的及其重要的地位。

pid控制技术的如下四个方面的缺陷：通过e(t)＝υ(t)-y(t)这样直接相减的方式产生的原始误差作为控制器的输入的控制效果并不理想；微分环节的输入没有良好的算法来计算；线性组合不一定是最好的组合方式；误差信号e(t)的积分—的引入有很多负作用。显然，上述四个方面的缺陷都是在pid出现那个年代的大环境下所产生的问题。在现如今数字信号处理发达的条件下，我们就有了更多的手段来弥补这些缺陷。贯彻pid控制技术的精髓——误差来减少误差的过程控制思想，利用现代先进的数字信号处理技术，并合理开发特殊的非线性效应来探讨克服上述pid控制技术的四个方面缺陷的过程中，催生了新型实用数字控制技术——自抗扰控制技术。

技术实现要素：

针对现有技术中存在的上述不足之处，本发明要解决的技术问题是提供一种无人直升机自抗扰飞行速度和姿态控制方法。

本发明为实现上述目的所采用的技术方案是：一种无人直升机自抗扰飞行速度和姿态控制方法，包括横向通道控制、纵向通道控制、高度通道控制、偏航通道控制，每个通道控制过程均包括以下步骤：

安排过渡阶段，将控制输入ν平滑化得到平滑的控制输入ν1；

根据第一级adrc的扩张状态观测器，得到观测状态量和扩张状态量；

根据平滑的控制输入ν1、观测状态量和所述扩张状态量，得到第一级自抗扰控制器的输出；

扰动补偿阶段，分别得到各个通道的控制器输出。

所述横向通道控制的控制器输出为δlat＝u03+u04，u03为横向通道姿态串级自抗扰控制的控制器输出，u04为横向通道速度串级自抗扰控制的控制器输出。

所述横向通道姿态串级自抗扰控制包括以下步骤：

安排过渡阶段，ν1＝ν1-hr0fal(ν1-ν′,0.5,h)，其中ν′＝ν+trimy(νb)，trimy为根据飞行经验得出的速度配平量映射关系，ub为y轴速度，h为控制周期，fal()为fal函数；

根据第一级adrc的扩张状态观测器，得到观测状态量z11＝z11+h(z12-c1e+u1)和扩张状态量z12＝z12+h(-d1fe)，其中，e＝z11-φ，fe＝fal(e,0.5,h)，φ为滚转角，u1为第一级自抗扰控制器的输出，z11和z12的初始值为0；

根据平滑的控制输入υ1、观测状态量和所述扩张状态量，得到第一级自抗扰控制器的输出u1＝β1fal(e1,α1,δ1)-z12，其中e1＝ν1-z11；

横向通道姿态串级自抗扰控制的控制器输出为u03＝tan^-1(b0u)，其中u＝β2fal(e2,α2,δ2)-z22，e2＝u1-z21，z22＝0.9z22+h(-d2fe)，fe＝fal(e,0.5,h)，e＝z21-p，p为x轴角速度，z21＝z21+h(z22-c2e+u)，z11和z12的初始值为0；

所述c1、d1、d2为扩张状态观测器的增益，需要根据实际系统调整；r0、β1、α1、δ1、β2、α2、δ2、b0为与无人直升机型号相关的控制器参数。

所述横向通道速度串级自抗扰控制包括以下步骤：

安排过渡阶段，ν1＝ν1-hr0fal(ν1-ν,0.5,h)，其中ν为控制输入，h为控制周期，fal()为fal函数；

根据第一级adrc的扩张状态观测器，得到观测状态量z11＝z11+h(z12-c1e+u1)和扩张状态量z12＝z12+h(-d1fe)，其中，e1＝ν1-z11，fe＝fal(e,0.5,h)，u1为第一级自抗扰控制器的输出，z11和z12的初始值为0；

根据平滑的控制输入υ1、观测状态量和所述扩张状态量，得到第一级自抗扰控制器的输出u1＝β1fal(e1,α1,δ1)-z12，其中e1＝ν1-z11；

横向通道速度串级自抗扰控制的控制器输出为u04＝tan^-1(b0u)，其中u＝β2fal(e2,α2,δ2)-z22，e2＝u1-z21z22＝0.9z22+h(-d2fe)，fe＝fal(e,0.5,h)，e＝z21-ay，ay为y轴方向加速度，z21＝z21+h(z22-c2e+u)，z11和z12的初始值为0；

所述c1、d1、d2为扩张状态观测器的增益，需要根据实际系统调整；r0、β1、α1、δ1、β2、α2、δ2、b0为与无人直升机型号相关的控制器参数。

所述纵向通道控制的控制器输出为δlon＝u01+u02，u01为纵向通道姿态串级自抗扰控制的控制器输出，u02为纵向通道速度串级自抗扰控制的控制器输出。

所述纵向通道姿态串级自抗扰控制包括以下步骤：

安排过渡阶段，ν1＝ν1-hr0fal(ν1-ν′,0.5,h)，其中ν′＝ν+trimx(ub)，trimx为根据飞行经验得出的速度配平量映射关系，ub为x轴速度，h为控制周期，fal()为fal函数；

根据第一级adrc的扩张状态观测器，得到观测状态量z11＝z11+h(z12-c1e+u1)和扩张状态量z12＝z12+h(-d1fe)，其中，e＝z11-θ，fe＝fal(e,0.5,h)，θ为俯仰角，u1为第一级自抗扰控制器的输出，z11和z12的初始值为0；

根据平滑的控制输入ν1、观测状态量和所述扩张状态量，得到第一级自抗扰控制器的输出u1＝β1fal(e1,α1,δ1)-z12，其中e1＝ν1-z11；

纵向通道姿态串级自抗扰控制的控制器输出为u01＝tan^-1(b0u)，其中u＝β2fal(e2,α2,δ2)-z22，e2＝u1-z21，z22＝0.9z22+h(-d2fe)，fe＝fal(e,0.5,h)，e＝z21-q，q为y轴角速度，z21＝z21+h(z22-c2e+u)，z11和z12的初始值为0；

所述c1、d1、d2为扩张状态观测器的增益，需要根据实际系统调整；r0、β1、α1、δ1、β2、α2、δ2、b0为与无人直升机型号相关的控制器参数。

所述纵向通道速度串级自抗扰控制包括以下步骤：

安排过渡阶段，ν1＝ν1-hr0fal(ν1-ν,0.5,h)，其中ν为控制输入，h为控制周期，fal()为fal函数；

根据第一级adrc的扩张状态观测器，得到观测状态量z11＝z11+h(z12-c1e+u1)和扩张状态量z12＝z12+h(-d1fe)，其中，e1＝υ1-z11，fe＝fal(e,0.5,h)，u1为第一级自抗扰控制器的输出，z11和z12的初始值为0；

根据平滑的控制输入υ1、观测状态量和所述扩张状态量，得到第一级自抗扰控制器的输出u1＝β1fal(e1,α1,δ1)-z12，其中e1＝υ1-z11；

横向通道速度串级自抗扰控制的控制器输出为u02＝tan^-1(b0u)，其中u＝β2fal(e2,α2,δ2)-z22，e2＝u1-z21，z22＝0.9z22+h(-d2fe)，fe＝fal(e,0.5,h)，e＝z21-ax，ax为x轴加速度，z21＝z21+h(z22-c2e+u)，z11和z12的初始值为0；

所述c1、d1、d2为扩张状态观测器的增益，需要根据实际系统调整；r0、β1、α1、δ1、β2、α2、δ2、b0为与无人直升机型号相关的控制器参数。

所述高度通道控制包括以下步骤：

安排过渡阶段，ν1＝ν1-hr0fal(ν1-ν,0.5,h)，其中ν表示控制输入，h为控制周期，fal()为fal函数；

根据第一级adrc的扩张状态观测器，得到观测状态量z11＝z11+h(z12-c1e+u1)和扩张状态量z12＝z12+h(-d1fe)，其中，e＝z11-ωb，fe＝fal(e,0.5,h)，ωb为z轴速度，u1为第一级自抗扰控制器的输出，z11和z12的初始值为0；

根据平滑的控制输入υ1、观测状态量和所述扩张状态量，得到第一级自抗扰控制器的输出u1＝β1fal(e1,α1,δ1)-z12，其中e1＝υ1-z11；

高度通道控制的控制器输出为δcollmr＝b0u，其中u＝β2fal(e2,α2,δ2)-z22，e2＝u1-z21，z22＝0.9z22+h(-d2fe)，fe＝fal(e,0.5,h)，e＝z21-az，az为z轴加速度，z21＝z21+h(z22-c2e+u)，z11和z12的初始值为0；

所述c1、d1、d2为扩张状态观测器的增益，需要根据实际系统调整；r0、β1、α1、δ1、β2、α2、δ2、b0为与无人直升机型号相关的控制器参数。

所述偏航通道控制包括以下步骤：

安排过渡阶段，ν1＝ν1-hr0fal(ν1-(ν-ψ0),0.5,h)，其中ν为控制器输入，h为控制周期，fal()为fal函数；

根据第一级adrc的扩张状态观测器，得到观测状态量z11＝z11+h(z12-c1e+u1)和扩张状态量z12＝z12+h(-d1fe)，其中，e＝z11-(ψ-ψ0)，fe＝fal(e,0.5,h)，ψ为偏航角，ψ0为当前的偏航角，u1为第一级自抗扰控制器的输出，z11和z12的初始值为0；

根据平滑的控制输入υ1、观测状态量和所述扩张状态量，得到第一级自抗扰控制器的输出u1＝β1fal(e1,α1,δ1)-z12，其中e1＝ν1-z11；

高度通道控制的控制器输出为δcolltr＝b0u，其中u＝β2fal(e2,α2,δ2)-z22，e2＝u1-z21，z22＝0.9z22+h(-d2fe)，fe＝fal(e,0.5,h)，e＝z21-r，r为x轴角速度，z21＝z21+h(z22-c2e+u)，z11和z12的初始值为0；

所述c1、d1、d2为扩张状态观测器的增益，需要根据实际系统调整；r0、β1、α1、δ1、β2、α2、δ2、b0为与无人直升机型号相关的控制器参数。

本发明各通道间的耦合则被视为扰动，自抗扰控制器可通过扩张状态观测器的扰动估计和扰动补偿，将其耦合抵消。该发明是一种无模型控制器，它不需要被控对象的精确模型，而是依靠误差来控制。

附图说明

图1为一阶自抗扰控制器的结构图；

图2为小型无人直升机控制系统通道示意框图；

图3为纵向通道自抗扰控制器结构图；

图4位纵向通道姿态串级自抗扰控制器结构图；

图5为纵向通道速度串级自抗扰控制器结构图；

图6为高度通道自抗扰控制器结构图；

图7为偏航通道自抗扰控制器结构图；

图8为自抗扰控制器仿真结果图；

图9为pid控制器仿真结果图；

图10为自抗扰控制器的输出图；

图11为pid控制器的输出图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

如图1所示，一种无人直升机自抗扰飞行速度和姿态控制方法，包括

各通道间的耦合则被视为扰动，自抗扰控制器可通过扩张状态观测器的扰动估计和扰动补偿，将其耦合抵消。从并联角度来看，小型无人直升机控制系统通道示意框图如图2所示。

抗扰控制器需要多次利用到fal函数，fal函数的定义如下：

扩张状态观测器是自抗扰算法的最重要组成部分，它将一切实际系统与已建立模型存在的差异看作一个干扰，通过观测这个总扰动值并补偿它来实现，消除稳态误差，抵消耦合，抵抗外扰等功能，也因此，自抗扰控制器一般不需要积分环节来参与控制。一个通用的一阶扩张状态观测器算法为

fe＝fal(e,0.5,h)

z1＝z1+h(z2-c1e+u1)

z2＝z2+h(-d1fe)

其中e为误差，z1为观测的状态量，z2为扩张状态量，也就是系统的总扰动观测量，从上述的这些自抗扰控制器算法中可以看出，扩张状态观测器的观测扰动z2的算法与传统pid中的离散积分算法类似，因此，它与一般的离散积分算法存在同样一个问题，就是会使得其算法延迟增加，容易造成系统整体的震荡。因此扩张状态观测器也可以与普通的积分环节采用相同的改进方法，即在累加过程中乘以一个小于1的常数λ，可将上述算法改写为

fe＝fal(e,0.5,h)

z1＝z1+h(z2-c1e+u1)

z2＝λz2+h(-d1fe)

这样能够使得距离当前时间越远的观测扰动，对当前输出量的作用越小，从而减小离散化过程中造成的误差累积，防止因误差累积产生的震荡。横向通道和纵向通道：

横向通道和纵向通道是稳定小型无人直升机和操纵其前飞最重要的两个通道。由于横向通道和纵向通道的工作方式类似，因此首先分析纵向通道如何设计，然后根据纵向通道的设计，用同样的方法可得出横向通道的设计。设计控制器的主要依据是有人直升机的操纵方式，这里首先分析两个飞行模态人工操纵的特点：

人工操纵悬停时，需要保持机体姿态稳定，飞行器的俯仰和滚转角处于0°附近，横纵向的速度目标为0。

人工操纵前飞时，首先需要让主旋翼拉力向前倾斜，此时直升机机体也会前倾，并且向前加速，达到预期速度后让拉力回收，让小型无人直升机处于一个稳定的姿态。

根据以上的分析可以得出纵向通道的两个控制子通道，纵向控制通道系统框图如图3所示。x轴、y轴均是在机体坐标系下。

俯仰角通道。控制各个姿态角的最主要原因是为了飞行器的稳定，当飞行器的俯仰角与0°有很大差距时，飞行器所受重力的各个分力与飞行器姿态角关系密切，在姿态角较小的时候，为了抵消重力的因素，主旋翼的推力可以较小，而当姿态角较大时，为了抵消重力，主旋翼推力会变得很大，因此保持飞行姿态稳定在较小的角度附近很重要。对于纵向通道来说，需要控制的姿态角就是俯仰角。对于实际的小型无人直升机来说，其角速度和姿态角可用传感器测量得出，因此可采用串级自抗扰来进行设计，姿态串级自抗扰控制器如图4所示。

速度通道。控制速度就是为了使小型无人直升机能够按照指定的速度飞行，从而实现小型无人直升机的悬停和前飞等模态。因此速度的控制是小型无人直升机实现自动飞行的基础。对于小型无人直升机来说，加速度、速度可由传感器测量得出，因此也速度控制器采用串级自抗扰进行设计。速度串级自抗扰控制器如图5所示。

纵向通道姿态串级自抗扰算法如下

首先是安排过渡过程：

ν′＝ν+trimx(ub)

其中，ν为控制输入，trimx为根据飞行经验得出的速度配平量映射关系，ub为x轴速度。

ν1＝ν1-hr0fal(ν1-ν′,0.5,h)

接下来是第一级adrc的扩张状态观测器，θ为俯仰角

e＝z11-θ

fe＝fal(e,0.5,h)

z11＝z11+h(z12-c1e+u1)

z12＝z12+h(-d1fe)

下面是第一级自抗扰控制器的输出

e1＝ν1-z11

u1＝β1fal(e1,α1,δ1)-z12

最后是第二级自抗扰控制器，q为y轴角速度

e＝z21-q

fe＝fal(e,0.5,h)

z21＝z21+h(z22-c2e+u)

z22＝0.9z22+h(-d2fe)

e2＝u1-z21

u＝β2fal(e2,α2,δ2)-z22

控制器输出

u01＝tan^-1(b0u)

上述算法需一直迭代计算，其中h为控制器的算法运行间隔，即控制周期，c1、d1、d2为扩张状态观测器的增益，需要根据实际系统调整。控制器的参数为r0、β1、α1、δ1、β2、α2、δ2、b0。主要需调节的参数为r0、β1、β2。

纵向通道速度串级自抗扰控制器如下，由于自抗扰控制器参数众多，不再为每个自抗扰控制器的参数单独命名，下列所说的速度自抗扰控制器参数与姿态自抗扰控制器的参数相互独立。

ν1＝ν1-hr0fal(ν1-ν,0.5,h)

e＝z11-ub

fe＝fal(e,0.5,h)

z11＝z11+h(z12-c1e+u1)

z12＝z12+h(-d1fe)

e1＝ν1-z11

u1＝β1fal(e1,α1,δ1)-z12

e＝z21-αx

fe＝fal(e,0.5,h)

z21＝z21+h(z22-c2e+u)

z22＝0.9z22+h(-d2fe)

e2＝u1-z21

u＝β2fal(e2,α2,δ2)-z22

u02＝tan^-1(b0u)

其中，ν为控制器输入，u02为控制器输出，αx为x轴加速度，ub为x轴速度，需要调整的参数同样为r0、β1、α1、δ1、β2、α2、δ2、b0。

无人直升机的纵向输入

δlon＝u01+u02

横向通道的控制器与纵向通道结构是相通的，由于横向通道与纵向通道相比来说较为敏感，所以其参数往往比纵向通道的控制器参数小许多。横向通道的算法如下，首先为横向通道姿态控制器的算法

ν′＝ν+trimy(νb)

其中，trimy为根据飞行经验得出的速度配平量映射关系，νb为y轴速度

ν1＝ν1-hr0fal(ν1-ν′,0.5,h)

e＝z11-φ

其中，φ为滚转角

fe＝fal(e,0.5,h)

z11＝z11+h(z12-c1e+u1)

z12＝z12+h(-d1fe)

e1＝ν1-z11

u1＝β1fal(e1,α1,δ1)-z12

e＝z21-p

其中，p为x轴角速度

fe＝fal(e,0.5,h)

z21＝z21+h(z22-c2e+u)

z22＝0.9z22+h(-d2fe)

e2＝u1-z21

u＝β2fal(e2,α2,δ2)-z22

控制器输出

u03＝tan^-1(b0u)

接下来为横向通道速度串级自抗扰控制器

ν1＝ν1-hr0fal(ν1-ν,0.5,h)

e＝z11-υb

fe＝fal(e,0.5,h)

z11＝z11+h(z12-c1e+u1)

z12＝z12+h(-d1fe)

e1＝ν1-z11

u1＝β1fal(e1,α1,δ1)-z12

e＝z21-αy

fe＝fal(e,0.5,h)

z21＝z21+h(z22-c2e+u)

z22＝0.9z22+h(-d2fe)

e2＝u1-z21

u＝β2fal(e2,α2,δ2)-z22

u04＝tan^-1(b0u)

与纵向通道相同需要调整的参数为两组自抗扰控制器的r0、β1、α1、δ1、β2、α2、δ2、b0。

无人直升机的横向输入

δlat＝u03+u04

高度通道指的是负责控制小型无人直升机主旋翼推力的通道，该通道虽然叫做高度通道，但它主要的状态变量均为机体坐标系z轴，而非地面坐标系的z轴，一般处于正常飞行状态时，小型无人直升机的推力影响最大的还是其升降，所以将该通道取名为高度通道。

高度通道与飞行器的姿态角无关，因此只有一个串级自抗扰控制器，高度通道的自抗扰控制器结构如图6所示。

高度通道的自抗扰控制器如下

ν1＝ν1-hr0fal(ν1-ν,0.5,h)

e＝z11-ωb

fe＝fal(e,0.5,h)

z11＝z11+h(z12-c1e+u1)

z12＝z12+h(-d1fe)

e1＝ν1-z11

u1＝β1fal(e1,α1,δ1)-z12

e＝z21-αz

fe＝fal(e,0.5,h)

z21＝z21+h(z22-c2e+u)

z22＝0.9z22+h(-d2fe)

e2＝u1-z21

u＝β2fal(e2,α2,δ2)-z22

δcollmr＝b0u

偏航通道是控制飞行器按照预定航线自动飞行的关键通道，它主要控制飞行器的偏航角。图7为偏航通道自抗扰控制器结构，由于偏航角可能取到圆周上任何一个角度，因此对于偏航角，需要做角度的换算处理，来保证偏航角能够以最短的旋转路径转动得到。在执行自抗扰算法之前，应先记录当前的偏航角，记为ψ0，并对目标偏航角ν重复执行如下算法，直到-180°＜υ-ψ0≤180°。

偏航通道的自抗扰控制器算法如下

ν1＝ν1-hr0fal(ν1-(ν-ψ0),0.5,h)

e＝z11-(ψ-ψ0)

其中，ψ为偏航角

fe＝fal(e,0.5,h)

z11＝z11+h(z12-c1e+u1)

z12＝z12+h(-d1fe)

e1＝ν1-z11

u1＝β1fal(e1,α1,δ1)-z12

e＝z21-r

其中，r为x轴角速度

fe＝fal(e,0.5,h)

z21＝z21+h(z22-c2e+u)

z22＝0.9z22+h(-d2fe)

e2＝u1-z21

u＝β2fal(e2,α2,δ2)-z22

控制器输出

δcolltr＝b0u

上述公式中，r0、β1、α1、δ1、β2、α2、δ2、b0为与无人直升机型号相关的控制器参数，应用本发明算法的一个仿真实施例中，r0取1，β1和β2分别取为0.5和50，α1、δ1、α2、δ2、b0分别为1、0.001、1、0.01、1。

为了能够更清晰的体现自抗扰控制器的优势，使用最简单的二阶积分模型来对比自抗扰控制器和pid控制器的控制效果，如图8-11所示。