CN114454167A - 一种种植牙机器人末端夹持器几何尺寸的标定方法 - Google Patents

Abstract

本发明提供一种种植牙机器人末端夹持器几何尺寸的标定方法，包括：建立从TCP坐标系到针筒末端点坐标系的齐次变换关系式；改变转角θ₁和转角θ₂的值，代入到理论夹持器模型中，得到针筒末端坐标系的理论位姿，同时由光学定位仪得到针筒末端坐标系在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}，包括：确定坐标原点o₀，确定x₀轴、y₀轴和z₀轴；

坐标原点o₀：以末端法兰盘(1)的轴线中心位置作为坐标原点；

z₀轴：以末端法兰盘(1)的轴线方向作为z₀轴；

y₀轴：以垂直向上方向作为y₀轴；

x₀轴：根据z₀轴和y₀轴，确定x₀轴；

步骤2.2，构建第一关节坐标系{S1}，包括：确定坐标原点o₁，确定x₁轴、y₁轴和z₁轴；

y₁轴：以z₀轴延长线的方向，作为y₁轴；

z₁轴：以第一关节(7)的轴线方向作为z₁轴；

x₁轴：根据y₁轴和z₁轴，确定x₁轴；

坐标原点o₁：以y₁轴和z₁轴的交点位置，作为坐标原点o₁；

步骤2.3，构建第二关节坐标系{S2}，包括：确定坐标原点o₂，确定x₂轴、y₂轴和z₂轴；

x₂轴：以x₁轴延长线的方向，作为x₂轴；

z₂轴：以第二关节(8)的轴线方向作为z₂轴；

y₂轴：根据x₂轴和z₂轴，确定y₂轴；

坐标原点o₂：以x₂轴和z₂轴的交点位置，作为坐标原点o₂；

步骤2.4，构建针筒末端点坐标系{S3}，包括：确定坐标原点o₃，确定x₃轴、y₃轴和z₃轴；

z₃轴：以针筒末端点(5)的轴线方向作为z₃轴；

坐标原点o₃：以针筒末端点(5)的底面中心位置作为坐标原点o₃；

y₃轴：通过坐标原点o₃，与y₂轴平行的方向作为y₃轴；

x₃轴：根据z₃轴和y₃轴，确定x₃轴；

步骤3，建立从TCP坐标系{S0}到针筒末端点坐标系{S3}的齐次变换关系式：

由此得到变换矩阵

的表达式，是与转角θ₁和转角θ₂有关的矩阵；

其中：

第一关节(7)的转角为θ₁；第二关节(8)的转角为θ₂；

TCP坐标系{S0}到第一关节坐标系{S1}的转换矩阵，为常数矩阵；

第一关节坐标系{S1}到第二关节坐标系{S2}的转换矩阵，是与转角θ₁有关的矩阵；

第二关节坐标系{S2}到针筒末端点坐标系{S3}的转换矩阵，是与转角θ₂有关的矩阵；

TCP坐标系{S0}到针筒末端点坐标系{S3}的转换矩阵，是与转角θ₁和转角θ₂有关的矩阵；

步骤4，对实际夹持器单元的姿态进行控制，改变转角θ₁和转角θ₂的值，使实际夹持器单元呈现第一种姿态；

在第一种姿态下，通过光学定位仪(6)测量得到针筒轴线向量

在光学定位仪坐标系下的三方向分量值；然后，根据光学定位仪坐标系和TCP坐标系{S0}的转换关系，将针筒轴线向量

在光学定位仪坐标系下的三方向分量值，转换到TCP坐标系{S0}下，得到针筒轴线向量

在TCP坐标系{S0}下三方向分量值，分别为：x_T，y_T，z_T，由此得到针筒轴线向量

在TCP坐标系{S0}下的表达V_tip^TCP＝{x_T，y_T，z_T}；

步骤5，根据V_tip^TCP＝{x_T，y_T，z_T}和步骤3建立的变换矩阵

的表达式，得到关于第一种姿态下转角θ₁和转角θ₂的方程组，求解该方程组，得到第一种姿态下转角θ₁的计算值和转角θ₂的计算值；将第一种姿态下转角θ₁的计算值表示为θ₁(1)，将第一种姿态下转角θ₂的计算值表示为θ₂(1)；

具体的，变换矩阵

中，具有以下元素：针筒轴线向量

