CN113686344A - 一种农机覆盖路径规划方法 - Google Patents

Abstract

本发明公开了一种农机覆盖路径规划方法，包括如下步骤：S1：确定路径规划的初始参数；S2：将所述原始田块边界点集P向所述田块边界内部平移距离L，以确定农机工作区域的边界；S3：建立x‑y轴直角坐标系，计算所述农机工作区域的最小跨度；S4：生成所述农机工作区域旋转最优旋转角度时的平行路径；S5：生成农机的转弯路径，并将农机的转弯路径与直线路径连接，以构成农机工作完整的路径。通过优化平行路径角度的方法，减少了农机转弯次数，提高农机的工作效率，由于是将田块旋转后，通过计算每条边界的倾斜角计算最优平行路径的角度的方法，避免通过角度递增的迭代方法，同时用平行于x轴的扫描线生成平行路径，简化计算工作量。

Description

一种农机覆盖路径规划方法

技术领域

本发明涉及农机作业控制技术领域，尤其涉及一种农机覆盖路径规划方法。

背景技术

近几年自动驾驶技术发展迅速，因为农机工作环境简单、运行方式比较单一的特点，非常适合当前阶段自动驾驶技术的应用。自动驾驶关键技术中，全局路径规划是保证车辆安全高效完成驾驶目标的基础。研究者已经设计了很多针对乘用车辆的路径规划方法，但是，针对农机的全局路径规划算法却少有研究。已有的方法没有考虑农机的工作特点，并不适用于自动驾驶的农机装备。考虑到农机的工作目标和工作方式，在这些算法中，用于扫地机器人的覆盖路径规划算法比较接近农机的工作方式。和农机的路径规划不同的是，因为扫地机器人可以实现原地任意角度转向，农机受制于转向机构的特点必须按照一定的转弯半径进行转向。

发明内容

本发明提供一种农机覆盖路径规划方法，针对自动驾驶农机，解决了上述提到的农机全局路径规划存在的问题，提出了一种根据输入点确定农机工作范围、确定绕行方式、规划平行路径和转弯路径的方法，提出了考虑其工作特点和覆盖路径规划算法的全局路径规划方法。

为了实现上述目的，本发明的技术方案是：一种农机覆盖路径规划方法，所述规划方法包括如下步骤：

S1：确定路径规划的初始参数，所述初始参数包括原始田块边界点集P、农机每一行扫描的宽度w、和农机的最小转弯半径；

S2：将所述原始田块边界点集P向所述田块边界内部平移距离L，以确定农机工作区域的边界；

S3：建立x-y轴直角坐标系，计算所述农机工作区域的最小跨度，以确定原始田块的最优旋转角度；

S4：生成所述农机工作区域旋转最优旋转角度时的平行路径，以确定农机的直线路径；

S5：生成农机的转弯路径，并将农机的转弯路径与直线路径连接，以构成农机工作完整的路径。

进一步的，所述S3中，计算所述农机工作区域的最小跨度方法如下：

当农机工作区域的边界为凸多边形时，农机转弯的次数n为：

式中D是农机工作区域的边界在垂直于扫描线的方向上的跨度，所述扫描线是x-y轴直角坐标系中平行于x轴的直线；

y＝y_min,x∈[x_min,x_max] (2)

其中，y_min是农机工作区域边界在y轴上的最小值；y_max是农机工作区域边界在y轴上的最大值；x_min是农机工作区域边界在x轴上的最小值；x_max是农机工作区域边界在x轴上的最大值；D＝y_max-y_min；

所述原始田块每一次的旋转角度为：

式中，[x₁,y₁]是农机工作区域边界中与x轴平行的边的起点坐标；[x₂,y₂]是农机工作区域边界中与x轴平行的边的终点坐标，θ_t是旋转角度，于是多次旋转后，当跨度D最小时的旋转角度即为农机工作区域的最优旋转角度θ^*；

当农机工作区域是凹多边形时，首先计算基于农机工作区域边界点的凸包，然后计算田块的最优旋转角度θ^*。

进一步的，所述S4中，确定农机的直线路径的方法为：

S41：将原始田块以凸多边形的质心为旋转中心，顺时针旋转最优旋转角度θ^*，得到跨度最小的农机工作区域的边界P'；

S42：将所述扫描线逐次向y轴正方向平移，每次平移w；每次平移后计算扫描线和农机工作区域的边界P'的交点的数量；

S43：若交点的数量为2，则认为还在农机工作区域的边界内，记录两个交点的坐标，继续进行扫描；若交点的数量为1或者0，则判断超出农机工作区域的边界范围，停止扫描，完成平行路径的生成过程。

