CN109839822B - 一种改进自抗扰的四旋翼无人机高度控制方法 - Google Patents

Abstract

本发明公开了一种改进自抗扰的四旋翼无人机高度控制方法，包括步骤：步骤一、基于机理建模法建立四旋翼无人机高度方向的非线性动态模型；步骤二、将非线性动态模型分解为线性项和不确定项；步骤三、利用跟踪微分器求取高度方向位置和速度期望值；步骤四、利用一阶扩张状态观测器估计***中的扰动项；步骤五、设计基于PD算法和前馈控制的误差反馈控制律，并利用扩张状态观测器估计出的扰动项的值对***不确定项进行补偿。本发明通过利用改进的跟踪微分器规划高度飞行策略，构造了一种加速‑匀速‑减速的飞行模式使四旋翼无人机可以以最短时间从一个高度上升(或下降)到另一高度；且飞行时的最大加速度和匀速段的速度可自主设置。

Description

一种改进自抗扰的四旋翼无人机高度控制方法

技术领域

本发明属于无人机自动控制领域，具体涉及一种改进自抗扰的四旋翼无人机高度控制方 法。

背景技术

无人机的稳定控制是无人机***研究的核心和重点之一，它的研究不仅具有重大的现实 意义，而且具有深远的理论意义。

目前四旋翼无人机在市场上已经相当普遍，但是设计抗干扰能力强的飞控***仍是比较 困难的问题。尤其是考虑无人机携带负载的情况时，实现高度方向的稳定控制是一个具有挑 战性的难题。

在过去的几十年，各种控制算法被研究、运用在无人机的稳定控制中。常用的控制器有 PID控制器、反步法控制器、滑模控制器等。其中PID控制器因为其简单、高效的优点被广 泛的应用于现代无人机控制***中。但是，PID控制器的控制效果过分的依赖于比例、积分、 微分这三个参数的设定，在受到外力干扰时，其控制效果容易受到较大的影响。因此设计一 种具有良好抗扰能力的控制器十分必要。

自抗扰控制技术由中国科学院***科学研究所韩京清研究员提出，该控制方法主要由三 部分组成：跟踪微分器、误差反馈控制器以及扩张状态观测器，在不依赖***精确模型的情 况下仍然具有良好的鲁棒控制效果。近年来，该控制方法在无人机的飞行控制中得到了一定 的应用，但是目前自抗扰控制器参数整定方法较少，非线性结构仍较为复杂，实际运用具有 一定难度；此外，目前应用事例多见于无人机姿态控制上，而在高度控制或者水平方向仍未 有成功应用的事例。因此对自抗扰控制器进行一定的简化，并应用于四旋翼无人机的高度抗 扰控制具有重要的工程意义。

发明内容

本发明所要解决的技术问题是：如何对自抗扰控制算法进行一定的改进后，应用于四旋 翼无人机的高度控制上，使之具备较好的鲁棒性能，因此，提供了一种改进自抗扰的四旋翼 无人机高度控制方法。

本发明采用如下技术方案来实现的：

一种改进自抗扰的四旋翼无人机高度控制方法，包括如下步骤：

步骤一、基于机理建模法建立四旋翼无人机高度方向的非线性动态模型；

步骤二、将非线性动态模型分解为线性项和不确定项；

步骤三、在步骤二的基础上，利用跟踪微分器求取高度方向位置和速度期望值；

步骤四、在步骤三的基础上，利用一阶扩张状态观测器估计***中的扰动项；

步骤五、在步骤四的基础上，设计基于PD算法和前馈控制的误差反馈控制律，并利用 扩张状态观测器估计出的扰动项的值对***不确定项进行补偿。

本发明进一步的改进在于，步骤一中，基于机理建模法建立的四旋翼无人机非线性动态 模型为：

式中，z为地理坐标系中无人机的高度，

b为旋翼的升力系数，ω_i是四个电 机的转速，i＝1,2,3,4，θ、φ分别是无人机的俯仰角和横滚角；m为无人机的质量，g为重 力加速度，D_fz为分解到高度方向的外力干扰，fz中的f是单词force的缩写，是力的意思，z 表示z轴，即高度方向；

