CN113804212A - 一种耙地作业的路径规划方法及装置 - Google Patents

Abstract

本发明提供了一种耙地作业的路径规划方法及装置，包括：获取地块的边界数据；根据所述地块的边界数据得到所述地块的外接矩形；按照预设规则在所述外接矩形内规划耙地作业路径，得到第一规划路径；按照地块边界对所述第一规划路径进行裁剪，得到第二规划路径；根据所述第二规划路径，得到符合农机行驶要求的第三规划路径。本发明可以用于不规则地块的耙地路径规划，扩展了农机自动驾驶系统用于耙地的场景，从而提高了耙地效率；通过对不规则地块提供对角耙地的路径规划，提高了耙地效果。

Description

一种耙地作业的路径规划方法及装置

技术领域

本发明涉及路径规划技术领域，尤指一种耙地作业的路径规划方法及装置。

背景技术

农机自动驾驶系统是一种以传统农机的转向机构作为被控对象，根据GNSS(Global Navigation Satellite System，全球导航卫星系统)进行精确的位置计算，以系统中规划好的行驶路径作为目标路径进行行走方向控制的系统。农机自动驾驶系统可以极大的解放生产力，提高工作效率，为农事作业提供便利。

耙地是用耙(一种农具)进行的一种表土耕作，通常在犁耕后、播种前或早春保墒时进行。通过耙地可以把耕地经过犁地后产生的大土块机械破碎，耙地有疏松土壤、保蓄水分、提高土温等作用。

耙地有直线耙地、对角耙地等。直线耙地较为简单，按照犁沟的方向进行耙地作业即可，到达地边界掉头。对角耙地较为复杂，需要把耕地内每个区域经过两次交叉的路径对角耙过，这样有助于将犁沟填平和大土块破碎的更小。

无论是直线耙地或对角耙地，目前多是对规则地块进行耙地的路径规划，如长方形、正方形地块，无法对不规则地块，如不规则四边形、多边形等进行路径规划。然而在实际的农业场景中，仅有少部分规则地块，绝大多数地块都是不规则图形，所以当前的耙地路径规划方法极大地限制了农机自动驾驶系统用于耙地的场景，无法满足用户对耙地作业的要求。

发明内容

本发明的目的之一是提供一种耙地作业的路径规划方法及装置，用于解决目前路径规划方法只能对规则地块进行规划的问题。

本发明提供的技术方案如下：

一种耙地作业的路径规划方法，包括：获取地块的边界数据；根据所述地块的边界数据得到所述地块的外接矩形；按照预设规则在所述外接矩形内规划耙地作业路径，得到第一规划路径；按照地块边界对所述第一规划路径进行裁剪，得到第二规划路径；根据所述第二规划路径，得到符合农机行驶要求的第三规划路径。

可选地，所述的根据所述地块的边界数据得到所述地块的外接矩形包括：对所述地块的边界数据进行凸包计算，得到包含所述边界数据中所有点的最小的凸多边形；计算所述凸多边形的最小外接矩形，将所述最小外接矩形作为所述地块的外接矩形。

可选地，所述按照预设规则在所述外接矩形内规划耙地作业路径包括：当一斜率为第一斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第二斜率作下一路径线段；当一斜率为第二斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第一斜率作下一路径线段；所述第一斜率与所述第二斜率不同。

可选地，设置所述地块的外接矩形的一条边为滑动边，其余边为非滑动边；

所述按照预设规则在所述外接矩形内规划耙地作业路径还包括：当在一交点处无法得到有效的下一路径线段，且该交点位于所述外接矩形的滑动边时，则沿所述滑动边滑动预设距离，再按第二斜率或第一斜率作下一路径线段。

可选地，所述的按照所述地块边界对所述第一规划路径进行裁剪，得到第二规划路径，包括：将所述第一规划路径中超出地块边界的路径线缩回到所述地块边界上，将与所述地块边界无交点的路径线删除，得到第二规划路径。

