一种水稻无人驾驶插秧机全路径规划及断点续航方法与流程

本发明涉及一种水稻无人驾驶插秧机全路径规划及断点续航方法，属于智能化农业装备领域。

背景技术：

我国是世界水稻种植大国，种植面积约3千万公顷。水稻插秧劳动强度大，季节性强，需抢农时种植。近年来随着农村劳动力向城市的大规模转移，人工插秧成本不断增加，且由于随着壮年劳动力向城市转移，农村剩余劳动力多为老人和妇女，劳动能力有限，人工插秧占用人工多、农村劳动力短缺与水稻抢时种植三者间的矛盾越来越突出。

随着国家对农业自动化机械化的投入越来越多，为解决劳动力不足的问题而研发的集自主导航、自动驾驶为一体的无人驾驶插秧机越来越多。但是无人驾驶插秧机的载秧量是固定的，所以无人驾驶插秧机在秧苗不足时需要离开当前作业点前去补充秧苗，加上运苗车无法下田所以补充秧苗的都在田头。如果没有断点续航，无人驾驶插秧机在离开当前作业点前往田头补充秧苗就需要人工驾驶，这就将大大增加人力成本和降低工作效率。而且传统的无人驾驶插秧机规划的路径无法覆盖上下田头。如果没有全路径规划，无人驾驶作业时上下田头会出现无法栽插的情况，只能后期人工驾驶栽插，这也将大大增加成本。加上插秧机的作业区域在机体的后方，如果没有合适的转弯控制无人驾驶插秧机在补充完秧苗回到原始作业点时将会出现压苗和漏插的情况，这将大大降低工作效率增加不必要的成本。所以性能好的全路径规划、断点续航和转弯控制方法对于提高作业效率、降低无人驾驶农机因自身需求离开规划作业区域后从新回到作业点继续完成作业这个过程所消耗的时间具有重要意义。

专利(专利号为201711367119.2)公开了一种旋翼机的断点续航控制方法及系统，在执行整个航线任务时检测到突发状况，启动返航时，记录当前返航点，并发送至所述旋翼机的地面控制端；在所述旋翼机完成突发任务后在地面控制端的操作人员的控制下返回到记录的返航点执行航线任务中的后继航线任务。实现了执行自动航线任务时完成了断点续航，可以有效的提高作业效率，降低用户的操作难度和繁琐性，但由于无人驾驶农机作业时，由于对导航精度要求很高，而该专利是大致范围的断点续航精度很低误差很大，按照该方法进行无人驾驶农机作业时的断点续航，则必然会造成插秧机的已插秧的秧苗进行压损，完全不适合精度要求很高的无人驾驶插秧机。

技术实现要素：

为弥补上述不足，本发明提出了一种水稻无人驾驶插秧机全路径规划及断点续航方法，旨在提高无人驾驶插秧机栽插区域和效率，降低转弯难度和缩短转弯时间。

为了解决以上技术问题，本发明采用的具体技术方案如下：

1一种水稻无人驾驶插秧机全路径规划及断点续航方法，包括以下步骤：

步骤一，获取所要作业区域的顶点坐标并计算短边和长边宽度：

利用北斗/gps导航定位装置获取作业区域的顶点坐标，考虑到一般机械化插秧的田块为平行四边形或近似平行四边形的形状，根据获取的田块顶点坐标，计算出长边长度j和短边长度k；

步骤二，把要作业的大田块分成四个部分并对各区域进行全路径规划：

考虑到一般机械化插秧的田块都较大，为了节约时间，方便补充秧苗把大田块分成四个部分并分别进行全路径规划；

步骤三，根据划分好的各区域的田块四边计算各区域作业插秧机距离田块四边a,b,c,d的距离：

通过步骤一可得知大田块的长短边大小，加上路宽l都是固定的，北斗/gps导航定位装置给是实时坐标信息就可以算出各区域作业插秧机到各自田块四边的距离a1，b1，c1，d1；

步骤四，在开启断点续航模式时在排除掉已栽插区域边后根据获得的到各边的距离规划路径前往距离最近的田块边界进行补苗；

若插秧机从左向右作业时则排除掉田块a边，然后比较b1，c1，d1的大小然后进行路径规划，若插秧机从右向左作业则排除掉田块c边，然后比较a1，b1，d1的大小进行路径规划；

步骤五，计算运苗车与各区域作业插秧机的实时距离e,f,g,h，当插秧机需要补苗而开启断点续航模式时根据实时的距离去给相距最近的插秧机最后进行补秧苗；

各区域作业的插秧机和运苗车都装有北斗/gps导航定位装置，根据它们在田块中的实时位置坐标就可以算出运苗车与各区域作业插秧机之间的距离；