该模型中，u为无人机高度方向的控制输入，也是无人机的四个旋翼能提供的升力。

本发明进一步的改进在于，步骤二中，将非线性动态模型分解为线性项和不确定项，具 体描述为：

式中u_z＝ucosφcosθ-mg，k_m＝1/m，q_z＝k_mD_fz，其中，k_mu_z是***模型的线性项，q_z是***模型的不确定项。

本发明进一步的改进在于，步骤三中，利用跟踪微分器求取高度方向位置和速度期望值， 如下：

式中，x₁(k)表示k时刻期望的高度，H表示最终期望到达的高度，x₂(k)表示k时刻期望 的高度方向速度；h表示步长，h₀表示滤波因子，用于防止x₁(k)到达到最终高度H出现震荡， 且h₀大于h；a_zmax是***允许的最大加速度，V_zmax是***允许的最大速度，Limit函数表示 将期望速度x₂(k)的绝对值限制在V_zmax内；fhan又称为最速控制综合函数，该函数按以下步骤 计算：

第一步、中间量计算：

第二步、求取fhan函数值：

式中，sign表示符号函数。

本发明进一步的改进在于，步骤四中，利用一阶扩张状态观测器估计不确定项的值：

式中，e_vz(k)表示k时刻速度估计的误差，z₁(k)表示估计的速度，v_z(k)表示传感器实测 的速度，h表示步长，z₂是观测器估计的不确定项，u_z是控制量，β₀₁和β₀₂是观测器参数；当β₀₁和β₀₂取的足够大时，z₂值将足够接近不确定项q_z；假设观测器带宽为ω_z0，那么取 β₀₁＝2ω_z0，

本发明进一步的改进在于，传感器实测速度通过超声波或气压计测量的高度和加速度计 测量的加速度数据进行融合滤波算法得到。

本发明进一步的改进在于，步骤五中，设计基于PD算法和前馈控制的误差反馈控制律， 并利用扩张状态观测器估计出的扰动项的值对***不确定项进行补偿，如下：

式中，e_z1(k+1)是第k+1时刻期望高度和实际高度之间的误差，z(k+1)是k+1时刻的高 度，e_z1(k+1)是第k+1时刻期望速度和实际速度之间的误差，u_z0(k+1)是针对线性项而选择 的PD算法，k_zp和k_zd是相应的PD参数，fh相当于前馈项；u_z(k+1)在此基础上减去了不确定项的估计量z₂(k+1)，即对不确定项进行补偿。

本发明具有如下有益的技术效果：

1、本发明通过利用改进的跟踪微分器规划高度飞行策略，构造了一种加速-匀速-减速的 飞行模式使四旋翼无人机可以以最短时间从一个高度上升(或下降)到另一高度；且飞行时 的最大加速度和匀速段的速度可自主设置；

2、本发明通过利用一阶扩张状态观测器和可观测的速度量对高度方向的干扰量进行估 计，并给出了观测器参数的选取方法；

3、本发明将高度方向的运动模型分为线性部分和不确定部分，并在最终的控制器中针对 这两个部分设计了对应的控制策略，克服了传统PID控制器抗扰能力不足的问题，尤其提高 了无人机携带负载时的抗扰能力。在仿真实验和实际飞行实验中，无人机都表现出了优良的 鲁棒稳定性能。

附图说明

图1是本发明流程示意图；

图2是无人机高度自抗扰控制仿真实验波形图，其中图2(a)为u_z不减去干扰补偿量z₂的仿真实验波形图，图2(b)为u_z减去干扰补偿量z₂的仿真实验波形图；

图3是无人机携带负载飞行实验的高度抗扰波形图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，下面结合附图、仿真实验以及实 际飞行实验对本发明进一步说明。

如图1所示，本发明提供的一种改进自抗扰的四旋翼无人机高度控制方法，按照如下的 步骤进行：

首先基于机理建模法建立四旋翼无人机高度方向的非线性动态模型。其数学表达式为：

该模型考虑了***的非线性结构以及外界干扰，较为准确的刻画了实际的飞行状况。模型中，