在TCP坐标系{S0}下x_T方向表示式为：f_x(θ₁，θ₂)，y_T方向表示式为：f_y(θ₁，θ₂)，z_T方向表示式为：f_z(θ₁，θ₂)；

由此得到以下方程组：

x_T＝f_x(θ₁，θ₂)

y_T＝f_y(θ₁，θ₂)

z_T＝f_z(θ₁，θ₂)

求解该方程组，得到第一种姿态下转角θ₁的计算值θ₁(1)和转角θ₂的计算值θ₂(1)；

步骤6，根据第一种姿态下转角θ₁的计算值θ₁(1)和转角θ₂的计算值θ₂(1)，对理论夹持器模型进行姿态调节，使理论夹持器模型变化到第一种姿态；

对呈第一种姿态的理论夹持器模型进行计算，得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的理论位置P₁_tip^idea；

对于第一种姿态的实际夹持器单元，通过光学定位仪测量得到光学定位仪坐标系下针筒末端点(5)的底面中心的位置坐标，再通过光学定位仪坐标系和TCP坐标系{S0}的转换关系，得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的实际位置P₁_tip^actual；

根据下式，计算实际位置P₁_tip^actual和理论位置P₁_tip^idea的差值，得到本次误差值errorX₁：

errorX₁＝P₁_tip^actual-P₁_tip^idea

步骤7，对实际夹持器单元的姿态进行控制，改变转角θ₁和转角θ₂的值，使实际夹持器单元呈现第二种姿态，循环执行步骤4-步骤6，得到第二种姿态下转角θ₁的计算值θ₁(2)、转角θ₂的计算值θ₂(2)以及误差值errorX₂；

一共重复m次，得到m组关节角度和误差值，即：

第一种姿态下转角θ₁的计算值θ₁(1)、转角θ₂的计算值θ₂(1)以及误差值errorX₁；

第二种姿态下转角θ₁的计算值θ₁(2)、转角θ₂的计算值θ₂(2)以及误差值errorX₂；

依此类推

第m种姿态下转角θ₁的计算值θ₁(m)、转角θ₂的计算值θ₂(m)以及误差值errorX_m；

步骤8，将各种姿态下转角的计算值合并起来，表示为：θC＝[{θ₁(1)，θ₂(1)}，{θ₁(2)，θ₂(2)}，...，{θ₁(m)，θ₂(m)}]^T；

将各种姿态下的误差值合并起来，表示为：errorC＝[errorX₁，errorX₂，...，errorX_m]^T

其中：T表示矩阵的转秩；

步骤9，确定待标定参数矩阵：

将TCP坐标系{S0}到第一关节坐标系{S1}的杆长表示为a₀，偏置表示为d₀；

将第一关节坐标系{S1}到第二关节坐标系{S2}的杆长表示为a₁，偏置表示为d₁：

将针筒末端点(5)的底面中心，即坐标原点o₂在第二关节坐标系{S2}的位置表示为：(x₃，y₃，z₃)；

由此确定待标定参数矩阵为：·X＝[Δa₀，Δd₀，Δa₁，Δd₁，Δx₃，Δy₃，Δz₃]^T；

其中：

Δa₀：杆长a₀的误差值；

Δd₀：偏置d₀的误差值；

Δa₁：杆长a₁的误差值；

Δd₁：偏置d₁的误差值；

Δx₃：位置x₃的误差值；

Δy₃：位置y₃的误差值；

Δz₃：位置z₃的误差值；

步骤10，确定偏导数矩阵A：

步骤10.1，建立误差传递模型为：

其中：

代表转换矩阵

产生的误差；

代表转换矩阵

产生的误差，通过以下方法获得：

代表转换矩阵

产生的误差，通过以下方法获得：

代表转换矩阵

产生的误差，通过以下方法获得：

步骤10.2，对误差传递模型进行整理，忽略高阶小项，得到误差矩阵

的表达式为：

步骤10.3，从误差矩阵

中提取得到偏导数矩阵A，表达式如下：

步骤11，构建误差方程为：

其中：

A{θ₁(1)，θ₂(1)}代表将θ₁(1)，θ₂(1)代入到偏导数矩阵A后，得到的矩阵；

A{θ₁(2)，θ₂(2)}代表将θ₁(2)，θ₂(2)代入到偏导数矩阵A后，得到的矩阵；

依此类推

