WO2023174425A1 - Magnetically controlled capsule system, and pose calibration representation method therefor - Google Patents

Abstract

A magnetically controlled capsule system (1000), and a pose calibration representation method therefor. The magnetically controlled capsule system (1000) comprises a magnetic control system (100), a capsule positioning system (200), a control magnet (10), and a capsule endoscope (201). The pose calibration representation method comprises: acquiring magnet local area coordinates and the orientation angle of a control magnet (10) (101); acquiring capsule local coordinates and Euler angles of a capsule endoscope (201) (102); establishing a global coordinate system (103); revising the described parameters in the global coordinate system (104, 105); and representing the pose of the control magnet (10) and the pose of the capsule endoscope (201) at the same time (106, 107). The present method allows for efficient and accurate closed-loop control of the capsule endoscope (201), thus making digestive tract examination easier.

Description

Magnetic capsule system and its pose calibration representation method

This application claims the priority of a Chinese patent application with an application date of March 18, 2022, an application number of 202210272501. incorporated in this application.

Technical field

The present invention relates to the technical field of medical equipment, and in particular to a magnetically controlled capsule system and its posture calibration and representation method.

Background technique

In vivo device positioning technology, such as wireless capsule endoscopes, invasive medical devices and other in vivo positioning technologies, has received more and more attention. The magnetically controlled capsule system uses magnetic force to drive the capsule endoscope to move in the body. Currently, the driving of the capsule endoscope still needs to be completed by experienced doctors. The doctor takes inspection images of the inner wall of the digestive tract through the built-in lens, determines the position and orientation of the capsule endoscope, and then drives the capsule endoscope to continue moving to the next position through an external control magnet.

Due to the extremely nonlinear and non-uniform spatial distribution characteristics of magnetism, the deformable environment of the digestive tract and the influence of friction, the capsule spin makes it difficult to determine the true orientation based on the image, making it impossible to accurately and quantitatively control the capsule to reach the target by relying solely on image visual feedback information. position and target attitude angle. After confirming the position and attitude angle of the capsule through the positioning method, since the positioning system and the magnetic drive system operate independently, the positioning system can only play a role in assisting the confirmation. The next movement of the capsule still needs to be judged by the doctor based on experience, and the attitude of the capsule Euler angles are often used as angles, but Euler angles cannot intuitively reflect the posture of the capsule and are inconvenient for actual control operations. Therefore, the current positioning system is complex to operate and not intuitive and accurate enough.

Contents of the invention

In order to solve at least one of the above-mentioned problems in the prior art, the object of the present invention is to provide a magnetically controlled capsule system and its pose calibration method that accurately controls the movement of a capsule endoscope in a closed-loop manner.

In order to achieve the above-mentioned object of the invention, one embodiment of the present invention provides a posture calibration and representation method of a magnetically controlled capsule system, which includes the following steps:

Obtain the magnet local coordinates and orientation angle of the control magnet in the first local coordinate system;

Obtain the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system;

Establish a world coordinate system;

Correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system;

Calculate the magnet attitude information of the control magnet in the world coordinate system according to the projection of the orientation angle in the world coordinate system;

Correct the capsule local coordinates to the capsule world coordinates in the world coordinate system;

Determine the projection of the capsule endoscope in the world coordinate system according to the Euler angle, and calculate the capsule posture information of the capsule endoscope in the world coordinate system;

Indicates the pose of the control magnet, which includes the magnet world coordinates and/or the magnet attitude information. interest;

Indicates the posture of the capsule endoscope, and the posture of the capsule endoscope includes capsule world coordinates and/or capsule posture information.

As a further improvement of the present invention, the magnet posture information includes a horizontal azimuth angle and a vertical tilt angle, wherein the horizontal azimuth angle is the projection vector of the magnetization direction vector of the control magnet on the XY plane of the world coordinate system and The angle between the positive direction of the Y axis, and the vertical tilt angle is the angle between the magnetization direction vector of the control magnet and the positive direction of the Z axis of the world coordinate system.

As a further improvement of the present invention, it also includes the steps:

Represents the posture of the control magnet. The posture of the control magnet is expressed as [Mx, My, Mz, Mh, Mv], where [Mx, My, Mz] is the position of the control magnet in the world coordinate system. , Mh is the horizontal azimuth angle, and Mv is the vertical tilt angle.

As a further improvement of the present invention, the capsule attitude information includes a horizontal azimuth angle, a vertical tilt angle and a capsule spin angle, wherein the horizontal azimuth angle is the head direction of the capsule endoscope in the world coordinate system The angle between the XY plane projection vector and the positive direction of the Y axis. The vertical tilt angle is the angle between the head direction of the capsule endoscope and the positive Z axis of the world coordinate system. The capsule spins Angle is the orientation angle of the lens of the capsule endoscope.

As a further improvement of the present invention, it also includes the steps:

represents the posture of the capsule endoscope, and the posture of the capsule endoscope is expressed as [Cx, Cy, Cz, Ch, Cv, Cs], where [Cx, Cy, Cz] is the The position coordinates of the looking glass in the world coordinate system, Ch is the horizontal azimuth angle, Cv is the vertical tilt angle, Cs is the capsule spin angle and the capsule world coordinates, [Ch, Cv, Cs] is the capsule world coordinate posture information.

As a further improvement of the present invention, the local coordinates of the magnet include the coordinates of the movable range of the control magnet in the first local coordinate system;

The steps "establishing a world coordinate system" include:

According to the movable range coordinates, the origin of the world coordinate system is determined.

As a further improvement of the present invention, the middle point of the movable range coordinates is used as the origin of the world coordinate system.

As a further improvement of the present invention, the step "correcting the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system" includes:

