一种面向机械臂采摘的莲蓬姿态判定方法

摘要: 本发明属于机器人采摘,特别是一种面向机械臂采摘的莲蓬姿态判定方法。背景技术:1、长期以来,莲蓬采摘主要依靠人工,效率低且受日渐凸显的劳动力短缺制约。机器人采摘是...
一种面向机械臂采摘的莲蓬姿态判定方法

本发明属于机器人采摘,特别是一种面向机械臂采摘的莲蓬姿态判定方法。

背景技术:

1、长期以来,莲蓬采摘主要依靠人工,效率低且受日渐凸显的劳动力短缺制约。机器人采摘是解决上述问题的技术途径,其中机械臂采摘是主要实施方式。若要实现莲蓬的机械臂采摘,先需计算荷梗采摘段(即荷梗上与莲蓬相连的短直段)上采摘点和采摘向量,为机械臂采摘执行提供采摘信息。然而,莲蓬结构特殊、朝向多变且个体间生长姿态往往不同。因此,处于不同姿态朝向的莲蓬,在相机视野中展现的特征可能存在较大差异,例如,有的姿态下荷梗采摘段能够完整展现,适合直接计算采摘信息;有的姿态下仅能够观测到莲蓬正面,而荷梗采摘段被莲蓬自身遮挡,此时无法计算采摘信息;有的姿态下能够观察到莲蓬背面,此时荷梗采摘段虽可见,但其姿态过于陡峭(其轴线与相机光轴方向之间的夹角较小,不便准确测量表面三维数据),而难以计算出准确的采摘信息。为保证不同姿态下莲蓬均能被机械臂采摘,需要对各类莲蓬姿态分类设计对应的采摘策略,而其前提是对莲蓬姿态类型进行准确判定。因此,亟需一种面向机械臂采摘的莲蓬姿态判定方法。

技术实现思路

1、为解决上述技术问题,本发明提供一种面向机械臂采摘的莲蓬姿态判定方法。

2、本发明采用的技术方案是:一种面向机械臂采摘的莲蓬姿态判定方法,包括以下步骤:

3、步骤1,建立莲蓬姿态感知系统,包括多轴机械臂、深度相机和采摘末端执行器;所述多轴机械臂上设置一基坐标系{r};所述深度相机安装于所述多轴机械臂末端的采摘末端执行器上,所述深度相机上设置一相机坐标系{c};

4、步骤2,使用莲蓬关键点检测模型对所述深度相机获取的莲蓬图像进行关键点检测并输出检测结果;所述检测结果包括目标检测框和关键点,所述目标检测框包含目标莲蓬及荷梗采摘段,所述荷梗采摘段是指与目标莲蓬相连接的荷梗短直段;所述关键点包括莲蓬轮廓处关键点、莲蓬连接处关键点、荷梗弯曲处关键点三类;所述莲蓬轮廓处关键点位于莲蓬蓬体边缘,所述莲蓬连接处关键点位于莲蓬与荷梗采摘段连接处,所述荷梗弯曲处关键点位于荷梗采摘段起始弯曲处;结合所述深度相机获取的深度数据,获得各关键点在相机坐标系{c}下的三维坐标;

5、步骤3,在相机坐标系{c}中定义位姿特征点和位姿特征向量,用于共同表征莲蓬位姿特征;其中,位姿特征点用于表征目标莲蓬及荷梗采摘段在相机坐标系{c}中的空间位置;位姿特征向量用于表征目标莲蓬及荷梗采摘段在相机坐标系{c}中的空间朝向;

6、步骤4,判断莲蓬的主导姿态,将所述检测结果中的莲蓬轮廓处关键点数量记为n,另设置莲蓬轮廓处关键点的数量阈值为nth,根据莲蓬轮廓处关键点数量n与所述数量阈值nth之间的关系作出以下判断:

7、当n < nth时,判定目标莲蓬处于荷梗主导姿态,此时,由莲蓬连接处关键点和荷梗弯曲处关键点确定位姿特征点和位姿特征向量;

8、当n ≥ nth时,判定目标莲蓬处于莲蓬主导姿态,此时,由莲蓬轮廓处关键点确定位姿特征点和位姿特征向量;

9、步骤5,进行坐标转换,将相机坐标系{c}下的位姿特征点与位姿特征向量以及相机坐标系{c}的zc轴单位向量转换至多轴机械臂的基坐标系{r}中;

10、步骤6,计算莲蓬朝向角,将位姿特征向量与所述zc轴单位向量投影至基坐标系{r}中的xr-yr平面,计算两者所在直线的夹角θ,定义夹角θ为莲蓬朝向角,所述夹角θ的取值范围为 0° ≤ θ ≤ 90°;

11、步骤7,判定莲蓬采摘姿态类型,设置一角度阈值θth,根据莲蓬朝向角θ与角度阈值θth的关系作出以下判断:

12、当θ > θth时,莲蓬以侧面对着深度相机,判定莲蓬为第一类采摘姿态类型,此时,适合多轴机械臂直接采摘;

