基于光学和电磁定位及卡尔曼滤波的骨针位置跟踪方法

本发明属于手术导航，尤其涉及骨针针尖位置的跟踪。

背景技术：

1、骨盆骨折是一种严重的高能量损伤，在所有骨折中致残率和死亡率最高。因此，骨盆复位机器人的复位力更大且复位路径更加复杂，这也对导航系统的精度产生影响，且手术过程中手术器械被遮挡的概率高，传统的手术导航光学定位方法很难满足不被遮挡的要求。

2、骨盆骨折复位手术由于骨盆具有复杂的解剖结构位置在于人体深处，术中难以显露，且其骨折类型复杂，种类繁多，故骨盆骨折手术是骨科领域难度最高的手术之一，被称为骨科领域的“专家级手术”。近年来，随着机器人辅助微创手术的发展，导航系统的引入能提高定位精度，减少手术器械的抖动，能够最大程度地帮助外科医生精准地完成手术。手术导航使外科医生能够在手术过程中实时可视化患者身体的内部结构。在手术过程中，外科医生可以使用该导航系统建立的地图来更合理地规划复位路径，从而确保更高的准确性并降低对周围组织损伤的风险。导航系统还可以在手术过程中提供器械的位置和运动的实时反馈。因此，构建一套精准的骨盆骨折复位手术导航系统对骨盆复位机器人至关重要。

3、考虑到不同的手术环境需求，导航系统会选择不同的定位方法，目前手术导航系统的定位方法主要有光学定位、电磁定位、超声波定位、图像定位等。约翰斯·霍普金斯大学研制出基于计算机断层扫描显像(compute-dtomography，简称ct)的导航机器人系统，哈尔滨工业大学机器人研究所开发了一种利用x光影像进行机器人辅助外科手术的hit-raos，德国研制出一套基于光学跟踪系统的导航系统。

4、到目前为止，光学和电磁跟踪系统代表了集成到手术导航系统中的两种主要技术，用于计算机辅助图像引导手术。光学跟踪系统具有很高的精度及对环境的鲁棒性，但是必须使光学靶点和相机之间有直接的视线范围；电磁跟踪系统不需要考虑传感器与外部参考间是否有视线的遮挡，但是空间内存在铁磁源会对测量结果产生显著影响。因此，单一的定位方法总存在不可避免的缺陷，故一些学者采用多种传感器融合的方法以提高导航精度。xinyang liu等通过传感器融合的方式，结合电磁跟踪技术和基于计算机视觉的(例如aruco)跟踪方法开发了一种在腹腔镜手术中，跟踪腹腔镜超声换能器的新方法，得到的传感跟踪数据精度要好于单一使用电磁发生器；chao shi等开发了一种新型的骨盆骨折复位的机器人系统，通过光学跟踪技术和非刚性配准算法的融合，实现了骨盆位置的实时3d导航技术；zhicheng yang等提出了一种利用卡尔曼滤波来融合光学跟踪数据和陀螺仪传感数据的方法，因此使用陀螺仪对其方向进行估计并补充，并通过卡尔曼滤波进行数据融合可以很好的提高跟踪精度；yongde zhang等提出将惯性导航和电磁导航的数据利用卡尔曼滤波进行了融合，实现了较为精确的术中器官运动的跟踪技术。

5、相对于长骨复位手术只需要轴向运动，骨盆骨针复位手术的路径更加复杂、复位力更加大，如采用传统的在复位骨针根部安装光学导航靶点进行光学导航，在复位的过程中，由于路径复杂很可能会导致光学靶点被遮挡丢失跟踪目标，使导航系统失效。且由于复位力较大导致骨针会发生较大变形，导致骨针末端的光学靶点不能真实的反映骨块的位姿。

技术实现思路

1、本发明是为了解决单一的电磁定位方法精度较低且会受磁场干扰，单一的光学定位方法光学信号易被遮挡导致丢失跟踪目标的问题，现提供基于光学和电磁定位及卡尔曼滤波的骨针位置跟踪方法。

2、基于光学和电磁定位及卡尔曼滤波的骨针位置跟踪方法，具体为：

3、利用光学传感器和电磁传感器跟踪骨针针尖的位置，分别获得光学跟踪和电磁跟踪时骨针针尖的位置点集，将光学跟踪和电磁跟踪时骨针针尖的位置点集统一到相同坐标系下，并构建骨针针尖的位置观测向量，然后将该位置观测向量代入骨针针尖的位置观测方程中，计算获得骨针针尖的运动状态向量，实现骨针针尖位置的实时跟踪。

