CN109093376A - 一种基于激光跟踪仪的多轴孔自动化对准方法 - Google Patents

Abstract

本发明提出一种基于激光跟踪仪的多轴孔自动化对准方法，属于大型零部件装配控制技术领域。该方法首先搭建由激光跟踪仪，机械臂和计算机组成的多轴孔自动化对准系统，将带轴部件安装在机械臂端面；分别建立带轴部件坐标系和带孔部件坐标系，得到两个坐标系间的旋转矩阵和平移向量；在带轴部件上安装4个不共线的反射靶球，计算每个靶球对应的目标位置；获取机械臂的初始的雅可比矩阵；从初始位置开始，通过迭代控制机械臂的运动，使得带轴部件逐渐靠近带孔部件，当4个反射靶球到达目标位置时，完成带轴部件和带孔部件的对准。本发明利用激光跟踪仪的测量数据闭环控制机械臂的运动，可实现多轴孔高精度自动化对准，提高装配效率和鲁棒性。

Description

一种基于激光跟踪仪的多轴孔自动化对准方法

技术领域

本发明属于大型零部件装配控制技术领域，尤其涉及一种基于激光跟踪仪的多轴孔自动化对准方法。

背景技术

多轴孔连接是大型零部件装配领域常用的一种装配连接方式，待装配的两个零件一个具有多个孔，称之为带孔部件；另一个具有多个与孔对应的轴，称之为带轴部件。通过将带轴部件上的多个轴插入带孔部件上对应的孔里便可实现两个零件的配合，然后再通过锁紧螺母或销钉等方式完成定位连接，从而完成装配。

现有关于大型零部件的装配主要由人工装配完成，效率低下且装配精度不高。基于机器人的大型零部件自动化装配大部分停留在研究阶段，尚未实用，而关于大型零部件多轴孔对齐的研究尚未见公开发表。

多轴孔自动化装配可以分为两个阶段：粗定位阶段和精定位阶段。其中粗定位阶段需完成轴孔的对准，精定位阶段实现轴插入孔完成配合。

在大型零部件的自动化装配过程中，一般采用激光跟踪仪进行高精度测量，从而实现闭环控制。激光跟踪仪是工业测量系统中一种高精度的大尺寸测量仪器。它集合了激光干涉测距技术、光电探测技术、精密机械技术、计算机及控制技术、现代数值计算理论等各种先进技术，对空间运动目标进行跟踪并实时测量目标的空间三维坐标。

激光跟踪仪的工作基本原理是在目标点上安置一个反射靶球，跟踪头发出的激光射到反射靶球，又返回到跟踪头，当目标移动时，跟踪头调整光束方向来对准目标。同时，返回光束为检测系统所接收，用来测算目标的空间位置。

发明内容

本发明的目的是为克服已有技术的不足之处，提出一种基于激光跟踪仪的多轴孔自动化对准方法。本发明利用激光跟踪仪的测量数据闭环控制机械臂的运动，可实现多轴孔高精度自动化对准，提高装配效率和鲁棒性。

本发明提出一种基于激光跟踪仪的多轴孔自动化对准方法，其特征在于，该方法包括以下步骤：

1)搭建多轴孔自动化对准系统；所述多轴孔自动化对准系统包括：一台激光跟踪仪，一个机械臂和一台计算机；所述激光跟踪仪与机械臂分别与计算机连接；

2)选取待对准的带孔部件和带轴部件，其中带孔部件具有N个孔，带轴部件具有N个轴，N大于等于3；令带孔部件固定不动，将带轴部件安装在机械臂的端面，令当前位置为初始位置；

3)在带孔部件中选取任意三个不共线的孔建立带孔部件坐标系W₁；具体步骤如下：

3-1)在带孔部件用于建立带孔部件坐标系的三个孔的孔壁上，分别用激光跟踪仪的球形反射靶球或T-Prob的测量头上的球形红宝石紧贴每个孔壁并利用激光跟踪仪测量对应的球心坐标；测量时，每次选取孔壁上的一个点，每个孔壁上共测量P个不同点，P大于等于5，根据每个孔壁上选取的所有点的测量结果通过最小二乘法拟合对应孔壁的圆柱面，得到每个孔壁圆柱面的轴线即为对应孔的孔轴线，最终得到三个孔分别对应的轴线方程；

