[发明专利]基于多传感器的内窥镜跟踪定位与数字人动态同步显示方法

说明书

技术领域

本发明属于多传感器跟踪定位领域，尤其适合内窥镜探头等微小区域的方位探测。

背景技术

在医疗上，医生一般通过内窥镜采集到的图像，依靠经验来对病变位置进行判断，具有较大误差。而在一些特殊场合下，医生需要结合内窥镜得到的病变位置信息，配合手术等措施进行综合治疗，因此对病变位置的方位精度要求较高。在日本、美国一些发达国家，胶囊内窥镜应用逐渐普遍，对其定位和控制也是目前的研究热点。

目前国内外采用的内窥镜定位技术，多为采用以磁场分布为核心的测算方法，分为体内置磁场传感器，体外多点磁场源，或者体内磁场发射源，体外数十个探测器阵列两种。第一种方式上，现有的一些专利技术提出了利用加速度传感器结合地磁传感器进行姿态解算，将内窥镜姿态和位置定位分别处理的方法，但由于欧拉旋转矩阵算法的“奇异性”以及地磁场的晃动问题，姿态解算的精度有限，连续性和鲁棒性不好。而且在内窥镜的位置定位上，采用三线圈平放，以线圈轴线的磁场分布代替全空间分布，缺乏科学性，定位范围狭小。采用第二种方式，通过数量优势来弥补磁场空间的分布复杂性和不稳定性，成本较高而且精度一般，算法复杂，实时性较差。

而目前的多传感器的姿态定位算法，其核心思想是利用陀螺仪的姿态累积保证姿态跟踪的连续性，以辅助传感器结合卡尔曼滤波方法，来修正陀螺仪的累积误差，从而保证姿态跟踪的精确性，这种方法适合于姿态变化缓慢的大型载体的定位，并不适用于内窥镜这种姿态转动幅度较大，实时性需求较高的场合。

同时，在内窥镜的绝对位置定位上，现有技术有利用机械特性进行定位，精度较低且结构复杂，还有对磁场源进行全空间建模太过于复杂，不易实现，而且全空间建模一般磁场源要求为点源，在远场下对磁场传感器的探测精度要求过高。

数字人技术，目前主要应用在医学图像还原，人体结构分析等领域，大多数是应用在静态的病理分析上，在同步跟踪方面的应用较少。

发明内容

本发明的目的是解决磁场定位系统体积大，成本高，算法复杂，实时性差等问题，将多传感技术引入内窥镜定位中，提供一种基于多传感器的内窥镜跟踪定位与数字人动态同步显示方法，以提高内窥镜镜头姿态的定位连续性和鲁棒性，增加内窥镜位置定位的科学性和定位范围，通过与数字人技术结合，实现了定位信息的动态显示，使内窥镜的定位跟踪结果更加直观。

本发明方法将多传感器模组微缩到内窥镜探头前端，探头作为载体在发生姿态旋转时，通过传感信息的解算，可以得到大地坐标系到载体坐标系的转换矩阵的具体表达，以大地坐标系为基准，转换矩阵就对应了当前载体的姿态。在完成姿态解算的同时，间歇通电人体附近的多个励磁线圈，利用线圈磁场的特殊分布，并结合当前具体姿态，综合得到载体所在的空间位置。之后，由OPEN GL的建模技术，将接受观测的人体的体重和身高输入，在MFC界面仿真得到一个包括器官模型在内的透视的数字人模型，并将姿态和位置信息传输给PC，在数字人内，以合适比例建立动态的内窥镜模型，从而完成姿态和位置的定位，实现内窥镜在人体内移动过程的同步再现。

本发明提供的基于多传感器的内窥镜跟踪定位与数字人动态同步显示方法主要包含姿态定位，位置定位，以及数字人同步显示三个主要部分，在一个定位周期内完成。首先以陀螺仪为核心，配合加速度计和磁场传感器收敛辅助，完成姿态定位与收敛，然后结合人体附近至少三个励磁线圈产生外部磁场完成位置定位，在得到姿态和位置定位结果后，对定位结果显示与更新。本发明所采用的方法，主要包含以下步骤：

第1、多传感器系统位于内窥镜载体上，多传感器系统包括三轴加速度传感器，三轴磁场传感器以及三轴陀螺仪，这三个传感器各自的X,Y,Z轴均与所在载体的X,Y,Z轴平行；首先将内窥镜载体水平放置，此时的坐标系定义为初始坐标系N，记录下三轴加速度传感器和三轴磁场传感器分别于坐标系N下的测量值S0=[0,0,-1]，H0=[x0,y0,z0]，作为姿态定位的初始信息；

第2、通过三轴陀螺仪测量载体在运动过程中的X,Y，Z轴的实时角速度，记载体在实际运动过程中，实时坐标系为B,结合四元数姿态的运动方程，通过积分得到坐标系N到坐标系B的转换矩阵C，通过转换矩阵C计算出B相对N的三轴旋转角α、β、γ，同真实的三轴旋转角相比，误差分别为Δα、Δβ、Δγ；

第3、不断重复第2步的过程，由于三轴陀螺仪的随机漂移效应，第2步的姿态旋转角误差会随着时间逐渐增大，待其总误差绝对值达到3°时进入第4步的收敛，此时载体的坐标系定义为B1，通过步骤2得到的旋转矩阵为C1；