Magnetic controlled capsule system and its pose calibration representation method

Abstract

The application discloses a magnetic control capsule system and a pose calibration expression method thereof, and the method comprises the following steps: acquiring the local coordinates of a control magnet in a first local coordinate system and the orientation angle; acquiring the local coordinates of a capsule endoscope in a second local coordinate system and the Euler angle; establishing a world coordinate system; correcting the above parameters in the world coordinate system; and simultaneously expressing the poses of the control magnet and the capsule endoscope. The control magnet and the capsule endoscope are unified into the same world coordinate system, and then the state of the whole magnetic control capsule system can be intuitively expressed, which provides convenience for subsequent efficient and accurate closed-loop control of the capsule endoscope to realize a digestive tract examination, expands the control method and application scene of the capsule endoscope, and improves the accuracy and precision of medical auxiliary diagnosis.

Description

Technical Field

[0001] This invention relates to the field of medical device technology, and in particular to a magnetically controlled capsule system and its position and orientation calibration method. Background Technology

[0002] In-vivo device positioning technologies, such as wireless capsule endoscopes and invasive medical devices, are receiving increasing attention. Magnetically controlled capsule systems use magnetic force to move the capsule endoscope within the body. Currently, driving the capsule endoscope still requires experienced physicians. Physicians use an internal lens to capture images of the digestive tract lining to determine the capsule endoscope's position and orientation, and then use external magnets to drive the capsule endoscope to the next location.

[0003] Due to the extremely nonlinear and non-uniform spatial distribution of magnetic force, the deformable environment of the digestive tract, and the influence of friction, the capsule's spin makes it difficult to determine its true orientation based on images. Therefore, relying solely on visual feedback from images is insufficient for precise quantitative control of the capsule's arrival at the target position and angle. Even after confirming the capsule's position and angle through positioning methods, the positioning system and the magnetic drive system operate independently, with the positioning system only providing auxiliary confirmation. The capsule's next movement still requires the physician's experience to determine. Furthermore, while Euler angles are often used to indicate the capsule's orientation, they do not directly reflect the capsule's posture and are inconvenient for practical control. Therefore, current systems are complex to operate and lack intuitiveness and precision. Summary of the Invention

[0004] To address at least one of the aforementioned problems in the prior art, the present invention aims to provide a magnetically controlled capsule system and its pose calibration method for accurately controlling the movement of a capsule endoscope through a closed-loop mechanism.

[0005] To achieve the above-mentioned objective, one embodiment of the present invention provides a pose calibration representation method for a magnetically controlled capsule system, comprising the following steps:

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

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

[0008] Establish a world coordinate system;

[0009] The local coordinates of the magnet are corrected to the world coordinates of the magnet in the world coordinate system;

[0010] The magnet attitude information of the control magnet in the world coordinate system is calculated based on the projection of the orientation angle in the world coordinate system.

[0011] The capsule local coordinates are corrected to the capsule world coordinates in the world coordinate system;

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

[0013] The pose of the control magnet is indicated, and the pose of the control magnet includes magnet world coordinates and / or magnet attitude information;

[0014] The pose of the capsule endoscope is indicated, which includes capsule world coordinates and / or capsule posture information.

[0015] As a further improvement to the present invention, the following steps are also included:

[0016] The pose of the control magnet is represented as [Mx,My,Mz,Mh,Mv], where [Mx,My,Mz] are the world coordinates of the magnet, and [Mh,Mv] are the magnet attitude information.

[0017] The pose of the capsule endoscope is represented as [Cx,Cy,Cz,Ch,Cv,Cs], where [Cx,Cy,Cz] are the world coordinates of the capsule, and [Ch,Cv,Cs] are the capsule pose information.

[0018] 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;

[0019] The step of "establishing a world coordinate system" includes:

[0020] The origin of the world coordinate system is determined based on the coordinates of the movable range.

[0021] As a further improvement of the present invention, the midpoint of the movable range coordinates is taken as the origin of the world coordinate system.

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

[0023] Calculate the 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;

[0024] Set the value of the magnet's world coordinates to the first set of offsets.

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

[0026] Calculate the second set of offsets of the origin of the second local coordinate system relative to the origin of the world coordinate system;

[0027] The difference between the capsule's local coordinates and the second set of offsets is used as the value of the capsule's world coordinates.

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

[0029] The step of "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:

[0030] The vertical projections of the capsule endoscope and the control magnet in the XY plane of the world coordinate system are aligned.

[0031] 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;

