CN116803330A - 磁控胶囊系统及其定量闭环控制方法 - Google Patents

磁控胶囊系统及其定量闭环控制方法 Download PDF

Info

Publication number
CN116803330A
CN116803330A CN202210272489.2A CN202210272489A CN116803330A CN 116803330 A CN116803330 A CN 116803330A CN 202210272489 A CN202210272489 A CN 202210272489A CN 116803330 A CN116803330 A CN 116803330A
Authority
CN
China
Prior art keywords
capsule
real
target
position information
control
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
Application number
CN202210272489.2A
Other languages
English (en)
Inventor
黄志威
张行
袁文金
杨戴天杙
张皓
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ankon Technologies Co Ltd
Original Assignee
Ankon Technologies Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Ankon Technologies Co Ltd filed Critical Ankon Technologies Co Ltd
Priority to CN202210272489.2A priority Critical patent/CN116803330A/zh
Priority to PCT/CN2023/082318 priority patent/WO2023174427A1/zh
Publication of CN116803330A publication Critical patent/CN116803330A/zh
Pending legal-status Critical Current

Links

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
    • 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
    • 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
    • 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/273—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 for the upper alimentary canal, e.g. oesophagoscopes, gastroscopes

Landscapes

  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Surgery (AREA)
  • Biomedical Technology (AREA)
  • Medical Informatics (AREA)
  • Optics & Photonics (AREA)
  • Pathology (AREA)
  • Radiology & Medical Imaging (AREA)
  • Biophysics (AREA)
  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Gastroenterology & Hepatology (AREA)
  • Endoscopes (AREA)

Abstract

本发明揭示了一种磁控胶囊系统及其定量闭环控制方法，方法包括：持续获取胶囊的实时位姿信息；获取胶囊的目标位姿信息；根据目标位姿信息，确定目标位姿范围；根据实时位姿信息和目标位姿信息，计算控制磁体的当前移动轨迹；控制控制磁体沿当前移动轨迹运动；若直至控制磁体停止运动，实时位姿信息在目标位姿范围外，则重复上述步骤直至实时位姿信息在目标位姿范围内。该磁控胶囊系统的定量闭环控制方法能够定量、闭环地对胶囊的运动过程进行控制，精准控制胶囊的运动量，控制过程的健壮性强，磁控胶囊系统的自动化、智能化检查及衍生应用提供便利。

Description

