CN115892064A - 一种无人驾驶拖拉机的作业路径规划方法 - Google Patents

Abstract

本发明公开了一种无人驾驶拖拉机的作业路径规划方法，包括：步骤1、确认目标地块的条件，根据目标地块条件采集地块位置坐标，所述目标地块的形状为矩形，标记该矩形顶点对应的田间实际位置，通过定位导航系统采集田间标记位置的经纬度坐标；步骤2、根据所采集的地块位置坐标和拖拉机基本参数，建立平面坐标系，在平面坐标系内生成拖拉机作业区域地图；步骤3、根据拖拉机作业区域地图规划路径，生成拖拉机作业的直线局部路径和曲线局部路径后，按路径顺序将直线和曲线连接起来，即完成了拖拉机作业的全局路径。本发明的规划的路径可以覆盖田间作业的全区域，适用于各种无人拖拉机的地块作业规划。同时简化了用户使用操作。

Description

一种无人驾驶拖拉机的作业路径规划方法

技术领域

本发明涉及无人驾驶拖拉机技术领域，尤其涉及一种无人驾驶拖拉机作业路径规划方法。

背景技术

近年来，精准农业和农机自动化对农业更高效、更经济、可持续发展的作用越来越重要，无人驾驶的作业方式逐步提上日程。目前无人驾驶技术已小规模应用于拖拉机技术领域，但对于非专业用户来说，还是难以驾驭标识地图、规划路线、设置任务等一系列与无人驾驶拖拉机使用方法相关的操作。为此本专利结合无人驾驶操作流程与田间作业过程需求，发明了一种可实现规则地块的全区域覆盖、不重耕、不漏耕的作业路径自动规划方法。

该方法实现了用户只需简单输入就能完成无人驾驶作业设置的全过程，简化了用户使用，具有较强的可操作性，有利于进一步推广无人驾驶拖拉机的实地应用。

发明内容

针对上述现有技术，本发明要解决的技术问题在于无人驾驶拖拉机如何在农田实地应用中，满足作业路径和简易操作的要求，为此提供了一种无人驾驶拖拉机作业路径规划方法。

为了解决上述技术问题，本发明提出的一种无人驾驶拖拉机的作业路径规划方法，主要包括：

步骤1、确认目标地块的条件，根据目标地块条件采集地块位置坐标，所述目标地块的形状为矩形，标记该矩形顶点对应的田间实际位置，通过定位导航系统采集田间标记位置的经纬度坐标；

步骤2、根据所采集的地块位置坐标和拖拉机基本参数，建立平面坐标系，在平面坐标系内生成拖拉机作业区域地图；

步骤3、根据拖拉机作业区域地图规划路径，生成拖拉机作业的全局路径。

其中，步骤2包括以下步骤：

2-1)获得如下参数的数值：拖拉机总长l₀，单位m；拖拉机总宽w，单位m；拖拉机作业转弯半径r₀，单位m；作业幅宽d，d≥w单位m；

2-2)采用高斯－克吕格平面直角坐标系，将所述矩形地块的4个顶点的坐标进行高斯投影解算，转换为高斯－克吕格平面直角坐标系中的平面坐标；过程如下：

从所述的4个顶点中任意选择一个点作为路径的起点，记为A点，并计算A点分别到另外三个点的距离，定义与该A点距离最短的点为B点，沿A点到B点的走向顺序定义另外两个点分别为C点、D点；以A点为坐标原点，直线AB为横轴，记为X轴；直线AD为纵轴，记为Y轴，所述矩形地块记为ABCD；

2-3)定义拖拉机的定位点在车辆中线上、且拖拉机沿纵轴方向行驶，车辆中线与矩形地块ABCD纵边之间的作业空间的宽度为d/2，车辆中线与矩形地块ABCD横边之间的作业空间的宽度为d/2+r₀，得到的拖拉机作业区域地图，记为abcd；

步骤3包括以下步骤：

3-1)生成直线局部路径，包括：

3-1-1)确定总行数n：将坐标点a和坐标点b之间的距离除以幅宽d后，向下取整再加1；沿X轴形成n行直线阵列；

3-1-2)定义n_a为当前行编号，n_b为下一目标行编号，n_end为最终行的编号，对所有行的编号求和，并记为N；路径由第一行开始，此时，n_a＝1，n_b＝0；

3-1-3)判断n_a是否小于等于N/n，若是执行步骤3-1-4)，否则执行步骤3-1-6)；

3-1-4)将n/2的结果向上取整得到m，定义n_b＝n_a+m；判断n_b是否大于n，若是，n_a＝n_end，执行步骤3-1-8)；否则保存n_a的值，执行步骤3-1-5)；

