一种机械臂抓取鲁棒性优化方法及抓取装置

1.本发明涉及机器人技术领域，特别是一种机械臂抓取鲁棒性优化方法及抓取装置。


背景技术：

2.现有技术中，在物体自动化抓取过程中，首先需要获取到待抓取物在相机坐标系下的位姿信息，并根据相机坐标系与抓取装置坐标系的对应关系(即，相机外参)，将上述位姿信息转换至抓取装置坐标系下，以便可以通过转换后的位姿信息控制抓取装置对待抓取物体进行抓取。
3.然而，当相机遭外力碰撞导致的相机位置发生位移时，此时相机坐标系也会发生改变，若仍采用之前的对应关系进行转换，则容易导致无法准确抓取到待抓取物体。


技术实现要素：

4.针对现有技术中，相机位姿发生变化，导致识别出错抓取失败的问题，本发明公开了一种机械臂抓取鲁棒性优化方法及抓取装置。
5.本发明的技术方案为，提出了一种机械臂抓取鲁棒性优化方法，包括：
6.在相机拍摄时，为所述相机的拍摄区域添加多个标志物；
7.对所述标志物以及待抓取物进行实时拍摄，获取所述标志物对应的第一实时图像、以及所述待抓取物对应的第二实时图像；
8.根据所述第一实时图像以及所述第二实时图像判断所述相机的位姿是否发生变化，并在判定所述相机的位姿发生变化时，对相机的外参数矩阵进行补偿，并更新所述待抓取物的位姿；
9.以预设策略抓取所述待抓取物。
10.进一步，根据所述第一实时图像以及所述第二实时图像判断所述相机的位姿是否发生变化，包括：
11.获取所述第一实时图像、以及所述第二实时图像中任意两个标志物之间的相对位置；
12.判断所述相对位置是否发生变化，若否，则判断所述标志物于所述第一实时图像以及所述第二实时图像中的位姿是否相同；
13.在判定所述第一实时图像以及所述第二实时图像中所述标志物的位姿不同时，判定所述相机的位姿发生变化。
14.进一步，在判定所述第一实时图像以及所述第二实时图像中的所述标志物的位姿相同时，所述机械臂抓取鲁棒性优化方法还包括：
15.依据所述第二实时图像确定所述待抓取物在相机坐标系下的位姿；
16.依据所述相机当前的外参矩阵计算所述待抓取物处于抓取装置坐标系下的位姿；
17.其中，所述相机的外参矩阵为相机坐标系与抓取装置坐标系之间的对应关系。
18.进一步，对相机的外参矩阵进行补偿，包括：
19.获取所述相机处于初始状态下的基准位姿、以及处于当前状态下的实时位姿；
20.依据最小二乘法对所述基准位姿和实时位姿进行拟合，确定所述实时位姿相对于所述基准位姿的变换关系；
21.依据所述变换关系对所述相机的外参矩阵进行补偿。
22.进一步，更新所述待抓取物的位姿，包括：
23.依据所述第二实时图像确定所述待抓取物在相机坐标系下的位姿；
24.依据进行补偿后的所述外参矩阵计算所述待抓取物处于抓取装置坐标系下的位姿；
25.依据计算的所述待抓取物处于抓取装置坐标系下的位姿对所述待抓取物的位姿进行更新。
26.进一步，在完成对任一待抓取物的抓取后，所述机械臂抓取鲁棒性优化方法还包括：
27.判断在对所述待抓取物进行抓取前后，所述第二实时图像中是否存在位姿未变化的待抓取物；
28.若是，则抓取位姿未变化的待抓取物；
29.若否，则判定所述待抓取物已全被抓取，并结束抓取动作。
30.进一步，抓取位姿未变化的待抓取物，包括：
31.获取抓取前所述第二实时图像中位姿未变化的待抓取物，并确定位姿未变化的待抓取物在相机坐标系下的位姿；
32.依据进行补偿后的所述外参矩阵计算位姿未变化的待抓取物处于待抓取物坐标系下的位姿；
33.对位姿未变化的待抓取物进行抓取。
34.进一步，所述标志物采用二维码。
35.进一步，所述预设策略为：
36.在所述待抓取物设置为多个时，对所述待抓取物的优先级进行划分；
37.以所述待抓取物的优先级从高至低依次抓取所述待抓取物；
38.其中，所述待抓取物的优先级依据所述待抓取物在待抓取物的集合中的位置、以及相互接触的待抓取物的数量确定。
39.本发明还提出了一种采用该上述机械臂抓取鲁棒性优化方法的抓取装置，包括：相机，其用于对标志物以及待抓取物进行实时拍摄，并获取第一实时图像以及第二实时图像；
40.机械臂，其用于抓取待抓取物；
41.主控单元，其存储有相机的外参矩阵，并用于根据第一实时图像和第二实时图像确定待抓取物的位姿。
42.与现有技术相比，本发明至少具有如下有益效果：
43.本发明通过增加标志物，并实时更新待抓取物位姿的方法，避免了在抓取过程中，因相机位姿发生变化，从而导致识别出错抓取失败的情形发生，同时可以对相机位姿的变换进行实时矫正，多个标志物的设置避免了误矫正的可能性。
附图说明
44.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
45.图1为本发明整体的工作流程图。
具体实施方式
46.为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
47.由此，本说明书中所指出的一个特征将用于说明本发明的一个实施方式的其中一个特征，而不是暗示本发明的每个实施方式必须具有所说明的特征。此外，应当注意的是本说明书描述了许多特征。尽管某些特征可以组合在一起以示出可能的系统设计，但是这些特征也可用于其他的未明确说明的组合。由此，除非另有说明，所说明的组合并非旨在限制。
48.下面结合附图以及实施例对本发明的原理及结构进行详细说明。
49.现有技术中，在物体自动化抓取过程中，首先需要获取到待抓取物体在相机坐标系下的位姿信息，并根据相机坐标系与抓取装置坐标系的对应关系(也即相机的外参矩阵)，将上述位姿信息转换至抓取装置坐标系下，以便可以通过转换后的位姿信息控制抓取装置对待抓取物进行抓取。在相机遭受外力碰撞导致相机位置发生变化时，相机坐标系也会发生改变，如果仍然采用之前的外参矩阵，容易导致抓取装置无法准确抓取到待抓取物。
50.本发明的思路在于，在相机拍摄时添加多个标志物，用来判断相机的位姿是否发生变化，并能够在相机位姿发生变化时，对外参矩阵进行补偿，并更新待抓取物的位姿，从而依据外参矩阵获取待抓取物在抓取装置坐标系下的位姿，避免抓取失败的问题。
51.具体的，本发明提出的机械臂抓取鲁棒性优化方法，包括以下步骤：
52.在相机拍摄时，为相机的拍摄区域添加多个标志物；
53.对标志物以及待抓取物进行实时拍摄，获取标志物对应的第一实时图像、以及待抓取物对应的第二实时图像；
54.根据第一实时图像以及第二实时图像判断相机的位姿是否发生变化，并在判定相机的位姿发生变化时，对相机的外参数矩阵进行补偿，并更新待抓取物的位姿；
55.以预设策略抓取待抓取物。
56.本发明通过上述方法，避免了在抓取过程中，因相机位姿发生变化，从而导致识别出错抓取失败的情形发生。
57.其中，上述步骤中，“根据第一实时图像以及第二实时图像判断相机的位姿是否发生变化”包括：
58.获取第一实时图像、以及第二实时图像中任意两个标志物之间的相对位置；
59.判断相对位置是否发生变化，若否，则判断标志物于第一实时图像以及第二实时图像中的位姿是否相同；
60.在判定第一实时图像以及第二实时图像中标志物的位姿不同时，判定相机的位姿发生变化。
61.这里，“获取第一实时图像、以及第二实时图像中任意两个标志物之间的相对位置”是用于判断标志物是否发生位移。因为对任意两个标志物之间的相对位置，其并不会因为处于第一实时图像或第二实时图像而改变，因此通过判断第一实时图像以及第二实时图像中任意两个标志物之间的相对位置是否变化，即可确定标志物是否发生位移，如果发生位移，则需要重新定位标志物，并进入后续流程。
62.如果判定标志物并未发生位移，则进入下一步判断，判断标志物于第一实时图像以及第二实时图像中的位姿是否相同。这里，第一实时图像和第二实时图像均是通过相机拍照的来，在相机的位姿并未发生变化时，第一实时图像以及第二实时图像中标志物的位姿应该相同，反之，若第一实时图像以及第二实时图像中标志物的位姿不同，则可以认定相机的位姿发生变化，则需要对相机的外参矩阵进行补偿，避免按照原本的外参矩阵计算待抓取物的位姿，从而导致的误判。
63.这里需要说明的是，虽然第一实时图像对应的是标志物，第二实时图像对应的是待抓取物，但由于标志物添加在相机的拍摄区域，故在第二实时图像中也具有标志物的图像，因此可以用于上述判断。
64.进一步的，上述判断“判断相机的位姿是否发生变化”应有两种情况，第一种情况为相机的位姿未发生变化，第二种情况为相机的位姿发生变化，其中，对相机的位姿未发生变化的情况，其无需对相机的外参矩阵进行补偿，此时上述机械臂抓取鲁棒性优化方法包括：
65.依据第二实时图像确定待抓取物在相机坐标系下的位姿；
66.依据相机当前的外参矩阵计算待抓取物处于抓取装置坐标系下的位姿；
67.其中，相机的外参矩阵为相机坐标系与抓取装置坐标系之间的对应关系。
68.该部分内容为相机的位姿未发生变化时的流程，该情况下由于不需要对外参矩阵进行补偿，因此在依据第二实时图像确定出待抓取物在相机坐标系下的位姿后，即可直接用于计算待抓取物在抓取装置坐标系下的位姿，得到该位姿信息后可以直接通过抓取装置对待抓取物进行抓取。
69.相应的，对相机的位姿发生变化的情况，此时若直接利用相机的外参矩阵计算待抓取物与抓取装置坐标系上的位姿，由于此时外参矩阵的变化，其会导致计算结果出现偏差，可能导致后续抓取过程失败。因此，在该情况下，需要先对相机的外参矩阵进行补充，其包括以下步骤：
70.获取相机处于初始状态下的基准位姿、以及处于当前状态下的实时位姿；
71.依据最小二乘法对基准位姿和实时位姿进行拟合，确定实时位姿相对于基准位姿的变换关系；
72.依据变换关系对相机的外参矩阵进行补偿。
73.其中，基准位姿未相机处于初始状态下的位姿，该情况下相机的位姿还未发生变化，实时位姿为相机当前的位姿，由于此前进行过相机位姿是否发生变化的判断，且判定为是，则该情况下的实时位姿为相机的位姿发生变化后的位姿。通过比较基准位姿和实时位姿之间的差异，即可确定相机的位姿具体发生的变化，并依据该变化对相机的外参矩阵进
行补偿，即可避免外参矩阵变换导致抓取失败的问题。其中该补偿的方法是先通过最小二乘法对基准位姿和实施位姿进行拟合，确定出实时位姿相对于基准位姿的变换关系，在依据该变化关系对相机的外参矩阵进行补偿即可。
74.其中，最小二乘法(又称最小平方法)是一种数学优化技术。通过最小化误差的平方和寻找数据的最佳函数匹配，利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小，最小二乘法还可用于曲线拟合。
75.在对相机的外参矩阵进行补偿后，即可用补偿后的外参矩阵对待抓取物的位姿进行更新，以用于后续抓取工作，具体的，该过程包括以下步骤：
76.依据第二实时图像确定待抓取物在相机坐标系下的位姿；
77.依据进行补偿后的外参矩阵计算待抓取物处于抓取装置坐标系下的位姿；
78.依据计算的待抓取物处于抓取装置坐标系下的位姿对待抓取物的位姿进行更新。
79.该更新的过程与此前相机的位姿未发生变化时待抓取物处于抓取装置坐标系下的位姿的获取步骤相同，只是所引用的外参矩阵发生了变化。
80.本发明通过上述流程，对外参矩阵进行补偿，并对待抓取物所处的抓取装置坐标系下的位姿进行更新，可以避免识别出错抓取失败的情形发生。
81.进一步的，本发明在完成对任一待抓取物的抓取后，上述机械臂抓取鲁棒性优化方法还包括：
82.判断在对待抓取物进行抓取前后，第二实时图像中是否存在位姿未变化的待抓取物；
83.若是，则抓取位姿未变化的待抓取物；
84.若否，则判定待抓取物已全被抓取，并结束抓取动作。
85.该步骤主要用于多个待抓取物的情形，用于判断待抓取物是否抓取完成，其中，待抓取物抓取前的第二实时图像中包括有所有的待抓取物的位姿信息，待抓取物抓取后的第二实时图像中包括有所有未抓取的待抓取物的位姿信息，两者的差异仅在待抓取物抓取后的第二实时图像中缺少了被抓取的待抓取物，因此比较待抓取物进行抓取前后，第二实时图像中是否存在位姿未变化的待抓取物，即可确定是否还有未被抓取的待抓取物，这里，当判定为是时，则表明还有待抓取物未被抓取，且未被抓取的待抓取物为位姿未变化的待抓取物，因此直接抓取位姿未变化的待抓取物即可。反之，当判定为否时，则表明此时待抓取物已经被全部抓取完毕，此时需要结束抓取动作。
86.其中，上述抓取步骤中，抓取位姿未变化的待抓取物，包括：
87.获取抓取前第二实时图像中位姿未变化的待抓取物，并确定位姿未变化的待抓取物在相机坐标系下的位姿；
88.依据进行补偿后的外参矩阵计算位姿未变化的待抓取物处于待抓取物坐标系下的位姿；
89.对位姿未变化的待抓取物进行抓取。
90.这里，由于位姿未发生变化的待抓取物在第二实时图像中并无变化，因此可以通过获取抓取前第二实时图像中位姿未变化的待抓取物，即可确定位姿未变化的待抓取物在相机坐标系下的位姿。获取该位姿信息后，剩余的抓取过程与前面的抓取过程相同，这里不再赘述。
91.进一步的，本发明中所指出的预设策略为：
92.在待抓取物设置为多个时，对待抓取物的优先级进行划分；
93.以待抓取物的优先级从高至低依次抓取待抓取物；
94.其中，待抓取物的优先级依据待抓取物在待抓取物的集合中的位置、以及相互接触的待抓取物的数量确定。
95.其中，对多个待抓取物的情况，由于先抓取中间部分的待抓取物可能会对其它抓取物的位姿造成影响，因此本发明对抓取物的优先级进行了划分，将待抓取物与待抓取物的集合中最边缘位置、且相互接触的待抓取物的数量最少的待抓取物设置为最高等级，这样就可以依据对其他待抓取物影响能力从低到高进行排序，并从低到高进行抓取。需要说明的是，在实际抓取过程中，由于待抓取物会被抓取的越来越少，因此中间部分的待抓取物对其他待抓取物的位姿造成的影响也会越小，也即通过上述方式，可以保证在所有待抓取物的抓取过程中，均不会对周围的待抓取物造成影响。
96.进一步的，本发明中采用的标志物的二维码。
97.在本发明其他实施例中，也可以采用其他标志物进行替代，其只要能用于判断相机的位姿是否变化，均应处于本发明的保护范围内。
98.请参见图1，其为本发明的整体流程，其包括步骤s101～s107，其中步骤s101中获取模板图像的步骤也即前文中第一实时图像的获取步骤；
99.步骤s102中获取实时图像的动作也即前文中第二实时图像的获取步骤(这里在前文有指出过，第二实时图像虽然与待抓取物对应，但其也包括有标志物，也即步骤s102中获取标志物计划和待抓取物的实时图像)；
100.步骤s103中对比模板图像和实施图像中标志物集合的位姿，也即上述步骤中根据第一实时图像以及第二实时图像判断相机的位姿是否发生变化；
101.步骤s104和步骤s105，也即上述步骤中在判定相机的位姿发生变化时，对相机的外参矩阵进行补偿的步骤；
102.步骤s106也即上述步骤中，对待抓取物的位姿进行处理，并进行抓取的步骤。
103.步骤107为多个待抓取物情况下，进行重复抓取的步骤。
104.基于上述机械臂抓取鲁棒性优化方法，本发明还提出了一种抓取装置，其包括：相机，其用于对标志物以及待抓取物进行实时拍摄，并获取第一实时图像以及第二实时图像；
105.机械臂，其用于抓取待抓取物；
106.主控单元，其存储有相机的外参矩阵，并用于根据第一实时图像和第二实时图像确定待抓取物的位姿。
107.与现有技术相比，本发明通过增加标志物，并实时更新待抓取物位姿的方法，避免了在抓取过程中，因相机位姿发生变化，从而导致识别出错抓取失败的情形发生，同时可以对相机位姿的变换进行实时矫正，多个标志物的设置避免了误矫正的可能性。
108.以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所做的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种机械臂抓取鲁棒性优化方法，其特征在于，包括：在相机拍摄时，为所述相机的拍摄区域添加多个标志物；对所述标志物以及待抓取物进行实时拍摄，获取所述标志物对应的第一实时图像、以及所述待抓取物对应的第二实时图像；根据所述第一实时图像以及所述第二实时图像判断所述相机的位姿是否发生变化，并在判定所述相机的位姿发生变化时，对相机的外参数矩阵进行补偿，并更新所述待抓取物的位姿；以预设策略抓取所述待抓取物。2.根据权利要求1所述的机械臂抓取鲁棒性优化方法，其特征在于，根据所述第一实时图像以及所述第二实时图像判断所述相机的位姿是否发生变化，包括：获取所述第一实时图像、以及所述第二实时图像中任意两个标志物之间的相对位置；判断所述相对位置是否发生变化，若否，则判断所述标志物于所述第一实时图像以及所述第二实时图像中的位姿是否相同；在判定所述第一实时图像以及所述第二实时图像中所述标志物的位姿不同时，判定所述相机的位姿发生变化。3.根据权利要求2所述的机械臂抓取鲁棒性优化方法，其特征在于，在判定所述第一实时图像以及所述第二实时图像中的所述标志物的位姿相同时，所述机械臂抓取鲁棒性优化方法还包括：依据所述第二实时图像确定所述待抓取物在相机坐标系下的位姿；依据所述相机当前的外参矩阵计算所述待抓取物处于抓取装置坐标系下的位姿；其中，所述相机的外参矩阵为相机坐标系与抓取装置坐标系之间的对应关系。4.根据权利要求1所述的机械臂抓取鲁棒性优化方法，其特征在于，对相机的外参矩阵进行补偿，包括：获取所述相机处于初始状态下的基准位姿、以及处于当前状态下的实时位姿；依据最小二乘法对所述基准位姿和实时位姿进行拟合，确定所述实时位姿相对于所述基准位姿的变换关系；依据所述变换关系对所述相机的外参矩阵进行补偿。5.根据权利要求1所述的机械臂抓取鲁棒性优化方法，其特征在于，更新所述待抓取物的位姿，包括：依据所述第二实时图像确定所述待抓取物在相机坐标系下的位姿；依据进行补偿后的所述外参矩阵计算所述待抓取物处于抓取装置坐标系下的位姿；依据计算的所述待抓取物处于抓取装置坐标系下的位姿对所述待抓取物的位姿进行更新。6.根据权利要求1所述的机械臂抓取鲁棒性优化方法，其特征在于，在完成对任一待抓取物的抓取后，所述机械臂抓取鲁棒性优化方法还包括：判断在对所述待抓取物进行抓取前后，所述第二实时图像中是否存在位姿未变化的待抓取物；若是，则抓取位姿未变化的待抓取物；若否，则判定所述待抓取物已全被抓取，并结束抓取动作。
7.根据权利要求6所述的机械臂抓取鲁棒性优化方法，其特征在于，抓取位姿未变化的待抓取物，包括：获取抓取前所述第二实时图像中位姿未变化的待抓取物，并确定位姿未变化的待抓取物在相机坐标系下的位姿；依据进行补偿后的所述外参矩阵计算位姿未变化的待抓取物处于待抓取物坐标系下的位姿；对位姿未变化的待抓取物进行抓取。8.根据权利要求1所述的机械臂抓取鲁棒性优化方法，其特征在于，所述标志物采用二维码。9.根据权利要求1所述的机械臂抓取鲁棒性优化方法，其特征在于，所述预设策略为：在所述待抓取物设置为多个时，对所述待抓取物的优先级进行划分；以所述待抓取物的优先级从高至低依次抓取所述待抓取物；其中，所述待抓取物的优先级依据所述待抓取物在待抓取物的集合中的位置、以及相互接触的待抓取物的数量确定。10.一种采用如权利要求1至9任意一项权利要求所述的机械臂抓取鲁棒性优化方法的抓取装置，其特征在于，包括：相机，其用于对标志物以及待抓取物进行实时拍摄，并获取第一实时图像以及第二实时图像；机械臂，其用于抓取待抓取物；主控单元，其存储有相机的外参矩阵，并用于根据第一实时图像和第二实时图像确定待抓取物的位姿。

技术总结
本发明公开了一种机械臂抓取鲁棒性优化方法及抓取装置，所述机械臂抓取鲁棒性优化方法，包括：在相机拍摄时，为所述相机的拍摄区域添加多个标志物；对所述标志物以及待抓取物进行实时拍摄，获取所述标志物对应的第一实时图像、以及所述待抓取物对应的第二实时图像；根据所述第一实时图像以及所述第二实时图像判断所述相机的位姿是否发生变化，并在判定所述相机的位姿发生变化时，对相机的外参数矩阵进行补偿，并更新所述待抓取物的位姿；以预设策略抓取所述待抓取物。与现有技术相比，本发明有效避免了在抓取过程中，相机位姿发生变化，从而导致识别出错抓取失败的情形。从而导致识别出错抓取失败的情形。从而导致识别出错抓取失败的情形。


技术研发人员：杨仕涛 郑铭岳 吴紫鑫 郭盼盼
受保护的技术使用者：深圳职业技术学院
技术研发日：2023.05.30
技术公布日：2023/8/13