CN120686837A - 一种菠萝采摘机器人的行走控制方法及控制系统 - Google Patents

Abstract

本发明公开了一种菠萝采摘机器人的行走控制方法及控制系统，本发明涉及农业设备控制技术领域。包括以下步骤：获取待采集菠萝地的植株空间分布数据，确定植株位置和区域密度，并基于密度规划可移动区域。获取目标菠萝特征信息，判断其是否符合采摘标准，若符合则标记为采摘目标。依据机器人的最大采摘半径确定采摘领域，检测目标是否在该范围内。若不在范围内，则获取目标和机器人的空间位置，通过激光雷达扫描生成高程网格地图，利用多维特征向量表征地形特征，并定义地形起伏度。设置起伏判断阈值，并根据比较结果动态调整机械悬挂行程和履带宽度，以便顺利到达采摘目标进行采摘。此方法提高了采摘效率和准确性。

Description

一种菠萝采摘机器人的行走控制方法及控制系统

技术领域

本发明涉及农业设备控制技术领域，具体为一种菠萝采摘机器人的行走控制方法及控制系统。

背景技术

在现代农业自动化中，机械化采摘已成为提高作物产量和降低劳动成本的关键技术。菠萝作为一种热带水果，其生长环境复杂，菠萝植株分布密集且地形起伏较大，给采摘过程带来了诸多挑战。传统的人工采摘不仅效率低下且难以实现精确的识别与统一的采摘标准。

为了提高菠萝的采摘效率和质量，研究者们逐渐将视觉识别、激光雷达等先进技术应用于菠萝采摘机器人中，这些技术能够帮助机器人识别目标菠萝并进行准确的路径规划。然而，现有技术在应对菠萝采摘过程中，仍存在许多不足之处。例如，传统的路径规划方法往往无法有效考虑地形的起伏特征，导致机器人在行走过程中无法适应多变的地面情况，从而影响采摘的效率和安全性。因此，如何利用菠萝植株的空间分布数据，结合地形起伏特征，实现机器人高效、稳定的行走控制成为亟待解决的技术问题。

现有技术中的，公开号为CN108575283A公开了一种菠萝采摘机器人的控制方法及控制系统，具体包括：所述菠萝采摘机器人自主规划路径，通过图像识别对菠萝进行识别以及获取采摘坐标，对所述采摘坐标进行数据分析，将所述剪切模块进行水平位置以及竖直位置进行调整，使得所述剪切模块移动至所述待采摘菠萝的梗部，便于对菠萝执行收拢剪切动作，直至完成采摘数量任务后返回停靠点，充分利用菠萝果实的独特生长形态，实现对菠萝进行自动化采摘，具有目标定位准确、采摘简单便捷等优点，但是该控制方法虽然能够自主规划路径，但未必能有效应对复杂地形和空间分布的变化，在复杂的地形条件下，控制系统可能未能充分考虑地面起伏和障碍物，导致机器人在实际行进过程中出现卡顿或无法顺利移动的问题。因此使得控制系统的稳定性、有效性降低。

在所述背景技术部分公开的上述信息仅用于加强对本公开的背景的理解，因此它可以包括不构成对本领域普通技术人员已知的现有技术的信息。

发明内容

本发明的目的在于提供一种菠萝采摘机器人的行走控制方法及控制系统，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：

一种菠萝采摘机器人的行走控制方法，具体步骤包括：

S1：获取待采集菠萝地的菠萝植株空间分布数据，基于菠萝植株空间分布数据确定待采集菠萝地内菠萝植株的空间分布位置，确定整个待采集菠萝地内各区域的空间分布密度，采用密度导向方法规划可移动区域；

S2：获取目标菠萝的特征信息，基于目标菠萝的特征信息判断当前目标菠萝是否满足采摘辨识标准，若满足将目标菠萝标记为采摘目标并执行步骤S3，否则更换目标菠萝返回执行步骤S2；

S3：获取菠萝采摘机器人采摘装置的最大采摘半径，基于最大采摘半径确定采摘领域范围，检测当前采摘目标是否处于采摘领域范围内，若是则对当前采摘目标进行采摘，否则执行步骤S4；

S4：获取当前采摘目标和菠萝采摘机器人所在的地的空间位置，基于空间位置信息和可移动区域规划目的行走路径，通过激光雷达扫描菠萝采摘机器人目的行走路径的地形数据，生成高程网格地图，使用多维特征向量表征目的行走路径区域的地形特征数据，并基于多维特征向量定义目的行走路径的地形起伏度；

S5：设置地形起伏判断阈值，将目的行走路径区域的地形起伏度与地形起伏判断阈值相对比，若超过地形起伏判断阈值，动态调整菠萝采摘机器人的悬挂行程和履带宽度，直至行走到达当前采摘目标位置处进行采摘。

进一步地，获取待采集菠萝地的菠萝植株空间分布数据，其中空间分布数据具体指菠萝植株点云数据，基于菠萝植株空间分布数据确定待采集菠萝地内菠萝植株的空间分布位置具体所依据的逻辑为：通过激光扫描获取菠萝植株的三维点云数据，在待采集菠萝地内建立空间坐标系，以待采集菠萝地的几何中心为原点，以正东方向为x轴正方向，正北方向为y轴正方向，并通过右手定则确定z轴方向，在建立的坐标系中，记录菠萝植株点云数据坐标，对菠萝植株点云数据坐标进行滤波、分割和分类，以识别出菠萝植株的空间分布位置；

确定整个待采集菠萝地内各区域的空间分布密度，采用密度导向路径规划可移动区域具体所依据的逻辑为：基于菠萝植株点云数据分布密度，通过聚类算法对处理后的点云数据进行聚类，识别并标记出菠萝植株的集群，将待采集菠萝地区域内未被标记为菠萝植株集群且空间连续的区域作为可选取区域；