进一步的，所述转弯路径包括三种类型：π型、鱼尾型和半圆型；

当所述转弯路径为鱼尾型时，w<2r；

当所述转弯路径为半圆型时，w＝2r；

当所述转弯路径为π型时，w>2r。

进一步的，所述S5中生成农机的转弯路径的方式包括第一种转弯路径生成方式和第二种转弯路径生成方式：

若已知的是第一条线段，则结合转弯半径、转弯方向及转弯角度，生成和第一条线段相切的圆弧；此时采用的是第一种转弯路径生成方式；

若已知的是第一条线段和第二条线段，则结合转弯半径，生成和所述第一条线段、第二条线段都相切的圆弧，此时采用的是第二种转弯路径生成方式。

进一步的，所述第一种转弯路径生成方式包括如下步骤：

S511：确定转弯路径的圆心坐标；

式中[x_c,y_c]是圆心坐标，[x₂,y₂]是切点坐标，clockwise表示顺时针旋转，unclockwise表示逆时针旋转，θ₁是第一条线段的倾斜角；α是转弯路径与第一条线段切点和圆心的连线与x轴正方向的夹角；

S512：确定转弯路径的范围；

用β₁表示起点(切点)和圆心连线的倾斜角，β₂表示终点和圆心连线的倾斜角，

式中，β₁表示转弯半径的起点(切点)和圆心连线与x轴正向的夹角，β₂表示转弯半径的终点和圆心连线与x轴正向的夹角，

是转弯路径的圆心角；

S513：生成转弯路径；

式中：[x_i,y_i]是从所述转弯路径起点开始的第i个离散点，Δs是圆弧上两个离散点间的弧长。

进一步的，所述第二种转弯路径生成方式包括如下步骤：

S521：确定转弯路径的旋转方向；

式中：[x₃,y₃]第二条线段起点的坐标；[x₄,y₄]是第二条线段终点的坐标，

是所述第一条线段的起点和终点构成的方向向量，

是所述第二条线段的起点和终点构成的方向向量，C是两个向量的叉乘，orientation是圆弧旋转方向；

S522：计算所述第一条线段的切点；

式中：θ_v是向量

和

之间的夹角，[x_t,y_t]是第一条线段的切点的坐标，[x₀,y₀]是所述第一条线段和第二条线段交点的坐标，ω是表示方向的中间变量；θ₁是第一条线段与x轴正方向的夹角；

S523：确定转弯路径的圆心角：

式中：

是转弯路径的圆心角。

有益效果：本发明所公开的一种农机覆盖路径规划方法，通过优化平行路径角度的方法，减少了农机转弯次数，提高农机的工作效率，由于是将田块旋转后，通过计算每条边界的倾斜角计算最优平行路径的角度的方法，避免通过角度递增的迭代方法，同时用平行于x轴的扫描线生成平行路径，简化计算工作量。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明规划平行路径示意图；

图2a为本发明鱼尾型转弯路径示意图；

图2b为本发明半圆型转弯路径示意图；

图2c为本发明π型转弯路径示意图；

图3为本发明通过第一种转弯路径生成转弯路径示意图；

图4为本发明通过第二种转弯路径生成转弯路径示意图；

图5为本发明鱼尾型转弯路径规划示意图。

其中：1、凸包；2、农机工作区域边界；3、扫描线；4、过渡直线路径；5、平行路径2；6、平行路径1；7、圆弧路径1；8、圆弧路径2；9、农机工作区域边界；10、路径规划区域边界。

具体实施方式

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

本实施例提供了一种农机覆盖路径规划方法，所述规划方法包括如下步骤：

S1：确定路径规划的初始参数，所述初始参数包括原始田块边界点集P、农机每一行扫描的宽度w、和农机的最小转弯半径；

具体的，所述田块边界点集P代表要规划田块的边界，为了方便描述和计算，点集P里的元素都按逆时针排序；根据农机的工作特点，农机的工作路径一般是逐行扫描，并保证尽可能覆盖全部所有面积，因此，确定农机每一行扫描的宽度(即作业需求的行间距)w，由于农机转弯的能力受制于转向机构的硬件结构，为了确保农机能够准确跟上规划的路径，确定农机的最小转弯半径r；

S2：将所述原始田块边界点集P向所述田块边界内部平移距离L，以确定农机工作区域的边界，避免农机覆盖区域超出田块边界，其中，L是农机自身宽度的一半；