磁控胶囊系统及其定量闭环控制方法
技术领域
本发明涉及医疗设备技术领域，尤其涉及一种磁控胶囊系统及其定量闭环控制方法。
背景技术
体内设备定位技术，如无线胶囊体、侵入式医疗器械等体内定位技术，受到越来越多的关注。磁控胶囊系统通过磁力驱动胶囊在体内运动，目前对胶囊的驱动，还需要通过有丰富经验的医师来完成。医师通过内置镜头拍摄消化道内壁的检查图像，确定胶囊的位置和姿态朝向，再通过外部控制磁体驱动胶囊继续运动到下一位置。
由于磁力的极度非线性、非均匀空间分布特点，消化道可形变环境以及摩擦力的影响，胶囊自旋导致难以依据图像判断真实方位，使得仅依靠图像视觉反馈信息，无法精准定量控制胶囊到达目标位置和目标姿态角度。现有的定位系统仅能起到辅助确认的作用，胶囊的下一步运动仍需要医师凭借经验判断，整个控制过程不够直观、精准。
尤其是在永磁体控制胶囊运动的过程中，由于永磁体无法像电磁铁一样高频地改变磁力大小和调整磁性方向，所以控制的速度慢，传递反馈的速度慢，很难根据反馈做如何进一步控制的判断，操作效率特别低，使用体验很不好。
发明内容
为解决上述现有技术问题中的至少其一，本发明的目的在于提供一种通过闭环的形式准确控制胶囊运动的磁控胶囊系统及其定量闭环控制方法。
为实现上述发明目的，本发明一实施方式提供一种磁控胶囊系统的定量闭环控制方法，包括如下步骤：
持续获取胶囊的实时位置信息；
获取所述胶囊的目标位置信息；
根据所述目标位置信息，确定目标位置范围；
根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；
控制所述控制磁体沿所述当前移动轨迹运动；
若直至所述控制磁体停止运动，所述实时位置信息在所述目标位置范围外，则重复步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；控制所述控制磁体沿所述当前移动轨迹运动”，直至所述实时位置信息在所述目标位置范围内。
作为本发明的进一步改进，还包括步骤：
持续获取所述控制磁体的实时磁体位置信息，其中，所述实时磁体位置信息、所述实时位置信息和所述目标位置信息均位于同一世界坐标系。
作为本发明的进一步改进，还包括步骤：
获取临界距离，其中，所述临界距离为所述胶囊在液体中悬浮时的所述胶囊与所述控制磁体之间的距离；
根据所述实时磁体位置信息、所述实时位置信息和所述临界距离，计算所述胶囊的状态信息，其中，所述状态信息包括沉底状态、液面悬浮状态和吸顶状态。
作为本发明的进一步改进，其中，所述目标位置信息包括目标状态；
还包括步骤：
若所述状态信息与目标状态不一致，调节所述控制磁体的高度，直到所述胶囊处于目标状态；
调节所述控制磁体的朝向，以使所述胶囊的内窥镜朝向指定方向。
作为本发明的进一步改进，
当所述目标状态为沉底状态时，控制所述胶囊与所述控制磁体之间的距离大于所述临界距离与第一阈值之和；
当所述目标状态为吸顶状态时，控制所述胶囊与所述控制磁体之间的距离小于所述临界距离与第二阈值之和。
作为本发明的进一步改进，所述步骤“获取临界距离”包括：
根据临界距离算法或者根据测量数据获取所述临界距离；
其中，所述临界距离算法，是根据液面悬浮状态下，所述控制磁体和所述胶囊之间的吸力与所述胶囊受到的浮力之和等于所述胶囊的重力，计算所述控制磁体与所述胶囊之间的距离。
作为本发明的进一步改进，还包括步骤：
控制将所述控制磁体移动至初始位置，其中，于所述初始位置，所述控制磁体与所述胶囊在垂直于所述世界坐标系的XY平面的同一条直线上。
作为本发明的进一步改进，还包括步骤：
获取所述控制磁体的运动方式，所述运动方式包括平移方式和/或翻滚方式。
作为本发明的进一步改进，所述步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹”包括：
根据所述实时位置信息和所述目标位置信息，计算当前偏移量，其中，所述当前偏移量包括在所述世界坐标系中的X轴方向的X轴差值、以及在Y轴方向的Y轴差值；
若所述控制磁体的运动方式为平移方式，所述当前移动轨迹为沿所述当前偏移量平移。
作为本发明的进一步改进，所述步骤“根据所述实时位置信息和所述目标位置信息，计算当前偏移量”包括：
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的X轴方向的第一差值，将所述第一差值与第一修正因子的乘积作为所述X轴差值；
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的Y轴方向的第二差值，将所述第二差值与第二修正因子的乘积作为所述Y轴差值；
其中，所述第一修正因子与所述第二修正因子的取值范围均为0到1之间。
作为本发明的进一步改进，所述步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹”包括：
获取所述胶囊的长轴周长，其中，所述长轴周长为所述胶囊沿其长轴翻转一周对应的长度；
根据所述实时位置信息和所述目标位置信息，计算当前翻滚量，其中，所述当前翻滚量包括所述胶囊的翻滚圈数和翻滚方向；
若所述控制磁体的运动方式包括翻滚方式，所述当前移动轨迹包括沿所述翻滚方向翻滚所述翻滚圈数。
作为本发明的进一步改进，其中，所述翻滚圈数为整数，每圈对应的所述控制磁体的转动量为360°。
作为本发明的进一步改进，还包括步骤：
当所述控制磁体的运动方式为平移方式，所述胶囊的运动未运动时，切换所述控制磁体的运动方式为翻滚方式。
作为本发明的进一步改进，其中，所述目标位置范围包括预设距离；
还包括步骤：
计算所述实时位置信息与所述目标位置信息在所述世界坐标系的XY平面上的投影的距离；
当所述距离小于等于所述预设距离，所述实时位置信息在所述目标位置范围内；
当所述距离大于所述预设距离，所述实时位置信息在所述目标位置范围外。
作为本发明的进一步改进，还包括步骤：
当所述实时位置信息在所述目标位置范围内时，控制所述控制磁体与所述胶囊在垂直于所述世界坐标系的XY平面的同一条直线上。
作为本发明的进一步改进，还包括步骤：
当所述实时磁体位置信息与所述实时位置信息对应的距离大于安全距离时，控制所述控制磁体与所述胶囊在垂直于所述世界坐标系的XY平面的同一条直线上。
为实现上述发明目的之一，本发明一实施例提供了一种磁控胶囊系统的定量闭环控制方法，包括如下步骤：
持续获取胶囊的实时姿态信息；
获取胶囊的目标姿态信息；
根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹；
控制所述控制磁体沿所述当前转动轨迹运动；
若直至所述控制磁体停止运动，所述实时姿态信息在所述目标姿态范围外，则重复步骤“根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹；控制所述控制磁体沿所述当前转动轨迹运动”，直至所述实时姿态信息在所述目标姿态范围内。
作为本发明的进一步改进，持续获取所述控制磁体的实时磁体姿态信息，其中，所述实时磁体姿态信息、所述实时姿态信息和所述目标姿态信息均位于同一世界坐标系。
作为本发明的进一步改进，控制所述控制磁体转动至初始姿态，其中，于所述初始姿态，所述控制磁体的磁化方向N极的朝向平行或垂直于所述世界坐标系的Z轴。
作为本发明的进一步改进，其中，所述当前转动轨迹包括当前转动量，所述当前转动量包括第一角度差和第二角度差；
所述初始姿态包括与Z轴正向的第一夹角，所述目标姿态信息包括与Z轴正向的第二夹角，所述第一角度差为所述第二夹角与所述第一夹角差值与第三修正因子的乘积；
所述初始姿态包括在XY平面与Y轴正向的第三夹角，所述目标姿态信息包括在XY平面与Y轴正向的第四夹角，所述第二角度差为所述第四夹角与所述第三夹角的差值与第四修正因子的乘积。
作为本发明的进一步改进，其中，所述第三修正因子和所述第四修正因子的取值范围均为0到1之间。
作为本发明的进一步改进，其中，所述目标姿态范围包括预设角度差；
还包括步骤：
计算所述实时姿态信息计对应的第一单位向量；
计算所述目标姿态信息对应的第二单位向量；
计算所述第一单位向量与所述第二单位向量的实时角度差；
当所述实时角度差小于等于所述预设角度差，所述实时姿态信息在所述目标姿态范围内；
当所述实时角度差大于所述预设角度差，所述实时姿态信息在所述目标姿态范围外。
为实现上述发明目的之一，本发明一实施例提供了一种磁控胶囊系统，包括：
第一获取模块，用于持续获取胶囊的实时位置信息；
第一目标获取模块，用于获取所述胶囊的目标位置信息，并根据所述目标位置信息，确定目标位置范围；
第一计算模块，用于根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；
第一控制模块，用于控制所述控制磁体沿所述当前移动轨迹运动；
第一循环模块，用于当所述控制磁体停止运动，若所述实时位置信息在所述目标位置范围外，则重复使“第一计算模块根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹，第一控制模块控制所述控制磁体沿所述当前移动轨迹运动”，直到所述实时位置信息在所述目标位置范围内。
为实现上述发明目的之一，本发明一实施例提供了一种磁控胶囊系统，包括：
第二获取模块，用于持续获取胶囊的实时姿态信息；
第二目标获取模块，用于获取胶囊的目标姿态信息；
第二计算模块，用于根据所述目标姿态信息，确定目标姿态范围和控制磁体的当前转动轨迹；
第二控制模块，用于控制所述控制磁体沿所述当前转动轨迹运动；
第二循环模块，用于当所述控制磁体停止运动，若所述实时姿态信息在所述目标姿态范围外，则重复“第二计算模块根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹，第二控制模块控制所述控制磁体沿所述当前转动轨迹运动”，直到所述实时姿态信息在所述目标姿态范围内。
为实现上述发明目的之一，本发明一实施例提供了一种电子设备，包括：
存储模块，存储计算机程序；
处理模块，执行所述计算机程序时可实现上述的磁控胶囊系统的定量闭环控制方法中的步骤。
为实现上述发明目的之一，本发明一实施例提供了一种可读存储介质，其存储有计算机程序，该计算机程序被处理模块执行时可实现上述的磁控胶囊系统的定量闭环控制方法中的步骤。
与现有技术相比，本发明具有以下有益效果：该磁控胶囊系统的定量闭环控制方法能够定量地对胶囊的运动过程进行控制，使胶囊准确的到达目标位置和/或姿态，且通过闭环的定量闭环控制方法，精准控制胶囊的运动量。且该方法运用与永磁体控制的胶囊运动的场景下，避免了永磁体因速度、状态的切换不及时而操作慢的问题，控制过程的健壮性强，另外该方法可以实现复杂的控制逻辑，进行衍生的消化道检查应用，为磁控胶囊系统的自动化、智能化检查及衍生应用提供便利。
附图说明
图1是本发明其一实施例的磁控胶囊系统控制方法的结构示意图；
图2是本发明另一实施例的磁控胶囊系统控制方法的结构示意图；
图3是本发明一实施例的胶囊在人体内沉底且位置调节完后的结构示意图；
图4是本发明一实施例的胶囊在人体内沉底且姿态调节完后的结构示意图；
图5是本发明一实施例的控制磁体的磁力线分布及其垂直磁场相对强度示意图；
图6是本发明一实施例的胶囊在人体内受力平衡状态示意图；
图7是本发明一实施例的胶囊以平移方式运动的示意图；
图8是本发明一实施例的胶囊以翻滚方式运动的示意图；
图9是本发明一实施例的胶囊调整姿态过程的示意图；
图10是本发明一实施例的磁控胶囊系统的模块示意图；
其中，1000、磁控胶囊系统；100、磁控系统；200、胶囊定位系统；201、胶囊；300、床面；400、人体；10、控制磁体；20、信号传输模块；30、存储模块；40、处理模块；50、磁传感器；60、加速度传感器；70、信号输送模块；80、摄像模块；90、通信总线。
具体实施方式
以下将结合附图所示的具体实施方式对本发明进行详细描述。但这些实施方式并不限制本发明，本领域的普通技术人员根据这些实施方式所做出的结构、方法、或功能上的变换均包含在本发明的保护范围内。
应该理解，本文使用的例如“上”、“上方”、“下”、“下方”等表示空间相对位置的术语是出于便于说明的目的来描述如附图中所示的一个单元或特征相对于另一个单元或特征的关系。空间相对位置的术语可以旨在包括设备在使用或工作中除了图中所示方位以外的不同方位。
本发明一实施例提供一种通过闭环的形式准确控制胶囊运动的磁控胶囊系统及其定量闭环控制方法，使胶囊准确的到达目标位置和/或姿态，且在永磁体控制胶囊运动的场景下具有更加显著的进步，为磁控胶囊系统的自动化、智能化检查及衍生应用提供便利。
本实施例的磁控胶囊系统1000，包括磁控系统100、胶囊定位系统200、控制磁体10和胶囊201，磁控系统100用于通过控制胶囊201的运动，胶囊定位系统200用于对胶囊201定位，胶囊201内部装有传感器模块，传感器模块包括用于检测磁场的磁传感器50(magsensor)，如霍尔传感器、磁阻传感器(AMR、GMR、TMR)等，控制磁体10包括用于发出磁场的磁源、用于控制磁源运动的伺服电机，胶囊201即为胶囊内窥镜，其内部也有磁性件，通过磁源对磁性件的作用力，实现磁控系统100对胶囊201的位置和姿态的控制。
图1和2为本申请一个实施方式的一种磁控胶囊系统1000的定量闭环控制方法的两种实施例，其中图1用于调整胶囊201的位置，图2用于调整胶囊201的姿态，图3和4为本申请一个实施方式的磁控胶囊系统1000应用于人体400时的示意图，其中图3完成了对胶囊位置的调整，图4完成了对胶囊201姿态的调整，胶囊201位于人体400内部，人体400躺平于床面300上，人体400外部设有磁控系统100。在检查时，控制磁体10发出的磁场，控制人体400内的胶囊201的运动。
这里，为清楚地表达本实施例中所描述的位置与方向，在本实施例中，定义人体400躺在床面300的上方，反方向为下方，控制磁体10可以在人体400的各个方向控制胶囊201，以图3和4为例，设定控制磁体10位于人体400的上方，另外浮力的方向向上，重力的方向向下，左右方向图3、4、7、8的左边和右边。
下面先结合图1、图3、图5～8对胶囊201的位置调整进行论述，虽然本申请提供了如下述实施方式或流程图所述的方法操作步骤，但是基于常规或者无需创造性的劳动，所述方法在逻辑性上不存在必要因果关系的步骤中，这些步骤的执行顺序不限于本申请实施方式中所提供的执行顺序。
具体的磁控胶囊系统1000的定量闭环控制方法，包括如下步骤：
步骤101：持续获取胶囊201的实时位置信息。
同时，还可以持续获取所述控制磁体10的实时磁体位置信息，
其中，所述实时磁体位置信息、所述实时位置信息和所述目标位置信息均位于同一世界坐标系。
胶囊201的实时位置信息可以通过胶囊定位系统200获取，实时磁体位置信息可以通过磁控系统100获取，将磁控系统100的第一局域坐标系与胶囊定位系统200的第二局域坐标系进行匹配，将胶囊定位系统200获取的坐标换算到世界坐标系中，将磁控系统100获取的也坐标换算到世界坐标系中，从而可以对其统一调整。
胶囊定位系统200给出的6-DOF状态数据为[Cx,Cy,Cz,Ch,Cv,Cs]，实时位置信息采用其中的[Cx,Cy,Cz]，表示胶囊201在世界坐标系的XYZ坐标。
磁控系统100给出的5-DOF状态数据表示为[Mx,My,Mz,Mh,Mv]，实时磁体位置采用其中的[Mx,My,Mz]，表示控制磁体10在世界坐标系的XYZ坐标。
另外为了减小移动胶囊位置时，控制磁体10姿态的影响，下文在论述位置调整的过程中也会涉及到[Mh,Mv]，其中，[Mh,Mv]表示控制磁体10磁场方向N极的朝向角度，水平方位角Mh(取值范围[-180,+180]度)为磁体磁化方向矢量在XY平面投影矢量与Y轴正向的夹角为水平方位角，垂直倾斜角Mv(取值范围[0,+180]度)为控制磁体10磁化方向矢量与Z轴正向的夹角。
步骤102：获取胶囊201的目标位置信息，并根据所述目标位置信息，确定目标位置范围。目标位置信息可以表示为[Tx,Ty,Tz]，由于精准运动到该点位置效率不高，所以在胶囊201到达在目标位置信息附近的一定范围内，即可视作已经运动到位。
在开始步骤103之前，可以先将控制磁体10移动到初始位置，然后对胶囊201的状态进行判断和调整。
移动到初始位置，是控制所述控制磁体10移动至初始位置，其中，于所述初始位置，所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。也就是此时控制磁体10和胶囊201在X轴、Y轴上的坐标相同，这里的XY平面指的即是上文的世界坐标系。
并且，控制磁体10移动，将磁化方向调整为竖直向上Mv＝0(竖直向下Mv＝180)，使得控制磁体10位于胶囊201正上方时，胶囊201也保持竖直向上Cv～0(竖直向下Cv～180)。
另外在为了避免控制磁体10在移动到胶囊201正上方的过程中，磁力吸引导致胶囊201发生位置移动，可以根据控制系统状态反馈信息，及时调整控制磁体10与胶囊201的Z轴距离，将距离控制在安全距离。
由于磁力的极度非线性、非均匀空间分布特点(参图5所示)，且控制磁体10在较远距离的非接触控制，尤其是永磁体不能快速地调整其磁力大小和方向，所以需要先对胶囊201的平衡状态进行判断，使其处于受力平衡的状态下进行后续的操作。
对胶囊201的状态进行判断和调整包括步骤：
获取临界距离，其中，所述临界距离为胶囊201在液体中悬浮时的胶囊201与所述控制磁体10之间的距离；
根据所述实时磁体位置信息、所述实时位置信息和所述临界距离，计算胶囊201的状态信息，其中，所述状态信息包括沉底状态、液面悬浮状态和吸顶状态。
三种状态的受力分析可以参考图6所示，沉底状态也可参图2或3所示，消化道内包括上壁、下壁和液面，沉底状态时，胶囊201抵着下壁，吸顶状态时，胶囊201抵着上壁，液面悬浮状态时，控制磁体10的吸力与浮力之和等于胶囊201的重力。另外，在撤去吸力时，胶囊201会是沉底的状态。
其中，所述步骤“获取临界距离”包括：
根据临界距离算法或者根据测量数据获取所述临界距离。
临界距离算法，是根据液面悬浮状态下，所述控制磁体10和胶囊201之间的吸力与胶囊201受到的浮力之和等于胶囊201的重力，计算所述控制磁体10与胶囊201之间的距离。
更具体的公式方面，控制磁体10对正下方胶囊201的吸力可以表示为：
其中，M,m分别为控制磁体10、胶囊201磁铁的磁矩，r为控制磁体10与胶囊201磁铁的中心距离,μ0为真空磁导率。
控制磁体10对正下方胶囊201的吸力与胶囊201重力G和胶囊201所受浮力Ff平衡时，
Fm(Z0)+ρVg＝mcg
其中ρ为胶囊201所处介质液体密度，V为胶囊201体积，g为重力加速度常量，mc为胶囊201质量。
结合上述公式，临界距离距离Z0的计算公式：
通过该公式可以计算出临界距离，当结合上述控制磁体10的初始位置，临界距离指的是高度的距离，即为临界高度。
实际环境下，控制磁体10、胶囊201受充磁影响导致磁矩大小变化，胶囊201质量及体积受型号的变化影响，Z0根据实际的控制系统和胶囊201类型进行实验测量更为方便可靠。
控制磁体10中心与胶囊201的高度差表示为：dZ≡Mz-Cz。
可以根据控制磁体10和胶囊201的状态数据实时计算获取该高度差。通过dZ判断胶囊201当前的状态，沉底的判断条件为dZ>Z0+δa，吸顶的判断条件为dZ<Z0-δa。
其中，冗余参量δa取值1～2cm，用于消除定位误差、消化道蠕动引起的噪声干扰。
另外，所述目标位置信息包括目标状态；
若所述状态信息与目标状态一致，控制磁体10的朝向正确，则无需调整；
若所述状态信息与目标状态不一致，调节所述控制磁体10的高度，直到胶囊201处于目标状态；
调节所述控制磁体10的朝向，以使胶囊201的内窥镜朝向指定方向。
具体地，通过dZ控制胶囊201转换为沉底、吸顶状态，驱动控制磁体10改变其上下方向的高度，当胶囊201当前沉底，需要改为吸顶时，调整dZ<Z0-δa；当胶囊201当前沉底，需要改为吸顶时，调整dZ>Z0+δa。
其中，冗余参量δb取值3cm，用于消除定位误差、消化道蠕动引起的噪声干扰，确保控制动作执行的成功率，使得胶囊201切换至目标状态。
另外，沉底与吸顶状态下，胶囊201依赖消化道的可变支持力、摩擦力实现控制磁体10磁力、胶囊201重力以及液体浮力的平衡。离开消化道壁的胶囊201无法在Z轴任意位置达到平衡，所以仅在液面附近有可能通过浮力的负反馈机制实现胶囊201在液面悬浮稳定。
以及，当所述目标状态为沉底状态时，控制胶囊201与所述控制磁体10之间的距离大于所述临界距离与第一阈值之和，第一阈值根据现场环境进行调整，当需要胶囊201沉底时，胶囊201从底部向上拍摄，此时需要避免胶囊201被控制磁体10吸起，所以使控制磁体10距离胶囊201的距离尽量的远。
当所述目标状态为吸顶状态时，控制胶囊201与所述控制磁体10之间的距离小于所述临界距离与第二阈值之和，第二阈值根据现场环境进行调整，当需要胶囊201吸顶时，胶囊201从上方向下拍摄，此时需要避免胶囊201掉落，所以使控制磁体10距离胶囊201的距离尽量的近。
在步骤“调节所述控制磁体10的朝向，以使胶囊201的内窥镜朝向指定方向”中，摄像模块80在胶囊201的一端，当胶囊201沉底时，需要摄像模块80向上拍摄，当胶囊201吸顶时，需要摄像模块80向下拍摄，所以根据胶囊201此时的状态，控制磁体10采用不同的朝向。例如摄像模块80位于胶囊201的N端，于沉底状态，控制磁体10的S端朝向胶囊201，于吸顶状态，控制磁体10的N端朝向胶囊201。
以及在开始步骤103，可以先执行该步骤：
获取所述控制磁体10的运动方式，所述运动方式包括平移方式和/或翻滚方式。胶囊201可以以平移的方式直线移动，也可以以翻滚的形式滚动向前运动，平移方式可参图7所示，翻滚方式参图8所示。平移方式和/或翻滚方式可以人为选择，也可以自动选择，下文将两种情况区分展开论述。
平移状态
在倾斜坡度较小的近似XY平面内的胶囊位置转移，可以通过控制磁体10拖动胶囊201平移实现，且根据上文，已知胶囊201的实时位置信息为[Cx,Cy,Cz]，胶囊201的目标位置信息为[Tx,Ty,Tz]。由于胶囊201在Z轴位置无法稳定连续控制，仅对XY平面内的胶囊位置进行控制操作。平移状态如图7所示。
步骤103：根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹。
根据所述实时位置信息和所述目标位置信息，计算当前偏移量，其中，所述当前偏移量包括在所述世界坐标系中的X轴方向的X轴差值、以及在Y轴方向的Y轴差值；
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的X轴方向的第一差值，将所述第一差值与第一修正因子的乘积作为所述X轴差值；第一差值的表达式为：ΔCx＝Tx-Cx，X轴差值的表达式为：ΔMx＝αxΔCx。
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的Y轴方向的第二差值，将所述第二差值与第二修正因子的乘积作为所述Y轴差值；第二差值的表达式为：ΔCy＝Ty-Cy，Y轴差值的表达式为：ΔMy＝αyΔCy。
其中，所述第一修正因子与所述第二修正因子的取值范围均为0到1之间，表达式为0<αx,αy<1，为方便计算，可以设定αx＝αy，一般选取αx,y＝0.7即可稳定、高效地反馈调整胶囊201到目标位置信息。
步骤104：控制所述控制磁体10沿所述当前移动轨迹运动。
若所述控制磁体10的运动方式为平移方式，所述当前移动轨迹为沿所述当前偏移量平移。
翻滚状态
胶囊201翻滚动作通常用于胶囊201无法被有效平移的情形，这类情形包括但不限于：控制磁体10离胶囊201较远无法提供足够大的牵引磁力；消化道壁坡度较大使得胶囊201无法有效地拖动平移；胶囊201移动方向存在无法有效越过的褶皱、障碍物等。翻滚状态如图8所示。
控制磁体10会与胶囊201反向转动，也就是说，当控制磁体10逆时针转动时，胶囊201会以顺时针翻滚，例如图8中，控制磁体10一边向右运动，一边逆时针转动，理想地，胶囊201贴着底壁做顺时针的纯滑滚动。
步骤103：根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹。
获取胶囊201的长轴周长，其中，所述长轴周长为胶囊201沿其长轴翻转一周对应的长度；
根据所述实时位置信息和所述目标位置信息，计算当前翻滚量，其中，所述当前翻滚量包括胶囊201的翻滚方向和翻滚圈数；
其中，控制磁体10的水平转动使得Mh＝Th，Th为上述的胶囊201待翻滚的目标水平方位角度。此时，控制磁体10的垂直转动面与Th方位重合，翻滚方向即为水平方位角度为Th的夹角方向。
胶囊201外形近似为两端为球面的圆柱体，则胶囊201的长轴周长可以表示为：
L＝2H+(π-2)D，其中，H为胶囊201的长轴长度，D为胶囊直径。
胶囊201翻滚一周后，在XY平面的位置转移近似为：
根据Δx、Δy，以及Tx-Cx和Ty-Cy，可以判断翻滚的量，翻滚圈数可以考虑以及/>
而其中，所述翻滚圈数为整数，每圈对应的所述控制磁体10的转动量为360°，使胶囊201的位置转移若干个胶囊201的长轴周长，并保持原有胶囊201朝向姿态，即摄像模块80仍朝着原来的方向。
为了降低胶囊201在翻滚时由于胶囊重心偏离轴心的不对称性导致的自旋横向滚动，调整控制磁体10往翻滚方向前移一个胶囊201长轴周长距离L，提供牵引导向磁吸力。导向磁吸力沿预期翻滚方向，与胶囊201自旋横向滚动分力垂直，两者合力指向目标方向附近。并且在发生细微横向滚动时，导向磁吸力提供负反馈分力，削弱胶囊201横向分力矩的影响，引导胶囊201朝预期方向翻滚，并且在翻滚一周后位于控制磁体10正下方附近。
步骤104：控制所述控制磁体10沿所述翻滚方向翻滚所述翻滚圈数运动。
另外，当所述控制磁体10的运动方式为平移方式，胶囊201的运动未运动时，切换所述控制磁体10的运动方式为翻滚方式。
每次执行胶囊201的平移操作，设定最大修正循环次数(例如，不超过5次修正)，如果达到修正次数上限，仍然未满足阈值条件，则结束胶囊201平移任务，以翻滚方式驱动胶囊201的运动。
由于翻滚对应的摩擦力小于平移时的摩擦力，尤其小于胶囊201未运动时的静摩擦力，所以当平移无法有效移动胶囊201时，改为翻滚方式运动。
步骤105：若直至所述控制磁体10停止运动，所述实时位置信息在所述目标位置范围外，则重复步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹；控制所述控制磁体10沿所述当前移动轨迹运动”，直至所述实时位置信息在所述目标位置范围内。
每次执行完一次胶囊201的动作后，更新胶囊201当前位置[Cx,Cy,Cz]，计算与目标位置[Tx,Ty,Tz]的XY平面上是否到达目标位置范围内。若未到达目标位置范围内，则继续重复执行胶囊201平移和/或翻滚的动作；直到胶囊201位于目标位置范围内，控制位置的任务执行成功。
所述目标位置范围包括预设距离distth，distth一般取值5～10mm即可达到较好的控制精度和控制效率的平衡。
在步骤105中，计算所述实时位置信息与所述目标位置信息在所述世界坐标系的XY平面上的投影的距离dist；
该距离dist的计算方法为：
当所述距离小于等于所述预设距离，即dist≤distth，所述实时位置信息在所述目标位置范围内；
当所述距离大于所述预设距离，即dist>distth，所述实时位置信息在所述目标位置范围外。
另外，由于将0<αx,αy<1，可以避免导致振荡现象，防止胶囊201运动过量，例如在图7中防止胶囊201运动到T点的右边，避免出现一直修正不到位的问题。0<αx,αy<1相当于加入阻尼效应，可以确保振荡效果衰减，一定能收敛到目标，在运用本申请的方法的实验中，一般将上述步骤循环两次，即控制磁体10运动两次，即可将胶囊201移动到目标位置。
步骤106：当所述实时位置信息在所述目标位置范围内时，控制所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上，此时[Mx,My]＝[Cx,Cy]。
在图7中的第3张图中，胶囊201运动到位时，控制磁体10可能会不在T点的位置，为了便于在后续的姿态调整，将胶囊201从固定位置开始调整姿态，可以方便姿态位置的计算，所以在胶囊201调整到位时，将控制磁体10切换到如图3所示、或者如图7的第四张图所示，是胶囊201运动到目标位置时，胶囊201和控制磁体10的姿态。
此时，磁化方向调整为竖直向上Mv＝0(竖直向下Mv＝180)，使得控制磁体10位于胶囊201正上方时，胶囊201也保持竖直向上Cv～0(竖直向下Cv～180)，也就是说，控制磁体10在胶囊201的正上方和/或正下方。
另外，当所述实时磁体位置信息与所述实时位置信息对应的距离大于安全距离时，控制所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。
当控制磁体10超出胶囊201的有效控制范围之后，驱动磁力将急剧衰减，此时将控制磁体10拉回到胶囊201正上方，重新开始位置调节的操作。必要时，需要调整控制磁体10距离胶囊201的高度到合适范围，以便提供足够的驱动磁力。
在胶囊201的位置转移完成后，进行胶囊201的朝向姿态角调整，将胶囊201镜头指向特定方位。
下面结合图2、图4、图9对胶囊201的姿态调整的方法进行论述，包括如下步骤：
步骤201：持续获取胶囊201的实时姿态信息。
同时，持续获取所述控制磁体10的实时磁体姿态信息，其中，所述实时磁体姿态信息、所述实时姿态信息和所述目标姿态信息均位于同一世界坐标系。
胶囊定位系统200给出的6-DOF状态数据为[Cx,Cy,Cz,Ch,Cv,Cs]，其中，胶囊201朝向姿态角通过球坐标形式的状态参数[Ch,Cv]描述，Cv为垂直倾斜角，Ch为水平方位角。[Ch,Cv]表示胶囊201头部的朝向角度，水平方位角Ch(取值范围[-180,+180]度)为胶囊201头部朝向在XY平面投影矢量与Y轴正向的夹角，并按照顺时针方向增加(Y轴正向时Ch＝0，X轴正向时Ch＝90，Y轴负向时Ch＝±180，X轴负向时Ch＝-90)；垂直倾斜角Cv(取值范围[0,+180]度)为胶囊201头部朝向与Z轴正向的夹角为胶囊201垂直倾斜角。
Cs为胶囊201自旋角度，胶囊201镜头拍摄图像正立时胶囊201自旋角Cs＝0，按照顺时针方向增加。本实施例朝向姿态角调整不涉及与之垂直的独立自由度胶囊201自旋角Cs。
步骤202：获取胶囊201的目标姿态信息[Th,Tv]，并且根据所述目标姿态信息，计算目标姿态范围。
所述目标姿态范围包括预设角度差distth，预设角度差distth一般取值5～10度即可达到较好的控制精度和控制效率；
在开始步骤203或204之前，控制所述控制磁体10转动至初始姿态，其中，于所述初始姿态，所述控制磁体10的磁化方向N极的朝向平行或垂直于所述世界坐标系的Z轴。
本实施例的图2中，采用控制磁体10的磁化方向N极的朝向平行Z轴的方案，也就是上文所述的开始姿态调节之前的姿态。
步骤203：根据所述目标姿态信息，计算控制磁体10的当前转动轨迹。
所述当前转动轨迹包括当前转动量，所述当前转动量包括第一角度差和第二角度差，可参图9所示，将姿态的调节分解为两个角度上的调整；
所述初始姿态包括与Z轴正向的第一夹角Cv，所述目标姿态信息包括与Z轴正向的第二夹角Tv，所述第二夹角与所述第一夹角差值为ΔCv＝Tv-Cv，所述第一角度差为该差值与第三修正因子的乘积，表达式为：ΔMv＝αvΔCv。
所述初始姿态包括在XY平面与Y轴正向的第三夹角Ch，所述目标姿态信息包括在XY平面与Y轴正向的第四夹角Th，所述第四夹角与所述第三夹角的差值为ΔCh＝eff_ang(Th-Ch)，所述第二角度差为该差值与第四修正因子的乘积，表达式为：ΔMh＝αhΔCh。
其中，引入等效角度变化函数使得角度始终按照劣弧路径调整，可以提高角度调整的效率。等效角度变化函数定义为：
所述第三修正因子αv和所述第四修正因子αh的取值范围均为0到1之间，表达式为0<αh,αv<1，为方便计算，可以设定αh＝αv，一般选取αh,v＝0.8即可稳定、高效地反馈调整胶囊201到目标位置信息。
步骤204：控制所述控制磁体10沿所述当前转动轨迹运动。
步骤205：若直至所述控制磁体10停止运动，所述实时姿态信息在所述目标姿态范围外，则重复步骤“根据所述目标姿态信息，计算目标姿态范围和控制磁体10的当前转动轨迹；控制所述控制磁体10沿所述当前转动轨迹运动”，直至所述实时姿态信息在所述目标姿态范围内，然后结束调整控制磁体10。
还包括步骤：
计算所述实时姿态信息计对应的第一单位向量；
计算所述目标姿态信息对应的第二单位向量；
计算所述第一单位向量与所述第二单位向量的实时角度差；
其中，单位球面投影矢量计算公式为：/>
将实时姿态信息和目标姿态信息代入上述公式可以计算第一单位向量和第二单位向量，实时角度差dist的计算公式如下：
当所述实时角度差小于等于所述预设角度差，也就是dist≤distth时，所述实时姿态信息在所述目标姿态范围内；
当所述实时角度差大于所述预设角度差，也就是dist>distth时，所述实时姿态信息在所述目标姿态范围外。
每次执行完一次胶囊201角度调整动作后，更新胶囊201的实时姿态信息[Ch,Cv]，根据上述的实时角度差计算实时姿态信息是否处于目标姿态范围内。若未满足条件，则继续重复执行胶囊201角度调整动作；若满足条件，则结束胶囊201角度调整任务，控制姿态的任务执行成功。
另外，与上述位置调整中的第一修正因子、第二修正因子原因相同的，0<αh,αv<1可以避免导致振荡现象，防止胶囊201运动过量，例如在图9中防止胶囊201运动越过P2点，避免出现一直修正不到位的问题。αh,αv相当于加入阻尼效应，可以确保振荡效果衰减，一定能收敛到目标，在运用本申请的方法的实验中，一般将上述步骤循环两次，即控制磁体10运动两次，即可将胶囊201移动到目标姿态。
通过上述的步骤，结合程序化的逻辑控制，将胶囊201控制路径转化为若干关键节点位置的目标状态控制，移动到关键节点后拍摄体内的照片，实现智能化程度较高的自动扫描检查功能。
该磁控胶囊系统1000的定量闭环控制方法能够定量地对胶囊201的运动过程进行控制，使胶囊201准确的到达目标位置和/或姿态，且通过定量闭环控制方法，精准控制胶囊201的运动量。且该方法运用与永磁体控制的胶囊201运动的场景下，避免了永磁体因速度、状态的切换不及时而操作慢的问题，控制过程的健壮性强，另外该方法可以实现复杂的控制逻辑，进行衍生的消化道检查应用，为磁控胶囊系统1000的自动化、智能化检查及衍生应用提供便利。
在一个实施例中，提供了一种磁控胶囊系统1000。该磁控胶囊系统1000可以包括第一获取模块、第一目标获取模块、第一计算模块、第一控制模块和第一循环模块，各模块具体功能如下：
第一获取模块，用于持续获取胶囊201的实时位置信息；
第一目标获取模块，用于获取胶囊201的目标位置信息，并根据所述目标位置信息，确定目标位置范围；
第一计算模块，用于根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹；
第一控制模块，用于控制所述控制磁体10沿所述当前移动轨迹运动；
第一循环模块，用于当所述控制磁体10停止运动，若所述实时位置信息在所述目标位置范围外，则重复使“第一计算模块根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹，第一控制模块控制所述控制磁体10沿所述当前移动轨迹运动”，直到所述实时位置信息在所述目标位置范围内。
在一个实施例中，本实施例的磁控胶囊系统1000还包括磁体位置获取模块，所述磁体位置获取模块用于持续获取所述控制磁体10的实时磁体位置信息，其中，所述实时磁体位置信息、所述实时位置信息和所述目标位置信息均位于同一世界坐标系。
在一个实施例中，本实施例的磁控胶囊系统1000还包括临界距离获取模块和状态计算模块，所述临界距离获取模块获取临界距离，其中，所述临界距离为胶囊201在液体中悬浮时的胶囊201与所述控制磁体10之间的距离；
所述状态计算模块根据所述实时磁体位置信息、所述实时位置信息和所述临界距离，计算胶囊201的状态信息，其中，所述状态信息包括沉底状态、液面悬浮状态和吸顶状态。
在一个实施例中，本实施例的磁控胶囊系统1000还包括高度调节模块和朝向调节模块，所述目标位置信息包括目标状态；若所述状态信息与目标状态不一致，所述高度调节模块调节所述控制磁体10的高度，直到胶囊201处于目标状态；
所述朝向调节模块调节所述控制磁体10的朝向，以使胶囊201的内窥镜朝向指定方向。
在一个实施例中，当所述目标状态为沉底状态时，高度调节模块控制胶囊201与所述控制磁体10之间的距离大于所述临界距离与第一阈值之和；
当所述目标状态为吸顶状态时，高度调节模块控制胶囊201与所述控制磁体10之间的距离小于所述临界距离与第二阈值之和。
在一个实施例中，第一控制模块控制将所述控制磁体10移动至初始位置，其中，于所述初始位置，所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。
在一个实施例中，本实施例的磁控胶囊系统1000还包括运动方式获取模块，获取所述控制磁体10的运动方式，所述运动方式包括平移方式和/或翻滚方式。
在一个实施例中，第一计算模块根据所述实时位置信息和所述目标位置信息，计算当前偏移量。
在一个实施例中，第一计算模块计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的X轴方向的第一差值，将所述第一差值与第一修正因子的乘积作为所述X轴差值；
第一计算模块计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的Y轴方向的第二差值，将所述第二差值与第二修正因子的乘积作为所述Y轴差值。
在一个实施例中，第一获取模块获取胶囊201的长轴周长，其中，所述长轴周长为胶囊201沿其长轴翻转一周对应的长度；
第一计算模块根据所述实时位置信息和所述目标位置信息，计算当前翻滚量，其中，所述当前翻滚量包括胶囊201的翻滚圈数和翻滚方向。
在一个实施例中，所述翻滚圈数为整数，每圈对应的所述控制磁体10的转动量为360°。
在一个实施例中，所述目标位置范围包括预设距离；第一计算模块计算所述实时位置信息与所述目标位置信息在所述世界坐标系的XY平面上的投影的距离。
在一个实施例中，当所述实时位置信息在所述目标位置范围内时，第一控制模块控制所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。
在一个实施例中，当所述实时磁体位置信息与所述实时位置信息对应的距离大于安全距离时，第一控制模块控制所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。
在一个实施例中，提供了一种磁控胶囊系统1000。该磁控胶囊系统1000可以包括第二获取模块、第二目标获取模块、第二计算模块、第二控制模块和第二循环模块，各模块具体功能如下：
第二获取模块，用于持续获取胶囊201的实时姿态信息；
第二目标获取模块，用于获取胶囊201的目标姿态信息；
第二计算模块，用于根据所述目标姿态信息，确定目标姿态范围和控制磁体10的当前转动轨迹；
第二控制模块，用于控制所述控制磁体10沿所述当前转动轨迹运动；
第二循环模块，用于当所述控制磁体10停止运动，若所述实时姿态信息在所述目标姿态范围外，则重复“第二计算模块根据所述目标姿态信息，计算目标姿态范围和控制磁体10的当前转动轨迹，第二控制模块控制所述控制磁体10沿所述当前转动轨迹运动”，直到所述实时姿态信息在所述目标姿态范围内。
在一个实施例中，磁控胶囊系统1000还包括磁体姿态获取模块，所述磁体姿态获取模块持续获取所述控制磁体10的实时磁体姿态信息，其中，所述实时磁体姿态信息、所述实时姿态信息和所述目标姿态信息均位于同一世界坐标系。
在一个实施例中，第二控制模块控制所述控制磁体10转动至初始姿态，其中，于所述初始姿态，所述控制磁体10的磁化方向N极的朝向平行或垂直于所述世界坐标系的Z轴。
在一个实施例中，第二计算模块计算所述实时姿态信息计对应的第一单位向量；第二计算模块计算所述目标姿态信息对应的第二单位向量；第二计算模块计算所述第一单位向量与所述第二单位向量的实时角度差。
所述磁控胶囊系统1000还可以包括计算机、笔记本、掌上电脑及云端服务器等计算设备。进一步可包括，但不仅限于，处理模块40、存储模块30。本领域技术人员可以理解，所述示意图仅仅是磁控胶囊系统1000的示例，并不构成对磁控胶囊系统1000终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述磁控胶囊系统1000还可以包括输入输出设备、网络接入设备、总线等。
需要说明的是，本发明实施例的磁控胶囊系统1000中未披露的细节，请参照本发明实施例的磁控胶囊系统1000的定量闭环控制方法中所披露的细节。
根据本发明的磁控胶囊系统1000能够定量地对胶囊201的运动过程进行控制，使胶囊201准确的到达目标位置和/或姿态，且通过闭环的定量闭环控制方法，精准控制胶囊201的运动量。且该磁控胶囊系统1000在永磁体控制的胶囊201运动的场景下的进步尤为突出，避免了永磁体因速度、状态的切换不及时而操作慢的问题，控制过程的健壮性强，另外该磁控胶囊系统1000可以实现复杂的控制逻辑，进行衍生的消化道检查应用，为磁控胶囊系统1000的自动化、智能化检查及衍生应用提供便利。
如图10所示，是本发明一实施例提供的磁控胶囊系统1000的模块示意图。磁控胶囊系统1000还包括上述的磁控系统100、胶囊定位系统200、控制磁体10和胶囊201、处理模块40、存储模块30、胶囊201内的各模块、以及存储在所述存储模块30中并可在所述处理模块40上运行的计算机程序，例如上述的定量闭环控制方法程序。所述处理模块40执行所述计算机程序时实现上述各个控制方法实施例中的步骤，例如图1和2所示的步骤。
控制磁体10的磁源通过伺服电机及传动机构控制驱动其运动至指定位置，通过磁控系统100的数据接口，获取伺服电机的传动数据，经过固定的比例转换公式，获取控制磁体10的位置、姿态角度状态原始数据。位置数据精确到1mm，角度数据精确到1度。
控制磁体10与传动机构固定连接，通过在一些位置的光电开关，如一些特殊垂直、水平角度标定零点，然后通过对驱动控制磁体10运动的伺服电机的驱动量的换算，可以精准地驱动控制磁体10在世界坐标系中运动到目标位置，无需通过现场测量控制磁体10磁场方向确定其姿态角度。
胶囊201可以包括磁场传感器、加速度传感器60、信号输送模块70、磁性件和摄像模块80，磁场传感器、加速度传感器60、磁性件如上文所述，可以通过内部三轴磁传感器50、三轴加速度传感器60、IMU传感器和外部多组磁定位设备协同工作，计算出胶囊201所在的位置和姿态，通过控制磁体10对磁性件的作用驱动胶囊201运动。信号输送模块70将信息传输至外界的处理模块40或服务器中，外界驱动无线胶囊201运动到指定位置后，摄像模块80拍摄人体400内的照片通过信号输出模块传输至外界，完成对体内的拍摄。
控制磁体10可以适当拉高以减弱对胶囊201的吸力，或者降低以增大对胶囊201的吸力，控制胶囊201在沉底、水面悬浮、吸顶等不同情境状态中切换。
磁控胶囊系统1000还可以包括信号传输模块20和通信总线90。信号传输模块20用于将数据发送至处理模块40或服务器，信号输送模块70和信号传输模块20可以通过无线连接的形式传输数据，如蓝牙、wifi、zigbee等，通信总线90用于将控制磁体10、信号传输模块20、处理模块40与存储模块30之间建立连接，通信总线90可包括一通路，在上述的控制磁体10、信号传输模块20、处理模块40与存储模块30之间传送信息。
另外，本发明还提出了一种电子设备，其包括存储模块30和处理模块40，处理模块40执行所述计算机程序时可实现上述的定量闭环控制方法中的步骤，也就是说，实现上述控制方法中的任意一个技术方案中的步骤。
该电子设备可以是集成于磁控胶囊系统1000内的一部分、或者是本地的终端设备、还可以是云端服务器的一部分。
所述处理模块40可以是中央处理单元或其他通用处理器、通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。所述处理模块40是所述磁控胶囊系统1000的控制中心，利用各种接口和线路连接整个磁控胶囊系统1000的各个部分。
所述存储模块30可用于存储所述计算机程序和/或模块，所述处理模块40通过运行或执行存储在所述存储模块30内的计算机程序和/或模块，以及调用存储在存储模块30内的数据，实现所述磁控胶囊系统1000的各种功能。
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储模块30中，并由所述处理模块40执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述磁控胶囊系统1000控制方法中的执行过程。
进一步地，本发明一实施例提供了一种可读存储介质，其存储有计算机程序，该计算机程序被处理模块40执行时可实现上述的磁控胶囊系统1000控制方法中的步骤，也就是说，实现上述磁控胶囊系统1000控制方法中的任意一个技术方案中的步骤。
所述磁控胶囊系统1000集成的模块如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。
应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施方式中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。
上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施方式或变更均应包含在本发明的保护范围之内。

