External and Internal Sensor Fusion Based Localization Strategy for 6-DOF Pose Estimation of a Magnetic Capsule Robot

摘要

This paper introduces a novel localization approach for active capsule endoscopy that, for the first time, combines external magnetic field sensing and internal inertial sensing to realize 6-DOF pose estimation of a magnetic capsule robot. It utilizes an inertial measurement unit embedded in the capsule with an external magnetic sensor array to estimate the 6-DOF pose of the capsule, which does not require complicated structures of the capsule and the actuator or the implementation of specific motions of the magnets, and can achieve accurate and real-time localization of the capsule in a large workspace. We formulate the localization model and analyze the singularities of the method, and present the design approach to determine the configuration of the localization system for efficient and accurate localization in a 0.5 × 0.5 × 0.2 m <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$^{3}$</tex-math></inline-formula> workspace. Simulation and real-wold experiments are conducted to validate the effectiveness of the proposed localization strategy. Our results show that the proposed method can achieve a localization accuracy of 5.35 <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$\pm$</tex-math></inline-formula> 2.89 mm and 1.46 <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$\pm$</tex-math></inline-formula> 1.09 <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$^\circ$</tex-math></inline-formula> in position and orientation in the real-time tracking task at an update rate of <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$60 \,\mathrm{Hz}$</tex-math></inline-formula> . The presented method can be integrated with any magnetic actuation method to achieve closed-loop control of a magnetic capsule robot in the human body.