一种复杂工件装配的三维位姿估计方法

技术领域

本发明主要涉及机器人三维目标测量与检测方法领域，具体涉及一种复杂工件装配的三维位姿估计方法。

背景技术

智能制造技术即通过机器人的智能感知、人机交互、决策和执行技术，实现高精度、高强度重复作业，在航空航天、海洋工程、轨道交通、新能源等高端精密制造业具有广阔的应用前景，能够完成复杂工件识别、定位抓取、扫描测量、焊接打磨、钻孔铆接等作业任务。

复杂工件指的是非标准化的复杂结构工件，在航空航天、海洋工程、轨道交通等高端精密制造业中广泛应用。因加工技术、材料强度、工艺限制等原因，复杂工件多维组合装配件，其精度除了与单个零部件的精度相关，同时也与零部件之间的装配情况密切相关。由于零部件装配过程会放大装配面之间的误差，零部件之间装配面误差会一定程度上影响装配成品的整体精度，而且这种装配面误差通常比较小，针对单个装配零部件的三维测量无法有效检测这种误差。因此，在装配过程中通常采用“试装-修配”模式，采用该模式只能在装配过程完成后才会对其进行三维测量，由于装配过程往往费时费力，若完成的组合装配件的尺寸精度不符合要求还可能需要进行拆分和重新装配，严重影响了装配和制造的效率。另外，由于现有方法中对装配误差的描述以欧氏距离差值为基础，而欧氏距离差值与装配面的物理约束使得装配误差函数的优化空间存在大量局部最优值，影响最终计算结果。

发明内容

本发明所有解决的技术问题是提供一种复杂工件装配的三维位姿估计方法，该方法充分考虑了装配需求及物理空间约束，通过对工件装配面的合并拼接，构建装配误差函数并对其优化求解，估计装配工件的装配位姿，得到整体工件装配精度数据。

为解决上述技术问题，本发明提供一种复杂工件装配的三维位姿估计方法，所述方法包括：

S1、通过三维扫描仪分别获取装配工件A和B的扫描点云数据，将装配工件A和B的扫描点云数据分别与各自的标准模型点云数据比对，确定装配工件A和B的装配面对应的扫描点云数据；

S2、预设装配工件A和B在装配状态下的初始位姿，并移动装配工件A和B至初始位姿，构建装配工件A和B的装配误差函数；

S3、通过Dog-Leg方法优化装配误差函数，得到装配工件A和B优化后的位姿；

S4、根据优化后的位姿分别更新装配工件A和B的扫描点云数据，得到装配工件A和B更新后的扫描点云数据，重新计算更新后的装配工件A中的每个点到装配工件B上最近点的距离，由此得出装配工件A和B装配估计结果。

优选地，S1中将装配工件A和B的扫描点云数据分别与各自的标准模型点云数据比对，确定装配工件A和B的装配面对应的扫描点云数据，具体包括：

S11、在装配工件A和B的标准模型上分别标记出各自的装配面上对应的模型点云数据；

S12、根据装配面上对应的模型点云数据，采用ICP配准的方法将装配工件A和B的标准模型点云数据与各自的扫描点云数据分别配准；

S13、建立Kd-Tree，并预设距离阈值，分别从装配工件A和B的扫描点云数据中分离出不超过预设距离阈值的扫描点云数据作为各自装配面上对应的扫描点云数据。

优选地，S2中构建装配工件A和B的装配误差函数，具体包括：

S21、计算装配工件A和B在初始位姿时的接触面积；

S22、计算装配工件A和B在初始位姿时的重叠体积；

S23、通过接触面积和重叠体积构建装配工件A和B的装配误差函数。

优选地，S23中的装配误差函数具体公式为：

式中，F为装配误差，μ和β为比例参数，R为装配工件A到B的旋转矩阵，t为装配工件A到B的平移矩阵，N为装配工件A的装配面对应的扫描点云数据的总数，a

优选地，S3具体包括：

S31、构建二次模型函数，预设第一迭代终止阈值ε

S32、计算第k次迭代的优化步长参数g

S33、将第k次迭代的优化步长参数g

S34、结束DL优化过程，输出当前装配工件A和B的位姿φ

S35、计算第k次迭代后DL优化步长δ

