CN111506062B - 一种全自动施药船及均匀洒药方法 - Google Patents

Abstract

本发明公开了一种全自动施药船及均匀洒药方法，该方法包括获取池塘边界顶点坐标，在顶点坐标范围内进行回字形路径规划，通过路径规划，可得到航线圈数和回字形路径；根据GPS/北斗导航系统接收施药船的位置信息S和施药船航向，计算药液覆盖区中点Q的位置；根据药液覆盖区中点位置Q及施药船航向，利用实时插点算法，获得施药船直线航行时的跟踪目标点；通过称重传感器测取药箱重量，得到药箱中剩余原液重量，计算回字形路径总长度；根据药液覆盖区中点Q的转弯轨迹，确定转弯时的跟踪目标点及目标航向；通过控制器及流量调节阀对药液流量的控制，实现全池塘均匀洒药。本发明能显著提高施药效率和施药质量，为作业船高效施药作业提供了保障。

Description

一种全自动施药船及均匀洒药方法

技术领域

本发明涉及自动导航路径控制领域，特别是全自动施药船的路径规划及均匀洒药方法。

技术背景

我国是水产养殖大国，河蟹养殖已成为中国渔业生产中发展最为迅速、最具潜力的支柱产业，然而当前河蟹养殖面临人工施药不均匀一大难题，人工作业效率与作业质量相对较低。由于人工施药的不均匀，会造成在施药浓度较低的地方不能充分抑制水草或藻类生物等的生长；而在施药浓度较高的地方可能引起溶氧量降低从而造成河蟹存活率下降，因此提高养殖施药的自动化、智能化水平成为亟待解决的问题。因此实现全自动施药船及均匀洒药是非常重要的，可以对河蟹养殖池塘进行全自动的均匀洒药，使工作效率提高、农民劳动强度降低及养殖成本降低，可促进水产养殖业的蓬勃发展。

发明内容

因此本发明基于GPS/北斗导航系统的基础上来进行实时插点算法，用以实现对作业船的直线航行高效作业控制，对于转弯作业控制，根据其转弯轨迹来确定其跟踪目标点及目标航向。

本发明的目的是为了解决现有人工施药的作业方式效率低、施药不均匀等问题，以及解决作业船控制精度不高的缺陷。本发明的作业路径规划方法及对药液进行均匀抛洒可实现高效率施药及均匀抛洒药液。

本发明设计的全自动施药主要是水产养殖池塘的均匀洒药。本发明为解决上述问题，采用的技术方案包含以下步骤：

第一步：在确定作业池塘呈多边形的条件下，通过GPS/北斗导航系统测取，可获得目标作业区域边界处各个角点经纬度坐标，通过回字形路径规划，确定其作业边界、作业圈数以及回字形作业路径；

第二步：根据GPS/北斗导航系统来接收作业船当前的天线位置S，通过计算得出药液覆盖区中点Q点的位置信息；

第三步：根据药液覆盖区域中点Q及自动施药船航向，通过实时插点的方法计算得到药液喷头的跟踪目标点P。根据点到直线的垂直交点的计算公式，得到药液覆盖区中点Q的当前位置，在当前目标路径上的投影点，接着将上述投影点作为起点，再沿着目标路径前进方向前伸一段距离得到作业船的跟踪目标点P以及施药船直线航行时明轮的控制量，其中前伸的距离长度由作业船的船体尺寸、航行速度等确定；

第四步：根据称重传感器测取药箱总重量，减去空药箱的重量，可得到药箱中剩余原液重量。计算回字形路径总长度，结合药液总重量，可得到每单位长度的施药量；

第五步：根据全自动施药船转弯时，药液覆盖区中点的运动轨迹，确定转弯时的跟踪目标点及目标航向；

第六步：根据控制器和流量调节阀，对管道内的药液流量进行控制，使作业过程中作业路径上每单位长度施药量接近(等于)分配的药液重量，从而实现均匀洒药。