可选地，所述的根据所述第二规划路径，得到符合农机行驶要求的第三规划路径，包括：所述第二规划路径为点集；按照所述第二规划路径初始化第三规划路径；从所述第二规划路径中选取除第一个点和最后一个点外的一个待处理点；从所述第三规划路径中获取与所述待处理点对应的点，将其前一个点作为起点，后一个点作为终点；计算从所述起点到所述终点的符合农机行驶要求的曲线点集；用去除所述起点和所述终点的曲线点集取代所述第三规划路径中与所述待处理点对应的点；选择所述第二规划路径中下一个待处理点，从所述第三规划路径中获取与所述下一个待处理点对应的点，重复上述过程，直至第二规划路径中除第一个点和最后一个点外的所有点都得到处理。

可选地，计算从所述起点到所述终点的符合农机行驶要求的曲线点集，包括：根据所述第三规划路径中与所述待处理点对应的点、所述起点、所述终点，得到起点方向和终点方向；根据农机转向半径、起点位置、终点位置、所述起点方向和所述终点方向，使用dubins曲线算法得到符合农机行驶要求的曲线点集。

本发明还提供一种耙地作业的路径规划装置，包括：

边界获取模块，用于获取地块的边界数据；矩形生成模块，用于根据所述地块的边界数据得到所述地块的外接矩形；第一规划模块，用于按照预设规则在所述外接矩形内规划耙地作业路径，得到第一规划路径；第二规划模块，用于按照地块边界对所述第一规划路径进行裁剪，得到第二规划路径；第三规划模块，用于根据所述第二规划路径，得到符合农机行驶要求的第三规划路径。

可选地，所述矩形生成模块，还用于对所述地块的边界数据进行凸包计算，得到包含所述边界数据中所有点的最小的凸多边形；计算所述凸多边形的最小外接矩形，将所述最小外接矩形作为所述地块的外接矩形。

可选地，所述第一规划模块，还用于当一斜率为第一斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第二斜率作下一路径线段；当一斜率为第二斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第一斜率作下一路径线段；所述第一斜率与所述第二斜率不同。

通过本发明提供的一种耙地作业的路径规划方法及装置，至少能够带来以下有益效果：本发明通过通过对地块的外接矩形进行路径规划，再对规划出来的路径进行裁剪，再对裁剪后的路径按农机行驶要求进行优化，从而实现不规则地块的路径规划，扩展了农机自动驾驶系统的应用场景，从而提高了耙地效率；通过对不规则地块提供对角耙地的路径规划，提高了耙地效果。

附图说明

下面将以明确易懂的方式，结合附图说明优选实施方式，对一种耙地作业的路径规划方法及装置的上述特性、技术特征、优点及其实现方式予以进一步说明。

图1是本发明的一种耙地作业的路径规划方法的一个实施例的流程图；

图2是对图1中第一规划路径进行裁剪的一种示意图；

图3是本发明的一种耙地作业的路径规划方法的另一个实施例的流程图；

图4是在外接矩形内规划的耙地作业路径的一种示意图；

图5是本发明的一种耙地作业的路径规划方法的另一个实施例的流程图；

图6是在外接矩形内规划的耙地作业路径的另一种示意图；

图7是本发明的一种耙地作业的路径规划装置的一个实施例的结构示意图；

图8是本发明一个具体应用实施例中农机自动驾驶系统的结构示意图；

图9是本发明一个具体应用实施例中一种地块边界图形的示意图；

图10是本发明一个具体应用实施例中一种地块的最小外接矩形的示意图；

图11是本发明一个具体应用实施例中在最小外接矩形内进行路径规划的操作示意图；

图12是本发明一个具体应用实施例中第一规划路径的示意图；

图13是本发明一个具体应用实施例中第二规划路径的示意图；

图14是本发明一个具体应用实施例中第三规划路径的示意图。

具体实施方式

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

为使图面简洁，各图中只示意性地表示出了与本发明相关的部分，它们并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部件，仅示意性地绘制了其中的一个，或仅标出了其中的一个。在本文中，“一个”不仅表示“仅此一个”，也可以表示“多于一个”的情形。

本发明的一个实施例，如图1所示，一种耙地作业的路径规划方法，包括：

步骤S100获取地块的边界数据；

步骤S200根据地块的边界数据得到地块的外接矩形；

步骤S300按照预设规则在外接矩形内规划耙地作业路径，得到第一规划路径；

步骤S400按照地块边界对第一规划路径进行裁剪，得到第二规划路径；

步骤S500根据第二规划路径，得到符合农机行驶要求的第三规划路径。