S36、计算第k次迭代后装配误差函数与二次模型函数的相似度ρ

S37、更新置信域半径、装配工件A和B的位姿、装配误差函数和二次模型函数的相似度以及DL优化步长的参数，返回步骤S32，进行第k+1次迭代优化。

优选地，S31中的二次模型函数具体为：

式中，∈

优选地，S32中计算第k次迭代的优化步长参数g

g

式中，g

计算第k次迭代的置信域半径Δ

式中，Δ

优选地，S34中输出当前装配工件A和B的位姿φ

式中，φ

优选地，S35具体为：

S351、计算高斯牛顿法步长δ

S352、将高斯牛顿法步长δ

S353、若高斯牛顿法步长δ

S354、其他情况下，通过高斯牛顿法步长δ

优选地，S36中计算第k次迭代后装配误差函数与二次模型函数的相似度ρ

式中，ρ

上述复杂工件装配的三维位姿估计方法，首先分别获取装配工件A和B的扫描点云数据，将扫描点云数据分别与各自的标准模型点云数据比对，确定装配工件A和B装配面对应的扫描点云数据；然后预设装配工件A和B在装配状态下的初始位姿，并将装配工件A和B移动至初始位姿，构建装配工件A和B的装配误差函数；接着通过Dog-Leg方法优化装配误差函数，得到装配工件A和B优化后的位姿；最后根据优化后的位姿分别更新装配工件A和B的扫描点云数据，并重新计算更新后的装配工件A中的每个点到装配工件B上最近点的距离，由此得出所述装配工件A和B装配估计结果。在复杂部件装配过程中，该方法充分考虑了装配需求及物理限制，通过模拟零部件的装配过程，构建基于接触面积和重叠体积的装配误差函数，并采用非线性优化算法求解，估计最终装配后各零部件的装配位姿，得到最终成品的误差分布情况，并由此判断成品装配质量，该方法能够降低装配失败概率，简化原有“试装-修配”的装配模式，大幅提高工件装配精度和装配效率。

附图说明

图1是本发明一实施例中复杂工件装配的三维位姿估计方法的流程图；

图2是本发明一实施例中采用Dog-Leg方法对装配误差函数进行优化的流程图；

图3是本发明一实施例中计算DL优化步长的流程图。

具体实施方式

为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。

一种复杂工件装配的三维位姿估计方法，具体包括：

S1、通过三维扫描仪分别获取装配工件A和B的扫描点云数据，将装配工件A和B的扫描点云数据分别与各自的标准模型点云数据比对，确定装配工件A和B的装配面对应的扫描点云数据；

S2、预设装配工件A和B在装配状态下的初始位姿，并移动装配工件A和B至初始位姿，构建装配工件A和B的装配误差函数；

S3、通过Dog-Leg方法优化装配误差函数，得到装配工件A和B优化后的位姿；

S4、根据优化后的位姿分别更新装配工件A和B的扫描点云数据，得到装配工件A和B更新后的扫描点云数据，重新计算更新后的装配工件A中的每个点到装配工件B上最近点的距离，由此得出装配工件A和B装配估计结果。

具体地，参见图1，图1为本发明一实施例中复杂工件装配的三维位姿估计方法的流程图。

一种复杂工件装配的三维位姿估计方法，首先通过三维扫描仪分别获取装配工件A和B的扫描点云数据，将装配工件A和B的扫描点云数据分别与各自的标准模型点云数据比对，确定装配工件A和B的装配面对应的扫描点云数据；然后预设装配工件A和B在装配状态下的初始位姿，并移动装配工件A和B至装配初始位姿，为使装配位姿在计算过程中快速收敛，工件装配初始位姿一般尽量靠近两个装配工件A和B的装配面，并使装配工件位姿与实际装配位姿基本一致，构建装配工件A和B的装配误差函数；接着通过Dog-Leg方法优化所述装配误差函数，得到装配工件A和B优化后的位姿；最后根据优化后的位姿分别更新所述装配工件A和B的扫描点云数据，得到装配工件A和B更新后的扫描点云数据，重新计算更新后的装配工件A中的每个点到装配工件B上最近点的距离，由此得出装配工件A和B装配估计结果。

