CN111839431B - 一种无线胶囊机器人系统及控制方法 - Google Patents
一种无线胶囊机器人系统及控制方法 Download PDFInfo
- Publication number
- CN111839431B CN111839431B CN202010722229.1A CN202010722229A CN111839431B CN 111839431 B CN111839431 B CN 111839431B CN 202010722229 A CN202010722229 A CN 202010722229A CN 111839431 B CN111839431 B CN 111839431B
- Authority
- CN
- China
- Prior art keywords
- communication module
- alternating
- electromagnetic
- capsule robot
- wireless
- 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 105
- 238000000034 method Methods 0.000 title claims abstract description 17
- 238000004891 communication Methods 0.000 claims abstract description 92
- 238000001514 detection method Methods 0.000 claims abstract description 9
- 230000005674 electromagnetic induction Effects 0.000 claims description 37
- 239000013598 vector Substances 0.000 claims description 12
- 230000005540 biological transmission Effects 0.000 claims description 10
- QVFWZNCVPCJQOP-UHFFFAOYSA-N chloralodol Chemical compound CC(O)(C)CC(C)OC(O)C(Cl)(Cl)Cl QVFWZNCVPCJQOP-UHFFFAOYSA-N 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 5
- 230000004907 flux Effects 0.000 claims description 3
- 239000002245 particle Substances 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 4
- 239000003814 drug Substances 0.000 description 4
- 238000001574 biopsy Methods 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000001839 endoscopy Methods 0.000 description 2
- 210000001035 gastrointestinal tract Anatomy 0.000 description 2
- 210000000936 intestine Anatomy 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000005507 spraying Methods 0.000 description 2
- 210000002784 stomach Anatomy 0.000 description 2
- RTAQQCXQSZGOHL-UHFFFAOYSA-N Titanium Chemical compound [Ti] RTAQQCXQSZGOHL-UHFFFAOYSA-N 0.000 description 1
- 239000003990 capacitor Substances 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000005284 excitation Effects 0.000 description 1
- 230000002496 gastric effect Effects 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000000338 in vitro Methods 0.000 description 1
- 230000006698 induction Effects 0.000 description 1
- 229910052751 metal Inorganic materials 0.000 description 1
- 239000002184 metal Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001575 pathological effect Effects 0.000 description 1
- 230000007170 pathology Effects 0.000 description 1
- 230000008855 peristalsis Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 210000000813 small intestine Anatomy 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 239000010936 titanium Substances 0.000 description 1
- 229910052719 titanium Inorganic materials 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 239000012780 transparent material Substances 0.000 description 1
Classifications
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
- A61B1/04—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor combined with photographic or television appliances
- A61B1/041—Capsule endoscopes for imaging
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
- A61B1/00002—Operational features of endoscopes
- A61B1/00011—Operational features of endoscopes characterised by signal transmission
- A61B1/00016—Operational features of endoscopes characterised by signal transmission using wireless means
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
- A61B1/00147—Holding or positioning arrangements
- A61B1/00158—Holding or positioning arrangements using magnetic field
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
- A61B1/04—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor combined with photographic or television appliances
- A61B1/045—Control thereof
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B2562/00—Details of sensors; Constructional details of sensor housings or probes; Accessories for sensors
- A61B2562/02—Details of sensors specially adapted for in-vivo measurements
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B2562/00—Details of sensors; Constructional details of sensor housings or probes; Accessories for sensors
- A61B2562/02—Details of sensors specially adapted for in-vivo measurements
- A61B2562/0271—Thermal or temperature sensors
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B2562/00—Details of sensors; Constructional details of sensor housings or probes; Accessories for sensors
- A61B2562/16—Details of sensor housings or probes; Details of structural supports for sensors
- A61B2562/162—Capsule shaped sensor housings, e.g. for swallowing or implantation
Abstract
本发明提供了医疗检测设备技术领域的一种无线胶囊机器人系统及控制方法，系统包括：胶囊机器人；永磁环，设于所述胶囊机器人的外侧；计算机；驱动装置，与所述计算机连接，并通过所述永磁环驱动胶囊机器人运动；第一无线通信模块，一端与所述计算机连接，另一端与所述胶囊机器人连接；无线交变电磁跟踪装置，与所述计算机连接，并与所述胶囊机器人电磁耦合。本发明的优点在于：极大的提升了胶囊机器人位置跟踪和位姿控制的精度以及系统稳定性。
Description
技术领域
本发明涉及医疗检测设备技术领域，特别指一种无线胶囊机器人系统及控制方法。
背景技术
无线胶囊机器人具有内窥镜检查、病理取样和施药等功能，可以完成无痛无创的消化道检查和手术，是对传统的有线胃镜和外科手术方式的革命性突破，具有更高的效率和安全性。
2001年5月以色列的Given Imaging公司推出名为“M2A”的第一款胶囊状内窥镜，可连续拍摄小肠内壁的图像6～8个小时以上；其驱动方式是采用基于消化道自然蠕动的被动运动方式，M2A在体内的定位通过面向无线通讯的体外射频天线阵列实现，但是存在精度较低的缺点。当胶囊机器人滞留在体内时，需要通过X射线拍摄其在体内实际位置并通过手术取出。
为了实现无线胶囊机器人在体内的实时跟踪定位，瑞士V.Schlageter、香港中文大学孟庆虎/胡超教授团队、重庆大学皮喜田教授团队、上海交通大学颜国正团队等通过磁传感器阵列对内嵌永磁铁的无线胶囊机器人实现位姿跟踪，但是其精度受到地磁场等环境磁场影响且没有与之有效协作的驱动方式相结合。一些科研机构及企业开始研发主动驱动或者外部驱动的胶囊机器人，比如RF System Lab公司公布了一种不使用电池的胶囊内窥镜样机原型“Norika3”。病人身穿嵌有三组发射线圈和接收线圈的背心，线圈发送射频。胶囊内同样有三组60度间隔的线圈，在经过磁线圈接收并经电容器转换后感应出电流，形成一个三极电机的模式，从而控制胶囊的旋转，以便不同方向观察病灶，但该方法存在系统复杂且稳定性不够的缺点。安翰科技(武汉)股份有限公司推出“磁控胶囊胃镜系统”系列产品，通过外部永磁体系统驱动内嵌永磁体磁环的磁控胶囊；由于外部驱动磁场远大于磁控胶囊内部磁环的磁场，基于永磁的胶囊机器人跟踪方式无法与磁驱动同时协同工作。
综上所述，传统的胶囊机器人存在位置跟踪和位姿控制精度低、稳定性差的缺点，而位置跟踪是位姿控制的基础，只有结合位置跟踪和位姿控制才能实现基于闭环方式的胶囊机器人的智能控制。因此，如何提供一种无线胶囊机器人系统及控制方法，实现提升胶囊机器人位置跟踪和位姿控制的精度以及系统稳定性，成为一个亟待解决的问题。
发明内容
本发明要解决的技术问题，在于提供一种无线胶囊机器人系统及控制方法，实现提升胶囊机器人位置跟踪和位姿控制的精度以及系统稳定性。
第一方面，本发明提供了一种无线胶囊机器人系统，包括：
胶囊机器人；
永磁环，设于所述胶囊机器人的外侧；
计算机；
驱动装置，与所述计算机连接，并通过所述永磁环驱动胶囊机器人运动；
第一无线通信模块，一端与所述计算机连接，另一端与所述胶囊机器人连接；
无线交变电磁跟踪装置，与所述计算机连接，并与所述胶囊机器人电磁耦合。
进一步地，所述胶囊机器人包括：
壳体，外侧环设所述永磁环；
MCU，设于所述壳体内；
电源模块，与所述MCU连接，并设于所述壳体内；
摄像头，与所述MCU连接，并设于所述壳体内；
温度传感器，与所述MCU连接，并设于所述壳体内；
第二无线通信模块，一端与所述MCU连接，另一端与所述第一无线通信模块连接，并设于所述壳体内；
电磁感应线圈，与所述MCU连接，与所述无线交变电磁跟踪装置电磁耦合，并设于所述壳体内；
PH检测仪，与所述MCU连接，并设于所述壳体的表面。
进一步地，所述驱动装置包括：
机械臂，与所述计算机连接；
永磁体，设于所述机械臂的末端。
进一步地，所述第一无线通信模块为2G通信模块、3G通信模块、4G通信模块、5G通信模块、NB-IOT通信模块、LORA通信模块、WIFI通信模块、蓝牙通信模块或者ZigBee通信模块。
进一步地，所述第二无线通信模块为2G通信模块、3G通信模块、4G通信模块、5G通信模块、NB-IOT通信模块、LORA通信模块、WIFI通信模块、蓝牙通信模块或者ZigBee通信模块。
进一步地，所述无线交变电磁跟踪装置包括：
多路交变电磁发射阵列，与所述胶囊机器人电磁耦合；
交变电路，一端与所述多路交变电磁发射阵列连接，另一端与所述计算机连接。
进一步地，所述多路交变电磁发射阵列包括：
若干个交变电磁发射线圈，一端与所述交变电路连接，另一端与所述胶囊机器人电磁耦合。
第二方面，本发明提供了一种无线胶囊机器人系统的控制方法，包括如下步骤：
步骤S10、计算机通过机械臂驱动永磁体运动，永磁体通过磁力驱动永磁环运动，进而联动胶囊机器人运动；
步骤S20、计算机依次通过第一无线通信模块和第二无线通信模块向MCU发送检测指令；
步骤S30、MCU基于接收的所述检测指令控制摄像头拍摄人体内部的照片、控制温度传感器采集人体内部的温度、控制PH检测仪采集人体内部的PH值，并将所述照片、温度以及PH值依次通过第二无线通信模块和第一无线通信模块发送给计算机；
步骤S40、计算机通过无线交变电磁跟踪装置以及电磁感应线圈对胶囊机器人的位置和姿态进行实时跟踪。
进一步地，所述步骤S40具体包括：
步骤S41、计算机通过交变电路控制各交变电磁发射线圈产生大小为的交变磁场；交变磁场/>在空间点(x1,y1,z1)的三个坐标方向的分量为：
其中(m,n,p)T表示交变电磁发射线圈的归一化方向向量，且m2+n2+p2＝1；(a,b,c)T表示交变电磁发射线圈的中心位置点；BT表示磁场常量；L表示交变电磁发射线圈的中心位置点到空间点(x1,y1,z1)的距离，且Bx1表示交变磁场/>在空间点(x1,y1,z1)的x轴方向的分量；By1表示交变磁场/>在空间点(x1,y1,z1)的y轴方向的分量；Bz1表示交变磁场/>在空间点(x1,y1,z1)的z轴方向的分量；/>
步骤S42、设电磁感应线圈的中心位置点为(x2,y2,z2)，(vx,vy,vz)T为电磁感应线圈归一化方向向量，且
当交变电磁发射线圈与电磁感应线圈的归一化方向向量不平行时，计算交变电磁发射线圈产生的磁场强度在电磁感应线圈方向的矢量投影/>的三个坐标方向的分量：
基于法拉第电磁感应原理，电磁感应线圈的感生电动势为：
其中N表示电磁感应线圈的匝数；表示电磁感应线圈的表面积；/>表示穿过电磁感应线圈的磁通量；
当交变电磁发射线圈的发射信号为频率为ω的正弦波，则电磁感应线圈感应的电动势为：
其中表示交变电磁发射线圈发射信号的最大幅值；i为正整数；m表示交变电磁发射线圈的总数量，为正整数，且m≥6；ε′imax表示第i个交变电磁发射线圈的感应电磁力；εimax表示第i个交变电磁发射线圈的理论电磁力；
基于电磁感应线圈感应的电动势得到误差方程：
步骤S43、利用粒子群优化算法求解所述误差方程的初值，再利用LM算法对所述初值迭代，得到位姿参数(x,y,z,vx,vy,vz)，基于所述位姿参数对胶囊机器人的位置和姿态进行实时跟踪。
本发明的优点在于：
通过在胶囊机器人的外侧设置永磁环，计算机通过控制机械臂抓取永磁体，通过磁力驱动永磁环运动，进而联动胶囊机器人进行位姿调整，极大的提升了胶囊机器人位姿控制的精度；通过在胶囊机器人内设置电磁感应线圈，通过多路交变电磁发射阵列向胶囊机器人发射交变磁场的信号，进而通过计算对应的电动势对胶囊机器人的位置进行定位，极大的提升了胶囊机器人位置跟踪的精度；而永磁体的位姿变化产生的旋转磁场频率与多路交变电磁发射阵列产生的信号频率互不干扰，极大的提升了系统的稳定性，最终实现基于闭环方式的胶囊机器人的智能控制。
附图说明
下面参照附图结合实施例对本发明作进一步的说明。
图1是本发明一种无线胶囊机器人系统的电路原理框图。
图2是本发明胶囊机器人的电路原理框图。
图3是本发明一种无线胶囊机器人系统的结构示意图。
图4是本发明一种无线胶囊机器人系统的控制方法的流程图。
图5是本发明交变电磁发射线圈和电磁感应线圈的耦合关系示意图。
图6是本发明胶囊机器人运动控制示意图。
标记说明：
100-一种无线胶囊机器人系统，1-胶囊机器人，2-永磁环，3-计算机，4-驱动装置，5-第一无线通信模块，6-无线交变电磁跟踪装置，11-壳体，12-MCU，13-电源模块，14-摄像头，15-温度传感器，16-第二无线通信模块，17-电磁感应线圈，18-PH检测仪，41-机械臂，42-永磁体，61-多路交变电磁发射阵列，62-交变电路，611-交变电磁发射线圈。
具体实施方式
请参照图1至图5所示，本发明一种无线胶囊机器人系统100的较佳实施例，包括：
胶囊机器人1，用于进入人体消化道任意位置，进而对人体进行检查与治疗，具备内窥镜检查、取样、施药、测量消化道温度和PH值测量等功能；
永磁环2，设于所述胶囊机器人1的外侧，用于联动所述胶囊机器人1；
计算机3，用于控制所述驱动装置4以及无线交变电磁跟踪装置6的工作，通过第一无线通信模块5与胶囊机器人1进行通信，下达控制指令；
驱动装置4，与所述计算机3连接，并通过所述永磁环2驱动胶囊机器人1运动，用于产生方向和强度可调的磁场以使所述胶囊机器人1进行运动，并拥有仰视、俯瞰、旋转等6个角度的视角；
第一无线通信模块5，一端与所述计算机3连接，另一端与所述胶囊机器人1连接，用于与所述胶囊机器人1通信；
无线交变电磁跟踪装置6，与所述计算机3连接，并与所述胶囊机器人1电磁耦合，用于在外部施加具有时序同步信号的多通道交变电磁信号，进而对内置的所述电磁感应线圈17的胶囊机器人1实时位姿进行跟踪。
所述胶囊机器人1包括：
壳体11，外侧环设所述永磁环2；所述壳体11为透明材质；
MCU12，设于所述壳体11内，用于控制所述摄像头14、温度传感器15、第二无线通信模块16以及PH检测仪18的工作，将所述电磁感应线圈17的感应信号进行放大、滤波、采样后计算得到实时位姿信息，并将位姿信息以及测量数据发送给计算机3；
电源模块13，与所述MCU12连接，并设于所述壳体11内，用于给所述胶囊机器人1供电；
摄像头14，与所述MCU12连接，并设于所述壳体11内，用于拍摄肠胃通道的图片；
温度传感器15，与所述MCU12连接，并设于所述壳体11内，用于采集人体肠胃内的温度；
第二无线通信模块16，一端与所述MCU12连接，另一端与所述第一无线通信模块5连接，并设于所述壳体11内，用于与所述计算机3通信；
电磁感应线圈17，与所述MCU12连接，与所述无线交变电磁跟踪装置6电磁耦合，并设于所述壳体11内；所述电磁感应线圈17采用成一定角度的1个以上单轴线圈或其组合；基于单轴线圈的定位可以得到其5维位姿信息，而基于两个单轴线圈的组合可以得到完整6维信息；
PH检测仪18，与所述MCU12连接，并设于所述壳体11的表面，用于采集人体肠胃内的PH值。
所述胶囊机器人1还设有一用于照明的LED(未图示)、一用于喷射药物的喷药仓(未图示)以及一用于提取病理检测样本的取活检仓(未图示)；所述取活检仓能够伸出微型钛金属针取活检。
所述驱动装置4包括：
机械臂41，与所述计算机3连接，用于夹持所述永磁体42产生旋转磁场或梯度磁场或旋转和梯度磁场组合以实现对套永磁环2的胶囊机器人1在消化道内的姿态进行控制和调整；
永磁体42，设于所述机械臂41的末端，用于通过磁力驱动所述永磁环2。
所述第一无线通信模块5为2G通信模块、3G通信模块、4G通信模块、5G通信模块、NB-IOT通信模块、LORA通信模块、WIFI通信模块、蓝牙通信模块或者ZigBee通信模块。
所述第二无线通信模块16为2G通信模块、3G通信模块、4G通信模块、5G通信模块、NB-IOT通信模块、LORA通信模块、WIFI通信模块、蓝牙通信模块或者ZigBee通信模块。
所述无线交变电磁跟踪装置6包括：
多路交变电磁发射阵列61，与所述胶囊机器人1电磁耦合；所述多路交变电磁发射阵列61加载交变电磁信号后能够被电磁感应线圈17有效耦合；
交变电路62，一端与所述多路交变电磁发射阵列61连接，另一端与所述计算机3连接；所述交变电路62用于产生至少6通道的交变电磁信号。
所述多路交变电磁发射阵列61包括：
若干个交变电磁发射线圈611，一端与所述交变电路62连接，另一端与所述胶囊机器人1电磁耦合；所述交变电磁发射线圈611采用单轴阵列或者正交线圈组合的方式排列。
本发明一种无线胶囊机器人系统的控制方法的较佳实施例，包括如下步骤：
步骤S10、计算机通过机械臂驱动永磁体运动，永磁体通过磁力驱动永磁环运动，进而联动胶囊机器人运动；
步骤S20、计算机依次通过第一无线通信模块和第二无线通信模块向MCU发送检测指令；
步骤S30、MCU基于接收的所述检测指令控制摄像头拍摄人体内部的照片、控制温度传感器采集人体内部的温度、控制PH检测仪采集人体内部的PH值，并将所述照片、温度以及PH值依次通过第二无线通信模块和第一无线通信模块发送给计算机；
步骤S40、计算机通过无线交变电磁跟踪装置以及电磁感应线圈对胶囊机器人的位置和姿态进行实时跟踪。
所述步骤S40具体包括：
步骤S41、通以交变电磁信号的交变电磁发射线圈的空间磁场分布可以等效为一个磁偶极子，即每一个交变电磁发射线圈可等效为一个磁偶极子；计算机通过交变电路控制各交变电磁发射线圈产生大小为的交变磁场；按照Biot-Savart法则，交变磁场/>在空间点(x1,y1,z1)的三个坐标方向的分量为：
其中(m,n,p)T表示交变电磁发射线圈的归一化方向向量，且m2+n2+p2＝1；(a,b,c)T表示交变电磁发射线圈的中心位置点；BT表示磁场常量，受线圈及加载电流特性影响；L表示交变电磁发射线圈的中心位置点到空间点(x1,y1,z1)的距离，且Bx1表示交变磁场/>在空间点(x1,y1,z1)的x轴方向的分量；By1表示交变磁场/>在空间点(x1,y1,z1)的y轴方向的分量；Bz1表示交变磁场/>在空间点(x1,y1,z1)的z轴方向的分量；/>多路交变电磁发射阵列在空间的磁场分布表现为具有不同信号源的多个磁偶极子的叠加；
由于多路交变电磁发射阵列的各交变电磁发射线圈采用分频激励的方式，为了实现收发的时序同步，在交变电磁发射线圈发射信号的同时固定输出一定时间间隔的零点信号，实现电磁感应线圈的相位同步。
电磁感应线圈端感应的交变电磁信号表现为不同频率的多通道发射交变信号的叠加，通过傅里叶变换实现不同频率分量的幅度分离，分离得到的不同通道幅度信号用于最优化算法对位姿求解。
步骤S42、设电磁感应线圈的中心位置点为(x2,y2,z2)，(vx,vy,vz)T为电磁感应线圈归一化方向向量，且
当交变电磁发射线圈与电磁感应线圈的归一化方向向量不平行时，计算交变电磁发射线圈产生的磁场强度在电磁感应线圈方向的矢量投影/>的三个坐标方向的分量：
基于法拉第电磁感应原理，电磁感应线圈的感生电动势为：
其中N表示电磁感应线圈的匝数；表示电磁感应线圈的表面积；/>表示穿过电磁感应线圈的磁通量；
当交变电磁发射线圈的发射信号为频率为ω的正弦波，则电磁感应线圈感应的电动势为：
其中表示交变电磁发射线圈发射信号的最大幅值；i为正整数；m表示交变电磁发射线圈的总数量，为正整数，且m≥6；ε′imax表示第i个交变电磁发射线圈的感应电磁力；εimax表示第i个交变电磁发射线圈的理论电磁力；
基于电磁感应线圈感应的电动势得到误差方程：
步骤S43、利用粒子群优化算法(PSO算法)求解所述误差方程的初值，再利用LM算法对所述初值迭代，得到位姿参数(x,y,z,vx,vy,vz)，基于所述位姿参数对胶囊机器人的位置和姿态进行实时跟踪。
由于所述误差方程是一个非线性最小二乘优化问题，LM算法(Levenberg-Marquardt)用以精确求解，但对初值要求较高，故采用找初值能力强(迭代速率快)但跟踪精度差的PSO算法，用以在跟踪定位输出作为LM算法的初值，接下来的跟踪过程中，上一次迭代过程的LM输出作为下一次迭代LM的初值。
综上所述，本发明的优点在于：
通过在胶囊机器人的外侧设置永磁环，计算机通过控制机械臂抓取永磁体，通过磁力驱动永磁环运动，进而联动胶囊机器人进行位姿调整，极大的提升了胶囊机器人位姿控制的精度；通过在胶囊机器人内设置电磁感应线圈，通过多路交变电磁发射阵列向胶囊机器人发射交变磁场的信号，进而通过计算对应的电动势对胶囊机器人的位置进行定位，极大的提升了胶囊机器人位置跟踪的精度；而永磁体的位姿变化产生的旋转磁场频率与多路交变电磁发射阵列产生的信号频率互不干扰，极大的提升了系统的稳定性，最终实现基于闭环方式的胶囊机器人的智能控制。
虽然以上描述了本发明的具体实施方式，但是熟悉本技术领域的技术人员应当理解，我们所描述的具体的实施例只是说明性的，而不是用于对本发明的范围的限定，熟悉本领域的技术人员在依照本发明的精神所作的等效的修饰以及变化，都应当涵盖在本发明的权利要求所保护的范围内。
Claims (5)
1.一种无线胶囊机器人系统，其特征在于：包括：
胶囊机器人；
永磁环，设于所述胶囊机器人的外侧；
计算机；
驱动装置，与所述计算机连接，并通过所述永磁环驱动胶囊机器人运动；
第一无线通信模块，一端与所述计算机连接，另一端与所述胶囊机器人连接；
无线交变电磁跟踪装置，所述无线交变电磁跟踪装置包括：
多路交变电磁发射阵列，与所述胶囊机器人电磁耦合；
交变电路，一端与所述多路交变电磁发射阵列连接，另一端与所述计算机连接；
所述多路交变电磁发射阵列包括：
若干个交变电磁发射线圈，一端与所述交变电路连接，另一端与所述胶囊机器人电磁耦合；
所述胶囊机器人包括：
壳体，外侧环设所述永磁环；
MCU，设于所述壳体内；
电源模块，与所述MCU连接，并设于所述壳体内；
摄像头，与所述MCU连接，并设于所述壳体内；
温度传感器，与所述MCU连接，并设于所述壳体内；
第二无线通信模块，一端与所述MCU连接，另一端与所述第一无线通信模块连接，并设于所述壳体内；
电磁感应线圈，与所述MCU连接，与所述无线交变电磁跟踪装置电磁耦合，并设于所述壳体内；
PH检测仪，与所述MCU连接，并设于所述壳体的表面；
所述驱动装置包括：
机械臂，与所述计算机连接；
永磁体，设于所述机械臂的末端；
所述计算机控制所述机械臂抓取所述永磁体，通过磁力驱动所述永磁环运动，进而联动所述胶囊机器人进行位姿调整，所述多路交变电磁发射阵列向所述胶囊机器人发射交变磁场的信号，进而通过计算对应的电动势对所述胶囊机器人的位置进行定位。
2.如权利要求1所述的一种无线胶囊机器人系统，其特征在于：所述第一无线通信模块为2G通信模块、3G通信模块、4G通信模块、5G通信模块、NB-IOT通信模块、LORA通信模块、WIFI通信模块、蓝牙通信模块或者ZigBee通信模块。
3.如权利要求1所述的一种无线胶囊机器人系统，其特征在于：所述第二无线通信模块为2G通信模块、3G通信模块、4G通信模块、5G通信模块、NB-IOT通信模块、LORA通信模块、WIFI通信模块、蓝牙通信模块或者ZigBee通信模块。
4.一种无线胶囊机器人系统的控制方法，其特征在于：所述方法需使用如权利要求1至3任一项所述的无线胶囊机器人系统，包括如下步骤：
步骤S10、计算机通过机械臂驱动永磁体运动，永磁体通过磁力驱动永磁环运动，进而联动胶囊机器人运动；
步骤S20、计算机依次通过第一无线通信模块和第二无线通信模块向MCU发送检测指令；
步骤S30、MCU基于接收的所述检测指令控制摄像头拍摄人体内部的照片、控制温度传感器采集人体内部的温度、控制PH检测仪采集人体内部的PH值，并将所述照片、温度以及PH值依次通过第二无线通信模块和第一无线通信模块发送给计算机；
步骤S40、计算机通过无线交变电磁跟踪装置以及电磁感应线圈对胶囊机器人的位置和姿态进行实时跟踪。
5.如权利要求4所述的一种无线胶囊机器人系统的控制方法，其特征在于：所述步骤S40具体包括：
步骤S41、计算机通过交变电路控制各交变电磁发射线圈产生大小为的交变磁场；交变磁场/>在空间点(x1,y1,z1)的三个坐标方向的分量为：
其中(m,n,p)T表示交变电磁发射线圈的归一化方向向量，且m2+n2+p2＝1；(a,b,c)T表示交变电磁发射线圈的中心位置点；BT表示磁场常量；L表示交变电磁发射线圈的中心位置点到空间点(x1,y1,z1)的距离，且Bx1表示交变磁场/>在空间点(x1,y1,z1)的x轴方向的分量；By1表示交变磁场/>在空间点(x1,y1,z1)的y轴方向的分量；Bz1表示交变磁场/>在空间点(x1,y1,z1)的z轴方向的分量；/>
步骤S42、设电磁感应线圈的中心位置点为(x2,y2,z2)，(vx,vy,vz)T为电磁感应线圈归一化方向向量，且
当交变电磁发射线圈与电磁感应线圈的归一化方向向量不平行时，计算交变电磁发射线圈产生的磁场强度在电磁感应线圈方向的矢量投影/>的三个坐标方向的分量：
基于法拉第电磁感应原理，电磁感应线圈的感生电动势为：
其中N表示电磁感应线圈的匝数；表示电磁感应线圈的表面积；/>表示穿过电磁感应线圈的磁通量；
当交变电磁发射线圈的发射信号为频率为ω的正弦波，则电磁感应线圈感应的电动势为：
其中表示交变电磁发射线圈发射信号的最大幅值；i为正整数；m表示交变电磁发射线圈的总数量，为正整数，且m≥6；εi'max表示第i个交变电磁发射线圈的感应电磁力；εimax表示第i个交变电磁发射线圈的理论电磁力；
基于电磁感应线圈感应的电动势得到误差方程：
步骤S43、利用粒子群优化算法求解所述误差方程的初值，再利用LM算法对所述初值迭代，得到位姿参数(x,y,z,vx,vy,vz)，基于所述位姿参数对胶囊机器人的位置和姿态进行实时跟踪。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010722229.1A CN111839431B (zh) | 2020-07-24 | 2020-07-24 | 一种无线胶囊机器人系统及控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010722229.1A CN111839431B (zh) | 2020-07-24 | 2020-07-24 | 一种无线胶囊机器人系统及控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111839431A CN111839431A (zh) | 2020-10-30 |
| CN111839431B true CN111839431B (zh) | 2024-03-01 |
Family
ID=72950016
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010722229.1A Active CN111839431B (zh) | 2020-07-24 | 2020-07-24 | 一种无线胶囊机器人系统及控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111839431B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113238291B (zh) * | 2021-06-21 | 2023-11-17 | 福建世新机器人科技有限公司 | 基于磁性部件定位跟踪的地磁矢量实时估计与分离方法 |
| CN114543645B (zh) * | 2022-02-21 | 2024-01-30 | 成都思瑞定生命科技有限公司 | 磁场目标定位系统和方法 |
| CN114577100B (zh) * | 2022-02-21 | 2023-11-21 | 成都思瑞定生命科技有限公司 | 磁场目标定位计算方法 |
| CN114732521B (zh) * | 2022-05-18 | 2023-09-05 | 成都思瑞定生命科技有限公司 | 一种磁定位装置跟踪定位精度评估系统 |
| CN115886692B (zh) * | 2023-03-03 | 2023-05-12 | 微纳动力(北京)科技有限责任公司 | 磁控悬浮胶囊机器人的实时定位方法、系统及存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN201365901Y (zh) * | 2009-01-20 | 2009-12-23 | 深圳先进技术研究院 | 一种胶囊内窥镜 |
| CN103356150A (zh) * | 2012-03-29 | 2013-10-23 | 上海理工大学 | 方位跟踪引导下的胶囊内窥镜磁场驱动系统 |
| CN103637803A (zh) * | 2013-11-14 | 2014-03-19 | 上海交通大学 | 基于永磁和感应线圈的胶囊内镜空间定位系统及定位方法 |
| CN106983487A (zh) * | 2017-03-14 | 2017-07-28 | 宜宾学院 | 无线胶囊内窥镜三维位置和三维姿态的定位系统及其定位方法 |
| CN108042094A (zh) * | 2017-12-22 | 2018-05-18 | 宜宾学院 | 无线胶囊内窥镜5自由度的定位系统及其定位方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101510196B1 (ko) * | 2013-10-10 | 2015-04-09 | 한국과학기술연구원 | 신체 내 모듈형 캡슐로봇 동작 제어 시스템 |
-
2020
- 2020-07-24 CN CN202010722229.1A patent/CN111839431B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN201365901Y (zh) * | 2009-01-20 | 2009-12-23 | 深圳先进技术研究院 | 一种胶囊内窥镜 |
| CN103356150A (zh) * | 2012-03-29 | 2013-10-23 | 上海理工大学 | 方位跟踪引导下的胶囊内窥镜磁场驱动系统 |
| CN103637803A (zh) * | 2013-11-14 | 2014-03-19 | 上海交通大学 | 基于永磁和感应线圈的胶囊内镜空间定位系统及定位方法 |
| CN106983487A (zh) * | 2017-03-14 | 2017-07-28 | 宜宾学院 | 无线胶囊内窥镜三维位置和三维姿态的定位系统及其定位方法 |
| CN108042094A (zh) * | 2017-12-22 | 2018-05-18 | 宜宾学院 | 无线胶囊内窥镜5自由度的定位系统及其定位方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111839431A (zh) | 2020-10-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111839431B (zh) | 一种无线胶囊机器人系统及控制方法 | |
| US20220061642A1 (en) | Micro-robot control apparatus | |
| Hu et al. | Locating intra-body capsule object by three-magnet sensing system | |
| Than et al. | A review of localization systems for robotic endoscopic capsules | |
| Hoang et al. | Independent electromagnetic field control for practical approach to actively locomotive wireless capsule endoscope | |
| Song et al. | 6-D magnetic localization and orientation method for an annular magnet based on a closed-form analytical model | |
| US20120149981A1 (en) | Magnetically maneuverable in-vivo device | |
| CN101849814B (zh) | 主动式红外无线胶囊内视镜系统 | |
| CN103637803A (zh) | 基于永磁和感应线圈的胶囊内镜空间定位系统及定位方法 | |
| EP2347699A1 (en) | Capsule type endoscope including magnetic drive | |
| CN1654027A (zh) | 用电磁线圈系统在操作空间无接触移动/定位磁体的设备 | |
| CN108042094B (zh) | 无线胶囊内窥镜5自由度的定位系统及其定位方法 | |
| Shi et al. | An optically aided magnetic tracking approach for magnetically actuated capsule robot | |
| KR20190043778A (ko) | 스퀴드를 이용한 의료용 마이크로/나노로봇의 자율 내비게이션 시스템 | |
| CN101297756A (zh) | 磁场与视觉相结合的医疗微型机器人体内姿态定位方法 | |
| Chen et al. | Wireless powered capsule endoscopy for colon diagnosis and treatment | |
| Wang et al. | A novel magnetic tracking approach for intrabody objects | |
| CN102499616A (zh) | 基于加速度传感器的内窥镜探头三维磁场定位系统及定位方法 | |
| Yang et al. | A novel wireless 5-D electromagnetic tracking system based on nine-channel sinusoidal signals | |
| Liu et al. | Three-dimensional localization of a robotic capsule endoscope using magnetoquasistatic field | |
| CN113288008B (zh) | 一种磁性胶囊内窥镜全姿态测定方法 | |
| Fu et al. | Research and experiment on magnetic positioning parameters of medical capsule robot | |
| CN115886692B (zh) | 磁控悬浮胶囊机器人的实时定位方法、系统及存储介质 | |
| CN115067853B (zh) | 胶囊内窥镜的无线充电系统、方法及存储介质 | |
| WO2013162033A1 (ja) | 位置検出装置、カプセル型内視鏡システム、及び位置検出プログラム |
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 |