3-2)在带孔部件的孔端面任意选取M个不共线的点，M大于等于3，将激光跟踪仪的球形反射靶球或T-Prob的测量头上的球形红宝石放置在每个点上并利用激光跟踪仪测量对应的球心坐标；利用每个点测量的球心坐标通过最小二乘法拟合带孔部件的孔端面平面得到初始孔端面平面方程，对初始孔端面平面方程沿平面法向向端面偏移靶球半径或球形红宝石半径的距离得到带孔部件孔端面的平面方程；

3-3)分别计算带孔部件用于建立带孔部件坐标系的三个孔的轴线和孔端面的交点，得到三个交点，用三个交点建立带孔部件坐标系W₁；

4)在带轴部件中选取与建立带孔部件坐标系的三个孔对应的三个轴建立带轴部件坐标系W₂；具体步骤如下：

4-1)在带轴部件中选取与建立带孔部件坐标系的三个孔对应的三个轴，重复步骤3-1)，得到带轴部件三个轴的轴线方程；

4-2)重复步骤3-2)，在带轴部件的轴端面任意选取L个不共线的点，L大于等于3，利用L个不共线的点的测量结果拟合带轴部件轴端面平面，得到带轴部件轴端面的平面方程；

4-3)分别计算带轴部件用于建立带轴部件坐标系的三条轴线和轴端面的三个交点，建立带轴部件坐标系W₂；

5)根据带孔部件坐标系W₁和带轴部件坐标系W₂分别计算两坐标系之间的旋转矩阵R和平移向量T；其中，

T＝O₁-O₂-[D 0 0]^T

式中，O₁为坐标系W₁的原点坐标列向量，O₂为坐标系W₂的原点坐标列向量，D为偏置距离；

R＝[X₁ Y₁ Z₁][X₂ Y₂ Z₂]^-1

式中，X₁为W₁中X轴的方向列向量，Y₁为W₁中Y轴的方向列向量，Z₁为W₁中Z轴的方向列向量，X₂为W₂中X轴的方向列向量，Y₂为W₂中Y轴的方向列向量，Z₂为W₂中Z轴的方向列向量；

6)在带轴部件上安装4个不共线的反射靶球，根据旋转矩阵R和平移向量T计算带轴部件上安装的4个反射靶球分别对应的目标位置；

设四个反射靶球在初始位置坐标分别为p₁、p₂、p₃、p₄，该4个反射靶球的目标位置分别为p₁′、p′₂、p′₃、p′₄，则：

p_i′＝Rp_i+T i＝1～4

7)获取机械臂的初始雅可比矩阵；从初始位置开始，通过迭代控制机械臂的运动，使得带轴部件逐渐靠近带孔部件，最终实现带轴部件和带孔部件的对准；具体步骤如下：

7-1)令机械臂通过六次独立的试探运动得到一个初始雅可比矩阵J₀，具体做法为：每次运动机械臂的一个自由度，设第i个自由度的运动量为Δα_i，则机械臂第i个自由度的运动导致的4个靶球坐标向量改变量分别为令

则：

7-2)从初始位置开始，通过迭代控制机械臂的运动，使得带轴部件逐渐靠近带孔部件，最终实现带轴部件和带孔部件的对准；具体步骤如下：

7-2-1)计算4个反射靶球在第k步迭代中期望实现的坐标增量ΔP_k：

ΔP_k＝λ(P′-P_k)

其中，P′为4个反射靶球的目标位置坐标向量，P_k为4个反射靶球第k步迭代时的坐标向量，P_ik为第i个靶球在第k步迭代时的坐标，λ是用来控制机械臂运动范围的步长系数，λ＜1；

7-2-2)利用第k-1步迭代时估计的第k步迭代的雅可比矩阵计算机械臂各关节在第k步迭代时的增量；表达式如下：

其中，Δθ_k为第k步迭代时机械臂各个关节运动的增量，是第k-1步迭代时估计的第k步迭代的雅可比矩阵；

