基于姿态匹配的工具追踪方法、装置、设备和介质与流程

本技术涉及光学定位，特别是涉及到一种基于姿态匹配的工具追踪方法、装置、设备和介质。

背景技术：

1、在近红外光学定位系统中，主动发光工具通过安装多个主动发光led灯珠，由定位系统识别和追踪各灯珠在左右图像中的亚像素坐标，利用立体视觉原理计算出各灯珠在空间中的三维坐标，进而通过刚体变换解算工具在定位系统坐标系中的完整空间姿态，包括三维位置和三维姿态。每个主动工具在投入使用前需要经过工具注册流程，在该流程中，工具被放置于定位仪的测量视场内，定位系统记录下各led灯珠在工具自身坐标系中的参考三维坐标，并同时记录工具在注册时刻相对于定位仪的空间旋转姿态信息。

2、目前，光学定位工具追踪主要采用基于单一注册角度参考数据的方法。在工具注册阶段，通常仅记录工具在单个姿态下的标记点坐标和旋转姿态作为追踪参考。在后续实时追踪过程中，系统始终使用该单一注册姿态下的参考数据与当前观测数据进行刚体变换解算，以确定工具的当前位置和姿态。

3、然而，在实际应用中，工具在使用过程中会绕其主轴连续旋转，呈现不同的空间姿态。由于led灯珠的发光各向异性以及定位系统在不同视角下的成像差异，当工具旋转至不同角度时，定位系统输出的工具姿态与工具注册时的参考姿态之间存在与旋转角度相关的系统性偏差。这种偏差具有较强的可重复性，在同一旋转角度下偏差大小和方向基本一致，但随旋转角度变化而变化。现有技术仅使用一个注册角度的参考数据，无法表征工具在不同旋转角度下的真实标记点布局和对应的姿态关系，亦无法覆盖工具在实际操作中可能经历的全部旋转范围，更缺乏根据工具当前旋转角度自适应选择匹配参考数据的机制，导致工具在非注册角度下使用时定位精度下降，追踪稳定性不足。

4、因此，现有光学定位工具追踪方法中，由于led发光各向异性和视角效应导致不同旋转角度下存在系统性定位偏差，以及缺乏根据当前旋转角度自适应选择匹配参考数据的机制，使得工具在非注册角度下追踪精度和稳定性不足。

技术实现思路

1、本发明的主要目的为提供一种基于姿态匹配的工具追踪方法、装置、设备和介质，旨在解决上述技术问题。

2、为了实现上述发明目的，本发明第一方面提出一种基于姿态匹配的工具追踪方法，所述方法包括：

3、获取光学定位工具的当前观测姿态；其中，所述光学定位工具上装配有标记对象，所述当前观测姿态包含当前标记对象坐标和当前旋转向量；

4、根据所述当前观测姿态，从预构建的工具姿态数据库中匹配得到当前参考姿态；其中，预构建的工具姿态数据库包含所述光学定位工具在若干个旋转角度下的参考姿态；

5、根据所述当前标记对象坐标、所述当前参考姿态中的标记对象坐标和预设的刚体变换算法，得到所述光学定位工具的当前追踪位姿。

6、进一步地，所述根据所述当前观测姿态，从预构建的工具姿态数据库中匹配得到当前参考姿态之前，包括：

7、控制所述光学定位工具处于若干个预设旋转角度，并获取在每个预设旋转角度下的标记对象图像；

8、对所述标记对象图像进行特征处理与空间解算，确定标记对象坐标和旋转向量，得到所述参考姿态；其中，所述旋转向量为所述光学定位工具相对于定位仪的旋转向量；

9、根据若干个预设旋转角度下的所述参考姿态，构建所述工具姿态数据库。

10、进一步地，所述获取在每个预设旋转角度下的标记对象图像，包括：

11、获取在每个预设旋转角度下的若干帧标记对象图像；

12、所述对所述标记对象图像进行特征处理与空间解算，确定标记对象坐标和旋转向量，得到所述参考姿态，包括：

13、对若干帧所述标记对象图像进行特征处理与空间解算，得到若干个标记对象坐标和若干个旋转向量；

14、剔除若干个所述标记对象坐标和若干个所述旋转向量中的离群值；

15、对剔除后的标记对象坐标进行算术平均，得到平均化后的标记对象坐标；

16、对剔除后的旋转向量进行算术平均，得到平均化后的旋转向量；

17、根据所述平均化后的标记对象坐标和所述平均化后的旋转向量，得到所述参考姿态。

18、进一步地，所述参考姿态包含标记对象坐标和旋转向量，所述根据所述当前观测姿态，从预构建的工具姿态数据库中匹配得到当前参考姿态，包括：

19、计算所述当前旋转向量与预构建的工具姿态数据库中各个旋转向量之间的差异度，得到差异度最小的旋转向量；

