一种基于Optitrack的探针尖定位方法

一种基于optitrack的探针尖定位方法
技术领域
1.本发明属于图像空间定位领域，具体涉及一种基于optitrack的探针尖定位方法。


背景技术：

2.探针是用于测量或采集数据的笔状工具，由用于手持的杆端和用于采集的尖端组成，使用探针尖端触碰待测物体可以获得相关信息。现有定位技术主要分为无线定位和视觉定位，无线定位需要无线传感器，视觉定位则需要标记物。定位目标点时需要在定位目标处安装传感器或标记物等定位器件，探针尖端细小，不适宜安装定位器件。optitrack是一套实现标记点高精度三维重建的光学系统，主要硬件是装配处理器的智能摄像机，内置高精度标记点处理算法，单个标记点的三维信息可用于创建刚体并获得六自由度数据。


技术实现要素：

3.为克服现有技术中的不足，本发明用于探针，optitrack相机组持续定位探针上的标记球，通过对采集数据进行球拟合实现对探针尖的标定，并对探针尖进行实时定位。具体包括：
4.(1)粘贴标记球：
5.在探针杆端粘贴红外标记球，标记球安装位置非对称，标记球安装数目大于三个；
6.(2)建立刚体：
7.在optitrack动作捕捉软件中将探针上的标记球建立为刚体资产，导出探针上的标记球在刚体坐标系下的三维非齐次坐标值；
8.(3)计算坐标系转换矩阵：
9.(3a)将探针尖放在标定孔上，使用optitrack采集探针上标记球在世界坐标系下的三维非齐次坐标值；
10.(3b)根据标记球在世界坐标系下的和刚体坐标系下的三维非齐次坐标值，计算世界坐标系到刚体坐标系的转换矩阵；
11.(4)标定探针尖：
12.(4a)将探针尖放在标定孔上，探针尖保持不动，探针绕其尖端旋转形成球形包络，同时用optitrack连续不断地采集刚体质心在世界坐标系下的三维非齐次坐标值；
13.(4b)对采集到的坐标数据进行球拟合，获得球心在世界坐标系下的坐标值；
14.(4c)使用步骤(3b)得到的坐标系转换矩阵，将球心在世界坐标系下的坐标值转化为刚体坐标系下的坐标值，将该坐标值作为探针尖在刚体坐标系下的坐标值；
15.(5)实时定位：
16.(5a)使用optitrack实时采集探针刚体的质心三维坐标和四元数；
17.(5b)使用刚体质心三维坐标和四元数构造刚体坐标系到世界坐标系的转换矩阵；
18.(5c)通过步骤(5b)得到的转换矩阵，将探针尖在刚体坐标系下的坐标值转换到世界坐标系下；
19.循环步骤(5)对探针尖进行实时定位。
20.本发明的有益效果在于：
21.本发明利用刚体位姿计算探针尖的坐标，不必在探针尖处安装定位器件，克服了定位探针尖时需要在探针尖安装定位器件的缺点，通过结合optitrack的刚体追踪能力，实现了探针尖的高精度实时定位，利用探针尖的实时定位信息，可进行物体的外观数据进行采集和建模。
附图说明
22.图1为本发明的流程图；
23.图2为标记球粘贴示例图；
具体实施方式
24.下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要注意的是，以下描述仅仅用于解释本发明，并不用于限定本发明。
25.本发明的流程图参见图1。
26.步骤1，粘贴标记球。
27.如图2所示，在探针杆端上粘贴红外标记球，标记球安装位置非对称，标记球安装数目大于三个。红外标记球指的是能够反射红外光的标记球。
28.步骤2，建立刚体。
29.在optitrack动作捕捉软件motive中将探针上的标记球建立为刚体资产，导出探针上的标记球在刚体坐标系下的三维非齐次坐标值{xi},i＝0...n-1,n≥3。一组红外标记球被相机捕捉后，可在motive软件中将其建立为刚体资产，生成该刚体的刚体坐标系和标记球在刚体坐标系下的坐标值，并能够实时获得在世界坐标系下刚体的位姿及标记球的坐标值。
30.步骤3，计算坐标系转换矩阵。
31.3.1)将探针尖放在标定孔上，采集探针上标记球在世界坐标系下的三维非齐次坐标值{yi},i＝0...n-1,n≥3，其中标定孔指的是可固定探针尖端的微小圆形凹槽。
32.3.2)根据标记球在世界坐标系下的坐标值{yi}和刚体坐标系下的坐标值{xi}计算世界坐标系到刚体坐标系的转换矩阵t
world2local
；所述坐标系转换矩阵是由以下算法得到的：给定两组相互对应的点集{mi}和{di}，i＝0...n-1,n≥3，满足
33.di＝rmi t viꢀꢀꢀꢀꢀꢀꢀ
(1)
34.其中，r为3
×
3维度旋转矩阵，t为3
×
1维度的平移向量，vi是噪声向量。
35.根据r、t构造4
×
4维度的坐标系的转换矩阵
36.以下为r、t计算方法：
37.根据最小均方误差准则，使下式为最小值时的r、t即为所求值：
[0038][0039]
为求解r，定义以下变量：
[0040][0041][0042]
公式(2)能够被重写为：
[0043][0044]
当公式(5)最后一项最大时，该式最小，等价于矩阵rh的迹最大，其中h是由下式定义的相关矩阵：
[0045][0046]
对h进行奇异值分解得到h＝uλv
t
，则r＝vu
t
，进一步根据r计算得到其中u指的是h的左奇异向量组成的矩阵，v指的是h的右奇异向量组成的矩阵。
[0047]
步骤4，标定探针尖。
[0048]
4.1)将探针尖放在标定孔上，探针尖保持不动，探针绕其尖端旋转形成球形包络，同时用optitrack连续不断地采集刚体质心在世界坐标系下的三维非齐次坐标值 {mi},i＝0...n-1,n＞＞4。
[0049]
4.2)对采集到的坐标数据进行球拟合，获得球心在世界坐标系下的坐标值。
[0050]
球拟合计算球心是用最小二乘法得到的：
[0051]
给定一组三维坐标数据{(xi,yi,zi)},i＝0...n-1,n＞＞4，这些数据分散在球面，满足 (x
i-x)2 (y
i-y)2 (z
i-z)2＝r2[0052]
求球心坐标(x,y,z)和半径r。
[0053]
首先计算采集数据的平均值：
[0054][0055]
采集数据和待求参数减去平均值得到新数据{(ui,vi,wi)},i＝0...n-1,n＞＞4和新待求参数(u,v,w)：
[0056][0057]
求解下述方程，待求参数为(u,v,w)：
[0058][0059]
解得(u,v,w)，进一步反变换可求得球心坐标(x,y,z)：
[0060][0061]
4.3)使用步骤3得到的转换矩阵将球心在世界坐标系下的坐标值转化为刚体坐标系下的坐标值：
[0062]
某点在源坐标系下的坐标为x，源坐标系到目标坐标系的转换矩阵为t，则该点在目标坐标系下的坐标y是由以下公式得到的：
[0063]
y＝tx。
[0064]
步骤5，实时定位。
[0065]
5.1)使用optitrack实时采集探针刚体的质心三维坐标和四元数。
[0066]
5.2)使用刚体质心的世界坐标和四元数构造刚体坐标系到世界坐标系的转换矩阵。
[0067]
转换矩阵由r、t组成，其中r为3
×
3旋转矩阵，t为3
×
1维度的平移向量。
[0068]
旋转矩阵r由四元数(w,x,y,z)计算得到：
[0069][0070]
刚体质心的世界坐标为pw，由于刚体质心的刚体坐标为零向量，则
[0071][0072]
5.3)根据步骤4.3)的计算方法，将探针刚体坐标系下的治疗源点和治疗靶点坐标转换到世界坐标系下。
[0073]
以上对本发明进行了详细的介绍，但是具体实施方式的描述仅用于解释本发明的方法及其核心思想，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。