CN113288008B - 一种磁性胶囊内窥镜全姿态测定方法 - Google Patents

Abstract

本发明公开了一种磁性胶囊内窥镜全姿态测定方法，所述胶囊内窥镜内设置的永磁体是径向充磁的永磁体。首先预估磁性胶囊内窥镜的初始姿态；然后调整外部大磁体的姿态，使外部大磁体的姿态与所述磁性胶囊内窥镜的初始姿态一致；接着利用外部大磁体控制所述磁性胶囊内窥镜绕其自身轴向中心轴旋转，计算旋转后磁性胶囊内窥镜内永磁体的当前径向磁化方向单位向量最后计算磁性胶囊内窥镜内永磁体的轴向方向单位向量即完全确定磁性胶囊内窥镜的当前姿态。本发明操作方法简单，没有累计误差，可以解决实际应用中需要同时实现胶囊主动运动与胶囊定位的问题。

Description

一种磁性胶囊内窥镜全姿态测定方法

技术领域

本发明涉及医疗设备技术领域，特别涉及一种磁性胶囊内窥镜全姿态测定方法。

背景技术

无线胶囊内窥镜(以后简称胶囊镜)，以吞服的方式对人体消化道进行医疗诊查，是一种非侵入式医检方式，是内窥镜发展史上的一场革命与未来发展方向。与传统的医用内窥镜相比，胶囊镜具有无创无痛、操作方便、小巧安全、无交叉感染等诸多优点，极大地减轻了患者的痛苦，拓展了医生的检查视野，填补了胃镜、肠镜检查的盲区，尤其对小肠可疑性病变具有很高的诊断价值，具有巨大的市场应用价值。

在胶囊镜的临床使用中，快速、精确、完整地测定胶囊镜空间位置和姿态的全部参数的是最为关键的问题之一。完整地获知胶囊镜在人体消化道内的准确位置和姿态信息能够带来以下技术优势：帮助医生判断病灶组织图片的准确拍摄位置和角度，从而对病症做出准确的判断；为主动控制胶囊镜的运动提供必要的位置姿态数据，从而为医生自主控制胶囊镜进行全方位的检查拍摄或实现指定位置或路线的检查、施药等医疗目的，提供重要的技术支撑。

目前，胶囊镜的空间姿态全参数的快速准确测定还存在着困难。现有的胶囊镜位姿测定方法中，采用在胶囊镜中放置小型永磁体的静磁定位方式，具有无需电源、实现简单、对人体无损伤的特点，很适合胶囊镜这种近距离、高精度的定位需求。

现有的基于静磁的胶囊镜位姿测定方法，是将永磁体作为磁源、利用磁偶极子模型及其修正模型，采用非线性优化算法计算胶囊的6个位姿参数(3个位置参数和3个姿态参数)。具有较好的磁定位精度；但所述的3个姿态参数(m,n,p)具有约束关系(m²+n²+p²＝1)，因此实际上只能确定二维姿态参数，无法确定全部三维姿态参数，即无法获得胶囊镜姿态参数中的横滚角。原因是，现有的胶囊镜设置的内部永磁体一般是轴向磁性的圆环状磁体，且磁体中心轴设置为与胶囊镜中心轴同向，当胶囊镜绕自身轴向旋转时，该轴向磁性永磁体的磁场方向并不发生改变，因此无法根据磁场变换检测出胶囊镜的姿态变化(即横滚角变化)。

虽然有些算法也可以实现矩形永磁体的六维磁定位，但由于矩形永磁体的形状因素，它并不能很好的与胶囊镜相组合，因而并不适合用于胶囊镜。同时，胶囊镜工作环境对于外部是完全视觉隔离的，使得大部分依赖视频识别姿态的测量手段无效。

另一类现有的胶囊镜位姿测定方法采用三轴角速度数据参与位姿求解。但该类方法存在累计误差，即：角速度仪的数据在换算成角度的时候，存在一个积分累计误差，并且该累计误差会随着时间的推移越来越大，导致测量误差较大。虽然现有技术中可使用三轴地磁传感器测出地磁向量对角度进行校准，以消除累计误差，然而由于主动式胶囊的使用环境中，存在较强的驱动胶囊运动的外部磁场，因而地磁传感器无法使用。