进一步，所述第一步中经回字形路径规划，得到作业边界、作业圈数以及作业路径，具体步骤如下：

步骤1.1，通过GPS/北斗导航系统确定矩形作业区域四个顶点,顺时针依次为A(x_α,y_α)、B(x_β,y_β)、C(x_γ,y_γ)、D(x_δ,y_δ)，四点即为作业船的作业边界顶点坐标；

步骤1.2，路径规划以回字形进行规划作业，从矩形作业区较长边开始进入作业规划，假设矩形窄边长为Y,施药船药液覆盖区宽度即作业幅宽为X，按照公式

(向上取整)可确定回字形圈数，即可完成规划作业；

步骤1.3，确定作业路径为T₀→T₁→...→T_K-1→T_K→T_K+1→...→T_N，并且将路径T₀→T₁为作业船起始的目标路径，则路径坐标对应的目标点集合为：

{T₀,T₁,...,T_K-1,T_K,T_K+1,...,T_N}

上述路径坐标对应的目标点集合的经纬度坐标为：

{(x_T0,y_T0),(x_T1,y_T1),...,(x_TK-1,y_TK-1),(x_TK,y_TK),(x_TK+1,y_TK+1),...,(x_TN,y_TN)}

其中x坐标表示经度，y坐标表示纬度，T_N为作业船目标航线的终点。

进一步，所述第二步中根据GPS/北斗导航系统的天线位置S及全自动施药船航向，计算出药液覆盖区中点Q点的位置信息，具体步骤如下：

步骤2.1，由GPS/北斗导航系统可以获得当前GPS/北斗天线位置为S(x₀,y₀)及当前的航向角为

由于喷头的存在，药液向前方呈一定角度喷洒，GPS/北斗天线距离药液覆盖区中点Q的水平距离为H，在施药船作业时，药液覆盖区域和施药船的相对位置，见图3；

步骤2.2，由于GPS/北斗导航装置与药液覆盖区中点Q处于同一条中轴线上，因此根据公式可得药液覆盖区中点Q(x₁,y₁)，计算公式为：

进一步，所述第三步中，采用实时插点的方法计算得到作业船的跟踪目标点P以及施药船直线航行时明轮的控制量，具体过程如下：

步骤3.1，计算获得药液覆盖区中点的当前位置Q(x₁,y₁)在当前目标路径

T_K(x_TK,y_TK)→T_K+1(x_TK+1,y_TK+1)上的投影点为M(x₂,y₂)，计算的公式为：

其中，横坐标x表示经度，纵坐标y表示纬度，T_K→T_K+1表示施药船的目标航向路径。

步骤3.2，由两点经纬度坐标及计算两点距离的公式得到投影点M点到目标路径终点T_K+1的距离

d，计算的公式为：

其中，R＝6378137m为地球半径，d单位为m；

步骤3.3，以投影点M点作为起始点沿着T_K→T_K+1方向前伸距离D₂至P(x₃,y₃)点，P(x₃,y₃)点坐标的计算公式为：

式中D₂的值由施药船的船体尺寸、航行速度以及明轮驱动力等因素确定。

步骤3.4，在施药船直线航行作业时，明轮控制量分为电流PID控制量和航向PID控制量，电流PID控制量的大小与施药船船速成正比，通过电流PID的控制，左右明轮产生速度差，控制施药船转向；而航向PID控制量则代表施药船当前位置Q(x₁,y₁)偏离目标航向点P(x₃,y₃)的方向航向。

进一步，所述第四步，通过称重传感器得到剩余药液重量及计算回字形路径总长度，具体步骤如下：

步骤4.1，通过称重传感器可对重量进行测量，在药箱中存放药液，减去空药箱的重量，可得到药箱中剩余原液重量，药箱与流量调节阀之间用软管进行连接，这样可消除其他部件对药箱称重的影响；

