双轴孔装配系统及其控制方法与流程

本发明涉及大型多轴孔部件装配领域，尤其涉及一种双轴孔装配系统及其控制方法。

背景技术：

在产品生产制造过程中，装配作业是非常重要的环节，该环节将直接影响最终产品质量。据统计，在机械电子类产品的整个制造工作中，装配工作量占到20％～70％，装配费用也占到了总成本的1/3～1/2。目前工业中的装配环节很多还是由装配工人完成，但是人工装配存在很多问题，例如效率低，成本高，工人操作要求高，而且还容易发生安全事故。尤其对于大型工件的装配，工件太重，人工装配很不方便。在这样的背景下，能够进行自动装配的机器人便显得尤为重要。相比于人工装配，机器人适用的范围更广，尤其适用于重型工件等特殊环境下的装配。

通过对已公开的文献、专利和工业产品调研发现，机器人装配主要可以采用两种方式，一种是基于视觉伺服的装配方式，视觉伺服是通过采集图像并进行对比，从而判断此时工件的位姿，并将判断结果反馈给机械臂进行调整的一种方法。然而视觉伺服控制机械臂进行自动装配的方法还存在以下不足：(1)视觉伺服控制无法精确地控制装配工件的接触力大小，有可能发生严重的磕碰以致损伤工件；(2)存在局部遮挡和特征点不明显时，造成位姿判断出错或者无法判断的问题。

另外一种装配方式是基于力觉的装配，也成为机器人的力控制。力控制可以分为被动力控制和主动力控制。被动力控制是设计柔顺的末端关节，帮助工件在位置不完全准确的情况下完成装配。主动力控制则是利用力传感器对机器人末端受力进行实时的测量，通过比较参考力与真实力，判断当前的接触情况，控制机械臂运用以减小接触力，从而可以更好地完成装配。然而 现有的力控制装配方法还有如下几个问题：(1)被动力控制方法柔顺程度有限，且针对不同的工件需要设计不同的柔顺机械装置，适用程度有限。且因为柔性装置自由空间太大，不方便精确地控制；(2)现有的主动力控制装配方法主要适用于单轴孔装配，很少适用于多轴孔装配，而且没有发现适用于柔性多轴孔装配的方法。对于大型工件，例如飞机，汽车等工件，其体积大质量高，本身会因为重力等作用造成变形，从而不能完全视为刚性体，且这些大型工件的装配主要为多轴孔装配，因此需要对柔性多轴孔的装配方法进行研究。

技术实现要素：

鉴于背景技术中存在的问题，本发明的目的在于提供一种双轴孔装配系统及其控制方法，其适用范围广，装配精度高，稳定性好。

为了实现上述目的，在第一方面，本发明提供了一种双轴孔装配系统，其用于将双轴工件装配于双孔工件中，包括：基座；机械臂，固定于基座且与双轴工件固定连接；激光跟踪测量仪，测量双轴工件、双孔工件以及机械臂末端的位姿；传感器，实时感测双轴孔装配过程中双轴工件的各轴与双孔工件的相应孔之间的接触力及接触力矩；以及上位机控制系统，电连接于机械臂，且通信连接于传感器并接收传感器传输的接触力和接触力矩的数据以实时操纵机械臂运动。

为了实现上述目的，在第二方面，本发明提供了一种双轴孔装配系统的控制方法，其用于控制本发明第一方面所述的双轴孔装配系统，包括步骤S1、S2、S3、S4、S5以及S6。

