一种指定下田口的往返式全自动路径规划方法与流程

1.本发明属于农机无人驾驶技术领域，具体涉及基于一种指定下田口的往返式全自动路径规划方法。


背景技术：

2.中国农业的特点是分散化程度高，作物种类多样，这导致大部分农用地块面积相对较小且地块形状不规则。因此对于这类农业地块，采用的农机自动驾驶控制技术可视为非线性、不确定的系统，面对复杂的作业环境。其中需要考虑的农机自动驾驶因素之一就是下田口的位置多样，结合不同形状的地块，农机自动驾驶系统常常难以实现全过程自动运行。现有的农机自动驾驶系统存在下列问题：1、不具备在不规则地块中指定作业下田口功能：农机作业需要指定开始作业点，同时作业完成后需要自动返回该点；2、在不规则地块中，指定下田口的情况下，如何实现寻找最佳起始点，以及如何在作业结束后，实现最优路径返回至下田口，目前现有技术是不具备的。上述问题虽然对单一地块影响较小，但需要农机对多个不连续的地块进行工作时，会大大延长人工操作农机的时间，并且在涉及的多个地块时，在农机完成各个地块作业之前，驾驶员无法脱离工作场地，无法实现真正的无人化农业，所需人手不能实质性减少。


技术实现要素：

