CN111522336A - 一种池塘水草自动清理船的作业路径规划及控制方法 - Google Patents

Abstract

本发明公开了一种池塘水草自动清理船的作业路径规划及控制方法。该方法包括以下步骤：第一步：在池塘水草生长区呈矩形分布条件下，经回字形路径规划，得到作业边界、圈数以及回字形路径；第二步：基于矩形对角线长度大于任意边长，对位于对角线的作业盲区进行叉字形路径规划，得到全部作业路径；第三步：利用GPS/BD导航系统实时接收位置信息，计算得到割刀中点位置；第四步：利用实时插点算法计算清理船直线航行的目标点；第五步：根据割刀中点的转弯路径，确定转弯时的目标点及目标航向；第六步：根据作业船当前运动状态与目标点及目标路径对比，对作业船进行控制。本发明能显著提高割草效率、实现对作业船的精确控制，为作业船高效作业提供保障。

Description

一种池塘水草自动清理船的作业路径规划及控制方法

技术领域

发明涉及路径规划及自动导航路径控制领域，特别是水草收割自动清理船的路径规划及导航跟踪控制。

背景技术

我国的虾蟹养殖产业居世界首位,当前虾蟹养殖面临水草清理一大难题,其作业效率与质量相对偏低。随着养殖规模的逐步扩大和农业劳动力结构的改变,农业作业劳动力严重短缺,因此如何提高虾蟹养殖自动化、智能化水平成为亟待解决的问题。实现水草清理作业船智能化高效作业的关键在于避免漏割现象的产生，以及对于水草清理船的精确控制。

发明内容

因此本发明在基于常规的回字形作业路径规划的基础上提出一种基于叉字形作业路径规划方法，从而解决转弯漏割现象的问题，同时基于GPS/BD导航系统来进行插点算法，用以实现对作业船的直线航行高效作业控制，对于转弯作业控制，根据其转弯轨迹,在圆弧(路径)上前伸点来确定其跟踪目标点及目标航向。

本发明的目的是为了解决现有的路径规划作业方式中存在的水草漏割问题，以及弥补作业船控制精度不高的不足。本发明的作业规划方法及控制方法可以实现水草高效收割，精确控制清理船按照设定航线作业。

本发明设计的水草清理主要是虾蟹养殖池塘的水草清理。

本发明为解决上述问题，采用的技术方案包含以下步骤：

一种池塘水草自动清理船的作业路径规划及控制方法，包括以下步骤：

第一步：在池塘水草生长区呈矩形分布条件下，使用基于GPS/BD导航系统采集能反映目标作业区轮廓的各个角点经纬度坐标，经回字形路径规划确定其作业边界、圈数以及回字形作业路径；

第二步：基于矩形作业区域对角线长度大于任意边长，对于在转弯处留下的作业盲区，需对其进行叉字形路径规划，得到叉字形作业路径；

第三步：根据GPS/BD导航系统的天线位置K及水草清理船航向，计算出割刀中点Q的位置信息；

第四步：根据割刀中点位置Q及水草清理船航向，采用实时插点的方法计算得到作业船的跟踪目标点P：根据点到直线的垂直交点的计算公式，得到作业船的当前位置在当前目标路径上的投影点；然后以上述得到的投影点为起点，沿着目标路径前进方向前伸一定距离得到作业船的跟踪目标点P，其中前伸的距离值由作业船的船体尺寸、航行速度以及明轮驱动力等因素确定；

第五步：水草清理船转弯时，割刀中点的规划路径是1/4圆弧，根据水草清理船转弯时，割刀中点的规划路径，确定转弯时的跟踪目标点及目标航向；

第六步：根据作业船的当前运动状态与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差，从而对作业船进行作业控制。

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

步骤1.1，通过GPS/BD导航系统确定矩形作业区域四个顶点,顺时针依次为A(x_A，y_A)、B(x_B，y_B)、C(x_C，y_C)、D(x_D，y_D)，四点即为作业船的作业边界；

步骤1.2，按回字形进行直线路径规划，从矩形较长边开始进入规划作业，假设矩形窄边长为Y,清理船割刀宽度即作业幅宽为X，按照公式

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

步骤1.3，确定多圈回字形直线路径的拐点T₀→T₁→...→T_k-1→T_k→T_k+1→...→T_N，并且确定路径T₀→T₁为作业船起始的目标路径。此处T₀,T₁...T_N为图2规划出的直线路径拐点。

进一步，所述第二步中由于按回字形进行作业规划时，在转弯处会留下作业盲区，需进行叉字形路径规划，才可将水草完全清理，具体步骤如下：

