CN113319855B - 一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法 - Google Patents

Abstract

一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法，属于诊疗机器人力控领域，为了解决现有的重力补偿方法对诊疗机器人重力补偿精度不足的问题。本发明实时记录机器人多个姿态下的姿态矩阵以及传感器的测量值，构造线性方程组，通过最小二乘法求解诊疗工具的参数向量；在诊疗工具处于初始位置时以及按照大地坐标系调整诊疗工具的姿态，使诊疗工具的重力只剩沿六维力与力矩传感器坐标的y轴方向时，分别记录传感器的测量值；在传感器坐标系下，利用降维解析法计算诊疗工具重心的位置；计算出诊疗工具的重力分量与力矩分量补偿值，实现对诊疗工具的重力补偿。有益效果为实现对多种诊疗工具在多姿态工作模式下的高精度重力补偿。

Description

一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法

技术领域

本发明属于诊疗机器人力控领域。

背景技术

诊疗机器人在重力环境下结合六维力与力矩传感器通过人手接触控制柔顺运动时，进入控制闭环的力为诊疗机器人末端与人手之间的接触力，换言之，要实现人手力对机器人的控制，首先就要精确检测出人手力的大小和方向；实际上，安装于诊疗机器人末端的六维力传感器测量到的力，不仅包含机器人末端与人手间的接触力，还包含安装于力传感器上诊疗工具的重力；但是传感器无法自主区分人手力和诊疗工具的重力，诊疗工具的重力属于干扰力，因此有必要进行精确的重力补偿；如果不进行重力补偿，系统的控制精度必定大大受其影响；其次，根据诊疗机器人的实际需求，机器人末端需要在超声阵列探头、诊疗光纤和治疗光纤这三种诊疗工具中来回切换，而这三种诊疗工具的质量特性各异，重心也随自身姿态的变化而变化；现有的重力补偿方法无法满足诊疗机器人多模式、多姿态和高精度的工作需求，每次对末端工具进行更换或者重新安装后，都需要对其进行精密测量来测定其质量特性，操作繁琐，不适用于诊疗机器人的工作环境。

发明内容

本发明的目的是为了解决现有的重力补偿方法对诊疗机器人重力补偿精度不足的问题，提出了一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法。

本发明所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法通过以下步骤实现的：

步骤一、将六维力与力矩传感器安装在多关节诊疗机器人末端，选取多关节诊疗机器人的多个姿态，并实时记录关节诊疗机器人多个姿态下对应的姿态矩阵以及记录关节诊疗机器人多个姿态下六维力与力矩传感器的测量值，构造线性方程组，通过最小二乘法求解诊疗工具的参数向量；

步骤二、在诊疗工具处于初始位置时，记录六维力与力矩传感器的测量值；

步骤三、按照大地坐标系调整诊疗工具的姿态，使诊疗工具的重力只剩沿六维力与力矩传感器坐标的y轴方向，并记录此时六维力与力矩传感器的测量值；

步骤四、根据步骤二记录的测量值与步骤三记录的测量值，在传感器坐标系下，利用降维解析法计算诊疗工具重心的位置；

步骤五、将步骤四计算出的诊疗工具重心的位置投影到六维力与力矩传感器坐标系上，并结合步骤一求解出的诊疗工具参数向量，计算出诊疗工具的重力分量与力矩分量补偿值，实现对诊疗工具的重力补偿。

本发明的有益效果是：该重力补偿方法能够实时提供重力补偿，来抵消多关节诊疗机器人末端诊疗工具自身重力的影响，使得补偿后的多关节诊疗机器人在任何末端位姿下不受外力作用时，多关节诊疗机器人在基坐标系下沿各方向的力和力矩都趋于零，因此该重力补偿方法重力补偿算法精度较高。

附图说明

图1为为具体实施方式所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法流程图；

图2为具体实施方式一中多关节诊疗机器人的实物图；

图3为具体实施方式二中基于坐标系变换的重力计算示意图；

图4为具体实施方式七中对计算出的诊疗工具重力分量与力矩分量补偿值进行验证的流程图。

具体实施方式

具体实施方式一：结合图1至图2说明本实施方式，本实施方式所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法通过以下步骤实现的：

步骤一、将六维力与力矩传感器安装在多关节诊疗机器人末端，选取多关节诊疗机器人的多个姿态，并实时记录关节诊疗机器人多个姿态下对应的姿态矩阵以及记录关节诊疗机器人多个姿态下六维力与力矩传感器的测量值，构造线性方程组，通过最小二乘法求解诊疗工具的参数向量；