13、当0° ≤ θ ≤ θth时,莲蓬以正面或背面对着深度相机,判定莲蓬为第二类采摘姿态类型,不适合多轴机械臂直接采摘。

14、作为本方案的进一步优化,所述莲蓬关键点检测模型为深度学习关键点检测模型。

15、作为本方案的进一步优化,所述莲蓬处于荷梗主导姿态时,所述位姿特征点为莲蓬连接处关键点和荷梗弯曲处关键点的中点,所述位姿特征向量为荷梗弯曲处关键点指向莲蓬连接处关键点形成的向量;所述莲蓬处于莲蓬主导姿态时,基于各莲蓬轮廓处关键点构建一空间拟合圆,将所述空间拟合圆圆心确定为位姿特征点,将所述空间拟合圆所在平面的法向量确定为位姿特征向量。

16、作为本方案的进一步优化,所述空间拟合圆采用基于平面约束的最小二乘空间圆拟合算法构建,包括以下步骤:

17、步骤a1,对各莲蓬轮廓处关键点进行空间平面拟合,采用最小二乘法使各莲蓬轮廓处关键点到所述拟合平面的距离平方和最小,并得到所述拟合平面的法向量,所述法向量指向莲蓬正面以外,所述莲蓬正面是指莲蓬蓬体远离荷梗连接处一侧的圆形平面,其上分布有莲子;

18、步骤a2,利用圆心位于所述拟合平面上的几何约束以及各莲蓬轮廓处关键点到圆心距离相等的条件,通过最小二乘法求解圆心坐标;

19、步骤a3,计算各莲蓬轮廓处关键点到圆心的平均距离作为所述空间拟合圆的半径。

20、作为本方案的进一步优化,所述步骤5根据相机坐标系{c}到基坐标系{r}的坐标转换关系进行坐标转换,所述坐标转换关系包括旋转关系和平移关系;所述位姿特征点进行旋转和平移变换,所述位姿特征向量及相机坐标系{c}的zc轴单位向量仅进行旋转变换。

21、作为本方案的进一步优化,所述步骤6包括以下步骤:

22、步骤b1,将经坐标转换后的位姿特征向量和所述zc轴单位向量分别正交投影至多轴机械臂的基坐标系{r}的xr-yr平面,并将所述xr-yr平面记为h平面;

23、步骤b2,取所述两个投影向量所在直线之间的夹角θ,并将所述夹角θ定义为莲蓬朝向角。

24、作为本方案的进一步优化,所述步骤2还包括对关键点的深度值进行补偿,包括以下步骤:

25、步骤c1,设第i个关键点的像素坐标对应的深度值为di,设置一深度阈值dt;当di =0或di > dt时,对所述第i个关键点的深度值进行补偿;

26、步骤c2,以所述第i个关键点的像素坐标为中心,构建逐级扩展的方形搜索窗口,第n层搜索窗口的边长为1+4n个像素,其中,n为搜索窗口的序号,n = 1, 2, 3;

27、步骤c3,按照从上到下、从左到右的顺序,依次读取当前搜索窗口内各像素点的深度值dk,其中,dk为搜索窗口内第k个像素点的深度值;当首次出现满足0 < dk ≤ dt条件的像素点时,将该像素点作为所述第i个关键点,并将该像素点的深度值dk作为所述第i个关键点的深度值;若当前搜索窗口内不存在满足所述条件的像素点,则进入下一层搜索窗口继续搜索;

28、步骤c4,当第3层搜索窗口内仍未读取到符合0 < dk ≤ dt条件的深度值时,所述第i个关键点不参与后续莲蓬位姿特征计算。

29、作为本方案的进一步优化,所述深度阈值dt为1000 mm。

30、作为本方案的进一步优化,所述莲蓬轮廓处关键点的数量阈值nth为8,所述角度阈值θth为60°。

31、作为本方案的进一步优化,所述多轴机械臂为串联机械臂,具体为六自由度协作机械臂、六自由度工业机械臂中的一种。

32、与现有技术相比,本发明的有益效果是:1)通过莲蓬轮廓处关键点数量与数量阈值的关系判断莲蓬主导姿态,能够在莲蓬姿态多变、特征差异大的情况下,有效区分荷梗主导姿态与莲蓬主导姿态,为后续位姿特征提取提供可靠依据,克服了单一视角下姿态判定困难的难题。2)针对不同主导姿态采用不同的位姿特征构建方式,提高了莲蓬位姿特征提取的适应性和准确性,能够更好地应对不同视角和自遮挡情况。3)通过计算莲蓬朝向角并结合角度阈值,将莲蓬划分为两类采摘姿态类型,为机械臂后续选择不同的采摘策略提供明确依据,有助于提高自动化采摘的决策效率和成功率。4)仅需单台深度相机即可实现莲蓬姿态判定,无需多相机系统,降低了硬件成本和系统复杂度,有利于实际部署和应用。