CN117073688B - 一种基于多层代价地图的覆盖路径规划方法 - Google Patents

Abstract

本发明涉及割草机器人应用技术领域，尤其涉及一种基于多层代价地图的覆盖路径规划方法。一种基于多层代价地图的覆盖路径规划方法，包括S1、生成多层代价地图：该多层地图中包括粗糙度代价地图与坡度代价地图；S2、生成可穿越性代价地图；S3、建立边界；S4、多层代价地图预处理；S5、路径规划。本发明根据机器人获取的点云地图数据建立有关粗糙度与坡度的代价地图，即多层代价地图，结合机器人本身的最大行驶粗糙度和坡度，得出机器人无法穿越区域，建立可穿越性代价地图，以可穿越性代价地图为基本层规划路径，利用启发式搜索算法，寻建立以坡度、转弯角度为目标的优化函数，寻找最优路径，使割草机器人高效运行的同时，保证安全行驶。

Description

一种基于多层代价地图的覆盖路径规划方法

技术领域

本发明涉及割草机器人应用技术领域，尤其是涉及的是一种基于多层代价地图的覆盖路径规划方法。

背景技术

在农业和园艺领域，割草是维持景观美观和植物生长的关键环节。为了提高效率并降低人工成本，自主导航割草机器人在近年来得到了广泛应用。然而，在复杂的三维环境中，如有坡度、高低起伏、不规则地形和障碍物存在的情况下，割草机器人的路径规划仍然面临一系列挑战。

目前的割草机器人路径规划方法主要基于传统的栅格地图和避障算法，这些方法通常采用单一的代价函数来评估路径，而忽视了地形的多方面信息。因此，在复杂地形下，这些方法往往难以产生高效且安全的割草路径。此外，现有方法在考虑多个目标，如最大覆盖范围、最小能量消耗、最小转弯次数等的权衡也不够充分，导致无法获取全面优化的路径。

发明内容

本发明的其他特征和优点将在随后的说明书中阐述，并且部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及其他说明书附图中所特别指出的结构来实现和获得。

本发明的目的在于克服上述不足，提供一种基于多层代价地图的覆盖路径规划方法，根据割草机器人激光获取的点云地图数据建立有关粗糙度与坡度的代价地图，即多层代价地图，结合机器人本身的最大行驶粗糙度和坡度，得出机器人无法穿越区域，建立可穿越性代价地图，以可穿越性代价地图为基本层规划路径，利用启发式搜索算法，寻建立以坡度、转弯角度为目标的优化函数，寻找最优路径，使割草机器人高效运行的同时，保证安全行驶。

本发明提供一种基于多层代价地图的覆盖路径规划方法，包括：

S1、生成多层代价地图：基于割草机器人获取的有关草地的点云地图数据，生成粗糙度代价地图与坡度代价地图，构建点云地图数据与多层代价地图之间的转换函数；

S2、生成可穿越性代价地图：基于粗糙度代价地图与坡度代价地图，建立代价函数，生成可穿越性代价地图，并以可穿越性代价地图层作为基本层进行路径规划；

S3、建立边界：由割草机器人绕草地边界行走一圈，实时跟踪割草机器人位姿，将割草机器人轨迹形成的多边形内部设置为可通行区域，多边形外部设置为禁行区域，创建虚拟边界地图层，建立边界并将边界拟合进可穿越性代价地图中；

S4、多层代价地图预处理：原有多层代价地图中的分辨率为单位栅格大小，现根据割草机器人实际大小，进行栅格地图分辨率的调整，使该多层代价地图的栅格大小与该割草机器人大小相适应；

S5、路径规划：在具有边界的可穿越性层地图上进行路径规划，同时调用粗糙度代价地图与坡度代价地图中的信息，构建以坡度、转弯角度为目标的多目标优化函数，获取最优覆盖路径。

在一些实施例中，在S1步骤中，草地的地形特征通过粗糙度与坡度来表示，而粗糙度与坡度从割草机器人获取的点云地图数据中转换得出；

粗糙度即地形表面不平整程度，由点云地图单位内的分散程度来表示，并通过最小二乘法计算；坡度层将点云地图数据中的高度信息拟合到平面中来表示，粗糙度与坡度的具体转换函数如下：

