CN108582067A - 一种基于喷药机械臂的喷药控制方法及系统 - Google Patents

Abstract

本发明公开一种基于喷药机械臂的喷药控制方法及系统，所述喷药控制方法包括：构建喷药机械臂的目标函数；建立所述目标函数的约束条件；根据所述目标函数和所述约束条件确定最优路径模型；根据所述最优路径模型确定最优目标函数值；确定所述最优目标函数值对应的第二角度和第三角度；根据所述第二角度和所述第三角度调整喷药机械臂进行喷药。本发明根据第二角度和第三角度调整机械臂每个关节转动相应的角度，进而定位到目标农作物根部位置对植株和病害部位进行对靶变量施药，实现较为精准喷药。

Description

一种基于喷药机械臂的喷药控制方法及系统

技术领域

本发明涉及喷药控制技术领域，特别是涉及一种基于喷药机械臂的喷药控制方法及系统。

背景技术

目前，中国植保机械80％仍为背负式等手动喷雾器，“跑、冒、滴、漏”现象严重，农药利用率只有20％左右，同时因施药中毒人数每年都在递增，特别是在温室等设施农业环境。现有的最热门施药方式为植保无人机低空施药，主要为人力喷洒，大范围铺盖，药量消耗大，农药利用率低，对土壤环境影响很大。

基于上述问题，如何针对植株和病害部位进行对靶变量施药成为本领域亟需解决的问题。

发明内容

本发明的目的是提供一种基于喷药机械臂的喷药控制方法及系统，以提高喷药的精准度。

为实现上述目的，本发明提供了一种基于喷药机械臂的喷药控制方法，所述喷药控制方法包括：

构建喷药机械臂的目标函数；

建立所述目标函数的约束条件；

根据所述目标函数和所述约束条件确定最优路径模型；

根据所述最优路径模型确定最优目标函数值；

确定所述最优目标函数值对应的第二角度和第三角度；

根据所述第二角度和所述第三角度调整喷药机械臂进行喷药。

可选的，所述构建喷药机械臂的目标函数，具体包括：

根据喷药机械臂确定喷药机械臂的转角关系；

根据喷药机械臂的转角关系确定喷药机械臂的目标函数。

可选的，所述根据所述喷药机械臂确定喷药机械臂的转角关系，具体包括：

根据所述喷药机械臂构建喷药机械臂的三维模型；

将所述三维模型转换成二维模型；

根据所述二维模型确定喷药机械臂的转角关系。

可选的，所述根据所述目标函数和所述约束条件确定最优路径模型，具体公式为：

st.-π＜θ₂＜π

-π＜θ₃＜π

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

可选的，所述喷药机械臂的转角关系为：

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

本发明还提供一种基于喷药机械臂的喷药控制系统，所述喷药控制系统包括：

目标函数构建模块，用于构建喷药机械臂的目标函数；

约束条件建立模块，用于建立所述目标函数的约束条件；

最优路径模型确定模块，用于根据所述目标函数和所述约束条件确定最优路径模型；

最优目标函数值确定模块，用于根据所述最优路径模型确定最优目标函数值；

角度确定模块，用于确定所述最优目标函数值对应的第二角度和第三角度；

调整喷药模块，用于根据所述第二角度和所述第三角度调整喷药机械臂进行喷药。

可选的，所述目标函数构建模块，具体包括：

转角关系确定单元，用于根据喷药机械臂确定喷药机械臂的转角关系；

目标函数确定单元，用于根据喷药机械臂的转角关系确定喷药机械臂的目标函数。

可选的，所述转角关系确定单元，具体包括：

三维模型确定子单元，用于根据所述喷药机械臂构建喷药机械臂的三维模型；

转换子单元，用于将所述三维模型转换成二维模型；

转角关系确定子单元，用于根据所述二维模型确定喷药机械臂的转角关系。

可选的，所述根据所述目标函数和所述约束条件确定最优路径模型，具体公式为：