可视为高度方向的控制输入，也是无人机的四个旋翼能提供的升力，b为旋翼的升 力系数，ω_i(i＝1,2,3,4)是四个电机的转速。z为地理坐标系中无人机的高度，θ、φ分别是无 人机的俯仰角和横滚角。m为无人机的质量，g为重力加速度，D_fz为分解到高度方向的外力 干扰。从模型中可以得出，无人机在高度方向的运动受旋翼升力、无人机姿态角、重力以及 干扰外力的共同影响。

事实上，现有资料大多对模型进行了简化，没有考虑干扰项，本发明充分考虑干扰外力。

然后，将非线性动态模型分解为线性项和不确定项。分解后的数学表达式可描述为：

式中u_z＝ucosφcosθ-mg，k_m＝1/m，q_z＝k_mD_fz。其中，k_mu_z是***模型的 线性项，q_z是***模型的不确定项。模型经过分解后，方便了后续分别针对线性项和不确定项进行控制器的设计。

步骤二、将非线性动态模型分解为线性项和不确定项。分解后的数学表达式可描述为：

式中u_z＝ucosφcosθ-mg，k_m＝1/m，q_z＝k_mD_fz。其中，k_mu_z是***模型的线性项，q_z是***模型的不确定项。模型经过分解后，方便了后续分别针对线性项和不确定项进行控制器的设计。

步骤三、利用跟踪微分器求取高度方向位置和速度期望值，按控制周期对应的递推方程 如下：

式中，x₁(k)表示k时刻期望的高度，H表示最终期望到达的高度，x₂(k)表示k时刻期望 的高度方向速度；h表示步长，h₀表示一种滤波因子，可防止x₁(k)到达到最终高度H出现震 荡，一般取h₀大于h，如取h₀＝10h；a_zmax是***允许的最大加速度，V_zmax是***允许的最大速度，Limit函数表示将期望速度x₂(k)的绝对值限制在V_zmax内。fhan又称为最速控制综合函数，该函数可按以下公式计算：

式中，sign表示符号函数。

步骤四、利用一阶扩张状态观测器估计***中的扰动项，按速度测量周期对应的递推方 程如下：

式中，e_vz(k)表示k时刻速度估计的误差，z₁(k)表示估计的速度，v_z(k)表示传感器实测 的速度，其中实测速度可通过超声波或气压计测量的高度和加速度计测量的加速度数据进行 一定的融合滤波算法得到(如卡尔曼滤波和互补滤波)。h表示步长，z₂是观测器估计的不确 定项，u_z是控制量，β₀₁和β₀₂是观测器参数。当β₀₁和β₀₂取的足够大时，z₂值将足够接近不 确定项q_z。假设观测器带宽为ω_z0，那么可取β₀₁＝2ω_z0，

步骤五、设计基于PD算法和前馈控制的误差反馈控制律，并利用扩张状态观测器估计 出的扰动项的值对***不确定项进行补偿。按控制周期对应的递推方程如下：

式中，e_z1(k+1)是第k+1时刻期望高度和实际高度之间的误差，z(k+1)是k+1时刻的高度， e_z1(k+1)是第k+1时刻期望速度和实际速度之间的误差，u_z0(k+1)是针对线性项而选择的PD 算法，k_zp和k_zd是相应的PD参数，fh相当于前馈项；u_z(k+1)在此基础上减去了不确定项的 估计量z₂(k+1)，即对不确定项进行补偿。

作为一种高度飞行规划策略，步骤三中，在经典跟踪微分器的基础上增加了对期望速度 的限制，即x₂(k+1)＝Limit(x₂(k+1),-V_zmax,V_zmax)。一般应用场景下，无人机在高度方向的运动 总是希望从某一高度悬停状态快速飞到另一高度后保持悬停状态，改进后的跟踪微分器使无 人机能够实现从零加速-匀速-减速到零-保持悬停的动态过程，且匀速过程的速度为V_zmax，而 加速或者减速过程中的加速度绝对值为a_zmax，通过设定这两个参数，就能使无人机按照期望 的加速度和速度飞行，且飞行时间最短。