步骤2.1，由于在步骤1确定的矩形作业区域中，矩形对角线长度大于任意边长，在转弯处会留下作业盲区，此时需要进行叉字形作业规划；

步骤2.2，根据作业船回字形作业完毕时停留的位置点①，按照叉字形顺序将所有回字形转弯处漏割区域进行再次作业；

步骤2.3，按照①→②→...→⑨→⑩路径规划出最终的叉字形作业路径，最终停留在位置⑩，①→②→...→⑨→⑩为叉字形作业规划拐点，水草即可清理完毕。此处①…⑩即为图3规划出的叉字形拐点。

进一步，所述第三步中由GPS/BD导航获得的位置，经计算得到割刀中点位置，具体过程如下：

步骤3.1，由GPS/BD导航系统可以获得当前接收机位置为K(x₀，y₀)，以及当前的航向角为

GPS/BD天线距割刀中点水平距离为H；

步骤3.2，由于安装的GPS/BD导航装置与割刀中点位于中轴线上，因此根据公式可得割刀中点为Q(x₁,y₁)，计算公式为：

进一步，所述第四步采用实时插点的方法计算得到作业船的跟踪目标点P，具体步骤如下：

步骤4.1，由第三步知割刀中点的当前位置Q(x₁,y₁)在当前目标路径T_K(x_TK,y_TK)→T_K+1(x_TK+1,y_TK+1)上的投影点为M(x₂,y₂)，计算的公式为：

其中，T_K→T_K+1为当前某一条目标作业路径，点M为当前割刀位置Q在当前作业路径的垂直投影点，横坐标x表示经度，纵坐标y表示纬度；

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

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

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

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

进一步，所述第五步中，根据水草清理船转弯时，割刀中点的规划路径，确定转弯时的跟踪目标点及目标航向，具体步骤如下：

步骤5.1，由于作业船轨迹跟踪是由割刀中点确定的，根据作业船割刀幅宽确定作业船转弯圆心O；

步骤5.2，由于在转弯割草作业阶段时，割刀中点Q应以相切的方式连接前后两段相互垂直的直线路径，从而可确定割刀中点的规划路径是以O为圆心的1/4圆弧DE；

步骤5.3，将作业船转弯时漏割区域的宽度设定为割刀幅宽的2倍，圆心O距顶点A的距离也是割刀幅宽的2倍，且圆弧半径为

此处L为割刀的作业幅宽；

步骤5.4，根据割刀与割刀规划转弯路径DE相交处H点的位置，沿圆弧(路径)前伸一段距离，确定跟踪目标点P，QP方向即为目标航向，沿圆弧前伸距离与船体大小、船速、明轮驱动力有关。

7.如权利要求1所述的一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，所述第六步中，根据作业船的当前运动状态与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差，从而对作业船进行作业控制，具体步骤如下：

步骤6.1，在给定左右明轮电机一个产生基速的电压基础上，将航向角偏差值e导入电机驱动控制系统，利用PID调节器产生左右明轮的差模电压信号，将差模电压信号导入左右明轮电机，产生左右电机的转速差，控制作业船转向，从而达到对作业船的航向控制。

本发明的有益效果为：该发明技术特点在于全区域，全覆盖池塘作业时，在利用GPS/BD导航系统进行回字形作业路径规划后，再进行叉字形作业规划，解决了转弯时水草漏割的问题，同时利用实时插点的方法对直线进行航行控制，利用割刀中点转弯的轨迹分析来进行转弯控制，最后通过PID调节器来产生差模信号，从而控制作业船的高效作业。

附图说明

图1池塘水草清理船结构示意图

图2回字形作业规划示意图

图3叉字形作业规划示意图

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

图5割刀中心转向示意图

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

具体实施方式

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

本实施方式采用的水草清理船的结构如图1所示，船上包括船体，明轮，割刀以及GPS/BD导航系统。

具体实施步骤如下：

第1步，设定作业船作业边界、作业圈数以及作业路径。

如图2回字形作业规划示意图所示，本发明在池塘水草生长区呈矩形分布环境下，使用高精度GPS/BD导航系统采集作业区域四个顶点的坐标位置，顺时针依次记为A(x_A，y_A)、B(x_B，y_B)、C(x_C，y_C)、D(x_D，y_D)，四点即为作业船的作业边界。

接着，测得矩形窄边长度为Y，清理船割刀宽度为X，按照公式

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

最后，以矩形较长边为作业起始位置。确定路径T₀→T₁→...→T_k-1→T_k→...→T_N，并且确定路径T₀→T₁为作业船起始目标路径。

对应的目标点集合为：

