CN107374566B - 一种基于变化磁场的胶囊内窥镜全姿态测定系统 - Google Patents

Abstract

本发明公开一种基于变化磁场的胶囊内窥镜全姿态测定方法，包括：检测胶囊内窥镜的当前加速度；判断胶囊内窥镜当前所处位置的外部预设磁场是否满足预设条件，如果是，则检测胶囊内窥镜当前所处位置的磁感应强度，并将其和加速度同时代入预设公式中计算胶囊内窥镜的当前姿态角；如果否，则检测胶囊内窥镜绕自身预设三轴转动的角速度，并将各角速度分别进行积分计算胶囊内窥镜的当前姿态角。如此，在外部预设磁场满足预设条件时，可通过胶囊内窥镜的加速度与所处位置的磁感应强度进行数据融合，精确地计算胶囊内窥镜的姿态角；反之，可通过胶囊内窥镜的加速度与其绕自身预设三轴转动的角速度进行数据融合，精确地计算胶囊内窥镜的姿态角。

Description

一种基于变化磁场的胶囊内窥镜全姿态测定系统

技术领域

本发明涉及医疗设备技术领域，特别涉及一种基于变化磁场的胶囊内窥镜全姿态测定系统。

背景技术

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

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

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

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

在现有技术中，比较成熟的测量刚体全姿态信息的方法一般是使用加速度传感器和磁场传感器用来完成主要的测量工作，通过胶囊的加速度向量和所处位置的空间磁场向量的数据融合，即可测定胶囊的姿态角。该空间磁场向量由外部预设磁场提供，而该外部预设磁场一般是由医护人员设置、用于兼做控制胶囊在人体消化道内移动用的，其产生的磁场处于周期性变化中。然而，在通过磁场传感器进行胶囊的空间姿态角测定时，必须使外部预设磁场满足一定预设条件，即该外部预设磁场需为水平定向磁场(或者磁感线平行度高于95％的水平磁场)，只有满足该条件的外部预设磁场，才能与加速度传感器数据融合后精确计算出胶囊的姿态角，但该外部预设磁场在医护人员正常使用时，不可能始终处于水平定向状态，在操作胶囊移动时，外部预设磁场的方向和磁场强度均不一致，此时再进行测定的话，角度误差太大，无法完成全姿态测定。

因此，如何在变化的空间磁场环境下，精确地测定胶囊内窥镜的姿态角，是本领域技术人员亟待解决的技术问题。

发明内容

本发明的目的是提供一种基于变化磁场的胶囊内窥镜全姿态测定系统，能够在变化的空间磁场环境下，精确地测定胶囊内窥镜的姿态角。

为解决上述技术问题，本发明提供一种基于变化磁场的胶囊内窥镜全姿态测定系统，包括：

加速度传感器，用于检测胶囊内窥镜的当前加速度；

模式判断模块，用于判断所述胶囊内窥镜当前所处位置的外部预设磁场是否满足预设条件；其中，所述预设条件为外部预设磁场为水平定向磁场或磁感线平行度高于95％的水平磁场；

第一检测模块，用于在所述模式判断模块的判断结果为是时，检测所述胶囊内窥镜当前所处位置的磁感应强度；

第一计算模块，用于将所述磁感应强度和加速度同时代入预设公式中计算所述胶囊内窥镜的当前姿态角；

第二检测模块，用于在所述模式判断模块的判断结果为否时，检测所述胶囊内窥镜绕自身预设三轴转动的角速度；

第二计算模块，用于将各所述角速度分别进行积分计算所述胶囊内窥镜的当前姿态角。

优选地，还包括：

记录模块，用于将外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻所计算的所述胶囊内窥镜的姿态角作为积分常数项，

修正模块，用于根据所述积分常数项对将各所述角速度分别进行积分计算的算式进行修正。

优选地，所述第一检测模块具体为磁场传感器，所述第二检测模块具体为陀螺仪。