4、进一步的，所述光学传感器包括：第一组光学靶点1、第二组光学靶点5和光学跟踪装置3，所述电磁传感器包括：电磁靶点2和电磁跟踪装置4，所述第一组光学靶点1和电磁靶点2相对静止地设置在骨针上，所述第二组光学靶点5设置在电磁跟踪装置4上；通过旋转标定的方式将第一组光学靶点1和电磁靶点2的跟踪位置均注册到骨针针尖的位置，并在旋转标定过程中实时跟踪骨针针尖的位置，利用骨针针尖的位置分别计算光学跟踪和电磁跟踪时骨针针尖的位置点集。

5、进一步的，所述光学跟踪和电磁跟踪时骨针针尖的位置点集的获得方法为：

6、根据下式计算光学跟踪第一组光学靶点1时骨针针尖在opm1坐标系下的位置点集

7、

8、根据下式计算电磁跟踪电磁靶点2时骨针针尖在em坐标系下的位置点集

9、

10、其中，为op坐标系至opm1坐标系的转换矩阵，为opm2坐标系至op坐标系的转换矩阵，为旋转标定过程中获得的opm2坐标系下骨针针尖的位置点集，为ems坐标系至em坐标系的转换矩阵，为旋转标定过程中获得的ems坐标系下骨针针尖的位置点集，

11、op坐标系表示光学跟踪装置3的坐标系，opm1坐标系表示第二组光学靶点5的坐标系，opm2坐标系表示第一组光学靶点1的坐标系，em坐标系表示电磁跟踪装置4的坐标系，ems表示电磁靶点2的坐标系。

12、进一步的，所述将光学跟踪和电磁跟踪时骨针针尖的位置点集统一到相同坐标系下，包括：

13、根据下式将统一到opm1坐标系下：

14、

15、其中，为em坐标系至opm1坐标系的转换矩阵，为电磁跟踪电磁靶点2时骨针针尖在opm1坐标系下的位置点集。

16、进一步的，利用最小二乘拟合配准方法计算

17、

18、其中，n为骨针针尖的位置点集中位置点的数量，i＝1,2,...,n，c为位置误差模型系数，为中第i个位置点的位置信息，(xi,yi,zi)为中第i个位置点的位置信息。

19、进一步的，所述骨针针尖的位置观测向量z(k)的表达式为：

20、z(k)＝[pot(k) pet(k)]t，

21、pot(k)为中k时刻骨针针尖的位置点坐标，pet(k)为中k时刻骨针针尖的位置点坐标。

22、进一步的，所述骨针针尖的位置观测方程表达式如下：

23、z(k)＝hx(k)+v(k)，

24、其中，h为观测矩阵且有i3为三维单位矩阵，03为三维零矩阵，v(k)为测量噪声矩阵，x(k)为骨针针尖的运动状态向量。

25、进一步的，所述测量噪声矩阵v(k)服从v(k)～n(0,r)，

26、其中，和分别为第一组光学靶点1和电磁靶点2的误差均方根。

27、进一步的，所述骨针针尖的运动状态向量x(k)的表达式为：

28、

29、其中，pt(k)为k时刻跟踪到骨针针尖的位置坐标，为k时刻跟踪到骨针针尖的运动速度。

30、进一步的，所述光学传感器为六自由度光学传感器，且所述第一组光学靶点1位于骨针末端；电磁传感器为六自由度电磁传感器，且所述电磁靶点2位于骨针内部且靠近针尖。

31、本发明首先在骨针上安装光学传感器和电磁传感器，然后分析了光学跟踪和电磁跟踪的误差模型。在此基础上，完成了智能骨针上光学靶点和电磁靶点的旋转标定，使光学六维传感器和电磁六维传感器同时跟踪骨针针尖位置，再通过点云配准得到了光学坐标系和电磁坐标系的转换关系，统一了手术导航系统的坐标系。最后利用卡尔曼滤波融合电磁和光学定位信息，建立了基于卡尔曼滤波的电磁和光学融合的定位导航系统。并在光学传感器被遮挡，缺失光学靶点信息的情况下采用电磁传感器来代替光学传感器信息进行传感器融合，达到在复杂手术环境下仍能鲁棒性跟踪骨针针尖位置的效果，让此系统在复杂路径和大负荷的手术中，能够更加准确的表示骨组织的位姿。