一种基于药液测量的植保四旋翼无人机高度控制方法

摘要

本发明公开了一种基于药液测量的植保四旋翼无人机高度控制方法。前馈控制器输入加速度和姿态信息后输出前馈补偿油门，再和改进自抗扰控制油门进行叠加后成总控制油门；经测量药液的流出量获得药液的余量，线性扩张状态观测器监测接收药液余量、油门和总控制油门处理得高度、速度观测值和扰动观测值，将高度、速度观测值和线性微分器输出的高度和速度参考控制量相减后获得速度和高度偏差，将扰动观测值和非线性控制律输出相叠加后形成改进自抗扰控制油门反馈。本发明通过采用基于药液信息的扩张状态观测器技术和前馈控制，显著提升了高度控制系统的抗干扰和快速响应能力，有效地抑制了多源干扰对植保四旋翼飞行器高度控制性能的影响。

权利要求

1.一种基于药液测量的植保四旋翼无人机高度控制方法，包括线性微分器TD和非线性控制律，线性微分器TD接收期望高度处理获得高度参考控制量和速度参考控制量；非线性控制律输入速度偏差和高度偏差，处理获得油门参考控制量；其特征在于：建立植保四旋翼无人机的基于加速度和姿态信息的前馈控制器，前馈控制器输入植保四旋翼无人机的加速度信息和姿态信息后处理输出前馈补偿油门，将前馈补偿油门和改进自抗扰控制油门进行叠加后形成总控制油门，总控制油门发送到线性扩张状态观测器中；

同时，植保四旋翼无人机上通过测量药液的流出量获得药液的余量进而发送到线性扩张状态观测器中；

线性扩张状态观测器实时监测接收药液的余量和总控制油门，处理获得高度观测值、高度方向上的速度观测值、药液带来的扰动观测值，将高度观测值、速度观测值分别和线性微分器TD输出的高度参考控制量和速度参考控制量进行相减计算后获得速度偏差和高度偏差，将扰动观测值和非线性控制律的输出相叠加后形成改进自抗扰控制油门。

2.根据权利要求1所述的一种基于药液测量的植保四旋翼无人机高度控制方法，其特征在于：所述的前馈控制器建立如下：其中，ub为前馈控制量，uc为当前油门，a为植保四旋翼无人机加速度计的z轴上产生加速度，z轴是沿植保四旋翼无人机自身机体竖直向上的方向，θ、φ分别是植保四旋翼无人机的俯仰角和横滚角，g为重力加速度。

3.根据权利要求1所述的一种基于药液测量的植保四旋翼无人机高度控制方法，其特征在于：所述的线性扩张状态观测器建立为：β1＝3wo

式中，m为植保四旋翼无人机的重量，g为重力加速度，y为测量的无人机的高度，z1、z2、z3为高度观测值、高度方向上的速度观测值、高度方向上的扰动观测值；β1、β2、β3为观测器的第一、第二、第三参数；e表示高度观测值和实际高度之间的偏差，wo表示观测器带宽；

上述植保四旋翼无人机的重量m是根据药液的重量而设置，具体计算为：m＝mu+ml

ml＝ρVg

公式中，mu为植保四旋翼无人机不含药液的重量，ml为药液的重量，ρ为药液的密度，V为药液的体积，g为重力加速度；

所述的观测器带宽wo根据剩余药液的体积计算为：式中，Vl为药箱中剩余药液的体积，VM为药箱的容积，wmin为带宽小值wmax为带宽大值。

4.根据权利要求1所述的一种基于药液测量的植保四旋翼无人机高度控制方法，其特征在于：所述的植保四旋翼无人机上装载有药箱，药箱中装有药液，药箱底部连接喷头，药液从喷头流出喷洒。

说明书

一种基于药液测量的植保四旋翼无人机高度控制方法

技术领域

[0001] 本发明属于无人机技术领域的一种旋翼无人机高度控制方法，涉及一种基于药液测量的植保四旋翼无人机高度控制方法。

背景技术