Calculate a first set of offsets of the local coordinates of the magnet relative to the origin of the world coordinate system in each coordinate axis direction;

Set the value of the magnet world coordinate to the first set of offsets.

As a further improvement of the present invention, the step "correcting the capsule local coordinates to the capsule world coordinates in the world coordinate system" includes:

Calculate a second set of offsets of the origin of the second local coordinate system relative to the origin of the world coordinate system;

The difference between the capsule local coordinates and the second set of offsets is used as the value of the capsule world coordinates.

As a further improvement of the present invention, the second set of offsets includes an X-axis difference and a Y-axis difference;

The step "calculating the second set of offsets of the origin of the second local coordinate system relative to the origin of the world coordinate system" includes:

Coincide the vertical projections of the capsule endoscope and the control magnet in the XY plane of the world coordinate system;

Obtain the first alignment coordinates of the capsule endoscope in the second local coordinate system and the second alignment coordinates of the control magnet in the world coordinate system at this time;

The X-axis difference in the X-axis direction and the Y-axis difference in the Y-axis direction between the first alignment coordinate and the second alignment coordinate are calculated.

As a further improvement of the present invention, the second set of offsets also includes a Z-axis difference;

The step "calculating the second set of offsets of the origin of the second local coordinate system relative to the origin of the world coordinate system" includes:

Obtain the hardware parameters of the magnetically controlled capsule system;

The Z-axis difference is determined according to the hardware parameters.

In order to achieve one of the above-mentioned objects of the invention, one embodiment of the present invention provides a magnetically controlled capsule system, which includes a control magnet and a capsule endoscope, and also includes:

The first acquisition module is used to acquire the magnet local coordinates and orientation angle of the control magnet in the first local coordinate system;

a second acquisition module, configured to acquire the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system;

Modeling module, used to establish the world coordinate system;

Control the magnet position correction module to correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system;

A control magnet attitude correction module, configured to calculate the magnet attitude information of the control magnet in the world coordinate system according to the projection of the orientation angle in the world coordinate system;

A capsule endoscope position correction module, used to correct the capsule local coordinates to the capsule world coordinates in the world coordinate system;

A capsule endoscope posture correction module, configured to determine the projection of the capsule endoscope in the world coordinate system according to the Euler angle, and calculate the capsule posture of the capsule endoscope in the world coordinate system. information;

A control magnet representation module, used to represent the pose of the control magnet, where the pose of the control magnet includes magnet world coordinates and/or magnet attitude information;

A capsule endoscope representation module is used to represent the pose of the capsule endoscope, where the pose of the capsule endoscope includes capsule world coordinates and/or capsule pose information.

In order to achieve one of the above-mentioned objects of the invention, an embodiment of the present invention provides an electronic device, including: a storage module and a processing module. The storage module stores a computer program that can run on the processing module. The processing module When the computer program is executed, the steps in the pose calibration and representation method of the magnetically controlled capsule system are implemented.

In order to achieve one of the above-mentioned objects of the invention, one embodiment of the present invention provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processing module, the position of the magnetically controlled capsule system is realized. Attitude calibration represents the steps in the method.

Compared with the existing technology, the present invention has the following beneficial effects: the two systems corresponding to the control magnet and the capsule endoscope are integrated to uniformly and intuitively represent the status of the entire magnetic control capsule system, thereby facilitating efficient and accurate closed loop Control the capsule endoscope to realize digestive tract examination, expand the control methods and application scenarios of capsule endoscope, and improve the accuracy and precision of medical auxiliary diagnosis.

And by uniformly calibrating the magnet world coordinates, magnet attitude information, capsule world coordinates and capsule attitude information in the world coordinate system, and converting Euler angles into capsule attitude information, the attitude of the capsule can be displayed intuitively, thus facilitating actual control. operational use. The corresponding relationship between the two systems corresponding to the control magnet and capsule endoscope is determined, and the two systems are matched. There is no need to repeat the conversion of the coordinate system in subsequent work.

Description of the drawings

Figure 1 is a flow chart of a pose calibration and representation method according to an embodiment of the present invention;

Figure 2 is a schematic structural diagram of a magnetically controlled capsule system applied to the human body according to an embodiment of the present invention;

Figure 3 is a schematic diagram of a method for establishing a world coordinate system according to an embodiment of the present invention;

Figure 4 is a schematic diagram of a method for calculating attitude information of a control magnet according to an embodiment of the present invention;

Figure 5 is a schematic diagram of a method for calculating the second set of offsets according to an embodiment of the present invention;

Figure 6 is a schematic diagram of the Euler angle of a capsule endoscope according to an embodiment of the present invention;

Figure 7 is a schematic diagram of a method for calculating posture information of a capsule endoscope according to an embodiment of the present invention;

Figure 8 is a structural block diagram of a magnetically controlled capsule system according to an embodiment of the present invention;

Figure 9 is a module schematic diagram of a magnetically controlled capsule system according to an embodiment of the present invention;

Among them, 1000, magnetic control capsule system; 100, magnetic control system; 200, capsule positioning system; 201, capsule endoscope; 300, bed surface; 400, human body; 10, control magnet; 20, signal transmission module; 30, Storage module; 40. Processing module; 50. Magnetic sensor; 60. Acceleration sensor; 70. Signal transmission module; 80. Camera module; 90. Communication bus.

Detailed ways

The present invention will be described in detail below with reference to the specific embodiments shown in the accompanying drawings. However, these embodiments do not limit the present invention. Structural, method, or functional changes made by those of ordinary skill in the art based on these embodiments are all included in the protection scope of the present invention.

