[发明专利]基于六维力/力矩传感器的机器人力跟随运动控制方法

摘要

本发明公开了基于六维力/力矩传感器的机器人力跟随运动控制方法，在利用机器人作示教等目的时，这种方法能获得很好的效果。本发明主要涉及4个重要步骤，第一个是利用较为简单的方法对力/力矩传感器的零位值进行标定，第二个是利用特定方法对力/力矩传感器在不同姿态下，由于自身和安装在传感器上面工具的重力对力/力矩传感器零位值造成影响进行补偿，然后是设定一个力/力矩传感器在相应姿态下的稳定区间，最后一个是利用力/力矩传感器的力和力矩值对机器人进行力跟随运动控制。

主权项

1.基于六维力/力矩传感器的机器人力跟随运动控制方法，其特征在于包括下述步骤：（1）通过反馈力/力矩传感器当前姿态并利用以下公式对力/力矩传感器的力和力矩数值进行补偿：Fxc=r31kxzG+Fx0Fyc=r32kyzG+Fy0Fzc=r33G+Fz0Mxc=Mx0Myz=My0Mzc=Mz0]]>式中，r₃₁、r₃₂、r₃₃分别从力传感器当前姿态T=r11r12r13r21r22r23r31r32r33]]>中获得；（2）先以力传感器x轴朝上作为初始位置1，测得当前力传感器的数据为F_x1，F_y1，F_z1，M_x1，M_y1，M_z1；在初始位置1基础上绕z轴逆时针旋转90°得到位置2，测得当前力传感器的变量为F_x2，F_y2，F_z2，M_x2，M_y2，M_z2；在初始位置2基础上绕z轴逆时针旋转90°得到位置3，测得当前力传感器的变量为F_x3，F_y3，F_z3，M_x3，M_y3，M_z3；在初始位置3基础上绕z轴逆时针旋转90°得到位置4，测得当前力传感器的变量为F_x4，F_y4，F_z4，M_x4，M_y4，M_z4；在初始位置1基础上绕y轴逆时针旋转90°得到位置5，测得当前力传感器的变量为F_x5，F_y5，F_z5，M_x5，M_y5，M_z5；在初始位置1基础上绕y轴顺时针旋转90°得到位置6，测得当前力传感器的变量为F_x6，F_y6，F_z6，M_x6，M_y6，M_z6通过以下公式获得相应的初值：Fx0=(Fx1+Fx3)/2Fy0=(Fy2+Fy4)/2Fz0=(Fz5+Fz6)/2]]>Mx0=(Mx2+Mx4+Mx5+Mx6)/4My0=(My1+My3+My5+My6)/4Mz0=(Mz1+Mz2+Mz3+Mz4)/4]]>算出初值后，计算x轴、y轴和z轴分别测得的重力值：Gx=|Fx3-Fx1|/2Gy=|Fy2-Fy4|/2Gz=|Fz6-Fz5|/2]]>根据这个重力值计算出传感器各轴线性度之间的偏差关系kxz=Gx/Gzkyz=Gy/Gz]]>以G_z为重力基准，即G=G_z根据当前力/力矩传感器的姿态，利用步骤（1）中的公式算出力/力矩传感器当前的姿态的零位值F_xc、F_yc、F_zc、M_xc、M_yc、M_zc，通过以上公式，对力/力矩传感器的基本参数进行标定；（3）利用步骤（1）中的公式分别求出步骤（2）中所测得的6个位置的零位值，并与测量值进行比较，通过下面公式算出此时各个测量量的误差最大绝对值：ηf=maxi=16|Fni-Fnc|,(n=x,y,z)]]>ηm=maxi=16|Mni-Mnc|,(n=x,y,z)]]>式中F_ni、M_ni表示传感器测量回来的量，F_nc、M_nc表示相应通道计算出来的补偿量；各通道误差的最大绝对值分别记为η_fx、η_fy、η_fz、η_mx、η_my、η_mz取稳定系数ε_fx、ε_fy、ε_fz（均大于1，建议选1.5～2.0），令力/力矩传感器的当前零位稳定区间为：δfx=ηfxϵfxδfy=ηfyϵfyδfz=ηfzϵfzδmx=ηmxϵmxδmy=ηmyϵmyδmz=ηmzϵmz]]>则力/力矩传感器的力端零位稳定区间分别为力矩端的零位稳定区间分别为[M_xc-δ_mx,M_xc+δ_mx]、[M_zc-δ_mz,M_zc+δ_mz]通过上述方法获得对力/力矩传感器当前姿态下的力和力矩数值稳定区间进行设置；（4）根据步骤（1）、（2）和（3）设计得到力跟随的控制方法力端为：uf=kfc(Fm-Fc+δ)Fm<Fc-δ0Fc-δ<Fm<Fc+δkfc(Fm-Fc-δ)Fm>Fc+δ]]>式中F_m表示在某个力方向上施加力后的测量值，F_c表示在该方向上对力的补偿量，δ表示该方向上的稳定区间；同理，力矩端的控制方法为：um=kmc(Mm-Mc+δ)Mm<Mc-δ0Mc-δ<Mm<Mc+δkmc(Mm-Mc-δ)Mm>Mc+δ]]>式中M_m表示在某个力矩方向上施加力后的测量值，M_c表示在该方向上对力的补偿量，δ表示该方向上的稳定区间；控制值将反馈到机器人上进行力跟随控制，并通过以下公式进行：q·=J-1u]]>其中J为机器人当前的雅可比矩阵，u为力和力矩的控制输入，表示为u=ufum=ufxufyufzumxumyumz]]>为机器人各关节的速度，算出各关节的速度后，利用该速度对机器人各关节进行控制，使其作力跟随运动，每当机器人姿态发生改变，都需要使用步骤（2）中的方法对传感器的零位值重新标定。