一种基于相机测量的高遮挡率物体位姿估计方法与流程

1.本发明属于机器视觉领域，尤其涉及一种基于相机测量的高遮挡率物体位姿估计方法。


背景技术：

2.以机器人科技为代表的智能产业蓬勃兴起，成为现时代科技创新的一个重要标志。机器视觉作为通过测量实现工业生产的关键技术，衍生出了很多应用领域，其中用光学仪器测量物体特征和位姿，实现物体位姿估计成为视觉领域在实际生产中的一项热门课题。
3.目前物体位姿估计系统包含视觉测量、点云生成、点云分隔、模型建立、模板匹配等技术。
4.现有技术方案中国专利cn113246140b，根据该专利描述用到了位姿估计中的技术包括视觉测量、点云生成、完整模型、子模型、模板匹配等技术；首先视觉测量是根据双目或双目加投影拍摄一系列图片，然后根据测量算法生成场景点云或者叫目标点云，然后用一个模型在点云里找与模型相似的部分，这个技术叫模板匹配，但是模板匹配有分数设定，主要是表面积匹配度，有些角度表面积匹配度大，但有些角度表面积匹配度小会造成匹配失败，比如杨桃在某个角度看就是个五角星，那么就引申出部分模型即子模型，部分模型就是只做杨桃的五角星部分专门为了解决场景中只有五角星的情况；
5.对点云处理和匹配用到完整点云与部分点云建模技术，但其应用范围有限，是同一种工件情况下无遮挡的位姿估计，如果存在其它物品挡住目标物体则匹配大概率会失败，并且点云匹配部分建模的生成规则为工件稳定状态下相机视角可能出现的情况，对形状复杂的物体适用范围有限。


技术实现要素：