步骤六，根据开启断点续航时所规划的路径选择转弯控制回到原始作业点。

进一步，根据步骤一获取的田块顶点坐标a(x1,y1)、b(x2,y2)、c(x3,y3)和d(x4,y4)，计算出长边长度j和短边长度k：

若j<k，则j与k的值交换。

进一步，步骤二中，根据大田块的长边与短边的长度平均分成四部分并根据分完各区域的长短边长度加上田块的上下田头进行全路径规划，然后根据实际情况选择从左到右作业还是从右到左作业。

进一步，步骤三中，已知大田块长边j、短边k和路宽l的大小，加上通过北斗/gps导航定位装置可获得各区域作业插秧机实时位置坐标e(x5,y5)，f(x6,y6)，g(x7,y7)，h(x8,y8)，则左上角作业区域插秧机距离田块四边的距离为：

距离田块a边距离为：

a1＝x5

距离田块b边距离为：

b1＝j-y5

距离田块c边距离为：

距离田块d边距离为：

左下角作业区域插秧机距离田块四边的距离为：

距离田块a边距离为：

a1＝x6

距离田块b边距离为：

距离田块c边距离为：

距离田块d边距离为：

d1＝y6

右上角作业区域插秧机距离田块四边的距离为：

距离田块a边距离为：

距离田块b边距离为：

b1＝j-y7

距离田块c边距离为：

c1＝k-x7

距离田块d边距离为：

右下角作业区域插秧机距离田块四边的距离为：

距离田块a边距离为：

距离田块b边距离为：

距离田块c边距离为：

c1＝k-x8

距离田块d边距离为：

d1＝y8。

进一步，所述步骤四中，若插秧机选择从左向右作业时则只比较b1，c1，d1的大小，选择距离最短的田块边为上秧苗点规划断点续航路径，如果开启断点续航的位置刚好离作业前进方向田块边距离最短，首先记录当前位置坐标并作为断点续航的起始点，然后以此点到上秧苗点减去二分之一作业幅宽那个点作为终点来规划前往上秧苗点路径；并根据插秧机作业幅宽和转弯半径在左边相隔1.5个作业幅宽规划补完秧苗回来的路径；如果开启断点续航的位置刚好离作业前进方向相反的田块边距离最短时，同样首先记录当前位置坐标并作为断点续航的起始点，然后以此点向下两米处为第二点作为转弯标志点，掉头前往上侧田边的上秧苗点，根据插秧机作业幅宽和转弯半径以转弯标志点向左侧两个作业幅宽规划转弯完成前往上秧苗点的路径及以开始断点续航点向左一个作业幅宽为终点规划上完秧苗回到原始作业点路径；当插秧机作业在田块四边的作业行时，因为作业行都在田块边，所以不再比较到各田块边界的距离，当插秧机需要补苗时，立刻停车原地等待运苗车前来补充秧苗。

进一步，所述步骤五中，通过北斗/gps导航定位装置可获得各区域作业插秧机实时位置坐标e(x5,y5)，f(x6,y6)，g(x7,y7)，h(x8,y8)和上秧苗车的实时位置i(x9,y9)，所以运苗车与左上区域插秧机实时距离：

运苗车与左下区域插秧机实时距离：

运苗车与右上区域插秧机实时距离：

运苗车与右下区域插秧机实时距离：

进一步，所述步骤六中，如果开启断点续航的位置刚好离作业前进方向田块边距离最短，上秧苗的点在前进方向，所以上完秧苗后回到原始作业点时选择的如下转弯控制：行驶到距离开启断点续航的初始位置5m时，首先把航姿传感器校准置零，控制电动方向盘向指定方向转动610°，然后保持转弯角度不变，即固定转向半径，同时实时检测航姿传感器的数值，当航姿传感器的值从零变化到-175时，控制电动方向盘向要转弯的相反方向打880°，接着根据导航给的航向偏差和横向偏差回到开启断点续航时的航向，最后再启动倒车模式回到开启断点续航时的位置；

如果开启断点续航的位置刚好离作业前进方向相反的田块边距离最短时，上秧苗的点与前进方向相反，所以选择的转弯控制为：行驶到距离开启断点续航的初始位置2m时，控制电动方向盘向指定方向转动300°，然后保持转弯角度不变，同时实时检测以开启断点续航初始点所在航线为基准的横向偏差(即同时实时检测以如图5所示中的①位置所在航线为基准的横向偏差)，当横向偏差小于10cm时，然后控制电动方向盘往反方向打350°，并把速度降到0.2m/s，此时实时检测航向偏差和横向偏差回到开启断点续航时的航向，然后在启动倒车模式回到开启断点续航时的位置。

本发明具有有益效果：