步骤二、在诊疗工具处于初始位置时，记录六维力与力矩传感器的测量值；

步骤三、按照大地坐标系调整诊疗工具的姿态，使诊疗工具的重力只剩沿六维力与力矩传感器坐标的y轴方向，并记录此时六维力与力矩传感器的测量值；

步骤四、根据步骤二记录的测量值与步骤三记录的测量值，在传感器坐标系下，利用降维解析法计算诊疗工具重心的位置；

步骤五、将步骤四计算出的诊疗工具重心的位置投影到六维力与力矩传感器坐标系上，并结合步骤一求解出的诊疗工具参数向量，计算出诊疗工具的重力分量与力矩分量补偿值，实现对诊疗工具的重力补偿。

在本实施方式中，采用降维解析法通过记录几个特殊位置的参数，结合多关节诊疗机器人编程和多关节诊疗机器人运动学计算，在短时间内计算出诊疗工具的重力和重心相对于机器人末端中心的位置；该方法在降低计算量的同时提高了计算精度，解决了不同诊疗工具的重力大小及其在不同工况下重心位置计算的问题。

本实施方式中，重力补偿方法的基本思想是，根据多关节诊疗机器人末端诊疗工具的工具参数和机器人姿态矩阵，求解六维力与力矩传感器上的重力作用分量和力矩分量，对实际测量的力觉信息进行补偿，从而获得传感器坐标系下的实际接触力状态；因此，在补偿过程中应首先确定末端被安装工具重力的大小和重心位置；重力补偿主要由两个步骤实现：首先用最小二乘法定位诊疗机器人基坐标系、大地坐标系和工具坐标系的关系，求解诊疗工具的理论参数向量(G,O_GF)，其次用特殊位置法求出机器人末端诊疗工具的重力和重心位置。诊疗机器人如图2所示。

具体实施方式二：结合图3说明本实施方式，本实施方式是对具体实施方式一所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法进一步限定，在本实施方式中，步骤一中通过最小二乘法求解诊疗工具的参数向量的具体方法为：

设诊疗工具的重力为G，则重力向量g_G为(0,0,G)^T，则诊疗工具重力作用在六维力与力矩传感器上的三维力向量g_F和三维力矩向量g_M分别为(g_FX,g_FY,g_FZ)^T和(g_MX,g_MY,g_MZ)^T；

根据如图3所示的基于坐标系变换的工具重力计算方法，三维力向量g_F可表示为：

式中，

为力觉传感器坐标系O_F到重力坐标系O_G的姿态矩阵；多关节诊疗机器人基坐标系O_B到机器人末端关节坐标系O_E的姿态矩阵为

并且

由多关节诊疗机器人正运动学方程确定；

为力觉传感器参数姿态矩阵；O_B到O_G的姿态矩阵

用旋转角(α,β,γ)表示；

由于重力坐标系Z_G轴与重力方向平行，X_GO_GY_G平面可绕Z_G轴进行任意旋转，因此设γ＝0，旋转角为Q(α,β)，

可表示为：

求解出诊疗工具的参数向量为(G,Q,O_GF)。

具体实施方式三：本实施方式是对具体实施方式二所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法进一步限定，在本实施方式中，步骤二中六维力与力矩传感器的测量值分别为：f_FX0、f_FY0、f_FZ0、f_MX0、f_MY0、f_MZ0；

其中，f_FX0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿X轴方向力的分量；f_FY0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿Y方向力的分量；f_FZ0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿Z轴方向力的分量；f_MX0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿X轴方向力矩的分量；f_MY0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿Y方向力矩的分量；f_MZ0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿Z轴方向力矩的分量。

具体实施方式四：本实施方式是对具体实施方式三所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法进一步限定，在本实施方式中，步骤三中六维力与力矩传感器的测量值分别为：f_FX00、f_FY00、f_FZ00、f_MX00、f_MY00、f_MZ00；

其中，f_FX00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿X轴方向力的分量；f_FY00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿Y方向力的分量；f_FZ00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿Z轴方向力的分量；f_MX00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿X轴方向力矩的分量；f_MX00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿Y方向力矩的分量；f_MZ00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿Z轴方向力矩的分量。

具体实施方式五：本实施方式是对具体实施方式四所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法进一步限定，在本实施方式中，步骤四中利用降维解析法计算诊疗工具重心位置的具体方法为：

步骤四一、利用公式(3)计算出六维力与力矩传感器坐标系下的诊疗工具重心坐标O_GF；

所述公式(3)为：

