[发明专利]一种种植牙机器人导航定位方法

权利要求书

1.一种种植牙机器人导航定位方法，其特征在于，包括以下步骤：

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

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

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

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

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

其中：

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

^CT_S为CT坐标系到患者示踪器坐标系的转换矩阵；

^ST_O为患者示踪器坐标系到光学定位仪坐标系的转换矩阵；

^OT_T为光学定位仪坐标系到工具示踪器坐标系的转换矩阵；

^TT_E为工具示踪器坐标系到机械臂末端TCP坐标系的转换矩阵；

^ET_B为机械臂末端TCP坐标系到机械臂基座坐标系的转换矩阵；

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

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

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

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

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

z轴：以与的叉乘结果，作为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；

步骤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；

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

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

其中，所述工具示踪器包括刚性支架和4个定位球，固定安装在机械臂末端，每个定位球在工具示踪器坐标系下的坐标已知；

其中，步骤S5中，各参数通过以下方式获得：

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

^ST_O：由光学定位仪对患者示踪器上的定位球坐标的测量和计算得到；

^OT_T：由光学定位仪对工具示踪器上的定位球坐标的测量和计算得到；

^TT_E：由手眼标定得到；

^ET_B：由机械臂控制器直接读取到。