S1，确定双轴工件在机械臂末端中心坐标系的转换矩阵T_t^p、双孔工件在机械臂基座坐标系的转换矩阵以及双轴工件在机械臂基座坐标系的转换矩阵包括步骤：S11，利用激光跟踪测量仪，在激光跟踪测量仪坐标系下，测出双轴工件的双轴底面圆心的三维坐标分别为(X_plm，Y_plm，Z_plm)、(X_prm，Y_prm，Z_prm)，双轴底面的法向量为(X_pnm，Y_pnm，Z_pnm)，双孔工件(H)的双孔顶面圆心的三维坐标分别为(X_hlm，Y_hlm，Z_hlm)、(X_hrm，Y_hrm，Z_hrm)，双孔顶面的法向量为(X_hnm，Y_hnm，Z_hnm)，机械臂末端的中心的三维坐标为(X_tm，Y_tm，Z_tm)；S12，建立双轴坐标系和双孔坐标系，并计算 出激光跟踪测量仪坐标系到双轴坐标系的转换矩阵和激光跟踪测量仪坐标系到双孔坐标系的转换矩阵S13，机械臂自动读取机械臂末端的中心在机械臂基座坐标系下的三维坐标(X_tw，Y_tw，Z_tw)和欧拉角(EX，EY，EZ)，并计算出机械臂末端中心坐标系到机械臂基座坐标系的转换矩阵和激光跟踪测量仪坐标系到机械臂基座坐标系的转换矩阵S14，根据步骤S12求得的和S13求得的和分别得到和的表达式，即：

S2，上位机控制系统初始化，检查所有传感器是否正常工作以及传感器与上位机控制系统之间通信是否正常，并对传感器进行回零标定。

S3，通过上位机控制系统操纵机械臂以实时调整轴孔的相对位置，包括步骤S31、S32和S33。

S31，调整机械臂以使双轴坐标系和双孔坐标系初步对齐，此时与中除了对应原点的Z坐标值不相等之外其它值均相等，对齐前机械臂末端中心坐标系到机械臂基座坐标系的转换矩阵为双轴工件坐标系到机械臂基座坐标系的转换矩阵令经调整对齐之后的机械臂末端中心坐标系到机械臂基座坐标系的转换矩阵为经调整对齐之后的双轴工件坐标系到机械臂基座坐标系的转换矩阵为

S32，根据S31中的初始值，计算出调整之后的设调整对齐前后双轴坐标系到机械臂基座坐标系的转换矩阵的变化矩阵为dT，机械臂末端中心坐标系到机械臂基座坐标系的转换矩阵的变化矩阵为dT2，计算过程为：

S33，根据S32中得到的解算出调整之后机械臂末端的中心在机械臂基座坐标系中的三维坐标(X_tw2，Y_tw2，Z_tw2)和欧拉角(EX2，EY2，EZ2)，解算公式为：

S4，上位机控制系统将机械臂末端的中心调整到步骤S33中得到的三维坐标为(X_tw2，Y_tw2，Z_tw2)、欧拉角为(EX2，EY2，EZ2)的位姿，完成轴孔对齐。

S5，上位机控制系统采用增量型P控制方法并基于双轴工件的位置反馈和双轴坐标系原点的Z坐标值与双孔坐标系原点的Z坐标值的差值大小操纵机械臂运动，以使双轴工件竖直下降、逐渐与双孔工件接近、直至接触。

S6，上位机控制系统采用阻抗控制方法并基于传感器实时感测到的双轴孔装配过程中双轴工件的各轴与双孔工件的相应孔之间的接触力以及接触力矩操纵机械臂运动，以使双轴工件继续下降直至与双孔工件完全配合。

本发明的有益效果如下：

在根据本发明的双轴孔装配系统中，上位机控制系统基于机械臂内部的自标定程序、激光跟踪测量仪测量出的双轴工件、双孔工件以及机械臂末端的位姿完成双轴工件和双孔工件的初步对准，并进一步基于与传感器的通信合作以实时操纵机械臂运动，进而完成双轴工件和双孔工件的装配，其装配精度高，稳定性好，适用范围广。

附图说明

图1是根据本发明的双轴孔装配系统的整体装配图；

图2至图5是根据本发明的双轴孔装配系统的控制方法控制双轴孔装配系统的过程示意图，其中图2是双轴工件P安装于双孔工件H前的位置示意 图，图3是双轴工件P与双孔工件H安装时且双轴工件P与双孔工件H对齐时的位置示意图，图4是双轴工件P下移并与双孔工件H接触时的位置示意图，图5是双轴工件P与双孔工件H完全装配时的示意图；

图6是根据本发明的双轴孔装配系统的控制方法中的各坐标系的转换关系示意图；

图7是根据本发明的双轴孔装配系统的控制方法中的增量型P控制方法的控制流程图；

图8是根据本发明的双轴孔装配系统的控制方法中的阻抗控制方法的控制流程图。

其中，附图标记说明如下：

W基座 P1轴

T机械臂 P2连接板

M激光跟踪测量仪 H双孔工件

S传感器 H1孔

C上位机控制系统 H2底板

P双轴工件

具体实施方式

下面参照附图来详细说明根据本发明的双轴孔装配系统及其控制方法。

首先说明本发明第一方面的双轴孔装配系统。

参照图1，根据本发明的双轴孔装配系统用于将双轴工件P装配于双孔工件H中，包括：基座W；机械臂T(内设有自标定程序)，固定于基座W且与双轴工件P固定连接；激光跟踪测量仪M，测量双轴工件P、双孔工件H以及机械臂末端T1的位姿；传感器S，实时感测双轴孔装配过程中双轴工件P的各轴P1与双孔工件H的相应孔H1之间的接触力及接触力矩；以及上位机控制系统C，电连接于机械臂T，且通信连接于传感器S并接收传感器S传输的接触力和接触力矩的数据以实时操纵机械臂T运动。

在根据本发明的双轴孔装配系统中，上位机控制系统C基于机械臂T内部的自标定程序、激光跟踪测量仪M测量出的双轴工件P、双孔工件H以及 机械臂末端T1的位姿完成双轴工件P和双孔工件H的初步对准，并进一步基于与传感器S的通信合作以实时操纵机械臂T运动，进而完成双轴工件P和双孔工件H的装配，其装配精度高，稳定性好，适用范围广。

在根据本发明的双轴孔装配系统中，双轴工件P包括两个轴P1和将两个轴P1连接为一体的连接板P2。双轴工件P可为刚性工件或柔性工件。

在根据本发明的双轴孔装配系统中，双孔工件H包括两个孔H1和用于设置两个孔H1的底板H2。双孔工件H可为刚性工件或柔性工件。

在这里补充说明的是，机械臂末端T1的位姿在本发明中具体指的是机械臂末端T1的中心的位置坐标。双孔工件H的位置应处于机械臂末端T1的运动范围之内。

根据本发明的双轴孔装配系统，在一实施例中，传感器S可为力传感器。

其次说明根据本发明第二方面的双轴孔装配系统的控制方法。

参照图2至图8，双轴孔装配系统的控制方法用于控制本发明第一方面所述的双轴孔装配系统，包括步骤S1、S2、S3、S4、S5以及S6。

S1，参照图6，确定双轴工件P在机械臂末端中心坐标系的转换矩阵T_t^p、双孔工件H在机械臂基座坐标系的转换矩阵以及双轴工件P在机械臂基座坐标系的转换矩阵包括步骤：S11，利用激光跟踪测量仪M，在激光跟踪测量仪坐标系下，测出双轴工件P中的两个轴P1的底面圆心的三维坐标分别为(X_plm，Y_plm，Z_plm)、(X_prm，Y_prm，Z_prm)，双轴底面的法向量为(X_pnm，Y_pnm，Z_pnm)，双孔工件H的双孔顶面圆心的三维坐标分别为(X_hlm，Y_hlm，Z_hlm)、(X_hrm，Y_hrm，Z_hrm)，双孔顶面的法向量为(X_hnm，Y_hnm，Z_hnm)，机械臂末端T1的中心的三维坐标为(X_tm，Y_tm，Z_tm)；S12，建立双轴坐标系和双孔坐标系，并计算出激光跟踪测量仪坐标系到双轴坐标系的转换矩阵 和激光跟踪测量仪坐标系到双孔坐标系的转换矩阵S13，机械臂T自动读取机械臂末端T1的中心在机械臂基座坐标系下的三维坐标(X_tw，Y_tw，Z_tw)和欧拉角(EX，EY，EZ)，并计算出机械臂末端中心坐标系到机械臂基座坐标系的转换矩阵和激光跟踪测量仪坐标系到机械臂基座坐标系的 转换矩阵S14，根据步骤S12求得的和S13求得的和分别得到T_t^p、和的表达式，即：