本发明所提供的基于变化磁场的胶囊内窥镜全姿态测定系统，在第一个步骤中，首先检测胶囊内窥镜当前的加速度，在第二个步骤中，由于通过空间磁场向量辅助测定胶囊姿态角需要外部预设磁场满足一定条件，而外部预设磁场在作业中又是阶段性变化的，因此首先需要判断胶囊内窥镜当前所处位置的外部预设磁场是否满足预设条件，如果是，则说明此时可以通过空间磁场向量的辅助测定手段进行姿态角测定，如此即可检测胶囊内窥镜当前所处位置的磁感应强度，然后将该检测的磁感应强度和胶囊内窥镜的当前加速度进行数据融合，两者同时代入预设公式中进行计算，即可精确地测定出胶囊内窥镜的当前姿态角。如果否，则说明此时无法通过空间磁场向量的辅助手段进行姿态角测定，考虑到胶囊的姿态角变化时，其绕自身预设三轴转动的角度也同步发生变化，因此此时可检测胶囊内窥镜绕自身预设三轴转动的角速度，然后将该三个角速度分别进行积分计算，即可计算出胶囊内窥镜绕自身预设三轴转动的角度，进而精确地测定胶囊内窥镜的当前姿态角。如此，本发明所提供的基于变化磁场的胶囊内窥镜全姿态测定方法，在外部预设磁场满足预设条件时，可通过胶囊内窥镜的加速度与所处位置的磁感应强度进行数据融合，精确地计算胶囊内窥镜的姿态角；而在外部预设磁场不满足预设条件时，可通过胶囊内窥镜的加速度与其绕自身预设三轴转动的角速度进行数据融合，精确地计算胶囊内窥镜的姿态角。综上所述，在外部预设磁场变化的环境下，本发明所提供的胶囊内窥镜全姿态测定系统，总能精确地实现胶囊内窥镜的全姿态测定。

附图说明

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

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

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

其中，图2中：

加速度传感器—1，模式判断模块—2，第一检测模块—3，第一计算模块—4，第二检测模块—5，第二计算模块—6，记录模块—7，修正模块—8。

具体实施方式

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

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

在本发明所提供的一种具体实施方式中，基于变化磁场的胶囊内窥镜全姿态测定方法主要包括三个步骤，分别为：检测胶囊内窥镜的当前加速度；判断胶囊内窥镜当前所处位置的外部预设磁场是否满足预设条件，如果是，则检测胶囊内窥镜当前所处位置的磁感应强度，并将其和加速度同时代入预设公式中计算胶囊内窥镜的当前姿态角；如果否，则检测胶囊内窥镜绕自身预设三轴转动的角速度，并将各角速度分别进行积分计算胶囊内窥镜的当前姿态角。

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

在第二个步骤中，由于通过空间磁场向量辅助测定胶囊姿态角需要外部预设磁场满足一定条件——一般为该外部预设磁场需为水平定向磁场，或者磁感线平行度高于95％的水平磁场，而外部预设磁场在作业中又是阶段性变化的，因此首先需要判断胶囊内窥镜当前所处位置的外部预设磁场是否满足预设条件。

如果是，则说明此时可以通过空间磁场向量的辅助测定手段进行姿态角测定。如此即可检测胶囊内窥镜当前所处位置的磁感应强度，然后将该检测的磁感应强度和胶囊内窥镜的当前加速度进行数据融合，两者同时代入预设公式中进行计算，即可精确地测定出胶囊内窥镜的当前姿态角。

如果否，则说明此时无法通过空间磁场向量的辅助手段进行姿态角测定，考虑到胶囊的姿态角变化时，其绕自身预设三轴转动的角度也同步发生变化，因此此时可检测胶囊内窥镜绕自身预设三轴转动的角速度，然后将该三个角速度分别进行积分计算，即可计算出胶囊内窥镜绕自身预设三轴转动的角度，进而精确地测定胶囊内窥镜的当前姿态角。

如此，本实施例所提供的基于变化磁场的胶囊内窥镜全姿态测定方法，在外部预设磁场满足预设条件时，可通过胶囊内窥镜的加速度与所处位置的磁感应强度进行数据融合，精确地计算胶囊内窥镜的姿态角；而在外部预设磁场不满足预设条件时，可通过胶囊内窥镜的加速度与其绕自身预设三轴转动的角速度进行数据融合，精确地计算胶囊内窥镜的姿态角。