3.本发明的目的是提供一种指定下田口的往返式全自动路径规划方法，用于解决现有技术中农机自动驾驶系统无法自动在指定的下田口与不规则地块情况下，遵循最短路径与覆盖面积最广原则，自动生成最优作业行驶轨迹的技术问题。
4.所述的一种指定下田口的往返式全自动路径规划方法，包括：
5.s1、基于作业区域平面图标记需要作业的不规则地块和对应的下田口位置；
6.s2、基于不规则地块的形状生成相应的地块内作业路径，满足地块作业的覆盖率最高原则；
7.s3、生成由地块内作业路径的起点和终点到下田口的最短路径，满足整体作业路径里程最短要求，形成最优的全程路径。
8.优选的，所述步骤s3包括：
9.s31、生成下田口到起点的路径，从下田口遍历搜索所有的边界点，计算出下田口到最长边的两个端点和离最长比最远边界点的距离，其中距离最短的路径，即为下田口到起点的路径；
10.s32、生成终点到下田口的路径，从终点遍历所有的边界点，找到距离下田口最短的一条路径，即为终点到下田口的路径。
11.优选的，所述步骤s2包括：
12.s21、直线行驶方向的确定，选择最长的边为直线行驶的方向；
13.s22、基于作业覆盖率最高原则和直线行驶方向生成可能的路径连线；
14.s23、基于贪婪思想生成全覆盖的地块内作业路径路径。
15.优选的，所述步骤s21包括，按逆时针选择边界点，遍历所有的边界点，存入数组中，计算相邻两点之间的长度，记为：连接相邻的两个边界点并记为边e
i，j
，遍历所有的边，记录最长边e
max
和最长边的两个端点ai(xi，yi)，a
i+1
(x
i+1
，y
i+1
)，0≤i，j≤n，n≥3，n表示边界点的个数；第一个点和最后一个点的距离为：第一个点和最后一个点连接形成的边为e
n，1
，以最长边e
max
对应的的方向为直线行驶的方向，即地块内作业路径中直线行驶的方向平行于最长边。
16.优选的，所述步骤s22包括：
17.1)基于步骤s1中已知最长边和最长边的端点坐标，假设端点坐标为ai(xi，yi)，a
i+1
(x
i+1
，y
i+1
)，根据这两点坐标求出最长边e
max
的直线方程；
18.2)比较各边界点到最长边e
max
的垂直距离，因需要对地块进行全覆盖，故需找到距离最长边e
max
的最远的边界点aj(xj，yj)，记录其距离dj；
19.3)计算相邻两个边界点之间的直线方程，即对应边的直线方程，除去最长边的直线方程；
20.4)判断最远边界点与最长边的位置；
21.5)根据幅宽和最长边的直线生成若干能作为路径直线部分的直线，求路径直线部分的直线与各个边的交点，基于不同相邻交点间的连线能生成若干地块内作业路径。
22.优选的，所述步骤4)包括下列情况：
23.(1)如果最长边e
max
平行于y轴，有：
24.若dj＞0，则最远的边界点aj(xj，yj)在最长边e
max
的右侧；
25.若dj＜0，则最远的边界点aj(xj，yj)在最长边e
max
的左侧；
26.(2)如果最长边e
max
平行于x轴，有：
27.若dj＞0，则最远的边界点aj(xj，yj)在最长边e
max
的上侧；
28.若dj＜0，则最远的边界点aj(xj，yj)在最长边e
max
的下侧；
29.(3)其他情况下，有：
30.若dj＞0，则最远的边界点aj(xj，yj)在最长边e
max
的右侧；
31.若dj＜0，则最远的边界点aj(xj，yj)在最长边e
max
的左侧。
32.优选的，所述步骤5)包括，当最远边界点aj(xj，yj)在最长边e
max
的特定方向的一侧，根据幅宽w，把步骤1)的直线方程形成的直线逐条向所述特定方向平移，直到最远的边界点；由此形成若干条能作为路径直线部分的直线，幅宽即相邻直线的间距；确定这些直线与步骤3)中所得所有边的交点，把每次移动w距离所产生的交点与移动方向上的相邻交点用直线方式加以连接。
33.优选的，最长边e
max
平行于y轴的情况下，若所述特定方向为最长边e
max
的右侧，则步骤1)的直线方程形成的直线逐条向右移动，若所述特定方向为最长边e
max
的左侧，则步骤1)的直线方程形成的直线逐条向左移动；
34.最长边e
max
平行于x轴的情况下，若所述特定方向为最长边e
max
的上侧，则步骤1)的直线方程形成的直线逐条向上移动，若所述特定方向为最长边e
max
的下侧，则步骤1)的直线方程形成的直线逐条向下移动；
35.其他情况下，基于前述步骤1)中的斜率和aj(xj，yj)，求出最长边的法线方程，若所述特定方向为最长边e
max
的右侧，在将由步骤1)的直线方程形成的直线沿法线方向逐条向右平移，若所述特定方向为最长边e
max
的左侧，在将由步骤1)的直线方程形成的直线沿法线方向逐条向左平移。
36.优选的，所述步骤s23包括，
37.1)以最长边e
max
的某一端点或最远的边界点aj(xj，yj)为起点；
38.2)计算得到步骤s22中步骤5)中各个交点之间的直线距离；
39.3)为使得后续算法能够优先遍历未通过的直线行驶路径，在此人为规定直线行驶路径的距离为0；
40.4)基于贪婪思想进行全覆盖的自动生成全覆盖的地块内作业路径，此过程不断搜索距离当前交点最近，且未被遍历过的交点，作为路径规划的下一个交点，直至遍历完所有的交点。
41.本发明具有以下优点：本方案通过指定下田口，能自动操控农机从该下田口出发，前往待作业地块，作业完成之后，回到下田口，全自动生成作业行驶轨迹。其次，在指定的下田口与不规则地块情况下，本方案生成的路径遵循最短路径与覆盖面积最广原则，是整体最优的作业行驶轨迹。采用本方案的农机即使对多个不相邻的基于多个下田口的田块，也能做到在下田口到田块整个作业过程中自动生成完整作业路径，减少人工操控农机的需要，提高了农机自动化运行的水平。
附图说明
42.图1为本发明一种指定下田口的往返式全自动路径规划方法的基本流程图。
43.图2为本发明中生成往返式全自动路径的示意图。
具体实施方式
44.下面对照附图，通过对实施例的描述，对本发明具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。
45.如图1-2所示，本发明提供了一种指定下田口的往返式全自动路径规划方法，包括。
46.s1、基于作业区域平面图标记需要作业的不规则地块和对应的下田口位置。标记后的不规则地块图形为s，下田口标记为a0。
47.s2、基于不规则地块的形状生成相应的地块内作业路径。
48.不规则地块图形为不规则多边形，采用逆时针选边界点并依次编号，可得不规则多边形s的顶点为a1(x1，y1)，a2(x2，y2)，...，an(xn，yn)，其中，n≥3，n表示边界点的个数，农机作业的幅宽为w。地块内作业路径的生成方法具体包括。
49.s21、直线行驶方向的确定。选择最长的边为直线行驶的方向，这样转弯次数最少，路径最短。这里直线行驶指地块内作物进行作业的行驶路径，与在直线行驶的作业路线间沿地块边缘移动的路径相区分。
50.按逆时针选择边界点，遍历所有的边界点，存入数组中，计算相邻两点之间的长
度，记为：连接相邻的两个边界点并记为边e
i，j
，遍历所有的边，记录最长边e
max
和最长边的两个端点(即边界点)ai(xi，yi)，a
i+1
(x
i+1
，y
i+1
)。0≤i，j≤n，n≥3，n表示边界点的个数；第一个点和最后一个点的距离为：第一个点和最后一个点连接形成的边为e
n，1
。应当以最长边e
max
对应的的方向为直线行驶的方向，即地块内作业路径中直线行驶的方向平行于最长边。
51.s22、基于作业覆盖率最高原则和直线行驶方向生成可能的路径连线。具体包括：1)基于上一步(即步骤s1)中已知最长边和最长边的端点坐标，假设端点坐标为ai(xi，yi)，a
i+1
(x
i+1
，y
i+1
)，根据这两点坐标求出最长边e
max
的直线方程(x，y表示方程的未知参数)：
52.斜率：
53.直线方程：y-yi＝k(x-xi)；
54.一般式：kx-y+(y
i-kxi)＝0。
55.2)比较各边界点到最长边e
max
的垂直距离，因需要对地块进行全覆盖，故需找到距离最长边e
max
的最远的边界点aj(xj，yj)，记录其距离dj。计算式如下。
56.最长的垂直距离：
57.3)计算相邻两个边界点之间的直线方程，即对应边的直线方程，除去最长边的直线方程。直线方程具体如下。
[0058][0059][0060]
……
[0061][0062][0063]
4)判断最远边界点与最长边的位置。
[0064]
(1)如果最长边e
max
平行于y轴，有：
[0065]
若dj＞0，则最远的边界点aj(xj，yj)在最长边e
max
的右侧；
[0066]
若dj＜0，则最远的边界点aj(xj，yj)在最长边e
max
的左侧。
[0067]
(2)如果最长边e
max
平行于x轴，有：
[0068]
若dj＞0，则最远的边界点aj(xj，yj)在最长边e
max
的上侧；
[0069]
若dj＜0，则最远的边界点aj(xj，yj)在最长边e
max
的下侧。
[0070]
(3)其他情况下，有：
[0071]
若dj＞0，则最远的边界点aj(xj，yj)在最长边e
max
的右侧；
[0072]
若dj＜0，则最远的边界点aj(xj，yj)在最长边e
max
的左侧。
[0073]
5)求交点。具体包括：
[0074]
(1)如果最长边e
max
平行于y轴：
[0075]
a、最远边界点aj(xj，yj)在最长边e
max
的右侧：根据幅宽w，把前述步骤1)的直线方程形成的直线逐条向右平移w，2w，...的距离，直到最远的边界点。由此形成若干条可作为路径直线部分的直线。之后计算这些直线与步骤3)中所得所有边(除最长边)形成的交点，把x轴坐标相同的交点用直线方式加以连接。
[0076]
b、最远边界点aj(xj，yj)在最长边e
max
的左侧：根据幅宽w，把前述步骤1)的直线方程形成的直线逐条向左平移w，2w，...的距离，直到最远的边界点。由此形成若干条可作为路径直线部分的直线。之后计算这些直线与步骤3)中所得所有边(除最长边)形成的交点，把x轴坐标相同的交点用直线方式加以连接。
[0077]
(2)如果最长边e
max
平行于x轴：
[0078]
a、最远边界点aj(xj，yj)在最长边e
max
的上侧：根据幅宽w，把前述步骤1)的直线方程形成的直线逐条向上平移w，2w，...的距离，直到最远的边界点。由此形成若干条可作为路径直线部分的直线。之后计算这些直线与步骤3)中所得所有边(除最长边)形成的交点，把y轴坐标相同的交点用直线方式加以连接。
[0079]
b、最远边界点aj(xj，yj)在最长边e
max
的下侧：根据幅宽w，把前述步骤1)的直线方程形成的直线逐条向下平移w，2w，...的距离，直到最远的边界点。由此形成若干条可作为路径直线部分的直线。之后计算这些直线与步骤3)中所得所有边(除最长边)形成的交点，把y轴坐标相同的交点用直线方式加以连接。
[0080]
(3)其他情况。
[0081]
a、最远边界点aj(xj，yj)在最长边e
max
的右侧：根据幅宽w，基于前述步骤1)中的斜率和aj(xj，yj)，求出最长边的法线方程。在将由步骤1)的直线方程形成的直线沿法线方向逐条向右平移w，2w，...的距离，直到最远的边界点。由此形成若干条可作为路径直线部分的直线。之后计算这些直线与步骤3)中所得所有边(除最长边)形成的交点，把每次移动w距离所产生的交点与移动方向上的相邻交点用直线方式加以连接。
[0082]
b、最远边界点aj(xj，yj)在最长边e
max
的左侧：根据幅宽w，基于前述步骤1)中的斜率和aj(xj，yj)，求出最长边的法线方程。在将由步骤1)的直线方程形成的直线沿法线方向逐条向左平移w，2w，...的距离，直到最远的边界点。由此形成若干条可作为路径直线部分的直线。之后计算这些直线与步骤3)中所得所有边(除最长边)形成的交点，把每次移动w距离所产生的交点与移动方向上的相邻交点用直线方式加以连接。
[0083]
s23、基于贪婪思想生成全覆盖的地块内作业路径路径。
[0084]
1)以最长边e
max
的某一端点(边界点)或最远的边界点aj(xj，yj)为起点。
[0085]
2)计算得到步骤s22中步骤5)中各个交点之间的直线距离。
[0086]
3)为使得后续算法能够优先遍历未通过的直线行驶路径，在此人为规定直线行驶路径的距离为0。
[0087]
4)基于贪婪思想进行全覆盖的自动生成全覆盖的地块内作业路径。此过程不断搜索距离当前交点最近，且未被遍历过的交点，作为路径规划的下一个交点，直至遍历完所有的交点。
[0088]
s3、生成由地块内作业路径的起点和终点到下田口的最短路径，形成最优的全程路径。
[0089]
s31、生成下田口到起点的路径。
[0090]
从下田口遍历搜索所有的边界点，计算出下田口到最长边的两个端点和离最长比最远边界点的距离，其中距离最短的路径，即为下田口到起点的路径。
[0091]
本实施例中，如图2所示，有四条路径：
[0092]
第一条路径：下田口-＞a2(最短)；
[0093]
第二条路径：下田口-＞a
2-＞a
1-＞a8；
[0094]
第三条路径：下田口-＞a3；
[0095]
第四条路径：下田口-＞a
3-＞a
4-＞a
5-＞a
6-＞a
7-＞a8[0096]
计算可得第一条路径最短，因此以a2为起点。
[0097]
s32、生成终点到下田口的路径。
[0098]
从终点遍历所有的边界点，找到距离下田口最短的一条路径，即为终点到下田口的路径。
[0099]
本实施例中，如图2所示，有两条路径：
[0100]
第一条路径：终点-＞a
1-＞a
2-＞下田口(最短)；
[0101]
第二条路径：终点-＞a
7-＞a
6-＞a
5-＞a
4-＞a
3-＞下田口
[0102]
计算可得第一条路径最短，因此从终点到下田口走第一条路径。
[0103]
基于确定后的地块内作业路径起点和终点的选择可能有2至3种，基于本步骤确定起点、终点和下田口到起点终点的最短路径，计算全程路径。这样既满足地块作业的覆盖率最高原则，又满足整体作业路径里程最短要求，实现全程路径最优的效果。
[0104]
上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的发明构思和技术方案进行的各种非实质性的改进，或未经改进将本发明构思和技术方案直接应用于其它场合的，均在本发明保护范围之内。