7-2-3)根据步骤7-2-2)的计算结果，控制机械臂按照Δθ_k实现各关节对应的运动增量；利用卡尔曼滤波算法估计第k+1步迭代的雅可比矩阵，表达式如下：

其中是第k步迭代后反射靶球实际坐标增量；

7-2-4)重复步骤7-2-1)至7-2-3)，直到4个反射靶球到达对应的目标位置，从而实现带轴部件与带孔部件的对准。

本发明的特点及有益效果在于：

本发明能够克服现有大型零部件采用人工装配效率低下且容易损伤工件的问题，通过激光跟踪仪的高精度测量获取待装配零部件的精确位置，利用激光跟踪仪的测量数据闭环控制机械臂的运动，弥补机械臂绝对运动不足的缺点，从而实现零部件高精度自动化对准。

本发明可严格流程化，无需依赖人工经验，可以实现稳定高效的装配作业。方法通用性较强，可用于各种轴孔装配场景中零部件的对准，尤其适合航空领域机翼和机身的对准。

附图说明

图1是本发明方法的整体流程框图。

图2是本发明方法涉及的多轴孔自动化对准系统部分结构示意图。

图3是本发明实施例中带孔部件坐标系建立示意图。

图4是本发明实施例中反射靶球示意图。

图5是本发明实施例中激光跟踪仪配件T-Prob示意图。

图6是本发明实施例中带轴部件初始位置和目标位置示意图。

图中，1为带孔部件，2为带轴部件，3为激光跟踪仪，4为机械臂，5为反射靶球，6为T-Prob。

具体实施方式

本发明提出一种基于激光跟踪仪的多轴孔自动化对准方法，下面结合附图和具体实施例对本发明进一步详细说明如下。

本发明提出一种基于激光跟踪仪的多轴孔自动化对准方法，整体流程如图1所示，包括以下步骤：

1)搭建多轴孔自动化对准系统；所述多轴孔自动化对准系统包括：一台激光跟踪仪，一个机械臂和一台计算机。所述激光跟踪仪与机械臂分别与计算机通过有线或者无线连接。其中激光跟踪仪和计算机可以是任意型号，机械臂大于六个自由度即可。

所述激光跟踪仪通过与反射靶球或者和T-Prob配合使用，用于对带孔部件和带轴部件进行测量，并将测量结果发送至计算机。所述机械臂用于控制带轴部件运动，完成带轴部件与带孔部件的对准。所述计算机用于接收激光跟踪仪测量得到的数据并进行处理，然后控制机械臂进行相应运动。

2)选取待对准的带孔部件和带轴部件；其中带孔部件具有N个孔，带轴部件具有N个轴，N大于等于3；

图2为本发明方法涉及的多轴孔自动化对准系统部分结构示意图，图中包括一台激光跟踪仪3及机械臂4(本实施例为一个六关节机械臂)。待对准的带孔部件1和带轴部件2组成装配体，其中，带孔部件1固定不动，带轴部件2安装在机械臂4端面，令当前位置为初始位置。本发明可完成多轴孔自动化装配的粗定位阶段，因此配准目标是将带轴部件2的轴与带孔部件1的孔大致对齐

3)在具有N个(N大于等于3)孔的带孔部件中选取任意三个不共线的孔建立带孔部件坐标系W₁。本实施例中N＝6。

图3为本实施例建立的带孔部件坐标系的示意图，任意三个不共线的孔都可以建立坐标系，本实施例是通过三个孔11,12和13定位建立带孔部件坐标系。

具体步骤如下：

3-1)在带孔部件用于建立带孔部件坐标系的三个孔的孔壁上(本实施例是孔11，孔12和孔13)，分别用激光跟踪仪的球形反射靶球(如图4所示，图4中5为反射靶球)或T-Prob(如图5所示，图5中6为T-Prob)的测量头上的球形红宝石紧贴每个孔壁并利用激光跟踪仪测量对应的球心坐标；测量时，每次选取孔壁上的一个点每个孔壁上共需测量P个不同点，P大于等于5，P数量越多精度越高耗时越多，根据每个孔壁上选取的所有点的测量结果通过最小二乘法拟合对应孔壁的圆柱面，得到每个孔壁圆柱面的轴线既是对应孔的孔轴线，于是可以得到三个孔对应的轴线方程；

