一种种植牙机器人末端夹持器几何尺寸的标定方法与流程

1.本发明属于手术机器人几何尺寸误差标定技术领域，具体涉及一种种植牙机器人末端夹持器几何尺寸的标定方法。


背景技术：

2.种植牙机器人末端连接有夹持器，在种植牙手术过程中，该夹持器末端针筒的姿态需要实时显示在终端屏幕上，从而方便种植医生观察。由于夹持器的各个零件在实际加工和装配过程中存在误差，因此，实际夹持器模型和理论夹持器模型的尺寸之间存在偏差，导致终端屏幕显示的理论夹持器末端针筒的实时位姿状态和实际夹持器末端针筒的姿态并不一致，从而不利于种植医生进行种植牙手术。


技术实现要素：

3.针对现有技术存在的缺陷，本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法，可有效解决上述问题。
4.本发明采用的技术方案如下：
5.本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法，包括以下步骤：
6.步骤1，种植牙机器人未端安装实际夹持器单元；构建与实际夹持器单元结构相同的理论夹持器模型；
7.所述实际夹持器单元和所述理论夹持器模型的结构相同，均包括：末端法兰盘(1)、第一连杆(2)、第二连杆(3)、夹持器手机弯头(4)、针筒末端点(5)、第一关节(7)和第二关节(8)；
8.所述末端法兰盘(1)的一端用于与种植牙机器人安装；所述末端法兰盘(1)的另一端与所述第一连杆(2)的一端连接；所述第一连杆(2)的另一端通过所述第一关节(7)与所述第二连杆(3)的一端铰接；所述第二连杆(3)的另一端通过所述第二关节(8)与所述夹持器手机弯头(4)的一端铰接；所述夹持器手机弯头(4)的另一端安装所述针筒末端点(5)；
9.步骤2，采用以下方式，分别构建tcp坐标系{s0}、第一关节坐标系{s1}、第二关节坐标系{s2}和针筒末端点坐标系{s3}：
10.步骤2.1，构建tcp坐标系{s0}，包括：确定坐标原点o0，确定x0轴、y0轴和z0轴；
11.坐标原点o0：以末端法兰盘(1)的轴线中心位置作为坐标原点；
12.z0轴：以末端法兰盘(1)的轴线方向作为z0轴；
13.y0轴：以垂直向上方向作为y0轴；
14.x0轴：根据z0轴和y0轴，确定x0轴；
15.步骤2.2，构建第一关节坐标系{s1}，包括：确定坐标原点o1，确定x1轴、y1轴和z1轴；
16.y1轴：以z0轴延长线的方向，作为y1轴；
17.z1轴：以第一关节(7)的轴线方向作为z1轴；
18.x1轴：根据y1轴和z1轴，确定x1轴；
19.坐标原点o1：以y1轴和z1轴的交点位置，作为坐标原点o1；
20.步骤2.3，构建第二关节坐标系{s2}，包括：确定坐标原点o2，确定x2轴、y2轴和z2轴；
21.x2轴：以x1轴延长线的方向，作为x2轴；
22.z2轴：以第二关节(8)的轴线方向作为z2轴；
23.y2轴：根据x2轴和z2轴，确定y2轴；
24.坐标原点o2：以x2轴和z2轴的交点位置，作为坐标原点o2；
25.步骤2.4，构建针筒末端点坐标系{s3}，包括：确定坐标原点o3，确定x3轴、y3轴和z3轴；
26.z3轴：以针筒末端点(5)的轴线方向作为z3轴；
27.坐标原点o3：以针筒末端点(5)的底面中心位置作为坐标原点o3；
28.y3轴：通过坐标原点o3，与y2轴平行的方向作为y3轴；
29.x3轴：根据z3轴和y3轴，确定x3轴；
30.步骤3，建立从tcp坐标系{s0}到针筒末端点坐标系{s3}的齐次变换关系式：由此得到变换矩阵的表达式，是与转角θ1和转角θ2有关的矩阵；
31.其中：
32.第一关节(7)的转角为θ1；第二关节(8)的转角为θ2；
33.tcp坐标系{s0}到第一关节坐标系{s1}的转换矩阵，为常数矩阵；
34.第一关节坐标系{s1}到第二关节坐标系{s2}的转换矩阵，是与转角θ1有关的矩阵；
35.第二关节坐标系{s2}到针筒末端点坐标系{s3}的转换矩阵，是与转角θ2有关的矩阵；
36.tcp坐标系{s0}到针筒末端点坐标系{s3}的转换矩阵，是与转角θ1和转角θ2有关的矩阵；
37.步骤4，对实际夹持器单元的姿态进行控制，改变转角θ1和转角θ2的值，使实际夹持器单元呈现第一种姿态；
38.在第一种姿态下，通过光学定位仪(6)测量得到针筒轴线向量在光学定位仪坐标系下的三方向分量值；然后，根据光学定位仪坐标系和tcp坐标系{s0}的转换关系，将针筒轴线向量在光学定位仪坐标系下的三方向分量值，转换到tcp坐标系{s0}下，得到针筒轴线向量在tcp坐标系{s0}下三方向分量值，分别为：x
t
，y
t
，z
t
，由此得到针筒轴线向量在tcp坐标系{s0}下的表达v_tip
tcp
＝{x
t
，y
t
，z
t
}；
39.步骤5，根据v_tip
tcp
＝{x
t
，y
t
，z
t
}和步骤3建立的变换矩阵的表达式，得到关于第一种姿态下转角θ1和转角θ2的方程组，求解该方程组，得到第一种姿态下转角θ1的计算值和转角θ2的计算值；将第一种姿态下转角θ1的计算值表示为θ1(1)，将第一种姿态下转角θ2的计算值表示为θ2(1)；
40.具体的，变换矩阵中，具有以下元素：针筒轴线向量在tcp坐标系{s0}下x
t
方向表示式为：f
x
(θ1，θ2)，y
t
方向表示式为：fy(θ1，θ2)，z
t
方向表示式为：fz(θ1，θ2)；
41.由此得到以下方程组：
42.x
t
＝f
x
(θ1，θ2)
43.y
t
＝fy(θ1，θ2)
44.z
t
＝fz(θ1，θ2)
45.求解该方程组，得到第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1)；
46.步骤6，根据第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1)，对理论夹持器模型进行姿态调节，使理论夹持器模型变化到第一种姿态；
47.对呈第一种姿态的理论夹持器模型进行计算，得到针筒末端点(5)的底面中心在tcp坐标系{s0}下的理论位置p1_tip
idea
；
48.对于第一种姿态的实际夹持器单元，通过光学定位仪测量得到光学定位仪坐标系下针筒末端点(5)的底面中心的位置坐标，再通过光学定位仪坐标系和tcp坐标系{s0}的转换关系，得到针筒末端点(5)的底面中心在tcp坐标系{s0}下的实际位置p1_tip
actual
；
49.根据下式，计算实际位置p1_tip
actual
和理论位置p1_tip
idea
的差值，得到本次误差值errorx1：
50.errorx1＝p1_tip
actual-p1_tip
idea
51.步骤7，对实际夹持器单元的姿态进行控制，改变转角θ1和转角θ2的值，使实际夹持器单元呈现第二种姿态，循环执行步骤4-步骤6，得到第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorx2；
52.一共重复m次，得到m组关节角度和误差值，即：
53.第一种姿态下转角θ1的计算值θ1(1)、转角θ2的计算值θ2(1)以及误差值errorx1；
54.第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorx2；
55.依此类推
56.第m种姿态下转角θ1的计算值θ1(m)、转角θ2的计算值θ2(m)以及误差值errorxm；
57.步骤8，将各种姿态下转角的计算值合并起来，表示为：θc＝[{θ1(1)，θ2(1)}，{θ1(2)，θ2(2)}，...，{θ1(m)，θ2(m)}]
t
；
[0058]
将各种姿态下的误差值合并起来，表示为：errorc＝[errorx1，errorx2，...，errorxm]
t
[0059]
其中：t表示矩阵的转秩；
[0060]
步骤9，确定待标定参数矩阵：
[0061]
将tcp坐标系{s0}到第一关节坐标系{s1}的杆长表示为a0，偏置表示为d0；
[0062]
将第一关节坐标系{s1}到第二关节坐标系{s2}的杆长表示为a1，偏置表示为d1：
[0063]
将针筒末端点(5)的底面中心，即坐标原点o2在第二关节坐标系{s2}的位置表示为：(x3，y3，z3)；
[0064]
由此确定待标定参数矩阵为：
·
x＝[δa0，δd0，δa1，δd1，δx3，δy3，δz3]
t
；
[0065]
其中：
[0066]
δa0：杆长a0的误差值；
[0067]
δd0：偏置d0的误差值；
[0068]
δa1：杆长a1的误差值；
[0069]
δd1：偏置d1的误差值；
[0070]
δx3：位置x3的误差值；
[0071]
δy3：位置y3的误差值；
[0072]
δz3：位置z3的误差值；
[0073]
步骤10，确定偏导数矩阵a：
[0074]
步骤10.1，建立误差传递模型为：
[0075][0076]
其中：
[0077]
代表转换矩阵产生的误差；
[0078]
代表转换矩阵产生的误差，通过以下方法获得：
[0079][0080]
代表转换矩阵产生的误差，通过以下方法获得：
[0081][0082]
代表转换矩阵产生的误差，通过以下方法获得：
[0083][0084]
步骤10.2，对误差传递模型进行整理，忽略高阶小项，得到误差矩阵的表达式为：
[0085][0086]
步骤10.3，从误差矩阵中提取得到偏导数矩阵a，表达式如下：
[0087][0088]
步骤11，构建误差方程为：
[0089][0090]
其中：
[0091]
a{θ1(1)，θ2(1)}代表将θ1(1)，θ2(1)代入到偏导数矩阵a后，得到的矩阵；
[0092]
a{θ1(2)，θ2(2)}代表将θ1(2)，θ2(2)代入到偏导数矩阵a后，得到的矩阵；
[0093]
依此类推
[0094]
a{θ1(m)，θ2(m)}代表将θ1(m)，θ2(m)代入到偏导数矩阵a后，得到的矩阵；
[0095]
步骤12，利用最小二乘法求解误差方程，得到待标定参数δa0，δd0，δa1，δd1，δx3，δy3，δz3的值，即为实际夹持器单元相对于理论夹持器模型的加工装配误差；
[0096]
步骤13，将步骤12得到的待标定参数的值，叠加到理论夹持器模型中，完成对理论夹持器模型的几何尺寸修正。
[0097]
优选的，m≥3。
[0098]
本发明提供的一种种植牙机器人末端夹持器几何尺寸的标定方法具有以下优点：
[0099]
具有操作简便，实施简单，计算精度高，且由于是封闭解，因此该方法的实时性强，可实现手术过程中，夹持器末端位姿实时准确的显示，有效的提升了种植医生手术中的观察体验。
附图说明
[0100]
图1为本发明提供的种植牙机器人末端夹持器的结构示意图；
[0101]
图2为本发明提供的种植牙机器人末端夹持器几何尺寸的标定方法的坐标系建立方法图。
具体实施方式
[0102]
为了使本发明所解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
[0103]
本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法，与传统末端夹持器的结构设计相比，具有结构简单，不需要附加额外的电机及控制设备，且该方法实现简单，计算精度高，实时性强，通过简单的计算就可以标定出夹持器加工和装配过程中的误差，进而通过修改理论夹持器模型的尺寸，使得夹持器针筒末端的实际位姿和理论位姿保持一致，从而实时的将修改dh参数后的理论夹持器模型显示在终端，提升了种植医生的观察体验。
[0104]
本发明主要思路为：
[0105]
利用机器人坐标系的建立规则，建立针筒末端坐标系和tcp坐标系之间的转换关系，从而利用该转换关系，以针筒轴线向量在tcp坐标系下的表达为约束，求解出在满足约束的前提下关节角1和关节角2的转动角度值，然后将该角度值代入到理论夹持器模型中，得到理论夹持器模型末端针筒坐标系原点的理论位置；再与光学定位仪检测到的针筒末端实际位置进行比较，得到当前该关节角度下夹持器末端针筒的位置误差，通过该误差以及建立好的偏导数矩阵，可以标定出末端夹持器中关键零部件的加工和装配误差。将标定出来的加工和装配误差，补偿到理论夹持器模型中，就完成末端加持器几何尺寸的标定。该方法具有操作简便，标定精度高，不需要额外的驱动设备就可以完成标定工作，从而降低了种植牙机器人使用者的劳动强度，有效的提升了种植牙医生的观察体验。
[0106]
本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法，参考图1和图2，包括以下步骤：
[0107]
步骤1，种植牙机器人未端安装实际夹持器单元；构建与实际夹持器单元结构相同的理论夹持器模型；
[0108]
所述实际夹持器单元和所述理论夹持器模型的结构相同，参考图1，均包括：末端法兰盘(1)、第一连杆(2)、第二连杆(3)、夹持器手机弯头(4)、针筒末端点(5)、第一关节(7)和第二关节(8)；
[0109]
所述末端法兰盘(1)的一端用于与种植牙机器人安装；所述末端法兰盘(1)的另一端与所述第一连杆(2)的一端连接；所述第一连杆(2)的另一端通过所述第一关节(7)与所述第二连杆(3)的一端铰接；所述第二连杆(3)的另一端通过所述第二关节(8)与所述夹持器手机弯头(4)的一端铰接；所述夹持器手机弯头(4)的另一端安装所述针筒末端点(5)；
[0110]
步骤2，采用以下方式，分别构建tcp坐标系{s0}、第一关节坐标系{s1}、第二关节坐标系{s2}和针筒末端点坐标系{s3}：
[0111]
步骤2.1，构建tcp坐标系{s0}，包括：确定坐标原点o0，确定x0轴、y0轴和z0轴；
[0112]
坐标原点o0：以末端法兰盘(1)的轴线中心位置作为坐标原点；
[0113]
z0轴：以末端法兰盘(1)的轴线方向作为z0轴；
[0114]
y0轴：以垂直向上方向作为y0轴；
[0115]
x0轴：根据z0轴和y0轴，确定x0轴；
[0116]
步骤2.2，构建第一关节坐标系{s1}，包括：确定坐标原点o1，确定x1轴、y1轴和z1轴；
[0117]
y1轴：以z0轴延长线的方向，作为y1轴；
[0118]
z1轴：以第一关节(7)的轴线方向作为z1轴；
[0119]
x1轴：根据y1轴和z1轴，确定x1轴；
[0120]
坐标原点o1：以y1轴和z1轴的交点位置，作为坐标原点o1；
[0121]
步骤2.3，构建第二关节坐标系{s2}，包括：确定坐标原点o2，确定x2轴、y2轴和z2轴；
[0122]
x2轴：以x1轴延长线的方向，作为x2轴；
[0123]
z2轴：以第二关节(8)的轴线方向作为z2轴；
[0124]
y2轴：根据x2轴和z2轴，确定y2轴；
[0125]
坐标原点o2：以x2轴和z2轴的交点位置，作为坐标原点o2；
[0126]
步骤2.4，构建针筒末端点坐标系{s3}，包括：确定坐标原点o3，确定x3轴、y3轴和z3轴；
[0127]
z3轴：以针筒末端点(5)的轴线方向作为z3轴；
[0128]
坐标原点o3：以针筒末端点(5)的底面中心位置作为坐标原点o3；
[0129]
y3轴：通过坐标原点o3，与y2轴平行的方向作为y3轴；
[0130]
x3轴：根据z3轴和y3轴，确定x3轴；
[0131]
步骤3，建立从tcp坐标系{s0}到针筒末端点坐标系{s3}的齐次变换关系式：由此得到变换矩阵的表达式，是与转角θ1和转角θ2有关的矩阵；
[0132]
其中：
[0133]
第一关节(7)的转角为θ1；第二关节(8)的转角为θ2；
[0134]
tcp坐标系{s0}到第一关节坐标系{s1}的转换矩阵，为常数矩阵；
的计算值表示为θ2(1)；
[0151]
具体的，变换矩阵中，具有以下元素：针筒轴线向量在tcp坐标系{s0}下x
t
方向表示式为：f
x
(θ1，θ2)，y
t
方向表示式为：fy(θ1，θ2)，z
t
方向表示式为：fz(θ1，θ2)；
[0152]
由此得到以下方程组：
[0153]
x
t
＝f
x
(θ1，θ2)
[0154]yt
＝fy(θ1，θ2)
[0155]zt
＝fz(θ1，θ2)
[0156]
求解该方程组，得到第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1)；
[0157]
步骤6，根据第一种姿态下转角θ1的计算值θ1(1)和转角θ2的计算值θ2(1)，对理论夹持器模型进行姿态调节，使理论夹持器模型变化到第一种姿态；
[0158]
对呈第一种姿态的理论夹持器模型进行计算，得到针筒末端点(5)的底面中心在tcp坐标系{s0}下的理论位置p1_tip
idea
；
[0159]
对于第一种姿态的实际夹持器单元，通过光学定位仪测量得到光学定位仪坐标系下针筒末端点(5)的底面中心的位置坐标，再通过光学定位仪坐标系和tcp坐标系{s0}的转换关系，得到针筒末端点(5)的底面中心在tcp坐标系{s0}下的实际位置p1_tip
actual
；
[0160]
根据下式，计算实际位置p1_tip
actual
和理论位置p1_tip
idea
的差值，得到本次误差值errorx1：
[0161]
errorx1＝p1_tip
actual-p1_tip
idea
[0162]
步骤7，对实际夹持器单元的姿态进行控制，改变转角θ1和转角θ2的值，使实际夹持器单元呈现第二种姿态，循环执行步骤4-步骤6，得到第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorx2；
[0163]
一共重复m次，其中，重复过程至少三次，即：m≥3，得到m组关节角度和误差值，即：
[0164]
第一种姿态下转角θ1的计算值θ1(1)、转角θ2的计算值θ2(1)以及误差值errorx1；
[0165]
第二种姿态下转角θ1的计算值θ1(2)、转角θ2的计算值θ2(2)以及误差值errorx2；
[0166]
依此类推
[0167]
第m种姿态下转角θ1的计算值θ1(m)、转角θ2的计算值θ2(m)以及误差值errorxm；
[0168]
步骤8，将各种姿态下转角的计算值合并起来，表示为：θc＝[{θ1(1)，θ2(1)}，{θ1(2)，θ2(2)}，...，{θ1(m)，θ2(m)}]
t
；
[0169]
将各种姿态下的误差值合并起来，表示为：errorc＝[errorx1，errorx2，...，errorxm]
t
[0170]
其中：t表示矩阵的转秩；
[0171]
步骤9，确定待标定参数矩阵：
[0172]
将tcp坐标系{s0}到第一关节坐标系{s1}的杆长表示为a0，偏置表示为d0；
[0173]
将第一关节坐标系{s1}到第二关节坐标系{s2}的杆长表示为a1，偏置表示为d1；
[0174]
将针筒末端点(5)的底面中心，即坐标原点o3在第二关节坐标系{s2}的位置表示为：(x3，y3，z3)；
[0175]
由此确定待标定参数矩阵为：
·
x＝[δa0，δd0，δa1，δd1，δx3，δy3，δz3]
t
；
[0176]
其中：
[0177]
δa0：杆长a0的误差值；
[0178]
δd0：偏置d0的误差值；
[0179]
δa1：杆长a1的误差值；
[0180]
δd1：偏置d1的误差值；
[0181]
δx2：位置x3的误差值；
[0182]
δy3：位置y3的误差值；
[0183]
δz3：位置z3的误差值；
[0184]
上述中杆长a0含义为：tcp坐标系{s0}的z0轴和第一关节坐标系{s1}的z1轴之间的公法线长度；
[0185]
杆长a1含义为：第一关节坐标系{s1}的z1轴和第二关节坐标系{s2}的z2轴之间的公法线长度；
[0186]
偏置d0含义为：tcp坐标系{s0}的x0轴和第一关节坐标系{s1}的x1轴之间的公法线长度；
[0187]
偏置d1含义为：第一关节坐标系{s1}的x1轴和第二关节坐标系{s2}的x2轴之间的公法线长度；
[0188]
步骤10，确定偏导数矩阵a：
[0189]
步骤10.1，建立误差传递模型为：
[0190][0191]
其中：
[0192]
代表转换矩阵产生的误差；
[0193]
代表转换矩阵产生的误差，通过以下方法获得：
[0194][0195]
代表转换矩阵产生的误差，通过以下方法获得：
[0196][0197]
代表转换矩阵产生的误差，通过以下方法获得：
[0198][0199]
步骤10.2，对误差传递模型进行整理，忽略高阶小项，得到误差矩阵的表达式为：
[0200][0201]
步骤10.3，从误差矩阵中提取得到偏导数矩阵a，表达式如下：
[0202][0203]
步骤11，构建误差方程为：
[0204][0205]
其中：
[0206]
a{θ1(1)，θ2(1)}代表将θ1(1)，θ2(1)代入到偏导数矩阵a后，得到的矩阵；
[0207]
a{θ1(2)，θ2(2)}代表将θ1(2)，θ2(2)代入到偏导数矩阵a后，得到的矩阵；
[0208]
依此类推
[0209]
a{θ1(m)，θ2(m)}代表将θ1(m)，θ2(m)代入到偏导数矩阵a后，得到的矩阵；
[0210]
步骤12，利用最小二乘法求解误差方程，得到待标定参数δa0，δd0，δa1，δd1，δx3，δy3，δz3的值，即为实际夹持器单元相对于理论夹持器模型的加工装配误差；
[0211]
步骤13，将步骤12得到的待标定参数的值，叠加到理论夹持器模型中对应的零件尺寸和装配尺寸中，完成对理论夹持器模型的几何尺寸修正，使得修正几何尺寸后的理论夹持器模型和实际夹持器单元的几何尺寸一致，从而完成对理论夹持器模型几何尺寸的标定。
[0212]
本发明提供的一种种植牙机器人末端夹持器几何尺寸标定方法，是一种简便准确标定种植牙机器人末端夹持器的标定方法，通过光学定位仪测量实际夹持器单元针筒末端的实际位姿信息，和理论夹持器模型针筒末端的位姿进行误差计算，标定出实际夹持器单元中关键零件的加工装配误差。
[0213]
本发明提供的一种种植牙机器人末端夹持器几何尺寸的标定方法，包括以下步骤：建立针筒坐标系到机械臂末端tcp坐标系的转换矩阵；计算在针筒目标位姿下实际针筒在tcp坐标系中的位置和理论针筒在tcp坐标系中位置的误差；将误差分解到从tcp到针筒坐标系转换矩阵的子矩阵中，建立误差传递矩阵模型，并计算从tcp到针筒末端关键零部件的几何尺寸误差和装配误差；将计算得到的几何尺寸误差和装配误差补偿到理论夹持器模型中；从而使种植牙手术中，终端显示的种植过程和实际的种植过程保持一致。本发明通过光学定位仪追踪针筒在tcp坐标系下的实际位置，需要移动针筒到有限数量的位置，就可以自动标定出来从tcp坐标系到针筒坐标系关键零部件的加工误差和装配误差；求解过程简单，不需要附加额外的设备，求解过程一次完成不需要迭代计算，求解精度高，实时性强，是一种简便的高精度的末端夹持器几何尺寸标定方法。
[0214]
综上所述，本发明提供的一种种植牙机器人末端夹持器几何尺寸的标定方法，具有以下优点：
[0215]
本发明构建了从tcp坐标系到针筒末端坐标系的这一运动链，进而构造针筒末端坐标系姿态在tcp坐标系下的变换关系通过对针筒轴线在tcp坐标系下的表达这一约束，求解出末端夹持器关节1和关节2的旋转角度θ1，θ2，将这两个角度代入到理论夹持器模型中，得到针筒末端坐标系的理论位姿p1，同时相对应的，由光学定位仪得到针筒末端坐标系在tcp坐标系下的实际位姿p2，进而得到在当前角度下的误差值p
2-p1，然后将中的各个子齐次变换矩对待标定的参数求解偏导，进而由偏导数表达式得到
偏导数矩阵，该矩阵就是由加工和装配误差引起的针筒末端坐标系的位姿误差的传递函数，通过人为的改变关节角1和关节角2的值，测得多组误差和偏导数矩阵，进而标定出加工和装配误差值，将这些误差值补偿到理论夹持器模型中，就完成标定。具有操作简便，实施简单，计算精度高，且由于是封闭解，因此该方法的实时性强，可实现手术过程中，夹持器末端位姿实时准确的显示，有效的提升了种植医生手术中的观察体验。
[0216]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视本发明的保护范围。