步骤4.2，通过称重传感器检测，在施药作业开始之前，可得药液总重量。计算回字形路径总长度，再用药液总重量除以回字形路径总长度，可得到作业过程中作业路径上每单位长度施药量。调节流量调节阀，使作业过程中作业路径上每单位长度施药量接近(等于)分配的药液重量。

进一步，所述第五步中，需要设定的转弯路径为以相切方式连接前后两段相互垂直直线路径的圆弧，根据全自动施药船转弯时，药液覆盖区中点Q的运动轨迹，确定转弯时的跟踪目标点及目标航向，具体步骤如下：

步骤5.1，由于作业船轨迹跟踪是由药液覆盖区中点Q确定的，根据作业船药液喷头覆盖区宽度L确定作业船转弯圆心O；

步骤5.2，由于在转弯施药作业阶段时，药液覆盖区中点Q应以相切方式连接前后两段相互垂直的直线路径，可确定药液覆盖区中点Q的设定转弯路径是以O为圆心的1/4圆弧DE；

步骤5.3，施药船转弯时，取圆心O距顶点A的距离为药液覆盖区宽度L的2倍，且圆弧半径为

见图6；

步骤5.4，药液覆盖区(以中心为Q，长度为L，垂直于施药船航向的一段直线表示)与圆弧相交处N点的位置，沿圆弧(路径)前伸一段距离，确定跟踪目标点R，QR方向即为目标航向，见图7。沿圆弧前伸距离与船体大小、船速、明轮驱动力有关。

进一步，所述第六步，根据作业船的速度，通过控制器和流量调节阀对软管内的药液流量进行调控，从而对作业船进行均匀洒药控制，具体步骤如下：

步骤6.1，根据作业开始时得到的每单位长度路径施药量Q，通过GPS/北斗导航系统，可得到作业船的速度V，再通过流量调节阀对药液流量q大小进行调控，船速越快，使药液流量越大，药液流量与船速为正比关系：

q＝Q×V

步骤6.2，转弯处的药液量根据D、E二点之间的折线长度计算。通过对DE间折线长度的计算，再乘以每单位长度路径施药量Q，可得转弯处的药液量。

本发明的有益效果为，由于水产养殖池塘大小的不规则和人工施药的不均匀，因此根据不同池塘情况、施药面积大小等因素的不同而进行适应各个池塘的个性均匀施药方法，解决了人工施药不均匀及人工施药效率低等问题，从而解放农民劳动力，提高施药效率。

附图说明

图1全自动施药船结构示意图；

图2全自动施药船结构示意图；

图3药液覆盖区与施药船相对位置示意图；

图4回字形作业规划示意图；

图5基于实时插点算法计算作业船跟踪目标点示意图；

图6施药点中心转向示意图；

图7转弯跟踪目标及航向确定示意图；

具体实施方式

下面结合附图对本发明的具体实施方式做进一步描述。本发明提出了一种全自动施药船及均匀洒药方法，用于实现全自动施药、均匀洒药及对全自动施药船控制等功能。

本实施方式以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施方式。

本实施方式采用的全自动施药船的结构如图1、图2所示，船上包括船体，明轮；船体上放置药箱，控制器，水泵，喷头，水管及GPS/北斗导航系统。图2中，喷头通过水管连接水泵，水泵通过水管连接流量调节阀，流量调节阀通过软管连接药箱，药箱上端设置GPS/北斗导航系统，药箱下端设置控制器，控制器下端放置在称重传感器上。

具体步骤如下：

第一步，确定作业船作业边界顶点坐标、作业圈数以及作业路径。

如图4回字形作业规划示意图所示，假定本发明所在作业池塘呈矩形分布环境下，使用高精度GPS/北斗导航系统测取作业区域四个顶点的坐标位置，顺时针依次记为A(x_α,y_α)、B(x_β,y_β)、C(x_γ,y_γ)、D(x_δ,y_δ)，四点即为作业船的作业边界顶点坐标；

