CN109124769B - 手术机器人的坐标系标定、控制的方法及系统 - Google Patents

Abstract

本发明公开了一种手术机器人的坐标系标定、控制的方法及系统，所述手术机器人的机械臂端设有力传感器和末端工具，力传感器和末端工具紧贴设置，末端工具的中心点系有一已知重量的测量物，坐标系标定方法包括：获取末端工具的末端工具坐标系相对手术机器人的机器坐标系的第一姿态角，末端工具坐标系以中心点为原点；根据第一姿态角将重力转换至末端工具坐标系得到第一转换力；获取力传感器在受到重力作用后在力传感器坐标系下的检测力；根据第一转换力和检测力得到力传感器坐标系相对末端工具坐标系的第二姿态角；第二姿态角用于表征力传感器坐标系与末端工具坐标系的转换关系。本发明无需外置设备即可完成本身坐标系标定，方法简单实用。

Description

手术机器人的坐标系标定、控制的方法及系统

技术领域

本发明属于手术机器人的控制领域，特别涉及一种手术机器人的坐标系标定、控制的方法及系统。

背景技术

手术机器人系统作为一种高精尖的医疗设备，人机协同控制依赖于力传感准确获知人力的大小及方位信息，为了实现对末端工具的控制，必须构建末端工具与机器人位姿之间的转换关系，而工具坐标系的创建与标定将直接关系到机器人的工作和精度，在市面上，关于坐标系标定的方法，其主要是集中在机器人工作坐标系的标定以及不同物体之间位姿关系的标定，例如不同设备间位置关系配准多采用在设备间安装Marker(标记)点，通过激光跟踪仪确定不同设备间的坐标转换关系，但设备标定过程复杂，需要较长时间的设备校准工作，存在效率不足；而针对传感器内部坐标系相对于其他设备的位置关系的配准方法多采用依赖于安装位置精度的方法来保证，而通过安装位置精度来确保传感器的位置本身的误差具有不确定性，对于有精度要求的机器人系统不适用。

发明内容

本发明要解决的技术问题是为了克服现有技术中手术机器人的坐标系标定效率低、精度低的缺陷，提供一种手术机器人的坐标系标定、控制的方法及系统。

本发明是通过下述技术方案来解决上述技术问题：

一种手术机器人的坐标系标定方法，所述手术机器人的机械臂端设有力传感器和末端工具，所述力传感器和所述末端工具紧贴设置，所述末端工具的中心点系有一已知重量的测量物，所述坐标系标定方法包括：

获取所述末端工具的末端工具坐标系相对所述手术机器人的机器坐标系的第一姿态角，所述末端工具坐标系以所述中心点为原点；

根据所述第一姿态角将重力转换至所述末端工具坐标系得到第一转换力；

获取所述力传感器在受到所述重力作用后在力传感器坐标系下的检测力；

根据所述第一转换力和所述检测力得到所述力传感器坐标系相对所述末端工具坐标系的第二姿态角；

所述第二姿态角用于表征所述力传感器坐标系与所述末端工具坐标系的转换关系。

较佳地，所述根据所述第一转换力和所述检测力得到所述力传感器坐标系相对所述末端工具坐标系的第二姿态角的步骤具体包括：

预设一转换矩阵，所述转换矩阵中的参数为未知的，所述转换矩阵用于表征所述力传感器坐标系和所述末端工具坐标系的转换关系；

根据所述转换矩阵将所述第一转换力转换至所述力传感器坐标系得到第二转换力；

令所述第二转换力等于所述检测力求解得到所述转换矩阵；

根据所述转换矩阵得到所述第二姿态角。

较佳地，所述获取所述末端工具的末端工具坐标系相对所述手术机器人的机器坐标系的第一姿态角的步骤之前，所述坐标系标定方法还包括：

设置所述手术机器人的机器坐标系与世界坐标系平行。

较佳地，根据以下公式计算所述坐标系标定方法中的所述第二姿态角：

其中，

为第一转换力，(Fx,Fy,Fz)为检测力，

