Magnetically controlled capsule system and method for calibrating and displaying its position and attitude

Abstract

The present invention discloses a magnetic control capsule system and its position and attitude calibration display method, which includes: obtaining magnet local coordinates and direction angle of a control magnet; obtaining capsule local coordinates and Euler angles of a capsule endoscope; establishing a world coordinate system; correcting the above parameters in the world coordinate system; and simultaneously displaying the positions and attitudes of the control magnet and the capsule endoscope, which facilitates the realization of subsequent efficient and precise closed-loop control of gastrointestinal examination using a capsule endoscope.

Description

[Technical field]

[0001] (Related Applications) This application claims priority to a Chinese patent application filed on March 18, 2022, with application number 202210272501.X and entitled "Magnetic Control Capsule System and Its Position and Attitude Calibration Display Method", the entire contents of which are incorporated herein by reference.

[0002] The present invention relates to the medical device technical field, and in particular to a magnetically controlled capsule system and a method for calibrating and displaying its position and attitude. [Background technology]

[0003] Positioning technologies for internal devices, such as wireless capsule endoscopes and invasive medical devices, are attracting more and more attention. Magnetically controlled capsule systems use magnetic force to move the capsule endoscope inside the body, but currently, a doctor with extensive experience is required to drive the capsule endoscope. The doctor uses the built-in camera to take an examination image of the inner wall of the digestive tract, check the position and posture of the capsule endoscope, and use an externally controlled magnet to move the capsule endoscope to the next position.

[0004] Due to the extreme nonlinearity of magnetic force, the non-uniform spatial distribution characteristics, the deformable environment of the digestive tract, and the influence of frictional force, the rotation of the capsule makes it difficult to judge the actual orientation by the image, and the image visual feedback information alone cannot accurately control the capsule to the target position and target attitude angle. Even after the positioning method confirms the position and attitude angle of the capsule, the positioning system and the magnetic driving system work independently, so the positioning system can only play the role of auxiliary confirmation, and the next movement of the capsule still relies on the doctor's empirical judgment. In addition, Euler angles are commonly used for the capsule's attitude angle, but Euler angles cannot intuitively express the capsule's attitude, and are not suitable for actual control operation. Therefore, the current positioning system is complex to operate, unintuitive, and lacks precision. Summary of the Invention

[0005] SUMMARY OF THE PRESENT DISCLOSURE In order to solve at least one of the problems of the prior art described above, an object of the present invention is to provide a magnetically controlled capsule system and a method for calibrating the position and attitude of the capsule endoscope, which precisely controls the motion of the capsule endoscope through a feedback system.

[0006] In order to achieve the above object of the present invention, an embodiment of the present invention provides a method for calibrating and displaying a position and attitude of a magnetically controlled capsule system, the method comprising the steps of: Acquiring magnet local coordinates and a direction angle (attitude angle) in a first local coordinate system of a control magnet; acquiring capsule local coordinates and Euler angles in a second local coordinate system of the capsule endoscope; establishing a world coordinate system; modifying the magnet local coordinates to magnet world coordinates in the world coordinate system; Calculating magnet attitude information of the control magnet in the world coordinate system based on a projection of the direction angle in the world coordinate system; modifying the capsule local coordinates to capsule world coordinates in the world coordinate system; determining a projection of the capsule endoscope in the world coordinate system based on the Euler angles, and calculating capsule posture information of the capsule endoscope in the world coordinate system; displaying the position and attitude of the control magnet; and displaying the position and orientation of the capsule endoscope. The position and attitude of the controlled magnet includes magnet world coordinates and / or magnet attitude information; The position and orientation of the capsule endoscope includes capsule world coordinates and / or capsule orientation information.

[0007] As a further improvement of the present invention, the magnet posture information includes a horizontal azimuth angle and a vertical tilt angle, the horizontal azimuth angle being the angle between a projection vector of the magnetization direction vector of the control magnet onto the XY plane of the world coordinate system and the positive Y-axis direction, and the vertical tilt angle being the angle between the magnetization direction vector of the control magnet and the positive Z-axis direction of the world coordinate system.

[0008] As a further improvement of the present invention, The control magnet further includes a step of displaying a position and attitude of the control magnet; The position and orientation 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.

[0009] As a further improvement of the present invention, the capsule posture information includes a horizontal azimuth angle, a vertical tilt angle and a capsule rotation angle, the horizontal azimuth angle being an angle between a projection vector of the head direction of the capsule endoscope onto the XY plane of the world coordinate system and the positive Y-axis direction, the vertical tilt angle being an angle between the head direction of the capsule endoscope and the positive Z-axis direction of the world coordinate system, and the capsule rotation angle being a directional angle of the lens of the capsule endoscope.

[0010] As a further improvement of the present invention, displaying the position and orientation of the capsule endoscope; The position and posture 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, Cv is the vertical tilt angle, Cs is the capsule rotation angle, and the capsule world coordinates are [Ch, Cv, Cs].

[0011] As a further refinement of the invention, the magnet local coordinates include movable range coordinates in a first local coordinate system of the control magnet; The "establishing a world coordinate system" step includes: The method includes determining the origin of the world coordinate system based on the movable range coordinates.

[0012] A further improvement of the present invention includes the step of setting the midpoint of the movable range coordinates as the origin of the world coordinate system.

[0013] As a further refinement of the present invention, the step of "modifying the magnet local coordinates to magnet world coordinates in the world coordinate system" includes: Calculating a first offset amount in each coordinate axis direction of the magnet local coordinate with respect to the origin of the world coordinate system; setting the value of the magnet world coordinate to the first offset amount.