将可选取区域分割为若干网格子区域，检测网格子区域上的水文特征数据，根据水文特征数据识别可选取区域中的积水区，直至遍历全部网格子区域，将识别为积水区的网格子区域在可选取区域中进行标记，则未被标记的剩余网格子区域作为可移动区域，其中水文特征数据包括路面积水面积和最大积水深度，判断积水区具体所依据的逻辑为：设置积水判断阈值，将水文特征数据与积水判断阈值相对比，将超过积水判断阈值的网格子区域标记为积水区。

进一步地，获取目标菠萝的特征信息，所述目标菠萝的特征信息包括菠萝轮廓圆形度、表面平均粗糙度、菠萝轮廓面积和果皮黄绿面积占比；

其中获取目标菠萝的特征信息具体的方法为：采集若干已知特征信息的菠萝植株图像作为样本图像，在样本图像中通过最小外接矩形标记出菠萝果实，形成人工标记，并将已知的特征信息在对应标记处进行标注，生成特征数据图像，将特征数据图像与原始样本图像一一映射，形成训练样本数据集；

基于训练样本数据集内的数据，建立神经网络模型，将训练样本数据集内的原始样本图像作为输入，并以对应的特征数据图像作为标签，对神经网络模型进行训练，完成训练的神经网络模型输入为待检测的菠萝植株图像，输出为识别出图像内菠萝目标并表征有特征信息的图像。

进一步地，基于目标菠萝的特征信息判断当前目标菠萝是否满足采摘辨识标准，其中采摘辨识标准约束条件具体所依据的公式为：

式中，R_U为目标菠萝的菠萝轮廓圆形度，R_yz为轮廓圆形度阈值，Ra为目标菠萝的表面平均粗糙度，Ra_yz为平均粗糙度阈值，S_E为目标菠萝的菠萝轮廓面积，S_yz为菠萝轮廓面积阈值，BL为目标菠萝的果皮黄绿面积占比，BL_min为最小占比阈值，BL_max为最大占比阈值；

对于满足约束条件的目标菠萝，将其记为采摘目标。

进一步地，获取菠萝采摘机器人采摘装置的最大采摘半径，其中所述最大采摘半径具体指菠萝采摘机器人采摘装置在水平方向和垂直方向的最大活动距离，根据水平方向和垂直方向的最大采摘半径形成矩形采摘领域范围，基于最大采摘半径确定的采摘领域范围，检测当前采摘目标是否处于采摘领域范围内，其中检测判断具体所依据的逻辑为：

检测当前采摘目标果实根部与秆茎连接处，在建立的空间坐标系中的坐标，生成检测点云集合，判断当前采摘目标检测点云集合所形成的领域是否全部处于采摘领域范围内，若是则对当前采摘目标进行采摘，若采摘领域范围没有全部覆盖所形成的领域，则控制菠萝采摘机器人进行移动。

进一步地，通过激光雷达扫描菠萝采摘机器人前方目的行走路径的地形数据，生成高程网格地图，所述地形数据包括：最大高程差、单位平均坡度和地表平均粗糙度，其中扫描菠萝采摘机器人实时扫描前方5m内的目的行走路径，获取地形数据，其中单位平均坡度计算所依据的公式为：

式中，θ_mean为单位平均坡度，θ^m表示在菠萝采摘机器人前方5m内目的行走路径中，第m个高程网格单元内中心点的单元坡度，M为在菠萝采摘机器人前方5m内目的行走路径中，高程网格单元总个数，m为在菠萝采摘机器人前方5m内目的行走路径中，高程网格单元的索引，m∈[1,2,…,M]；

式中，h(i,j)_m为在菠萝采摘机器人前方5m内目的行走路径中，第m个高程网格单元内中心点(i,j)的海拔高度，表示沿x方向高程梯度，表示沿y方向高程梯度；

基于多维特征向量定义目的行走路径的地形起伏度，其中地形起伏度计算所依据的公式为：

式中，ECI为在菠萝采摘机器人前方5m内目的行走路径的地形起伏度，H_max为在菠萝采摘机器人前方5m内目的行走路径的最大高程差，Ra_d为在菠萝采摘机器人前方5m内目的行走路径的地表平均粗糙度，ω₁、ω₂和ω₃分别为单位平均坡度、最大高程差和地表平均粗糙度的权重系数，其中ω₂≥ω₁>ω₃，且ω₁、ω₂和ω₃均大于0。

进一步地，动态调整菠萝采摘机器人的悬挂行程和履带宽度具体所依据的逻辑为：基于地形起伏度对悬挂行程和履带宽度进行调整，其中，基于地形起伏度对悬挂行程进行动态修正所依据的公式为：

式中，SR表示菠萝采摘机器人动态调整后的悬挂行程，SR₀为悬挂行程初始值，α为悬挂行程对地形变化的敏感度系数；

其中，基于地形起伏度对履带宽度进行动态修正所依据的公式为：

SK＝SK₀*[1+β*ECI^2/3]

式中，SK为菠萝采摘机器人动态调整后的履带宽度，SK₀为履带宽度初始值，β为履带宽度对地形变化的敏感度系数。

本发明还提供一种菠萝采摘机器人的行走控制系统，所述一种菠萝采摘机器人的行走控制系统用于执行上述一种菠萝采摘机器人的行走控制方法，包括：

移动路径规划模块，用于获取待采集菠萝地的菠萝植株空间分布数据，基于菠萝植株空间分布数据确定待采集菠萝地内菠萝植株的空间分布位置，确定整个待采集菠萝地内各区域的空间分布密度，采用密度导向方法规划可移动区域；

采摘目标确定模块，用于获取目标菠萝的特征信息，基于目标菠萝的特征信息判断当前目标菠萝是否满足采摘辨识标准，若满足将目标菠萝标记为采摘目标并执行步骤S3，否则更换目标菠萝返回执行步骤S2；