为第一姿态角，R_TtoW为末工具坐标系转换到世界坐标系的转换矩阵，(α,β,γ)为第二姿态角，R_StoT为预设的力传感器坐标系转换到末端工具坐标系的转换矩阵，G为测量物的重力。

一种手术机器人控制方法，所述手术机器人控制方法包括：

利用如上所述手术机器人的坐标系标定方法得到所述力传感器坐标系相对于所述末端工具坐标系的第二姿态角；

通过所述力传感器实时获取所述手术机器人的机械臂在受到外部操作力时所述力传感器坐标系下的实时作用力；

根据所述第二姿态角将所述实时作用力由所述力传感器坐标系转换至所述末端工具坐标系得到第一转换作用力；

获取所述末端工具坐标系相对所述手术机器人的机器坐标系的实时姿态角；

根据所述实时具姿态角将所述第一转换作用力由所述末端工具坐标系转换至所述机器坐标系得到第二转换作用力；

根据所述第二转换作用力控制所述机械臂带动所述末端工具运动。

一种手术机器人的坐标系标定系统，所述坐标系标定系统包括姿态角获取模块、转换模块、力传感器和计算模块，所述手术机器人的机械臂端设有所述力传感器和末端工具，所述力传感器和所述末端工具紧贴设置，所述末端工具的中心点系有一已知重量的测量物；

所述姿态角获取模块用于获取所述末端工具的末端工具坐标系相对所述手术机器人的机器坐标系的第一姿态角，所述末端工具坐标系以所述中心点为原点；

所述转换模块用于根据所述第一姿态角将重力转换至所述末端工具坐标系得到第一转换力；

所述力传感器用于获取所述力传感器在受到所述重力作用后在力传感器坐标系下的检测力；

所述计算模块用于根据所述第一转换力和所述检测力得到所述力传感器坐标系相对所述末端工具坐标系的第二姿态角；

所述第二姿态角用于表征所述力传感器坐标系与所述末端工具坐标系之间的转换关系。

较佳地，所述计算模块包括预设单元；

所述预设单元用于预设一转换矩阵，所述转换矩阵中的参数为未知的，所述转换矩阵用于表征所述力传感器坐标系和所述末端工具坐标系的转换关系；

所述转换模块还用于根据所述转换矩阵将所述第一转换力转换至所述力传感器坐标系得到第二转换力；

所述计算模块用于令所述第二转换力等于所述检测力求解得到所述转换矩阵，并根据所述转换矩阵得到所述第二姿态角。

较佳地，所述手术机器人的机器坐标系与世界坐标系是平行的。

较佳地，所述坐标系标定系统根据以下公式计算所述第二姿态角：

其中，

为第一转换力，(Fx,Fy,Fz)为检测力，

为第一姿态角，R_TtoW为末工具坐标系转换到世界坐标系的转换矩阵，(α,β,γ)为第二姿态角，R_StoT为预设的力传感器坐标系转换到末端工具坐标系的转换矩阵，G为测量物的重力。

一种手术机器人控制系统，所述手术机器人控制系统包括控制器和如上所述的手术机器人的坐标系标定系统，所述控制器包括第二姿态角获取单元、转换单元、实时姿态角获取单元和运动控制单元；

所述第二姿态角获取单元用于利用所述手术机器人的坐标系标定系统获取所述力传感器坐标系相对于所述末端工具的坐标系的第二姿态角；

所述力传感器用于实时获取所述手术机器人的机械臂受到外部操作力时所述力传感器坐标系下的实时作用力；

所述转换单元用于根据所述第二姿态角将所述实时作用力由所述力传感器坐标系转换至所述末端工具坐标系得到第一转换作用力；

所述实时姿态角获取单元用于获取所述末端工具坐标系相对所述手术机器人的机器坐标系的实时姿态角；

所述转换单元还用于根据所述实时姿态角将所述第一转换作用力由所述末端工具坐标系转换至所述机器坐标系得到第二转换作用力；