其中，为的逆矩阵，为的逆矩阵。

在这里补充说明的是，“机械臂T自动读取(基于内部设置的自标定程序)”属于机械臂T自带功能，属于公知常识。

S2，上位机控制系统C初始化，检查所有传感器S是否正常工作以及传感器S与上位机控制系统C之间通信是否正常，并对传感器S进行回零标定。

S3，通过上位机控制系统C操纵机械臂T以实时调整轴孔的相对位置，包括步骤S31、S32和S33。

S31，调整机械臂T以使双轴坐标系和双孔坐标系初步对齐(即双轴坐标系的原点和双孔坐标系的原点的连线垂直于双孔顶面)，此时可以机械臂基座坐标系为参考，则中除了双轴坐标系的原点的Z坐标值与中的双孔坐标系的原点的Z坐标值不相等之外，与中的其它值均相等。对齐前机械臂末端中心坐标系到机械臂基座坐标系的转换矩阵为双轴工件P坐标系到机械臂基座坐标系的转换矩阵令经调整对齐之后的机械臂末端中心坐标系到机械臂基座坐标系的转换矩阵为经调整对齐之后的双轴工件P坐标系到机械臂基座坐标系的转换矩阵为

S32，根据S31中的初始值，计算出调整之后的设调整对齐前后双轴坐标系到机械臂基座坐标系的转换矩阵的变化矩阵为dT，机械臂末端中心坐标系到机械臂基座坐标系的转换矩阵的变化矩阵为dT2，计算过程为：

其中，为T_t^p的逆矩阵，为的逆矩阵。

在这里补充说明的是，为了防止双轴工件P和双孔工件H发生磕碰，双轴坐标系和双孔坐标系在初步对齐操作中始终不会下移，即其中，表示中的第三行第四列的值(即对齐调整之前双轴坐标系的原点的Z坐标值)，表示中的第三行第四列的值(即对齐调整之后双轴坐标系的原点的Z坐标值)。

S33，根据S32中得到的解算出调整之后机械臂末端T1的中心在机械臂基座坐标系中的三维坐标(X_tw2，Y_tw2，Z_tw2)和欧拉角(EX2，EY2，EZ2)，解算公式为：

S4，上位机控制系统C将机械臂末端T1的中心调整到步骤S33中得到的三维坐标为(X_tw2，Y_tw2，Z_tw2)、欧拉角为(EX2，EY2，EZ2)的位姿，完成轴孔对齐(如图3所示)。

S5，参照图3、图4和图7，上位机控制系统C采用增量型P控制方法并基于双轴工件P的位置反馈和双轴坐标系原点的Z坐标值与双孔坐标系原点的Z坐标值的差值大小操纵机械臂T运动，以使双轴工件P竖直下降、逐渐与双孔工件H接近、直至接触(如图4所示)，此时双轴坐标系的原点在机械臂基座坐标系下的Z坐标值为Z₀(预设为100mm)。

S6，参照图4、图5和图8，上位机控制系统C采用阻抗控制方法并基于传感器S实时感测到的双轴孔装配过程中双轴工件P的各轴P1与双孔工件H的相应孔H1之间的接触力以及接触力矩操纵机械臂T运动，以使双轴工件P继续下降直至与双孔工件H完全配合。

根据本发明的双轴孔装配系统的控制方法，在步骤S12中，可以以双轴底面圆心连线的中心为原点、连线为Y_p轴、双轴工件(P)的连接板(P2) 的法向量为Z_p轴、由Y_p叉乘Z_p轴得到X_p轴建立双轴坐标系(如图1所示)，并用激光跟踪测量仪M测出双轴坐标系的原点在激光跟踪测量仪坐标系下的三维坐标O_p，则激光跟踪测量仪坐标系与双轴坐标系的转换关系如下：

可以以双孔顶面圆心连线的中心为原点、连线为Y_h轴、双孔顶面所在平面的法向量为Z_h轴、由Y_h叉乘Z_h轴得到X_h轴建立双孔坐标系(如图1所示)，并用激光跟踪测量仪M测出双孔坐标系的原点在激光跟踪测量仪坐标系下的三维坐标O_h，则激光跟踪测量仪坐标系与双孔坐标系的转换关系如下：