st.-π＜θ₂＜π

-π＜θ₃＜π

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

可选的，所述根据喷药机械臂确定喷药机械臂的转角关系，具体公式为：

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明先构建喷药机械臂的目标函数；建立所述目标函数的约束条件；然后根据所述目标函数和所述约束条件确定最优路径模型；根据所述最优路径模型确定最优目标函数值；最后确定所述最优目标函数值对应的第二角度和第三角度；根据所述第二角度和所述第三角度调整喷药机械臂进行喷药。本发明根据上述第二角度和第三角度调整机械臂每个关节转动相应的角度，进而定位到目标农作物根部位置对植株和病害部位进行对靶变量施药，实现较为精准喷药。

附图说明

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

图1为本发明实施例喷药机械臂的实物图；

图2为本发明实施例单片机与驱动器的接线图；

图3为本发明实施例基于喷药机械臂的喷药控制方法流程图；

图4为本发明实施例机械臂数学模型示意图；

图5为本发明实施例二维平面几何多边形示意图；

图6为本发明实施例基于喷药机械臂的喷药控制系统结构图。

具体实施方式

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

本发明的目的是提供一种基于喷药机械臂的喷药控制方法及系统，以提高喷药的精准度。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明实施例喷药机械臂的实物图；图2为本发明实施例单片机与驱动器的接线图；如图1-图2所示，本发明采用的喷药机械臂为自主设计，所述喷药机械臂的机械臂长为80cm，能够完成完成对点喷药动作。根据实际多次调试中舵机的扭力、臂长等因素，最后确定步进电机86BYG250H作为喷药机械臂的驱动电机，同时配和MA860H驱动器获取更大的驱动电流。在实际使用时，由于喷药机械臂在运动到特定姿态时所需的电机转矩过大，则使用行星减速箱对步进电机进行减速，以获取更大的电机扭力，所述驱动电机设置在图1中椭圆位置处。该喷药机械臂总共有3个关节，通过步进电机调节使得机械臂末端对准弄作物根部，喷药机械臂上带有喷药管，当机械臂末端喷药器定位到农作物根部时，通过喷药管对其喷药。

如图2所示，将步进电机AB相接到驱动器对应的接口，单片机与驱动器接线，其中ENA+为驱动器的使能端，可悬空或通入高电平使驱动器工作。通过设置DIR+不同的高低电平控制步进电机的转向。PUL+为单片机输出的脉冲，驱动器根据输入脉冲的个数和频率，控制步进电机的转动的角度和速度。

图3为本发明实施例基于喷药机械臂的喷药控制方法流程图；如图3所示，本发明提供一种基于喷药机械臂的喷药控制方法，所述方法包括：

步骤11：构建喷药机械臂的目标函数。

步骤12：建立所述目标函数的约束条件。

步骤13：根据所述目标函数和所述约束条件确定最优路径模型。

步骤14：根据所述最优路径模型确定最优目标函数值。

步骤15：确定所述最优目标函数值对应的第二角度和第三角度。

步骤16：根据所述第二角度和所述第三角度调整喷药机械臂进行喷药。

下面对各个步骤进行详细分析。

步骤11：所述构建喷药机械臂的目标函数，具体包括：

步骤111：所述根据喷药机械臂确定喷药机械臂的转角关系，具体包括：

步骤1111：根据所述喷药机械臂构建喷药机械臂的三维模型；

实际的机械臂处在三维空间，运动过程描述较为复杂，考虑将三维空间转换为二维平面来进行研究，对于实际某个农作物根位置，机械臂底座转角是确定，因此只要先计算好机械臂底座在空间中转角情况，那么将其固定，只需研究剩余关节转角值即可，由此建立模型如下。首先定义模型所需的假设：

假设将目标物体视为一个质点。

机械臂底座始终绕z轴，从正x轴方向开始转动，不考虑其他方式。