所述运动控制单元用于根据所述第二转换作用力控制所述机械臂带动所述刀头运动。

本发明的积极进步效果在于：本发明在降低了基于安装位置确认坐标系方位的误差不确定性风险，在保证了力传感器坐标系标定准确性的同时提高了标定效率，无需外置设备即可完成本身坐标系标定，方法简单实用。

附图说明

图1为本发明实施例1中的手术机器人的部分结构示意图。

图2为本发明实施例1的手术机器人的坐标系标定方法的流程图。

图3为本发明实施例2的手术机器人的坐标系标定方法中步骤140的具体流程图。

图4为本发明实施例3的手术机器人控制方法的流程图。

图5本发明实施例4的手术机器人的坐标系标定系统的模块示意图。

图6本发明实施例5的手术机器人的坐标系标定系统的模块示意图。

图7本发明实施例6的手术机器人控制系统的模块示意图。

具体实施方式

下面通过实施例的方式进一步说明本发明，但并不因此将本发明限制在所述的实施例范围之中。

实施例1

一种手术机器人的坐标系标定方法，如图1所示，所述手术机器人的机械臂端设有力传感器和末端工具，所述力传感器和所述末端工具紧贴设置，所述末端工具的中心点系有一已知重量的测量物，测量物可以使用砝码，如图2所示，所述坐标系标定方法包括：

步骤110、获取末端工具的末端工具坐标系相对手术机器人的机器坐标系的第一姿态角；所述末端工具坐标系以所述中心点为原点；

步骤120、根据第一姿态角将重力转换至末端工具坐标系得到第一转换力；

步骤130、获取力传感器在受到重力作用后在力传感器坐标系下的检测力；

步骤140、根据第一转换力和检测力得到力传感器坐标系相对末端工具坐标系的第二姿态角；所述第二姿态角用于表征力传感器坐标系与末端工具坐标系的转换关系；

另外，步骤110之前，所述坐标系标定方法还包括：

步骤100、设置手术机器人的机器坐标系与世界坐标系平行。

本实施例中，手术机器人可以直接获取末端工具的姿态角，基于此，通过转转换关系进一步得到力传感器与末端工具之间的转换关系，从后准确得知末端工具上的作用力数据，本实施例降低了基于安装位置确认坐标系方位的误差不确定性风险，在保证了力传感器坐标系标定准确性的同时提高了标定效率，无需外置设备即可完成本身坐标系标定，方法简单实用。

实施例2

本实施例的手术机器人的坐标系标定方法是在实施例1的基础上进一步改进，如图3所示，步骤140具体包括：

步骤1401、预设一转换矩阵；所述转换矩阵中的参数为未知的，所述转换矩阵用于表征所述力传感器坐标系和所述末端工具坐标系的转换关系；

步骤1402、根据转换矩阵将第一转换力转换至力传感器坐标系得到第二转换力；

步骤1403、令第二转换力等于检测力求解得到转换矩阵；

步骤1404、根据转换矩阵得到第二姿态角。

举一具体示例，使用20N的砝码作为测量物，获取到末端工具的第一姿态角

为(3.04532°,1.57058°,3.04259°)，通过力传感器读取到的检测力(Fx,Fy,Fz)为(18.6163044,5.312244,0)，假设第二姿态角为(α,β,γ)，进一步根据以下公式计算所述坐标系标定方法中的所述第二姿态角：

其中，

为第一转换力，(Fx,Fy,Fz)为检测力，

为第一姿态角，R_TtoW为末工具坐标系转换到世界坐标系的转换矩阵，(α,β,γ)为第二姿态角，R_StoT为预设的力传感器坐标系转换到末端工具坐标系的转换矩阵，G为测量物的重力；

为了便于计算，可以构建误差向量：

根据最小二乘法分别对α,β,γ角求偏导取0，求解得到(α,β,γ)的值，为(1.3316°,-165.3054°,-0.6644°)。

需要说明的是，为了确保计算得到的第二姿态角更加精准，可以使手术机器人运动到多个不同位姿，取多次数据进行计算验证。