图4为本发明方法涉及的反射靶球示意图，反射靶球中间具有三面正交放置的反射镜，可以沿入射反方向反射激光跟踪仪射出的激光，使得激光跟踪仪可以测得反射靶球的球心坐标。

图5为本发明方法涉及的激光跟踪仪配件T-Prob示意图，其与激光跟踪仪配合可以测得其下部的球形红宝石的球心坐标。

3-2)在带孔部件的孔端面任意选取M个不共线的点，M大于等于3，将激光跟踪仪的球形反射靶球或T-Prob的测量头上的球形红宝石放置在每个点并利用激光跟踪仪测量对应的球心坐标；利用每个点测量的球心坐标通过最小二乘法拟合带孔部件的孔端面平面得到初始孔端面平面方程，对初始孔端面平面方程沿平面法向向端面偏移靶球半径(若采用T-Prob的测量头，则为球形红宝石半径)的距离即可得到带孔部件孔端面的平面方程；

3-3)分别计算带孔部件用于建立带孔部件坐标系的三个孔的(本实施例为孔11、孔12和孔13)轴线和孔端面的交点，得到三个交点，用三个交点建立带孔部件坐标系W₁。

本实施例的带孔部件坐标系W₁如图3所示。本实施例中孔12轴线与端面的交点为原点，孔13到孔12连线方向决定Y轴方向，孔12到孔11连线方向决定Z轴方向，通过Z轴方向和Y轴方向叉乘得到X轴方向，然后再用X轴线和Y轴方向叉乘得到新的Z轴方向，此时X轴，Y轴，Z轴才互相正交；

4)在带轴部件中选取与建立带孔部件坐标系的三个孔对应的三个轴建立带轴部件坐标系W₂；具体步骤如下：

4-1)在具有N个轴的带轴部件中选取与建立带孔部件坐标系的三个孔对应的三个轴，重复步骤3-1)，得到带轴部件三个轴的轴线方程；

4-2)重复步骤3-2)，在带轴部件的轴端面任意选取L个不共线的点，L大于等于3，利用L个不共线的点的测量结果拟合带轴部件轴端面平面，得到带轴部件轴端面的平面方程；

4-3)重复步骤3-3)，分别计算带轴部件用于建立带轴部件坐标系的三条轴线和轴端面的三个交点，建立带轴部件坐标系W₂；

5)根据带孔部件坐标系W₁和带轴部件坐标系W₂分别计算两坐标系之间的旋转矩阵R和平移向量T；其中，

T＝O₁-O₂-[D 0 0]^T

式中，O₁为坐标系W₁的原点坐标列向量，O₂为坐标系W₂的原点坐标列向量，D为一个偏置距离，使得对准时带孔部件和带轴部件在X轴方向相距一段D的距离(D的取值可根据实际零部件的形状大小等自行选择)不会碰撞。

R＝[X₁ Y₁ Z₁][X₂ Y₂ Z₂]^-1

式中X₁为W₁中X轴的方向列向量，Y₁为W₁中Y轴的方向列向量，Z₁为W₁中Z轴的方向列向量，X₂为W₂中X轴的方向列向量，Y₂为W₂中Y轴的方向列向量，Z₂为W₂中Z轴的方向列向量。

6)在带轴部件上安装4个反射靶球，这4个反射靶球不得共线，且需在激光跟踪仪可测范围内。根据旋转矩阵R和平移向量T计算带轴部件上安装的4个反射靶球分别对应的目标位置。

本发明将反射靶球固定在带轴部件的侧面，只要在整个工作过程中每个反射靶球都能接收并反射激光跟踪仪的测量光束即可，反射靶球之间的距离尽可能拉大，且所有反射靶球不应在一条直线上。

设四个反射靶球的初始位置坐标分别为p₁、p₂、p₃、p₄，该4个反射靶球的目标位置分别为p₁′、p′₂、p′₃、p′₄，则：

p_i′＝Rp_i+T(i＝1～4)

图6为本发明方法涉及的带轴部件初始位置和目标位置示意图，其中反射靶球5与带轴部件2固结并一起运动，所以可以根据带轴部件的2的目标位置推算反射靶球5的目标位置。

