一种无线胶囊机器人系统及控制方法
阅读说明：本技术 一种无线胶囊机器人系统及控制方法 (Wireless capsule robot system and control method ) 是由 阳万安 戴厚德 于 2020-07-24 设计创作，主要内容包括：本发明提供了医疗检测设备技术领域的一种无线胶囊机器人系统及控制方法,系统包括：胶囊机器人；永磁环,设于所述胶囊机器人的外侧；计算机；驱动装置,与所述计算机连接,并通过所述永磁环驱动胶囊机器人运动；第一无线通信模块,一端与所述计算机连接,另一端与所述胶囊机器人连接；无线交变电磁跟踪装置,与所述计算机连接,并与所述胶囊机器人电磁耦合。本发明的优点在于：极大的提升了胶囊机器人位置跟踪和位姿控制的精度以及系统稳定性。(The invention provides a wireless capsule robot system and a control method in the technical field of medical detection equipment, wherein the system comprises: a capsule robot; the permanent magnet ring is arranged on the outer side of the capsule robot; a computer; the driving device is connected with the computer and drives the capsule robot to move through the permanent magnet ring; one end of the first wireless communication module is connected with the computer, and the other end of the first wireless communication module is connected with the capsule robot; and the wireless alternating electromagnetic tracking device is connected with the computer and is electromagnetically coupled with the capsule robot. The invention has the advantages that: the precision of capsule robot position tracking and position appearance control and system stability have greatly been promoted.)
技术领域
本发明涉及医疗检测设备技术领域，特别指一种无线胶囊机器人系统及控制方法。
背景技术
无线胶囊机器人具有内窥镜检查、病理取样和施药等功能，可以完成无痛无创的消化道检查和手术，是对传统的有线胃镜和外科手术方式的革命性突破，具有更高的效率和安全性。
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、计算机通过交变电路控制各交变电磁发射线圈产生大小为的交变磁场；交变磁场
其中(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、通以交变电磁信号的交变电磁发射线圈的空间磁场分布可以等效为一个磁偶极子，即每一个交变电磁发射线圈可等效为一个磁偶极子；计算机通过交变电路控制各交变电磁发射线圈产生大小为
其中(m,n,p)T表示交变电磁发射线圈的归一化方向向量，且m2+n2+p2＝1；(a,b,c)T表示交变电磁发射线圈的中心位置点；BT表示磁场常量，受线圈及加载电流特性影响；L表示交变电磁发射线圈的中心位置点到空间点(x1,y1,z1)的距离，且Bx1表示交变磁场
由于多路交变电磁发射阵列的各交变电磁发射线圈采用分频激励的方式，为了实现收发的时序同步，在交变电磁发射线圈发射信号的同时固定输出一定时间间隔的零点信号，实现电磁感应线圈的相位同步。
电磁感应线圈端感应的交变电磁信号表现为不同频率的多通道发射交变信号的叠加，通过傅里叶变换实现不同频率分量的幅度分离，分离得到的不同通道幅度信号用于最优化算法对位姿求解。
步骤S42、设电磁感应线圈的中心位置点为(x2,y2,z2)，(vx,vy,vz)T为电磁感应线圈归一化方向向量，且
当交变电磁发射线圈与电磁感应线圈的归一化方向向量不平行时，计算交变电磁发射线圈产生的磁场强度
基于法拉第电磁感应原理，电磁感应线圈的感生电动势为：
其中N表示电磁感应线圈的匝数；表示电磁感应线圈的表面积；
当交变电磁发射线圈的发射信号为频率为ω的正弦波，则电磁感应线圈感应的电动势为：
其中
基于电磁感应线圈感应的电动势得到误差方程：
步骤S43、利用粒子群优化算法(PSO算法)求解所述误差方程的初值，再利用LM算法对所述初值迭代，得到位姿参数(x,y,z,vx,vy,vz)，基于所述位姿参数对胶囊机器人的位置和姿态进行实时跟踪。
由于所述误差方程是一个非线性最小二乘优化问题，LM算法(Levenberg-Marquardt)用以精确求解，但对初值要求较高，故采用找初值能力强(迭代速率快)但跟踪精度差的PSO算法，用以在跟踪定位输出作为LM算法的初值，接下来的跟踪过程中，上一次迭代过程的LM输出作为下一次迭代LM的初值。
综上所述，本发明的优点在于：
通过在胶囊机器人的外侧设置永磁环，计算机通过控制机械臂抓取永磁体，通过磁力驱动永磁环运动，进而联动胶囊机器人进行位姿调整，极大的提升了胶囊机器人位姿控制的精度；通过在胶囊机器人内设置电磁感应线圈，通过多路交变电磁发射阵列向胶囊机器人发射交变磁场的信号，进而通过计算对应的电动势对胶囊机器人的位置进行定位，极大的提升了胶囊机器人位置跟踪的精度；而永磁体的位姿变化产生的旋转磁场频率与多路交变电磁发射阵列产生的信号频率互不干扰，极大的提升了系统的稳定性，最终实现基于闭环方式的胶囊机器人的智能控制。
虽然以上描述了本发明的具体实施方式，但是熟悉本技术领域的技术人员应当理解，我们所描述的具体的实施例只是说明性的，而不是用于对本发明的范围的限定，熟悉本领域的技术人员在依照本发明的精神所作的等效的修饰以及变化，都应当涵盖在本发明的权利要求所保护的范围内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种单筒型内窥镜快速防雾装置及其使用方法