实施例3

一种手术机器人控制方法，如图4所示，所述手术机器人控制方法包括：

步骤210、利用如实施例1或2的手术机器人的坐标系标定方法得到力传感器坐标系相对于末端工具坐标系的第二姿态角；

步骤220、通过力传感器实时获取手术机器人的机械臂在受到外部操作力时力传感器坐标系下的实时作用力；

步骤230、根据第二姿态角将实时作用力由力传感器坐标系转换至末端工具坐标系得到第一转换作用力；

步骤240、获取末端工具坐标系相对手术机器人的机器坐标系的实时姿态角；

步骤250、根据实时具姿态角将第一转换作用力由末端工具坐标系转换至机器坐标系得到第二转换作用力；

步骤260、根据第二转换作用力控制机械臂带动末端工具运动。

本实施例中，获取到力传感器与末端工具的转换关系后，当用户操作手术机器人时，力传感器可以检测到力传感器坐标系下的力，然后根据上述的转换关系将其转换至施加到末端工具上的力，进而由控制器实现对末端工具的控制，或者，若需要操控机械臂往某一个方向运动或施加某一个力，也可以将力传感器检测的值实时反馈至控制器，并根据坐标系的转换关系，获取用户在机器坐标系下需要施加的操控力的大小或者方向。

实施例4

一种手术机器人的坐标系标定系统，如图5所示，所述坐标系标定系统包括姿态角获取模块11、转换模块12、力传感器13和计算模块14，所述手术机器人的机械臂端设有所述力传感器13和末端工具，所述力传感器13和所述末端工具紧贴设置，所述末端工具的中心点系有一已知重量的测量物，测量物可以使用砝码；

所述姿态角获取模块11用于获取所述末端工具的末端工具坐标系相对所述手术机器人的机器坐标系的第一姿态角，所述末端工具坐标系以所述中心点为原点；其中，所述手术机器人的机器坐标系与世界坐标系是平行的；

所述转换模块12用于根据所述第一姿态角将所述重力转换至所述末端工具坐标系得到第一转换力；

所述力传感器13用于获取所述力传感器13在受到所述重力作用后在力传感器坐标系下的检测力；

所述计算模块14用于根据所述第一转换力和所述检测力得到所述力传感器坐标系相对所述末端工具坐标系的第二姿态角；

所述第二姿态角用于表征所述力传感器坐标系与所述末端工具坐标系之间的转换关系。

本实施例中，手术机器人可以直接获取末端工具的姿态角，基于此，通过转转换关系进一步得到力传感器13与末端工具之间的转换关系，从后准确得知末端工具上的作用力数据，本实施例降低了基于安装位置确认坐标系方位的误差不确定性风险，在保证了力传感器坐标系标定准确性的同时提高了标定效率，无需外置设备即可完成本身坐标系标定，方法简单实用。

实施例5

本实施例的手术机器人的坐标系标定系统是在实施例4的基础上进一步改进，如图6所示，所述计算模块14包括预设单元141；

所述预设单元141用于预设一转换矩阵，所述转换矩阵中的参数为未知的，所述转换矩阵用于表征所述力传感器坐标系和所述末端工具坐标系的转换关系；

所述转换模块12还用于根据所述转换矩阵将所述第一转换力转换至所述力传感器坐标系得到第二转换力；

所述计算模块14用于令所述第二转换力等于所述检测力求解得到所述转换矩阵，并根据所述转换矩阵得到所述第二姿态角。

举一具体示例，使用20N的砝码作为测量物，获取到末端工具的第一姿态角

为(3.04532°,1.57058°,3.04259°)，通过力传感器读取到的检测力(Fx,Fy,Fz)为(18.6163044,5.312244,0)，假设第二姿态角为(α,β,γ)，进一步根据以下公式计算所述第二姿态角：

其中，

为第一转换力，(Fx,Fy,Fz)为检测力，