采摘范围捕获模块，用于获取菠萝采摘机器人采摘装置的最大采摘半径，基于最大采摘半径确定采摘领域范围，检测当前采摘目标是否处于采摘领域范围内，若是则对当前采摘目标进行采摘，否则执行步骤S4；

路径地形检测模块，用于获取当前采摘目标和菠萝采摘机器人所在的地的空间位置，基于空间位置信息和可移动区域规划目的行走路径，通过激光雷达扫描菠萝采摘机器人目的行走路径的地形数据，生成高程网格地图，使用多维特征向量表征目的行走路径区域的地形特征数据，并基于多维特征向量定义目的行走路径的地形起伏度；

行走动态调整模块，用于设置地形起伏判断阈值，将目的行走路径区域的地形起伏度与地形起伏判断阈值相对比，若超过地形起伏判断阈值，动态调整菠萝采摘机器人的悬挂行程和履带宽度，直至行走到达当前采摘目标位置，进行采摘。

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

该方法首先获取菠萝植株的空间分布数据，通过菠萝分布数据，确定可行走路径，并通过地面水文特征参数剔除异常路段，优化了行走路径，减少异常状况的发生，从而显著提高了采摘效率。

其次通过实时扫描地形数据生成高程网格地图，并基于多维特征向量对地形起伏度进行分析，能够实现对地形变化的快速响应。当机器人进入起伏较大的地形区域时，动态调整悬挂行程和履带宽度，提高了机器人在复杂地形下的稳定性和通过性。有效降低了因地形变化导致的设备损坏风险，提高了采摘的安全性。使得机器人可以在广泛的地形条件下自主作业，提升了作业效率和采摘质量。

附图说明

图1为本发明整体方法流程示意图；

图2为菠萝植株的空间分布位置散点图；

图3为菠萝植株的空间分布模拟柱状图；

图4为悬挂行程和履带宽度调整幅度示意图；

图5为地形起伏度-悬挂行程拟合曲线图；

图6为地形起伏度-履带宽度拟合曲线图；

图7为调整后平均振动加速度减小量；

图8为本发明整体系统结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，对本发明进一步详细说明。

需要说明的是，除非另外定义，本发明使用的技术术语或者科学术语应当为本发明所属领域内具有一般技能的人士所理解的通常意义。本发明中使用的“第一”“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。“包括”或者“包含”等类似的词语意指出现该词前面的元件或者物件涵盖出现在该词后面列举的元件或者物件及其等同，而不排除其他元件或者物件。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。“上”“下”“左”“右”等仅用于表示相对位置关系，当被描述对象的绝对位置改变后，则该相对位置关系也可能相应地改变。

实施例：

请参阅图1-图7，本发明提供一种技术方案：

一种菠萝采摘机器人的行走控制方法，具体步骤包括：

步骤1：获取待采集菠萝地的菠萝植株空间分布数据，基于菠萝植株空间分布数据确定待采集菠萝地内菠萝植株的空间分布位置，确定整个待采集菠萝地内各区域的空间分布密度，采用密度导向方法规划可移动区域。

获取待采集菠萝地的菠萝植株空间分布数据，其中空间分布数据具体指菠萝植株点云数据，基于菠萝植株空间分布数据确定待采集菠萝地内菠萝植株的空间分布位置具体所依据的逻辑为：通过激光扫描获取菠萝植株的三维点云数据，在待采集菠萝地内建立空间坐标系，以待采集菠萝地的几何中心为原点，以正东方向为x轴正方向，正北方向为y轴正方向，并通过右手定则确定z轴方向，在建立的坐标系中，记录菠萝植株点云数据坐标，对菠萝植株点云数据坐标进行滤波、分割和分类，以识别出菠萝植株的空间分布位置；

确定整个待采集菠萝地内各区域的空间分布密度，采用密度导向路径规划可移动区域具体所依据的逻辑为：基于菠萝植株点云数据分布密度，通过聚类算法对处理后的点云数据进行聚类，识别并标记出菠萝植株的集群，将待采集菠萝地区域内未被标记为菠萝植株集群且空间连续的区域作为可选取区域；

首先，使用激光雷达，无人机或其他传感器采集菠萝地的点云数据。采集的点云数据包含每个点的三维坐标信息，在进行聚类之前，需对点云数据进行预处理，噪声去除：使用滤波器，如统计离群点移除法来去除噪声和不必要的点，如果点云数据量很大，可以使用Voxel Grid滤波器等方法进行下采样，减少计算量，选择合适的聚类算法来处理点云数据，常用的聚类算法包括：DBSCAN算法通过选择合适的邻域半径和最小点数。DBSCAN的优点在于能够识别任意形状的聚类并处理噪声；实施步骤：对于每个点，查找其邻域内的点的数量。如果邻域内点的数量大于或等于最小点数，将其标记为核心点，并形成聚类。递归地将邻域内的核心点加入到聚类中。标记未被包含在任何聚类中的点为噪声。或使用MeanShift算法，通过计算数据点的均值位置，并不断更新聚类中心，直到达到收敛。适用于聚类形状不规则的情况。

从聚类结果中识别出未被标记为菠萝植株集群的空间连续区域，通过邻接矩阵或空间图分析，检查未被标记的点是否形成连续的区域，将符合条件的区域标记为可选取区域，便于后续路径规划。

将可选取区域分割为若干网格子区域，检测网格子区域上的水文特征数据，根据水文特征数据识别可选取区域中的积水区，直至遍历全部网格子区域，将识别为积水区的网格子区域在可选取区域中进行标记，将未被标记的剩余网格子区域作为可移动区域，其中水文特征数据包括路面积水面积和最大积水深度，判断积水区具体所依据的逻辑为：设置积水判断阈值，将水文特征数据与积水判断阈值相对比，将超过积水判断阈值的网格子区域标记为积水区。

其中路面积水面积的获取可以使用影像数据，通过图像处理技术具体例如阈值分割，识别积水区域，将图像转换为灰度图。应用自适应阈值或Otsu法进行二值化处理，区分积水和非积水区域，使用连通域分析找到积水区域，并计算其面积。

最大积水深度在积水区域通过水位传感器，可以直接读取传感器的实时数据。

设置积水判断阈值，将水文特征数据与积水判断阈值相对比，将超过积水判断阈值的网格子区域标记为积水区，其中判断积水区的具体逻辑为：基于水文特征数据计算不可通行系数，将不可通行系数与积水判断阈值相对比，根据对比结果判断积水区，其中不可通行系数计算具体所依据的公式为：

式中，SCI为不可通行系数，MS_m为路面积水面积，L_max为最大积水深度。

需要说明的是，不可通行系数SCI其数值越大，表示当前区域内的路面积水越多，积水深度越大，越不适宜菠萝采摘机器人进行行走。

其中，路面积水面积MS_m反映了积水的广泛程度，直接影响到区域的通行能力。积水面积越大，意味着可能影响的区域越大，通行的风险也越高，因此路面积水面积MS_m与不可通行系数SCI成正比，通过平方根函数对水面积的影响是非线性的，意味着随着积水面积的增加，SCI的增长速度会减缓。这是因为在实际情况中，面积增加到一定程度后，影响的边际效应会下降，即进一步增加的面积对通行能力的影响相对降低，平方根可以避免在积水面积极大的情况下，SCI的值异常增大，从而保持了判断的合理性。

最大积水深度L_max量化了积水的深度，深度越大，意味着对通行的影响越显著。深水可能导致菠萝采摘机器人无法通行，甚至对机器的安全构成威胁，通过对数函数ln(1+L_max)同样是一个非线性函数，能够有效地反映深度对通行能力的影响。积水深度小的时候，影响相对较小，而当深度增加到一定程度后，效果会显著增强。

根据对比结果判断积水区具体所依据的具体逻辑为：

当0≤SCI<1.0*yz时，判断该网格子区域可以通行；

当SCI≥1.0*yz时，判断该网格子区域为积水区，表示该网格子区域不可通行；

其中yz为积水判断阈值。具体可根据菠萝采摘机器人履带支撑高度结合专家经验进行判断。

步骤2：获取目标菠萝的特征信息，基于目标菠萝的特征信息判断当前目标菠萝是否满足采摘辨识标准，若满足将目标菠萝标记为采摘目标并执行步骤S3，否则更换目标菠萝返回执行步骤S2。

获取目标菠萝的特征信息，所述目标菠萝的特征信息包括菠萝轮廓圆形度、表面平均粗糙度、菠萝轮廓面积和果皮黄绿面积占比；

其中获取目标菠萝的特征信息具体的方法为：采集若干已知特征信息的菠萝植株图像作为样本图像，在样本图像中通过最小外接矩形标记出菠萝果实，形成人工标记，并将已知的特征信息在对应标记处进行标注，生成特征数据图像，将特征数据图像与原始样本图像一一映射，形成训练样本数据集；

基于训练样本数据集内的数据，建立神经网络模型，将训练样本数据集内的原始样本图像作为输入，并以对应的特征数据图像作为标签，对神经网络模型进行训练，完成训练的神经网络模型输入为待检测的菠萝植株图像，输出为识别出图像内菠萝目标并表征有特征信息的图像。

所述神经网络模型具体为长短期记忆网络模型LSTM模型，选取激活函数和优化算法，其中选择Tanh函数作为激活函数，选择Adam作为LSTM模型的优化算法；Tanh函数其公式为：

式中，f(x)表示Tanh函数，自变量r表示神经元的输入加权和，即神经元接收到的来自上一层的输入经过加权求和后的结果；

同时设定LSTM模型的超参数，所述LSTM模型的超参数包括：网络层数、迭代次数、学习率、批量数大小、训练次数、批处理数量和隐藏层神经元个数；

其中网络层数设置为3层网络结构，迭代次数设定为200，学习率设置为0.001，批量数大小设为32，训练次数设为100，批处理数量设为256，隐藏层神经元个数为32。

基于目标菠萝的特征信息判断当前目标菠萝是否满足采摘辨识标准，其中采摘辨识标准约束条件具体所依据的公式为：

式中，R_U为目标菠萝的菠萝轮廓圆形度，R_yz为轮廓圆形度阈值，Ra为目标菠萝的表面平均粗糙度，Ra_yz为平均粗糙度阈值，S_E为目标菠萝的菠萝轮廓面积，S_yz为菠萝轮廓面积阈值，BL为目标菠萝的果皮黄绿面积占比，BL_min为最小占比阈值，BL_max为最大占比阈值；

对于满足约束条件的目标菠萝，将其记为采摘目标。其中各阈值具体通过专家经验结合菠萝品种进行设置。

步骤3：获取菠萝采摘机器人采摘装置的最大采摘半径，基于最大采摘半径确定采摘领域范围，检测当前采摘目标是否处于采摘领域范围内，若是则对当前采摘目标进行采摘，否则执行步骤S4。

获取菠萝采摘机器人采摘装置的最大采摘半径，其中所述最大采摘半径具体指菠萝采摘机器人采摘装置在水平方向和垂直方向的最大活动距离，根据水平方向和垂直方向的最大采摘半径形成矩形采摘领域范围，基于最大采摘半径确定的采摘领域范围，检测当前采摘目标是否处于采摘领域范围内，其中检测判断具体所依据的逻辑为：