作为一种不确定项估计方法，步骤四中，选择一阶扩张状态观测器对干扰量进行估计。 该观测器要求高度方向无人机的运动速度能够获取。在实际应用中，速度量一般可采取气压 计或超声波测量的高度信息和加速度计测量的加速度信息通过一定的融合滤波算法得到，通 常使用的滤波算法有卡尔曼滤波和互补滤波。若速度数据的更新频率是ω_z0，那么观测器参数 可取为β₀₁＝2ω_z0，

作为一种控制策略，步骤五对步骤二中***模型的线性项和不确定项分别设计对应的控 制器进行控制。对于线性项的控制，主要由u_z0完成，对于不确定项，由u_z减去不确定项的估 计量，即减去z₂对不确定项q_z进行补偿，从而实现高度方向的抗扰能力。

下面对仿真实验和飞行实验进行介绍。

假设在某一应用场景下，无人机从平地起飞，需要飞到100cm的高度后保持悬停，并且 要求无人机飞行过程中最大加速度的绝对值为30cm/s²，最大速度为30cm/s，仿真仿真30s 时间，且在8s<t<15s时施加2cos(2t)+3(单位：N)垂直方向的外力干扰。无人机的质量为 1.076kg，旋翼可提供的最大升力为21N。

步骤一、利用跟踪微分器求取高度方向位置和速度期望值，按控制周期对应的递推方程 如下：

若高度方向的控制周期为0.01s，那么取h＝0.01，h₀＝0.1。

步骤二、利用一阶扩张状态观测器估计***中的扰动项，按速度测量周期对应的递推方 程如下：

若速度测量周期为0.01s，那么取h＝0.01，β₀₁＝2ω_z0＝2/h＝200，β₀₂＝ω_z0 ²＝10000。 又由于m＝1.076，所以可取k_m＝1/1.076≈1。

步骤三、利用一阶扩张状态观测器估计***中的扰动项，按速度测量周期对应的递推方 程如下：

若速度测量周期为0.01s，那么取h＝0.01，β₀₁＝2ω_z0＝2/h＝200，β₀₂＝ω_z0 ²＝10000。 又由于m＝1.076，所以可取k_m＝1/1.076≈1。

步骤三、设计基于PD算法和前馈控制的误差反馈控制律，并利用扩张状态观测器估计 出的扰动项的值对***不确定项进行补偿。按控制周期对应的递推方程如下：

对于PD参数，只要取k_zp和k_zd均大于零即可满足稳定性要求，针对仿真实验中所取的无 人机参数特点，可取k_zp＝k_zd＝4。

图2给出了两组仿真对比实验的结果。(a)图中，使用了整定PD参数后的PD控制算法， 没有减去干扰补偿量z₂；(b)图中，使用了本发明所提出的高度自抗扰控制算法，减去干扰 补偿量z₂。可以看出，前8s没有干扰时，有无干扰补偿量z₂均不影响高度控制效果，无人机 均能快速无超调的到达指定高度；在8s<t<15s加入干扰后，无干扰补偿量z₂时，即一般的 PID算法下，实际高度曲线((a)图中绿色曲线)偏离了预期高度，且15s后取消干扰，也不能恢 复到期望高度，最终超调量达到了33％；而有干扰补偿量z₂时，即自抗扰控制算法下，虽然 在8s<t<15s内高度曲线出现了一点波动，但是波动非常微小，整个飞行过程中最大超调量 约为0.022％，可见抗扰效果明显。

对于实际飞行实验，选取市场上常见的F450机架作为实验对象，装配好后，机体具体参 数如下：

表1四旋翼无人机机体参数

表1中，m为无人机的质量，b为旋翼的升力系数，l为无人机机臂长度，I_x、I_y、I_z分别为绕x轴、y轴、z轴的转动惯量，g为重力加速度。

实验时，起飞前，在四旋翼无人机正下方悬挂一个325g的Li-Po电池，悬绳长0.1m，无 人机飞行目标高度为1.1m，当飞行高度达到0.1m时开始受到电池的重量干扰，电池离地后， 无人机还受到电池晃动所带来的干扰。飞行实验如图3所示。

将实验数据导入MATLAB，加干扰(加电池，定高起飞)和不加干扰(不加电池，定高起飞)的实验数据进行对比，结果如图3所示。

