[发明专利]无线胶囊内窥镜三维位置和三维姿态的定位系统及其定位方法

权利要求书

1.无线胶囊内窥镜三维位置和三维姿态的定位系统，包括设置于人体外且三轴正交的发射线圈、体外无线接收模块、体外位姿计算模块和位于体内的无线胶囊内窥镜，其特征在于：

所述发射线圈由三轴正交的线圈I、线圈II和线圈III组成，所述线圈I、线圈II和线圈III顺序发射各自固定频率的信号，线圈I、线圈II、线圈III发射完一次信号形成一个周期；

所述无线胶囊内窥镜内设置有二轴正交的感应线圈，所述二轴正交的感应线圈对应发射线圈的一个周期内输出三组不同频率的电压信号，且二轴正交的感应线圈输出的每组不同频率的电压信号个数为二个；

所述发射线圈与无线胶囊内窥镜之间通过交变磁场形成磁路，所述无线胶囊内窥镜与体外无线接收模块之间通过无线信号连接，所述位姿计算模块与体外无线接收模块之间直接连接。

2.根据权利要求1所述的无线胶囊内窥镜三维位置和三维姿态的定位系统，其特征在于，所述无线胶囊内窥镜还包括信号放大模块、AD转换模块和无线发送模块，所述二轴正交的感应线圈与信号放大模块直接连接，所述信号放大模块与AD转换模块直接连接，所述AD转换模块与无线发送模块直接连接。

3.无线胶囊内窥镜三维位置和三维姿态的定位方法，所述无线胶囊内窥镜内设置有二轴正交的感应线圈，其特征在于，包括以下步骤：

步骤1、在人体外设置三轴正交的发射线圈，发射线圈由三轴正交的线圈I、线圈II和线圈III组成；

步骤2、发射线圈所在三个轴建立的坐标系OXYZ作为参考坐标系，感应线圈二个轴的交点设置在无线胶囊内窥镜中心点，作为无线胶囊内窥镜的位置，其在参考坐标系中表示为(x,y,z)，感应线圈所在的二个轴建立的坐标系O'u_xu_y u_z作为物体坐标系，用单位向量u_x和u_y指示无线胶囊内窥镜的姿态；

步骤3、上电后，发射线圈的线圈I、线圈II和线圈III在每一个周期内顺序发射各自固定频率的信号；

步骤4、无线胶囊内窥镜内的放大模块放大感应线圈的输出电压；

步骤5、无线胶囊内窥镜内AD转换模块采样经过放大的输出电压；

步骤6、无线胶囊内窥镜内的无线发送模块发送采样信号；

步骤7、体外无线接收模块接收采样信号，并发送给位姿计算模块；

步骤8、位姿计算模块的定位过程如下：

求解9个参数(x,y,z,u_xx,u_xy,u_xz,u_yx,u_yy,u_yz)，其中(u_xx,u_xy,u_xz)和(u_yx,u_yy,u_yz)分别表示u_x和u_y在参考坐标系X、Y、Z轴的投影分量；

将每个轴的发射线圈都等效为磁偶极子，根据毕奥萨法尔定律，磁偶极子在无线胶囊内窥镜位置处产生的磁通量密度沿参考坐标系的X、Y、Z轴的三个正交分量如公式(1)、(2)、(3)所示：

其中，(m，n，p)是各轴发射线圈的方向向量，(x，y，z)是感应线圈的位置，(a，b，c)是发射线圈的位置，B_T是与发射线圈有关的一个常量，L是感应线圈到发射线圈的距离，L如公式(4)所示：

由于物体坐标系发生了偏转，物体坐标系的坐标轴与参考坐标系的坐标轴不重合，发射线圈在无线胶囊内窥镜位置产生的磁通量密度在物体坐标系下的值为公式(5)所示：

其中，R为方位矩阵，如公式(6)所示，由于只有两个感应线圈，所以B'_z不激发线圈输出电压；

其中

(u_xx,u_xy,u_xz)＝u_x (7)

(u_yx,u_yy,u_yz)＝u_y (8)