7)获取机械臂的初始雅可比矩阵；从初始位置开始，通过迭代控制机械臂的运动，使得带轴部件逐渐靠近带孔部件，最终实现带轴部件和带孔部件的对准。具体步骤如下：

7-1)令机械臂通过六次独立的试探运动得到一个初始雅可比矩阵J₀，具体做法为每次只运动机械臂的一个自由度。设第i个自由度的运动量为Δα_i，由于机械臂第i个自由度的运动导致的4个靶球坐标向量改变量分别为令

则：

7-2)从初始位置开始，通过迭代控制机械臂的运动，使得带轴部件逐渐靠近带孔部件，最终实现带轴部件和带孔部件的对准。具体步骤如下：

由于雅可比矩阵只在局部有效，机械臂每一步的运动范围需控制在一定范围内，具体范围大小根据对稳定性和效率的需求进行权衡。

7-2-1)在第k步迭代中首先计算4个反射靶球在本步迭代中期望实现的坐标增量ΔP_k：

ΔP_k＝λ(P′-P_k)

其中，P′为4个反射靶球的目标位置坐标向量，P_k为4个反射靶球的第k步迭代时坐标向量，P_ik为第i个靶球在第k步迭代时的坐标，λ＜1是用来控制机械臂运动范围的步长系数，λ越小，机械臂每一步运动的范围越小，λ过大时会导致机械臂运动发散，具体λ大小的选择需根据稳定性和和效率进行权衡设定，例如可以设为0.1。

7-2-2)利用k-1步迭代时估计的第k步迭代的雅可比矩阵计算机械臂各关节在第k步迭代时的增量，当k＝1时使用中试探运动得到的雅可比矩阵J₀进行反算；表达式如下：

其中，Δθ_k为第k步迭代时机械臂各个关节所需运动的增量，是一个6维列向量，是第k-1步迭代时估计的第k步迭代的雅可比矩阵。

7-2-3)根据步骤7-2-2)的计算结果，控制机械臂按照Δθ_k实现各关节对应的运动增量。利用卡尔曼滤波算法估计第k+1步迭代的雅可比矩阵，表达式如下：

其中是是第k步迭代后反射靶球实际坐标增量。

7-2-4)重复步骤7-2-1)至7-2-3)，直到4个反射靶球到达对应的目标位置，从而实现带轴部件与带孔部件的对准，完成多轴孔的对准。

本发明中基于跟踪仪的使用，利用卡尔曼滤波算法实时估计雅可比矩阵，控制机械臂运动实现多轴孔高精度自动化对准，提高装配效率和鲁棒性。

Claims (1)

1.一种基于激光跟踪仪的多轴孔自动化对准方法，其特征在于，该方法包括以下步骤：

1)搭建多轴孔自动化对准系统；所述多轴孔自动化对准系统包括：一台激光跟踪仪，一个机械臂和一台计算机；所述激光跟踪仪与机械臂分别与计算机连接；

2)选取待对准的带孔部件和带轴部件，其中带孔部件具有N个孔，带轴部件具有N个轴，N大于等于3；令带孔部件固定不动，将带轴部件安装在机械臂的端面，令当前位置为初始位置；

3)在带孔部件中选取任意三个不共线的孔建立带孔部件坐标系W₁；具体步骤如下：

3-1)在带孔部件用于建立带孔部件坐标系的三个孔的孔壁上，分别用激光跟踪仪的球形反射靶球或T-Prob的测量头上的球形红宝石紧贴每个孔壁并利用激光跟踪仪测量对应的球心坐标；测量时，每次选取孔壁上的一个点，每个孔壁上共测量P个不同点，P大于等于5，根据每个孔壁上选取的所有点的测量结果通过最小二乘法拟合对应孔壁的圆柱面，得到每个孔壁圆柱面的轴线即为对应孔的孔轴线，最终得到三个孔分别对应的轴线方程；