其中，X_GF为诊疗工具重心坐标O_GF的X轴坐标，Y_GF为诊疗工具重心坐标O_GF的Y轴坐标，Z_GF为诊疗工具重心坐标O_GF的Z轴坐标；

步骤四二、求解出诊疗工具重心位置为：

具体实施方式六：本实施方式是对具体实施方式五所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法进一步限定，在本实施方式中，步骤五中计算出诊疗工具的重力分量与力矩分量补偿值的具体方法为：

由于多关节诊疗机器人的姿态决定了重力在六维力与力矩传感器上的分量，因此将旋转角Q换算成坐标系旋转矩阵，将重力投影到六维力与力矩传感器坐标系上就得到重力的三个分量和力矩的三个分量：

其中，g_FX重力在X轴上的分量，g_FY为重力在Y轴上的分量，g_FZ为重力在Z轴上的分量，g_MX为重力力矩在X轴上的分量，g_MY为重力力矩在Y轴上的分量，g_MZ为重力力矩在Z轴上的分量；

从而得出多关节诊疗机器人末端诊疗工具重力分量与力矩分量的补偿值：

其中，f_CF为诊疗工具的重力分量补偿值，f_CM为诊疗工具的重力力矩分量补偿值，f_FX为诊疗工具所受的力在X轴上的分量，f_FY为诊疗工具所受的力在Y轴上的分量，f_FZ为诊疗工具所受的力在Z轴上的分量，f_MX为诊疗工具所受的力矩在X轴上的分量，f_MY为诊疗工具所受的力矩在Y轴上的分量，f_MZ为诊疗工具所受的力矩在Z轴上的分量。

具体实施方式七：结合图4说明本实施方式，本实施方式是对具体实施方式六所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法进一步限定，在本实施方式中，该重力补偿方法还包括步骤六、对步骤五计算出的诊疗工具的重力分量与力矩分量补偿值进行验证；

对步骤五计算出的诊疗工具的重力分量与力矩分量补偿值进行验证的具体方法为：

步骤六一、对六维力与力矩传感器进行校零；

步骤六二、安装诊疗工具，记录误差补偿值；

步骤六三、计算诊疗工具的重心位置，得出相对误差；

步骤六四、计算重力补偿值；

步骤六五、判断诊疗工具是否测试结束，如果测试未结束，则执行步骤六六，否则执行步骤六七；

步骤六六、更换诊疗工具的种类；

步骤六七、验证结束。

具体实施方式八：本实施方式是对具体实施方式七所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法进一步限定，在本实施方式中，步骤六一中对六维力与力矩传感器进行校零的具体方法为：

将六维力与力矩传感器安装在多关节诊疗机器人末端的法兰上，且六维力与力矩传感器的三个坐标轴与多关节诊疗机器人末端的三个坐标轴重合；

用示教器控制关节诊疗机器人，使六维力与力矩传感器坐标系下的Z轴平行于基座坐标系的Z轴，并且六维力与力矩传感器坐标系下的Z轴竖直向下，第一次点击六维力与力矩传感器的校零按钮；

沿六维力与力矩传感器坐标系下的Z轴负方向施加一个大小恒定的力，再次点击六维力与力矩传感器的校零按钮，完成对六维力与力矩传感器的校零。

具体实施方式九：本实施方式是对具体实施方式七所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法进一步限定，在本实施方式中，步骤六二中记录误差补偿值的具体方法为：

将诊疗工具固定在六维力与力矩传感器上；

用示教器控制多关节诊疗机器人，使多关节诊疗机器人各轴旋转角度都为0°，即初始位置，记录六维力与力矩传感器测得的力在Y、Z方向和力矩在X、Z方向上的分量：F_y、F_z、T_x、T_z；

用示教器控制多关节诊疗机器人，使多关节诊疗机器人出各个轴转动90°，记录六维力与力矩传感器测得的力在X方向上的分量F_x和力矩在Y方向上的分量T_y。

具体实施方式十：本实施方式是对具体实施方式七所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法进一步限定，在本实施方式中，步骤六三中计算诊疗工具的重心位置，得出相对误差的具体步骤为：

步骤六三一、用示教器移动多关节诊疗机器人到一个任意位置，用六维力与力矩传感器示值减去误差值得到的力与力矩：F_x0、F_y0、F_z0、T_x0、T_y0、T_z0；

步骤六三二、用示教器按照多关节诊疗机器人基坐标系调整多关节诊疗机器人末端姿态，使诊疗工具重力只剩传感器坐标的Y轴分量，用六维力与力矩传感器示值减去误差值得到的力与力矩：F_x00、F_y00、F_z00、T_x00、T_y00、T_z00；

