无人机农林喷洒实时防漂移修正的方法、装置和系统
技术领域
本发明属于自动控制领域,尤其涉及一种用于对无人机农林喷洒进行实时防漂移修正的方法和系统。
背景技术
精准农业对农药作业要求越来越高,对农林作业的喷洒精度要求也相应提高,在病虫害防治中,需要杜绝重喷、漏喷等作业缺陷。现有产品在农林喷洒时虽然可以做到精准喷洒、变量喷洒,但是在实际作业过程中影响喷洒效果的因素有很多,包括地理位置、地理环境、风速、温度等。如图1,飞机按一定的方向漂移并在空中进行农林喷洒。当飞机正常作业(即,喷洒区域无风或其他因素干扰)且定位在GPS 1点时,无人机的农林喷洒区域对应为Z1。若风向和风速发生变化,农药有向Z2区域漂移趋势。如果此时不对无人机的位置进行修正,将会使得无人机实际喷洒的区域变为Z2。这样一来,就会出现漏喷区域Z3和重喷区域Z4。在这种情况下,就不能从根本上保证喷洒作业的质量,导致重喷、漏喷等问题时有发生,甚至农药会飘散到相邻田地、鱼塘,给他人财产造成损失。
当前无人机作业均无法实时进行防漂移修正,目前修正方式主要有:(1)提前设定好喷幅后自动喷洒,根据飞行速度变化自动调整喷洒流量大小;(2)飞手根据天气、田地环境等情况靠经验自行调整飞机位置。
然而,目前两种方式都不能实现飞机随风速的变化自行修正飞机位置。即使能修正位置但也只能让飞机回到原航线位置点,不能根据风速变化自动测算需漂移的距离,来保证不会漏喷、重喷。
发明内容
发明目的:针对现有的技术存在的上述问题,提供一种无人机农林喷洒实时防漂移修正的方法、装置和系统。
技术方案:本发明的无人机农林喷洒实时防漂移修正的方法包括:
(S1)建立的药液雾滴在下降过程中的运动模型:
其中,p为药液雾滴颗粒的位置;v为药液雾滴颗粒实际速度;g为重力加速度;v air 为空气速度;ρ air 为空气密度;m为药液雾滴颗粒的质量;C d 为空气的阻力系数;
(S2)对药液雾滴在下降过程中的所述运动模型进行离散化,得到离散化后的运动模型:
其中,为当前时刻状态变量,表示药液雾滴颗粒的当前位置
的坐标;为上一时刻状态变量;u k-1为上一时刻的系统输入量,包括上一
时刻的风速和药液雾滴颗粒上一时刻的速度v k-1;其中,通过实时气象数据得到;当等于0时,和即分别为药液雾滴颗粒在水平面两个正交方向上的漂移距离;
(S3)对离散化后的运动模型进行修正,得到修正后的运动模型:
其中,w k-1为上一时刻的系统模型噪声;
(S4)确定观测量z k 随当前时刻状态变量x k 和当前时刻观测噪声v k 的变化关系,得到观测方程:
(S5)机载端采集无人机农林喷洒时的雾滴颗粒大小、药液密度、飞行高度,结合从地面端接收的气象数据,并基于修正后的运动模型来推算药液雾滴颗粒的漂移距离;
(S6)机载端根据推测出的漂移距离结合机载端的GPS定位实时调整无人机飞行航迹偏移量。
进一步地,步骤(S1)中气象数据包括以下各项中的一或多项:环境温度、气压、湿度、风向和风速。
进一步地,步骤(S1)中地面端还采集位置差分数据,并实时发送至机载端,用于辅助GPS精确定位飞机位置。
进一步地,步骤(S5)具体包括:
(S51)确定修正后的运动模型中的参数并进行模型初始化:基于来自地面端的气象数
据确定ρ air 的值;基于采集的雾滴颗粒大小、药液密度确定药液雾滴颗粒的质量m;基于ρ air
和采集的雾滴颗粒大小确定空气的阻力系数C d ;模型初始化包括根据飞机的飞行高度h 0 设
置药液雾滴颗粒初始时刻的状态变量x 0 、设置药液雾滴颗粒初始时刻的速度;
(S52)时间更新:基于修正后的运动模型,用上一时刻状态变量和协方差变量的估计值来预测当前时刻状态变量和协方差变量的估计值:
其中,和分别为预测的当前时刻状态变量和协方差变量的估计值;和分别为上一时刻状态变量和协方差变量的估计值;A k 和为修正后的运动模型中函数f对
于变量x的导数矩阵;W k 为修正后的运动模型中函数f对w的导数矩阵;为上一时刻的模
型噪声方差;
(S53)状态更新:基于步骤(S4)中得到的观测方程通过下式来修正步骤(S52)中预测的当前时刻的状态变量和协方差变量的估计值:
K k 为卡尔曼增益,和分别为当前时刻状态变量和协方差变量的估计值,H k 为观测
方程中函数h对x的导数矩阵,V k 为观测方程中函数h对v的导数矩阵,R k 为k时刻的观测噪声
方差,I为单位矩阵。
进一步地,步骤(S3)中无人机的飞行航迹偏移量等于推测出的漂移距离。
本发明的无人机防漂移农林喷洒实时防漂移修正的装置包括:无线通信模块,用于接收地面端发送的实时气象数据;采集模块,用于采集雾滴颗粒大小、药液密度和飞行高度数据;校正模块,用于结合采集模块和无线通信模块接收到的各种实时数据,基于修正后的药液雾滴在下降过程中的运动模型和观测方程来推算药液雾滴颗粒的漂移距离;GPS定位器,用于获取所述无人机的当前定位位置;控制器,用于接收GPS定位器获取的位置信息,并根据校正模块推测出来的漂移距离结合GPS定位实时调整无人机飞行航迹偏移量;其中,修正后的药液雾滴在下降过程中的运动模型是对药液雾滴在下降过程中的运动模型进行离散化和噪声修正后得到的;药液雾滴在下降过程中的运动模型表示为:
其中,p为药液雾滴颗粒的位置;v为药液雾滴颗粒实际速度;g为重力加速度;v air 为空气速度;ρ air 为空气密度;m为药液雾滴颗粒的质量;C d 为空气的阻力系数;
修正后的运动模型记为:
其中,为当前时刻状态变量,表示药液雾滴颗粒的当前位置
的坐标;为上一时刻状态变量;u k-1为上一时刻的系统输入量,包括上一
时刻的风速和药液雾滴颗粒上一时刻的速度v k-1;其中,通过实时气象数据得到;当等于0时,和即分别为药液雾滴颗粒在水平面两个正交方向上的漂移距离;w k-1为上
一时刻的系统模型噪声;是通过对式(1.6)离散化后得到的;
观测方程记为:
其中,z k 为观测量;v k 为当前时刻观测噪声。
本发明的无人机防漂移农林喷洒实时防漂移修正的系统包括地面端和机载端;所述地面端包括:气象站,用于采集气象数据;RTK基站,用于采集无人机地面端的位置差分数据;所述机载端包括:无线通信模块,用于接收地面端发送的实时气象数据和差分数据;采集模块,用于采集雾滴颗粒大小、药液密度和飞行高度的数据;校正模块,用于通过采集模块和无线通信模块接收到的各种实时数据,基于修正后的药液雾滴在下降过程中的运动模型和观测方程来推算药液雾滴颗粒的漂移距离;GPS定位器,用于获取所述无人机的当前GPS定位位置信息;控制器,用于根据校正模块推测出来的漂移距离控制调整具体的飞行航迹;其中,修正后的药液雾滴在下降过程中的运动模型是对药液雾滴在下降过程中的运动模型进行离散化和噪声修正后得到的;药液雾滴在下降过程中的运动模型表示为:
其中,p为药液雾滴颗粒的位置;v为药液雾滴颗粒实际速度;g为重力加速度;v air 为空气速度;ρ air 为空气密度;m为药液雾滴颗粒的质量;C d 为空气的阻力系数;
修正后的运动模型记为:
其中,为当前时刻状态变量,表示药液雾滴颗粒的当前位置
的坐标;为上一时刻状态变量;u k-1为上一时刻的系统输入量,包括上一
时刻的风速和药液雾滴颗粒上一时刻的速度v k-1;其中,通过实时气象数据得到;当等于0时,和即分别为药液雾滴颗粒在水平面两个正交方向上的漂移距离;w k-1为上
一时刻的系统模型噪声;是通过对式(1.6)离散化后得到的;
观测方程记为:
其中,z k 为观测量;v k 为当前时刻观测噪声。
有益效果:与现有技术相比,本发明具有以下优点:
(1)通过利用差分系统,实时将修正的地理信息传输到飞机飞控器上,保证了GPS点定位的准确度;
(2)能够根据传感器测得的环境因素变量自适应调整飞机偏移量,进而实时改变航线进行精准作业,从而有效地杜绝重喷和漏喷现象;
(3)能够根据环境因素结合运动学模型推算漂移距离,避免添加额外的漂移距离采集设备,有效精简系统,降低成本;
(4)建模时兼顾了求解的准确性需求和计算量,结合卡尔曼滤波,具有较强的实时性、环境适应性和准确性。
附图说明
图1是本发明实时防漂移修正系统的示意图;
图2是本发明实时防漂移修正系统机载端中校正模块推测漂移距离的原理图。
具体实施方式
以下是结合附图对本发明进行详细说明。
如图1,本发明的对无人机防漂移农林喷洒进行实时防漂移修正的系统包括地面端和机载端。其中,地面端包括气象站和RTK(Real - time kinematic,实时动态)基站。气象站采集气象数据,并通过无线通信模块发送至机载端。气象站采集的气象数据包括:环境温度、气压、湿度、风向、风速等信息。RTK基站测量差分数据,并将测得的差分数据通过无线通信模块发送至机载端。该差分数据能够对GPS定位系统测定的飞机定位数据进行校正,从而确保飞机的定位精度。机载端包括无线通信模块、采集模块、校正模块、GPS定位器和控制器。其中,无线通信模块用于接收地面端发送的实时气象数据和差分数据。采集模块包括多个传感器,分别用于采集雾滴颗粒大小、药液密度、飞行高度等数据。校正模块基于采集模块和无线通信模块接收到的各种实时数据来推算药液雾滴颗粒的漂移距离,并将漂移距离发送至控制器用于控制。GPS定位器采集无人机的定位数据。控制器从GPS定位器获取定位数据,并根据校正模块推测出来的漂移距离结合GPS定位数据和RTK基站发送的差分数据实时调整无人机的偏移量,从而控制调整具体的飞行航迹,保证药液不出现漏喷和重喷。无线通信模块还可以在校正模块推算出修正结果后将结果及时反馈到地面站。
其中,校正模块是基于内部建好的模型和观测方程并结合卡尔曼滤波算法进行校正的。模型的建立具体为:
(a)建立的药液雾滴在下降过程中的运动模型
假设药液雾滴颗粒初始速度为v 0,实际速度为v,风速为v air ,雾滴相对空气运动速度为v f 。根据牛顿第二定律,其运动方程可表示为:
式(1.1)中,F为雾滴颗粒所受外力,m为药液雾滴颗粒的质量,包括雾滴颗粒的运动过程受到重力、空气阻力作用;t为时间变量。
式(1.2)中,F f 为空气阻力,可表示为:
式中,ρ air 为空气密度,与环境温度、湿度、气压有关。C d 为空气的阻力系数,与雾滴体积形状有关。雾滴相对空气运动速度v f 满足:
将药液雾滴颗粒的位置记为p,则
结合式(1.1-1.5)得到药液雾滴在下降过程中的运动模型为:
(b)对药液雾滴在下降过程中的所述运动模型进行离散化,得到离散化后的运动模型:
其中,为当前时刻状态变量,表示药液雾滴颗粒的当前位置
的坐标;为上一时刻状态变量;u k-1为上一时刻的系统输入量,包括上一
时刻的风速和药液雾滴颗粒上一时刻的速度v k-1;其中,通过实时气象数据得到;当等于0(代表此时药液雾滴到达地面)时,和即分别为药液雾滴颗粒在水平面两个正
交方向上的漂移距离。
需要说明的是,之所以选择让上一时刻的系统输入量u k-1包括上一时刻的风速
和药液雾滴颗粒上一时刻的速度v k-1,是考虑到这两个量(即和v k-1)在整个喷洒过程中
的时变性较为明显。虽然ρ air 在风速变化时值也会发生变化,但其值的变化程度相对较小,
对计算结果的影响也较小,因此可以在计算时视为常量,而不纳入u k-1中,以减小计算量。换
言之,这仅是本发明一种优选的考虑,其他实施例中也可以结合实际情况调整u k-1包含的变
量。
(c)对离散化后的运动模型进行修正,得到修正后的运动模型:
其中,w k-1为上一时刻的系统模型噪声;
观测方程描述的是观测量z k 随当前时刻状态变量x k 和当前时刻观测噪声v k 的变化关系,用式子表示为:
其中,h为非线性函数。
校正模块进一步包括预处理组件、时间更新组件和状态更新组件,这些组件可以通过在硬件电路上编程实现。
预处理组件确定修正后的运动模型中的参数并进行模型初始化。其中,确定修正
后的运动模型中的参数包括:基于来自地面端的气象数据确定ρ air 的值;基于采集的雾滴颗
粒大小、药液密度确定药液雾滴颗粒的质量m;基于ρ air 和采集的雾滴颗粒大小确定空气的
阻力系数C d 。模型初始化包括根据飞机的飞行高度h 0 设置药液雾滴颗粒初始时刻的状态变
量、设置药液雾滴颗粒初始时刻的速度等。
时间更新组件修正后的运动模型用上一时刻状态变量和协方差变量的估计值来预测当前时刻状态变量和协方差变量的估计值,预测方式具体为:
1、基于修正后的运动模型预测当前时刻(即k时刻)状态变量
2、预测误差协方差阵:
在式(3)和(4)中,和分别为预测的当前时刻状态变量和协方差变量的估计
值;和分别为上一时刻状态变量和协方差变量的估计值;A k 为辨识出的系统模型中
函数f对于变量x的导数矩阵;W k 为修正后的运动模型中函数f对w的导数矩阵;为上一
时刻的模型噪声方差。需要注意的是,式(3)是基于式(1)得到的,虽然修正后的运动模型中
含有上一时刻的系统模型噪声变量w k-1,但在计算预测的当前时刻状态变量时,令其
等于0进行计算。
状态更新组件基于观测方程来修正预测的当前时刻的状态变量和协方差变量的估计值,修正方式具体为:
1、计算卡尔曼增益K k :
2、利用观测量更新预测值:
3、更新误差协方差阵:
在式(5)-(7)中,和分别为当前时刻状态变量和协方差变量的估计值,H k 为观测
方程中函数h对x的导数矩阵,V k 为观测方程中函数h对v的导数矩阵,R k 为k时刻的观测噪声
方差,I为单位矩阵。
校正模块中的时间更新组件和状态更新组件的更新过程如图2所示。
如图1,通过上述系统对无人机进行实时防漂移修正后,飞机由GPS1点向GPS2点漂移,此时飞机经过Z3漏喷区域,从而防止了漏喷现象的发生。飞机到达GPS2点后又根据当前情况实时调整原有规划的航线,从而保证后续作业任务无漏喷现象。