CN110355557A - 一种大尺寸轴孔工件装配的螺旋插装方法 - Google Patents

Abstract

本发明提出一种大尺寸轴孔工件装配的螺旋插装方法，属于大尺寸轴孔工件装配领域。该方法的实现过程中，机械臂接收上位机的运动指令，操纵大尺寸孔工件轴以往复式螺旋运动的形式插入大尺寸工件孔中，力传感器实时监测轴孔之间的接触力/力矩，上位机利用装配过程中机械臂末端的位姿和轴孔之间的接触力/力矩，实现调整轴孔之间相对位姿的功能，同时用于监测轴孔装配的卡阻状态，调整螺旋插装的运动参数，以减小大尺寸轴孔工件之间的摩擦阻力，避免机械卡阻的发生。本发明使得机械臂在大尺寸轴孔工件的装配过程中更容易避免机械卡阻的发生，操作方法简单易行，能有效地提高装配效率和装配成功率。

Description

一种大尺寸轴孔工件装配的螺旋插装方法

技术领域

本发明涉及大尺寸轴孔工件装配领域，尤其涉及一种大尺寸轴孔工件装配的螺旋插装方法。

背景技术

装配作业是生产制造过程中至关重要的环节之一，直接决定了产品的最终质量，而轴孔装配则是装配作业中最常见且最重要的一种配合形式。目前工业领域的大部分装配环节仍由人工操作或半自动化工装完成，随着自动化技术的发展，使用机器人实现自动化装配操作已成为一种趋势。

轴孔装配过程中的机械卡阻问题，是机器人自动化轴孔装配失败的主要原因。轴孔配合的间隙根据工艺需求，变化范围由几毫米到几微米，而现有的机器人定位精度往往难以满足高精密轴孔装配的精度要求，轴孔之间微小的位姿偏差都会造成较大的接触力，导致机械卡阻的发生。

为了克服轴孔装配过程中的机械卡阻问题，现有的轴孔装配方法往往采用力控来实现装配过程的柔顺运动，主要包括被动柔顺和主动柔顺两种方式。被动柔顺方法是指设计弹性元件组成的具有特性柔顺特性的柔性手腕，利用结构柔性自适应补偿轴孔工件之间的位姿偏差，但是这种方法的适用范围有限，对于大尺寸工件、非垂直装配操作以及精密轴孔装配等情况都很难奏效。主动柔顺方法则是利用力传感器实时监测轴孔装配过程中的接触力的大小，并依据阻抗控制算法或者力位混合控制算法对轴孔之间的位姿偏差进行补偿，这种方法适用范围更广。但是现有的主动柔顺装配方法在面对大尺寸工件之间的轴孔装配时仍然有一定的困难。在航空航天、船舶制造、汽车制造等领域，工件的结构较复杂，尺寸较大(跨度在0.5m以上，配合孔径在80mm以上)，重载操作使得轴孔工件在装配过程中产生了更大的接触压力和摩擦阻力，尤其是轴向的摩擦阻力的增大，使得轴孔之间更容易发生机械卡阻，而且一旦出现卡阻就很难恢复和调整。同时，过大的接触力也使得阻抗控制算法的运动步长较小，影响了轴孔装配的效率。因此，一种针对大尺寸轴孔工件装配的机械卡阻问题的装配方法十分有必要。

发明内容

本发明的目的是为克服大尺寸轴孔工件(跨度大于0.5m，配合孔径大于80mm)的装配中容易出现的机械卡阻问题，提出一种大尺寸轴孔工件装配的螺旋插装方法。本发明在现有主动柔顺装配方法的基础上，使得机械臂在大尺寸轴孔工件的装配过程中更容易避免机械卡阻的发生，操作方法简单易行，能有效地提高装配效率和装配成功率。

本发明提出一种大尺寸轴孔工件装配的螺旋插装方法，其特征在于，包括以下步骤：

1)搭建大尺寸轴孔装配系统；所述系统包括：基座、待装配轴工件、待装配孔工件、力传感器、末端执行器、机械臂和上位机；所述待装配孔工件和基座分别固定于操作台上，机械臂通过螺纹连接固定在基座上，力传感器通过螺纹连接固定于机械臂的最末关节，力传感器通过电缆连接机械臂，末端执行器通过螺纹连接固定于力传感器上，待装配轴工件通过螺纹连接固定于末端执行器上，上位机通过网线连接机械臂；

2)操作机械臂完成轴孔粗对准，使得待装配轴工件的轴位于待装配孔工件的孔的上方且轴与孔的同轴度不超过待装配孔工件孔倒角宽度；

在待装配轴工件上建立轴心坐标系{O_p},{O_p}以轴的末端圆心为原点，沿轴线朝外方向为Z轴正方向；设机械臂末端中心坐标系为{O_t}，机械臂与待装配轴连接固定后轴心坐标系原点在坐标系{O_t}下的坐标为(x_p,y_p,z_p)，轴心坐标系的X轴、Y轴和Z轴在{O_t}下的单位方向向量分别为(n_x,n_y,n_z)、(a_x,a_y,a_z)和(o_x,o_y,o_z)，得到工具坐标系T_peg，表达式如下：

3)机械臂操纵待装配轴工件的轴完成柔顺入孔阶段，使得待装配孔工件的孔的插入深度超过孔的倒角深度；具体步骤如下：

3-1)设定初始的阻抗控制参数，包括插入速度v_a、参考力和参考力矩组成的向量控制刚度K_p＝[K_x,K_y,K_z,K_θx,K_θy,0]和调姿刚度K_d＝[K_x,K_y,0,K_θx,K_θy,0]，其中和分别是沿X、Y、Z方向的参考接触力，和分别是绕X、Y轴的参考接触力矩，K_x、K_y和K_z分别是X、Y、Z方向的位置刚度，K_θx和K_θy分别是绕X方向和Y方向的姿态刚度；

3-2)上位机获取机械臂在工具坐标系T_peg下的当前位姿T₀和力传感器测量的当前的接触力/力矩F，并计算下一时刻机械臂的目标位姿T_d＝T₀+K_p(F-F_ref)，上位机发送运动指令使机械臂运动到目标位姿T_d；其中位姿T₀＝[X₀,Y₀,Z₀,θ_x0,θ_y0,θ_z0]，X₀、Y₀和Z₀分别是机械臂在工具坐标系下X、Y、Z轴的位置坐标，θ_x0、θ_y0和θ_z0分别是机械臂在工具坐标系下绕X、Y、Z轴的欧拉角，接触力/力矩F＝[F_x,F_y,F_z,M_x,M_y,M_z]，F_x、F_y和F_z分别是轴孔之间接触力沿X、Y、Z轴方向的分量，M_x、M_y和M_z分别是轴孔之间接触力矩绕X、Y、Z轴的分量；

3-3)重复步骤3-2)，直至待装配轴工件的轴插入待装配孔工件的孔的深度超过孔的倒角深度,进入步骤4)；

4)机械臂操纵待装配轴工件的轴执行往复螺旋式装配运动，具体步骤如下：

4-1)计算螺旋式装配运动参数的取值范围，所述参数包括：螺旋插装的往复运动频率f_screw、旋转角度幅值θ_screw和螺旋运动节距h，约束条件表达式如下：

其中，δ是轴孔配合的间隙，d是机械臂绕工具坐标系Z轴旋转时待装配轴工件的端面圆心到实际旋转轴线的距离，f_res为机械臂的共振频率，k_int为惯量匹配因子，k_mate为间隙配合因子，k_f为轴向摩擦力减小的比例；

第一次执行螺旋式装配运动时，在满足上述约束条件的范围内，选取螺旋式装配运动参数的初始值；

4-2)计算螺旋式装配运动过程中机械臂在工具坐标系下的位置总增量ΔX、ΔY、ΔZ和绕X、Y轴的欧拉角总增量Δθ_x、Δθ_y；

上位机获取并更新机械臂当前位姿T₀和力传感器测量的接触力/力矩F，依据阻抗控制算法，表达式如下：

4-3)计算螺旋插装运动指令的步数n_s和绕工具坐标系Z轴旋转的角度幅值θ_screw，表达式如下：

4-4)计算每一步螺旋运动指令的目标位姿T_i ^d，i＝1,2…n_s，表达式如下：

其中每一步的位姿增量且满足：

其中ΔY_i ^d和是第i步指令中机械臂在工具坐标系下分别沿X、Y、Z轴的位置增量，和分别是第i步指令中机械臂在工具坐标系下绕X、Y、Z轴的欧拉角增量；

4-5)上位机将4-4)中计算得到的n_s步螺旋运动指令发送给机械臂，使机械臂操纵工件轴进行往复螺旋式的装配运动；

5)机械臂进行完n_s步往复螺旋式的装配运动后，上位机获取并更新机械臂当前位姿T₀和力传感器测量的接触力/力矩F，并依据更新后的当前位姿下的轴向插装力F_z的大小判断轴孔装配当前的卡阻状态，具体方法如下：

5-1)当时，判断卡阻状态为安全状态，则令插入速度v_a增大10％，螺旋插装参数h增大10％，然后进入步骤6)；

5-2)当时，判断卡阻状态为危险状态，则令插入速度v_a减小20％，螺旋插装参数h增大20％，同时调整轴孔的相对姿态，计算目标位姿T_g＝T₀+K_d(F-F_r)，上位机发送运动指令使机械臂运动到目标位姿T_g，然后进入步骤6)；