[0032] Calculate the X-axis difference between the first alignment coordinate and the second alignment coordinate in the X-axis direction, and the Y-axis difference in the Y-axis direction.

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

[0034] The step of "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:

[0035] Obtain the hardware parameters of the magnetically controlled capsule system;

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

[0037] To achieve one of the above-mentioned objectives, one embodiment of the present invention provides a magnetically controlled capsule system, which includes a control magnet and a capsule endoscope, and further includes:

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

[0039] The second acquisition module is used to acquire the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system;

[0040] The modeling module is used to establish the world coordinate system;

[0041] A magnet position correction module is used to correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system;

[0042] A control magnet attitude correction module is used to calculate the magnet attitude information of the control magnet in the world coordinate system based on the projection of the orientation angle in the world coordinate system.

[0043] The capsule endoscope position correction module is used to correct the capsule local coordinates to the capsule world coordinates in the world coordinate system;

[0044] The 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 to calculate the capsule attitude information of the capsule endoscope in the world coordinate system.

[0045] A control magnet representation module is used to represent the pose of the control magnet, wherein the pose of the control magnet includes magnet world coordinates and / or magnet attitude information;

[0046] A capsule endoscope representation module is used to represent the pose of the capsule endoscope, the pose of which includes capsule world coordinates and / or capsule posture information.

[0047] To achieve one of the above-mentioned objectives, one embodiment of the present invention provides an electronic device, comprising:

[0048] Storage module, used to store computer programs;

[0049] The processing module, when executing the computer program, can implement the steps in the above-described pose calibration representation method for the magnetically controlled capsule system.

[0050] To achieve one of the above-mentioned objectives, one embodiment of the present invention provides a readable storage medium storing a computer program that, when executed by a processing module, can implement the steps in the above-described pose calibration representation method for a magnetically controlled capsule system.

[0051] Compared with the prior art, the present invention has the following beneficial effects: it integrates the two systems corresponding to the control magnet and the capsule endoscope, and represents the state of the entire magnetically controlled capsule system in a unified and intuitive way, thereby facilitating efficient and accurate closed-loop control of the capsule endoscope to achieve gastrointestinal examination, expanding the control methods and application scenarios of the capsule endoscope, and improving the accuracy and precision of medical auxiliary diagnosis.

[0052] Furthermore, by uniformly calibrating the world coordinates of the magnet, the magnet's attitude information, the capsule's world coordinates, and the capsule's attitude information within the world coordinate system, and converting Euler angles into capsule attitude information, the capsule's attitude can be intuitively displayed, thus facilitating actual control and operation. Once the correspondence between the control magnet and the capsule endoscope systems is determined and the two systems are matched, subsequent work does not require repeated coordinate system conversions. Attached Figure Description

[0053] Figure 1 This is a flowchart of a pose calibration representation method according to an embodiment of the present invention;

[0054] Figure 2 This is a schematic diagram of the structure of a magnetically controlled capsule system according to an embodiment of the present invention applied to the human body;

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

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

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

[0058] Figure 6 This is a schematic diagram of the Euler angles of a capsule endoscope according to an embodiment of the present invention;

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

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

[0061] Figure 9 This is a schematic diagram of a magnetically controlled capsule system according to an embodiment of the present invention;

[0062] Among them, 1000 is the magnetically controlled capsule system; 100 is the magnetic control system; 200 is the capsule positioning system; 201 is the capsule endoscope; 300 is the bed surface; 400 is the human body; 10 is the control magnet; 20 is the signal transmission module; 30 is the storage module; 40 is the processing module; 50 is the magnetic sensor; 60 is the accelerometer; 70 is the signal transmission module; 80 is the camera module; and 90 is the communication bus. Detailed Implementation

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

[0064] One embodiment of the present invention provides a magnetically controlled capsule system and its pose calibration method for accurately controlling the movement of a capsule endoscope in a closed-loop manner. The magnetically controlled capsule system is a device applied to the human body, such as a wireless capsule endoscope or invasive medical device, used to locate the position of a wireless capsule inside the body. This method unifies the control magnet and the capsule endoscope into the same world coordinate system, facilitating subsequent accurate control of the wireless capsule.

[0065] The magnetically controlled 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, and the capsule positioning system 200 is used to position the capsule endoscope 201. The capsule endoscope 201 is equipped with a sensor module, which includes a magnetic sensor 50 for detecting magnetic fields, such as a Hall sensor, a magnetoresistive sensor (AMR, GMR, TMR), 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 also has magnetic components inside. The position and orientation of the capsule endoscope 201 are controlled by the magnetic control system 100 through the force exerted by the magnetic source on the magnetic components.

[0066] Figure 1 This application provides a method for pose calibration representation of a magnetically controlled capsule system 1000 according to one embodiment. Figure 2 This is a structural diagram of a magnetically controlled capsule system 1000 according to one embodiment of this application. The capsule endoscope 201 is located inside a human body 400, which lies flat on a bed surface 300. A magnetic control system 100 is provided outside the human body 400. During the examination, the magnetic field emitted by the magnet 10 is controlled to control the movement of the capsule endoscope 201 inside the human body 400.

[0067] Although this application provides the method operation steps as described in the following embodiments or flowcharts, the execution order of these steps is not limited to the execution order provided in the embodiments of this application, based on conventional or non-creative labor, where there is no logically necessary causal relationship between the steps.

[0068] The specific method for representing the pose calibration of the magnetically controlled capsule system 1000 includes the following steps:

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

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

[0071] 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.

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

[0073] In the first local coordinate system, the magnet's local coordinates include parameter values ​​in the three directions of the X, Y, and Z axes: [mag x mag y mag z The control magnet 10 has axisymmetry, and the orientation angle can be determined based on the orientation angle of the N pole in the magnetization direction. The orientation angle can be taken as a unit vector of its direction, or it can be normalized later and used in the calculation. Here, the orientation angle of the control magnet 10 is taken as its unit vector: [p x ,p y ,p z ].

[0074] Furthermore, based on the travel trajectory of the servo motor, the movable range of the control magnet 10 in each direction can be calculated. The endpoints of these ranges are the limit positions of the control magnet 10, which can be denoted as: {X:[mag xmin ,mag xmax ],Y:[mag ymin ,mag ymax ],Z:[mag zmin ,mag zmax ]}.

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

