一种基于四旋翼无人机的姿态控制的方法及其系统与流程

本发明涉及无人机技术领域，尤其涉及一种基于四旋翼无人机的姿态控制的方法及其系统。

背景技术：

四旋翼无人机飞行器具有可以垂直起降、结构简单、机动性强等特点，近年来成为研究热点，在消费类领域得到巨大发展。四旋翼无人机飞行器一般采用对称结构布局，机械结构相对简单，四个旋翼均匀分布，具有欠驱动、强耦合和高阶非线性，对无人机飞行控制系统的设计提出了高要求。飞行控制系统是由控制器硬件及软件算法两部分组成，在现在消费类无人机硬件平台基本趋于一致的前提下，高性能、运算效率的软件算法是飞行器系统稳定性、可靠性、飞行性能的重要保证，而由其动力学模型的复杂性、模型参数的不确定性和建模不精确性，对控制算法的鲁棒性、抗干扰性提出了更高要求。

现在的消费类四旋翼无人机基本采用的是传统PID控制算法及基于该算法的改进PID算法，根据无人机飞行状态及目标状态的误差反馈构建控制律，最终得到控制输出量。传统PID控制以其简单易懂的控制结构、良好的实用性而受到广泛应用。

传统PID算法的控制缺陷在于：(1)直接以e＝v-y的方式产生原始误差。控制目标v是有可能产生突变的，而对象输出y一定是连续的，用连续的缓变的变量追踪可能跳变的变量本身就是不合理的。(2)产生e的微分信号没有太好的办法。(3)线性组合不一定是最好的组合方式。(4)误差信号e的积分反馈的引入有很多负作用。

在无人机控制上主要有两点不足：

1)基于无人机系统处于平衡点附近的假设，其控制性能在飞行器状态大范围偏离平衡点后会大打折扣。

2)算法对系统模型的不确定、外界干扰的鲁棒性较差。在无人机系统模型参数出现变动和干扰时，其控制器性能会变差。需要重新调节参数、寻优最佳PID参数。

3)PID控制中，积分作用项I参数难调，微分作用项D参数难调。

基于自抗扰技术的四旋翼无人机控制技术对飞行过程中的干扰和飞行器自身状态的不确定性具有更好的鲁棒性，在整个飞行状态内控制都具有极高的控制性能。

技术实现要素：

本发明的目的在于克服现有技术的不足，本发明提供了一种基于四旋翼无人机的姿态控制的方法及其系统，在继承了传统PID控制优点，不依赖模型，只利用飞行器的误差反馈进行控制律设计，同时采用扩张状态观测器对扰动(外部干扰和模型不确定性)进行实时估计和补偿，对无人机飞行控制中的干扰和不确定性具有较好的鲁棒性，参数具有更好的适用性。

为了解决上述问题，本发明提出了一种基于四旋翼无人机的姿态控制的方法，包括如下步骤：

接收控制参考状态输入值；

采用跟踪微分器提取控制参考状态输入值的微分信号；

基于扩展状态观测器获取当前姿态调整下的扩展状态输出量；

根据扩展状态观测器获取的扩展状态输出量与微分信号构建误差，基于误差设计得到非线性反馈控制器的初始控制输出量；

采用扩展状态跟踪器中未知干扰的观测值对初始控制输出量进行扰动补偿，得到内环姿态控制器的控制输出值。

所述参考状态输入值包括：横滚状态、俯仰状态、偏航状态。

所述采用跟踪微分器提取控制参考状态输入值的微分信号包括：

采用如下约束值进行微分信号的提取：

其中：

v₁：跟踪微分器输出量之一，用来跟踪控制参考状态输入值θ_c

v₂：跟踪微分器输出量之一，用来表示v₁的变化量；

e：表示v₁与跟踪量参考输入θ_c之间的误差；

r：跟踪微分器参数之一，表示速度因子，表征过渡过程跟踪输入信号的快慢程度，与跟踪速度成正比，由过渡过程快慢及无人机系统承受能力决定；

h：跟踪微分器参数之一，表示采样步长，具有滤波效果，由控制系统的采样频率决定；

fh：离散系统最速控制函数fhan的简写。

所述基于扩展状态观测器获取当前姿态调整下的扩展状态输出量包括：

采用如下约束值进行扩展状态输出量的提取：

其中：