5-3)当时，判断卡阻状态为已发生卡阻，首先，调整运动参数，即轴向插入速度v_a减小30％，螺旋插装参数h减小30％；然后，机械臂运动到目标位姿位姿其中为步骤4-4)中第n_s-1步螺旋式装配运动指令的目标位姿，使得工件轴反向螺旋后退至上一条运动位姿以缓解卡阻，运动完成后上位机获取接触力/力矩F并再次判断卡阻状态，如果仍满足|F_z|≥0.95|F_z ^r|，则机械臂运动到目标位置直到机械臂运动目标位置时消除已发生卡阻状态，上位机获取并更新机械臂的当前位姿T₀和力传感器测量的当前的接触力/力矩F；最后，调整轴孔的相对姿态,上位机获取并更新机械臂的当前位姿T₀和力传感器测量的当前的接触力/力矩F,上位机发送运动指令使机械臂运动到目标位姿T_g＝T₀+K_d(F-F_r)，然后进入步骤6)；

6)上位机获取并更新机械臂当前位姿T₀，判断当前轴向插入深度Z₀是否到达目标位置深度Z_goal：如果已达到目标位置深度，则装配任务结束；否则重新返回步骤4)。

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

本发明方法可控制机械臂实现大尺寸轴孔的螺旋插装，能够利用螺旋运动的轴向摩擦抑制作用，有效地减小轴向摩擦阻力，并利用力传感器测量的接触力/力矩数据，结合阻抗控制算法，针对装配过程中不同的卡阻状态，调整螺旋插装参数和轴孔的相对位姿，该螺旋插装方法使得机械臂在大尺寸轴孔工件的装配过程中更容易避免机械卡阻的发生。该方法使用机械臂完成大尺寸轴孔工件的自动化装配，操作方法简单易行，能有效地提高装配效率和装配成功率。

附图说明

图1是本发明方法中减小轴向摩擦力的原理图。

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

图3是本发明中大尺寸轴孔装配系统结构示意图。

图4是本发明中大尺寸轴孔装配系统的工具坐标系示意图。

图5是本发明方法中工件轴往复式螺旋运动的示意图。

图中，M为机械臂，G为末端执行器，S为力传感器，P为待装配轴工件，H为待装配孔工件，W为基座，C为上位机。

具体实施方式

本发明提出提出一种大尺寸轴孔工件装配的螺旋插装方法，下面结合附图和具体实施例对本发明进一步详细说明如下。以下实施例用于说明本发明，但不限于本发明的范围。

本发明提出一种大尺寸轴孔工件装配的螺旋插装方法，该方法通过构造轴孔之间的相对螺旋运动以减小轴向摩擦阻力，图1给出了本发明方法中减小装配过程中轴向摩擦力的基本原理：螺旋插装过程中，工件轴以轴向线速度v_a和绕轴线的角速度w与工件孔接触于点C，螺旋运动改变了轴孔之间的摩擦阻力，即在接触压力N和总摩擦阻力f_C不变的同时，引入了绕轴向的切向摩擦力f_t，对轴孔之间的摩擦阻力产生了重新分配的效果，使得轴向摩擦力切向摩擦力其中r是待装配轴工件的轴半径，螺旋运动的节距通过减小螺旋运动的节距h，可以减小轴向摩擦力，进而减小机械卡阻的发生概率。轴向摩擦抑制的效果对于更大的工件轴尺寸更明显，适用于大尺寸轴孔的装配应用。

在本发明方法的具体实现过程中，机械臂接收上位机的运动指令，操纵大尺寸孔工件轴以往复式螺旋运动的形式插入大尺寸工件孔中，过程中力传感器实时监测轴孔之间的接触力/力矩，上位机利用装配过程中的力/位信息(机械臂末端的位姿和轴孔之间的接触力/力矩)，实现调整轴孔之间相对位姿的功能，同时用于监测轴孔装配的卡阻状态，调整螺旋插装的运动参数，以减小大尺寸轴孔工件之间的摩擦阻力，避免机械卡阻的发生。

本发明提出一种大尺寸轴孔工件装配的螺旋插装方法，整体流程如图2所示，包括以下步骤：

1)搭建大尺寸轴孔装配系统；所述系统结构如图3所示，包括：基座W、待装配轴工件P、待装配孔工件H、力传感器S、末端执行器G、机械臂M以及上位机C；所述待装配孔工件H和基座W分别固定于操作台上，机械臂M通过螺纹连接固定于基座W上，力传感器S通过螺纹连接固定于机械臂M的最末关节，并通过电缆连接机械臂M，末端执行器G通过螺纹连接固定于力传感器S上，待装配轴工件P通过螺纹连接固定于末端执行器G上，上位机C通过网线连接机械臂M；本系统各部件均可采用常规型号，其中所述机械臂M采用多关节串联式六自由度机器人，力传感器S采用六维力传感器，末端执行器G的结构和连接方式可根据待装配工件的结构自行设计，上位机C选用工控机或商用笔记本，所述待装配轴孔工件的配合孔径大于80mm。

2)手动操作机械臂完成轴孔粗对准，使得待装配轴工件的轴位于待装配孔工件的孔的上方，并保证轴与孔的同轴度不超过待装配孔工件孔倒角宽度；如图4所示，建立轴孔装配系统的工具坐标系：在待装配轴工件上建立轴心坐标系{O_p},{O_p}以轴的末端圆心为原点，沿轴线朝外方向为Z轴正方向，X轴方向可任意指定；设机械臂末端中心坐标系为{O_t}，由待装配轴工件的设计尺寸和配合尺寸得到机械臂与待装配轴连接固定后轴心坐标系原点在坐标系{O_t}下的坐标(x_p,y_p,z_p)和轴心坐标系的X轴、Y轴和Z轴在{O_t}下的单位方向向量分别为(n_x,n_y,n_z)、(a_x,a_y,a_z)和(o_x,o_y,o_z)，最终可以得到用于装配操作的工具坐标系T_peg，表达式如下：

3)机械臂操纵待装配轴工件的轴完成柔顺入孔阶段，使得待装配孔工件的孔的插入深度(相对于孔端面)超过孔的倒角深度；具体步骤如下：

3-1)设定初始的阻抗控制参数，包括插入速度v_a、参考力和参考力矩组成的向量控制刚度K_p＝[K_x,K_y,K_z,K_θx,K_θy,0]和调姿刚度K_d＝[K_x,K_y,0,K_θx,K_θy,0]，其中和分别是沿X、Y、Z方向的参考接触力，和分别是绕X、Y轴的参考接触力矩，K_x、K_y和K_z分别是X、Y、Z方向的位置刚度，K_θx和K_θy分别是绕X方向和Y方向的姿态刚度；一般情况下，参考接触力参考接触力矩 均设为0，参考接触力可为20N～100N(正负号由实际接触力在力传感器测得的数值的符号决定，通常为负号)，以上参考力/力矩参数不限于给定的范围，可根据轴孔装配过程中的实际测得的接触力/力矩大小进行适当调整；位置刚度K_x和K_y取值在0～0.01范围内，K_z取值在0～0.05范围内并大于或等于K_x和K_y的2倍，姿态刚度K_θx和K_θy取值在0～0.005范围内，以上刚度参数可根据轴孔装配工件的材料特性做适当调整；

3-2)上位机获取机械臂在工具坐标系T_peg下的当前位姿T₀和力传感器测量的当前的接触力/力矩F，并计算下一时刻机械臂的目标位姿T_d＝T₀+K_p(F-F_ref)，上位机发送运动指令使机械臂运动到目标位姿T_d。其中位姿T₀＝[X₀,Y₀,Z₀,θ_x0,θ_y0,θ_z0]，X₀、Y₀和Z₀分别是机械臂在工具坐标系下X、Y、Z轴的位置坐标，θ_x0、θ_y0和θ_z0分别是机械臂在工具坐标系下绕X、Y、Z轴的欧拉角，接触力/力矩F＝[F_x,F_y,F_z,M_x,M_y,M_z]，F_x、F_y和F_z分别是轴孔之间接触力沿X、Y、Z轴方向的分量，M_x、M_y和M_z分别是轴孔之间接触力矩绕X、Y、Z轴的分量；

3-3)重复步骤3-2)，直至待装配轴工件的轴插入待装配孔工件的孔的深度超过孔的倒角深度,进入步骤4)；

这里需要补充说明的是，所述“上位机获取机械臂在工具坐标系T_peg下的当前位姿T₀和力传感器测量的当前的接触力/力矩F”，“上位机发送运动指令使机械臂运动到目标位姿”，都是基于机械臂提供的上位机通讯接口实现的，是大多数商用机械臂自带的功能。

4)机械臂操纵待装配轴工件的轴执行如图5所示的往复螺旋式装配运动，即待装配轴工件的轴以轴向线速度v_a和绕轴线的角速度w(旋转方向周期性变换)向下插入待装配孔工件的孔中，具体步骤如下：

4-1)计算螺旋式装配运动参数的取值范围，参数包括螺旋插装的往复运动频率f_screw、旋转角度幅值θ_screw和螺旋运动节距h(轴孔接触点处的轴向线速度和绕轴旋转的角速度的比值)，约束条件包括配合精度、惯量匹配和轴向摩擦抑制等轴孔装配工艺需求，表达式如下：

其中，δ是轴孔配合的间隙，d是机械臂绕工具坐标系Z轴旋转时待装配轴工件的端面圆心到实际旋转轴线的距离，f_res为机械臂的共振频率，k_int为惯量匹配因子，k_mate为间隙配合因子，k_f为轴向摩擦力减小的比例，一般取k_int≤0.3，k_mate≤0.3，k_f≤1；

第一次执行螺旋式装配运动时，在满足上述约束条件的范围内，选取螺旋式装配运动参数的初始值，其中取较大的节距h；

4-2)计算螺旋式装配运动过程中机械臂在工具坐标系下的位置总增量ΔX、ΔY、ΔZ和绕X、Y轴的欧拉角总增量Δθ_x、Δθ_y。上位机获取并更新机械臂当前位姿T₀和力传感器测量的接触力/力矩F，依据阻抗控制算法，表达式如下：

4-3)计算螺旋插装运动指令的步数n_s和绕工具坐标系Z轴旋转的角度幅值θ_screw，表达式如下：

其中n_s的计算首先由步骤4-1)中给出的f_screw的取值范围计算得到n_s的取值范围，然后选取较大的整数作为n_s的取值；

4-4)计算每一步螺旋运动指令的目标位姿T_i ^d，i＝1,2…n_s，表达式如下：

其中每一步的位姿增量且满足：

其中△Y_i ^d和是第i步指令中机械臂在工具坐标系下分别沿X、Y、Z轴的位置增量，和分别是第i步指令中机械臂在工具坐标系下绕X、Y、Z轴的欧拉角增量；

4-5)上位机将4-4)中计算得到的n_s步螺旋运动指令发送给机械臂，使机械臂操纵工件轴进行往复螺旋式的装配运动；

5)机械臂进行完n_s步往复螺旋式的装配运动后，上位机获取并更新机械臂当前位姿T₀和力传感器测量的接触力/力矩F，并依据更新后的当前位姿下的轴向插装力F_z的大小判断轴孔装配当前的卡阻状态，具体方法是：

当|F_z|≤0.8|F_z ^r|时，判断卡阻状态为安全状态，则令插入速度v_a增大10％，螺旋插装参数h增大10％，然后进入步骤6)；

当0.8|F_z ^r|＜|F_z|＜0.95|F_z ^r|时，判断卡阻状态为危险状态，则令插入速度v_a减小20％，螺旋插装参数h增大20％，以增强轴向摩擦抑制效果，同时调整轴孔的相对姿态，计算目标位姿T_g＝T₀+K_d(F-F_r)，上位机发送运动指令使机械臂运动到目标位姿T_g，然后进入步骤6)；

当|F_z|≥0.95|F_z ^r|时，判断卡阻状态为已发生卡阻，首先，调整运动参数，即轴向插入速度v_a减小30％，螺旋插装参数h减小30％；然后，机械臂运动到目标位姿位姿其中为步骤4-4)中第n_s-1步螺旋式装配运动指令的目标位姿，使得工件轴反向螺旋后退至上一条运动位姿以缓解卡阻，运动完成后上位机获取接触力/力矩F并再次判断卡阻状态，如果仍满足|F_z|≥0.95|F_z ^r|，则机械臂运动到目标位置依次类推，直至脱离“已发生卡阻”状态；最后，调整轴孔的相对姿态,上位机获取并更新机械臂的当前位姿T₀和力传感器测量的当前的接触力/力矩F,上位机发送运动指令使机械臂运动到目标位姿T_g＝T₀+K_d(F-F_r)，然后进入步骤6)；

6)上位机获取并更新机械臂当前位姿T₀，判断当前轴向插入深度Z₀是否到达目标位置深度Z_goal：如果已达到目标位置深度，则装配任务结束；否则重新返回步骤4)。

下面结合一个具体实施例，对本发明进一步详细说明如下：

在本发明的实施例中，机械臂M接收上位机控制系统C的运动指令，操纵大尺寸孔工件轴P以往复螺旋运动的形式插入大尺寸工件孔H中，插入深度为60mm。本实施例提出一种大尺寸轴孔工件装配的螺旋插装方法，具体步骤如下：

1)搭建大尺寸轴孔装配系统；所述系统结构如图3所示，包括：基座W、待装配轴工件P、待装配孔工件H、力传感器S、末端执行器G、机械臂M以及上位机C；所述待装配孔工件H和基座W分别固定于操作台上，机械臂M通过螺纹连接固定于基座W上，力传感器S通过螺纹连接固定于机械臂M的最末关节，并通过电缆连接机械臂M，末端执行器G通过螺纹连接固定于力传感器上，待装配轴工件P通过螺纹连接固定于末端执行器G夹最末关节上，上位机C通过网线连接机械臂M。

本实施例中待装配轴工件尺寸为520mm×300mm×100mm，轴长度为150mm，直径为100mm，待装配孔工件外径为120mm,内径为100mm，轴孔配合间隙δ为0.1mm，孔倒角5mm，机械臂绕工具坐标系Z轴旋转时待装配轴工件的端面圆心到实际旋转轴线的距离d为0.2mm，机械臂的共振频率f_r为30Hz。ABB6自由度商业机器人IRB7600作为完成装配操作的机械臂M，联想ThinkPad T440P笔记本作为上位机控制系统；

2)手动操作机械臂完成轴孔粗对准，使得待装配轴工件的轴位于待装配孔工件的孔的上方，并保证轴与孔的同轴度不超过5mm；建立轴孔装配系统的工具坐标系：在待装配轴工件上建立轴心坐标系{O_p},{O_p}以轴的末端圆心为原点，沿轴线朝外方向为Z轴正方向，X轴方向可任意指定；设机械臂末端中心坐标系为{O_t}，由待装配轴工件的设计尺寸和配合尺寸得到机械臂与待装配轴连接固定后轴心坐标系原点在坐标系{O_t}下的坐标(0,0,300)和轴心坐标系的X轴、Y轴和Z轴在{O_t}下的方向向量分别为(1,0,0)、(0,1,0)和(0,0,1)，最终可以得到用于装配操作的工具坐标系T_peg：

在机械臂控制器中设置装配工具坐标系，参数为T_peg；

3)机械臂操纵待装配轴工件的轴完成柔顺入孔阶段，使得待装配孔工件的孔的插入深度(相对于孔端面)超过孔的倒角深度；具体步骤如下：

3-1)设定初始的阻抗控制参数，包括插入速度v_a＝5mm/s、参考力和参考力矩组成的向量F_ref＝[0,0,-50N,0,0,0]、控制刚度K_p＝[0.005,0.005,0.01,0.001,0.001,0]和调姿刚度K_d＝[0.005,0.005,0,0.001,0.001,0]，其中和分别是沿X、Y、Z方向的参考接触力，和分别是绕X、Y轴的参考接触力矩，K_x、K_y和K_z分别是X、Y、Z方向的位置刚度，K_θx和K_θy分别是绕X方向和Y方向的姿态刚度；

3-2)上位机获取机械臂在工具坐标系T_peg下的当前位姿T₀和力传感器测量的当前的接触力/力矩F，并计算下一时刻机械臂的目标位姿T_d＝T₀+K_p(F-F_ref)，上位机发送运动指令使机械臂运动到目标位姿T_d。其中位姿T₀＝[X₀,Y₀,Z₀,θ_x0,θ_y0,θ_z0]，X₀、Y₀和Z₀分别是机械臂在工具坐标系下X、Y、Z轴的位置坐标，θ_x0、θ_y0和θ_z0分别是机械臂在工具坐标系下绕X、Y、Z轴的欧拉角，接触力/力矩F＝[F_x,F_y,F_z,M_x,M_y,M_z]，F_x、F_y和F_z分别是轴孔之间接触力沿X、Y、Z轴方向的分量，M_x、M_y和M_z分别是轴孔之间接触力矩绕X、Y、Z轴的分量；

3-3)重复步骤3-2)，直至待装配轴工件的轴插入待装配孔工件的孔的深度超过5mm,进入步骤4)；

4)机械臂操纵待装配轴工件的轴执行如图5所示的往复螺旋式装配运动，即待装配轴工件的轴以轴向线速度v_a和绕轴线的角速度w(旋转方向周期性变换)向下插入待装配孔工件的孔中，具体步骤如下：

4-1)计算螺旋式装配运动参数的取值范围，参数包括螺旋插装的往复运动频率f_screw、旋转角度幅值θ_screw和螺旋运动节距h(轴孔接触点处的轴向线速度和绕轴旋转的角速度的比值)，约束条件包括配合精度、惯量匹配和轴向摩擦抑制等轴孔装配工艺需求，取惯量匹配因子k_int＝0.1，间隙配合因子k_mate＝0.2，轴向摩擦力减小的比例k_f＝0.8；计算得到取值范围如下：

第一次执行螺旋插装操作时，在满足上述约束条件的范围内，螺旋插装参数节距h取较大的初始值66.6mm；

4-2)计算螺旋式装配运动过程中机械臂在工具坐标系下的位置总增量ΔX、ΔY、ΔZ和绕X、Y轴的欧拉角总增量Δθ_x、Δθ_y。上位机获取并更新机械臂当前位姿T₀和力传感器测量的接触力/力矩F，依据阻抗控制算法，表达式如下：

4-3)计算螺旋插装运动指令的步数n_s和绕工具坐标系Z轴旋转的角度幅值θ_screw，表达式如下：

其中n_s的计算首先由步骤4-1)中给出的f_screw的取值范围计算得到n_s的取值范围，然后选取较大的整数作为n_s的取值；

4-4)计算每一步螺旋运动指令的目标位姿T_i ^d，i＝1,2…n_s，表达式如下：当前位姿下，表达式如下：

其中每一步的位姿增量且满足：

其中ΔY_i ^d和是第i步指令中机械臂在工具坐标系下分别沿X、Y、Z轴的位置增量，和分别是第i步指令中机械臂在工具坐标系下绕X、Y、Z轴的欧拉角增量；

4-5)上位机将4-4)中计算得到的n_s步螺旋运动指令发送给机械臂，使机械臂操纵工件轴进行往复螺旋式的装配运动；

5)机械臂进行完n_s步运动后，上位机获取并更新机械臂当前位姿T₀和力传感器测量的接触力/力矩F，并依据更新后的当前位姿下的轴向插装力F_z的大小判断轴孔装配当前的卡阻状态，具体方法是：

当|F_z|≤40N时，判断卡阻状态为安全状态，则令插入速度v_a增大10％，螺旋插装参数h增大10％，然后进入步骤6)；

当40N＜|F_z|＜47.5N时，判断卡阻状态为危险状态，则令插入速度v_a减小20％，螺旋插装参数h增大20％，以增强轴向摩擦抑制效果，同时调整轴孔的相对姿态，计算目标位姿T_g＝T₀+K_d(F-F_r)，上位机发送运动指令使机械臂运动到目标位姿T_g，然后进入步骤6)；

当|F_z|≥47.5N时，判断卡阻状态为已发生卡阻，首先，调整运动参数，即轴向插入速度v_a减小30％，螺旋插装参数h减小30％；然后，机械臂运动到目标位姿位姿其中为步骤4-4)中第n_s-1步螺旋式装配运动指令的目标位姿，使得工件轴反向螺旋后退至上一条运动位姿以缓解卡阻，运动完成后上位机获取接触力/力矩F并再次判断卡阻状态，如果仍满足|F_z|≥47.5N，则机械臂运动到目标位置依次类推，直至脱离“已发生卡阻”状态；；最后，调整轴孔的相对姿态,上位机获取并更新机械臂的当前位姿T₀和力传感器测量的当前的接触力/力矩F,上位机发送运动指令使机械臂运动到目标位姿T_g＝T₀+K_d(F-F_r)，然后进入步骤6)；

6)上位机获取并更新机械臂当前位姿T₀，判断当前轴向插入深度Z₀是否到达目标位置深度60mm：如果已达到目标位置深度，则装配任务结束；否则重复返回步骤4)。

Claims (1)

1.一种大尺寸轴孔工件装配的螺旋插装方法，其特征在于，包括以下步骤：

1)搭建大尺寸轴孔装配系统；所述系统包括：基座、待装配轴工件、待装配孔工件、力传感器、末端执行器、机械臂和上位机；所述待装配孔工件和基座分别固定于操作台上，机械臂通过螺纹连接固定在基座上，力传感器通过螺纹连接固定于机械臂的最末关节，力传感器通过电缆连接机械臂，末端执行器通过螺纹连接固定于力传感器上，待装配轴工件通过螺纹连接固定于末端执行器上，上位机通过网线连接机械臂；

2)操作机械臂完成轴孔粗对准，使得待装配轴工件的轴位于待装配孔工件的孔的上方且轴与孔的同轴度不超过待装配孔工件孔倒角宽度；

在待装配轴工件上建立轴心坐标系{O_p},{O_p}以轴的末端圆心为原点，沿轴线朝外方向为Z轴正方向；设机械臂末端中心坐标系为{O_t}，机械臂与待装配轴连接固定后轴心坐标系原点在坐标系{O_t}下的坐标为(x_p,y_p,z_p)，轴心坐标系的X轴、Y轴和Z轴在{O_t}下的单位方向向量分别为(n_x,n_y,n_z)、(a_x,a_y,a_z)和(o_x,o_y,o_z)，得到工具坐标系T_peg，表达式如下：

3)机械臂操纵待装配轴工件的轴完成柔顺入孔阶段，使得待装配孔工件的孔的插入深度超过孔的倒角深度；具体步骤如下：

3-1)设定初始的阻抗控制参数，包括插入速度v_a、参考力和参考力矩组成的向量控制刚度K_p＝[K_x,K_y,K_z,K_θx,K_θy,0]和调姿刚度K_d＝[K_x,K_y,0,K_θx,K_θy,0]，其中和分别是沿X、Y、Z方向的参考接触力，和分别是绕X、Y轴的参考接触力矩，K_x、K_y和K_z分别是X、Y、Z方向的位置刚度，K_θx和K_θy分别是绕X方向和Y方向的姿态刚度；

3-2)上位机获取机械臂在工具坐标系T_peg下的当前位姿T₀和力传感器测量的当前的接触力/力矩F，并计算下一时刻机械臂的目标位姿T_d＝T₀+K_p(F-F_ref)，上位机发送运动指令使机械臂运动到目标位姿T_d；其中位姿T₀＝[X₀,Y₀,Z₀,θ_x0,θ_y0,θ_z0]，X₀、Y₀和Z₀分别是机械臂在工具坐标系下X、Y、Z轴的位置坐标，θ_x0、θ_y0和θ_z0分别是机械臂在工具坐标系下绕X、Y、Z轴的欧拉角，接触力/力矩F＝[F_x,F_y,F_z,M_x,M_y,M_z]，F_x、F_y和F_z分别是轴孔之间接触力沿X、Y、Z轴方向的分量，M_x、M_y和M_z分别是轴孔之间接触力矩绕X、Y、Z轴的分量；

3-3)重复步骤3-2)，直至待装配轴工件的轴插入待装配孔工件的孔的深度超过孔的倒角深度,进入步骤4)；

4)机械臂操纵待装配轴工件的轴执行往复螺旋式装配运动，具体步骤如下：

4-1)计算螺旋式装配运动参数的取值范围，所述参数包括：螺旋插装的往复运动频率f_screw、旋转角度幅值θ_screw和螺旋运动节距h，约束条件表达式如下：

其中，δ是轴孔配合的间隙，d是机械臂绕工具坐标系Z轴旋转时待装配轴工件的端面圆心到实际旋转轴线的距离，f_res为机械臂的共振频率，k_int为惯量匹配因子，k_mate为间隙配合因子，k_f为轴向摩擦力减小的比例；

第一次执行螺旋式装配运动时，在满足上述约束条件的范围内，选取螺旋式装配运动参数的初始值；

4-2)计算螺旋式装配运动过程中机械臂在工具坐标系下的位置总增量ΔX、ΔY、ΔZ和绕X、Y轴的欧拉角总增量Δθ_x、Δθ_y；

上位机获取并更新机械臂当前位姿T₀和力传感器测量的接触力/力矩F，依据阻抗控制算法，表达式如下：

4-3)计算螺旋插装运动指令的步数n_s和绕工具坐标系Z轴旋转的角度幅值θ_screw，表达式如下：

4-4)计算每一步螺旋运动指令的目标位姿T_i ^d，i＝1,2…n_s，表达式如下：

其中每一步的位姿增量且满足：

其中ΔY_i ^d和是第i步指令中机械臂在工具坐标系下分别沿X、Y、Z轴的位置增量，和分别是第i步指令中机械臂在工具坐标系下绕X、Y、Z轴的欧拉角增量；

4-5)上位机将4-4)中计算得到的n_s步螺旋运动指令发送给机械臂，使机械臂操纵工件轴进行往复螺旋式的装配运动；

5)机械臂进行完n_s步往复螺旋式的装配运动后，上位机获取并更新机械臂当前位姿T₀和力传感器测量的接触力/力矩F，并依据更新后的当前位姿下的轴向插装力F_z的大小判断轴孔装配当前的卡阻状态，具体方法如下：

5-1)当时，判断卡阻状态为安全状态，则令插入速度v_a增大10％，螺旋插装参数h增大10％，然后进入步骤6)；

5-2)当时，判断卡阻状态为危险状态，则令插入速度v_a减小20％，螺旋插装参数h增大20％，同时调整轴孔的相对姿态，计算目标位姿T_g＝T₀+K_d(F-F_r)，上位机发送运动指令使机械臂运动到目标位姿T_g，然后进入步骤6)；

5-3)当时，判断卡阻状态为已发生卡阻，首先，调整运动参数，即轴向插入速度v_a减小30％，螺旋插装参数h减小30％；然后，机械臂运动到目标位姿位姿其中为步骤4-4)中第n_s-1步螺旋式装配运动指令的目标位姿，使得工件轴反向螺旋后退至上一条运动位姿以缓解卡阻，运动完成后上位机获取接触力/力矩F并再次判断卡阻状态，如果仍满足则机械臂运动到目标位置直到机械臂运动目标位置时消除已发生卡阻状态，上位机获取并更新机械臂的当前位姿T₀和力传感器测量的当前的接触力/力矩F；最后，调整轴孔的相对姿态,上位机获取并更新机械臂的当前位姿T₀和力传感器测量的当前的接触力/力矩F,上位机发送运动指令使机械臂运动到目标位姿T_g＝T₀+K_d(F-F_r)，然后进入步骤6)；

6)上位机获取并更新机械臂当前位姿T₀，判断当前轴向插入深度Z₀是否到达目标位置深度Z_goal：如果已达到目标位置深度，则装配任务结束；否则重新返回步骤4)。