一种采摘机器人机械臂抓取避障方向确定的方法及其系统

1.本发明属于农业水果收获机器人视觉技术领域，特别涉及一种采摘机器人机械臂抓取避障方向确定的方法及其系统。


背景技术：

2.设施番茄株形高大，番茄产量高，可长季栽植，周期长和产量大，反季节生产，可适应恶劣环境，受病虫害影响小，品质易保证等，全球年总产量达1.7亿吨，其中设施番茄愈发受到重视，我国产量6483.2万吨，其中设施番茄面积64.2万公顷。设施采摘存在用工难、用工贵、劳动强度大等问题，使用智能化设备替代人工采摘，提高视觉系统在整个采摘过程中的效率，可以提高机器人采摘效率。
3.番茄采摘机器人在进行果实抓取作业时，容易因枝叶、藤条及大棚设施等障碍影响采摘效率，导致机械臂不能正常抓取，严重时甚至损坏末端执行器和机械臂。目前提出的方法多归结于机械臂的路径规划问题，且多用于工业生产制造，农业水果采摘方面少有涉及。
4.机械手抓取的精度是农业采摘机器人采摘效率的一个重要指标，直接影响到采摘的成功率。目前采摘机器人的视觉系统多只为采摘机械臂提供空间三维坐标，少有对机械臂抓取时的避障问题的研究，部分研究者将其归结为农艺问题，所以所设计的视觉系统未能有很好的普适性。


技术实现要素：