具体地，地块的边界数据是一数据点集，其反映了地块的边界。可以由人工驾驶农机沿着地块边界行驶一圈，以采集地块的边界数据。

地块的形状可能是不规则的，所以根据地块的边界数据得到的地块边界图形也可能是不规则图形。

计算包含地块边界图形的外接矩形。比如，在一XOY坐标系中，找到地块边界图形在X轴方向的最小坐标和最大坐标，Y轴方向的最小坐标和最大坐标，根据这4个坐标值就可确定该地块边界图形的外接矩形的四个顶点，从而确定外接矩形。

上面仅是一种简单外接矩形的计算方法，并不是面积最小的外接矩形。为了得到面积最小的矩形，可采用下面方法：

对地块的边界数据进行凸包计算，得到包含该边界数据中所有点的最小的凸多边形；计算该凸多边形的最小外接矩形，将该最小外接矩形作为地块的外接矩形。

可采用Graham扫描法或Jarvis步进法计算地块的边界数据的凸包，得到最小的凸多边形。

可采用以下方法计算凸多边形的最小外接矩形：计算凸多边形的中心；按预设角度逐步旋转凸多边形(比如在0-90°范围旋转，间距设为1°)，计算每次旋转后的凸多边形的简单外接矩形；从所有外接矩形中选择面积最小的外接矩形，将该外接矩形朝相反的方向旋转相同度数，得到凸多边形的最小外接矩形。

外接矩形/最小外接矩形所包含区域是一规则地块，可采用对规则地块的路径规划技术对外接矩形/最小外接矩形区域进行耙地作业路径规划。比如，直线耙地路径规划、对角耙地路径规划。通过直线耙地路径规划得到若干平行线段，通过对角耙地路径规划得到若干交叉线段。

在得到第一规划路径后，按照地块边界对第一规划路径进行裁剪，得到第二规划路径。具体包括：将第一规划路径中超出地块边界的路径线缩回到地块边界上，将与地块边界无交点的路径线删除，得到第二规划路径。

比如，如图2所示，假设虚线部分为地块边界，第一规划路径中有AB、BC两条线段，这两条线段与地块边界交于A1、A2、C1、C点。按照地块边界对第一规划路径进行裁剪，则A1B线段被缩回到A1点,BC1线段被缩回到C1点，AA2线段被缩回到A2点。若第一规划路径是用按序排列的点集来表达，比如{A,B,C}，则得到对应的第二规划路径为{A2,A1,C1,C}，其也是按序排列。若第一规划路径是用按序排列的线段集来表达，比如{AB,BC},则得到对应的第二规划路径为{A2A1,A1C1,C1C}，其也是按序排列。

根据第二规划路径，得到符合农机行驶要求的第三规划路径。假设第二规划路径为按序排列的点集，步骤S500可以进一步细化为：

步骤S510按照第二规划路径初始化第三规划路径。

第三规划路径中的点与第二规划路径中的点一一对应；比如，第三规划路径的点集等于第二规划路径的点集。

步骤S520从第二规划路径中选取除第一个点和最后一个点外的一个待处理点P_x；

步骤S530从第三规划路径中获取与该待处理点P_x对应的点，将其前一个点作为起点，其后一个点作为终点；

步骤S540计算从起点到终点的符合农机行驶要求的曲线点集；

步骤S550用去除起点和终点的曲线点集取代第三规划路径中与该待处理点P_x对应的点；

步骤S560判断第二规划路径中除第一个点和最后一个点外的所有点是否都得到处理；若否，则选择第二规划路径中下一个待处理点，跳转到步骤S530。若是，则结束。

步骤S540又可以进一步包括：

根据第三规划路径中与待处理点P_x对应的点、起点、终点，得到起点方向和终点方向；根据农机转向半径、起点位置、终点位置、起点方向和终点方向，得到符合农机行驶要求的曲线点集。

农机行驶要求包括农机转向时符合农机转向半径要求。起点方向为从起点至第三规划路径中与待处理点P_x对应的点的方向；终点方向为从第三规划路径中与待处理点P_x对应的点至终点的方向。

可以根据农机转向半径、起点位置、终点位置、起点方向和终点方向，使用dubins曲线算法得到符合农机行驶要求的曲线点集。这样可以得到从起点到终点的最短路径，有利于农机按规划路径耙地时缩短路程，降低农机的能耗。