The magnetically controlled capsule system is a medical device used for human digestive tract examination, including a magnetically controlled positioning device located outside the human body and a wireless capsule endoscope located inside the body. During the examination, the movement of the wireless capsule endoscope in the body is controlled through an external magnetically controlled positioning device and the position of the wireless capsule endoscope is positioned in the body, thereby assisting medical diagnosis.

One embodiment of the present invention provides a magnetically controlled capsule system that accurately controls the movement of a capsule endoscope in a closed-loop manner and its pose calibration representation method. This method unifies the control magnet and the capsule endoscope into the same world coordinate system. This facilitates subsequent accurate control of the wireless capsule.

The magnetically controlled capsule system 1000 in this embodiment includes a magnetically controlled system 100, a capsule positioning system 200, a control magnet 10 and a capsule endoscope 201. The magnetic control system 100 is used to control the movement of the capsule endoscope 201 . The capsule positioning system 200 is used to position the capsule endoscope 201 . The capsule endoscope 201 is equipped with a sensor module inside. The sensor module includes a magnetic sensor 50 (magnetic sensor) for detecting a magnetic field. The magnetic sensor can be: Hall sensor, anisotropic magnetoresistance (AMR) sensor, giant magnet sensor. resistance sensor (GMR) sensor, tunnel magnetoresistance (TMR) sensor, etc. The control magnet 10 includes a magnetic source for emitting a magnetic field and a servo motor for controlling the movement of the magnetic source. The capsule endoscope 201 is provided with a magnetic component inside, which interacts with the magnetic source of the control magnet 10 . Through the force exerted by the magnetic source on the magnetic components, the magnetic control system 100 controls the position and attitude of the capsule endoscope 201 .

Figure 1 is a flow chart of a posture calibration and representation method of a magnetically controlled capsule system 1000 according to an embodiment of the present application. Figure 2 is a schematic structural diagram of a magnetically controlled capsule system 1000 applied to the human body according to an embodiment of the present application. The capsule endoscope 201 is located inside the human body 400. The human body 400 lies flat on the bed 300. The control magnet 10 is provided outside the human body 400. The magnetic control system 100. During inspection, the magnetic field emitted by the magnet 10 is controlled to control the movement of the capsule endoscope 201 in the human body 400 .

Although this application provides method operation steps as described in the following embodiments or flow charts, there are steps in the method that do not logically have a necessary causal relationship based on routine or no creative effort, and the order of execution of these steps does not exist. It is limited to the execution sequence provided in the embodiment of this application.

As shown in Figure 1, the specific pose calibration representation method of the magnetically controlled capsule system 1000 includes the following steps:

Step 101: Obtain the magnet local coordinates and orientation angle of the control magnet 10 in the first local coordinate system.

The local coordinates of the magnet may include coordinates of the movable range of the control magnet 10 in the first local coordinate system.

The local coordinates and orientation angle of the magnet can be obtained from the transmission data of the servo motor through the data interface of the magnetic control system 100 .

The first local coordinate system is a local coordinate system in the magnetic control system 100. The origin of the first local coordinate system is defined, and then the current position of the control magnet 10 can be calculated based on the driving amount of the servo motor. and orientation angle.

In the first local coordinate system, the local coordinates of the magnet include parameter values in the three directions of the XYZ axis: [mag _x , mag _y , mag _z ]. The control magnet 10 has axial symmetry, and the orientation angle of the control magnet 10 can be determined according to the orientation angle of the N pole in the magnetization direction. The orientation of the control magnet 10 may be the unit vector of its direction, or may be involved in the calculation after normalization at a later stage. Here, the direction of the control magnet 10 is taken as its unit vector: [p _x , p _y , p _z ].

In addition, according to the stroke trajectory of the servo motor, the movable range of the control magnet 10 in each direction of the XYZ axis can be calculated. The end points of these ranges are the limit positions of the control magnet 10. These limit positions can be expressed as: {X:[ mag _xmin ,mag _xmax ],Y:[mag _ymin ,mag _ymax ],Z:[mag _zmin ,mag _zmax ]}.

Step 102: Obtain the capsule local coordinates and Euler angles of the capsule endoscope 201 in the second local coordinate system;

In the second local coordinate system, the local coordinates of the capsule are the parameter values of the capsule endoscope 201 in the three directions of the XYZ axis: [cap _x , cap _y , cap _z ]. Euler angles include roll angle roll, pitch angle pitch and yaw angle, which reflect the glue The orientation of the capsule endoscope 201 is. The roll angle roll, pitch angle pitch, and yaw angle can be expressed as [θ ₀ , θ ₁ , θ ₂ ] in sequence. The Euler angle of the capsule endoscope 201 can be shown in FIG. 6 .

The second local coordinate system is a local coordinate system in the capsule endoscope 201. The calibration of this local coordinate system depends on the calibration of the capsule positioning system 200. The capsule positioning system 200 is generally fixed below the bed surface 300. The height position of its origin (along the Z-axis direction) is relatively fixed, but the local coordinates may move in the XY plane.

The capsule local coordinates and Euler angles can be used to obtain the position and attitude angle status data of the capsule endoscope 201 through the relevant software interface functions of the capsule positioning system 200 . The position data is accurate to 1mm. In order to avoid the loss of matrix calculation accuracy, the attitude angle status data can be expressed in floating point radians.

The capsule endoscope 201 can be provided with a three-axis magnetic sensor 50 and a three-axis acceleration sensor 60 inside. Several sets of magnetic positioning devices are provided outside the human body 400 to work in conjunction with the capsule endoscope 201 to calculate the location and location of the capsule endoscope 201. attitude.

Step 103: Establish the world coordinate system.

The world coordinate system is used to combine the first local coordinate system of the magnet control system 100 and the second local coordinate system of the capsule positioning system 200, and combine the above-mentioned magnet local coordinates and orientation angle of the control magnet 10, capsule endoscope The capsule local coordinates and Euler angles of 201 are put into the same coordinate system.

