CN116880497A - 一种自动化农业机械的全覆盖路径规划方法、装置和设备 - Google Patents
一种自动化农业机械的全覆盖路径规划方法、装置和设备 Download PDFInfo
- Publication number
- CN116880497A CN116880497A CN202310957836.XA CN202310957836A CN116880497A CN 116880497 A CN116880497 A CN 116880497A CN 202310957836 A CN202310957836 A CN 202310957836A CN 116880497 A CN116880497 A CN 116880497A
- Authority
- CN
- China
- Prior art keywords
- boundary
- agricultural machine
- parallel
- parallel line
- area
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 93
- 238000005457 optimization Methods 0.000 claims abstract description 14
- 238000012545 processing Methods 0.000 claims abstract description 10
- 230000006870 function Effects 0.000 claims description 105
- 230000008569 process Effects 0.000 claims description 28
- 238000009499 grossing Methods 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 7
- 241000270295 Serpentes Species 0.000 claims description 5
- 230000011218 segmentation Effects 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 4
- 238000010845 search algorithm Methods 0.000 claims description 4
- 230000004927 fusion Effects 0.000 claims description 2
- 238000004519 manufacturing process Methods 0.000 claims 1
- 239000004065 semiconductor Substances 0.000 claims 1
- 238000004364 calculation method Methods 0.000 description 14
- 238000010586 diagram Methods 0.000 description 8
- WYTGDNHDOZPMIW-RCBQFDQVSA-N alstonine Natural products C1=CC2=C3C=CC=CC3=NC2=C2N1C[C@H]1[C@H](C)OC=C(C(=O)OC)[C@H]1C2 WYTGDNHDOZPMIW-RCBQFDQVSA-N 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000013507 mapping Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- KUVIULQEHSCUHY-XYWKZLDCSA-N Beclometasone Chemical compound C1CC2=CC(=O)C=C[C@]2(C)[C@]2(Cl)[C@@H]1[C@@H]1C[C@H](C)[C@@](C(=O)COC(=O)CC)(OC(=O)CC)[C@@]1(C)C[C@@H]2O KUVIULQEHSCUHY-XYWKZLDCSA-N 0.000 description 2
- 210000004027 cell Anatomy 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000007667 floating Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 210000001316 polygonal cell Anatomy 0.000 description 2
- 238000012163 sequencing technique Methods 0.000 description 2
- 101150008563 spir gene Proteins 0.000 description 2
- 241000163925 Bembidion minimum Species 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种自动化农业机械的全覆盖路径规划方法、装置和设备,涉及自动化农机的路径规划技术领域。其中,这种全覆盖路径规划方法包含S1获取全局优化成本目标函数。S2获取自动化农业机械的模型,及其工作区域的地图数据。S3根据地图数据,对边界进行凸子区域化的处理,分别根据处理后的凸子区域,基于生成Headlands边界的代价函数生成Headlands边界。S4根据Headlands边界,基于自动化农业机械的模型和平行线束划分角度选取的代价函数进行平行线束划分,获取凸子区域内的平行线集合。S5根据平行线集合,进行全覆盖路径规划,获取平行线束集合的遍历顺序。S6根据遍历顺序,基于路径长度的代价函数,通过利用连续曲率的Dubins曲线对平行线进行连接,获取规划路径。
Description
技术领域
本发明涉及自动化农机的路径规划技术领域,具体而言,涉及一种自动化农业机械的全覆盖路径规划方法、装置和设备。
背景技术
无人驾驶技术,是多门前沿学科的综合体,在近些年以来在多方面不断取得了新的突破,并应用在各个行业领域中。尤其在乘用车领域,无人驾驶技术取得了高速发展并受到越来越广泛的关注,并实现了一定程度的成熟落地。
在农业机械自动作业领域,无人驾驶技术却处于一个前期发展阶段。履带式旋耕机是小型农业机械中的代表产品,多应用于小型农田耕种,特殊农田场景作业等方面。但是,农业机械的自动作业技术的落地面临着农田场景复杂,农机自动化成本高等问题。
在自动作业领域主要分为场景定位,轨迹规划,轨迹跟踪,作业实施等环节。其中,传统的轨迹规划无法适用于自动化农业机械。
有鉴于此,申请人在研究了现有的技术后特提出本申请。
发明内容
本发明提供了一种自动化农业机械的全覆盖路径规划方法、装置和设备,以改善上述技术问题中的至少一个。
第一方面、
本发明实施例提供了一种自动化农业机械的全覆盖路径规划方法,其包含:
S1、获取全局优化成本目标函数。其中,全局优化成本目标函数包括生成Headlands边界的代价函数、平行线束划分角度选取的代价函数,以及路径长度的代价函数。
S2、获取自动化农业机械的模型,及其工作区域的地图数据。
S3、根据地图数据,对边界进行凸子区域化的处理,分别根据处理后的凸子区域,基于生成Headlands边界的代价函数生成Headlands边界。
S4、根据Headlands边界,基于自动化农业机械的模型和平行线束划分角度选取的代价函数对边界包围的区域进行平行线束划分,或者根据用户定义的平行线角度进行平行线束划分,获取凸子区域内的平行线集合。
S5、根据平行线集合,进行全覆盖路径规划,获取平行线束集合的遍历顺序。其中,遍历顺序包括每条平行线的进入点和离开点。
S6、根据平行线集合、遍历顺序和自动化农业机械的模型,基于路径长度的代价函数,通过利用连续曲率的Dubins曲线对平行线进行连接,获取规划路径。
第二方面、
本发明实施例提供了一种自动化农业机械的全覆盖路径规划装置,其包含:
目标函数获取模块,用于获取全局优化成本目标函数。其中,全局优化成本目标函数包括生成Headlands边界的代价函数、平行线束划分角度选取的代价函数,以及路径长度的代价函数。
初始数据获取模块,用于获取自动化农业机械的模型,及其工作区域的地图数据。
边界生成模块,用于根据地图数据,对边界进行凸子区域化的处理,分别根据处理后的凸子区域,基于生成Headlands边界的代价函数生成Headlands边界。
平行线划分模块,用于根据Headlands边界,基于自动化农业机械的模型和平行线束划分角度选取的代价函数对边界包围的区域进行平行线束划分,或者根据用户定义的平行线角度进行平行线束划分,获取凸子区域内的平行线集合。
路径规划模块,用于根据平行线集合,进行全覆盖路径规划,获取平行线束集合的遍历顺序。其中,遍历顺序包括每条平行线的进入点和离开点。
平行线连接模块,用于根据平行线集合、遍历顺序和自动化农业机械的模型,基于路径长度的代价函数,通过利用连续曲率的Dubins曲线对平行线进行连接,获取规划路径。
第三方面、
本发明实施例提供了一种自动化农业机械的全覆盖路径规划设备,其包括处理器、存储器,以及存储在存储器内的计算机程序。计算机程序能够被处理器执行,以实现如第一方面任意一段所说的一种自动化农业机械的全覆盖路径规划方法。
通过采用上述技术方案,本发明可以取得以下技术效果:
本发明实施例能够为自动化农业机械规划出完全覆盖其工作区域的路径。为农业机械的自动化提供了基础,具有很好的实际意义。
附图说明
为了更清楚地说明本发明实施例的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本发明的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他相关的附图。
图1是全覆盖路径规划方法的流程示意图。
图2是平滑处理后的自动化农业机械的工作区域的地图示意图。
图3是生成Headlands边界的地图示意图。
图4是平行线划分后的的地图示意图。
图5是连续曲率的Dubins曲线的六种曲线示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述。显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
实施例一、
请参阅图1至图5,本发明第一实施例提供一种自动化农业机械的全覆盖路径规划方法,其可由自动化农业机械的全覆盖路径规划设备来执行(以下简称:路径规划设备)。特别地,由路径规划设备中的一个或多个处理器来执行,以实现步骤S1至步骤S6。
可以理解的是,所述路径规划设备可以是便携笔记本计算机、台式机计算机、服务器、智能手机或者平板电脑等具有计算性能的电子设备。优选的,路径规划设备为自动化农业机械的车机。
S1、获取全局优化成本目标函数。其中,所述全局优化成本目标函数包括生成Headlands边界的代价函数、平行线束划分角度选取的代价函数,以及路径长度的代价函数。
S2、获取自动化农业机械的模型,及其工作区域的地图数据。
具体的,自动化农业机械的模型包括自动化农业机械的尺寸、工作宽度、移动速度,以及最小转弯半径等参数,本发明对自动化农业机械的模型不做具体限定,只要和自动化农业机械本身相关的参数,均属于模型的内容。
优选的,根据自动化农业机械的模型,设计自动化农业机械转弯时的转弯半径和曲率线速度映射函数参数,使自动化农业机械在转弯时线速度能够根据曲率动态调整。其中,映射关系主要是基于当前点曲率的线速度的映射限幅和计算。输入参数:当前规划点的曲率值、最小曲率值/>、最大线速度/>、最小线速度,以及当前转弯半径/>。映射关系如下:
1、根据当前的转弯半径计算值,具体公式为:/>。表示转弯半径越大,曲率系数越小。
2、比较和/>的曲率值大小。如果/>小于则直接将线速度设置为最大的速度。如果/>大于/>则根据以下公式计算当前的线速度:
3、限制速度下限,具体公式为:
由此得到线速度随曲率的动态调整。
S3、根据所述地图数据,对边界进行凸子区域化的处理,分别根据处理后的凸子区域,基于生成Headlands边界的代价函数生成Headlands边界。
S4、根据Headlands边界,基于所述自动化农业机械的模型和平行线束划分角度选取的代价函数对边界包围的区域进行平行线束划分,或者根据用户定义的平行线角度进行平行线束划分,获取凸子区域内的平行线集合。
S5、根据所述平行线集合,进行全覆盖路径规划,获取平行线束集合的遍历顺序。其中,所述遍历顺序包括每条平行线的进入点和离开点。
S6、根据所述平行线集合、所述遍历顺序和自动化农业机械的模型,基于路径长度的代价函数,通过利用连续曲率的Dubins曲线对平行线进行连接,获取规划路径。
本发明实施例很好的利用了农田的规整化分布,以及小型农机的转向灵活的特性。首先,从农业机械的工作环境中获取规划控制所需的地图边界,障碍区域,被访问区域等信息。然后,采用了更具有效率性的平行线束划分方法以及复杂地形的凸子区域化,解决了复杂边界农田的耕地路径遍历效率问题,更好地对目标作业区域进行划分规划。最后,利用启发式搜索算法进行最优路径搜素,并通过连续曲率的Dubins曲线对平行线进行连接,从而得到农业机械的规划路径。为自动化高效作业提供了必备条件,具有很好的实际意义。
在上述实施例的基础上,本发明的一个可选地实施例中,生成Headlands边界的代价函数为:
式中,为生成Headlands边界后剩余的面积、/>为原始面积。
具体的,该函数主要用于评估生成Headlands边界后剩余的面积和原始面积的比值。
在上述实施例的基础上,本发明的一个可选地实施例中,平行线束划分角度选取的代价函数包括场覆盖最大化函数、线束数量最小化约束函数、场重叠最大化函数和线束路径长度最小化函数。其中,
场覆盖最大化函数为:
式中,为凸子区域之间相交部分的面积之和、/>为工作区域/>的总面积。
具体的,计算线束覆盖的字段的百分比。成本是一个介于[0,1]之间的值,它被定义为一个最大化问题。其中,对于一个大区域,设其与多个小区域,相交部分的面积之和为/>,/>为大区域/>的总面积。表示所有小区域和大区域的重叠部分之和,该指标可以计算大区域被所有小区域的覆盖程度。
线束数量最小化约束函数为:
式中,是线束数量、/>是Headlands内边界的包围区域、/>是自动化农业机械的作业宽度。
具体的,线束数量最小化约束函数取决于场地的形状和面积以及自动化农业机械的作业宽度。线束的数量受以下条件制约:
式中,是给定角度/>扫描的线束数量,/>是Headlands内边界的包围区域,/>是机器人的作业宽度。
根据该条件约束得到线束数量最小化约束函数。
场重叠最大化函数为:
式中,为多边形区域/>的面积、/>为多边形区域/>之外的多边形区域/>的面积、为多边形区域/>和多边形区域/>之间重叠的面积。
具体的,场重叠函数用于计算一个多边形区域和另外若干个多边形区域之间的重叠部分面积。举例如下:
设A为多边形,B为多个多边形区域,O为它们的重叠部分,则有:
式中,、/>、/>分别为多边形A、B、O各自的顶点数、(/>)为多边形A的第/>个顶点的坐标,/>为通过格林公式计算重叠部分的面积/>。
即:,从而得到场重叠最大化函数/>。
线束路径长度最小化函数为:,式中,/>是线束的数量、/>为第/>条线束的起点位置、/>为第/>条线束的终点位置、/>是第/>条线束欧几里得范数。
具体的,线束路径长度可以由以下的公式计算:
式中,为所有线束的长度求和、/>是线束的数量、/>是第/>条平行边界上的节点数、/>是第/>条平行边界的第/>个节点、/>是节点/>和节点/>的欧几里得范数。根据线束路径长度的计算公式可得到线束路径长度最小化函数。
在上述实施例的基础上,本发明的一个可选地实施例中,路径长度的代价函数为:
式中,为线束总数、/>为第/>条线束的Dubins曲线的曲线长度、/>为第/>条线束的起点位置、/>为第/>条线束的终点位置、/>是第/>条线束欧几里得范数。
具体的,路径计算由平行线束路径和转弯路径组成。平行线束路径计算较为简单,仅需要计算每条平行边界路径起始点和路径终点的欧几里得范数。转弯路径需要将Dubins曲线生成的路径进分段计算。
在上述实施例的基础上,本发明的一个可选地实施例中,全局优化成本目标函数还包含转弯距离的代价函数。优选的,自动化农业机械为履带式旋耕机。
在本实施例中,将履带式机器人的运动模型抽象简化为两轮差速驱动机器人模型,然后考虑设计代价函数如下:
式中,表示转弯半径,/>表示转向角度,/>,/>,/>是调整因子。
该代价函数的含义是:对于给定的转向角度和车辆尺寸,计算履带式机器人的转弯半径,并将其与期望转弯半径进行比较。如果实际转弯半径与期望转弯半径之差较大,即机器人旋转的曲线比较绕或转弯的半径过大,则代价函数会变大,表示该转弯方式不太适合履带式机器人;反之,如果实际转弯半径与期望转弯半径相差较小,则代价函数会变小,表示该转弯方式比较适合履带式机器人。
在上述实施例的基础上,本发明的一个可选地实施例中,
所述自动化农业机械的模型包括自动化农业机械的尺寸模型和运动学模型。
所述自动化农业机械的地图数据为栅格化地图、卫星定位地图、感知传感器生成地图,或者卫星定位地图和感知传感器生成地图的融合信息地图。
在上述实施例的基础上,本发明的一个可选地实施例中,步骤S2具体包括S21至步骤S23。
S21、获取自动化农业机械的模型。
S22、获取自动化农业机械的工作区域的地图数据,并判断所述地图数据是否为栅格化地图。
S23、当判断到所述地图数据不是栅格化地图时,对地图数据的边缘进行提取以及平滑处理。其中,平滑处理通过滤波或B-Spline曲线拟合方式进行。
具体的,地图数据不是栅格化地图时,需要对工作区域的边缘进行提取和处理。通过B-Spline曲线拟合进行平滑处理,包括两部分:参数化和B-Spline基函数。
首先、进行参数化,将数据点按照一定规则映射到参数空间中。常用的方法有等距参数化和Chord长度参数化。以等距参数化为例,假设我们有个数据点/>,则可以按照如下方式进行参数化:
式中,表示第i个数据点在参数空间中对应的参数值。
接下来、是B-Spline基函数的计算。B-Spline基函数是一组递归定义的函数,具体形式如下:
式中,表示节点序列,/>表示B-Spline的阶数。当/>时,只有一个非零的基函数。当/>时,基函数是线性的,当/>时,基函数是二次的,以此类推。
最终,B-Spline曲线的表达式可以表示为:
式中,表示数据点/>的坐标,n表示数据点的个数
用以上的方法对工作边界进行B-Spline平滑处理和滤波处理可以有效提取来自原始地图的工作区域边界
在上述实施例的基础上,本发明的一个可选地实施例中,步骤S3具体包括步骤S31至步骤S33。
S31、根据所述地图数据,获取工作区域的边界信息,并判断所述边界信息中是否包含凹边界。
S32、当判断到所述边界信息中包含凹边界时,根据所述边界信息的凹点进行边界分割,获取多个凸子区域。否则,直接以边界信息作为凸子区域。
S33、分别根据多个凸子区域,基于生成Headlands边界的扩张系数和代价函数,生成Headlands边界。其中,扩张系数为自动化农业机械的宽度的倍数。
具体的,Headlands边界的生成过程通过以下步骤实现:
1、如果边界较为复杂(凹边界)则执行根据凹点的边界分割,即凸子区域化,得到多个共享边界的凸多边形子区域,再分别对各个凸子区域执行后续操作,得到多个多边形环对象;
2、接收输入参数,即经过平滑处理的边界、预留出的Headlands边界扩张系数w(w的选择和机器人宽度有关,本实例中设置为3倍的机器人宽度)以及生成Headlands边界的代价函数;
3、将多边形的每个环(顶点序列)转换成一条线段,将线段按照顶点划分成若干小段;
4、遍历每个小线段,使每个小线段按照法向量的方向向外扩张w,生成一个新的几何表示cell,此时为扩张后的多边形cell;
5、将生成的多边形cell与原场地表示相减,得到Headlands边界,该边界为多边形环。
6、根据生成Headlands边界的代价函数对生成的Headlands进行分数计算。
在上述实施例的基础上,本发明的一个可选地实施例中,步骤S4具体包括步骤S41至步骤S44。
S41、根据Headlands边界,获取平行线的边界。
S42、根据所述自动化农业机械的模型,获取自动化农业机械的作业宽度。
S43、根据所述作业宽度和所述平行线的边界,生成不同平行线生成角度的平行线集合。
S44、根据不同平行线生成角度的平行线集合,选取平行线束划分角度选取的代价函数值最小的平行线集合作为最终确定的平行线集合。
可以理解的是,平行线束划分的实现过程可以概括为:采用暴力求解的方法尝试所有的角度生成平行线,再根据目标函数成本最小化来选择最合适的平行线生成角度。此外,用户还可以自定义平行线角度来生成平行线束划分。
平行线束划分的实现过程,具体通过以下步骤实现:
1、接收输入参数,即生成的Headlands边界、机器人的操作范围(主要是针对于工作宽度)、平行线束划分角度选取的代价函数。
2、遍历每一个多边形环对象,并对每个多边形环执行以下操作:
a、数据转换:将多边形环转换成线段列表,每个线段的长度为行宽,并根据需要将两端末尾点与原始几何体相贴合或保持缝隙,生成一系列新的线段。注意,如果这一步中产生的线段与原始几何体有交叉或重叠,则要进行剪裁或拆分,保证线段不会跨越限制。
b、最小成本角度平行线束划分:利用遍历的方式分别通过每个划分角对多边形内环进行平行线束划分。
将多边形按照最小成本角度逆时针旋转,生成一个旋转后的多边形变量rotpoly。根据旋转后多边形的宽度和机器人操作宽度,计算出需要生成的平行线束数量n,具体来说将旋转后的多边形rot poly的宽度除以操作宽度得到一个浮点数值,再向上取整数。
从旋转后的多边形中初始化种子曲线seed curve,一个自由空间轮廓中的水平直线段,用于生成平行线束,具体以rot poly左下角的位置(rot_poly.getDimMinX(), rot_poly.getDimMinY())为起点,方向是竖直方向。
迭代生成剩下n-1个平行线束,具体来说,对于第i个迭代(从0开始),首先计算从种子曲线向右移动一个距离为(i + 0.5) * op width的点(op width为机器人操作宽度),并将其绕种子曲线上的某个点逆时针旋转-度,得到曲线端点path,再将生成的平行线束添加到路径集合中完成整个划分过程。
在上述实施例的基础上,本发明的一个可选地实施例中,步骤S5具体用于:根据所述平行线集合,通过Boustrophedon元启发式搜索算法、Snake非启发式搜索方法,或者Spiral非启发式搜索方法进行全覆盖路径规划,获取平行线束集合的遍历顺序。其中,所述遍历顺序包括每条平行线的进入点和离开点。
Boustrophedon规划的具体步骤为:
1、状态点定义:将地图划分为有限个状态点(这里仅用平行线束的起点和终点作为状态点,具体更多状态点可以用户自定义)。
2、状态选择:在地图中选择一个合适的起始状态,本实例中起始状态点均为多边形左下角的顶点,在当前状态下选择相邻且没有访问过的状态,作为下一步的目标状态,并根据两状态间的距离和预估的到达目标状态所需的时间和能量计算行进代价(Cost)。依次遍历所有相邻状态,选择Cost最小的状态作为下一步的目标状态,进入下一个状态后,以该状态为原点,建立正向或反向(取决于当前状态)的Boustrophedon路径,直到遇到边界或障碍物。当不能继续前进时,回到起始点并构建另外一条路径,重复此过程,直到遍历了所有状态点完成整个规划过程。
Snake规划的具体步骤为:
1、蛇形排序:将待规划的平行线束进行循环排序,循环的初始值是1,从路径数组的第二个元素开始对数组进行循环操作。循环的结束条件是数组中心位置。在循环内部,将位于当前索引位置i和i+1之间的元素进行旋转,实现蛇形顺序排序。
2、蛇形排序转弯:将数组中从i+1个元素到最后一个元素之间的元素进行反转,以实现“蛇”的拐弯。最后,如果数组长度为奇数,则再次将位于i和i+1位置之间的元素进行旋转,以完成蛇形顺序排序的最后一步。
Spiral规划的具体步骤为:
1、规划排序区域划分:创建Spiral规划划分标准sp_size,将待规划的平行线束(以下用swath代替)分成大小为sp_size的部分,特别的sp_size大小应该至少大于2。
2、螺旋排序:对子swath执行螺旋排序,具体过程为,定义初始参数offset和size,分别表示要排序的子数组在原数组中的起始位置和长度,在本例中offset为sp_size和子swath索引i的乘积,size为sp_size,将位于swath_0 + offset + size - 1位置的元素移到当前循环索引位置swath_0 + offset + i处,其中swath_0为子swath的初始位置。因此,经过每个循环迭代,子swath中的最后一个元素都会被移动到当前索引位置,直到整个子数组被完全排序。
(3)拼接完整路径:将排序后的子swath按照原顺序拼接成完整的路径,Spiral规划过程完成。
在上述实施例的基础上,本发明的一个可选地实施例中,S6具体包括步骤S61至步骤S63。
S61、根据所述自动化农业机械的模型,获取自动化农业机械的最小转弯半径。
S62、根据所述平行线集合和所述遍历顺序,获取相邻两条平行线之间的待连接离开点和待连接进入点。
S63、根据所述最小转弯半径,通过连续曲率的Dubins曲线平滑连接所述相邻两条平行线之间的待连接离开点和待连接进入点,获取规划路径。
可以理解的是,对所述复杂边界进行凸子区域化的处理过程可以概括为:对外部多边形向内缩小一定距离并形成割裂线,将外部复杂凹图形分割成多个带孔的凸多边形。生成Headlands边界过程可以概括为:对原场地中的边界沿法向量方向向外平移,得到稍大于原场地边界的几何表示,再将扩张后的几何表示减去原场地的几何表示得到田垄部分的几何表示。
采用连续曲率的Dubins曲线对平行线束进行平滑连接的具体步骤如下:
1、构建连续曲率的Dubins路径:连续曲率的Dubins路径(后面统称CC-Dubins)是一种特定的Dubins曲线模型,其由一条直线段、两个圆弧线段组成。循环选择相邻的状态点构建CC-Dubins曲线,根据CC-Dubins曲线类型计算Dubins曲线的长度,连续曲率Dubins曲线是在平面上连接两个点的最短路径,而且限制目标只能向前行进。它由基本的LSL、LSR、RSL、RSR、RLR和LRL六种曲线中的一种组成。
假设起点和终点分别为和/>,朝向方向分别为/>和/>,两点之间的欧氏距离为D,则Dubins曲线的计算公式如下:
LSL路径:
路径形状:左转,直行,左转
计算公式:
RSR路径:
路径形状:右转,直行,右转
计算公式:
RSL路径:
路径形状:右转,直行,左转
计算公式:
LSR路径:
路径形状:左转,直行,右转
计算公式:
RLR路径:
路径形状:右转,左转,右转
计算公式:
LRL路径:
路径形状:左转,右转,左转
计算公式:
式中,、/>分别为起点和终点,r为转弯半径,d是两点之间的距离,/>是取模函数,atan2是反正切函数。
2、Dubins曲线平滑路径计算和路径长度的代价评估:Dubins曲线长度可由、/>、/>的分别计算再加和得到。再将计算的结果带入路径长度的代价函数得到代价分数。选择代价分数最低,即总路径最短的方案作为待实施的方案,用以连接平行线束,从而获取规路径规划。
本发明实施例的各个步骤能够在计算机模型中的不同模块进行处理,仅存在数据上的耦合,不存在方法上的耦合,具有处理结构化的特点。本发明实施例涉及整体自动作业方案中轨迹规划的部分,可以无缝配合定位建图部分、轨迹跟踪和自动作业部分完成农机的自动化作业。本发明实施例的全覆盖路径规划方法能够适配客制化的农机。只需要调整自动化农业机械的模型的参数,即可实现为不同的农机进行路径规划。能够有效的实现不同作业场景和功能下的全覆盖路径规划。
实施例二、
本发明实施例提供了一种自动化农业机械的全覆盖路径规划装置,其包含:
目标函数获取模块,用于获取全局优化成本目标函数。其中,所述全局优化成本目标函数包括生成Headlands边界的代价函数、平行线束划分角度选取的代价函数,以及路径长度的代价函数。
初始数据获取模块,用于获取自动化农业机械的模型,及其工作区域的地图数据。
边界生成模块,用于根据所述地图数据,对边界进行凸子区域化的处理,分别根据处理后的凸子区域,基于生成Headlands边界的代价函数生成Headlands边界。
平行线划分模块,用于根据Headlands边界,基于所述自动化农业机械的模型和平行线束划分角度选取的代价函数对边界包围的区域进行平行线束划分,或者根据用户定义的平行线角度进行平行线束划分,获取凸子区域内的平行线集合。
路径规划模块,用于根据所述平行线集合,进行全覆盖路径规划,获取平行线束集合的遍历顺序。其中,所述遍历顺序包括每条平行线的进入点和离开点。
平行线连接模块,用于根据所述平行线集合、所述遍历顺序和自动化农业机械的模型,基于路径长度的代价函数,通过利用连续曲率的Dubins曲线对平行线进行连接,获取规划路径。
在上述实施例的基础上,本发明的一个可选地实施例中,初始数据获取模块具体包括:
模型获取单元,用于获取自动化农业机械的模型。
地图判断单元,用于获取自动化农业机械的工作区域的地图数据,并判断所述地图数据是否为栅格化地图。
栅格化单元,用于当判断到所述地图数据不是栅格化地图时,对地图数据的边缘进行提取以及平滑处理。其中,平滑处理通过滤波或B-Spline曲线拟合方式进行。
在上述实施例的基础上,本发明的一个可选地实施例中,边界生成模块具体包括:
凹边界判断单元,用于根据所述地图数据,获取工作区域的边界信息,并判断所述边界信息中是否包含凹边界。
边界分割单元,用于当判断到所述边界信息中包含凹边界时,根据所述边界信息的凹点进行边界分割,获取多个凸子区域。否则,直接以边界信息作为凸子区域。
边界生成单元,用于分别根据多个凸子区域,基于生成Headlands边界的扩张系数和代价函数,生成Headlands边界。其中,扩张系数为自动化农业机械的宽度的倍数。
在上述实施例的基础上,本发明的一个可选地实施例中,平行线划分模块具体包括:
边界获取单元,用于根据Headlands边界,获取平行线的边界。
作业宽度获取单元,用于根据所述自动化农业机械的模型,获取自动化农业机械的作业宽度。
平行线遍历单元,用于根据所述作业宽度和所述平行线的边界,生成不同平行线生成角度的平行线集合。
平行线选取单元,用于根据不同平行线生成角度的平行线集合,选取平行线束划分角度选取的代价函数值最小的平行线集合作为最终确定的平行线集合。
在上述实施例的基础上,本发明的一个可选地实施例中,路径规划模块具体用于:根据所述平行线集合,通过Boustrophedon元启发式搜索算法、Snake非启发式搜索方法,或者Spiral非启发式搜索方法进行全覆盖路径规划,获取平行线束集合的遍历顺序。其中,所述遍历顺序包括每条平行线的进入点和离开点。
在上述实施例的基础上,本发明的一个可选地实施例中,平行线连接模块具体包括:
最小转弯半径获取单元,用于根据所述自动化农业机械的模型,获取自动化农业机械的最小转弯半径。
待连接点获取单元,用于根据所述平行线集合和所述遍历顺序,获取相邻两条平行线之间的待连接离开点和待连接进入点。
平滑连接单元,用于根据所述最小转弯半径,通过连续曲率的Dubins曲线平滑连接所述相邻两条平行线之间的待连接离开点和待连接进入点,获取规划路径。
实施例三、
本发明实施例提供了一种自动化农业机械的全覆盖路径规划设备,其包括处理器、存储器,以及存储在所述存储器内的计算机程序。所述计算机程序能够被所述处理器执行,以实现如实施例一任意一段所说的一种自动化农业机械的全覆盖路径规划方法。
在本发明实施例所提供的几个实施例中,应该理解到,所揭露的装置和方法,也可以通过其它的方式实现。以上所描述的装置和方法实施例仅仅是示意性的,例如,附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分,所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意,在有些作为替换的实现方式中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个连续的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。也要注意的是,框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合,可以用执行规定的功能或动作的专用的基于硬件的系统来实现,或者可以用专用硬件与计算机指令的组合来实现。
另外,在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分,也可以是各个模块单独存在,也可以两个或两个以上模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,电子设备,或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。需要说明的是,在本文中,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
在本发明实施例中使用的术语是仅仅出于描述特定实施例的目的,而非旨在限制本发明。在本发明实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式,除非上下文清楚地表示其他含义。
应当理解,本文中使用的术语“和/或”仅仅是一种描述关联对象的关联关系,表示可以存在三种关系,例如,A和/或B,可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。另外,本文中字符“/”,一般表示前后关联对象是一种“或”的关系。
取决于语境,如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”或“响应于检测”。类似地,取决于语境,短语“如果确定”或“如果检测(陈述的条件或事件)”可以被解释成为“当确定时”或“响应于确定”或“当检测(陈述的条件或事件)时”或“响应于检测(陈述的条件或事件)”。
实施例中提及的“第一\第二”仅仅是是区别类似的对象,不代表针对对象的特定排序,可以理解地,“第一\第二”在允许的情况下可以互换特定的顺序或先后次序。应该理解“第一\第二”区分的对象在适当情况下可以互换,以使这里描述的实施例能够以除了在这里图示或描述的那些内容以外的顺序实施。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (9)
1.一种自动化农业机械的全覆盖路径规划方法,其特征在于,包含:
获取全局优化成本目标函数;其中,所述全局优化成本目标函数包括生成Headlands边界的代价函数、平行线束划分角度选取的代价函数,以及路径长度的代价函数;
获取自动化农业机械的模型,及其工作区域的地图数据;
根据所述地图数据,对边界进行凸子区域化的处理,分别根据处理后的凸子区域,基于生成Headlands边界的代价函数生成Headlands边界;
根据Headlands边界,基于所述自动化农业机械的模型和平行线束划分角度选取的代价函数对边界包围的区域进行平行线束划分,或者根据用户定义的平行线角度进行平行线束划分,获取凸子区域内的平行线集合;
根据所述平行线集合,进行全覆盖路径规划,获取平行线束集合的遍历顺序;其中,所述遍历顺序包括每条平行线的进入点和离开点;
根据所述平行线集合、所述遍历顺序和自动化农业机械的模型,基于路径长度的代价函数,通过利用连续曲率的Dubins曲线对平行线进行连接,获取规划路径。
2.根据权利要求1所述的一种自动化农业机械的全覆盖路径规划方法,其特征在于,
生成Headlands边界的代价函数为:/> ,式中,/>为生成Headlands边界后剩余的面积、/>为原始面积;
路径长度的代价函数为:/>,式中,/>为线束总数、/>为第/>条线束的Dubins曲线的曲线长度、/>为第/>条线束的起点位置、/>为第/>条线束的终点位置、/>是第/>条线束欧几里得范数;
平行线束划分角度选取的代价函数包括场覆盖最大化函数、线束数量最小化约束函数、场重叠最大化函数和线束路径长度最小化函数;其中,
场覆盖最大化函数为:/> ,式中,/>为凸子区域之间相交部分的面积之和、/>为工作区域/>的总面积;
线束数量最小化约束函数为:/> ,式中,/>是线束数量、/>是Headlands内边界的包围区域、/>是自动化农业机械的作业宽度;
场重叠最大化函数为:/>,式中,/>为多边形区域/>的面积、/>为多边形区域/>之外的多边形区域/>的面积、/>为多边形区域/>和多边形区域/>之间重叠的面积;
线束路径长度最小化函数为:/>,式中,/>是线束的数量、/>为第/>条线束的起点位置、/>为第/>条线束的终点位置、是第/>条线束欧几里得范数。
3.根据权利要求1所述的一种自动化农业机械的全覆盖路径规划方法,其特征在于,所述自动化农业机械的模型包括自动化农业机械的尺寸模型和运动学模型;所述自动化农业机械的地图数据为栅格化地图、卫星定位地图、感知传感器生成地图,或者卫星定位地图和感知传感器生成地图的融合信息地图;
获取自动化农业机械的模型,及其工作区域的地图数据,具体包括:
获取自动化农业机械的模型;
获取自动化农业机械的工作区域的地图数据,并判断所述地图数据是否为栅格化地图;
当判断到所述地图数据不是栅格化地图时,对地图数据的边缘进行提取以及平滑处理;其中,平滑处理通过滤波或B-Spline曲线拟合方式进行。
4.根据权利要求1所述的一种自动化农业机械的全覆盖路径规划方法,其特征在于,根据所述地图数据,对边界进行凸子区域化的处理,分别根据处理后的凸子区域,基于生成Headlands边界的代价函数生成Headlands边界,具体包括:
根据所述地图数据,获取工作区域的边界信息,并判断所述边界信息中是否包含凹边界;
当判断到所述边界信息中包含凹边界时,根据所述边界信息的凹点进行边界分割,获取多个凸子区域;否则,直接以边界信息作为凸子区域;
分别根据多个凸子区域,基于生成Headlands边界的扩张系数和代价函数,生成Headlands边界;其中,扩张系数为自动化农业机械的宽度的倍数。
5.根据权利要求1所述的一种自动化农业机械的全覆盖路径规划方法,其特征在于,根据Headlands边界,基于所述自动化农业机械的模型和平行线束划分角度选取的代价函数对边界包围的区域进行平行线束划分,获取凸子区域内的平行线集合,具体包括:
根据Headlands边界,获取平行线的边界;
根据所述自动化农业机械的模型,获取自动化农业机械的作业宽度;
根据所述作业宽度和所述平行线的边界,生成不同平行线生成角度的平行线集合;
根据不同平行线生成角度的平行线集合,选取平行线束划分角度选取的代价函数值最小的平行线集合作为最终确定的平行线集合。
6.根据权利要求1所述的一种自动化农业机械的全覆盖路径规划方法,其特征在于,根据所述平行线集合,进行全覆盖路径规划,获取平行线束集合的遍历顺序,具体包括:
根据所述平行线集合,通过Boustrophedon元启发式搜索算法、Snake非启发式搜索方法,或者Spiral非启发式搜索方法进行全覆盖路径规划,获取平行线束集合的遍历顺序;其中,所述遍历顺序包括每条平行线的进入点和离开点。
7.根据权利要求1值6任意一项所述的一种自动化农业机械的全覆盖路径规划方法,其特征在于,根据所述平行线集合、所述遍历顺序和自动化农业机械的模型,基于路径长度的代价函数,通过利用连续曲率的Dubins曲线对平行线进行连接,获取规划路径,具体包括:
根据所述自动化农业机械的模型,获取自动化农业机械的最小转弯半径;
根据所述平行线集合和所述遍历顺序,获取相邻两条平行线之间的待连接离开点和待连接进入点;
根据所述最小转弯半径,通过连续曲率的Dubins曲线平滑连接所述相邻两条平行线之间的待连接离开点和待连接进入点,获取规划路径。
8.一种自动化农业机械的全覆盖路径规划装置,其特征在于,包含:
目标函数获取模块,用于获取全局优化成本目标函数;其中,所述全局优化成本目标函数包括生成Headlands边界的代价函数、平行线束划分角度选取的代价函数,以及路径长度的代价函数;
初始数据获取模块,用于获取自动化农业机械的模型,及其工作区域的地图数据;
边界生成模块,用于根据所述地图数据,对边界进行凸子区域化的处理,分别根据处理后的凸子区域,基于生成Headlands边界的代价函数生成Headlands边界;
平行线划分模块,用于根据Headlands边界,基于所述自动化农业机械的模型和平行线束划分角度选取的代价函数对边界包围的区域进行平行线束划分,或者根据用户定义的平行线角度进行平行线束划分,获取凸子区域内的平行线集合;
路径规划模块,用于根据所述平行线集合,进行全覆盖路径规划,获取平行线束集合的遍历顺序;其中,所述遍历顺序包括每条平行线的进入点和离开点;
平行线连接模块,用于根据所述平行线集合、所述遍历顺序和自动化农业机械的模型,基于路径长度的代价函数,通过利用连续曲率的Dubins曲线对平行线进行连接,获取规划路径。
9.一种自动化农业机械的全覆盖路径规划设备,其特征在于,包括处理器、存储器,以及存储在所述存储器内的计算机程序;所述计算机程序能够被所述处理器执行,以实现如权利要求1至7任意一项所述的一种自动化农业机械的全覆盖路径规划方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310957836.XA CN116880497A (zh) | 2023-08-01 | 2023-08-01 | 一种自动化农业机械的全覆盖路径规划方法、装置和设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310957836.XA CN116880497A (zh) | 2023-08-01 | 2023-08-01 | 一种自动化农业机械的全覆盖路径规划方法、装置和设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116880497A true CN116880497A (zh) | 2023-10-13 |
Family
ID=88254861
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310957836.XA Pending CN116880497A (zh) | 2023-08-01 | 2023-08-01 | 一种自动化农业机械的全覆盖路径规划方法、装置和设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116880497A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117268401A (zh) * | 2023-11-16 | 2023-12-22 | 广东碧然美景观艺术有限公司 | 一种动态围栏的园艺路径生成方法 |
CN117553804A (zh) * | 2024-01-11 | 2024-02-13 | 深圳市普渡科技有限公司 | 路径规划方法、装置、计算机设备和存储介质 |
-
2023
- 2023-08-01 CN CN202310957836.XA patent/CN116880497A/zh active Pending
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117268401A (zh) * | 2023-11-16 | 2023-12-22 | 广东碧然美景观艺术有限公司 | 一种动态围栏的园艺路径生成方法 |
CN117268401B (zh) * | 2023-11-16 | 2024-02-20 | 广东碧然美景观艺术有限公司 | 一种动态围栏的园艺路径生成方法 |
CN117553804A (zh) * | 2024-01-11 | 2024-02-13 | 深圳市普渡科技有限公司 | 路径规划方法、装置、计算机设备和存储介质 |
CN117553804B (zh) * | 2024-01-11 | 2024-04-09 | 深圳市普渡科技有限公司 | 路径规划方法、装置、计算机设备和存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Yahja et al. | An efficient on-line path planner for outdoor mobile robots | |
CN116880497A (zh) | 一种自动化农业机械的全覆盖路径规划方法、装置和设备 | |
Tovar et al. | Planning exploration strategies for simultaneous localization and mapping | |
US8352174B2 (en) | Targeted marching | |
Faigl | An application of self‐organizing map for multirobot multigoal path planning with minmax objective | |
Song et al. | Online coverage and inspection planning for 3D modeling | |
CN114035572B (zh) | 一种割草机器人的避障巡回方法及系统 | |
CN110196602A (zh) | 目标导向集中优化的快速水下机器人三维路径规划方法 | |
Kuffner | Efficient optimal search of uniform-cost grids and lattices | |
CN115164907A (zh) | 基于动态权重的a*算法的森林作业机器人路径规划方法 | |
Al-Dahhan et al. | Voronoi boundary visibility for efficient path planning | |
Pivtoraiko et al. | Differentially constrained motion replanning using state lattices with graduated fidelity | |
Plaku et al. | Clearance-driven motion planning for mobile robots with differential constraints | |
Tormagov et al. | Coverage path planning for 3d terrain with constraints on trajectory curvature based on second-order cone programming | |
Dogru et al. | Towards fully autonomous energy efficient coverage path planning for autonomous mobile robots on 3d terrain | |
CN116136417B (zh) | 一种面向越野环境的无人驾驶车辆局部路径规划方法 | |
Wooden et al. | Oriented visibility graphs: Low-complexity planning in real-time environments | |
Feld et al. | Optimizing geometry compression using quantum annealing | |
Gottlieb et al. | Multi-target motion planning amidst obstacles for autonomous aerial and ground vehicles | |
Yang et al. | Survey of 3D map in SLAM: Localization and navigation | |
Geraerts et al. | Creating small roadmaps for solving motion planning problems | |
CN114815899A (zh) | 基于3d激光雷达传感器的无人机三维空间路径规划方法 | |
Jan et al. | A new maze routing approach for path planning of a mobile robot | |
Bouras et al. | Multi-uavs coverage path planning | |
Qin et al. | Dynamic Detection of Topological Information from Grid‐Based Generalized Voronoi Diagrams |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |