(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210705532.X (22)申请日 2022.06.21 (71)申请人 福建 （泉州） 哈工大工程 技术研究院 地址 362000 福建省泉州市丰泽区软件园9 号楼 (72)发明人 赵紫阳　李瑞峰　罗冠泰　张陈涛　 汤思榕　梁培栋　 (74)专利代理 机构 泉州君典专利代理事务所 (普通合伙) 35239 专利代理师 杜慧真 (51)Int.Cl. B25J 9/16(2006.01) G06T 7/11(2017.01) G06T 7/33(2017.01) G06T 7/80(2017.01)G06T 19/00(2011.01) G06V 10/25(2022.01) G06V 10/762(2022.01) G06V 10/764(2022.01) (54)发明名称 一种基于 混叠工件的机械臂抓取方法、 装置 及可读介质 (57)摘要 本发明公开了一种基于混叠工件的机械臂 抓取方法、 装置及可读介质， 属于智能制造 领域， 包含机械臂手眼标定步骤、 三维模板设置步骤、 三维点云预处理步骤、 全局多目标三维点云配准 算法和抓取决策算法， 实现多工件混叠场景下的 无碰撞抓取， 提高了计算效率。 该方法通过点云 预处理提高算法计算效率和稳定性； 通过全局多 目标三维点 云配准算法， 同时获得多工件混叠场 景下多个工件的位姿信息； 通过抓取决策算法， 对已获得的多个工件进行抓取目标筛选， 保证了 最优无碰撞抓取， 解决了工件弱纹理、 混叠复杂 工件识别与定位的难度大的问题。 权利要求书3页 说明书10页 附图4页 CN 115284279 A 2022.11.04 CN 115284279 A 1.一种基于混叠 工件的机 械臂抓取 方法， 其特 征在于， 包括以下步骤： S1， 采用手眼标定 获取眼在手外的手眼关系矩阵 并构建模板工件的三维模型， 获取 模板工件的至少一个抓取姿态 所述抓取姿态为 不同的坐标原点和坐标系下的点 集； S2， 获取目标工件的三维点云并进行 预处理， 得到预处 理后的三维点云； S3， 对预处理后的三维点云进行欧式聚类， 并设置筛选条件筛选出目标工件的感兴趣 点集； S4， 对所述感兴趣点集进行全局多目标三维点云配准， 得到目标工件在模板工件下的 转换矩阵 根据目标工件在模板工件下的转换矩阵 和抓取姿态 获得目标工件在模 板工件下的多个位姿信息 并根据手眼关系矩阵 和位姿信息 计算出目标工件在机 械臂基坐标系下的抓取姿态 根据所述目标工件在机械臂基坐标系下的抓取姿态 计 算每个抓取目标的抓取优先值并进行降序排序； S5， 在所述模板工件处生成碰撞检测空间， 所述碰撞检测空间为机械臂抓取模板工件 时以及Z轴进给过程中机械臂本体与夹具 的空间区域， 并根据所述抓取优先值的排列顺序 以及所述目标工件在模板工件下的多 个位姿信息 进行碰撞检测， 获得最 先检测到的所述 碰撞检测空间内无点云数据的可抓取目标， 并将所述可抓取目标所对应的目标工件在机械 臂基坐标系下的抓取姿态 发送至机械臂执行抓取任务。 2.根据权利要求1所述的基于混叠工件的机械臂抓取方法， 其特征在于， 所述步骤S1的 采用手眼标定获取眼在手外的手眼关系矩阵 具体包括： 将棋盘格标定板固定安装在机械臂末端， 移动一定次数机械臂位置， 相机拍摄标定板 图像， 并记录 机械臂位姿， 根据手眼标定计算模型计算手眼关系矩阵 3.根据权利要求1所述的基于混叠工件的机械臂抓取方法， 其特征在于， 所述模板工件 的至少一个抓取姿态 的点集的坐标系 原点为最优抓取位姿时机械臂的末端工具坐标系 原点， 且坐标系 方向与最优抓取位姿时机械臂的末端工具方向一致； 所述模板工件的不同 抓取姿态的坐标系表征为其到初始 抓取姿态坐标系的刚性变换矩阵， 所述刚性变换矩阵包 括旋转矩阵和平 移矩阵。 4.根据权利要求1所述的基于混叠工件的机械臂抓取方法， 其特征在于， 所述步骤S2具 体包括： 采用三维相机采集混乱堆叠的目标工件的三维点云； 采用直通滤波算法去除所述目标工件的三维点云中感兴趣区域外的三维点云， 得到感 兴趣区内的三维点云； 对感兴趣区内的三维点云进行降采样， 降采样方式为体素滤波， 得到预处理后的三维 点云。 5.根据权利要求1所述的基于混叠工件的机械臂抓取方法， 其特征在于， 所述步骤S3对 预处理后的三维点云进行加速欧式聚类， 具体包括： S31， 以空间中某点P10为起始点， 通过KD ‑Tree近邻搜索算法找到n个离p点最近的点， 分别计算这n个点到P10的欧式距离， 如果距离小于 设定的阈值 R， 则将小于阈值的点放到类权　利　要　求　书 1/3 页 2 CN 115284279 A 2Q(P10)中； S32， 在类Q(P10)中随机 选出P20， 重复步骤S31； S33， 如果类Q(P10)的集合不再增加， 整个聚类过程便终止， 否则一直重复步骤S31 ‑ S32。 6.根据权利要求1所述的基于混叠工件的机械臂抓取方法， 其特征在于， 所述步骤S3 中 的筛选条件为同时满足三维点云的数量大于指定数值和三维点云的最小外包盒长、 宽、 高 不偏离指定数值的百分之15 。 7.根据权利要求1所述的基于混叠工件的机械臂抓取方法， 其特征在于， 所述步骤S4具 体包括： 采用KD‑Tree算法分割所述目标工件的感兴趣点 集获得其空间数据结构； 通过移动最小二乘法对所述目标工件的所述感兴趣点集的KD ‑Tree叶节点进行局部拟 合； 采用FPFH描述子根据KD ‑Tree计算的空间数据结构 分别计算目标工件的感兴趣点集和 模板工件的多个不同抓取姿态的特征信息， 第一步先计算每个查询点Pq的一系列 值， 记为SPFH， 第二步重新计算每个点的k近邻， 使用邻近的SPFH值计算Pq的最终直方图， 计 算式为： 式中， k为当前查询点的临近点数， Pq为当前计算点， Pk当前待计算点的相邻点， ωk为当 前计算点Pq与其相邻点Pk间的权值， FPFH为快速点特 征直方图， S PFH为简化 点特征直方图； 根据计算得到的目标工件感兴趣点集的FPFH特征信息和多个不同抓取姿态模板工件 的FPFH特征信息， 选取满足相似度约束的点对作为配准点对， 进而计算目标工件在模板工 件下的多个位姿信息 所述位姿信息为所述目标工件刚性变换到所述模 板工件的旋转矩阵和平移矩阵， 采用下式计算所述目标工件在机械臂基坐标系下的抓取姿 态 所述目标工件在机械臂基坐标系下的抓取姿态 包含(X,Y,Z, θx, θy, θz)； 根据每个抓取姿态的Z、 θx、 θy值进行排序， 并分别赋予权重因子a、 b、 c， 并对第i个抓取 姿态的优先级Di赋值， 则第i个抓取姿态的抓取优先值计算式为： 其中， Zi、 θxi、 θyi分别为第i个抓取姿态的Z、 θx、 θy值； 对识别到的目标工件在机 械臂基坐标系下的抓取姿态的抓取优先值进行降序排列。 8.根据权利要求1所述的基于混叠工件的机械臂抓取方法， 其特征在于， 所述步骤S5具 体包括： 在所述模板工件处预生成碰撞检测空间， 根据所述目标工件在模板工件下的多个位姿 信息 将所述目标工件及其周围点云刚性变换到碰撞检测空间处， 若碰撞检测空间内无 点云数据则视为可抓取目标， 若存在点云视为 不可抓取目标； 从抓取优先值最高目标开始逐序进行碰撞检测， 以最先检测到的可抓取目标作为最终 的执行抓取目标， 并将所述执 行抓取目标所对应的在机 械臂基坐标系下的抓取姿态 发送权　利　要　求　书 2/3 页 3 CN 115284279 A 3