从图3中可以看出，不加干扰时(不悬挂电池)，位置和速度实际飞行曲线都能很好的跟 踪期望的位置-速度曲线，较好的实现了加速，匀速和减速三个过程，且几乎没有超调，位置 稳态误差小于1cm；当悬挂电池增加扰动后，虽然位置和速度曲线出现了一定的波动，但实 际波动范围较小，且最终仍能趋近于稳定，且位置误差逐渐减小，抗扰作用良好。

根据仿真实验和实际飞行实验，可知，本发明所提出的高度自抗扰方法能够达到预期的 抗扰效果。

Claims (4)

1.一种改进自抗扰的四旋翼无人机高度控制方法，其特征在于，包括如下步骤：

步骤一、基于机理建模法建立四旋翼无人机高度方向的非线性动态模型，具体为：

式中，z为地理坐标系中无人机的高度，

b为旋翼的升力系数，ω_i是四个电机的转速，i＝1,2,3,4，θ、φ分别是无人机的俯仰角和横滚角；m为无人机的质量，g为重力加速度，D_fz为分解到高度方向的外力干扰，fz中的f是单词force的缩写，是力的意思，z表示z轴，即高度方向；

该模型中，u为无人机高度方向的控制输入，也是无人机的四个旋翼能提供的升力；

步骤二、将非线性动态模型分解为线性项和不确定项，具体描述为：

式中u_z＝u cosφcosθ-mg，k_m＝1/m，q_z＝k_mD_fz，其中，k_mu_z是***模型的线性项，q_z是***模型的不确定项；

步骤三、在步骤二的基础上，利用跟踪微分器求取高度方向位置和速度期望值，如下：

式中，x₁(k)表示k时刻期望的高度，H表示最终期望到达的高度，x₂(k)表示k时刻期望的高度方向速度；h表示步长，h₀表示滤波因子，用于防止x₁(k)到达到最终高度H出现震荡，且h₀大于h；a_zmax是***允许的最大加速度，V_zmax是***允许的最大速度，Limit函数表示将期望速度x₂(k)的绝对值限制在V_zmax内；fhan又称为最速控制综合函数，该函数按以下步骤计算：

第一步、中间量计算：

第二步、求取fhan函数值：

式中，sign表示符号函数；

步骤四、在步骤三的基础上，利用一阶扩张状态观测器估计***中的扰动项；

步骤五、在步骤四的基础上，设计基于PD算法和前馈控制的误差反馈控制律，并利用扩张状态观测器估计出的扰动项的值对***不确定项进行补偿。

2.根据权利要求1所述的一种改进自抗扰的四旋翼无人机高度控制方法，其特征在于，步骤四中，利用一阶扩张状态观测器估计不确定项的值：

式中，e_vz(k)表示k时刻速度估计的误差，z₁(k)表示估计的速度，v_z(k)表示传感器实测的速度，h表示步长，z₂是观测器估计的不确定项，u_z是控制量，β₀₁和β₀₂是观测器参数；当β₀₁和β₀₂取的足够大时，z₂值将足够接近不确定项q_z；假设观测器带宽为ω_z0，那么取β₀₁＝2ω_z0，

3.根据权利要求2所述的一种改进自抗扰的四旋翼无人机高度控制方法，其特征在于，传感器实测速度通过超声波或气压计测量的高度和加速度计测量的加速度数据进行融合滤波算法得到。

4.根据权利要求2所述的一种改进自抗扰的四旋翼无人机高度控制方法，其特征在于，步骤五中，设计基于PD算法和前馈控制的误差反馈控制律，并利用扩张状态观测器估计出的扰动项的值对***不确定项进行补偿，如下：

式中，e_z1(k+1)是第k+1时刻期望高度和实际高度之间的误差，z(k+1)是k+1时刻的高度，e_z1(k+1)是第k+1时刻期望速度和实际速度之间的误差，u_z0(k+1)是针对线性项而选择的PD算法，k_zp和k_zd是相应的PD参数，fh相当于前馈项；u_z(k+1)在此基础上减去了不确定项的估计量z₂(k+1)，即对不确定项进行补偿。

Images