CN111552305B - 一种无人机姿态控制方法、装置及设备 - Google Patents

Abstract

本发明公开了一种一种无人机姿态控制方法，其特征在于，包括如下步骤：S1，获取无人机姿态；S2，根据自抗扰串级PID控制器获取无人机新的姿态；其中所述自抗扰串级PID控制器包括：一外环PID控制器；一外环扩展状态观测器，一内环PID控制器；一内环扩展状态观测器；S3，根据所述新的姿态控制无人机。通过改进扩展状态观测器，降低了干扰对观测性能的影响，从而可以减少无人机受到未知风场的干扰，提高了无人机姿态的控制精度，增加了抗扰能力，减少控制误差。

Description

一种无人机姿态控制方法、装置及设备

技术领域

本发明涉及一种自动化技术领域，具体来说，涉及一种无人机姿态控制方法。

背景技术

PID控制是根据反馈量与期望值的误差来消除系统误差的一种控制策略。不依赖于系统的模型，算法简单，参数易于整定，鲁棒性强，采用实际反馈与期望值的误差作为控制策略，通过比例、积分、微分环节的线性加权得到控制量，在各个领域被广泛应用。

针对PID控制的问题，韩京清(“自干扰控制技术”，前沿前沿科学，2007年第1期)改进PID的缺陷，结合现代控制理论的思想，形成了自抗扰控制技术。自抗扰控制，将系统的不确定因素和外部环境造成的扰动进行实时的估计，将所有扰动统称为“总扰动”，将非线性、不确定的对象线性化、确定化，使得系统转化为简单的“积分串联型”线性系统。

在四旋翼无人机上也广泛使用PID技术来实现无人机的姿态的估计，但是，四旋翼无人机飞行时，往往会受到未知风场的干扰，传统的PID控制无法对此未知扰动进行估算和补偿，所以无法有效的进行克服，抗干扰能力较弱。

文献1：“自干扰控制技术”，韩京清，前沿前沿科学，2007年第1期

发明内容

针对相关技术中的上述技术问题，本发明提出一种无人机姿态控制方法，能够提高控制精度，增加系统抗扰能力，减少控制误差。

为实现上述技术目的，本发明的技术方案是这样实现的：

一种无人机姿态控制方法，其特征在于，包括如下步骤：

S1，获取无人机姿态；

S2，根据自抗扰串级PID控制器获取无人机新的姿态；

其中所述自抗扰串级PID控制器包括：

一外环PID控制器，外环PID控制器为：

其中，K_p2，K_i2，K_d2为外环PID控制器的比例、积分、微分系数，b₂为外环控制器增益，r₁为期望输入r与输出姿态的状态估计量z₂₁之间的系统误差；

一外环扩展状态观测器，其表达式为：

z₂₁为外环扩展观测器对于所述外环PID控制器输出y的状态观测量，z₂₂为外环PID控制器输出y的微分观测量，通道之间的耦合作为外环PID控制器扰动，把所述扰动扩张成一个新的变量，其观测量为z₃₃；

一内环PID控制器，其中内环PID控制器为

其中，K_p1，K_i1，K_d1为内环PID控制器的比例、积分、微分系数，b₁为内环PID控制器增益，r₃为内环PID控制器输出

的状态估计量z₁₁与外环PID控制器输出控制量r₂，r₂＝u₂₂之间的误差值；

一内环扩展状态观测器，其表达式为：

其中：

S3，根据所述新的姿态控制无人机。

优选地，姿态至少包括横向或纵向或偏航中的一个或多个。

优选地，β₁₁＝3h,β₁₂＝3h²,β₁₃＝h³，其中h为系统的带宽。

优选地，K_p1＝29,K_i1＝1.8,K_d1＝1,b₁＝0.4。

优选的，β₂₁＝15,β₂₂＝75,β₂₃＝125,α₂₁＝0.5,α₂₂＝0.25,δ＝0.01。

优选的，K_p2＝1.2,K_i2＝0.1,K_d2＝0.12,b₂＝0.5。

为实现本发明的目的，还提供了一种控制无人机的装置，其包括如下单元：

获取单元：用于获取无人机姿态；

新姿态获取单元，用于根据自抗扰串级PID控制器获取无人机新的姿态；

其中所述自抗扰串级PID控制器包括：

一外环PID控制器，外环PID控制器为：

其中，K_p2，K_i2，K_d2为外环PID控制器的比例、积分、微分系数，b₂为外环控制器增益，r₁为期望输入r与输出姿态的状态估计量z₂₁之间的系统误差；

一外环扩展状态观测器，其表达式为：