3-1-5)定义n_a＝n_b，判断n_a是否大于N/n，若是，执行步骤3-1-6)，否则返回步骤3-1-3)；

3-1-6)判断n_a是否等于总行数n且为偶数，若是，n_a＝n_end，执行步骤3-1-8)；否则，保存n_a的值，执行步骤3-1-7)；

3-1-7)将(n/2)-1的结果向上取整后更新m，定义n_b＝n_a–m；以n_b更新当前行：n_a＝n_b，执行步骤3-1-3)；

3-1-8)将所有的当前行编号，按保存的顺序排列，生成直线路径；

3-2)生成曲线局部路径，包括有转弯及倒车局部路径和转弯及平移局部路径：

3-2-1)当d*|n_a-n_b|＜2r₀时，采用倒车路径，由当前行终点驶出，向目标行方向在调头区域中以r₀为半径转弯规划90°弧线路径，到达第一次驻车点；第二次驻车点驶入目标行的弧线路径满足半径为r₀；按照：自当前行终点、依次经过第一次驻车点和第二次驻车点后至目标行起点连接生成倒车路径；

3-2-2)当d*|na-nb|≥2r₀时，采用转弯路径，由当前行终点驶出，向目标行方向在调头区域中以r₀为半径规划90°弧线路径，到达第一拐点；第二拐点驶入目标行的弧线路径满足半径r₀；按照：自当前行终点、依次经过第一拐点和第二拐点后至目标行起点连接生成转弯路径；

3-3)按路径顺序将上述步骤3-1)和步骤3-2)生成的各直线和曲线连接起来，即为拖拉机作业的全局路径。

与现有技术相比，本发明的有益效果是：

本发明提出的路径规划方法可以覆盖无人驾驶拖拉机田间作业的全区域，根据地图坐标自动规划局部最优路径和全局路径，整个规划过程无需人工干预，有效的避免了重耕、漏耕，适用于各种无人驾驶拖拉机的田间作业。

附图说明

图1是本发明路径规划方法中目标矩形地块和拖拉机作业区域地图；

图2是本发明路径规划方法中直线阵列示意图；

图3是本发明路径规划方法中直线局部路径示意图；

图4是本发明路径规划方法中转弯及倒车局部路径示意图；

图5是本发明路径规划方法中转弯及平移局部路径示意图；

图6是本发明路径规划方法中完整路径示意图

图7是本发明路径规划方法中直线局部路径设计流程图。

具体实施方式

下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。

本发明提出了一种无人驾驶拖拉机的作业路径规划方法，包括以下步骤：

步骤1、确认目标地块的条件，根据目标地块条件采集地块位置坐标，该目标地块的形状为矩形，标记矩形顶点对应的田间实际位置；检查目标地块所处范围内是否存在固定障碍物，诸如树木、电杆等，如果有的话，进行移除，然后，通过GPS/BDS定位导航系统采集田间标记位置的经纬度坐标。

步骤2、根据所采集的地块位置坐标和拖拉机基本参数，通过无人驾驶拖拉机的自动驾驶控制器建立平面坐标系，在平面坐标系内生成拖拉机作业区域地图；具体过程如下：

2-1)获得如下参数的数值：

(1)位置坐标(无顺序要求)：P1、P2、P3和P4

(2)选择起点：P1、P2、P3和P4中的任何一个点

(3)拖拉机总长l₀，单位m；

(4)拖拉机总宽w，单位m；

(5)拖拉机作业转弯半径r₀，单位m；

(6)作业幅宽d，d≥w单位m；

需保证车宽不超过幅宽，即w≤d，否则，是不会生成地图的。

2-2)采用高斯－克吕格平面直角坐标系，将所述矩形地块的4个顶点的坐标进行高斯投影解算，转换为高斯－克吕格平面直角坐标系中的平面坐标。

由于我国的地形图采用高斯－克吕格平面直角坐标系，所以在不考虑地面复杂地形影响的情况下，以当地中央子午线投影为纵轴，赤道投影为横轴，其交点为坐标原点，将通过步骤1得到的4个位置空间坐标进行高斯投影解算，转换为平面坐标P1、P2、P3、P4。

以下过程由无人驾驶拖拉机自动完成，具体方法如下：

选择4个坐标(P1、P2、P3、P4)中的任意一个为路径的起点，本实施例将起点设为P1示例。如图1所示，首先定义起点P1为A点，计算|P1-P2|、|P1-P3|、|P1-P4|的直线距离；定义距离最短的点为B点，以P2为B点示例，按P1至P2的时针顺序，依次定义其它两个坐标点为C点、D点。根据各点的平面坐标建立平面直角坐标系，以A为坐标原点，直线AB为横轴，用X表示；垂直于AB的直线AD为纵轴，用Y表示，所述矩形地块记为ABCD，如图1所示。

考虑到拖拉机的定位点是在车辆中线上，与目标地块边界之间需预留半个幅宽(d/2)的作业空间，所以将ABCD矩形的四个点分别以

和

的方向，向内平移50％幅宽(d/2)的距离；再考虑到拖拉机作业转弯半径r₀，需在目标地块两端预留调头区域以满足连续作业的需要。所以再将ABCD四个位置分别以

和

的方向向内平移d/2+r₀，由此生成拖拉机作业区域为矩形abcd，该矩形abcd顶点对应的平面坐标为别为Pa、Pb、Pc和Pd。由此在平面直角坐标系内，生成了目标地块对应的地图ABCD和作业区域对应的地图abcd，如图1所示。

步骤3、根据拖拉机作业区域地图规划路径，生成拖拉机作业的全局路径，主要包括：生成直线局部路径，生成曲线局部路径和按路径顺序将生成的各直线和曲线连接起来，即为拖拉机作业的全局路径。以下由无人驾驶拖拉机自动完成，具体方法如下：

1)定义过程变量，包括

n：全局路径总行数，n＝Floor(|Pa-Pb|/d)+1，计算坐标点a和坐标点b之间的距离除以幅宽d后，向下取整再加1。沿X轴形成n行直线阵列，如图2所示。

n_a：当前行驶行编号，

n_b：当前行行驶结束后，需驶入的下一目标行，n_b为下一目标行编号；

n_end：行驶的最后一行，即最终行的编号，将所有行的编号数值求和记为N；

d：作业幅宽

l₀：拖拉机总长

w：拖拉机总宽

r₀：拖拉机作业转弯半径

2)生成直线阵列

以5行地块为例，如图2所示，以ad为第1行，根据幅宽按a→b的方向共生成n行直线，依次给各行编号1、2、3、4、5即沿X轴方向

方向生成5行直线阵列，该直线阵列的路径要求完全行驶但不能重复。

3)生成路径规划，包括生成直线路径规划和曲线局部路径，所述的曲线局部路径由包括转弯及倒车局部路径和转弯及平移局部路径。

3-1)生成直线路径规划，如图7所示，过程如下：

3-1-1)按照n＝Floor(|Pa-Pb|/d)+1计算总行数n＝5；所有行的编号数值求和为N＝1+2+3+4+5＝15

3-1-2)路径由第一行开始，此时，初始值n_a＝1，n_b＝0；

3-1-3)判断当前行n_a是否小于等于N/n，具体讲，判断当前行n_a是否在1至3行内，若是，执行步骤3-1-4)，否则执行步骤3-1-6)；

3-1-4)将n/2的结果向上取整得到m的值，n_a的下一行是第n_a+m行，本实施例的该步骤中，m＝3，定义n_b＝n_a+m；判断n_b是否大于n，若是，则说明所有路径已完成，当前行是最后一行，即n_a＝n_end，不再执行新的n_b行，将所有的当前行编号，按保存的顺序排列，完成生成直线路径的过程；否则保存n_a的值，执行步骤3-1-5)；

3-1-5)定义n_a＝n_b，判断n_a是否大于N/n，即新的当前行n_a是1、2、3，还是4、5，如果是在1至3行内，执行步骤3-1-6)，如果是4或5，则返回步骤3-1-3)；

3-1-6)判断na是否等于总行数n且为偶数，若是则说明所有路径已完成，即当前行是最后一行，n_a＝n_end，将所有的当前行编号，按保存的顺序排列，完成生成直线路径的过程；否则，保存n_a的值，执行步骤3-1-7)；

3-1-7)将(n/2)-1的结果向上取整后得到新的m的值，n_a的下一行是第n_a-m行，本实施例的该步骤中，新的m的值是2，定义n_b＝n_a–m；以n_b更新当前行：n_a＝n_b，执行步骤3-1-3)。

以5行的地图为例：生成直线路径如图3所示，行编号的顺序依次为1→4→2→5→3。

3-2)生成曲线局部路径，包括有转弯及倒车局部路径和转弯及平移局部路径中的一种或两种情形，因为根据总行数n、作业幅宽d和作业转弯半径r₀的具体数值，以上两种曲线局部路径的情形可能同时存在，也可能只存在任意一种，但其一种或两种的路径情形会固定贯穿整个地块路径规划。