z₁：扩展状态观测器输出量之一，用来跟踪过渡过程输出v₁；

z₂：扩展状态观测器输出量之一，用来跟踪过渡过程输出v₂；

z₃：扩展状态观测器输出量之一，用来跟踪系统中的未知扰动；

u：扩展状态观测器输入量之一，控制系统输出值；

fe：跟观测误差e相关函数，影响观测器输出z₂的收敛性；

fe₁：跟观测误差e相关函数，影响观测器输出z₃的收敛性；

β₀₁：状态观测器参数之一，影响观测器输出z₁的收敛速度；

β₀₂：状态观测器参数之一，影响观测器输出z₂的收敛速度；

β₀₃：状态观测器参数之一，影响观测器输出z₃的收敛速度；

h：状态观测器参数之一，采样步长，由控制系统的采样频率决定；

b₀：补偿因子，跟无人机系统的时间尺度相关，为常数；

β₀₁,β₀₂,β₀₃：输出误差校正增益，由无人机控制频率来决定。

所述根据扩展状态观测器获取的扩展状态输出量与微分信号构建误差，基于误差设计得到非线性反馈控制器的初始控制输出量包括：

采用如下约束值进行初始控制输出量的提取：

其中：

e₁：非线性反馈控制器输入量之一，过渡过程输出v₁与扩展状态观测器z₁之间误差，表征的是系统控制目标角度值与实际值偏差；

e₂：非线性反馈控制器输入量之一，过渡过程输出v₂与扩展状态观测器z₂之间误差，表征的是系统角度误差值的微分量；

b₁：fal函数参数之一，跟扩展状态观测器参数一致；

b₂：fal函数参数之一，跟扩展状态观测器参数一致；

δ：fal函数参数之一，跟扩展状态观测器参数一致；

k₁：比例因子，误差量的偏差比例系数；

k₂：比例因子，微分量的反馈系数。

所述采用扩展状态跟踪器中未知干扰的观测值对初始控制输出量进行扰动补偿，得到内环姿态控制器的控制输出值包括：

采用如下约束值进行内环姿态控制器的控制输出值的提取：

其中b₀是补偿因子，跟无人机系统的时间尺度相关，为常数。

相应的，本发明还提供一种基于四旋翼无人机的姿态控制的系统，其特征在于，所述系统包括：

输入模块，用于接收控制参考状态输入值；

跟踪微分器模块，用于采用跟踪微分器提取控制参考状态输入值的微分信号；

扩展状态观测器模块，用于基于扩展状态观测器获取当前姿态调整下的扩展状态输出量；

非线性组合模块，用于根据扩展状态观测器获取的扩展状态输出量与微分信号构建误差，基于误差设计得到非线性反馈控制器的初始控制输出量；

控制输出值模块，用于采用扩展状态跟踪器中未知干扰的观测值对初始控制输出量进行扰动补偿，得到内环姿态控制器的控制输出值。

所述参考状态输入值包括：横滚状态、俯仰状态、偏航状态。

在本发明实施例中，在整个具体实施过程中，安排过渡过程，采用跟踪微分器对被控对象提取微分信号；通过跟踪参考输入信号，柔化参考输入信号的变化，减少系统输出的超调，增强鲁棒性。由非线性扩张观测器实现对未知扰动的估计；扩展状态观测器除了观测过渡过程输出值v₁,v₂外，进一步的估计了系统未知扰动，为后面对扰动的补偿提供了基础；引入扰动补偿过程。最终实现了更好的抗干扰能力及适用更大范围的参数设计。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是本发明实施例的无人机控制系统原理图；

图2是本发明实施例的基于无人机系统的控制方法原理图；

图3是本发明实施例的基于四旋翼无人机的姿态控制的方法流程图；

图4是本发明实施例的扩展状态观测器原理图；

图5是本发明实施例的基于四旋翼无人机的姿态控制的系统结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例中的基于四旋翼无人机的姿态控制的方法，其采用：接收控制参考状态输入值；采用跟踪微分器提取控制参考状态输入值的微分信号；基于扩展状态观测器获取当前姿态调整下的扩展状态输出量；根据扩展状态观测器获取的扩展状态输出量与微分信号构建误差，基于误差设计得到非线性反馈控制器的初始控制输出量；采用扩展状态跟踪器中未知干扰的观测值对初始控制输出量进行扰动补偿，得到内环姿态控制器的控制输出值。

