CN107561462A - 基于变化磁场的胶囊全姿态测定系统及空间磁场检测装置 - Google Patents
基于变化磁场的胶囊全姿态测定系统及空间磁场检测装置 Download PDFInfo
- Publication number
- CN107561462A CN107561462A CN201710791663.3A CN201710791663A CN107561462A CN 107561462 A CN107561462 A CN 107561462A CN 201710791663 A CN201710791663 A CN 201710791663A CN 107561462 A CN107561462 A CN 107561462A
- Authority
- CN
- China
- Prior art keywords
- magnetic field
- magnetic
- capsule
- mrow
- space
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 239000002775 capsule Substances 0.000 title claims abstract description 108
- 238000001514 detection method Methods 0.000 title claims abstract description 46
- 238000005259 measurement Methods 0.000 claims abstract description 59
- 230000004927 fusion Effects 0.000 claims abstract description 24
- 230000001133 acceleration Effects 0.000 claims description 30
- 230000006698 induction Effects 0.000 claims description 20
- 239000003292 glue Substances 0.000 claims description 6
- 238000012360 testing method Methods 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 claims description 4
- 238000000034 method Methods 0.000 description 8
- 230000008859 change Effects 0.000 description 5
- 238000004891 communication Methods 0.000 description 3
- 230000033001 locomotion Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000003745 diagnosis Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 210000000936 intestine Anatomy 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 239000008280 blood Substances 0.000 description 1
- 210000004369 blood Anatomy 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000005253 cladding Methods 0.000 description 1
- 238000000205 computational method Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 238000005034 decoration Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 210000001035 gastrointestinal tract Anatomy 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 208000028774 intestinal disease Diseases 0.000 description 1
- 230000005923 long-lasting effect Effects 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000011002 quantification Methods 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 238000004611 spectroscopical analysis Methods 0.000 description 1
- 210000002784 stomach Anatomy 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Abstract
本发明公开了一种空间磁场检测装置,包括:磁场测量组件,包括至少两个用于测量外部牵引磁体所生成外部预设磁场内的特定位置的磁场向量的三轴磁传感器;下位机,用于采集三轴磁传感器所测得的数据并传输至上位机;上位机,用于接收下位机的数据并进行数据融合以确定外部牵引磁体的磁场方向。此种空间磁场检测装置可以实时对空间磁场情况进行准确获取。本发明还公开了一种包括上述空间磁场检测装置的基于变化磁场的胶囊全姿态测定系统,其空间磁场检测装置能够实时对空间磁场情况进行准确获取。
Description
技术领域
本发明涉及医疗设备技术领域,特别涉及一种空间磁场检测装置。此外,本发明还涉及一种包括上述空间磁场检测装置的基于变化磁场的胶囊全姿态测定系统。
背景技术
在医疗诊断中,胶囊内窥镜极大地拓展了医生的消化道检查视野,填补了胃镜、肠镜检查的盲区,解决了多年来小肠疾病和胃肠道隐血诊断方面的难题。
而目前广泛使用外部磁场驱动方式的主动可控式胶囊系统,该系统包括内置在胶囊内的永磁体或在胶囊外表面上包覆永磁体层以及外部牵引磁体,该外部牵引磁体可以采用能够通过人工控制的磁力臂,或者也可以采用三轴亥姆霍兹线圈,通过该三轴亥姆霍兹线圈的万向均匀旋转磁场驱动胶囊进行滚动、俯仰、偏航和前进等运动。
在主动式胶囊(以后简称胶囊)的使用中,胶囊空间姿态的全解析对于检测过程来说十分有必要。目前,胶囊的空间姿态全解析的实现方式是:胶囊内部集成传感器,对自身的各个姿态角进行测量,然后将数据实时的传递出来,在上位计算机中解析出胶囊的全姿态信息。
在现有技术中一种测量刚体全姿态信息的方法是使用加速度传感器和磁场传感器来完成主要的测量工作,通过胶囊的加速度向量和所处位置的空间磁场向量的数据融合,即可测定胶囊的姿态角。该空间磁场向量为由外部牵引磁体产生的外部预设磁场,该外部预设磁场一般是由医护人员设置、兼做控制胶囊在人体消化道内移动用,其产生的磁场处于周期性变化中。
然而,在通过磁场传感器进行胶囊的空间姿态角测定时,必须使外部预设磁场满足一定预设条件,即该外部预设磁场需为水平定向磁场(或者磁感线平行度高于95%的水平磁场),只有满足该条件的外部预设磁场,才能与加速度传感器数据融合后精确计算出胶囊的姿态角,但该在医护人员正常使用外部牵引磁体时,外部预设磁场不可能始终处于水平定向状态,在外部牵引磁体操作胶囊移动时,外部预设磁场的方向和磁场强度均不一致,此时再进行测定的话,角度误差太大,无法完成全姿态测定。而现有技术中还不存在有效可行的手段对空间磁场的情况实时进行准确获取,进而影响胶囊的姿态确定的准确性。
因此,如何实现实时对空间磁场情况的准确获取,是本领域技术人员目前需要解决的技术问题。
发明内容
有鉴于此,本发明的目的是提供一种空间磁场检测装置,能够实时对空间磁场情况进行准确获取。本发明的另一目的是提供一种包括上述空间磁场检测装置的基于变化磁场的胶囊全姿态测定系统,其空间磁场检测装置能够实时对空间磁场情况进行准确获取。
为实现上述目的,本发明提供如下技术方案:
一种空间磁场检测装置,包括:
磁场测量组件,包括至少两个用于测量外部牵引磁体所生成外部预设磁场内的特定位置的磁场向量的三轴磁传感器;
下位机,用于采集所述三轴磁传感器所测得的数据并传输至上位机;
所述上位机,用于接收所述下位机的数据并进行数据融合以确定所述外部牵引磁体的磁场方向。
优选地,每个所述磁场测量组件中的所有所述三轴磁传感器设于同一设定平面上。
优选地,所述设定平面平行于所述外部牵引磁体处于标准测量方位时所述外部预设磁场的磁场方向。
优选地,所述磁场测量组件为至少两个,且所有所述磁场测量组件沿距离所述外部牵引磁体由近至远的方向依次排列。
优选地,所述磁场测量组件中的所有所述三轴磁传感呈M×M矩阵排列,其中,M为≥2的正整数。
优选地,每个所述磁场测量组件中的所述三轴磁传感器与横向相邻所述三轴磁传感器、纵向相邻所述三轴磁传感器的距离均相同。
优选地,每个所述磁场测量组件对应一个所述下位机,且所述磁场测量组件与对应的所述下位机集成于同一测量装置中。
优选地,每个所述三轴磁传感器所测得的磁场向量为:
所述上位机经如下方式进行数据融合并得到的磁场向量
其中:
设定一个所述磁场测量组件中包括n个所述三轴磁传感器,n≥2且为正整数,对每个所述三轴磁传感器对应设定一个编号k,1≤k≤n且k为正整数;
以空间中一预设点为中心、以垂直于所述设定平面的方向为Z轴建立空间直角坐标系,X轴、Y轴均平行于所述设定平面,则X轴基向量为[1 0 0],Y轴基向量为[0 1 0],Z轴基向量为[0 0 1]。
一种基于变化磁场的胶囊全姿态测定系统,包括如上述任意一项所述的空间磁场检测装置;还包括:
加速度传感器,用于检测胶囊的当前加速度;
磁场传感器,用于检测所述胶囊当前所处位置的磁感应强度并驱动第一计算模块;
角速度传感器,用于检测所述胶囊绕自身预设三轴转动的角速度并驱动第二计算模块;
处理器,包括模式判断模块,用于根据所述空间磁场检测装置的检测结果判断所述胶囊当前所处位置的外部预设磁场是否满足预设条件,若是,驱动所述磁场传感器,若否,驱动所述角速度传感器;所述第一计算模块,用于将所述磁场传感器检测的磁感应强度和加速度同时代入预设公式中计算所述胶囊的当前姿态角;所述第二计算模块,用于将各所述角速度分别进行积分计算所述胶囊的当前姿态角。
优选地,所述第二计算模块具体包括:
记录单元,用于将所述外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻所计算的所述胶囊的姿态角作为积分常数项;
修正单元,用于根据所述积分常数项对将各所述角速度分别进行积分计算的算式进行修正。
本发明提供的空间磁场检测装置中,包括磁场测量组件、下位机和上位机。依次通过三轴磁传感器的测量、下位机的数据采集以及上位机的数据融合,至少两点的测量结合数据融合技术,可保证测量精度,可以实时对空间磁场情况进行准确获取,进而为之后进行胶囊全姿态确定的过程提供准确的依据,提高胶囊姿态确定的准确性。
本发明提供的包括上述空间磁场检测装置的基于变化磁场的胶囊全姿态测定系统,其空间磁场检测装置能够实时对空间磁场情况进行准确获取。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据提供的附图获得其他的附图。
图1为本发明所提供空间磁场检测装置所测量的外部预设磁场分布图,其中,实现表示磁感线,虚线矩形框中为外部预设磁场;
图2为本发明所提供空间磁场检测装置的结构示意图。
图1和图2中:
1-外部牵引磁体,2-胶囊,3-三轴磁传感器,4-下位机,5-测量装置,6-通讯线缆,7-上位机。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
本发明的核心是提供一种空间磁场检测装置,能够实时对空间磁场情况进行准确获取。本发明的另一核心是提供一种包括上述空间磁场检测装置的基于变化磁场的胶囊全姿态测定系统,其空间磁场检测装置能够实时对空间磁场情况进行准确获取。
请参考图1和图2,图1为本发明所提供空间磁场检测装置所测量的外部预设磁场分布图;图2为本发明所提供空间磁场检测装置的结构示意图。
本发明所提供空间磁场检测装置的一种具体实施例中,包括磁场测量组件、下位机4和上位机7。
磁场测量组件包括至少两个用于测量外部牵引磁体1所生成外部预设磁场内的特定位置的磁场向量的三轴磁传感器3。其中,磁场测量组件中可以根据实际需要设置两个或多个三轴磁传感器3。
下位机4用于采集三轴磁传感器3所测得的数据并传输至上位机7。
上位机7用于接收下位机4的数据并进行数据融合以确定外部牵引磁体1的磁场方向。
其中,每个磁场测量组件中的所有三轴磁传感器3可以设于同一设定平面上,在便于进行三轴磁传感器3的安装操作,且可以简化上位机7进行数据融合的复杂程度。
其中,设定平面可以平行于外部牵引磁体1处于标准测量方位时外部预设磁场的磁场方向,以进一步简化上位机7进行数据融合的复杂程度。当然,设定平面与外部牵引磁体1处于标准测量方位时外部预设磁场的磁场方向之间也可以具有一定的夹角,通常需要保证在30°之内。
在外部牵引磁体1处于一些特定姿态的情况下,在其某一划定区域内的磁场分布基本是同向的,其区域或此区域中的一部分为外部预设磁场,胶囊2在外部预设磁场中进行相应的运动。在应用磁场传感器进行胶囊2的空间姿态角测定时,外部牵引磁体1应处于标准测量方位,在此方位下,外部牵引磁体1所提供的磁场为水平定向磁场或者磁感线平行度高于95%的水平磁场。
以外部牵引磁体1处于标准测量方位时其下方所设定区域的外部预设磁场的磁场为水平定向磁场为例,则每个磁场测量组件中的所有三轴磁传感器3所在的同一设定平面为一水平面。在外部牵引磁体1处于标准测量方位的状态下,其下方外部预设磁场的磁感线分布基本是水平的,此时,三轴磁传感器3的测量值经上位机7的数据融合后可得到外部牵引磁体1处于标准测量方位时对应的磁场向量。
若在任意时刻,上位机7经数据融合后得到的结果与外部牵引磁体1处于标准测量方位时对应的磁场向量不同,而可以认定此时外部牵引磁体1相对于标准测量方位发生变化,同时,根据数据融合的结果,可以判定外部预设磁场的磁场大小与方向的具体变化情况,获得外部预设磁场的水平方向偏转角,进一步地,根据该空间磁场检测装置的检测结果即可判断是否可以利用磁场传感器确定胶囊2的姿态。当然,上位机7进行数据融合的具体算法不限于本实施例。
其中,磁场测量组件具体可以设置外部预设磁场的较边缘处,胶囊2位于磁场测量组件与外部牵引磁体1之间,以保证磁场测量组件的测量范围,此时,在实际测量中,磁场测量组件具体可以位于人体至下。当然,磁场测量组件在外部预设磁场中的位置设置不限于此。
可见,此种空间磁场检测装置中,依次通过三轴磁传感器3的测量、下位机4的数据采集以及上位机7的数据融合,至少两点的测量结合数据融合技术,可保证测量精度,可以实时对空间磁场情况进行准确获取,进而为之后进行胶囊2全姿态确定的过程提供准确的依据,提高胶囊2姿态确定的准确性,同时,不需要对外部牵引磁体1本身的姿态进行确定,测量的效率以及使用的安全性较高,成本较低。
具体地,磁场测量组件可以为至少两个,且所有磁场测量组件沿距离外部牵引磁体1由近至远的方向依次排列,即,磁场测量组件并列设置在外部牵引磁体1的一侧。
本实施例中,磁场测量组件具体可以为两个或多个,具体可以根据实际可占用空间或者其他因素确定数量。至少两个磁场测量组件的配合测定,可以提高测量精度。当然,磁场测量组件也可以为一个。
具体地,磁场测量组件中的所有三轴磁传感器3可以呈M×M矩阵排列,其中,M为≥2的正整数,以便于上位机7中数据融合算法的设定。
进一步地,每个磁场测量组件中的三轴磁传感器3可以与横向相邻三轴磁传感器3、纵向相邻三轴磁传感器3的距离均相同,以进一步便于上位机7中数据融合算法的设定。
显然,每个磁场测量组件中的三轴磁传感器3的排列方式不限于上述各个实施例。例如,在每个磁场测量组件中,所有三轴磁传感器3也可以排列形成一个圆圈。
上述各个实施例中,下位机4可以通过通讯线缆6连接于上位机7。由于在胶囊2的使用环境中存在磁场,通过通讯线缆6进行信号传输可以有效避免环境中的磁场对下位机4与上位机7之间的信号产生干扰。
上述各个实施例中每个磁场测量组件对应一个下位机4,且磁场测量组件与对应的下位机4集成于同一测量装置5中,以便于对磁场测量组件于下位机4进行取用操作。
上述各个实施例中,上位机7可以通过多种方式进行数据融合。一种具体的实施例中,每个三轴磁传感器3所测得的磁场向量为:
上位机7经如下方式进行数据融合并得到的磁场向量
其中:
设定一个磁场测量组件中包括n(n≥2且为正整数)个三轴磁传感器3,对每个三轴磁传感器3对应设定一个编号k,1≤k≤n且k为正整数;
以空间中一预设点为中心、以垂直于设定平面的方向为Z轴建立空间直角坐标系,X轴、Y轴均平行于设定平面,则X轴基向量为[1 0 0],Y轴基向量为[0 1 0],Z轴基向量为[0 0 1]。
以外部牵引磁体1处于标准测量方位时其下方所提供的预设外部磁场为水平定向磁场为例,则此时,以空间中一预设点为中心建立地面直角坐标系即为上述空间直角坐标系。其中,Z轴指向铅垂方向,X轴、Y轴均平行于水平面。根据上位机7数据融合后的结果,可方便计算磁场的水平偏转角,数据融合的方式较为简单,从而可以提高上位机7的运算效率。
除了上述空间磁场检测装置,本发明还提供了一种包括上述实施例公开的空间磁场检测装置的基于变化磁场的胶囊全姿态测定系统,其空间磁场检测装置能够实时对空间磁场情况进行准确获取。
进一步地,该胶囊全姿态测定系统还可以包括:加速度传感器、磁场传感器、角速度传感器和处理器,处理器具体可以包括模式判断模块、第一计算模块和第二计算模块。
其中,通过加速度传感器检测胶囊2当前的加速度。该加速度为矢量,具有大小和方向,该加速度传感器可以为设置在胶囊2内的三轴加速度传感器,以此来检测胶囊2三轴方向上的加速度。此处所指三轴方向即以胶囊2为基准制定的三维坐标系,其中的X、Y、Z轴可在胶囊2上任意指定,不影响其姿态的准确测定。但为方便论述,本实施例以下内容均以胶囊2的轴向方向为Z轴,且为右手坐标系为标准进行说明。如此,三轴加速度传感器检测出胶囊2的每轴方向上的加速度之后,即可将三轴上的各个加速度相加,计算出合加速度。
模式判断模块根据空间磁场检测装置的检测结果判断胶囊2当前所处位置的外部预设磁场是否满足预设条件。
由于通过空间磁场向量辅助测定胶囊2姿态角需要外部预设磁场满足一定条件—一般为该外部预设磁场需为水平定向磁场,或者磁感线平行度高于95%的水平磁场,而外部预设磁场在作业中又是阶段性变化的,因此首先需要根据空间磁场检测装置的检测结果判断胶囊2当前所处位置的外部预设磁场是否满足预设条件。
如果是,则说明此时可以通过空间磁场向量的辅助测定手段进行姿态角测定,此时,驱动磁场传感器,磁场传感器检测胶囊2当前所处位置的磁感应强度并驱动第一计算模块,然后第一计算模块将该磁场传感器检测的磁感应强度和胶囊2的当前加速度进行数据融合,将该磁感应强度和加速度同时代入预设公式中计算胶囊2的当前姿态角。
如果否,则说明此时无法通过空间磁场向量的辅助测定手段进行姿态角测定,考虑到胶囊2的姿态角变化时,其绕自身预设三轴转动的角度也同步发生变化,因此,此时可通过角速度传感器检测胶囊2绕自身预设三轴转动的角速度,然后将该三个角速度分别进行积分计算,即可计算出胶囊2绕自身预设三轴转动的角度并驱动第二计算模块,进而通过第二计算模块将各角速度分别进行积分计算胶囊2的当前姿态角。
如此,本实施例所提供的基于变化磁场的胶囊全姿态测定系统能够通过空间磁场装置准确获知外部预设磁场的情况,在外部预设磁场满足预设条件时,可通过胶囊2的加速度与所处位置的磁感应强度进行数据融合,精确地计算胶囊2的姿态角;而在外部预设磁场不满足预设条件时,可通过胶囊2的加速度与其绕自身预设三轴转动的角速度进行数据融合,精确地计算胶囊2的姿态角,因而,在外部预设磁场变化的环境下,本实施例所提供的胶囊2全姿态测定方法,总能精确地实现胶囊2的全姿态测定。
其中,用于检测胶囊2当前所处位置的磁感应强度的磁场传感器具体可设置在胶囊2内。与胶囊2的加速度一样,该磁感应强度同样为矢量,具有大小和方向,由于外部预设磁场在正常运行时,是呈阶段性变化的,因此其磁感应强度也随时处于变化中。
外部预设磁场在满足预设条件,形成水平定向磁场时,其磁场方向相对于预设的标准轴(一般可为以人体为中心建立的地面坐标系的水平坐标轴)的水平方向偏转角度可以通过空间磁场检测装置获知,设其角度为ξ。同时在将磁感应强度和加速度同时代入预设公式中计算胶囊2的当前姿态角时,具体的,首先可对胶囊2进行三轴坐标系构建。本实施例中均以胶囊2的轴向方向为Z轴方向,横截面方向为X-Y平面方向为例进行说明。
如此,胶囊2的x轴基向量即为[1 0 0],而胶囊2的y轴基向量即为[0 1 0],其z轴基向量即为[0 0 1]。同时可设检测出的当前加速度为[gx gy gz],胶囊2当前所处位置的磁感应强度为[mx my mz]。
同时,胶囊2的姿态由姿态角确定,而姿态角主要包括俯仰角、横滚角和偏航角,因此,可令俯仰角为pitch,横滚角为roll,偏航角为yaw。
其中,俯仰角pitch可以通过胶囊2的加速度和胶囊2自身坐标系Z轴之间的夹角来求得,所以俯仰角可以通过公式:
进行计算。
横滚角roll的计算方法同理,首先可设胶囊2的加速度在胶囊2自身坐标系xy平面上的投影向量为则为[gx gy 0],如此横滚角roll可通过公式:
进行计算。
其中,当且A∈R,A>0时,roll=|roll|,
当且A∈R,A≤0时,roll=-|roll|。
偏航角yaw需要将加速度和磁感应强度进行数据融合处理,同时还需要利用前述外部预设磁场在水平方向上的偏转角度ξ。
首先设θ为磁感应强度与胶囊2自身坐标系Z轴方向的水平方向夹角,其次,令其中,为与组成平面的法向量,与为组成平面的法向量,如此则有:
其中,当且A∈R,A>0时,θ=|θ|,
当且A∈R,A≤0时,θ=-|θ|;
上述A均为系数。
θ计算出之后,将其与ξ相加即为偏航角yaw,即yaw=ξ+θ。
至此,胶囊2的俯仰角pitch、横滚角roll和偏航角yaw均已计算出来,可以顺利获知胶囊2的姿态角,实现对胶囊2的全姿态解析。
而当外部预设磁场不满足预设条件时,需要检测胶囊2绕自身预设三轴转动的角速度,并将各角速度分别进行积分计算,以测定姿态角。
具体的,用于检测胶囊2绕自身预设三轴转动的角速度的角速度传感器可以设置在胶囊2内。进一步地,角速度传感器可以为陀螺仪。
同时,考虑到在MEMS领域,角速度传感器因为测量的是角速度而不是角度本身,会为系统带来误差。角度可以由角速度定积分求得,积分的过程中由于测量误差、采样误差等因素的影响,得到的是一个引入了误差的量,在短时间之内,对姿态测定的精度影响并不大,可以忽略,但是当长时间持续通过角速度积分的方式测定姿态角时,系统累计误差会随着时间的推移越来越大。针对此,本实施例中第二计算模块具体可以包括记录单元和修正单元,记录单元用于将外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻所计算的胶囊2的姿态角作为积分常数项;修正单元用于根据积分常数项对将各角速度分别进行积分计算的算式进行修正。
由于当外部预设磁场满足预设条件时,其测定的姿态角是精确无误差的,因此可将此段时间内测定的姿态角进行记录,当外部预设磁场突然发生变化,而不满足预设条件时,在外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻所记录的姿态角即为最后一组无累积误差数据,并将其作为积分常数项,作为后续通过角速度积分运算的修正参数,以此消除在长时间通过角速度积分求解姿态角的过程中形成的系统累积误差。
具体的,在将各角速度分别进行积分计算胶囊2的当前姿态角时,首先可通过公式:
计算t时刻胶囊2绕自身预设x轴转动的角度,
同时可通过公式:
计算t时刻胶囊2绕自身预设y轴转动的角度,
同时可通过公式:
计算t时刻胶囊2绕自身预设z轴转动的角度。
其中,wx、wy和wz分别为胶囊2绕自身预设三轴转动的角速度。
t为从外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻开始计时的时间,而α0、β0和γ0分别为积分常数项中胶囊2绕自身预设x、y和z轴的初始转动角度,各初始转动角度均与记录的最后一组无累积误差数据相关,可通过该组数据中的俯仰角pitch、横滚角roll和偏航角yaw转换而得。
之后,即可根据上述三个计算胶囊2绕自身预设三轴转动的角度的计算公式,将胶囊2的姿态角解出,为方便论述,本实施例使用动力学中的滚动-俯仰-偏摆表示法,通过旋转矩阵Rrpy(φ,θ,ψ)将胶囊2的当前姿态角表示为:
而通过对该矩阵内的数据进行运算即可分别获得俯仰角pitch、横滚角roll和偏航角yaw的一般表达式,此处不再赘述。
本说明书中各个实施例采用递进的方式描述,每个实施例重点说明的都是与其他实施例的不同之处,各个实施例之间相同相似部分互相参见即可。
以上对本发明所提供的基于变化磁场的胶囊全姿态测定系统及空间磁场检测装置进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以对本发明进行若干改进和修饰,这些改进和修饰也落入本发明权利要求的保护范围内。
Claims (10)
1.一种空间磁场检测装置,其特征在于,包括:
磁场测量组件,包括至少两个用于测量外部牵引磁体(1)所生成外部预设磁场内的特定位置的磁场向量的三轴磁传感器(3);
下位机(4),用于采集所述三轴磁传感器(3)所测得的数据并传输至上位机(7);
所述上位机(7),用于接收所述下位机(4)的数据并进行数据融合以确定所述外部牵引磁体(1)的磁场方向。
2.根据权利要求1所述的空间磁场检测装置,其特征在于,每个所述磁场测量组件中的所有所述三轴磁传感器(3)设于同一设定平面上。
3.根据权利要求2所述的空间磁场检测装置,其特征在于,所述设定平面平行于所述外部牵引磁体(1)处于标准测量方位时所述外部预设磁场的磁场方向。
4.根据权利要求2所述的空间磁场检测装置,其特征在于,所述磁场测量组件为至少两个,且所有所述磁场测量组件沿距离所述外部牵引磁体(1)由近至远的方向依次排列。
5.根据权利要求2所述的空间磁场检测装置,其特征在于,所述磁场测量组件中的所有所述三轴磁传感呈M×M矩阵排列,其中,M为≥2的正整数。
6.根据权利要求5所述的空间磁场检测装置,其特征在于,每个所述磁场测量组件中的所述三轴磁传感器(3)与横向相邻所述三轴磁传感器(3)、纵向相邻所述三轴磁传感器(3)的距离均相同。
7.根据权利要求1所述的空间磁场检测装置,其特征在于,每个所述磁场测量组件对应一个所述下位机(4),且所述磁场测量组件与对应的所述下位机(4)集成于同一测量装置(5)中。
8.根据权利要求2至7任意一项所述的空间磁场检测装置,其特征在于,每个所述三轴磁传感器(3)所测得的磁场向量为:
<mrow>
<mover>
<msub>
<mi>V</mi>
<mi>k</mi>
</msub>
<mo>&RightArrow;</mo>
</mover>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>V</mi>
<mrow>
<mi>k</mi>
<mi>x</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>V</mi>
<mrow>
<mi>k</mi>
<mi>y</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>V</mi>
<mrow>
<mi>k</mi>
<mi>z</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
所述上位机(7)经如下方式进行数据融合并得到的磁场向量
<mrow>
<mover>
<mi>V</mi>
<mo>&RightArrow;</mo>
</mover>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mn>1</mn>
<mi>n</mi>
</munderover>
<msub>
<mi>V</mi>
<mrow>
<mi>k</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>/</mo>
<mi>n</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mn>1</mn>
<mi>n</mi>
</munderover>
<msub>
<mi>V</mi>
<mrow>
<mi>k</mi>
<mi>y</mi>
</mrow>
</msub>
<mo>/</mo>
<mi>n</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mn>1</mn>
<mi>n</mi>
</munderover>
<msub>
<mi>V</mi>
<mrow>
<mi>k</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>/</mo>
<mi>n</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中:
设定一个所述磁场测量组件中包括n个所述三轴磁传感器(3),n≥2且为正整数,对每个所述三轴磁传感器(3)对应设定一个编号k,1≤k≤n且k为正整数;
以空间中一预设点为中心、以垂直于所述设定平面的方向为Z轴建立空间直角坐标系,X轴、Y轴均平行于所述设定平面,则X轴基向量为[1 0 0],Y轴基向量为[0 1 0],Z轴基向量为[0 0 1]。
9.一种基于变化磁场的胶囊全姿态测定系统,其特征在于,包括权利要求1至8任意一项所述的空间磁场检测装置;还包括:
加速度传感器,用于检测胶囊(2)的当前加速度;
磁场传感器,用于检测所述胶囊(2)当前所处位置的磁感应强度并驱动第一计算模块;
角速度传感器,用于检测所述胶囊(2)绕自身预设三轴转动的角速度并驱动第二计算模块;
处理器,包括模式判断模块,用于根据所述空间磁场检测装置的检测结果判断所述胶囊(2)当前所处位置的外部预设磁场是否满足预设条件,若是,驱动所述磁场传感器,若否,驱动所述角速度传感器;所述第一计算模块,用于将所述磁场传感器检测的磁感应强度和加速度同时代入预设公式中计算所述胶囊(2)的当前姿态角;所述第二计算模块,用于将各所述角速度分别进行积分计算所述胶囊(2)的当前姿态角。
10.根据权利要求9所述的胶囊全姿态测定系统,其特征在于,所述第二计算模块具体包括:
记录单元,用于将所述外部预设磁场在从满足预设条件转变为不满足预设条件的瞬时时刻所计算的所述胶囊(2)的姿态角作为积分常数项;
修正单元,用于根据所述积分常数项对将各所述角速度分别进行积分计算的算式进行修正。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710791663.3A CN107561462A (zh) | 2017-09-05 | 2017-09-05 | 基于变化磁场的胶囊全姿态测定系统及空间磁场检测装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710791663.3A CN107561462A (zh) | 2017-09-05 | 2017-09-05 | 基于变化磁场的胶囊全姿态测定系统及空间磁场检测装置 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN107561462A true CN107561462A (zh) | 2018-01-09 |
Family
ID=60979068
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710791663.3A Pending CN107561462A (zh) | 2017-09-05 | 2017-09-05 | 基于变化磁场的胶囊全姿态测定系统及空间磁场检测装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107561462A (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109324298A (zh) * | 2018-09-06 | 2019-02-12 | 北京理工大学 | 一种基于检测阵列运动规划的磁源磁场信号检测方法 |
CN113367640A (zh) * | 2021-06-09 | 2021-09-10 | 东北大学秦皇岛分校 | 一种无线胶囊机器人 |
CN113679328A (zh) * | 2020-05-19 | 2021-11-23 | 紫东信息科技(苏州)有限公司 | 一种基于磁阵列的胶囊内窥镜位置测定方法及装置 |
CN113805126A (zh) * | 2021-10-09 | 2021-12-17 | 深圳市资福医疗技术有限公司 | 磁场测量设备 |
CN116027232A (zh) * | 2023-03-29 | 2023-04-28 | 中国科学院国家天文台 | 火星表面磁场信号的确定方法和装置 |
CN116449204A (zh) * | 2023-06-16 | 2023-07-18 | 德电北斗电动汽车有限公司 | 用于对置活塞磁力线性发电机的故障检测方法及相关装置 |
Citations (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102302357A (zh) * | 2011-06-22 | 2012-01-04 | 清华大学 | 用于球囊内窥镜的姿态感知系统及球囊内窥镜 |
CN103637803A (zh) * | 2013-11-14 | 2014-03-19 | 上海交通大学 | 基于永磁和感应线圈的胶囊内镜空间定位系统及定位方法 |
CN103809133A (zh) * | 2012-11-02 | 2014-05-21 | 高雄应用科技大学 | 具有磁通导引器的三轴磁场感测装置 |
CN104203068A (zh) * | 2012-05-14 | 2014-12-10 | 奥林巴斯医疗株式会社 | 胶囊型医疗装置以及医疗系统 |
CN104347444A (zh) * | 2013-07-29 | 2015-02-11 | 美格纳半导体有限公司 | 用于测试磁场传感器的装置及其方法 |
CN204405819U (zh) * | 2014-12-30 | 2015-06-17 | 北京时代民芯科技有限公司 | 一种集成三轴磁传感器 |
TW201621341A (zh) * | 2014-12-01 | 2016-06-16 | Univ Nat Taiwan | 磁場感測裝置及方法 |
CN106152921A (zh) * | 2015-05-12 | 2016-11-23 | 波音公司 | 磁性目标的感测 |
CN106618456A (zh) * | 2016-12-21 | 2017-05-10 | 重庆金山医疗器械有限公司 | 胶囊内窥镜控制系统 |
CN106767804A (zh) * | 2016-12-28 | 2017-05-31 | 华中科技大学 | 一种运动物体的多维数据测量装置及方法 |
CN106885566A (zh) * | 2017-01-03 | 2017-06-23 | 浙江大学 | 一种可穿戴式运动传感器及其抗磁场干扰的方法 |
CN106963326A (zh) * | 2017-04-12 | 2017-07-21 | 重庆金山医疗器械有限公司 | 胃部轮廓检测及模型生成方法 |
CN107076808A (zh) * | 2014-09-26 | 2017-08-18 | 旭化成微电子株式会社 | 磁传感器 |
CN207366718U (zh) * | 2017-09-05 | 2018-05-15 | 重庆金山医疗器械有限公司 | 用于变化磁场胶囊全姿态测定系统的空间磁场检测装置 |
-
2017
- 2017-09-05 CN CN201710791663.3A patent/CN107561462A/zh active Pending
Patent Citations (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102302357A (zh) * | 2011-06-22 | 2012-01-04 | 清华大学 | 用于球囊内窥镜的姿态感知系统及球囊内窥镜 |
CN104203068A (zh) * | 2012-05-14 | 2014-12-10 | 奥林巴斯医疗株式会社 | 胶囊型医疗装置以及医疗系统 |
CN103809133A (zh) * | 2012-11-02 | 2014-05-21 | 高雄应用科技大学 | 具有磁通导引器的三轴磁场感测装置 |
CN104347444A (zh) * | 2013-07-29 | 2015-02-11 | 美格纳半导体有限公司 | 用于测试磁场传感器的装置及其方法 |
CN103637803A (zh) * | 2013-11-14 | 2014-03-19 | 上海交通大学 | 基于永磁和感应线圈的胶囊内镜空间定位系统及定位方法 |
CN107076808A (zh) * | 2014-09-26 | 2017-08-18 | 旭化成微电子株式会社 | 磁传感器 |
TW201621341A (zh) * | 2014-12-01 | 2016-06-16 | Univ Nat Taiwan | 磁場感測裝置及方法 |
CN204405819U (zh) * | 2014-12-30 | 2015-06-17 | 北京时代民芯科技有限公司 | 一种集成三轴磁传感器 |
CN106152921A (zh) * | 2015-05-12 | 2016-11-23 | 波音公司 | 磁性目标的感测 |
CN106618456A (zh) * | 2016-12-21 | 2017-05-10 | 重庆金山医疗器械有限公司 | 胶囊内窥镜控制系统 |
CN106767804A (zh) * | 2016-12-28 | 2017-05-31 | 华中科技大学 | 一种运动物体的多维数据测量装置及方法 |
CN106885566A (zh) * | 2017-01-03 | 2017-06-23 | 浙江大学 | 一种可穿戴式运动传感器及其抗磁场干扰的方法 |
CN106963326A (zh) * | 2017-04-12 | 2017-07-21 | 重庆金山医疗器械有限公司 | 胃部轮廓检测及模型生成方法 |
CN207366718U (zh) * | 2017-09-05 | 2018-05-15 | 重庆金山医疗器械有限公司 | 用于变化磁场胶囊全姿态测定系统的空间磁场检测装置 |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109324298A (zh) * | 2018-09-06 | 2019-02-12 | 北京理工大学 | 一种基于检测阵列运动规划的磁源磁场信号检测方法 |
CN109324298B (zh) * | 2018-09-06 | 2020-05-15 | 北京理工大学 | 一种基于检测阵列运动规划的磁源磁场信号检测方法 |
CN113679328A (zh) * | 2020-05-19 | 2021-11-23 | 紫东信息科技(苏州)有限公司 | 一种基于磁阵列的胶囊内窥镜位置测定方法及装置 |
CN113367640A (zh) * | 2021-06-09 | 2021-09-10 | 东北大学秦皇岛分校 | 一种无线胶囊机器人 |
CN113805126A (zh) * | 2021-10-09 | 2021-12-17 | 深圳市资福医疗技术有限公司 | 磁场测量设备 |
CN116027232A (zh) * | 2023-03-29 | 2023-04-28 | 中国科学院国家天文台 | 火星表面磁场信号的确定方法和装置 |
CN116449204A (zh) * | 2023-06-16 | 2023-07-18 | 德电北斗电动汽车有限公司 | 用于对置活塞磁力线性发电机的故障检测方法及相关装置 |
CN116449204B (zh) * | 2023-06-16 | 2023-08-11 | 德电北斗电动汽车有限公司 | 用于对置活塞磁力线性发电机的故障检测方法及相关装置 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107561462A (zh) | 基于变化磁场的胶囊全姿态测定系统及空间磁场检测装置 | |
CN107348931B (zh) | 一种胶囊内窥镜空间姿态测定系统 | |
AU675077B2 (en) | Position location system | |
US8818751B2 (en) | Interpreting angular orientation data | |
CN106153077B (zh) | 一种用于m-imu人体运动捕获系统的初始化校准方法 | |
US20150149104A1 (en) | Motion Tracking Solutions Using a Self Correcting Three Sensor Architecture | |
US8396684B2 (en) | Method and system for motion tracking | |
CN104739411B (zh) | 一种使用磁传感器对磁性目标进行检测定位的方法 | |
US20050284221A1 (en) | Shape-acceleration measurement device and method | |
CN109620104B (zh) | 胶囊内窥镜及其定位方法及系统 | |
CN106937860A (zh) | 获知实时位姿的胶囊内窥镜系统及其工作方法 | |
CN107374566B (zh) | 一种基于变化磁场的胶囊内窥镜全姿态测定系统 | |
CN103175502A (zh) | 一种基于数据手套低速运动的姿态角检测方法 | |
CN109813336A (zh) | 惯性测量单元标定方法 | |
JP2022547189A (ja) | デジタル磁力計を用いた可撓性カテーテルの磁気追跡システムおよび方法 | |
CN207366718U (zh) | 用于变化磁场胶囊全姿态测定系统的空间磁场检测装置 | |
CN102778965B (zh) | 3d指示装置与补偿3d指示装置的转动的方法 | |
CN108451490A (zh) | 一种在消化腔体内搜寻胶囊内窥镜的系统及方法 | |
KR20200143728A (ko) | 휴대용 시스템 및 원격 오브젝트의 위치 결정 및 방향 결정을 위한 방법 | |
CN107361722A (zh) | 消化道诊断仪及胶囊内窥镜显示图像视角调整方法和系统 | |
WO2011016302A1 (ja) | モーションキャプチャ用のマーカ | |
EP1955013B1 (en) | Shape-acceleration measurement device and method | |
EP2550908A1 (en) | Apparatus for determining a spatial path of a flexible or semi-rigid elongated body | |
EP1744165B1 (en) | Shape-acceleration measurement device and method | |
CN111374784A (zh) | 一种增强现实ar定位系统及方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
TA01 | Transfer of patent application right | ||
TA01 | Transfer of patent application right |
Effective date of registration: 20210817 Address after: 401120 1-1, 2-1, 3-1, building 5, No. 18, Cuiping Lane 2, Huixing street, Yubei District, Chongqing Applicant after: Chongqing Jinshan Medical Technology Research Institute Co.,Ltd. Address before: 401120 1 office buildings, Jinshan International Industrial City, 18 of Nei sang Road, Hui Xing street, Yubei District, Chongqing. Applicant before: CHONGQING JINSHAN MEDICAL APPLIANCE Co.,Ltd. |