A{θ₁(m)，θ₂(m)}代表将θ₁(m)，θ₂(m)代入到偏导数矩阵A后，得到的矩阵；

步骤12，利用最小二乘法求解误差方程，得到待标定参数Δa₀，Δd₀，Δa₁，Δd₁，Δx₃，Δy₃，Δz₃的值，即为实际夹持器单元相对于理论夹持器模型的加工装配误差；

步骤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}，包括：确定坐标原点o₀，确定x₀轴、y₀轴和z₀轴；

坐标原点o₀：以末端法兰盘(1)的轴线中心位置作为坐标原点；

z₀轴：以末端法兰盘(1)的轴线方向作为z₀轴；

y₀轴：以垂直向上方向作为y₀轴；

x₀轴：根据z₀轴和y₀轴，确定x₀轴；

步骤2.2，构建第一关节坐标系{S1}，包括：确定坐标原点o₁，确定x₁轴、y₁轴和z₁轴；

y₁轴：以z₀轴延长线的方向，作为y₁轴；

z₁轴：以第一关节(7)的轴线方向作为z₁轴；

x₁轴：根据y₁轴和z₁轴，确定x₁轴；

坐标原点o₁：以y₁轴和z₁轴的交点位置，作为坐标原点o₁；

步骤2.3，构建第二关节坐标系{S2}，包括：确定坐标原点o₂，确定x₂轴、y₂轴和z₂轴；

x₂轴：以x₁轴延长线的方向，作为x₂轴；

z₂轴：以第二关节(8)的轴线方向作为z₂轴；

y₂轴：根据x₂轴和z₂轴，确定y₂轴；

坐标原点o₂：以x₂轴和z₂轴的交点位置，作为坐标原点o₂；

步骤2.4，构建针筒末端点坐标系{S3}，包括：确定坐标原点o₃，确定x₃轴、y₃轴和z₃轴；

z₃轴：以针筒末端点(5)的轴线方向作为z₃轴；

坐标原点o₃：以针筒末端点(5)的底面中心位置作为坐标原点o₃；

y₃轴：通过坐标原点o₃，与y₂轴平行的方向作为y₃轴；

x₃轴：根据z₃轴和y₃轴，确定x₃轴；

步骤3，建立从TCP坐标系{S0}到针筒末端点坐标系{S3}的齐次变换关系式：

由此得到变换矩阵

的表达式，是与转角θ₁和转角θ₂有关的矩阵；

其中：

第一关节(7)的转角为θ₁；第二关节(8)的转角为θ₂；

TCP坐标系{S0}到第一关节坐标系{S1}的转换矩阵，为常数矩阵；

第一关节坐标系{S1}到第二关节坐标系{S2}的转换矩阵，是与转角θ₁有关的矩阵；

第二关节坐标系{S2}到针筒末端点坐标系{S3}的转换矩阵，是与转角θ₂有关的矩阵；

TCP坐标系{S0}到针筒末端点坐标系{S3}的转换矩阵，是与转角θ₁和转角θ₂有关的矩阵；

作为一种具体实施例，各转换矩阵为以下形式：

x₃＝1.0e-5c2-0.005977s2-0.0001

y₃＝0.04c1-0.1387s1+0.005977c2c1+1.0e-5s2c1

z₃＝0.1387c1+0.04s1+0.005977c2s1+1.0e-5s1s2+0.156

其中：

c1＝cos(θ₁)，s1＝sin(θ₁)，c2＝cos(θ₂)，s2＝sin(θ₂)

步骤4，对实际夹持器单元的姿态进行控制，改变转角θ₁和转角θ₂的值，使实际夹持器单元呈现第一种姿态；

在第一种姿态下，通过光学定位仪(6)测量得到针筒轴线向量

在光学定位仪坐标系下的三方向分量值；然后，根据光学定位仪坐标系和TCP坐标系{S0}的转换关系，将针筒轴线向量

在光学定位仪坐标系下的三方向分量值，转换到TCP坐标系{S0}下，得到针筒轴线向量

在TCP坐标系{S0}下三方向分量值，分别为：x_T，y_T，z_T，由此得到针筒轴线向量

在TCP坐标系{S0}下的表达V_tip^TCP＝{x_T，y_T，z_T}；

步骤5，根据V_tip^TCP＝{x_T，y_T，z_T}和步骤3建立的变换矩阵