a_i,b_i,c_i＝LSM({x_i,y_i,z_i)∈B_r(π_i)})

其中，S_slope(π)表示地图中某处的坡度，S_rough(π)代表粗糙度，通过最小二乘法将点云地图数据中的高度信息拟合到平面ax+by+c＝z中计算坡度，a_i,b_i,c_i为拟合平面的系数，x、y、z为点云地图中的各轴信息，LSM(x_i,y_i,z_i)即为最小二乘法简化公式，B_r(π_i)为地图中某处周围的点云信息集合，将点云地图中的点云信息的各轴数据代入粗糙度与坡度的具体转换函数中求算出a_i,b_i,c_i的值；σ_i用于估计地面与拟合平面的高度偏差，从而计算粗糙度。

在一些实施例中，在S2步骤中，通过对草地地形与机器人运动学模型进行分析，寻找割草机器人可行走的最大坡度与最大粗糙度，将割草机器人无法穿越的地图部分设置为障碍物，其余部分设置为可通行区域，生成可穿越性地图。

在一些实施例中，在S2步骤中，具体代价函数如下：

其中，α₁和α₂是和为1的权重，s_max和r_max表示割草机器人能穿越的最大坡度与最大粗糙度，即导致机器人倾翻的临界值，s和r表示地图中某局部区域的坡度与粗糙度；

Tra代表机器人在地图中某处的可穿越性，Tra值的范围为[0,1]，当地形中某一处的Tra值不属于[0,1]时，则将地形标记为障碍物，即不可穿越。

在一些实施例中，在S4步骤中，设原地图大小为mx*my，机器人大小为model_size，则调整后的分辨率应为：

row＝mx/model_size

col＝my/model_size

其中，row表示调整后的地图x轴方向的分辨率大小，col为y轴方向的分辨率大小。

在一些实施例中，在S5步骤中，首先定义割草机器人全覆盖路径规划损耗代价函数，而后利用启发式搜索算法，计算使其代价最小的方式进行割草作业，涉及以坡度、转弯角度为目标的多目标优化函数具体如下：

E(p,n)＝d_r(p,n)+N(n)+E_slope

E_slope＝max(mg(μcosω+sinα)·‖d_r‖,0)

其中，p＝(x,y,θ)表示机器人当前姿态，θ表示机器人当前的姿态角，n表示下一个将到达的位置，旋转距离d_r以π/4为单位测量；函数N(n)表示N周围的8个邻居Nb₈(n)中尚未访问的位置的数量，此函数会产生跟踪行为；k表示Nb₈(n)中的成员，L是已访问位置的集合，|k∩L|表示N周围邻居是否已被访问，ω是梯度的倾角，mg是机器人的重量，移动表面和全地形表面之间的摩擦系数μ设置为0.2，α即地图中该处的坡度角。

在一些实施例中，若在p的8邻域中有未访问的邻居，则选择其中一个作为下一个机器人姿势；若已经访问了p周围的所有邻居，则从未访问的网格单元的集合中提取潜在的下一个姿势n。

通过采用上述的技术方案，本发明的有益效果是：

本发明通过点云地图数据建立多层代价栅格地图，结合坡度、转弯角度等目标，构建了多目标优化函数，以获取最大覆盖范围的无碰撞覆盖式路径，同时最小化能量损耗与转弯次数，使得割草机器人能够在复杂障碍物的非结构化环境下进行高效而安全的割草操作，同时在考虑地形起伏的情况下获得相对最优的路径规划。

本发明建立的多层代价地图与单一代价函数相比，能够更准确的描绘地形特征，在提高了路径规划的精度和可行性，引入多目标优化函数，使得路径规划更具有综合性和全面性，从而在不同目标间进行权衡，获得更优路径，适用于复杂障碍物和不平坦地形，为割草机器人在各种环境下的自主规划提供了强大支持。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。

无疑的，本发明的此类目的与其他目的在下文以多种附图与绘图来描述的较佳实施例细节说明后将变为更加显见。

