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

技术特征：

1.一种双轴孔装配系统，用于将双轴工件(P)装配于双孔工件(H)中，包括：

基座(W)；

机械臂(T)，固定于基座(W)且与双轴工件(P)固定连接；

激光跟踪测量仪(M)，测量双轴工件(P)、双孔工件(H)以及机械臂末端(T1)的位姿；

传感器(S)，实时感测双轴孔装配过程中双轴工件(P)的各轴(P1)与双孔工件(H)的相应孔(H1)之间的接触力及接触力矩；以及

上位机控制系统(C)，电连接于机械臂(T)，且通信连接于传感器(S)并接收传感器(S)传输的接触力和接触力矩的数据以实时操纵机械臂(T)运动。

2.根据权利要求1所述的双轴孔装配系统，其特征在于，

双轴工件(P)包括两个轴(P1)和将两个轴(P1)连接为一体的连接板(P2)；

双轴工件(P)为刚性工件或柔性工件。

3.根据权利要求1所述的双轴孔装配系统，其特征在于，

双孔工件(H)包括两个孔(H1)和用于设置两个孔(H1)的底板(H2)；

双孔工件(H)为刚性工件或柔性工件。

4.根据权利要求1所述的双轴孔装配系统，其特征在于，传感器(S)为力传感器。

5.一种双轴孔装配系统的控制方法，用于控制权利要求1-4中任一项所述的双轴孔装配系统，包括步骤：

S1，确定双轴工件(P)在机械臂末端中心坐标系的转换矩阵T_t^p、双孔工件(H)在机械臂基座坐标系的转换矩阵以及双轴工件(P)在机械臂基座坐标系的转换矩阵包括步骤：