[0076] The capsule local coordinates are the parameter values ​​of the capsule endoscope 201 in the three directions including the X, Y, and Z axes: [cap x ,cap y ,cap z Euler angles include roll, yaw, and pitch, reflecting the attitude and orientation of the capsule endoscope 201. Roll, yaw, and pitch can be denoted as [θ0, θ1, θ2] respectively. The Euler angles of the capsule endoscope 201 can be referenced... Figure 6 As shown.

[0077] 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 examination bed surface 300, and the height position of its origin is relatively fixed, but the XY horizontal plane of this local coordinate system may move.

[0078] The capsule's local coordinates and Euler angles can be obtained through the relevant software interface functions of the capsule positioning system 200, allowing the acquisition of position and attitude angle data for the capsule endoscope 201. Position data is accurate to 1 mm, and attitude angle data can be represented in floating-point radians to avoid loss of precision in matrix calculations.

[0079] The capsule endoscope 201 may be equipped with a triaxial magnetic sensor 50 and a triaxial accelerometer 60. Several sets of magnetic positioning devices are set on the outside of the human body 400 to work in conjunction with the capsule endoscope 201 to calculate the position and orientation of the capsule endoscope 201.

[0080] Step 103: Establish the world coordinate system.

[0081] The world coordinate system is used to integrate 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 orientation angle of the control magnet 10, and the capsule local coordinates and Euler angles of the capsule endoscope 201 into the same coordinate system.

[0082] The pose calibration representation method of the magnetically controlled capsule system 1000 in this embodiment involves a calibration process performed during the assembly of the magnetically controlled system 100 and the capsule positioning system 200, ensuring that the coordinate systems of the two systems are matched to the same world coordinate system. If subsequent upgrades to functional modules or relocation of the positioning system lead to changes in the world coordinate system, the method described in this embodiment can be used for recalibration. After the initial calibration, the correspondence between the coordinate systems of the magnetically controlled system 100 and the positioning system is determined. Through the algorithm conversion described in this embodiment, the first local coordinate system and the second local coordinate system are matched with the world coordinate system, forming a unified world coordinate system.

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

[0084] Specifically, the midpoint of the movable range coordinates is taken as the origin of the world coordinate system. Figure 3 As shown, specifically, the origin [mag] x0 ,mag y0 ,mag z0 The following formula can be used for calculation:

[0085]

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

[0087] The magnet attitude information of the control magnet in the world coordinate system is calculated based on the projection of the orientation angle into the world coordinate system.

[0088] After the world coordinate system is established as described above, the 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 is calculated, and then the value of the world coordinates of the magnet is set as the first set of offsets.

[0089] Specifically, after calibrating the origin of the world coordinate system, the position [Mx, My, Mz] of the control magnet 10 in the world coordinate system is represented as follows:

[0090]

[0091] Z0 is a constant correction height introduced for ease of application. It can be set to Z0 = 0 or to the height from the origin to the inspection bed surface 300, which is equivalent to taking the inspection bed surface 300 as the plane with Mz = 0.

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

[0093] In addition, Figure 3 As shown, the magnet attitude information of the control magnet 10 in the world coordinate system is calculated based on the projection of the orientation angle in the world coordinate system.

[0094] The control magnet exhibits axisymmetry, and the orientation angle [Mh, Mv] of the magnetization direction N pole is only related to the coordinate axis definitions in the world coordinate system. The magnet attitude information of the control magnet 10 in the world coordinate system is similar to a spherical coordinate angle representation. Specifically, the angle between the magnetization direction vector of the control magnet 10 and the positive Z-axis can be defined as the vertical tilt angle M. v (Value range [0, +180] degrees); Define the angle between the projection vector of the magnetization direction vector of the control magnet 10 onto the XY plane and the positive Y-axis as the horizontal azimuth angle M. h (Value range [-180, +180] degrees), and increases clockwise, such as M in the positive Y-axis direction. h=0, M in the positive X-axis direction h =90, M when Y-axis is negative h =±180, M in the negative X-axis direction h =-90.

[0095] Specifically,

[0096]

[0097] Wherein, [px,py,pz] are the projection components of the unit vector of the control magnet 10 in the magnetization direction of the N pole onto the XYZ coordinate axes.

[0098] Optionally, the control magnet is fixedly connected to the control mechanism. The zero points of the vertical and horizontal angles are calibrated by photoelectric switches at specific angle positions. Then, the relative rotation state of the servo motor that drives the control magnet to rotate is directly equivalently converted, which can accurately obtain the angle state data without having to determine the above-mentioned attitude angle by measuring the direction of the magnetic field of the control magnet on site.

[0099] Step 105: Correct the capsule local coordinates to the capsule world coordinates in the world coordinate system;

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

[0101] Calculate a second set of offsets between the origin of the second local coordinate system and the origin of the world coordinate system, the second set of offsets including the X-axis difference, the Y-axis difference, and the Z-axis difference;

[0102] The difference between the capsule's local coordinates and the second set of offsets is used as the value of the capsule's world coordinates.

[0103] The calculation method for the second set of offsets can be found in [reference]. Figure 5 As shown. Specifically, the vertical projections of the capsule endoscope 201 and the control magnet 10 in the XY plane of the world coordinate system are aligned. The control magnet 10 is appropriately raised to reduce the attraction on the capsule endoscope 201, preventing the capsule endoscope 201 from translating or rolling when the control magnet 10 moves. The control magnet is then rotated so that the magnetization direction is vertically upward, i.e., the N pole is vertically upward. The capsule endoscope 201 is then moved to a position near the bottom of the control magnet. The control magnet 10 moves in the XY plane, and adjustments are made through 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 are completely aligned, completing the XY plane calibration and alignment of the capsule positioning system.

[0104] Obtain the first alignment coordinates of the capsule endoscope 201 in the second local coordinate system at this time [cap xu ,cap yu ], and the second alignment coordinates [M] of the control magnet 10 in the world coordinate system. xu M yu The subscript u indicates that the control magnet has moved directly above the capsule. At this time, the capsule is directly below the control magnet, marked by the subscript u (under).

[0105] Calculate the first alignment coordinates [cap xu ,cap yu ] and the second alignment coordinate [M xu M yu The X-axis difference cap in the X-axis direction x0 and the Y-axis difference cap in the Y-axis direction. y0 .

[0106] cap x0 and cap y0 This refers to the deviation of the coordinate values ​​on the X and Y axes between the second local coordinate system and the world coordinate system, thus unifying the two into a single coordinate system for comparison.

[0107] The calculation of the second set of offsets is set at the beginning of system operation. After the calculation of the values ​​of this second set of offsets, it is not necessary to move the control magnet 10 above the capsule endoscope 201 during subsequent use. x0 and cap y0 This will not be calculated again in subsequent use. The position of the capsule endoscope 201 can be arbitrary, and the capsule local coordinates of the capsule endoscope 201 in the second local coordinate system are as described above [cap x ,cap y ,cap z ].

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

[0109] Obtain the hardware parameters of the magnetically controlled capsule system 1000;

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

[0111] The Z-axis calibration of the capsule endoscope 201 in the world coordinate system is crucial, involving the balance of capsule gravity, buoyancy, friction, magnetic attraction, and torque. The Z-axis distance between the control magnet 10 and the capsule endoscope 201 directly determines the magnitude of the magnetic attraction, and the relationship between the magnetic force and the distance r is highly nonlinear. The distance affects the force balance of the capsule endoscope 201, causing it to switch between different states such as sinking to the bottom, floating on the surface, and being attached to the top. Accurate Z-axis calibration facilitates real-time acquisition of the height and distance of the capsule endoscope 201, providing data for subsequent control actions.

[0112] The origin calibration of the second local coordinate system depends on the hardware settings of the control system and the capsule positioning system 200, so its specific value is determined with reference to the parameters of the specific equipment.

[0113] Taking the example where 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 height difference between the origin of the first local coordinate system and the inspection bed surface 300 and the height difference between the inspection bed surface 300 and the origin of the second local coordinate system is used as the Z-axis difference value.

[0114] Specifically, the formula for calculating the second set of offsets is as follows:

[0115]

[0116] Where Z1 is the height difference between the origin of the first local coordinate system and the inspection bed surface 300, and Z2 is the height difference between the inspection bed surface 300 and the origin of the second local coordinate system.

[0117] Combining the above steps of "taking the difference between the capsule's local coordinates and the second set of offsets as the value of the capsule's world coordinates", the formula for calculating the capsule's world coordinates [Cx, Cy, Cz] is as follows:

[0118]

[0119] In addition, Figure 7 As shown, the projection of the capsule endoscope 201 in the world coordinate system is determined based on the Euler angles, and the capsule attitude information of the capsule endoscope 201 in the world coordinate system is calculated. Converting the Euler angles into capsule attitude information can intuitively display the capsule's attitude, thereby facilitating actual control and operation.

[0120] Specifically, the capsule attitude information of the capsule endoscope in the world coordinate system is represented similarly to spherical coordinate angles. Numerically, it can be defined similarly to magnet attitude information, converting Euler angles into orientation angles [Ch, Cv] and capsule spin angle Cs (which can be corrected for a fixed phase difference related to the positive direction of the lens).s0 ).

[0121] In other words, the capsule attitude information of the capsule endoscope 201 is represented by [Ch,Cv,Cs], where: h represents the horizontal azimuth angle, v represents the vertical tilt angle, and s represents the capsule spin angle. Figure 7 As shown.

[0122] Define the angle between the head of the capsule endoscope 201 and the positive Z-axis as the capsule vertical tilt angle C. v (Value range [0, +180] degrees); Define the angle between the projection vector of the head orientation of the capsule endoscope 201 onto the XY plane and the positive Y-axis as the capsule horizontal azimuth angle C. h (Value range [-180, +180] degrees), and increases clockwise (C in the positive Y-axis direction). h =0, C in the positive X-axis direction h =90, C when the Y-axis is negative h =±180, C in the negative X-axis direction h =-90); The capsule spin angle is the orientation angle of the lens of the capsule endoscope, and the capsule spin angle C is defined as the angle when the lens of the capsule endoscope 201 captures an image in an upright position. s =0, increasing clockwise, the formula for calculating the attitude information [Ch,Cv,Cs] of the capsule endoscope 201 is:

[0123]

[0124] The projection of the Z-axis of the second local coordinate system onto the world coordinate system is:

[0125] P = R·[0 0 1] T =[p x p y p z ] T

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

[0127]

[0128] The rotation matrices Z(roll), Y(pitch), and X(yaw) are denoted as follows:

[0129]

[0130] Among them, c k ≡cos(θ k ), s k ≡sin(θ k ), θk Let k be the corresponding Euler angles mentioned above, where k = 0, 1, 2.

[0131] The specific value of R in Formula 1 can be found in Formula 2. For example, R in Formula 1... 20 The result of this query in Formula 2 is c0s1c2+s0s2.

[0132] Step 106: Indicate the pose of the control magnet, the pose of the control magnet includes magnet world coordinates and / or magnet attitude information;

[0133] After step 104, the pose of the control magnet 10 in the world coordinate system can be represented as:

[0134] [Mx,My,Mz,Mh,Mv], where [Mx,My,Mz] are the world coordinates of the magnet, and [Mh,Mv] are the magnet's attitude information.

[0135] Step 107: Indicate the pose of the capsule endoscope, which includes capsule world coordinates and / or capsule posture information.

[0136] After step 105, the pose of the capsule endoscope 201 in the world coordinate system can be represented as:

[0137] [Cx,Cy,Cz,Ch,Cv,Cs], where [Cx,Cy,Cz] are the world coordinates of the capsule, and [Ch,Cv,Cs] are the capsule's pose information.