为让本发明的上述和其他目的、特征和优点能更明显易懂，下文特举一个或数个较佳实施例，并配合所示附图，作详细说明如下。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例共同用于解释本发明，并不构成对本发明的限制。

在附图中，相同的部件使用相同的附图标记，并且附图是示意性的，并不一定按照实际的比例绘制。

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

图1为本发明一些实施例中的割草机器人路径规划整体流程示意图；

图2为本发明一些实施例中的多层代价地图结构示意图；

图3为本发明一些实施例中的割草机器人建立边界示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合具体实施方式对本发明进一步详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，但并不用于限定本发明。

另外，在本发明的描述中，需要理解的是，术语“中心”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。但注明直接连接则说明连接地两个主体之间并不通过过渡结构构建连接关系，只通过连接结构相连形成一个整体。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。

参照图1-3，图1为本发明一些实施例中的割草机器人路径规划整体流程示意图；图2为本发明一些实施例中的多层代价地图结构示意图；图3为本发明一些实施例中的割草机器人建立边界示意图。

根据本发明的一些实施例，本发明提供了一种基于多层代价地图的覆盖路径规划方法，包括：

S1、生成多层代价地图：基于割草机器人获取的有关草地的点云地图数据，生成粗糙度代价地图与坡度代价地图，构建点云地图数据与多层代价地图之间的转换函数；

在S1步骤中，草地的地形特征通过粗糙度与坡度来表示，而粗糙度与坡度从割草机器人获取的点云地图数据中转换得出，粗糙度代价地图反映了地表的不平整程度，坡度代价地图则揭示了地形的坡度变化，通过综合这些信息，可以得出地形的复杂性和机器人的通行难度；点云地图数据的获取可参照现有技术，现有技术中常在机器人上设置激光雷达或相机获取地形的点云信息；

粗糙度即地形表面不平整程度，由点云地图单位内的分散程度来表示，并通过最小二乘法计算；坡度层将点云地图数据中的高度信息拟合到平面中来表示，粗糙度与坡度的具体转换函数如下：

a_i,b_i,c_i＝LSM({x_i,y_i,z_i)∈B_r(π_i)})

其中，S_slope(π)表示地图中某处的坡度，S_rough(π)代表粗糙度，通过最小二乘法将点云地图数据中的高度信息拟合到平面ax+by+c＝z中计算坡度，a_i,b_i,c_i为拟合平面的系数，x、y、z为点云地图中的各轴信息，LSM(x_i,y_i,z_i)即为最小二乘法简化公式，B_r(π_i)为地图中某处周围的点云信息集合，将点云地图中的点云信息的各轴数据代入粗糙度与坡度的具体转换函数中求算出a_i,b_i,c_i的值；σ_i用于估计地面与拟合平面的高度偏差，从而计算粗糙度。

S2、生成可穿越性代价地图：基于粗糙度代价地图与坡度代价地图，建立代价函数，生成可穿越性代价地图，并以可穿越性代价地图层作为基本层进行路径规划；

如图2所示，在S2步骤中，通过对草地地形与机器人运动学模型进行分析，寻找割草机器人可行走的最大坡度与最大粗糙度，将割草机器人无法穿越的地图部分设置为障碍物，其余部分设置为可通行区域，生成可穿越性地图；

具体的，机器人运动学模型为现有技术，在实际运用过程中，不同类型的割草机器人形态与穿越能力不同，因此得出的可行走最大坡度与最大粗糙度也不同，应与实际的机器人结构相结合，在此不再赘述；

具体代价函数如下：

其中，α₁和α₂是和为1的权重，s_max和r_max表示割草机器人能穿越的最大坡度与最大粗糙度，即导致机器人倾翻的临界值，s和r表示地图中某局部区域的坡度与粗糙度；

Tra代表机器人在地图中某处的可穿越性，Tra值的范围为[0,1]，当地形中某一处的Tra值不属于[0,1]时，则将地形标记为障碍物，即不可穿越，当Tra值较小时代表地形较为平坦，Tra值较大时代表地形较为崎岖，在实际运用过程中，Tra值要根据具体机器人具体设置，Tra的设置有参数文件参考，可以根据不同机器人能翻越的最大坡度与平坦度来设置。