5.针对上述技术问题，本发明提供一种采摘机器人机械臂抓取避障方向确定的方法及其系统，从视觉定位下手，传输给机械臂控制系统的不只是三维坐标，还包括传输机械臂接近路径姿势的信息，包括果实二维中心定位和三维点云向量计算，二维中心定位部分为三维点云向量计算部分做中心定位基础，三维点云向量计算部分是二维中心定位部分延申和进一步定位，获取点云向量，所述点云向量的方向作为机械臂采摘果实时的抓取姿势，通过点云向量控制机械臂进行避障抓取，解决机械臂抓取果实时由于被障碍物遮挡而不能有效抓取的问题。
6.本发明的技术方案是：一种采摘机器人机械臂抓取避障方向确定的方法，包括以下步骤：
7.二维中心定位：制作果实数据集，建立图像分割模型，将采集的果实二维rgb图像输入到图像分割模型，基于颜色对果实的可见部分进行分割，在分割果实区域和障碍物区域之后，利用边缘检测算法检测边界，从两个区域中搜索果实的弧形边界和公共边界像素，最终只显示果实的弧形边界和公共边界，假设果实可见部分的弧形边界为圆弧，计算得到果实可见弧形边界的弧高h和弧宽w，进而计算得到目标果实的二维中心；
8.三维点云向量计算：获取果实的点云，通过三维重建技术将目标果实重建为一个球体，对果实点云进行处理，找出果实的几何中心和可见表面中心，结合目标果实的二维中
心计算得到目标果实的中心点云三维坐标和可见表面中心点云三维坐标，从而得到目标果实的几何中心点和可见表面中心点连线的点云向量，所述点云向量的方向作为机械臂采摘果实时的抓取姿势，通过所述点云向量控制机械臂进行避障抓取。
9.上述方案中，所述二维中心定位具体包括以下步骤：
10.步骤s1、果实数据集的制作：采集果实的图像，对所采集的图像进行增强处理、打标签后训练得到图像分割模型；
11.步骤s2、将采集的果实二维rgb图像输入到图像分割模型；
12.步骤s3、基于果实及其背景的颜色差异对果实二维rgb图像进行分割，提取目标果实的可见部分；
13.步骤s4、利用边缘检测算法提取目标果实与果实二维rgb图像背景间的公共边界以及目标果实的弧形边界；
14.步骤s5、设目标果实可见部分为圆弧，计算出目标果实可见部分弧形边界的弧高h和弧宽w；
15.步骤s6、利用几何关系计算出目标果实的半径；
16.步骤s7、利用几何关系计算出目标果实的二维中心。
17.进一步的，所述步骤s1中果实的图像采集包括以下步骤：
18.数据采集时从不同的时间段、不同的光照条件和不同角度对每个果实拍摄多组图片，采用改变图像的光照强弱、对图片进行翻转处理、对图片进行旋转处理、对图片进行随机剪切处理对所采集的图像进行增强。
19.进一步的，所述不同的时间段为一天从早到晚多个时段；所述不同的光照条件包括正对阳光、背对阳光、阴天、晴天。
20.上述方案中，述步骤s1中将被遮挡部分超过70％的目标果实不做打标签处理。
21.上述方案中，所述三维点云向量计算具体包括以下步骤：
22.步骤s1)、使用深度相机获取果实点云，通过三维重建技术使用获取的果实点云集合将目标果实重建为一个球体；
23.步骤s2)、将深度相机获取的三维点云进行处理，找出目标果实的几何中心和可见表面中心；
24.步骤s3)、将目标果实的几何中心的点云结合目标果实的二维中心计算得到目标果实的中心点云三维坐标；
25.步骤s4)、将目标果实的可见表面中心的点云结合目标果实的二维中心计算得到目标果实可见表面中心点云三维坐标；
26.步骤s5)、根据步骤s3)得到的目标果实的中心点云三维坐标和步骤s4)得到的目标果实可见表面中心点云三维坐标计算得到目标果实的几何中心点和可见表面中心点连线的点云向量；
27.步骤s6)、将步骤s5)得到的点云向量输入机械臂控制系统，所述点云向量的方向作为机械臂采摘果实时的抓取姿势，控制机械臂末端执行器沿点云向量方向进行抓取，避开障碍物。
28.上述方案中，所述机械臂采摘果实时的抓取姿势通过以下公式表示：
29.抓取姿势
30.使用参数列表[x，y，z，θ,φ]来表示果实的抓取姿势，其中(x，y，z)是机械臂末端执行器的三维坐标，θ为机械臂末端执行器绕z轴旋转的角度，φ为机械臂末端执行器绕y轴旋转的角度。
[0031]
上述方案中，所述深度相机为：kinect相机。
[0032]
上述方案中，所述三维点云向量计算中采用pointnet网络对番茄点云进行处理。
[0033]
一种实现所述采摘机器人机械臂抓取避障方向确定方法的系统，包括二维中心定位模块和三维点云向量计算模块；
[0034]
所述二维中心定位模块用于制作果实数据集，建立图像分割模型，将采集的果实二维rgb图像输入到图像分割模型，基于颜色对果实的可见部分进行分割，在分割果实区域和障碍物区域之后，利用边缘检测算法检测边界，从两个区域中搜索果实的弧形边界和公共边界像素，最终只显示果实的弧形边界和公共边界，假设果实可见部分的弧形边界为圆弧，计算得到果实可见弧形边界的弧高h和弧宽w，进而计算得到目标果实的二维中心；
[0035]
所述三维点云向量计算模块用于获取果实的点云，通过三维重建技术将目标果实重建为一个球体，对果实点云进行处理，找出果实的几何中心和可见表面中心，结合目标果实的二维中心计算得到目标果实的中心点云三维坐标和可见表面中心点云三维坐标，从而得到目标果实的几何中心点和可见表面中心点连线的点云向量，所述点云向量的方向作为机械臂采摘果实时的抓取姿势，通过所述点云向量控制机械臂进行避障抓取。
[0036]
与现有技术相比，本发明的有益效果是：
[0037]
1.本发明针对目前果实采摘机器人机械臂避障问题，解决了机械臂抓取番茄时由于被障碍物遮挡而不能有效抓取等问题。
[0038]
2.本发明包括果实二维中心定位和三维点云向量计算，二维中心定位部分为三维点云向量计算部分做中心定位基础，三维点云向量计算部分是二维中心定位部分延申和进一步定位，获取点云向量从而引导机械臂避障抓取。
附图说明
[0039]
图1是本发明一实施方式的方法流程图；
[0040]
图2是本发明一实施方式的数据集采集方法角度前视图；
[0041]
图3是本发明一实施方式的数据集采集方法角度俯视示意图；
[0042]
图4是本发明一实施方式的被遮挡番茄可见部分轮廓示意图；
[0043]
图5是本发明一实施方式的被遮挡番茄近似圆弦关系示意图；
[0044]
图6是本发明一实施方式的被遮挡番茄近似圆几何关系示意图；
[0045]
图7是本发明一实施方式的机械臂抓取姿势表示示意图，其中图7a为绕z轴旋转θ角度后的姿势示意图，图7b为绕y轴旋转角度后的姿势示意图，图7c为完成图7a和图7b旋转后姿势示意图。
具体实施方式
[0046]
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
[0047]
实施例1
[0048]
图1所示为所述采摘机器人机械臂抓取避障方向确定方法的一种较佳实施方式，本实施例以番茄为例，但不仅限于番茄，所述采摘机器人机械臂抓取避障方向确定的方法，包括以下步骤：
[0049]
二维中心定位：制作番茄数据集，建立图像分割模型，将采集的番茄二维rgb图像输入到图像分割模型，基于颜色对番茄的可见部分进行分割，在分割番茄区域和障碍物区域之后，利用边缘检测算法检测边界，从两个区域中搜索番茄的弧形边界和公共边界像素，最终只显示番茄的弧形边界和公共边界，假设番茄可见部分的弧形边界为圆弧，计算得到番茄可见弧形边界的弧高h和弧宽w，进而利用圆的几何关系和公式计算得到目标番茄的二维中心；
[0050]
三维点云向量计算：获取番茄的点云，通过三维重建技术将目标番茄重建为一个球体，对番茄点云进行处理，找出番茄的几何中心和可见表面中心，结合目标番茄的二维中心计算得到目标番茄的中心点云三维坐标和可见表面中心点云三维坐标，从而得到目标番茄的几何中心点和可见表面中心点连线的点云向量，所述点云向量的方向作为机械臂采摘番茄时的抓取姿势，通过所述点云向量控制机械臂进行避障抓取。
[0051]
根据本实施例，优选的，所述二维中心定位具体包括以下步骤：
[0052]
步骤s1、番茄数据集的制作：采集番茄的图像，对所采集的图像进行增强处理、打标签后训练得到图像分割模型；
[0053]
具体的：采用工具为kinect v2深度相机和计算机，数据集采时从各个不同角度如图2和3所示，对每个番茄拍摄8张为一组的图片，从一天从早到晚多个时段各拍摄图片，挑选正对阳光、背对阳光、阴天、大阳光等情况来拍摄图片，可以将一个番茄拍全面，可以获取更为丰富全面的目标果实图像数据信息，从而训练出鲁棒性更强的分割模型，也可用于其他水果类采摘数据集的采集，对数据集进行增强处理，采用改变图像的光照强弱、对图片进行翻转处理、对图片进行旋转处理、对图片进行随机剪切处理等方法对所采集的图像进行增强，进一步提高图像分割模型的鲁棒性，采用labelimg标签工具进行打标签，标注时将半熟和全熟的番茄果实标记为“red”，绿色未熟番茄果实标为“green”，其中果实遮挡部分超过约70％的番茄果实不做打标签处理。
[0054]
步骤s2、将采集的番茄二维rgb图像输入到图像分割模型；
[0055]
步骤s3、基于番茄及其背景的颜色差异对番茄二维rgb图像进行分割，利用视觉算法首先去除rgb颜色空间中的背景，然后结合rgb、hsi和yiq空间提取成熟的目标番茄可见部分；
[0056]
步骤s4、提取出被遮挡番茄的可见部分，利用边缘检测算法提取目标番茄与番茄二维rgb图像背景间的公共边界以及目标番茄的弧形边界；
[0057]
步骤s5、设目标番茄可见部分为圆弧，计算出目标番茄可见部分弧形边界的弧高h和弧宽w；
[0058]
具体的：
[0059]
从输入的rgb彩色图像中，基于颜色对番茄的可见部分进行分割，在分割番茄区域和叶子区域之后，利用边缘检测算法检测边界，从两个区域中搜索番茄的弧形边界和公共边界像素，最终只显示番茄的弧形边界和公共边界如图4所示，利用相关公式可以计算得到番茄可见弧的宽度w和高度h。
[0060]
其中，番茄二维rgb图像可见部分弧宽图像分割后的番茄二维rgb图像的可见部分弧形边界，成熟番茄与背景的公共边界，两边界交点分别为p1,p2，两交点连线中点pm(xm，ym)，其中xm＝(x1+x2)/2，ym＝(y1+y2)/2，随机向量)/2，随机向量长度pm p1即随机向量的模长由几何关系可得，番茄二维rgb图像可见部分弧高其中，x1为交点p1横坐标，y1为交点p1纵坐标，x2为交点p2横坐标，y2为交点p2纵坐标，xm为交点pm横坐标，ym为交点pm纵坐标，x
p
为交点pp横坐标，y
p
为交点pp纵坐标，i为正整数，i＝1,2,3,
…
,n。
[0061]
步骤s6、利用几何关系计算出目标番茄的半径；
[0062]
如图5和6所示，成熟番茄半径计算时，确定弧高h和弧宽w后，它的半径可以用圆弧半径方程求出。如果圆中的两根弦互相垂直相交，则满足关系其中半弦长:根据这一特性，即可根据半径公式ra＝(h+l)/2计算出番茄果实的半径。
[0063]
步骤s7、利用几何关系计算出目标番茄的二维中心。
[0064]
在计算成熟番茄二维中心时，圆弧如图4所示的中心(pc)是给出θ＝90
°
的坐标点，假设图中向量且根据相似原理可得出成熟番茄中心pc(xc，yc)。
[0065]
番茄二维中心定位采用简单的几何理论对复杂的采摘环境进行计算，计算简单，有效简化中心计算步骤，计算精度高。从输入的rgb彩色图像中，基于颜色对番茄的可见部分进行分割，在分割番茄区域和障碍物区域之后，利用边缘检测算法检测边界，从两个区域中搜索番茄的弧形边界和公共边界像素，最终只显示番茄的弧形边界和公共边界，假设番茄可见部分的弧形边界为圆弧，则可利用相关公式计算得到番茄可见弧形边界的宽度和高度，进而利用圆的几何关系和公式计算得到被遮挡番茄的中心。
[0066]
根据本实施例，优选的，所述三维点云向量计算具体包括以下步骤：
[0067]
步骤s1)、使用kinect v2深度相机获取番茄点云，通过三维重建技术使用获取的番茄点云集合将目标番茄重建为一个球体，其中，对被遮挡部分超过70％的目标番茄自动忽略；
[0068]
步骤s2)、将kinect v2深度相机获取的三维点云采用pointnet网络进行处理，找出目标番茄的几何中心和可见表面中心；
[0069]
步骤s3)、将目标番茄的几何中心的点云结合目标番茄的二维中心计算得到目标番茄的不可见的中心点云三维坐标；
[0070]
步骤s4)、将目标番茄的可见表面中心的点云结合目标番茄的二维中心计算得到目标番茄可见表面中心点云三维坐标；
[0071]
步骤s5)、根据步骤s3)得到的目标番茄的中心点云三维坐标和步骤s4)得到的目标番茄可见表面中心点云三维坐标计算得到目标番茄的几何中心点和可见表面中心点连线的点云向量；
[0072]
步骤s6)、将步骤s5)得到的点云向量输入机械臂控制系统，所述点云向量的方向作为机械臂采摘番茄时的抓取姿势，控制机械臂末端执行器沿点云向量方向进行抓取，避开障碍物。
[0073]
番茄三维点云向量计算部分中，通过三维重建技术，番茄可以被重建为一个球体，通过点云向量引导机械臂进行避障抓取，有效避免机械臂及末端执行器受损。自然环境下，从rgb-d相机视角下看到的番茄是被遮挡状态的，因此，rgb-d视角下番茄可见部分正好可以引导机械手进行抓取。运用pointnet网络对番茄点云进行处理，找出番茄的几何中心和番茄的可见表面中心，得到番茄的几何中心点和番茄可见表面中心点连线的向量，则当前视角下该向量可作为机械臂采摘番茄时的接近姿势。
[0074]
在机械臂抓取时，机械臂的抓取姿势可表示为如下，三维空间中番茄果实的姿势具有6个自由度，包括三个位置和三个旋转采用欧拉角来表示抓取姿势的方向，假设蕃茄果实不会沿其茎部方向x轴旋转，因此将ψ的值设置为零如图7所示，其中图7a为绕z轴旋转θ角度后的姿势表示，图7b为绕y轴旋转角度后的姿势表示，图7c为完成图7a和图7b旋转后姿势表示，
[0075]
所述机械臂采摘番茄时的抓取姿势通过以下公式表示：
[0076]
抓取姿势
[0077]
使用参数列表[x，y，z，θ,φ]来表示番茄的抓取姿势，其中(x，y，z)是机械臂末端执行器的三维坐标，θ为机械臂末端执行器绕z轴旋转的角度，φ为机械臂末端执行器绕y轴旋转的角度。
[0078]
根据本实施例，优选的，所述深度相机为：kinect相机。采用的kinect相机是基于tof方式对图像信息进行获取，不同于传统双目视觉，该方法受光照影响较小，可以适用于不同光照条件下温室番茄的采摘，普适性强。
[0079]
番茄三维点云向量计算通过三维重建技术将成熟番茄重建为一个球体，通过点云向量引导机械臂进行避障抓取，有效避免机械臂及末端执行器受损。
[0080]
本实施例针对目前温室番茄采摘机器人机械臂避障问题，解决了机械臂抓取番茄时由于被障碍物遮挡而不能有效抓取等问题。本发明包括番茄二维中心定位和番茄三维点云向量计算，番茄二维中心定位部分为番茄三维点云向量计算部分做中心定位基础，番茄三维点云向量计算部分是番茄二维中心定位部分延申和进一步定位，获取点云向量从而引导机械臂避障抓取。本发明可适用于不同光照条件下温室番茄的采摘，且提出的数据集采集方案可用于其他水果类采摘数据集的采集，对水果采摘领域做出贡献。
[0081]
实施例2
[0082]
一种实现实施例1所述采摘机器人机械臂抓取避障方向确定方法的系统，包括二维中心定位模块和三维点云向量计算模块；
[0083]
所述二维中心定位模块用于制作番茄数据集，建立图像分割模型，将采集的番茄
二维rgb图像输入到图像分割模型，基于颜色对番茄的可见部分进行分割，在分割番茄区域和障碍物区域之后，利用边缘检测算法检测边界，从两个区域中搜索番茄的弧形边界和公共边界像素，最终只显示番茄的弧形边界和公共边界，假设番茄可见部分的弧形边界为圆弧，计算得到番茄可见弧形边界的弧高h和弧宽w，进而利用圆的几何关系和公式计算得到目标番茄的二维中心；
[0084]
所述三维点云向量计算模块用于获取番茄的点云，通过三维重建技术将目标番茄重建为一个球体，对番茄点云进行处理，找出番茄的几何中心和可见表面中心，结合目标番茄的二维中心计算得到目标番茄的中心点云三维坐标和可见表面中心点云三维坐标，从而得到目标番茄的几何中心点和可见表面中心点连线的点云向量，所述点云向量的方向作为机械臂采摘番茄时的抓取姿势，通过所述点云向量控制机械臂进行避障抓取。
[0085]
本发明将视觉和机械臂运动控制结合在一起，基于三维点云空间向量提出一种机械臂抓取果实的末端执行器接近姿势，对解决果实采摘机器人机械臂在抓取被遮挡果实时机械臂避障问题具有重要的应用价值。
[0086]
本实施例应用了具体采摘对象番茄对本发明的原理及实施方式进行了阐述，同时，对于本领域的一般技术人员，在不改变本发明核心思想和实施方法的实质的情况下，本领域技术人员参照本发明做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。