如图2所示，假设第二规划路径为{A2,A1,C1,C}。初始化第三规划路径为{A2,A1,C1,C}。排除第一个点A2和最后一个点C有两个待处理点A1、C1。首先选取A1点，A1的前一个点A2为起点，A1的后一个点C1为终点，根据农机转向半径，采用dubins曲线算法得到从A2到C1的曲线点集M，比如

{A2,A11,A12,A13,C1}；从M中去除起点和终点，得到集合M′{A11,A12,A13}，用集合M′取代第三规划路径中的A1点，得到更新的第三规划路径

{A2,A11,A12,A13,C1,C}。

接着处理第二规划路径的下一个待处理点C1。在更新的第三规划路径中，C1的前一个点A13为起点，C1的后一个点C为终点，得到从A13到C的符合农机行驶要求的曲线点集M，比如{A13,C11,C12，C}；去除起点和终点，得到集合M′{C11,C12}，用集合M′取代第三规划路径中的C1点，得到更新的第三规划路径

{A2,A11,A12,A13,C11,C12，C}。

第二规划路径中需要处理的点都已处理完毕，所以处理结束，得到符合农机行驶要求的第三规划路径为{A2,A11,A12,A13,C11,C12，C}。

假设第二规划路径为线段集，可以从第二规划路径中取出任意一条折线段，将折线段转换成符合农机行驶要求的曲线段；折线段由两条相邻的线段构成。根据所有曲线段得到第三规划路径。

如图2所示，假设第二规划路径为{A2A1,A1C1,C1C}。A2A1、A1C1构成一条折线段A2C1，A2、C1为该折线段的起点和终点，根据农机转向半径，采用dubins曲线算法得到对应的曲线段；类似的，A1C1、C1C构成另一条折线段A1C，得到A1C折线段对应的曲线段；将这两条曲线段组合得到第三规划路径。第三规划路径为满足农机行驶要求的路径。

本实施例，通过对地块的外接矩形进行路径规划，再对规划出来的路径进行裁剪，再对裁剪后的路径按农机行驶要求进行优化，从而实现不规则地块的路径规划，扩展了农机自动驾驶系统用于耙地的场景，从而提高了耙地效率。

本发明的另一个实施例，如图3所示，一种耙地作业的路径规划方法，与图1所示实施例相比，区别在于步骤S300包括：

步骤S310当一斜率为第一斜率K1的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第二斜率K2作下一路径线段；

步骤S311当一斜率为第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段。

第一斜率K1与第二斜率K2不同。

具体地，可以选择地块的外接矩形上的任意一点为起点，按照第一斜率K1的直线向前移动，当该直线遇到外接矩形的边时，则调头从交点处作第二斜率K2的路径线段，将其作为下一路径线段；若第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段；如此循环，直至无法得到有效的下一路径线段。有效的含义可以根据设计需要确定，比如，若下一路径线段与相邻的平行线段之间的间距小于农具幅宽，则该下一路径线段为无效；若下一路径线段完全在外接矩形外，则该下一路径线段为无效。

如图4所示，假设虚线框代表地块的外接矩形ABCD，以顶点D点为起点进行外接矩形内的路径规划，按照上述规则，按序得到斜率K1、K2交叉出现的一系列路径线段，这些路径线段构成了第一规划路径。可以看出，该规划路径符合对角耙地要求，农机按此规划路径耙地可以提高耙地效果。

本发明的另一个实施例，如图5所示，一种耙地作业的路径规划方法，与图1所示实施例相比，区别在于步骤S300包括：

设置地块的外接矩形的一条边为滑动边，其余边为非滑动边。

步骤S320当一斜率为第一斜率K1的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第二斜率K2作下一路径线段；

步骤S321当一斜率为第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段；

步骤S322当在一交点处无法得到有效的下一路径线段，且该交点位于外接矩形的滑动边时，则沿滑动边滑动预设距离，再按第二斜率或第一斜率作下一路径线段。

具体地，第一斜率K1与第二斜率K2不同。

在有些场景下，根据步骤S320、步骤S321进行路径规划，不能得到符合对角耙地要求的第一规划路径，比如，当与外接矩形的交点为一矩形的顶点时，由于顶点处无法得到有效的下一路径线段，导致路径规划结束。

