一种割草机路径规划方法

技术领域

本发明涉及农业机器人领域，具体为一种割草机路径规划方法。

背景技术

割草机路径规划技术是一种专门用于设计割草机运行路径的方法，目的是在有效覆盖所有割草区域的同时，减少重复工作和空驶，从而提高割草效率。

目前，有多种割草机路径规划的技术，其中最常见的是基于预设路径的割草机路径规划。这种技术的主要优点是简单易行，只需要预设一条或多条路径，然后让割草机按照预设路径进行割草。然而，这种技术的缺点也很明显，首先，预设路径往往不能很好地适应复杂多变的割草环境，导致割草机在某些区域重复割草或者遗漏某些区域。其次，这种技术无法根据实时环境信息进行调整，因此不能很好地应对环境变化。并且现在的割草机规划路径的覆盖率低，割草机转弯的次数比较多，造成割草效率低下，成本增高。尤其有些割草机路径规划算法具有避障功能，需要大量的计算资源，因此对硬件设备的要求较高。

此外，还有一些基于机器学习的割草机路径规划技术。这种技术的主要优点是能够根据历史割草数据学习最优的割草路径，从而避免重复工作和遗漏。然而，这种技术也有一些缺点，首先，机器学习模型可能过拟合或者欠拟合，导致规划的路径不准确。其次，这种技术需要大量的历史数据作为训练集，因此不适用于数据量较小的场景。

发明内容

为解决割草机规划路径的覆盖率低，割草机转弯的次数比较多，当障碍物较多时，割草效率低下，成本高的问题。本发明提供了一种割草机路径规划方法。

本发明的目的在于提供一种割草机路径规划方法，以解决上述背景技术中提出的问题。本方法是在全局信息已知的情况下，涉及一种结合牛耕式运动的全覆盖路径规划方法。该方法能够用于割草机在任意地块下的全覆盖路径规划，并且能够避障。

为了解决上述技术问题，本发明提供如下技术方案：

一种割草机路径规划方法，该路径规划方法包括：

S100：定义地块区域边界，基于栅格地图把地块区域内部划分为障碍物区域和非障碍物区域；

S200：确定作业起始点，以该点为起点，以地块区域边界中最长边的斜率作为第一斜率，以障碍物区域边界做参考，平行于障碍物区域边界绕过障碍物或者在快碰到障碍物区域边界时转弯一定角度避开障碍物区域，在非障碍物区域内做牛耕式运动的全覆盖路径规划做为第一斜率路径；

S300：计算生成的第一斜率路径的总体代价，如果总体代价超过预设值进行下一步运算，否则保持第一斜率路径；

S400：当第一斜率路径总体代价超过预设值时，根据地块区域边界和非障碍物区域边界在x方向和y方向上的最大值和最小值，计算边距规范指标和覆盖率指数；

S500：在边距规范指标和覆盖率指数最大时，以第一斜率路径确定初始解，所述的初始解包括：第一斜率在x、y方向的偏移量为0，第一斜率的偏移角度为0，基于模拟退火算法生成新解，所述的新解包括：第一斜率在x、y方向的偏移量，第一斜率的偏移角度，基于Metropolis准则ap做判断，如果ap大于随机数U(0,1)，则接受新解；否则保持初始解；当保持初始解时，继续保持第一斜率路径，当接受新解时，更新为第二斜率路径。

优选的，所述的步骤S100后面，还包括步骤S101：对障碍物区域边界和地块区域边界做膨胀化处理，设置安全距离。

优选的，所述的步骤S101中，安全距离需增加车辆尺寸。

优选的，所述的步骤S200中，在快碰到障碍物区域边界时转弯90度避开障碍物区域。

优选的，所述的步骤S200中，如何判断路径是否与障碍物区域边界相交，是通过路径对应的线段和障碍物区域边界的任意一条边对应的线段是否相交做出判断，具体判断过程通过以下两种判断方式共同判断完成；

①叉积计算路径所处的线段与障碍物区域边界的任意一条边对应的线段是否相交；

Val(p1,p2,p0)＝(p1.x-p0.x)*(p2.y-p0.y)-(p2.x-p0.x)*(p1.y-p0.y)

式中为叉积计算公式，p1、p2、p0为坐标系的三个点，p1.x、p0.x、p2.x、p1.y、p0.y、p2.y分别为p1、p2、p0三个点的横纵坐标，如果val等于0，这表示点p1、p2、p0共线；如果val大于0，这表示三个点按逆时针方向排列；如果叉积val小于0表示三个点顺时针方向排列，

Temp＝Val(p3,p4,p5)*Val(p3,p4,p6)，

式中p3,p4,p5,p6为两条线段的端点，如果Temp小于0，表示两条路径相交否则不相交；

②计算两条线段之间交点的方法二；

a.首先，定义点坐标：

x1,y1:线段1的起点坐标

x2,y2:线段1的终点坐标

x3,y3:线段2的起点坐标

x4,y4:线段2的终点坐标

b.然后，计算参数t，这个参数被定义为t＝lamta/(1+lamta)，其中lamta是一个中间变量；

c.计算lamta，它的数学表达式如下：

lamta＝((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1))/((x2-x1)*(y3-y4)-(x3-x4)*(y2-y1))

d.最后，计算交点坐标，表示为(x,y)，其数学表达式如下：

x＝x3+t*(x4-x3)

y＝y3+t*(y4-y3)

这个方法计算两条线段的交点，两条线段的交点，如果线段不相交，将无法计算出结果。

优选的，所述的步骤S300中，W＝w

式中W为总体代价，w

优选的，所述的步骤S400中，先获取地块区域边界在x方向和y方向的最大值和最小值、非障碍物区域边界在x方向和y方向上的最大值和最小值；

计算边距规范指标，数学表示如下：

其中：

SSI为边距规范指标，xBoxMax、xBoxMin、yBoxMax、yBoxMin，分别表示地块区域边界在x方向和y方向的最大值，最小值；xMax、xMin、yMax、yMin分别表示非障碍物区域边界在x方向和y方向上的最大值和最小值；

边距规范指标计算完成的同时也生成了路径节点密度和地块边距均匀性；

覆盖率指数的数学公式可以表示为：

optimizationIndex＝a*nodesInTerm+b*minBBAreaTerm-c*equalMarginsTerm；

其中：

optimizationIndex：覆盖率指数；

a、b、c：权重参数，控制各项对指数的影响程度；

nodesInTerm：路径节点密度；

minBBAreaTerm：最小外包矩形面积，minBBAreaTerm＝(xMax-xMin)*(yMax-yMin)；

equalMarginsTerm：地块边距均匀性。

优选的，所述的步骤S500中，模拟退火参数包括，T：当前温度，Tmin：温度下限，算法停止的条件，alpha：温度下降率，numIterations：每个温度下的迭代次数；

根据第一斜率定义初始解：currentSol，参数值包括旋转角度theta＝0、x轴偏移shiftX＝0和y轴偏移shiftY＝0；

模拟退火主循环：在每个温度T下进行多次迭代，每次尝试生成新解：newSol，并接受或拒绝新解，可以是随机生成的或者在邻域内变化的解；

接受新解的条件是基于Metropolis准则ap:

其中：

OptimizationIndexCurrent表示初始解的覆盖率指数，newSol.getOptimizationIndex表示新解的覆盖率指数，如果ap大于随机数U(0,1)，则接受新解；否则保持初始解；

新解的参数值包括optimalTheta、optimalShiftX和optimalShiftY，其中optimalShiftX、optimalShiftY，optimalTheta分别为第一斜率在x、y方向的偏移量，第一斜率的偏移角度；第一斜率的偏移角度，可以求反正切值求得即optimalTheta＝arctan(yi+optimalShiftY)/(xj+optimalShiftX)，其中，xi、xj、yi、yj为地块边界最长边两个端点的x方向和y方向上的坐标值。

与现有技术相比，本发明所达到的有益效果是：基于本发明算法生成的全覆盖路径规划，总体代价小，割草机转弯的次数少，覆盖率高，生成路径运算量小，路径生成速度快，而且能够避障，设置安全距离同时考虑了车辆尺寸，生成的路径运行安全性高，避免了割草作业中的重复工作和遗漏，割草效率低高，成本大大降低。该方法能够用于割草机在任意地块下的全覆盖路径规划，并且能够避障。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：

图1是本发明的流程示意图；

图2是本发明的非规则地块路径规划示意图。

具体实施方式

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

本发明提供技术方案，具体流程图参照图1，非规则地块路径规划示意图参照图2，该方法的具体执行过程如下。

为解决割草机规划路径的覆盖率低，割草机转弯的次数比较多，当障碍物较多时，割草效率低下，成本高的问题。本发明提供了一种基于DARP+模拟退火算法+几何方法的全覆盖路径规划算法。

1、定义地块区域边界，基于栅格地图把地块区域内部划分为障碍物区域和非障碍物区域；对障碍物区域边界和地块区域边界做膨胀化处理，设置安全距离；2、确定作业起始点，以该点为起点，以地块区域边界中最长边的斜率作为第一斜率，以障碍物区域边界做参考，平行于障碍物区域边界绕过障碍物或者在快碰到障碍物区域边界时转弯一定角度避开障碍物区域，在非障碍物区域内做牛耕式运动的全覆盖路径规划做为第一斜率的路径；3、计算生成的第一斜率路径的总体代价，如果总体代价超过预设值进行下一步运算，否则保持第一斜率路径；4、当第一斜率路径总体代价超过预设值时，根据地块区域边界和非障碍物区域边界在x方向和y方向上的最大值和最小值，计算边距规范指标和覆盖率指数；5、在边距规范指标和覆盖率指数最大时，以第一斜率路径确定初始解(第一斜率在x、y方向的偏移量为0，第一斜率的偏移角度为0)，基于模拟退火算法生成新解(第一斜率在x、y方向的偏移量，第一斜率的偏移角度)，基于Metropolis准则ap做判断，如果ap大于随机数U(0,1)，则接受新解；否则保持初始解；当保持初始解时，继续保持第一斜率路径，当接受新解时，更新为第二斜率路径，即在第一斜率基础上增加x、y方向的偏移量，偏移角度。采用第二斜率生成第二斜率路径。

本发明通过已知的地块全局信息，包括障碍物数据、地块边界数据等，障碍物和地块的处理上用到了这些数据，加安全距离时需要在此数据上加，栅格地图处理时也需要用到障碍物和地块的数据，根据地块大小、障碍物大小建立一定范围的地图，把地块中障碍物区域与非障碍物区域分开，为后边规划做铺垫。当开始以第一斜率进行规划时，以边界最长边的斜率为参考，也需要这些数据做计算。对障碍物做膨胀化安全处理。膨胀化处理即在障碍物原来的数据上往外扩大一定的数值，防止实际过程中车辆与障碍物发生碰撞。

路径要发生偏移，偏移也不是随意偏移的；偏移的范围一要总体代价最小，覆盖率尽量最大化，另外要能够避障。结合模拟退火算法做路径优化，路径规划时间快，以此方法生成全局路径。此处的总体代价最小，通过预设值的设置保持一定的总体代价小的范围，超过总体代价的预设值，则尝试寻求更优解，如果在覆盖率最大化的基础上，基于模拟退火算法生成新解，基于Metropolis准则ap做判断，如果ap大于随机数U(0,1)，则接受新解；否则保持初始解，如果接受新解，则生成第二斜率路径，此时的第二斜率路径是在第一斜率路径的基础上偏移新解计算出的在x、y方向的偏移量和偏移角度获得的新的路径。

1、定义地块区域边界，基于栅格地图把地块区域内部划分为障碍物区域和非障碍物区域；对障碍物区域边界和地块区域边界做膨胀化处理，设置安全距离。

对障碍物做膨胀化处理，设置安全距离。安全距离为车辆到地块区域边界安全距离、车辆到障碍物区域边界安全距离，地块区域边界安全距离为L

总的障碍物区域边界安全距离为：L＝L

2、确定作业起始点，以该点为起点，以地块区域边界中最长边的斜率作为第一斜率，

第一斜率＝(yi-yj)/(xi-xj),其中xi、xj、yi、yj为地块边界最长边两个端点的x方向和y方向上的坐标值。

以障碍物区域边界做参考，平行于障碍物区域边界绕过障碍物或者在快碰到障碍物区域边界时转弯一定角度避开障碍物区域，在非障碍物区域内做牛耕式运动的全覆盖路径规划做为第一斜率路径。

在以牛耕式运动生成路径的过程中，以障碍物区域边界做参考，平行于障碍物区域边界绕过障碍物或者在快碰到障碍物区域边界时转弯一定角度避开障碍物区域。如何判断路径是否与障碍物区域边界相交，是通过路径对应的线段和障碍物区域边界的任意一条边对应的线段是否相交做出判断，具体判断过程通过运行以下基于几何算法的避障检测的两种形式来完成。

①叉积计算路径所处的线段与障碍物区域边界的任意一条边对应的线段是否相交。

Val(p1,p2,p0)＝(p1.x-p0.x)*(p2.y-p0.y)-(p2.x-p0.x)*(p1.y-p0.y)

式中为叉积计算公式，p1、p2、p0为坐标系的三个点，p1.x、p0.x、p2.x、p1.y、p0.y、p2.y分别为p1、p2、p0三个点的横纵坐标。如果val等于0，这表示点p1、p2、p0共线；如果val大于0，这表示三个点按逆时针方向排列；如果叉积val小于0表示三个点顺时针方向排列。

Temp＝Val(p3,p4,p5)*Val(p3,p4,p6)，

式中p3,p4,p5,p6为两条线段的端点，如果Temp小于0，表示两条路径相交否则不相交。

②计算两条线段之间交点的方法二。

a.首先，定义点坐标：

x1,y1:线段1的起点坐标

x2,y2:线段1的终点坐标

x3,y3:线段2的起点坐标

x4,y4:线段2的终点坐标

b.然后，计算参数t，这个参数被定义为t＝lamta/(1+lamta)，其中lamta是一个中间变量。

c.计算lamta，它的数学表达式如下：

lamta＝((x2-x1)*(y3-y1)-(x3-x1)*(y2-y1))/((x2-x1)*(y3-y4)-(x3-x4)*(y2-y1))

d.最后，计算交点坐标，表示为(x,y)，其数学表达式如下：

x＝x3+t*(x4-x3)

y＝y3+t*(y4-y3)

这个方法计算两条线段的交点，两条线段的交点。如果线段不相交，将无法计算出结果。

利用以上两种形式，多重检测，避免障碍物与规划路径相交。

3、计算生成的第一斜率路径的总体代价，如果总体代价超过预设值进行下一步运算，否则保持第一斜率路径。

转弯次数太多，规划路径过长等，都会导致路径的总体代价高，为了便于判断已生成路径是否属于最优路径，本申请引入了总体代价方程：

W＝w

式中W为总体代价，w

如果总体代价超过预设值进行下一步运算，如果基于第一斜率路径计算得到的总体代价超过预设值，说明第一斜率路径的总体代价过大，此时需要寻求新的路径以降低总体代价，将进行后续的运算；如果基于第一斜率路径计算得到的总体代价小于预设值，说明第一斜率路径的总体代价不大，满足规划路径的覆盖率高，割草机转弯次数少等路径规划质量高、效率高的条件，则无需进行后续运算。

4、当第一斜率路径总体代价超过预设值时，根据地块区域边界和非障碍物区域边界在x方向和y方向上的最大值和最小值，计算边距规范指标和覆盖率指数。

先获取地块区域边界在x方向和y方向的最大值和最小值、非障碍物区域边界在x方向和y方向上的最大值和最小值；

计算边距规范指标，数学表示如下：

其中：

SSI为边距规范指标，xBoxMax、xBoxMin、yBoxMax、yBoxMin，分别表示地块区域边界在x方向和y方向的最大值，最小值；xMax、xMin、yMax、yMin分别表示非障碍物区域边界在x方向和y方向上的最大值和最小值。通过SSI公式边距规范，为了求下面的覆盖率指数。

这个公式的目的是计算x方向和y方向上的边距规范，它衡量了数据在这两个方向上的分布与指定边界之间的差异。计算方式是将两个方向的边距之差绝对值化，然后归一化到指定边距的范围内。最后，将x方向和y方向的归一化值相加，得到最终的SSI值，用于衡量地块区域规范性。

边距规范指标计算完成的同时也生成了路径节点密度和地块边距均匀性，而路径节点密度和地块边距均匀性又用于后续覆盖率指数的计算。

覆盖率指数的数学公式可以表示为：

optimizationIndex＝a*nodesInTerm+b*minBBAreaTerm-c*equalMarginsTerm

其中：

optimizationIndex：覆盖率指数；

a、b、c：权重参数，控制各项对指数的影响程度；

nodesInTerm：路径节点密度；

minBBAreaTerm：最小外包矩形面积，minBBAreaTerm＝(xMax-xMin)*(yMax-yMin)；

equalMarginsTerm：地块边距均匀性；

这个公式的目的是根据权重参数a、b和c对不同的优化因素进行加权组合，以得出一个综合的覆盖率指数，权重参数a、b和c可以根据具体情况来调整。

当边距规范指标值和覆盖率指数值同时最大时，此时覆盖率最大。

5、在边距规范指标和覆盖率指数最大时，以第一斜率路径确定初始解(第一斜率在x、y方向的偏移量为0，第一斜率的偏移角度为0)，基于模拟退火算法生成新解(第一斜率在x、y方向的偏移量，第一斜率的偏移角度)，基于Metropolis准则ap做判断，如果ap大于随机数U(0,1)，则接受新解；否则保持初始解；当保持初始解时，继续保持第一斜率路径，当接受新解时，更新为第二斜率，即在第一斜率基础上增加x、y方向的偏移量，偏移角度，生成。

模拟退火参数包括，T：当前温度，Tmin：温度下限，算法停止的条件，alpha：温度下降率，numIterations：每个温度下的迭代次数；

根据第一斜率定义初始解：currentSol，参数值包括旋转角度theta＝0、x轴偏移shiftX＝0和y轴偏移shiftY＝0；

模拟退火主循环：在每个温度T下进行多次迭代，每次尝试生成新解：newSol，并接受或拒绝新解，可以是随机生成的或者在邻域内变化的解；

通过覆盖率指数衡量一个解的质量；

接受新解的条件是基于Metropolis准则ap:

ap用于判断当前解与生成的解的关系，

其中：

OptimizationIndexCurrent表示初始解的覆盖率指数，newSol.getOptimizationIndex表示新解的覆盖率指数，如果ap大于随机数U(0,1)，则接受新解；否则保持初始解。

最终输出找到的具有最大覆盖率指数的新解的参数值(optimalTheta、optimalShiftX和optimalShiftY)，这些值用于优化路径。其中optimalShiftX、optimalShiftY，optimalTheta分别为第一斜率在x、y方向的偏移量，第一斜率的偏移角度。

第一斜率的偏移角度，可以求反正切值求得即optimalTheta＝arctan(yi+optimalShiftY)/(xj+optimalShiftX)。

如果ap大于随机数U(0,1)的值，以新解作为路径规划的参数值，对路径做优化，如果ap小于1，说明初始解为最优解，当前路径为最优，无需做优化。

综上，当接受新解时，更新为第二斜率，即在第一斜率基础上增加x、y方向的偏移量，偏移角度。采用第二斜率生成第二斜率路径。

图2是用本算法的实际效果图，那个地块四亩多地，54个障碍物，当设定幅宽大小为0.8米时，路径规划时间最快时不到1秒，非常快，消耗算力小，对硬件要求不高。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。