Claims (20)

1.一种磁控胶囊系统的定量闭环控制方法，其特征在于，包括如下步骤：
持续获取胶囊的实时位置信息；
获取所述胶囊的目标位置信息；
根据所述目标位置信息，确定目标位置范围；
根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；
控制所述控制磁体沿所述当前移动轨迹运动；
若直至所述控制磁体停止运动，所述实时位置信息在所述目标位置范围外，则重复步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；控制所述控制磁体沿所述当前移动轨迹运动”，直至所述实时位置信息在所述目标位置范围内。
2.根据权利要求1所述的定量闭环控制方法，其特征在于，还包括步骤：
持续获取所述控制磁体的实时磁体位置信息，其中，所述实时磁体位置信息、所述实时位置信息和所述目标位置信息均位于同一世界坐标系。
3.根据权利要求2所述的定量闭环控制方法，其特征在于，还包括步骤：
获取临界距离，其中，所述临界距离为所述胶囊在液体中悬浮时的所述胶囊与所述控制磁体之间的距离；
根据所述实时磁体位置信息、所述实时位置信息和所述临界距离，计算所述胶囊的状态信息，其中，所述状态信息包括沉底状态、液面悬浮状态和吸顶状态。
4.根据权利要求3所述的定量闭环控制方法，其特征在于，其中，所述目标位置信息包括目标状态；
还包括步骤：
若所述状态信息与目标状态不一致，调节所述控制磁体的高度，直到所述胶囊处于目标状态；
调节所述控制磁体的朝向，以使所述胶囊的内窥镜朝向指定方向。
5.根据权利要求4所述的定量闭环控制方法，其特征在于，
当所述目标状态为沉底状态时，控制所述胶囊与所述控制磁体之间的距离大于所述临界距离与第一阈值之和；
当所述目标状态为吸顶状态时，控制所述胶囊与所述控制磁体之间的距离小于所述临界距离与第二阈值之和。
6.根据权利要求3所述的定量闭环控制方法，其特征在于，所述步骤“获取临界距离”包括：
根据临界距离算法或者根据测量数据获取所述临界距离；
其中，所述临界距离算法，是根据液面悬浮状态下，所述控制磁体和所述胶囊之间的吸力与所述胶囊受到的浮力之和等于所述胶囊的重力，计算所述控制磁体与所述胶囊之间的距离。
7.根据权利要求2所述的定量闭环控制方法，其特征在于，还包括步骤：
控制所述控制磁体移动至初始位置，其中，于所述初始位置，所述控制磁体与所述胶囊在垂直于所述世界坐标系的XY平面的同一条直线上。
8.根据权利要求2所述的定量闭环控制方法，其特征在于，还包括步骤：
获取所述控制磁体的运动方式，所述运动方式包括平移方式和/或翻滚方式；
当所述控制磁体的运动方式为平移方式、所述胶囊未运动时，切换所述控制磁体的运动方式为翻滚方式。
9.根据权利要求8所述的定量闭环控制方法，其特征在于，所述步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹”包括：
根据所述实时位置信息和所述目标位置信息，计算当前偏移量，其中，所述当前偏移量包括在所述世界坐标系中的X轴方向的X轴差值、以及在Y轴方向的Y轴差值；
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的X轴方向的第一差值，将所述第一差值与第一修正因子的乘积作为所述X轴差值；
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的Y轴方向的第二差值，将所述第二差值与第二修正因子的乘积作为所述Y轴差值；
若所述控制磁体的运动方式为平移方式，所述当前移动轨迹为沿所述当前偏移量平移。
10.根据权利要求8所述的定量闭环控制方法，其特征在于，所述步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹”包括：
获取所述胶囊的长轴周长，其中，所述长轴周长为所述胶囊沿其长轴翻转一周对应的长度；
根据所述实时位置信息和所述目标位置信息，计算当前翻滚量，其中，所述当前翻滚量包括所述胶囊的翻滚圈数和翻滚方向；
若所述控制磁体的运动方式包括翻滚方式，所述当前移动轨迹包括沿所述翻滚方向翻滚所述翻滚圈数。
11.根据权利要求2所述的定量闭环控制方法，其特征在于，其中，所述目标位置范围包括预设距离；
还包括步骤：
计算所述实时位置信息与所述目标位置信息在所述世界坐标系的XY平面上的投影的距离；
当所述距离小于等于所述预设距离，所述实时位置信息在所述目标位置范围内；
当所述距离大于所述预设距离，所述实时位置信息在所述目标位置范围外。
12.根据权利要求2所述的定量闭环控制方法，其特征在于，还包括步骤：
当所述实时位置信息在所述目标位置范围内时，或者，当所述实时磁体位置信息与所述实时位置信息对应的距离大于安全距离时；
控制所述控制磁体与所述胶囊在垂直于所述世界坐标系的XY平面的同一条直线上。
13.一种磁控胶囊系统的定量闭环控制方法，其特征在于，包括如下步骤：
持续获取胶囊的实时姿态信息；
获取胶囊的目标姿态信息；
根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹；
控制所述控制磁体沿所述当前转动轨迹运动；
若直至所述控制磁体停止运动，所述实时姿态信息在所述目标姿态范围外，则重复步骤“根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹；控制所述控制磁体沿所述当前转动轨迹运动”，直至所述实时姿态信息在所述目标姿态范围内。
14.根据权利要求13所述的定量闭环控制方法，其特征在于，持续获取所述控制磁体的实时磁体姿态信息，其中，所述实时磁体姿态信息、所述实时姿态信息和所述目标姿态信息均位于同一世界坐标系。
15.根据权利要求14所述的定量闭环控制方法，其特征在于，其中，所述当前转动轨迹包括当前转动量，所述当前转动量包括第一角度差和第二角度差；
控制所述控制磁体10转动至初始姿态；
所述初始姿态包括与Z轴正向的第一夹角，所述目标姿态信息包括与Z轴正向的第二夹角，所述第一角度差为所述第二夹角与所述第一夹角差值与第三修正因子的乘积；
所述初始姿态包括在XY平面与Y轴正向的第三夹角，所述目标姿态信息包括在XY平面与Y轴正向的第四夹角，所述第二角度差为所述第四夹角与所述第三夹角的差值与第四修正因子的乘积。
16.根据权利要求14所述的定量闭环控制方法，其特征在于，其中，所述目标姿态范围包括预设角度差；
还包括步骤：
计算所述实时姿态信息计对应的第一单位向量；
计算所述目标姿态信息对应的第二单位向量；
计算所述第一单位向量与所述第二单位向量的实时角度差；
当所述实时角度差小于等于所述预设角度差，所述实时姿态信息在所述目标姿态范围内；
当所述实时角度差大于所述预设角度差，所述实时姿态信息在所述目标姿态范围外。
17.一种磁控胶囊系统，其特征在于，包括：
第一获取模块，用于持续获取胶囊的实时位置信息；
第一目标获取模块，用于获取所述胶囊的目标位置信息，并根据所述目标位置信息，确定目标位置范围；
第一计算模块，用于根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；
第一控制模块，用于控制所述控制磁体沿所述当前移动轨迹运动；
第一循环模块，用于当所述控制磁体停止运动，若所述实时位置信息在所述目标位置范围外，则重复使“第一计算模块根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹，第一控制模块控制所述控制磁体沿所述当前移动轨迹运动”，直到所述实时位置信息在所述目标位置范围内。
18.一种磁控胶囊系统，其特征在于，包括：
第二获取模块，用于持续获取胶囊的实时姿态信息；
第二目标获取模块，用于获取胶囊的目标姿态信息；
第二计算模块，用于根据所述目标姿态信息，确定目标姿态范围和控制磁体的当前转动轨迹；
第二控制模块，用于控制所述控制磁体沿所述当前转动轨迹运动；
第二循环模块，用于当所述控制磁体停止运动，若所述实时姿态信息在所述目标姿态范围外，则重复“第二计算模块根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹，第二控制模块控制所述控制磁体沿所述当前转动轨迹运动”，直到所述实时姿态信息在所述目标姿态范围内。
19.一种电子设备，其特征在于，包括：
存储模块，存储计算机程序；
处理模块，执行所述计算机程序时可实现权利要求1至16中任意一项所述的磁控胶囊系统的定量闭环控制方法中的步骤。
20.一种可读存储介质，其存储有计算机程序，其特征在于，该计算机程序被处理模块执行时可实现权利要求1至16中任意一项所述的磁控胶囊系统的定量闭环控制方法中的步骤。
CN202210272489.2A 2022-03-18 2022-03-18 磁控胶囊系统及其定量闭环控制方法 Pending CN116803330A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202210272489.2A CN116803330A (zh) 2022-03-18 2022-03-18 磁控胶囊系统及其定量闭环控制方法
PCT/CN2023/082318 WO2023174427A1 (zh) 2022-03-18 2023-03-17 磁控胶囊系统及其定量闭环控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210272489.2A CN116803330A (zh) 2022-03-18 2022-03-18 磁控胶囊系统及其定量闭环控制方法

Publications (1)

Publication Number Publication Date
CN116803330A true CN116803330A (zh) 2023-09-26

Family

ID=88022430

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210272489.2A Pending CN116803330A (zh) 2022-03-18 2022-03-18 磁控胶囊系统及其定量闭环控制方法

Country Status (2)

Country Link
CN (1) CN116803330A (zh)
WO (1) WO2023174427A1 (zh)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3170442A4 (en) * 2015-01-06 2018-04-04 Olympus Corporation Guiding apparatus and capsulated medical apparatus guiding system
WO2016167042A1 (ja) * 2015-04-17 2016-10-20 オリンパス株式会社 カプセル型医療装置誘導システム
CN109044250A (zh) * 2018-08-28 2018-12-21 深圳市资福医疗技术有限公司 一种胶囊内窥镜运动控制方法、装置及终端设备
CN110236474B (zh) * 2019-06-04 2020-10-27 北京理工大学 一种主动式磁控胶囊机器人检测系统及检测方法
CN110575118B (zh) * 2019-09-11 2022-01-11 安翰科技(武汉)股份有限公司 胶囊内窥镜控制系统及电子设备
CN111035349B (zh) * 2020-03-11 2020-07-17 上海安翰医疗技术有限公司 胶囊内窥镜的姿态定位方法及胶囊内窥镜系统