接着，路径规划以回字形进行直线规划作业，从较长的矩形边开始进入规划作业，测得矩形窄边长为Y,施药船药液覆盖区宽度即作业幅宽为X，按照公式

(向上取整)，计算出回字形规划的作业圈数N。

最后，确定作业路径T₀→T₁→...→T_K-1→T_K→T_K+1→...→T_N，并将路径T₀→T₁为作业船起始的目标路径，则路径坐标对应的目标点集合为：

{T₀,T₁,...,T_K-1,T_K,T_K+1,...,T_N}

上述路径坐标对应的目标点集合的经纬度坐标为：

{(x_T0,y_T0),(x_T1,y_T1),...,(x_TK-1,y_TK-1),(x_TK,y_TK),(x_TK+1,y_TK+1),...,(x_TN,y_TN)}

其中x坐标表示经度，y坐标表示纬度，T_N为作业船目标航线的终点。

第二步，通过GPS/北斗导航系统天线实时测得作业船定位数据S及施药船航向，通过计算得到药液覆盖区中点位置Q；由于喷头的存在，药液向前方呈一定角度喷洒，在施药船作业时，GPS/北斗天线与药液覆盖区中点Q存在相对水平距离为H。

由于GPS/北斗导航装置与药液覆盖区中点Q处于同一条中轴线上，因此根据公式可得药液覆盖区中点Q(x₁,y₁)，计算公式为：

第三步，采用实时插点的方式得到作业船的跟踪目标点以及施药船直线航行时明轮的控制量。

本发明采用实时插点的方法，实时地计算出作业船的直线跟踪目标点，其计算示意图如图5所示；

Q(x₁,y₁)为药液覆盖区中点的当前位置，T_K(X_TK,Y_TK)、T_K+1(X_TK+1,Y_TK+1)为当前目标路径的起始点。其中，横坐标x表示经度，纵坐标y表示纬度，T_K→T_K+1表示施药船的目标航向路径；若此时作业船直接将T_K+1点作为跟踪目标点，其行走路线Q→T_K+1将会较大地偏离目标路径T_K→T_K+1。过Q点作垂直于路径T_K→T_K+1的直线相交于点M(x₂,y₂)，根据点到直线的垂直交点计算公式可得M点的坐标为：

其中QM为作业船偏离目标路径的距离，由经纬度坐标计算两点距离的公式可得M点到点T_K+1的距离d为：

其中，R＝6378137m为地球半径，d单位为m。

以投影点M点作为起始点沿着T_K→T_K+1方向前伸距离D₂至P(x₃,y₃)点，可得P(x₃,y₃)点坐标：

其中，D₂的值由施药船的船体尺寸、航行速度以及明轮驱动力等因素确定。假若D₂的值偏大则会导致算出的跟踪目标点与作业船位置相差较大，并且该误差很难回调；如若偏小则又会导致作业船对于航向的变化过于敏感，不利于系统的稳定。

在施药船直线航行作业时，明轮控制量分为电流PID控制量和航向PID控制量，电流PID控制量的大小与施药船船速成正比，通过电流PID的控制，左右明轮产生速度差，控制施药船的转向；而航向PID控制量则代表施药船当前位置Q(x₁,y₁)偏离目标航向点P(x₃,y₃)的方向航向。

第四步，通过称重传感器得到剩余药液重量及计算回字形路径总长度。

首先通过称重传感器对空药箱进行测量，得到空药箱的重量，再往药箱中存放药液后测得药箱与药液总体的重量，减去空药箱的重量，可得到药箱中剩余原液重量，通过流量调节阀之间用软管的连接，可消除其他部件对药箱称重的影响。

通过称重传感器检测，在施药作业开始之前，可得药液总重量。计算回字形路径总长度，再用药液总重量除以回字形路径总长度，可得到作业过程中作业路径上每单位长度施药量。对流量调节阀进行调节，使作业过程中作业路径上每单位长度施药量接近(等于)分配的药液重量。

