一种猕猴桃采摘的果轴姿态估计、预分级与抓取位姿求解方法

本发明涉及农业机器人，具体涉及一种基于rgb-d视觉的猕猴桃果轴姿态估计、赤道直径预分级及抓取位姿求解的方法。

背景技术：

1、在猕猴桃机械化采摘作业中，果实位姿的在线感知是实现无损抓取与采端预分级的前提。果轴方向（果萼至果顶的连线方向）作为果实最主要的几何基准，直接影响夹爪进给姿态的规划与抓取成功率。当前，果实关键点定位主要依赖基于深度学习的热图回归方法（如yolo-pose、hrnet），该类方法需对大量图像人工标注果萼与果顶位置，标注成本高、耗时长，且模型在不同品种或生长环境下的泛化能力受限。此外，现有采摘机器人视觉系统多采用检测框中心或点云主成分分析法近似估计果实姿态，缺乏对果萼、果顶等具有明确生物学语义的关键点的显式定位，导致抓取方向与果实实际主轴之间存在偏差，易引发夹爪偏斜、果实滑脱或果梗撕裂等问题。在果实尺寸测量方面，现有方法通常将检测框的宽度或高度近似为果实直径，未充分利用实例分割掩码所提供的精细轮廓信息，难以准确获取赤道直径用于采端预分级与夹爪开度自适应控制。因此，亟需一种无需额外关键点标注、能够自动定位果萼与果顶语义关键点、并据此解算果轴方向与赤道尺寸的方法，以满足猕猴桃采摘机器人对实时性、鲁棒性与无损作业的需求。

技术实现思路

1、本发明提供了一种猕猴桃采摘的果轴姿态估计、预分级与抓取位姿求解方法，采用该方法无需额外关键点标注，能够在采摘瞬间同步完成果实姿态识别、采摘顺序规划、大小分级与最优抓取位姿求解，从而有效地解决了现有技术中出现的采摘与分级分离、抓取方向缺乏几何基准、关键点依赖人工标注等问题。

2、本发明采用了以下技术方案：

3、一种猕猴桃采摘的果轴姿态估计、预分级与抓取位姿求解方法，其特征在于，包括以下步骤：

4、步骤s1，果实粗定位与粗分类：

5、基于yolo目标检测网络对果实实现快速粗定位与遮挡状态分类，按照果实的空间遮挡状态完成检测目标的类别划分，具体分为无遮挡的独立果ka与被树叶、树枝、其他果实挡住的遮挡果kf两类，为多目标采摘顺序规划提供优先级判定依据；

6、步骤s2，果轴关键点自动定位与果轴方向解算：

7、通过实例分割模型获取果实二值掩码，对果实二值掩码进行预处理与轮廓提取，并进行椭圆拟合，根据椭圆长轴端点并结合果实自然生长姿态先验，自动定位果萼点与果顶点的二维像素坐标；结合深度图与相机内参，将所述、像素坐标变换至相机坐标系下，解算果轴单位方向向量；

8、步骤s3，基于空间聚类与类别深度优先的采摘顺序规划：

9、基于果实中心坐标进行空间聚类，将物理空间邻近的果实划分为同一采摘簇，针对各簇内部果实，并按“独立果优先、同类果实按深度由近及远”的规则生成簇内采摘顺序；

10、步骤s4，果实赤道直径预分级测算：

11、定义赤道中心为果轴中点与掩码几何中心在果轴直线上的正交投影点的加权融合，通过加权融合，可以保留实际观测信息的同时抑制因姿态倾斜或分割误差导致的中心偏移，提高测量的鲁棒性，以所述赤道中心为基准沿果轴法向量方向采样掩码边界，获取赤道两端点，分别计算投影直径与三维空间直径，将二者加权融合得到最终赤道直径，将作为猕猴桃果实直径和采端预分级依据，用于控制夹爪开度并实现果实按尺寸粗分拣；

12、步骤s5，抓取位姿求解与执行：

13、以赤道中心为抓取目标位置，以果轴方向为夹爪进给方向，通过枚举侯选滚转角生成候选抓取位姿集合，再通过逆运动学求解与综合代价函数评估，选取最优抓取位姿，经坐标变换链路转换至协作机械臂基坐标系，控制机械臂执行采摘动作。

14、进一步说，本发明步骤s2中预处理过程为利用训练完成的yolo11x-seg模型对输入的rgb图像进行推理，获得猕猴桃果实的原始二值分割掩码，采用高斯模糊对掩码进行平滑处理，消除掩码边缘的锯齿与孔洞，然后采用固定阈值进行二值化并对二值图像提取所有外部轮廓，选取面积最大的连通域，由此获得形态完整、边缘平滑且去除了微小噪声的掩码图像。

15、进一步说，本发明步骤s2中所述椭圆拟合采用fitzgibbon椭圆拟合，即直接最小二乘椭圆拟合算法对轮廓点集进行椭圆拟合，获取椭圆的几何参数，即中心坐标、半轴长度及旋转角，并通过校正逻辑确保长半轴与旋转角的一致性，所述校正逻辑为：若短半轴大于长半轴，则交换长短半轴并将旋转角增加90°，且对增加后的旋转角进行模180°处理。