为第一姿态角，R_TtoW为末工具坐标系转换到世界坐标系的转换矩阵，(α,β,γ)为第二姿态角，R_StoT为预设的力传感器坐标系转换到末端工具坐标系的转换矩阵，G为测量物的重力；

为了便于计算，可以构建误差向量：

根据最小二乘法分别对α,β,γ角求偏导取0，求解得到(α,β,γ)的值，为(1.3316°,-165.3054°,-0.6644°)。

需要说明的是，为了确保计算得到的第二姿态角更加精准，可以使手术机器人运动到多个不同位姿，取多次数据进行计算验证。

实施例6

一种手术机器人控制系统，如图7所示，所述手术机器人控制系统包括控制器2和如实施例4或5所述的手术机器人的坐标系标定系统1，所述控制器2包括第二姿态角获取单元21、转换单元22、实时姿态角获取单元23和运动控制单元24；

所述第二姿态角获取单元21用于利用所述手术机器人的坐标系标定系统获取所述力传感器坐标系相对于所述末端工具的坐标系的第二姿态角；

所述力传感器用于实时获取所述手术机器人的机械臂受到外部操作力时所述力传感器坐标系下的实时作用力；

所述转换单元22用于根据所述第二姿态角将所述实时作用力由所述力传感器坐标系转换至所述末端工具坐标系得到第一转换作用力；

所述实时姿态角获取单元23用于获取所述末端工具坐标系相对所述手术机器人的机器坐标系的实时姿态角；

所述转换单元22还用于根据所述实时姿态角将所述第一转换作用力由所述末端工具坐标系转换至所述机器坐标系得到第二转换作用力；

所述运动控制单元24用于根据所述第二转换作用力控制所述机械臂带动所述刀头运动。

本实施例中，获取到力传感器与末端工具的转换关系后，当用户操作手术机器人时，力传感器可以检测到力传感器坐标系下的力，然后根据上述的转换关系将其转换至施加到末端工具上的力，进而由控制器实现对末端工具的控制，或者，若需要操控机械臂往某一个方向运动或施加某一个力，也可以将力传感器检测的值实时反馈至控制器，并根据坐标系的转换关系，获取用户在机器坐标系下需要施加的操控力的大小或者方向。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这仅是举例说明，本发明的保护范围是由所附权利要求书限定的。本领域的技术人员在不背离本发明的原理和实质的前提下，可以对这些实施方式做出多种变更或修改，但这些变更和修改均落入本发明的保护范围。

Claims (9)

1.一种手术机器人的坐标系标定方法，其特征在于，所述手术机器人的机械臂端设有力传感器和末端工具，所述力传感器和所述末端工具紧贴设置，所述末端工具的中心点系有一已知重量的测量物，所述坐标系标定方法包括：

获取所述末端工具的末端工具坐标系相对所述手术机器人的机器坐标系的第一姿态角，所述末端工具坐标系以所述中心点为原点；

根据所述第一姿态角将重力转换至所述末端工具坐标系得到第一转换力；

获取所述力传感器在受到所述重力作用后在力传感器坐标系下的检测力；

根据所述第一转换力和所述检测力得到所述力传感器坐标系相对所述末端工具坐标系的第二姿态角；

所述第二姿态角用于表征所述力传感器坐标系与所述末端工具坐标系的转换关系。

2.如权利要求1所述的手术机器人的坐标系标定方法，其特征在于，所述根据所述第一转换力和所述检测力得到所述力传感器坐标系相对所述末端工具坐标系的第二姿态角的步骤具体包括：

预设一转换矩阵，所述转换矩阵中的参数为未知的，所述转换矩阵用于表征所述力传感器坐标系和所述末端工具坐标系的转换关系；

根据所述转换矩阵将所述第一转换力转换至所述力传感器坐标系得到第二转换力；

令所述第二转换力等于所述检测力求解得到所述转换矩阵；

根据所述转换矩阵得到所述第二姿态角。

