CN113720261B - 一种基于平面的标靶球头标定方法、系统及应用 - Google Patents

Abstract

本发明提供了一种基于平面的标靶球头标定方法、系统及应用，标定方法的步骤包括：取各标靶结构上的同一位置的点U,获取所有标靶上的点U在视觉坐标系下的坐标，采用标靶姿态相近或相同的三个不同位置处标靶上的点U在视觉坐标系下的坐标集合{Ui}，使用{Ui}拟合平面S"，得到该平面S"法向量N的初值；使用所有位置的{(R _i ,T _i )}，以及平面S"法向量N的初值，求解标靶球头的球心在标靶本地坐标系下的坐标H的初值；系统采用了前述标靶球头标定方法，将其应用于手术器械，标定手术器械核心工作部位的坐标和姿态。本发明具有稳定性好、精度高，易于实施等优点。

Description

一种基于平面的标靶球头标定方法、系统及应用

技术领域

本发明属于视觉测量技术领域，尤其涉及一种基于平面的标靶球头标定方法、系统及应用。

背景技术

在视觉测量中，带有球头的标靶(简称标靶)可以用于高精度工件尺寸测量。标靶一般具有灯源结构或图像的刚体结构(简称标靶结构)，使用相机对标靶进行拍摄，可以获取标靶结构在相机坐标系下的坐标。如果球头在标靶结构本地坐标系的坐标已知，那么可以换算出球头在相机坐标系的坐标。使用球头接触工件，便可以得到这些接触点在相机坐标系的坐标，从而完成对工件的尺寸测量。这一测量过程与传统三坐标仪相同，区别在于传统三坐标仪使用机械结构定位球头坐标，而基于视觉测量的标靶通过视觉系统定位球头的坐标。标靶结构一般是稳定的，可以通过精加工的方式获取其结构，或使用视觉系统获取标靶结构。但标靶的球头一般很难靠加工来保证其位置的精确性，并且视觉系统无法直接识别球头。因此，需要通过标定方法来获取球头在标靶结构坐标系下的坐标，即对标靶球头进行标定。而且，在实际使用过程中，标靶的球头部分经常与工件接触，或与手术部位接触，相对容易损坏或变形。因此，在现场快速进行球头标定和精度评估具有很强的实用价值。

如果将灯源结布置在手术器械上，如果手术器械的工作端是球形或针状的，例如穿刺器，则也可使用同样/类似的方法实时定位手术器械“球头”在视觉坐标系下的坐标，达到视觉实时手术导航的目的。

目前，三坐标仪、机械测量臂和视觉测量系统常见的球头标定方法有锥形标定和球形标定。

锥形标定方法是将球头放入到一个锥形体里，当球放于锥形体中，理论上球体与锥面相切，那么无论如何转动标靶，只要球头与锥形充分接触，那么球头与锥面始终相切，球头球心的位置是不动的。因此，标靶实际上是在绕球头的球心做球面运动，可以通过球拟合的方法计算得到球头球心在视觉系统下的坐标，然后将该坐标反算到标靶结构坐标系下，得到球头在标靶结构坐标系下的坐标。然而，锥形标定法的缺点主要有四方面。其一，需要牢固的固定锥形体，特别是对于视觉系统，标靶一般是手持的，而不像机械测量臂等有辅助机械结构，锥体需要承受较大的力量；标靶标定一般需要达到0.1mm以上的标定精度，那么牢固的固定锥形体对一般的用户来说是困难的，一般只有在有重型台面的车间才有具有这样的条件。其二，锥形体放置球头的是内锥形面，锥面很容易与安装球头的金属杆(测针)发生碰撞，实际上为了不发生碰撞，一般测针相对锥形体轴线的夹角不会超过45度，因此被标定的球面角度小于90度，而完整球面的角度是360度，对于球拟合，数据覆盖范围小于180度时，从数学理论上球心容易发生较大误差，当覆盖小于90度时，球心误差会增大一倍以上。其三，当手持标靶标定时，人很难感知球头与锥形体的接触情况，在标靶偏移锥形体轴线较远时，容易发生在重力影响下球头实际并没有与锥形面充分接触，即球与锥形面没有完全相切，球心位置发生了移动。因此，在操作者手持标靶的情况下，很难保证球心稳定性。其四，高精度的锥形体加工难度大，成本高。

