具体实施方式

为了使本发明所解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明提供一种种植牙机器人导航定位方法，与医生手工操作相比，机械臂具有定位准确、颤动小、工作稳定的优点，可降低医生工作强度，保证手术精度，通过机器人导航定位可引导机械臂带动手术刀具实现精准手术。

参考图1，本发明提供的一种种植牙机器人导航定位方法，是一种高效准确引导手术刀具到达目标路径的种植牙机器人导航定位方法，通过光学定位仪测量和坐标系转换，进行机器人的导航定位，引导刀具到达目标路径，包括以下步骤：

步骤S1，种植牙患者的非种植牙牙齿佩戴患者示踪器，使患者示踪器和种植牙目标位置之间的相对位姿保持不变；

其中，如图2所示，患者示踪器包括9个钛珠、4个定位球和不易在CT扫描图像中成像的刚性支架；

其中，每个钛珠和定位球在患者示踪器坐标系下的坐标已知，在导航定位过程中患者始终佩戴患者示踪器。

步骤S2，采用CT扫描仪对包含患者示踪器和种植牙目标位置的牙齿区域进行CT扫描，得到CT扫描图像；

步骤S3，在CT扫描图像中规划种植牙路径，确定CT坐标系下目标路径的进入点位置A_C和进入点方向其中，进入点位置A_C和进入点方向形成CT坐标系下的进入点目标位姿；

步骤S4，种植牙机器人包括机械臂基座、多关节机械臂、手术刀具和工具示踪器；参考图1，其中，1代表机械臂基座，2代表多关节机械臂，3代表手术刀具，4代表工具示踪器。5代表患者示踪器，6代表光学定位仪。其中，机械臂基座固定设置，位置保持不变；机械臂基座的上面安装多关节机械臂；多关节机械臂的末端安装手术刀具和工具示踪器；在手术过程中，工具示踪器、手术刀具和多关节机械臂末端之间的相对位姿保持不变，即：工具示踪器坐标系、手术刀具坐标系Tip和机械臂末端TCP坐标系之间的相对位姿保持不变，但是，在手术刀具运动过程中，工具示踪器坐标系、手术刀具坐标系Tip和机械臂末端TCP坐标系会实时发生变化，而机械臂基座坐标系保持不变；

另外，参考图3，工具示踪器包括刚性支架和4个定位球，固定安装在机械臂末端，每个定位球在工具示踪器坐标系下的坐标已知。

步骤S5，将步骤S3确定的CT坐标系下目标路径的进入点位置A_C和进入点方向转换为机械臂基座坐标系下的目标路径的进入点位置A_B和进入点方向

其中：

I为3行3列的单位向量；

^CT_S为CT坐标系到患者示踪器坐标系的转换矩阵；通过CT扫描仪对患者示踪器上的钛珠坐标的测量和计算得到；

^ST_O为患者示踪器坐标系到光学定位仪坐标系的转换矩阵；由光学定位仪对患者示踪器上的定位球坐标的测量和计算得到；

^OT_T为光学定位仪坐标系到工具示踪器坐标系的转换矩阵；由光学定位仪对工具示踪器上的定位球坐标的测量和计算得到；

^TT_E为工具示踪器坐标系到机械臂末端TCP坐标系的转换矩阵；由手眼标定得到；手眼标定方法为：通过外部测量方式建立机械臂末端TCP坐标系与固定安装在机械臂上的工具示踪器坐标系的转换关系。

^ET_B为机械臂末端TCP坐标系到机械臂基座坐标系的转换矩阵；由机械臂控制器直接读取到。

步骤S6，建立刀具目标位姿坐标系Tip′到机械臂基座坐标系的转换矩阵^Tip′T_B：

这一步骤是因为机械臂的运动控制是基于坐标系的转换关系，只给出目标路径的进入点位置和方向无法进行机械臂的运动控制，因此需要根据目标路径的进入点位置和方向建立刀具目标位姿坐标系，以进一步计算坐标系转换关系，将刀具目标位姿坐标系Tip′转换到机械臂基座坐标系，才能够对机械臂进行机械控制。

步骤S6.1，建立手术刀具目标位姿坐标系Tip′，包括：确定坐标原点o，确定x轴、y轴和z轴；

坐标原点o：以机械臂基座坐标系下的目标路径的进入点位置A_B为坐标原点；

x轴：以机械臂基座坐标系下的目标路径的进入点方向为x轴；

y轴：以机械臂末端TCP坐标系的z轴在机械臂基座坐标系下的方向向量与的叉乘结果，作为y轴在机械臂基座坐标系下的方向向量