基于三维空间，用某个3*1位置矢量确定该空间内任一点的位置，对于直角坐标系{A}，假设空间中某点(x₀,y₀,z₀)表示农作物根部点坐标，用点O表示基座，点A，B分别表示竖直关节轴，点P表示末端，(x,y,z)为末端位置的空间坐标值，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₁为底座的转角值，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值。

根据以上假设，构建喷药机械臂的三维模型如图4所示。

步骤1112：将所述三维模型转换成二维模型。

根据所述三维模型计算底座转角值θ₁，底座转角值θ₁即为农作物根部点位置在XOY平面投影正切，可以得到：

其中，θ₁为底座的转角值，(x₀,y₀,z₀)为农作物根部点坐标。

由于θ₁已知，则将所述三维模型转换成为二维模型，即二维平面几何多边形，具体如图5所示。

步骤1113：根据所述二维模型确定喷药机械臂的转角关系。

由竖向投影可知，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，两者在XY平面上的投影之和等于末端位置的空间坐标值(x,y,z)在XY平面上的投影到0点的距离具体公式为：

其中，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值。

对(2)式进行整理，可得到：

其中，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值。

可由横向投影可知，l₁,l₂,l₃,z₀,构成了二维五边形，l₂,l₃相交于B点，l₁,z₀均垂直于XY平面。则可知l₂在Z轴方向的投影长度加上l₁的长度等于l₃在Z轴方向的投影长度加上z₀,具体公式为：

l₁+l₂×cos(π-θ₂)＝z₀+l₃×cos(π-θ₂-θ₃) (4)；

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

对(4)式进行整理，可得到：

z₀＝l₁-l₂×cosθ₂+l₃×cos(θ₂+θ₃) (5)；

根据公式(3)和公式(5)可得到转角关系：

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

步骤112：根据喷药机械臂的转角关系确定喷药机械臂的目标函数。

基于坐标误差平方和最小的原理建立喷药机械臂的目标函数：具体公式为：

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

步骤12：建立所述目标函数的约束条件。所述约束条件具体公式为：

步骤13：所述根据所述目标函数和所述约束条件确定最优路径模型，具体公式为：

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

步骤14：根据所述最优路径模型确定最优目标函数值。

为达到喷药机械臂末端坐标(x,y,z)尽可能趋近农作物根部位置(x₀,y₀,z₀),则只要满足趋近z趋近于z₀，得到最优目标函数值，即最小的W值。

步骤15：确定所述最优目标函数值对应的第二角度θ₂和第三角度θ₃。

步骤16：根据所述第二角度θ₂和所述第三角度θ₃调整喷药机械臂进行喷药。

图6为本发明实施例基于喷药机械臂的喷药控制系统结构图，如图6所示，本发明还提供一种基于喷药机械臂的喷药控制系统，所述喷药控制系统包括：

目标函数构建模块21，用于构建喷药机械臂的目标函数；

约束条件建立模块22，用于建立所述目标函数的约束条件；

最优路径模型确定模块23，用于根据所述目标函数和所述约束条件确定最优路径模型；

最优目标函数值确定模块24，用于根据所述最优路径模型确定最优目标函数值；

角度确定模块25，用于确定所述最优目标函数值对应的第二角度和第三角度；

调整喷药模块26，用于根据所述第二角度和所述第三角度调整喷药机械臂进行喷药。

本发明所述目标函数构建模块21，具体包括：

转角关系确定单元，用于根据喷药机械臂确定喷药机械臂的转角关系；

目标函数确定单元，用于根据喷药机械臂的转角关系确定喷药机械臂的目标函数。

本发明所述转角关系确定单元，具体包括：

三维模型确定子单元，用于根据所述喷药机械臂构建喷药机械臂的三维模型；

转换子单元，用于将所述三维模型转换成二维模型；

转角关系确定子单元，用于根据所述二维模型确定喷药机械臂的转角关系。

可选的，所述根据所述目标函数和所述约束条件确定最优路径模型，具体公式为：