6.针对现有技术的不足，本发明设计了一种基于相机测量的高遮挡率物体位姿估计方法。
7.一种基于相机测量的高遮挡率物体位姿估计方法，具体包括以下步骤：
8.步骤1：对场景中出现的所有物体分别进行建模；分析物体特征，根据物体几何特征提取若干子模型，若干子模型与物体的完整模型组成一个子模型组，并计算物体每个子模型与完整模型的相对位姿关系；
9.使用光学仪器对参与位姿估计的物体进行测量，生成物体对应的完整模型，针对完整模型建立若干子模型，若干子模型与完整模型构成一个子模型组，将场景中的所有物体都进行上述操作，得到所有物体的子模型组；其中，子模型的建立具体为以下步骤：
10.将一个完整模型视作一个有内外区分的三维连通域，通过计算完整模型所有点坐标(x，y，z)平均值获得完整模型的中心点，若中心点落在完整模型范围外，则根据完整模型实际几何情况将中心点设置在完整模型内部；
11.在三维连通域内部，取距离中心点路程较远的几个子模型为备选点，备选点的确定与其距离中心点路程有关和也与备选点附近其它子模型备选点的情况有关；
12.将符合条件的子模型备选点确立为子模型点后，以该模型点为球心，r为半径做球，将该模型末端包住，球内部即为子模型，所有子模型与完整模型本身构成子模型组，计算所有子模型与完整模型的相对位姿关系subpose矩阵，最终一个物体的子模型组结构如下：
13.{(完整模型，四阶单位矩阵)，
14.(子模型1，subpose1矩阵)，
15.(子模型2，subpose2矩阵)...
16.(子模型n，subposen矩阵)}
17.一个位姿subpose矩阵包括三阶旋转矩阵r和三维平移向量t，rt矩阵为四阶矩阵；
18.步骤2：对比同一物体子模型组内各子模型，分析子模型特征，合并相似度高的子模型；首先对同一组内子模型进行去重复，如果子模型组内两个以上子模型相似度极高则合并子模型，合并后的子模型组为：{...(子模型左手，subpose左手矩阵,subpose右手矩阵)...}；
19.步骤3：对比不同物体子模型组内各子模型，分析子模型特征，对子模型进行评级；设子模型组内完整模型为1级，若存在不同组之间子模型高度相似的情况，则各相似子模型评为3级，反之为2级；1级2级模型对物体识别重要作用，基于模板匹配根据1级2级模型识别情况确定物体种类与位姿，3级模型无法单独确定物体种类与位姿，需要与2级模型配合使用；
20.匹配过程使用模板匹配方法，首先使用1级模型进行匹配，匹配成功后使用2、3级模型匹配成功，如果匹配成功，则需要同一物体所有子模型组中2、3级模型与物体进行匹配；
21.步骤4：根据各子模型组中子模型的特征对子模型进行置信度设置，1级模型置信度100％，2级3级模型置信度根据模型特征设置；
22.其中，2、3级模型置信度取值受以下因素影响：
23.(1)子模型包含物体特征的情况，如果物体子模型是动物头部的置信度分数要高；
24.(2)面积占比越大所包含的信息越多，因此其它情况相同的面积大的子模型置信度也应比面积较少的子模型高；
25.(3)曲率变化越激烈的所包含的信息越多；
26.所以最后模型组完整信息为：
27.{(完整子模型，四阶单位矩阵，置信度100％，1级)
28.(子模型1，subpose11，subpose12，
…
，subpose1m，置信度p1，2级)
29.(子模型2，subpose21，置信度p2，3级)
30.…
31.(子模型n，subposen1，subposen2，置信度pn，2级)}
32.其中，置信度∑pi《＝1；
33.步骤5：使用光学相机对工作区域进行拍摄测量，得到工作区域的三维点云，记作场景点云；对场景点云按距离进行区域聚类，将场景点云分割为许多点云：目标点云1，目标
点云2，
…
，目标点云n；
34.步骤6：根据目标物体，挑选对应的子模型组对步骤5中所有目标点云进行模板匹配，并对位姿进行聚类，计算位姿置信度；
35.进行模板匹配的方法具体为：
36.根据目标物体子模型组中的所有子模型，使用目标点云与其进行匹配，即在目标点云中寻找与子模型特征相似的比例，根据相似特征进行模型位姿计算，同一位姿，占比越高，得分越高，最高得分是1，最低得分是0；
37.对目标点云1分别与所有子模型组中所有子模型进行模板匹配，得到对应的匹配分数score11，score12
…
score1n与绝对匹配位姿pose11，pose12
…
pose1n；
38.对目标点云2也进行同样操作得到匹配分数score21，score22
…
score2n与匹配位姿pose21，pose22
…
pose2n；
39.其他目标点云也如此操作最后所有目标点云都会有对应的匹配模型与匹配分数，每一个匹配模型对应一个或多个相对位姿，用绝对匹配位姿*相对位姿得到完整模型位姿,即：pose11*subpose11，每一个得出得模型相对位姿也要用匹配分数*置信度得到完整模型位姿分数,即score11*p1；
40.步骤7：对步骤6得到的位姿聚类结果进行排序处理与重复部分判定处理，还原真实物体位姿；
41.将所有完整模型相同的位姿进行合并，对应完整模型位姿分数相加，这个和一定不大于1，因为1代表模型与场景完全重合，如果一个位姿对应模型中存在完整子模型则匹配成功，如果一个位姿只有3级模型匹配成功则无法明确是何种物体，分数归0，如果一个位姿对应至少一个2级匹配模型则认为匹配成功，对模型进行排序，如有模型交叉部分则交叉部分归高分位姿所有，对应低分位姿分数归0，如此还原完整物体位姿；
42.物体位姿最后得分finalsocre最后得分为：
43.finalsocre＝∑pi*scorejk(i，j，k＝1，2
…
n)
44.根据需要计算位姿的物体的种类，循环执行步骤5-7，直到所有需要位姿估计目标都计算一遍后工作结束。
45.本发明有益技术效果：
46.通过对测量生成的三维点云进行处理，发明一种方法使被遮挡物体位姿估计速度更快与位姿估计更准确。现有技术只对俯视角度进行子模型提取，并没有对子模型提取进行过多的思考，所以应用范围比较局限，只针对同一物品适用，且对互相遮挡中被遮挡物体识别率差，本发明可以对多种混合物体提供位姿匹配方法，而且对互相遮挡中被遮挡物体识别有良好效果。
附图说明
47.图1本发明实施例一种基于相机测量的高遮挡率物体位姿估计方法的流程图；
48.图2本发明实施例子模型组内各1级2级3级模型示意图；其中图a为1级模型，图b-f为2级模型，图g为3级模型，图h和图i共同构成子模型组；
49.图3本发明实施例被分割为多个目标点云得场景点云。
具体实施方式
50.下面根据附图和实施例进一步介绍本发明：
51.一种基于相机测量的高遮挡率物体位姿估计方法，如附图1所示，具体包括以下步骤：
52.步骤1：对场景中出现的所有物体分别进行建模；分析物体特征，根据物体几何特征提取若干子模型，若干子模型与物体的完整模型组成一个子模型组，并计算物体每个子模型与完整模型的相对位姿关系；
53.使用光学仪器对参与位姿估计的物体进行测量，生成物体对应的完整模型，针对完整模型建立若干子模型，若干子模型与完整模型构成一个子模型组，将场景中的所有物体都进行上述操作，得到所有物体的子模型组；其中，子模型的建立具体为以下步骤：
54.将一个完整模型视作一个有内外区分的三维连通域，通过计算完整模型所有点坐标(x，y，z)平均值获得完整模型的中心点，若中心点落在完整模型范围外(如香蕉，月亮等模型)则根据完整模型实际几何情况将中心点设置在完整模型内部；
55.在三维连通域内部，取距离中心点路程较远的几个子模型为备选点，备选点的确定与其距离中心点路程有关和也与备选点附近其它子模型备选点的情况有关；下面举例说明：
56.比如人没有尾巴那就是5个备选点，如果是蜻蜓，则四个翅膀、脑袋、尾巴、加上6个爪子就是12个备选点，如果是鱼，分别有鱼头、鱼尾、鱼鳍等等，有的鱼尾巴还有两个，根据不同物体的实际情况来确定备选点；
57.以一个副栉龙模型为例解释，副栉龙模型中心点在腹部，则头顶、双手中指、双脚中指、尾尖为6个较远端点，都为子模型备选点，若尾尖备选点不与其他部位(手，头，手，脚，后背，前胸)连在一起则选取尾尖备选点确定为子模型点，如果尾尖与背部贴在一起则因为与中心点路程较近，不作为子模型点；同理双手如果握在一起形成一个环则双手中指两个备选点距离过近只取一个点，或双手贴在胸前则与尾尖一样因与中心距离过近不作为子模型点，双手若前伸则满足条件，左右手中指都设为子模型点。
58.将符合条件的子模型备选点确立为子模型点后，以该模型点为球心，r为半径做球，将该模型末端包住(r的大小根据实际情况确定)，球内部即为子模型，所有子模型与完整模型本身构成子模型组，计算所有子模型与完整模型的相对位姿关系subpose矩阵，最终一个物体的子模型组结构如下：
59.{(完整模型，四阶单位矩阵)，
60.(子模型1，subpose1矩阵)，
61.(子模型2，subpose2矩阵)...
62.(子模型n，subposen矩阵)}
63.一个位姿subpose矩阵包括三阶旋转矩阵r和三维平移向量t，rt矩阵为四阶矩阵；
64.步骤2：对比同一物体子模型组内各子模型，分析子模型特征，合并相似度高的子模型；首先对同一组内子模型进行去重复，如果子模型组内两个以上子模型相似度极高则合并子模型，以恐龙双手为例，若双手形状基本相同则匹配子模型时只匹配其中一个，但算位姿的时候要计算两个,合并后的子模型组为：{...(子模型左手，subpose左手矩阵,subpose右手矩阵)...}；
65.步骤3：对比不同物体子模型组内各子模型，分析子模型特征，对子模型进行评级；设子模型组内完整模型为1级，若存在不同组之间子模型高度相似的情况，则各相似子模型评为3级，反之为2级；1级2级模型对物体识别重要作用，基于模板匹配根据1级2级模型识别情况确定物体种类与位姿，3级模型无法单独确定物体种类与位姿，需要与2级模型配合使用；子模型组内各1级2级3级模型如附图2所示；
66.例如副栉龙与霸王龙尾巴形状相似，则匹配成功一个尾巴后无法确定是那种物体，所以将两种物体的尾巴都设为3级子模型；
67.匹配过程使用模板匹配方法，首先使用1级模型进行匹配，匹配成功后使用2、3级模型匹配成功，如果匹配成功，则需要同一物体所有子模型组中2、3级模型与物体进行匹配；
68.步骤4：根据各子模型组中子模型的特征对子模型进行置信度设置，1级模型置信度100％，2级3级模型置信度根据模型特征设置；
69.其中，2、3级模型置信度取值受以下因素影响：
70.(1)子模型包含物体特征的情况，例如副栉龙之所以叫副栉龙是因为头上有冠，这是一个很明显的特征，大部分动物的头部特征都很明显，所以如果物体子模型是动物头部的置信度分数要高；
71.(2)面积占比越大所包含的信息越多，因此其它情况相同的面积大的子模型置信度也应比面积较少的子模型高；
72.(3)曲率变化越激烈的所包含的信息越多，比如副栉龙同等面积的手脚一定比同等面积的尾部置信度高；
73.所以最后模型组完整信息为：
74.{(完整子模型，四阶单位矩阵，置信度100％，1级)
75.(子模型1，subpose11，subpose12，
…
，subpose1m，置信度p1，2级)
76.(子模型2，subpose21，置信度p2，3级)
77.…
78.(子模型n，subposen1，subposen2，置信度pn，2级)}
79.其中，置信度∑pi《＝1；
80.步骤5：使用光学相机对工作区域进行拍摄测量，得到工作区域的三维点云，记作场景点云；对场景点云按距离进行区域聚类，将场景点云分割为许多点云：目标点云1，目标点云2，
…
，目标点云n；场景点云与目标点云示意图如附图3所示；
81.步骤6：根据目标物体，挑选对应的子模型组对步骤5中所有目标点云进行模板匹配，并对位姿进行聚类，计算位姿置信度；
82.进行模板匹配的方法具体为：
83.根据目标物体子模型组中的所有子模型，使用目标点云与其进行匹配，即在目标点云中寻找与子模型特征相似的比例，根据相似特征进行模型位姿计算，同一位姿，占比越高，得分越高，最高得分是1，最低得分是0；匹配阈值一般设置在0.2-0.4之间；
84.对目标点云1分别与所有子模型组中所有子模型进行模板匹配，得到对应的匹配分数score11，score12
…
score1n与绝对匹配位姿pose11，pose12
…
pose1n；
85.对目标点云2也进行同样操作得到匹配分数score21，score22
…
score2n与匹配位
姿pose21，pose22
…
pose2n；
86.其他目标点云也如此操作最后所有目标点云都会有对应的匹配模型与匹配分数，每一个匹配模型对应一个或多个相对位姿，用绝对匹配位姿*相对位姿得到完整模型位姿,即：pose11*subpose11，每一个得出得模型相对位姿也要用匹配分数*置信度得到完整模型位姿分数,即score11*p1；
87.步骤7：对位姿聚类结果进行排序处理与重复部分判定处理，还原真实物体位姿；
88.将所有完整模型相同的位姿进行合并，对应完整模型位姿分数相加，这个和一定不大于1，因为1代表模型与场景完全重合，如果一个位姿对应模型中存在完整子模型则匹配成功，如果一个位姿只有3级模型匹配成功则无法明确是何种物体，分数归0，如果一个位姿对应至少一个2级匹配模型则认为匹配成功，对模型进行排序，如有模型交叉部分则交叉部分归高分位姿所有，对应低分位姿分数归0，如此还原完整物体位姿；
89.物体位姿最后得分finalsocre最后得分为：
90.finalsocre＝∑pi*scorejk(i，j，k＝1，2
…
n)
91.根据需要计算位姿的物体的种类，循环执行步骤5-7，直到所有需要位姿估计目标都计算一遍后工作结束。