3-2)在带孔部件的孔端面任意选取M个不共线的点，M大于等于3，将激光跟踪仪的球形反射靶球或T-Prob的测量头上的球形红宝石放置在每个点上并利用激光跟踪仪测量对应的球心坐标；利用每个点测量的球心坐标通过最小二乘法拟合带孔部件的孔端面平面得到初始孔端面平面方程，对初始孔端面平面方程沿平面法向向端面偏移靶球半径或球形红宝石半径的距离得到带孔部件孔端面的平面方程；

3-3)分别计算带孔部件用于建立带孔部件坐标系的三个孔的轴线和孔端面的交点，得到三个交点，用三个交点建立带孔部件坐标系W₁；

4)在带轴部件中选取与建立带孔部件坐标系的三个孔对应的三个轴建立带轴部件坐标系W₂；具体步骤如下：

4-1)在带轴部件中选取与建立带孔部件坐标系的三个孔对应的三个轴，重复步骤3-1)，得到带轴部件三个轴的轴线方程；

4-2)重复步骤3-2)，在带轴部件的轴端面任意选取L个不共线的点，L大于等于3，利用L个不共线的点的测量结果拟合带轴部件轴端面平面，得到带轴部件轴端面的平面方程；

4-3)分别计算带轴部件用于建立带轴部件坐标系的三条轴线和轴端面的三个交点，建立带轴部件坐标系W₂；

5)根据带孔部件坐标系W₁和带轴部件坐标系W₂分别计算两坐标系之间的旋转矩阵R和平移向量T；其中，

T＝O₁-O₂-[D 0 0]^T

式中，O₁为坐标系W₁的原点坐标列向量，O₂为坐标系W₂的原点坐标列向量，D为偏置距离；

R＝[X₁ Y₁ Z₁][X₂ Y₂ Z₂]^-1

式中，X₁为W₁中X轴的方向列向量，Y₁为W₁中Y轴的方向列向量，Z₁为W₁中Z轴的方向列向量，X₂为W₂中X轴的方向列向量，Y₂为W₂中Y轴的方向列向量，Z₂为W₂中Z轴的方向列向量；

6)在带轴部件上安装4个不共线的反射靶球，根据旋转矩阵R和平移向量T计算带轴部件上安装的4个反射靶球分别对应的目标位置；

设四个反射靶球在初始位置坐标分别为p₁、p₂、p₃、p₄，该4个反射靶球的目标位置分别为p′₁、p′₂、p′₃、p′₄，则：

p′_i＝Rp_i+T i＝1～4

7)获取机械臂的初始雅可比矩阵；从初始位置开始，通过迭代控制机械臂的运动，使得带轴部件逐渐靠近带孔部件，最终实现带轴部件和带孔部件的对准；具体步骤如下：

7-1)令机械臂通过六次独立的试探运动得到一个初始雅可比矩阵J₀，具体做法为：每次运动机械臂的一个自由度，设第i个自由度的运动量为Δα_i，则机械臂第i个自由度的运动导致的4个靶球坐标向量改变量分别为令

则：

7-2)从初始位置开始，通过迭代控制机械臂的运动，使得带轴部件逐渐靠近带孔部件，最终实现带轴部件和带孔部件的对准；具体步骤如下：

7-2-1)计算4个反射靶球在第k步迭代中期望实现的坐标增量ΔP_k：

ΔP_k＝λ(P′-P_k)

其中，P′为4个反射靶球的目标位置坐标向量，P_k为4个反射靶球第k步迭代时的坐标向量，P_ik为第i个靶球在第k步迭代时的坐标，λ是用来控制机械臂运动范围的步长系数，λ＜1；

7-2-2)利用第k-1步迭代时估计的第k步迭代的雅可比矩阵计算机械臂各关节在第k步迭代时的增量；表达式如下：

其中，Δθ_k为第k步迭代时机械臂各个关节运动的增量，是第k-1步迭代时估计的第k步迭代的雅可比矩阵；

7-2-3)根据步骤7-2-2)的计算结果，控制机械臂按照Δθ_k实现各关节对应的运动增量；利用卡尔曼滤波算法估计第k+1步迭代的雅可比矩阵，表达式如下：

其中是第k步迭代后反射靶球实际坐标增量；

7-2-4)重复步骤7-2-1)至7-2-3)，直到4个反射靶球到达对应的目标位置，从而实现带轴部件与带孔部件的对准。