在一个实施例中，S1中将装配工件A和B的扫描点云数据分别与各自的标准模型点云数据比对，确定装配工件A和B的装配面对应的扫描点云数据，具体包括：

S11、在装配工件A和B的标准模型上分别标记出各自的装配面上对应的模型点云数据；

S12、根据装配面上对应的模型点云数据，采用ICP配准的方法将装配工件A和B的标准模型点云数据与各自的扫描点云数据分别配准；

S13、建立Kd-Tree，并预设距离阈值，分别从装配工件A和B的扫描点云数据中分离出不超过预设距离阈值的扫描点云数据作为各自装配面上对应的扫描点云数据。

具体地，首先在装配工件A和B各自的标准模型上分别标记出装配面上对应的模型点云数据；然后采用ICP配准的方法分别将装配工件A和B各自的标准模型点云数据与扫描点云数据配准，接着建立Kd-Tree并预设距离阈值，分别从装配工件A和B各自的扫描点云数据中分离出不超过距离阈值的扫描点云数据作为各自装配面上对应的扫描点云数据。通过分离，可以去除装配零件上非装配区域的扫描点云数据，减少数据处理量，另外，距离阈值可以适当增大的取值，以避免漏掉属于装配面上的扫描点云数据。

在一个实施例中，S2中构建装配工件A和B的装配误差函数，具体包括：

S21、计算装配工件A和B在初始位姿时的接触面积；

S22、计算装配工件A和B在初始位姿时的重叠体积；

S23、通过接触面积和重叠体积构建装配工件A和B的装配误差函数。

在一个实施例中，S23中的装配误差函数具体公式为：

式中，F为装配误差，μ和β为比例参数，R为装配工件A到B的旋转矩阵，t为装配工件A到B的平移矩阵，N为装配工件A的装配面对应的扫描点云数据的总数，a

具体地，利用两个装配工件在装配时的接触面积和重叠体积来构建装配误差函数F(R,t)，过程如下：

1)计算装配工件A和B在装配初始位姿时的接触面积：

通过三维扫描仪分别获取装配工件A和B的扫描点云数据，将所述装配工件A和B的扫描点云数据分别与各自的标准模型点云数据比对，确定装配工件A和B的装配面对应的扫描点云数据X＝{x

计算装配工件A的装配面对应的扫描点云数据中第i个数据点x

d

式中，R为装配工件A到B的旋转矩阵，t为装配工件A到B的平移矩阵。

计算装配工件A的装配面对应的扫描点云数据中的点x

dens(i)＝s

其中，s

式中，dens(i)为装配工件A的装配面上的第i个数据点x

计算装配工件A和B在装配时的接触面积:

a

式中，1(d

将装配工件A的装配面上的第i个数据点x

2)计算所述装配工件A和B在装配初始位姿时的重叠体积：

重叠体积v

重叠体积v

其中，dens(i)＝s

s

式中，v

3)构建所述装配工件A和B的装配误差函数：

装配模拟优化计算是将求取装配接触面积a

式中，F为装配误差，μ和β为比例参数，R为装配旋转矩阵，t为装配平移矩阵。

在一个实施例中，S3具体包括：

S31、构建二次模型函数，预设第一迭代终止阈值ε

S32、计算第k次迭代的优化步长参数g

S33、将第k次迭代的优化步长参数g

S34、结束DL优化过程，输出当前装配工件A和B的位姿φ

S35、计算第k次迭代后DL优化步长δ

S36、计算第k次迭代后装配误差函数与二次模型函数的相似度ρ

S37、更新置信域半径、装配工件A和B的位姿、装配误差函数和二次模型函数的相似度以及DL优化步长的参数，返回步骤S32，进行第k+1次迭代优化。

在一个实施例中，S31中的二次模型函数具体为：

式中，∈

在一个实施例中，S32中计算第k次迭代的优化步长参数g

式中，g

计算第k次迭代的置信域半径Δ

式中，Δ

在一个实施例中，S34中输出当前装配工件A和B的位姿φ

式中，φ