S3、建立边界：由割草机器人绕草地边界行走一圈，实时跟踪割草机器人位姿，如图3所示，将割草机器人轨迹形成的多边形内部设置为可通行区域，多边形外部设置为禁行区域，创建虚拟边界地图层，建立边界并将边界拟合进可穿越性代价地图中；

具体的，该割草机器人的位姿跟踪通过IMU实现，该IMU可的得出加速度、角速度及方向等，为割草机器人的运行提供支撑。

S4、多层代价地图预处理：原有多层代价地图中的分辨率为单位栅格大小，现根据割草机器人实际大小，进行栅格地图分辨率的调整，使该多层代价地图的栅格大小与该割草机器人大小相适应；

在S4步骤中，设原地图大小为mx*my，机器人大小为model_size，则调整后的分辨率应为：

row＝mx/model_size

col＝my/model_size

其中，row表示调整后的地图x轴方向的分辨率大小，col为y轴方向的分辨率大小，根据割草机器人的尺寸调整栅格地图的分辨率，可以更精确地规划路径。

S5、路径规划：在具有边界的可穿越性层地图上进行路径规划，同时调用粗糙度代价地图与坡度代价地图中的信息，构建以坡度、转弯角度为目标的多目标优化函数，获取最优覆盖路径；

在S5步骤中，首先定义割草机器人全覆盖路径规划损耗代价函数，而后利用启发式搜索算法，计算使其代价最小的方式进行割草作业，涉及以坡度、转弯角度为目标的多目标优化函数具体如下：

E(p,n)＝d_r(p,n)+N(n)+E_slope

E_slope＝max(mg(μcosω+sinα)·‖d_r‖,0)

其中，p＝(x,y,θ)表示机器人当前姿态，θ表示机器人当前的姿态角，n表示下一个将到达的位置，旋转距离d_r以π/4为单位测量；函数N(n)表示N周围的8个邻居Nb₈(n)中尚未访问的位置的数量，此函数会产生跟踪行为；k表示Nb₈(n)中的成员，L是已访问位置的集合，|k∩L|表示N周围邻居是否已被访问，ω是梯度的倾角，mg是机器人的重量，移动表面和全地形表面之间的摩擦系数μ设置为0.2，α即地图中该处的坡度角；

若在p的8邻域中有未访问的邻居，则选择其中一个作为下一个机器人姿势；若已经访问了p周围的所有邻居，则从未访问的网格单元的集合中提取潜在的下一个姿势n。

应该理解的是，本发明所公开的实施例不限于此处所公开的特定处理步骤或材料，而应当延伸到相关领域的普通技术人员所理解的此类特征的等同替代。还应当理解的是，在此使用的术语仅用于描述特定实施例的目的，而并不意味着限制。

说明书中提到的“实施例”意指结合实施例描述的特定特征、或特性包括在本发明的至少一个实施例中。因此，说明书通篇各个地方出现的短语或“实施例”并不一定均指同一个实施例。

此外，所描述的特征或特性可以任何其他合适的方式结合到一个或多个实施例中。在上面的描述中，提供一些具体的细节，例如厚度、数量等，以提供对本发明的实施例的全面理解。然而，相关领域的技术人员将明白，本发明无需上述一个或多个具体的细节便可实现或者也可采用其他方法、组件、材料等实现。

Claims (5)

1.一种基于多层代价地图的覆盖路径规划方法，其特征在于，包括

S1、生成多层代价地图：基于割草机器人获取的有关草地的点云地图数据，生成粗糙度代价地图与坡度代价地图，构建点云地图数据与多层代价地图之间的转换函数；

草地的地形特征通过粗糙度与坡度来表示，而粗糙度与坡度从割草机器人获取的点云地图数据中转换得出；

粗糙度即地形表面不平整程度，由点云地图单位内的分散程度来表示，并通过最小二乘法计算；坡度层将点云地图数据中的高度信息拟合到平面中来表示，粗糙度与坡度的具体转换函数如下：

a_i,b_i,c_i＝LSM({x_i,y_i,z_i)∈B_r(π_i)})