如图6所示，虚线框代表地块的外接矩形ABCD，以顶点D点为起点进行外接矩形内的路径规划，按照步骤S320、步骤S321进行路径规划，当交点为A点时，由于A点无法得到有效的下一路径线段，路径规划结束。此时得到的路径不符合对角耙地要求。

为了解决这个问题，引入滑动边。若交点在滑动边上可以通过滑动预设距离，尝试得到有效的下一路径线段。若滑动后能得到一有效的下一路径线段，则继续规划；若滑动后仍无法得到有效的下一路径线段，则规划结束；或无法滑动，则规划结束。所谓无法滑动是指在滑动过程中遇到已有的交点。

如图6所示，假设AB为滑动边，当交点为A点时，在滑动边上滑动预设距离到E点，再在E点处按斜率K2或斜率K1(未示出)作下一路径线段，若该路径线段有效，则继续路径规划，如此得到图6所示符合对角耙地要求的第一规划路径。

建议从与起点相对的两条外接矩形边中，选择不与第一条路径线段相交的边作为滑动边。

本发明的一个实施例，如图6所示，一种耙地作业的路径规划装置，包括：

边界获取模块100，用于获取地块的边界数据；

矩形生成模块200，用于根据地块的边界数据得到地块的外接矩形；

第一规划模块300，用于按照预设规则在外接矩形内规划耙地作业路径，得到第一规划路径；

第二规划模块400，用于按照地块边界对第一规划路径进行裁剪，得到第二规划路径；

第三规划模块500，用于根据第二规划路径，得到符合农机行驶要求的第三规划路径。

具体地，可以由人工驾驶农机沿着地块边界行驶一圈，以获得地块的边界数据。

地块的形状可能是不规则的，所以根据地块的边界数据得到的地块边界图形也可能是不规则图形。

通过矩形生成模块计算包含地块边界图形的外接矩形；或，对地块的边界数据进行凸包计算，得到包含该边界数据中所有点的最小的凸多边形，再计算该凸多边形的最小外接矩形，将最小外接矩形作为地块的外接矩形。

外接矩形/最小外接矩形所包含区域是一规则地块，可采用对规则地块的路径规划技术对外接矩形/最小外接矩形区域进行耙地作业路径规划。

在得到第一规划路径后，按照地块边界对第一规划路径进行裁剪，得到第二规划路径。第二规划模块400包括：将第一规划路径中超出地块边界的路径线缩回到地块边界上，将与地块边界无交点的路径线删除，得到第二规划路径。

根据第二规划路径，得到符合农机行驶要求的第三规划路径。假设第二规划路径为按序排列的点集，第三规划模块500可进一步用于：

按照第二规划路径初始化第三规划路径；从第二规划路径中选取除第一个点和最后一个点外的一个待处理点P_x；从第三规划路径中获取与该待处理点P_x对应的点，将其前一个点作为起点，其后一个点作为终点；计算从起点到终点的符合农机行驶要求的曲线点集；用去除起点和终点的曲线点集取代第三规划路径中与该待处理点P_x对应的点；选择第二规划路径中下一个待处理点，从第三规划路径中获取与下一个待处理点对应的点，重复上述过程，直至第二规划路径中除第一个点和最后一个点外的所有点都得到处理。

第三规划模块500还可进一步用于：根据第三规划路径中与待处理点P_x对应的点、起点、终点，得到起点方向和终点方向；根据农机转向半径、起点位置、终点位置、起点方向和终点方向，得到符合农机行驶要求的曲线点集。

可以根据农机转向半径、起点位置、终点位置、起点方向和终点方向，使用dubins曲线算法得到符合农机行驶要求的曲线点集。这样可以得到从折线段的起点到终点的最短路径，有利于农机按规划路径耙地时缩短路程，降低农机的能耗。

本实施例，通过对地块的外接矩形进行路径规划，再对规划出来的路径进行裁剪，再对裁剪后的路径按农机行驶要求进行优化，从而实现不规则地块的路径规划，扩展了农机自动驾驶系统的应用场景，从而提高了耙地效率。

本发明的另一个实施例，如图6所示，一种耙地作业的路径规划装置，与图1所示实施例相比，区别在于：

第一规划模块300，用于当一斜率为第一斜率K1的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第二斜率K2作下一路径线段；当一斜率为第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段。