z轴：以与的叉乘结果，作为z轴在机械臂基座坐标系下的方向向量

此种手术刀具目标位姿坐标系Tip′定义方式的优点为：1)可以保证刀具目标位姿坐标系的x轴、y轴和z轴之间相互垂直；2)刀具目标位姿坐标系的z轴与机械臂末端TCP坐标系的z轴接近平行，方便后续计算和控制机械臂运动。

步骤S6.2，构建得到刀具目标位姿坐标系Tip′到机械臂基座坐标系的转换矩阵^Tip′T_B：

步骤S7，建立手术刀具坐标系Tip到机械臂末端TCP坐标系的转换矩阵^TipT_E：

步骤S7.1，建立手术刀具坐标系Tip，包括：确定坐标原点o₁，确定x₁轴、y₁轴和z₁轴；

坐标原点o₁：以刀具尖端在机械臂末端TCP坐标系下的坐标P_E为坐标原点；

x₁轴：以刀具方向在机械臂末端TCP坐标系下的方向向量作为x₁轴在机械臂末端TCP坐标系下的方向向量

y₁轴：以机械臂末端TCP坐标系的z轴与的叉乘结果，作为y₁轴在机械臂末端TCP坐标系下的方向向量y_E；

z₁轴：以和y_E的叉乘结果，作为z₁轴在机械臂末端TCP坐标系下的方向向量z_E；

此种手术刀具坐标系Tip定义方式优点为：1)可以保证手术刀具坐标系Tip的x₁轴、y₁轴和z₁轴之间相互垂直；2)手术刀具坐标系Tip的z₁轴与机械臂末端TCP坐标系和刀具目标位姿坐标系的z轴接近平行，则刀具在到达目标位姿的过程中，机械臂末端不需要摆动较大的角度以调整刀具的z轴方向，方便机械臂的运动控制。

步骤S7.2，构建得到手术刀具坐标系Tip到机械臂末端TCP坐标系的转换矩阵^TipT_E：

步骤S8，计算刀具到达目标位姿时，机械臂末端TCP坐标系到机械臂基座坐标系的转换矩阵^E′T_B，方法为：

当刀具到达目标位姿时，手术刀具坐标系Tip和刀具目标位姿坐标系Tip′重合，因此，手术刀具坐标系Tip到机械臂基座坐标系的转换矩阵^TipT_B，等于刀具目标位姿坐标系Tip′到机械臂基座坐标系的转换矩阵^Tip′T_B，由此有以下关系式：

^TipT_B＝^Tip′T_B

又由于手术刀具坐标系Tip到机械臂基座坐标系的转换矩阵^TipT_B为：

^TipT_B＝^TipT_E·^ET_B

因此，刀具到达目标位姿时，机械臂末端TCP坐标系到机械臂基座坐标系的转换矩阵^E′T_B为：

^E′T_B＝(^TipT_E)^-1·^TipT_B＝(^TipT_E)^-1·^Tip′T_B

步骤S9，根据步骤S8确定的刀具到达目标位姿时，机械臂末端TCP坐标系到机械臂基座坐标系的转换矩阵^E′T_B，生成对种植牙机器人的机械臂各关节的控制指令，从而使种植牙机器人的机械臂各关节发生运动；

步骤S10，当种植牙机器人的机械臂各关节运动结束后，计算刀具尖端在机械臂基座坐标系下的位置坐标A"_B，以及刀具方向在机械臂基座坐标系下的方向向量

将刀具尖端在机械臂基座坐标系下的位置坐标A"_B与步骤S5得到的机械臂基座坐标系下的目标路径的进入点位置A_B比较，得到位置坐标误差；

将刀具方向在机械臂基座坐标系下的方向向量与步骤S5得到的机械臂基座坐标系下的目标路径的进入点方向比较，得到方向向量误差；

步骤S11，判断位置坐标误差和方向向量误差是否均小于设定阈值，如果是，则导航结束，表明种植牙机器人的刀具已运动到目标位姿；否则，重复执行步骤S9-步骤S11。

本发明利用CT扫描仪和光学定位仪的测量，建立患者种植牙路径和手术刀具之间的坐标系转换关系，从而引导机械臂运动，带动手术刀具到达目标路径的进入点位置和方向，采用机械臂带动手术刀具运动，具有操作简便，安全可靠，可实现手术刀具位置和方向的精确定位，降低医生的工作强度，有效提高种植牙手术的精度。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视本发明的保护范围。