球形标定方法与锥形标定方法类似，但标定装置从锥形体变为了球体。假设标定球体的半径为R，标靶球头半径为r。在标定过程中，标定球头的球心是不动的。因此，标靶的球头球心是在以标定球的球心为球心、半径为R+r的球面上运动，而不是标靶结构在球面上运动，基于这一特征可以计算得到球头球心的坐标。然而，球形标定的缺点主要有三方面。其一，球形标定方法需要牢固固定，否则球心位置会发生变化，球体一般通过支撑杆固定，支撑杆一般直径几倍小于球体半径，这样的结构下，球体更难于固定。其二，标定球体和标靶球头一般陶瓷或钨钢，高精度球体表面都非常的光滑，但手持标靶将球头放置于标定球体上时，非常容易打滑，很难保持两者充分接触。其三，即使保持了不打滑，充分接触，这时一般会施加较大力量来稳定标靶和标定球，从而导致标定球体的支撑杆发生变形。

发明内容

本发明目的在于提供一种基于平面的标靶球头标定方法、系统及应用，至少能够解决现有标靶球头标定过程中存在的稳定性差、标定精度低的问题。

为了实现上述目的，本发明采用了如下技术方案。

一种基于平面的标靶球头标定方法，其特征在于，步骤包括：

步骤1，建立标靶本地坐标系{Q_i}；

步骤2，使用标靶测量平面S，采集n(通常取n≥6)个位置，使用视觉测量系统采集每个位置下标靶的结构点{P_i}，并确保至少三个位置处的标靶姿态相近或相同，并计算{Q_i}到{P_i}的刚体变换关系(R_i,T_i)，其中R_i为旋转矩阵，T_i为平移矢量，得P_i＝R_i·Q_i+c；作为优选，确保前三个不同位置处的标靶姿态相近或相同；

步骤3，取各标靶结构上的同一位置的点U(例如，标靶1、标靶2、标靶3的左上角的点为同一位置的点),获取所有标靶上的点U在视觉坐标系下的坐标，采用标靶姿态相近或相同的三个不同位置处标靶上的点U在视觉坐标系下的坐标集合{Ui}，使用{Ui}拟合平面S"，得到该平面S"法向量N的初值；

步骤4，计算所有旋转矩阵R_i的平均值以及平移量T_i的平均值并重定义R_i和T_i:/>后续计算步骤在没有特殊说明下的R_i和T_i皆指重定义后R_i和T_i。

步骤5，使用所有位置的{(R_i,T_i)},以及平面S"法向量N的初值，基于式(Ⅰ)求解标靶球头的球心在标靶本地坐标系下的坐标H(Hx,Hy,Hz)的初值，

(a_i，b_i，c_i)·(Hx，Hy，Hx)+d_i＝0.........................(Ⅰ)

其中，d_i＝T_i·N， 是矩阵R_i的转置矩阵，N是平面法向量，它们的乘积得到一个向量(a_i，b_i，c_i)，T_i是个向量，其与法向量N的点积得到一个标量d_i；

步骤6，基于目标函数式(Ⅱ)，使用迭代算法优化求解∑||e_i||²最小值，获取最优(H,N)组合；其中，e_i为残差；作为优选，迭代优化方法可使用Levenberg–Marquardt(LM)、高斯-牛顿迭代法等非线性迭代方法，在本发明中采用LM迭代法具有较优的效果；

argminΣ||e_i||²＝argminΣ||(R_i·H)·N+T_i·N||².......(II)

步骤7，计算雅可比梯度矩阵式(Ⅲ)，本发明是具有复杂解结构的问题，使用该梯度矩阵可以极大的避免在复杂解结构下陷入局部最优解，很好的达到全局最优解。

J_iN＝R_i·H+T_i.....................(III)

其中，J_iH为e_i对H的导数，J_iN为e_i对N的导数。

一种用于标靶球头标定的系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序；其特征在于，所述处理器执行所述程序时实现以下步骤：

步骤11，读取标靶本地坐标系{Q_i}，以及基于视觉测量系统采集到的多个位置下标靶的结构点{P_i}，计算{Q_i}到{P_i}的刚体变换关系(R_i,T_i)，其中R_i为旋转矩阵，T_i为平移矢量，使得P_i＝R_i·Q_i+T_i；

步骤12，获取所有标靶上的点U在视觉坐标系下的坐标，读取前三个不同位置处标靶上的点U在视觉坐标系下的坐标集合{Ui}，基于{Ui}拟合平面S"，得到该平面S"法向量N的初值；

步骤13，计算所有旋转矩阵R_i的平均值以及平移量T_i的平均值并重定义R_i和T_i:/>后续计算步骤在没有特殊说明下的R_i和T_i皆指重定义后R_i和T_i。

步骤14，使用所有位置的{(R_i,T_i)},以及平面S"法向量N的初值，基于式(Ⅰ)求解标靶球头的球心在标靶本地坐标系下的坐标H(Hx,Hy,Hz)的初值，

(a_i，b_i，c_i)·(Hx，Hy，Hx)+d_i＝0.........................(Ⅰ)

其中，d_i＝T_i·N，

步骤15，基于目标函数式(Ⅱ)，使用迭代算法优化求解∑||e_i||²最小值，获取最优(H,N)组合；其中，e_i为残差；

argminΣ||e_i||²＝argminΣ||(R_i·H)·N+T_i·N||².......(II)

步骤16，计算雅可比梯度矩阵式(Ⅲ)，并输出计算结果；

其中，J_iH为e_i对H的导数，J_iN为e_i对N的导数。

一种基于平面的标靶球头标定方法的应用，其特征在于，将其应用于手术器械，标定手术器械核心工作部位的坐标和姿态，如手术器械的针头；标定后的手术器械可被实时跟踪，当与医疗影像数据配准后，可实时计算手术器械核心工作部位在医疗影像数据坐标系下的坐标和姿态，达到实时引导的作用。

本发明基于平面对标靶球头进行标定，所用标定平面(如量块表面、精加工的工件表面，手机平面，玻璃平面)在工作现场很容易找到；所用标定平面容易固定，无需重型工作台固定；所用台面或标定平面在发生水平位移时，平面方程S(O，N)并不会发生改变，即使台面或标定平面有轻微偏移，其对标靶球头标定的影响很小，一般而言，台面的垂直变动量一般是水平变动量的百分之一，也即台面轻微偏移对标靶球头标定造成的误差可忽略不计，该误差仅约锥面或球体标定法误差的百分之一。

采用本发明对标靶球头进行标定，标定平面成本低，只有标定锥面体或球体的十分之不到，且无需面积限制，几十毫米到几平米的平面都可。采用本发明方案，只需要标靶与平面充分接触，非常容易测量，既不会出现打滑，也不会出现因为重力造成无法感知球头是否与标定物体接触的问题，可靠性非常高；在几乎不会与标靶发生物理干涉的情况下，可以获得很大范围的测量姿态和测量位置，标定精度高。

具体实施方式

下面对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明部分实施例，而不是全部的实施例。

实施例

先对基于平面的标靶球头标定方法中构建式(Ⅰ)的步骤进行说明。

固定一平面S，使用标靶测量平面S，测量时保持标靶球头与平面S充分接触，并在平面S上测量10个不同位置。在其余实施例中，可以在平面S上测量6、7、8、9、11、12、13、14或15个不同位置。

假设标靶结构Sp由n个点组成，设这些点在标靶结构Sp定义的本地坐标系的坐标为{Q_i}，{Q_i}可通过精密加工和装配获得，或者通过视觉测量系统直接定位这些点坐标获得。

在某次测量时，设{Q_i}在视觉测量系统坐标系下测量得到的坐标为{Qw_i}，那么可以计算得到一个刚体变换{R_i,T_i}，使得Qw_i＝R_i*Q_i+T_i，其中R_i为刚体旋转，T_i为刚体平移量。

设标靶球头的球心在标靶结构Sp本地坐标系下的坐标为H，H是未知的。那么球头的球心在视觉测量系统坐标系下的坐标为{P_i},P_i＝H*R_i+T_i。由于平面S(O,N)是固定的，在任意一个位置测量平面时，球头都是与平面S相切的，如果切点为Ki,则P_i＝Ki+N*r,r是标靶球头的半径，Ki定义了平面S(O,N)，那么P_i显然也定义了一个平面S’，S’与S平行，并且两平面距离为r。