在步骤S13中，机械臂末端中心坐标系到机械臂基座坐标系的转换矩阵 的计算公式为：

激光跟踪测量仪坐标系到机械臂基座坐标系的转换矩阵的计算公式为：

即，

根据本发明的双轴孔装配系统的控制方法，在步骤S5中，上位机控制系统C中设置有竖直方向的接触力阈值，当双轴工件P与双孔工件H接触时的接触力到达该阈值后，上位机控制系统C控制双轴工件P停止下降。通常上位机控制系统C中设置的接触力的阈值可为1N～3N，但不仅限如此，该接触力的阈值可根据双轴工件P与双孔工件H的不同程度的柔性(或刚性)进行适当调整。

根据本发明的双轴孔装配系统的控制方法，在步骤S5中，参照图7，增量型P控制方法的算法为：

P1，计算第k次循环中机械臂基座坐标系到双轴坐标系的转换矩阵以及此时双轴坐标系原点的Z坐标值与双孔坐标系原点的Z坐标值的差值，即：

其中，表示双孔坐标系的原点的Z坐标值，表示双轴坐标系的原点的Z坐标值；

P2，第k次循环中双轴坐标系原点的下移量为dZ_k，由于增量型P控制为比例控制，则dZ_k＝K_pe_zk，计算第k+1次循环中机械臂末端T1的中心的三维坐标(X_tw(k+1)，Y_tw(k+1)，Z_tw(k+1))，即：

(X_tw(k+1),Y_tw(k+1),Z_tw(k+1))＝(X_twk,Y_twk,Z_twk+dZ_k)

其中K_p表示比例因子，且为一常数。

在这里补充说明的是，增量型P控制方法的控制过程为：将第k次循环测量得到的Z(k)值与Z₀值(上述已设定Z₀＝100mm)进行比较，当Z(k)值大于Z₀时，说明此时轴P1还未下移到指定位置，因此位置误差e_zk为负值，此时再乘以比例系数K_p得到的位置增量dZ_k也为负值，轴P1便会继续下降(反 之说明下移过度，位置增量便为正，轴P1会上移)并接着进行第k+1次循环。其中，k取整数。

在一实施例中，K_p＝0.9。

根据本发明的双轴孔装配系统的控制方法，在增量型P控制循环过程中，当双轴坐标系原点的Z坐标值与双孔坐标系原点的Z坐标值的差值(即dZ_k＝Z_tw(k+1)-Z_twk)小于0.001mm时，循环停止。

根据本发明的双轴孔装配系统的控制方法，在步骤S6中，参照图8，阻抗控制方法的算法为：

