一种基于点云凸结构感知的织物抓取位姿生成方法与系统

本发明涉及机器人视觉感知与柔性物体抓取，具体涉及一种基于点云凸结构感知的织物抓取位姿生成方法、系统及计算机设备。

背景技术：

1、在医疗护理、工业洗涤、服装整理和织物回收等应用场景中，堆叠织物的自动化抓取分拣是提升作业效率的关键环节。织物具有柔性强、易形变、自遮挡严重和表面褶皱不规则等特点，当多件织物堆叠缠绕时，机器人难以从视觉信息中稳定确定可操作抓取区域，因此对堆叠织物进行可靠的抓取位姿生成是实现自动化分拣的核心任务。

2、现有技术cn107138432b 公开了一种针对非刚性可变形物体的机器人分拣抓取方法。该方法首先通过3d相机获取工作区域深度图像并生成三维点云，然后计算点云中各点的表面法向量和曲率，再基于法向量夹角和距离的区域增长方法对点云进行初始分割，最后通过最小包围盒对各分割物体进行位姿估计并计算抓取点。该方案主要存在以下不足：（1）区域增长以曲率最小的点作为种子，优先选取表面平坦区域，未能从点云中识别和提取凸起结构作为抓取候选，在堆叠织物场景中褶皱凸脊等可操作区域难以被主动定位；（2）点云分割在原始三维空间内基于法向量夹角和距离进行，缺乏对凸结构点云、凸结构区域以及抓取位姿之间关系的系统建模；（3）采用最小包围盒进行位姿估计，生成的抓取姿态反映物体整体外廓形状，与织物局部凸脊结构的延展方向之间对应关系不稳定；（4）抓取对象选取依赖置信度排序，未引入夹爪物理尺寸对候选区域进行可行性筛选，部分输出位姿可能超出夹爪操作范围。

3、因此，亟需一种能够从堆叠织物点云中基于曲率与凸性联合筛选提取凸特征区域、并将凸特征区域的局部几何方向与夹爪物理约束相融合的候选抓取位姿生成方法，以提升堆叠织物抓取位姿的稳定性与可操作性。

技术实现思路

1、本发明的目的是为了克服现有技术中堆叠织物可操作区域识别不稳定、候选抓取位姿生成缺乏结构约束以及复杂形变场景下抓取位姿可行性不足的问题，提供一种基于点云凸结构感知的织物抓取位姿生成方法。

2、为实现上述目的，第一方面，本发明提供一种基于点云凸结构感知的织物抓取位姿生成方法，包括以下步骤：

3、s1. 获取堆叠织物场景的原始点云，并对原始点云进行预处理，得到工作区域内的织物点云集合；

4、s2. 基于所述织物点云集合构建各点的局部邻域，并计算局部邻域的特征值、法线方向和局部主切线方向；

5、s3. 根据所述局部邻域的特征值计算织物点云中各点的局部曲率，将局部曲率大于预设曲率阈值的点提取为高曲率点，得到高曲率点云集；

6、s4. 基于点云凸性判断准则，从高曲率点云集中提取凸特征点云集；

7、s5. 针对所述凸特征点云集中的各凸特征点，构建由空间坐标、法线方向、局部主切线方向和局部曲率组成的联合几何特征，并基于联合几何特征进行无监督聚类，得到若干凸特征区域；

8、s6. 针对所述凸特征区域，得到凸特征区域的候选抓取位姿；

9、s7. 对所述候选抓取位姿基于夹爪的最大张开宽度，进行抓取宽度约束筛选，得到具有操作可行性的候选抓取位姿集合。

10、进一步地，所述的步骤s1包括以下步骤：

11、s11. 通过设备采集堆叠织物场景的原始点云；

12、s12. 对所述原始点云进行工作区域裁剪，得到裁剪后点云；

13、s13. 对所述裁剪后点云进行体素下采样，得到降采样点云；

14、s14. 对降采样点云进行统计滤波离群点剔除，得到预处理后的织物点云集合:；其中，表示当前场景下预处理后的织物点云集合；表示第个点云；表示点云总数。

15、进一步地，所述步骤s2包括以下步骤：

16、s21. 对所述织物点云中的任一点，采用kd-tree方法搜索其近邻点，以构建局部邻域；

17、s22. 对所述局部邻域内的点云进行主成分分析，构建局部协方差矩阵；

18、s23. 对所述局部协方差矩阵进行特征值分解，得到法线方向对应的特征值、切平面两个正交方向对应的特征值，以及各特征值对应的特征向量；

19、s24. 根据所述特征向量确定点的法线方向和局部主切线方向。

20、进一步地，所述步骤s3包括以下步骤：

21、s31. 根据局部邻域的特征值计算点的局部曲率，其数学表达式为：

22、

23、其中，表示点所在局部区域的曲率；表示法线方向对应的特征值；表示切平面两个正交方向对应的特征值；

24、s32. 将局部曲率与预设曲率阈值比较；

25、s33. 将局部曲率大于预设曲率阈值的点提取为高曲率点，得到高曲率点云集：

26、

27、其中，表示高曲率点云集；表示预设的曲率阈值。

28、进一步地，所述步骤s4包括以下步骤：

29、s41. 计算高曲率点云集中点的凹凸性判别量，其数学表达式为：

30、

31、其中，表示点的凹凸性判别量；为点的法线方向；表示局部邻域的几何中心；当时，将点判定为凸特征点云，即点位于局部凸起区域。

32、s42. 从所述高曲率点云集中保留满足凸性判别条件的点，得到凸特征点云集：

33、。

34、进一步地，所述步骤s5包括以下步骤：

35、s51. 针对所述凸特征点云集中的任一凸特征点，构建联合几何特征：

36、

37、其中，表示点的空间坐标；表示法线方向；表示局部主切线方向；表示局部曲率。

38、s52. 对局部主切线方向进行符号统一，得到符号统一后的联合几何特征；

39、s53. 对符号统一后的联合几何特征中的空间坐标和局部曲率进行尺度归一化处理，得到归一化后的联合几何特征；

40、s54. 基于归一化后的联合几何特征，采用基于密度的无监督聚类方法对凸特征点云集进行聚类；

41、s55. 聚类后将凸特征点云集划分为若干个具有几何一致性的凸特征区域：

42、

43、其中，表示提取的第个凸特征区域。

44、进一步地，所述步骤s54中，所述基于密度的无监督聚类方法为dbscan聚类方法；对于任意两个凸特征点，其联合几何特征空间距离为：

45、

46、其中，分别表示任意两个凸特征点的联合几何特征；所述dbscan聚类方法包括：在给定邻域半径和最小样本数的条件下，若在邻域内包含的样本数满足：

47、

48、则将点定义为核心点；若点落在核心点的邻域内，则称点对点直接密度可达；当存在一条由若干核心点连接而成的点链，使得链上相邻点之间满足直接密度可达关系，则将所述点链上的点划分为同一凸特征区域。

49、进一步地，所述步骤s6包括以下步骤：

50、s61. 针对第个凸特征区域，定义其对应的候选抓取位姿为：

51、

52、其中，表示候选抓取中心的位置；表示候选抓取姿态的旋转矩阵；

53、s62. 根据所述凸特征区域内所有点云的几何中心、预设抓取高度和抓取接近方向，计算候选抓取中心位置：

54、

55、其中，表示凸特征区域中的点云数量；表示凸特征区域中点的空间坐标；表示预设的抓取高度；表示抓取接近方向；

56、s63. 根据凸特征区域的法线方向及局部主切线方向，计算抓取接近方向、脊线延展方向和夹爪张开方向，其数学表达式为：

57、

58、其中，表示点的法线方向；表示点的局部主切线方向；

59、s64. 根据所述夹爪张开方向、脊线延展方向和抓取接近方向共同构建候选抓取姿态的旋转矩阵：

