CN115886692B - 磁控悬浮胶囊机器人的实时定位方法、系统及存储介质 - Google Patents
磁控悬浮胶囊机器人的实时定位方法、系统及存储介质 Download PDFInfo
- Publication number
- CN115886692B CN115886692B CN202310198790.8A CN202310198790A CN115886692B CN 115886692 B CN115886692 B CN 115886692B CN 202310198790 A CN202310198790 A CN 202310198790A CN 115886692 B CN115886692 B CN 115886692B
- Authority
- CN
- China
- Prior art keywords
- magnetic field
- capsule robot
- sensor
- calculating
- real
- 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.)
- Active
Links
- 239000002775 capsule Substances 0.000 title claims abstract description 139
- 238000000034 method Methods 0.000 title claims abstract description 57
- 239000000725 suspension Substances 0.000 title claims abstract description 39
- 238000003860 storage Methods 0.000 title claims abstract description 13
- 238000005339 levitation Methods 0.000 claims abstract description 35
- 230000007613 environmental effect Effects 0.000 claims abstract description 10
- 239000013598 vector Substances 0.000 claims description 26
- 230000005540 biological transmission Effects 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000005457 optimization Methods 0.000 claims description 9
- 230000005415 magnetization Effects 0.000 claims description 4
- 238000004891 communication Methods 0.000 claims description 3
- 230000035699 permeability Effects 0.000 claims 3
- 230000006870 function Effects 0.000 description 11
- 230000008901 benefit Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 210000001035 gastrointestinal tract Anatomy 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 230000009471 action Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000007689 inspection Methods 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 3
- 239000000203 mixture Substances 0.000 description 3
- 230000004075 alteration Effects 0.000 description 2
- 238000001839 endoscopy Methods 0.000 description 2
- 230000033001 locomotion Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 230000009747 swallowing Effects 0.000 description 2
- 208000018522 Gastrointestinal disease Diseases 0.000 description 1
- 208000012671 Gastrointestinal haemorrhages Diseases 0.000 description 1
- 206010018001 Gastrointestinal perforation Diseases 0.000 description 1
- 241000135164 Timea Species 0.000 description 1
- 238000007792 addition Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000740 bleeding effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000013480 data collection Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000003745 diagnosis Methods 0.000 description 1
- 230000026058 directional locomotion Effects 0.000 description 1
- 238000007599 discharging Methods 0.000 description 1
- 230000005672 electromagnetic field Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000002496 gastric effect Effects 0.000 description 1
- 208000030304 gastrointestinal bleeding Diseases 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 210000000056 organ Anatomy 0.000 description 1
- 230000008855 peristalsis Effects 0.000 description 1
- 230000006798 recombination Effects 0.000 description 1
- 238000005215 recombination Methods 0.000 description 1
- 238000002922 simulated annealing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种磁控悬浮胶囊机器人的实时定位方法、系统及存储介质。其中，磁控悬浮胶囊机器人的实时定位方法，包括：获取环境磁场数据BE1；根据磁偶极子模型，计算胶囊机器人内部永磁体在空间的理论磁场
；将胶囊机器人置入工作空间，实时获取悬浮电磁线圈及偏转电磁线圈的电流，计算悬浮电磁线圈及偏转电磁线圈在各个设定位置处产生的磁场BE2；实时获取工作空间内的磁场数据Bw1；基于磁场数据Bw1、环境磁场数据BE1和磁场BE2计算胶囊机器人内部永磁体所产生的磁场
；基于理论磁场
和磁场
计算胶囊机器人的实时位置和姿态参数。达到在电磁线圈控制系统中精确控制胶囊机器人的目的。
Description
技术领域
本发明属于医疗技术领域，尤其是涉及一种磁控悬浮胶囊机器人的实时定位方法、系统及存储介质。
背景技术
内窥镜检查是目前最为常见且有效的胃肠道疾病临床诊断手段。但现阶段的内窥镜能插入消化道的长度有限，存在漏检风险，而且这种侵入性的检查通常会使患者感到极大的不适，并有一定几率造成肠胃穿孔、出血等。
胶囊机器人作为胃镜、肠镜等传统内窥镜检查手段的一种新型替代技术，可以像胶囊一样通过吞咽进入人体胃肠道，并对人体消化道及各种器官的健康状况进行医学探查。前期的胶囊机器人在检查时，患者只需简单吞服胶囊机器人，并佩戴好随身数据接收装置，后期正常排出胶囊即可完成胃肠道的拍照检查。然而，这种通过被动吞咽依靠胃肠道蠕动完成拍照检查的方式，检查时间长，照片数量大，给医生后期诊断带来很大麻烦，并且由于胶囊在人体内部的运动不可控制，很容易造成漏检。因此，基于主动控制的胶囊机器人应运而生，其中磁控胶囊机器人成为主流控制驱动方式。其原理是胶囊机器人内部内置一块永磁体，胶囊外部有一套磁场控制系统，该系统可产生所需的磁场来控制胶囊机器人在人体内部的运动。
主动磁控胶囊机器人在人体内部工作时，由外部磁控系统进行主动控制，由于人体内部胃肠道结构复杂，为了更全面的对患者身体内部进行检测，需要胶囊机器人不断变换姿态和位置，以避免出现漏检，要达到这个目的，就需要实时掌握胶囊机器人在人体内部的位置和姿态，胶囊机器人的实时定位是十分必要的。
目前，对磁控胶囊机器人的定位方法根据控制系统以及胶囊机器人内部永磁体形状的不同而有所不同。有的系统采用永磁驱动，即利用机械臂带动外部永磁体吸引胶囊机器人内部永磁体以实现胶囊机器人的运动，其定位方法针对两个永磁体，采用非线性算法不断迭代，实现胶囊机器人的定位，但该种方法要求磁场环境相对简单，且需同时进行多次迭代计算，用时较长，很难满足实时定位的需要。如公开号为CN113288008 A，名称为一种磁性胶囊内窥镜全姿态测定方法中公开的技术方案适用于机械臂式永磁体控制系统的定位，不能解决电磁线圈控制系统的胶囊机器人定位。
有的胶囊内部采用立方体形状的永磁体，可以实现矩形永磁体的六个维度定位，但矩形永磁体由于形状原因与胶囊机器人的融合难度较大。有的定位方法采用角速度传感器实现定位，但是角速度传感器的数据在转换为角度数据时存在累计误差，导致定位结果不准确。还有一类方法可以通过多次旋转胶囊方向实现全姿态定位，但是该方法操作过程复杂，不利于实现胶囊的快速实时定位。
发明内容
针对现有技术中存在的问题，本发明提供了一种磁控悬浮胶囊机器人的实时定位方法、系统及存储介质，至少部分的解决现有技术中存在的电磁线圈控制系统的胶囊机器人不能精确控制的问题。
第一方面，本公开实施例提供了一种磁控悬浮胶囊机器人的实时定位方法，胶囊机器人内部安装有永磁体，所述永磁体磁化方向与胶囊机器人轴向方向一致或垂直，所述胶囊机器人工作在工作空间，所述工作空间为磁控悬浮系统，所述磁控悬浮系统包括悬浮电磁线圈及偏转电磁线圈，所述方法包括：
获取环境磁场数据BE1 ；
根据磁偶极子模型，计算胶囊机器人内部永磁体在空间的理论磁场 ；
将胶囊机器人置入工作空间，实时获取悬浮电磁线圈及偏转电磁线圈的电流，计算悬浮电磁线圈及偏转电磁线圈在各个设定位置处产生的磁场BE2；
实时获取工作空间内的磁场数据Bw1；
基于磁场数据Bw1、环境磁场数据BE1和磁场BE2计算胶囊机器人内部永磁体所产生的磁场 ；
基于理论磁场 和磁场 计算胶囊机器人的实时位置和姿态参数。
可选的，所述基于理论磁场 和磁场 计算胶囊机器人的实时位置和姿态参数，包括：
基于理论磁场 和磁场 构建目标函数，设定约束条件和初始值，利用全局最优化算法经过有限次迭代寻找最优解附近的解，将解表示为向量 ；
利用非线性方程组优化算法，设置最大迭代次数和迭代精度，以向量 作为初值，经过多次迭代，得到胶囊机器人的实时位置参数和姿态参数。
可选的，磁控悬浮系统在设定位置处设定s个传感器，所述基于理论磁场 和磁场 构建目标函数构建的目标函数为：
其中， 、 和 分别为磁场 第s个传感器处的磁场数据的 、 和轴的数据分量； 、 和 分别为理论磁场 第s个传感器处的磁场数据的 、 和 轴的数据分量， 为常数。
可选的，所述设定约束条件和初始值，包括：
位置约束条件和姿态参数约束条件，所述位置约束条件为 ，姿态参数约束条件为 ，且满足 。
可选的，所述磁控悬浮系统在设定位置处设定s个传感器；
所述根据磁偶极子模型，计算胶囊机器人内部永磁体在空间的理论磁场 ，包括：
根据磁偶极子模型，计算胶囊机器人内部永磁体在第s个传感器处的理论磁场，具体计算表达式为：
其中， ， 为胶囊机器人位置， ， 胶囊机器人姿态， 为胶囊机器人内部永磁体参数， 为胶囊机器人到第s个传感器的距离， ，传感器的位置坐标 。
可选的， ， ， 。
可选的，所述磁控悬浮系统在设定位置处设定s个传感器，所述计算悬浮电磁线圈及偏转电磁线圈在各个设定位置处产生的磁场BE2，包括：
计算悬浮电磁线圈及偏转电磁线圈在第s个传感器处产生的三个方向上的磁场向量 ，具体计算公式为：
其中， ，N为线圈匝数，I为线圈电流，r为电流元到第s个的距离向量。
可选的，所述磁控悬浮系统在设定位置处设定s个传感器，所述基于磁场数据Bw1、环境磁场数据BE1和磁场BE2计算胶囊机器人内部永磁体所产生的磁场 ，包括：计算胶囊机器人内部永磁体在第s个传感器处的三轴方向的磁场向量；
在第s个传感器处的三轴方向的磁场向量表示为：
其中， 为磁场数据Bw1第s个传感器处的磁场数据， 为环境磁场数据BE1第s个传感器处的磁场数据， 为磁场BE2第s个传感器处的磁场数据。
第二方面，本公开实施例还提供了一种磁控悬浮胶囊机器人的定位系统，使用第一方面任一所述的磁控悬浮胶囊机器人的实时定位方法，包括定位板、传感器阵列、传输芯片、嵌入式微控制器、数据采集卡和电脑端，所述传输芯片包括多路复用芯片或片选芯片；
所述定位板用于安装传感器阵列和标定定位系统的坐标系；
所述传感器阵列用于实时采集工作空间的磁场信号，经所述传输芯片发送至嵌入式微控制器，所述传输芯片根据嵌入式微控制器指令对传感器进行有序选择并建立通信通道；
所述嵌入式微控制器通过所述传输芯片控制传感器信号的采集与传输，所述嵌入式微控制器将采集到的信号传输至电脑端，所述嵌入式微控制器接收电脑端的程序指令；
所述数据采集卡实时采集磁控悬浮系统的电磁线圈中的电流，并将采集数据传输给所述电脑端；
所述电脑端用于接收所述嵌入式微控制器发送的数据，接收所述数据采集卡发送的数据，计算并显示定位结果，并发送指令给嵌入式微控制器和数据采集卡。
第三方面，本公开实施例还提供了一种计算机可读存储介质，该计算机可读存储介质存储计算机指令，该计算机指令用于使计算机执行第一方面任一所述的磁控悬浮胶囊机器人的实时定位方法。
本发明提供的磁控悬浮胶囊机器人的实时定位方法、系统及存储介质，其中该磁控悬浮胶囊机器人的实时定位方法，通过基于磁场数据Bw1、环境磁场数据BE1和磁场BE2计算胶囊机器人内部永磁体所产生的磁场 ；基于理论磁场 和磁场 计算胶囊机器人的实时位置和姿态参数，从而达到在电磁线圈控制系统中精确控制胶囊机器人的目的。
可以有效解决永磁驱动胶囊机器人在人体内部定位效率较低、定位时间较长的问题，以及解决根据角速度传感器数据实现定位方法中的累计误差问题。能够快速、准确的实现复杂电磁场环境下悬浮胶囊机器人的实时定位，操作流程简单，单次定位耗时短，定位精度高，不产生累计误差。
附图说明
通过结合附图对本公开示例性实施例进行更详细的描述，本公开的上述以及其它目的、特征和优势将变得更加明显，其中，在本公开示例性实施例中，相同的参考标号通常代表相同部件。
图1为本公开实施例提供的磁控悬浮系统的结构示意图；
图2为本公开实施例提供的一种磁控悬浮胶囊机器人的定位系统的原理框图；
图3为本公开实施例提供的一种磁控悬浮胶囊机器人的实时定位方法的流程图；
图4为本公开实施例提供的以传感器阵列所在平面为水平面建立的空间坐标系示意图。
具体实施方式
下面结合附图对本公开实施例进行详细描述。
应当明确，以下通过特定的具体实例说明本公开的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本公开的其他优点与功效。显然，所描述的实施例仅仅是本公开一部分实施例，而不是全部的实施例。本公开还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本公开的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本公开中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。
需要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本公开，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。
还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本公开的基本构想，图示中仅显示与本公开中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践所述方面。
为了便于理解，本实施例公开了一种磁控悬浮胶囊机器人的实时定位方法，胶囊机器人内部安装有永磁体，所述永磁体磁化方向与胶囊机器人轴向方向一致或垂直，所述胶囊机器人工作在工作空间，所述工作空间为磁控悬浮系统，所述磁控悬浮系统包括悬浮电磁线圈及偏转电磁线圈，所述方法包括：
获取环境磁场数据BE1 ；
根据磁偶极子模型，计算胶囊机器人内部永磁体在空间的理论磁场 ；
将胶囊机器人置入工作空间，实时获取悬浮电磁线圈及偏转电磁线圈的电流，计算悬浮电磁线圈及偏转电磁线圈在各个设定位置处产生的磁场BE2；
实时获取工作空间内的磁场数据Bw1；
基于磁场数据Bw1、环境磁场数据BE1和磁场BE2计算胶囊机器人内部永磁体所产生的磁场 ；
基于理论磁场 和磁场 计算胶囊机器人的实时位置和姿态参数。
可选的，所述基于理论磁场 和磁场 计算胶囊机器人的实时位置和姿态参数，包括：
基于理论磁场 和磁场 构建目标函数，设定约束条件和初始值，利用全局最优化算法经过有限次迭代寻找最优解附近的解，将解表示为向量 ；
利用非线性方程组优化算法，设置最大迭代次数和迭代精度，以向量 作为初值，经过多次迭代，得到胶囊机器人的实时位置参数和姿态参数。
可选的，磁控悬浮系统在设定位置处设定s个传感器，所述基于理论磁场 和磁场 构建目标函数构建的目标函数为：
其中， 、 和 分别为磁场 第s个传感器处的磁场数据的 、 和轴的数据分量； 、 和 分别为理论磁场 第s个传感器处的磁场数据的 、 和 轴的数据分量， 为常数。
可选的，所述设定约束条件和初始值，包括：
位置约束条件和姿态参数约束条件，所述位置约束条件为 ，姿态参数约束条件为 ，且满足 。
可选的，所述磁控悬浮系统在设定位置处设定s个传感器；
所述根据磁偶极子模型，计算胶囊机器人内部永磁体在空间的理论磁场 ，包括：
根据磁偶极子模型，计算胶囊机器人内部永磁体在第s个传感器处的理论磁场，具体计算表达式为：
其中， ， 为胶囊机器人位置， ，胶囊机器人姿态， 为胶囊机器人内部永磁体参数， 为胶囊机器人到第s个传感器的距离， ，传感器的位置坐标 。
可选的， ， ， 。
可选的，所述磁控悬浮系统在设定位置处设定s个传感器，所述计算悬浮电磁线圈及偏转电磁线圈在各个设定位置处产生的磁场BE2，包括：
计算悬浮电磁线圈及偏转电磁线圈在第s个传感器处产生的三个方向上的磁场向量 ，具体计算公式为：
其中， ，N为线圈匝数，I为线圈电流，r为电流元到第s个的距离向量， 为函数参数。
可选的，所述磁控悬浮系统在设定位置处设定s个传感器，所述基于磁场数据Bw1、环境磁场数据BE1和磁场BE2计算胶囊机器人内部永磁体所产生的磁场 ，包括：计算胶囊机器人内部永磁体在第s个传感器处的三轴方向的磁场向量；
在第s个传感器处的三轴方向的磁场向量表示为：
其中， 为磁场数据Bw1第s个传感器处的磁场数据， 为环境磁场数据BE1第s个传感器处的磁场数据， 为磁场BE2第s个传感器处的磁场数据。
在一个具体的实施场景中，
如图1所示，本实施例公开磁控悬浮系统，胶囊机器人内部安装有形状规则的永磁体，永磁体磁化方向与胶囊机器人轴向方向一致或垂直，其形状可为圆柱形或圆环形；胶囊机器人工作在磁控悬浮系统中，磁控悬浮系统由悬浮电磁线圈及偏转电磁线圈组成，用于精确控制胶囊机器人的稳定悬浮、定向运动和两个自由度的偏转。定位系统包括定位板、传感器阵列、多路复用芯片/片选芯片、嵌入式微控制器、数据采集卡、电脑端，如图2所示。定位板安装传感器阵列和标定定位系统的坐标系，传感器阵列用于实时采集工作空间的磁场信号，经多路复用芯片/片选芯片发送至嵌入式微控制器，多路复用芯片/片选芯片根据嵌入式微控制器指令对传感器进行有序选择并建立通信通道，嵌入式微控制器通过多路复用芯片/片选芯片控制传感器信号的采集与传输，将采集到的信号传输至电脑端，并接收电脑端的程序指令，数据采集卡实时采集各电磁线圈中的电流，并将采集数据传输给电脑端；电脑端用于接收所述嵌入式微控制器发送的数据，接收数据采集卡发送的数据，计算并显示定位结果，并发送指令给嵌入式微控制器和数据采集卡。
如图3所示，磁控悬浮胶囊机器人的实时定位方法，包括：
所述的方法包括如下步骤：
步骤1：利用传感器获取环境磁场数据BE1；
利用单轴或多轴磁场传感器阵列，磁传感器阵列满足： ，优选的，传感器为三轴磁场传感器，按照对称或阵列方式分布，安装在磁控悬浮系统底部，也可随悬浮电磁线圈同步移动，本实施例中，传感器为 阵列分布的三轴磁传感器。如图4所示，以传感器阵列所在平面为水平面，建立空间坐标系，传感器的位置坐标 。将第s个传感器测得的x,y,z三轴方向上的磁场数据表示为： ；
步骤2：根据磁偶极子模型，计算胶囊机器人内部永磁体在空间的理论磁场 ；
具体为计算胶囊机器人内部永磁体在第s个传感器处的理论磁场 ，具体计算表达式为：
其中， ，胶囊机器人位置 ，胶囊机器人姿态 ，胶囊机器人内部永磁体参数由试验测得 ，胶囊机器人到第s个传感器的距离 。
步骤3：将胶囊机器人置入工作空间，实时获取磁控悬浮系统中各线圈的电流，计算线圈在各个传感器处产生的磁场BE2；
将胶囊机器人置入工作空间，通过数据采集卡实时获取磁控悬浮系统中各线圈的电流，计算线圈在第s个传感器处产生的三个方向上的磁场向量 ，具体计算公式为：
其中，N表示线圈匝数，I表示线圈电流，r表示电流元到第s个的距离向量。
步骤4：利用传感器实时获取工作空间内的磁场数据Bw1；
在第s个传感器处的三轴方向的磁场向量可表示为：
。
步骤5：计算胶囊机器人内部永磁体所产生的磁场 ；
在第s个传感器处的三轴方向的磁场向量可表示为：
。
步骤6：计算当前时刻胶囊机器人的位置和姿态参数；
步骤6计算当前时刻胶囊机器人的位置和姿态参数具体实现过程包括以下子步骤：
步骤6.1：构建目标函数，设定约束条件和初始值，利用全局最优化算法经过有限次迭代寻找最优解附近的解，表示为向量 ，将向量 作为后面计算的初值；
构建目标函数：
设定约束条件：
位置约束条件为工作空间边界，本实施例中设置为 ；姿态参数约束条件为 ，且满足 ；
设定初始值，本实施例中，全局优化算法优选的可采用模拟退火粒子群算法，位置和姿态参数的初始值采用约束条件下的随机数，粒子群的初始化速度也采用随机数；
步骤6.2：利用非线性方程组优化算法，设置最大迭代次数和迭代精度，以 作为初值，经过多次迭代，计算得出当前时刻胶囊机器人的位置参数和姿态参数，表示为向量，将向量 作为下一步计算的初值， ；
步骤7：重复步骤4、步骤5和步骤6.2，将上一时刻的计算结果 作为步骤6.2的迭代初值，计算下一时刻胶囊机器人的位置和姿态参数，如此循环，便可实现胶囊机器人的实时定位。
在一个具体的场景中，传感器个数为16，各传感器在定位板上安装，坐标 分别为 ， ， ， ，， ， ， ，， ， ， ，，，， 。
胶囊机器人的几何中心与内部永磁体的几何中心为同一点，内部永磁体沿轴向方向磁化，垂直于XOY平面放置，理论位置参数为 ，理论姿态参数为。
环境磁场BE1在各个传感器处的值分别为：
，， ，， ， ， ， ， ， ，， ， ， ，， 。
胶囊机器人内部永磁体的理论磁场 在各个传感器处的值分别为： ， ， ， ， ， ， ， ， ， ， ,， ， ， ， 。
磁控悬浮系统中各线圈的电流产生的磁场 在各个传感器处的值分别为： ， ， ， ， ， ， ， ， ， ， ， ， ， ， ， 。
传感器阵列实时获取的工作空间内的磁场数据，经过步骤5和步骤6的计算，得到胶囊机器人的位置和姿态参数 ，理论值，若以均方根误差为评判标准，则位置误差为，姿态误差为，转化为角度为： 。
本公开实施例的计算机可读存储介质，其上存储有非暂时性计算机可读指令。当该非暂时性计算机可读指令由处理器运行时，执行前述的本公开各实施例的磁控悬浮胶囊机器人的实时定位方法的全部或部分步骤。
上述计算机可读存储介质包括但不限于：光存储介质（例如：CD－ROM和DVD）、磁光存储介质（例如：MO）、磁存储介质（例如：磁带或移动硬盘）、具有内置的可重写非易失性存储器的媒体（例如：存储卡）和具有内置ROM的媒体（例如：ROM盒）。
有关本实施例的详细说明可以参考前述各实施例中的相应说明，在此不再赘述。
以上结合具体实施例描述了本公开的基本原理，但是，需要指出的是，在本公开中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本公开的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本公开为必须采用上述具体的细节来实现。
在本公开中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序，本公开中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。
另外，如在此使用的，在以“至少一个”开始的项的列举中使用的“或”指示分离的列举，以便例如“A、B或C的至少一个”的列举意味着A或B或C，或AB或AC或BC，或ABC（即A和B和C）。此外，措辞“示例的”不意味着描述的例子是优选的或者比其他例子更好。
还需要指出的是，在本公开的系统和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本公开的等效方案。
可以不脱离由所附权利要求定义的教导的技术而进行对在此所述的技术的各种改变、替换和更改。此外，本公开的权利要求的范围不限于以上所述的处理、机器、制造、事件的组成、手段、方法和动作的具体方面。可以利用与在此所述的相应方面进行基本相同的功能或者实现基本相同的结果的当前存在的或者稍后要开发的处理、机器、制造、事件的组成、手段、方法或动作。因而，所附权利要求包括在其范围内的这样的处理、机器、制造、事件的组成、手段、方法或动作。
提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本公开。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本公开的范围。因此，本公开不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本公开的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。
Claims (9)
1.一种磁控悬浮胶囊机器人的实时定位方法，其特征在于，胶囊机器人内部安装有永磁体，所述永磁体磁化方向与胶囊机器人轴向方向一致或垂直，所述胶囊机器人工作在工作空间，所述工作空间为磁控悬浮系统，所述磁控悬浮系统包括悬浮电磁线圈及偏转电磁线圈，所述方法包括：
获取环境磁场数据BE1；
根据磁偶极子模型，计算胶囊机器人内部永磁体在空间的理论磁场Bm；
将胶囊机器人置入工作空间，实时获取悬浮电磁线圈及偏转电磁线圈的电流，计算悬浮电磁线圈及偏转电磁线圈在各个设定位置处产生的磁场BE2；
实时获取工作空间内的磁场数据Bw1；
基于磁场数据Bw1、环境磁场数据BE1和磁场BE2计算胶囊机器人内部永磁体所产生的磁场
基于理论磁场Bm和磁场
计算胶囊机器人的实时位置和姿态参数；
所述基于理论磁场Bm和磁场
计算胶囊机器人的实时位置和姿态参数，包括：
基于理论磁场Bm和磁场
构建目标函数，
磁控悬浮系统在设定位置处设定s个传感器，所述基于理论磁场Bm和磁场
构建目标函数构建的目标函数为：
其中，
和
分别为磁场
第s个传感器处的磁场数据的x、y和z轴的数据分量；Bm，sx、Bm，sy和Bm，sz分别为理论磁场Bm第s个传感器处的磁场数据的x、y和z轴的数据分量，N1为常数。
2.根据权利要求1所述的磁控悬浮胶囊机器人的实时定位方法，其特征在于，所述基于理论磁场Bm和磁场
计算胶囊机器人的实时位置和姿态参数，包括：
设定约束条件和初始值，利用全局最优化算法经过有限次迭代寻找最优解附近的解，将解表示为向量X0；
利用非线性方程组优化算法，设置最大迭代次数和迭代精度，以向量X0作为初值，经过多次迭代，得到胶囊机器人的实时位置参数和姿态参数。
3.根据权利要求2所述的磁控悬浮胶囊机器人的实时定位方法，其特征在于，所述设定约束条件和初始值，包括：
位置约束条件和姿态参数约束条件，所述位置约束条件为x，y，z∈[0，180]，姿态参数约束条件为a，b，c∈[-1，1]，且满足a2+b2+c2＝1。
4.根据权利要求1所述的磁控悬浮胶囊机器人的实时定位方法，其特征在于，所述磁控悬浮系统在设定位置处设定s个传感器；
所述根据磁偶极子模型，计算胶囊机器人内部永磁体在空间的理论磁场Bm，包括：
根据磁偶极子模型，计算胶囊机器人内部永磁体在第s个传感器处的理论磁场Bm，s，具体计算表达式为：
其中，μ0为真空磁导率，μ为介质磁导率，P为胶囊机器人位置，P＝(x，y，z)，H0为胶囊机器人姿态，M为胶囊机器人内部永磁体参数，rs为胶囊机器人到第s个传感器的距离，
传感器的位置坐标Ps＝(xs，ys，zs)。
5.根据权利要求4所述的磁控悬浮胶囊机器人的实时定位方法，其特征在于，
μ0＝4π×10-7N/A2，μ≈1，M＝1.13×107A/m。
6.根据权利要求1所述的磁控悬浮胶囊机器人的实时定位方法，其特征在于，所述磁控悬浮系统在设定位置处设定s个传感器，所述计算悬浮电磁线圈及偏转电磁线圈在各个设定位置处产生的磁场BE2，包括：
计算悬浮电磁线圈及偏转电磁线圈在第s个传感器处产生的三个方向上的磁场向量BE2，s＝(BE2，sx，BE2，sy，BE2，sz)，具体计算公式为：
其中，μ0为真空磁导率，N为线圈匝数，I为线圈电流，r为电流元到第s个的距离向量。
7.根据权利要求1所述的磁控悬浮胶囊机器人的实时定位方法，其特征在于，所述磁控悬浮系统在设定位置处设定s个传感器，所述基于磁场数据Bw1、环境磁场数据BE1和磁场BE2计算胶囊机器人内部永磁体所产生的磁场
包括：计算胶囊机器人内部永磁体在第s个传感器处的三轴方向的磁场向量；
在第s个传感器处的三轴方向的磁场向量表示为：
其中，Bw1，s为磁场数据Bw1第s个传感器处的磁场数据，BE1，s为环境磁场数据BE1第s个传感器处的磁场数据，BB2，s为磁场BE2第s个传感器处的磁场数据。
8.一种磁控悬浮胶囊机器人的定位系统，使用权利要求1-7任一所述的磁控悬浮胶囊机器人的实时定位方法，其特征在于，包括定位板、传感器阵列、传输芯片、嵌入式微控制器、数据采集卡和电脑端，所述传输芯片包括多路复用芯片或片选芯片；
所述定位板用于安装传感器阵列和标定定位系统的坐标系；
所述传感器阵列用于实时采集工作空间的磁场信号，经所述传输芯片发送至嵌入式微控制器，所述传输芯片根据嵌入式微控制器指令对传感器进行有序选择并建立通信通道；
所述嵌入式微控制器通过所述传输芯片控制传感器信号的采集与传输，所述嵌入式微控制器将采集到的信号传输至电脑端，所述嵌入式微控制器接收电脑端的程序指令；
所述数据采集卡实时采集磁控悬浮系统的电磁线圈中的电流，并将采集数据传输给所述电脑端；
所述电脑端用于接收所述嵌入式微控制器发送的数据，接收所述数据采集卡发送的数据，计算并显示定位结果，并发送指令给嵌入式微控制器和数据采集卡。
9.一种计算机可读存储介质，其特征在于，该计算机可读存储介质存储计算机指令，该计算机指令用于使计算机执行权利要求1-7任一所述的磁控悬浮胶囊机器人的实时定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310198790.8A CN115886692B (zh) | 2023-03-03 | 2023-03-03 | 磁控悬浮胶囊机器人的实时定位方法、系统及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310198790.8A CN115886692B (zh) | 2023-03-03 | 2023-03-03 | 磁控悬浮胶囊机器人的实时定位方法、系统及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115886692A CN115886692A (zh) | 2023-04-04 |
| CN115886692B true CN115886692B (zh) | 2023-05-12 |
Family
ID=85739202
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310198790.8A Active CN115886692B (zh) | 2023-03-03 | 2023-03-03 | 磁控悬浮胶囊机器人的实时定位方法、系统及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115886692B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116735151A (zh) * | 2023-08-10 | 2023-09-12 | 西南交通大学 | 基于磁定位的颗粒运动轨迹监测系统及其定位方法 |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114469057A (zh) * | 2022-04-01 | 2022-05-13 | 安翰科技(武汉)股份有限公司 | 无线胶囊定位装置、磁场传感器定位方法和装置 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11122965B2 (en) * | 2017-10-09 | 2021-09-21 | Vanderbilt University | Robotic capsule system with magnetic actuation and localization |
| CN107773205A (zh) * | 2017-10-31 | 2018-03-09 | 嘉兴复尔机器人有限公司 | 一种胶囊式内窥镜磁控系统 |
| CN112842320B (zh) * | 2019-11-28 | 2023-03-24 | 安翰科技(武汉)股份有限公司 | 可吞服设备定位系统及其方法 |
| CN111839431B (zh) * | 2020-07-24 | 2024-03-01 | 福建世新机器人科技有限公司 | 一种无线胶囊机器人系统及控制方法 |
| CN114947692B (zh) * | 2022-04-01 | 2024-06-14 | 桂林电子科技大学 | 一种基于磁传感器阵列的永磁体定位方法 |
| CN115067863B (zh) * | 2022-05-31 | 2023-03-14 | 元化智能科技(深圳)有限公司 | 基于球型驱动器的无线胶囊内窥镜驱动系统 |
-
2023
- 2023-03-03 CN CN202310198790.8A patent/CN115886692B/zh active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114469057A (zh) * | 2022-04-01 | 2022-05-13 | 安翰科技(武汉)股份有限公司 | 无线胶囊定位装置、磁场传感器定位方法和装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115886692A (zh) | 2023-04-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Xu et al. | A novel system for closed-loop simultaneous magnetic actuation and localization of WCE based on external sensors and rotating actuation | |
| Munoz et al. | Analysis of magnetic interaction in remotely controlled magnetic devices and its application to a capsule robot for drug delivery | |
| CN115886692B (zh) | 磁控悬浮胶囊机器人的实时定位方法、系统及存储介质 | |
| CN112493970B (zh) | 无线胶囊内窥镜的追踪定位方法及系统 | |
| CN111839431B (zh) | 一种无线胶囊机器人系统及控制方法 | |
| CN211633195U (zh) | 胶囊内窥镜的磁定位系统 | |
| CN112515611B (zh) | 无线胶囊内窥镜的定位方法、装置和终端设备 | |
| CN112515610B (zh) | 无线胶囊内窥镜的驱动方法、装置及系统 | |
| Shi et al. | An optically aided magnetic tracking approach for magnetically actuated capsule robot | |
| Wang et al. | A novel magnetic tracking approach for intrabody objects | |
| Véron et al. | Geometric analysis of the singularities of a magnetic manipulation system with several mobile coils | |
| Liu et al. | Design of a unified active locomotion mechanism for a capsule-shaped laparoscopic camera system | |
| CN114947692B (zh) | 一种基于磁传感器阵列的永磁体定位方法 | |
| Hu et al. | Efficient linear algorithm for magnetic localization and orientation in capsule endoscopy | |
| Yuan et al. | Design of a novel electromagnetic actuation system for actuating magnetic capsule robot | |
| Li et al. | Orientation control of an electromagnetically actuated soft-tethered colonoscope based on 2OR pseudo-rigid-body model | |
| Sun et al. | A novel control method of magnetic navigation capsule endoscope for gastrointestinal examination | |
| Hu et al. | A new 6D magnetic localization technique for wireless capsule endoscope based on a rectangle magnet | |
| CN202489952U (zh) | 胶囊内窥镜的驱动装置 | |
| Salerno et al. | Force calculation for localized magnetic driven capsule endoscopes | |
| Jeong et al. | Remote controlled micro-robots using electromagnetic actuation (EMA) systems | |
| Guo et al. | Design of a novel drug-delivery module for active locomotive intestinal capsule endoscopy | |
| CN108460820B (zh) | 基于图像反馈的微型移动设备控制装置和方法 | |
| CN113647893B (zh) | 无线胶囊内窥镜的自主导航系统、设备及介质 | |
| WO2022193411A1 (zh) | 胶囊内窥镜系统及其胶囊内窥镜磁定位方法 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |