[发明专利]结合力控的搜索装配方法

权利要求书

1.一种结合力控的搜索装配方法，其特征在于，包括如下步骤：

步骤S1，搭建柔性装配平台，所述柔性装配平台包括：协作机器人本体、协作机器人控制器、上位机、末端执行器、第一工件、第二工件，其中，由协作机器人本体的关节内部的力矩传感器采集关节力矩信息；所述上位机与所述协作机器人控制器连接，以采集协作机器人的状态信息，并发送协作机器人状态控制指令至所述协作机器人控制器，以由所述协作机器人控制器对所述协作机器人本体进行控制；

步骤S2，将夹持工件的末端执行器的质量和惯性矩阵补偿给协作机器人控制器；

步骤S3，所述协作机器人开始执行力控搜索装配，到达指定工位夹取待装配的第一工件，然后进行装配，包括以下阶段：

接近阶段：由所述协作机器人夹持第一工件到达待装配的第二工件上方的同轴心位置；

搜索阶段：将控制方式调整为力位混合控制，自动查找出第一工件的轴和第二工件的孔对齐的位置；

在所述搜索阶段，将控制方式调整为笛卡尔空间的力位混合阻抗控制方式，分别计算位置控制回路和力控制回路，位置控制采用逆运动学计算力矩控制方式，得到的力控回路的控制力矩τ_输出1与位置环控制输出力矩τ_输出2叠加输出，输出总力矩为：τ_输出＝τ_输出1+τ_输出2；

计算力控回路的控制力矩τ_输出1，包括：首先标定搜索运动过程中力矩传感器的零偏：

τ_零偏＝τ_初测-τ_初重，

其中，τ_初测为进入搜索阶段时测得的关节力矩，τ_初重为进入搜索阶段时根据关节角度计算出的重力矩；

然后通过实时通信接口实时采集各关节力矩传感器的值，进而实时计算出力矩差:

τ_偏差＝τ_测-τ_重-τ_零偏；

其中，τ_测搜索运动过程中力矩传感器的测量力矩值；τ_重搜索运动过程中机器人重力矩；τ_零偏搜索运动过程中力矩传感器的零偏；

设定笛卡尔空间的期望力矩为F_d＝[0,0,F_z,0,0,0],即设定Z方向的力恒定保持F_z，根据期望力矩和雅克比矩阵J，计算出期望关节力矩:

τ_d＝J^TF_d

计算出的期望关节力矩再通过PI调节，最后输出一个关节力矩至所述协作机器人控制器，PI调节方程为：

k_p为比例调节系数,k_i为积分系数，通过PI调节，使得最后Z方向的力处于一个恒定状态；

计算环控制输出力矩τ_输出2，包括：

在XY平面上做圆弧搜索运动，给定初始最大运动半径为r_max,最大线速度为V_max,加减速时间为t_a,总的搜索时间即每次最大装配时间为t_max,这里的加减速采用梯形加减速的方法，装配过程中协作机器人运动速度为：

经过时间t，协作机器人走过的圆弧角度是：

其中，V是时间t时的线速度；

相对于搜索开始时协作机器人的初始位置，经过时间t，X方向的位移量和Y方向的位移量分别为：

Δx＝r_max(1-cos(θ)),Δy＝r_maxsin(θ)；

其中，θ是时间t时，圆弧运动相对于初始位置的角度；

除了在XY方向上有一个偏移量，还有一个绕Z轴旋转的运动，设搜索开始时的协作机器人姿态矩阵是：

经过时间t,协作机器人绕Z轴旋转的角度θ_zt＝kθ_t,系数根据实际情况调节，一般取0.01-0.05，旋转后的姿态矩阵为：

其中，姿态矩阵是3×3矩阵，其中n o a分别是姿态矩阵中的3列向量；

θ_zt＝kθ_t中θ_t是时间t时绕Z方向相对于初始位置的旋转角度；

经过时间t，协作机器人的位姿矩阵为:

其中，p_z的大小随着实际情况变化，第一工件和第二工件在Z方向上的压力保持F_z，

通过实时通信接口可以实时采集当前关节角度q、当前关节角速度d_q和当前末端位姿矩阵T_当前,根据目标位姿反解出目标关节角度q_d,其中目标位姿用一个六维向量Q[x,y,z,α,β,γ]来表示，将目标末端位姿矩阵表示成Q_目标，将当前位姿矩阵T_当前表示为Q_当前，据此位姿差Q_差＝Q_目标-Q_当前；其中，p_x,p_y分别是搜索运动开始前机器人在X方向和Y方向上的位置，Δ_x，Δ_y分别是搜索运动过程中机器人目标位置在x方向和y方向上相对于初始位置的增量；J是机器人的在对应关节角度的雅可比矩阵；

设置适当的刚度矩阵和阻尼矩阵，可以计算出此刻的期望关节力矩：

τ_输出2＝J^T(-B_d·Q_差-D_d·J·d_q)+τ_coriolis，

其中，τ_coriolis为科氏力，

B_d为刚度矩阵，将B_d设置为：diag(3000,3000,0,200,200,200)∈R^n×n，D_d为阻尼矩阵，将其设置为：diag(100,100,0,20,20,20)∈R^n×n，

当B_d＝diag(3000,3000,3000,200,200,200)时，认为此时的协作机器人是刚性的；

插入阶段：将第一工件的轴和第二工件的孔对齐后，采用Z方向力控模式，将第一工件的轴向下插入第二工件的孔中；

插入完成阶段：通过检测Z方向的位置判断是否装配完成，如果装配成功则所述协作机器人松开所述第一工件后退出，如果装配超时则判断本次装配失败。