The pose calibration representation method of the magnetic control capsule system 1000 in this embodiment is a calibration process performed when the magnetic control system 100 and the capsule positioning system 200 are assembled, so that the coordinate systems of the two systems are matched to the same world coordinate system. If the world coordinate system changes later due to functional module upgrades, positioning system movement, etc., the method of this embodiment can be used to calibrate again. After a calibration, the corresponding relationship between the two system coordinate systems of the magnetic control system 100 and the capsule positioning system 200 is determined. After the algorithm conversion of this embodiment, the first local coordinate system and the second local coordinate system are equal to the world coordinate system. The matching is completed and a unified world coordinate system is formed.

The world coordinate system can be established manually, or the origin of the world coordinate system can be determined based on the movable range coordinates.

Specifically, the middle point of the coordinates of the movable range is used as the origin of the world coordinate system, as shown in Figure 3. Specifically, the coordinates [mag _x0 , mag _y0 , mag _z0 ] of the origin of the world coordinate system mag _o can be It is calculated using the following formula:

Step 104: Correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system;

According to the projection of the orientation angle in the world coordinate system, the magnet attitude information of the control magnet in the world coordinate system is calculated.

After the above world coordinate system is established, the first set of offsets of the magnet's local coordinates relative to the origin of the world coordinate system in each coordinate axis direction is calculated, and then the value of the magnet's world coordinate is set to the desired value. The first set of offsets quantity.

Specifically, after calibrating the origin of the world coordinate system, the position coordinates [Mx, My, Mz] of the control magnet 10 in the world coordinate system can be calculated by the following formula:

Among them, Z ₀ is a constant correction height parameter introduced to facilitate application habits and is used to select the reference zero position of Mz. You can set Z ₀ = 0, and at this time the zero point of Mz is located at the midpoint of the movement range of the control magnet 10 along the Z-axis direction; you can also set the bed surface 300 as the zero point of Mz, and at this time the control magnet 10 always moves along the Z-axis direction. Located above the origin, the value of Mz is always positive.

For example, the boundary range of the control magnet 10 in the XYZ three-axis direction is {X:[20,550],Y:[-30,450],Z:[-80,220]}, then the origin coordinate of the world coordinate system mag ₀ =[285,210 ,70], according to the original data mag=[200,150,100] of the current position point coordinate obtained by the control magnet 10, the calibrated position state [Mx, My, Mz]=[-85,-60,30].

In addition, as shown in FIG. 4 , based on the projection of the orientation angle in the world coordinate system, the magnet attitude information of the control magnet 10 in the world coordinate system is calculated.

The control magnet has axial symmetry, and the orientation angle [Mh, Mv] of the N pole in the magnetization direction is only related to the definition of the coordinate axis direction of the world coordinate system. The magnet attitude information of the control magnet 10 in the world coordinate system can be expressed in angles similar to spherical coordinates. In terms of specific numerical values, the angle between the magnetization direction vector of the control magnet 10 and the positive direction of the Z-axis can be defined as the vertical tilt angle M _v (value range [0, +180] degrees); define the magnetization direction vector of the control magnet 10 in the XY plane The angle between the projection vector and the positive Y-axis is the horizontal azimuth angle M _h (value range [-180, +180] degrees), and changes in the clockwise direction. For example, when the Y-axis is positive, M _h = 0, the X-axis In the positive direction, M _h = 90, in the negative direction of the Y axis, M _h = ±180, and in the negative direction of the X axis, M _h = -90.

specifically,

[px, py, pz] represents the vector that controls the direction of the magnet 10 , that is, the projection component of the unit vector that controls the magnetization direction of the N pole of the magnet 10 on the XYZ coordinate axes.

Optionally, the control magnet is fixedly connected to the control mechanical structure, and the zero point of the vertical and horizontal angles is calibrated through a photoelectric switch at a special angular position, and then the relative rotational state of the servo motor that drives the rotation of the control magnet is directly equivalently converted, so that the angle can be accurately obtained status data, there is no need to determine the above attitude angle through on-site measurement of the magnetic field direction of the control magnet.

Step 105: Correct the local coordinates of the capsule to the world coordinates of the capsule in the world coordinate system;

The projection of the capsule endoscope in the world coordinate system is determined according to the Euler angle, and the capsule posture information of the capsule endoscope in the world coordinate system is calculated.

Calculate a second set of offsets of the origin of the second local coordinate system relative to the origin of the world coordinate system, the second set of offsets including X-axis difference, Y-axis difference and Z-axis difference value;

The difference between the capsule local coordinates and the second set of offsets is used as the value of the capsule world coordinates.

The calculation method of the second set of offsets can be seen in Figure 5. Specifically, the vertical projections of the capsule endoscope 201 and the control magnet 10 in the XY plane of the world coordinate system are coincident. Pull the control magnet 10 up appropriately to weaken the suction force on the capsule endoscope 201 to avoid translation or rolling of the capsule endoscope 201 when the control magnet 10 moves, and rotate the control magnet 10 until the magnetization direction is vertically upward, that is, N Extremely vertical and upward. Then, the capsule endoscope 201 is moved to near the bottom of the control magnet 10, and the control magnet 10 moves in the XY plane, and is adjusted through negative feedback correction, so that the capsule endoscope 201 reaches the vertical upward state. At this time, the vertical projections of the control magnet 10 and the capsule endoscope 201 in the XY plane completely overlap, and the XY plane calibration alignment of the capsule positioning system 200 is completed.