综上所述，在外部预设磁场变化的环境下，本实施例所提供的胶囊内窥镜全姿态测定方法，总能精确地实现胶囊内窥镜的全姿态测定。

在检测胶囊内窥镜当前所处位置的磁感应强度时，具体可通过设置在胶囊内窥镜内的磁场传感器检测其磁感应强度。与胶囊内窥镜的加速度一样，该磁感应强度同样为矢量，具有大小和方向，由于外部预设磁场在正常运行时，是呈阶段性变化的，因此其磁感应强度也随时处于变化中。

而外部预设磁场在满足预设条件，形成水平定向磁场时，其磁场方向相对于预设的标准轴(一般可为以人体为中心建立的地面坐标系的水平坐标轴)的水平方向偏转角度是已知的，设其角度为ξ。同时在将磁感应强度和加速度同时代入预设公式中计算所述胶囊内窥镜的当前姿态角时，具体的，首先可对胶囊内窥镜进行三轴坐标系构建。本实施例中均以胶囊内窥镜的轴向方向为Z轴方向，横截面方向为X-Y平面方向为例进行说明。

如此，胶囊内窥镜的x轴基向量即为[1 0 0]，而胶囊内窥镜的y轴基向量即为[0 1 0]，其z轴基向量即为[0 0 1]。同时可设检测出的当前加速度为[gx gy gz]，胶囊内窥镜当前所处位置的磁感应强度为[mx my mz]。

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

其中，俯仰角pitch可以通过胶囊内窥镜的加速度和胶囊内窥镜自身坐标系Z轴之间的夹角来求得，所以俯仰角可以通过公式：

进行计算。

横滚角roll的计算方法同理，首先可设胶囊内窥镜的加速度在胶囊内窥镜自身坐标系xy平面上的投影向量为则为[gx gy 0]，如此横滚角roll可通过公式：

进行计算。

其中，当且A∈R，A＞0时，roll＝|roll|，

当且A∈R，A≤0时，roll＝-|roll|。

偏航角yaw需要将加速度和磁感应强度进行数据融合处理，同时还需要利用前述外部预设磁场在水平方向上的偏转角度ξ。

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

其中，当且A∈R，A＞0时，θ＝|θ|，

当且A∈R，A≤0时，θ＝-|θ|；

上述A均为系数。

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

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

而当外部预设磁场不满足预设条件时，需要检测胶囊内窥镜绕自身预设三轴转动的角速度，并将各角速度分别进行积分计算，以测定姿态角。

具体的，在检测胶囊内窥镜绕自身预设三轴转动的角速度时，具体可通过设置在胶囊内窥镜内的角速度传感器检测其角速度。

同时，考虑到在MEMS领域，角速度传感器因为测量的是角速度而不是角度本身，会为系统带来误差。角度可以由角速度定积分求得，积分的过程中由于测量误差、采样误差等因素的影响，得到的是一个引入了误差的量，在短时间之内，对姿态测定的精度影响并不大，可以忽略，但是当长时间持续通过角速度积分的方式测定姿态角时，系统累计误差会随着时间的推移越来越大。针对此，本实施例中增设了修正环节。

由于当外部预设磁场满足预设条件时，其测定的姿态角是精确无误差的，因此可将此段时间内测定的姿态角进行记录，当外部预设磁场突然发生变化，而不满足预设条件时，在外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻所记录的姿态角即为最后一组无累积误差数据，并将其作为积分常数项，作为后续通过角速度积分运算的修正参数，以此消除在长时间通过角速度积分求解姿态角的过程中形成的系统累积误差。

具体的，在将各角速度分别进行积分计算胶囊内窥镜的当前姿态角时，首先可通过公式：

计算t时刻所述胶囊内窥镜绕自身预设x轴转动的角度，

同时可通过公式：

计算t时刻所述胶囊内窥镜绕自身预设y轴转动的角度，

同时可通过公式：

计算t时刻所述胶囊内窥镜绕自身预设z轴转动的角度。

其中，ω_x、ω_y和ω_z分别为胶囊内窥镜绕自身预设三轴转动的角速度。

t为从外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻开始计时的时间，而α₀、β₀和γ₀分别为积分常数项中胶囊内窥镜绕自身预设x、y和z轴的初始转动角度，各初始转动角度均与记录的最后一组无累积误差数据相关，可通过该组数据中的俯仰角pitch、横滚角roll和偏航角yaw转换而得。

之后，即可根据上述三个计算胶囊内窥镜绕自身预设三轴转动的角度的计算公式，将胶囊内窥镜的姿态角解出，为方便论述，本实施例使用动力学中的滚动-俯仰-偏摆表示法，通过旋转矩阵R_rpy(φ，θ，ψ)将胶囊内窥镜的当前姿态角表示为：

而通过对该矩阵内的数据进行运算即可分别获得俯仰角pitch、横滚角roll和偏航角yaw的一般表达式，此处不再赘述。

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

本实施例还提供一种基于变化磁场的胶囊内窥镜全姿态测定系统，主要包括加速度传感器1、模式判断模块2、第一检测模块3、第一计算模块4、第二检测模块5和第二计算模块6。

其中，加速度传感器1主要用于检测胶囊内窥镜的当前加速度，该加速度传感器1一般可为三轴加速度检测仪。

模式判断模块2主要用于判断胶囊内窥镜当前所处位置的外部预设磁场是否满足预设条件，该预设条件一般为外部预设磁场形成水平定向磁场。

第一检测模块3主要用于在模式判断模块2的判断结果为是时，检测胶囊内窥镜当前所处位置的磁感应强度，一般的，该第一检测模块3具体可为磁场传感器等。第一计算模块4主要用于将磁感应强度和加速度同时代入预设公式中计算胶囊内窥镜的当前姿态角。

第二检测模块5主要用于在模式判断模块2的判断结果为否时，检测胶囊内窥镜绕自身预设三轴转动的角速度，一般的，该第二检测模块5具体可为陀螺仪等。第二计算模块6主要用于将各角速度分别进行积分计算胶囊内窥镜的当前姿态角。

另外，本实施例还增设了记录模块7和修正模块8。其中，记录模块7主要用于将外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻所计算的胶囊内窥镜的姿态角进行记录，并将其作为角速度积分运算的积分常数项，而修正模块8主要用于根据积分常数项对将各角速度分别进行积分计算的算式进行修正。

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

Claims (3)

1.一种基于变化磁场的胶囊内窥镜全姿态测定系统，其特征在于，包括：

加速度传感器，用于检测胶囊内窥镜的当前加速度；

模式判断模块，用于判断所述胶囊内窥镜当前所处位置的外部预设磁场是否满足预设条件；其中，所述预设条件为外部预设磁场为水平定向磁场或磁感线平行度高于95％的水平磁场；

第一检测模块，用于在所述模式判断模块的判断结果为是时，检测所述胶囊内窥镜当前所处位置的磁感应强度；

第一计算模块，用于将所述磁感应强度和加速度同时代入预设公式中计算所述胶囊内窥镜的当前姿态角；

第二检测模块，用于在所述模式判断模块的判断结果为否时，检测所述胶囊内窥镜绕自身预设三轴转动的角速度；

第二计算模块，用于将各所述角速度分别进行积分计算所述胶囊内窥镜的当前姿态角。

2.根据权利要求1所述的基于变化磁场的胶囊内窥镜全姿态测定系统，其特征在于，还包括：

记录模块，用于将外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻所计算的所述胶囊内窥镜的姿态角作为积分常数项；

修正模块，用于根据所述积分常数项对将各所述角速度分别进行积分计算的算式进行修正。

3.根据权利要求2所述的基于变化磁场的胶囊内窥镜全姿态测定系统，其特征在于，所述第一检测模块具体为磁场传感器，所述第二检测模块具体为陀螺仪。