[发明专利]一种遥操作机器人的机械手末端三维接触力测量方法

权利要求书

1.一种遥操作机器人的机械手末端三维接触力测量方法，所述机械手包括机械手臂与机械手爪的联接总体，机械手爪设置在机械手臂的前端，三维力传感器设置在机械手爪与机械手臂之间，其特征在于：设置两个三维力传感器，第一三维力传感器设置在机械手臂前臂与机械手爪之间；第二三维力传感器设置在机械手臂后臂与机械手臂前臂之间；定义机械手爪的质量为m，机械手爪、机械手臂前臂以及第一三维力传感器的总质量为M；

开启第一、第二两个三维力传感器，当机械手爪和机械手臂前臂进行动作并做非匀速运动时，两个三维力传感器同时测力，第一三维力传感器的测得的力为：F_x1，F_y1，F_z1，包括机械手爪与周围环境的接触力f_x，f_y，f_z和机械手爪非匀速运动产生的惯性力f_xm，f_ym，f_zm；第二三维力传感器的测得的力为：F_x2，F_y2，F_z2，包括机械手爪与周围环境的接触力f_x，f_y，f_z和机械手爪、机械臂前臂、第一三维力传感器非匀速运动产生的惯性力f_xM，f_yM，f_zM；

则两个传感器所测得的力分别为：

第一三维力传感器Fx1=fx+fxmFy1=fy+fymFz1=fz+fzm---(1)]]>

第二三维力传感器Fx2=fx+fxMFy2=fy+fyMFz2=fz+fzM---(2);]]>

设机械手爪与机械手臂前臂三个方向的线加速度分别为a_x，a_y，a_z，则机械手爪由于非匀速运动产生的惯性力f_xm，f_ym，f_zm与机械手爪、机械臂前臂、第一三维力传感器非匀速运动产生的惯性力f_xM，f_yM，f_zM表示为：

fxm=-m×axfym=-m×ayfzm=-m×az---(3)]]>

fxM=-M×axfyM=-M×ayfzM=-M×az---(4)]]>

将式(3)、式(4)分别代入式(1)、式(2)得：

Fx1=fx-m×axFy1=fy-m×ayFz1=fz-m×az---(5)]]>

Fx2=fx-M×axFy2=fy-M×ayFz2=fz-M×az---(6)]]>

将第一三维力传感器的测得力F_x1，F_y1，F_z1和第二三维力传感器的测得力F_x2，F_y2，F_z2分别带入式(5)、式(6)得

fx=MFx1-mFx2M-mfy=MFy1-mFy2M-mfz=MFz1-mFz2M-m---(7)]]>

通过(7)式直接求解出遥操作机械手爪与周围环境的三维接触力f_x，f_y，f_z，从三维力传感器的输出信号中除去由于遥操作机械臂及机械手爪非匀速运动而产生的惯性力成分，消除了惯性力的干扰，再由控制模块将求解出的f_x，f_y，f_z通过PID控制算法，对遥操作机械手爪和遥操作机械臂进行反馈控制。