首先，先介绍一些无人机控制原理，在无人机的控制中，有一类是设定飞行轨迹由无人机自主飞行，另一类是通过人为操作控制无人机的飞行轨迹，其中人为操控时输入的是无人机的姿态指令，无人机在姿态变化后达到位置相应变化的目的。简而言之，无人机先有姿态变化再有位置变化，先改变姿态再改变位置，基于以上特点我们优先考虑内环姿态控制环节的高效、高性能控制算法，所设计的控制算法既能够服务于无人机的自主飞行控制，也可以服务于人工操作时的姿态控制。

本方案适用于无人机的控制架构是采用内外两个独立闭环控制结构的情况，其中内环控制无人机的飞行姿态，外环控制无人机的飞行位置，外环的控制算法只需计算出要改变无人机飞行位置状态所需的姿态角和总矩，输入到内环，由内环的姿态控制算法实现飞行姿态的控制，进而实现无人机位置控制的目的。

以小型无人机为例，其控制原理图如图1中所示，在进行自主飞行控制时，输入位置控制输入量p_r(x,y,z)即无人机的期望位置(不同坐标体系下，含义可能不同，如经纬度x、y，高度z)，结合位置数据信息反馈x_m,y_m,z_m(无人机的真实位置)一起输入外环控制器，通过外环控制(外环控制一般也设计为两环的控制框架，但不是此处重点，可以简化修改)得到下一级的控制量δ_col(油门控制量)，φ_r(横滚角度Roll期望值)，θ_r(俯仰角Pitch期望值)，(偏航角Yaw期望值)。其中，φ_r，θ_r，作为内环控制器的控制量输入。在由人工操控时，直接由操控手输入控制量δ_col，φ_r，θ_r，

控制输入φ_r，θ_r，结合姿态数据信息反馈φ，θ，(无人机的真实姿态)经过内环控制器后，得到下一级控制量δ_lat，δ_lon，δ_yaw，这部分是本方案研究创新的重点部分。提出了具有高效、鲁棒性的内环姿态控制算法。

将内环控制器输出δ_lat,δ_col,δ_ped，结合δ_col一起输入到下一级，通过距分配及限幅(或者是解耦混控并限幅)后得到了每个电机的控制量，在四旋翼无人机中控制四个电机转速的快慢，从而引起四个电机升力发生变化，使得飞机姿态发生相应变化，进一步的使得飞机的位置发生相应变化。

本发明实施过程中，基于自抗扰技术的内环姿态控制器的设计方法，该方法设计控制框架相同、控制参数差异的内环姿态控制器，对无人机姿态的Roll(横滚φ)、Pitch(俯仰θ)、Yaw(偏航))控制，变成了单输入单输出系统。期望角度及真实测量角度作为输入，经过该控制器后得到相应的控制量，互相之间独立，分别计算得到相应的控制量，以pitch俯仰角度的控制为例，θ_c为参考输入，控制流程框图如图2中所示，roll及yaw角控制流程相同，只是参数表示不同，具体基于四旋翼无人机的姿态控制的方法流程图如图3，结合图2详细说明如下：

S31、接收控制参考状态输入值；

控制算法的输入v为参考输入θ_c(此处，内环姿态pitch角目标值)，控制输出为u。本发明实施例子中以pitch俯仰角度的控制为例，θ_c为控制参考输入，roll及yaw角控制流程相同，只是参数表示不同。

S32、采用跟踪微分器提取控制参考状态输入值的微分信号；

利用跟踪微分器(TD，Tracking Differentiator)，对参考输入θ_c(内环姿态pitch角目标值)安排过渡过程并提取其微分信号，过渡过程保障对θ_c的跟踪输出结果没有超调。

跟踪微分器设计过程如下，以v₁跟踪θ_c，v₂表示v₁的变化量，跟踪微分器约束条件如下：

其中：

v₁：跟踪微分器输出量之一，用来跟踪参考输入θ_c；

v₂：跟踪微分器输出量之一，用来表示v₁的变化量；

e：表示v₁与跟踪量参考输入θ_c之间的误差；

r：跟踪微分器参数之一。速度因子，表征过渡过程跟踪输入信号的快慢程度，与跟踪速度成正比，由过渡过程快慢及无人机系统承受能力决定。

h：跟踪微分器参数之一。采样步长，具有滤波效果。由控制系统的采样频率决定；

fh：离散系统最速控制函数fhan的简写，详细说明如下。

无人机系统在实际控制处理时是一个高频率的离散控制系统，按一定的采样频率，每隔相应的采样周期，更新整个系统的状态量。基于离散系统的特性，采用递推的方法，跟踪微分器的实现原理简单说明如下：

v1＝v1+hv2，v2是v1的变化量，h是采样步长，v1下一采样时间的值就是当前采用值加上采样步长乘以变化量。类似原理V2＝v2+hfh得到v2的值。

函数fhan是离散系统最速控制函数，根据离散系统控制理论，其构造如下：

fhan＝-r(a/d)fsg(a,d)-r·sign(a)[1-fsg(a,d)]

其中，fhan里面的部分参数由下列式子构造得出：

fsg函数是跟符号相关函数，表达如下：

fsg(x,d)＝[sign(x+d)-sign(x-d)]/2

本步骤中，得到了跟踪微分器(TD)的输出v₁，v₂，选择合适的参数可以使得出v₁，v₂，快速跟踪上参考输入θ_c，达到稳定值。

S33、基于扩展状态观测器获取当前姿态调整下的扩展状态输出量；

系统在运行过程中与外界环境进行信息交流，其中系统的外部变量就是系统传给外部的输出变量，包括部分状态变量信息和外部给系统的输入量。可以根据外部变量来观测确定系统内部状态变量的装置就是状态观测器。

无人机控制系统中的扩展状态观测器(ESO，Extended State Observer)，在这里以四旋翼无人机的姿态输出(pitch角度值)θ值和控制输入u作为观测器的输入来设计用来估计四旋翼无人机的系统状态的观测器，扩展观测器的输出量为z1,z₂,z₃，实现了用z₁用来追踪跟踪微分器输出结果v₁，z₂用来追踪跟踪微分器输出结果v₂，z₃用来跟踪四旋翼无人机系统的未知扰动。整个扩展状态观测器原理图如图4中所示，根据离散系统性质，结合其状态观测器设计框架及无人机控制系统实际情况，约束扩展状态观测器条件如下：

其中：

z₁：状态观测器输出量之一，用来跟踪过渡过程输出v₁；

z₂：状态观测器输出量之一，用来跟踪过渡过程输出v₂；

z₃：状态观测器输出量之一，用来跟踪系统中的未知扰动；

u：状态观测器输入量之一，控制系统输出；

fe：跟观测误差e相关函数，影响观测器输出z₂的收敛性；

fe₁：跟观测误差e相关函数，影响观测器输出z₃的收敛性；

β₀₁：状态观测器参数之一，影响观测器输出z₁的收敛速度；

β₀₂：状态观测器参数之一，影响观测器输出z₂的收敛速度；

β₀₃：状态观测器参数之一，影响观测器输出z₃的收敛速度；

h：状态观测器参数之一。采样步长，由控制系统的采样频率决定；

b₀：补偿因子，跟无人机系统的时间尺度相关，为常数；

β₀₁,β₀₂,β₀₃：输出误差校正增益，由无人机控制频率来决定。

fe、fe₁是跟观测误差量e相关的函数，表达式如下：

其中，fal函数表达式如下：

fal：fal函数是构造设计出来具有一定特性的函数，后面说明

e：fal函数的输入量；

b：fal函数参数之一，影响滤波速度；

δ：fal函数参数之一，影响滤波效果。

fal函数具有变增益的数学拟合作用及快速收敛特性，根据不同的误差e分别输出不同的结果，最终能使误差e在有限时间内快速收敛到0，具有较好的滤波效果。

具体实施过程中，调节fal函数中的参数a,δ可以得到良好的滤波效果。其中，δ影响滤波效果，增大δ滤波效果加强，但会延长跟踪输入信号的响应时间，一般取5T<δ<10T,其中T是采样时间。b是0～1之间常数，b越小跟踪越快，但会导致滤波效果变差。

具体实施过程汇总，误差校正增益β₀₁,β₀₂,β₀₃与积分步长近似满足以下关系时：

扩展状态观测器具有很高的估计效率，跟踪性能良好。