60、

61、其中，表示夹爪张开方向；表示脊线延展方向；表示抓取接近方向；

62、s65. 将所述候选抓取中心位置和候选抓取姿态的旋转矩阵组合，得到凸特征区域对应的候选抓取位姿。

63、进一步地，所述步骤s7包括以下步骤：

64、s71. 将凸特征区域内的所有点云投影到夹爪张开方向上；

65、s72. 根据投影结果计算该凸特征区域在夹爪张开方向上的区域宽度；

66、s73. 判断所述区域宽度是否大于夹爪的最大张开宽度；

67、s74. 若所述区域宽度大于夹爪的最大张开宽度，则剔除该凸特征区域对应的候选抓取位姿；

68、s75. 若所述区域宽度不大于夹爪的最大张开宽度，则保留该候选抓取位姿；

69、s76. 对所有凸特征区域对应的候选抓取位姿执行上述抓取宽度约束筛选，得到具有操作可行性的候选抓取位姿集合：

70、

71、其中，表示满足抓取宽度约束的候选抓取位姿集合。

72、第二方面，本发明还提供基于点云凸结构感知的织物抓取位姿生成系统，用于实现上述方法，包括：

73、点云采集与预处理模块，用于获取堆叠织物场景的原始点云，并对原始点云进行预处理，得到工作区域内的织物点云集合；

74、局部几何特征计算模块，用于基于所述织物点云集合构建各点的局部邻域，并计算局部邻域的特征值、法线方向和局部主切线方向；

75、高曲率点云提取模块，用于根据所述局部邻域的特征值计算各点的局部曲率，将局部曲率大于预设曲率阈值的点提取为高曲率点，得到高曲率点云集；

76、凸特征点云提取模块，用于基于点云凸性判断准则，从所述高曲率点云集中提取凸特征点云集；

77、凸特征区域聚类模块，用于针对所述凸特征点云集中的各凸特征点，构建由空间坐标、法线方向、局部主切线方向和局部曲率组成的联合几何特征，并基于所述联合几何特征进行无监督聚类，得到若干凸特征区域；

78、候选抓取位姿生成模块，用于针对所述凸特征区域，得到凸特征区域的候选抓取位姿；

79、抓取宽度约束筛选模块，用于对所述候选抓取位姿基于夹爪的最大张开宽度，进行抓取宽度约束筛选，得到具有操作可行性的候选抓取位姿集合。

80、第三方面，本发明还提供一种计算机设备，包括处理器以及用于存储处理器可执行程序的存储器，所述处理器执行存储器存储的程序时，实现上述基于点云凸结构感知的织物抓取位姿生成方法。

81、与现有技术相比，其有益效果在于：

82、（1）本发明通过局部曲率计算和凸性判断准则相结合，从高曲率点云中进一步区分凸起区域与凹陷区域，能够有效提取更适合夹爪抓取的凸特征点云，避免将凹陷褶皱或无效高曲率区域误判为抓取区域。

83、（2）本发明将空间坐标、法线方向、局部主切线方向和局部曲率构建为联合几何特征，并采用基于密度的无监督聚类方法获取凸特征区域，能够将离散凸特征点组织为具有几何一致性的区域级结构单元。

84、（3）本发明候选抓取位姿的生成不仅利用凸特征区域的几何中心确定抓取中心，还结合抓取接近方向、脊线延展方向和夹爪张开方向构建抓取姿态，使生成的抓取位姿与织物局部凸脊结构相匹配。

85、（4）本发明引入夹爪的最大张开宽度约束后，将凸特征区域投影到夹爪张开方向上进行宽度筛选，能够剔除夹爪无法覆盖的候选区域，提高候选位姿在实际抓取执行中的可操作性。

86、（5）本发明输出结果为候选抓取位姿集合，可作为后续最优抓取位姿选择、模糊决策模型、抓取评分模型或机器人执行控制模块的输入，具有较好的模块化扩展能力。