具体地，第一斜率K1与第二斜率K2不同。可以选择地块的外接矩形上的任意一点为起点，按照第一斜率K1的直线向前移动，当该直线遇到外接矩形的边时，则调头从交点处作第二斜率K2的路径线段，将其作为下一路径线段；若第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段；如此循环，直至无法得到有效的下一路径线段。有效的含义可以根据设计需要确定。

本发明的另一个实施例，如图6所示，一种耙地作业的路径规划装置，与图1所示实施例相比，区别在于：

设置地块的外接矩形的一条边为滑动边，其余边为非滑动边。

第一规划模块300，用于当一斜率为第一斜率K1的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第二斜率K2作下一路径线段；当一斜率为第二斜率K2的路径线段在前进方向与地块的外接矩形的一条边有交点时，则调头从交点处按第一斜率K1作下一路径线段；当在一交点处无法得到有效的下一路径线段，且该交点位于外接矩形的滑动边时，则沿滑动边滑动预设距离，再按第二斜率或第一斜率作下一路径线段。

具体地，第一斜率K1与第二斜率K2不同。

在有些场景下，根据步骤S320、步骤S321进行路径规划，不能得到符合对角耙地要求的第一规划路径，比如，当与外接矩形的交点为一矩形的顶点时，由于顶点处无法得到有效的下一路径线段，导致路径规划结束。

为了解决这个问题，引入滑动边。若交点在滑动边上可以通过滑动预设距离，尝试得到有效的下一路径线段。若滑动后能得到一有效的下一路径线段，则继续规划；若滑动后仍无法得到有效的下一路径线段，则规划结束；或无法滑动，则规划结束。所谓无法滑动是指在滑动过程中遇到已有的交点。

需要说明的是，本发明提供的耙地作业的路径规划装置的实施例与前述提供的耙地作业的路径规划方法的实施例均基于同一发明构思，能够取得相同的技术效果。因而，耙地作业的路径规划装置的实施例的其它具体内容可以参照前述耙地作业的路径规划方法的实施例内容的记载。

本发明还提供一种具体应用场景实施例，将前述的耙地作业的路径规划方法及装置应用于耙地作业的路径规划中。

本实施例要求一台安装有农机自动驾驶系统的农机。农机可以是四轮农机也可以是履带式农机，记该农机的转弯半径为R。农机可以悬挂耙地农具，记耙地农具的幅宽为width。

农机自动驾驶系统，如图8所示，包括GNSS天线60、一个GNSS接收机50、一个用于人机交互的显示终端10、一个用于对转向机构进行控制的控制电机30、一个用于反馈前轮转角的接触式或非接触式角度传感器40。显示终端10内置控制器20。GNSS接收机50可以内置在显示终端10中，也可以外置；图8是一种外置情况。

GNSS天线安装在农机车顶，通过线缆与GNSS接收机连接，用于接收卫星信号并且将信号传输到GNSS接收机中。

GNSS接收机通过线缆与显示终端连接，可以接收多个卫星系统的信号，并且进行高精度定位或定向，同时将定位定向信息传输到显示终端内。

控制电机30安装在农机转向轴上，或以电磁液压阀形式安装在农机的转向助力机构内，同时通过线缆连接到显示终端的控制器上，用于接收控制信号并对农机转向系统进行控制。

显示终端安装在农机驾驶室内，用于人机交互、自动驾驶算法运行、路径规划等功能，通过控制器20对方向盘电机或液压阀(即控制电机30)进行控制。

接触式角度传感器安装在前轮转轴上，或非接触式角度传感器安装在农机底盘上，通过线缆与控制器20连接，用于计算前轮转角值并反馈给控制器20进行闭环控制。

耙地作业的路径规划方案按如下步骤实现：

第一步：人工驾驶农机在需要进行耙地作业的地块周围沿着地块边界行驶一圈，用于采集地块的边界数据。该数据采集完成后，得到一系列点的坐标，作为基本的地块数据用于后续步骤。地块边界图形如图9所示。

第二步：对获得的地块边界的坐标点进行处理。首先去除重复的坐标点，对剩下的坐标点进行凸包计算，获取这些点的最小外接多边形(即包含地块的边界数据的最小的凸多边形)，并计算得到该最小外接多边形的最小外接矩形。最小外接矩形如图10所示。

第三步：设置滑动边和两种斜率。