的表达式，得到关于第一种姿态下转角θ₁和转角θ₂的方程组，求解该方程组，得到第一种姿态下转角θ₁的计算值和转角θ₂的计算值；将第一种姿态下转角θ₁的计算值表示为θ₁(1)，将第一种姿态下转角θ₂的计算值表示为θ₂(1)；

具体的，变换矩阵

中，具有以下元素：针筒轴线向量

在TCP坐标系{S0}下x_T方向表示式为：f_x(θ₁，θ₂)，y_T方向表示式为：f_y(θ₁，θ₂)，z_T方向表示式为：f_z(θ₁，θ₂)；

由此得到以下方程组：

x_T＝f_x(θ₁，θ₂)

y_T＝f_y(θ₁，θ₂)

z_T＝f_z(θ₁，θ₂)

求解该方程组，得到第一种姿态下转角θ₁的计算值θ₁(1)和转角θ₂的计算值θ₂(1)；

步骤6，根据第一种姿态下转角θ₁的计算值θ₁(1)和转角θ₂的计算值θ₂(1)，对理论夹持器模型进行姿态调节，使理论夹持器模型变化到第一种姿态；

对呈第一种姿态的理论夹持器模型进行计算，得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的理论位置P₁_tip^idea；

对于第一种姿态的实际夹持器单元，通过光学定位仪测量得到光学定位仪坐标系下针筒末端点(5)的底面中心的位置坐标，再通过光学定位仪坐标系和TCP坐标系{S0}的转换关系，得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的实际位置P₁_tip^actual；

根据下式，计算实际位置P₁_tip^actual和理论位置P₁_tip^idea的差值，得到本次误差值errorX₁：

errorX₁＝P₁_tip^actual-P₁_tip^idea

步骤7，对实际夹持器单元的姿态进行控制，改变转角θ₁和转角θ₂的值，使实际夹持器单元呈现第二种姿态，循环执行步骤4-步骤6，得到第二种姿态下转角θ₁的计算值θ₁(2)、转角θ₂的计算值θ₂(2)以及误差值errorX₂；

一共重复m次，其中，重复过程至少三次，即：m≥3，得到m组关节角度和误差值，即：

第一种姿态下转角θ₁的计算值θ₁(1)、转角θ₂的计算值θ₂(1)以及误差值errorX₁；

第二种姿态下转角θ₁的计算值θ₁(2)、转角θ₂的计算值θ₂(2)以及误差值errorX₂；

依此类推

第m种姿态下转角θ₁的计算值θ₁(m)、转角θ₂的计算值θ₂(m)以及误差值errorX_m；

步骤8，将各种姿态下转角的计算值合并起来，表示为：θC＝[{θ₁(1)，θ₂(1)}，{θ₁(2)，θ₂(2)}，...，{θ₁(m)，θ₂(m)}]^T；

将各种姿态下的误差值合并起来，表示为：errorC＝[errorX₁，errorX₂，...，errorX_m]^T

其中：T表示矩阵的转秩；

步骤9，确定待标定参数矩阵：

将TCP坐标系{S0}到第一关节坐标系{S1}的杆长表示为a₀，偏置表示为d₀；

将第一关节坐标系{S1}到第二关节坐标系{S2}的杆长表示为a₁，偏置表示为d₁；

将针筒末端点(5)的底面中心，即坐标原点o₃在第二关节坐标系{S2}的位置表示为：(x₃，y₃，z₃)；

由此确定待标定参数矩阵为：·X＝[Δa₀，Δd₀，Δa₁，Δd₁，Δx₃，Δy₃，Δz₃]^T；

其中：

Δa₀：杆长a₀的误差值；

Δd₀：偏置d₀的误差值；

Δa₁：杆长a₁的误差值；

Δd₁：偏置d₁的误差值；

Δx₂：位置x₃的误差值；

Δy₃：位置y₃的误差值；

Δz₃：位置z₃的误差值；

上述中杆长a₀含义为：TCP坐标系{S0}的z₀轴和第一关节坐标系{S1}的z₁轴之间的公法线长度；

杆长a₁含义为：第一关节坐标系{S1}的z₁轴和第二关节坐标系{S2}的z₂轴之间的公法线长度；

偏置d₀含义为：TCP坐标系{S0}的x₀轴和第一关节坐标系{S1}的x₁轴之间的公法线长度；

