CN110327011A - 一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法 - Google Patents
一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法 Download PDFInfo
- Publication number
- CN110327011A CN110327011A CN201910559960.4A CN201910559960A CN110327011A CN 110327011 A CN110327011 A CN 110327011A CN 201910559960 A CN201910559960 A CN 201910559960A CN 110327011 A CN110327011 A CN 110327011A
- Authority
- CN
- China
- Prior art keywords
- video camera
- point
- double
- camera
- angle
- 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.)
- Granted
Links
- 239000002775 capsule Substances 0.000 title claims abstract description 159
- 238000000034 method Methods 0.000 title claims abstract description 65
- 230000036544 posture Effects 0.000 claims abstract description 86
- 239000011159 matrix material Substances 0.000 claims abstract description 47
- 210000001035 gastrointestinal tract Anatomy 0.000 claims abstract description 16
- 230000003287 optical effect Effects 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 13
- 238000001514 detection method Methods 0.000 claims description 10
- 210000002784 stomach Anatomy 0.000 claims description 10
- 238000003384 imaging method Methods 0.000 claims description 9
- 230000005284 excitation Effects 0.000 claims description 8
- 210000000936 intestine Anatomy 0.000 claims description 8
- 238000013519 translation Methods 0.000 claims description 8
- 230000008859 change Effects 0.000 claims description 7
- 239000003292 glue Substances 0.000 claims description 4
- 230000006698 induction Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 4
- 230000003321 amplification Effects 0.000 claims description 3
- 238000004458 analytical method Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000003199 nucleic acid amplification method Methods 0.000 claims description 3
- 230000009467 reduction Effects 0.000 claims description 3
- 230000004044 response Effects 0.000 claims description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 2
- 230000001154 acute effect Effects 0.000 claims description 2
- 230000002496 gastric effect Effects 0.000 claims description 2
- 230000000155 isotopic effect Effects 0.000 claims description 2
- 239000007787 solid Substances 0.000 claims description 2
- 230000014616 translation Effects 0.000 claims 5
- 238000004364 calculation method Methods 0.000 claims 1
- 230000000694 effects Effects 0.000 description 9
- 230000003902 lesion Effects 0.000 description 7
- 230000005540 biological transmission Effects 0.000 description 6
- 238000003745 diagnosis Methods 0.000 description 5
- QJVKUMXDEUEQLH-UHFFFAOYSA-N [B].[Fe].[Nd] Chemical compound [B].[Fe].[Nd] QJVKUMXDEUEQLH-UHFFFAOYSA-N 0.000 description 3
- 238000013475 authorization Methods 0.000 description 3
- 210000001072 colon Anatomy 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 230000005415 magnetization Effects 0.000 description 3
- 229910001172 neodymium magnet Inorganic materials 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000005355 Hall effect Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 206010011409 Cross infection Diseases 0.000 description 1
- 241000167880 Hirundinidae Species 0.000 description 1
- 206010029803 Nosocomial infection Diseases 0.000 description 1
- 210000000683 abdominal cavity Anatomy 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 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
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 239000003814 drug Substances 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000005611 electricity Effects 0.000 description 1
- 238000001839 endoscopy Methods 0.000 description 1
- 230000005176 gastrointestinal motility Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000008855 peristalsis Effects 0.000 description 1
- 230000035479 physiological effects, processes and functions Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 210000000813 small intestine Anatomy 0.000 description 1
- 238000002626 targeted therapy Methods 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/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/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/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
- A61B5/00—Measuring for diagnostic purposes; Identification of persons
- A61B5/06—Devices, other than using radiation, for detecting or locating foreign bodies ; determining position of probes within or on the body of the patient
- A61B5/061—Determining position of a probe within the body employing means separate from the probe, e.g. sensing internal probe position employing impedance electrodes on the surface of the body
- A61B5/062—Determining position of a probe within the body employing means separate from the probe, e.g. sensing internal probe position employing impedance electrodes on the surface of the body using magnetic field
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B5/00—Measuring for diagnostic purposes; Identification of persons
- A61B5/06—Devices, other than using radiation, for detecting or locating foreign bodies ; determining position of probes within or on the body of the patient
- A61B5/065—Determining position of the probe employing exclusively positioning means located on or in the probe, e.g. using position sensors arranged on the probe
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B5/00—Measuring for diagnostic purposes; Identification of persons
- A61B5/06—Devices, other than using radiation, for detecting or locating foreign bodies ; determining position of probes within or on the body of the patient
- A61B5/065—Determining position of the probe employing exclusively positioning means located on or in the probe, e.g. using position sensors arranged on the probe
- A61B5/066—Superposing sensor position on an image of the patient, e.g. obtained by ultrasound or x-ray imaging
Landscapes
- Health & Medical Sciences (AREA)
- Life Sciences & Earth Sciences (AREA)
- Surgery (AREA)
- Engineering & Computer Science (AREA)
- General Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Pathology (AREA)
- Veterinary Medicine (AREA)
- Public Health (AREA)
- Biophysics (AREA)
- Biomedical Technology (AREA)
- Heart & Thoracic Surgery (AREA)
- Medical Informatics (AREA)
- Physics & Mathematics (AREA)
- Animal Behavior & Ethology (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Radiology & Medical Imaging (AREA)
- Optics & Photonics (AREA)
- Human Computer Interaction (AREA)
- Gynecology & Obstetrics (AREA)
- Endoscopes (AREA)
Abstract
一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法，属于自动化工程技术领域。该方法利用空间万向旋转磁矢量的误差分布特性，首先确定磁矢量最小方位误差方向为双半球胶囊机器人姿态的初始图像标定位置，通过摄像机获取调姿前后的图像信息，对图像进行特征点的识别和匹配，再结合对极几何和旋转矩阵求出调姿前后的相对姿态变化，实现了对双半球胶囊机器人的实际姿态进行有效的估计。本发明无需任何外部硬件资源，仅通过机器人自身携带的摄像机，即可实现双半球胶囊机器人在胃肠道的宽裕环境中进行姿态估计。
Description
技术领域
本发明属于自动化工程技术领域，涉及一种以空间万向旋转磁矢量误差模型确定的最小误差方位为双半球胶囊机器人的初始图像标定位置，根据内嵌无线摄像机所获得的调姿后与初始标定两幅图像信息的特征点识别与匹配，并结合对极几何约束，在胃肠道和结肠等宽裕环境内获得双半球胶囊机器人轴线方位的姿态检测方法。
背景技术
在胃肠道内窥镜检测领域，由于传统的内窥镜检测方式具有局限性，不能检测生理环境复杂的小肠等区域，并且检测过程会对病人带来很大的痛苦，重复使用也会在一定程度上引发交叉感染。随着科学技术的发展，胶囊内窥镜机器人的研究填补了这一领域的空白。胶囊内窥镜机器人主要是一个带有摄像机和相关硬件设施的胶囊型结构装置，方便患者吞咽，由胶囊内窥镜内嵌摄像机所得到的胃肠道等区域的内部图像信息对医生诊断起到很好的辅助作用，如结合相应的图像处理与特征识别技术，能迅速判断出病变位置，同时，胶囊内窥镜诊断方式减轻了患者的痛苦。
胶囊机器人工作环境为生理结构复杂的人体胃肠道，目前以色列Given Imaging公司生产的Pillcam系列胶囊机器人和重庆金山公司生产的OMOM胶囊机器人已投入临床应用，它们属于靠胃肠道蠕动方式运动的被动型的胶囊内窥镜机器人，被动型胶囊机器人的位置和姿态不可控，检测时间多达7-8个小时。
上述被动型胶囊机器人存在以下问题：(1)检测时间长。需要借助肠道蠕动前进，不能迅速到达相应的病变区域，而且，容易错过病变区域导致漏检。(2)占用图像内存资源大。被动型胶囊机器人在遍历人体胃肠道区域时在不断地拍摄图像，整个过程中保存的图像多达上万幅，这对病变区域的寻找造成了很大的困难。
为了增加对胶囊机器人的操控性，主动型胶囊机器人成为研究的热点。现阶段的驱动方式主要可以分为电能驱动和磁能驱动。Rossella Fontana等人提出一种新型的球形内窥镜机器人，采用电能驱动的方式。Yingke Gu等人也提出一种配置六个摄像机的微球内窥镜***，并且在球形机器人中加入三轴MEMS加速度计和小型磁力计，对球形机器人的姿态进行实时估计。然而由于采用电能控制，电池电量有限，不能满足人体腹腔的全程检查；并且在球形机器人中加入过多的硬件设施，在一定程度上增大了球形机器人的尺寸，消耗了过多的硬件资源。
为了实现胶囊机器人在胃肠道中的主动控制，我们课题组在获得的国家发明专利“体内医疗微型机器人万向旋转磁场驱动控制方法”(专利授权号：ZL 200810011110.2)和“空间万向叠加旋转磁场旋转轴线方位与旋向的控制方法”(专利授权号：ZL201210039753.4)中，实现了通过三轴正交亥姆霍兹线圈的数字化控制，通过向三组线圈中分别通入计算后的电流，可以得到空间万向旋转磁场。为了实现胶囊机器人在胃部和结肠等宽裕环境内的驱动，本课题组研制了双半球胶囊机器人，并获得国家发明专利“一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法”(专利号：CN201510262778.4)，扩展了胶囊机器人的工作环境的范围。
专利“一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法”(专利号：CN201510262778.4)中提出的一种双半球胶囊机器人的总体结构为：双半球形胶囊机器人结构包括主动半球体和被动半球体两部分，将径向磁化钕铁硼圆环内驱动器7和摄像机与图像传输装置8过盈装配，将阶梯轴6也与摄像机与图像传输装置8过盈装配，最后将摄像机与图像传输装置8组件再与主动半球壳1过盈配合构成主动半球体；轴承定位套筒3与被动半球壳2过盈配合构成被动半球体，主动半球体和被动半球体由轴承4悬浮连接的过程如下：将轴承4安装在主动半球体组件的阶梯轴6上，再将主动半球体组件阶梯轴6上的轴承4一并装入轴承定位套筒3中，轴承定位套筒3内部有一台阶实现轴承4外圈轴向定位，圆螺母5装入阶梯轴6上将轴承4内圈轴向定位，圆螺母5不能突出到球面以外，以防止主动半球转动过程中带动圆螺母5与肠道接触影响姿态调整。双半球胶囊机器人的工作原理为：旋转磁场与径向磁化钕铁硼圆环内驱动器7的耦合磁矩带动包括摄像机与图像传输装置8的主动半球体绕被动半球体相对空转，主动半球体处于驱动状态，被动半球体在配重作用下始终处于下方，处于欠驱动状态，与胃肠道接触的摩擦力约束下的被动半球体处于静止状态，可防止双半球胶囊机器人调姿时发生滚动，主动半球处于上方不与胃肠道接触或者与胃肠道接触区域较小，主动半球体相对位于下面静止的被动半球体空转，摄像机与图像传输装置8中的摄像机光轴与双半球形胶囊机器人轴线一致，调整姿态时在胃肠道接触面上方施加旋转磁矢量，主动半球体相对被动半球体空转，随动效应使双半球胶囊机器人轴线一直追随相应方位角旋转磁场的轴线实现胃肠道内的姿态任意调整。其主动模态实现滚动行走；被动模态实现定点“悬停”调姿。定点“悬停”调姿是实现胶囊姿态准确控制的唯一手段。双半球胶囊机器人定点“悬停”调姿与双重工作模式转换技术的突破为肠道三维宽裕环境内的诊断、取样、定点靶向治疗等临床应用奠定了基础。
尽管一种主被动双半球胶囊机器人结构可实现主被动模态运动控制与转换，但没考虑被动模态下双半球胶囊机器人轴线实际姿态的控制误差问题。姿态控制误差主要涉及两方面原因：(1)万向旋转磁矢量的驱动过程是，调整DSP控制***输入的电流幅值和电压相位控制参数实现三相交变电流的数字化控制，并驱动三轴正交亥姆霍兹线圈产生万向旋转磁矢量。但当线圈中通入交流电时，由于电感的影响会使施加的电流与电压产生一个相位差，由于相位差的存在使得理想旋转磁矢量和实际旋转磁矢量之间存在方位误差。由于双半球胶囊机器人在随动效应作用下会追随磁矢量轴线，因此，磁矢量方位误差会直接影响双半球胶囊机器人的实际姿态；(2)双半球胶囊机器人轴线姿态的调整误差。在磁场方位改变时，双半球胶囊机器人轴线与磁场随动过程中，由于双半球胶囊机器人存在质量偏心，而且，双半球胶囊机器人在胃肠道不同环境下的阻尼系数也不同，因此，双半球胶囊机器人轴线随动下的动态响应的稳定特性会影响它的轴线方位，产生轴线方位误差。
上述原因对双半球胶囊机器人轴线实际姿态产生很大的影响。由于磁驱动控制***是一种开环***，不能实现对双半球胶囊机器人的实际姿态的反馈控制，原因是双半球胶囊机器人的实际姿态还不能实现在线检测。因此，拟申请专利提出一种双半球胶囊机器人轴线姿态的视觉检测方法，以便为实现双半球胶囊机器人轴线姿态的反馈控制，提高轴线方位的控制精度奠定基础。在医学工程领域，准确获取双半球胶囊机器人的姿态信息，对病变区域的准确定位与诊疗非常重要。
现阶段对胶囊机器人的姿态估计一般采用以下三种方法：
(1)采用二维霍尔效应传感器阵列的方法。Natali等人引入了一种定位方法，将测量的传感数据与外部磁场的预先计算的数据进行比较，该方法需要***内的多个磁传感器和惯性传感器。Donghoon Son课题组使用外部霍尔效应传感器阵列和外部电磁铁可变方向磁力驱动实现了胶囊内窥镜实时5-D定位，此定位方法的关键点是将胶囊机器人内部永磁体的磁场与致动器的磁场分开。然而这种方法使用过多的硬件资源，在胶囊机器人中还需加入额外的永磁体或传感器，减少了在有限空间中的资源利用。并且由于本课题组采用的驱动方式是磁驱动，主要依靠三轴亥氏线圈产生的旋转磁场带动胶囊机器人中的永磁体进行旋转驱动，线圈内部的磁场变化复杂，采用磁传感器阵列的方法不能有效区分旋转磁场和永磁体自身的磁场，从而直接影响姿态估计效果。
(2)基于视觉的胶囊机器人运动估计方法。在胶囊机器人研究领域，可以通过摄像机拍摄的相邻帧之间的图像，对其进行特征匹配，可以得到二者之间的位移和旋转变化，并能进行测距。Worcester Polytechnic Institute的Guanqun Bao等人提出通过胶囊机器人所获取的图像信息，对胶囊机器人的速度进行估计，得到了93％的准确度。Khan Wahid等人通过采用ASIFT和RANSAC结合的方法，对胶囊机器人进行位移估计，并且分析了胶囊机器人的运动方向，准确度也高达93.7％。然而采用视觉获得的姿态信息主要是胶囊机器人在肠道等窄域环境中的运动情况，分别为绕自身轴线的旋转和沿着肠道轴线方向的位移，并不能得到胶囊机器人在宽裕环境中的俯仰角、侧摆角等更复杂的姿态信息，具有一定的局限性，同时对识别病变区域也造成了很大的障碍。
目前，尚未有人提出利用万向旋转磁场的均匀性和双半球胶囊机器人在磁场内的随动性，在双半球胶囊机器人内无须加装任何传感器或其它装置的条件下，通过磁矢量误差模型和内嵌摄像机获取的图像特征来确定双半球胶囊机器人在宽裕环境中的俯仰角、侧摆角等复杂姿态信息的检测方法。该方法的显著优点是不需要在双半球胶囊机器人内加装其它装置，减少了硬件资源的消耗，在一定程度上增加了双半球胶囊机器人的空间利用率，并且这种方法结合本课题组的双半球胶囊机器人的结构特征与随动效应，能够实现在胃部等宽裕环境中实现对双半球胶囊机器人自身俯仰角和侧摆角的估计。
发明内容
本发明提供一种以空间万向旋转磁矢量误差模型确定的最小误差方位为双半球胶囊机器人的初始图像标定位置，根据内嵌无线摄像机所获得的调姿后与初始标定两幅图像信息的特征点识别与匹配，并结合对极几何约束，在胃肠道和结肠等宽裕环境内获得双半球胶囊机器人轴线的俯仰角和侧摆角的姿态信息。该方法的优点是不需要在双半球胶囊机器人内加装其它装置，减少了硬件资源的消耗，在一定程度上增加了双半球胶囊机器人的空间利用率，该方法结合双半球胶囊机器人的结构特征与磁场随动效应原理，能够在胃部等宽裕环境中实现对双半球胶囊机器人自身俯仰角和侧摆角的估计，为实现双半球胶囊机器人轴线姿态的反馈控制奠定了基础，可望实现病变区域的准确定位与诊疗。
本发明的技术方案为：
一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法，步骤如下：
第一步：首先标定双半球胶囊机器人的初始姿态位置，以此位置为基准，才能确保获得的其他姿态评估的有效性与准确性。初始姿态位置的确定涉及空间万向旋转磁矢量误差模型分析，找到在双半球胶囊机器人工作状态下的磁场方位误差最小的位姿方位。
技术方案第一步中，所述的寻找双半球胶囊机器人初始姿态位置的确定方法为：首先对空间万向磁场进行误差模型的建立。采用专利“一种空间万向旋转磁场人机交互控制方法”(专利授权号：ZL201610009285.4)中提出的降维后的旋转磁矢量控制模型。设O-XYZ为与三轴正交亥姆霍兹线圈固定的坐标系，旋转磁矢量所在旋转平面为x2oz2，其中轴x2在固定坐标系XOY平面内，那么旋转磁矢量B的法向量在旋转坐标系O-x2y2z2中可以表达为n2＝(0,1,0)，在固定坐标系O-XYZ中，定义旋转磁矢量B的法向量在XOY平面内的投影与Y轴的夹角α为侧摆角，在XOY平面内的投影与y2轴的夹角β为俯仰角，经过旋转变换后，旋转磁矢量B的法向量经过正交变换得到其在固定坐标系O-XYZ内的表达，为n＝[sinαcosβ,cosαcosβ,sinβ]。
其中，正交变换矩阵为：
由于产生的旋转磁场形成圆极化时才能保证双半球胶囊机器人正常平稳运作，故旋转磁矢量B在旋转坐标系中的表达式应为：
Box2y2z2＝[B0 sinωt 0 B0 cosωt]T (2)
其中B0为产生旋转磁场的磁感应强度，B0＝I0·Kz，ω为磁场旋转的角速度，其方向为逆时针；I0为输入电流的幅值，为Kz大线圈的结构参数。通过坐标变换即可推得旋转磁矢量B在固定坐标系O-XYZ中的表达形式为：
其中，
根据毕奥萨法尔定律，单轴线圈中心的磁感应强度与电流之间的关系如下：
Bi＝Ki·Ii (4)
公式(4)中，i＝x,y,z，Kx、Ky、Kz分别为小线圈、中间线圈和大线圈的结构参数，Ix、Iy、Iz分别为小线圈、中间线圈和大线圈中施加的交变电流；
通过上式得到三组参数不同的线圈匹配后的电流输入公式：
在调姿过程中，我们需要确定双半球胶囊机器人的初始位置，以此位置为基准进行旋转磁场方位的改变。根据降维后磁场的误差分析，课题组得出由于电流和电压相位差引起方位误差后的实际旋转磁矢量法向量的表达式：
分别为x轴、y轴、z轴亥姆霍兹线圈的电流电压相位差。
当旋转磁矢量的法向量是竖直向上的方向时，此时在经纬坐标系中β值为π/2，α为任意值。将β＝π/2带入上式实际旋转磁矢量的法线方向表达式中得：
又因为故那么B′的表达式为：
由此可以得出，当双半球胶囊机器人处于竖直位置处时，尽管实际旋转磁矢量B′的大小随相位差变化，但实际旋转磁矢量B′与理论旋转磁矢量B法向量之间没有方位误差。
并且通过上述通入磁场中的电流公式可知，在竖直方向上亥姆霍兹线圈中实际通入的电流为：
由公式(10)可知，三组线圈中只有x和y两组线圈中通入的交流电，z轴线圈未通电。
通过以上分析我们将竖直向上的方向作为双半球胶囊机器人被动模态下标定的初始位置，原因有如下两点：
(1)此位置处的方位误差为零，避免由于通入电流和电压的相位差带来的影响：
(2)此位置处双半球胶囊机器人的质量偏心力矩不起作用，随动效应下对双半球胶囊机器人的姿态影响最小。
第二步：建立双半球胶囊机器人和摄像机的坐标系，确定绝对坐标系和相对坐标系。
技术方案第二步中，确定相应坐标系的具体方法为：将双半球胶囊机器人坐标系建立在双半球胶囊机器人与胃肠壁的接触处，摄像机坐标系建立在双半球胶囊机器人顶部，以摄像机的光心作为其原点，其中Ow-XwYwZw为通过矢量运算的可平移性建立的双半球胶囊机器人固定坐标系，Ow为双半球胶囊机器人坐标系的原点，即双半球胶囊机器人与胃壁的接触点处，Oc-XcYcZc为双半球胶囊机器人上的摄像机坐标系，其中Oc为摄像机坐标系的光心位置处，其中摄像机坐标系相对于双半球胶囊机器人坐标系仅存在一个确定距离的平移。当被动模态下双半球胶囊机器人进行调姿的过程，由于其随动效应的产生主要依靠双半球胶囊机器人内部的永磁体，故在调姿的过程中，由于胃肠内部物理环境，双半球胶囊机器人是以自身球心M进行转动，并不发生滚动，实现定点调姿，双半球胶囊机器人与胃肠道的接触点依然是胃肠内壁上的Ow点，并且由于在调姿过程中，主动半球空转，故此时与被动半球固连的摄像机不存在绕其自身轴线的自转，摄像机在这个过程中既有平移运动，又有在俯仰角和侧摆角方向，以M点为旋转中心的旋转运动。结构上，摄像机轴线即为双半球胶囊机器人的轴线，由于摄像机与双半球胶囊机器人的被动半球固结在一起，被动模态下双半球胶囊机器人进行调姿时，摄像机不随主动半球旋转，因此，摄像机轴线的姿态即为双半球胶囊机器人的轴线的姿态，即通过摄像机拍摄的图像估计出的摄像机姿态就是双半球胶囊机器人的姿态。由于摄像机内部并没有陀螺仪，所以只根据摄像机拍摄的一副图像不能确定双半球胶囊机器人轴线的自转角度，也不能确定侧摆与俯仰角度，在进行侧摆与俯仰姿态估计前，需要先校正摄像机的自转角，根据已申请国家发明专利“一种双半球型胶囊机器人弯曲肠道内视觉导航方法”(申请号：201910056648.3)中提出的方法：对图像进行校正，驱动磁场仅改变双半球胶囊机器人侧摆角姿态，对图像中的特征点进行识别与匹配，特征点之间的匹配连线与图像水平线之间的夹角即为自转角，获得自转角信息后对图像在自转方向进行校正
第三步：获得初始姿态处的图像信息和进行调姿过程后的图像信息，在图像层面上进行预处理操作，对二者进行特征点的识别和匹配。
技术方案第三步中，所述的图像间的特征点识别和匹配，具体确定方法为：由于双半球胶囊机器人与其携带摄像机的自身特性，在工作过程中，调姿前后获得的图像之间会存在公共区域，可以进行图像特征的识别和匹配，在胃肠环境中的特征主要是内壁褶皱和凸起，并且在姿态估计过程中要求一定程度的实时性，结合这两点要求，采用ORB(OrientedFAST and Rotated BRIEF)，此方法是结合FAST算法和BRIEF描述子，既能识别有效的特征点，在响应速度上也能达到要求。在特征匹配时，采用BFMatch(BruteForceMatcher)的方法，逐个匹配特征点直到找到最佳匹配，并通过设置一定的距离阈值消除误匹配的特征点，保证特征点匹配的正确性。
第四步：通过对极几何的相关知识和第二步相互匹配的特征点，获得调姿前后的旋转矩阵。再通过反求旋转矩阵获得双半球胶囊机器人的俯仰角和侧摆角。
技术方案第四步中，通过对极几何求得旋转矩阵的方法具体为：
首先需要对摄像机进行标定，以获得摄像机的内部参数。二维图像与三维空间点之间的关系可以用下面的式子表示：
λ·[x y 1]＝[X Y Z]·P (11)
λ是放大因子，(x,y)是图像物理坐标系O1-xy中的一点，(X,Y,Z)是空间坐标系O-XYZ中的一点，P是摄像机的参数矩阵，P可以表示为：
P＝[Rc t]TK (12)
Rc为摄像机姿态变换前后的旋转矩阵，t为摄像机姿态变换前后的平移矩阵，由Rc和t组成了摄像机的姿态变换矩阵，K是摄像机的内部参数矩阵：
fx和fy分别表示摄像机在x和y方向上的焦距，cx和cy是摄像机光心在图像像素坐标系O′-uv中的位置。
根据多视图几何的知识，可以求出摄像机变换前后的相对姿态变化。设C1和C2分别是摄像机变换前后的光心位置，Q是三维空间中一个实际的点，q1和q2分别是点Q在两个摄像机成像面上的投影，在几何上表示为摄像机光心C1和C2与空间点Q分别在两个成像平面上的交点；e1和e2分别是两摄像机的中心与像平面的交点，π是对极平面；对极几何描述的是左右两幅图像与以C1 C2为轴的平面束的几何约束。
对极几何中一个重要的约束是空间点Q、摄像机光心C1和C2三点在一个平面π上，対极约束中同时包含了平移和旋转，以此可以得到两个匹配点q1和q2的空间位置关系：
E＝t×Rc (15)
w1＝K-1q1 w2＝K-1q2 (16)
其中E为本质矩阵，而w1和w2是像素归一化之后的坐标。由于对极约束是等式为零的约束，所以对E乘以任意非零常数后，对极约束依然满足，故E在不同尺度下是等价的。由公式(15)可知，摄像机的姿态变化矩阵t和Rc能够对E进行SVD分解获得，由于平移和旋转各有三个自由度，共有六个自由度。但由于尺度等价性，E实际上有五个自由度。但是，E的内在性质是一种非线性性质，在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用八对点来估计E，八点法只利用了E的线性性质，因此可以在线性代数框架下求解。
在求解公式(14)中的E值时，需要K，q1和q2这些已知条件，其中K是摄像机的内部固有参数，可以通过相机标定获得K，q1和q2是通过特征点识别与匹配到的多个特征点对。由于匹配到的特征点的对数会超过矩阵E的自由度，并且不可避免的会存在误匹配，所以在求解本质矩阵E时，采用随机采样一致性(RANSAC)来进行求解的优化，进而提高计算E结果的准确性。根据上式(15)可知，摄像机的姿态变化矩阵t和R可以对E进行SVD(Singular valuedecomposition)分解获得。
所述的BFMatch方法，即对每一个特征点，与其他所有的特征点测量描述子的距离，然后排序，取最近的一个作为匹配点；描述子距离表示了两个特征之间的相似程度，使用汉明距离做为度量，即两个二进制串之间的汉明距离，指的是它们不同位数的个数；当描述子之间的距离大于两倍的最小距离时，即认为匹配有误。
其中旋转矩阵Rc的一般形式为：
根据磁矢量误差模型所标定的初始位置为竖直向上的方向，此位置用方向余弦表示为s1＝(0,0,1)T，则调姿之后的方向余弦为：
s2＝Rc·s1 (18)
其中s2为调姿之后双半球胶囊机器人轴线的方向余弦。将结果转换为侧摆角和俯仰角的表达形式，对旋转矩阵中的各项进行相互运算并进行反三角函数运算可得到侧摆角α，俯仰角β的值，具体求解的方法如下：
侧摆角α的两个可能值为：
α2＝π-α1 (21)
俯仰角β的两个可能值为：
β1＝arccos(R33) (22)
β2＝π-β1 (23)
其中俯仰角或侧摆角会存在两种情况，侧摆角的两个可能值α1和α2互为补角，俯仰角的两个可能值β1和β2互为补角，根据实际双半球胶囊机器人调姿的情况，取其中的锐角作为估计所得的角度。
本发明的有益效果：在双半球胶囊机器人内部有限的空间条件下，不需要在双半球胶囊机器人内部加装传感器或其它辅助装置，结合空间万向旋转磁场的自身特性，标定出双半球胶囊机器人的初始姿态位置，充分利用的双半球胶囊机器人上携带的摄像机，根据调姿前后获得的两幅胃肠内壁的图像，经过特征点的识别和匹配，由对极几何约束建立求解姿态角的方程，将匹配的特征点当做有效值带入进行求解，能够估计出双半球胶囊机器人的俯仰角和侧摆角。这种方法减少了双半球胶囊机器人内部和外部硬件资源，有效地提高了双半球胶囊机器人内部空间的利用效率，并且避免了三轴亥姆霍兹线圈产生的旋转磁场的干扰。获得双半球胶囊机器人的姿态信息为实现对它更加精确的闭环控制和双半球胶囊机器人自身的精确定位奠定了基础。
附图说明
图1(a)是双半球胶囊机器人外部结构局部放大图。
图1(b)是双半球胶囊机器人内部结构局部放大图。
图2是空间万向旋转磁场的正交变换过程示意图。其中：O-XYZ为固定坐标系；O-x2y2z2为旋转坐标系；B为空间万向旋转磁矢量；n为空间万向旋转磁矢量的法线方向；α为侧摆角；β为俯仰角。
图3是双半球胶囊机器人调姿过程示意图。其中，Ow-XwYwZw为双半球胶囊机器人坐标系；Oc-XcYcZc为摄像机坐标系，M为双半球胶囊机器人的球心。
图4是摄像机小孔成像原理示意图。其中，O-XYZ为固定坐标系，Oc-XcYcZc为摄像机坐标系，O1-xy为图像物理坐标系，O′-uv为图像像素坐标系，I为空间中的一点，i为I在图像物理坐标系上的投影。
图5是两幅图像对极几何关系示意图。其中，Q为三维空间中一个实际的点；C1和C2分别是摄像机变换前后的光心位置；q1和q2分别是点X在两个摄像机成像面上的投影；e1和e2分别是两摄像机的中心与像平面的交点；π是对极平面。
图6(a)是摄像机获得的第一幅图像识别出的与第二幅图像相互匹配之后的特征点。
图6(b)是摄像机获得的第二幅图像识别出的与第一幅图像相互匹配之后的特征点。
图中：1主动半球壳；2被动半球壳；3轴承定位套筒；4轴承；5圆螺母；6阶梯轴；7径向磁化钕铁硼圆环内驱动器；8摄像机与图像传输装置；9摄像机光心；10摄像机光轴。
具体实施方式
以下结合技术方案和附图6(a)、6(b)详细叙述本发明的实施步骤和具体实施例。
实施步骤：
第一步：调整通入亥姆霍兹线圈的电流，从而改变空间万向旋转磁矢量，使其法向量方向处于竖直位置，将此位置作为初始位置，此时截取由摄像机呈现的图像。
第二步：在双半球胶囊机器人被动模态下进行调姿，在允许范围内改变双半球胶囊机器人的姿态角，截取在此姿态下的摄像机所呈现的图像。
第三步：将调姿前后获得的图像进行特征点的识别和匹配，获得相互匹配的特征点在图像中的像素坐标信息，结合对极几何和旋转矩阵从而反求出姿态角的变化。
具体实施例：
(1)如图6(a)所示是双半球胶囊机器人拍摄的胃肠内壁的第一幅图像特征点在图像中的位置坐标信息，采用ORB特征点识别算法和暴力匹配算法进行两幅图像上特征点的匹配，调用OpenCV函数库进行编程，从而得到两幅图像互相匹配的数对特征点。本实例中使用的摄像机像素为640*480，为了便于观察，以此作为图像中的坐标轴，这些特征点都是与第二幅图像相互匹配后筛选出来的，特征点坐标信息如图6(a)所示。
(2)如图6(b)所示是双半球胶囊机器人调姿之后拍摄的第二幅图像特征点，在本例中进行的调姿操作是俯仰角改变10°，侧摆角改变15°，这些特征点是与第一幅图像相互匹配后筛选出来的，特征点坐标信息如图6(b)所示。此次特征点匹配获得了16对特征点，在求解本质矩阵E时，采用RANSAC算法对求解过程进行优化，以上所得到的特征点在两图像之间都存在相互的对应关系，即每一个特征点在调姿前都对应其调姿后的相同特征点。
(3)根据所获得的的特征点坐标信息，结合对极几何的相关约束，每对相互匹配的特征点之间存在几何上的变换关系，二者之间的联系由旋转矩阵R和平移矩阵t组成，使用SVD分解对极几何公式中的本质矩阵E，采用正确情况下SVD分解得到的旋转矩阵R，在此实例中经计算得到：
然后再运用第四步中计算调姿后机双半球胶囊机器人轴线方向余弦的方法，求得s2为：
结合获得以上两张肠道内壁的图像的实际摄像机坐标系，可以得到该方向余弦的俯仰角与侧摆角为：
俯仰角β的估计值为：
侧摆角α的估计值为：
通过以上范例验证，该方法在一定误差范围内能够估计出实际的双半球胶囊机器人的姿态信息。
Claims (5)
1.一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法，其特征在于，步骤如下：
第一步：标定双半球胶囊机器人的初始姿态位置
设O-XYZ为与三轴正交亥姆霍兹线圈固定的坐标系，旋转磁矢量所在旋转平面为x2oz2，其中轴x2在固定坐标系XOY平面内，旋转磁矢量B的法向量在旋转坐标系O-x2y2z2中表达为n2＝(0,1,0)，在固定坐标系O-XYZ中，定义旋转磁矢量B的法向量在XOY平面内的投影与Y轴的夹角α为侧摆角，在XOY平面内的投影与y2轴的夹角β为俯仰角，经过旋转变换后，旋转磁矢量B的法向量经过正交变换得到其在固定坐标系O-XYZ内的表达为n＝[sinαcosβ,cosαcosβ,sinβ]；
其中，正交变换矩阵为：
旋转磁矢量B在旋转坐标系中的表达式应为：
其中，B0为产生旋转磁场的磁感应强度，B0＝I0·Kz，ω为磁场旋转的角速度，其方向为逆时针；I0为输入电流的幅值，Kz为大线圈的结构参数；通过坐标变换推得旋转磁矢量B在固定坐标系O-XYZ中的表达形式为：
其中，
根据毕奥萨法尔定律，单轴线圈中心的磁感应强度与电流之间的关系如下：
Bi＝Ki·Ii (4)
公式(4)中，i＝x,y,z，Kx、Ky、Kz分别为小线圈、中间线圈和大线圈的结构参数，Ix、Iy、Iz分别为小线圈、中间线圈和大线圈中施加的交变电流；
通过公式(4)得到三组参数不同的线圈匹配后的电流输入公式：
根据降维后磁场的误差分析，得出由于电流和电压相位差引起方位误差后的实际旋转磁矢量法向量的表达式：
其中，分别为x轴、y轴、z轴亥姆霍兹线圈的电流电压相位差；
当旋转磁矢量B的法向量的方向是竖直向上时，在经纬坐标系中β值为π/2，α为任意值；将β＝π/2带入式(6)中得：
又因为故那么B′的表达式为：
由此可以得出，当双半球胶囊机器人处于竖直位置处时，尽管实际旋转磁矢量B′的大小随相位差变化，但实际旋转磁矢量B′与理论旋转磁矢量法向量B之间没有方位误差；
并且通过公式(5)得知，在竖直方向上亥姆霍兹线圈中实际通入的电流为：
由公式(10)得知，三组线圈中只有x和y两组线圈中通入的交流电，z轴线圈未通电；
通过以上分析，最终将竖直向上的方向作为双半球胶囊机器人被动模态下标定的初始姿态位置；
第二步：建立双半球胶囊机器人和摄像机的坐标系，确定绝对坐标系和相对坐标系；
将双半球胶囊机器人坐标系建立在双半球胶囊机器人与胃肠壁的接触处，摄像机坐标系建立在双半球胶囊机器人顶部，以摄像机的光心作为摄像机坐标系原点，摄像机坐标系相对于双半球胶囊机器人坐标系仅存在一个确定距离的平移；其中Ow-XwYwZw为通过矢量运算的可平移性建立的双半球胶囊机器人固定坐标系，Ow为双半球胶囊机器人坐标系的原点，即双半球胶囊机器人与胃壁的接触点处，Oc-XcYcZc为双半球胶囊机器人上的摄像机坐标系，其中Oc为摄像机坐标系的光心位置处；
被动模态下双半球胶囊机器人进行调姿的过程中，双半球胶囊机器人是以自身球心M进行转动，并不发生滚动，实现定点调姿，双半球胶囊机器人与胃肠道的接触点依然是胃肠内壁上的Ow点，并且主动半球与被动半球固连的摄像机不存在绕其自身轴线的自转，摄像机在调姿过程中既有平移运动，又有在俯仰角和侧摆角方向，以M点为旋转中心的旋转运动；
结构上，摄像机轴线即为双半球胶囊机器人的轴线，摄像机轴线的姿态即为双半球胶囊机器人的轴线的姿态，即通过摄像机拍摄的图像估计出的摄像机姿态就是双半球胶囊机器人的姿态；在进行侧摆与俯仰姿态估计前，先校正摄像机的自转角；
第三步：将获得的初始姿态处的图像信息和进行调姿过程后的图像信息，在图像层面上进行预处理操作，对二者进行特征点的识别和匹配；
第四步：利用对极几何和第三步相互匹配的特征点，获得调姿前后的旋转矩阵；再通过反求旋转矩阵获得双半球胶囊机器人的俯仰角和侧摆角；
俯仰角和侧摆角的求取过程为：
磁矢量误差模型所标定的初始姿态位置用方向余弦表示为s1＝(0,0,1)T，则调姿之后的方向余弦为：
s2＝Rc·s1 (18)
其中s2为调姿之后双半球胶囊机器人轴线的方向余弦；将结果转换为侧摆角和俯仰角的表达形式，对旋转矩阵Rc中的各项进行相互运算并进行反三角函数运算得到侧摆角α，俯仰角β的值，具体求解的方法如下：
侧摆角α的两个可能值为：
α2＝π-α1 (21)
俯仰角β的两个可能值为：
β1＝arccos(R33) (22)
β2＝π-β1 (23)
其中，俯仰角或侧摆角会存在两种情况，侧摆角的两个可能值α1和α2互为补角，俯仰角的两个可能值β1和β2互为补角，根据实际双半球胶囊机器人调姿的情况，取其中的锐角作为估计所得的角度。
2.根据权利要求1所述的一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法，其特征在于，步骤二中，校正摄像机的自转角的过程为：对图像进行校正，驱动磁场仅改变双半球胶囊机器人侧摆角姿态，对图像中的特征点进行识别与匹配，特征点之间的匹配连线与图像水平线之间的夹角即为自转角，获得自转角信息后对图像在自转方向进行校正。
3.根据权利要求1或2所述的一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法，其特征在于，步骤三中，所述特征点识别和匹配的具体确定方法为：采用ORB，此方法是结合FAST算法和BRIEF描述子，既能识别有效的特征点，在响应速度上也能达到要求；在特征匹配时，采用BFMatch的方法，逐个匹配特征点直到找到最佳匹配，并消除误匹配的特征点，保证特征点匹配的正确性；所述的BFMatch方法，即对每一个特征点，与其他所有的特征点测量描述子的距离，然后排序，取最近的一个作为匹配点；描述子距离表示了两个特征之间的相似程度，使用汉明距离做为度量，即两个二进制串之间的汉明距离，指的是它们不同位数的个数；当描述子之间的距离大于两倍的最小距离时，即认为匹配有误。
4.根据权利要求1或2所述的一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法，其特征在于，步骤四中，获得调姿前后的旋转矩阵的方法具体为：首先对摄像机进行标定，以获得摄像机的内部参数；二维图像与三维空间点之间的关系用下式表示：
λ·[xy1]＝[XYZ]·P (11)
其中，λ是放大因子，(x,y)是图像物理坐标系O1-xy中的一点，(X,Y,Z)是固定坐标系O-XYZ中的一点，P是摄像机的参数矩阵，表示为：
P＝[Rc t]TK (12)
其中，Rc为摄像机姿态变换前后的旋转矩阵，t为摄像机姿态变换前后的平移矩阵，由Rc和t组成了摄像机的姿态变换矩阵，K是摄像机的内部参数矩阵，通过相机标定获得：
其中，fx和fy分别表示摄像机在x和y方向上的焦距，cx和cy是摄像机光心在图像像素坐标系O′-uv中的位置；
根据对极几何，求出摄像机变换前后的相对姿态变化；设C1和C2分别是摄像机变换前后的光心位置，Q是三维空间中一个实际的点，q1和q2分别是点Q在两个摄像机成像面上的投影，在几何上表示为摄像机光心C1和C2与空间点Q分别在两个成像平面上的交点；e1和e2分别是两摄像机的中心与像平面的交点，π是对极平面；对极几何描述的是左右两幅图像与以C1 C2为轴的平面束的几何约束；
对极几何中的约束是空间点Q、摄像机光心C1和C2三点在一个平面π上，对极几何的约束中同时包含了平移和旋转，得到两个匹配点q1和q2的空间位置关系：
E＝t×Rc (15)
w1＝K-1q1 w2＝K-1q2 (16)
其中，E为本质矩阵，而w1和w2是像素归一化之后的坐标；在求解本质矩阵E时，采用随机采样一致性RANSAC来进行求解的优化，提高计算E结果的准确性；根据上式(15)可知，摄像机的姿态变化矩阵t和Rc能够对E进行SVD分解获得；
其中旋转矩阵Rc的一般形式为：
5.根据权利要求3所述的一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法，其特征在于，步骤四中，获得调姿前后的旋转矩阵的方法具体为：首先对摄像机进行标定，以获得摄像机的内部参数；二维图像与三维空间点之间的关系用下式表示：
λ·[xy1]＝[XYZ]·P (11)
其中，λ是放大因子，(x,y)是图像物理坐标系O1-xy中的一点，(X,Y,Z)是固定坐标系O-XYZ中的一点，P是摄像机的参数矩阵，表示为：
P＝[Rc t]TK (12)
其中，Rc为摄像机姿态变换前后的旋转矩阵，t为摄像机姿态变换前后的平移矩阵，由Rc和t组成了摄像机的姿态变换矩阵，K是摄像机的内部参数矩阵，通过相机标定获得：
其中，fx和fy分别表示摄像机在x和y方向上的焦距，cx和cy是摄像机光心在图像像素坐标系O′-uv中的位置；
根据对极几何，求出摄像机变换前后的相对姿态变化；设C1和C2分别是摄像机变换前后的光心位置，Q是三维空间中一个实际的点，q1和q2分别是点Q在两个摄像机成像面上的投影，在几何上表示为摄像机光心C1和C2与空间点Q分别在两个成像平面上的交点；e1和e2分别是两摄像机的中心与像平面的交点，π是对极平面；对极几何描述的是左右两幅图像与以C1 C2为轴的平面束的几何约束；
对极几何中的约束是空间点Q、摄像机光心C1和C2三点在一个平面π上，对极几何的约束中同时包含了平移和旋转，得到两个匹配点q1和q2的空间位置关系：
E＝t×Rc (15)
w1＝K-1q1 w2＝K-1q2 (16)
其中，E为本质矩阵，而w1和w2是像素归一化之后的坐标；在求解本质矩阵E时，采用随机采样一致性RANSAC来进行求解的优化，提高计算E结果的准确性；根据上式(15)可知，摄像机的姿态变化矩阵t和Rc能够对E进行SVD分解获得；
其中旋转矩阵Rc的一般形式为：
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910559960.4A CN110327011B (zh) | 2019-06-26 | 2019-06-26 | 一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910559960.4A CN110327011B (zh) | 2019-06-26 | 2019-06-26 | 一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110327011A true CN110327011A (zh) | 2019-10-15 |
| CN110327011B CN110327011B (zh) | 2021-04-20 |
Family
ID=68142748
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910559960.4A Expired - Fee Related CN110327011B (zh) | 2019-06-26 | 2019-06-26 | 一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110327011B (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111207737A (zh) * | 2020-03-01 | 2020-05-29 | 中北大学 | 基于三维线圈的胶囊机器人姿态测定***及方法 |
| CN111256692A (zh) * | 2020-03-01 | 2020-06-09 | 中北大学 | 基于传感器和一维线圈的胶囊机器人姿态测定***及方法 |
| CN112372635A (zh) * | 2020-10-15 | 2021-02-19 | 北京航空航天大学 | 一种胶囊机器人的控制方法、装置、存储介质及终端 |
| CN115089092A (zh) * | 2022-06-20 | 2022-09-23 | 元化智能科技(深圳)有限公司 | 胶囊内窥镜运动控制装置 |
| WO2023174425A1 (zh) * | 2022-03-18 | 2023-09-21 | 安翰科技(武汉)股份有限公司 | 磁控胶囊***及其位姿标定表示方法 |
| CN117462061A (zh) * | 2023-11-24 | 2024-01-30 | 元化智能科技(深圳)有限公司 | 肠道内无线胶囊内窥镜的驱动装置和驱动方法 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050096526A1 (en) * | 2003-10-08 | 2005-05-05 | Siemens Aktiengesellschaft | Endoscopy device comprising an endoscopy capsule or an endoscopy head with an image recording device, and imaging method for such an endoscopy device |
| WO2006087289A2 (de) * | 2005-02-18 | 2006-08-24 | Siemens Aktiengesellschaft | Verfahren zum antreiben einer kapsel innerhalb eines patienten mit hilfe eines elektrischen spulensystems |
| DE102011075498A1 (de) * | 2011-05-09 | 2012-06-28 | Siemens Aktiengesellschaft | Endoskopiekapsel, Anordnung mit einer Endoskopiekapsel und Verfahren zum Navigieren einer Endoskopiekapsel |
| US20140148643A1 (en) * | 2012-05-07 | 2014-05-29 | Olympus Medical Systems Corp. | Guiding apparatus and capsule medical device guiding system |
| CN104983385A (zh) * | 2015-05-21 | 2015-10-21 | 大连理工大学 | 一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法 |
| CN105662318A (zh) * | 2016-01-08 | 2016-06-15 | 大连理工大学 | 一种空间万向旋转磁场人机交互控制方法 |
| CN108759812A (zh) * | 2018-03-28 | 2018-11-06 | 大连理工大学 | 三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 |
-
2019
- 2019-06-26 CN CN201910559960.4A patent/CN110327011B/zh not_active Expired - Fee Related
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050096526A1 (en) * | 2003-10-08 | 2005-05-05 | Siemens Aktiengesellschaft | Endoscopy device comprising an endoscopy capsule or an endoscopy head with an image recording device, and imaging method for such an endoscopy device |
| WO2006087289A2 (de) * | 2005-02-18 | 2006-08-24 | Siemens Aktiengesellschaft | Verfahren zum antreiben einer kapsel innerhalb eines patienten mit hilfe eines elektrischen spulensystems |
| DE102011075498A1 (de) * | 2011-05-09 | 2012-06-28 | Siemens Aktiengesellschaft | Endoskopiekapsel, Anordnung mit einer Endoskopiekapsel und Verfahren zum Navigieren einer Endoskopiekapsel |
| US20140148643A1 (en) * | 2012-05-07 | 2014-05-29 | Olympus Medical Systems Corp. | Guiding apparatus and capsule medical device guiding system |
| CN104983385A (zh) * | 2015-05-21 | 2015-10-21 | 大连理工大学 | 一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法 |
| CN105662318A (zh) * | 2016-01-08 | 2016-06-15 | 大连理工大学 | 一种空间万向旋转磁场人机交互控制方法 |
| CN108759812A (zh) * | 2018-03-28 | 2018-11-06 | 大连理工大学 | 三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111207737A (zh) * | 2020-03-01 | 2020-05-29 | 中北大学 | 基于三维线圈的胶囊机器人姿态测定***及方法 |
| CN111256692A (zh) * | 2020-03-01 | 2020-06-09 | 中北大学 | 基于传感器和一维线圈的胶囊机器人姿态测定***及方法 |
| CN111256692B (zh) * | 2020-03-01 | 2023-03-10 | 中北大学 | 基于传感器和一维线圈的胶囊机器人姿态测定***及方法 |
| CN111207737B (zh) * | 2020-03-01 | 2023-03-24 | 中北大学 | 基于三维线圈的胶囊机器人姿态测定***及方法 |
| CN112372635A (zh) * | 2020-10-15 | 2021-02-19 | 北京航空航天大学 | 一种胶囊机器人的控制方法、装置、存储介质及终端 |
| CN112372635B (zh) * | 2020-10-15 | 2022-04-01 | 北京航空航天大学 | 一种胶囊机器人的控制方法、装置、存储介质及终端 |
| WO2023174425A1 (zh) * | 2022-03-18 | 2023-09-21 | 安翰科技(武汉)股份有限公司 | 磁控胶囊***及其位姿标定表示方法 |
| CN115089092A (zh) * | 2022-06-20 | 2022-09-23 | 元化智能科技(深圳)有限公司 | 胶囊内窥镜运动控制装置 |
| CN117462061A (zh) * | 2023-11-24 | 2024-01-30 | 元化智能科技(深圳)有限公司 | 肠道内无线胶囊内窥镜的驱动装置和驱动方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110327011B (zh) | 2021-04-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110327011A (zh) | 一种基于磁矢量误差标定图像的双半球胶囊机器人姿态检测方法 | |
| CN104983385B (zh) | 一种主被动双半球形胶囊机器人及其姿态调整与转弯驱动控制方法 | |
| Rong et al. | Improving attitude detection performance for spherical motors using a MEMS inertial measurement sensor | |
| Hu et al. | Locating intra-body capsule object by three-magnet sensing system | |
| CN108972559A (zh) | 基于红外立体视觉定位***和机械臂的手眼标定方法 | |
| CN103308028B (zh) | 三轴气浮台姿态角双目立体视觉测量装置及其测量方法 | |
| CN105662318B (zh) | 一种空间万向旋转磁场人机交互控制方法 | |
| CN101297756A (zh) | 磁场与视觉相结合的医疗微型机器人体内姿态定位方法 | |
| CN110617813B (zh) | 单目视觉信息和imu信息相融合的尺度估计***及方法 | |
| CN103027657A (zh) | 基于多传感器的内窥镜跟踪定位与数字人动态同步显示方法 | |
| Liu et al. | Design of a magnetic actuated fully insertable robotic camera system for single-incision laparoscopic surgery | |
| Zhang et al. | Control theorem of a universal uniform-rotating magnetic vector for capsule robot in curved environment | |
| CN108759812A (zh) | 三维封闭非结构空间内胶囊机器人及待测目标的相对定位方法 | |
| CN107561462A (zh) | 基于变化磁场的胶囊全姿态测定***及空间磁场检测装置 | |
| CN109685852A (zh) | 相机与惯性传感器的标定方法、***、设备及存储介质 | |
| Xu et al. | Towards external sensor based simultaneous magnetic actuation and localization for WCE | |
| CN106251323A (zh) | 一种裸眼立体跟踪的方法、装置及电子设备 | |
| Fu et al. | Wearable permanent magnet tracking system for wireless capsule endoscope | |
| Li et al. | An orientation measurement system of the two-DOF permanent magnet spherical motor based on hall ring detectors | |
| CN115316918B (zh) | 全方位观察欠驱动胶囊机器人及其轴线翻转磁控操作方法 | |
| CN109144274B (zh) | 质心偏移和振动结合的力反馈方向指导***及控制方法 | |
| CN110132272A (zh) | 一种用于空间碎片运动参数的测量方法及*** | |
| JP4155546B2 (ja) | 球面超音波モータ | |
| CN113288008A (zh) | 一种磁性胶囊内窥镜全姿态测定方法 | |
| Min et al. | A real-time tracking method for magnetism-driven capsule robot |
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 | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20210420 |