基于3D模板匹配的机器人角度定位精度测试方法和系统与流程

基于3d模板匹配的机器人角度定位精度测试方法和系统
技术领域
1.本发明涉及机器人检测技术领域，具体涉及一种基于3d模板匹配的机器人角度定位精度测试方法和一种基于3d模板匹配的机器人角度定位精度测试系统。


背景技术：

2.在计算机视觉和机器人感知领域，保证机器人准确定位是实现自动化生产的关键。诸如机器人搬运、切割、打磨等场景，均需要保证机器人准确定位，因此对机器人定位精度进行检测成为了自动化生产过程的重要一环。但是，现有的定位精度检测方案一般是进行模糊的定位精度估计，难以定量检测定位精度，导致定位精度检测的准确性较低。


技术实现要素：

3.本发明为解决上述技术问题，提供了一种基于3d模板匹配的机器人角度定位精度测试方法，能够实现完全自动化测试，从而能够避免人因误差的引入，并且能够通过定量数据反应角度定位精度，从而能够保证测试结果的准确性。
4.本发明采用的技术方案如下：
5.一种基于3d模板匹配的机器人角度定位精度测试方法，包括以下步骤：通过相机获取待测机器人所在场景的3d点云数据；获取所述待测机器人的3d模板；根据所述3d点云数据和所述3d模板识别所述待测机器人；根据识别结果得到所述待测机器人的定位角度数据；控制所述待测机器人变换场景以得到所述待测机器人的定位角度数据集；根据所述定位角度数据集判断所述待测机器人的角度定位精度。
6.根据本发明的一个实施例，所述根据所述3d点云数据和所述3d模板识别所述待测机器人，具体包括以下步骤：获取所述3d模板任意旋转角度的参考特征直方图集；根据所述参考特征直方图集和所述3d点云数据识别所述待测机器人。
7.根据本发明的一个实施例，所述3d模板为待测机器人特征结构的3d模板，通过识别所述特征结构识别所述待测机器人。
8.根据本发明的一个实施例，所述根据所述参考特征直方图集和所述3d点云数据识别所述待测机器人，具体包括以下步骤：构建3d模板窗，其中，所述3d模板窗包围所述3d模板；构建3d点云立方体，其中，所述3d点云立方体包裹所述3d点云数据中的每一个点；构建所述3d模板窗在所述3d点云立方体中的移动路径，其中，所述移动路径包括多个位置点；根据所述移动路径控制所述3d模板窗在所述3d点云立方体中移动，以得到所述3d模板窗在每个所述位置点的测试特征直方图；根据所述测试特征直方图和所述参考特征直方图集识别所述待测机器人特征结构。
9.根据本发明的一个实施例，所述根据所述测试特征直方图和所述参考特征直方图集识别所述待测机器人，具体包括以下步骤：计算每个所述位置点对应的所述测试特征直方图和所述参考特征直方图集中各特征直方图的相似度；判断所述相似度是否大于设定阈值；若是，则判断识别到所述待测机器人，并选取所述相似度最高的位置点对应的所述测试
特征直方图来表征所述待测机器人特征结构。
10.根据本发明的一个实施例，所述根据识别结果得到所述待测机器人的定位角度数据，具体包括以下步骤：根据所述识别结果得到所述待测机器人的特征结构的姿态数据；根据所述姿态数据得到所述待测机器人特征结构的定位角度数据。
11.根据本发明的一个实施例，所述根据所述姿态数据得到所述待测机器人特征结构的定位角度数据，具体包括以下步骤：根据所述姿态数据得到所述待测机器人特征结构的测试方向向量；以所述相机坐标系的z轴正向方向向量作为所述待测机器人特征结构的参考方向向量；以所述测试方向向量与所述参考方向的夹角作为所述待测机器人特征结构的定位角度数据。
12.根据本发明的一个实施例，所述根据所述定位角度数据集判断所述待测机器人的角度定位精度，具体包括以下步骤：计算所述定位角度数据集的方差；根据所述方差判断所述待测机器人的角度定位精度。
13.根据本发明的一个实施例，所述待测机器人特征结构为所述待测机器人的上层壳体。
14.一种基于3d模板匹配的机器人角度定位精度测试系统，包括：获取模块，所述获取模块用于获取待测机器人所在场景的3d点云数据；运动控制模块，所述运动控制模块用于控制所述待测机器人变换场景；计算处理模块，所述计算处理模块用于获取所述待测机器人的3d模板，并根据所述3d点云数据和所述3d模板识别所述待测机器人，然后根据识别结果得到所述待测机器人的定位角度数据，此外，所述计算处理模块还用于计算所述待测机器人在不同场景的定位角度数据集，并根据所述定位角度数据集判断所述待测机器人的角度定位精度。
15.本发明的有益效果如下：
16.1)、本发明能够实现完全自动化测试，从而能够避免人因误差的引入，进而能够提高测试数据的可靠性和测试结果的准确性；
17.2)、本发明能够自动采集数据样本，从而能够为数据分析提供支持，进而能够降低定位精度的统计误差；
18.3)、本发明能够通过方差数据量化角度定位精度的结果，从而能够实现机器人角度定位精度的定量测试。
附图说明
19.图1(a)为现有技术机器人上层壳体的左视图；
20.图1(b)为现有技术机器人上层壳体的主视图；
21.图1(c)为现有技术机器人上层壳体的俯视图；
22.图2为本发明实施例的基于3d模板匹配的机器人角度定位精度测试方法的流程图；
23.图3为本发明一个实施例的测试平台的布局示意图；
24.图4(a)为本发明一个实施例的3d模板以及其对应的特征直方图；
25.图4(b)为本发明另一个实施例的3d模板以及其对应的特征直方图；
26.图5为本发明一个实施例的根据参考特征直方图和3d点云数据识别待测机器人的
流程图；
27.图6为本发明实施例的基于3d模板匹配的机器人角度定位精度测试系统的方框示意图；
28.图7为本发明另一个实施例的测试平台的布局示意图。
具体实施方式
29.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
30.需要说明的是，本发明基于以下认知：如图1(a)、图1(b)和图1(c)所示，机器人的特征结构，即上层壳体的几何外形是简单固定的，因此可通过识别机器人的特征结构，即上层壳体的位置和姿态作为机器人的位置和姿态。下面将结合图2和图6具体阐述本发明的基于3d模板匹配的机器人角度定位精度测试方法和系统。
31.如图2所示，本发明的基于3d模板匹配的机器人角度定位精度测试方法，包括以下步骤：
32.s1，通过相机获取待测机器人所在场景的3d点云数据。
33.具体地，可构建测试平台以获取待测机器人所在场景的3d点云数据。其中，如图3所示，测试平台可包括3d视觉相机放置区a和测试区b，并且测试区b可包括第一测试区b1和第二测试区b2，当待测机器人位于测试区b，例如位于第一测试区b1时，可通过3d视觉相机获取待测机器人所在场景，即第一测试区b1的3d点云数据。
34.s2，获取待测机器人的3d模板。
35.具体地，可获取待测机器人特征结构，即上层壳体的3d模板。
36.s3，根据3d点云数据和3d模板识别待测机器人。
37.具体地，可先调整3d模板的坐标系与3d视觉相机的坐标系重合，然后可获取3d模板任意旋转角度的参考特征直方图，最后可根据参考特征直方图和3d点云数据识别待测机器人。其中，需要说明的是，3d模板的参考特征直方图具体为3d模板的法线集合的统计图，如图4(a)和图4(b)所示；进一步需要参照图4(a)和图4(b)说明的是，3d模板的法线可为三元向量，因此3d模板上可存在一系列法线，从而形成法线集合，由此，当对3d模板的法线集合进行统计时，可采用直方图进行表示，从而可得到对应的特征直方图，本发明将其定义为参考特征直方图。
38.此外，还需要说明的是，为获取3d模板任意旋转角度的参考特征直方图集，可将待测机器人上层壳体旋转不同角度，并对应每一个旋转角度制作一个3d模板，进而提取每一个旋转角度的3d模板的参考特征直方图。
39.更具体地，如图5所示，根据参考特征直方图和3d点云数据识别待测机器人，还可包括以下步骤：
40.s301，构建3d模板窗，其中，3d模板窗包围3d模板。
41.s302，构建3d点云立方体，其中，3d点云立方体包裹3d点云数据中的每一个点。
42.s303，构建3d模板窗在3d点云立方中的移动路径，其中，移动路径包括多个位置
点。
43.具体地，可根据3d点云立方中包裹的3d点云数据构建移动路径，例如可将3d点云数据中的每一个点作为位置点构建移动路径。
44.s304，根据移动路径控制3d模板窗在3d点云立方体中移动，以得到3d模板窗在每个位置点的测试特征直方图。
45.具体地，可提取3d模板窗内点的法线集合，并可根据该法线集合得到对应的测试特征直方图。
46.s305，根据测试特征直方图和参考特征直方图集识别3d模板窗所在位置区域是否包含待测机器人上层壳体。
47.具体地，可计算每个位置点对应的测试特征直方图和参考特征直方图集中各特征直方图的相似度，并判断该相似度是否超过特征相似度的设定阈值。其中，若超过特征相似度设定阈值，则可判断3d模板窗所在位置区域包含待测机器人上层壳体并可执行步骤s306；若未超过特征相似度设定阈值，则可判断3d模板窗所在位置区域不包含待测机器人上层壳体，则可控制待测机器人移动至下一测试区，例如第二测试区b2，并重复上述步骤s1-s305。
48.s306，判断3d模板窗所在位置区域为初始目标区域，其中，初始目标区域包括真目标区域和假目标区域。
49.需要说明的是，当3d模板窗在3d点云立方体中遍历所有位置点后，可能得到多个初始目标区域，也就是在一个场景中可能找到多个目标物体，即多个待测机器人上层壳体，显然，得到的初始目标区域中存在一个真目标区域和多个假目标区域。
50.s307，计算初始目标区域的相似度以识别待测机器人上层壳体。
51.具体地，计算所有初始目标区域的相似度，并可将该相似度最高的初始目标区域作为真目标区域保留，其他初始目标区域作为假目标区域剔除；进一步地，可将真目标区域对应的3d模板的位置、姿态和角度作为目标物体，即待测机器人上层壳体的位置、姿态和角度，从而完成待测机器人上层壳体的识别。
52.其中，若未识别到待测机器人上层壳体，则判断识别失败，从而控制待测机器人移动至下一测试区，例如第二测试区b2，并重复上述步骤s1-s307。
53.s4，根据识别结果得到待测机器人的定位角度数据。
54.具体地，可构建待测机器人上层壳体的参考方向向量，并可根据真目标区域对应的3d模板窗得到待测机器人上层壳体的位置数据和姿态数据，进而可根据姿态数据得到待测机器人上层壳体的测试方向向量，然后可根据参考方向向量和测试方向向量得到待测机器人上层壳体的定位角度数据。
55.更具体地，可将3d视觉相机坐标系的z轴正向方向向量作为待测机器人上层壳体的参考方向向量，并可根据识别结果，即3d模板匹配结果得到待测机器人上层壳体的位置数据和姿态数据，进而可根据姿态数据得到待测机器人上层壳体的测试方向向量，例如可将待测机器人上层壳体的姿态的z轴正向方向向量作为待测机器人上层壳体的测试方向向量，然后可计算待测机器人上层壳体的测试方向向量与参考方向向量的夹角，即待测机器人上层壳体的定位角度数据，也就是待测机器人的定位角度数据。
56.s5，控制待测机器人变换场景以得到待测机器人的定位角度数据集。
57.具体地，可控制待测机器人变化场景，例如可移动至第二测试区b2，并重复执行上述步骤s1-s4，以得到待测机器人在第二测试区b2的定位角度数据，从而可根据待测机器人在不同测试区的定位角度数据构成待测机器人的定位角度数据集。
58.s6，根据定位角度数据集判断待测机器人的角度定位精度。
59.具体地，可计算定位角度数据集的方差，然后可根据方差判断待测机器人的角度定位精度。
60.更具体地，可设定位角度数据集为s，并可通过以下公式计算定位角度数据集的方差：
61.可令定位角度数据集s＝{s0,...,s
n-1
}；
62.进一步令期望
63.进一步令标准差
64.进一步令方差
65.其中，n为待测机器人重复测试次数，标准差为机器人角度定位精度测试总体的无偏估计，方差为机器人角度定位精度测试的具体数值。
66.本发明的有益效果如下：
67.1)、本发明能够实现完全自动化测试，从而能够避免人因误差的引入，进而能够提高测试数据的可靠性和测试结果的准确性；
68.2)、本发明能够自动采集数据样本，从而能够为数据分析提供支持，进而能够降低定位精度的统计误差；
69.3)、本发明能够通过方差数据量化角度定位精度的结果，从而能够实现机器人角度定位精度的定量测试。
70.对应上述实施例的基于3d模板匹配的机器人角度定位精度测试方法，本发明还提出了一种基于3d模板匹配的机器人角度定位精度测试系统。
71.如图6所示，本发明实施例的基于3d模板匹配的机器人角度定位精度测试系统，包括获取模块10、运动控制模块20和计算处理模块30。其中，获取模块10用于获取待测机器人所在场景的3d点云数据；运动控制模块20用于控制待测机器人变换场景；计算处理模块30用于获取待测机器人的3d模板，并根据3d点云数据和3d模板识别待测机器人，然后根据识别结果得到待测机器人的定位角度数据，此外，计算处理模块30还用于计算待测机器人在不同场景的定位角度数据集，并根据定位角度数据集判断待测机器人的角度定位精度。
72.具体地，可根据获取模块10、运动控制模块20和计算处理模块30构建测试平台。如图7所示，测试平台可包括3d视觉相机放置区a、测试区b、控制区c、调度区d，其中，测试区b可包括第一测试区b1和第二测试区b2；3d视觉相机放置区a可用于设置获取模块10，即3d视
觉相机；控制区c可用于放置计算处理模块30，即工控机；调度区d可用于放置运动控制模块20，即机器人调度装置。此外，需要说明的是，3d视觉相机放置区a放置的获取模块10可采用工作视野为1000mm*700mm(at 1240mm)、工作距离为1-2m的3d视觉相机；3d视觉相机放置区a还可设置相机配件和相机转接板以用于固定3d视觉相机，并保证3d视觉相机的位置和姿态相对于固定面不变。
73.更具体地，参照图7，计算处理模块30，即工控机可与获取模块10，即3d视觉相机相连，可用于控制获取模块10，即3d视觉相机开启扫描、并获取扫描结果，即待测机器人所在场景的3d点云数据；此外，计算处理模块30，即工控机还可通过无线网络连接运动控制模块20，即机器人调度装置，以接收运动控制模块20，即机器人调度装置的工作调度信息，以及向运动控制模块20，即机器人调度装置发送任务状态信息。其中，工作调度信息包括待测机器人的场景变换信息，例如，若运动控制模块20，即机器人调度装置控制待测机器人由第一测试区b1变换至第二测试区b2，则可将该场景变换信息发送至计算处理模块30，即工控机，以通过计算处理模块30，即工控机控制获取模块10，即3d视觉相机开启扫描、并根据扫描结果，即待测机器人所在场景的3d点云数据进行待测机器人的角度定位精度的计算；任务状态信息包括计算处理模块30，即工控机的计算处理进程，例如，若计算处理模块30，即工控机完成一轮待测机器人的角度定位精度测试过程，则可向运动控制模块20，即机器人调度装置发送当前的计算处理进程，以通过运动控制模块20，即机器人调度装置控制待测机器人变换场景。
74.由此，能够实现完全自动化测试，从而能够避免人因误差的引入，进而能够提高测试数据的可靠性和测试结果的准确性；此外，还能够自动采集数据样本，从而能够为数据分析提供支持，进而能够降低定位精度的统计误差。
75.在本发明的一个实施例中，获取模块10，即3d视觉相机可在待测机器人位于测试区b，例如位于第一测试区b1时，获取待测机器人所在场景，即第一测试区b1的3d点云数据。
76.在本发明的一个实施例中，计算处理模块30可获取待测机器人特征结构，即上层壳体的3d模板。
77.在本发明的一个实施例中，计算处理模块30还可具体用于调整3d模板的坐标系与3d视觉相机的坐标系重合，并获取3d模板任意旋转角度的参考特征直方图，然后根据参考特征直方图和3d点云数据识别待测机器人。其中，需要说明的是，3d模板的参考特征直方图具体为3d模板的法线集合的统计图，如图4(a)和图4(b)所示；进一步需要参照图4(a)和图4(b)说明的是，3d模板的法线可为三元向量，因此3d模板上可存在一系列法线，从而形成法线集合，由此，当对3d模板的法线集合进行统计时，可采用直方图进行表示，从而可得到对应的特征直方图，本发明将其定义为参考特征直方图。
78.此外，还需要说明的是，为获取3d模板任意旋转角度的参考特征直方图集，可将待测机器人上层壳体旋转不同角度，并对应每一个旋转角度制作一个3d模板，进而提取每一个旋转角度的3d模板的参考特征直方图。
79.更具体地，如图5所示，计算处理模块30可根据参考特征直方图和3d点云数据识别待测机器人的过程，可包括以下步骤：
80.s301，构建3d模板窗，其中，3d模板窗包围3d模板。
81.s302，构建3d点云立方体，其中，3d点云立方体包裹3d点云数据中的每一个点。
82.s303，构建3d模板窗在3d点云立方中的移动路径，其中，移动路径包括多个位置点。
83.具体地，可根据3d点云立方中包裹的3d点云数据构建移动路径，例如可将3d点云数据中的每一个点作为位置点构建移动路径。
84.s304，根据移动路径控制3d模板窗在3d点云立方体中移动，以得到3d模板窗在每个位置点的测试特征直方图。
85.具体地，可提取3d模板窗内点的法线集合，并可根据该法线集合得到对应的测试特征直方图。
86.s305，根据测试特征直方图和参考特征直方图集识别3d模板窗所在位置区域是否包含待测机器人上层壳体。
87.具体地，可计算每个位置点对应的测试特征直方图和参考特征直方图集中各特征直方图的相似度，并判断该相似度是否超过特征相似度设定阈值。其中，若超过特征相似度设定阈值，则可判断3d模板窗所在位置区域包含待测机器人上层壳体并可执行步骤s306；若未超过特征相似度设定阈值，则可判断3d模板窗所在位置区域不包含待测机器人上层壳体，则可控制待测机器人移动至下一测试区，例如第二测试区b2，并重复上述步骤s1-s305。
88.s306，判断3d模板窗所在位置区域为初始目标区域，其中，初始目标区域包括真目标区域和假目标区域。
89.需要说明的是，当3d模板窗在3d点云立方体中遍历所有位置点后，可能得到多个初始目标区域，也就是在一个场景中可能找到多个目标物体，即多个待测机器人上层壳体，显然，得到的初始目标区域中存在一个真目标区域和多个假目标区域。
90.s307，计算初始目标区域的相似度以识别待测机器人上层壳体。
91.具体地，计算所有初始目标区域的相似度，并可将该相似度最高的初始目标区域作为真目标区域保留，其他初始目标区域作为假目标区域剔除；进一步地，可将真目标区域对应的3d模板的位置、姿态和角度作为目标物体，即待测机器人上层壳体的位置、姿态和角度，从而完成待测机器人上层壳体的识别。
92.其中，若未识别到待测机器人上层壳体，则判断识别失败，从而控制待测机器人移动至下一测试区，例如第二测试区b2，并重复上述步骤s1-s307。
93.在本发明的一个实施例中，计算处理模块30可具体用于构建待测机器人上层壳体的参考方向向量，并根据真目标区域对应的3d模板窗得到待测机器人上层壳体的位置数据和姿态数据，进而根据姿态数据得到待测机器人上层壳体的测试方向向量，然后根据参考方向向量和测试方向向量得到待测机器人上层壳体的定位角度数据。
94.更具体地，可将3d视觉相机坐标系的z轴正向方向向量作为待测机器人上层壳体的参考方向向量，并可根据识别结果，即3d模板匹配结果得到待测机器人上层壳体的位置数据和姿态数据，进而可根据姿态数据得到待测机器人上层壳体的测试方向向量，例如可将待测机器人上层壳体的姿态的z轴正向方向向量作为待测机器人上层壳体的测试方向向量，然后可计算待测机器人上层壳体的测试方向向量与参考方向向量的夹角，即待测机器人上层壳体的定位角度数据，也就是待测机器人的定位角度数据。
95.在本发明的一个实施例中，运动控制模块20可具体用于控制待测机器人变化场景，例如可移动至第二测试区b2，并重复执行获取模块10和计算处理模块30动作，以得到待
测机器人在第二测试区b2的定位角度数据。由此，可根据待测机器人在不同测试区的定位角度数据构成待测机器人的定位角度数据集。
96.在本发明的一个实施例中，计算处理模块30还可具体用于计算定位角度数据集的方差，并根据方差判断待测机器人的角度定位精度。
97.更具体地，可设定位角度数据集为s，并可通过以下公式计算定位角度数据集的方差：
98.可令定位角度数据集s＝{s0,...,s
n-1
}；
99.进一步令期望
100.进一步令标准差
101.进一步令方差
102.其中，n为待测机器人重复测试次数，标准差为机器人角度定位精度测试总体的无偏估计，方差为机器人角度定位精度测试的具体数值。
103.本发明的有益效果如下：
104.1)、本发明能够实现完全自动化测试，从而能够避免人因误差的引入，进而能够提高测试数据的可靠性和测试结果的准确性；
105.2)、本发明能够自动采集数据样本，从而能够为数据分析提供支持，进而能够降低定位精度的统计误差；
106.3)、本发明能够通过方差数据量化角度定位精度的结果，从而能够实现机器人角度定位精度的定量测试。
107.在本发明的描述中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。“多个”的含义是两个或两个以上，除非另有明确具体的限定。
108.在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
109.在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第
一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。
110.在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。