平面S’的方程为：((Pi-O)·N＝0

其中P_i为在视觉测量系统坐标系下的坐标，O定义为平面上的任意一点，N为平面法向量。

考到刚体变换，得到(H*R_i+T_i-O)·N＝0

由最小二乘法得知，测量一平面时，测量点{P_i}的平均值一定在平面上，因此可设

消除O得到

(R_i·H+T_i)·N＝0，以及

其中R_i和T_i是按:重新定义的R_i和T_i。旋转矩阵R_i的平均值/>平移量T_i的平均值/>后续计算步骤在没有特殊说明下的R_i和T_i皆指重定义后R_i和T_i。

由得到式(Ⅰ)

(a_i，b_i，c_i)·(Hx，Hy，Hz)+d_i＝0.........................(Ⅰ)

其中，d＝T_i·N， 是矩阵R_i的转置矩阵，N是平面法向量，它们的乘积得到一个向量(a_i，b_i，c_i)，T_i是个向量，其与法向量N的点积得到一个标量d_i；

接下来对基于平面的标靶球头标定方法进行说明，其流程示意图和步骤包括：

步骤1，建立标靶本地坐标系{Q_i}；

步骤2，使用标靶测量平面S，采集n(通常取15≥n≥6)个位置，使用视觉测量系统采集每个位置下标靶的结构点{P_i}，并确保至少三个位置处的标靶姿态相近或相同，并计算{Q_i}到{P_i}的刚体变换关系{R_ii,T_i}；作为优选，确保前三个不同位置处的标靶姿态相近或相同；

步骤3，取各标靶结构上的同一位置的点U(例如，标靶1、标靶2、标靶3的左上角的点为同一位置的点),获取所有标靶上的点U在视觉坐标系下的坐标，采用前三个不同位置处标靶上的点U在视觉坐标系下的坐标集合{Ui}，使用{Ui}拟合平面S"，得到该平面S"法向量N的初值；

步骤4，计算所有旋转矩阵R_i的平均值以及平移量T_i的平均值并重定义R_i和T_i:/>后续计算步骤在没有特殊说明下的R_i和T_i皆指重定义后R_i和T_i。

步骤5，使用所有位置的{(R_ii,T_i)},以及平面S"法向量N的初值，基于式(Ⅰ)求解标靶球头的球心在标靶本地坐标系下的坐标H(Hx,Hy,Hz)的初值，

(a_i，b_i，c_i)·(Hx，Hy，Hz)+d_i＝0.........................(Ⅰ)

其中，d_i＝T_i·N， 是矩阵R_i的转置矩阵，N是平面法向量，它们的乘积得到一个向量(a_i，b_i，c_i)，T_i是个向量，其与法向量N的点积得到一个标量d_i；；

步骤6，基于目标函数式(Ⅱ)，使用迭代算法优化求解∑||e_i||²最小值，获取最优(H,N)组合；其中，e_i为残差；作为优选，迭代优化方法可使用Levenberg–Marquardt(LM)、高斯-牛顿迭代法等非线性迭代方法，在本发明中采用LM迭代法具有较优的效果；

argmin∑||e_i||²＝argmin∑||(R_i·H)·N+T_i·N||²…………………(Ⅱ)

步骤7，计算雅可比梯度矩阵式(Ⅲ)，本发明是具有复杂解结构的问题，使用该梯度矩阵可以极大的避免在复杂解结构下陷入局部最优解，很好的达到全局最优解。

其中，J_iH为e_i对H的导数，J_iN为e_i对N的导数。

接下来对标靶球头标定的系统进行说明，其核心内容的原理和算法与基于平面的标靶球头标定方法的原理和算法相同。

一种用于标靶球头标定的系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序；利用视觉测量系统的相机获取的所有数据传输至用于标靶球头标定的系统中进行存储，其特征在于，所述处理器执行所述程序时实现以下步骤：

步骤11，读取标靶本地坐标系{Q_i}，以及基于视觉测量系统采集到的多个位置下标靶的结构点{P_i}，计算{Q_i}到{P_i}的刚体变换关系(R_i,T_i)；

步骤12，获取所有标靶上的点U在视觉坐标系下的坐标，读取前三个不同位置处标靶上的点U在视觉坐标系下的坐标集合{Ui}，基于{Ui}拟合平面S"，得到该平面S"法向量N的初值；

步骤13，计算所有旋转矩阵R_i的平均值以及平移量T_i的平均值并重定义R_i和T_i:/>后续计算步骤在没有特殊说明下的R_i和T_i皆指重定义后R_i和T_i。

步骤14，使用所有位置的{(R_i，T_i)}，以及平面S″法向量N的初值，基于式(Ⅰ)求解标靶球头的球心在标靶本地坐标系下的坐标H(Hx，Hy，Hz)的初值

步骤15，基于目标函数式(II)，使用迭代算法优化求解∑||e_i||²最小值，获取最优(H，N)组合；其中，e_i为残差；

argmin∑||e_i||²＝argmin∑||(R_i·H)·N+T_i·N||².......(II)

步骤15，计算雅可比梯度矩阵式(Ⅲ)，并输出计算结果；

其中，J_iH为e_i对H的导数，J_iN为e_i对N的导数。

一种基于平面的标靶球头标定方法的应用，将其应用于手术器械，标定手术器械核心工作部位的坐标和姿态，如手术器械的针头。具体是将灯源结构固定在带有球头或针头的手术器械上，然后在手术前采用步骤1-步骤6进行标定，或者借助于前述用于标靶球头标定的系统进行标定。

Claims (6)

1.一种基于平面的标靶球头标定方法，其特征在于，步骤包括：

步骤1，建立标靶本地坐标系{Q_i}；

步骤2，使用标靶测量平面S，采集多个位置，使用视觉测量系统采集每个位置下标靶的结构点{P_i}，并确保至少三个位置处的标靶姿态相近或相同，并计算{Q_i}到{P_i}的刚体变换关系(R_i,T_i)，其中R_i为旋转矩阵，T_i为平移矢量，得P_i＝R_i·Q_i+T_i；

步骤3，取各标靶结构上的同一位置的点U,获取所有标靶上的点U在视觉坐标系下的坐标，采用标靶姿态相近或相同的三个不同位置处标靶上的点U在视觉坐标系下的坐标集合{Ui}，使用{Ui}拟合平面S"，得到该平面S"法向量N的初值；

步骤4，计算所有旋转矩阵R_i的平均值以及平移量T_i的平均值/>并重定义R_i和T_i:/>后续计算步骤在没有特殊说明下的R_i和T_i皆指重定义后R_i和T_i；

步骤5，使用所有位置的{(R_i,T_i)}，以及平面S"法向量N的初值，基于式(Ⅰ)求解标靶球头的球心在标靶本地坐标系下的坐标H(Hx,Hy,Hz)的初值，

(a_i,b_i，c_i)·(Hx,Hy,Hz)+d_i＝0………………(I)

其中，

是矩阵R_i的转置矩阵，N是平面法向量，它们的乘积得到一个向量(a_i，b_i，c_i)，T_i是个向量，其与法向量N的点积得到一个标量d_i；

步骤6，基于目标函数式(Ⅱ)，使用迭代算法优化求解∑||e_i||²最小值，获取最优(H,N)组合；其中，e_i为残差；

arg minΣ||e_i||²＝arg minΣ||(R_i·H)·N+T_i·N||²……(II)

步骤7，计算雅可比梯度矩阵式(Ⅲ)，

其中，J_iH为e_i对H的导数，J_iN为e_i对N的导数。

2.根据权利要求1所述的标靶球头标定方法，其特征在于：步骤2中，采集多个位置是指采集n个位置，取n≥6。

3.根据权利要求1所述的标靶球头标定方法，其特征在于：步骤2中，确保前三个不同位置处的标靶姿态相近或相同。

4.一种如权利要求1-3任一项所述标靶球头标定方法的应用，其特征在于，将其应用于手术器械，标定手术器械核心工作部位的坐标和姿态。

5.一种用于标靶球头标定的系统，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序；其特征在于，所述处理器执行所述程序时实现以下步骤：

步骤11，读取标靶本地坐标系{Q_i}，以及基于视觉测量系统采集到的多个位置下标靶的结构点{P_i}，计算{Q_i}到{P_i}的刚体变换关系(R_i,T_i)，其中R_i为旋转矩阵，T_i为平移矢量，使得P_i＝R_i·Q_i+T_i；

步骤12，获取所有标靶上的点U在视觉坐标系下的坐标，读取前三个不同位置处标靶上的点U在视觉坐标系下的坐标集合{Ui}，基于{Ui}拟合平面S"，得到该平面S"法向量N的初值；

步骤13，计算所有旋转矩阵R_i的平均值以及平移量T_i的平均值/> 并重定义R_i和T_i:/>后续计算步骤在没有特殊说明下的R_i和T_i皆指重定义后R_i和T_i；

步骤14，使用所有位置的{(R_i,T_i)}，以及平面S"法向量N的初值，基于式(Ⅰ)求解标靶球头的球心在标靶本地坐标系下的坐标H(Hx,Hy,Hz)的初值

(a_i，b_i，c_i)·(Hx,Hy,Hz)+d_i＝0…………………(I)

其中，d_i＝T_i·N，

步骤15，基于目标函数式(Ⅱ)，使用迭代算法优化求解∑||e_i||²最小值，获取最优(H,N)组合；其中，e_i为残差；

arg minΣ||e_i||²＝arg minΣ||(R_i·H)·N+T_i·N||²……(II)

步骤16，计算雅可比梯度矩阵式(Ⅲ)，并输出计算结果；

其中，J_iH为e_i对H的导数，J_iN为e_i对N的导数。

6.一种如权利要求5所述用于标靶球头标定的系统的应用，其特征在于，将其应用于手术器械，标定手术器械核心工作部位的坐标和姿态。