Obtain the first alignment coordinates [cap _xu , cap _yu ] of the capsule endoscope 201 in the second local coordinate system (in Figure 4, represented by the Oc-XcYcZc coordinate system) at this time, and the The second alignment coordinate [M _xu , M _yu ] of the control magnet 10 in the world coordinate system (in Figure 4, represented by the O-XYZ coordinate system), the subscript u means that the control magnet 10 has moved to the capsule Directly above the endoscope 201, at this time, the capsule endoscope 201 is located directly below the control magnet 10, marked with a subscript u (under).

Calculate the X-axis difference cap x0 between the first alignment coordinate [cap _xu , cap _yu ] and the second alignment coordinate [M _xu , M _yu ] in the X-axis direction, and the X-axis difference cap _x0 in the Y-axis direction Y-axis difference cap _y0 .

cap _x0 and cap _y0 are the deviations of the coordinate values on the X-axis and Y-axis between the second local coordinate system and the world coordinate system, thus unifying the two into one coordinate system for comparison.

The calculation of the second set of offsets is set at the beginning of the system operation. After the values of the second set of offsets are calculated, there is no need to move the control magnet 10 to the capsule endoscope during subsequent use. Above 201, the cap _x0 and cap _y0 will not be calculated again in subsequent uses. The position of the capsule endoscope 201 can be any position, and the capsule local coordinates of the capsule endoscope 201 in the second local coordinate system are the above-mentioned [cap _x , cap _y , cap _z ].

In addition, the calculation steps for the Z-axis difference are as follows:

Obtain the hardware parameters of the magnetically controlled capsule system 1000;

The Z-axis difference is determined according to the hardware parameters.

The Z-axis calibration of the capsule endoscope 201 in the world coordinate system is very important, involving the balance of capsule gravity, capsule buoyancy, friction, magnet attraction and torque. The distance in the Z-axis direction between the control magnet 10 and the capsule endoscope 201 directly determines The size of the magnetic attraction, and the magnetic force and distance r satisfy an extremely non-linear relationship. The distance affects the force balance of the capsule endoscope 201, causing the capsule endoscope 201 to switch to different situational states such as sinking to the bottom, floating on the water surface, and ceiling. . Accurate Z-axis calibration facilitates real-time acquisition of the height distance of the capsule endoscope 201 and provides data basis for subsequent control actions.

The origin calibration of the second local coordinate system depends on the hardware settings of the control system 100 and the capsule positioning system 200, so its specific values are determined with reference to the parameters of the specific equipment.

For example, assuming that the origin of the first local coordinate system is above the bed 300 and the origin of the second local coordinate system is below the bed 300, the height difference between the origin of the first local coordinate system and the bed 300 is the same as the height difference between the origin of the first local coordinate system and the bed 300. The sum of height differences between the surface 300 and the origin of the second local coordinate system is used as the Z-axis difference.

Specifically, the calculation formula for the second set of offsets is:

Among them, Z ₁ is the height difference between the origin of the first local coordinate system and the bed surface 300 , and Z ₂ is the height difference between the bed surface 300 and the origin of the second local coordinate system.

Combined with the above steps of "taking the difference between the local coordinates of the capsule and the second set of offsets as the value of the world coordinates of the capsule", the calculation formula of the capsule world coordinates [Cx, Cy, Cz] is:

In addition, as shown in FIG. 7 , the projection of the capsule endoscope 201 in the world coordinate system is determined according to the Euler angle, and the capsule posture of the capsule endoscope 201 in the world coordinate system is calculated. information. Converting Euler angles into capsule attitude information can intuitively display the capsule's attitude, making it easier to use in actual control operations.

Specifically, the capsule posture information of the capsule endoscope 201 in the world coordinate system can be represented by angles similar to spherical coordinates. In terms of specific numerical values, a method similar to the definition of magnet attitude information can be used to convert the Euler angle into an orientation angle [Ch, Cv] and a capsule spin angle Cs (a fixed phase difference related to the forward direction of the lens can be defined) Correction C _s0 ).

That is to say, the capsule attitude information of the capsule endoscope 201 is represented by [Ch, Cv, Cs], where: Ch represents the horizontal azimuth angle, Cv represents the vertical tilt angle, and Cs represents the capsule spin angle, as shown in Figure 7 .

