Capsule endoscope system and its magnetic positioning method

A capsule endoscope system comprising a capsule endoscope, a magnetic control device, and a processing device, wherein:

• the capsule endoscope comprises a built-in space and a first magnet, an inertial measurement unit, and at least two magnetic sensors arranged in the built-in space, two of the at least two magnetic sensors being respectively close to two ends of the built-in space along a longitudinal direction;
• the magnetic control device comprises a second magnet, and the magnetic control device conducts magnetic control on the capsule endoscope through an action of the second magnet on the first magnet;
• the inertial measurement unit senses an attitude of the capsule endoscope and acquires a measurement value of the attitude, and the at least two magnetic sensors sense a magnetic field of the second magnet and acquire at least a measurement value of a first magnetic field and another measurement value of a second magnetic field;
• and
• the processing device calculates a spin angle of the capsule endoscope based on a preset attitude solving algorithm and the measurement value of the attitude, wherein:
• the spin angle comprises a precisely estimated pitch angle, a precisely estimated roll angle, and an estimated yaw angle, and
• the processing device calculates an estimated position of the capsule endoscope relative to the second magnet based on a driving magnetic field algorithm, the measurement value of the first magnetic field, the measurement value of the second magnetic field and the spin angle, optimizes the estimated position and the estimated yaw angle using an unscented Kalman filtering algorithm to obtain a precisely estimated position and a precisely estimated yaw angle of the capsule endoscope relative to the second magnet, and determines a position and the attitude of the capsule endoscope in a world coordinate system based on the position and the attitude of the second magnet.

A capsule endoscope magnetic positioning method, comprising:

• obtaining a measurement value of an attitude of a capsule endoscope;
• sensing a magnetic field of a second magnet and obtaining a measurement value of a first magnetic field and another measurement value of a second magnetic field at a first position and a second position of the capsule endoscope respectively, wherein the first position and the second position are respectively close to two ends along a longitudinal direction of a built-in space of the capsule endoscope, and the second magnet is used in magnetic control for the capsule endoscope;
• calculating a spin angle of the capsule endoscope based on a preset attitude solution algorithm and the measurement value of the attitude, wherein the spin angle comprises a precisely estimated pitch angle, a precisely estimated roll angle, and an estimated yaw angle;
• calculating an estimated position of the capsule endoscope relative to the second magnet based on a driving magnetic field algorithm and the measurement value of the first magnetic field, the measurement value of the second magnetic field, and the spin angle;
• optimizing the estimated position and the estimated yaw angle using an unscented Kalman filter algorithm to obtain a precisely estimated position and a precisely estimated yaw angle of the capsule endoscope relative to the second magnet;
• and
• determining a position and the attitude of the capsule endoscope in a world coordinate system based on a position and the attitude of the second magnet.