20、根据所述差异度最小的旋转向量，确定所述当前参考姿态。

21、进一步地，所述计算所述当前旋转向量与预构建的工具姿态数据库中各个旋转向量之间的差异度，得到差异度最小的旋转向量，包括：

22、计算所述当前旋转向量与预构建的工具姿态数据库中各个所述旋转向量之间的欧氏距离，得到欧氏距离最小的旋转向量。

23、进一步地，所述根据所述当前观测姿态，从预构建的工具姿态数据库中匹配得到当前参考姿态之后，还包括：

24、获取上一参考姿态；

25、若所述当前参考姿态与上一参考姿态不同，则启动预设长度的过渡期；

26、在所述过渡期内，所述根据所述当前标记对象坐标、所述当前参考姿态中的标记对象坐标和预设的刚体变换算法，得到所述光学定位工具的当前追踪位姿，包括：

27、根据所述当前标记对象坐标、所述当前参考姿态中的标记对象坐标和预设的刚体变换算法，计算所述光学定位工具的第一追踪位姿；

28、根据所述当前标记对象坐标、所述上一参考姿态中的标记对象坐标和预设的刚体变换算法，计算所述光学定位工具的第二追踪位姿；

29、获取权重系数；其中，所述权重系数在所述过渡期内动态变化；

30、根据所述权重系数，对所述第一追踪位姿和所述第二追踪位姿进行加权融合，得到所述光学定位工具的当前追踪位姿。

31、进一步地，所述第一追踪位姿包括第一旋转矩阵和第一平移向量，所述第二追踪位姿包括第二旋转矩阵和第二平移向量，所述根据所述权重系数，对所述第一追踪位姿和所述第二追踪位姿进行加权融合，得到所述光学定位工具的当前追踪位姿，包括：

32、根据所述第一旋转矩阵、所述第二旋转矩阵、所述权重系数和预设的球面线性插值函数，计算所述当前旋转矩阵；

33、根据所述第一旋转矩阵、所述第二旋转矩阵、所述权重系数和预设的平移线性插值函数，计算所述当前平移向量；

34、根据所述当前旋转矩阵和所述当前平移向量，得到所述光学定位工具的当前追踪位姿。

35、本发明第二方面提出一种基于姿态匹配的工具追踪装置，包括：

36、获取单元，用于获取光学定位工具的当前观测姿态；其中，所述光学定位工具上装配有标记对象，所述当前观测姿态包含当前标记对象坐标和当前旋转向量；

37、第一处理单元，用于根据所述当前观测姿态，从预构建的工具姿态数据库中匹配得到当前参考姿态；其中，预构建的工具姿态数据库包含所述光学定位工具在若干个旋转角度下的参考姿态；

38、第二处理单元，用于根据所述当前标记对象坐标、所述当前参考姿态中的标记对象坐标和预设的刚体变换算法，得到所述光学定位工具的当前追踪位姿。

39、本发明第三方面提出一种计算机设备，包括存储器和处理器，所述存储器中存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如上述第一方面所述的基于姿态匹配的工具追踪方法的步骤。

40、本发明第四方面提出一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上述第一方面所述的基于姿态匹配的工具追踪方法的步骤。

41、本发明通过多角度注册建立了覆盖工具常用旋转范围的参考姿态数据库，运行时基于当前观测姿态中的旋转向量自适应匹配最接近的参考姿态，有效补偿了因led发光各向异性和视角效应导致的与旋转角度相关的系统性定位偏差。与现有技术仅使用单一注册角度参考数据的方法相比，本发明确保工具在任意旋转角度下都能获得与当前姿态最匹配的参考数据，显著提升了工具在非注册角度下的定位精度和旋转测量重复性。

42、其次，本发明将复杂的姿态匹配问题转化为旋转向量空间中基于欧氏距离的快速搜索，仅需进行简单的三维向量代数运算即可完成匹配，计算复杂度极低，避免了传统方法中需要逐参考姿态进行完整刚体拟合和全排列枚举的高昂计算开销，对系统帧率和响应速度几乎没有影响，完美适配光学定位系统对实时性的严格要求。

43、再次，本发明允许用户根据实际精度需求灵活选择注册角度的数量和分布密度，在精度和注册效率之间自由权衡。同时，多角度参考姿态的覆盖确保了工具在连续旋转过程中定位输出始终平滑、连续，有效避免了因参考数据与实际姿态不匹配导致的定位跳变。

44、综上所述，本发明在保证实时性的前提下显著提升了光学定位工具在旋转过程中的定位精度和追踪稳定性，突破了传统单一注册角度方法的技术局限，为手术导航等对高精度定位有严格要求的光学追踪应用提供了可靠的技术支撑。