z₂₁为外环扩展观测器对于所述外环PID控制器输出y的状态观测量，z₂₂为外环PID控制器输出y的微分观测量，通道之间的耦合作为外环PID控制器扰动，把所述扰动扩张成一个新的变量，其观测量为z₃₃；

一内环PID控制器，其中内环PID控制器为

其中，K_p1，K_i1，K_d1为内环PID控制器的比例、积分、微分系数，b₁为内环PID控制器增益，r₃为内环PID控制器输出

的状态估计量z₁₁与外环PID控制器输出控制量r₂，r₂＝u₂₂之间的误差值；

一内环扩展状态观测器，其表达式为：

其中：

无人机控制单元，用于根据新的姿态控制无人机。

进一步的，本发明的实施例还公开了一种无人机控制设备，其包括处理器，一非易失性存储介质，以及存储在所述非仪式性存储介质中并可在所述处理器上运行的计算机程序，所述计算机程序被执行时，用于实现所述的无人机姿态的方法。

本实施例的串级PID控制控制器上引入了自抗扰控制器结构以及改进fal函数的扩展状态观测器，本实施例的方法可无超调快速达到期望姿态，而传统串级PID控制方法存在超调量，且调节时间较大；本实施例的方法抗的总体干扰能力由于传统串级PID控制。本实施例通过改进扩展状态观测器，降低了干扰对观测性能的影响，从而可以减少无人机受到未知风场的干扰，提高了无人机姿态的控制精度，增加了抗扰能力，减少控制误差。

附图说明

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

图1是根据本发明实施例所述的无人机姿态控制方法流程图；

图2是根据本发明实施例的自抗扰串级PID控制器结构示意图；

图3是本发明实施例中的fal函数的示意图；

图4是本发明实施例的fal函数与newfal函数对比示意图；

图5是传统串级PID控制方法在无扰动下的仿真响应曲线；

图6是传统串级PID控制方法在风场扰动下的仿真响应曲线；

图7是本发明实施例的方法在无扰动下的仿真响应曲线；

图8是本发明实施例的方法在风场扰动下的仿真响应曲线；

图9是本发明实施例的无人机姿态控制装置示意图；

图10是本发明实施例的无人机姿态控制设备示意图。

具体实施方式

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

文献1中关于自抗扰技术的内容通过引用文件的形式引入本文，本文不在对自抗扰技术做详细赘述，本领域技术人员可以参考文献1中关于自抗扰技术的介绍。

实施例一

如图1所示，根据本发明实施例所述的一种无人机姿态控制方法，其包括如下步骤：

S1，获取无人机姿态；

无人机安装了多种传感器例如四旋翼无人机，例如电子罗盘，可用于实时获取无人机的姿态；在这种情况下，直接从所述传感器获取无人机姿态。本实施例以四旋翼无人机为例进行介绍，本领域人员知晓其他种类的无人机也适用于本实施例。

另一种情况，四旋翼无人机的传感器输出的数据不能直接代表其姿态，需要使用卡尔曼滤波器来处理相应的传感器输出的数据来计算四旋翼无人机的姿态，在这种情况下，获取的是所述卡尔曼滤波器处理后获取的四旋翼无人机的姿态。

四旋翼无人机的姿态控制包括横向(x轴)、纵向(y轴)、偏航(z轴)，三个通道的控制，且设计思路均一致，故以下仅说明偏航通道的控制设计。

参见图2，图2是本实施例的自抗扰串级PID控制器的结构示意图，图2中r为期望输入的航向角，y为输出航向角(外环PID控制器)，y为航向角角速度(内环PID控制器)，经过一次积分可得到航向角。

S2，根据自抗扰串级PID控制器获取无人机新的姿态；

其中所述自抗扰串级PID控制器包括：

一外环PID控制器，外环PID控制器为：

其中，K_p2，K_i2，K_d2为外环PID控制器的比例、积分、微分系数，b₂为外环控制器增益，r₁为期望输入r与输出姿态的状态估计量z₂₁之间的系统误差；

优选地，K_p2＝1.2,K_i2＝0.1,K_d2＝0.12,b₂＝0.5。

一外环扩展状态观测器，其表达式为：

z₂₁为外环扩展观测器对于所述外环PID控制器输出y的状态观测量，z₂₂为外环PID控制器输出y的微分观测量，通道之间的耦合作为外环PID控制器扰动，把所述扰动扩张成一个新的变量，其观测量为z₃₃。

优选地，β₂₁＝15,β₂₂＝75,β₂₃＝125,α₂₁＝0.5,α₂₂＝0.25,δ＝0.01。

一内环PID控制器，其中内环PID控制器为