第五步，根据全自动施药船转弯轨迹，确定转弯的跟踪点及目标航向。

如图6所示，可得作业船药液喷头覆盖区宽度为L,并且通过药液覆盖区中点Q确定作业船转弯圆心为O；

在转弯施药作业阶段时，药液覆盖区中点Q应以相切方式连接前后两段相互垂直的直线路径，可确定药液覆盖区中点Q的设定转弯路径是以O为圆心的1/4圆弧DE；

取圆心O距顶点A的距离为药液覆盖区宽度L的2倍，再根据三角形边长关系可得

从而求得转弯圆弧半径为

如图7所示，药液覆盖区(以中心Q为，长度为L，垂直于施药船航向的一段直线表示)与圆弧相交处N点的位置，沿圆弧(路径)前伸一段距离，确定跟踪目标点R，QR方向即为目标航向。沿圆弧前伸距离与船体大小、船速、明轮驱动力有关。

第六步，根据作业船的速度，通过控制器和流量调节阀对软管内的药液流量进行调控，使作业过程中作业路径上每单位长度施药量接近(等于)分配的药液重量，从而对作业船进行均匀洒药控制。

根据作业开始时得到的每单位长度路径施药量Q，通过GPS/北斗导航系统，可得到作业船的速度V，再通过流量调节阀对药液流量q大小进行调控，船速越快，则控制药液流量越大，药液流量与船速为正比关系。

q＝Q×V

通过对D、E二点间折线长度的计算，再乘以每单位长度施药量Q，可得转弯处的药液量。

综上，本发明的一种全自动施药船及均匀洒药方法，用于实现池塘全自动均匀施药。全自动施药船的作业方式从某一顶点开始，沿塘边间隔一段行程，在作业池塘中呈回字形路径作业。通过实时插点的算法以及转弯轨迹分析等控制方法来实现作业船的直线与转弯控制。最后本发明操作简单，可解决人工施药不均匀及人工效率低的问题，方便用户使用，可显著提高水产养殖效率，促进水产养殖业发展。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (4)

1.一种全自动施药船的均匀洒药方法，其特征在于，包括以下步骤：

第一步：在确定作业池塘呈多边形的条件下，使用GPS/北斗导航系统测取目标作业区边界处各个角点经纬度坐标，通过回字形路径规划，确定其作业边界、作业圈数以及回字形作业路径；

第二步：根据GPS/北斗导航系统的天线位置S及全自动施药船航向，计算出药液覆盖区中点Q点的位置信息；具体步骤如下：

步骤2.1，由GPS/北斗导航系统可以获得当前GPS/北斗天线位置为S(x₀,y₀)及当前的航向角为

由于喷头的存在，药液向前方呈一定角度喷洒，在施药船作业时，GPS/北斗天线与药液覆盖区中点Q存在相对水平距离为H；

步骤2.2，由于GPS/北斗导航装置与药液覆盖区中点Q处于同一条中轴线上，因此根据公式可得药液覆盖区中点Q(x₁,y₁)，计算公式为：

第三步：根据药液覆盖区域中点Q及自动施药船航向，通过实时插点算法得到施药船的跟踪目标点P，根据点到直线垂直交点的计算公式，得到药液覆盖区域中点Q的当前位置，在当前目标路径上的投影点，接着将上述投影点作为起点，再沿目标路径前进方向前伸一段距离可得到施药船的跟踪目标点P以及施药船航行时明轮的控制量，其中前伸的距离长度与施药船的船体尺寸、航行速度及明轮驱动力有关；

第四步：根据称重传感器测取药箱总重量，减去空药箱的重量后，可得到药箱中剩余药液重量，计算回字形路径总长度，结合药液总重量，可得到每单位长度的施药量；

第五步：根据全自动施药船转弯时，药液覆盖区中点Q的运动轨迹，确定转弯时的跟踪目标点及目标航向；

