一种基于点云位姿估计的随机堆放工件定位方法

1.本发明属于工件定位技术领域，尤其涉及一种基于点云位姿估计的随机堆放工件定位方法。

背景技术：

2.在工件自动化组装的过程中，需要通过机械手对工件进行识别和抓取操作。基于视觉的工业机器人的应用提高了机械手的智能化水平、提高生产效率，可用于工件识别、工件定位等。然而，对于处于堆叠状态、位姿各异且体积较小的工件而言，现有的视觉定位方法的定位精度有限、位姿识别度不高，报错率高，无法满足实际的生产需求。

技术实现要素：

3.本发明的目的在于提供一种基于点云位姿估计的随机堆放工件定位方法，旨在解决随机抓取工件过程中的位姿估计问题。
4.本发明是这样实现的，一种基于点云位姿估计的随机堆放工件定位方法，该方法包括以下步骤：
5.s1、使用数字建模软件获取工件3d点云数据，剔除部分冗余数据；
6.s2、计算步骤s1中工件点云数据法线，并约束法线方向统一指向模型外部，对带有法线的点云下采样，根据每个点的形状索引特征和可见概率计算投票权重；
7.s3、离线训练阶段，提取点对特征，将特征存储至查表中的16邻域；计算每个模型点的局部参考系，用以后期假设验证；
8.s4、在线匹配阶段，计算场景点云的点对特征，在预训练查表中寻匹配关系，利用预计算的投票权重在投票表中投票；计算场景点局部参考系用以验证投票生成的位姿矩阵；对通过验证的位姿矩阵进行聚类获取更高精度的位姿估计结果；
9.s5、使用票数高的一定百分比的位姿假设进行欧式聚类，提取属于不同实例的位姿；对聚类结果，提取模型在粗位姿下的可见点，使用改进的icp算法进行精确配准以定位目标；计算模型点与场景点的重叠率，筛选未被遮挡的实例作为待抓取对象。
10.优选地，步骤s2具体包括以下步骤：
11.s2-1、计算点云分辨率；
12.s2-2、使用主成分分析法计算所有点的法线；
13.s2-3、对所计算的法线重定向，将法线统一指向模型外部；
14.s2-4、对所述模型的点云进行体素栅格均匀采样，对下采样后的点云数据计算形状索引特征和可见概率；
15.s2-5、计算点对的投票权重。
16.优选地，所述步骤s2-3包括以下具体步骤：
17.s2-31、构造模型点云欧式距离最小生成树；
18.s2-32、在欧式最小生成树中加入邻域信息构造黎曼图；
19.s2-33、计算黎曼图最小生成树；
20.s2-34、选择初始点，以该点法线为参考，遍历黎曼最小生成树进行法向传播。
21.优选地，步骤s3包括以下具体步骤：
22.s3-1、计算点对特征
23.使用模型中两点及其法线计算点对特征，点m1和其法线n1，与点m2和其法线n2组成的点对特征f(m1,m2)计算方法为：
24.f(m1,m2)＝(||d||2,∠(n1,d),∠(n2,d),∠(n1,n2)) (8)
25.式中，d表示两点连线，∠(n1,d),∠(n2,d),∠(n1,n2)为两法线与两点连线两两组合的夹角；
26.s3-2、计算模型点局部参考系：
27.查模型点p半径r邻域内的临近点，假设有n个临近点，以点p近似作为重心，计算协方差矩阵m：
[0028][0029]
式(9)中，di表示临近点pi到重心点p的欧氏距离；
[0030]
使用奇异值分解计算协方差矩阵m的特征值和特征向量，对向量方向进行约束，作为点p的局部参考系，约束方法为：
[0031]
将最小特征值对应的特征向量作为局部坐标系z轴，将其约束至与法线np方向一致：
[0032][0033]
其中，z
+
为最小特征值对应的特征向量的方向，z-为其相反方向，n
p
为p点法线；
[0034]
局部参考系x轴的方向约束方法为：
[0035][0036]
其中，x
+
为最大特征值对应的特征向量的方向，x-为相反方向；和的计算方法为：
[0037][0038]
在z轴与x轴方向固定的情况下，y轴的方向由z叉乘x计算：
[0039]
y＝z
×
x (13)。
[0040]
优选地，步骤s4包括以下步骤：
[0041]
s4-1、首先对场景进行下采样处理，并计算场景中的点对特征及其哈希值，每个点对特征可根据其哈希值，在预训练的哈希表中快速寻匹配对，对于每一哈希匹配对，计算其对应的点对特征向量之间的相似性，若满足相似性要求，则利用预计算的投票权重在投票表中进行投票；
[0042]
s4-2、使用局部参考系进行假设验证：
[0043]
假设以场景点sr为参考点(第一个点)组成的点对完成投票后，在(mr,αn)处获得最高票数，据此投票结果可计算模型到场景的变换t
m-》s
。假设场景点sr对应的局部参考系为rs，模型点mr对应的局部参考系为rm，首先将模型点局部参考系变换至场景坐标系下，变换后的模型点局部参考系记作
[0044][0045]
根据罗德里格斯公式，计算两个坐标系的测地距离d：
[0046][0047]
如若测地距离满足约束条件则位姿假设t
m-》s
被接受为候选位姿，否则被剔除；
[0048]
s4-3、位姿假设聚类：
[0049]
假设投票位姿按得票数由高到底排序后为{t1,t2,
…
,tk}，以每个候选位姿作为聚类重心进行贪婪聚类，聚类过程允许一个位姿假设加入多个聚类中心，位姿聚类条件为：平移偏差和测地距离(旋转偏差)在设置阈值之内。
[0050]
优选地，步骤s5包括以下步骤：
[0051]
s5-1、候选位姿欧式聚类：
[0052]
对得票数前百分比的候选位姿进行欧式聚类，通过设置欧氏距离保证聚类后的每个类别的候选位姿属于场景中不同实例，以每个类别中得票数最高的位姿作为一个实例的粗位姿估计结果；
[0053]
s5-2、在粗位姿下提取场景视角下的模型可见点：
[0054]
模型可见点的计算通过三维网格快速遍历方法计算，首先对模型进行网格划分，从视点发射光线，光线路径上遇到的第一个包含模型点的网格视作可见网格，网格内的点作为可见点，该路径上其他网格内的点为不可见点。
[0055]
s5-3、将模型粗位姿下的可见点在场景中使用一种由粗到精的icp算法进行精确配准；
[0056]
s5-4、计算重叠率，筛选未被遮挡的实例作为待抓取目标。
[0057]
优选地，步骤s3包括以下具体步骤：
[0058]
s5-31、模型数据下采样；
[0059]
s5-32、寻匹配关系，在一对多的配对关系中保留距离最近的匹配关系；
[0060]
s5-33、根据匹配点对计算标准差，作为阈值进一步剔除距离较远的配对关系；
[0061]
s5-34、构造点到面误差矩阵，使用qr分解最小化误差，求解出对应的旋转平移变换矩阵；
[0062]
s5-35、根据步骤s5-34计算的变换矩阵更新模型位姿，然后不断重复s5-31～s5-34的过程，以迭代更优的刚体变换矩阵，直到达到最大迭代次数或满足误差要求。
[0063]
本发明克服现有技术的不足，提供一种基于点云位姿估计的随机堆放工件定位方法，该方法分为离线训练和在线匹配两个部分；其中，模型点云通过数字建模的方式获取，场景点云由深度相机得到；在离线训练阶段，对模型数据进行法线重定向、量化采样、计算权重，以提高特征区分度和离散度；在线匹配分为粗配准和精配准两个部分，粗配准阶段计
算场景点特征描述子，在预训练的结果中寻与模型点的匹配关系，通过投票的方式计算场景中多个实例的粗位姿；精配准阶段，筛选属于不同实例的位姿，提出与重叠率假设验证相结合的icp算法计算精位姿，以提高配准速度和精度。
[0064]
相比于现有技术的缺点和不足，本发明具有以下有益效果：本发明旨在解决工业零件在散乱堆叠场景下的位姿识别，通过单次拍摄即可鲁棒地提取场景中多个目标零件，具有工件识别率高、抗噪声能力强、抗遮挡性好等优点，可满足工业现场实际应用需求。
具体实施方式
[0065]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0066]
本发明实施例公开了一种基于点云位姿估计的随机堆放工件定位方法，该方法包括以下步骤：
[0067]
s1、使用数字建模软件获取工件3d点云数据，剔除部分冗余数据
[0068]
载物平台上随机堆放工件，3d相机固定在工件上方进行数据采集，由于相机的视野范围超过载物平台，因此采集到的数据包括工件点云和背景点云。为了便于后续定位算法运行，本发明首先需要去除场景中的冗余数据。
[0069]
步骤s1具体包括以下过程：
[0070]
s1-1、背景干扰点云通常分布在相机视野边界和距离相机较远的地方，因此采用直通滤波器去除此类干扰，即仅保留每个维度限定范围内的点云：
[0071]
x1≤x≤x2,y1≤y≤y2,z1≤z≤z2,
[0072]
其中，(x1,x2)、(y1,y2)、(z1,z2)分别为x、y、z方向的范围阈值。
[0073]
s1-2、由于目标对象为堆放的工件，因此直通滤波后的载物平台点云也属于冗余数据，本发明使用随机采用一致性算法(ransac)对载物平台进行平面方程拟合，再从直通滤波后点云数据中剔除属于载物平台的点云。
[0074]
s2、计算步骤s1中工件点云数据法线，并约束法线方向统一指向模型外部，对带有法线的点云下采样，根据每个点的形状索引特征和可见概率以计算投票权重
[0075]
步骤s2具体包括以下过程：
[0076]
s2-1、计算点云分辨率：
[0077]
假设模型点云由n个点组成，对每个点p寻其最近点pi，并计算其距离di。以所有点的距离di的平均值作为点云分辨率，如下式所示：
[0078][0079]
s2-2、计算模板点云法线：使用主成分分析法计算所有点的法线，具体步骤为：对任一点p，搜索其r邻域内的临近点{pi,pj,
…
,pk}，计算协方差矩阵m：
[0080][0081]
其中，k表示r邻域内的临近点个数，t代表矩阵转置。
[0082]
对协方差矩阵进行奇异值分解计算特征值和特征向量，以最小特征值对应的特征向量作为法线。
[0083]
s2-3、对步骤s2-2中计算的法线重定向，将法线统一指向模型外部，具体方法为：
[0084]
s2-31、构造模型点云欧式距离最小生成树；
[0085]
s2-32、在欧式最小生成树中加入邻域信息构造黎曼图；
[0086]
s2-33、计算黎曼图最小生成树；
[0087]
s2-34、选择初始点，以该点法线为参考，遍历黎曼最小生成树进行法向传播。
[0088]
s2-4、对s2-3中模型点云进行体素栅格均匀采样，对下采样后的点云数据计算形状索引特征和可见概率，具体方法为：
[0089]
s2-41、计算形状索引特征si(p)：
[0090][0091]
式中，k1(p)和k2(p)表示点p处的主曲率；
[0092]
s2-42、计算点云可见概率：从均匀分布在球面上的m个视角下观察模型点云，统计每个点出现的次数k，将出现总次数与视角数的比值定义为点的可见概率v(p)，如下式所示：
[0093][0094]
s2-5、计算点对(pi,pj)的投票权重w(pi,pj)，如下式所示：
[0095]
w(pi,pj)＝vis(pi,pj)+λdis(pi,pj)
ꢀꢀ
(5)
[0096][0097][0098]
式中，vis(pi,pj)表示点对的可见性权重，dis(pi,pj)表示点对的区分性权重。
[0099]
s3、离线训练阶段，提取点对特征，将特征存储至查表中的16邻域；计算每个模型点的局部参考系，用以后期假设验证
[0100]
步骤s3具体包括以下过程：
[0101]
s3-1、计算点对特征
[0102]
使用模型中两点及其法线计算点对特征，点m1和其法线n1，与点m2和其法线n2组成的点对特征f(m1,m2)计算方法为：
[0103]
f(m1,m2)＝(||d||2,∠(n1,d),∠(n2,d),∠(n1,n2))
ꢀꢀ
(8)
[0104]
式中，d表示两点连线，∠(n1,d),∠(n2,d),∠(n1,n2)为两法线与两点连线两两组合的夹角。
[0105]
s3-2、计算模型点局部参考系：
[0106]
查模型点p半径r邻域内的临近点，假设有n个临近点，以点p近似作为重心，计算协方差矩阵m：
[0107][0108]
其中，di表示临近点pi到重心点p的欧氏距离，这里给每个距离p点较近的临近点分配更大的权重，以抵抗场景中噪声干扰。
[0109]
然后，使用奇异值分解计算协方差矩阵m的特征值和特征向量，对向量方向进行约束，作为点p的局部参考系，约束方法为：
[0110]
将最小特征值对应的特征向量作为局部坐标系z轴，将其约束至与法线n
p
方向一致：
[0111][0112]
其中，z
+
为最小特征值对应的特征向量的方向，z-为其相反方向，n
p
为p点法线。
[0113]
局部参考系x轴的方向约束方法为：
[0114][0115]
其中，x
+
为最大特征值对应的特征向量的方向，x-为相反方向；和的计算方法为：
[0116][0117]
在z轴与x轴方向固定的情况下，y轴的方向由z叉乘x计算：
[0118]
y＝z
×
x
ꢀꢀ
(13)
[0119]
s4、在线匹配阶段，计算场景点云的点对特征，在预训练查表中寻匹配关系，利用预计算的投票权重在投票表中投票；计算场景点局部参考系用以验证投票生成的位姿矩阵；对通过验证的位姿矩阵进行聚类获取更高精度的位姿估计结果
[0120]
步骤s4的具体过程为：
[0121]
s4-1、首先对场景进行下采样处理，并计算场景中的点对特征及其哈希值，每个点对特征可根据其哈希值，在预训练的哈希表中快速寻匹配对，对于每一哈希匹配对，计算其对应的点对特征向量之间的相似性，若满足相似性要求，则利用预计算的投票权重在投票表中进行投票。
[0122]
s4-2、使用局部参考系进行假设验证：
[0123]
假设以场景点sr为参考点(第一个点)组成的点对完成投票后，在(mr,αn)处获得最高票数，据此投票结果可计算模型到场景的变换t
m-》s
。假设场景点sr对应的局部参考系为rs，模型点mr对应的局部参考系为rm，首先将模型点局部参考系变换至场景坐标系下，变换后的模型点局部参考系记作
[0124][0125]
根据罗德里格斯公式，计算两个坐标系的测地距离d：
[0126][0127]
如若测地距离满足约束条件则位姿假设t
m-》s
被接受为候选位姿，否则被剔除。
[0128]
s4-3、位姿假设聚类：
[0129]
假设投票位姿按得票数由高到底排序后为{t1,t2,
…
,tk}，以每个候选位姿作为聚类重心进行贪婪聚类，聚类过程允许一个位姿假设加入多个聚类中心，位姿聚类条件为：平移偏差和测地距离(旋转偏差)在设置阈值之内。
[0130]
s5、使用票数高的一定百分比的位姿假设进行欧式聚类，提取属于不同实例的位姿；对聚类结果，提取模型在粗位姿下的可见点，使用改进的icp算法进行精确配准以定位目标；计算模型点与场景点的重叠率，筛选未被遮挡的实例作为待抓取对象
[0131]
步骤s5包括以下具体过程：
[0132]
s5-1、候选位姿欧式聚类：
[0133]
对得票数前百分比的候选位姿进行欧式聚类，通过设置欧氏距离保证聚类后的每个类别的候选位姿属于场景中不同实例，以每个类别中得票数最高的位姿作为一个实例的粗位姿估计结果。
[0134]
s5-2、在粗位姿下提取场景视角下的模型可见点：
[0135]
模型可见点的计算通过三维网格快速遍历方法计算，首先对模型进行网格划分，从视点发射光线，光线路径上遇到的第一个包含模型点的网格视作可见网格，网格内的点作为可见点，该路径上其他网格内的点为不可见点。
[0136]
s5-3、将模型粗位姿下的可见点在场景中使用一种由粗到精的icp算法进行精确配准，icp配准具体步骤为：
[0137]
s5-31、模型数据下采样；
[0138]
s5-32、寻匹配关系，在一对多的配对关系中保留距离最近的匹配关系；
[0139]
s5-33、根据匹配点对计算标准差，作为阈值进一步剔除距离较远的配对关系；
[0140]
s5-34、构造点到面误差矩阵，使用qr分解最小化误差，求解出对应的旋转平移变换矩阵；
[0141]
s5-35、根据步骤s5-34计算的变换矩阵更新模型位姿，然后不断重复s5-31～s5-34的过程，以迭代更优的刚体变换矩阵，直到达到最大迭代次数或满足误差要求。
[0142]
s5-4、计算重叠率，筛选未被遮挡的实例作为待抓取目标：
[0143]
构造场景点云多维树形查询结构，统计模型在步骤s5-3计算的目标精确位姿下的重叠点数，以重叠点数与模型总点数的比值作为重叠率，重叠率大于设置阈值的实例作为待抓取实例。
[0144]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