S11，利用激光跟踪测量仪(M)，在激光跟踪测量仪坐标系下，测出双轴工件(P)的双轴底面圆心的三维坐标分别为(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={\left(T_w^t\right)}^{-1}{T}_w^m{T}_m^p,T_w^h=T_w^m{T}_m^h$

$T_w^p=T_w^m{T}_m^p,T_w^h=T_w^m{T}_m^h$

$T_h^p={\left(T_w^h\right)}^{-1}{T}_w^t{T}_t^p$

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

S3，通过上位机控制系统(C)操纵机械臂(T)以实时调整轴孔的相对位置，包括步骤：

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

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

$T_w^p=T_w^t{T}_t^p$

$T_{w2}^p=T_w^h$

$T_{w2}^p(3,4)=T_w^p(3,4)$

$T_{w2}^p=T_w^{p}dT\→dT={\left(T_w^p\right)}^{-1}{T_w^p}_2$

${T_w^p}_2=T_w^t{T}_t^{p}dT=T_w^t{\mathrm{dT}}_2{T}_t^p\→{\mathrm{dT}}_2=T_t^{p}dT{\left(T_t^p\right)}^{-1}\→T_{w2}^t=T_w^t{\mathrm{dT}}_2$

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

$(X_{tw2},Y_{tw2},Z_{tw2})=\left(T_{w2}^t\right(1,4),T_{w2}^t(2,4),T_{w2}^t(3,4\left)\right)$

$\left\{\begin{array}{c}{\mathrm{EZ}}_2=a\tan 2\{T_{w2}^t(2,1),T_{w2}^t(1,1)\}\\ {\mathrm{EY}}_2=a\tan 2\{-T_{w2}^t(3,1),\cos \left({\mathrm{EZ}}_2\right)T_{w2}^t(1,1)+\sin \left({\mathrm{EZ}}_2\right)T_{w2}^t(2,1)\}\\ {\mathrm{EX}}_2=a\tan 2\{\sin \left({\mathrm{EZ}}_2\right)*T_{w2}^t(1,3)-\cos \left({\mathrm{EZ}}_2\right)*T_{w2}^t(2,3),-\sin \left({\mathrm{EZ}}_2\right)*T_{w2}^t(1,2)+\cos \left({\mathrm{EZ}}_2\right)*T_{w2}^t(2,2)\}\end{array}\right.$

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

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

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

6.根据权利要求5所述的双轴孔装配系统的控制方法，其特征在于，在步骤S12中，

以双轴底面圆心连线的中心为原点、连线为Y_p轴、双轴工件(P)的连接板(P2)的法向量为Z_p轴、由Y_p叉乘Z_p轴得到X_p轴建立双轴坐标系，并用激光跟踪测量仪(M)测出双轴坐标系的原点在激光跟踪测量仪坐标系下的三维坐标O_p，激光跟踪测量仪坐标系与双轴坐标系的转换关系如下：

$T_m^p=\left[\begin{array}{cccc}{X}_p& Y_p& Z_p& O_p\\ 0& 0& 0& 1\end{array}\right]$

$\left\{\begin{array}{c}{Y}_p={\left(\right(X_{prm}-X_{plm})/C_p,(Y_{prm}-Y_{plm})/C_p,(Z_{prm}-Z_{plm})/C_p)}^T\\ C_p=\sqrt{{(X_{prm}-X_{plm})}^2+{(Y_{prm}-Y_{plm})}^2+{(Z_{prm}-Z_{prm})}^2}\\ Z_p={(X_{pnm},Y_{pnm},Z_{pnm})}^T\\ O_p={\left(\right(X_{prm}+X_{plm})/2,(Y_{prm}+Y_{plm})/2,(Z_{prm}+Z_{plm})/2)}^T\\ X_p=Y_p\×Z_p\end{array}\right.$

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

$T_m^h=\left[\begin{array}{cccc}{X}_h& Y_h& Z_h& O_h\\ 0& 0& 0& 1\end{array}\right]$

$\left\{\begin{array}{c}{Y}_h={\left(\right(X_{hrm}-X_{hlm})/C_h,(Y_{hrm}-Y_{hlm})/C_h,(Z_{hrm}-Z_{hlm})/C_h)}^T\\ C_h=\sqrt{{(X_{hrm}-X_{hlm})}^2+{(Y_{hrm}-Y_{hlm})}^2+{(Z_{hrm}-Z_{hrm})}^2}\\ Z_h={(X_{hnm},Y_{hnm},Z_{hnm})}^T\\ O_h={\left(\right(X_{hrm}+X_{hlm})/2,(Y_{hrm}+Y_{hlm})/2,(Z_{hrm}+Z_{hlm})/2)}^T\\ X_h=Y_h\×Z_h\end{array}\right.;$

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

$T_w^t=\left[\begin{array}{cccc}\cos \left(EZ\right)\cos \left(EY\right)& \cos \left(EZ\right)\sin \left(EY\right)\sin \left(EX\right)-\sin \left(EZ\right)\cos \left(EX\right)& \cos \left(EZ\right)\sin \left(EY\right)\cos \left(EX\right)+\sin \left(EZ\right)\sin \left(EX\right)& X_{tw}\\ \sin \left(EZ\right)\cos \left(EY\right)& \sin \left(EZ\right)\sin \left(EY\right)\sin \left(EX\right)+\cos \left(EZ\right)\cos \left(EX\right)& \sin \left(EZ\right)\sin \left(EY\right)\cos \left(EX\right)-\cos \left(EZ\right)\sin \left(EX\right)& Y_{tw}\\ -\sin \left(EY\right)& \cos \left(EY\right)\sin \left(EX\right)& \cos \left(EY\right)\cos \left(EX\right)& Z_{tw}\\ 0& 0& 0& 1\end{array}\right]$

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

$T_w^m{(X_{tm},Y_{tm},Z_{tm},1)}^T={(X_{tw},Y_{tw},Z_{tw},1)}^T.$

7.根据权利要求5所述的双轴孔装配系统的控制方法，其特征在于，在步骤S5中，上位机控制系统(C)中设置有竖直方向的接触力阈值，当双轴工件(P)与双孔工件(H)接触时的接触力到达该阈值后，上位机控制系统(C)控制双轴工件(P)停止下降。

8.根据权利要求5所述的双轴孔装配系统的控制方法，其特征在于，在步骤S5中，增量型P控制方法的算法为：

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

$T_{wk}^p=T_{wk}^t{T}_t^p$

$e_{zk}=T_w^h(3,4)-T_w^p(3,4)$

其中，表示孔坐标系的原点的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表示比例因子，且为一常数。

9.根据权利要求8所述的双轴孔装配系统的控制方法，其特征在于，在增量型P控制循环过程中，当双轴坐标系原点的Z坐标值与双孔坐标系原点的Z坐标值的差值小于0.001mm时，循环停止。

10.根据权利要求5所述的双轴孔装配系统的控制方法，其特征在于，在步骤S6中，阻抗控制方法的算法为：

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)和各坐标轴的转动量，计算公式为：

$\left\{\begin{array}{c}{\mathrm{dX}}_k={\mathrm{dX}}_{k-1}+0.1K_p\left({\mathrm{dF}}_k\right(1)-{\mathrm{dF}}_{k-1}(1)+0.1K_d({\mathrm{dF}}_k\left(1\right)-2{\mathrm{dF}}_{k-1}\left(1\right)+{\mathrm{dF}}_{k-2}\left(1\right)\left)\right)\\ {\mathrm{dY}}_k={\mathrm{dY}}_{k-1}+0.1K_p\left({\mathrm{dF}}_k\right(2)-{\mathrm{dF}}_{k-1}(2)+0.1K_d({\mathrm{dF}}_k\left(2\right)-2{\mathrm{dF}}_{k-1}\left(2\right)+{\mathrm{dF}}_{k-2}\left(2\right)\left)\right)\\ {\mathrm{dZ}}_k={\mathrm{dZ}}_{k-1}+K_p\left({\mathrm{dF}}_k\right(3)-{\mathrm{dF}}_{k-1}(3)+K_d({\mathrm{dF}}_k\left(3\right)-2{\mathrm{dF}}_{k-1}\left(3\right)+{\mathrm{dF}}_{k-2}\left(3\right)\left)\right)\\ {\mathrm{d\θ}}_{xk}=\left\{\begin{array}{ccc}0.01{\mathrm{dF}}_k\left(4\right)/\left|{\mathrm{dF}}_k\left(4\right)\right|,& if& \left|{\mathrm{dF}}_k\left(4\right)\right|\>0.5\\ 0,& if& \left|{\mathrm{dF}}_k\left(4\right)\right|\≤0.5\end{array}\right.\\ {\mathrm{d\θ}}_{yk}=\left\{\begin{array}{ccc}0.01{\mathrm{dF}}_k\left(5\right)/\left|{\mathrm{dF}}_k\left(5\right)\right|,& if& \left|{\mathrm{dF}}_k\left(5\right)\right|\>0.5\\ 0,& if& \left|{\mathrm{dF}}_k\left(5\right)\right|\≤0.5\end{array}\right.\\ {\mathrm{d\θ}}_{zk}=\left\{\begin{array}{ccc}0.01{\mathrm{dF}}_k\left(6\right)/\left|{\mathrm{dF}}_k\left(6\right)\right|,& if& \left|{\mathrm{dF}}_k\left(6\right)\right|\>0.5\\ 0,& if& \left|{\mathrm{dF}}_k\left(6\right)\right|\≤0.5\end{array}\right.\\ V_{zk}=K_v|{\mathrm{dF}}_k\left(1\right)+{\mathrm{dF}}_k\left(2\right)+{\mathrm{dF}}_k\left(3\right)|\end{array}\right.$

其中，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-1为第k-1次循环中接触力与接触力参考值的差以及接触力矩与接触力矩参考值的差构成的六维力，dF_k-2为第k-2次循环中接触力与接触力参考值的差以及接触力矩与接触力矩参考值的差构成的六维力，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，计算公式为：

$\left\{\begin{array}{c}{\mathrm{dT}}_{kpos}=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{dX}}_k\\ 0& 1& 0& {\mathrm{dY}}_k\\ 0& 0& 1& {\mathrm{dZ}}_k\\ 0& 0& 0& 1\end{array}\right]\\ {\mathrm{dT}}_{kx}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos\left({\mathrm{d\θ}}_{xk}\right)& -sin\left({\mathrm{d\θ}}_{xk}\right)& 0\\ 0& \sin \left({\mathrm{d\θ}}_{xk}\right)& \cos \left({\mathrm{d\θ}}_{xk}\right)& 0\\ 0& 0& 0& 1\end{array}\right]\\ {\mathrm{dT}}_{ky}=\left[\begin{array}{cccc}\cos \left({\mathrm{d\θ}}_{yk}\right)& 0& sin\left({\mathrm{d\θ}}_{yk}\right)& 0\\ 0& 1& 0& 0\\ -sin\left({\mathrm{d\θ}}_{yk}\right)& 0& \cos \left({\mathrm{d\θ}}_{yk}\right)& 0\\ 0& 0& 0& 1\end{array}\right]\\ {\mathrm{dT}}_{kz}=\left[\begin{array}{cccc}\cos \left({\mathrm{d\θ}}_{zk}\right)& -\sin \left({\mathrm{d\θ}}_{zk}\right)& 0& 0\\ \sin \left({\mathrm{d\θ}}_{zk}\right)& \cos \left({\mathrm{d\θ}}_{zk}\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}\right.;$

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

dT_k＝dT_kxdT_kydT_kzdT_kpos

$T_{wk}^t=T_t^p{\mathrm{dT}}_{kx}{\mathrm{dT}}_{ky}{\mathrm{dT}}_{kz}{\mathrm{dT}}_{kpos}{\left(T_t^p\right)}^{-1}$

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

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值。