偏置d₁含义为：第一关节坐标系{S1}的x₁轴和第二关节坐标系{S2}的x₂轴之间的公法线长度；

步骤10，确定偏导数矩阵A：

步骤10.1，建立误差传递模型为：

其中：

代表转换矩阵

产生的误差；

代表转换矩阵

产生的误差，通过以下方法获得：

代表转换矩阵

产生的误差，通过以下方法获得：

代表转换矩阵

产生的误差，通过以下方法获得：

步骤10.2，对误差传递模型进行整理，忽略高阶小项，得到误差矩阵

的表达式为：

步骤10.3，从误差矩阵

中提取得到偏导数矩阵A，表达式如下：

步骤11，构建误差方程为：

其中：

A{θ₁(1)，θ₂(1)}代表将θ₁(1)，θ₂(1)代入到偏导数矩阵A后，得到的矩阵；

A{θ₁(2)，θ₂(2)}代表将θ₁(2)，θ₂(2)代入到偏导数矩阵A后，得到的矩阵；

依此类推

A{θ₁(m)，θ₂(m)}代表将θ₁(m)，θ₂(m)代入到偏导数矩阵A后，得到的矩阵；

步骤12，利用最小二乘法求解误差方程，得到待标定参数Δa₀，Δd₀，Δa₁，Δd₁，Δx₃，Δy₃，Δz₃的值，即为实际夹持器单元相对于理论夹持器模型的加工装配误差；

步骤13，将步骤12得到的待标定参数的值，叠加到理论夹持器模型中对应的零件尺寸和装配尺寸中，完成对理论夹持器模型的几何尺寸修正，使得修正几何尺寸后的理论夹持器模型和实际夹持器单元的几何尺寸一致，从而完成对理论夹持器模型几何尺寸的标定。

本发明提供的一种种植牙机器人末端夹持器几何尺寸标定方法，是一种简便准确标定种植牙机器人末端夹持器的标定方法，通过光学定位仪测量实际夹持器单元针筒末端的实际位姿信息，和理论夹持器模型针筒末端的位姿进行误差计算，标定出实际夹持器单元中关键零件的加工装配误差。

本发明提供的一种种植牙机器人末端夹持器几何尺寸的标定方法，包括以下步骤：建立针筒坐标系到机械臂末端TCP坐标系的转换矩阵；计算在针筒目标位姿下实际针筒在TCP坐标系中的位置和理论针筒在TCP坐标系中位置的误差；将误差分解到从TCP到针筒坐标系转换矩阵的子矩阵中，建立误差传递矩阵模型，并计算从TCP到针筒末端关键零部件的几何尺寸误差和装配误差；将计算得到的几何尺寸误差和装配误差补偿到理论夹持器模型中；从而使种植牙手术中，终端显示的种植过程和实际的种植过程保持一致。本发明通过光学定位仪追踪针筒在TCP坐标系下的实际位置，需要移动针筒到有限数量的位置，就可以自动标定出来从TCP坐标系到针筒坐标系关键零部件的加工误差和装配误差；求解过程简单，不需要附加额外的设备，求解过程一次完成不需要迭代计算，求解精度高，实时性强，是一种简便的高精度的末端夹持器几何尺寸标定方法。

综上所述，本发明提供的一种种植牙机器人末端夹持器几何尺寸的标定方法，具有以下优点：

本发明构建了从TCP坐标系到针筒末端坐标系的这一运动链，进而构造针筒末端坐标系姿态在TCP坐标系下的变换关系

通过对针筒轴线在TCP坐标系下的表达这一约束，求解出末端夹持器关节1和关节2的旋转角度θ₁，θ₂，将这两个角度代入到理论夹持器模型中，得到针筒末端坐标系的理论位姿P₁，同时相对应的，由光学定位仪得到针筒末端坐标系在TCP坐标系下的实际位姿P₂，进而得到在当前角度下的误差值P₂-P₁，然后将

中的各个子齐次变换矩对待标定的参数求解偏导，进而由偏导数表达式得到偏导数矩阵，该矩阵就是由加工和装配误差引起的针筒末端坐标系的位姿误差的传递函数，通过人为的改变关节角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}，包括：确定坐标原点o₀，确定x₀轴、y₀轴和z₀轴；

坐标原点o₀：以末端法兰盘(1)的轴线中心位置作为坐标原点；

z₀轴：以末端法兰盘(1)的轴线方向作为z₀轴；