K1，设置相关参数(参数可以随操作者的具体实验进行调整)，K_p＝0.02，K_d＝0.002，K_v＝5，[F_x0,F_y0,F_z0]＝[5,10,20](N)，[M_x0,M_y0,M_z0]＝[0,0,0]((N·m)，Z_C＝-5mm，其中K_p为阻抗控制中的比例因子，K_d为阻抗控制中的微分参数，K_v为阻阻抗控制中的阻尼参数，[F_x0,F_y0,F_z0]为接触力参考值，[M_x0,M_y0,M_z0]为接触力矩参考值，Z_C为机械臂末端T1的中心的Z坐标总下移量；

K2，在第k次循环中，由力传感器S采集到的接触力和接触力矩分别为[F_xk,F_yk,F_zk]、[M_xk,M_yk,M_zk]，即：

F_k＝[F_xk,F_yk,F_zk,M_xk,M_yk,M_zk]

dF_k＝[F_x0-F_xk,F_y0-F_yk,F_z0-F_zk,M_x0-M_xk,M_y0-M_yk,M_z0-M_zk]

其中，F_k为第k次循环中接触力和接触力矩构成的六维力，dF_k为接触力与接触力参考值的差以及接触力矩与接触力矩参考值的差构成的六维力；

K3，计算第k次循环中需要调整的位姿，即机械臂末端中心坐标系的原点平移量(dX_k,dY_k,dZ_k)和各坐标轴的转动量，计算公式为：

其中，dX_k为第k次循环中机械臂末端中心坐标系的原点在X方向的平移量、dY_k为第k次循环中机械臂末端中心坐标系的原点在Y方向的平移量、dZ_k为第k次循环中机械臂末端中心坐标系的原点在Z方向的平移量、dθ_xk为第k次循环中机械臂末端中心坐标系绕X坐标轴的转动量、dθ_yk为第k次循环中机械臂末端中心坐标系绕Y坐标轴的转动量、dθ_zk为第k次循环中机械臂末端中心坐标系绕Z坐标轴的转动量，dX_k-1为第k-1次循环中机械臂末端中心坐标系的原点在X方向的平移量、dY_k-1为第k-1次循环中机械臂末端中心坐标系的原点在Y方向的平移量，dZ_k-1为第k-1次循环中机械臂末端中心坐标系的原点在Z方向的平移量，dF_k为第k次循环中接触力与接触力参考值的差以及接触力矩与接触力矩参考值的差构成的六维力，dF_k-1为第k-1次循环中接触力与接触力参考值的差以及接触力矩与接触力矩参考值的差构成的六维力，dF_k-2为第k-2次循环中接触力与接触力参考值的差以及接触力矩与接触力矩参考值的差构成的六维力，V_Zk为第k次循环中双轴工件P的下移速度。dF_k(1)为dF_k表达式中的第一个值，dF_k(2)为dF_k表达式中的第二个值，dF_k(3)、dF_k(4)、dF_k(5)、dF_k(6)依次类推。同理，dF_k-1(1)为dF_k-1表达式中的第一个值，dF_k-2(1)为dF_k-2表达式中的第一个值，依次类推。

K4，根据K3中得到的dX_k、dY_k、dZ_k、dθ_xk、dθ_yk、dθ_zk，计算机械臂末端中心坐标系的原点需要调整的变换矩阵dT_kpos、以及各坐标轴需要调整的变换矩阵dT_kx、dT_ky、dT_kz，计算公式为：

K5，根据K4中得到的dT_kx、dT_ky、dT_kz，计算机械臂末端中心坐标系需要调整的总变换矩阵dT_k，以及变换之后的机械臂基座坐标系到机械臂末端中心坐标系的转换矩阵计算公式为：

dT_k＝dT_kx dT_ky dT_kz dT_kpos

K6，根据K2-K5中给出的计算式，分别计算每次循环调整后的机械臂末端T1的中心的位姿，直到机械臂末端T1的中心的Z坐标总下移量接近设定的Z_C值(此步骤中Z_C＝-5mm)。

在这里补充说明的是，图8中的Z_k(T1)为在第K次循环时机械臂T自动读取出的机械臂末端T1的中心的Z坐标值，dZ_k(T1)为第k次循环中机械臂末端T1的中心的下移量。其中，在阻抗控制中，因为接触力方向为负，所以当第k次循环中测量得到的接触力F_k的绝对值小于设定的参考力F₀的绝对值时，此时计算出来的接触力误差dF_k为负值，接触力误差dF_k乘上比例因子K_p后得到的Z坐标增量dZ_k(T1)也为负值，说明此时没有卡阻，双轴工件P可以继续下降(反之得到的Z坐标增量dZ_k(T1)为正值，说明此时轴P1被卡住，需要抽出一点，轴P1便上移)并接着进行第k+1次循环，直到机械臂 末端T1的中心的Z坐标总下移量接近设定的Z_C值，循环停止。其中，k取整数。

K7，改变K1中的相关参数，即接触力参考值为[F_x,F_y,F_z]＝[0,0,50](N)，接触力矩参考值为[M_x,M_y,M_z]＝[0,0,0](N·m)，Z坐标总下移量Z_C＝-100mm，继续按照K2-K5中给出的计算式计算出每次循环调整后的机械臂末端T1的中心的位姿，直到机械臂末端T1的中心的Z坐标总下移量接近设定的Z_C值(此步骤中Z_C＝-100mm)。