步骤六三三、得出诊疗工具重心对于六维力与力矩传感器坐标系的位置，即测量值：

步骤六三四、比较理论值O_GF与测量值，计算相对误差。

在本实施方式中，计算重力补偿值具体方法为：

1)在步骤2中诊疗工具固定位置的基础上，用示教器控制多关节诊疗机器人到一个任意位置，读取多关节诊疗机器人末端的诊疗工具的Q向量，在已知诊疗工具重力的情况下，计算该位置时诊疗工具重力在六维力与力矩传感器坐标系上沿X、Y、Z方向的投影F_gx、F_gy、F_gz；

2)读取此时六维力与力矩传感器测得的力在X、Y、Z方向上的分量F_x、F_y、F_z，减去步骤2中的误差值，得到重力补偿的实际值，与步骤1)中的理论值进行对比，计算相对误差；

3)重复步骤1)、2)操作，得到多个不同位置时，重力补偿的理论值和实际值的相对误差。

Claims (6)

1.一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法，其特征在于，该重力补偿方法通过以下步骤实现的：

步骤一、将六维力与力矩传感器安装在多关节诊疗机器人末端，选取多关节诊疗机器人的多个姿态，并实时记录关节诊疗机器人多个姿态下对应的姿态矩阵以及记录关节诊疗机器人多个姿态下六维力与力矩传感器的测量值，构造线性方程组，通过最小二乘法求解诊疗工具的参数向量；

步骤二、在诊疗工具处于初始位置时，记录六维力与力矩传感器的测量值；

步骤三、按照大地坐标系调整诊疗工具的姿态，使诊疗工具的重力只剩沿六维力与力矩传感器坐标的y轴方向，并记录此时六维力与力矩传感器的测量值；

步骤四、根据步骤二记录的测量值与步骤三记录的测量值，在传感器坐标系下，利用降维解析法计算诊疗工具重心的位置；

步骤五、将步骤四计算出的诊疗工具重心的位置投影到六维力与力矩传感器坐标系上，并结合步骤一求解出的诊疗工具参数向量，计算出诊疗工具的重力分量与力矩分量补偿值，实现对诊疗工具的重力补偿；

步骤六、对步骤五计算出的诊疗工具的重力分量与力矩分量补偿值进行验证；

对步骤五计算出的诊疗工具的重力分量与力矩分量补偿值进行验证的具体方法为：

步骤六一、对六维力与力矩传感器进行校零；

步骤六二、安装诊疗工具，记录误差补偿值；

步骤六三、计算诊疗工具的重心位置，得出相对误差；

步骤六四、计算重力补偿值；

步骤六五、判断诊疗工具是否测试结束，如果测试未结束，则执行步骤六六，否则执行步骤六七；

步骤六六、更换诊疗工具的种类；

步骤六七、验证结束；

步骤六一中对六维力与力矩传感器进行校零的具体方法为：

将六维力与力矩传感器安装在多关节诊疗机器人末端的法兰上，且六维力与力矩传感器的三个坐标轴与多关节诊疗机器人末端的三个坐标轴重合；

用示教器控制关节诊疗机器人，使六维力与力矩传感器坐标系下的Z轴平行于基座坐标系的Z轴，并且六维力与力矩传感器坐标系下的Z轴竖直向下，第一次点击六维力与力矩传感器的校零按钮；

沿六维力与力矩传感器坐标系下的Z轴负方向施加一个大小恒定的力，再次点击六维力与力矩传感器的校零按钮，完成对六维力与力矩传感器的校零；

步骤六二中记录误差补偿值的具体方法为：

将诊疗工具固定在六维力与力矩传感器上；

用示教器控制多关节诊疗机器人，使多关节诊疗机器人各轴旋转角度都为0°，即初始位置，记录六维力与力矩传感器测得的力在Y、Z方向和力矩在X、Z方向上的分量：F_y、F_z、T_x、T_z；

用示教器控制多关节诊疗机器人，使多关节诊疗机器人出各个轴转动90°，记录六维力与力矩传感器测得的力在X方向上的分量F_x和力矩在Y方向上的分量T_y；

步骤六三中计算诊疗工具的重心位置，得出相对误差的具体步骤为：

步骤六三一、用示教器移动多关节诊疗机器人到一个任意位置，用六维力与力矩传感器示值减去误差值得到的力与力矩：F_x0、F_y0、F_z0、T_x0、T_y0、T_z0；

步骤六三二、用示教器按照多关节诊疗机器人基坐标系调整多关节诊疗机器人末端姿态，使诊疗工具重力只剩传感器坐标的Y轴分量，用六维力与力矩传感器示值减去误差值得到的力与力矩：F_x00、F_y00、F_z00、T_x00、T_y00、T_z00；

步骤六三三、得出诊疗工具重心对于六维力与力矩传感器坐标系的位置，即测量值：

步骤六三四、比较理论值O_GF与测量值，计算相对误差。

2.根据权利要求1所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法，其特征在于，步骤一中通过最小二乘法求解诊疗工具的参数向量的具体方法为：

设诊疗工具的重力为G，则重力向量g_G为(0,0,G)^T，则诊疗工具重力作用在六维力与力矩传感器上的三维力向量g_F和三维力矩向量g_M分别为(g_FX,g_FY,g_FZ)^T和(g_MX,g_MY,g_MZ)^T；

基于坐标系变换的工具重力计算方法，三维力向量g_F可表示为：

式中，

为力觉传感器坐标系O_F到重力坐标系O_G的姿态矩阵；多关节诊疗机器人基坐标系O_B到机器人末端关节坐标系O_E的姿态矩阵为

并且

由多关节诊疗机器人正运动学方程确定；

为力觉传感器参数姿态矩阵；O_B到O_G的姿态矩阵

用旋转角(α,β,γ)表示；

由于重力坐标系Z_G轴与重力方向平行，X_GO_GY_G平面可绕Z_G轴进行任意旋转，因此设γ＝0，旋转角为Q(α,β)，

可表示为：

求解出诊疗工具的参数向量为(G,Q,O_GF)。

3.根据权利要求2所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法，其特征在于，步骤二中六维力与力矩传感器的测量值分别为：f_FX0、f_FY0、f_FZ0、f_MX0、f_MY0、f_MZ0；

其中，f_FX0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿X轴方向力的分量；f_FY0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿Y方向力的分量；f_FZ0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿Z轴方向力的分量；f_MX0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿X轴方向力矩的分量；f_MY0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿Y方向力矩的分量；f_MZ0为：诊疗工具处于初始位置时，六维力与力矩传感器测得的沿Z轴方向力矩的分量。

4.根据权利要求3所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法，其特征在于，步骤三中六维力与力矩传感器的测量值分别为：f_FX00、f_FY00、f_FZ00、f_MX00、f_MY00、f_MZ00；

其中，f_FX00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿X轴方向力的分量；f_FY00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿Y方向力的分量；f_FZ00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿Z轴方向力的分量；f_MX00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿X轴方向力矩的分量；f_MX00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿Y方向力矩的分量；f_MZ00为：诊疗工具按照大地坐标系调整后，六维力与力矩传感器测得的沿Z轴方向力矩的分量。

5.根据权利要求4所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法，其特征在于，步骤四中利用降维解析法计算诊疗工具重心位置的具体方法为：

步骤四一、利用公式(3)计算出六维力与力矩传感器坐标系下的诊疗工具重心坐标O_GF；

所述公式(3)为：

其中，X_GF为诊疗工具重心坐标O_GF的X轴坐标，Y_GF为诊疗工具重心坐标O_GF的Y轴坐标，Z_GF为诊疗工具重心坐标O_GF的Z轴坐标；

步骤四二、求解出诊疗工具重心位置为：

6.根据权利要求4所述的一种多关节诊疗机器人柔顺力控制模式下的重力补偿方法，其特征在于，步骤五中计算出诊疗工具的重力分量与力矩分量补偿值的具体方法为：

由于多关节诊疗机器人的姿态决定了重力在六维力与力矩传感器上的分量，因此将旋转角Q换算成坐标系旋转矩阵，将重力投影到六维力与力矩传感器坐标系上就得到重力的三个分量和力矩的三个分量：

其中，g_FX重力在X轴上的分量，g_FY为重力在Y轴上的分量，g_FZ为重力在Z轴上的分量，g_MX为重力力矩在X轴上的分量，g_MY为重力力矩在Y轴上的分量，g_MZ为重力力矩在Z轴上的分量；

从而得出多关节诊疗机器人末端诊疗工具重力分量与力矩分量的补偿值：

其中，f_CF为诊疗工具的重力分量补偿值，f_CM为诊疗工具的重力力矩分量补偿值，f_FX为诊疗工具所受的力在X轴上的分量，f_FY为诊疗工具所受的力在Y轴上的分量，f_FZ为诊疗工具所受的力在Z轴上的分量，f_MX为诊疗工具所受的力矩在X轴上的分量，f_MY为诊疗工具所受的力矩在Y轴上的分量，f_MZ为诊疗工具所受的力矩在Z轴上的分量。

Images