技术特征：
1.一种指定下田口的往返式全自动路径规划方法，其特征在于：包括：s1、基于作业区域平面图标记需要作业的不规则地块和对应的下田口位置；s2、基于不规则地块的形状生成相应的地块内作业路径，满足地块作业的覆盖率最高原则；s3、生成由地块内作业路径的起点和终点到下田口的最短路径，满足整体作业路径里程最短要求，形成最优的全程路径。2.根据权利要求1所述的一种指定下田口的往返式全自动路径规划方法，其特征在于：所述步骤s3包括：s31、生成下田口到起点的路径，从下田口遍历搜索所有的边界点，计算出下田口到最长边的两个端点和离最长比最远边界点的距离，其中距离最短的路径，即为下田口到起点的路径；s32、生成终点到下田口的路径，从终点遍历所有的边界点，找到距离下田口最短的一条路径，即为终点到下田口的路径。3.根据权利要求1所述的一种指定下田口的往返式全自动路径规划方法，其特征在于：所述步骤s2包括：s21、直线行驶方向的确定，选择最长的边为直线行驶的方向；s22、基于作业覆盖率最高原则和直线行驶方向生成可能的路径连线；s23、基于贪婪思想生成全覆盖的地块内作业路径路径。4.根据权利要求3所述的一种指定下田口的往返式全自动路径规划方法，其特征在于：所述步骤s21包括，按逆时针选择边界点，遍历所有的边界点，存入数组中，计算相邻两点之间的长度，记为：连接相邻的两个边界点并记为边e
i，j
，遍历所有的边，记录最长边e
max
和最长边的两个端点a
i
(x
i
，y
i
)，a
i+1
(x
i+1
，y
i+1
)，0≤i，j≤n，n≥3，n表示边界点的个数；第一个点和最后一个点的距离为：第一个点和最后一个点连接形成的边为e
n，1
，以最长边e
max
对应的的方向为直线行驶的方向，即地块内作业路径中直线行驶的方向平行于最长边。5.根据权利要求4所述的一种指定下田口的往返式全自动路径规划方法，其特征在于：所述步骤s22包括：1)基于步骤s1中已知最长边和最长边的端点坐标，假设端点坐标为a
i
(x
i
，y
i
)，a
i+1
(x
i+1
，y
i+1
)，根据这两点坐标求出最长边e
max
的直线方程；2)比较各边界点到最长边e
max
的垂直距离，因需要对地块进行全覆盖，故需找到距离最长边e
max
的最远的边界点a
j
(x
j
，y
j
)，记录其距离d
j
；3)计算相邻两个边界点之间的直线方程，即对应边的直线方程，除去最长边的直线方程；4)判断最远边界点与最长边的位置；5)根据幅宽和最长边的直线生成若干能作为路径直线部分的直线，求路径直线部分的直线与各个边的交点，基于不同相邻交点间的连线能生成若干地块内作业路径。6.根据权利要求5所述的一种指定下田口的往返式全自动路径规划方法，其特征在于：所述步骤4)包括下列情况：
(1)如果最长边e
max
平行于y轴，有：若d
j
＞0，则最远的边界点a
j
(x
j
，y
j
)在最长边e
max
的右侧；若d
j
＜0，则最远的边界点a
j
(x
j
，y
j
)在最长边e
max
的左侧；(2)如果最长边e
max
平行于x轴，有：若d
j
＞0，则最远的边界点a
j
(x
j
，y
j
)在最长边e
max
的上侧；若d
j
＜0，则最远的边界点a
j
(x
j
，y
j
)在最长边e
max
的下侧；(3)其他情况下，有：若d
j
＞0，则最远的边界点a
j
(x
j
，y
j
)在最长边e
max
的右侧；若d
j
＜0，则最远的边界点a
j
(x
j
，y
j
)在最长边e
max
的左侧。7.根据权利要求6所述的一种指定下田口的往返式全自动路径规划方法，其特征在于：所述步骤5)包括，当最远边界点a
j
(x
j
，y
j
)在最长边e
max
的特定方向的一侧，根据幅宽w，把步骤1)的直线方程形成的直线逐条向所述特定方向平移，直到最远的边界点；由此形成若干条能作为路径直线部分的直线，幅宽即相邻直线的间距；确定这些直线与步骤3)中所得所有边的交点，把每次移动w距离所产生的交点与移动方向上的相邻交点用直线方式加以连接。8.根据权利要求7所述的一种指定下田口的往返式全自动路径规划方法，其特征在于：最长边e
max
平行于y轴的情况下，若所述特定方向为最长边e
max
的右侧，则步骤1)的直线方程形成的直线逐条向右移动，若所述特定方向为最长边e
max
的左侧，则步骤1)的直线方程形成的直线逐条向左移动；最长边e
max
平行于x轴的情况下，若所述特定方向为最长边e
max
的上侧，则步骤1)的直线方程形成的直线逐条向上移动，若所述特定方向为最长边e
max
的下侧，则步骤1)的直线方程形成的直线逐条向下移动；其他情况下，基于前述步骤1)中的斜率和a
j
(x
j
，y
j
)，求出最长边的法线方程，若所述特定方向为最长边e
max
的右侧，在将由步骤1)的直线方程形成的直线沿法线方向逐条向右平移，若所述特定方向为最长边e
max
的左侧，在将由步骤1)的直线方程形成的直线沿法线方向逐条向左平移。9.根据权利要求4所述的一种指定下田口的往返式全自动路径规划方法，其特征在于：所述步骤s23包括：1)以最长边e
max
的某一端点或最远的边界点a
j
(x
j
，y
j
)为起点；2)计算得到步骤s22中步骤5)中各个交点之间的直线距离；3)为使得后续算法能够优先遍历未通过的直线行驶路径，在此人为规定直线行驶路径的距离为0；4)基于贪婪思想进行全覆盖的自动生成全覆盖的地块内作业路径，此过程不断搜索距离当前交点最近，且未被遍历过的交点，作为路径规划的下一个交点，直至遍历完所有的交点。

技术总结
本发明公开了一种指定下田口的往返式全自动路径规划方法，包括：S1、基于作业区域平面图标记需要作业的不规则地块和对应的下田口位置；S2、基于不规则地块的形状生成相应的地块内作业路径，满足地块作业的覆盖率最高原则；S3、生成由地块内作业路径的起点和终点到下田口的最短路径，满足整体作业路径里程最短要求，形成最优的全程路径。本发明通过指定下田口，能自动操控农机从该下田口出发，前往待作业地块，作业完成之后，回到下田口，全自动生成作业行驶轨迹。成作业行驶轨迹。成作业行驶轨迹。


技术研发人员：俞鹏飞 黄河 孙友强 吴晓伟
受保护的技术使用者：安徽中科智能感知科技股份有限公司
技术研发日：2023.07.10
技术公布日：2023/9/9