其中，K_p1，K_i1，K_d1为内环PID控制器的比例、积分、微分系数，b₁为内环PID控制器增益，r₃为内环PID控制器输出

的状态估计量z₁₁与外环PID控制器输出控制量r₂，r₂＝u₂₂之间的误差值；

优选地，K_p1＝29,K_i1＝1.8,K_d1＝1,b₁＝0.4。

一内环扩展状态观测器，其表达式为：

其中：

本实施例中的newfal(e,α,δ)在

当|e|≤δ时,newfal(ε,α,δ)函数表达式改为由线性函数和正切函数拟合的函数形式，拟合函数代替fal(ε,α,δ)函数的指数形式，这样避免了拐点和较大的误差增益。

当δ<|e|≤1时，保持fal(ε,α,δ)函数原有形式。

当|e|>1时，newfal(ε,α,δ)函数绝对值限幅为1，且具有一定通用性，幅值不需根据具体的被控对象而设计，降低大误差产生的增益量。

相对于抗扰控制器采用非线性fal(ε,α,δ)函数，本实施例newfal(e,α,δ)在原点具有更好的平滑连续性，收敛性。参考图3，在δ＝0.3，α＝0.2,0.4,0.6情况下，fal(ε,α,δ)函数在原点附近存在拐点，导致斜率较大引起观测器观测效果不佳的问题，在误差较大时，对干扰的观察性能会降低，参考图4，

δ＝0.01，α＝0.25时，newfal(ε,α,δ)与fal(ε,α,δ)的对比，有图4可知，本实施例的newfal(e,α,δ)在原点具有更好的平滑连续性，收敛性。

其中,newfal(ε,α_i,δ)函数的非线性形状取决于α的大小，本实施例中，α₁₁一般取0.5，α₁₂一般取0.25；δ表示该函数线性区间的宽度，主要用来消除零点处的高频震动，一般取0.01左右。β₁₁,β₁₂,β₁₃为观测器的增益，可根据带宽法整定：

β₁₁＝3h,β₁₂＝3h²,β₁₃＝h³

其中h为系统的带宽，其余参数的选取方法与参考文件1提及的方法相同。优选地，β₁₁＝15,β₁₂＝75,β₁₃＝125,α₁₁＝0.5,α₁₂＝0.25,δ＝0.01。

S3，根据所述新的姿态控制无人机。

本实施例的串级PID控制控制器上引入了自抗扰控制器结构以及改进fal函数的扩展状态观测器，在Matlab/Simulink中搭建四旋翼无人机仿真模型，设定期望姿态为滚转角10°，俯仰角15°，偏航角40°，在无扰动情况下，由图5和图7对比可以看出，本实施例的方法可无超调快速达到期望姿态，而传统串级PID控制方法存在超调量，且调节时间较大；在仿真模型中引入风场扰动，由图6和图8可以看出，本实施例的方法抗的总体干扰能力由于传统串级PID控制。

由此可知，本实施例通过改进扩展状态观测器，降低了干扰对观测性能的影响，从而可以减少无人机受到未知风场的干扰，提高了无人机姿态的控制精度，增加了抗扰能力，减少控制误差。

实施例二

参考图9，本实施例提供了一种无人机姿态控制装置，其包括如下单元：

获取单元：用于获取无人机姿态；

新姿态获取单元，用于根据自抗扰串级PID控制器获取无人机新的姿态；

其中所述自抗扰串级PID控制器包括：

一外环PID控制器，外环PID控制器为：

其中，K_p2，K_i2，K_d2为外环PID控制器的比例、积分、微分系数，b₂为外环控制器增益，r₁为期望输入r与输出姿态的状态估计量z₂₁之间的系统误差；

一外环扩展状态观测器，其表达式为：

z₂₁为外环扩展观测器对于所述外环PID控制器输出y的状态观测量，z₂₂为外环PID控制器输出y的微分观测量，通道之间的耦合作为外环PID控制器扰动，把所述扰动扩张成一个新的变量，其观测量为z₃₃；

一内环PID控制器，其中内环PID控制器为

其中，K_p1，K_i1，K_d1为内环PID控制器的比例、积分、微分系数，b₁为内环PID控制器增益，r₃为内环PID控制器输出

的状态估计量z₁₁与外环PID控制器输出控制量r₂，r₂＝u₂₂之间的误差值；

一内环扩展状态观测器，其表达式为：

其中：

无人机控制单元，用于根据新的姿态控制无人机。

本实施例通过改进扩展状态观测器，降低了干扰对观测性能的影响，从而可以减少无人机受到未知风场的干扰，提高了无人机姿态的控制精度，增加了抗扰能力，减少控制误差。