第六步：根据控制器和流量调节阀，对管道内的药液流量进行控制，使作业过程中作业路径上每单位长度施药量接近或者等于分配的药液重量，从而实现均匀洒药，具体步骤如下：

步骤6.1，根据施药船开始时得到的每单位长度路径施药量Q，通过GPS/北斗导航系统，可得到施药船的速度V，再通过流量调节阀对药液流量q大小进行调控，船速越快，使药液流量越大，药液流量与船速为正比关系：

q＝Q×V

步骤6.2，转弯处的药液量根据转弯路径的两个端点之间的折线长度计算，通过计算得到转弯路径的两个端点间的折线长度，再乘以每单位长度路径施药量Q后，可得到转弯处的药液量。

2.如权利要求1所述的一种全自动施药船的均匀洒药方法，其特征在于，所述第一步中通过回字形路径规划，确定其作业边界、作业圈数以及回字形作业路径，具体步骤如下：

步骤1.1，通过GPS/北斗导航系统确定池塘作业区域顶点,顺时针依次为A(x_α,y_α)、B(x_β,y_β)、C(x_γ,y_γ)、D(x_δ,y_δ)，上述四点即为施药船的作业边界顶点坐标；

步骤1.2，路径规划以回字形进行规划作业，从较长的矩形边开始进入规划作业，假设矩形窄边长为Y,施药船药液覆盖区宽度即作业幅宽为X，按照公式

确定回字形圈数，即可完成规划作业；

步骤1.3，确定作业路径T₀→T₁→...→T_K-1→T_K→T_K+1→...→T_N，并且将路径段T₀→T₁为施药船起始的目标路径，此处T₀,T₁...T_N为规划出的直线路径拐点。

3.如权利要求2所述的一种全自动施药船的均匀洒药方法，其特征在于，所述第三步采用实时插点的方法计算得到施药船的跟踪目标点P以及施药船航行时明轮的控制量，具体步骤如下：

步骤3.1，由第三步可知药液覆盖区中点的当前位置Q(x₁,y₁)在当前目标路径

T_K(x_TK,y_TK)→T_K+1(x_TK+1,y_TK+1)上的投影点为M(x₂,y₂)，计算的公式为：

其中，横坐标x表示经度，纵坐标y表示纬度，T_K→T_K+1表示施药船的目标航向路径；

步骤3.2，由两点经纬度坐标及计算两点之间距离的公式得到投影点M点到目标路径终点T_K+1的距离d，计算的公式为：

其中，R＝6378137m为地球半径，d单位为m；

步骤3.3，以投影点M点作为起始点沿着T_K→T_K+1方向前伸距离D₂至P(x₃,y₃)点，则P(x₃,y₃)点坐标的计算公式为：

式中D₂的值由施药船的船体尺寸、航行速度以及明轮驱动力因素确定；

步骤3.4，在施药船直线航行作业时，明轮控制量分为电流PID控制量和航向PID控制量，电流PID控制量的大小与施药船船速成正比，通过电流PID的控制，左右明轮产生速度差，控制施药船转向；而航向PID控制量则代表施药船药液覆盖区中点的当前位置Q(x₁,y₁)偏离跟踪目标点P(x₃,y₃)的方向航向。

4.如权利要求1所述的一种全自动施药船的均匀洒药方法，其特征在于，所述第四步通过称重传感器得到药箱中剩余药液重量及计算回字形路径总长度，具体步骤如下：

步骤4.1，通过称重传感器可对药液重量进行测量，在药箱中存放药液，减去空药箱的重量，可得到药箱中剩余药液重量，药箱与流量调节阀之间用软管进行连接，这样可消除其他部件对药箱称重的影响；

步骤4.2，通过称重传感器检测，在施药作业开始之前，可得药液总重量，计算回字形路径总长度，再用药液总重量除以回字形路径总长度，可得到作业过程中作业路径上每单位长度施药量，调节流量调节阀，使作业过程中作业路径上每单位长度施药量接近或者等于分配的药液重量。

Images