扩展状态观测器(ESO)是自抗扰设计ADRC的核心，采用双通道补偿的方法改造对象模型，将非线性、不确定的系统近似线性化和确定性化。利用扩张状态观测器(ESO)对系统对象进行估计，不仅能得到各个状态变量的估计，而且能得到对象方程右端估计，即扰动估计。在通过跟踪微分器完成过渡过程，并且用扩展状态观测器对系统状态量和不确定性完成估计之后，可以结合跟踪微分器的输出和扩展状态观测器的输出，设计非线性反馈控制器并进行扰动补偿。

S34、根据扩展状态观测器获取的扩展状态输出量与微分信号构建误差；

S35、基于误差设计得到非线性反馈控制器的初始控制输出量；

步骤S34至S35接下来将过渡过程输出v₁,v₂与扩展状态观测器输出z₁,z₂构建误差，基于误差反馈设计原理，设计得到非线性反馈控制器(NF，Nonlinear Feedback)。

其中：

e₁：非线性反馈控制器输入量之一，过渡过程输出v₁与扩展状态观测器z₁之间误差，表征的是系统控制目标角度值与实际值偏差；

e₂：非线性反馈控制器输入量之一，过渡过程输出v₂与扩展状态观测器z₂之间误差，表征的是系统角度误差值的微分量(变化)；

b₁：fal函数参数之一，跟扩展状态观测器参数一致；

b₂：fal函数参数之一，跟扩展状态观测器参数一致；

δ：fal函数参数之一，跟扩展状态观测器参数一致；

k₁：比例因子，误差量的偏差比例系数；

k₂：比例因子，微分量的反馈系数；

u₀：初始控制输出量。

类似于传统PID参数，需整定到合适的系数得到良好的控制效果。非线性反馈控制器通过调节这两个参数可以兼顾无人机系统反应的快速性和超调性两项指标，另外对消除静差不在依赖于积分器(I)，避免了积分反馈给系统带来的负面影响(对比PID控制)。

S306、采用扩展状态跟踪器中未知干扰的观测值对初始控制输出量进行扰动补偿，得到内环姿态控制器的控制输出值。

最后对扩展状态跟踪器(ESO)中未知干扰的观测值z₃进行补偿，得到最终内环姿态控制器的输出u的表达式。

其中b₀是补偿因子，跟无人机系统的时间尺度相关，为常数。

综上，进过以上步骤实现了基于自抗扰的角度控制器设计(单轴)。综上，控制算法的输入v(就是参考输入θ_c，此处，内环姿态pitch角目标值)，经过ADRC自抗扰算法，最终得到控制输出为u。以俯仰pitch角为例，即由期望的俯仰角度v＝θ_r输入内环自抗扰控制器，最终得到了控制量u＝δ_lon。其他角度可以采用类似过程推导得到。然后按第一个框图的控制流程继续，通过解耦混控，最终得到每个电机(执行机构)的输出。

图5示出了本发明实施例中的基于四旋翼无人机的姿态控制的系统结构示意图，包括如下：

输入模块，用于接收控制参考状态输入值；

跟踪微分器模块，用于采用跟踪微分器提取控制参考状态输入值的微分信号；

扩展状态观测器模块，用于基于扩展状态观测器获取当前姿态调整下的扩展状态输出量；

非线性组合模块，用于根据扩展状态观测器获取的扩展状态输出量与微分信号构建误差，基于误差设计得到非线性反馈控制器的初始控制输出量；

控制输出值模块，用于采用扩展状态跟踪器中未知干扰的观测值对初始控制输出量进行扰动补偿，得到内环姿态控制器的控制输出值。

在具体实施过程中，该参考状态输入值包括：横滚状态、俯仰状态、偏航状态。在整个具体实施过程中，安排过渡过程，采用跟踪微分器对被控对象提取微分信号；通过跟踪参考输入信号，柔化参考输入信号的变化，减少系统输出的超调，增强鲁棒性。由非线性扩张观测器实现对未知扰动的估计；扩展状态观测器除了观测过渡过程输出值外，进一步的估计了系统未知扰动，为后面对扰动的补偿提供了基础；引入扰动补偿过程。最终实现了更好的抗干扰能力及适用更大范围的参数设计。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁盘或光盘等。

另外，以上对本发明实施例所提供的基于四旋翼无人机的姿态控制的方法及其系统进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。