[發(fā)明專利]一種種植牙機器人導(dǎo)航定位方法

有效

• 申請?zhí)枺?/td>: 202110698441.3
• 申請日: 2021-06-23
• 公開（公告）號: CN113400325B
• 公開（公告）日: 2022-03-25
• 發(fā)明（設(shè)計）人: 汪陽;房鶴;孫孝男;崔小飛;田忠正
• 申請（專利權(quán)）人: 四川鋒準(zhǔn)機器人科技有限公司
• 主分類號: B25J11/00
• 分類號: B25J11/00;B25J9/16;A61C8/00;A61B34/20
• 代理公司: 北京市盛峰律師事務(wù)所 11337
• 代理人: 席小東

1.一種種植牙機器人導(dǎo)航定位方法，其特征在于，包括以下步驟：

步驟S1，種植牙患者的非種植牙牙齒佩戴患者示蹤器，使患者示蹤器和種植牙目標(biāo)位置之間的相對位姿保持不變；

步驟S2，采用CT掃描儀對包含患者示蹤器和種植牙目標(biāo)位置的牙齒區(qū)域進行CT掃描，得到CT掃描圖像；

步驟S3，在CT掃描圖像中規(guī)劃種植牙路徑，確定CT坐標(biāo)系下目標(biāo)路徑的進入點位置A_C和進入點方向其中，進入點位置A_C和進入點方向形成CT坐標(biāo)系下的進入點目標(biāo)位姿；

步驟S4，種植牙機器人包括機械臂基座、多關(guān)節(jié)機械臂、手術(shù)刀具和工具示蹤器；其中，機械臂基座固定設(shè)置，位置保持不變；機械臂基座的上面安裝多關(guān)節(jié)機械臂；多關(guān)節(jié)機械臂的末端安裝手術(shù)刀具和工具示蹤器；在手術(shù)過程中，工具示蹤器、手術(shù)刀具和多關(guān)節(jié)機械臂末端之間的相對位姿保持不變，即：工具示蹤器坐標(biāo)系、手術(shù)刀具坐標(biāo)系Tip和機械臂末端TCP坐標(biāo)系之間的相對位姿保持不變，但是，在手術(shù)刀具運動過程中，工具示蹤器坐標(biāo)系、手術(shù)刀具坐標(biāo)系Tip和機械臂末端TCP坐標(biāo)系會實時發(fā)生變化，而機械臂基座坐標(biāo)系保持不變；

步驟S5，將步驟S3確定的CT坐標(biāo)系下目標(biāo)路徑的進入點位置A_C和進入點方向轉(zhuǎn)換為機械臂基座坐標(biāo)系下的目標(biāo)路徑的進入點位置A_B和進入點方向

其中：

I為3行3列的單位向量；

^CT_S為CT坐標(biāo)系到患者示蹤器坐標(biāo)系的轉(zhuǎn)換矩陣；

^ST_O為患者示蹤器坐標(biāo)系到光學(xué)定位儀坐標(biāo)系的轉(zhuǎn)換矩陣；

^OT_T為光學(xué)定位儀坐標(biāo)系到工具示蹤器坐標(biāo)系的轉(zhuǎn)換矩陣；

^TT_E為工具示蹤器坐標(biāo)系到機械臂末端TCP坐標(biāo)系的轉(zhuǎn)換矩陣；

^ET_B為機械臂末端TCP坐標(biāo)系到機械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣；

步驟S6，建立刀具目標(biāo)位姿坐標(biāo)系Tip′到機械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣^Tip′T_B：

步驟S6.1，建立手術(shù)刀具目標(biāo)位姿坐標(biāo)系Tip′，包括：確定坐標(biāo)原點o，確定x軸、y軸和z軸；

坐標(biāo)原點o：以機械臂基座坐標(biāo)系下的目標(biāo)路徑的進入點位置A_B為坐標(biāo)原點；

x軸：以機械臂基座坐標(biāo)系下的目標(biāo)路徑的進入點方向為x軸；

y軸：以機械臂末端TCP坐標(biāo)系的z軸在機械臂基座坐標(biāo)系下的方向向量與的叉乘結(jié)果，作為y軸在機械臂基座坐標(biāo)系下的方向向量

z軸：以與的叉乘結(jié)果，作為z軸在機械臂基座坐標(biāo)系下的方向向量

步驟S6.2，構(gòu)建得到刀具目標(biāo)位姿坐標(biāo)系Tip′到機械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣^Tip′T_B：

步驟S7，建立手術(shù)刀具坐標(biāo)系Tip到機械臂末端TCP坐標(biāo)系的轉(zhuǎn)換矩陣^TipT_E：

步驟S7.1，建立手術(shù)刀具坐標(biāo)系Tip，包括：確定坐標(biāo)原點o₁，確定x₁軸、y₁軸和z₁軸；

坐標(biāo)原點o₁：以刀具尖端在機械臂末端TCP坐標(biāo)系下的坐標(biāo)P_E為坐標(biāo)原點；

x₁軸：以刀具方向在機械臂末端TCP坐標(biāo)系下的方向向量作為x₁軸在機械臂末端TCP坐標(biāo)系下的方向向量

y₁軸：以機械臂末端TCP坐標(biāo)系的z軸與的叉乘結(jié)果，作為y₁軸在機械臂末端TCP坐標(biāo)系下的方向向量y_E；

z₁軸：以和y_E的叉乘結(jié)果，作為z₁軸在機械臂末端TCP坐標(biāo)系下的方向向量z_E；

步驟S7.2，構(gòu)建得到手術(shù)刀具坐標(biāo)系Tip到機械臂末端TCP坐標(biāo)系的轉(zhuǎn)換矩陣^TipT_E：

步驟S8，計算刀具到達目標(biāo)位姿時，機械臂末端TCP坐標(biāo)系到機械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣^E′T_B，方法為：

當(dāng)?shù)毒叩竭_目標(biāo)位姿時，手術(shù)刀具坐標(biāo)系Tip和刀具目標(biāo)位姿坐標(biāo)系Tip′重合，因此，手術(shù)刀具坐標(biāo)系Tip到機械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣^TipT_B，等于刀具目標(biāo)位姿坐標(biāo)系Tip′到機械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣^Tip′T_B，由此有以下關(guān)系式：

^TipT_B＝^Tip′T_B

又由于手術(shù)刀具坐標(biāo)系Tip到機械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣^TipT_B為：

^TipT_B＝^TipT_E·^ET_B

因此，刀具到達目標(biāo)位姿時，機械臂末端TCP坐標(biāo)系到機械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣^E′T_B為：

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

步驟S9，根據(jù)步驟S8確定的刀具到達目標(biāo)位姿時，機械臂末端TCP坐標(biāo)系到機械臂基座坐標(biāo)系的轉(zhuǎn)換矩陣^E′T_B，生成對種植牙機器人的機械臂各關(guān)節(jié)的控制指令，從而使種植牙機器人的機械臂各關(guān)節(jié)發(fā)生運動；

步驟S10，當(dāng)種植牙機器人的機械臂各關(guān)節(jié)運動結(jié)束后，計算刀具尖端在機械臂基座坐標(biāo)系下的位置坐標(biāo)A_B，以及刀具方向在機械臂基座坐標(biāo)系下的方向向量

將刀具尖端在機械臂基座坐標(biāo)系下的位置坐標(biāo)A_B與步驟S5得到的機械臂基座坐標(biāo)系下的目標(biāo)路徑的進入點位置A_B比較，得到位置坐標(biāo)誤差；

將刀具方向在機械臂基座坐標(biāo)系下的方向向量與步驟S5得到的機械臂基座坐標(biāo)系下的目標(biāo)路徑的進入點方向比較，得到方向向量誤差；

步驟S11，判斷位置坐標(biāo)誤差和方向向量誤差是否均小于設(shè)定閾值，如果是，則導(dǎo)航結(jié)束，表明種植牙機器人的刀具已運動到目標(biāo)位姿；否則，重復(fù)執(zhí)行步驟S9-步驟S11；

其中，所述患者示蹤器包括9個鈦珠、4個定位球和不易在CT掃描圖像中成像的剛性支架；

其中，每個鈦珠和定位球在患者示蹤器坐標(biāo)系下的坐標(biāo)已知，在導(dǎo)航定位過程中患者始終佩戴患者示蹤器；

其中，所述工具示蹤器包括剛性支架和4個定位球，固定安裝在機械臂末端，每個定位球在工具示蹤器坐標(biāo)系下的坐標(biāo)已知；

其中，步驟S5中，各參數(shù)通過以下方式獲得：

^CT_S：通過CT掃描儀對患者示蹤器上的鈦珠坐標(biāo)的測量和計算得到；

^ST_O：由光學(xué)定位儀對患者示蹤器上的定位球坐標(biāo)的測量和計算得到；

^OT_T：由光學(xué)定位儀對工具示蹤器上的定位球坐標(biāo)的測量和計算得到；

^TT_E：由手眼標(biāo)定得到；

^ET_B：由機械臂控制器直接讀取到。