S3：建立x-y轴直角坐标系，计算所述农机工作区域的最小跨度，以确定原始田块的最优旋转角度；定义与原始田块旋转后计算农机工作区域跨度的边界平行的坐标轴为x轴；

具体的，由于农机转弯速度慢、转弯半径较大且转弯时至少需要多次启停，因此转弯需要比较长的时间。为了减少总的工作时间，提高工作效率，在进行路径规划时，应尽量减少转弯的次数。

所述S3中计算农机工作区域的最小跨度方法如下：

当农机工作区域的边界为凸多边形时，农机转弯的次数n为：

式中D是农机工作区域的边界在垂直于扫描线的方向上的跨度，所述扫描线是x-y轴直角坐标系中平行于x轴的直线：如图1所示；在本发明中为了简化描述和计算，

y＝y_min,x∈[x_min,x_max] (2)

其中，y_min是农机工作区域边界在y轴上的最小值；y_max是农机工作区域边界在y轴上的最大值；x_min是农机工作区域边界在x轴上的最小值；x_max是农机工作区域边界在x轴上的最大值；D＝y_max-y_min，因此要减少转弯次数即减小D；由于在任意凸多边形上的最小跨度总出现在‘点边式’，即一个顶点到不含这个顶点的一条边上的距离；以凸多边形的质心为旋转中心顺时针逐次旋转凸多边形，使多边形的某一条边和x轴平行，并计算跨度D。每一次的旋转角度为：

式中，[x₁,y₁]是农机工作区域边界中与x轴平行的边的起点坐标；[x₂,y₂]是农机工作区域边界中与x轴平行的边的终点坐标，θ_t是旋转角度，即所述旋转后与x轴平行的边的倾斜角；旋转角从最小角度开始，直到旋转角度大于或等于180°停止，于是多次旋转后，当跨度D最小时的旋转角度即为农机工作区域的最优旋转角度θ^*；

当农机工作区域是凹多边形时，由于凹多边形上凹的点和边计算的跨度不能覆盖整个区域，因此对于凹的多边形，本发明首先用成熟的Graham扫描法计算基于农机工作区域边界点的凸包(即由田块边界顶点P中部分顶点组成的，包含田块所有面积的凸多边形)，然后按照上述步骤计算田块的最优旋转角度θ^*；

S4：生成农机工作区域旋转最优旋转角度时的平行路径，以确定农机的直线路径；所述确定农机的直线路径的方法为：

S41：将原始田块以凸多边形的质心为旋转中心，顺时针旋转最优旋转角度θ^*，得到跨度最小的农机工作区域的边界P'；

S42：将扫描线逐次向y轴正方向平移，每次平移w；每次平移后计算扫描线和农机工作区域的边界P'的交点的数量；

S43：若交点的数量是2，则认为还在农机工作区域的边界内，记录每个交点的坐标，继续进行扫描；若交点的数量是1或者0，则判断超出农机工作区域的边界范围，停止扫描，完成平行路径的生成。

S5：生成农机的转弯路径，并将农机的转弯路径与直线路径连接，以构成农机工作完整的路径；

具体的，根据行间距w和转弯半径r的关系，所述转弯路径包括三种类型：π型、鱼尾型和半圆型，如图2所示；

当所述转弯路径为鱼尾型时，w<2r；

当所述转弯路径为半圆型时，w＝2r；

当所述转弯路径为π型时，w>2r；

由于农机运行速度慢，所以近似认为转弯过程是沿着固定半径的圆弧前进；因此，三种转弯路径都是直线和定曲率圆弧的相切连接；根据圆弧生成的条件可以将其生成方式分为两种，分别是第一种转弯路径生成方式和第二种转弯路径生成方式：

若已知的是第一条线段，则结合转弯半径、转弯方向及转弯角度，生成和第一条线段相切的圆弧；此时采用的是第一种转弯路径生成方式；

若已知的是第一条线段和第二条线段，则结合转弯半径，生成和所述第一条线段、第二条线段都相切的圆弧，此时采用的是第二种转弯路径生成方式；

所述第二种转弯路径生成方式与所述第一种转弯路径生成方式的第一条线段(及切点)、转弯半径、转弯方向，后续曲线路径的生成的过程和方式均相同。

当所述转弯路径为半圆型时，选用第一种转弯路径生成方式，避免车辆超过农机工作区域边界，转弯路径的起点为第一条线段的终点向前进的反方向平移r的距离点；半圆形的转弯路径对应的圆心角θ＝180°；