[0002] 四旋翼无人机是一种能够垂直起降的无人机，具有其体积小、机动性能好、控制简单、制造成本低廉等优点，已经广泛的应用于现代化农业作业中。植保四旋翼可以执行农药
喷洒、种子播撒等任务，对于防治病虫害、提高农作物产量具有十分重要的作用。

[0003] 然而，植保四旋翼无人机在执行喷洒任务时，携带药液的减少和晃动会导致高度控制不稳定，从而影响植保四旋翼无人机喷洒作业的精确性和均匀性。因此，有必要提供一
种稳定可靠的高度控制方法能够在外界有扰动情况下保证控制的稳定以及精度等一系列
要求。

发明内容

[0004] 为了克服上述问题，本发明提出了一种基于药液测量的植保四旋翼无人机高度控制方法。

[0005] 为了实现上述技术目的，本发明的技术方案为：

[0006] 本发明包括线性微分器TD和非线性控制律，线性微分器TD接收期望高度处理获得高度参考控制量和速度参考控制量；非线性控制律输入速度偏差和高度偏差，处理获得油
门参考控制量；其特征在于：

[0007] 建立植保四旋翼无人机的基于加速度和姿态信息的前馈控制器，前馈控制器输入植保四旋翼无人机的加速度信息和姿态信息后处理输出前馈补偿油门，

[0008] 将前馈补偿油门和改进自抗扰控制油门进行叠加后形成总控制油门，总控制油门发送到线性扩张状态观测器中；

[0009] 同时，植保四旋翼无人机上通过测量药液的流出量获得药液的余量(药液的质量和体积)进而发送到线性扩张状态观测器中；

[0010] 线性扩张状态观测器实时监测接收药液的余量和总控制油门，处理获得高度观测值、高度方向上的速度观测值、药液带来的扰动观测值，将高度观测值、速度观测值分别和
线性微分器TD输出的高度参考控制量和速度参考控制量进行相减计算后获得速度偏差和
高度偏差，将扰动观测值和非线性控制律的输出相叠加后形成改进自抗扰控制油门。

[0011] 本发明设计线性扩张状态观测器来估计高度方向上的药液扰动，并且利用药液测量的先验信息来调整线性扩张状态观测器的参数；线性扩张状态观测器、线性微分器TD和
非线性控制律三者结合构建了自抗扰高度控制器。

[0012] 本发明结合前馈控制器和线性扩张状态观测器的扰动估计，利用线性微分器和非线性控制率来构建自抗扰控制回路/自抗扰高度控制器。

[0013] 所述的前馈控制器建立如下：

[0014]

[0015] 其中，ub为前馈控制量，uc为当前油门，a为植保四旋翼无人机加速度计的z轴上产生加速度，z轴是沿植保四旋翼无人机自身机体竖直向上的方向，θ、φ分别是植保四旋翼无
人机的俯仰角和横滚角，g为重力加速度。

[0016] 本发明建立以下植保四旋翼无人机高度方向的非线性动态模型：

[0017]

[0018]

[0019] 式中，z为导航坐标系中植保四旋翼无人机的高度，表示高度z的加速度； U为植保四旋翼无人机的总升力，b为植保四旋翼无人机的升力系数，wi为植保四旋翼无人机中四
个电机的转数，i表示植保四旋翼无人机中电机的序数，具体实施中i＝1、2、3、4，θ、φ分别
是植保四旋翼无人机的俯仰角和横滚角；m 为植保四旋翼无人机的质量，g为重力加速度，d
为高度方向上的扰动。U为植保四旋翼无人机高度方向的控制输入。

[0020] 上述非线性动态模型考虑了重力，外力干扰和姿态角对高度控制系统的影响，较为准确地体现了实际飞行的状况和为后面设计自抗扰控制器提供了依据。

[0021] 再根据非线性动态模型建立以下的线性扩张状态观测器。所述的线性扩张状态观测器建立为：

[0022]

[0023] β1＝3wo

[0024]

[0025]