[0014] As a further refinement of the present invention, the step of "modifying the capsule local coordinates to capsule world coordinates in the world coordinate system" comprises: calculating a second offset amount of an origin of the second local coordinate system with respect to an origin of the world coordinate system; The step of setting a difference value between the capsule local coordinates and the second offset amount as a value of the capsule world coordinates.

[0015] As a further refinement of the present invention, the second offset amount comprises an X-axis difference value and a Y-axis difference value; The step of "calculating a second offset amount of the origin of the second local coordinate system with respect to the origin of the world coordinate system" includes: arranging the capsule endoscope and the control magnet so that their vertical projections overlap in an XY plane of the world coordinate system; acquiring a first alignment coordinate in the second local coordinate system of the capsule endoscope and a second alignment coordinate in the world coordinate system of the control magnet; Calculating the X-axis difference value in the X-axis direction and the Y-axis difference value in the Y-axis direction between the first alignment coordinate and the second alignment coordinate.

[0016] As a further refinement of the present invention, the second offset value further comprises a Z-axis differential value; The step of "calculating a second offset amount of the origin of the second local coordinate system with respect to the origin of the world coordinate system" includes: acquiring hardware parameters of the magnetically controlled capsule system; determining the Z-axis delta value based on the hardware parameters.

[0017] In order to achieve one of the above objects of the present invention, there is provided, as an embodiment of the present invention, a magnetically controlled capsule system including a control magnet and a capsule endoscope. This magnetically controlled capsule system comprises: a first acquisition module for acquiring magnet local coordinates and an orientation angle in a first local coordinate system of the control magnet; a second acquisition module for acquiring capsule local coordinates and Euler angles in a second local coordinate system of the capsule endoscope; a modeling module for establishing a world coordinate system; a control magnet position correction module for correcting the magnet local coordinates to magnet world coordinates in the world coordinate system; a control magnet attitude correction module for calculating magnet attitude information of the control magnet in the world coordinate system based on a projection of the direction angle in the world coordinate system; a capsule endoscope position correction module for correcting the capsule local coordinates to capsule world coordinates in the world coordinate system; a capsule endoscope attitude correction module for determining a projection of the capsule endoscope in the world coordinate system based on the Euler angles and calculating capsule attitude information of the capsule endoscope in the world coordinate system; a control magnet display module for displaying a position and attitude of the control magnet; a capsule endoscope display module for displaying the position and posture of the capsule endoscope; The position and attitude of the controlled magnet includes magnet world coordinates and / or magnet attitude information; The position and orientation of the capsule endoscope includes capsule world coordinates and / or capsule orientation information.

[0018] In order to achieve one of the objects of the invention, an embodiment of the present invention provides an electronic device, the electronic device including a storage module and a processing module, the storage module storing a computer program executable on the processing module, and implementing the steps of the method for calibrating and displaying the position and attitude of the magnetically controlled capsule system when the processing module executes the computer program.

[0019] In order to achieve one of the objects of the above invention, one embodiment of the present invention provides a computer-readable storage medium.

[0020] The computer storage medium stores a computer program, When the computer program is executed by a processing module, the steps of the method for calibrating and displaying the position and attitude of the magnetically controlled capsule system are realized. Effect of the Invention

[0021] Compared with the prior art, the present invention has the following beneficial effects: By integrating the two systems corresponding to the control magnet and the capsule endoscope, and expressing the state of the entire magnetic control capsule system in a unified and intuitive manner, it is easy to realize efficient and accurate closed-loop control of the capsule endoscope for gastrointestinal examination, expand the control method and application scenario of the capsule endoscope, and improve the accuracy and precision of medical auxiliary diagnosis.

[0022] In addition, by uniformly calibrating the magnet world coordinates, magnet attitude information, capsule world coordinates, and capsule attitude information in the world coordinate system and converting the Euler angles into capsule attitude information, the capsule attitude can be expressed intuitively, making it easier to use in actual control operations. Once the correspondence between the two systems corresponding to the control magnet and capsule endoscope has been established and matching of the two systems has been completed, there is no need to repeatedly convert the coordinate system in subsequent work. [Brief description of the drawings]

[0023] [Figure 1] 1 is a flowchart of a position and orientation calibration display method according to an embodiment of the present invention. [Diagram 2] FIG. 11 is a structural diagram illustrating the application of a magnetically controlled capsule system to the human body according to another embodiment of the present invention. [Diagram 3] FIG. 2 is a schematic diagram of a method for establishing a world coordinate system in one embodiment of the present invention. [Figure 4] 4 is a schematic diagram of a method for calculating attitude information of a control magnet according to an embodiment of the present invention; FIG. [Diagram 5] FIG. 13 is a schematic diagram of a method for calculating a second offset amount according to an embodiment of the present invention. [Figure 6] FIG. 2 is a schematic diagram of Euler angles of the capsule endoscope according to one embodiment of the present invention. [Figure 7] 4 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] FIG. 1 is a structural block diagram of a magnetically controlled capsule system according to an embodiment of the present invention. [Figure 9] FIG. 2 is a module schematic diagram of a magnetically controlled capsule system according to one embodiment of the present invention.

[0024] Here, 1000 represents a magnetically controlled capsule system, 100 represents a magnetic control system, 200 represents a capsule positioning system, 201 represents a capsule endoscope, 300 represents a bed surface, 400 represents a human body, 10 represents a control magnet, 20 represents a signal transmission module, 30 represents a memory module, 40 represents a processing module, 50 represents a magnetic sensor, 60 represents an acceleration sensor, 70 represents a signal transmission module, 80 represents an imaging module, and 90 represents a communication bus. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0025] The present invention will be described in detail below with reference to specific embodiments shown in the drawings, however, these embodiments do not limit the present invention, and any structural, method or functional modifications made by those skilled in the art based on these embodiments are within the scope of protection of the present invention.

[0026] The magnetically controlled capsule system is a medical device used for human digestive tract examination, which includes a magnetically controlled positioning device outside the human body and a wireless capsule endoscope inside the body. During the examination, the external magnetically controlled positioning device is used to control the movement of the wireless capsule endoscope inside the body and identify its position inside the body, thereby assisting medical diagnosis.

[0027] An embodiment of the present invention provides a magnetically controlled capsule system and its position and attitude calibration and display method for precisely controlling the motion of a capsule endoscope in a closed-loop fashion, which unifies the control magnet and the capsule endoscope into the same world coordinate system, facilitating the precise control of the subsequent wireless capsule.

[0028] The magnetic control capsule system 1000 of this embodiment includes a magnetic control 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 determine the position of the capsule endoscope 201. A sensor module is provided inside the capsule endoscope 201, and the sensor module includes a magnetic sensor 50 for detecting a magnetic field. The magnetic sensor includes a Hall sensor, an anisotropic magnetoresistance (AMR) sensor, a giant magnetoresistance (GMR) sensor, a tunnel magnetoresistance (TMR) sensor, and the like. The control magnet 10 includes a magnetic source for generating a magnetic field and a servo motor for controlling the movement of the magnetic source. A magnetic body is disposed inside the capsule endoscope 201 and interacts with the magnetic source of the control magnet 10. The magnetic control system 100 controls the position and attitude of the capsule endoscope 201 through the force that the magnetic source exerts on the magnetic body.

[0029] Fig. 1 is a flowchart of a position and attitude calibration display method of a magnetically controlled capsule system 1000 according to an embodiment of the present application. Fig. 2 is a schematic diagram of a magnetically controlled capsule system 1000 according to an embodiment of the present application applied to a human body 400. A capsule endoscope 201 is located inside the human body 400, the human body 400 is lying on a bed surface 300, and a magnetic control system 100 including a control magnet 10 is installed outside the human body 400. During an examination, the movement of the capsule endoscope 201 inside the human body 400 is controlled by a magnetic field generated by the control magnet 10.

[0030] Although the present application provides method operation steps shown in the embodiments and flowcharts described below, the order of execution of non-causally related steps that are logically necessary in the method is not limited to the order of execution provided in the embodiments of the present application unless common sense or creative effort is required.

[0031] As shown in FIG. 1, a specific position and attitude calibration and display method for the magnetically controlled capsule system 1000 includes the following steps.

[0032] Step 101: The magnet local coordinates and the direction angle in the first local coordinate system of the control magnet 10 are obtained.

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

[0034] The magnet local coordinates and direction angle can be obtained from the servo motor's transmission data through the data interface of the magnetic control system 100.

[0035] The first local coordinate system is a local coordinate system within the magnetic control system 100. The origin of this first local coordinate system is defined, and the current position and direction angle of the control magnet 10 can be calculated based on the driving amount of the servo motor.

