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

权利要求书

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）中的方法对传感器的零位值重新标定。