其中，S_slope(π)表示地图中某处的坡度，S_rough(π)代表粗糙度，通过最小二乘法将点云地图数据中的高度信息拟合到平面ax+by+c＝z中计算坡度，a_i,b_i,c_i为拟合平面的系数，x、y、z为点云地图中的各轴信息，LSM(x_i,y_i,z_i)即为最小二乘法简化公式，B_r(π_i)为地图中某处周围的点云信息集合，将点云地图中的点云信息的各轴数据代入粗糙度与坡度的具体转换函数中求算出a_i,b_i,c_i的值；σ_i用于估计地面与拟合平面的高度偏差，从而计算粗糙度；

S2、生成可穿越性代价地图：基于粗糙度代价地图与坡度代价地图，建立代价函数，生成可穿越性代价地图，并以可穿越性代价地图层作为基本层进行路径规划；

S3、建立边界：由割草机器人绕草地边界行走一圈，实时跟踪割草机器人位姿，将割草机器人轨迹形成的多边形内部设置为可通行区域，多边形外部设置为禁行区域，创建虚拟边界地图层，建立边界并将边界拟合进可穿越性代价地图中；

S4、多层代价地图预处理：原有多层代价地图中的分辨率为单位栅格大小，现根据割草机器人实际大小，进行栅格地图分辨率的调整，使该多层代价地图的栅格大小与该割草机器人大小相适应；

S5、路径规划：在具有边界的可穿越性层地图上进行路径规划，同时调用粗糙度代价地图与坡度代价地图中的信息，构建以坡度、转弯角度为目标的多目标优化函数，获取最优覆盖路径；

首先定义割草机器人全覆盖路径规划损耗代价函数，而后利用启发式搜索算法，计算使其代价最小的方式进行割草作业，涉及以坡度、转弯角度为目标的多目标优化函数具体如下：

E(p,n)＝d_r(p,n)+N(n)+E_slope

E_slope＝max(mg(μcosω+sinα)·‖d_r‖,0)

其中，p＝(x,y,θ)表示机器人当前姿态，θ表示机器人当前的姿态角，n表示下一个将到达的位置，旋转距离d_r以π/4为单位测量；函数N(n)表示N周围的8个邻居Nb₈(n)中尚未访问的位置的数量，此函数会产生跟踪行为；k表示Nb₈(n)中的成员，L是已访问位置的集合，|k∩L|表示N周围邻居是否已被访问，ω是梯度的倾角，mg是机器人的重量,移动表面和全地形表面之间的摩擦系数μ设置为0.2，α即地图中该处的坡度角。

2.根据权利要求1所述的基于多层代价地图的覆盖路径规划方法，其特征在于，在S2步骤中，通过对草地地形与机器人运动学模型进行分析，寻找割草机器人可行走的最大坡度与最大粗糙度，将割草机器人无法穿越的地图部分设置为障碍物，其余部分设置为可通行区域，生成可穿越性地图。

3.根据权利要求2所述的基于多层代价地图的覆盖路径规划方法，其特征在于，在S2步骤中，具体代价函数如下：

其中，α₁和α₂是和为1的权重，s_max和r_max表示割草机器人能穿越的最大坡度与最大粗糙度，即导致机器人倾翻的临界值，s和r表示地图中某局部区域的坡度与粗糙度；

Tra代表机器人在地图中某处的可穿越性，Tra值的范围为[0,1]，当地形中某一处的Tra值不属于[0,1]时，则将地形标记为障碍物，即不可穿越。

4.根据权利要求1所述的基于多层代价地图的覆盖路径规划方法，其特征在于，在S4步骤中，设原地图大小为mx*my，机器人大小为model_size，则调整后的分辨率应为：

row＝mx/model_size

col＝my/model_size

其中，row表示调整后的地图x轴方向的分辨率大小，col为y轴方向的分辨率大小。

5.根据权利要求1所述的基于多层代价地图的覆盖路径规划方法，其特征在于，若在p的8邻域中有未访问的邻居，则选择其中一个作为下一个机器人姿势；若已经访问了p周围的所有邻居，则从未访问的网格单元的集合中提取潜在的下一个姿势n。