检测当前采摘目标果实根部与秆茎连接处，在建立的空间坐标系中的坐标，生成检测点云集合，判断当前采摘目标检测点云集合所形成的领域是否全部处于采摘领域范围内，若是则对当前采摘目标进行采摘，若没有采摘领域范围全部覆盖所形成的领域，则控制菠萝采摘机器人进行移动。

其中在检测当前采摘目标时，需要关注菠萝的根部与秆茎连接处的坐标，具体步骤如下：获取采摘目标的坐标：通过激光雷达获取当前菠萝的根部坐标，设定点云的密度和分布，例如，使用球形或立方体的方式围绕目标生成点云，确保生成的点云覆盖菠萝的各个部分，获取生成的点云集合的边界坐标，确定其在空间坐标系中的最小和最大范围，如果点云的所有点都在定义的采摘领域范围内，则可以判断当前采摘目标在采摘领域范围内。

步骤4：获取当前采摘目标和菠萝采摘机器人所在的地的空间位置，基于空间位置信息和可移动区域规划目的行走路径，通过激光雷达扫描菠萝采摘机器人目的行走路径的地形数据，生成高程网格地图，使用多维特征向量表征目的行走路径区域的地形特征数据，并基于多维特征向量定义目的行走路径的地形起伏度。

其中使用激光雷达、深度相机或其他传感器获取当前菠萝采摘目标的空间位置坐标。具体步骤包括：通过传感器扫描周围环境，识别出菠萝的位置，并提取其当前采摘目标的几何中心的坐标。利用全局定位系统如GPS，或本地定位系统如激光SLAM，获取菠萝采摘机器人的空间位置。

其中基于空间位置信息和可移动区域规划目的行走路径的具体方式包括：以菠萝采摘机器人的中心空间坐标为起始坐标，以当前采摘目标的几何中心为目标空间坐标，提取其平面坐标，基于平面坐标通过路径规划算法，在可选取区域中进行路径规划，得到目的行走路径，其中所述路径规划算法具体可选择A*算法：适合在已知地图中进行寻路，效率高且能避免障碍；Dijkstra算法：适用于寻找最短路径，但效率相对较低。结合当前采摘目标位置和机器人所在位置，计算机器人到达采摘目标的最优路径，具体以最短路径作为最优路径，使用选择的路径规划算法，从起点到终点生成路径，确保路径经过的所有区域都是可移动的。

通过激光雷达扫描菠萝采摘机器人前方目的行走路径的地形数据，生成高程网格地图，所述地形数据包括：最大高程差、单位平均坡度和地表平均粗糙度，其中扫描菠萝采摘机器人实时扫描前方5m内的目的行走路径，获取地形数据，其中单位平均坡度计算所依据的公式为：

式中，θ_mean为单位平均坡度，θ^m表示在菠萝采摘机器人前方5m内目的行走路径中，第m个高程网格单元内中心点的单元坡度，M为在菠萝采摘机器人前方5m内目的行走路径中，高程网格单元总个数，m为在菠萝采摘机器人前方5m内目的行走路径中，高程网格单元的索引，m∈[1,2,…,M]；

式中，h(i,j)_m为在菠萝采摘机器人前方5m内目的行走路径中，第m个高程网格单元内中心点(i,j)的海拔高度，表示沿x方向高程梯度，表示沿y方向高程梯度；

基于多维特征向量定义目的行走路径的地形起伏度，其中地形起伏度计算所依据的公式为：

式中，ECI为在菠萝采摘机器人前方5m内目的行走路径的地形起伏度，H_max为在菠萝采摘机器人前方5m内目的行走路径的最大高程差，Ra_d为在菠萝采摘机器人前方5m内目的行走路径的地表平均粗糙度，θ₀为基准坡度，通常为零，表示平坦地面下的坡度，ω₁、ω₂和ω₃分别为单位平均坡度、最大高程差和地表平均粗糙度的权重系数，其中ω₂≥ω₁>ω₃，且ω₁、ω₂和ω₃均大于0。

需要说明的是，地形起伏度ECI反映了地形对机器人移动的影响，数值越大，表示地形越复杂，越需要对机器人进行参数调整，使其行走运行平稳。

其中单位平均坡度θ_mean，表示地面平均的倾斜角度，其数值越大，坡度越大，机器人的运动越困难，影响地面影响机器人在行走过程中的稳定性和移动效率，因此单位平均坡度θ_mean与地形起伏度ECI成正比，通过计算平均坡度与基准坡度的差异，反映了地面倾斜对移动的影响。分母中的1+θ_mean项则用于归一化坡度值，避免坡度过大导致的极端值影响。

最大高程差H_max，表示地形中最高点与最低点之间的高度差，其值越大表示说明地形中的高点和低点之间的垂直差距较大，地形更加不平坦，起伏更明显。影响机器人在高低起伏地形上的移动，对移动的困难程度显著增加，尤其是对于机器人而言，行走过程中需要克服较大的高程变化。因此与地形起伏度ECI成正比。最大高程差的平方根形式用于反映高低起伏对地形复杂度的影响。相较于线性关系，平方根形式有助于减小高程差较大时的影响程度。

地表平均粗糙度Ra_d描述地表的光滑程度，当地形的起伏度增加且表面较为粗糙时，机器人在行走时面临的挑战将会显著增大，可能导致行驶不稳定、速度减慢和更高的能耗。因此，在路径规划和操作策略中，需要同时考虑这两个因素，以确保机器人的运动效率和安全性。地表平均粗糙度的自然对数形式ln(1+Ra_d)用于处理粗糙度对运动的影响，允许较小的粗糙度变化产生相对较小的影响，而较大的粗糙度变化时，影响将显著增加。