{T₀，T₁，...，T_k-1，T_k，T_k+1，...，T_N}；

其中T₀,T₁...T_N为图2规划出的直线路径拐点；

其经纬度坐标为：

{(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为作业船目标航线的终点。

如图5所示，这种方式可以在经过最少的作业圈数之后，实现全覆盖作业，但是在转弯处会存在作业盲区，导致水草漏割问题。

第2步，基于矩形的对角线长度大于任意边长，回字形规划将会产生作业盲区，需要进行叉字形路径规划。

如图3叉字形作业规划示意图所示，作业船回字形作业完毕时停留在位置点①；

按照叉字形顺序将所有回字形转弯处漏割区域进行再次路径规划,可以在经过最短距离之后，实现漏割区域的收割；

按照①→②→...→⑨→⑩路径规划出最终的叉字形作业路径，最终停留在位置⑩，水草即可清理完毕。

其中具体叉字形顺序是根据回字形作业最终的停留点确定的，同时①…⑩即为图3规划出的叉字形拐点。

第3步，通过GPS/BD导航系统实时测得定位数据K，通过计算得到割刀中点位置Q；

由GPS/BD导航系统可以获得当前接收机位置为K(x₀，y₀)，同时测得当前的航向角为

GPS/BD导航天线距割刀中点水平距离为H；

由于安装的GPS/BD导航装置与割刀中点位于中轴线上，因此根据公式可得割刀中点为Q(x₁,y₁)，计算公式为：

第4步，采用实时插点的方式得到作业船的跟踪目标点。

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

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点，可求得P(x₃，y₃)点的坐标为：

其中，D₂的值由作业船的船体尺寸、航行速度以及明轮驱动力等因素确定，D₂的值过大时会导致计算得出的跟踪目标点距离作业船较远，当作业船偏离目标路径时很难回调；D₂的值过小时又会导致作业船对于航向的变化过于敏感，造成系统的震荡。

接着，割刀将以点P作为跟踪目标点对作业船进行自动导航控制。当船体由Q运动至Q'时，跟踪目标点也同时向前运动至P'，直至到达T_k+1点为止。

这种方法能够实时地反映出航向的误差，将有限个角点细化成无数个跟踪目标点，可以使作业船的航行轨迹更加地逼近设定路径，能显著提高作业船自动导航路径跟踪的效果。

第5步，水草清理船转弯时，割刀中点的规划路径是1/4圆弧。根据割刀中点的规划路径，确定作业船转弯跟踪点及目标航向。

如图5所示，割刀作业幅宽设为L,同时将作业转弯时漏割区域的宽度设定为割刀的2倍，即2L；

在转弯割草作业阶段，割刀中点Q应以相切方式连接前后两段相互垂直的直线路径，从而可确定割刀中点是以O为圆心的1/4圆弧DE；

圆心O点距顶点A的距离也为割刀宽度的两倍，即2L，同时由三角形关系知

从而求得转弯圆弧半径

其中L为割刀的作业幅宽；

如图6所示，根据割刀与割刀转弯路径DE相交点H的位置，沿圆弧(路径)前伸一段距离，即可确定跟踪目标点P，QP方向即为目标航向。其中，沿圆弧前伸距离与船体大小、船速以及明轮驱动力有关。

第六步，由作业船的当前运动状态与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差，从而对作业船进行作业航行控制。

在给定左右明轮电机一个产生基速的电压基础上，将航向角偏差值e导入电机驱动控制系统，利用PID调节器产生左右明轮的差模电压信号，将差模电压信号导入左右明轮电机，产生左右电机的转速差，控制作业船转向。

从而达到对作业船的航向控制。

综上，本发明的一种池塘水草自动清理船的作业路径规划及控制方法，用于实现池塘水草自动清理。自动作业船的作业方式一方面是从某点出发，沿塘堤平行，间隔一定行程的回字形路径，另一方面是基于回字形终点开始的叉字形作业路径。控制方法是利用实时插点的算法以及转弯轨迹分析来实现明轮的直线与转弯控制。最后利用PID调节器产生左右明轮的差模电压信号，将差模电压信号导入左右明轮电机，产生左右电机的转速差，控制作业船转向。

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

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

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

Claims (7)

1.一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，包括以下步骤：

第一步：在池塘水草生长区呈矩形分布条件下，使用基于GPS/BD导航系统采集能反映目标作业区轮廓的各个角点经纬度坐标，经回字形路径规划确定其作业边界、圈数以及回字形作业路径；

第二步：基于矩形作业区域对角线长度大于任意边长，对于在转弯处留下的作业盲区，需对其进行叉字形路径规划，得到叉字形作业路径；

第三步：根据GPS/BD导航系统的天线位置K及水草清理船航向，计算出割刀中点Q的位置信息；

第四步：根据割刀中点位置Q及水草清理船航向，采用实时插点的方法计算得到作业船的跟踪目标点P：根据点到直线的垂直交点的计算公式，得到作业船的当前位置在当前目标路径上的投影点；然后以上述得到的投影点为起点，沿着目标路径前进方向前伸一定距离得到作业船的跟踪目标点P，其中前伸的距离值由作业船的船体尺寸、航行速度以及明轮驱动力等因素确定；

第五步：水草清理船转弯时，割刀中点的规划路径是1/4圆弧，根据水草清理船转弯时，割刀中点的规划路径，确定转弯时的跟踪目标点及目标航向；

第六步：根据作业船的当前运动状态与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差，从而对作业船进行作业控制。

2.如权利要求1所述的一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，所述第一步中经回字形路径规划，得到作业船作业边界、作业圈数以及回字形作业路径，具体步骤如下：

步骤1.1，通过GPS/BD导航系统确定矩形作业区域四个顶点,顺时针依次为A(x_A，y_A)、B(x_B，y_B)、C(x_C，y_C)、D(x_D，y_D)，四点即为作业船的作业边界；

步骤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.如权利要求1所述的一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，所述第二步中由于按回字形进行作业规划时，在转弯处会留下作业盲区，需进行叉字形路径规划，才可将水草完全清理，具体步骤如下：

步骤2.1，由于在步骤1确定的矩形作业区域中，矩形对角线长度大于任意边长，在转弯处会留下作业盲区，此时需要进行叉字形作业规划；

步骤2.2，根据作业船回字形作业完毕时停留的位置点①，按照叉字形顺序将所有回字形转弯处漏割区域进行再次作业；

步骤2.3，按照①→②→...→⑨→⑩路径规划出最终的叉字形作业路径，最终停留在位置⑩，水草即可清理完毕，此处①…⑩为规划出的叉字形拐点。

4.如权利要求1所述的一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，所述第三步中由GPS/BD导航获得的位置，经计算得到割刀中点位置，具体过程如下：

步骤3.1，由GPS/BD导航系统可以获得当前接收机位置为K(x₀，y₀)，以及当前的航向角为

GPS/BD天线距割刀中点水平距离为H；

步骤3.2，由于安装的GPS/BD导航装置与割刀中点位于中轴线上，因此根据公式可得割刀中点为Q(x₁,y₁)，计算公式为：

。

5.如权利要求1所述的一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，所述第四步采用实时插点的方法计算得到作业船的跟踪目标点P，具体步骤如下：

步骤4.1，由第三步知割刀中点的当前位置Q(x₁,y₁)在当前目标路径T_K(x_TK,y_TK)→T_K+1(x_TK+1,y_TK+1)上的投影点为M(x₂,y₂)，计算的公式为：

其中，T_K→T_K+1为当前某一条目标作业路径，点M为当前割刀位置Q在当前作业路径的垂直投影点，横坐标x表示经度，纵坐标y表示纬度；

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

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

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

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

6.如权利要求1所述的一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，所述第五步中，根据水草清理船转弯时，割刀中点的规划路径，确定转弯时的跟踪目标点及目标航向，具体步骤如下：

步骤5.1，由于作业船轨迹跟踪是由割刀中点确定的，根据作业船割刀幅宽确定作业船转弯圆心O；

步骤5.2，由于在转弯割草作业阶段时，割刀中点Q应以相切的方式连接前后两段相互垂直的直线路径，从而可确定割刀中点的规划路径是以O为圆心的1/4圆弧DE；

步骤5.3，将作业船转弯时漏割区域的宽度设定为割刀幅宽的2倍，圆心O距转弯顶点A的距离也是割刀幅宽的2倍，且圆弧半径为

此处L为割刀的作业幅宽；

步骤5.4，根据割刀与割刀规划转弯路径DE相交处H点的位置，沿圆弧(路径)前伸一段距离，确定跟踪目标点P，QP方向即为目标航向，沿圆弧前伸距离与船体大小、船速、明轮驱动力有关。

7.如权利要求1所述的一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，所述第六步中，根据作业船的当前运动状态与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差，从而对作业船进行作业控制，具体步骤如下：

步骤6.1，在给定左右明轮电机一个产生基速的电压基础上，将航向角偏差值e导入电机驱动控制系统，利用PID调节器产生左右明轮的差模电压信号，将差模电压信号导入左右明轮电机，产生左右电机的转速差，控制作业船转向，从而达到对作业船的航向控制。

Images