(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202110579630.9 (22)申请日 2021.05.26 (65)同一申请的已公布的文献号 申请公布号 CN 113177365 A (43)申请公布日 2021.07.27 (73)专利权人 上海交通大 学 地址 200240 上海市闵行区东川路80 0号 (72)发明人 张俊楠　田林睿　吴建华　熊振华　 朱向阳　 (74)专利代理 机构 上海光华专利事务所(普通 合伙) 31219 专利代理师 徐秋平 (51)Int.Cl. G06F 30/27(2020.01) G06T 19/20(2011.01) G06V 10/774(2022.01)(56)对比文件 CN 111845872 A,2020.10.3 0 CN 105058795 A,2015.1 1.18 CN 112802105 A,2021.0 5.14 CN 111626211 A,2020.09.04 CN 111340886 A,2020.0 6.26 US 201916 3342 A1,2019.0 5.30 US 2020302187 A1,2020.09.24 QihanWu et al. .Coordinated co ntrol of a dual-arm robot for surgical i nstrument sorting tasks. 《Robotics and Auto nomous Systems》 .2019, 朱统帅.基 于单目立体 视觉的自由工件堆分 拣研究. 《中国优秀博硕士学位 论文全文数据库 (硕士)信息科技 辑》 .2019, 审查员 王彦蕴 (54)发明名称 启发式不规则物体垂直堆叠方法及系统、 存 储介质及终端 (57)摘要 本发明提供一种启发式不规则物体垂直堆 叠方法及系统、 存储介质及终端， 引入包络面数 据获取初始位姿， 采用基于主包络面对齐的姿态 选择方式代 替随机姿态选择方式， 提高初始位姿 生成稳定位姿的概率和效率； 通过增加第四层稳 定度过滤层， 增加 后续层的堆叠以及结构整体稳 定度； 通过准静态放置机制中嵌入物体掉落预测 网络， 大幅缩短某些极端情况判定物体稳定与否 的计算耗时， 提高了方法的整体效率。 本发明的 启发式不规则物 体垂直堆叠方法及系统、 存储介 质及终端具有堆叠能力强， 普适性高， 整体效率 更高的优点。 权利要求书3页 说明书12页 附图3页 CN 113177365 B 2022.12.06 CN 113177365 B 1.一种启发式不 规则物体垂直 堆叠方法， 其特 征在于， 包括以下步骤： 步骤S1、 初始化待堆叠物体集合中每个待堆叠物体的包络面数据， 并根据所述包络面 数据获取主包络面； 步骤S2、 对于每个待堆叠物体， 根据待堆叠位置的(x， y)坐标及FMi， j与z轴对齐 的方式 设置所述待堆叠物体的初始位姿， 其中， FMi， j表示第i个物体的第j个主包络面； 步骤S3、 基于准静态放置方式根据所述待堆叠物体的初始位姿生成可行位姿， 并将构 成稳定的位姿记录 至可行位姿集F中； 步骤S4、 基于上表面倾斜程度、 接触面积和稳定度对所述可行位姿集F进行筛选， 获取 集合Ffiltered， 从所述集合Ffiltered中随机选择一组可行解(oi， pi， stable)， 并将所述可行解对 应的待堆叠物体 oi的位姿设置为pi， stable且作为第一层物体， 其中pi， stable表示待堆叠物体的 某个构成稳定的位姿； 步骤S5、 计算当前最上层的堆叠物体的潜在可对齐上表面(US1， US2， ...， USk， ...)； 步骤S6、 对于每个剩余待堆叠物体， 将所述当前最上层的堆叠物体的中心关于x轴、 y轴 进行离散化， z坐标向上偏置来设置初始位置以保证物体间不会接触； 对于每个初始位置， 分别将每个剩余待堆叠物体的初始位姿根据FMi， j法向与USk法向对齐的方式进行设置， 并 且初始位姿绕着 FMi， j与USk的共同法向进行离散化； 待堆叠物体的6D位姿仅剩一个自由度， 以此时位姿为初始值， 围绕两个主包络面的共同法向作旋转， 获得多个初始位姿； 步骤S7、 基于准静态放置方式根据所述剩余待堆叠物体的初始位姿生成可行解， 并将 构成稳定的位姿记录 至可行位姿集F中； 步骤S8、 根据上表面倾斜程度、 接触面倾斜度、 接触面积和稳定度对所述可行位姿集F 进行筛选， 获取集合Ffiltered， 从所述集合Ffiltered中随机选择一组可行解(oi， pi， stable)并将 所述可行解对应的待堆叠物体的位姿设置为pi， stable且作为下一层物体； 步骤S9、 重复步骤S5 ‑S8， 直到待堆叠物体的个数为0或者所述 集合Ffiltered为空； 所述步骤S7包括以下步骤： 71)对于每个剩余待堆叠物体， 利用物理引擎的距离检测功能将所述剩余待堆叠物体 与属于已堆叠结构最上层物体之间的距离设置为0； 72)触发物理引擎 直到所述剩余待堆叠物体接触到已堆叠结构； 73)将已堆叠结构中所有物体设置成动态的； 74)触发物 理引擎十次， 检测所述剩余待堆叠物体两次间位姿的欧氏距离； 若所述欧氏 距离小于第一预设阈值则进行一次记录， 同时进 行触地检测； 若 所述欧氏距离已触底， 则表 明此次稳定位姿生成并未成功； 75)采集已堆叠层数、 所述剩余待堆叠物体句柄、 所述属于已堆叠结构最上层物体句 柄、 接触面积与接触面法向五维特征信息， 并输入训练好的物体跌落预测网络， 若 预测结果 为稳定， 则继续执 行步骤76) ‑77)， 否则流 程结束； 76)若步骤74)中的记录次数小于9且第一次与第十次检测到的欧氏距离大于第二预设 阈值， 则重复执 行步骤74)； 77)重置已堆叠结构中所有物体的位姿并且设置为静态的。 2.根据权利要求1所述的启发式不规则物体垂直堆叠方法， 其特征在于： 所述步骤S6包 括以下步骤：权　利　要　求　书 1/3 页 2 CN 113177365 B 2将所述剩余待堆叠物体的位置设置为离 散化位置集合中的一个； 选定所述剩余待堆叠物体的一个主包络面与属于已堆叠结构最上层物体的上表面的 一个主包络面进行对齐； 以对齐的两个主包络面的共同法向为轴， 将360度进行离散化来分别设置所述剩余待 堆叠物体的初始位姿。 3.根据权利要求1所述的启发式不规则物体垂直堆叠方法， 其特征在于： 所述物体跌落 预测网络采用基于RUSBo ost集成树模型的二分类 器。 4.根据权利要求1所述的启发式不规则物体垂直堆叠方法， 其特征在于： 所述第 一预设 阈值为0.01， 所述第二预设阈值 为0.02。 5.根据权利要求1所述的启发式不规则物体垂直堆叠方法， 其特征在于： 所述步骤S8包 括以下步骤： 所述可行位姿集F中上表面倾斜度大于平均值的可行位姿被保 留， 过滤得到集合XF1， 所 述上表面 倾斜度由属于上表面的所有 主包络面的法向量z轴分量加权和表征； 所述集合XF1中接触面倾斜度大于平均值的可行位姿被保留， 过滤得到集合XF2， 所述接 触面倾斜度由接触面法向量的z轴分量表征； 所述集合XF2中接触面 面积大于平均值的可 行位姿被保留， 过 滤得到集合XF3； 所述集合XF3中稳定度大于平均值的可行位姿被保留， 过滤得到所述集合Ffiltered， 所述 稳定度由接触面 面积与所述剩余待堆叠物体物体中心到 接触面距离平方的比值表征； 从所述集合Ffiltered中随机选择一组可行解(oi， pi， stable)， 并将所述可行解对应的待堆 叠物体的位姿设置为pi， stable且作为下一层物体。 6.根据权利要求1所述的启发式不规则物体垂直堆叠方法， 其特征在于： 所述主包络面 取包络面中面积大小为前 预设百分比的面。 7.一种启发式不规则物体垂直堆叠系统， 其特征在于， 包括初始化模块、 第 一初始位姿 设置模块、 第一可行位姿生成模块、 第一层生成模块、 上表面计算模块、 第二初始位姿设置 模块、 第二可 行位姿生成模块、 下层物体生成模块和迭代模块； 所述初始化模块用于初始化待堆叠物体集合中每个待堆叠物体的包络面数据， 并根据 所述包络面数据获取主包络面； 所述第一初始位姿设置模块用于对于每个待堆叠物体， 根据待堆叠位置的(x,y)坐标 及FMi， j与z轴对齐的方式设置所述待堆叠物体的初始位姿， 其中， FMi， j表示第i个物体的第j 个主包络面； 所述第一可行位姿生成模块用于基于准静态放置方式根据所述待堆叠物体的初始位 姿生成可 行位姿， 并将构成稳定的位姿记录 至可行位姿集F中； 所述第一层生成模块用于基于上表面倾斜程度、 接触面积和稳定度对所述可行位姿集 F进行筛选， 获取集合Ffiltered， 从所述集合Ffiltered中随机选择一组可行解(oi， pi， stable)， 并 将所述可行解对应的待堆叠物体oi的位姿设置 为pi， stable且作为第一层物体， 其中pi， stable表 示待堆叠物体的某个构成稳定的位姿； 所述上表面计算模块用于计算当前最上层的堆叠物体的潜在可对齐上表面(US1， US2， ...， USk， ...)； 所述第二初始位姿设置模块用于对于每个剩余待堆叠物体， 将所述当前最上层的堆叠权　利　要　求　书 2/