实施例三

参考附图10，本实施例提供了一种无人机姿态控制设备20的结构示意图。该实施例的无人机姿态控制设备20包括处理器21、存储器22以及存储在所述存储器22中并可在所述处理器21上运行的计算机程序。所述处理器21执行所述计算机程序时实现上述无人机姿态控制方法实施例中的步骤，例如图2所示的步骤S1。或者，所述处理器21执行所述计算机程序时实现上述各装置实施例中各模块/单元的功能，例如第一获取模块11。

示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器22中，并由所述处理器21执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述无人机姿态控制设备20中的执行过程。

所述无人机姿态控制设备20可包括，但不仅限于，处理器21、存储器22。本领域技术人员可以理解，所述示意图仅仅是无人机姿态控制设备20的示例，并不构成对无人机姿态控制设备20的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述无人机姿态控制设备20还可以包括输入输出设备、网络接入设备、总线等。

所述处理器21可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器21是所述无人机姿态控制设备20的控制中心，利用各种接口和线路连接整个无人机姿态控制设备20的各个部分。

所述存储器22可用于存储所述计算机程序和/或模块，所述处理器21通过运行或执行存储在所述存储器22内的计算机程序和/或模块，以及调用存储在存储器22内的数据，实现所述无人机姿态控制设备20的各种功能。所述存储器22可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器22可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

其中，所述无人机姿态控制设备20集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器21执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种无人机姿态控制方法，其特征在于，包括如下步骤：

S1，获取无人机姿态；

S2，根据自抗扰串级PID控制器获取无人机新的姿态；

其中所述自抗扰串级PID控制器包括：

一外环PID控制器，外环PID控制器为：

其中，

，

，

为外环PID控制器的比例、积分、微分系数，

为外环控制器增益，

为期望输入r与输出姿态的状态估计量z₂₁之间的系统误差；

一外环扩展状态观测器，其表达式为：

为外环扩展观测器对于所述外环PID控制器输出

的状态观测量，

为外环PID控制器输出

的微分观测量，通道之间的耦合作为外环PID控制器扰动，把所述扰动扩张成一个新的变量，其观测量为

;

一内环PID控制器，其中内环PID控制器为

其中，

，

，

为内环PID控制器的比例、积分、微分系数，

为内环PID控制器增益，

为内环PID控制器输出

的状态估计量

与外环PID控制器输出控制量

，

之间的误差值；

一内环扩展状态观测器，其表达式为：

其中：

,

，

在当

时，

函数表达式改为由线性函数和正切函数拟合的函数形式，拟合函数代替

函数的指数形式，以避免拐点和较大的误差增益；

当

时，保持

函数原有形式；

当

时，

函数绝对值限幅为1，且具有一定通用性，幅值不需根据具体的被控对象而设计；

S3，根据所述新的姿态控制无人机。

2.根据权利要求1所述的方法，其特征在于，姿态至少包括横向或纵向或偏航中的一个或多个。

3.根据权利要求1所述的方法，其中

，其中h为系统的带宽。

4.根据权利要求1所述的方法，其中

。

5.根据权利要求1所述的方法，其中

,

。

6.根据权利要求1所述的方法，其中

。

7.一种控制无人机的装置，其包括如下单元：

获取单元：用于获取无人机姿态；

新姿态获取单元，用于根据自抗扰串级PID控制器获取无人机新的姿态；

其中所述自抗扰串级PID控制器包括：

一外环PID控制器，外环PID控制器为：

其中，

，

，

为外环PID控制器的比例、积分、微分系数，

为外环控制器增益，

为期望输入r与输出姿态的状态估计量z₂₁之间的系统误差；

一外环扩展状态观测器，其表达式为：

为外环扩展观测器对于所述外环PID控制器输出

的状态观测量，

为外环PID控制器输出

的微分观测量，通道之间的耦合作为外环PID控制器扰动，把所述扰动扩张成一个新的变量，其观测量为

;

一内环PID控制器，其中内环PID控制器为

其中，

，

，

为内环PID控制器的比例、积分、微分系数，

为内环PID控制器增益，

为内环PID控制器输出

的状态估计量

与外环PID控制器输出控制量

，

之间的误差值；

一内环扩展状态观测器，其表达式为：

其中：

,

，

无人机控制单元，用于根据新的姿态控制无人机。

8.一种无人机控制设备，其包括处理器，一非易失性存储介质，以及存储在所述非易失性存储介质中并可在所述处理器上运行的计算机程序，所述计算机程序被执行时，用于实现如权利要求1-5所述的方法。

Images