[0138] 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 thus intuitively represent the state of the entire magnetically controlled capsule system 1000. This provides convenience for subsequent efficient and accurate closed-loop control of the capsule endoscope 201 to realize gastrointestinal examination, expands the control methods and application scenarios of the capsule endoscope 201, and improves the accuracy and precision of medical auxiliary diagnosis.

[0139] In one embodiment, a magnetically controlled capsule system 1000 is provided, such as Figure 8 As shown. In addition to controlling the magnet and capsule endoscope, the magnetically controlled capsule system 1000 may also include:

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

[0141] The second acquisition module is used to acquire the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system;

[0142] The modeling module is used to establish the world coordinate system;

[0143] A magnet position correction module is used to correct the local coordinates of the magnet to the world coordinates of the magnet in the world coordinate system;

[0144] A control magnet attitude correction module is used to calculate the magnet attitude information of the control magnet in the world coordinate system based on the projection of the orientation angle in the world coordinate system.

[0145] The capsule endoscope position correction module is used to correct the capsule local coordinates to the capsule world coordinates in the world coordinate system;

[0146] The 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 to calculate the capsule attitude information of the capsule endoscope in the world coordinate system.

[0147] A control magnet representation module is used to represent the pose of the control magnet, wherein the pose of the control magnet includes magnet world coordinates and / or magnet attitude information;

[0148] A capsule endoscope representation module is used to represent the pose of the capsule endoscope, the pose of which includes capsule world coordinates and / or capsule posture information.

[0149] 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] are the world coordinates of the magnet, and [Mh, Mv] are the magnet attitude information.

[0150] 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] are the capsule world coordinates, and [Ch,Cv,Cs] are the capsule pose information.

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

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

[0153] In one embodiment, the control magnet position correction module calculates 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, and sets the value of the world coordinates of the magnet to the first set of offsets.

[0154] In one embodiment, the capsule endoscope position correction module calculates a second set of offsets between the origin of the second local coordinate system and the origin of the world coordinate system; and uses the difference between the capsule local coordinates and the second set of offsets as the value of the capsule world coordinates.

[0155] In one embodiment, the capsule endoscope posture correction module makes the vertical projections of the capsule endoscope 201 and the control magnet 10 coincide in the XY plane of the world coordinate system;

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

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

[0158] In one embodiment, the magnetically controlled capsule system 1000 further includes a data interface for obtaining hardware parameters of the magnetically controlled capsule system 1000.

[0159] The capsule endoscope coordinate correction module determines the Z-axis difference based on the hardware parameters.

[0160] The magnetically controlled capsule system 1000 may further include computing devices such as computers, laptops, handheld computers, 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 will understand that the schematic diagram is merely an example of the magnetically controlled capsule system 1000 and does not constitute a limitation on the terminal device of the magnetically controlled capsule system 1000. It may include more or fewer components than illustrated, or combine certain components, or use different components. For example, the magnetically controlled capsule system 1000 may also include input / output devices, network access devices, buses, etc.

[0161] It should be noted that for details not disclosed in the magnetically controlled capsule system 1000 of this embodiment, please refer to the details disclosed in the pose calibration representation method of the magnetically controlled capsule system 1000 of this embodiment.

[0162] 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 state of the entire magnetically controlled capsule system 1000. This facilitates 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.

[0163] like Figure 9 The diagram shown is a schematic representation of a magnetically controlled capsule system 1000 according to an embodiment of the present invention. The magnetically controlled capsule system 1000 further includes the aforementioned magnetic control system 100, capsule positioning system 200, control magnet 10 and capsule endoscope 201, processing module 40, storage module 30, various modules within the capsule endoscope 201, and a computer program stored in the storage module 30 and executable on the processing module 40, such as the aforementioned pose calibration representation method program. When the processing module 40 executes the computer program, it implements the steps in the various pose calibration method embodiments described above, for example... Figure 1 The steps are shown.

[0164] The magnetic source of the control magnet 10 is controlled and driven to move to a designated position by a servo motor and transmission mechanism. The transmission data of the servo motor is obtained through the data interface of the magnetic control system 100. After a fixed proportional conversion formula, the original data of the position, attitude angle state of the control magnet 10 are obtained. The position data is accurate to 1 mm, and the angle data is accurate to 1 degree.

