一种胶囊内窥镜空间姿态测定方法、系统及装置与流程

本发明涉及医疗技术领域，特别涉及一种胶囊内窥镜空间姿态测定方法。本发明还涉及一种胶囊内窥镜空间姿态测定系统以及一种胶囊内窥镜空间姿态测定装置。

背景技术：

随着微光机电技术、机器人技术、无线通信技术的发展，医疗仪器开始向微型化、智能化、无创诊查方向发展，电子胶囊逐渐成为研究热点。胶囊内窥镜极大地拓展了医生的消化道检查视野，填补了胃镜、肠镜检查的盲区，解决了多年来小肠疾病和胃肠道隐血诊断方面的难题。与传统医用内窥镜相比，胶囊内窥镜具有操作简单、检查方便、无创伤、无痛苦、无交叉感染、不影响患者的正常工作等优点，尤其对小肠可疑性病变具有很高的诊断价值，被医学界誉为21世纪内窥镜发展的革命与方向。

而目前广泛使用外部磁场驱动方式的主动可控式胶囊内窥镜系统，该系统包括内置在胶囊内窥镜内的永磁体或在胶囊内窥镜外表面上包覆永磁体层，和外部磁场装置，该外部磁场装置可以采用能够通过人工控制的磁力臂，或者也可以采用三轴亥姆霍兹线圈，通过该三轴亥姆霍兹线圈的万向均匀旋转磁场驱动胶囊内窥镜进行滚动、俯仰、偏航和前进等运动。

在主动式胶囊内窥镜(以后简称胶囊)的使用中，胶囊空间姿态的全解析对于检测过程来说十分有必要，胶囊空间姿态的全解析能够带来以下技术优势：辅助操作者或者是计算机进行胶囊指向位置判定、帮助操作者或者是计算机进行后续检查路线的规划、为胶囊空间定位提供必要的姿态数据。

目前，胶囊的空间姿态全解析也存在着困难，表现为：胶囊工作环境于外部完全视觉隔离，使得大部分依赖视频识别姿态测量手段无效；人体组织对于电磁波的衰减效应使得大部分依赖于电磁波的姿态测量手段失效；由于成本问题和用户体验问题，使得放射性成像和核磁共振造影等手段不适用。在使用环境中可行的方法就是，胶囊内部集成传感器，对自身的各个姿态角进行测量，然后将数据实时的传递出来，在上位计算机中解析出胶囊的全姿态信息。

在现有技术中，比较成熟的测量刚体全姿态信息的方法一般是使用3轴加速度计+3轴角速度仪+3轴地磁传感器。其中，3轴加速度计和3轴角速度仪用来完成主要的测量工作，通过两者的数据融合，即可得到刚体在各个轴方向上受到的加速度大小以及刚体绕着各个轴旋转的角度信息。两轴数据融合有一个巨大的缺陷，就是3轴角速度仪的数据在换算成角度的时候，存在一个积分累计误差。随着时间的推移，测量的姿态精度会变得越来越不可控。所以此时使用3轴的地磁传感器所测得的地磁向量对刚体角度进行校准，即可消除累计误差。此种9轴数据融合的姿态测量方法广泛应用于飞控、航空、导航等领域。

然而，胶囊使用内部传感器进行全姿态信息测量时，由于主动式胶囊内窥镜的使用环境中，其驱动力来自于外部磁场对内部磁体的作用力，所以存在很强的磁场干扰，3轴的地磁传感器无法使用，并且使用加速度计和角速度仪进行数据融合产生的积分累计误差是不可接受的。

因此，如何顺利完全对胶囊内窥镜的全姿态测定，并提高测量精度，是本领域技术人员亟待解决的技术问题。

技术实现要素：

本发明的目的是提供一种胶囊内窥镜空间姿态测定方法，能够顺利完成对胶囊内窥镜的全姿态测定，并提高测量精度。本发明的另一目的是提供一种胶囊内窥镜空间姿态测定系统以及一种胶囊内窥镜空间姿态测定装置。

为解决上述技术问题，本发明提供一种胶囊内窥镜空间姿态测定方法，包括：

检测胶囊内窥镜当前的合加速度向量；

检测所述胶囊内窥镜当前位置处由外部预设定向磁场产生的磁场向量；

将所述合加速度向量和磁场向量代入预设公式计算所述胶囊内窥镜的当前姿态角。

优选地，检测所述胶囊内窥镜当前的合加速度向量，具体包括：

通过设置于所述胶囊内窥镜内的三轴加速度传感器检测其预设三轴方向上的加速度，再将各轴上的加速度相加计算所述合加速度向量。

优选地，检测所述胶囊内窥镜当前位置处由外部预设定向磁场产生的磁场向量，具体包括：

通过设置于所述胶囊内窥镜内的磁场传感器检测所述外部预设定向磁场产生的磁场向量。

优选地，将所述合加速度向量和磁场向量代入预设公式计算所述胶囊内窥镜的当前姿态角，具体包括：

通过公式

计算所述胶囊内窥镜的俯仰角pitch；

通过公式

计算所述胶囊内窥镜的横滚角roll；

通过公式

yaw＝β+θ

计算所述胶囊内窥镜的偏航角yaw；且

其中，

β为外部预设定向磁场在水平方向上的偏转角度；

θ为与在水平方向上的夹角；

为所述胶囊内窥镜的x轴基向量，设

为所述胶囊内窥镜的y轴基向量，设

为所述胶囊内窥镜的z轴基向量，设

为所述胶囊内窥镜的合加速度向量，设

为所述胶囊内窥镜当前所处位置的由外部预设定向磁场产生的磁场向量，设

为在所述胶囊内窥镜的x-y平面上的投影向量，设

且

当且a∈r,a>0时，roll＝|roll|，

当且a∈r,a≤0时，roll＝-|roll|；

当且a∈r,a>0时，θ＝|θ|，

当且a∈r,a≤0时，θ＝-|θ|。

本发明还提供一种胶囊内窥镜空间姿态测定系统，包括

第一检测模块，用于检测胶囊内窥镜当前的合加速度向量；

第二检测模块，用于检测所述胶囊内窥镜当前位置处由外部预设定向磁场产生的磁场向量；

计算模块，用于将所述合加速度向量和磁场向量代入预设公式计算所述胶囊内窥镜的当前姿态角。

优选地，所述第一检测模块具体包括设置于所述胶囊内窥镜内、用于检测其预设三轴方向上加速度的三轴加速度传感器。

优选地，所述第二检测模块具体包括设置于所述胶囊内窥镜内、用于检测其当前位置处由外部预设定向磁场产生的磁场向量的磁场传感器。

本发明还提供一种胶囊内窥镜空间姿态测定装置，包括用于进入病患体内进行图像采集的胶囊内窥镜、用于通过磁力吸引控制所述胶囊内窥镜运动状态的控制磁场发生器，以及用于在病患体表周围形成定向磁场的测定磁场发生器。

本发明所提供的胶囊内窥镜空间姿态测定方法，主要包括三个步骤，分别为：检测胶囊内窥镜当前的合加速度向量；检测所述胶囊内窥镜当前位置处由外部预设定向磁场产生的磁场向量；将所述合加速度向量和磁场向量代入预设公式计算所述胶囊内窥镜的当前姿态角。其中，在第一步中，首先检测胶囊内窥镜在进入人体后在各个方向上受到的加速度和合加速度(包括数值和方向)，因此为合加速度向量。在第二步中，胶囊内窥镜的外部空间内预先设置有定向磁场(该磁感线方向在水平方向上平行或部分区域具有较高平行度，比如95％以上等，且与预设的水平轴具有一定偏转角度)，便于定位胶囊内窥镜的姿态，在本步骤中检测胶囊内窥镜在其当前位置处的磁场向量。在第三步中，结合胶囊内窥镜的当前合加速度向量与检测到的当前磁场向量，即可根据该两种数据确定胶囊内窥镜的当前姿态角(也称“欧拉角”，eulerangle)。其中，根据运动学领域的公式，胶囊内窥镜的合加速度向量与其3个方向的移动自由度相关，用于确定其位置，而胶囊内窥镜在其位置处检测的磁场向量与其3个方向的转动自由度相关，用于确定其姿态。因此，两者结合计算，即可顺利完成对胶囊内窥镜的全姿态测定。同时，相比于现有技术，本发明中的磁场向量由外部预先设置的定向磁场产生，磁场强度较高，且方向固定，可以避免通过地磁感应器检测地磁场的磁场向量时受到的强磁干扰而引起的测量不精问题；并且避开了现有技术中使用角速度仪的检测值来换算角度引起的积分累计误差问题，提高了姿态测量精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明所提供的一种具体实施方式的流程图；

图2为胶囊内窥镜的三轴坐标系与检测到的合加速度向量、磁场向量的示意图；

图3为本发明所提供的一种具体实施方式的模块图；

图4为本发明所提供的一种具体实施方式的结构图。

其中，图4中：

胶囊内窥镜—1，控制磁场发生器—2，测定磁场发生器—3。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1，图1为本发明所提供的一种具体实施方式的流程图。

在本发明所提供的一种具体实施方式中，胶囊内窥镜空间姿态测定方法主要包括三个步骤，分别为：检测胶囊内窥镜当前的合加速度向量；检测所述胶囊内窥镜当前位置处由外部预设定向磁场产生的磁场向量；将所述合加速度向量和磁场向量代入预设公式计算所述胶囊内窥镜的当前姿态角。

其中，在第一步中，首先检测胶囊内窥镜在进入人体后在各个方向上受到受到的加速度和合加速度(包括数值和方向)，因此为合加速度向量。

具体的，在本步骤中，可以通过三轴加速度传感器来检测胶囊内窥镜在其三轴方向上的加速度，此处所指三轴方向，即以胶囊内窥镜为基准制定的三维坐标系，其中的x、y、z轴可在胶囊内窥镜上任意指定，不影响其姿态的准确测定。但为方便论述，本实施例以下内容均以胶囊内窥镜的轴向方向为z轴，且为右手坐标系为标准进行说明。一般的，三轴加速度传感器可设置在胶囊内窥镜内。如此，三轴加速度传感器检测出胶囊内窥镜的每轴方向上的加速度之后，之后即可将三轴上的各个加速度相加，计算出合加速度向量。之后再将该合加速度向量与胶囊内窥镜的三轴基向量进行融合计算，即可首先测定出胶囊内窥镜当前的俯仰角和横滚角。

但胶囊内窥镜的空间姿态仅由三轴加速度传感器还不能完全测定，还需测定其偏航角。如此，在第二步中，胶囊内窥镜的外部空间中预先设置有定向磁场，此处优选地，该定向磁场的磁感线方向可在水平方向上平行或者部分区域具有较高的平行度，比如95％以上等，且与预设的水平轴具有一定偏转角度β，比如匀强磁场等。该外部预设定向磁场主要用于与三轴加速度传感器一同测定胶囊内窥镜的偏航角，在本步骤中，主要检测胶囊内窥镜在其当前位置处的磁场向量，及其磁场强度和方向。如此，虽然外部空间中预设的定向磁场中，磁场方向处处相同甚至磁场也相等，但是对于胶囊内窥镜的自身坐标系而言，当其姿态产生变化时，在相同位置处检测到的磁场向量也会产生变化。因此，根据胶囊内窥镜在定向磁场范围内所检测到的磁场向量，可以辅助确定胶囊内窥镜的偏航角。

具体的，在本步骤中，可以通过磁场传感器来检测当前位置的外部预设定向磁场产生的磁场向量。一般的，该磁场传感器可设置在胶囊内窥镜内部。

在第三步中，即可结合胶囊内窥镜的当前合加速度向量与检测到的当前磁场向量精确地计算出胶囊内窥镜的当前姿态角(也称“欧拉角”，eulerangle)。其中，根据运动学领域的公式，胶囊内窥镜的合加速度向量与其3个方向的移动自由度相关，用于确定其位置，而胶囊内窥镜在其位置处检测的磁场向量与其3个方向的转动自由度相关，用于确定其姿态。因此，两者结合计算，即可顺利完成对胶囊内窥镜的全姿态测定。同时，相比于现有技术，本实施例中的磁场向量由外部预先设置的定向磁场产生，磁场强度较高，且方向固定，可以避免通过地磁感应器检测地磁场的磁场向量时受到的强磁干扰而引起的测量不精问题；并且避开了现有技术中使用角速度仪的检测值来换算角度引起的积分累计误差问题，提高了姿态测量精度。

如图2所示，图2为胶囊内窥镜的三轴坐标系与检测到的合加速度向量、磁场向量的示意图。

具体的，关于胶囊内窥镜的全姿态解析计算过程，首先可对胶囊内窥镜进行三轴坐标系构建。本实施例中均以胶囊内窥镜的轴向方向为z轴方向，横截面方向为x-y平面方向为例进行说明。显然，其余的三轴坐标系构建方式也是可行的。如此，胶囊内窥镜的x轴基向量即为[100]，而胶囊内窥镜的y轴基向量即为[010]，其z轴基向量即为[001]。同时可设检测出的合加速度向量为[gxgygz]，检测出的磁场向量为[mxmymz]。

因为外部预设定向磁场是根据定位需要预先设定的，因此其磁场方向相对于预设的标准轴的水平方向偏转角度是已知的，设其角度为β。

同时，胶囊内窥镜的姿态由姿态角确定，而姿态角主要包括俯仰角、横滚角和偏航角，因此，可令俯仰角为pitch，横滚角为roll，偏航角为yaw。

其中，俯仰角pitch可以通过合加速度向量和胶囊自身坐标系z轴之间的夹角来求得，所以俯仰角

横滚角同样可以通过合加速度向量求得，设合加速度向量在胶囊自身坐标系xy平面上的投影向量为则为[gxgy0]，且有横滚角

其中，当且a∈r,a>0时，roll＝|roll|，

当且a∈r,a≤0时，roll＝-|roll|。

偏航角需要通过合加速度向量和磁场向量进行数据融合处理，同时还需要引入外部定向磁场在水平方向上的偏转角度β。

首先设θ为磁场向量与胶囊内窥镜自身坐标系z轴方向的水平方向夹角，其次，令其中，为与组成平面的法向量，为与组成平面的法向量，如此：

其中，当且a∈r,a>0时，θ＝|θ|，

当且a∈r,a≤0时，θ＝-|θ|；

上述a均为系数。

θ计算出之后，将其与β相加即为偏航角yaw。

至此，胶囊内窥镜的俯仰角为pitch，横滚角为roll，偏航角为yaw均已计算出来，可以顺利获知胶囊内窥镜的姿态角，实现对胶囊内窥镜的全姿态解析。

如图3所示，图3为本发明所提供的一种具体实施方式的模块图。

本实施例还提供一种胶囊内窥镜空间姿态测定系统，主要包括第一检测模块、第二检测模块和计算模块。其中，第一检测模块主要用于检测胶囊内窥镜当前的合加速度向量，而第二检测模块主要用于检测所述胶囊内窥镜当前位置处由外部预设定向磁场产生的磁场向量，计算模块与第一检测模块和第二检测模块信号连接，主要用于根据两者的检测值与预设公式计算胶囊内窥镜的当前姿态角。

在关于第一检测模块的一种优选实施方式中，该第一检测模块具体可为设置在胶囊内窥镜内的三轴加速度传感器，主要用于检测胶囊内窥镜上预设三轴方向上的加速度。

而在关于第二检测模块的一种优选实施方式中，该第二检测模块具体可为设置在胶囊内窥镜内的磁场传感器，主要用于检测胶囊内窥镜当前所处位置由外部预设定向磁场产生的磁场向量。

如图4所示，图4为本发明所提供的一种具体实施方式的结构图。

本实施例还提供一种胶囊内窥镜空间姿态测定装置，主要包括胶囊内窥镜1、控制磁场发生器2和测定磁场发生器3。其中，胶囊内窥镜1主要用于进入到病患体内进行图像采集，在胶囊内窥镜1内一般设置有磁铁、通讯模块、控制模块等，其中磁铁用于与外部的控制磁场发生器2产生磁力吸引作用，使得控制磁场发生器2通过磁场变化牵引胶囊内窥镜1内的磁铁，进而驱动胶囊内窥镜1进行运动。而测定磁场发生器3也用于产生定向磁场，不同的是，测定磁场发生器3产生的是笼罩在病患体表周围的定向磁场，并且也不是为了牵引胶囊内窥镜1内的磁铁，而是作为地磁场的替代品，辅助胶囊内窥镜1内的加速度传感器进行对胶囊内窥镜1的姿态测定，提高测定精度。

另外，控制磁场发生器2与测定磁场发生器3还可以一体成型设计，如此，其产生的磁场将由其控制器决定，比如，在需要正常牵引胶囊内窥镜1进行运动时，则使控制磁场发生器2运行，并产生对应磁场；而在需要测定胶囊内窥镜1的姿态时，则使测定磁场发生器3运行，并产生定向磁场。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。