具体地，通过Dog-Leg方法优化装配误差函数，Dog-Leg方法结合了高斯牛顿优化方法和最速下降法，并利用置信域方法精确控制装配误差函数的迭代过程。在置信域方法中，装配误差函数的信息被重构为一个二次模型函数，该二次模型函数在某一数据点邻域内的情况与装配误差函数基本相同，通过将装配误差函数F(R,t)的旋转矩阵R和平移矩阵t替换为装配工件A和B的位姿φ，将装配误差函数F(R,t)的迭代优化问题转化为二次模型函数L(δ)的最小化问题。

二次模型函数具体为：

式中，∈

上述二次模型函数可以在半径为Δ的邻域内精确表示装配误差函数，因此装配误差函数的迭代优化问题可转为二次模型函数在置信域半径Δ内的最小化问题：

min

置信域半径Δ对于迭代计算至关重要：如果置信域的区域太大，则该二次模型函数可能与装配误差函数相差过大，导致二次模型函数的最小值可能远离该区域中装配误差函数的最小值；如果置信域的区域太小，计算的候选优化步长δ(即DL优化步长，也叫迭代优化向量)可能不足以使当前优化点接近装配误差函数的最小值点。在实践中，置信域半径的选择是基于二次模型函数在前一次迭代中逼近装配误差函数的程度(相似程度)：如果二次模型函数可靠，即准确预测装配误差函数的变化趋势，则置信域半径Δ会增加，尝试更长的优化步长；如果二次模型函数无法预测当前置信域上的装配误差函数，则减小置信域半径Δ，并在较小的置信域上再次求解。

参见图2，图2为本发明一实施例中采用Dog-Leg方法对装配误差函数进行优化的流程图。

采用DL(Dog-Leg)方法对装配误差函数进行优化，具体优化过程如下：

1)将装配误差函数的中的旋转矩阵R和平移矩阵t替换为装配工件A和B的位姿φ，构建二次模型函数L(δ)，预设第一迭代终止阈值ε

2)计算第k次迭代的优化步长参数g

式中，J

计算第k次迭代的置信域半径Δ

根据历史经验设置相似度阈值范围，并进行判断：当第k-1次迭代后，装配误差函数和二次模型函数的相似度ρ

3)将第k次迭代的优化步长参数g

4)结束DL优化过程，输出装配工件A和B的当前位姿φ

5)计算第k次迭代后DL优化步长δ

6)计算第k次迭代后装配误差函数F与二次模型函数L的相似度ρ

7)根据相似度ρ

在一个实施例中，S35具体为：

S351、计算高斯牛顿法步长δ

S352、将高斯牛顿法步长δ

S353、若高斯牛顿法步长δ

S354、其他情况下，通过高斯牛顿法步长δ

具体地，第k次迭代的高斯牛顿法步长δ

式中，δ

第k次迭代的最速下降法步长δ

设置比例参数α，计算柯西点(Cauchy Point)，柯西点为当前优化量最快衰减方向上的最小值，通过柯西点将高斯牛顿法步长δ

当前置信域半径为Δ

式中，δ

在一个实施例中，S36中计算第k次迭代后装配误差函数与二次模型函数的相似度ρ

式中，ρ

具体地，根据装配工件A和B在第k次迭代后的位姿φ

上述一种复杂工件装配的三维位姿估计方法，首先分别获取装配工件A和B的扫描点云数据，将扫描点云数据分别与各自的标准模型点云数据比对，确定装配工件A和B装配面对应的扫描点云数据；然后预设装配工件A和B在装配状态下的初始位姿，并将其移动至初始位姿，构建装配工件A和B的装配误差函数；接着通过Dog-Leg方法优化装配误差函数，得到装配工件A和B优化后的位姿；最后根据优化后的位姿分别更新装配工件A和B的扫描点云数据，并重新计算更新后的装配工件A中的每个点到装配工件B上最近点的距离，由此得出装配工件A和B装配估计结果。在复杂部件装配过程中，该方法充分考虑了装配需求及物理限制，通过模拟零部件的装配过程，构建基于接触面积和重叠体积的装配误差函数，并采用非线性优化算法求解，估计最终装配后各零部件的装配位姿，得到最终成品的误差分布情况，并由此判断成品装配质量，该方法能够降低装配失败概率，简化原有“试装-修配”的装配模式，大幅提高工件装配精度和装配效率。

以上对本发明所提供的一种复杂工件装配的三维位姿估计方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。