y₀轴：以垂直向上方向作为y₀轴；

x₀轴：根据z₀轴和y₀轴，确定x₀轴；

步骤2.2，构建第一关节坐标系{S1}，包括：确定坐标原点o₁，确定x₁轴、y₁轴和z₁轴；

y₁轴：以z₀轴延长线的方向，作为y₁轴；

z₁轴：以第一关节(7)的轴线方向作为z₁轴；

x₁轴：根据y₁轴和z₁轴，确定x₁轴；

坐标原点o₁：以y₁轴和z₁轴的交点位置，作为坐标原点o₁；

步骤2.3，构建第二关节坐标系{S2}，包括：确定坐标原点o₂，确定x₂轴、y₂轴和z₂轴；

x₂轴：以x₁轴延长线的方向，作为x₂轴；

z₂轴：以第二关节(8)的轴线方向作为z₂轴；

y₂轴：根据x₂轴和z₂轴，确定y₂轴；

坐标原点o₂：以x₂轴和z₂轴的交点位置，作为坐标原点o₂；

步骤2.4，构建针筒末端点坐标系{S3}，包括：确定坐标原点o₃，确定x₃轴、y₃轴和z₃轴；

z₃轴：以针筒末端点(5)的轴线方向作为z₃轴；

坐标原点o₃：以针筒末端点(5)的底面中心位置作为坐标原点o₃；

y₃轴：通过坐标原点o₃，与y₂轴平行的方向作为y₃轴；

x₃轴：根据z₃轴和y₃轴，确定x₃轴；

步骤3，建立从TCP坐标系{S0}到针筒末端点坐标系{S3}的齐次变换关系式：

由此得到变换矩阵

的表达式，是与转角θ₁和转角θ₂有关的矩阵；

其中：

第一关节(7)的转角为θ₁；第二关节(8)的转角为θ₂；

TCP坐标系{S0}到第一关节坐标系{S1}的转换矩阵，为常数矩阵；

第一关节坐标系{S1}到第二关节坐标系{S2}的转换矩阵，是与转角θ₁有关的矩阵；

第二关节坐标系{S2}到针筒末端点坐标系{S3}的转换矩阵，是与转角θ₂有关的矩阵；

TCP坐标系{S0}到针筒末端点坐标系{S3}的转换矩阵，是与转角θ₁和转角θ₂有关的矩阵；

步骤4，对实际夹持器单元的姿态进行控制，改变转角θ₁和转角θ₂的值，使实际夹持器单元呈现第一种姿态；

在第一种姿态下，通过光学定位仪(6)测量得到针筒轴线向量

在光学定位仪坐标系下的三方向分量值；然后，根据光学定位仪坐标系和TCP坐标系{S0}的转换关系，将针筒轴线向量

在光学定位仪坐标系下的三方向分量值，转换到TCP坐标系{S0}下，得到针筒轴线向量

在TCP坐标系{S0}下三方向分量值，分别为：x_T，y_T，z_T，由此得到针筒轴线向量

在TCP坐标系{S0}下的表达V_tip^TCP＝{x_T，y_T，z_T}；

步骤5，根据V_tip^TCP＝{x_T，y_T，z_T}和步骤3建立的变换矩阵

的表达式，得到关于第一种姿态下转角θ₁和转角θ₂的方程组，求解该方程组，得到第一种姿态下转角θ₁的计算值和转角θ₂的计算值；将第一种姿态下转角θ₁的计算值表示为θ₁(1)，将第一种姿态下转角θ₂的计算值表示为θ₂(1)；

具体的，变换矩阵

中，具有以下元素：针筒轴线向量

在TCP坐标系{S0}下x_T方向表示式为：f_x(θ₁，θ₂)，y_T方向表示式为：f_y(θ₁，θ₂)，z_T方向表示式为：f_z(θ₁，θ₂)；

由此得到以下方程组：

x_T＝f_x(θ₁，θ₂)

y_T＝f_y(θ₁，θ₂)

z_T＝f_z(θ₁，θ₂)

求解该方程组，得到第一种姿态下转角θ₁的计算值θ₁(1)和转角θ₂的计算值θ₂(1)；

步骤6，根据第一种姿态下转角θ₁的计算值θ₁(1)和转角θ₂的计算值θ₂(1)，对理论夹持器模型进行姿态调节，使理论夹持器模型变化到第一种姿态；