记矩形四个顶点为A、B、C、D，其中AB与CD之间的距离相等，AB与CD为短边，长度为W；AD与BC之间的距离相等，AD与BC为长边，长度为H。取长边BC的中点记为E，以A为起点，E为终点做线段，斜率记为K1。以E为起点,D为终点做线段，斜率记为K2。将CD设置为滑动边，AB、BC、AD为非滑动边。

滑动边的定义为：当某一路径线段与滑动边有交点时，则将下一个路径点向DC方向滑动一定的距离d，且下一路径线段的斜率为K2。记与滑动边有交点的路径线段与滑动边的夹角记为δ，该距离d的计算方式为：

width为农具幅宽。

若距离d大于

则平行路径线段之间的间距将大于农具幅宽，这样，耙地的间距过大，导致有一些地块被遗漏，未被耙到。若距离d小于

则平行路径线段之间的间距将小于农具幅宽，这样，耙地的间距过小，一些地块被多次耙到，整个耙地时间长，农机比较费油。所以优选将距离d设为等于

则平行路径线段之间的间距将等于农具幅宽，这样不会出现地块遗漏，又控制了耙地时长。

非滑动边的定义为：当某一路径线段与非滑动边有交点时，如果该线段的斜率为K1，则下一路径线段的斜率需为K2。如果该线段的斜率为K2，则下一路径线段的斜率需为K1。

第四步：根据对角耙地作业的要求，作业路径主要由上述斜率为K1和K2两种斜率的线段构成，以A为起点进行路径规划，规划方法如下：以A为起点，做斜率为K1的直线，该直线与BC交于一点PP2，则第二个路径点为PP2。过PP2做斜率为K2的直线，该直线与CD交于一点PP3，则第三个路径点为PP3。由于CD为滑动边，根据上述设定，将PP3沿DC方向滑动距离d，得到第四个路径点PP4。过PP4做斜率为K2的直线，该直线与BC交于一点PP5，则第五个路径点为PP5，如图11。依上述描述执行下去，直到当某一路径线段与滑动边有交点且该交点到的距离小于滑动距离d时，规划任务停止，得到路径点的集合，即得到最小外接矩形内的第一规划路径，如图12所示。

第五步：根据地块轮廓(即地块边界)对路径线进行裁剪，将超出地块边界的路径线缩回到地块边界上，将与地块轮廓无交点的路径线删除，得到裁剪后的路径点的集合，记为P_{x,y}，即得到第二规划路径，如图13所示。

第六步：遍历路径点的集合P_{x,y}，取其中除第一个和最后一个点外的任意一个点P_x，该点的前一个路径点为P_x-1，后一个路径点为P_x+1，记P_x-1到P_x的方位角为

记P_x到P_x+1的方位角为

根据P_x-1、

P_x+1、

农机转向半径R使用dubins曲线算法，计算得到农机从P_x-1行走到P_x+1，同时车身朝向从

转向到

的理想路径点集合，记为M_{x,y}，将P_x从集合P_{x,y}中移除，同时将集合M_{x,y}插入到集合P_{x,y}的P_x-1与P_x+1之间。对集合P_{x,y}中除第一个和最后一个点外的每一个点执行以上步骤，最终得到集合P′_{x,y}，记为规划完成的耙地路径，为方便用户寻找入口，将集合中的第一个点反向延长20米，如图14所示。

dubins曲线是在满足曲率约束和规定的起点和终点的切线方向的条件下，连接两个二维平面的最短路径，而且限制目标只能向前行进。由于采用dubins曲线替换原始规划路径，所以该耙地路径不仅是满足农机行驶要求的第三规划路径，还为满足耙地作业要求的最短路径。

第七步：农机自动驾驶系统根据第三规划路径控制农机进行作业，即可实现自动耙地作业。

本实施例，提出一种用于耙地路径的规划方法，该方法可以对任意地形的地块进行耙地路径的规划，极大的丰富了农机自动驾驶系统用于耙地作业的场景，可以满足绝大多数用户对耙地作业的要求。本方法在进行路径规划的过程中充分考虑用户对节能降耗的要求，规划出来的路径为满足耙地作业要求的最短路径，更加节能环保。同时由于使用数据方法进行路径规划，不存在不确定性因素与非线性因素，所以本方法能在极短的时间内完成大面积地块的路径规划。

