[發(fā)明專利]一種種植牙機器人末端夾持器幾何尺寸的標(biāo)定方法在審
| 申請?zhí)枺?/td> | 202210128908.5 | 申請日： | 2022-02-11 |
| 公開（公告）號： | CN114454167A | 公開（公告）日： | 2022-05-10 |
| 發(fā)明（設(shè)計）人： | 汪陽;房鶴;祝勝山;崔小飛;田忠正 | 申請（專利權(quán)）人： | 四川鋒準(zhǔn)機器人科技有限公司 |
| 主分類號： | B25J9/16 | 分類號： | B25J9/16;B25J19/00 |
| 代理公司： | 北京市盛峰律師事務(wù)所 11337 | 代理人： | 席小東 |
| 地址： | 641300 四川省資陽市雁*** | 國省代碼： | 四川;51 |
| 權(quán)利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關(guān)鍵詞： | 一種 種植 機器人 末端 夾持 幾何 尺寸 標(biāo)定 方法 | ||
1.一種種植牙機器人末端夾持器幾何尺寸的標(biāo)定方法，其特征在于，包括以下步驟：
步驟1，種植牙機器人未端安裝實際夾持器單元；構(gòu)建與實際夾持器單元結(jié)構(gòu)相同的理論夾持器模型；
所述實際夾持器單元和所述理論夾持器模型的結(jié)構(gòu)相同，均包括：末端法蘭盤(1)、第一連桿(2)、第二連桿(3)、夾持器手機彎頭(4)、針筒末端點(5)、第一關(guān)節(jié)(7)和第二關(guān)節(jié)(8)；
所述末端法蘭盤(1)的一端用于與種植牙機器人安裝；所述末端法蘭盤(1)的另一端與所述第一連桿(2)的一端連接；所述第一連桿(2)的另一端通過所述第一關(guān)節(jié)(7)與所述第二連桿(3)的一端鉸接；所述第二連桿(3)的另一端通過所述第二關(guān)節(jié)(8)與所述夾持器手機彎頭(4)的一端鉸接；所述夾持器手機彎頭(4)的另一端安裝所述針筒末端點(5)；
步驟2，采用以下方式，分別構(gòu)建TCP坐標(biāo)系{S0}、第一關(guān)節(jié)坐標(biāo)系{S1}、第二關(guān)節(jié)坐標(biāo)系{S2}和針筒末端點坐標(biāo)系{S3}：
步驟2.1，構(gòu)建TCP坐標(biāo)系{S0}，包括：確定坐標(biāo)原點o0，確定x0軸、y0軸和z0軸；
坐標(biāo)原點o0：以末端法蘭盤(1)的軸線中心位置作為坐標(biāo)原點；
z0軸：以末端法蘭盤(1)的軸線方向作為z0軸；
y0軸：以垂直向上方向作為y0軸；
x0軸：根據(jù)z0軸和y0軸，確定x0軸；
步驟2.2，構(gòu)建第一關(guān)節(jié)坐標(biāo)系{S1}，包括：確定坐標(biāo)原點o1，確定x1軸、y1軸和z1軸；
y1軸：以z0軸延長線的方向，作為y1軸；
z1軸：以第一關(guān)節(jié)(7)的軸線方向作為z1軸；
x1軸：根據(jù)y1軸和z1軸，確定x1軸；
坐標(biāo)原點o1：以y1軸和z1軸的交點位置，作為坐標(biāo)原點o1；
步驟2.3，構(gòu)建第二關(guān)節(jié)坐標(biāo)系{S2}，包括：確定坐標(biāo)原點o2，確定x2軸、y2軸和z2軸；
x2軸：以x1軸延長線的方向，作為x2軸；
z2軸：以第二關(guān)節(jié)(8)的軸線方向作為z2軸；
y2軸：根據(jù)x2軸和z2軸，確定y2軸；
坐標(biāo)原點o2：以x2軸和z2軸的交點位置，作為坐標(biāo)原點o2；
步驟2.4，構(gòu)建針筒末端點坐標(biāo)系{S3}，包括：確定坐標(biāo)原點o3，確定x3軸、y3軸和z3軸；
z3軸：以針筒末端點(5)的軸線方向作為z3軸；
坐標(biāo)原點o3：以針筒末端點(5)的底面中心位置作為坐標(biāo)原點o3；
y3軸：通過坐標(biāo)原點o3，與y2軸平行的方向作為y3軸；
x3軸：根據(jù)z3軸和y3軸，確定x3軸；
步驟3，建立從TCP坐標(biāo)系{S0}到針筒末端點坐標(biāo)系{S3}的齊次變換關(guān)系式：由此得到變換矩陣的表達(dá)式，是與轉(zhuǎn)角θ1和轉(zhuǎn)角θ2有關(guān)的矩陣；
其中：
第一關(guān)節(jié)(7)的轉(zhuǎn)角為θ1；第二關(guān)節(jié)(8)的轉(zhuǎn)角為θ2；
TCP坐標(biāo)系{S0}到第一關(guān)節(jié)坐標(biāo)系{S1}的轉(zhuǎn)換矩陣，為常數(shù)矩陣；
第一關(guān)節(jié)坐標(biāo)系{S1}到第二關(guān)節(jié)坐標(biāo)系{S2}的轉(zhuǎn)換矩陣，是與轉(zhuǎn)角θ1有關(guān)的矩陣；
第二關(guān)節(jié)坐標(biāo)系{S2}到針筒末端點坐標(biāo)系{S3}的轉(zhuǎn)換矩陣，是與轉(zhuǎn)角θ2有關(guān)的矩陣；
TCP坐標(biāo)系{S0}到針筒末端點坐標(biāo)系{S3}的轉(zhuǎn)換矩陣，是與轉(zhuǎn)角θ1和轉(zhuǎn)角θ2有關(guān)的矩陣；
步驟4，對實際夾持器單元的姿態(tài)進行控制，改變轉(zhuǎn)角θ1和轉(zhuǎn)角θ2的值，使實際夾持器單元呈現(xiàn)第一種姿態(tài)；
在第一種姿態(tài)下，通過光學(xué)定位儀(6)測量得到針筒軸線向量在光學(xué)定位儀坐標(biāo)系下的三方向分量值；然后，根據(jù)光學(xué)定位儀坐標(biāo)系和TCP坐標(biāo)系{S0}的轉(zhuǎn)換關(guān)系，將針筒軸線向量在光學(xué)定位儀坐標(biāo)系下的三方向分量值，轉(zhuǎn)換到TCP坐標(biāo)系{S0}下，得到針筒軸線向量在TCP坐標(biāo)系{S0}下三方向分量值，分別為：xT，yT，zT，由此得到針筒軸線向量在TCP坐標(biāo)系{S0}下的表達(dá)V_tipTCP＝{xT，yT，zT}；
步驟5，根據(jù)V_tipTCP＝{xT，yT，zT}和步驟3建立的變換矩陣的表達(dá)式，得到關(guān)于第一種姿態(tài)下轉(zhuǎn)角θ1和轉(zhuǎn)角θ2的方程組，求解該方程組，得到第一種姿態(tài)下轉(zhuǎn)角θ1的計算值和轉(zhuǎn)角θ2的計算值；將第一種姿態(tài)下轉(zhuǎn)角θ1的計算值表示為θ1(1)，將第一種姿態(tài)下轉(zhuǎn)角θ2的計算值表示為θ2(1)；
具體的，變換矩陣中，具有以下元素：針筒軸線向量在TCP坐標(biāo)系{S0}下xT方向表示式為：fx(θ1，θ2)，yT方向表示式為：fy(θ1，θ2)，zT方向表示式為：fz(θ1，θ2)；
由此得到以下方程組：
xT＝fx(θ1，θ2)
yT＝fy(θ1，θ2)
zT＝fz(θ1，θ2)
求解該方程組，得到第一種姿態(tài)下轉(zhuǎn)角θ1的計算值θ1(1)和轉(zhuǎn)角θ2的計算值θ2(1)；
步驟6，根據(jù)第一種姿態(tài)下轉(zhuǎn)角θ1的計算值θ1(1)和轉(zhuǎn)角θ2的計算值θ2(1)，對理論夾持器模型進行姿態(tài)調(diào)節(jié)，使理論夾持器模型變化到第一種姿態(tài)；
對呈第一種姿態(tài)的理論夾持器模型進行計算，得到針筒末端點(5)的底面中心在TCP坐標(biāo)系{S0}下的理論位置P1_tipidea；
對于第一種姿態(tài)的實際夾持器單元，通過光學(xué)定位儀測量得到光學(xué)定位儀坐標(biāo)系下針筒末端點(5)的底面中心的位置坐標(biāo)，再通過光學(xué)定位儀坐標(biāo)系和TCP坐標(biāo)系{S0}的轉(zhuǎn)換關(guān)系，得到針筒末端點(5)的底面中心在TCP坐標(biāo)系{S0}下的實際位置P1_tipactual；
根據(jù)下式，計算實際位置P1_tipactual和理論位置P1_tipidea的差值，得到本次誤差值errorX1：
errorX1＝P1_tipactual-P1_tipidea
步驟7，對實際夾持器單元的姿態(tài)進行控制，改變轉(zhuǎn)角θ1和轉(zhuǎn)角θ2的值，使實際夾持器單元呈現(xiàn)第二種姿態(tài)，循環(huán)執(zhí)行步驟4-步驟6，得到第二種姿態(tài)下轉(zhuǎn)角θ1的計算值θ1(2)、轉(zhuǎn)角θ2的計算值θ2(2)以及誤差值errorX2；
一共重復(fù)m次，得到m組關(guān)節(jié)角度和誤差值，即：
第一種姿態(tài)下轉(zhuǎn)角θ1的計算值θ1(1)、轉(zhuǎn)角θ2的計算值θ2(1)以及誤差值errorX1；
第二種姿態(tài)下轉(zhuǎn)角θ1的計算值θ1(2)、轉(zhuǎn)角θ2的計算值θ2(2)以及誤差值errorX2；
依此類推
第m種姿態(tài)下轉(zhuǎn)角θ1的計算值θ1(m)、轉(zhuǎn)角θ2的計算值θ2(m)以及誤差值errorXm；
步驟8，將各種姿態(tài)下轉(zhuǎn)角的計算值合并起來，表示為：θC＝[{θ1(1)，θ2(1)}，{θ1(2)，θ2(2)}，...，{θ1(m)，θ2(m)}]T；
將各種姿態(tài)下的誤差值合并起來，表示為：errorC＝[errorX1，errorX2，...，errorXm]T
其中：T表示矩陣的轉(zhuǎn)秩；
步驟9，確定待標(biāo)定參數(shù)矩陣：
將TCP坐標(biāo)系{S0}到第一關(guān)節(jié)坐標(biāo)系{S1}的桿長表示為a0，偏置表示為d0；
將第一關(guān)節(jié)坐標(biāo)系{S1}到第二關(guān)節(jié)坐標(biāo)系{S2}的桿長表示為a1，偏置表示為d1；
將針筒末端點(5)的底面中心，即坐標(biāo)原點o3在第二關(guān)節(jié)坐標(biāo)系{S2}的位置表示為：(x3，y3，z3)；
由此確定待標(biāo)定參數(shù)矩陣為：·X＝[Δa0，Δd0，Δa1，Δd1，Δx3，Δy3，Δz3]T；
其中：
Δa0：桿長a0的誤差值；
Δd0：偏置d0的誤差值；
Δa1：桿長a1的誤差值；
Δd1：偏置d1的誤差值；
Δx3：位置x3的誤差值；
Δy3：位置y3的誤差值；
Δz3：位置z3的誤差值；
步驟10，確定偏導(dǎo)數(shù)矩陣A：
步驟10.1，建立誤差傳遞模型為：
其中：
代表轉(zhuǎn)換矩陣產(chǎn)生的誤差；
代表轉(zhuǎn)換矩陣產(chǎn)生的誤差，通過以下方法獲得：
代表轉(zhuǎn)換矩陣產(chǎn)生的誤差，通過以下方法獲得：
代表轉(zhuǎn)換矩陣產(chǎn)生的誤差，通過以下方法獲得：
步驟10.2，對誤差傳遞模型進行整理，忽略高階小項，得到誤差矩陣的表達(dá)式為：
步驟10.3，從誤差矩陣中提取得到偏導(dǎo)數(shù)矩陣A，表達(dá)式如下：
步驟11，構(gòu)建誤差方程為：
其中：
A{θ1(1)，θ2(1)}代表將θ1(1)，θ2(1)代入到偏導(dǎo)數(shù)矩陣A后，得到的矩陣；
A{θ1(2)，θ2(2)}代表將θ1(2)，θ2(2)代入到偏導(dǎo)數(shù)矩陣A后，得到的矩陣；
依此類推
A{θ1(m)，θ2(m)}代表將θ1(m)，θ2(m)代入到偏導(dǎo)數(shù)矩陣A后，得到的矩陣；
步驟12，利用最小二乘法求解誤差方程，得到待標(biāo)定參數(shù)Δa0，Δd0，Δa1，Δd1，Δx3，Δy3，Δz3的值，即為實際夾持器單元相對于理論夾持器模型的加工裝配誤差；
步驟13，將步驟12得到的待標(biāo)定參數(shù)的值，疊加到理論夾持器模型中，完成對理論夾持器模型的幾何尺寸修正。
2.根據(jù)權(quán)利要求1所述的一種種植牙機器人末端夾持器幾何尺寸的標(biāo)定方法，其特征在于，m≥3。
該專利技術(shù)資料僅供研究查看技術(shù)是否侵權(quán)等信息，商用須獲得專利權(quán)人授權(quán)。該專利全部權(quán)利屬于四川鋒準(zhǔn)機器人科技有限公司，未經(jīng)四川鋒準(zhǔn)機器人科技有限公司許可，擅自商用是侵權(quán)行為。如果您想購買此專利、獲得商業(yè)授權(quán)和技術(shù)合作，請聯(lián)系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/202210128908.5/1.html，轉(zhuǎn)載請聲明來源鉆瓜專利網(wǎng)。