[0026] 式中，m为植保四旋翼无人机的重量，g为重力加速度，y为测量的无人机的高度，z1、z2、z3为高度观测值、高度方向上的速度观测值、药液影响的高度方向上的扰动观测值，
z1、z2是状态变量，z3是扰动；β1、β2、β3为观测器的第一、第二、第三参数，由观测器带宽决定
wo决定，带宽越大，对扰动跟踪速度越快；e表示高度观测值和实际高度之间的偏差，wo表示
观测器带宽；本发明的植保四旋翼无人机中，通过毫米波雷达测量获得距离地面的实际高
度。

[0027] 这样本发明中利用药液测量的先验信息来设置并调整线性扩张状态观测器的参数。具体是：

[0028] 上述植保四旋翼无人机的重量m是根据药液的重量而设置，具体计算为：

[0029] m＝mu+ml

[0030] ml＝ρVg

[0031] 公式中，mu为植保四旋翼无人机不含药液的重量，ml为药液的重量，ρ为药液的密度，V为药液的体积，g为重力加速度；

[0032] 所述的观测器带宽wo根据剩余药液的体积计算为：

[0033]

[0034] 式中，Vl为药箱中剩余药液的体积，VM为药箱的容积，wmin为预设的带宽小值来估计小扰动，wmax为预设的带宽大值，估计大的扰动。

[0035] 具体实施中，药液的体积利用植保四旋翼无人机上的流量计来计算。容纳药液的药箱经管道和喷头连接，管道中安装有流量计。药液灌满药箱，已知药箱体积和流量计测量
的经过流量即可实时获知药箱内剩余药液的体积。

[0036] 由此可见，本发明建立了植保四旋翼无人机高度方向的非线性动态模型，由非线性动态模型进而建立了带有药液扰动和药液测量的线性扩张状态观测器，利用线性扩张状
态观测器实现了植保四旋翼无人机在喷洒药液情况下更精准的高度控制。

[0037] 所述的植保四旋翼无人机上装载有药箱，药箱中装有药液，药箱底部连接喷头，药液从喷头流出喷洒。

[0038] 采用上述技术方案带来的有益效果：

[0039] 1、利用加速度信息和姿态信息，求出四旋翼高度平衡时候的油门，作为前馈控制，使高度控制响应更快。

[0040] 2、本发明通过扩张状态观测器观测系统不确定的部分(扰动)，提高了无人机携带负载时的抗扰能力和对负载质量变化的适应性。克服传统PID对模型变化的自适应能力。

[0041] 3、本发明根据植保四旋翼的药液信息，来自动调整线性扩张状态观测器的参数，使观测器对系统的不确定扰动估计更快，更准确。

附图说明

[0042] 图1是本发明流程示意图。

具体实施方式

[0043] 下面结合说明书附图，对本发明的具体工作过程进一步详细说明。

[0044] 本发明实施的植保四旋翼无人机上装载有药箱，药箱中装有药液，药箱底部连接喷头，药液从喷头流出喷洒。容纳药液的药箱经管道和喷头连接，管道中安装有流量计。药
液灌满药箱，已知药箱体积和流量计测量的经过流量即可实时获知药箱内剩余药液的体
积。

[0045] 建立以下控制系统：

[0046] 包括线性微分器TD，线性微分器TD接收用户输入的期望高度处理获得高度参考控制量和速度参考控制量；

[0047] 包括非线性控制律，非线性控制律输入速度偏差和高度偏差，处理获得油门参考控制量；

[0048] 包括前馈控制器，接收采集的植保四旋翼无人机的加速度信息和姿态信息后处理输出前馈补偿油门，加速度信息为加速度计测量的信息，姿态信息可以采取陀螺仪测量的
角速度信息和加速度计测量的加速度信息通过非线性卡尔曼滤波算法得到。将前馈补偿油
门和改进自抗扰控制油门进行叠加后形成总控制油门，总控制油门发送到线性扩张状态观
测器中；