本发明一种水稻无人驾驶插秧机全路径规划及断点续航方法，通过采用大田块作业分区域且各区域去路径规划、根据判断距离各田块边界大小智能规划断点续航路径和根据规划的路径智能选择转弯控制，降低插秧机地头无法插秧和当插秧机秧苗不足需要离开当前作业点补充秧苗后回到初始作业点的转弯对行难度，缩短插秧机转弯和准确对行时间，大幅度提高作业区域的机插面积和作业效率，为插秧机的无人驾驶奠定基础。

附图说明

图1大田块分区域全路径规划、断点续航路径规划和配合运苗车作业总示意图；

图2开启断点续航时离作业前进方向田块边距离最短时规划的断点续航路径示意图；

图3规划如图2所示断点续航路径回到原始作业点选择的转弯控制各阶段实际效果图；

图4规划如图2所示断点续航路径回到原始作业点选择的转弯控制示意图

图5开启断点续航时离作业前进相反方向田块边距离最短时规划的断点续航路径示意图；

图6规划如图5所示断点续航路径回到原始作业点选择的转弯控制各阶段实际效果图；

图7规划如图5所示断点续航路径回到原始作业点选择的转弯控制示意图

图中：a(x1,y1)、b(x2,y2)、c(x3,y3)和d(x4,y4)-大田块四个顶点坐标；e(x5,y5)、f(x6,y6)、g(x7,y7)和h(x8,y8)–各区域作业插秧机实时位置坐标；i(x9,y9)–运苗车实时位置坐标。

具体实施方式

下面结合附图及实施方式对本发明的技术方案作进一步详细说明。

如图1所示，为大田块分区域全路径规划、断点续航路径规划和配合运苗车作业总示意图；本发明的方法包括以下步骤：

步骤一，获取所要作业区域的顶点坐标并计算短边和长边宽度：

利用北斗/gps导航定位装置获取作业区域的顶点坐标，考虑到一般机械化插秧的田块为平行四边形或接近平行四边形的形状，根据获取的田块顶点坐标，计算出长边长度j和短边长度k；

步骤二，把要作业的大田块分成四个部分并对各区域进行全路径规划：

考虑到一般机械化插秧的田块都非常大，为了节约时间，方便补充秧苗把大田块分成四个部分并分别进行全路径规划。

步骤三，根据划分好的各区域的田块四边计算各区域作业插秧机距离田块四边a,b,c,d的距离：

通过步骤一可得知大田块长边j、短边k和路宽l的大小，加上通过北斗/gps导航定位装置可获得各区域作业插秧机实时位置坐标e(x5,y5)，f(x6,y6)，g(x7,y7)，h(x8,y8)。则左上角作业区域插秧机距离田块四边的距离为：

距离田块a边距离为：

a1＝x5

距离田块b边距离为：

b1＝j-y5

距离田块c边距离为：

距离田块d边距离为：

左下角作业区域插秧机距离田块四边的距离为：

距离田块a边距离为：

a1＝x6

距离田块b边距离为：

距离田块c边距离为：

距离田块d边距离为：

d1＝y6

右上角作业区域插秧机距离田块四边的距离为：

距离田块a边距离为：

距离田块b边距离为：

b1＝j-y7

距离田块c边距离为：

c1＝k-x7

距离田块d边距离为：

右下角作业区域插秧机距离田块四边的距离为：

距离田块a边距离为：

距离田块b边距离为：

距离田块c边距离为：

c1＝k-x8

距离田块d边距离为：

d1＝y8

步骤四，在开启断点续航模式时在排除掉已栽插区域边后根据获得的到各边的距离规划路径前往距离最近的田块边界进行补苗：

若插秧机选择从左向右作业时则只比较b1，c1，d1的大小，选择距离最短的田块边为上秧苗点规划断点续航路径，如果开启断点续航的位置刚好离作业前进方向田块边距离最短，即规划如图2所示路径，首先记录当前位置坐标并作为断点续航的起始点，然后以此点到上秧苗点减去二分之一作业幅宽那个点作为终点来规划前往上秧苗点路径。并根据插秧机作业幅宽和转弯半径在左边相隔1.5个作业幅宽规划补完秧苗回来的路径。如果开启断点续航的位置刚好离作业前进方向相反的田块边距离最短时，即规划如图5所示路径，同样首先记录当前位置坐标并作为断点续航的起始点，然后以此点向下两米处为第二点作为转弯标志点，掉头前往上侧田边的上秧苗点，根据插秧机作业幅宽和转弯半径以转弯标志点向左侧两个作业幅宽规划转弯完成前往上秧苗点的路径及以开始断点续航点向左一个作业幅宽为终点规划上完秧苗回到原始作业点路径。当插秧机作业在田块四边的作业行时，因为作业行都在田块边，所以不再比较到各田块边界的距离，当插秧机需要补苗时，立刻停车原地等待运苗车前来补充秧苗。