Also Published As

Publication number Publication date
WO2023174427A1 (zh) 2023-09-21

Similar Documents

Publication Publication Date Title
CN109658460A (zh) 一种机械臂末端相机手眼标定方法和系统
US10240714B2 (en) Stabilizer and angle adjusting method thereof
CN110099600B (zh) 一种用于操控胶囊装置的系统和方法
WO2019223271A1 (zh) 飞行器偏航角修正方法、装置及飞行器
CN108436909A (zh) 一种基于ros的相机和机器人的手眼标定方法
JP2011147785A (ja) 磁気駆動部を含むカプセル型内視鏡
US11612308B2 (en) Control method, control system, electronic device and readable storage medium for capsule endoscope
US10750935B2 (en) Guidance device and capsule medical device guidance system
CN105045293B (zh) 云台控制方法、外部载体控制方法及云台
CN113741554A (zh) 云台及云台控制方法
US20150148594A1 (en) Endoscope operation system
US8641603B2 (en) Coil system for the contact-free magnetic navigation of a magnetic body in a working space
CN107925713A (zh) 摄像系统及摄像控制方法
CN110785601A (zh) 手持云台的控制方法和手持云台
CN108375983A (zh) 一种遥控装置、控制系统以及控制方法
CN110431507A (zh) 一种云台控制方法及云台
US11826015B2 (en) System and method for controlling capsule endoscope
CN109250004A (zh) 一种全景信息采集滚动机器人
CN106691366A (zh) 一种可控胶囊内镜的控制方法
CN105559738A (zh) 一种基于磁场空间分布变化的胶囊内镜运动控制方法
CN110811497A (zh) 一种胶囊内窥镜的控制方法及应用其的胶囊内窥镜
CN116803330A (zh) 磁控胶囊系统及其定量闭环控制方法
CN107105981B (zh) 胶囊型医疗装置引导系统
WO2023174425A1 (zh) 磁控胶囊系统及其位姿标定表示方法
CN207306187U (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