当所述转弯路径为鱼尾型和π型时，首先确定要连接的两条平行路径以及其与农机工作区域边界的交点，将两个交点的连线即过渡直线向农机工作区域边界内部平移w/2，得到转弯过程中的直线路径，然后生成平行路径1(即第一条线段)与过渡直线路径间的圆弧路径1，和过渡直线路径与平行路径2(即第二条线段)间的圆弧路径2。当所述转弯路径为π型时，转弯路径中的过渡直线路径需要农机向前直行，而当所述转弯路径为鱼尾型时，转弯路径中的过渡直线路径需要农机向后直行(即倒车通过)，如附图5所示。

所述第一种转弯路径生成方式包括如下步骤：如附图3所示：

所述第一种转弯路径生成方式生成圆弧的约束条件是生成的圆弧和第一条线段相切，切点是第一条线段的终点；

S511：确定转弯路径的圆心坐标；

式中[x_c,y_c]是圆心坐标，[x₂,y₂]是切点坐标，clockwise表示顺时针旋转，unclockwise表示逆时针旋转，θ₁是已知直线的第一条线段的倾斜角；

S512：确定转弯路径的范围；

转弯路径范围是通过转弯圆弧的起点和终点与圆心的连线的夹角确定，用β₁表示转弯半径的起点(切点)和圆心连线与x轴正向的夹角，β₂表示转弯半径的终点和圆心连线与x轴正向的夹角，

是转弯路径的圆心角；

S513：生成转弯路径；

本发明中的路径用有序离散点表示，因此生成的圆弧路径可以表示为：

式中：[x_i,y_i]是从转弯半径起点(切点)开始第i个离散点，Δs是圆弧上两个离散点间的弧长。

所述第二种转弯路径生成方式包括如下步骤：如附图4所示；

所述第二种转弯路径生成方式生成圆弧的约束条件是生成的圆弧和所述第一条线段、第二条线段分别相切，根据所述第一条线段、第二条线段的几何关系确定圆弧旋转方向。

S521：确定转弯路径的旋转方向：生成的转弯路径是从第一条线段过渡到第二条线段，因此可以根据两条线段构成的两个向量间的叉乘确定旋转方向；

式中：[x₃,y₃]第二条线段起点的坐标；[x₄,y₄]是第二条线段终点的坐标，

是所述第一条线段的起点和终点构成的方向向量，

是所述第二条线段的起点和终点构成的方向向量，C是两个向量的叉乘，orientation是圆弧旋转方向；

S522：计算第一条线段的切点；

所述第二种转弯路径生成方式中的第一条线段的切点相当于所述第一种转弯路径生成方式中第一条线段的端点，是转弯半径的起点；所述第一条线段的切点通过两条直线的交点和夹角计算得到，具体计算方法如下：

式中：θ_v是向量

和

之间的夹角，[x_t,y_t]是第一条线段的切点的坐标，[x₀,y₀]是所述第一条线段和第二条线段交点的坐标，ω是表示方向的中间变量；θ₁是第一条线段与x轴正方向的夹角；

S523：确定转弯路径的圆心角：

根据几何关系可以知道，圆心角和向量

和

间的夹角互补，圆心角为：

式中：

是转弯路径的圆心角。

本发明具有如下有益效果：

1：针对外边界为凹形的田块，本发明通过Graham扫描法确定包围田块的凸的边界，以便于确定最优的平行路径的角度。

2：本发明提出通过优化平行路径角度的方法，减少了农机转弯次数，可以提高农机的工作效率。并且是将田块旋转后，用平行于x轴的扫描线生成平行路径，可以简化计算。

3：本发明提出仅通过计算每条边界的倾斜角计算最优平行路径的角度的方法，避免通过角度递增的迭代方法，减少计算量。

4：本发明通过农机转弯半径和行间距的关系提出了三种不同的转弯路径，确保农机能平顺地沿着规划的路径运行。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (7)

1.一种农机覆盖路径规划方法，其特征在于，所述规划方法包括如下步骤：

S1：确定路径规划的初始参数，所述初始参数包括原始田块边界点集P、农机每一行扫描的宽度w、和农机的最小转弯半径；

S2：将所述原始田块边界点集P向所述田块边界内部平移距离L，以确定农机工作区域的边界；

S3：建立x-y轴直角坐标系，计算所述农机工作区域的最小跨度，以确定原始田块的最优旋转角度；

S4：生成所述农机工作区域旋转最优旋转角度时的平行路径，以确定农机的直线路径；

S5：生成农机的转弯路径，并将农机的转弯路径与直线路径连接，以构成农机工作完整的路径。

2.根据权利要求1所述的一种农机覆盖路径规划方法，其特征在于，所述S3中，计算所述农机工作区域的最小跨度方法如下：