[0036] In the first local coordinate system, the magnet local coordinates are the parameter values ​​in the three directions of the XYZ axes [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 based on the orientation angle of the magnetization direction north pole. The orientation of the control magnet 10 can be determined by taking the unit vector of that direction or normalizing it later and using it in the calculation. Here, the orientation of the control magnet 10 can be determined by its unit vector [p x ,p y ,p z ] will be treated as

[0037] In addition, the movable range of the control magnet 10 in each of the X, Y, and Z axis directions can be calculated based on the movement trajectory of the servo motor. The end points of these ranges are the limit positions of the control magnet 10, and these limit positions are respectively {X:[mag xmin ,mag xmax ],Y:[mag ymin ,mag ymax ],Z:[mag zmin ,mag zmax ]}.

[0038] Step 102: The capsule local coordinates and Euler angles in the second local coordinate system of the capsule endoscope 201 are obtained.

[0039] In the second local coordinate system, the capsule local coordinates are the parameter values ​​[cap x ,cap y ,cap z The Euler angles include the roll angle, pitch angle, and yaw angle, and reflect the orientation of the capsule endoscope 201. The roll angle, pitch angle, and yaw angle can be expressed as [θ0, θ1, θ2], respectively, and the Euler angles of the capsule endoscope 201 are as shown in FIG.

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

[0041] The capsule local coordinates and Euler angles can obtain the position and attitude angle state data of the capsule endoscope 201 through the relevant software interface function of the capsule positioning system 200. The position data is accurate to 1 mm, and the attitude angle state data can be expressed in radians of floating-point numbers to avoid precision loss in matrix calculations.

[0042] A three-axis magnetic sensor 50 and a three-axis acceleration sensor 60 can be placed inside the capsule endoscope 201, and a plurality of magnetic positioning devices can be placed outside the human body 400 and operated in cooperation with the capsule endoscope 201 to calculate the position and posture of the capsule endoscope 201.

[0043] Step 103: Establish a world coordinate system.

[0044] The world coordinate system is used to unify the first local coordinate system of the magnetic control system 100 and the second local coordinate system of the capsule positioning system 200, and to place the magnet local coordinates and direction angle of the above-mentioned control magnet 10, and the capsule local coordinates and Euler angles of the capsule endoscope 201 in the same coordinate system.

[0045] In the position and attitude calibration display method of the magnetic control capsule system 1000 of this embodiment, a calibration process is performed once when the magnetic control system 100 and the capsule positioning system 200 are assembled, and the coordinate systems of the two systems are aligned to the same world coordinate system. If the world coordinate system is changed later due to an upgrade of a functional module or a movement of the positioning system, calibration can be performed again using the method of this embodiment. After one calibration, the correspondence relationship between the two system coordinate systems of the magnetic control system 100 and the capsule positioning system 200 is determined, and the first local coordinate system and the second local coordinate system are aligned with the world coordinate system through conversion by the algorithm of this embodiment, and a unified world coordinate system is formed.

[0046] The world coordinate system can be established either manually or by determining the origin of the world coordinate system based on the movable range coordinates.

[0047] Specifically, the midpoint of the movable range coordinates is set as the origin of the world coordinate system. As shown in FIG. 3, specifically, the coordinates [mag x0 ,mag y0 ,mag z0 ] can be calculated using the following formula: mag x0 =(mag xmax +mag xmin ) / 2 mag y0 =(mag ymax +mag ymin ) / 2 mag z0 =(mag zmax +mag zmin ) / 2

[0048] Step 104: The magnet local coordinates are corrected to magnet world coordinates in the world coordinate system.

[0049] Magnet attitude information of the control magnet in the world coordinate system is calculated based on the projection of the direction angle in the world coordinate system.

[0050] After the world coordinate system is established, a first offset amount in each coordinate axis direction of the magnet local coordinate with respect to the origin of the world coordinate system is calculated, and the value of the magnet world coordinate is set to the first offset amount.

[0051] 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. Mx=mag x -mag x0 My=mag y -mag y0 Mz=mag z -mag z0 +Z0

[0052] where is a constant correction height parameter introduced for ease of application practice, which is used to select the reference zero point position of Mz. When Z0=0 is set, the zero point of Mz is the midpoint position of the Z-axis direction movement range of the control magnet 10. It is also possible to set the bed surface 300 as the zero point of Mz, in which case the Z-axis direction movement of the control magnet 10 is always above the origin, and the value of Mz is always positive.

[0053] For example, if the boundary range of the control magnet 10 in the three axial directions of X, Y and Z is {X: [20, 550], Y: [-30, 450], Z: [-80, 220]}, the origin coordinates of the world coordinate system are mag0 = [285, 210, 70]. If the original coordinate data of the current position point acquired from the control magnet 10 is mag = [200, 150, 100], the position state after calibration is [Mx, My, Mz] = [-85, -60, 30].

[0054] Furthermore, as shown in FIG. 4, magnet attitude information of the control magnet 10 in the world coordinate system is calculated based on the projection of the direction angle in the world coordinate system.

[0055] The control magnet has axial symmetry, and the direction angle [Mh, Mv] of the magnetization direction N pole is related only to the definition of the coordinate axis direction of the world coordinate system. The magnet posture information of the control magnet 10 in the world coordinate system can be expressed in a manner similar to the spherical coordinate angle. As for specific numerical values, the angle between the magnetization direction vector of the control magnet 10 and the positive direction of the Z axis is defined as the vertical tilt angle Mv (range [0, +180] degrees), and the angle between the projection vector of the magnetization direction vector of the control magnet 10 onto the XY plane and the positive direction of the Y axis is defined as the horizontal azimuth angle Mh (range [-180, +180] degrees), which changes clockwise. For example, Mh = 0 when the Y axis is in the positive direction, Mh = 90 when the X axis is in the positive direction, Mh = ±180 when the Y axis is in the negative direction, and Mh = -90 when the X axis is in the negative direction.

[0056] Specifically, the following applies: Mh = 180*arctan2(p x ,p y ) / π Mv=180*arccos(p z ) / π Here, [p x ,p y ,p z ] represents the direction vector of the above-mentioned control magnet 10, that is, the projection components of the unit vector of the magnetization direction of the N pole of the control magnet 10 onto the XYZ coordinate axes.

[0057] As an option, the control magnet is fixedly connected to the control mechanical structure, and the vertical and horizontal angle zero points can be calibrated using a photoelectric switch with a special angle position, and then the relative rotation state of the servo motor that rotates the control magnet can be directly converted into an equivalent value to accurately obtain the angle state data, which eliminates the need to determine the above attitude angle by measuring the magnetic field direction of the control magnet on-site.

[0058] Step 105: The capsule local coordinates are modified to capsule world coordinates in the world coordinate system.

[0059] A projection of the capsule endoscope in the world coordinate system is determined based on the Euler angles, and capsule attitude information of the capsule endoscope in the world coordinate system is calculated.

[0060] A second offset amount of the origin of the second local coordinate system with respect to the origin of the world coordinate system is calculated, the second offset amount includes an X-axis difference value, a Y-axis difference value, and a Z-axis difference value.

[0061] The difference between the capsule local coordinates and the second offset amount is set as the value of the capsule world coordinates.

[0062] The calculation method of the second offset amount is as shown in FIG. 5. Specifically, the capsule endoscope 201 and the control magnet 10 are arranged so that their vertical projections overlap in the XY plane of the world coordinate system. The control magnet 10 is appropriately raised to weaken the attraction force to the capsule endoscope 201, so that the capsule endoscope 201 does not move parallel or rotate when the control magnet 10 moves, and the control magnet 10 is rotated so that the magnetization direction is vertically upward, that is, the N pole is vertically upward. Next, the capsule endoscope 201 is moved to the vicinity below the control magnet 10, and the control magnet 10 is moved in the XY plane and adjusted by negative feedback correction so that the capsule endoscope 201 is in a vertically upward state. At this point, 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.

[0063] At this time, the first alignment coordinates [cap xu ,cap yu ], and the second alignment coordinates [M xu ,M yu The subscript u indicates that the control magnet 10 has moved directly above the capsule endoscope 201. At this time, the capsule endoscope 201 is directly below the control magnet 10, which is indicated by the subscript u (under).

[0064] The first alignment coordinate [cap xu ,capyu ] and the second alignment coordinate [M xu ,M yu ] X-axis difference value cap x0 , and the Y-axis difference value cap y0 Calculate.

[0065] cap x0 and cap y0 is the deviation of the coordinate values ​​on the X-axis and Y-axis of the second local coordinate system and the world coordinate system, which allows the two to be compared within the same coordinate system.

[0066] The second offset amount is calculated when the system starts to operate. After the second offset amount is calculated, there is no need to move the control magnet 10 above the capsule endoscope 201 during subsequent use. x0 and cap y0 is not recalculated during subsequent use. The position of the capsule endoscope 201 is an arbitrary position, and the capsule local coordinates in the second local coordinate system of the capsule endoscope 201 are calculated by the above [cap x ,cap y ,cap z ].

[0067] The calculation steps for the Z-axis difference value are as follows:

[0068] The hardware parameters of the magnetically controlled capsule system 1000 are obtained.

[0069] The Z-axis difference value is determined based on the hardware parameters.

[0070] The Z-axis calibration of the capsule endoscope 201 in the world coordinate system is very important and is related to the balance of gravity, buoyancy, friction, magnetic attraction and torque of the capsule. The Z-axis distance between the control magnet 10 and the capsule endoscope 201 directly determines the magnitude of the magnetic attraction force, and the magnetic force and the distance r satisfy an extremely nonlinear relationship, so the distance affects the force balance of the capsule endoscope 201, causing the capsule endoscope 201 to switch to different situation states such as sinking to the bottom, floating on the water surface, and adsorbing to the ceiling. Accurate Z-axis calibration is convenient for obtaining the altitude distance of the capsule endoscope 201 in real time, and provides data basis for subsequent control operations.

[0071] Since 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, the specific values ​​are determined with reference to the parameters of a specific device.

[0072] For example, if the origin of the first local coordinate system is above the bed surface 300 and the origin of the second local coordinate system is below the bed surface 300, the sum of the difference in height from the origin of the first local coordinate system to the bed surface 300 and the difference in height from the bed surface 300 to the origin of the second local coordinate system is defined as the Z-axis difference value.

[0073] Specifically, the calculation formula for the second offset amount is as follows: cap x0 =cap xu -M xu cap y0 =cap yu -M yu cap z0 =Z1+Z2 Here, Z1 is the difference in height from the origin of the first local coordinate system to the bed surface 300, and Z2 is the difference in height from the bed surface 300 to the origin of the second local coordinate system.

[0074] When combined with the above step of "setting the difference between the capsule local coordinates and the second offset amount as the value of the capsule world coordinates", the calculation formula for the capsule world coordinates [Cx, Cy, Cz] is as follows. Cx=cap x -cap x0 Cy=cap y -cap y0 Cz=cap z -cap z0

[0075] 7, the projection of the capsule endoscope 201 in the world coordinate system is determined based on the Euler angles, and capsule posture information in the world coordinate system of the capsule endoscope 201 is calculated. By converting the Euler angles into capsule posture information, the posture of the capsule can be expressed intuitively, which can be easily used in actual control operations.

[0076] Specifically, the capsule attitude information in the world coordinate system of the capsule endoscope 201 can be expressed in a manner similar to the spherical coordinate angle. As for specific numerical values, in the same way as the method for defining the magnet attitude information, the Euler angles are the direction attitude angle [Ch, Cv] and the capsule rotation angle Cs (fixed phase difference correction C related to the positive direction definition of the lens). s0 (which can be done by

[0077] That is, the capsule posture information of the capsule endoscope 201 is expressed as [Ch, Cv, Cs], where Ch represents the horizontal azimuth angle, Cv represents the vertical tilt angle, and Cs represents the capsule rotation angle (see FIG. 7).

[0078] The angle between the head direction of the capsule endoscope 201 and the positive direction of the Z axis is defined as the capsule vertical inclination angle Cv (range [0, +180] degrees), and the angle between the projection vector of the head direction of the capsule endoscope 201 onto the XY plane and the positive direction of the Y axis is defined as the capsule horizontal azimuth angle Ch (range [-180, +180] degrees), which changes clockwise (Ch=0 when the Y axis is positive, Ch=90 when the X axis is positive, Ch=±180 when the Y axis is negative, and Ch=-90 when the X axis is negative). Here, the head direction of the capsule endoscope 201 refers to the direction of the end where the lens of the capsule endoscope 201 is provided. The capsule rotation angle represents the directional angle of the lens of the capsule endoscope, and the capsule rotation angle when the image captured by the lens of the capsule endoscope 201 is upright is defined as Cs=0, which changes clockwise. The calculation formula for the posture information [Ch, Cv, Cs] of the capsule endoscope 201 is as follows.

[0079] Ch=180*arctan2(p x ,p y ) / π=180*arctan2(R 02 ,R 12 ) / π Cv=180*arccos(p z ) / π=180*arccos(R 22 ) / π Cs = 180*arctan2(R 20 ,-R 21 ) / π+C s0 (Formula 1)

[0080] Here, the projection of the Z axis of the second local coordinate system onto the world coordinate system is expressed by the following equation. P=R

[0001] T =[p x p y p z ] T

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

number

[0082] The rotation matrices for Z(roll), Y(pitch), and X(yaw) are expressed as follows:

number

[0083] where c k =cos(θ k ), sk=sin(θ k ) θ k are the corresponding Euler angles as above, k=0,1,2.

[0084] For specific values ​​of R in Formula 1, see Formula 2. For example, R in Formula 1 20 In equation 2, this becomes c0s1c2+s0s2.

[0085] Step 106: Display the position and attitude of the controlled magnet. The position and attitude of the controlled magnet includes magnet world coordinates and / or magnet attitude information.

[0086] After step 104, the position and orientation of the control magnet 10 in the world coordinate system can be expressed as follows: [Mx, My, Mz, Mh, Mv] Here, [Mx, My, Mz] are the magnet world coordinates, and [Mh, Mv] are the magnet attitude information.

[0087] Step 107: Display the position and orientation of the capsule endoscope. The position and orientation of the capsule endoscope includes capsule world coordinates and / or capsule orientation information.

[0088] After step 105, the position and orientation of the capsule endoscope 201 in the world coordinate system can be expressed as follows: [Cx, Cy, Cz, Ch, Cv, Cs] Here, [Cx, Cy, Cz] are the capsule world coordinates, and [Ch, Cv, Cs] are the capsule attitude information.

[0089] In the magnetically controlled capsule system 1000 of this embodiment, the control magnet 10 and the capsule endoscope 201 are unified into the same world coordinate system, and the Euler angles are converted into capsule attitude information, so that the attitude of the capsule can be expressed intuitively. This makes it possible to intuitively display the overall state of the magnetically controlled capsule system 1000, facilitating the realization of subsequent efficient and precise closed-loop control of a gastrointestinal examination using the capsule endoscope 201. In addition, the control method and application scenario of the capsule endoscope 201 are expanded, improving the accuracy and precision of medical auxiliary diagnosis.

[0090] In one embodiment, a magnetically controlled capsule system 1000 is provided as shown in Fig. 8. The magnetically controlled capsule system 1000 can include the following modules in addition to a control magnet and a capsule endoscope.

[0091] A first acquisition module is used for acquiring magnet local coordinates and a direction angle in a first local coordinate system of the control magnet.

[0092] A second acquisition module is used for acquiring capsule local coordinates and Euler angles in a second local coordinate system of the capsule endoscope.

[0093] Modeling Module: Used to establish the world coordinate system.

[0094] Control magnet position correction module: used to correct the magnet local coordinates to magnet world coordinates in the world coordinate system.

[0095] Control magnet attitude correction module: used to calculate magnet attitude information of the control magnet in the world coordinate system based on the projection of the direction angle in the world coordinate system.

[0096] Capsule endoscope position correction module: used to correct the capsule local coordinates to capsule world coordinates in the world coordinate system.

[0097] A capsule endoscope attitude correction module is used to determine the projection of the capsule endoscope in the world coordinate system based on the Euler angles, and calculate capsule attitude information of the capsule endoscope in the world coordinate system.

[0098] Control magnet display module: used to display the position and orientation of the control magnet. The position and orientation of the control magnet includes magnet world coordinates and / or magnet orientation information.

[0099] Capsule endoscope display module: used for displaying the position and orientation of the capsule endoscope, which includes capsule world coordinates and / or capsule orientation information.

[0100] In one embodiment, the control magnet display module represents the position and orientation of the control magnet as [Mx, My, Mz, Mh, Mv], where [Mx, My, Mz] are magnet world coordinates and [Mh, Mv] are magnet orientation information.

[0101] In one embodiment, the capsule endoscope display module represents the position and orientation of the capsule endoscope as [Cx, Cy, Cz, Ch, Cv, Cs], where [Cx, Cy, Cz] are capsule world coordinates and [Ch, Cv, Cs] are capsule orientation information.

[0102] In one embodiment, a modeling module determines an origin of the world coordinate system based on the range of motion coordinates.

[0103] In one embodiment, the modeling module sets the midpoint of the movable range coordinates as the origin of the world coordinate system.

[0104] In one embodiment, the control magnet position correction module calculates a first offset amount in each coordinate axis direction of the magnet local coordinate relative to the origin of the world coordinate system, and sets the value of the magnet world coordinate to the first offset amount.

[0105] In one embodiment, the capsule endoscope position correction module calculates a second offset amount of the origin of the second local coordinate system relative to the origin of the world coordinate system, and sets the difference value between the capsule local coordinate and the second offset amount as the value of the capsule world coordinate.

[0106] In one embodiment, a capsule endoscope attitude correction module positions the capsule endoscope 201 and the control magnet 10 so that their vertical projections overlap in the XY plane of the world coordinate system.

[0107] A second acquisition module acquires a first alignment coordinate of the capsule endoscope 201 in the second local coordinate system at this time, and a first acquisition module acquires a second alignment coordinate of the control magnet 10 in the world coordinate system.

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

[0109] In one embodiment, the magnetically controlled capsule system 1000 further includes a data interface through which hardware parameters of the magnetically controlled capsule system 1000 are obtained.

[0110] A capsule endoscope coordinate correction module determines the Z-axis difference value based on the hardware parameters.

[0111] The magnetic control capsule system 1000 may further include a computing device such as a computer, a laptop, a handheld computer, a cloud server, etc. It may further include, but is not limited to, a processing module 40 and a storage module 30. As can be understood by those skilled in the art, the schematic diagram is only an example of the magnetic control capsule system 1000 and does not limit the terminal device of the magnetic control capsule system 1000. More or fewer components may be included than shown, or certain components may be combined or different components may be included. For example, the magnetic control capsule system 1000 may include an input / output device, a network access device, a bus, etc.

[0112] 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 position and attitude calibration and display method of the magnetically controlled capsule system 1000 according to the embodiment of the present invention.

[0113] 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 expressing the state of the entire magnetically controlled capsule system 1000. This facilitates the realization of subsequent efficient and precise closed-loop control of gastrointestinal examination using the capsule endoscope 201, expands the control method and application scenario of the capsule endoscope 201, and improves the accuracy and precision of medical auxiliary diagnosis.

[0114] 9 shows a module configuration diagram of a magnetically controlled capsule system 1000 provided by an embodiment of the present invention. The magnetically controlled capsule system 1000 includes the magnetic control system 100, the capsule positioning system 200, the control magnet 10, the capsule endoscope 201, a processing module 40, a storage module 30, each module in the capsule endoscope 201, and a computer program stored in the storage module 30 and executable on the processing module 40, such as the position and attitude calibration display method program. The processing module 40 executes the computer program to realize steps of each embodiment of the position and attitude calibration method as shown in FIG. 1 and FIG. 2.

[0115] The magnetic force source of the controlled magnet 10 is controlled to move to its position by a servo motor and a transmission mechanism, and the transmission data of the servo motor is obtained through the data interface of the magnetic control system 100, and the original data of the position and attitude angle of the controlled magnet 10 is obtained through a fixed proportional conversion formula. The accuracy of the position data is 1 mm, and the accuracy of the angle data is 1 degree.

[0116] The control magnet 10 is fixedly connected to the transmission mechanism, and the control magnet 10 can be accurately moved to a target position in the world coordinate system by converting the drive amount of the servo motor that drives the movement of the control magnet 10 through photoelectric switches at several positions (for example, zero-point calibration at special vertical and horizontal angles). At this time, there is no need to measure the attitude angle of the control magnet 10 according to the magnetic field direction on site.

[0117] 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 an imaging module 80. As described above, the magnetic sensor 50, the acceleration sensor 60, and the magnetic component work in conjunction with the internal three-axis magnetic sensor 50, the three-axis acceleration sensor 60, the IMU sensor, and a plurality of external magnetic positioning devices to calculate the position and attitude of the capsule endoscope 201, and act on the magnetic component by the control magnet 10 to move the capsule endoscope 201. The signal transmission module 70 transfers information to the external processing module 40 or server, and after the external moves the wireless capsule 201 to a specified position, the imaging module 80 takes a picture of the inside of the human body 400 and transmits it to the outside via the signal output module, completing the internal imaging.

[0118] The control magnet 10 is appropriately raised to weaken the attraction force on the capsule 201, or pulled down to strengthen the attraction force on the capsule 201, thereby switching the capsule endoscope 201 to a state corresponding to different situations, such as sinking to the bottom, floating to the surface, or being attracted to the top.

[0119] The magnetic control system 100 may also include a signal transmission module 20 and a communication bus 90. The signal transmission module 20 is for transmitting data to the processing module 40 or a server, and the signal transmission module 70 and the signal transmission module 20 can transmit data in a wireless connection format such as Bluetooth, Wi-Fi, Zigbee, etc. The communication bus 90 is for establishing a connection between the control magnet 10, the signal transmission module 20, the processing module 40, and the storage module 30, and the communication bus 90 includes a path to transmit information between the control magnet 10, the signal transmission module 20, the processing module 40, and the storage module 30.

[0120] Furthermore, an embodiment of the present invention provides an electronic device, which includes a storage module and a processing module, the storage module storing a computer program executable on the processing module, and the steps of the above-mentioned method for calibrating and displaying a position and attitude of a magnetically controlled capsule system can be realized when the processing module executes the computer program.

[0121] Furthermore, an embodiment of the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processing module, realizes the steps of the position and attitude calibration and display method for the magnetically controlled capsule system described above.

[0122] It should be understood that, although the description is based on the embodiments, each embodiment does not include only a single technical solution, but is merely a form for explanation. Those skilled in the art should interpret the description as a whole, and the technical solutions of each embodiment can be appropriately combined to form other embodiments that are understandable to those skilled in the art.

[0123] The above detailed description is merely a specific description of the possible embodiments of the present invention, and does not limit the scope of protection of the present invention. As long as it does not deviate from the spirit of the present invention, equivalent embodiments and modifications are included in the scope of protection of the present invention.

Claims

1. A method for calibrating and displaying a position and attitude of a magnetically controlled capsule system including a control magnet and a capsule endoscope, comprising: obtaining magnet local coordinates and a direction angle in a first local coordinate system of a control magnet; acquiring capsule local coordinates and Euler angles in a second local coordinate system of the capsule endoscope; establishing a world coordinate system; modifying the magnet local coordinates to magnet world coordinates in the world coordinate system; Calculating magnet attitude information of the control magnet in the world coordinate system based on a projection of the direction angle in the world coordinate system; modifying the capsule local coordinates to capsule world coordinates in the world coordinate system; determining a projection of the capsule endoscope in the world coordinate system based on the Euler angles, and calculating capsule posture information of the capsule endoscope in the world coordinate system; displaying the position and attitude of the control magnet; and displaying the position and orientation of the capsule endoscope. The position and attitude of the controlled magnet includes magnet world coordinates and / or magnet attitude information; The position and orientation of the capsule endoscope includes capsule world coordinates and / or capsule orientation information; A method for calibrating and displaying the position and attitude of a magnetically controlled capsule system.

2. The magnet attitude information includes a horizontal azimuth angle and a vertical tilt angle, the horizontal azimuth angle is an angle between a projection vector of the magnetization direction vector of the control magnet onto the XY plane of the world coordinate system and the positive Y-axis direction, The vertical tilt angle is an angle between the magnetization direction vector of the control magnet and the positive direction of the Z axis of the world coordinate system.

2. The method for calibrating and displaying a position and attitude of a magnetically controlled capsule system according to claim 1.

3. displaying the position and attitude of the control magnet; The position and orientation 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 method for calibrating and displaying a position and attitude of a magnetically controlled capsule system according to claim 2 .

4. The capsule attitude information includes a horizontal azimuth angle, a vertical tilt angle, and a capsule rotation angle; the horizontal azimuth angle is an angle between a projection vector of the head direction of the capsule endoscope onto the XY plane of the world coordinate system and the positive Y-axis direction; the vertical inclination angle is an angle between the head direction of the capsule endoscope and the positive direction of the Z axis of the world coordinate system; The capsule rotation angle is the direction angle of the lens of the capsule endoscope; 2. The method for calibrating and displaying a position and attitude of a magnetically controlled capsule system according to claim 1.

5. displaying the position and orientation of the capsule endoscope; The position and orientation 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, Cv is the vertical tilt angle, and Cs is the capsule rotation angle. The method for calibrating and displaying the position and attitude of a magnetically controlled capsule system according to claim 4 .

6. the magnet local coordinates include movable range coordinates in a first local coordinate system of the control magnet, The "establishing a world coordinate system" step includes: determining an origin of the world coordinate system based on the movable range coordinates; 2. The method for calibrating and displaying a position and attitude of a magnetically controlled capsule system according to claim 1.

7. The midpoint of the movable range coordinates is set as the origin of the world coordinate system. The method for calibrating and displaying the position and attitude of a magnetically controlled capsule system according to claim 6.

8. The step of "correcting the magnet local coordinates to magnet world coordinates in the world coordinate system" includes: Calculating a first offset amount in each coordinate axis direction of the magnet local coordinate with respect to the origin of the world coordinate system; setting the magnet world coordinate value to the first offset amount; The method for calibrating and displaying the position and attitude of a magnetically controlled capsule system according to claim 6.

9. The step of "modifying the capsule local coordinates to capsule world coordinates in the world coordinate system" includes: calculating a second offset amount of an origin of the second local coordinate system with respect to an origin of the world coordinate system; A step of setting a difference value between the capsule local coordinate and the second offset amount as a value of the capsule world coordinate, 2. The method for calibrating and displaying a position and attitude of a magnetically controlled capsule system according to claim 1.

10. the second offset amount includes an X-axis difference value and a Y-axis difference value; The step of "calculating a second offset amount of the origin of the second local coordinate system with respect to the origin of the world coordinate system" includes: arranging the capsule endoscope and the control magnet so that their vertical projections overlap in an XY plane of the world coordinate system; acquiring a first alignment coordinate in the second local coordinate system of the capsule endoscope and a second alignment coordinate in the world coordinate system of the control magnet; Calculating the X-axis difference value in the X-axis direction and the Y-axis difference value in the Y-axis direction between the first alignment coordinate and the second alignment coordinate, The method for calibrating and displaying a position and attitude of a magnetically controlled capsule system according to claim 9.

11. The second offset amount further includes a Z-axis difference value, the target position range includes a predetermined distance; The step of "calculating a second offset amount of the origin of the second local coordinate system with respect to the origin of the world coordinate system" includes: acquiring hardware parameters of the magnetically controlled capsule system; determining the Z-axis delta value based on the hardware parameters. The method for calibrating and displaying a position and attitude of a magnetically controlled capsule system according to claim 10.

12. A magnetically controlled capsule system including a control magnet and a capsule endoscope, a first acquisition module for acquiring magnet local coordinates and an orientation angle in a first local coordinate system of the control magnet; a second acquisition module for acquiring capsule local coordinates and Euler angles in a second local coordinate system of the capsule endoscope; a modeling module for establishing a world coordinate system; a control magnet position correction module for correcting the magnet local coordinates to magnet world coordinates in the world coordinate system; a control magnet attitude correction module for calculating magnet attitude information of the control magnet in the world coordinate system based on a projection of the direction angle in the world coordinate system; a capsule endoscope position correction module for correcting the capsule local coordinates to capsule world coordinates in the world coordinate system; a capsule endoscope attitude correction module for determining a projection of the capsule endoscope in the world coordinate system based on the Euler angles and calculating capsule attitude information of the capsule endoscope in the world coordinate system; a control magnet display module for displaying a position and attitude of the control magnet; a capsule endoscope display module for displaying a position and posture of the capsule endoscope; The position and attitude of the controlled magnet includes magnet world coordinates and / or magnet attitude information; The position and orientation of the capsule endoscope includes capsule world coordinates and / or capsule orientation information; Magnetically controlled capsule system.

13. 1. An electronic device comprising: A storage module and a processing module are included, the storage module stores a computer program executable on the processing module; When the processing module executes the computer program, a position and attitude calibration and display method for a magnetically controlled capsule system, obtaining magnet local coordinates and a direction angle in a first local coordinate system of a control magnet; acquiring capsule local coordinates and Euler angles in a second local coordinate system of the capsule endoscope; establishing a world coordinate system; modifying the magnet local coordinates to magnet world coordinates in the world coordinate system; Calculating magnet attitude information of the control magnet in the world coordinate system based on a projection of the direction angle in the world coordinate system; modifying the capsule local coordinates to capsule world coordinates in the world coordinate system; determining a projection of the capsule endoscope in the world coordinate system based on the Euler angles, and calculating capsule posture information of the capsule endoscope in the world coordinate system; displaying the position and attitude of the control magnet; and displaying the position and orientation of the capsule endoscope. The position and attitude of the controlled magnet includes magnet world coordinates and / or magnet attitude information; The position and orientation of the capsule endoscope includes capsule world coordinates and / or capsule orientation information; Electronic devices.

14. A computer-readable storage medium having a computer program stored thereon, When the computer program is executed by a processing module, a method for calibrating and displaying a position and attitude of a magnetically controlled capsule system, obtaining magnet local coordinates and a direction angle in a first local coordinate system of a control magnet; acquiring capsule local coordinates and Euler angles in a second local coordinate system of the capsule endoscope; establishing a world coordinate system; modifying the magnet local coordinates to magnet world coordinates in the world coordinate system; Calculating magnet attitude information of the control magnet in the world coordinate system based on a projection of the direction angle in the world coordinate system; modifying the capsule local coordinates to capsule world coordinates in the world coordinate system; determining a projection of the capsule endoscope in the world coordinate system based on the Euler angles, and calculating capsule posture information of the capsule endoscope in the world coordinate system; displaying the position and attitude of the control magnet; and displaying the position and orientation of the capsule endoscope. The position and attitude of the controlled magnet includes magnet world coordinates and / or magnet attitude information; The position and orientation of the capsule endoscope includes capsule world coordinates and / or capsule orientation information; A computer-readable storage medium.

Images