若插秧机从左向右作业时则排除掉田块a边，然后比较b1，c1，d1的大小然后进行路径规划，若插秧机从右向左作业则排除掉田块c边，然后比较a1，b1，d1的大小进行路径规划；

步骤五，通过北斗/gps导航定位装置可获得各区域作业插秧机实时位置坐标e(x5,y5)，f(x6,y6)，g(x7,y7)，h(x8,y8)和上秧苗车的实时位置i(x9,y9)。所以运苗车与左上区域插秧机实时距离：

运苗车与左下区域插秧机实时距离：

运苗车与右上区域插秧机实时距离：

运苗车与右下区域插秧机实时距离：

当插秧机需要补苗而开启断点续航模式时根据实时的距离去给相距最近的插秧机最后进行补秧苗：

步骤六，根据开启断点续航时所规划的路径选择转弯控制回到原始作业点：

步骤六中，如果开启断点续航的位置刚好离作业前进方向田块边距离最短，上秧苗的点在前进方向，所以上完秧苗后回到原始作业点时选择的转弯控制为如图4所示：行驶到距离开启断点续航的初始位置5m时，首先把航姿传感器校准置零，控制电动方向盘向指定方向转动610°，然后保持转弯角度不变，即固定转向半径，同时实时检测航姿传感器的数值，当航姿传感器的值从零变化到-175时，控制电动方向盘向要转弯的相反方向打880°，接着根据导航给的航向偏差和横向偏差回到开启断点续航时的航向。最后再启动倒车模式回到开启断点续航时的位置。如果开启断点续航的位置刚好离作业前进方向相反的田块边距离最短时，上秧苗的点与前进方向相反，所以选择的转弯控制为如图7所示：行驶到距离开启断点续航的初始位置2m时，控制电动方向盘向指定方向转动300°，然后保持转弯角度不变，同时实时检测以如图5所示中的①位置所在航线为基准的横向偏差，当横向偏差小于10cm时，然后控制电动方向盘往反方向打350°，并把速度降到0.2m/s，此时实时检测航向偏差和横向偏差回到开启断点续航时的航向。然后在启动倒车模式回到开启断点续航时的位置。

若开启断点续航的位置刚好离作业前进方向田块边距离最短，上秧苗的点在前进方向，所以上完秧苗后回到原始作业点时选择的转弯控制为，当插秧机补完秧苗到达如图3所示的②位置时，首先把速度降到0.3m/s，通过电动方向盘转动固定角度然后根据航姿传感器先使插秧机到达如图3所示③位置，然后根据导航的航向偏差和横向偏差使插秧机到达如图3所示④位置，最后倒车回到初始作业点如图3所示①位置，继续作业。如果开启断点续航的位置刚好离作业前进方向相反的田块边距离最短时，上秧苗的点与前进方向相反，所以上完秧苗后回到原始作业点时选择的转弯控制为当到达如图6所示②位置时，首先把速度降到0.3m/s，通过电动方向盘转动固定角度然后以如图6所示①位置所在航线为基准的航向偏差和横向偏差使插秧机到达如图6所示③位置，然后把速度降到0.2m/s根据导航的航向偏差和横向偏差使插秧机到达如图6所示④位置，最后倒车回到初始作业点如图6所示①位置，继续作业。

综上，本发明的一种水稻无人驾驶插秧机全路径规划及断点续航方法，首先获取作业田块的顶点坐标并计算短边和长边宽度，将作业田块分成四个部分并对各区域进行全路径规划；其次根据各划分区域的田块边界计算作业插秧机距各区域边界的距离，然后开启断点续航模式排除已栽插的区域并根据获取的各边距离规划路径，可使插秧机前往距离最近的田块边界进行补苗，同时计算运苗车与各区域作业插秧机的实时距离，根据实时距离对插秧机就近进行补苗，最后根据开启断点续航时所规划的路径控制机器转弯回到原始作业点。

本发明提供的全路径规划及断点续航方法，针对作业区域的上下转弯区域进行路径规划，通过开启断点续航模式排除已栽插区域边界并根据所获取的插秧机位置规划路径，可使作业区域机插面积全覆盖，实现机器田间作业时因秧苗不足而暂时离开当前作业点补苗然后回到初始作业点继续作业的整套动作。本发明不仅解决了插秧机地头转弯而导致的田头无法插秧的问题，同时减少作业过程中机器压苗及漏插的情况，有效提高了作业效率，为插秧机的无人驾驶奠定了基础。

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

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