在图4中，各行的曲线连接只是路径顺序的示意，曲线路径具体规划分为转弯+倒车的路径和转弯+平移的路径两种方式，如下：

3-2-1)当d*|n_a-n_b|＜2r₀时，采用倒车路径，由当前行终点驶出，向目标行方向在调头区域中以r₀为半径转弯规划90°弧线路径，到达第一次驻车点；第二次驻车点驶入目标行的弧线路径满足半径为r₀；按照：自当前行终点、依次经过第一次驻车点和第二次驻车点后至目标行起点连接生成倒车路径。本实施例中以5行的地图为例，假设2d＜2r₀＜3d，第4行驶入第2行、第5行驶入第3行，要求拖拉机在空间直径为2d的范围内调头，由于空间直径小于拖拉机的转弯直径(2d＜2r₀)，无法满足拖拉机转弯调头的要求，所以需采用倒车路径方式，过程描述如下：如图4所示，由当前行终点I驶出，向目标行方向(顺时针或逆时针)在调头区域中以作业半径r₀转弯规划90°弧线路径，到达第一次驻车点Ⅱ；计算第二次驻车点Ⅲ，保证该点驶入目标行起点Ⅳ的弧线路径满足作业半径r₀；将当前行终点Ⅰ→第一次驻车点Ⅱ→第二次驻车点Ⅲ→目标行起点Ⅳ，依次连接生成路径。

以第4行驶入第2行为例，如图4所示，步骤如下，

步骤1：第4行终点设定为点Ⅰ，可根据幅宽和调头区域预留的纵向深度数据计算得到点Ⅰ坐标(x,y)；

步骤2：点Ⅰ以顺时针方向、作业半径r₀规划90°弧线路径，到达第一次驻车点，设定为点Ⅱ，计算得到点Ⅱ坐标(x-r₀,y-r₀)

步骤3：第2行起点设定为点Ⅳ，同步骤1方法可得到点Ⅳ坐标(x-2d,y)；

步骤4：点Ⅳ以逆时针方向、作业半径r₀规划90°弧线路径，到达第二次驻车点，设定为点Ⅲ，计算得到点Ⅲ坐标(x-2d+r₀,y-r₀)

按Ⅰ→Ⅱ→Ⅲ→Ⅳ的顺序，连接各点坐标，生成局部路径，由于Ⅱ→Ⅲ需倒车行驶，所以称为转弯+倒车路径。

3-2-2)当d*|na-nb|≥2r₀时，采用转弯路径，由当前行终点驶出，向目标行方向在调头区域中以r₀为半径规划90°弧线路径，到达第一拐点；第二拐点驶入目标行的弧线路径满足半径r₀；按照：自当前行终点、依次经过第一拐点和第二拐点后至目标行起点连接生成转弯路径；本实施例中以5行的地图为例，如图5所示，第1行驶入第4行、第2行驶入第5行，要求拖拉机在空间直径为3d的范围内调头，由于空间直径大于拖拉机的转弯直径(2r₀＜3d)，可以满足拖拉机转弯调头的要求，所以采用转弯路径方式，过程描述如下：由当前行终点Ⅰ’驶出，向目标行方向(顺时针或逆时针)在调头区域中规划90°弧线路径，到达第一拐点Ⅱ’；再计算第二拐点Ⅲ’，保证该点驶入目标行起点Ⅳ’的弧线路径满足作业半径r₀。将当前行终点Ⅰ’→第一拐点Ⅱ’→第二拐点Ⅲ’→目标行起点Ⅳ’依次连接生成路径。

以第1行驶入第4行为例，如图5所示，步骤如下，

步骤1：第1行终点设定为点Ⅰ’，可根据幅宽d和调头区域预留的纵向深度d/2+r₀数据计算得到点Ⅰ’坐标(x,y)；

步骤2：点Ⅰ’以顺时针方向、作业半径r₀规划90°弧线路径，到达第一拐点，设定为点Ⅱ’，计算得到点Ⅱ’坐标(x+r₀,y+r₀)

步骤3：第4行起点设定为点Ⅳ’，同步骤1方法可得到点Ⅳ’坐标(x+3d,y)；

步骤4：点Ⅳ’以逆时针方向、作业半径r₀规划90°弧线路径，到达第二拐点，设定为点Ⅲ’，计算得到点Ⅲ’坐标(x+3d-r₀,y+r₀)

按Ⅰ’→Ⅱ’→Ⅲ’→Ⅳ’的顺序，连接各点坐标，生成调头局部路径，由于可以平移完成调头，所以称为转弯+平移路径。