st.-π＜θ₂＜π

-π＜θ₃＜π

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

本发明先构建喷药机械臂的目标函数；建立所述目标函数的约束条件；然后根据所述目标函数和所述约束条件确定最优路径模型；根据所述最优路径模型确定最优目标函数值；最后确定所述最优目标函数值对应的第二角度和第三角度；根据所述第二角度和所述第三角度调整喷药机械臂进行喷药。本发明根据上述确定的第二角度和第三角度调整机械臂每个关节转动相应的角度，进而定位到目标农作物根部位置对植株和病害部位进行对靶变量施药，实现较为精准喷药。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种基于喷药机械臂的喷药控制方法，其特征在于，所述喷药控制方法包括：

构建喷药机械臂的目标函数；

建立所述目标函数的约束条件；

根据所述目标函数和所述约束条件确定最优路径模型；

根据所述最优路径模型确定最优目标函数值；

确定所述最优目标函数值对应的第二角度和第三角度；

根据所述第二角度和所述第三角度调整喷药机械臂进行喷药。

2.根据权利要求1所述的喷药控制方法，其特征在于，所述构建喷药机械臂的目标函数，具体包括：

根据喷药机械臂确定喷药机械臂的转角关系；

根据喷药机械臂的转角关系确定喷药机械臂的目标函数。

3.根据权利要求2所述的喷药控制方法，其特征在于，所述根据所述喷药机械臂确定喷药机械臂的转角关系，具体包括：

根据所述喷药机械臂构建喷药机械臂的三维模型；

将所述三维模型转换成二维模型；

根据所述二维模型确定喷药机械臂的转角关系。

4.根据权利要求1所述的喷药控制方法，其特征在于，所述根据所述目标函数和所述约束条件确定最优路径模型，具体公式为：

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

5.根据权利要求2所述的喷药控制方法，其特征在于，所述喷药机械臂的转角关系为：

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

6.一种基于喷药机械臂的喷药控制系统，其特征在于，所述喷药控制系统包括：

目标函数构建模块，用于构建喷药机械臂的目标函数；

约束条件建立模块，用于建立所述目标函数的约束条件；

最优路径模型确定模块，用于根据所述目标函数和所述约束条件确定最优路径模型；

最优目标函数值确定模块，用于根据所述最优路径模型确定最优目标函数值；

角度确定模块，用于确定所述最优目标函数值对应的第二角度和第三角度；

调整喷药模块，用于根据所述第二角度和所述第三角度调整喷药机械臂进行喷药。

7.根据权利要求6所述的喷药控制系统，其特征在于，所述目标函数构建模块，具体包括：

转角关系确定单元，用于根据喷药机械臂确定喷药机械臂的转角关系；

目标函数确定单元，用于根据喷药机械臂的转角关系确定喷药机械臂的目标函数。

8.根据权利要求7所述的喷药控制系统，其特征在于，所述转角关系确定单元，具体包括：

三维模型确定子单元，用于根据所述喷药机械臂构建喷药机械臂的三维模型；

转换子单元，用于将所述三维模型转换成二维模型；

转角关系确定子单元，用于根据所述二维模型确定喷药机械臂的转角关系。

9.根据权利要求6所述的喷药控制系统，其特征在于，所述根据所述目标函数和所述约束条件确定最优路径模型，具体公式为：

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。

10.根据权利要求7所述的喷药控制系统，其特征在于，所述根据喷药机械臂确定喷药机械臂的转角关系，具体公式为：

其中，l₁为水平底座关节O与竖直关节A之间支杆的长度，l₂为竖直关节A与竖直关节B之间支杆的长度，l₃为竖直关节B与末端执行器之间支杆的长度，θ₂为竖直关节轴A的转角值，θ₃为竖直关节轴B的转角值，(x,y,z)为末端位置的空间坐标值，(x₀,y₀,z₀)为农作物根部点坐标。