最大高程差H_max是地形起伏度的一个关键指标，因为它直接影响机器人的移动能力。较大的高程差意味着机器人在行驶时需要克服更大的重力势能变化，因此对其影响最大。平均坡度θ_mean也很重要，但通常情况下，坡度对机器人的行驶稳定性影响较小于高程差，地表平均粗糙度Ra_d对机器人的影响相对较小，主要影响摩擦力。尽管粗糙度会影响行驶的效率和稳定性，但其影响通常低于坡度和高程差，因此设置其中ω₂≥ω₁>ω₃，且ω₁、ω₂和ω_s均大于0。同时ω₁、ω₂和ω₃一般小于1。

其中地形数据具体的获取方法为：在扫描区域内，机器人通过激光雷达或立体相机不断采集地面高度信息，生成地形的三维点云数据，对获取的点云数据进行处理，提取地面点云，从处理后的点云中提取最高点和最低点，以计算最大高程差。分析地面点云的高度变化，计算平均粗糙度，具体为计算每个点与其邻域内点的高度差，取这些高度差的绝对值，求其平均值作为粗糙度指标。

步骤5：设置地形起伏判断阈值，将目的行走路径区域的地形起伏度与地形起伏判断阈值相对比，若超过地形起伏判断阈值，动态调整菠萝采摘机器人的悬挂行程和履带宽度，直至行走到达当前采摘目标位置处进行采摘。

其中设置地形起伏判断阈值，将目的行走路径区域的地形起伏度与地形起伏判断阈值相对比，若当前检测路段的地形起伏度小于等于地形起伏判断阈值时，不对菠萝采摘机器人的悬挂行程和履带宽度进行调整，表示使用当前的悬挂行程和履带宽度可以完成采摘工作。

基于地形起伏度对悬挂行程和履带宽度进行调整，其中，基于地形起伏度对悬挂行程进行动态修正所依据的公式为：

式中，SR表示菠萝采摘机器人动态调整后的悬挂行程，SR₀为悬挂行程初始值，α为悬挂行程对地形变化的敏感度系数；

需要说明的是悬挂系统在机器人中的主要作用是吸收地面不平带来的冲击和振动，保持机器人的稳定性和舒适性。悬挂行程的长度直接影响到机器人的通过能力和地形适应性，在较大起伏的地形中，机器人在行驶时会经历更大的冲击和振动。增加悬挂行程可以帮助机器人更有效地吸收这些冲击，降低对机器人结构的损伤风险，提高其在复杂地形中的稳定性。

通过反映地形起伏度对悬挂行程的影响。随着ECI的增加，地形的复杂性和起伏度也增加，因此需要更大的悬挂行程来适应这些变化。使用平方根形式使得悬挂行程的增加与地形起伏度之间的关系呈现非线性，反映了在较高起伏度的情况下，悬挂行程的需求增长会更为显著。这种非线性关系更符合实际情况，因为在较为复杂的地形中，额外的悬挂行程对稳定性的贡献更为重要。

α是一个调节参数，表示悬挂系统对地形变化的敏感性。通过调整α，可以根据不同的地形特性和机器人设计需求，灵活地控制悬挂行程的变化，一般α取0.01至0.05之间。

其中，基于地形起伏度对履带宽度进行动态修正所依据的公式为：

SK＝SK₀*[1+β*ECI^2/3]

式中，SK为菠萝采摘机器人动态调整后的履带宽度，SK₀为履带宽度初始值，β为履带宽度对地形变化的敏感度系数。

需要说明的是，在较大起伏的地形中，机器人在行驶时可能会经历更多的侧向倾斜和上下振动。增加履带宽度可以有效增加与地面的接触面积，从而提高机器人的稳定性，减少翻覆的风险，在起伏较大的地形中，履带宽度的增加可以显著提高机器人的抓地力，避免在松软或不平坦的地面上打滑。宽履带可以更好地适应地形的变化，确保机器人在各种条件下的顺利行驶。因此履带宽度的调整幅度与地形起伏度成正比。使用ECI^2/3来表示履带宽度的增加相对于地形起伏度的需求关系，确保在较高的起伏度下，履带宽度的增加更加显著。

β是一个调节参数，表示履带宽度对地形变化的敏感性。通过调整β，可以根据不同的地形特性和机器人设计需求，灵活地控制履带宽度的变化。一般β取0.01至0.1之间。

其中表1显示的是根据地形起伏度进行调节后，悬挂行程和履带宽度的部分统计数据。

表1：悬挂行程和履带宽度部分调节数据

由数据可以看出，随着区域地形起伏度的变化，调整后的悬挂行程和履带宽度的设计也相应发生了调整，以优化设备在不同地形条件下的表现。例如，从区域1到区域5，虽然初始悬挂行程和履带宽度保持不变，但随着区域起伏度的增加，调整后的悬挂行程和履带宽度也有相应的增大。这一现象表明，在更高地形起伏度的区域，设备需要更大的悬挂行程和履带宽度来保持稳定性和适应性。区域5的起伏度为3.00，调整后的履带宽度为0.26，显示在此条件下设备性能得到了有效改善，能够更好地适应复杂地形，在振动加速度的表现上，随着区域起伏度的变化，振动加速度减小量也展现出不同的趋势。区域3的起伏度为2.50，调整后平均振动加速度减小量达到0.15，表明该区域的设计优化能够有效减少振动，提升操作的平稳性。

请参阅图8，本发明还提供一种菠萝采摘机器人的行走控制系统，所述一种菠萝采摘机器人的行走控制系统用于执行上述一种菠萝采摘机器人的行走控制方法，包括：

移动路径规划模块，用于获取待采集菠萝地的菠萝植株空间分布数据，基于菠萝植株空间分布数据确定待采集菠萝地内菠萝植株的空间分布位置，确定整个待采集菠萝地内各区域的空间分布密度，采用密度导向方法规划可移动区域；