对呈第一种姿态的理论夹持器模型进行计算，得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的理论位置P₁_tip^idea；

对于第一种姿态的实际夹持器单元，通过光学定位仪测量得到光学定位仪坐标系下针筒末端点(5)的底面中心的位置坐标，再通过光学定位仪坐标系和TCP坐标系{S0}的转换关系，得到针筒末端点(5)的底面中心在TCP坐标系{S0}下的实际位置P₁_tip^actual；

根据下式，计算实际位置P₁_tip^actual和理论位置P₁_tip^idea的差值，得到本次误差值errorX₁：

errorX₁＝P₁_tip^actual-P₁_tip^idea

步骤7，对实际夹持器单元的姿态进行控制，改变转角θ₁和转角θ₂的值，使实际夹持器单元呈现第二种姿态，循环执行步骤4-步骤6，得到第二种姿态下转角θ₁的计算值θ₁(2)、转角θ₂的计算值θ₂(2)以及误差值errorX₂；

一共重复m次，得到m组关节角度和误差值，即：

第一种姿态下转角θ₁的计算值θ₁(1)、转角θ₂的计算值θ₂(1)以及误差值errorX₁；

第二种姿态下转角θ₁的计算值θ₁(2)、转角θ₂的计算值θ₂(2)以及误差值errorX₂；

依此类推

第m种姿态下转角θ₁的计算值θ₁(m)、转角θ₂的计算值θ₂(m)以及误差值errorX_m；

步骤8，将各种姿态下转角的计算值合并起来，表示为：θC＝[{θ₁(1)，θ₂(1)}，{θ₁(2)，θ₂(2)}，...，{θ₁(m)，θ₂(m)}]^T；

将各种姿态下的误差值合并起来，表示为：errorC＝[errorX₁，errorX₂，...，errorX_m]^T

其中：T表示矩阵的转秩；

步骤9，确定待标定参数矩阵：

将TCP坐标系{S0}到第一关节坐标系{S1}的杆长表示为a₀，偏置表示为d₀；

将第一关节坐标系{S1}到第二关节坐标系{S2}的杆长表示为a₁，偏置表示为d₁；

将针筒末端点(5)的底面中心，即坐标原点o₃在第二关节坐标系{S2}的位置表示为：(x₃，y₃，z₃)；

由此确定待标定参数矩阵为：·X＝[Δa₀，Δd₀，Δa₁，Δd₁，Δx₃，Δy₃，Δz₃]^T；

其中：

Δa₀：杆长a₀的误差值；

Δd₀：偏置d₀的误差值；

Δa₁：杆长a₁的误差值；

Δd₁：偏置d₁的误差值；

Δx₃：位置x₃的误差值；

Δy₃：位置y₃的误差值；

Δz₃：位置z₃的误差值；

步骤10，确定偏导数矩阵A：

步骤10.1，建立误差传递模型为：

其中：

代表转换矩阵

产生的误差；

代表转换矩阵

产生的误差，通过以下方法获得：

代表转换矩阵

产生的误差，通过以下方法获得：

代表转换矩阵

产生的误差，通过以下方法获得：

步骤10.2，对误差传递模型进行整理，忽略高阶小项，得到误差矩阵

的表达式为：

步骤10.3，从误差矩阵

中提取得到偏导数矩阵A，表达式如下：

步骤11，构建误差方程为：

其中：

A{θ₁(1)，θ₂(1)}代表将θ₁(1)，θ₂(1)代入到偏导数矩阵A后，得到的矩阵；

A{θ₁(2)，θ₂(2)}代表将θ₁(2)，θ₂(2)代入到偏导数矩阵A后，得到的矩阵；

依此类推

A{θ₁(m)，θ₂(m)}代表将θ₁(m)，θ₂(m)代入到偏导数矩阵A后，得到的矩阵；

步骤12，利用最小二乘法求解误差方程，得到待标定参数Δa₀，Δd₀，Δa₁，Δd₁，Δx₃，Δy₃，Δz₃的值，即为实际夹持器单元相对于理论夹持器模型的加工装配误差；

步骤13，将步骤12得到的待标定参数的值，叠加到理论夹持器模型中，完成对理论夹持器模型的几何尺寸修正。

2.根据权利要求1所述的一种种植牙机器人末端夹持器几何尺寸的标定方法，其特征在于，m≥3。

Images