本发明提供一种种植牙机器人导航系统及方法，系统包括：控制器、机械臂、手术刀具、光学定位仪、工具示踪器、患者示踪器和刀具标定器；所述手术刀具和所述工具示踪器固定安装在所述机械臂的末端；所述患者示踪器固定在患者的非种植牙牙齿上。优点为：机器人导航系统利用术前CT扫描影像规划种植牙手术路径进行导航，并在导航过程中，对种植牙手术路径进行实时修正，另外，利用光学定位仪跟踪手术刀具的位置和姿态，从而引导机械臂运动，带动手术刀具到达种植牙路径，可实现手术刀具位置和方向的精确定位，具有定位准确、工作稳定的优点，可保证手术的精度，有效降低对医生操作的要求，并降低医生的工作强度。
1.一种种植牙机器人导航系统的方法，其特征在于，包括：控制器、机械臂、手术刀具、光学定位仪、工具示踪器、患者示踪器和刀具标定器；所述手术刀具和所述工具示踪器固定安装在所述机械臂的末端；所述患者示踪器固定在患者的非种植牙牙齿上；
其中：所述刀具标定器包括4个光学定位球和一个标定孔；所述刀具标定器的标定孔套在所述手术刀具的末端后，手术刀具末端顶点与标定孔顶端中心点重合，手术刀具轴线方向与标定孔轴线方向重合；
步骤S1，预先标定得到工具示踪器坐标系{T}到机械臂TCP坐标系{E}的转换矩阵 TE：步骤S1.1，控制机械臂TCP随机运动n个位姿，得到以下n个方程：O O T E1
TB：代表光学定位仪坐标系{O}到机械臂基座坐标系{B}的转换矩阵，由于机械臂TCP运O动过程中，机械臂基座与光学定位仪均固定不动，所以 TB始终不变，为已知固定值；
TTi，i＝1,2,...,n：代表机械臂TCP随机运动到第i个位姿时，光学定位仪坐标系{O}到工具示踪器坐标系{T}的转换矩阵，通过以下方法获得：当机械臂TCP随机运动到第i个位姿时,此时，通过光学定位仪，测量得到工具示踪器上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；再结合已知的工具示踪器上每个光学定位球在工具示踪器坐标系O{T}下的三维位置坐标，得到转换矩阵 TTi；
TE：代表工具示踪器坐标系{T}到机械臂TCP坐标系{E}的转换矩阵 TE，在机械臂TCP运T动过程中，TE固定不变，为待求值；
TB，i＝1,2,...,n：代表机械臂TCP随机运动到第i个位姿时，机械臂TCP坐标系{E}到机械臂基座坐标系{B}的转换矩阵，为已知值；
步骤S1.2，对n个方程形成的方程组，采用最小二乘方法求解，得到工具示踪器坐标系T{T}到机械臂TCP坐标系{E}的转换矩阵 TE；
步骤S2，预先标定得到手术刀具与工具示踪器之间相对位姿关系，即：得到手术刀具末端顶点Q在工具示踪器坐标系{T}的三维位置坐标QT，以及手术刀具轴线方向在工具示踪器坐标系{T}的方向向量步骤S2.1，将刀具标定器套在手术刀具的末端，使得手术刀具末端顶点与刀具标定器的标定孔顶端中心点重合，使手术刀具轴线方向与刀具标定器的标定孔轴线方向重合；
步骤S2.2，通过光学定位仪，获得工具示踪器坐标系{T}到光学定位仪坐标系{O}的转T K换矩阵 TO，以及刀具标定器坐标系{K}到光学定位仪坐标系{O}的转换矩阵 TO；
具体方法为：通过光学定位仪，测量得到工具示踪器上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；再结合已知的工具示踪器上每个光学定位球在工具示踪器T坐标系{T}下的三维位置坐标，得到转换矩阵 TO；
通过光学定位仪，测量得到刀具标定器上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；再结合已知的刀具标定器上每个光学定位球在刀具标定器坐标系{K}下K的三维位置坐标，得到转换矩阵 TO；
步骤S2.3，采用下式，得到刀具标定器坐标系{K}到工具示踪器坐标系{T}的转换矩阵KTT：
步骤S2.4，获得刀具标定器的标定孔顶端中心点在刀具标定器坐标系{K}中的三维位置坐标PK，以及刀具标定器的标定孔轴线方向在刀具标定器坐标系{K}中的方向向量根据下式，将刀具标定器的标定孔顶端中心点在刀具标定器坐标系{K}中的三维位置坐标PK，转换到工具示踪器坐标系{T}中，得到标定孔顶端中心点在工具示踪器坐标系{T}中的三维位置坐标PT：根据下式，将刀具标定器的标定孔轴线方向在刀具标定器坐标系{K}中的方向向量转换到工具示踪器坐标系{T}中，得到标定孔轴线方向在工具示踪器坐标系{T}中的方向向量其中：
步骤S2.5，由于标定孔顶端中心点在工具示踪器坐标系{T}中的三维位置坐标PT，等于手术刀具末端顶点Q在工具示踪器坐标系{T}的三维位置坐标QT；标定孔轴线方向在工具示踪器坐标系{T}中的方向向量 等于手术刀具轴线方向在工具示踪器坐标系{T}的方向向量 因此标记得到手术刀具与工具示踪器之间相对位姿关系；
步骤S3，获得CT扫描机坐标系{C}到光学定位仪坐标系{O}的转换矩阵 TO：步骤S3.1，CT扫描机对患者示踪器进行扫描成像，使患者示踪器的钛珠成像在CT影像中；对CT影像进行分析，得到钛珠在CT扫描机坐标系{C}的三维位置坐标；
步骤S3.2，通过光学定位仪，测量得到患者示踪器的上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；
步骤S3.3，又由于患者示踪器的钛珠在患者示踪器坐标系{S}的三维位置坐标，以及患者示踪器的每个光学定位球在患者示踪器坐标系{S}的三维位置坐标均为已知固定值，因此，结合钛珠在CT扫描机坐标系{C}的三维位置坐标，以及患者示踪器的上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标，得到CT扫描机坐标系{C}到光学定位仪坐标系C{O}的转换矩阵 TO；
步骤4，手术开始前，患者佩戴患者示踪器进行CT扫描，获得CT扫描机坐标系{C}下的种植牙影像，通过对种植牙影像进行分析，规划得到在CT扫描机坐标系{C}下的种植牙路径的进入点AC和路径方向采用下式，将CT扫描机坐标系{C}下的种植牙路径的进入点AC和路径方向 转换到光学定位仪坐标系{O}下，得到光学定位仪坐标系{O}下的种植牙路径的进入点AO和路径方向该光学定位仪坐标系{O}下的种植牙路径的进入点AO和路径方向 同时为手术刀具目标位姿；
步骤5，在导航过程中，采用以下方法，通过光学定位仪，实时测量得到手术刀具实时位姿；其中，所述手术刀具实时位姿包括手术刀具末端顶点Q在光学定位仪坐标系{O}下的三维位置坐标QO，以及手术刀具轴线方向在光学定位仪坐标系{O}下的方向向量通过比较手术刀具实时位姿与手术刀具目标位姿，得到手术刀具在光学定位仪坐标系{O}下的位姿偏差；根据手术刀具在光学定位仪坐标系{O}下的位姿偏差，生成对机械臂控制指令，控制机械臂带动手术刀具朝向手术刀具目标位姿运动，使手术刀具实时位姿不断逼近手术刀具目标位姿。
2.根据权利要求1所述的种植牙机器人导航系统的方法，其特征在于，步骤5中，根据手术刀具位姿偏差，生成对机械臂控制指令，控制机械臂带动手术刀具朝向手术刀具目标位姿运动，具体为：步骤5.1，手术刀具在光学定位仪坐标系{O}下的位姿偏差，包括三维位置坐标偏差ΔQO和方向向量偏差O
步骤5.2，获取光学定位仪坐标系{O}到刀具标定器坐标系{K}的转换矩阵 TK；其中，刀具标定器坐标系{K}与刀具坐标系完全相同；因此得到光学定位仪坐标系{O}到刀具坐标系O{F}的转换矩阵 TF；
根据光学定位仪坐标系{O}到刀具坐标系{F}的转换矩阵TF，将手术刀具在光学定位仪坐标系{O}下的位姿偏差，转换为手术刀具在刀具坐标系{F}下的位姿偏差；
步骤5.3，根据步骤S2预先标定得到的手术刀具与工具示踪器之间相对位姿关系，将手术刀具在刀具坐标系{F}下的位姿偏差，转换为手术刀具在工具示踪器坐标系{T}下的位姿偏差；
步骤5.4，根据步骤S1预先标定得到的工具示踪器坐标系{T}到机械臂TCP坐标系{E}的T转换矩阵 TE，将手术刀具在工具示踪器坐标系{T}下的位姿偏差，转换为手术刀具在机械臂TCP坐标系{E}下的位姿偏差；
步骤5.5，根据手术刀具在机械臂TCP坐标系{E}下的位姿偏差，生成对机械臂控制指令，控制机械臂带动手术刀具朝向手术刀具目标位姿运动。
3.根据权利要求1所述的种植牙机器人导航系统的方法，其特征在于，还包括：步骤6，在导航过程中，实时更新步骤4得到的手术刀具目标位姿，即：光学定位仪坐标系{O}下的种植牙路径的进入点AO和路径方向
4.根据权利要求3所述的种植牙机器人导航系统的方法，其特征在于，步骤6具体为：步骤6.1，通过光学定位仪，实时测量得到患者示踪器上每个光学定位球在光学定位仪坐标系{O}下的实时位姿；
步骤6.2，根据光学定位仪坐标系{O}到患者示踪器坐标系{S}的转换矩阵，将患者示踪器上每个光学定位球在光学定位仪坐标系{O}下的实时位姿，转换为患者示踪器上每个光学定位球在患者示踪器坐标系{S}下的实时位姿；
步骤6.3，根据患者示踪器坐标系{S}到CT扫描机坐标系{C}的转换矩阵，将患者示踪器上每个光学定位球在患者示踪器坐标系{S}下的实时位姿，转换为患者示踪器上每个光学定位球在CT扫描机坐标系{C}下的实时位姿；
步骤6.4，由于患者病患牙齿与患者示踪器的相对位姿关系固定不变，因此，将患者示踪器上每个光学定位球在CT扫描机坐标系{C}下的实时位姿，转换为患者病患牙齿在CT扫描机坐标系{C}下的实时位姿；
患者病患牙齿在CT扫描机坐标系{C}下的实时位姿，即为种植牙路径的进入点当前值和路径方向当前值；比较种植牙路径的进入点当前值和路径方向当前值，与步骤4规划得到在CT扫描机坐标系{C}下的种植牙路径的进入点AC和路径方向 如果一致，则表明手术过程中患者头部位置没有发生变化，不需要更新手术刀具目标位姿；否则，采用患者病患牙齿在CT扫描机坐标系{C}下的实时位姿，更新手术刀具目标位姿。
5.根据权利要求3所述的种植牙机器人导航系统的方法，其特征在于，所述机械臂是6自由度协作机械臂，设于机械臂基座上面；所述机械臂基座形成机械臂基座坐标系{B}，机械臂的末端机械接口称为TCP，形成机械臂TCP坐标系{E}。
6.根据权利要求3所述的种植牙机器人导航系统的方法，其特征在于，所述工具示踪器包括刚性支架，所述刚性支架上固定安装有4个光学定位球。
7.根据权利要求3所述的种植牙机器人导航系统的方法，其特征在于，所述患者示踪器包括不易在CT影像中成像的刚性支架，所述患者示踪器的刚性支架的一端固定在患者的非种植牙牙齿上，另一端装有9个钛珠和4个光学定位球。
一种种植牙机器人导航系统及方法
技术领域
[0001] 本发明属于手术机器人导航定位技术领域，具体涉及一种种植牙机器人导航系统及方法。
背景技术
[0002] 传统的种植牙手术方法为：手术开始前，需要透视扫描植牙的情况，从而制定手术方案；然后，手术过程中，医生手持手术刀，凭借经验进行手术。此种种植牙手术方法具有以下问题：医生手持手术刀进行手术，操作精度和稳定性水平较低，无法保证手术的精度。
发明内容
[0003] 针对现有技术存在的缺陷，本发明提供一种种植牙机器人导航系统及方法，可有效解决上述问题。
[0004] 本发明采用的技术方案如下：
[0005] 本发明提供一种种植牙机器人导航系统，包括：控制器、机械臂、手术刀具、光学定位仪、工具示踪器、患者示踪器和刀具标定器；所述手术刀具和所述工具示踪器固定安装在所述机械臂的末端；所述患者示踪器固定在患者的非种植牙牙齿上；
[0006] 其中：所述刀具标定器包括4个光学定位球和一个标定孔；所述刀具标定器的标定孔套在所述手术刀具的末端后，手术刀具末端顶点与标定孔顶端中心点重合，手术刀具轴线方向与标定孔轴线方向重合。
[0007] 优选的，所述机械臂是6自由度协作机械臂，设于机械臂基座上面；所述机械臂基座形成机械臂基座坐标系{B}，机械臂的末端机械接口称为TCP，形成机械臂TCP坐标系{E}。
[0008] 优选的，所述工具示踪器包括刚性支架，所述刚性支架上固定安装有4个光学定位球。
[0009] 优选的，所述患者示踪器包括不易在CT影像中成像的刚性支架，所述患者示踪器的刚性支架的一端固定在患者的非种植牙牙齿上，另一端装有9个钛珠和4个光学定位球。
[0010] 本发明还提供一种种植牙机器人导航系统的方法，包括以下步骤：
[0011] 步骤S1，预先标定得到工具示踪器坐标系{T}到机械臂TCP坐标系{E}的转换矩阵TTE：
[0012] 步骤S1.1，控制机械臂TCP随机运动n个位姿，得到以下n个方程：
[0013] OTB＝OTT1·TTE·E1TB
[0014] OTB＝OTT2·TTE·E2TB
[0015] ...
[0016] OTB＝OTTn·TTE·EnTB
[0017] 其中：
[0018] OTB：代表光学定位仪坐标系{O}到机械臂基座坐标系{B}的转换矩阵，由于机械臂OTCP运动过程中，机械臂基座与光学定位仪均固定不动，所以 TB始终不变，为已知固定值；
[0019] OTTi，i＝1,2,...,n：代表机械臂TCP随机运动到第i个位姿时，光学定位仪坐标系{O}到工具示踪器坐标系{T}的转换矩阵，通过以下方法获得：当机械臂TCP随机运动到第i个位姿时,此时，通过光学定位仪，测量得到工具示踪器上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；再结合已知的工具示踪器上每个光学定位球在工具示踪器坐O标系{T}下的三维位置坐标，得到转换矩阵 TTi；
[0020] TTE：代表工具示踪器坐标系{T}到机械臂TCP坐标系{E}的转换矩阵TTE，在机械臂TTCP运动过程中，TE固定不变，为待求值；
[0021] EiTB，i＝1,2,...,n：代表机械臂TCP随机运动到第i个位姿时，机械臂TCP坐标系{E}到机械臂基座坐标系{B}的转换矩阵，为已知值；
[0022] 步骤S1.2，对n个方程形成的方程组，采用最小二乘方法求解，得到工具示踪器坐T标系{T}到机械臂TCP坐标系{E}的转换矩阵 TE；
[0023] 步骤S2，预先标定得到手术刀具与工具示踪器之间相对位姿关系，即：得到手术刀具末端顶点Q在工具示踪器坐标系{T}的三维位置坐标QT，以及手术刀具轴线方向在工具示踪器坐标系{T}的方向向量
[0024] 步骤S2.1，将刀具标定器套在手术刀具的末端，使得手术刀具末端顶点与刀具标定器的标定孔顶端中心点重合，使手术刀具轴线方向与刀具标定器的标定孔轴线方向重合；
[0025] 步骤S2.2，通过光学定位仪，获得工具示踪器坐标系{T}到光学定位仪坐标系{O}T K的转换矩阵 TO，以及刀具标定器坐标系{K}到光学定位仪坐标系{O}的转换矩阵 TO；
[0026] 具体方法为：通过光学定位仪，测量得到工具示踪器上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；再结合已知的工具示踪器上每个光学定位球在工具示T踪器坐标系{T}下的三维位置坐标，得到转换矩阵 TO；
[0027] 通过光学定位仪，测量得到刀具标定器上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；再结合已知的刀具标定器上每个光学定位球在刀具标定器坐标系K{K}下的三维位置坐标，得到转换矩阵 TO；
[0028] 步骤S2.3，采用下式，得到刀具标定器坐标系{K}到工具示踪器坐标系{T}的转换K矩阵 TT：
[0029] KTT＝KTO·(TTO)‑1
[0030] 步骤S2.4，获得刀具标定器的标定孔顶端中心点在刀具标定器坐标系{K}中的三维位置坐标PK，以及刀具标定器的标定孔轴线方向在刀具标定器坐标系{K}中的方向向量[0031] 根据下式，将刀具标定器的标定孔顶端中心点在刀具标定器坐标系{K}中的三维位置坐标PK，转换到工具示踪器坐标系{T}中，得到标定孔顶端中心点在工具示踪器坐标系{T}中的三维位置坐标PT：
[0032]
[0033] 根据下式，将刀具标定器的标定孔轴线方向在刀具标定器坐标系{K}中的方向向量 转换到工具示踪器坐标系{T}中，得到标定孔轴线方向在工具示踪器坐标系{T}中的方向向量
[0034]
[0035] 其中：
[0036] I为3×3的单位矩阵；
[0037] 步骤S2.5，由于标定孔顶端中心点在工具示踪器坐标系{T}中的三维位置坐标PT，等于手术刀具末端顶点Q在工具示踪器坐标系{T}的三维位置坐标QT；标定孔轴线方向在工具示踪器坐标系{T}中的方向向量 等于手术刀具轴线方向在工具示踪器坐标系{T}的方向向量 因此标记得到手术刀具与工具示踪器之间相对位姿关系；
[0038] 步骤S3，获得CT扫描机坐标系{C}到光学定位仪坐标系{O}的转换矩阵CTO：
[0039] 步骤S3.1，CT扫描机对患者示踪器进行扫描成像，使患者示踪器的钛珠成像在CT影像中；对CT影像进行分析，得到钛珠在CT扫描机坐标系{C}的三维位置坐标；
[0040] 步骤S3.2，通过光学定位仪，测量得到患者示踪器的上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；
[0041] 步骤S3.3，又由于患者示踪器的钛珠在患者示踪器坐标系{S}的三维位置坐标，以及患者示踪器的每个光学定位球在患者示踪器坐标系{S}的三维位置坐标均为已知固定值，因此，结合钛珠在CT扫描机坐标系{C}的三维位置坐标，以及患者示踪器的上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标，得到CT扫描机坐标系{C}到光学定位仪C坐标系{O}的转换矩阵 TO；
[0042] 步骤4，手术开始前，患者佩戴患者示踪器进行CT扫描，获得CT扫描机坐标系{C}下的种植牙影像，通过对种植牙影像进行分析，规划得到在CT扫描机坐标系{C}下的种植牙路径的进入点AC和路径方向
[0043] 采用下式，将CT扫描机坐标系{C}下的种植牙路径的进入点AC和路径方向 转换到光学定位仪坐标系{O}下，得到光学定位仪坐标系{O}下的种植牙路径的进入点AO和路径方向 该光学定位仪坐标系{O}下的种植牙路径的进入点AO和路径方向 同时为手术刀具目标位姿；
[0044]
[0045]
[0046] 其中：
[0047] I为3×3的单位矩阵；
[0048] 步骤5，在导航过程中，采用以下方法，通过光学定位仪，实时测量得到手术刀具实时位姿；其中，所述手术刀具实时位姿包括手术刀具末端顶点Q在光学定位仪坐标系{O}下的三维位置坐标QO，以及手术刀具轴线方向在光学定位仪坐标系{O}下的方向向量[0049] 通过比较手术刀具实时位姿与手术刀具目标位姿，得到手术刀具在光学定位仪坐标系{O}下的位姿偏差；根据手术刀具在光学定位仪坐标系{O}下的位姿偏差，生成对机械臂控制指令，控制机械臂带动手术刀具朝向手术刀具目标位姿运动，使手术刀具实时位姿不断逼近手术刀具目标位姿。
[0050] 优选的，步骤5中，根据手术刀具位姿偏差，生成对机械臂控制指令，控制机械臂带动手术刀具朝向手术刀具目标位姿运动，具体为：
[0051] 步骤5.1，手术刀具在光学定位仪坐标系{O}下的位姿偏差，包括三维位置坐标偏差ΔQO和方向向量偏差
[0052] 步骤5.2，获取光学定位仪坐标系{O}到刀具标定器坐标系{K}的转换矩阵OTK；其中，刀具标定器坐标系{K}与刀具坐标系完全相同；因此得到光学定位仪坐标系{O}到刀具O坐标系{F}的转换矩阵 TF；
[0053] 根据光学定位仪坐标系{O}到刀具坐标系{F}的转换矩阵OTF，将手术刀具在光学定位仪坐标系{O}下的位姿偏差，转换为手术刀具在刀具坐标系{F}下的位姿偏差；
[0054] 步骤5.3，根据步骤S2预先标定得到的手术刀具与工具示踪器之间相对位姿关系，将手术刀具在刀具坐标系{F}下的位姿偏差，转换为手术刀具在工具示踪器坐标系{T}下的位姿偏差；
[0055] 步骤5.4，根据步骤S1预先标定得到的工具示踪器坐标系{T}到机械臂TCP坐标系T{E}的转换矩阵TE，将手术刀具在工具示踪器坐标系{T}下的位姿偏差，转换为手术刀具在机械臂TCP坐标系{E}下的位姿偏差；
[0056] 步骤5.5，根据手术刀具在机械臂TCP坐标系{E}下的位姿偏差，生成对机械臂控制指令，控制机械臂带动手术刀具朝向手术刀具目标位姿运动。
[0057] 优选的，还包括：
[0058] 步骤6，在导航过程中，实时更新步骤4得到的手术刀具目标位姿，即：光学定位仪坐标系{O}下的种植牙路径的进入点AO和路径方向
[0059] 优选的，步骤6具体为：
[0060] 步骤6.1，通过光学定位仪，实时测量得到患者示踪器上每个光学定位球在光学定位仪坐标系{O}下的实时位姿；
[0061] 步骤6.2，根据光学定位仪坐标系{O}到患者示踪器坐标系{S}的转换矩阵，将患者示踪器上每个光学定位球在光学定位仪坐标系{O}下的实时位姿，转换为患者示踪器上每个光学定位球在患者示踪器坐标系{S}下的实时位姿；
[0062] 步骤6.3，根据患者示踪器坐标系{S}到CT扫描机坐标系{C}的转换矩阵，将患者示踪器上每个光学定位球在患者示踪器坐标系{S}下的实时位姿，转换为患者示踪器上每个光学定位球在CT扫描机坐标系{C}下的实时位姿；
[0063] 步骤6.4，由于患者病患牙齿与患者示踪器的相对位姿关系固定不变，因此，将患者示踪器上每个光学定位球在CT扫描机坐标系{C}下的实时位姿，转换为患者病患牙齿在CT扫描机坐标系{C}下的实时位姿；
[0064] 患者病患牙齿在CT扫描机坐标系{C}下的实时位姿，即为种植牙路径的进入点当前值和路径方向当前值；比较种植牙路径的进入点当前值和路径方向当前值，与步骤4规划得到在CT扫描机坐标系{C}下的种植牙路径的进入点AC和路径方向 如果一致，则表明手术过程中患者头部位置没有发生变化，不需要更新手术刀具目标位姿；否则，采用患者病患牙齿在CT扫描机坐标系{C}下的实时位姿，更新手术刀具目标位姿。
[0065] 本发明提供的一种种植牙机器人导航系统及方法具有以下优点：
[0066] 本发明提供的一种种植牙机器人导航系统及方法，机器人导航系统利用术前CT扫描影像规划种植牙手术路径进行导航，并在导航过程中，对种植牙手术路径进行实时修正，另外，利用光学定位仪跟踪手术刀具的位置和姿态，从而引导机械臂运动，带动手术刀具到达种植牙路径，可实现手术刀具位置和方向的精确定位，具有定位准确、工作稳定的优点，可保证手术的精度，有效降低对医生操作的要求，并降低医生的工作强度。
附图说明
[0067] 图1为本发明提供的种植牙机器人导航系统的结构图；
[0068] 图2为工具示踪器示意图；
[0069] 图3为患者示踪器示意图；
[0070] 图4为刀具标定器示意图。
[0071] 其中：
[0072] 1代表工具示踪器；2代表手术刀具；3代表机械臂；4代表患者示踪器；5代表光学定位仪；6代表刀具标定器；
[0073] 4‑1代表患者示踪器上的光学定位球；
[0074] 4‑2代表患者示踪器上的钛珠；
[0075] 6‑1代表刀具标定器上的光学定位球；
[0076] 6‑2代表刀具标定器上的标定孔。
具体实施方式
[0077] 为了使本发明所解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
[0078] 本发明提供一种种植牙机器人导航系统及方法，机器人导航系统利用术前CT扫描影像规划种植牙手术路径进行导航，并在导航过程中，对种植牙手术路径进行实时修正，另外，利用光学定位仪跟踪手术刀具的位置和姿态，从而引导机械臂运动，带动手术刀具到达种植牙路径，可实现手术刀具位置和方向的精确定位，具有定位准确、工作稳定的优点，可保证手术的精度，有效降低对医生操作的要求，并降低医生的工作强度。
[0079] 参考图1，本发明提供一种种植牙机器人导航系统，包括：控制器、机械臂、手术刀具、光学定位仪、工具示踪器、患者示踪器和刀具标定器；所述手术刀具和所述工具示踪器固定安装在所述机械臂的末端；所述患者示踪器固定在患者的非种植牙牙齿上；
[0080] 参考图4，所述刀具标定器包括4个光学定位球和一个标定孔；所述刀具标定器的标定孔套在所述手术刀具的末端后，手术刀具末端顶点与标定孔顶端中心点重合，手术刀具轴线方向与标定孔轴线方向重合。光学定位球在刀具标定器坐标系{K}下的坐标、标定孔顶端中心点在刀具标定器坐标系{K}下的坐标PK和标定孔轴线方向在刀具标定器坐标系{K}下的方向向量 事先经过标定。
[0081] 所述机械臂是6自由度协作机械臂，设于机械臂基座上面；所述机械臂基座形成机械臂基座坐标系{B}，机械臂的末端机械接口称为TCP，形成机械臂TCP坐标系{E}。通过控制器可实时读取机械臂TCP坐标系{E}与机械臂基座坐标系{B}之间的转换关系，也即机械臂TCP的位姿。其中，在机械臂运动过程中，机械臂基座坐标系{B}固定不变，机械臂TCP坐标系{E}随机械臂运动而变化。
[0082] 参考图2，所述工具示踪器包括刚性支架，所述刚性支架上固定安装有4个光学定位球。4个光学定位球在工具示踪器坐标系{T}下的坐标事先经过标定。在机械臂运动过程中，工具示踪器坐标系{T}随机械臂运动而变化。
[0083] 参考图3，所述患者示踪器包括不易在CT影像中成像的刚性支架，所述患者示踪器的刚性支架的一端固定在患者的非种植牙牙齿上，另一端装有9个钛珠和4个光学定位球。
[0084] 在一实施例中，患者示踪器上的钛珠可在CT影像中成像，且根据钛珠可建立患者示踪器坐标系{S}，4个光学定位球在患者示踪器坐标系{S}下的坐标事先经过标定；通过CT扫描机对钛珠的测量和光学定位仪对光学定位球的测量，可通过坐标系转换计算，得到CTC扫描机坐标系{C}与光学定位仪坐标系{O}之间的转换矩阵 TO。
[0085] 在一实施例中，所述光学定位仪通过测量可获得光学定位球在光学定位仪坐标系{O}下的坐标，则光学定位仪通过对工具示踪器、患者示踪器和刀具标定器上光学定位球的测量，再经过坐标系转换计算，可获得工具示踪器坐标系{T}与光学定位仪坐标系{O}之间的转换矩阵、患者示踪器坐标系{S}与光学定位仪坐标系{O}之间的转换矩阵，以及刀具标定器坐标系{K}与光学定位仪坐标系{O}之间的转换矩阵。
[0086] 本发明还提供一种种植牙机器人导航系统的方法，包括以下步骤：
[0087] 步骤S1，预先标定得到工具示踪器坐标系{T}到机械臂TCP坐标系{E}的转换矩阵TTE，即机械臂TCP与工具示踪器之间的相对位姿关系。
[0088] 步骤S1.1，控制机械臂TCP随机运动n个位姿，例如，10个位姿，得到以下n个方程：
[0089] OTB＝OTT1·TTE·E1TB
[0090] OTB＝OTT2·TTE·E2TB
[0091] ...
[0092] OTB＝OTTn·TTE·EnTB
[0093] 其中：
[0094] OTB：代表光学定位仪坐标系{O}到机械臂基座坐标系{B}的转换矩阵，由于机械臂OTCP运动过程中，机械臂基座与光学定位仪均固定不动，所以 TB始终不变，为已知固定值；
[0095] OTTi，i＝1,2,...,n：代表机械臂TCP随机运动到第i个位姿时，光学定位仪坐标系{O}到工具示踪器坐标系{T}的转换矩阵，通过以下方法获得：当机械臂TCP随机运动到第i个位姿时,此时，通过光学定位仪，测量得到工具示踪器上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；再结合已知的工具示踪器上每个光学定位球在工具示踪器坐O标系{T}下的三维位置坐标，得到转换矩阵 TTi；
[0096] TTE：代表工具示踪器坐标系{T}到机械臂TCP坐标系{E}的转换矩阵TTE，在机械臂TTCP运动过程中，TE固定不变，为待求值；
[0097] EiTB，i＝1,2,...,n：代表机械臂TCP随机运动到第i个位姿时，机械臂TCP坐标系{E}到机械臂基座坐标系{B}的转换矩阵，为已知值，可由控制器读取得到；
[0098] 步骤S1.2，对n个方程形成的方程组，采用最小二乘方法求解，得到工具示踪器坐T标系{T}到机械臂TCP坐标系{E}的转换矩阵 TE；
[0099] 步骤S2，预先标定得到手术刀具与工具示踪器之间相对位姿关系，即：得到手术刀具末端顶点Q在工具示踪器坐标系{T}的三维位置坐标QT，以及手术刀具轴线方向在工具示踪器坐标系{T}的方向向量
[0100] 步骤S2.1，将刀具标定器套在手术刀具的末端，使得手术刀具末端顶点与刀具标定器的标定孔顶端中心点重合，使手术刀具轴线方向与刀具标定器的标定孔轴线方向重合；
[0101] 步骤S2.2，通过光学定位仪，获得工具示踪器坐标系{T}到光学定位仪坐标系{O}T K的转换矩阵 TO，以及刀具标定器坐标系{K}到光学定位仪坐标系{O}的转换矩阵 TO；
[0102] 具体方法为：通过光学定位仪，测量得到工具示踪器上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；再结合已知的工具示踪器上每个光学定位球在工具示T踪器坐标系{T}下的三维位置坐标，得到转换矩阵 TO；
[0103] 通过光学定位仪，测量得到刀具标定器上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；再结合已知的刀具标定器上每个光学定位球在刀具标定器坐标系K{K}下的三维位置坐标，得到转换矩阵 TO；
[0104] 步骤S2.3，采用下式，得到刀具标定器坐标系{K}到工具示踪器坐标系{T}的转换K矩阵 TT：
[0105] KTT＝KTO·(TTO)‑1
[0106] 步骤S2.4，获得刀具标定器的标定孔顶端中心点在刀具标定器坐标系{K}中的三维位置坐标PK，以及刀具标定器的标定孔轴线方向在刀具标定器坐标系{K}中的方向向量[0107] 根据下式，将刀具标定器的标定孔顶端中心点在刀具标定器坐标系{K}中的三维位置坐标PK，转换到工具示踪器坐标系{T}中，得到标定孔顶端中心点在工具示踪器坐标系{T}中的三维位置坐标PT：
[0108]
[0109] 根据下式，将刀具标定器的标定孔轴线方向在刀具标定器坐标系{K}中的方向向量 转换到工具示踪器坐标系{T}中，得到标定孔轴线方向在工具示踪器坐标系{T}中的方向向量
[0110]
[0111] 其中：
[0112] I为3×3的单位矩阵；
[0113] 步骤S2.5，由于标定孔顶端中心点在工具示踪器坐标系{T}中的三维位置坐标PT，等于手术刀具末端顶点Q在工具示踪器坐标系{T}的三维位置坐标QT；标定孔轴线方向在工具示踪器坐标系{T}中的方向向量 等于手术刀具轴线方向在工具示踪器坐标系{T}的方向向量 因此标记得到手术刀具与工具示踪器之间相对位姿关系；
[0114] 步骤S3，获得CT扫描机坐标系{C}到光学定位仪坐标系{O}的转换矩阵CTO：
[0115] 步骤S3.1，CT扫描机对患者示踪器进行扫描成像，使患者示踪器的钛珠成像在CT影像中；对CT影像进行分析，得到钛珠在CT扫描机坐标系{C}的三维位置坐标；
[0116] 步骤S3.2，通过光学定位仪，测量得到患者示踪器的上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标；
[0117] 步骤S3.3，又由于患者示踪器的钛珠在患者示踪器坐标系{S}的三维位置坐标，以及患者示踪器的每个光学定位球在患者示踪器坐标系{S}的三维位置坐标均为已知固定值，因此，结合钛珠在CT扫描机坐标系{C}的三维位置坐标，以及患者示踪器的上每个光学定位球在光学定位仪坐标系{O}下的三维位置坐标，得到CT扫描机坐标系{C}到光学定位仪C坐标系{O}的转换矩阵 TO；
[0118] 步骤4，手术开始前，患者佩戴患者示踪器进行CT扫描，获得CT扫描机坐标系{C}下的种植牙影像，通过对种植牙影像进行分析，规划得到在CT扫描机坐标系{C}下的种植牙路径的进入点AC和路径方向
[0119] 采用下式，将CT扫描机坐标系{C}下的种植牙路径的进入点AC和路径方向 转换到光学定位仪坐标系{O}下，得到光学定位仪坐标系{O}下的种植牙路径的进入点AO和路径方向 该光学定位仪坐标系{O}下的种植牙路径的进入点AO和路径方向 同时为手术刀具目标位姿；
[0120]
[0121]
[0122] 其中：
[0123] I为3×3的单位矩阵；
[0124] 步骤5，在导航过程中，采用以下方法，通过光学定位仪，实时测量得到手术刀具实时位姿；其中，所述手术刀具实时位姿包括手术刀具末端顶点Q在光学定位仪坐标系{O}下的三维位置坐标QO，以及手术刀具轴线方向在光学定位仪坐标系{O}下的方向向量[0125] 通过比较手术刀具实时位姿与手术刀具目标位姿，得到手术刀具在光学定位仪坐标系{O}下的位姿偏差；根据手术刀具在光学定位仪坐标系{O}下的位姿偏差，生成对机械臂控制指令，控制机械臂带动手术刀具朝向手术刀具目标位姿运动，使手术刀具实时位姿不断逼近手术刀具目标位姿。
[0126] 作为一种具体实施例，本步骤中，根据手术刀具位姿偏差，生成对机械臂控制指令，控制机械臂带动手术刀具朝向手术刀具目标位姿运动，具体为：
[0127] 步骤5.1，手术刀具在光学定位仪坐标系{O}下的位姿偏差，包括三维位置坐标偏差ΔQO和方向向量偏差
[0128] 步骤5.2，获取光学定位仪坐标系{O}到刀具标定器坐标系{K}的转换矩阵OTK；其中，刀具标定器坐标系{K}与刀具坐标系完全相同；因此得到光学定位仪坐标系{O}到刀具O坐标系{F}的转换矩阵 TF；
[0129] 根据光学定位仪坐标系{O}到刀具坐标系{F}的转换矩阵OTF，将手术刀具在光学定位仪坐标系{O}下的位姿偏差，转换为手术刀具在刀具坐标系{F}下的位姿偏差；
[0130] 步骤5.3，根据步骤S2预先标定得到的手术刀具与工具示踪器之间相对位姿关系，将手术刀具在刀具坐标系{F}下的位姿偏差，转换为手术刀具在工具示踪器坐标系{T}下的位姿偏差；
[0131] 步骤5.4，根据步骤S1预先标定得到的工具示踪器坐标系{T}到机械臂TCP坐标系T{E}的转换矩阵TE，将手术刀具在工具示踪器坐标系{T}下的位姿偏差，转换为手术刀具在机械臂TCP坐标系{E}下的位姿偏差；
[0132] 步骤5.5，根据手术刀具在机械臂TCP坐标系{E}下的位姿偏差，生成对机械臂控制指令，控制机械臂带动手术刀具朝向手术刀具目标位姿运动。
[0133] 另外，还包括：
[0134] 步骤6，在导航过程中，实时更新步骤4得到的手术刀具目标位姿，即：光学定位仪坐标系{O}下的种植牙路径的进入点AO和路径方向
[0135] 8、根据权利要求7所述的种植牙机器人导航系统的方法，其特征在于，步骤6具体为：
[0136] 步骤6.1，通过光学定位仪，实时测量得到患者示踪器上每个光学定位球在光学定位仪坐标系{O}下的实时位姿；
[0137] 步骤6.2，根据光学定位仪坐标系{O}到患者示踪器坐标系{S}的转换矩阵，将患者示踪器上每个光学定位球在光学定位仪坐标系{O}下的实时位姿，转换为患者示踪器上每个光学定位球在患者示踪器坐标系{S}下的实时位姿；
[0138] 步骤6.3，根据患者示踪器坐标系{S}到CT扫描机坐标系{C}的转换矩阵，将患者示踪器上每个光学定位球在患者示踪器坐标系{S}下的实时位姿，转换为患者示踪器上每个光学定位球在CT扫描机坐标系{C}下的实时位姿；
[0139] 步骤6.4，由于患者病患牙齿与患者示踪器的相对位姿关系固定不变，因此，将患者示踪器上每个光学定位球在CT扫描机坐标系{C}下的实时位姿，转换为患者病患牙齿在CT扫描机坐标系{C}下的实时位姿；
[0140] 患者病患牙齿在CT扫描机坐标系{C}下的实时位姿，即为种植牙路径的进入点当前值和路径方向当前值；比较种植牙路径的进入点当前值和路径方向当前值，与步骤4规划得到在CT扫描机坐标系{C}下的种植牙路径的进入点AC和路径方向 如果一致，则表明手术过程中患者头部位置没有发生变化，不需要更新手术刀具目标位姿；否则，采用患者病患牙齿在CT扫描机坐标系{C}下的实时位姿，更新手术刀具目标位姿。
[0141] 因此，本发明中，在手术过程中，如果患者头部位置发生变化，可根据患者种植牙齿的实时位姿更新光学定位仪坐标系{O}下的种植牙路径，重新计算刀具目标位姿。
[0142] 本发明提供一种种植牙机器人导航系统及方法，通过坐标系转换将种植牙路径和手术刀具的位姿都转换到光学定位仪坐标系下进行导航，并引导机械臂带动手术刀具运动，可提供高效准确的种植牙手术导航。
[0143] 以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视本发明的保护范围。
