CN114454167B - 一种种植牙机器人末端夹持器几何尺寸的标定方法 - Google Patents
一种种植牙机器人末端夹持器几何尺寸的标定方法 Download PDFInfo
- Publication number
- CN114454167B CN114454167B CN202210128908.5A CN202210128908A CN114454167B CN 114454167 B CN114454167 B CN 114454167B CN 202210128908 A CN202210128908 A CN 202210128908A CN 114454167 B CN114454167 B CN 114454167B
- Authority
- CN
- China
- Prior art keywords
- axis
- coordinate system
- rotation angle
- matrix
- joint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 239000004053 dental implant Substances 0.000 title claims abstract description 31
- 230000009466 transformation Effects 0.000 claims abstract description 39
- 230000003287 optical effect Effects 0.000 claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims description 94
- 230000036544 posture Effects 0.000 claims description 56
- 238000006243 chemical reaction Methods 0.000 claims description 12
- 238000003754 machining Methods 0.000 claims description 9
- 238000012546 transfer Methods 0.000 claims description 8
- 238000012937 correction Methods 0.000 claims description 4
- 238000000605 extraction Methods 0.000 claims description 3
- 239000007943 implant Substances 0.000 abstract description 4
- 238000012545 processing Methods 0.000 abstract description 3
- 230000006978 adaptation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1679—Programme controls characterised by the tasks executed
- B25J9/1692—Calibration of manipulator
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/0095—Means or methods for testing manipulators
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法,包括:建立从TCP坐标系到针筒末端点坐标系的齐次变换关系式;改变转角θ1和转角θ2的值,代入到理论夹持器模型中,得到针筒末端坐标系的理论位姿,同时由光学定位仪得到针筒末端坐标系在TCP坐标系下的实际位姿,进而得到在当前角度下的误差值,然后将齐次变换关系式中的各个子齐次变换矩对待标定的参数求解偏导,测得多组误差和偏导数矩阵,进而标定出加工和装配误差值。具有操作简便,实施简单,计算精度高,且由于是封闭解,因此该方法的实时性强,可实现手术过程中,夹持器末端位姿实时准确的显示,有效的提升了种植医生手术中的观察体验。
Description
技术领域
本发明属于手术机器人几何尺寸误差标定技术领域,具体涉及一种种植牙机器人末端夹持器几何尺寸的标定方法。
背景技术
种植牙机器人末端连接有夹持器,在种植牙手术过程中,该夹持器末端针筒的姿态需要实时显示在终端屏幕上,从而方便种植医生观察。由于夹持器的各个零件在实际加工和装配过程中存在误差,因此,实际夹持器模型和理论夹持器模型的尺寸之间存在偏差,导致终端屏幕显示的理论夹持器末端针筒的实时位姿状态和实际夹持器末端针筒的姿态并不一致,从而不利于种植医生进行种植牙手术。
发明内容
针对现有技术存在的缺陷,本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法,可有效解决上述问题。
本发明采用的技术方案如下:
本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法,包括以下步骤:
步骤1,种植牙机器人未端安装实际夹持器单元;构建与实际夹持器单元结构相同的理论夹持器模型;
所述实际夹持器单元和所述理论夹持器模型的结构相同,均包括:末端法兰盘(1)、第一连杆(2)、第二连杆(3)、夹持器手机弯头(4)、针筒末端点(5)、第一关节(7)和第二关节(8);
所述末端法兰盘(1)的一端用于与种植牙机器人安装;所述末端法兰盘(1)的另一端与所述第一连杆(2)的一端连接;所述第一连杆(2)的另一端通过所述第一关节(7)与所述第二连杆(3)的一端铰接;所述第二连杆(3)的另一端通过所述第二关节(8)与所述夹持器手机弯头(4)的一端铰接;所述夹持器手机弯头(4)的另一端安装所述针筒末端点(5);
步骤2,采用以下方式,分别构建TCP坐标系{S0}、第一关节坐标系{S1}、第二关节坐标系{S2}和针筒末端点坐标系{S3}:
步骤2.1,构建TCP坐标系{S0},包括:确定坐标原点o0,确定x0轴、y0轴和z0轴;
坐标原点o0:以末端法兰盘(1)的轴线中心位置作为坐标原点;
z0轴:以末端法兰盘(1)的轴线方向作为z0轴;
y0轴:以垂直向上方向作为y0轴;
x0轴:根据z0轴和y0轴,确定x0轴;
步骤2.2,构建第一关节坐标系{S1},包括:确定坐标原点o1,确定x1轴、y1轴和z1轴;
y1轴:以z0轴延长线的方向,作为y1轴;
z1轴:以第一关节(7)的轴线方向作为z1轴;
x1轴:根据y1轴和z1轴,确定x1轴;
坐标原点o1:以y1轴和z1轴的交点位置,作为坐标原点o1;
步骤2.3,构建第二关节坐标系{S2},包括:确定坐标原点o2,确定x2轴、y2轴和z2轴;
x2轴:以x1轴延长线的方向,作为x2轴;
z2轴:以第二关节(8)的轴线方向作为z2轴;
y2轴:根据x2轴和z2轴,确定y2轴;
坐标原点o2:以x2轴和z2轴的交点位置,作为坐标原点o2;
步骤2.4,构建针筒末端点坐标系{S3},包括:确定坐标原点o3,确定x3轴、y3轴和z3轴;
z3轴:以针筒末端点(5)的轴线方向作为z3轴;
坐标原点o3:以针筒末端点(5)的底面中心位置作为坐标原点o3;
y3轴:通过坐标原点o3,与y2轴平行的方向作为y3轴;
x3轴:根据z3轴和y3轴,确定x3轴;
步骤3,建立从TCP坐标系{S0}到针筒末端点坐标系{S3}的齐次变换关系式:由此得到变换矩阵/>的表达式,是与转角θ1和转角θ2有关的矩阵;
其中:
第一关节(7)的转角为θ1;第二关节(8)的转角为θ2;
TCP坐标系{S0}到第一关节坐标系{S1}的转换矩阵,为常数矩阵;
第一关节坐标系{S1}到第二关节坐标系{S2}的转换矩阵,是与转角θ1有关的矩阵;
第二关节坐标系{S2}到针筒末端点坐标系{S3}的转换矩阵,是与转角θ2有关的矩阵;
TCP坐标系{S0}到针筒末端点坐标系{S3}的转换矩阵,是与转角θ1和转角θ2有关的矩阵;
步骤4,对实际夹持器单元的姿态进行控制,改变转角θ1和转角θ2的值,使实际夹持器单元呈现第一种姿态;
在第一种姿态下,通过光学定位仪(6)测量得到针筒轴线向量在光学定位仪坐标系下的三方向分量值;然后,根据光学定位仪坐标系和TCP坐标系{S0}的转换关系,将针筒轴线向量/>在光学定位仪坐标系下的三方向分量值,转换到TCP坐标系{S0}下,得到针筒轴线向量/>在TCP坐标系{S0}下三方向分量值,分别为:xT,yT,zT,由此得到针筒轴线向量/>在TCP坐标系{S0}下的表达V_tipTCP={xT,yT,zT};
步骤5,根据V_tipTCP={xT,yT,zT}和步骤3建立的变换矩阵的表达式,得到关于第一种姿态下转角θ1和转角θ2的方程组,求解该方程组,得到第一种姿态下转角θ1的计算值和转角θ2的计算值;将第一种姿态下转角θ1的计算值表示为θ1(1),将第一种姿态下转角θ2的计算值表示为θ2(1);
具体的,变换矩阵中,具有以下元素:针筒轴线向量/>在TCP坐标系{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)
求解该方程组,得到第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1);
步骤6,根据第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1),对理论夹持器模型进行姿态调节,使理论夹持器模型变化到第一种姿态;
对呈第一种姿态的理论夹持器模型进行计算,得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的理论位置P1_tipidea;
对于第一种姿态的实际夹持器单元,通过光学定位仪测量得到光学定位仪坐标系下针筒末端点(5)的底面中心的位置坐标,再通过光学定位仪坐标系和TCP坐标系{S0}的转换关系,得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的实际位置P1_tipactual;
根据下式,计算实际位置P1_tipactual和理论位置P1_tipidea的差值,得到本次误差值errorX1:
errorX1=P1_tipactual-P1_tipidea
步骤7,对实际夹持器单元的姿态进行控制,改变转角θ1和转角θ2的值,使实际夹持器单元呈现第二种姿态,循环执行步骤4-步骤6,得到第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorX2;
一共重复m次,得到m组关节角度和误差值,即:
第一种姿态下转角θ1的计算值θ1(1)、转角θ2的计算值θ2(1)以及误差值errorX1;
第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorX2;
依此类推
第m种姿态下转角θ1的计算值θ1(m)、转角θ2的计算值θ2(m)以及误差值errorXm;
步骤8,将各种姿态下转角的计算值合并起来,表示为:θC=[{θ1(1),θ2(1)},{θ1(2),θ2(2)},...,{θ1(m),θ2(m)}]T;
将各种姿态下的误差值合并起来,表示为:errorC=[errorX1,errorX2,...,errorXm]T
其中:T表示矩阵的转秩;
步骤9,确定待标定参数矩阵:
将TCP坐标系{S0}到第一关节坐标系{S1}的杆长表示为a0,偏置表示为d0;
将第一关节坐标系{S1}到第二关节坐标系{S2}的杆长表示为a1,偏置表示为d1:
将针筒末端点(5)的底面中心,即坐标原点o2在第二关节坐标系{S2}的位置表示为:(x3,y3,z3);
由此确定待标定参数矩阵为:·X=[Δa0,Δd0,Δa1,Δd1,Δx3,Δy3,Δz3]T;
其中:
Δa0:杆长a0的误差值;
Δd0:偏置d0的误差值;
Δa1:杆长a1的误差值;
Δd1:偏置d1的误差值;
Δx3:位置x3的误差值;
Δy3:位置y3的误差值;
Δz3:位置z3的误差值;
步骤10,确定偏导数矩阵A:
步骤10.1,建立误差传递模型为:
其中:
代表转换矩阵/>产生的误差;
代表转换矩阵/>产生的误差,通过以下方法获得:
代表转换矩阵/>产生的误差,通过以下方法获得:
代表转换矩阵/>产生的误差,通过以下方法获得:
步骤10.2,对误差传递模型进行整理,忽略高阶小项,得到误差矩阵的表达式为:
步骤10.3,从误差矩阵中提取得到偏导数矩阵A,表达式如下:
步骤11,构建误差方程为:
其中:
A{θ1(1),θ2(1)}代表将θ1(1),θ2(1)代入到偏导数矩阵A后,得到的矩阵;
A{θ1(2),θ2(2)}代表将θ1(2),θ2(2)代入到偏导数矩阵A后,得到的矩阵;
依此类推
A{θ1(m),θ2(m)}代表将θ1(m),θ2(m)代入到偏导数矩阵A后,得到的矩阵;
步骤12,利用最小二乘法求解误差方程,得到待标定参数Δa0,Δd0,Δa1,Δd1,Δx3,Δy3,Δz3的值,即为实际夹持器单元相对于理论夹持器模型的加工装配误差;
步骤13,将步骤12得到的待标定参数的值,叠加到理论夹持器模型中,完成对理论夹持器模型的几何尺寸修正。
优选的,m≥3。
本发明提供的一种种植牙机器人末端夹持器几何尺寸的标定方法具有以下优点:
具有操作简便,实施简单,计算精度高,且由于是封闭解,因此该方法的实时性强,可实现手术过程中,夹持器末端位姿实时准确的显示,有效的提升了种植医生手术中的观察体验。
附图说明
图1为本发明提供的种植牙机器人末端夹持器的结构示意图;
图2为本发明提供的种植牙机器人末端夹持器几何尺寸的标定方法的坐标系建立方法图。
具体实施方式
为了使本发明所解决的技术问题、技术方案及有益效果更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅用以解释本发明,并不用于限定本发明。
本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法,与传统末端夹持器的结构设计相比,具有结构简单,不需要附加额外的电机及控制设备,且该方法实现简单,计算精度高,实时性强,通过简单的计算就可以标定出夹持器加工和装配过程中的误差,进而通过修改理论夹持器模型的尺寸,使得夹持器针筒末端的实际位姿和理论位姿保持一致,从而实时的将修改DH参数后的理论夹持器模型显示在终端,提升了种植医生的观察体验。
本发明主要思路为:
利用机器人坐标系的建立规则,建立针筒末端坐标系和TCP坐标系之间的转换关系,从而利用该转换关系,以针筒轴线向量在TCP坐标系下的表达为约束,求解出在满足约束的前提下关节角1和关节角2的转动角度值,然后将该角度值代入到理论夹持器模型中,得到理论夹持器模型末端针筒坐标系原点的理论位置;再与光学定位仪检测到的针筒末端实际位置进行比较,得到当前该关节角度下夹持器末端针筒的位置误差,通过该误差以及建立好的偏导数矩阵,可以标定出末端夹持器中关键零部件的加工和装配误差。将标定出来的加工和装配误差,补偿到理论夹持器模型中,就完成末端加持器几何尺寸的标定。该方法具有操作简便,标定精度高,不需要额外的驱动设备就可以完成标定工作,从而降低了种植牙机器人使用者的劳动强度,有效的提升了种植牙医生的观察体验。
本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法,参考图1和图2,包括以下步骤:
步骤1,种植牙机器人未端安装实际夹持器单元;构建与实际夹持器单元结构相同的理论夹持器模型;
所述实际夹持器单元和所述理论夹持器模型的结构相同,参考图1,均包括:末端法兰盘(1)、第一连杆(2)、第二连杆(3)、夹持器手机弯头(4)、针筒末端点(5)、第一关节(7)和第二关节(8);
所述末端法兰盘(1)的一端用于与种植牙机器人安装;所述末端法兰盘(1)的另一端与所述第一连杆(2)的一端连接;所述第一连杆(2)的另一端通过所述第一关节(7)与所述第二连杆(3)的一端铰接;所述第二连杆(3)的另一端通过所述第二关节(8)与所述夹持器手机弯头(4)的一端铰接;所述夹持器手机弯头(4)的另一端安装所述针筒末端点(5);
步骤2,采用以下方式,分别构建TCP坐标系{S0}、第一关节坐标系{S1}、第二关节坐标系{S2}和针筒末端点坐标系{S3}:
步骤2.1,构建TCP坐标系{S0},包括:确定坐标原点o0,确定x0轴、y0轴和z0轴;
坐标原点o0:以末端法兰盘(1)的轴线中心位置作为坐标原点;
z0轴:以末端法兰盘(1)的轴线方向作为z0轴;
y0轴:以垂直向上方向作为y0轴;
x0轴:根据z0轴和y0轴,确定x0轴;
步骤2.2,构建第一关节坐标系{S1},包括:确定坐标原点o1,确定x1轴、y1轴和z1轴;
y1轴:以z0轴延长线的方向,作为y1轴;
z1轴:以第一关节(7)的轴线方向作为z1轴;
x1轴:根据y1轴和z1轴,确定x1轴;
坐标原点o1:以y1轴和z1轴的交点位置,作为坐标原点o1;
步骤2.3,构建第二关节坐标系{S2},包括:确定坐标原点o2,确定x2轴、y2轴和z2轴;
x2轴:以x1轴延长线的方向,作为x2轴;
z2轴:以第二关节(8)的轴线方向作为z2轴;
y2轴:根据x2轴和z2轴,确定y2轴;
坐标原点o2:以x2轴和z2轴的交点位置,作为坐标原点o2;
步骤2.4,构建针筒末端点坐标系{S3},包括:确定坐标原点o3,确定x3轴、y3轴和z3轴;
z3轴:以针筒末端点(5)的轴线方向作为z3轴;
坐标原点o3:以针筒末端点(5)的底面中心位置作为坐标原点o3;
y3轴:通过坐标原点o3,与y2轴平行的方向作为y3轴;
x3轴:根据z3轴和y3轴,确定x3轴;
步骤3,建立从TCP坐标系{S0}到针筒末端点坐标系{S3}的齐次变换关系式:由此得到变换矩阵/>的表达式,是与转角θ1和转角θ2有关的矩阵;
其中:
第一关节(7)的转角为θ1;第二关节(8)的转角为θ2;
TCP坐标系{S0}到第一关节坐标系{S1}的转换矩阵,为常数矩阵;
第一关节坐标系{S1}到第二关节坐标系{S2}的转换矩阵,是与转角θ1有关的矩阵;
第二关节坐标系{S2}到针筒末端点坐标系{S3}的转换矩阵,是与转角θ2有关的矩阵;
TCP坐标系{S0}到针筒末端点坐标系{S3}的转换矩阵,是与转角θ1和转角θ2有关的矩阵;
作为一种具体实施例,各转换矩阵为以下形式:
x3=1.0e-5c2-0.005977s2-0.0001
y3=0.04c1-0.1387s1+0.005977c2c1+1.0e-5s2c1
z3=0.1387c1+0.04s1+0.005977c2s1+1.0e-5s1s2+0.156
其中:
c1=cos(θ1),s1=sin(θ1),c2=cos(θ2),s2=sin(θ2)
步骤4,对实际夹持器单元的姿态进行控制,改变转角θ1和转角θ2的值,使实际夹持器单元呈现第一种姿态;
在第一种姿态下,通过光学定位仪(6)测量得到针筒轴线向量在光学定位仪坐标系下的三方向分量值;然后,根据光学定位仪坐标系和TCP坐标系{S0}的转换关系,将针筒轴线向量/>在光学定位仪坐标系下的三方向分量值,转换到TCP坐标系{S0}下,得到针筒轴线向量/>在TCP坐标系{S0}下三方向分量值,分别为:xT,yT,zT,由此得到针筒轴线向量/>在TCP坐标系{S0}下的表达V_tipTCP={xT,yT,zT};
步骤5,根据V_tipTCP={xT,yT,zT}和步骤3建立的变换矩阵的表达式,得到关于第一种姿态下转角θ1和转角θ2的方程组,求解该方程组,得到第一种姿态下转角θ1的计算值和转角θ2的计算值;将第一种姿态下转角θ1的计算值表示为θ1(1),将第一种姿态下转角θ2的计算值表示为θ2(1);
具体的,变换矩阵中,具有以下元素:针筒轴线向量/>在TCP坐标系{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)
求解该方程组,得到第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1);
步骤6,根据第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1),对理论夹持器模型进行姿态调节,使理论夹持器模型变化到第一种姿态;
对呈第一种姿态的理论夹持器模型进行计算,得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的理论位置P1_tipidea;
对于第一种姿态的实际夹持器单元,通过光学定位仪测量得到光学定位仪坐标系下针筒末端点(5)的底面中心的位置坐标,再通过光学定位仪坐标系和TCP坐标系{S0}的转换关系,得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的实际位置P1_tipactual;
根据下式,计算实际位置P1_tipactual和理论位置P1_tipidea的差值,得到本次误差值errorX1:
errorX1=P1_tipactual-P1_tipidea
步骤7,对实际夹持器单元的姿态进行控制,改变转角θ1和转角θ2的值,使实际夹持器单元呈现第二种姿态,循环执行步骤4-步骤6,得到第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorX2;
一共重复m次,其中,重复过程至少三次,即:m≥3,得到m组关节角度和误差值,即:
第一种姿态下转角θ1的计算值θ1(1)、转角θ2的计算值θ2(1)以及误差值errorX1;
第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorX2;
依此类推
第m种姿态下转角θ1的计算值θ1(m)、转角θ2的计算值θ2(m)以及误差值errorXm;
步骤8,将各种姿态下转角的计算值合并起来,表示为:θC=[{θ1(1),θ2(1)},{θ1(2),θ2(2)},...,{θ1(m),θ2(m)}]T;
将各种姿态下的误差值合并起来,表示为:errorC=[errorX1,errorX2,...,errorXm]T
其中:T表示矩阵的转秩;
步骤9,确定待标定参数矩阵:
将TCP坐标系{S0}到第一关节坐标系{S1}的杆长表示为a0,偏置表示为d0;
将第一关节坐标系{S1}到第二关节坐标系{S2}的杆长表示为a1,偏置表示为d1;
将针筒末端点(5)的底面中心,即坐标原点o3在第二关节坐标系{S2}的位置表示为:(x3,y3,z3);
由此确定待标定参数矩阵为:·X=[Δa0,Δd0,Δa1,Δd1,Δx3,Δy3,Δz3]T;
其中:
Δa0:杆长a0的误差值;
Δd0:偏置d0的误差值;
Δa1:杆长a1的误差值;
Δd1:偏置d1的误差值;
Δx2:位置x3的误差值;
Δy3:位置y3的误差值;
Δz3:位置z3的误差值;
上述中杆长a0含义为:TCP坐标系{S0}的z0轴和第一关节坐标系{S1}的z1轴之间的公法线长度;
杆长a1含义为:第一关节坐标系{S1}的z1轴和第二关节坐标系{S2}的z2轴之间的公法线长度;
偏置d0含义为:TCP坐标系{S0}的x0轴和第一关节坐标系{S1}的x1轴之间的公法线长度;
偏置d1含义为:第一关节坐标系{S1}的x1轴和第二关节坐标系{S2}的x2轴之间的公法线长度;
步骤10,确定偏导数矩阵A:
步骤10.1,建立误差传递模型为:
其中:
代表转换矩阵/>产生的误差;
代表转换矩阵/>产生的误差,通过以下方法获得:
代表转换矩阵/>产生的误差,通过以下方法获得:
代表转换矩阵/>产生的误差,通过以下方法获得:
步骤10.2,对误差传递模型进行整理,忽略高阶小项,得到误差矩阵的表达式为:
步骤10.3,从误差矩阵中提取得到偏导数矩阵A,表达式如下:
步骤11,构建误差方程为:
其中:
A{θ1(1),θ2(1)}代表将θ1(1),θ2(1)代入到偏导数矩阵A后,得到的矩阵;
A{θ1(2),θ2(2)}代表将θ1(2),θ2(2)代入到偏导数矩阵A后,得到的矩阵;
依此类推
A{θ1(m),θ2(m)}代表将θ1(m),θ2(m)代入到偏导数矩阵A后,得到的矩阵;
步骤12,利用最小二乘法求解误差方程,得到待标定参数Δa0,Δd0,Δa1,Δd1,Δx3,Δy3,Δz3的值,即为实际夹持器单元相对于理论夹持器模型的加工装配误差;
步骤13,将步骤12得到的待标定参数的值,叠加到理论夹持器模型中对应的零件尺寸和装配尺寸中,完成对理论夹持器模型的几何尺寸修正,使得修正几何尺寸后的理论夹持器模型和实际夹持器单元的几何尺寸一致,从而完成对理论夹持器模型几何尺寸的标定。
本发明提供的一种种植牙机器人末端夹持器几何尺寸标定方法,是一种简便准确标定种植牙机器人末端夹持器的标定方法,通过光学定位仪测量实际夹持器单元针筒末端的实际位姿信息,和理论夹持器模型针筒末端的位姿进行误差计算,标定出实际夹持器单元中关键零件的加工装配误差。
本发明提供的一种种植牙机器人末端夹持器几何尺寸的标定方法,包括以下步骤:建立针筒坐标系到机械臂末端TCP坐标系的转换矩阵;计算在针筒目标位姿下实际针筒在TCP坐标系中的位置和理论针筒在TCP坐标系中位置的误差;将误差分解到从TCP到针筒坐标系转换矩阵的子矩阵中,建立误差传递矩阵模型,并计算从TCP到针筒末端关键零部件的几何尺寸误差和装配误差;将计算得到的几何尺寸误差和装配误差补偿到理论夹持器模型中;从而使种植牙手术中,终端显示的种植过程和实际的种植过程保持一致。本发明通过光学定位仪追踪针筒在TCP坐标系下的实际位置,需要移动针筒到有限数量的位置,就可以自动标定出来从TCP坐标系到针筒坐标系关键零部件的加工误差和装配误差;求解过程简单,不需要附加额外的设备,求解过程一次完成不需要迭代计算,求解精度高,实时性强,是一种简便的高精度的末端夹持器几何尺寸标定方法。
综上所述,本发明提供的一种种植牙机器人末端夹持器几何尺寸的标定方法,具有以下优点:
本发明构建了从TCP坐标系到针筒末端坐标系的这一运动链,进而构造针筒末端坐标系姿态在TCP坐标系下的变换关系通过对针筒轴线在TCP坐标系下的表达这一约束,求解出末端夹持器关节1和关节2的旋转角度θ1,θ2,将这两个角度代入到理论夹持器模型中,得到针筒末端坐标系的理论位姿P1,同时相对应的,由光学定位仪得到针筒末端坐标系在TCP坐标系下的实际位姿P2,进而得到在当前角度下的误差值P2-P1,然后将/>中的各个子齐次变换矩对待标定的参数求解偏导,进而由偏导数表达式得到偏导数矩阵,该矩阵就是由加工和装配误差引起的针筒末端坐标系的位姿误差的传递函数,通过人为的改变关节角1和关节角2的值,测得多组误差和偏导数矩阵,进而标定出加工和装配误差值,将这些误差值补偿到理论夹持器模型中,就完成标定。具有操作简便,实施简单,计算精度高,且由于是封闭解,因此该方法的实时性强,可实现手术过程中,夹持器末端位姿实时准确的显示,有效的提升了种植医生手术中的观察体验。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视本发明的保护范围。
Claims (2)
1.一种种植牙机器人末端夹持器几何尺寸的标定方法,其特征在于,包括以下步骤:
步骤1,种植牙机器人未端安装实际夹持器单元;构建与实际夹持器单元结构相同的理论夹持器模型;
所述实际夹持器单元和所述理论夹持器模型的结构相同,均包括:末端法兰盘(1)、第一连杆(2)、第二连杆(3)、夹持器手机弯头(4)、针筒末端点(5)、第一关节(7)和第二关节(8);
所述末端法兰盘(1)的一端用于与种植牙机器人安装;所述末端法兰盘(1)的另一端与所述第一连杆(2)的一端连接;所述第一连杆(2)的另一端通过所述第一关节(7)与所述第二连杆(3)的一端铰接;所述第二连杆(3)的另一端通过所述第二关节(8)与所述夹持器手机弯头(4)的一端铰接;所述夹持器手机弯头(4)的另一端安装所述针筒末端点(5);
步骤2,采用以下方式,分别构建TCP坐标系{S0}、第一关节坐标系{S1}、第二关节坐标系{S2}和针筒末端点坐标系{S3}:
步骤2.1,构建TCP坐标系{S0},包括:确定坐标原点o0,确定x0轴、y0轴和z0轴;
坐标原点o0:以末端法兰盘(1)的轴线中心位置作为坐标原点;
z0轴:以末端法兰盘(1)的轴线方向作为z0轴;
y0轴:以垂直向上方向作为y0轴;
x0轴:根据z0轴和y0轴,确定x0轴;
步骤2.2,构建第一关节坐标系{S1},包括:确定坐标原点o1,确定x1轴、y1轴和z1轴;
y1轴:以z0轴延长线的方向,作为y1轴;
z1轴:以第一关节(7)的轴线方向作为z1轴;
x1轴:根据y1轴和z1轴,确定x1轴;
坐标原点o1:以y1轴和z1轴的交点位置,作为坐标原点o1;
步骤2.3,构建第二关节坐标系{S2},包括:确定坐标原点o2,确定x2轴、y2轴和z2轴;
x2轴:以x1轴延长线的方向,作为x2轴;
z2轴:以第二关节(8)的轴线方向作为z2轴;
y2轴:根据x2轴和z2轴,确定y2轴;
坐标原点o2:以x2轴和z2轴的交点位置,作为坐标原点o2;
步骤2.4,构建针筒末端点坐标系{S3},包括:确定坐标原点o3,确定x3轴、y3轴和z3轴;
z3轴:以针筒末端点(5)的轴线方向作为z3轴;
坐标原点o3:以针筒末端点(5)的底面中心位置作为坐标原点o3;
y3轴:通过坐标原点o3,与y2轴平行的方向作为y3轴;
x3轴:根据z3轴和y3轴,确定x3轴;
步骤3,建立从TCP坐标系{S0}到针筒末端点坐标系{S3}的齐次变换关系式:
由此得到变换矩阵/>的表达式,是与转角θ1和转角θ2有关的矩阵;
其中:
第一关节(7)的转角为θ1;第二关节(8)的转角为θ2;
TCP坐标系{S0}到第一关节坐标系{S1}的转换矩阵,为常数矩阵;
第一关节坐标系{S1}到第二关节坐标系{S2}的转换矩阵,是与转角θ1有关的矩阵;
第二关节坐标系{S2}到针筒末端点坐标系{S3}的转换矩阵,是与转角θ2有关的矩阵;
TCP坐标系{S0}到针筒末端点坐标系{S3}的转换矩阵,是与转角θ1和转角θ2有关的矩阵;
步骤4,对实际夹持器单元的姿态进行控制,改变转角θ1和转角θ2的值,使实际夹持器单元呈现第一种姿态;
在第一种姿态下,通过光学定位仪(6)测量得到针筒轴线向量在光学定位仪坐标系下的三方向分量值;然后,根据光学定位仪坐标系和TCP坐标系{S0}的转换关系,将针筒轴线向量/>在光学定位仪坐标系下的三方向分量值,转换到TCP坐标系{S0}下,得到针筒轴线向量/>在TCP坐标系{S0}下三方向分量值,分别为:xT,yT,zT,由此得到针筒轴线向量/>在TCP坐标系{S0}下的表达V_tipTCP={xT,yT,zT};
步骤5,根据V_tipTCP={xT,yT,zT}和步骤3建立的变换矩阵的表达式,得到关于第一种姿态下转角θ1和转角θ2的方程组,求解该方程组,得到第一种姿态下转角θ1的计算值和转角θ2的计算值;将第一种姿态下转角θ1的计算值表示为θ1(1),将第一种姿态下转角θ2的计算值表示为θ2(1);
变换矩阵中,具有以下元素:针筒轴线向量/>在TCP坐标系{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)
求解该方程组,得到第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1);
步骤6,根据第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1),对理论夹持器模型进行姿态调节,使理论夹持器模型变化到第一种姿态;
对呈第一种姿态的理论夹持器模型进行计算,得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的理论位置P1_tipidea;
对于第一种姿态的实际夹持器单元,通过光学定位仪测量得到光学定位仪坐标系下针筒末端点(5)的底面中心的位置坐标,再通过光学定位仪坐标系和TCP坐标系{S0}的转换关系,得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的实际位置P1_tipactual;
根据下式,计算实际位置P1_tipactual和理论位置P1_tipidea的差值,得到本次误差值errorX1:
errorX1=P1_tipactual-P1_tipidea
步骤7,对实际夹持器单元的姿态进行控制,改变转角θ1和转角θ2的值,使实际夹持器单元呈现第二种姿态,循环执行步骤4-步骤6,得到第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorX2;
一共重复m次,得到m组关节角度和误差值,即:
第一种姿态下转角θ1的计算值θ1(1)、转角θ2的计算值θ2(1)以及误差值errorX1;
第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorX2;
依此类推
第m种姿态下转角θ1的计算值θ1(m)、转角θ2的计算值θ2(m)以及误差值errorXm;
步骤8,将各种姿态下转角的计算值合并起来,表示为:θC=[{θ1(1),θ2(1)},{θ1(2),θ2(2)},...,{θ1(m),θ2(m)}]T;
将各种姿态下的误差值合并起来,表示为:errorC=[errorX1,errorX2,...,errorXm]T
其中:T表示矩阵的转置;
步骤9,确定待标定参数矩阵:
将TCP坐标系{S0}到第一关节坐标系{S1}的杆长表示为a0,偏置表示为d0;
将第一关节坐标系{S1}到第二关节坐标系{S2}的杆长表示为a1,偏置表示为d1;
将针筒末端点(5)的底面中心,即坐标原点o3在第二关节坐标系{S2}的位置表示为:(x3,y3,z3);
由此确定待标定参数矩阵为:·X=[Δa0,Δd0,Δa1,Δd1,Δx3,Δy3,Δz3]T;
其中:
Δa0:杆长a0的误差值;
Δd0:偏置d0的误差值;
Δa1:杆长a1的误差值;
Δd1:偏置d1的误差值;
Δx3:位置x3的误差值;
Δy3:位置y3的误差值;
Δz3:位置z3的误差值;
步骤10,确定偏导数矩阵A:
步骤10.1,建立误差传递模型为:
其中:
代表转换矩阵/>产生的误差;
代表转换矩阵/>产生的误差,通过以下方法获得:
代表转换矩阵/>产生的误差,通过以下方法获得:
代表转换矩阵/>产生的误差,通过以下方法获得:
步骤10.2,对误差传递模型进行整理,忽略高阶小项,得到误差矩阵的表达式为:
步骤10.3,从误差矩阵中提取得到偏导数矩阵A,表达式如下:
步骤11,构建误差方程为:
其中:
A{θ1(1),θ2(1)}代表将θ1(1),θ2(1)代入到偏导数矩阵A后,得到的矩阵;
A{θ1(2),θ2(2)}代表将θ1(2),θ2(2)代入到偏导数矩阵A后,得到的矩阵;
依此类推
A{θ1(m),θ2(m)}代表将θ1(m),θ2(m)代入到偏导数矩阵A后,得到的矩阵;
步骤12,利用最小二乘法求解误差方程,得到待标定参数Δa0,Δd0,Δa1,Δd1,Δx3,Δy3,Δz3的值,即为实际夹持器单元相对于理论夹持器模型的加工装配误差;
步骤13,将步骤12得到的待标定参数的值,叠加到理论夹持器模型中,完成对理论夹持器模型的几何尺寸修正。
2.根据权利要求1所述的一种种植牙机器人末端夹持器几何尺寸的标定方法,其特征在于,m≥3。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210128908.5A CN114454167B (zh) | 2022-02-11 | 2022-02-11 | 一种种植牙机器人末端夹持器几何尺寸的标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210128908.5A CN114454167B (zh) | 2022-02-11 | 2022-02-11 | 一种种植牙机器人末端夹持器几何尺寸的标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114454167A CN114454167A (zh) | 2022-05-10 |
CN114454167B true CN114454167B (zh) | 2024-06-07 |
Family
ID=81413724
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210128908.5A Active CN114454167B (zh) | 2022-02-11 | 2022-02-11 | 一种种植牙机器人末端夹持器几何尺寸的标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114454167B (zh) |
Citations (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO1990011872A1 (en) * | 1989-03-31 | 1990-10-18 | Fanuc Ltd | Method for setting tool center point for robot |
WO2009059323A1 (en) * | 2007-11-01 | 2009-05-07 | Rimrock Automation, Inc. Dba Wolf Robotics | A method and system for finding a tool center point for a robot using an external camera |
WO2011143576A2 (en) * | 2010-05-14 | 2011-11-17 | Cognex Technology And Investment Corporation | System and method for robust calibration between a machine vision system and a robot |
JP2015147280A (ja) * | 2014-02-07 | 2015-08-20 | キヤノン株式会社 | ロボット較正方法 |
CN107972071A (zh) * | 2017-12-05 | 2018-05-01 | 华中科技大学 | 一种基于末端点平面约束的工业机器人连杆参数标定方法 |
WO2019019432A1 (zh) * | 2017-07-28 | 2019-01-31 | 深圳市圆梦精密技术研究院 | 机器人末端工具的位姿测量方法 |
CN110202582A (zh) * | 2019-07-03 | 2019-09-06 | 桂林电子科技大学 | 一种基于三坐标平台的机器人标定方法 |
CN111267106A (zh) * | 2020-03-19 | 2020-06-12 | 仲恺农业工程学院 | 一种基于视觉的智能体温检测机器人及检测方法 |
CN111329551A (zh) * | 2016-03-12 | 2020-06-26 | P·K·朗 | 用于脊柱和关节手术的增强现实引导 |
CN111367236A (zh) * | 2020-03-11 | 2020-07-03 | 北京卫星制造厂有限公司 | 一种面向加工过程的移动机器人***标定方法及*** |
CN111409077A (zh) * | 2020-05-09 | 2020-07-14 | 南京工程学院 | 一种基于关节角代偿的机器人末端多目标位姿逼近方法 |
CN111407443A (zh) * | 2020-02-25 | 2020-07-14 | 浙江工业大学 | 一种用于口腔种植机器人的精确定位及智能导航方法 |
CN112873199A (zh) * | 2021-01-08 | 2021-06-01 | 西北工业大学 | 基于运动学与空间插值的机器人绝对定位精度标定方法 |
CN113211436A (zh) * | 2021-05-07 | 2021-08-06 | 南京埃斯顿机器人工程有限公司 | 基于遗传算法的六自由度串联机器人误差标定方法 |
CN113400325A (zh) * | 2021-06-23 | 2021-09-17 | 四川锋准机器人科技有限公司 | 一种种植牙机器人导航定位方法 |
WO2021238617A1 (zh) * | 2020-05-28 | 2021-12-02 | 中国科学院宁波材料技术与工程研究所 | 工业机器人绝对精度标定***及标定方法 |
CN113843792A (zh) * | 2021-09-23 | 2021-12-28 | 四川锋准机器人科技有限公司 | 一种手术机器人的手眼标定方法 |
CN113855286A (zh) * | 2021-09-24 | 2021-12-31 | 四川锋准机器人科技有限公司 | 一种种植牙机器人导航***及方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107685330B (zh) * | 2017-10-18 | 2018-12-18 | 佛山华数机器人有限公司 | 一种六自由度手腕偏置串联机器人的运动学逆解求解方法 |
-
2022
- 2022-02-11 CN CN202210128908.5A patent/CN114454167B/zh active Active
Patent Citations (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO1990011872A1 (en) * | 1989-03-31 | 1990-10-18 | Fanuc Ltd | Method for setting tool center point for robot |
WO2009059323A1 (en) * | 2007-11-01 | 2009-05-07 | Rimrock Automation, Inc. Dba Wolf Robotics | A method and system for finding a tool center point for a robot using an external camera |
WO2011143576A2 (en) * | 2010-05-14 | 2011-11-17 | Cognex Technology And Investment Corporation | System and method for robust calibration between a machine vision system and a robot |
JP2015147280A (ja) * | 2014-02-07 | 2015-08-20 | キヤノン株式会社 | ロボット較正方法 |
CN111329551A (zh) * | 2016-03-12 | 2020-06-26 | P·K·朗 | 用于脊柱和关节手术的增强现实引导 |
WO2019019432A1 (zh) * | 2017-07-28 | 2019-01-31 | 深圳市圆梦精密技术研究院 | 机器人末端工具的位姿测量方法 |
CN107972071A (zh) * | 2017-12-05 | 2018-05-01 | 华中科技大学 | 一种基于末端点平面约束的工业机器人连杆参数标定方法 |
CN110202582A (zh) * | 2019-07-03 | 2019-09-06 | 桂林电子科技大学 | 一种基于三坐标平台的机器人标定方法 |
CN111407443A (zh) * | 2020-02-25 | 2020-07-14 | 浙江工业大学 | 一种用于口腔种植机器人的精确定位及智能导航方法 |
CN111367236A (zh) * | 2020-03-11 | 2020-07-03 | 北京卫星制造厂有限公司 | 一种面向加工过程的移动机器人***标定方法及*** |
CN111267106A (zh) * | 2020-03-19 | 2020-06-12 | 仲恺农业工程学院 | 一种基于视觉的智能体温检测机器人及检测方法 |
CN111409077A (zh) * | 2020-05-09 | 2020-07-14 | 南京工程学院 | 一种基于关节角代偿的机器人末端多目标位姿逼近方法 |
WO2021238617A1 (zh) * | 2020-05-28 | 2021-12-02 | 中国科学院宁波材料技术与工程研究所 | 工业机器人绝对精度标定***及标定方法 |
CN112873199A (zh) * | 2021-01-08 | 2021-06-01 | 西北工业大学 | 基于运动学与空间插值的机器人绝对定位精度标定方法 |
CN113211436A (zh) * | 2021-05-07 | 2021-08-06 | 南京埃斯顿机器人工程有限公司 | 基于遗传算法的六自由度串联机器人误差标定方法 |
CN113400325A (zh) * | 2021-06-23 | 2021-09-17 | 四川锋准机器人科技有限公司 | 一种种植牙机器人导航定位方法 |
CN113843792A (zh) * | 2021-09-23 | 2021-12-28 | 四川锋准机器人科技有限公司 | 一种手术机器人的手眼标定方法 |
CN113855286A (zh) * | 2021-09-24 | 2021-12-31 | 四川锋准机器人科技有限公司 | 一种种植牙机器人导航***及方法 |
Non-Patent Citations (2)
Title |
---|
基于封闭尺寸链的工业机器人结构参数标定;陆艺;于丽梅;郭斌;;仪器仪表学报(02);全文 * |
多轴激光加工机器人光路几何误差建模方法研究;彭艳;谢少荣;罗均;罗振军;;仪器仪表学报;20100915(09);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114454167A (zh) | 2022-05-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109794938B (zh) | 一种适用于曲面结构的机器人制孔误差补偿装置及其方法 | |
CN110757504B (zh) | 高精度可移动机器人的定位误差补偿方法 | |
CN107717993B (zh) | 一种高效便捷的简易机器人标定方法 | |
CN108789404B (zh) | 一种基于视觉的串联机器人运动学参数标定方法 | |
CN111055273B (zh) | 一种用于机器人的两步误差补偿方法 | |
CN113001535B (zh) | 机器人工件坐标系自动校正***与方法 | |
CN111203861B (zh) | 一种机器人工具坐标系的标定方法及标定*** | |
CN110000790B (zh) | 一种SCARA机器人eye-to-hand手眼***的标定方法 | |
CN112589787B (zh) | 进料转盘机械臂上下样的视觉定位及手眼标定方法 | |
CN107817682B (zh) | 一种基于手眼相机的空间机械臂在轨标定方法及*** | |
CN111862221B (zh) | Uvw平台标定方法、设备、纠偏方法、装置及对位*** | |
CN110253574B (zh) | 一种多任务机械臂位姿检测和误差补偿方法 | |
WO1993005479A1 (en) | Robot end-effector terminal control frame (tcf) calibration method and device | |
CN114643578B (zh) | 一种提高机器人视觉引导精度的标定装置及方法 | |
WO2022183761A1 (zh) | 一种基于联合标定的的空间位姿实时测调方法 | |
CN114001653B (zh) | 一种机器人工具中心点标定方法 | |
CN110490942A (zh) | 一种基于scara机械手第二臂的移动相机标定方法 | |
US7957834B2 (en) | Method for calculating rotation center point and axis of rotation, method for generating program, method for moving manipulator and positioning device, and robotic system | |
CN115284292A (zh) | 基于激光相机的机械臂手眼标定方法及装置 | |
CN111274696B (zh) | 一种实时获取凿岩台车双三角钻臂空间位置姿态的方法 | |
CN116026252A (zh) | 一种点云测量方法及*** | |
CN114454167B (zh) | 一种种植牙机器人末端夹持器几何尺寸的标定方法 | |
CN112762822B (zh) | 一种基于激光跟踪仪的机械臂校准方法及*** | |
CN110533727A (zh) | 一种基于单个工业相机的机器人自定位方法 | |
CN214323368U (zh) | 进料转盘机械臂上下样的视觉定位及手眼标定装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant |