CN106937860A - 获知实时位姿的胶囊内窥镜***及其工作方法 - Google Patents

Abstract

本发明提出了一种获知实时位姿的胶囊内窥镜***及其工作方法，包括如下步骤：S1，从胶囊内窥镜的射频通讯设备中采集惯性传感器数据，将获得的数据在远程服务器上运算处理，通过远程服务器进行数据降噪，去除不规则的传感器数据；S2，根据胶囊内窥镜的MEMS惯性传感器获取的加速度、角速度数据，进行位姿求解，得到胶囊内窥镜的实时位置和姿态信息；S3，将得到的实时位姿信息进行误差补偿，通过运算调整得到的位置和姿态信息值，从而使胶囊内窥镜定位定姿更加准确。

Description

获知实时位姿的胶囊内窥镜***及其工作方法

技术领域

本发明涉及医疗器械自动控制领域，尤其涉及一种获知实时位姿的胶囊内窥镜***及其工作方法。

背景技术

胶囊内镜是一种形状大小类似胶囊药丸的消化道内窥镜***，被检查者吞服后，胶囊内镜将沿消化道运动，沿途对消化道内壁进行拍摄检查，并通过无线通讯将拍摄的图像传输至外部终端，医生通过拍摄的图像资料来进行诊断。

目前市场上的胶囊内镜产品都是靠人体消化道自然蠕动来进行随机运动，在被检查者吞服过后，胶囊内镜在人体内属于盲区，医生无法得知检查过程中胶囊内镜的实时位置以及姿态信息，只能根据检查完毕后的图像信息，依靠医生的经验来对病灶进行定位和诊断，这样的检查手段增加了漏诊和误诊的概率，因此能实时获知胶囊内镜在被检查者体内的运动状态和相对位置信息，对于医生诊断是极其重要的，这也致使胶囊内镜在人体内的位置姿态测量方法成为该技术领域的研究热点。现阶段理论上提出的关于胶囊内镜的定位和姿态测量的方法，主要是利用磁场定位原理，通过胶囊内镜内嵌磁传感器，体外工作站建立磁体阵列，计算磁场理论模型，根据感应到的实际磁场强度和理论磁场模型来实现定位，此种方法受外界干扰严重，且实施困难，另外也有利用X-ray射线测距来进行胶囊位置确定，但这种方法不能确定胶囊姿态并且X-ray射线存在发射性对人体有害。这就亟需本领域技术人员解决相应的技术问题。

发明内容

本发明旨在至少解决现有技术中存在的技术问题，特别创新地提出了一种获知实时位姿的胶囊内窥镜***及其工作方法。

为了实现本发明的上述目的，本发明提供了一种获知实时位姿的胶囊内窥镜***，包括胶囊内窥镜，数据处理平台，胶囊内窥镜内部设有用于获取胶囊运动状态信息的MEMS惯性传感器1-4，所述胶囊内窥镜与数据处理平台无线通信连接。

所述的获知实时位姿的胶囊内窥镜***，优选的，所述数据处理平台包括：

无线通信数据收发模块，用于从胶囊内窥镜的射频通讯设备中获取惯性传感器数据和图像数据，并下发指令信息等；

胶囊运动位姿求解模块，用于根据胶囊内窥镜的MEMS惯性传感器获取的运动状态数据，进行位置和姿态的求解并对误差进行补偿，得到胶囊内窥镜的实时位置和姿态信息。

所述的胶囊内窥镜***，优选的：还包括用于显示胶囊内镜的运动状态和轨迹的显控平台。

所述的获知实时位姿的胶囊内窥镜***，优选的：所述数据处理平台还包括将处理后的数据信息发送给显控平台并从显控平台获取指令信息的通信模块。

所述的胶囊内窥镜***，优选的：所述MEMS惯性传感器获取的运动状态信息为三轴加速度信息和/或三轴角速度信息。

本发明还公开一种获知实时位姿的胶囊内窥镜***的工作方法，包括如下步骤：

S1，获取数据：获取胶囊内窥镜惯性传感器的数据并进行降噪处理；

S2，位姿求解：根据S1处理后的数据求解胶囊内窥镜的实时位置和姿态信息；

S3，误差补偿：对S2得到的位姿信息进行误差补偿得到准确的位姿信息值。

所述的基于获知实时位姿的胶囊内窥镜***的工作方法，优选的：还包括S4：将S3误差补偿后的数据输出至显控平台。

所述的基于获知实时位姿的胶囊内窥镜***的工作方法，优选的：所述S2包括：

S2-1，依靠MEMS惯性传感器的输出加速度和角速度信息实时计算出姿态矩阵从姿态矩阵中提取得到胶囊内镜的姿态信息，包括偏航角、俯仰角和横滚角；

S2-2，利用姿态矩阵将加速度输出值进行换算，得到姿态变换后的加速度值；

S2-3，经过M次加速度积分更新速度后，做一次位置更新，确定位置信息，根据不断更新的位置信息，完成胶囊内镜的运动轨迹描绘。

所述的基于获知实时位姿的胶囊内窥镜***的工作方法，优选的：还包括对姿态信息的偏差进行补偿的步骤。

所述的基于MEMS惯性传感器获知实时位姿的胶囊内窥镜***的工作方法，优选的，所述S2-1包括：

首先利用惯性加速度传感器数据a_x、a_y、a_z求解初始姿态角并确定此时的初始速度和初始位置，根据初始的俯仰角、横滚角、偏航角确定初始四元数q₀、q₁、q₂、q₃；而后利用惯性陀螺仪传感器测量的角速度信息ω_x、ω_y、ω_z更新四元数得到然后根据更新后的四元数计算得到更新的姿态矩阵最后利用公式从姿态矩阵中提取得到胶囊内镜的姿态信息，包括偏航角、俯仰角和横滚角。

所述的获知实时位姿的胶囊内窥镜***的工作方法，优选的，采用具有适当增益的扩展卡尔曼滤波方法对姿态信息的偏差进行补偿，具体包括：

在得到初始X0位置，进行过程方程计算x_k＝F_k,k-1x_k-1+w_k，

式中，M×1向量x_k表示***在离散时间k的状态向量，它是不可观测的；M×M矩阵F_k,k-1称为状态转移矩阵，描述动态***在时间k的状态到k+1的状态之间的转移；通过上式预测状态，然后通过进行更新状态操作，从而得到更新后的x_k；

通过随机线性***基于卡尔曼滤波方程如下：

a、一步预测方程时间更新：

b、一步预测均方误差矩阵：

c、卡尔曼增益：

d、状态向量滤波输出：

e、滤波协方差阵：

综上所述，由于采用了上述技术方案，本发明的有益效果是：

在胶囊内镜中嵌入MEMS惯性传感器，能在被检查者体内，不受外界环境干扰，通过自主感知的加速度和角速度信息，应用一系列惯性导航算法获取姿态、航向、速度和位置信息，此***不改变胶囊内镜整体结构，只需内嵌微小集成的惯性传感器件，便于实现。

获取胶囊内镜在人体内的位置和姿态，通过人机交互界面图形化描绘胶囊内镜姿态、航向、位置、速度等信息；并通过***不断的实时测量数据，连续描绘出胶囊内镜在被检查者体内的运动轨迹，结合后期拍摄的图像信息，使医生快捷方便的确定病变区域，降低漏诊和误诊概率。

使用数据处理平台来作为数据的处理中心，提高***实时性，使用显控平台图形化显示软件直观的展示胶囊在被检查者体内运动情况。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是本发明总体示意图；

图2是本发明胶囊示意图；

图3是本发明工作***示意图；

图4是本发明运行轨迹示意图；

图5是本发明工作过程示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

如图1所示，基于MEMS惯性传感器的胶囊内镜实时位姿测量***由三大部分组成，分别为嵌有MEMS惯性传感器的胶囊内镜本体、外部嵌入式数据处理平台、PC显控平台，这三部分在本***的主要功能如下：

嵌有MEMS惯性传感器的胶囊内镜本体

胶囊内镜本体主要完成检查拍摄等工作，其主要组成如图2所示，由1-1高清摄像装置、1-2微型MCU、1-3电池、1-4MEMS惯性传感器、1-5射频通讯设备、1-6胶囊外壳等部分组成。

感知胶囊内镜在人体内运动状态的核心即为嵌入在胶囊内镜内部的MEMS惯性传感器，本专利***选取6轴惯性测量单元传感器，其集成XYZ三轴加速度计和XYZ三轴陀螺仪，尺寸大小满足胶囊内镜要求，MEMS惯性传感器的主要功能是敏感胶囊内镜在被检查者体内的三轴加速度信息和三轴角速度信息，这些数据通过胶囊内镜的无线发射装置实时发送给体外设备。

外部嵌入式数据处理平台

由于胶囊内镜的运动传感器数据通过无线设备实时发送至体外，因此为了保证***的实时性，本专利***选取基于ARM的嵌入式数据处理平台，此平台主要完成无线数据收发、运动姿态和运动轨迹解算以及与PC显控平台的通信任务，其中无线数据收发包括两部分，分别为惯性传感器数据和图像数据；运动姿态和轨迹解算是胶囊内镜完成位姿测量的核心，主要包括信号滤波平滑处理、位姿解算、误差补偿等；数据计算处理完成以后通过USB通信方式传输给PC显控平台，实时呈现于人机交互界面。

PC显控平台

为了让医生能够更加直观的观察胶囊内镜在被检查者体内的情况，包括胶囊内镜的姿态、相对位置以及胶囊在被检查者体内运动的轨迹，本专利选取PC显控终端来完成这一人机交互的任务，通过图形化显示软件同步显示胶囊内镜运动状态，医生结合胶囊内镜拍摄的图像信息，可轻易的判断检查区域是否完整，快速诊断病灶的位置。

2、胶囊内镜姿态和位置求解算法介绍

本发明通过内嵌于胶囊内镜的6轴MEMS惯性传感器来感知胶囊在被检查者体内的运动状态信息，包括三轴线运动加速度和三轴角运动角速度信息，将上述信息通过捷联式惯性导航算法计算出胶囊内镜的姿态和位置信息，其基本的解算原理如图3所示：

如图3所示，利用惯性器件做胶囊内镜的位姿测量是一个很复杂的算法模型，其中姿态矩阵的更新算法是关键，假设胶囊内镜本体坐标系为b，我们用于观察测量的坐标系为n，则由b系到n系的坐标变换矩阵为称为胶囊内镜的姿态矩阵，姿态更新就是依靠MEMS惯性器件的输出实时计算出矩阵。本专利采用四元数算法来计算更新姿态矩阵，其基本的算法过程：首先利用惯性加速度传感器数据a_x、a_y、a_z求解初始姿态角并确定此时的初始速度和初始位置，根据初始的俯仰角、横滚角、偏航角确定初始四元数q₀、q₁、q₂、q₃，而后利用惯性陀螺仪传感器测量的角速度信息ω_x、ω_y、ω_z更新四元数得到 然后根据更新后的四元数计算得到更新的姿态矩阵最后利用公式从姿态矩阵中提取得到胶囊内镜的姿态信息，包括偏航角、俯仰角和横滚角。

确定了胶囊内镜的姿态矩阵以后，利用姿态矩阵将加速度输出值进行换算，胶囊内镜在观察测量坐标系中速度的微分方程可以表示为：

上式中，是b系中的加速度值经姿态换算以后得到的数值，在绝对理想环境中，即是胶囊内镜在测量坐标系中的加速度，但由于惯性传感器敏感的地球环境下所有的加速度，所以必须去除有害加速度，上式中被减去的分别是科氏加速度地向加速度以及重力加速度gⁿ。

设速度的更新周期为T，在每一个更新周期内对角速度和加速度做N次采样。对式1-1积分，得t_m时刻胶囊内镜在测量坐标系n内的速度：

上式中，和分别为t_m和t_m-1时刻胶囊内镜的速度；是t_m-1时刻的姿态矩阵。

经过M次速度更新以后，做一次位置更新，根据公式1-3：

其中只要确定了就能确定胶囊内镜在某一时刻位置信息，根据不断更新的位置信息，我们即可以完成胶囊内镜的运动轨迹描绘，如图4所示，胶囊内镜位置数据更新的速度越快，轨迹描绘就越直观。

由于胶囊内镜在被检查者体内属于高动态环境，是复杂空间角运动和线运动的复合，因此在算法模型中必须要考虑误差补偿模型，本专利针对姿态和角速度偏差通过具有适当增益的多态卡尔曼滤波得到最优估计，适用于胶囊内镜的工作环境，提高了姿态、航向、速度和位置的解算精度。

由上简述的姿态测量理论可知，尽管陀螺仪与加速度计组合均可得到载体的姿态角，但是姿态测量的传感器各有优缺点。陀螺仪动态响应特性良好，但计算姿态时会产生累计误差；加速度计和磁力计组合测量姿态虽然没有累计误差，但是其所确定的姿态角只适用于静态载体，因为此时的重力矢量和地磁场矢量在机体坐标系上的投影是始终不变的，但对于动态情况，载体的加速度计将敏感于载体机动而引起的附加加速度，所以必须对其进行补偿。因此，可以把加速度计和磁力计的输出作为一个参考基准，在对四元数更新时，纠正其积分的累计误差，实现传感器的数据融合，从而提高姿态测量精度和***的动态性能。本发明采用的误差补偿算法为扩展卡尔曼滤波算法，由描述状态向量的过程方程和描述观测向量的观测方程共同表示。

如图5所示，过程方程

x_k＝F_k,k-1x_k-1+w_k

式中，M×1向量x_k表示***在离散时间k的状态向量，它是不可观测的；M×M矩阵F_k,k-1称为状态转移矩阵，描述动态***在时间k的状态到k+1的状态之间的转移，它应该是已知的；而M×1向量w_k为过程噪声向量，它描述状态转移中间的加性噪声或误差。过程方程也称状态方程。

测量方程

z_k＝H_kx_k+v_k (3.51)

式中，z_k代表动态***在时间k的N×1观测向量；N×M矩阵H_k称为测量矩阵描述状态经过其作用变成可观测的，要求它也是已知的；v_k表示观测噪声向量，其维数与观测向量的相同。为了分析的方便，通常假定过程噪声w_k和观测噪声v_k为互不相关零均值的白噪声过程，分别代表***过程噪声和观测噪声，对应协方差矩阵分别为Q_k和R_k。

随机线性***基于卡尔曼滤波方程如下：

a。一步预测方程时间更新：

b。一步预测均方误差矩阵：

c。卡尔曼增益：

d。状态向量滤波输出：

e。滤波协方差阵：

以上公式中右上角带“-”标号的为预测值。

以上步骤构成了卡尔曼滤波的一个单循环流程。只要给定初始状态x₀和初始协方差矩阵P₀，根据k时刻的观测值z_k，就可以利用式(3.52)～式(3.55)递推计算得k时刻的最佳状态估计x_k。

根据上述步骤卡尔曼滤波的结构图如图5所示。卡尔曼滤波器是一种线性的离散时间有限维***，因而只能处理线性***。但是，实际***总是存在不同程度的非线性，因此对卡尔曼滤波算法做了改进，引入了扩展卡尔曼滤波算法。

扩展卡尔曼滤波方法处理非线性***的思想是将非线性函数在估计点附近进行泰勒展开，并用一个等价于常规卡尔曼滤波方程的近似矩阵来代替非线性函数，即将非线性方程近似认为是叠加白噪声后的一阶泰勒近似的线性化后的标准卡尔曼滤波方法。

在胶囊内镜中嵌入MEMS惯性测量单元，能在被检查者体内，不受外界环境干扰，通过自主感知的加速度和角速度信息，应用一系列惯性导航算法获取姿态、航向、速度和位置信息，此***不改变胶囊内镜整体结构，只需内嵌微小集成的惯性传感器件，便于实现。

获取胶囊内镜在人体内的位置和姿态，通过人机交互界面图形化描绘胶囊内镜俯姿态、航向、位置、速度等信息；并通过***不断的实时测量数据，连续描绘出胶囊内镜在被检查者体内的运动轨迹，结合后期拍摄的图像信息，使医生快捷方便的确定病变区域，降低漏诊和误诊概率。

使用外部嵌入式处理器来作为数据的处理中心，提高***实时性，使用PC计算机图形化显示软件直观的展示胶囊在被检查者体内运动情况。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (11)

1.一种获知实时位姿的胶囊内窥镜***，包括胶囊内窥镜，数据处理平台，其特征在于：胶囊内窥镜内部设有用于获取胶囊运动状态信息的MEMS惯性传感器(1-4)，所述胶囊内窥镜与数据处理平台无线通信连接。

2.根据权利要求1所述的获知实时位姿的胶囊内窥镜***，其特征在于，所述数据处理平台包括：

无线通信数据收发模块，用于从胶囊内窥镜的射频通讯设备中获取惯性传感器数据和图像数据，并下发指令信息等；

胶囊运动位姿求解模块，用于根据胶囊内窥镜的MEMS惯性传感器获取的运动状态数据，进行位置和姿态的求解并对误差进行补偿，得到胶囊内窥镜的实时位置和姿态信息。

3.根据权利要求1或2所述的获知实时位姿的胶囊内窥镜***，特征在于：还包括用于显示胶囊内镜的运动状态和轨迹的显控平台。

4.根据权利要求3所述的获知实时位姿的胶囊内窥镜***，其特征在于：所述数据处理平台还包括将处理后的数据信息发送给显控平台并从显控平台获取指令信息的通信模块。

5.根据权利要求1或2所述的胶囊内窥镜***，其特征在于：所述MEMS惯性传感器获取的运动状态信息为三轴加速度信息和/或三轴角速度信息。

6.一种获知实时位姿的胶囊内窥镜***的工作方法，其特征在于，包括如下步骤：

S1，获取数据：获取胶囊内窥镜惯性传感器的数据并进行降噪处理；

S2，位姿求解：根据S1处理后的数据求解胶囊内窥镜的实时位置和姿态信息；

S3，误差补偿：对S2得到的位姿信息进行误差补偿得到准确的位姿信息值。

7.根据权利要求6所述获知实时位姿的胶囊内窥镜***的工作方法，其特征在于：还包括S4：将S3误差补偿后的数据输出至显控平台。

8.根据权利要求6或7所述的基于获知实时位姿的胶囊内窥镜***的工作方法，其特征在于，所述S2包括：

S2-1，依靠MEMS惯性传感器的输出加速度和角速度信息实时计算出姿态矩阵从姿态矩阵中提取得到胶囊内镜的姿态信息，包括偏航角、俯仰角和横滚角；

S2-2，利用姿态矩阵将加速度输出值进行换算，得到姿态变换后的加速度值；

S2-3，经过M次加速度积分更新速度后，做一次位置更新，确定位置信息，根据不断更新的位置信息，完成胶囊内镜的运动轨迹描绘。

9.根据权利要求8所述的基于获知实时位姿的胶囊内窥镜***的工作方法，其特征在于：还包括对姿态信息的偏差进行补偿的步骤。

10.根据权利要求8所述的基于MEMS惯性传感器获知实时位姿的胶囊内窥镜***的工作方法，其特征在于，所述S2-1包括：

首先利用惯性加速度传感器数据a_x、a_y、a_z求解初始姿态角并确定此时的初始速度和初始位置，根据初始的俯仰角、横滚角、偏航角确定初始四元数q₀、q₁、q₂、q₃；而后利用惯性陀螺仪传感器测量的角速度信息ω_x、ω_y、ω_z更新四元数得到然后根据更新后的四元数计算得到更新的姿态矩阵最后利用公式从姿态矩阵中提取得到胶囊内镜的姿态信息，包括偏航角、俯仰角和横滚角。

11.根据权利要求9所述的获知实时位姿的胶囊内窥镜***的工作方法，其特征在于，采用具有适当增益的扩展卡尔曼滤波方法对姿态信息的偏差进行补偿，具体包括：

在得到初始X0位置，进行过程方程计算x_k＝F_k,k-1x_k-1+w_k，

式中，M×1向量x_k表示***在离散时间k的状态向量，它是不可观测的；M×M矩阵F_k,k-1称为状态转移矩阵，描述动态***在时间k的状态到k+1的状态之间的转移；通过上式预测状态，然后通过进行更新状态操作，从而得到更新后的x_k；

通过随机线性***基于卡尔曼滤波方程如下：

a、一步预测方程，根据时间更新：

$\stackrel{-}{x_k}=F_{k,k-1}{x}_{k-1}$

b、一步预测均方误差矩阵：

$P_k^{-}=F_{k,k-1}{P}_{k-1}{F_{k,k-1}}^T+Q_k$

c、卡尔曼增益：

$K_k\left(k\right)=P_k^{-}{H_k}^T{\[H_k{P}_k^{-}{H_k}^T+R_k\]}^{-1}$

d、状态向量滤波输出：

$x_k=\stackrel{-}{x_k}+K_k\[z_k-H\stackrel{-}{x_k}\]$

e、滤波协方差阵：

$P_k=P_k^{-}-K_k{H}_k{P}_k^{-}.$