采摘目标确定模块，用于获取目标菠萝的特征信息，基于目标菠萝的特征信息判断当前目标菠萝是否满足采摘辨识标准，若满足将目标菠萝标记为采摘目标并执行步骤S3，否则更换目标菠萝返回执行步骤S2；

采摘范围捕获模块，用于获取菠萝采摘机器人采摘装置的最大采摘半径，基于最大采摘半径确定采摘领域范围，检测当前采摘目标是否处于采摘领域范围内，若是则对当前采摘目标进行采摘，否则执行步骤S4；

路径地形检测模块，用于获取当前采摘目标和菠萝采摘机器人所在的地的空间位置，基于空间位置信息和可移动区域规划目的行走路径，通过激光雷达扫描菠萝采摘机器人目的行走路径的地形数据，生成高程网格地图，使用多维特征向量表征目的行走路径区域的地形特征数据，并基于多维特征向量定义目的行走路径的地形起伏度；

行走动态调整模块，用于设置地形起伏判断阈值，将目的行走路径区域的地形起伏度与地形起伏判断阈值相对比，若超过地形起伏判断阈值，动态调整菠萝采摘机器人的悬挂行程和履带宽度，直至行走到达当前采摘目标位置，进行采摘。

上述公式均是去量纲取其数值计算，公式是由采集大量数据进行软件模拟得到最近真实情况的一个公式，公式中的预设参数由本领域的技术人员根据实际情况进行设置。

上述实施例，可以全部或部分地通过软件、硬件、固件或其他任意组合来实现。当使用软件实现时，上述实施例可以全部或部分地以计算机程序产品的形式实现。本领域技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够通过电子硬件，或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方法来执行，取决于技术方案的特定应用和设计约束条件。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，既可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。

Claims (8)

1.一种菠萝采摘机器人的行走控制方法，其特征在于，具体步骤包括：

S1：获取待采集菠萝地的菠萝植株空间分布数据，基于菠萝植株空间分布数据确定待采集菠萝地内菠萝植株的空间分布位置，确定整个待采集菠萝地内各区域的空间分布密度，采用密度导向方法规划可移动区域；

S2：获取目标菠萝的特征信息，基于目标菠萝的特征信息判断当前目标菠萝是否满足采摘辨识标准，若满足将目标菠萝标记为采摘目标并执行步骤S3，否则更换目标菠萝返回执行步骤S2；

S3：获取菠萝采摘机器人采摘装置的最大采摘半径，基于最大采摘半径确定采摘领域范围，检测当前采摘目标是否处于采摘领域范围内，若是则对当前采摘目标进行采摘，否则执行步骤S4；

S4：获取当前采摘目标和菠萝采摘机器人所在的地的空间位置，基于空间位置信息和可移动区域规划目的行走路径，通过激光雷达扫描菠萝采摘机器人目的行走路径的地形数据，生成高程网格地图，使用多维特征向量表征目的行走路径区域的地形特征数据，并基于多维特征向量定义目的行走路径的地形起伏度；

S5：设置地形起伏判断阈值，将目的行走路径区域的地形起伏度与地形起伏判断阈值相对比，若超过地形起伏判断阈值，动态调整菠萝采摘机器人的悬挂行程和履带宽度，直至行走到达当前采摘目标位置处进行采摘。

2.根据权利要求1所述的一种菠萝采摘机器人的行走控制方法，其特征在于：获取待采集菠萝地的菠萝植株空间分布数据，其中空间分布数据具体指菠萝植株点云数据，基于菠萝植株空间分布数据确定待采集菠萝地内菠萝植株的空间分布位置具体所依据的逻辑为：通过激光扫描获取菠萝植株的三维点云数据，在待采集菠萝地内建立空间坐标系，以待采集菠萝地的几何中心为原点，以正东方向为x轴正方向，正北方向为y轴正方向，并通过右手定则确定z轴方向，在建立的坐标系中，记录菠萝植株点云数据坐标，对菠萝植株点云数据坐标进行滤波、分割和分类，以识别出菠萝植株的空间分布位置；

确定整个待采集菠萝地内各区域的空间分布密度，采用密度导向路径规划可移动区域具体所依据的逻辑为：基于菠萝植株点云数据分布密度，通过聚类算法对处理后的点云数据进行聚类，识别并标记出菠萝植株的集群，将待采集菠萝地区域内未被标记为菠萝植株集群且空间连续的区域作为可选取区域；

将可选取区域分割为若干网格子区域，检测网格子区域上的水文特征数据，根据水文特征数据识别可选取区域中的积水区，直至遍历全部网格子区域，将识别为积水区的网格子区域在可选取区域中进行标记，将未被标记的剩余网格子区域作为可移动区域，其中水文特征数据包括路面积水面积和最大积水深度，判断积水区具体所依据的逻辑为：设置积水判断阈值，将水文特征数据与积水判断阈值相对比，将超过积水判断阈值的网格子区域标记为积水区。

3.根据权利要求2所述的一种菠萝采摘机器人的行走控制方法，其特征在于：获取目标菠萝的特征信息，所述目标菠萝的特征信息包括菠萝轮廓圆形度、表面平均粗糙度、菠萝轮廓面积和果皮黄绿面积占比；

其中获取目标菠萝的特征信息具体的方法为：采集若干已知特征信息的菠萝植株图像作为样本图像，在样本图像中通过最小外接矩形标记出菠萝果实，形成人工标记，并将已知的特征信息在对应标记处进行标注，生成特征数据图像，将特征数据图像与原始样本图像一一映射，形成训练样本数据集；

基于训练样本数据集内的数据，建立神经网络模型，将训练样本数据集内的原始样本图像作为输入，并以对应的特征数据图像作为标签，对神经网络模型进行训练，完成训练的神经网络模型输入为待检测的菠萝植株图像，输出为识别出图像内菠萝目标并表征有特征信息的图像。