[0165] The control magnet 10 is fixedly connected to the transmission mechanism. By using photoelectric switches at certain positions, such as some special vertical and horizontal angles, to calibrate the zero point, and then by converting the drive amount of the servo motor that drives the control magnet 10, the control magnet 10 can be accurately driven to move to the target position in the world coordinate system without having to determine its attitude angle by measuring the magnetic field direction of the control magnet 10 on site.

[0166] The capsule endoscope 201 may include a magnetic field sensor, an accelerometer 60, a signal transmission module 70, a magnetic component, and a camera module 80. As described above, the magnetic field sensor, accelerometer 60, and magnetic component can work collaboratively with an internal triaxial magnetic sensor 50, a triaxial accelerometer 60, an IMU sensor, and multiple external magnetic positioning devices to calculate the position and orientation of the capsule endoscope 201. The movement of the capsule endoscope 201 is driven by controlling the action of the magnet 10 on the magnetic component. The signal transmission module 70 transmits information to an external processing module 40 or server. After the external drive propels the wireless capsule to a designated position, the camera module 80 captures images of the inside of the human body 400 and transmits them to the outside via the signal output module, completing the internal imaging.

[0167] The control magnet 10 can be appropriately raised to weaken the suction force on the capsule, or lowered to increase the suction force on the capsule, and the capsule endoscope 201 can be switched between different scenarios such as sinking to the bottom, floating on the water surface, and being suctioned to the top.

[0168] The magnetically controlled capsule system 1000 may further 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 a server. The signal transmission module 70 and the signal transmission module 20 can transmit data wirelessly, such as via Bluetooth, Wi-Fi, or Zigbee. The communication bus 90 is used to establish a connection between the control magnet 10, the signal transmission module 20, the processing module 40, and the storage module 30. The communication bus 90 may include a path for transmitting information between the control magnet 10, the signal transmission module 20, the processing module 40, and the storage module 30.

[0169] In addition, the present invention also proposes an electronic device, which includes a storage module 30 and a processing module 40. When the processing module 40 executes the computer program, it can implement the steps in the above-mentioned pose calibration representation method, that is, implement the steps in any of the above-mentioned pose calibration methods.

[0170] The electronic device may be part of the magnetically controlled capsule system 1000, a local terminal device, or part of a cloud server.

[0171] The processing module 40 can be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor. The processing module 40 is the control center of the magnetically controlled capsule system 1000, connecting all parts of the magnetically controlled capsule system 1000 via various interfaces and lines.

[0172] The storage module 30 can be used to store the computer program and / or modules. The processing module 40 realizes various functions of the magnetically controlled capsule system 1000 by running or executing the computer program and / or modules stored in the storage module 30 and calling the data stored in the storage module 30. The memory may mainly include a program storage area and a data storage area. The program storage area may store the operating system, at least one application program required for a function (such as sound playback function, image playback function, etc.), etc.; the data storage area may store data created according to the use of the mobile phone (such as audio data, phonebook, etc.). In addition, the memory may include high-speed random access memory, and may also include non-volatile memory, such as hard disk, memory, plug-in hard disk, smart media card (SMC), secure digital card (SD card), flash card, at least one disk storage device, flash memory device, or other volatile solid-state storage device.

[0173] For example, the computer program can be divided into one or more modules / units, which are stored in the storage module 30 and executed by the processing module 40 to complete the present invention. The one or more modules / units can be a series of computer program instruction segments capable of performing specific functions, which describe the execution process of the computer program in the pose calibration representation method of the magnetically controlled capsule system.

[0174] Furthermore, one embodiment of the present invention provides a readable storage medium storing a computer program. When the computer program is executed by the processing module 40, it can implement the steps in the above-described pose calibration representation method of the magnetically controlled capsule system, that is, implement the steps in any one of the technical solutions of the above-described pose calibration representation method of the magnetically controlled capsule system.

[0175] If the integrated modules of the magnetically controlled capsule system 1000 are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments of the present invention can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above.

[0176] The computer program includes computer program code, which can be in the form of source code, object code, executable file, or some intermediate form. The computer-readable medium can include any entity or device capable of carrying the computer program code, recording media, U disks, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added to or subtracted according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0177] It should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This way of describing the specification is only for clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

