CN106983487B - 无线胶囊内窥镜三维位置和三维姿态的定位***及其定位方法 - Google Patents

Abstract

本发明提供了无线胶囊内窥镜三维位置和三维姿态的定位***及其定位方法，包括设置于人体外且三轴正交的发射线圈、体外无线接收模块、体外位姿计算模块和位于体内的无线胶囊内窥镜，采用在人体外设置一个三轴正交的发射线圈，在无线胶囊内窥镜内只布置一个二轴正交的感应线圈，发射线圈放置在人体附近，发射线圈的线圈I、线圈II和线圈III顺序发射各自固定频率的信号，三轴发射线圈发射完一次信号称为一个周期，二轴感应线圈在一个周期内感应输出三组不同频率的电压信号，从而建立方程组进行三维位置和三维姿态的计算，该方法集成方便、二轴感应线圈占用无线胶囊内窥镜空间小，能实时连续对无线胶囊内窥镜定位，方便后续操作，安全可靠，成本低廉。

Description

无线胶囊内窥镜三维位置和三维姿态的定位***及其定位 方法

【技术领域】

本发明涉及三维定位技术，尤其涉及无线胶囊内窥镜三维位置和三维姿态的定位***及其定位方法。

【背景技术】

当前，临床的无线胶囊内窥镜缺点之一就是不能主动受控，要实现这一主动受控功能并方便临床使用，无线胶囊内窥镜的三维位置和三维姿态信息要实时反馈给控制***；同时，无线胶囊内窥镜在重建的消化道里视觉导航也需要三维位置和三维姿态信息，医生才能方便操控无线胶囊窥镜。

针对以上问题，现有技术中普遍利用X光成像、CT(计算机断层扫描)和MRI(核磁共振)成像技术进行三维重建来定位无线胶囊内窥镜，这种由重建技术间接定位的速度和精度都受到影响，并且射线存在损害，不宜长时间定位。

以色列的Given Imaging公司最早提出了一种应用于无线胶囊内窥镜定位的无线射频(RF)信号定位技术。其利用在人体外的8个无线射频天线接收体内无线胶囊内窥镜发射的无线射频信号，并利用算法获取无线胶囊内窥镜的位置。该方法直接利用了无线胶囊内窥镜传输图象的无线射频信号，缺点是定位精度低，平均定位精度为37.7毫米，临床应用效果不好。

也有人提出了利用永磁定位技术定位无线胶囊内窥镜。在无线胶囊内窥镜内部放置永磁体作为磁标记，在人体周围布置多个磁场传感器测量不同点的磁场，用算法计算出无线胶囊内窥镜的三维位置和镜头对准(二维)方向。该技术具有精度高，定位速度快的优点，但无法确定无线胶囊内窥镜绕主轴旋转的方向变化信息，这一维信息缺失就不能利用无线胶囊内窥镜拍摄的图像进行消化道三维重建；另外，由于永磁体的磁场强度随距离增加快速衰减，磁场传感器和磁标记间的有效距离难以满足人体尺寸要求。

也有人提出基于永磁和感应线圈的定位方法，即利用机械臂控制永磁体的位置和方向，让永磁体在震动模块的作用下作往复运动产生变化的磁场，胶囊内的三轴感应线圈输出感应电动势。此方法需要在无线胶囊内窥镜内布置两个三轴感应线圈才能定位，要增大无线胶囊内窥镜的体积。

【发明内容】

为了解决现有技术不足，本发明的目的在于提供一种只采用一个二轴感应线圈，集成方便，占用无线胶囊内窥镜空间小，可实时连续对无线胶囊内窥镜定位，方便后续操作的无线胶囊内窥镜三维位置和三维姿态的定位***及其定位方法。

为了实现上述发明目的，本发明采用的技术方案是：

该发明的第一发明目的，提供无线胶囊内窥镜三维位置和三维姿态的定位***，包括设置于人体外且三轴正交的发射线圈、体外无线接收模块、体外位姿计算模块和位于体内的无线胶囊内窥镜，所述发射线圈由三轴正交的线圈I、线圈II和线圈III组成，所述线圈I、线圈II和线圈III顺序发射各自固定频率的信号，线圈I、线圈II、线圈III发射完一次信号形成一个周期；所述无线胶囊内窥镜内设置有二轴正交的感应线圈，所述二轴正交的感应线圈对应发射线圈的一个周期内输出三组不同频率的电压信号，且二轴正交的感应线圈输出的每组不同频率的电压信号个数为二个；所述发射线圈与无线胶囊内窥镜之间通过交变磁场形成磁路，所述无线胶囊内窥镜与体外无线接收模块之间通过无线信号连接，所述位姿计算模块与体外无线接收模块之间直接连接。

优选地，所述无线胶囊内窥镜还包括信号放大模块、AD转换模块和无线发送模块，所述二轴正交的感应线圈与信号放大模块直接连接，所述信号放大模块与AD转换模块直接连接，所述AD转换模块与无线发送模块直接连接。

该发明第二发明目的，提供无线胶囊内窥镜三维位置和三维姿态的定位方法，包括以下步骤：

步骤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)如下：

ε_{x max}＝-ωN_x·B'_{x max}·S_x＝E_Tx·B'_{x max} (19)

ε_{y max}＝-ωN_y·B'_{y max}·S_y＝E_Ty·B'_{y max} (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_{ix max}、B_{iy max}和B_{iz max}分别是三轴发射线圈的线圈I、线圈II和线圈III发射时，在感应线圈处产生的磁通量密度沿参考坐标系的X、Y、Z轴三个分量的幅值，即是B_ix、B_iy和B_iz的幅值；ε_{ix max}和ε_{iy max}分别是三轴发射线圈的线圈I、线圈II和线圈III发射时，无线胶囊内窥镜内二轴感应线圈的u_x和u_y轴感应电压的理论幅值，ε_{iz max}没有感应线圈输出，所以不参与计算；

设ε′_{ix max}和ε′_{iy max}分别是发射线圈的线圈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、位姿计算模块将无线胶囊内窥镜的位姿信息发送给显示终端，实时反映当前无线胶囊内窥镜的位姿，便于操作者观察或后续应用。

本发明的有益效果是：

本发明采用在人体外设置一个三轴正交的发射线圈，在无线胶囊内窥镜内只布置一个二轴正交的感应线圈，发射线圈放置在人体附近，发射线圈的线圈I、线圈II和线圈III顺序发射各自固定频率的信号，三轴发射线圈发射完一次信号称为一个周期，二轴感应线圈在一个周期内感应输出三组不同频率的电压信号，从而建立方程组进行无线胶囊内窥镜三维位置和三维姿态的计算，该方法集成方便、二轴感应线圈占用无线胶囊内窥镜空间小，能实时连续对无线胶囊内窥镜定位，方便后续操作，安全可靠、成本低廉。

【附图说明】

图1是本发明体外三轴发射线圈及其对应的无线胶囊内窥镜内部二轴线圈的放大结构示意图；

图2是本发明的定位流程图。

【具体实施方式】

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。

无线胶囊内窥镜三维位置和三维姿态的定位***，如图1所示，包括设置于人体外且三轴正交的发射线圈、体外无线接收模块、体外位姿计算模块和位于体内的无线胶囊内窥镜，所述发射线圈由三轴正交的线圈I、线圈II和线圈III组成，所述线圈I、线圈II和线圈III顺序发射各自固定频率的信号，线圈I、线圈II、线圈III发射完一次信号形成一个周期；所述无线胶囊内窥镜内设置有二轴正交的感应线圈，所述二轴正交的感应线圈对应发射线圈的一个周期内输出三组不同频率的电压信号，且二轴正交的感应线圈输出的每组不同频率的电压信号个数为二个；所述发射线圈与无线胶囊内窥镜之间通过交变磁场形成磁路，所述无线胶囊内窥镜与体外无线接收模块之间通过无线信号连接，所述位姿计算模块与体外无线接收模块之间直接连接。

其中，无线胶囊内窥镜还包括信号放大模块、AD转换模块和无线发送模块，所述二轴正交的感应线圈与信号放大模块直接连接，所述信号放大模块与AD转换模块直接连接，所述AD转换模块与无线发送模块直接连接。

该实施例的无线胶囊内窥镜三维位置和三维姿态的定位方法，如图2所示，包括以下步骤：

步骤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)如下：

ε_{x max}＝-ωN_x·B'_{x max}·S_x＝E_Tx·B'_{x max} (19)

ε_{y max}＝-ωN_y·B'_{y max}·S_y＝E_Ty·B'_{y max} (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_{ix max}、B_{iy max}和B_{iz max}分别是三轴发射线圈的线圈I、线圈II和线圈III发射时，在感应线圈处产生的磁通量密度沿参考坐标系的X、Y、Z轴三个分量的幅值，即是B_ix、B_iy和B_iz的幅值；ε_{ix max}和ε_{iy max}分别是三轴发射线圈的线圈I、线圈II和线圈III发射时，无线胶囊内窥镜内二轴感应线圈的u_x和u_y轴感应电压的理论幅值，ε_{iz max}没有感应线圈输出，所以不参与计算；

设ε′_{ix max}和ε′_{iy max}分别是发射线圈的线圈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、位姿计算模块将无线胶囊内窥镜的位姿信息发送给显示终端，实时反映当前无线胶囊内窥镜的位姿，便于操作者观察或后续应用。

以上所述实施例只是为本发明的较佳实施例，并非以此限制本发明的实施范围，除了具体实施例中列举的情况外，凡依本发明原理所作的等效变化，均应涵盖于本发明的保护范围内。

Claims (3)

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