3.如权利要求1所述的手术机器人的坐标系标定方法，其特征在于，所述获取所述末端工具的末端工具坐标系相对所述手术机器人的机器坐标系的第一姿态角的步骤之前，所述坐标系标定方法还包括：

设置所述手术机器人的机器坐标系与世界坐标系平行。

4.如权利要求2所述的手术机器人的坐标系标定方法，其特征在于，根据以下公式计算所述坐标系标定方法中的所述第二姿态角：

其中，

为第一转换力，(Fx,Fy,Fz)为检测力，

为第一姿态角，R_TtoW为末工具坐标系转换到世界坐标系的转换矩阵，(α,β,γ)为第二姿态角，R_StoT为预设的力传感器坐标系转换到末端工具坐标系的转换矩阵，G为测量物的重力。

5.一种手术机器人的坐标系标定系统，其特征在于，所述坐标系标定系统包括姿态角获取模块、转换模块、力传感器和计算模块，所述手术机器人的机械臂端设有所述力传感器和末端工具，所述力传感器和所述末端工具紧贴设置，所述末端工具的中心点系有一已知重量的测量物；

所述姿态角获取模块用于获取所述末端工具的末端工具坐标系相对所述手术机器人的机器坐标系的第一姿态角，所述末端工具坐标系以所述中心点为原点；

所述转换模块用于根据所述第一姿态角将重力转换至所述末端工具坐标系得到第一转换力；

所述力传感器用于获取所述力传感器在受到所述重力作用后在力传感器坐标系下的检测力；

所述计算模块用于根据所述第一转换力和所述检测力得到所述力传感器坐标系相对所述末端工具坐标系的第二姿态角；

所述第二姿态角用于表征所述力传感器坐标系与所述末端工具坐标系之间的转换关系。

6.如权利要求5所述的手术机器人的坐标系标定系统，其特征在于，所述计算模块包括预设单元；

所述预设单元用于预设一转换矩阵，所述转换矩阵中的参数为未知的，所述转换矩阵用于表征所述力传感器坐标系和所述末端工具坐标系的转换关系；

所述转换模块还用于根据所述转换矩阵将所述第一转换力转换至所述力传感器坐标系得到第二转换力；

所述计算模块用于令所述第二转换力等于所述检测力求解得到所述转换矩阵，并根据所述转换矩阵得到所述第二姿态角。

7.如权利要求5所述的手术机器人的坐标系标定系统，其特征在于，所述手术机器人的机器坐标系与世界坐标系是平行的。

8.如权利要求6所述的手术机器人的坐标系标定系统，其特征在于，所述坐标系标定系统根据以下公式计算所述第二姿态角：

其中，

为第一转换力，(Fx,Fy,Fz)为检测力，

为第一姿态角，R_TtoW为末工具坐标系转换到世界坐标系的转换矩阵，(α,β,γ)为第二姿态角，R_StoT为预设的力传感器坐标系转换到末端工具坐标系的转换矩阵，G为测量物的重力。

9.一种手术机器人控制系统，其特征在于，所述手术机器人控制系统包括控制器和如权利要求5-8中任意一项所述的手术机器人的坐标系标定系统，所述控制器包括第二姿态角获取单元、转换单元、实时姿态角获取单元和运动控制单元；

所述第二姿态角获取单元用于利用所述手术机器人的坐标系标定系统获取所述力传感器坐标系相对于所述末端工具的坐标系的第二姿态角；

所述力传感器用于实时获取所述手术机器人的机械臂受到外部操作力时所述力传感器坐标系下的实时作用力；

所述转换单元用于根据所述第二姿态角将所述实时作用力由所述力传感器坐标系转换至所述末端工具坐标系得到第一转换作用力；

所述实时姿态角获取单元用于获取所述末端工具坐标系相对所述手术机器人的机器坐标系的实时姿态角；

所述转换单元还用于根据所述实时姿态角将所述第一转换作用力由所述末端工具坐标系转换至所述机器坐标系得到第二转换作用力；

所述运动控制单元用于根据所述第二转换作用力控制所述机械臂带动刀头运动。

Images