应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种耙地作业的路径规划方法，其特征在于，包括：

获取地块的边界数据；

根据所述地块的边界数据得到所述地块的外接矩形；

按照预设规则在所述外接矩形内规划耙地作业路径，得到第一规划路径；

按照地块边界对所述第一规划路径进行裁剪，得到第二规划路径；

根据所述第二规划路径，得到符合农机行驶要求的第三规划路径。

2.根据权利要求1所述的路径规划方法，其特征在于，所述的根据所述地块的边界数据得到所述地块的外接矩形包括：

对所述地块的边界数据进行凸包计算，得到包含所述边界数据中所有点的最小的凸多边形；

计算所述凸多边形的最小外接矩形，将所述最小外接矩形作为所述地块的外接矩形。

3.根据权利要求1、2任一项所述的路径规划方法，其特征在于，所述按照预设规则在所述外接矩形内规划耙地作业路径包括：

当一斜率为第一斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第二斜率作下一路径线段；

当一斜率为第二斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第一斜率作下一路径线段；

所述第一斜率与所述第二斜率不同。

4.根据权利要求3所述的路径规划方法，其特征在于：

设置所述地块的外接矩形的一条边为滑动边，其余边为非滑动边；

所述按照预设规则在所述外接矩形内规划耙地作业路径还包括：

当在一交点处无法得到有效的下一路径线段，且该交点位于所述外接矩形的滑动边时，则沿所述滑动边滑动预设距离，再按第二斜率或第一斜率作下一路径线段。

5.根据权利要求1所述的路径规划方法，其特征在于，所述的按照所述地块边界对所述第一规划路径进行裁剪，得到第二规划路径，包括：

将所述第一规划路径中超出地块边界的路径线缩回到所述地块边界上，将与所述地块边界无交点的路径线删除，得到第二规划路径。

6.根据权利要求1所述的路径规划方法，其特征在于，所述的根据所述第二规划路径，得到符合农机行驶要求的第三规划路径，包括：

所述第二规划路径为点集；

按照所述第二规划路径初始化第三规划路径；

从所述第二规划路径中选取除第一个点和最后一个点外的一个待处理点；

从所述第三规划路径中获取与所述待处理点对应的点，将其前一个点作为起点，后一个点作为终点；

计算从所述起点到所述终点的符合农机行驶要求的曲线点集；

用去除所述起点和所述终点的曲线点集取代所述第三规划路径中与所述待处理点对应的点；

选择所述第二规划路径中下一个待处理点，从所述第三规划路径中获取与所述下一个待处理点对应的点，重复上述过程，直至所述第二规划路径中除第一个点和最后一个点外的所有点都得到处理。

7.根据权利要求6所述的路径规划方法，其特征在于，计算从所述起点到所述终点的符合农机行驶要求的曲线点集，包括：

根据所述第三规划路径中与所述待处理点对应的点、所述起点、所述终点，得到起点方向和终点方向；

根据农机转向半径、起点位置、终点位置、所述起点方向和所述终点方向，使用dubins曲线算法得到符合农机行驶要求的曲线点集。

8.一种耙地作业的路径规划装置，其特征在于，包括：

边界获取模块，用于获取地块的边界数据；

矩形生成模块，用于根据所述地块的边界数据得到所述地块的外接矩形；

第一规划模块，用于按照预设规则在所述外接矩形内规划耙地作业路径，得到第一规划路径；

第二规划模块，用于按照地块边界对所述第一规划路径进行裁剪，得到第二规划路径；

第三规划模块，用于根据所述第二规划路径，得到符合农机行驶要求的第三规划路径。

9.根据权利要求8所述的路径规划装置，其特征在于：

所述矩形生成模块，还用于对所述地块的边界数据进行凸包计算，得到包含所述边界数据中所有点的最小的凸多边形；计算所述凸多边形的最小外接矩形，将所述最小外接矩形作为所述地块的外接矩形。

10.根据权利要求8所述的路径规划装置，其特征在于：

所述第一规划模块，还用于当一斜率为第一斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第二斜率作下一路径线段；当一斜率为第二斜率的路径线段在前进方向与所述地块的外接矩形的一条边有交点时，则调头从所述交点处按第一斜率作下一路径线段；所述第一斜率与所述第二斜率不同。

Images