技术特征：

1.一种基于点云位姿估计的随机堆放工件定位方法，其特征在于，该方法包括以下步骤：s1、使用数字建模软件获取工件3d点云数据，剔除部分冗余数据；s2、计算步骤s1中工件点云数据法线，并约束法线方向统一指向模型外部，对带有法线的点云下采样，根据每个点的形状索引特征和可见概率计算投票权重；s3、离线训练阶段，提取点对特征，将特征存储至查表中的16邻域；计算每个模型点的局部参考系，用以后期假设验证；s4、在线匹配阶段，计算场景点云的点对特征，在预训练查表中寻匹配关系，利用预计算的投票权重在投票表中投票；计算场景点局部参考系用以验证投票生成的位姿矩阵；对通过验证的位姿矩阵进行聚类获取更高精度的位姿估计结果；s5、使用票数高的一定百分比的位姿假设进行欧式聚类，提取属于不同实例的位姿；对聚类结果，提取模型在粗位姿下的可见点，使用改进的icp算法进行精确配准以定位目标；计算模型点与场景点的重叠率，筛选未被遮挡的实例作为待抓取对象。2.如权利要求1所述的方法，其特征在于，步骤s2具体包括以下步骤：s2-1、计算点云分辨率；s2-2、使用主成分分析法计算所有点的法线；s2-3、对所计算的法线重定向，将法线统一指向模型外部；s2-4、对所述模型的点云进行体素栅格均匀采样，对下采样后的点云数据计算形状索引特征和可见概率；s2-5、计算点对的投票权重。3.如权利要求2所述的方法，其特征在于，所述步骤s2-3包括以下具体步骤：s2-31、构造模型点云欧式距离最小生成树；s2-32、在欧式最小生成树中加入邻域信息构造黎曼图；s2-33、计算黎曼图最小生成树；s2-34、选择初始点，以该点法线为参考，遍历黎曼最小生成树进行法向传播。4.如权利要求1所述的方法，其特征在于，步骤s3包括以下具体步骤：s3-1、计算点对特征使用模型中两点及其法线计算点对特征，点m1和其法线n1，与点m2和其法线n2组成的点对特征f(m1,m2)计算方法为：f(m1,m2)＝(||d||2,∠(n1,d),∠(n2,d),∠(n1,n2))
ꢀꢀꢀꢀ
(8)式中，d表示两点连线，∠(n1,d),∠(n2,d),∠(n1,n2)为两法线与两点连线两两组合的夹角；s3-2、计算模型点局部参考系：查模型点p半径r邻域内的临近点，假设有n个临近点，以点p近似作为重心，计算协方差矩阵m：式(9)中，d
i
表示临近点p
i
到重心点p的欧氏距离；
使用奇异值分解计算协方差矩阵m的特征值和特征向量，对向量方向进行约束，作为点p的局部参考系，约束方法为：将最小特征值对应的特征向量作为局部坐标系z轴，将其约束至与法线n
p
方向一致：其中，z
+
为最小特征值对应的特征向量的方向，z-为其相反方向，n
p
为p点法线；局部参考系x轴的方向约束方法为：其中，x
+
为最大特征值对应的特征向量的方向，x-为相反方向；和的计算方法为：在z轴与x轴方向固定的情况下，y轴的方向由z叉乘x计算：y＝z
×
x
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)。5.如权利要求1所述的方法，其特征在于，步骤s4包括以下步骤：s4-1、首先对场景进行下采样处理，并计算场景中的点对特征及其哈希值，每个点对特征可根据其哈希值，在预训练的哈希表中快速寻匹配对，对于每一哈希匹配对，计算其对应的点对特征向量之间的相似性，若满足相似性要求，则利用预计算的投票权重在投票表中进行投票；s4-2、使用局部参考系进行假设验证：假设以场景点s
r
为参考点(第一个点)组成的点对完成投票后，在(m
r
,α
n
)处获得最高票数，据此投票结果可计算模型到场景的变换t
m->s
。假设场景点s
r
对应的局部参考系为r
s
，模型点m
r
对应的局部参考系为r
m
，首先将模型点局部参考系变换至场景坐标系下，变换后的模型点局部参考系记作型点局部参考系记作根据罗德里格斯公式，计算两个坐标系的测地距离d：如若测地距离满足约束条件则位姿假设t
m->s
被接受为候选位姿，否则被剔除；s4-3、位姿假设聚类：假设投票位姿按得票数由高到底排序后为{t1,t2,
…
,t
k
}，以每个候选位姿作为聚类重心进行贪婪聚类，聚类过程允许一个位姿假设加入多个聚类中心，位姿聚类条件为：平移偏差和测地距离(旋转偏差)在设置阈值之内。6.如权利要求1所述的方法，其特征在于，步骤s5包括以下步骤：s5-1、候选位姿欧式聚类：
对得票数前百分比的候选位姿进行欧式聚类，通过设置欧氏距离保证聚类后的每个类别的候选位姿属于场景中不同实例，以每个类别中得票数最高的位姿作为一个实例的粗位姿估计结果；s5-2、在粗位姿下提取场景视角下的模型可见点：模型可见点的计算通过三维网格快速遍历方法计算，首先对模型进行网格划分，从视点发射光线，光线路径上遇到的第一个包含模型点的网格视作可见网格，网格内的点作为可见点，该路径上其他网格内的点为不可见点。s5-3、将模型粗位姿下的可见点在场景中使用一种由粗到精的icp算法进行精确配准；s5-4、计算重叠率，筛选未被遮挡的实例作为待抓取目标。7.如权利要求6所述的方法，其特征在于，步骤s3包括以下具体步骤：s5-31、模型数据下采样；s5-32、寻匹配关系，在一对多的配对关系中保留距离最近的匹配关系；s5-33、根据匹配点对计算标准差，作为阈值进一步剔除距离较远的配对关系；s5-34、构造点到面误差矩阵，使用qr分解最小化误差，求解出对应的旋转平移变换矩阵；s5-35、根据步骤s5-34计算的变换矩阵更新模型位姿，然后不断重复s5-31～s5-34的过程，以迭代更优的刚体变换矩阵，直到达到最大迭代次数或满足误差要求。

技术总结

本发明公开了一种基于点云位姿估计的随机堆放工件定位方法，该方法分为离线训练和在线匹配两个部分；其中，模型点云通过数字建模的方式获取，场景点云由深度相机得到；在离线训练阶段，对模型数据进行法线重定向、量化采样、计算权重，以提高特征区分度和离散度；在线匹配分为粗配准和精配准两个部分，粗配准阶段计算场景点特征描述子，在预训练的结果中寻与模型点的匹配关系，通过投票的方式计算场景中多个实例的粗位姿；精配准阶段，筛选属于不同实例的位姿，提出与重叠率假设验证相结合的ICP算法计算精位姿，以提高配准速度和精度。以提高配准速度和精度。