4.根据权利要求3所述的一种菠萝采摘机器人的行走控制方法，其特征在于：基于目标菠萝的特征信息判断当前目标菠萝是否满足采摘辨识标准，其中采摘辨识标准约束条件具体所依据的公式为：

式中，R_U为目标菠萝的菠萝轮廓圆形度，R_yz为轮廓圆形度阈值，Ra为目标菠萝的表面平均粗糙度，Ra_yz为平均粗糙度阈值，S_E为目标菠萝的菠萝轮廓面积，S_yz为菠萝轮廓面积阈值，BL为目标菠萝的果皮黄绿面积占比，BL_min为最小占比阈值，BL_max为最大占比阈值；

对于满足约束条件的目标菠萝，将其记为采摘目标。

5.根据权利要求4所述的一种菠萝采摘机器人的行走控制方法，其特征在于：获取菠萝采摘机器人采摘装置的最大采摘半径，其中所述最大采摘半径具体指菠萝采摘机器人采摘装置在水平方向和垂直方向的最大活动距离，根据水平方向和垂直方向的最大采摘半径形成矩形采摘领域范围，基于最大采摘半径确定的采摘领域范围，检测当前采摘目标是否处于采摘领域范围内，其中检测判断具体所依据的逻辑为：

检测当前采摘目标果实根部与秆茎连接处，在建立的空间坐标系中的坐标，生成检测点云集合，判断当前采摘目标检测点云集合所形成的领域是否全部处于采摘领域范围内，若是则对当前采摘目标进行采摘，若采摘领域范围没有全部覆盖所形成的领域，则控制菠萝采摘机器人进行移动。

6.根据权利要求5所述的一种菠萝采摘机器人的行走控制方法，其特征在于：通过激光雷达扫描菠萝采摘机器人前方目的行走路径的地形数据，生成高程网格地图，所述地形数据包括：最大高程差、单位平均坡度和地表平均粗糙度，其中扫描菠萝采摘机器人实时扫描前方5m内的目的行走路径，获取地形数据，其中单位平均坡度计算所依据的公式为：

式中，θ_mean为单位平均坡度，θ^m表示在菠萝采摘机器人前方5m内目的行走路径中，第m个高程网格单元内中心点的单元坡度，M为在菠萝采摘机器人前方5m内目的行走路径中，高程网格单元总个数，m为在菠萝采摘机器人前方5m内目的行走路径中，高程网格单元的索引，m∈[1,2,…,M]；

式中，h(i,j)_m为在菠萝采摘机器人前方5m内目的行走路径中，第m个高程网格单元内中心点(i,j)的海拔高度，表示沿x方向高程梯度，表示沿y方向高程梯度；

基于多维特征向量定义目的行走路径的地形起伏度，其中地形起伏度计算所依据的公式为：

式中，ECI为在菠萝采摘机器人前方5m内目的行走路径的地形起伏度，H_max为在菠萝采摘机器人前方5m内目的行走路径的最大高程差，Ra_d为在菠萝采摘机器人前方5m内目的行走路径的地表平均粗糙度，ω₁、ω₂和ω₃分别为单位平均坡度、最大高程差和地表平均粗糙度的权重系数，其中ω₂≥ω₁>ω₃，且ω₁、ω₂和ω₃均大于0。

7.根据权利要求6所述的一种菠萝采摘机器人的行走控制方法，其特征在于：动态调整菠萝采摘机器人的悬挂行程和履带宽度具体所依据的逻辑为：基于地形起伏度对悬挂行程和履带宽度进行调整，其中，基于地形起伏度对悬挂行程进行动态修正所依据的公式为：

式中，SR表示菠萝采摘机器人动态调整后的悬挂行程，SR₀为悬挂行程初始值，α为悬挂行程对地形变化的敏感度系数；

其中，基于地形起伏度对履带宽度进行动态修正所依据的公式为：

SK＝SK₀*[1+β*ECI^2/3]

式中，SK为菠萝采摘机器人动态调整后的履带宽度，SK₀为履带宽度初始值，β为履带宽度对地形变化的敏感度系数。

8.一种菠萝采摘机器人的行走控制系统，其特征在于：所述一种菠萝采摘机器人的行走控制系统用于执行权利要求1-7任一项所述的一种菠萝采摘机器人的行走控制方法，包括：

移动路径规划模块，用于获取待采集菠萝地的菠萝植株空间分布数据，基于菠萝植株空间分布数据确定待采集菠萝地内菠萝植株的空间分布位置，确定整个待采集菠萝地内各区域的空间分布密度，采用密度导向方法规划可移动区域；

采摘目标确定模块，用于获取目标菠萝的特征信息，基于目标菠萝的特征信息判断当前目标菠萝是否满足采摘辨识标准，若满足将目标菠萝标记为采摘目标并执行步骤S3，否则更换目标菠萝返回执行步骤S2；

采摘范围捕获模块，用于获取菠萝采摘机器人采摘装置的最大采摘半径，基于最大采摘半径确定采摘领域范围，检测当前采摘目标是否处于采摘领域范围内，若是则对当前采摘目标进行采摘，否则执行步骤S4；

路径地形检测模块，用于获取当前采摘目标和菠萝采摘机器人所在的地的空间位置，基于空间位置信息和可移动区域规划目的行走路径，通过激光雷达扫描菠萝采摘机器人目的行走路径的地形数据，生成高程网格地图，使用多维特征向量表征目的行走路径区域的地形特征数据，并基于多维特征向量定义目的行走路径的地形起伏度；

行走动态调整模块，用于设置地形起伏判断阈值，将目的行走路径区域的地形起伏度与地形起伏判断阈值相对比，若超过地形起伏判断阈值，动态调整菠萝采摘机器人的悬挂行程和履带宽度，直至行走到达当前采摘目标位置，进行采摘。