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

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

【技术实现步骤摘要】
一种池塘水草自动清理船的作业路径规划及控制方法
专利技术涉及路径规划及自动导航路径控制领域，特别是水草收割自动清理船的路径规划及导航跟踪控制。
技术介绍
我国的虾蟹养殖产业居世界首位,当前虾蟹养殖面临水草清理一大难题,其作业效率与质量相对偏低。随着养殖规模的逐步扩大和农业劳动力结构的改变,农业作业劳动力严重短缺,因此如何提高虾蟹养殖自动化、智能化水平成为亟待解决的问题。实现水草清理作业船智能化高效作业的关键在于避免漏割现象的产生，以及对于水草清理船的精确控制。
技术实现思路
因此本专利技术在基于常规的回字形作业路径规划的基础上提出一种基于叉字形作业路径规划方法，从而解决转弯漏割现象的问题，同时基于GPS/BD导航系统来进行插点算法，用以实现对作业船的直线航行高效作业控制，对于转弯作业控制，根据其转弯轨迹,在圆弧(路径)上前伸点来确定其跟踪目标点及目标航向。本专利技术的目的是为了解决现有的路径规划作业方式中存在的水草漏割问题，以及弥补作业船控制精度不高的不足。本专利技术的作业规划方法及控制方法可以实现水草高效收割，精确控制清理船按照设定航线作业。本专利技术设计的水草清理主要是虾蟹养殖池塘的水草清理。本专利技术为解决上述问题，采用的技术方案包含以下步骤：一种池塘水草自动清理船的作业路径规划及控制方法，包括以下步骤：第一步：在池塘水草生长区呈矩形分布条件下，使用基于GPS/BD导航系统采集能反映目标作业区轮廓的各个角点经纬度坐标，经回字形路径规划确定其作业边界、圈数以及回字形作业路径；第二步：基于矩形作业区域对角线长度大于任意边长，对于在转弯处留下的作业盲区，需对其进行叉字形路径规划，得到叉字形作业路径；第三步：根据GPS/BD导航系统的天线位置K及水草清理船航向，计算出割刀中点Q的位置信息；第四步：根据割刀中点位置Q及水草清理船航向，采用实时插点的方法计算得到作业船的跟踪目标点P：根据点到直线的垂直交点的计算公式，得到作业船的当前位置在当前目标路径上的投影点；然后以上述得到的投影点为起点，沿着目标路径前进方向前伸一定距离得到作业船的跟踪目标点P，其中前伸的距离值由作业船的船体尺寸、航行速度以及明轮驱动力等因素确定；第五步：水草清理船转弯时，割刀中点的规划路径是1/4圆弧，根据水草清理船转弯时，割刀中点的规划路径，确定转弯时的跟踪目标点及目标航向；第六步：根据作业船的当前运动状态与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差，从而对作业船进行作业控制。进一步，所述第一步中经回字形路径规划，得到作业船作业边界、作业圈数以及回字形作业路径，具体步骤如下：步骤1.1，通过GPS/BD导航系统确定矩形作业区域四个顶点,顺时针依次为A(xA，yA)、B(xB，yB)、C(xC，yC)、D(xD，yD)，四点即为作业船的作业边界；步骤1.2，按回字形进行直线路径规划，从矩形较长边开始进入规划作业，假设矩形窄边长为Y,清理船割刀宽度即作业幅宽为X，按照公式(向上取整)确定回字形圈数，即可完成作业；步骤1.3，确定多圈回字形直线路径的拐点T0→T1→...→Tk-1→Tk→Tk+1→...→TN，并且确定路径T0→T1为作业船起始的目标路径。此处T0,T1...TN为图2规划出的直线路径拐点。进一步，所述第二步中由于按回字形进行作业规划时，在转弯处会留下作业盲区，需进行叉字形路径规划，才可将水草完全清理，具体步骤如下：步骤2.1，由于在步骤1确定的矩形作业区域中，矩形对角线长度大于任意边长，在转弯处会留下作业盲区，此时需要进行叉字形作业规划；步骤2.2，根据作业船回字形作业完毕时停留的位置点①，按照叉字形顺序将所有回字形转弯处漏割区域进行再次作业；步骤2.3，按照①→②→...→⑨→⑩路径规划出最终的叉字形作业路径，最终停留在位置⑩，①→②→...→⑨→⑩为叉字形作业规划拐点，水草即可清理完毕。此处①…⑩即为图3规划出的叉字形拐点。进一步，所述第三步中由GPS/BD导航获得的位置，经计算得到割刀中点位置，具体过程如下：步骤3.1，由GPS/BD导航系统可以获得当前接收机位置为K(x0，y0)，以及当前的航向角为GPS/BD天线距割刀中点水平距离为H；步骤3.2，由于安装的GPS/BD导航装置与割刀中点位于中轴线上，因此根据公式可得割刀中点为Q(x1,y1)，计算公式为：进一步，所述第四步采用实时插点的方法计算得到作业船的跟踪目标点P，具体步骤如下：步骤4.1，由第三步知割刀中点的当前位置Q(x1,y1)在当前目标路径TK(xTK,yTK)→TK+1(xTK+1,yTK+1)上的投影点为M(x2,y2)，计算的公式为：其中，TK→TK+1为当前某一条目标作业路径，点M为当前割刀位置Q在当前作业路径的垂直投影点，横坐标x表示经度，纵坐标y表示纬度；步骤4.2，由两点经纬度坐标及计算两点距离的公式得到投影点M点到目标路径终点Tk+1的距离d，计算的公式为：其中，R＝6378137m为地球半径，d单位为m；步骤4.3，以投影点M点作为起始点沿着TK→TK+1方向前伸距离D2至P(x3,y3)点，P(x3,y3)点坐标的计算公式为：式中D2的值由作业船的船体尺寸、航行速度以及明轮驱动力等因素确定。进一步，所述第五步中，根据水草清理船转弯时，割刀中点的规划路径，确定转弯时的跟踪目标点及目标航向，具体步骤如下：步骤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导航系统进行回字形作业路径规划后，再本文档来自技高网...