Define the angle between the head orientation of the capsule endoscope 201 and the positive direction of the Z axis as the capsule vertical tilt angle C _v (value range [0, +180] degrees); define the head orientation of the capsule endoscope 201 in XY The angle between the plane projection vector and the positive Y-axis is the horizontal azimuth angle C _h of the capsule (value range [-180, +180] degrees), and changes in the clockwise direction (when the Y-axis is positive _Ch = 0 _{, Ch} = ₉₀ when the Refers to the direction of the end of the capsule endoscope 201 provided with the lens. The capsule spin angle is the orientation angle of the lens of the capsule endoscope. When the image captured by the lens of the capsule endoscope 201 is upright, the capsule spin angle C _s =0 is defined. According to the change in the clockwise direction, the capsule endoscope 201 The calculation formula of attitude information [Ch, Cv, Cs] is:

Among them, the projection of the Z axis of the second local coordinate system in the world coordinate system is:

P＝R·[0 0 1] ^T ＝[p _x p _y p _z ] ^T

The direction cosine matrix of the capsule endoscope 201 rotating in the order of Z (roll), Y (pitch), and X (yaw) is expressed as:

The Z(roll), Y(pitch), and X(yaw) rotation matrices are respectively recorded as:

Among them, c _k ≡ cos (θ _k ), s _k ≡ sin (θ _k ), θ _k is the corresponding Euler angle mentioned above, k = 0, 1, 2.

The specific value of R in Formula 1 can be found in Formula 2. For example, R20 in Formula 1 is queried as c ₀ s ₁ c ₂ +s ₀ s ₂ in Formula 2.

Step 106: Represent the pose of the control magnet. The pose of the control magnet includes magnet world coordinates and/or magnet attitude information;

After step 104, the pose of the control magnet 10 in the world coordinate system can be expressed as:

[Mx,My,Mz,Mh,Mv], where [Mx,My,Mz] is the world coordinate of the magnet, and [Mh,Mv] is the magnet attitude information.

Step 107: Represent the pose of the capsule endoscope. The pose of the capsule endoscope includes capsule world coordinates and/or capsule pose information.

After step 105, the position and orientation of the capsule endoscope 201 in the world coordinate system can be expressed as:

[Cx, Cy, Cz, Ch, Cv, Cs], where [Cx, Cy, Cz] is the capsule world coordinate, [Ch, Cv, Cs] is the capsule posture information.

The magnetically controlled capsule system 1000 of this embodiment unifies the control magnet 10 and the capsule endoscope 201 into the same world coordinate system, and converts Euler angles into capsule posture information, which can intuitively display the posture of the capsule, and then intuitively Indicates the status of the entire magnetically controlled capsule system 1000, which facilitates the subsequent efficient and accurate closed-loop control of the capsule endoscope 201 for gastrointestinal examination, expands the control methods and application scenarios of the capsule endoscope 201, and improves the accuracy of medical auxiliary diagnosis. and precision.

In one embodiment, a magnetically controlled capsule system 1000 is provided, as shown in FIG. 8 . In addition to controlling the magnet and capsule endoscope, the magnetically controlled capsule system 1000 may also include:

The first acquisition module is used to acquire the magnet local coordinates and orientation angle of the control magnet in the first local coordinate system;

a second acquisition module, configured to acquire the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system;

Modeling module, used to establish the world coordinate system;

Control the magnet position correction module to correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system;

A control magnet attitude correction module, configured to calculate the magnet attitude information of the control magnet in the world coordinate system according to the projection of the orientation angle in the world coordinate system;

A capsule endoscope position correction module, used to correct the capsule local coordinates to the capsule world coordinates in the world coordinate system;

A capsule endoscope posture correction module, configured to determine the projection of the capsule endoscope in the world coordinate system according to the Euler angle, and calculate the capsule posture of the capsule endoscope in the world coordinate system. information;

A control magnet representation module, used to represent the pose of the control magnet, where the pose of the control magnet includes magnet world coordinates and/or magnet attitude information;

A capsule endoscope representation module is used to represent the pose of the capsule endoscope, where the pose of the capsule endoscope includes capsule world coordinates and/or capsule pose information.

In one embodiment, the control magnet representation module represents the pose of the control magnet as [Mx, My, Mz, Mh, Mv], where [Mx, My, Mz] is the world coordinate of the magnet, [Mh, Mv ] is the magnet attitude information

In one embodiment, the capsule endoscope representation module represents the pose of the capsule endoscope as [Cx, Cy, Cz, Ch, Cv, Cs], where [Cx, Cy, Cz] is the capsule world Coordinates, [Ch, Cv, Cs] are capsule attitude information

In one embodiment, the modeling module determines the origin of the world coordinate system based on the movable range coordinates.

In one embodiment, the modeling module uses the middle point of the movable range coordinates as the origin of the world coordinate system.

In one embodiment, the magnet position correction module is controlled to calculate a first set of offsets of the magnet's local coordinates relative to the origin of the world coordinate system in each coordinate axis direction, and set the value of the magnet's world coordinate is the first set of offsets.

In one embodiment, the capsule endoscope position correction module calculates a second set of offsets of the origin of the second local coordinate system relative to the origin of the world coordinate system; and compares the capsule local coordinates with The difference between the second set of offsets is used as the value of the capsule world coordinate.

In one embodiment, the capsule endoscope attitude correction module coincides with the vertical projections of the capsule endoscope 201 and the control magnet 10 in the XY plane of the world coordinate system;

The second acquisition module acquires the first alignment coordinate of the capsule endoscope 201 in the second local coordinate system at this time, and the first acquisition module acquires the third alignment coordinate of the control magnet 10 in the world coordinate system. Two alignment coordinates;

The capsule endoscope position correction module calculates the X-axis difference in the X-axis direction between the first alignment coordinate and the second alignment coordinate, and the Y-axis difference in the Y-axis direction.

In one embodiment, the magnetically controlled capsule system 1000 also includes a data interface, through which the hardware parameters of the magnetically controlled capsule system 1000 are obtained;

The capsule endoscope coordinate correction module determines the Z-axis difference according to the hardware parameters.

The magnetically controlled capsule system 1000 may also include computing devices such as computers, laptops, PDAs, and cloud servers. It may further include, but is not limited to, a processing module 40 and a storage module 30 . Those skilled in the art can understand that the schematic diagram is only an example of the magnetically controlled capsule system 1000 and does not constitute a limitation on the terminal equipment of the magnetically controlled capsule system 1000. It may include more or fewer components than shown in the figure, or a combination of certain components. Some components, or different components, for example, the magnetically controlled capsule system 1000 may also include input and output devices, network access devices, buses, etc.

It should be noted that for details not disclosed in the magnetically controlled capsule system 1000 according to the embodiment of the present invention, please refer to the details disclosed in the pose calibration and representation method of the magnetically controlled capsule system 1000 according to the embodiment of the present invention.

According to the magnetically controlled capsule system 1000 of the present invention, the control magnet coordinate correction module and the capsule endoscope coordinate correction module unify the control magnet 10 and the capsule endoscope 201 into the same world coordinate system, thereby intuitively representing the entire magnetically controlled capsule. The status of the system 1000 facilitates the subsequent efficient and accurate closed-loop control of the capsule endoscope 201 for gastrointestinal examination, expands the control methods and application scenarios of the capsule endoscope 201, and improves the accuracy and precision of medical auxiliary diagnosis.

As shown in Figure 9, it is a schematic module diagram of a magnetically controlled capsule system 1000 provided by an embodiment of the present invention. The magnetic control capsule system 1000 also includes the above-mentioned magnetic control system 100, capsule positioning system 200, control magnet 10 and capsule endoscope 201, processing module 40, storage module 30, each module in the capsule endoscope 201, and the modules stored in the capsule endoscope 201. The computer program in the storage module 30 and that can be run on the processing module 40 is, for example, the above-mentioned pose calibration method program. When the processing module 40 executes the computer program, it implements the steps in each of the above embodiments of the pose calibration method, such as the steps shown in FIG. 1 .

The magnetic source of the control magnet 10 is controlled by the servo motor and transmission mechanism to drive it to a designated position. Through the data interface of the magnetic control system 100, the transmission data of the servo motor is obtained. Through a fixed proportion conversion formula, the position of the control magnet 10 is obtained. Attitude angle status raw data. Position data is accurate to 1mm, and angle data is accurate to 1 degree.

The control magnet 10 is fixedly connected to the transmission mechanism. The zero point is calibrated through photoelectric switches at some positions, such as some special vertical and horizontal angles. Then, by converting the driving amount of the servo motor that drives the movement of the control magnet 10, it can be accurately The ground-driven control magnet 10 moves to the target position in the world coordinate system, and there is no need to measure the magnetic field direction of the control magnet 10 on site to determine its attitude angle.

The capsule endoscope 201 may include a magnetic sensor 50, an acceleration sensor 60, a signal transmission module 70, a magnetic component (not shown), and a camera module 80. The magnetic sensor 50, the acceleration sensor 60, and the magnetic component may pass through the interior as described above. The three-axis magnetic sensor 50, the three-axis acceleration sensor 60, the IMU sensor and multiple sets of external magnetic positioning equipment work together to calculate the position and attitude of the capsule endoscope 201, and drive the capsule endoscope by controlling the action of the magnet 10 on the magnetic parts. Mirror 201 Movement. The signal transmission module 70 transmits information to the external processing module 40 or server. After the external world drives the wireless capsule to move to a designated position, the camera module 80 takes photos of the human body 400 and transmits them to the outside world through the signal output module, completing the internal photography.

The control magnet 10 can be appropriately raised to weaken the suction force to the capsule, or lowered to increase the suction force to the capsule, to control the capsule endoscope 201 to switch between different situational states such as sinking to the bottom, floating on the water surface, and ceiling.

The magnetic control system 100 may also include a signal transmission module 20 and a communication bus 90 . The signal transmission module 20 is used to send data to the processing module 40 or the server. The signal transmission module 70 and the signal transmission module 20 can transmit data through wireless connections, such as Bluetooth, wifi, zigbee, etc., and the communication bus 90 is used to control the magnet. 10. Establish a connection between the signal transmission module 20, the processing module 40 and the storage module 30. The communication bus 90 may include a channel to transmit information between the above-mentioned control magnet 10, the signal transmission module 20, the processing module 40 and the storage module 30. .

Further, an embodiment of the present invention provides an electronic device, including a storage module and a processing module. The storage module stores a computer program that can be run on the processing module. When the processing module executes the computer program, the The steps in the pose calibration method of the magnetically controlled capsule system are as described above.

Furthermore, one embodiment of the present invention provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by the processing module, the steps in the pose calibration and representation method of the magnetically controlled capsule system are implemented as described above. .

It should be understood that although this specification is described in terms of implementations, not each implementation only contains an independent technical solution. This description of the specification is only for the sake of clarity. Persons skilled in the art should take the specification as a whole and understand each individual solution. The technical solutions in the embodiments can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

The series of detailed descriptions listed above are only specific descriptions of feasible implementations of the present invention. They are not intended to limit the protection scope of the present invention. Any equivalent implementations or implementations that do not deviate from the technical spirit of the present invention are not intended to limit the protection scope of the present invention. All changes should be included in the protection scope of the present invention.

Claims (14)

A method for position and orientation calibration representation of a magnetically controlled capsule system. The magnetically controlled capsule system includes a control magnet and a capsule endoscope, which is characterized by including the following steps: Obtain the magnet local coordinates and orientation angle of the control magnet in the first local coordinate system; Obtain the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system; Establish a world coordinate system; Correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system; Calculate the magnet attitude information of the control magnet in the world coordinate system according to the projection of the orientation angle in the world coordinate system; Correct the capsule local coordinates to the capsule world coordinates in the world coordinate system; Determine the projection of the capsule endoscope in the world coordinate system according to the Euler angle, and calculate the capsule posture information of the capsule endoscope in the world coordinate system; Represents the pose of the control magnet, where the pose of the control magnet includes magnet world coordinates and/or magnet attitude information; Indicates the posture of the capsule endoscope, and the posture of the capsule endoscope includes capsule world coordinates and/or capsule posture information. The posture calibration representation method according to claim 1, characterized in that the magnet posture information includes a horizontal azimuth angle and a vertical tilt angle, wherein the horizontal azimuth angle is the magnetization direction vector of the control magnet in the The angle between the projection vector of the XY plane of the world coordinate system and the positive direction of the Y axis. The vertical tilt angle is the angle between the magnetization direction vector of the control magnet and the positive direction of the Z axis of the world coordinate system. The pose calibration and representation method according to claim 2, further comprising the steps of: represents the posture of the control magnet, and the posture of the control magnet is expressed as [Mx, My, Mz, Mh, Mv], where, [Mx, My, Mz] are the position coordinates of the control magnet in the world coordinate system, Mh is the horizontal azimuth angle, and Mv is the vertical tilt angle. The pose calibration representation method according to claim 1, characterized in that the capsule pose information includes a horizontal azimuth angle, a vertical tilt angle and a capsule spin angle, wherein the horizontal azimuth angle is the capsule endoscope The angle between the head direction of the capsule endoscope and the positive direction of the Y-axis in the XY plane projection vector of the world coordinate system. The capsule spin angle is the orientation angle of the lens of the capsule endoscope. The pose calibration and representation method according to claim 4, further comprising the step of: representing the pose of the capsule endoscope, and the pose of the capsule endoscope is expressed as [Cx, Cy, Cz, Ch, Cv, Cs], where [Cx, Cy, Cz] are the position coordinates of the capsule endoscope in the world coordinate system, Ch is the horizontal azimuth angle, and Cv is the vertical tilt angle, Cs is the spin angle of the capsule. The pose calibration representation method according to claim 1, wherein the local coordinates of the magnet include the coordinates of the movable range of the control magnet in the first local coordinate system; The steps "establishing a world coordinate system" include: According to the movable range coordinates, the origin of the world coordinate system is determined. The pose calibration and representation method according to claim 6, wherein the middle point of the movable range coordinates is used as the origin of the world coordinate system. The pose calibration and representation method according to claim 6, characterized in that the step of "correcting the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system" includes: Calculate a first set of offsets of the local coordinates of the magnet relative to the origin of the world coordinate system in each coordinate axis direction; Set the value of the magnet world coordinate to the first set of offsets. The pose calibration representation method according to claim 1, wherein the step of "correcting the capsule local coordinates to the capsule world coordinates in the world coordinate system" includes: Calculate a second set of offsets of the origin of the second local coordinate system relative to the origin of the world coordinate system; The difference between the capsule local coordinates and the second set of offsets is used as the value of the capsule world coordinates. The pose calibration representation method according to claim 9, wherein the second set of offsets includes an X-axis difference and a Y-axis difference; The step "calculating the second set of offsets of the origin of the second local coordinate system relative to the origin of the world coordinate system" includes: Coincide the vertical projections of the capsule endoscope and the control magnet in the XY plane of the world coordinate system; Obtain the first alignment coordinates of the capsule endoscope in the second local coordinate system and the second alignment coordinates of the control magnet in the world coordinate system at this time; The X-axis difference in the X-axis direction and the Y-axis difference in the Y-axis direction between the first alignment coordinate and the second alignment coordinate are calculated. The pose calibration representation method according to claim 10, wherein the second set of offsets also includes a Z-axis difference; The step "calculating the second set of offsets of the origin of the second local coordinate system relative to the origin of the world coordinate system" includes: Obtain the hardware parameters of the magnetically controlled capsule system; The Z-axis difference is determined according to the hardware parameters. A magnetically controlled capsule system, which includes a control magnet and a capsule endoscope, is characterized in that it also includes: The first acquisition module is used to acquire the magnet local coordinates and orientation angle of the control magnet in the first local coordinate system; a second acquisition module, configured to acquire the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system; Modeling module, used to establish the world coordinate system; Control the magnet position correction module to correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system; A control magnet attitude correction module, configured to calculate the magnet attitude information of the control magnet in the world coordinate system according to the projection of the orientation angle in the world coordinate system; A capsule endoscope position correction module, used to correct the capsule local coordinates to the capsule world coordinates in the world coordinate system; A capsule endoscope posture correction module, configured to determine the projection of the capsule endoscope in the world coordinate system according to the Euler angle, and calculate the capsule posture of the capsule endoscope in the world coordinate system. information; A control magnet representation module, used to represent the pose of the control magnet, where the pose of the control magnet includes magnet world coordinates and/or magnet attitude information; A capsule endoscope representation module is used to represent the pose of the capsule endoscope, where the pose of the capsule endoscope includes capsule world coordinates and/or capsule pose information. An electronic device, characterized in that it includes: a storage module and a processing module. The storage module stores a computer program that can be run on the processing module. When the processing module executes the computer program, a magnetic control module is implemented. The pose calibration of the capsule system represents the steps in the method, and the steps include: Obtain the magnet local coordinates and orientation angle of the control magnet in the first local coordinate system; Obtain the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system; Establish a world coordinate system; Correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system; Calculate the magnet attitude information of the control magnet in the world coordinate system according to the projection of the orientation angle in the world coordinate system; Correct the capsule local coordinates to the capsule world coordinates in the world coordinate system; Determine the projection of the capsule endoscope in the world coordinate system according to the Euler angle, and calculate the capsule posture information of the capsule endoscope in the world coordinate system; Represents the pose of the control magnet, where the pose of the control magnet includes magnet world coordinates and/or magnet attitude information; Indicates the posture of the capsule endoscope, and the posture of the capsule endoscope includes capsule world coordinates and/or capsule posture information. A computer-readable storage medium with a computer program stored thereon. When the computer program is executed by a processing module, the steps in a posture calibration and representation method of a magnetically controlled capsule system are implemented. The steps include: Obtain the magnet local coordinates and orientation angle of the control magnet in the first local coordinate system; Obtain the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system; Establish a world coordinate system; Correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system; Calculate the magnet attitude information of the control magnet in the world coordinate system according to the projection of the orientation angle in the world coordinate system; Correct the capsule local coordinates to the capsule world coordinates in the world coordinate system; Determine the projection of the capsule endoscope in the world coordinate system according to the Euler angle, and calculate the capsule posture information of the capsule endoscope in the world coordinate system; Represents the pose of the control magnet, where the pose of the control magnet includes magnet world coordinates and/or magnet attitude information; Indicates the posture of the capsule endoscope, and the posture of the capsule endoscope includes capsule world coordinates and/or capsule posture information.