3-3)最后按路径顺序将上述步骤3-1)和步骤3-2)生成的各直线和曲线连接起来，即为拖拉机作业的全局路径。

以5行地图为例，最终形成的全局路径如图6中箭头所示，从始点a→直线段1→转弯+平移路段14→直线段4→转弯+倒车路段42→直线段2→转弯+平移路段25→直线段5→转弯+倒车路段53→直线段5→终点END。

尽管上面结合附图对本发明进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨的情况下，还可以做出很多变形，这些均属于本发明的保护之内。

Claims (1)

1.一种无人驾驶拖拉机的作业路径规划方法，其特征在于，包括以下步骤：

步骤1、确认目标地块的条件，根据目标地块条件采集地块位置坐标，所述目标地块的形状为矩形，标记该矩形顶点对应的田间实际位置，通过定位导航系统采集田间标记位置的经纬度坐标；

步骤2、根据所采集的地块位置坐标和拖拉机基本参数，建立平面坐标系，在平面坐标系内生成拖拉机作业区域地图；包括

2-1)获得如下参数的数值：拖拉机总长l₀，单位m；拖拉机总宽w，单位m；拖拉机作业转弯半径r₀，单位m；作业幅宽d，d≥w单位m；

2-2)采用高斯－克吕格平面直角坐标系，将所述矩形地块的4个顶点的坐标进行高斯投影解算，转换为高斯－克吕格平面直角坐标系中的平面坐标；过程如下：

从所述的4个顶点中任意选择一个点作为路径的起点，记为A点，并计算A点分别到另外三个点的距离，定义与该A点距离最短的点为B点，沿A点到B点的走向顺序定义另外两个点分别为C点、D点；以A点为坐标原点，直线AB为横轴，记为X轴；直线AD为纵轴，记为Y轴，所述矩形地块记为ABCD；

2-3)定义拖拉机的定位点在车辆中线上、且拖拉机沿纵轴方向行驶，车辆中线与矩形地块ABCD纵边之间的作业空间的宽度为d/2，车辆中线与矩形地块ABCD横边之间的作业空间的宽度为d/2+r₀，得到的拖拉机作业区域地图，记为abcd；

步骤3、根据拖拉机作业区域地图规划路径，生成拖拉机作业的全局路径；

3-1)生成直线局部路径，包括：

3-1-1)确定总行数n：将坐标点a和坐标点b之间的距离除以幅宽d后，向下取整再加1；沿X轴形成n行直线阵列；

3-1-2)定义n_a为当前行编号，n_b为下一目标行编号，n_end为最终行的编号，对所有行的编号求和，并记为N；路径由第一行开始，此时，n_a＝1，n_b＝0；

3-1-3)判断n_a是否小于等于N/n，若是执行步骤3-1-4)，否则执行步骤3-1-6)；

3-1-4)将n/2的结果向上取整得到m，定义n_b＝n_a+m；判断n_b是否大于n，若是，n_a＝n_end，执行步骤3-1-8)；否则保存n_a的值，执行步骤3-1-5)；

3-1-5)定义n_a＝n_b，判断n_a是否大于N/n，若是，执行步骤3-1-6)，否则返回步骤3-1-3)；

3-1-6)判断na是否等于总行数n且为偶数，若是，n_a＝n_end，执行步骤3-1-8)；否则，保存n_a的值，执行步骤3-1-7)；

3-1-7)将(n/2)-1的结果向上取整后更新m，定义n_b＝n_a–m；以n_b更新当前行：n_a＝n_b，执行步骤3-1-3)；

3-1-8)将所有的当前行编号，按保存的顺序排列，生成直线路径；

3-2)生成曲线局部路径，包括有转弯及倒车局部路径和转弯及平移局部路径两种形式，生成规范如下：

3-2-1)当d*|n_a-n_b|＜2r₀时，采用倒车路径，由当前行终点驶出，向目标行方向在调头区域中以r₀为半径转弯规划90°弧线路径，到达第一次驻车点；第二次驻车点驶入目标行的弧线路径满足半径为r₀；按照：自当前行终点、依次经过第一次驻车点和第二次驻车点后至目标行起点连接生成倒车路径；

3-2-2)当d*|na-nb|≥2r₀时，采用转弯路径，由当前行终点驶出，向目标行方向在调头区域中以r₀为半径规划90°弧线路径，到达第一拐点；第二拐点驶入目标行的弧线路径满足半径r₀；按照：自当前行终点、依次经过第一拐点和第二拐点后至目标行起点连接生成转弯路径；

3-3)按路径顺序将上述步骤3-1)和步骤3-2)生成的各直线和曲线连接起来，即为拖拉机作业的全局路径。

Images