【技术保护点】
1.一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，包括以下步骤：/n第一步：在池塘水草生长区呈矩形分布条件下，使用基于GPS/BD导航系统采集能反映目标作业区轮廓的各个角点经纬度坐标，经回字形路径规划确定其作业边界、圈数以及回字形作业路径；/n第二步：基于矩形作业区域对角线长度大于任意边长，对于在转弯处留下的作业盲区，需对其进行叉字形路径规划，得到叉字形作业路径；/n第三步：根据GPS/BD导航系统的天线位置K及水草清理船航向，计算出割刀中点Q的位置信息；/n第四步：根据割刀中点位置Q及水草清理船航向，采用实时插点的方法计算得到作业船的跟踪目标点P：根据点到直线的垂直交点的计算公式，得到作业船的当前位置在当前目标路径上的投影点；然后以上述得到的投影点为起点，沿着目标路径前进方向前伸一定距离得到作业船的跟踪目标点P，其中前伸的距离值由作业船的船体尺寸、航行速度以及明轮驱动力等因素确定；/n第五步：水草清理船转弯时，割刀中点的规划路径是1/4圆弧，根据水草清理船转弯时，割刀中点的规划路径，确定转弯时的跟踪目标点及目标航向；/n第六步：根据作业船的当前运动状态与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差，从而对作业船进行作业控制。/n...

【技术特征摘要】
1.一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，包括以下步骤：
第一步：在池塘水草生长区呈矩形分布条件下，使用基于GPS/BD导航系统采集能反映目标作业区轮廓的各个角点经纬度坐标，经回字形路径规划确定其作业边界、圈数以及回字形作业路径；
第二步：基于矩形作业区域对角线长度大于任意边长，对于在转弯处留下的作业盲区，需对其进行叉字形路径规划，得到叉字形作业路径；
第三步：根据GPS/BD导航系统的天线位置K及水草清理船航向，计算出割刀中点Q的位置信息；
第四步：根据割刀中点位置Q及水草清理船航向，采用实时插点的方法计算得到作业船的跟踪目标点P：根据点到直线的垂直交点的计算公式，得到作业船的当前位置在当前目标路径上的投影点；然后以上述得到的投影点为起点，沿着目标路径前进方向前伸一定距离得到作业船的跟踪目标点P，其中前伸的距离值由作业船的船体尺寸、航行速度以及明轮驱动力等因素确定；
第五步：水草清理船转弯时，割刀中点的规划路径是1/4圆弧，根据水草清理船转弯时，割刀中点的规划路径，确定转弯时的跟踪目标点及目标航向；
第六步：根据作业船的当前运动状态与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差，从而对作业船进行作业控制。


2.如权利要求1所述的一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，所述第一步中经回字形路径规划，得到作业船作业边界、作业圈数以及回字形作业路径，具体步骤如下：
步骤1.1，通过GPS/BD导航系统确定矩形作业区域四个顶点,顺时针依次为A(xA，yA)、B(xB，yB)、C(xC，yC)、D(xD，yD)，四点即为作业船的作业边界；
步骤1.2，按回字形进行直线路径规划，从矩形较长边开始进入规划作业，假设矩形窄边长为Y,清理船割刀宽度即作业幅宽为X，按照公式(向上取整)确定回字形圈数，即可完成作业；
步骤1.3，确定多圈回字形直线路径的拐点T0→T1→...→Tk-1→Tk→Tk+1→...→TN，并且确定路径T0→T1为作业船起始的目标路径，此处T0,T1...TN为规划出的直线路径拐点。


3.如权利要求1所述的一种池塘水草自动清理船的作业路径规划及控制方法，其特征在于，所述第二步中由于按回字形进行作业规划时，在转弯处会留下作业盲区，需进行叉字形路径规划，才可将水草完全清理，具体步骤如下：
步骤2.1，由于在步骤1确定的矩形作业区域中，矩形对角线长度大于任意边长，在转弯处会留下作业盲区，此时需要进行叉字形作业规划；
步骤2.2，根据作业船回字形作业完毕时停留的位置点①，按照叉字形顺序将所有回字形转弯处漏割区域进行再次作业；
步骤2.3，按照①→②→...→⑨→⑩路径规划出最终的叉字形作业路径，最终停留在位置⑩，水草即可清理完毕，此处①…⑩为规划出的叉字形拐点。


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

【专利技术属性】
技术研发人员：赵德安，戚浩，
申请(专利权)人：江苏大学，
类型：发明
国别省市：江苏;32