16、进一步说，本发明步骤s2中所述果顶点的二维像素坐标判别规则为：比较椭圆长轴两个端点的纵坐标，将纵坐标较大的端点标记为果萼点，纵坐标较小的端点标记为果顶点，所述纵坐标较大的端点为果萼点m的判别规则，依赖于猕猴桃在目标采摘区域中以果萼端朝下、果顶端朝上的自然悬挂姿态生长的生物学先验；系统在采摘前通过手眼标定与相机位姿约束，保证图像y轴方向近似平行于重力方向，从而使所述纵坐标大小关系稳定对应果萼与果顶的语义。结合深度图与相机内参，将、的二维像素坐标变换至相机坐标系下，得到两者的三维空间坐标、，进而计算果轴单位方向向量，方向由指向，其中，所述深度图与rgb图通过时序对齐建立像素级对应关系，保证同一果实关键点、的像素坐标能取到同源深度值。

17、进一步说，本发明步骤s3中所述空间聚类采用k-means聚类算法，利用果实中心在相机坐标系下的、坐标进行k-means聚类，将物理空间中邻近的果实划分为同一采摘簇；针对各簇内部果实，按“独立果优先、同类果实按深度由近及远”的规则生成簇内采摘序列；对于不同簇之间，以包含独立果且距离相机较近的簇优先遍历。

18、进一步说，本发明步骤s4中定义赤道中心为果轴中点与掩码几何中心在果轴直线上的正交投影点的加权融合，其的计算公式为 ，

19、其中，由果萼点与果顶点的中点确定，代表了果实沿果轴方向的几何先验中心；为掩码几何中心在果轴直线上的正交投影，消除了垂直于果轴方向的观测偏移，为预设的加权系数，

20、在步骤s4中，以赤道中心为基准，沿果轴法向量方向向两侧延伸至掩码边界，得到赤道两端点、的像素坐标，计算像素跨度；结合相机等效焦距与赤道中心参考深度，通过相似三角形原理，计算物理直径

21、为相机等效焦距。

22、进一步说，本发明步骤s4中所述三维空间直径通过将赤道两端点的像素坐标变换至相机坐标系，计算两点间的三维欧氏距离得到，其方法为将、变换至相机坐标系，计算三维空间下两者的真实欧氏距离；将与进行加权融合得到最终赤道直径，将其作为猕猴桃果实直径（果实大小判断依据）和采端预分级依据，用于控制夹爪开度并实现果实按尺寸粗分拣。

23、进一步说，本发明步骤s5中所述候选滚转角在[-180°,180°]范围内以固定步长枚举，生成候选抓取位姿集合。

24、进一步说，本发明步骤s5中所述综合代价函数至少包括以下四项中的一项或多项组合：运动幅度代价、关节限位代价、运动奇异性代价、姿态偏好代价。

25、进一步说，本发明步骤s5中所述坐标变换链路包括猕猴桃坐标系{m}、相机坐标系{c}、法兰坐标系{t}、夹爪工具坐标系{g}与协作机械臂基坐标系{b}之间的依次变换。

26、本发明具有以下有益效果：采用了以上技术方案后，本发明无需额外关键点标注、能够在采摘瞬间同步完成果实姿态识别、采摘顺序规划、大小分级与最优抓取位姿求解的一体化方法，以解决现有技术中采摘与分级分离、抓取方向缺乏几何基准、关键点依赖人工标注等问题，本发明在实施过程中无需额外关键点标注数据，仅依赖实例分割掩码即可自动定位果萼点与果顶点，大幅降低数据准备成本，通过空间聚类与类别深度优先的采摘顺序规划，减少机械臂运动冗余，提高采摘效率。以果轴为进给方向，通过候选姿态枚举与代价函数优选，实现无损抓取，避免果实损伤。本发明通过实例分割掩码的椭圆拟合获取长轴端点，并结合猕猴桃自然悬挂姿态的生物学先验与相机位姿约束，将端点语义化为果萼点与果顶点，从而建立了具有明确物理意义的果轴方向向量，从根本上解决了抓取方向与果实主轴不对齐的问题。本发明首先通过果轴中点与掩码重心投影点的加权融合确定赤道中心，再沿果轴法向在掩码边界上采样得到赤道两端点，分别计算图像平面内的投影直径与三维空间中的真实欧氏距离，经交叉校验与加权融合后输出最终赤道直径，赤道中心加权融合与投影/三维直径交叉校验，实现高精度直径测算，例如平均绝对误差为0.85mm，支持采端预分级，本发明以“tcp与果实赤道中心点重合、夹爪进给轴与果轴方向共线”为硬约束，将抓取位姿的自由度缩减为绕进给轴的单一滚转角，通过枚举候选位姿、逆运动学求解与综合代价函数（运动幅度、关节限位、奇异性、姿态偏好）优选，输出最优抓取位姿。该方法在保证无损抓取的同时，兼顾了机械臂的运动效率与安全性。本发明在采摘瞬间同步完成赤道直径的测算与采端预分级，将尺寸信息同时用于夹爪开度控制与果实粗分拣，实现了“边采边分”的一体化作业，从流程上减少了二次搬运环节，有效保护了果实的外观品质。本发明基于同一套视觉感知链（果轴定向、赤道直径测算）与高精度定位平台，在采摘季执行采摘与采端预分级作业，在非采摘季可切换至生长巡检模式，沿预设路径对同一果丛进行重复扫描，记录各果实赤道直径随时间的变化数据，用于判断果实发育阶段与最佳采收期。该设计实现了采摘机器人的一机多用，显著提高了设备的全年利用率与农业数据采集能力。