感应线圈输出的是感应电压信号，根据法拉第电磁感应定律，感应线圈产生的感应电动势为公式(9)所示：

其中，N为感应线圈匝数，φ为穿过曲面S的磁通量；

在u_x和u_y方向上，感应线圈输出的电压信号与磁通量密度之间的关系如下：

由于感器线圈很小，故将其体积忽略，认为感应线圈各处磁通量密度相等，所以公式(10)和(11)变为如下公式(12)和(13)，

因为感应线圈的方向与物体坐标系的坐标轴方向相同，所以得到如下公式(14)和(15)，

若发射已知频率的正弦信号，物体坐标系下的磁通量密度可以描述为如下公式(16)：

至此，可以得出感应线圈u_x、u_y轴输出电压值与物体坐标系各轴上磁通量密度之间的关系式，如公式(17)、(18)所示：

感应线圈的输出电压信号是与发射信号同频率的余弦信号，取该信号的幅值来建立方程组，设E_Tx＝-ω_xN_x·S_x，E_Ty＝-ω_yN_y·S_y，E_Tz＝-ω_zN_z·S_z，得到方程(19)、(20)如下：

ε_xmax＝-ωN_x·B'_xmax·S_x＝E_Tx·B'_xmax (19)

ε_ymax＝-ωN_y·B'_ymax·S_y＝E_Ty·B'_ymax (20)

提取余弦信号幅值和相位的方法有快速傅里叶变换或函数拟合方法；

若三个轴的发射线圈依次激励各自固定频率的正弦信号，二轴感应线圈总计感应到6组交变信号，从而可以建立6个方程，由于要求解9个未知参数，所以还需要3个方程；由于u_x和u_y取单位向量，并且相互垂直，所以再增加如下3个约束方程，3个方程式(21)、(22)、(23)如下所示：

u_xx.u_yx+u_xy.u_yy+u_xz.u_yz＝0 (23)

下面分别对发射线圈的三个轴进行分析：

线圈I对应于参考坐标系的X轴，其位置和方向参数为

(a，b，c)＝(0，0，0)

(m，n，p)＝(1，0，0)

将其带入公式(1)、(2)和(3)中，得到公式(24)、(25)和(26)如下：

线圈II对应于参考坐标系的Y轴，其位置和方向参数为

(a，b，c)＝(0，0，0)

(m，n，p)＝(0，1，0)

将其带入公式(1)、(2)和(3)中，得到公式(27)、(28)和(29)如下：

线圈III对应于参考坐标系的Z轴，其位置和方向参数为

(a，b，c)＝(0，0，0)

(m，n，p)＝(0，0，1)

将其带入公式(1)、(2)和(3)中，得到公式(30)、(31)和(32)如下：

根据公式(5)、(19)和(20)，再定义公式(33)如下：

其中，B_ixmax、B_iymax和B_izmax分别是三轴发射线圈的线圈I、线圈II和线圈III发射时，在感应线圈处产生的磁通量密度沿参考坐标系的X、Y、Z轴三个分量的幅值，即是B_ix、B_iy和B_iz的幅值；ε_ixmax和ε_iymax分别是三轴发射线圈的线圈I、线圈II和线圈III发射时，无线胶囊内窥镜内二轴感应线圈的u_x和u_y轴感应电压的理论幅值，ε_izmax没有对应的感应线圈输出，所以不参与计算；

设ε'_ixmax和ε'_iymax分别是发射线圈的线圈I、线圈II和线圈III发射时，无线胶囊内窥镜内二轴感应线圈的u_x和u_y轴感应电压的实际输出值，即测量值，定义误差E的公式(34)如下:

利用优化算法Levenberg-Marquardt或Gauss-Newton算法，使E最小，可以求解出无线胶囊内窥镜的位姿参数(x,y,z,u_xx,u_xy,u_xz,u_yx,u_yy,u_yz)；

步骤9、位姿计算模块将无线胶囊内窥镜的位姿信息发送给显示终端，实时反映当前无线胶囊内窥镜的位姿，便于操作者观察或后续应用。