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

摘要

基于多传感器的内窥镜跟踪定位与数字人动态同步显示方法。本发明以三轴陀螺仪，三轴加速度计和三轴磁场传感器组成的多传感器系统为核心，通过陀螺仪的四元数跟踪得到初始姿态，并通过加速度计和磁场传感器的逐步收敛，将陀螺仪的全空间发散误差进行周期修正，从而达到旋转姿态的精确跟踪。将该系统与人体附近数个励磁线圈相结合，利用励磁线圈的中心对称性，实现了对传感载体的实时位置确定。在医学上为了便于医生进行观察对比，本发明结合数字人技术将姿态和位置信息导入，实现了对内镜在人体内姿态和方位的模型跟踪和透视观察。

主权项

1.基于多传感器的内窥镜跟踪定位与数字人动态同步显示方法，其特征在于该方法的步骤是：第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；第4、利用三轴加速度传感器和三轴磁场传感器对C1的发散误差进行全局收敛，阻止误差的进一步发散，该收敛过程分为两步，具体如下：第4.1、采用三轴加速度传感器，通过该传感器得到坐标系B1下的重力加速度测量值S1=[gx,gy，gz]，将该测量值与第1步的初始测量值S0比较，建立误差观测函数Fg=|S1-C1*S0|，进行一阶梯度微分迭代，将带有误差的转换矩阵C1进行收敛，得到转换矩阵Cg，此方法的效果是将发散误差限制到围绕重力加速度的旋转角这一自由度上；第4.2、采用三轴磁场传感器，通过该传感器得到坐标系B1下地磁场测量值H1=[x1,y1,z1]，将该测量值与第1步的初始测量值H0比较，在第4.1步收敛的基础上建立地磁场误差观测函数Fm=|H1-Cg*H0|，将第4.1步收敛后的姿态绕重力轴旋转，旋转过程中，搜索观测函数Fm的最小值，得到全自由度收敛结果C2，并以C2作为下个定位周期第2步的积分初值；第1步仅在初始化运行一次即可，由第2步至第4步得到一个周期下全空间姿态的收敛定位结果，第5、载体坐标位置定位在人体体腔下侧空间分布有一组至少3个不同位置和朝向的励磁线圈，每一个励磁线圈均为多匝的圆柱形铜线密绕电感线圈，在不同时刻对这些励磁线圈循环供电，每一个励磁线圈在产生磁场时，由于励磁线圈磁场分布的几何对称性，使得励磁线圈在载体处的磁场强度矢量M2始终位于励磁线圈中心与载体连线所在的一个平面A上，该平面A垂直于励磁线圈平面Q，且将Q平分；每一个中心对称的励磁线圈结合该线圈在磁场传感器上的分布就能够确定载体所在的一个法平面A；具体过程如下，首先依据三轴磁场传感器获得励磁线圈在载体上的磁场分布M2=[x2,y2,z2]，然后结合第2步至4步得到的姿态结果C2，将磁场矢量M2投影到励磁线圈所在的平面Q上，M2在平面Q的X轴、Y轴上分解为MQX，MQY，从而得到法平面A相对平面Q的圆柱坐标系下的旋转角a=arctan(MQY/MQX)，通过空间建模，得到载体所在的法平面A的方程表达，由至少三个法平面相交得到空间坐标位置；第6、数字人同步显示在PC中，对多传感器系统的信息进行第2步至第5步的处理后，得到载体的角度和位置结果，利用多线程技术，在另一个线程中，结合数字人技术，根据人体的身高和体重的不同比例，缩放数字人的各个器官和外形轮廓；同步显示前首先要输入被测人体的身高和体重，然后在该线程中将数字人各器官，按照3D模型显示出来，同时从传感计算的线程中传输姿态角度和位置信息，并反映在3D人体模型下的内窥镜的位置比例和朝向上，通过不断刷新，实现对内窥镜的同步模型跟踪，在软件操作下，对3D模型进行旋转，放大，和高亮显示，方便医生进行可视化诊断。