当农机工作区域的边界为凸多边形时，农机转弯的次数n为：

式中D是农机工作区域的边界在垂直于扫描线的方向上的跨度，所述扫描线是x-y轴直角坐标系中平行于x轴的直线；

y＝y_min，x∈[x_min，x_max] (2)

其中，y_min是农机工作区域边界在y轴上的最小值；x_min是农机工作区域边界在x轴上的最小值；x_max是农机工作区域边界在x轴上的最大值；D＝y_max-y_min；

所述原始田块每一次的旋转角度为：

式中，[x₁，y₁]是农机工作区域边界中与x轴平行的边的起点坐标；[x₂，y₂]是农机工作区域边界中与x轴平行的边的终点坐标，θ_t是旋转角度，于是多次旋转后，当跨度D最小时的旋转角度即为农机工作区域的最优旋转角度θ^*；

当农机工作区域是凹多边形时，首先计算基于农机工作区域边界点的凸包，然后计算田块的最优旋转角度θ^*。

3.根据权利要求2所述的一种农机覆盖路径规划方法，其特征在于，所述S4中，确定农机的直线路径的方法为：

S41：将原始田块以凸多边形的质心为旋转中心，顺时针旋转最优旋转角度θ^*，得到跨度最小的农机工作区域的边界P′；

S42：将所述扫描线逐次由y_min向y_max方向平移，每次平移w；每次平移后计算扫描线和农机工作区域的边界P′的交点的数量；其中，y_max是农机工作区域边界在y轴上的最大值；

S43：若交点的数量为2，则认为还在农机工作区域的边界内，记录两个交点的坐标，继续进行扫描；若交点的数量为1或者0，则判断超出农机工作区域的边界范围，停止扫描，完成平行路径的生成过程。

4.根据权利要求1所述的一种农机覆盖路径规划方法，其特征在于，所述转弯路径包括三种类型：π型、鱼尾型和半圆型；

当所述转弯路径为鱼尾型时，w＜2r；

当所述转弯路径为半圆型时，w＝2r；

当所述转弯路径为π型时，w＞2r。

5.根据权利要求1所述的一种农机覆盖路径规划方法，其特征在于，所述S5中生成农机的转弯路径的方式包括第一种转弯路径生成方式和第二种转弯路径生成方式：

若已知的是第一条线段，则结合转弯半径、转弯方向及转弯角度，生成和第一条线段相切的圆弧；此时采用的是第一种转弯路径生成方式；

若已知的是第一条线段和第二条线段，则结合转弯半径，生成和所述第一条线段、第二条线段都相切的圆弧，此时采用的是第二种转弯路径生成方式。

6.根据权利要求5所述的一种农机覆盖路径规划方法，其特征在于，所述第一种转弯路径生成方式包括如下步骤：

S511：确定转弯路径的圆心坐标；

式中[x_c，y_c]是圆心坐标，[x₂，y₂]是切点坐标，clockwise表示顺时针旋转，unclockwise表示逆时针旋转，θ₁是第一条线段的倾斜角；α是转弯路径与第一条线段切点和圆心的连线与x轴正方向的夹角；

S512：确定转弯路径的范围；

用β₁表示起点(切点)和圆心连线的倾斜角，β₂表示终点和圆心连线的倾斜角，

式中，β₁表示转弯半径的起点(切点)和圆心连线与x轴正向的夹角，β₂表示转弯半径的终点和圆心连线与x轴正向的夹角，

是转弯路径的圆心角；

S513：生成转弯路径；

式中：[x_i，y_i]是从所述转弯路径起点开始的第i个离散点，Δs是圆弧上两个离散点间的弧长。

7.根据权利要求5所述的一种农机覆盖路径规划方法，其特征在于，所述第二种转弯路径生成方式包括如下步骤：

S521：确定转弯路径的旋转方向；

式中：[x₃，y₃]第二条线段起点的坐标；[x₄，y₄]是第二条线段终点的坐标，

是所述第一条线段的起点和终点构成的方向向量，

是所述第二条线段的起点和终点构成的方向向量，C是两个向量的叉乘，orientation是圆弧旋转方向；

S522：计算所述第一条线段的切点；

式中：θ_v是向量

和

之间的夹角，[x_t，y_t]是第一条线段的切点的坐标，[x₀，y₀]是所述第一条线段和第二条线段交点的坐标，ω是表示方向的中间变量；θ₁是第一条线段与x轴正方向的夹角；

S523：确定转弯路径的圆心角：

式中：

是转弯路径的圆心角。

Images