[0178] The detailed descriptions listed above are merely specific descriptions of feasible embodiments of the present invention, and are not intended to limit the scope of protection of the present invention. All equivalent embodiments or modifications made without departing from the spirit of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for calibrating and representing the position of a magnetically controlled capsule system, the magnetically controlled capsule system comprising a control magnet and a capsule endoscope, characterized in that, Includes the following steps: Obtain the local coordinates and orientation angle of the control magnet in a first local coordinate system, wherein the local coordinates of the magnet include the coordinates of the movable range 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; Establishing a world coordinate system includes: determining the origin of the world coordinate system based on the coordinates of the movable range; The local coordinates of the magnet are corrected to the world coordinates of the magnet in the world coordinate system; The magnet attitude information of the control magnet in the world coordinate system is calculated based on the projection of the orientation angle in the world coordinate system. The capsule local coordinates are corrected to the capsule world coordinates in the world coordinate system; The projection of the capsule endoscope in the world coordinate system is determined based on the Euler angles, and the capsule attitude information of the capsule endoscope in the world coordinate system is calculated. The pose of the control magnet is indicated, and the pose of the control magnet includes magnet world coordinates and / or magnet attitude information; The pose of the capsule endoscope is indicated, which includes capsule world coordinates and / or capsule posture information.

2. The pose calibration representation method according to claim 1, characterized in that, The magnet attitude information includes a horizontal azimuth angle and a vertical tilt angle, wherein the horizontal azimuth angle is the angle between the 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, and the vertical tilt angle is the angle between the magnetization direction vector of the control magnet and the positive Z-axis of the world coordinate system.

3. The pose calibration representation method according to claim 2, characterized in that, It also includes the following steps: The pose of the control magnet is represented 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.

4. The pose calibration representation method according to claim 1, characterized in that, The capsule attitude information includes a horizontal azimuth angle, a vertical tilt angle, and a capsule spin angle. The horizontal azimuth angle is the angle between the projection vector of the capsule endoscope head onto the XY plane of the world coordinate system and the positive Y-axis. The vertical tilt angle is the angle between the capsule endoscope head and the positive Z-axis of the world coordinate system. The capsule spin angle is the orientation angle of the capsule endoscope lens.

5. The pose calibration representation method according to claim 4, characterized in that, The method also includes the step of: representing the pose of the capsule endoscope, wherein the pose of the capsule endoscope is represented as [Cx,Cy,Cz,Ch,Cv,Cs], where [Cx,Cy,Cz] is the position 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 spin angle.

6. The pose calibration representation method according to claim 1, characterized in that, in, The midpoint of the movable range coordinates is taken as the origin of the world coordinate system.

7. The pose calibration representation method according to claim 1, 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 the 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's world coordinates to the first set of offsets.

8. The pose calibration representation method according to claim 1, characterized in that, in, The step of "correcting the capsule local coordinates to the capsule world coordinates in the world coordinate system" includes: Calculate the 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's local coordinates and the second set of offsets is used as the value of the capsule's world coordinates.

9. The pose calibration representation method according to claim 8, characterized in that, in, The second set of offsets includes the X-axis difference and the 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: The vertical projections of the capsule endoscope and the control magnet in the XY plane of the world coordinate system are aligned. 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; Calculate the X-axis difference between the first alignment coordinate and the second alignment coordinate in the X-axis direction, and the Y-axis difference in the Y-axis direction.

10. The pose calibration representation method according to claim 9, characterized in that, The second set of offsets also includes the 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 based on the hardware parameters.

11. A magnetically controlled capsule system, comprising a control magnet and a capsule endoscope, characterized in that, Also includes: The first acquisition module is used to acquire the local coordinates and orientation angle of the control magnet in a first local coordinate system, 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 second acquisition module is used to acquire the capsule local coordinates and Euler angles of the capsule endoscope in the second local coordinate system; The modeling module is used to establish a world coordinate system, including: determining the origin of the world coordinate system based on the coordinates of the movable range; A magnet position correction module is used 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 is used to calculate the magnet attitude information of the control magnet in the world coordinate system based on the projection of the orientation angle in the world coordinate system. The capsule endoscope position correction module is used to correct the capsule local coordinates to the capsule world coordinates in the world coordinate system; The 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 to calculate the capsule attitude information of the capsule endoscope in the world coordinate system. A control magnet representation module is used to represent the pose of the control magnet, wherein 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, the pose of which includes capsule world coordinates and / or capsule posture information.

12. An electronic device, characterized in that, include: Storage module, used to store computer programs; The processing module, when executing the computer program, can implement the steps in the pose calibration representation method of the magnetically controlled capsule system according to any one of claims 1 to 10.

13. A readable storage medium storing a computer program, characterized in that, When executed by the processing module, the computer program can implement the steps in the pose calibration representation method of the magnetically controlled capsule system according to any one of claims 1 to 10.

Images