(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210746771.X (22)申请日 2022.06.28 (71)申请人 上海仙工智能科技有限公司 地址 201206 上海市浦东 新区新金桥路27 号13号楼 2楼 (72)发明人 沈锴　邓辉　李华伟　陈丁　 陈忠伟　石岩　王益亮　赵越　 (74)专利代理 机构 北京中济纬天专利代理有限 公司 11429 专利代理师 季永康 (51)Int.Cl. B25J 19/04(2006.01) B25J 9/16(2006.01) (54)发明名称 一种基于物体平面特征的复合机器人3D抓 取方法及系统 (57)摘要 本发明提供了一种基于物体平面特征的复 合机器人3D抓取方法及系统， 其中方法步骤包 括： S1标定机械臂与相机之间的手 眼关系； S2设 定标准拍照位姿， 采集场景点云， 分出目标物体 点云制作第一模板； S3  AGV保持不动再次获取场 景点云， 以与第一模板匹配， 获取场景中目标物 体相对于第一模板的第一变换矩阵； S4令AGV和 目标物体保持不动， 示教机械臂， 获取可抓取目 标物体的第一位姿以结合第一变换矩阵， 计算标 准抓取位姿变换矩阵； S5机械臂保持标准拍照位 姿， 移动AGV到新的位置后获取场景点 云， 以与第 一模板进行匹配， 获取场景中目标物体相对于第 一模板的第二变换矩阵； S6根据第二变换矩阵与 标准抓取位姿变换矩阵， 从而无需借助额外标志 物解得机 械臂抓取位姿。 权利要求书3页 说明书11页 附图5页 CN 114939891 A 2022.08.26 CN 114939891 A 1.一种基于物体平面特 征的复合机器人3D抓取 方法， 其特 征在于步骤 包括： S1标定机械臂与相机之间的手眼关系； S2设定标准拍照位姿， 并采集场景点云， 以分离出目标物体点云来制作第一模板； S3 AGV在保持不动的条件下获取场景点云， 以供与第一模板进行模板匹配， 进而获取 场景中目标物体相对于第一模板的第一变换矩阵； S4在AGV和目标物体保持不动的条件下， 示教机械臂， 直至获取可抓取目标物体的第一 位姿以结合 步骤S3的第一变换矩阵， 计算出 标准抓取位姿变换矩阵； S5机械臂保持标准拍照位姿， 移动AGV到新的位置后获取场景点云， 以供与第一模板进 行模板匹配， 进 而获取场景中目标物体相对于第一模板的第二变换矩阵； S6根据第二变换矩阵与步骤S4标准 抓取位姿变换矩阵， 解得机 械臂抓取位姿。 2.根据权利要求1所述的基于物体平面特征的复合机器人3D抓取方法， 其特征在于步 骤S2中制作第一模板的步骤 包括： S21在目标物体点云的平面特 征区域创建ROI， 将ROI内点云拟合平面后； S22建立旋转矩阵R来 转换ROI内点云， 以旋转ROI内点云使其所在平面法向与Z轴平行； S23计算旋转后ROI 内点云的中心， 移动ROI 内点云使其 中心与参考坐标系原点重合后， 将该ROI内点云定为第一模板 。 3.根据权利要求1所述的基于物体平面特征的复合机器人3D抓取方法， 其特征在于步 骤S3中模板匹配步骤 包括： S31对场景点云使用区域增长算法分割出 各初始曲面 点集； S32使用随机抽样一致性算法在各初始曲面点集中提取其所包含的平面模型及其对应 的平面点云； S33将第一模板以及步骤S32中获得的各平面点云分别在参考坐标系内映射成相同分 辨率的2D栅格图； S34将平面点云的2D栅格图与第一模板的2D栅格图进行重合匹配， 若匹配成功， 记录平 面点云在参 考坐标系内的旋转角度及偏移量， 籍以建立目标物体到相机的第一变换矩阵。 4.根据权利要求3所述的基于物体平面特征的复合机器人3D抓取方法， 其特征在于步 骤S33中平面点云的2D栅格图建立 步骤包括： S331建立旋转矩阵R1来转换平面点云， 以旋转平面点云使其平面法向与Z轴 平行； 将旋 转矩阵R1转换为齐次变换矩阵 ； S332 为根据步骤S3 31转换后的平面 点云建立包围盒； S333根据预设尺寸等距栅格化该包围盒， 并将存在点云的栅格进行填充标记， 以转换 为平面点云的2D栅格图， 并记录对应二 值变换矩阵为 。 5.根据权利要求3所述的基于物体平面特征的复合机器人3D抓取方法， 其特征在于步 骤S33中第一模板的2D栅格图建立 步骤包括： S334 建立第一模板点云包围盒； S335根据 预设尺寸等距栅格化第一模板点云包围盒， 并将存在点云的栅格进行填充标 记， 以转换为第一模板的2D栅格图。 6.根据权利要求3所述的基于物体平面特征的复合机器人3D抓取方法， 其特征在于步权　利　要　求　书 1/3 页 2 CN 114939891 A 2骤S34中所述重合匹配步骤 包括， 粗匹配步骤： S341以预设步长对平面点云的2D栅格图进行旋转、 偏移操作， 直至与第一模板的2D栅 格图匹配后， 记录匹配旋转角度及偏移量； S342 根据匹配旋转角度反算第一模板的2D栅格图的相对旋转角度， 以建立变换矩阵 。 7.根据权利要求6所述的基于物体平面特征的复合机器人3D抓取方法， 其特征在于步 骤S34中所述重合匹配步骤 还包括， 精匹配步骤： S343 将第一模板的点云根据 步骤S342 获得的变换矩阵进行转换， 得到第二模板点云； 以建立第二模板点云包围盒； S344 根据预设尺寸等距栅格化第二模板点云包围盒， 并将存在点云的栅格进行填充 标记， 以转换为第二模板的2D栅格图； 并建立对应的二 值变换矩阵为 ； S345以预设步长对平面点云的2D栅格图进行旋转、 偏移操作， 直至与第二模板的2D栅 格图匹配后， 记录匹配旋转角度及偏移量； S346根据匹配旋转角度及偏移量反算第二模板的2D栅格图的相对旋转角度及偏移量， 以建立3D空间变换矩阵 ， 进而反算出目标物体在3D空间内的姿态得出第一变 换矩阵 。 8.根据权利要求7所述的基于物体平面特征的复合机器人3D抓取方法， 其特征在于步 骤S4中所述标准 抓取位姿变换矩阵计算 步骤包括： 将示教机 械臂后获得的第一 位姿记为Pose， 其中 其中 是预设的目标物体到机械臂基座的变换矩阵； 为第一变换矩阵； 是预设的相机到机械臂末端的变换矩阵； 是预设的机械臂末端到基座的标准拍 照位 姿变换 矩阵 ； 进 而结 合 第一 变换 矩阵 ， 计算出 标 准 抓取 位 姿变换 矩阵 。 9.根据权利要求8所述的基于物体平面特征的复合机器人3D抓取方法， 其特征在于步 骤S6中计算机 械臂抓取位姿Pose1的步骤 包括： 计算 ， 其中 为第二变换矩阵。 10.一种基于物体平面特 征的复合机器人3D抓取系统， 其特 征在于包括： 存储单元， 其存有实现如权利要求1至9任一所述3D抓取方法步骤的程序， 以供控制单 元及处理单元适时调取 执行包括； 控制单元操控机 械臂及相机以标准拍照位姿， 采集场景点云以向处 理单元发送； 处理单元从场景点云中分离出目标物体点云以制作第一模板； 控制单元操控AGV在 保持不动的条件下， 令相机获取场景点云， 以并向处理单元发送供权　利　要　求　书 2/3 页 3 CN 114939891 A 3