[0049] 包括线性扩张状态观测器，接收流量计的信号获得药液的流出量通过测量药液的流出量获得药液的质量和体积进而发送到线性扩张状态观测器中；实时监测接收药液的质
量和体积和总控制油门，预测下一时刻的植保四旋翼无人机的高度位置和速度的状态变量
和药液重量变化的扰动量，处理获得高度观测值、高度方向上的速度观测值、药液带来的扰
动观测值，将高度观测值、速度观测值分别和线性微分器TD输出的高度参考控制量和速度
参考控制量进行相减计算后获得速度偏差和高度偏差，将非线性控制律的输出和线性扩张
状态观测器输出的扰动观测值相叠加后反馈形成改进自抗扰控制油门。

[0050] 如图1所示，本发明的实施例及其实施过程如下：

[0051] 步骤1：

[0052] 利用机体加速度和姿态信息来估计质量变化，然后求解出不同质量下植保四旋翼无人机高度保持平衡所需的油门量，并利用该油门作为高度控制器的前馈补偿。

[0053] 忽略空气阻力等扰动，植保四旋翼无人机的高度方向重力与升力平衡公式为：

[0054] Ubcosφcosθ＝mg

[0055] 其中，Ub表示植保四旋翼无人机的高度方向受力平衡所需的升力。

[0056] 如果无人机高度方向重力与升力不平衡，就会在机体加速度计的z轴上产生加速度a，具体公式：

[0057]

[0058] Uc表示无人机当前升力；

[0059] 根据上述两公式建立当前植保四旋翼无人机升力与高度方向受力平衡所需的升力的关系模型；

[0060]

[0061] 在实际中，作为线性来处理，将控制器油门和升力近似为线性关系，转换为当前油门和高度方向保持平衡所需油门的关系模型作为前馈控制器：

[0062]

[0063] 步骤2：建立线性扩张状态观测器、线性微分器TD和非线性控制律三者构成的自抗扰高度控制器，利用线性扩张状态观测器来估计高度方向上的药液扰动，并且利用药液测
量的先验信息来调整线性扩张状态观测器的参数。

[0064] 线性微分器TD接收期望高度处理获得高度参考控制量和速度参考控制量，将高度参考控制量和速度参考控制量分别与线性扩张状态观测器输出的高度观测值、速度观测值
相减做差后获得速度偏差和高度偏差输入到非线性控制律，经非线性控制律处理获得油门
参考控制量；将油门参考控制量和扰动观测值相叠加后形成改进自抗扰控制油门；

[0065] 前馈控制器输入植保四旋翼无人机的加速度信息和姿态信息后处理输出前馈补偿油门，将前馈补偿油门和改进自抗扰控制油门进行叠加后形成总控制油门，总控制油门
发送到线性扩张状态观测器中；

[0066] 同时，植保四旋翼无人机上通过测量药液的流出量获得药液的余量(药液的质量和体积)进而发送到线性扩张状态观测器中，线性扩张状态观测器实时监测接收药液的余
量和总控制油门，处理获得高度观测值、高度方向上的速度观测值、药液带来的扰动观测
值，形成闭环。

[0067] 线性扩张状态观测器建立为：

[0068]

[0069] β1＝3wo

[0070]

[0071]

[0072] 上述植保四旋翼无人机的重量m是根据药液的重量而设置，具体计算为：

[0073] m＝mu+ml

[0074] ml＝ρVg

[0075] 一般药液扰动大小跟药液的量有关。当药液充满药箱，药液不会产生扰动，扩张状态观测器只需要观测药液质量变化。当药液喷洒到药箱容量的一半时，药液晃动是最严重
的，所以扩张状态观测器需要增加带宽，使其能同时跟踪药液晃动和药液质量的变化。当药
液快喷洒完的时，药液产生的扰动较小，所以扩张状态观测器只需要观测药液质量变化即
可。

[0076] 所述的观测器带宽wo根据剩余药液的体积计算为：

[0077]

[0078] 对于观测器的参数，要调试两组，一组参数是飞机药液只有药箱容积的一半，另外一组药液是装满或者没有药液时候的最优参数。这样得到参数就是wmax和wmin。

[0079] 由此实施可见，本发明通过采用基于药液信息的扩张状态观测器技术和前馈控制，显著提升了高度控制系统的抗干扰和快速响应能力，有效地抑制了多源干扰对植保四
旋翼飞行器高度控制性能的影响。