因此，如何快速、完整地测定胶囊镜的全部姿态参数，是本领域技术人员亟待解决的技术问题。

发明内容

为了解决上述技术问题，本发明提供了一种磁性胶囊内窥镜全姿态测定方法，能够简单、准确、完整地测定胶囊内窥镜的全部姿态参数和位置参数。

本发明提供的一种磁性胶囊内窥镜全姿态测定方法，采用磁性胶囊内窥镜，所述胶囊内窥镜内设置的永磁体是径向充磁的小永磁体；所述小永磁体是圆柱状或圆环状，固定设置于所述磁性胶囊内窥镜的内部或固定于圆环形磁性胶囊内窥镜外壳壁上，且所述小永磁体的轴向方向与所述磁性胶囊内窥镜的轴向方向一致；所述胶囊内窥镜外部设置有大永磁体，所述大永磁体是径向充磁的圆柱状强永磁铁；

所述方法包括以下步骤：

步骤1：估计磁性胶囊内窥镜的初始姿态，确定所述磁性胶囊内窥镜内小永磁体的径向磁化方向单位向量和轴向方向单位向量

具体实现包括以下子步骤：

步骤1.1：计算所述小永磁体的径向磁化方向单位向量

步骤1.2：根据所述小永磁体的径向磁化方向向量，利用外部大永磁体控制所述磁性胶囊内窥镜旋转；

步骤1.3：计算所述小永磁体旋转后的径向磁化方向单位向量

步骤1.4：由和计算所述磁性胶囊内窥镜的预估轴向方向单位向量

步骤2：调整外部大永磁体的姿态，使外部大永磁体的姿态与所述磁性胶囊内窥镜的当前姿态一致；

步骤3：利用外部大永磁体控制所述磁性胶囊内窥镜绕其自身轴向中心轴旋转；

步骤4：计算旋转后的所述磁性胶囊内窥镜内小永磁体的当前径向磁化方向单位向量

步骤5：计算所述磁性胶囊内窥镜的轴向方向单位向量 即完全确定所述胶囊内窥镜的当前姿态。

本发明可以解决现有的根据角速度数据计算姿态角度方法中的积分累计误差问题，以及解决现有静磁定位方法不能完整测定胶囊内窥镜全部姿态参数的问题，主要解决不能测定胶囊内窥镜绕自身轴向旋转角度的问题。本发明能够完整地测定胶囊内窥镜的全部姿态参数；且操作方法简单，没有累计误差；同时，该方法中的外部永磁体可以同时用于对胶囊内窥镜的主动控制，可很好的解决实际应用中需要同时实现胶囊内窥镜主动运动与胶囊内窥镜定位的问题。

附图说明

图1为本发明实施例的胶囊镜内永磁体与外部大磁体、人体及磁传感器阵列的空间关系示意图；

图2为本发明实施的方法流程图；

图3为本发明实施例的外部大磁体控制磁性胶囊内窥镜旋转原理图；

图4为本发明实施例的磁传感器阵列与OXYZ坐标系、以及小永磁体旋转方向的空间关系示意图。

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。

本发明提供的一种磁性胶囊内窥镜全姿态测定方法，采用磁性胶囊内窥镜；胶囊内窥镜内设置的永磁体是径向充磁的小永磁体；小永磁体是圆柱状或圆环状，固定设置于磁性胶囊内窥镜的内部或固定于圆环形磁性胶囊内窥镜外壳壁上，且小永磁体的轴向方向与磁性胶囊内窥镜的轴向方向一致；胶囊内窥镜外部设置有大永磁体，大永磁体是径向充磁的圆柱状强永磁铁，如图1所示。

请见图2，本发明提供的一种磁性胶囊内窥镜全姿态测定方法，具体包括以下步骤：

步骤1：估计磁性胶囊内窥镜的初始姿态，确定磁性胶囊内窥镜内小永磁体的径向磁化方向单位向量和轴向方向单位向量

具体实现包括以下子步骤：

步骤1.1：计算小永磁体的径向磁化方向单位向量具体过程为：

(1)通过磁传感器阵列，检测得到由所述小磁体产生的当前磁感应强度其中，N(≥5)是磁传感器阵列中三轴磁性传感器的数量。如图4所示，以传感器阵列所在平面为XOY平面，建立三维直角坐标系OXYZ。第n个磁传感器处的磁感应强度当前值记作其中，B_0,nx、B_0,ny、B_0,nz分别表示第n个磁传感器在坐标系OXYZ中x轴、y轴和z轴三个方向上的磁感应强度分量。磁传感器阵列固定于所述人体外部(如人体下方)，如图1所示。

(2)根据(1)中检测到的磁感应强度利用磁偶极子模型的方法，将小磁体视作点源，计算小磁体的磁化方向的单位向量(即m₀ ²+n₀ ²+p₀ ²＝1)。具体方法为：

①建立目标函数：

其中，b_0,n＝B_0,nxy_n-B_{0, ny}x_n，(x_n,y_n,z_n)是第n个磁传感器在OXYZ坐标系中的位置坐标，(a,b,c)是小磁体在OXYZ坐标系中的位置坐标，m₀'＝m₀/p₀，n₀'＝n₀/p₀。

②利用非线性优化算法求解上述目标函数，求出小磁体(即磁性胶囊内窥镜)的姿态参数m₀'和n₀'。

③根据下式计算小磁体的磁化方向单位向量

m₀＝m₀'p₀

n₀＝n₀'p₀

步骤1.2：根据小磁体的径向磁化方向向量，利用外部大磁体控制磁性胶囊内窥镜旋转；

本实施例中，外部大磁体控制磁性胶囊内窥镜旋转的方式为：首先放置外部大磁体，使大磁体的径向磁化方向与小磁体的径向磁化方向(即向量)同向，如图3(a)所示，然后控制外部大磁体绕自身轴向中心轴逆时针旋转微小角度2°，导致外部大磁体产生的磁场发生旋转，该旋转磁场驱动小磁体(连同磁性胶囊内窥镜一起)绕自身轴向中心轴顺时针旋转(磁性胶囊内窥镜旋转方向与外部大磁铁的旋转方向相反，如图3(b)所示)。

步骤1.3：计算旋转后的小永磁体的径向磁化方向单位向量 具体过程为：先屏蔽或移除外部大磁体的磁场，然后通过磁传感器阵列，获得由旋转后的小磁体产生的当前磁感应强度再按照下述步骤计算小磁体的当前磁化方向单位向量

①建立目标函数：

其中，b_1，n＝B_1，nxy_n-B_{1， ny}x_n， 是旋转后的小磁体在坐标为(x_n,y_n,z_n)的第n个磁传感器处产生的磁感应强度矢量，m₁'＝m₁/p₁，n₁'＝n₁/p₁。

②利用非线性优化算法求解上述目标函数，求得旋转后的小磁体(即磁性胶囊内窥镜)的姿态参数m₁'和n₁'。

③根据下式计算小磁体旋转后的磁化方向单位向量

m₁＝m₁′p₁

n₁＝n₁′p₁

步骤1.4：由和计算磁性胶囊内窥镜的预估轴向方向单位向量计算公式为：

r＝n₀p₁-n₁p₀

s＝m₁p₀-m₀p₁

t＝m₀n₁-m₁n₀

其中，和分别表示旋转前和旋转后小磁体的磁化方向单位矢量，为磁性胶囊内窥镜的预估轴向方向单位矢量(即)。

步骤2：调整外部大磁体的姿态，使外部大磁体的姿态与磁性胶囊内窥镜的初始姿态一致，即：使外部大磁体的轴向方向与磁性胶囊内窥镜的预估轴向方向(即)相同，如图1所示，且外部大磁铁的径向磁化方向与永磁体的当前径向磁化方向(即)同向，如图3(a)所示。

步骤3：利用外部大磁体控制磁性胶囊内窥镜绕其自身轴向中心轴旋转。

本实施例中，外部大磁体控制磁性胶囊内窥镜旋转的方式为：控制外部大磁体绕自身轴向中心轴顺时针旋转4°，导致外部大磁体产生的磁场发生旋转，该旋转磁场驱动小磁体(连同磁性胶囊内窥镜一起)绕自身轴向中心轴逆时针旋转，如图4所示。(磁性胶囊内窥镜旋转方向与外部大磁铁的旋转方向相反。)

步骤4：计算旋转后磁性胶囊内窥镜内小磁体的当前径向磁化方向单位向量具体过程为：

①屏蔽外部大磁体磁场，通过磁传感器阵列，检测得到由小磁体经步骤3旋转后产生的磁感应强度当前值并建立目标函数：

其中，b_2，n＝B_2，nxy_n-B_{2， ny}x_n， 是小磁

体在坐标为(x_n,y_n,z_n)的第n个磁传感器处产生的磁感应强度值，m₂'＝m₂/p₂，n₂'＝n₂/p₂。

②利用非线性优化算法求解上述目标函数，求得旋转后的小磁体的当前位置坐标(a,b,c)和姿态参数m₂'和n₂'。

③根据下式计算旋转后的小磁体的当前磁化方向单位向量

m₂＝m₂'p₂；

n₂＝n₂'p₂；

步骤5：计算所述磁性胶囊内窥镜的轴向方向单位向量 即完全确定胶囊内窥镜的当前姿态。

计算公式为：

r＝n₁p₂-n₂p₁；

s＝m₂p₁-m₁p₂；

t＝m₁n₂-m₂n₁；

其中，和分别表示步骤3旋转(即第二次旋转)前和旋转后小磁体的径向磁化方向的单位矢量，如图4所示，为磁性胶囊内窥镜的轴向方向的单位矢量(即)。由于小磁体的径向磁化方向，与磁性胶囊内窥镜具有固定关系，且与磁性胶囊内窥镜的轴向方向不同，因此，由即完全确定磁性胶囊内窥镜的当前姿态。

由和计算磁性胶囊内窥镜的横滚角增量 由(Δθ)即确定磁性胶囊内窥镜旋转前的姿态。

本实施例中，用更新重复步骤3至步骤5，可连续地完全确定磁性胶囊内窥镜的姿态。

本实施例中，N＝25，已知传感器坐标(x₁,y₁,z₁)＝(-0.16,-0.1685,0.017)，(x₂,y₂,z₂)＝(-0.08,-0.1685,0.017)，(x₃,y₃,z₃)＝(0,-0.1685,0.017)，(x₄,y₄,z₄)＝(0.08,-0.1685,0.017)，(x₅,y₅,z₅)＝(0.16,-0.1685,0.017)，(x₆,y₆,z₆)＝(-0.16,0.0885,0.017)，(x₇,y₇,z₇)＝(-0.08,0.0885,0.017)，(x₈,y₈,z₈)＝(0,0.0885,0.017)，(x₉,y₉,z₉)＝(0.08,0.0885,0.017)，(x₁₀,y₁₀,z₁₀)＝(0.16,0.0885,0.017)，(x₁₁,y₁₁,z₁₁)＝(-0.16,0.0085,0.017)，(x₁₂,y₁₂,z₁₂)＝(-0.08,0.0085,0.017)，(x₁₃,y₁₃,z₁₃)＝(0,0.0085,0.017)，(x₁₄,y₁₄,z₁₄)＝(0.08,0.0085,0.017)，(x₁₅,y₁₅,z₁₅)＝(0.16,0.0085,0.017)，(x₁₆,y₁₆,z₁₆)＝(-0.16,-0.0715,0.017)，(x₁₇,y₁₇,z₁₇)＝(-0.08,-0.0715,0.017)，(x₁₈,y₁₈,z₁₈)＝(0,-0.0715,0.017)，(x₁₉,y₁₉,z₁₉)＝(0.08,-0.0715,0.017)，(x₂₀,y₂₀,z₂₀)＝(0.16,-0.0715,0.017)，(x₂₁,y₂₁,z₂₁)＝(-0.16,-0.1515,0.017)，(x₂₂,y₂₂,z₂₂)＝(-0.08,-0.1515,0.017)，(x₂₃,y₂₃,z₂₃)＝(0,-0.1515,0.017)，(x₂₄,y₂₄,z₂₄)＝(0.08,-0.1515,0.017)，(x₂₅,y₂₅,z₂₅)＝(0.16,-0.1515,0.017)。

本实施例中，磁性胶囊内窥镜中心点坐标与小永磁体的中心点坐标相同，理论值均为磁性胶囊内窥镜姿态设为轴向与正X轴同向(磁性胶囊内窥镜摄像头端为正向)，径向磁化方向与正Z轴同向，即：

(r,s,t)理论值为(1,0,0)，(m,n,p)理论值为(0,0,1)，横滚角理论值为0度。

由磁传感器阵列采集到的磁感应强度为

通过设置过小(小于1)和过大(超传感器量程)的值为无效值，去除1号传感器的数据和由剩余24个传感器数据和计算得：

然后由和计算得胶囊镜的初始预估姿态

由计算得：磁性胶囊内窥镜中心位置坐标(,b,c)＝(0.002388,-0.002005,0.118974)。然后由和计算得胶囊镜姿态参数中的轴向单位向量由和计算横滚角Δθ＝1.76°，逆时针方向为正。

由于位置坐标的理论值(r,s,t)理论值为(1,0,0)，

(m,n,p)理论值为(0,0,1)，横滚角理论值为0°，因此本实施例中，胶囊镜的 胶囊镜姿态参数中的轴向单位向量误差＝|(r₂,s₂,t₂)-(1,0,0)|＝(0.0026,0.0543,0.0463)，转换为(轴向向量分别与XYZ三轴的夹角)角度误差为(4.13°,3.11°,2.65°)；姿态参数中的横滚角误差＝Δθ-0＝1.76°。

本发明所涉及的小磁体的形状还可以是横截面为正方形或多边形的立方体。

本发明所涉及的步骤1.1、步骤1.3、步骤4中，计算小磁体的径向磁化方向向量，可由现有的磁偶极子模型及其改进方法或现有的根据磁感应强度计算磁源磁化方向的任意方法替代。

本发明所涉及的步骤2中，使外部大磁体的姿态与磁性胶囊内窥镜的初始姿态一致，还可以是：使外部大磁体的轴向方向与磁性胶囊内窥镜的预估轴向方向轴(即)重合(即外部大磁体与磁性胶囊内窥镜同轴)，且所述大磁体的径向磁化方向与小磁体的径向磁化方向(即)相反。(此种情况下，步骤3中，所述小磁体的旋转方向与外部大磁体的旋转方向是同向的。)

本发明所设计的步骤5中，还可以由和计算磁性胶囊内窥镜的轴向方向单位向量计算公式为：

r＝n₀p₂-n₂p₀；

s＝m₂p₀-m₀p₂；

t＝m₀n₂-m₂n₀；

应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

Claims (6)

1.一种磁性胶囊内窥镜全姿态测定系统，采用磁性胶囊内窥镜；其特征在于：所述胶囊内窥镜内设置的永磁体是径向充磁的小永磁体；所述小永磁体是圆柱状或圆环状，固定设置于所述磁性胶囊内窥镜的内部或固定于圆环形磁性胶囊内窥镜外壳壁上，且所述小永磁体的轴向方向与所述磁性胶囊内窥镜的轴向方向一致；所述胶囊内窥镜外部设置有大永磁体，所述大永磁体是径向充磁的圆柱状强永磁铁；

所述系统工作流程包括以下步骤：

步骤1：估计磁性胶囊内窥镜的初始姿态，确定所述磁性胶囊内窥镜内小永磁体的径向磁化方向单位向量和轴向方向单位向量

具体实现包括以下子步骤：

步骤1.1：计算所述小永磁体的径向磁化方向单位向量

步骤1.2：根据所述小永磁体的径向磁化方向向量，利用外部大永磁体控制所述磁性胶囊内窥镜旋转；

首先放置所述外部大永磁铁于所述胶囊内窥镜或人体的上方，使所述大永磁体的径向磁化方向与小永磁体的径向磁化方向同向；然后控制所述外部大永磁体绕自身轴向中心轴旋转任一微小角度，导致所述外部大永磁体产生的磁场发生旋转，该旋转磁场驱动所述小永磁体及所述磁性胶囊内窥镜一起绕自身轴向中心轴旋转，所述磁性胶囊内窥镜旋转方向与所述外部大磁铁的旋转方向相反；

步骤1.3：计算所述小永磁体旋转后的径向磁化方向单位向量

通过磁传感器阵列，检测由所述小永磁体旋转后产生的磁感应强度根据将所述小永磁体视作点源，计算所述小永磁体的磁化方向单位向量

步骤1.4：由和计算所述磁性胶囊内窥镜的预估轴向方向单位向量

计算所述磁性胶囊内窥镜的预估轴向方向单位向量

其中，r＝n₀p₁-n₁p₀，s＝m₁p₀-m₀p₁，t＝m₀n₁-m₁n₀；和分别表示旋转前和旋转后小永磁体的径向磁化方向的单位矢量；

步骤2：调整外部大永磁体的姿态，使外部大永磁体的姿态与所述磁性胶囊内窥镜的当前姿态一致；

其中，所述外部大永磁体位于所述胶囊内窥镜或人体的上方，使所述外部大永磁体的轴向方向与所述胶囊内窥镜的预估轴向方向相同，且所述外部大永磁铁的径向磁化方向与所述小永磁体的当前径向磁化方向同向；

步骤3：利用外部大永磁体控制所述磁性胶囊内窥镜绕其轴向中心轴旋转；

步骤4：计算旋转后的所述磁性胶囊内窥镜内小永磁体的当前径向磁化方向单位向量

步骤5：计算所述磁性胶囊内窥镜的轴向方向单位向量由和计算所述磁性胶囊内窥镜的横滚角增量Δθ，由确定所述磁性胶囊内窥镜旋转前的姿态。

2.根据权利要求1所述的磁性胶囊内窥镜全姿态测定系统，其特征在于：步骤1.1中，通过磁传感器阵列，检测由所述小永磁体旋转前产生的磁感应强度根据将所述小永磁体视作点源，计算所述小永磁体的磁化方向单位向量

3.根据权利要求2所述的磁性胶囊内窥镜全姿态测定系统，其特征在于：所述磁传感器阵列由不少于5个三轴磁性传感器构成，固定于所述磁性胶囊内窥镜的外部。

4.根据权利要求1所述的磁性胶囊内窥镜全姿态测定系统，其特征在于：步骤3中，控制所述外部大永磁体绕自身轴向中心轴旋转微小角度，导致所述外部大永磁体产生的磁场发生旋转，该旋转磁场驱动所述小永磁体及所述磁性胶囊内窥镜一起绕自身轴向中心轴旋转，所述磁性胶囊内窥镜旋转方向与所述外部大磁铁的旋转方向相反。

5.根据权利要求1所述的磁性胶囊内窥镜全姿态测定系统，其特征在于：步骤4中，通过磁传感器阵列，检测由所述小永磁体经步骤3旋转后产生的磁感应强度根据将所述小永磁体视作点源，计算所述小永磁体经步骤3旋转后的磁化方向单位向量即

6.根据权利要求1-5任意一项所述的磁性胶囊内窥镜全姿态测定系统，其特征在于：步骤5中，计算所述磁性胶囊内窥镜的轴向方向单位向量

其中，r＝n₁p₂-n₂p₁，s＝m₂p₁-m₁p₂，t＝m₁n₂-m₂n₁；由于所述小永磁体的径向磁化方向，与所述磁性胶囊内窥镜具有固定关系，且与所述磁性胶囊内窥镜的轴向方向不同，因此，由所述小永磁体的径向磁化方向向量和轴向方向向量确定所述磁性胶囊内窥镜的当前姿态；由和计算所述磁性胶囊内窥镜的横滚角增量 由确定所述磁性胶囊内窥镜旋转前的姿态。