WO2023174427A1 - 磁控胶囊系统及其定量闭环控制方法 - Google Patents

磁控胶囊系统及其定量闭环控制方法 Download PDF

Info

Publication number
WO2023174427A1
WO2023174427A1 PCT/CN2023/082318 CN2023082318W WO2023174427A1 WO 2023174427 A1 WO2023174427 A1 WO 2023174427A1 CN 2023082318 W CN2023082318 W CN 2023082318W WO 2023174427 A1 WO2023174427 A1 WO 2023174427A1
Authority
WO
WIPO (PCT)
Prior art keywords
capsule
target
real
time
control
Prior art date
Application number
PCT/CN2023/082318
Other languages
English (en)
French (fr)
Inventor
黄志威
张行
袁文金
杨戴天杙
张皓
Original Assignee
安翰科技(武汉)股份有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 安翰科技(武汉)股份有限公司 filed Critical 安翰科技(武汉)股份有限公司
Priority to JP2024555034A priority Critical patent/JP2025510641A/ja
Publication of WO2023174427A1 publication Critical patent/WO2023174427A1/zh

Links

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/04—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor combined with photographic or television appliances
    • A61B1/041—Capsule endoscopes for imaging
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/00147—Holding or positioning arrangements
    • A61B1/00158—Holding or positioning arrangements using magnetic field
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/04—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor combined with photographic or television appliances
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/04—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor combined with photographic or television appliances
    • A61B1/045—Control thereof
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B1/00—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor
    • A61B1/273—Instruments for performing medical examinations of the interior of cavities or tubes of the body by visual or photographical inspection, e.g. endoscopes; Illuminating arrangements therefor for the upper alimentary canal, e.g. oesophagoscopes, gastroscopes

Definitions

  • the invention relates to the technical field of medical equipment, and in particular to a magnetically controlled capsule system and a quantitative closed-loop control method thereof.
  • In vivo device positioning technology such as wireless capsules, invasive medical devices and other in vivo positioning technologies, has received more and more attention.
  • the magnetically controlled capsule system uses magnetic force to drive the capsule to move in the body.
  • the doctor takes inspection images of the inner wall of the digestive tract through the built-in lens, determines the position and orientation of the capsule, and then uses an external control magnet to drive the capsule to continue moving to the next position.
  • the capsule spin Due to the extremely nonlinear and non-uniform spatial distribution characteristics of magnetism, the deformable environment of the digestive tract and the influence of friction, the capsule spin makes it difficult to determine the true orientation based on the image, making it impossible to accurately and quantitatively control the capsule to reach the target by relying solely on image visual feedback information. position and target attitude angle.
  • the existing positioning system can only serve as an auxiliary confirmation. The next movement of the capsule still requires doctors to judge based on experience, and the entire control process is not intuitive and precise enough.
  • the object of the present invention is to provide a magnetically controlled capsule system that accurately controls capsule movement in a closed-loop form and a quantitative closed-loop control method thereof.
  • one embodiment of the present invention provides a quantitative closed-loop control method for a magnetically controlled capsule system, which includes the following steps:
  • critical distance is the distance between the capsule and the control magnet when the capsule is suspended in the liquid
  • the status information of the capsule is calculated, wherein the status information includes a bottom-sinking state, a liquid-level suspension state and a ceiling-suction state.
  • the target location information includes target status
  • the orientation of the control magnet is adjusted so that the endoscope of the capsule faces a specified direction.
  • the distance between the control capsule and the control magnet is greater than the sum of the critical distance and the first threshold
  • the distance between the capsule and the control magnet is controlled to be less than the sum of the critical distance and the second threshold.
  • the step "obtaining the critical distance" includes:
  • the critical distance algorithm is based on the fact that the sum of the suction force between the control magnet and the capsule and the buoyancy force on the capsule is equal to the gravity of the capsule when the liquid surface is suspended, and the distance between the control magnet and the capsule is calculated. The distance between the capsules.
  • Control moves the control magnet to an initial position, wherein in the initial position, the control magnet and the capsule are on the same straight line perpendicular to the XY plane of the world coordinate system.
  • the movement mode includes a translation mode and/or a rolling mode
  • the movement mode of the control magnet is the translation mode and the capsule is not moving, the movement mode of the control magnet is switched to the tumbling mode.
  • the step "calculating the current movement trajectory of the control magnet based on the real-time position information and the target position information" includes:
  • the current movement trajectory is a translation along the current offset amount.
  • the step "calculating the current movement trajectory of the control magnet based on the real-time position information and the target position information" includes:
  • the long axis circumference is the length corresponding to the rotation of the capsule along its long axis;
  • the current movement trajectory includes the number of tumbling circles along the tumbling direction.
  • the target position range includes a preset distance
  • the real-time location information is within the target location range
  • the real-time location information is outside the target location range.
  • control magnet and the capsule are controlled to be on the same straight line perpendicular to the XY plane of the world coordinate system.
  • one embodiment of the present invention provides a quantitative closed-loop control method for a magnetically controlled capsule system, which includes the following steps:
  • the target attitude information calculate the target attitude range and the current rotation trajectory of the control magnet
  • real-time magnet attitude information of the control magnet is continuously acquired, wherein the real-time magnet attitude information, the real-time attitude information and the target attitude information are all located in the same world coordinate system.
  • the current rotation trajectory includes a current rotation amount, and the current rotation amount includes a first angle difference and a second angle difference;
  • the initial attitude includes a first included angle with the positive direction of the Z-axis
  • the target attitude information includes a second included angle with the positive direction of the Z-axis
  • the first angle difference is the difference between the second included angle and the third included angle.
  • the initial posture includes a third included angle between the XY plane and the positive direction of the Y axis
  • the target posture information includes a fourth included angle between the XY plane and the positive direction of the Y axis
  • the second angle difference is the fourth included angle. The product of the difference between the included angle and the third included angle and the fourth correction factor.
  • the target posture range includes a preset angle difference
  • the real-time attitude information is within the target attitude range
  • the real-time attitude information is outside the target attitude range.
  • one embodiment of the present invention provides a magnetically controlled capsule system, including: a first acquisition module, a first target acquisition module, a first calculation module, a first control module and a first judgment module;
  • the first acquisition module is used to continuously acquire the real-time location information of the capsule
  • the first target acquisition module is used to obtain the target position information of the capsule, and determine the target position range according to the target position information
  • the first calculation module is used to calculate the current movement trajectory of the control magnet based on the real-time position information and the target position information;
  • the first control module is used to control the movement of the control magnet along the current movement trajectory
  • the first judgment module is used to judge whether the real-time position information is within the target range. When the control magnet stops moving and the real-time position information is outside the target position range, repeat the "first The calculation module calculates the current movement trajectory of the control magnet based on the real-time location information and the target location information. The first control module controls the control magnet to move along the current movement trajectory until the real-time location information is in the within the target location.
  • one embodiment of the present invention provides a magnetically controlled capsule system, including: a second acquisition module, a second target acquisition module, a second calculation module, a second control module and a second judgment module;
  • the second acquisition module is used to continuously acquire the real-time posture information of the capsule
  • the second target acquisition module is used to acquire the target posture information of the capsule
  • the second calculation module is used to determine the target attitude range and the current rotation trajectory of the control magnet according to the target attitude information
  • the second control module is used to control the movement of the control magnet along the current rotation trajectory
  • the second judgment module is used to judge whether the real-time location information is within the target range.
  • the magnet stops moving and the real-time attitude information is outside the target attitude range then repeat "The second calculation module calculates the target attitude range and the current rotation trajectory of the control magnet based on the target attitude information, and the second control module controls all The "control magnet moves along the current rotation trajectory" until the real-time attitude information is within the target attitude range.
  • an embodiment of the present invention provides an electronic device, including a storage module and a processing module.
  • the storage module stores a computer program that can be run on the processing module.
  • the processing module executes The computer program implements the steps in the quantitative closed-loop control method of the magnetically controlled capsule system described in claim 1.
  • one embodiment of the present invention provides a readable storage medium that stores a computer program.
  • the computer program is executed by the processing module, the quantitative control of the magnetically controlled capsule system according to claim 1 is realized. Steps in a closed-loop control approach.
  • the present invention has the following beneficial effects: the quantitative closed-loop control method of the magnetically controlled capsule system can quantitatively control the movement process of the capsule, so that the capsule accurately reaches the target position and/or target posture, and through Closed-loop quantitative closed-loop control method accurately controls the amount of capsule movement. And this method is used in the scenario of capsule movement controlled by permanent magnets, which avoids the problem of slow operation of permanent magnets due to untimely switching of speed and state. The control process is highly robust. In addition, this method can implement complex control logic. Carry out derivative digestive tract examination applications to facilitate the automated, intelligent inspection and derivative applications of the magnetically controlled capsule system.
  • Figure 1 is a flow chart of a magnetic control capsule system control method according to an embodiment of the present invention
  • Figure 2 is a flow chart of a magnetic control capsule system control method according to another embodiment of the present invention.
  • Figure 3 is a schematic structural diagram of a capsule according to an embodiment of the present invention after it has sunk to the bottom in the human body and its position has been adjusted;
  • Figure 4 is a schematic structural diagram of the capsule according to an embodiment of the present invention after it has sunk to the bottom in the human body and its posture has been adjusted;
  • Figure 5 is a schematic diagram of the magnetic field line distribution and the relative intensity of the vertical magnetic field of the control magnet according to an embodiment of the present invention
  • Figure 6 is a schematic diagram of the force balance state of a capsule in the human body according to an embodiment of the present invention.
  • Figure 7 is a schematic diagram of a capsule moving in a translational manner according to an embodiment of the present invention.
  • Figure 8 is a schematic diagram of a capsule moving in a tumbling manner according to an embodiment of the present invention.
  • Figure 9 is a schematic diagram of the capsule posture adjustment process according to an embodiment of the present invention.
  • Figure 10 is a module schematic diagram of a magnetically controlled capsule system according to an embodiment of the present invention.
  • magnetic control capsule system 1000, magnetic control capsule system; 100, magnetic control system; 200, capsule positioning system; 201, capsule; 300, bed surface; 400, human body; 10, control magnet; 20, signal transmission module; 30, storage module; 40. Processing module; 50. Magnetic sensor; 60. Acceleration sensor; 70. Signal transmission module; 80. Camera module; 90. Communication bus.
  • One embodiment of the present invention provides a magnetically controlled capsule system that accurately controls capsule movement in a closed-loop form and a quantitative closed-loop control method thereof, so that the capsule can accurately reach the target position and/or target posture, and in the scenario where the permanent magnet controls the capsule movement.
  • the magnetically controlled capsule system 1000 in this embodiment includes a magnetically controlled system 100, a capsule positioning system 200, a control magnet 10 and a capsule 201.
  • the magnetically controlled system 100 is used to control the movement of the capsule 201.
  • the capsule positioning system 200 is used to position the capsule 201.
  • the capsule 201 is equipped with a sensor module inside.
  • the sensor module includes a magnetic sensor 50 (magnetic sensor) for detecting a magnetic field.
  • the magnetic sensor can be: Hall sensor, anisotropic magnetoresistance (AMR) sensor, giant magnetoresistance (GMR) sensor. , tunnel magnetoresistance (TMR) sensor, etc.
  • the control magnet 10 includes a magnetic source for emitting a magnetic field, and a servo motor for controlling the movement of the magnetic source.
  • the magnetic source is preferably a permanent magnet.
  • the capsule 201 is a capsule endoscope, and a magnetic component is provided inside the capsule 201 to interact with the magnetic source of the control magnet 10 . Through the force exerted by the magnetic source on the magnetic components, the magnetic control system 100 controls the position and attitude of the capsule 201 .
  • Figures 1 and 2 are two examples of a quantitative closed-loop control method of a magnetically controlled capsule system 1000 according to an embodiment of the present application.
  • Figure 1 is used to adjust the position of the capsule 201
  • Figure 2 is used to adjust the posture of the capsule 201.
  • Figures 3 and 4 are schematic diagrams of the magnetically controlled capsule system 1000 according to an embodiment of the present application when applied to the human body 400.
  • Figure 3 completes the adjustment of the position of the capsule 201
  • Figure 4 completes the adjustment of the posture of the capsule 201.
  • the capsule 201 Located inside the human body 400 , the human body 400 lies flat on the bed 300 , and a magnetic control system 100 including a control magnet 10 is provided outside the human body 400 . During the inspection, the magnetic field emitted by the magnet 10 is controlled to control the movement of the capsule 201 in the human body 400 .
  • the human body 400 lies above the bed 300, the upper part of the human body 400 is the forward direction, and the lower part of the human body 400 is the reverse direction.
  • the control magnet 10 can control the capsule 201 in various directions of the human body 400 . Taking Figure 3 and Figure 4 as an example, the control magnet 10 is set to be located above the human body 400, and the direction of buoyancy is upward and the direction of gravity is downward.
  • the left direction parallel to the direction of the paper is defined as the left direction
  • the right direction parallel to the direction of the paper is defined as the right direction.
  • the specific quantitative closed-loop control method of the magnetically controlled capsule system 1000 includes the following steps:
  • Step 101 Continue to obtain the real-time location information of the capsule 201.
  • the real-time magnet position information of the control magnet 10 can also be continuously obtained.
  • the real-time magnet position information, the real-time position information and the following target position information are all located in the same world coordinate system.
  • the real-time position information of the capsule 201 can be obtained through the capsule positioning system 200, and the real-time magnet position information can be obtained through Acquired through the magnetic control system 100, the coordinate system of the magnetic control system 100 is matched with the coordinate system of the capsule positioning system 200 to form a unified world coordinate system.
  • the capsule position information coordinates obtained by the capsule positioning system 200 are converted into the world coordinate system, and the magnet position information coordinates obtained by the magnet control system 100 are also converted into the world coordinate system, so that they can be adjusted uniformly.
  • the 6-DOF status data given by the capsule positioning system 200 is [Cx, Cy, Cz, Ch, Cv, Cs], and the real-time position information uses [Cx, Cy, Cz], which represents the XYZ of the capsule 201 in the world coordinate system. coordinate.
  • [Ch, Cv, Cs] is used to represent the posture information of the capsule 201, which will be described in detail below when discussing the posture adjustment method of the capsule 201.
  • the 5-DOF state data given by the magnetic control system 100 is expressed as [Mx, My, Mz, Mh, Mv], and the real-time magnet position adopts [Mx, My, Mz] to represent the XYZ of the control magnet 10 in the world coordinate system. coordinate.
  • [Mh, Mv] will also be involved in the process of position adjustment below, where [Mh, Mv] represents the N pole of the magnetic field direction of the control magnet 10
  • the orientation angle of the , +180] degree is the angle between the magnetization direction vector of the control magnet 10 and the positive direction of the Z axis.
  • Step 102 Obtain the target position information of the capsule 201, and determine the target position range based on the target position information.
  • the target position information can be expressed as [Tx, Ty, Tz]. Since it is inefficient to accurately move to this point, when the capsule 201 reaches a certain range near the target position information, it can be regarded as having moved to the target position.
  • control magnet 10 Before performing step 103, the control magnet 10 can be moved to the initial position, and then the state of the capsule 201 can be judged and adjusted.
  • Moving to the initial position is to control the control magnet 10 to move to the initial position, where in the initial position, the control magnet 10 and the capsule 201 are on the same straight line perpendicular to the XY plane of the world coordinate system. That is to say, at this time, the coordinates of the control magnet 10 and the capsule 201 on the X-axis and the Y-axis are the same.
  • the XY plane here refers to the above world coordinate system.
  • the distance between the control magnet 10 and the capsule 201 in the Z-axis direction can be adjusted in time according to the control system status feedback information. Keep this distance within a safe distance.
  • Judging and adjusting the status of capsule 201 includes steps:
  • the critical distance is the distance between the capsule 201 and the control magnet 10 when the capsule 201 is suspended in the liquid
  • the status information of the capsule 201 is calculated, where the status information includes a bottom-sinking state, a liquid level suspension state and a ceiling-suction state.
  • the force analysis of the three states can be referred to Figure 6.
  • the force conditions of the capsule 201 are in the bottom-sinking state, the liquid surface suspension state and the ceiling-suction state, respectively, where G is the capsule. 201 gravity, Ff is the buoyancy force on the capsule 201, Fm is the suction force of the control magnet 10 on the capsule 201, and N is the support force of the inner wall of the digestive tract on the capsule 201.
  • the bottom-sinking state can also be shown in Figure 2 or 3.
  • the digestive tract includes an upper wall, a lower wall and liquid. In the bottom-sinking state, the capsule 201 is against the lower wall.
  • the capsule 201 In the ceiling-suction state, the capsule 201 is against the upper wall, and the liquid level is In the suspended state, the sum of the suction force and the buoyancy force of the control magnet 10 is equal to the gravity of the capsule 201 . In addition, when the suction force is removed, the capsule 201 will sink to the bottom.
  • the step “obtaining critical distance” includes:
  • the critical distance is obtained based on a critical distance algorithm or based on measurement data.
  • the critical distance algorithm calculates the distance between the control magnet 10 and the capsule 201 based on the fact that the sum of the suction force between the control magnet 10 and the capsule 201 and the buoyancy force experienced by the capsule 201 is equal to the gravity of the capsule 201 when the liquid surface is suspended. distance.
  • suction force of the control magnet 10 on the capsule 201 directly below can be expressed as:
  • M and m are the magnetic moments of the control magnet 10 and the built-in magnet of the capsule 201 respectively, r is the center distance between the control magnet 10 and the built-in magnet of the capsule 201, and ⁇ 0 is the vacuum magnetic permeability.
  • ⁇ is the density of the medium liquid in which the capsule 201 is located
  • V is the volume of the capsule 201
  • g is the gravitational acceleration constant
  • m c is the mass of the capsule 201
  • Z 0 is the critical distance
  • the critical distance can be calculated through this formula.
  • the critical distance refers to the height distance in the Z-axis direction, which is the critical height.
  • control magnet 10 and capsule 201 are affected by magnetization, resulting in changes in the size of the magnetic moment.
  • the mass and volume of the capsule 201 are affected by changes in the model. It is more convenient and reliable to conduct experimental measurements of Z 0 based on the actual types of control magnet 10 and capsule 201. .
  • the height difference dZ between the center of the control magnet 10 and the capsule 201 is expressed as: dZ ⁇ M z -C z , where Mz is the coordinate value of the control magnet 10 in the Z-axis direction, and Cz is the coordinate value of the capsule 201 in the Z-axis direction. .
  • the height difference can be calculated and obtained in real time based on the status data of the control magnet 10 and the capsule 201 .
  • the current state of the capsule 201 is determined by dZ. When dZ>Z 0 + ⁇ a , the capsule 201 is in the bottom-sinking state; when dZ ⁇ Z 0 - ⁇ a , the capsule 201 is in the ceiling-suction state.
  • the redundant parameter ⁇ a has a value of 1 to 2 cm, which is used to eliminate positioning errors and noise interference caused by digestive tract peristalsis.
  • the target location information includes target status
  • the orientation of the control magnet 10 is adjusted so that the lens end of the capsule 201 faces a specified direction.
  • the capsule 201 is controlled to switch to the bottom-sinking or ceiling-suctioning state, and the control magnet 10 is driven to change its height in the up and down direction.
  • adjust dZ Make it satisfy dZ ⁇ Z 0 - ⁇ b ;
  • adjust dZ to make it satisfy dZ>Z 0 + ⁇ b .
  • the redundant parameter ⁇ b has a value of 3cm, which is used to eliminate positioning errors and noise interference caused by digestive tract peristalsis, ensure the success rate of control action execution, and switch the capsule 201 to the target state.
  • the capsule 201 relies on the variable support force and friction of the digestive tract to control the balance of the magnetic force of the magnet 10, the gravity of the capsule 201, and the buoyancy of the liquid.
  • the capsule 201 leaving the wall of the digestive tract cannot reach equilibrium at any position on the Z-axis, so it is only possible to stabilize the suspension of the capsule 201 on the liquid surface through the negative feedback mechanism of buoyancy near the liquid surface.
  • the distance between the capsule 201 and the control magnet 10 is controlled to be greater than the sum of the critical distance and the first threshold.
  • the first threshold is adjusted according to the on-site environment.
  • the distance between the control capsule 201 and the control magnet 10 is less than the sum of the critical distance and the second threshold.
  • the second threshold is adjusted according to the on-site environment.
  • the camera module 80 is at one end of the capsule 201.
  • the camera module 80 needs to shoot upward.
  • the magnet 10 is controlled to adopt different magnetic pole orientations according to the state of the capsule 201 at this time.
  • the camera module 80 is located at the N end of the magnetic pole of the capsule 201.
  • the magnetic pole S end of the magnet 10 is controlled to face the capsule 201.
  • the N end of the magnetic pole of the magnet 10 is controlled to face the capsule 201.
  • step 103 you can first perform the following steps:
  • the movement mode of the control magnet 10 is obtained, and the movement mode includes a translation mode and/or a rolling mode.
  • the capsule 201 can move linearly in a translational manner, or can roll forward in a tumbling manner.
  • the translational manner is shown in Figure 7, and the tumbling manner is shown in Figure 8.
  • the panning mode and/or the rolling mode can be selected manually or automatically. The two situations will be discussed below.
  • the capsule position movement in the approximate XY plane with a small slope can be achieved by controlling the magnet 10 to drag the capsule 201 for translation.
  • the real-time position information of the capsule 201 is [Cx, Cy, Cz]
  • the capsule The target position information of 201 is [Tx, Ty, Tz]. Since the position of the capsule 201 in the Z-axis direction cannot be stably and continuously controlled, only the capsule position in the XY plane is controlled.
  • the translation mode movement is shown in Figure 7.
  • Step 103 Calculate the current movement trajectory of the control magnet 10 according to the real-time position information and the target position information.
  • the value range of the first correction factor and the second correction factor is between 0 and 1, and the expression is 0 ⁇ x , ⁇ y ⁇ 1.
  • ⁇ x ⁇ y
  • Step 104 Control the control magnet 10 to move along the current movement trajectory.
  • the current movement trajectory is a translation along the current offset amount.
  • the tumbling motion of the capsule 201 is usually used in situations where the capsule 201 cannot be effectively translated. Such situations include but are not limited to: the control magnet 10 is far away from the capsule 201 and cannot provide sufficient traction magnetic force; the large slope of the digestive tract wall makes the capsule 201 Unable to effectively drag and translate; there are folds, obstacles, etc. in the moving direction of capsule 201 that cannot be effectively overcome.
  • the rolling motion is shown in Figure 8.
  • the control magnet 10 will rotate in the opposite direction to the capsule 201, that is, when the control magnet 10 rotates counterclockwise, the capsule 201 will roll clockwise.
  • the motion state changes from the first picture to the fifth picture, and the magnet 10 is controlled to rotate counterclockwise while moving to the right.
  • the capsule 201 makes a purely smooth clockwise roll against the lower wall.
  • Step 103 Calculate the current movement trajectory of the control magnet 10 according to the real-time position information and the target position information.
  • the long axis circumference is the length corresponding to the rotation of the capsule 201 along its long axis;
  • Th is the target horizontal azimuth angle for the capsule 201 to be rolled.
  • the vertical rotation surface of the control magnet 10 coincides with the Th orientation, and the rolling direction is the angular direction of the horizontal orientation angle Th.
  • capsule 201 is approximately a cylinder with spherical ends. Then the long axis circumference of capsule 201 can be expressed as:
  • L 2H+( ⁇ -2)D, where H is the long axis length of the capsule 201 and D is the capsule diameter.
  • the amount of roll can be judged.
  • the X-axis direction and the number of rolling turns in the Y-axis direction can be expressed as as well as
  • the number of tumbling turns is an integer, and the rotation amount of the control magnet 10 corresponding to each turn is 360°, so that the position of the capsule 201 is shifted by several long axis circumferences of the capsule 201, and the original orientation of the capsule 201 is maintained.
  • the posture, that is, the camera module 80 is still facing the original direction.
  • the control magnet 10 is adjusted to move forward by a length L of the long axis circumference of the capsule 201 in the rolling direction to provide traction and guidance magnetic attraction.
  • the guiding magnetic attraction force is along the expected rolling direction, perpendicular to the lateral rolling component of the spin of the capsule 201, and the combined force of the two points near the target direction.
  • the guiding magnetic attraction force provides a negative feedback component, weakening the influence of the lateral component moment of the capsule 201, guiding the capsule 201 to roll in the expected direction, and after one rotation, it is located near the center directly below the control magnet 10.
  • Step 104 Control the control magnet 10 to move along the current movement trajectory.
  • the current movement trajectory is the tumbling circle movement along the tumbling direction.
  • the movement mode of the control magnet 10 is the translation mode and the capsule 201 is not moving, the movement mode of the control magnet 10 is switched to the tumbling mode.
  • the maximum number of correction cycles is set (for example, no more than 5 corrections). If the upper limit of the number of corrections is reached and the threshold condition is still not met, the movement mode of the control magnet 10 is switched. , driving the movement of capsule 201 in a tumbling manner.
  • the friction force corresponding to tumbling is less than the friction force during translation, especially the static friction force when the capsule 201 is not moving, when the translation motion method cannot effectively move the capsule 201, the tumbling motion mode is changed.
  • Step 105 Determine whether the real-time position information is within the target position range; if the real-time position information is outside the target position range until the control magnet 10 stops moving, repeat steps 103 and 104 "according to The real-time position information and the target position information are used to calculate the current movement trajectory of the control magnet 10; and the control magnet 10 is controlled to move along the current movement trajectory until the real-time location information is within the target location range.
  • the current position [Cx, Cy, Cz] of capsule 201 is updated, and the calculation and target Check whether the target position [Tx, Ty, Tz] reaches the target position range on the XY plane. If it does not reach the target position range, the translation and/or rolling actions of the capsule 201 continue to be repeated; until the capsule 201 is within the target position range, the task of controlling the capsule position is successfully executed.
  • the target position range includes a preset distance dist th , and dist th generally takes a value of 5-10 mm to achieve a good balance between control accuracy and control efficiency.
  • step 105 it also includes calculating the distance dist 1 between the real-time position information and the projection of the target position information on the XY plane of the world coordinate system;
  • the distance dist 1 is calculated as:
  • the real-time location information is within the target location range
  • the real-time location information is outside the target location range.
  • setting the value range of the first correction factor ⁇ x and the second correction factor ⁇ y between 0 and 1 can avoid the oscillation phenomenon and prevent the capsule 201 from excessive movement.
  • the capsule 201 is prevented from moving to point T. on the right side to avoid problems that cannot be corrected in place. 0 ⁇ ⁇ Two times, the capsule 201 can be moved to the target position.
  • the control magnet 10 may not be in the same vertical direction as the capsule 201.
  • the attitude of the capsule 201 can be adjusted from the fixed position. It is convenient to calculate the posture position, so when the capsule 201 is adjusted in place, the control magnet 10 is adjusted to be in the same vertical direction as the capsule 201 (as shown in Figure 3, or as shown in the fourth picture of Figure 7), which is the capsule The ideal posture of capsule 201 and control magnet 10 when 201 moves to the target position.
  • the capsule 201 also remains vertically upward.
  • control magnet 10 and the capsule 201 are controlled to be on the same straight line perpendicular to the XY plane of the world coordinate system.
  • the control magnet 10 When the control magnet 10 exceeds the effective control range of the capsule 201, the driving magnetic force will rapidly attenuate. At this time, the control magnet 10 will be pulled back directly above the capsule 201, and the position adjustment operation will be restarted. If necessary, the height of the control magnet 10 from the capsule 201 needs to be adjusted to a suitable range in order to provide sufficient driving magnetic force.
  • the orientation angle of the capsule 201 is adjusted to point the lens end of the capsule 201 to a specific direction.
  • Step 201 Continue to obtain real-time posture information of the capsule 201.
  • the real-time magnet attitude information of the control magnet 10 is continuously acquired, wherein the real-time magnet attitude information, the real-time attitude information and the following target attitude information are all located in the same world coordinate system.
  • the 6-DOF state data given by the capsule positioning system 200 is [Cx, Cy, Cz, Ch, Cv, Cs], where the orientation angle of the capsule 201 is described by the state parameters [Ch, Cv] in the form of spherical coordinates, and Cv is Vertical tilt angle, Ch is the horizontal azimuth angle. [Ch, Cv] represents the orientation angle of the capsule 201 head (lens end).
  • the horizontal azimuth angle Ch (value range [-180, +180] degrees) is the projection vector of the capsule 201 head orientation in the XY plane and the Y axis.
  • Cs is the spin angle of the capsule 201.
  • the spin angle C s of the capsule 201 is 0 and increases in the clockwise direction.
  • the orientation angle adjustment does not involve the spin angle Cs of the capsule 201.
  • Step 202 Obtain the target posture information [Th, Tv] of the capsule 201, and calculate the target posture range based on the target posture information.
  • the target posture range includes a preset angle difference dist th2 .
  • the preset angle difference dist th2 generally takes a value of 5-10 degrees to achieve better control accuracy and control efficiency;
  • control magnet 10 is controlled to rotate to an initial posture, wherein, in the initial posture, the magnetization direction of the control magnet 10 is oriented parallel or perpendicular to the world coordinate system. Z axis.
  • Step 203 Calculate the current rotation trajectory of the control magnet 10 based on the target attitude information.
  • the current rotation trajectory includes the current rotation amount, and the current rotation amount includes the first angle difference and the second angle difference.
  • the posture adjustment can be decomposed into two angle adjustments;
  • the initial posture includes a first included angle C v that is positive to the Z-axis
  • the target posture information includes a second included angle T v that is positive to the Z-axis.
  • the second included angle is the same as the first included angle.
  • the first angle difference ⁇ Mv is the product of the difference and the third correction factor ⁇ v.
  • the initial posture includes a third included angle Ch between the XY plane and the positive direction of the Y axis.
  • the target posture information includes a fourth included angle Th between the XY plane and the positive direction of the Y axis.
  • the fourth included angle is between
  • the second angle difference ⁇ Mh is the product of the difference and the fourth correction factor ⁇ h.
  • the equivalent angle change function is introduced so that the angle is always adjusted according to the minor arc path, which can improve the efficiency of angle adjustment.
  • the equivalent angle change function is defined as:
  • the value range of the third correction factor ⁇ v and the fourth correction factor ⁇ h is between 0 and 1, and the expression is 0 ⁇ h , ⁇ v ⁇ 1.
  • Step 204 Control the control magnet 10 to move along the current rotation trajectory.
  • Step 205 Determine whether the real-time attitude information is within the target attitude range; if the real-time attitude information is outside the target attitude range until the control magnet 10 stops moving, repeat steps 203 and 204 "According to the Target attitude information, calculate the target attitude range and the current rotation trajectory of the control magnet 10; control the control magnet 10 to move along the current rotation trajectory" until the real-time attitude information is within the target attitude range, and then perform step 206 "End adjustment of control magnet 10".
  • the first unit vector and the second unit vector can be calculated.
  • the calculation formula of the real-time angle difference dist 2 is as follows:
  • the real-time attitude information is within the target attitude range
  • the real-time attitude information is outside the target attitude range.
  • the real-time attitude information [Ch, Cv] of the capsule 201 is updated, and whether the real-time attitude information is within the target attitude range is calculated based on the above-mentioned real-time angle difference. If the condition is not met, the capsule 201 angle adjustment action continues to be repeatedly performed; if the condition is met, the capsule 201 angle adjustment task is ended, and the task of controlling the capsule attitude is successfully executed.
  • 0 ⁇ h and ⁇ v ⁇ 1 can avoid the oscillation phenomenon and prevent the capsule 201 from excessive movement.
  • the capsule 201 is prevented from moving The movement crosses the P2 point to avoid the problem of incomplete correction.
  • ⁇ h and ⁇ v are equivalent to adding damping effects, which can ensure that the oscillation effect is attenuated and must converge to the target.
  • the above steps are generally cycled twice, that is, the magnet 10 is controlled to move twice, that is Capsule 201 can be moved to a target posture.
  • control path of the capsule 201 is converted into target state control of several key node positions. After moving to the key node, photos of the body are taken to achieve a highly intelligent automatic scanning and inspection function.
  • the quantitative closed-loop control method of the magnetically controlled capsule system 1000 can quantitatively control the movement process of the capsule 201.
  • the capsule 201 is allowed to accurately reach the target position and/or target posture, and the amount of movement of the capsule 201 is accurately controlled through a quantitative closed-loop control method.
  • this method is used in the scenario where the capsule 201 is controlled by a permanent magnet. It avoids the problem of slow operation of the permanent magnet due to untimely switching of speed and state.
  • the control process is highly robust.
  • this method can implement complex control logic. , carry out derivative digestive tract examination applications, and provide convenience for the automated, intelligent examination and derivative applications of the magnetically controlled capsule system 1000.
  • a magnetically controlled capsule system 1000 may include a first acquisition module, a first target acquisition module, a first calculation module, a first control module and a first judgment module.
  • the specific functions of each module are as follows:
  • the first acquisition module is used to continuously acquire the real-time location information of the capsule 201;
  • the first target acquisition module is used to acquire the target position information of the capsule 201, and determine the target position range according to the target position information;
  • the first calculation module is used to calculate the current movement trajectory of the control magnet 10 based on the real-time position information and the target position information;
  • the first control module is used to control the movement of the control magnet 10 along the current movement trajectory
  • the first judgment module is used to judge whether the real-time position information is within the target position range; when the control magnet 10 stops moving and the real-time position information is outside the target position range, repeat the "first The calculation module calculates the current movement trajectory of the control magnet 10 based on the real-time location information and the target location information, and the first control module controls the control magnet 10 to move along the current movement trajectory until the real-time location information is within the target location.
  • the magnetically controlled capsule system 1000 of this embodiment also includes a magnet position acquisition module, which is used to continuously acquire real-time magnet position information of the control magnet 10, wherein the real-time magnet position The information, the real-time position information and the target position information are all located in the same world coordinate system.
  • the magnetically controlled capsule system 1000 of this embodiment also includes a critical distance acquisition module and a state calculation module.
  • the critical distance acquisition module acquires the critical distance, where the critical distance is when the capsule 201 is suspended in the liquid. The distance between the capsule 201 and the control magnet 10;
  • the state calculation module calculates the state information of the capsule 201 based on the real-time magnet position information, the real-time position information and the critical distance, where the state information includes the bottom-sinking state, the liquid level suspension state and the ceiling-ceiling state. .
  • the magnetically controlled capsule system 1000 of this embodiment also includes a height adjustment module and an orientation adjustment module.
  • the target position information includes a target state; if the state information is inconsistent with the target state, the height adjustment module adjusts The height of the magnet 10 is controlled until the capsule 201 is in the target state;
  • the orientation adjustment module adjusts the orientation of the control magnet 10 so that the endoscope of the capsule 201 faces a specified direction.
  • the distance between the height adjustment module control capsule 201 and the control magnet 10 is greater than the sum of the critical distance and the first threshold
  • the distance between the height adjustment module control capsule 201 and the control magnet 10 is less than the sum of the critical distance and the second threshold.
  • the first control module controls the movement of the control magnet 10 to an initial position, wherein, in the initial position, the control magnet 10 and the capsule 201 are in an XY plane perpendicular to the world coordinate system. on the same straight line.
  • the magnetically controlled capsule system 1000 of this embodiment also includes a motion mode acquisition module to acquire the motion mode of the control magnet 10, and the motion mode includes a translation mode and/or a rolling mode.
  • the first calculation module calculates the current offset based on the real-time location information and the target location information.
  • the first calculation module calculates a first difference between the real-time position information and the target position information in the X-axis direction in the world coordinate system, and compares the first difference with the first correction
  • the product of the factors is used as the X-axis difference
  • the first calculation module calculates the second difference between the real-time position information and the target position information in the Y-axis direction in the world coordinate system, and uses the product of the second difference and the second correction factor as the Y-axis difference.
  • the first acquisition module acquires the long axis circumference of the capsule 201, where the long axis circumference is the length corresponding to the rotation of the capsule 201 along its long axis;
  • the first calculation module calculates the current rolling amount according to the real-time position information and the target position information, where the current rolling amount includes the number of rolling circles and the rolling direction of the capsule 201 .
  • the number of tumbling turns is an integer, and the rotation amount of the control magnet 10 corresponding to each turn is 360°.
  • the target position range includes a preset distance; the first calculation module calculates the distance between the real-time position information and the projection of the target position information on the XY plane of the world coordinate system.
  • the first control module controls the control magnet 10 and the capsule 201 to be on the same straight line perpendicular to the XY plane of the world coordinate system. .
  • the first control module controls the control magnet 10 and the capsule 201 to move in the XY direction perpendicular to the world coordinate system. on the same straight line of the plane.
  • a magnetically controlled capsule system 1000 is provided.
  • the magnetically controlled capsule system 1000 may include a second The acquisition module, the second target acquisition module, the second calculation module, the second control module and the second judgment module, the specific functions of each module are as follows:
  • the second acquisition module is used to continuously acquire the real-time posture information of the capsule 201;
  • the second target acquisition module is used to acquire the target posture information of the capsule 201;
  • the second calculation module is used to determine the target attitude range and the current rotation trajectory of the control magnet 10 according to the target attitude information
  • a second control module used to control the movement of the control magnet 10 along the current rotation trajectory
  • the second judgment module is used to judge whether the real-time position information is within the target position range; when the control magnet 10 stops moving and the real-time attitude information is outside the target attitude range, repeat the "second The calculation module calculates the target attitude range and the current rotation trajectory of the control magnet 10 according to the target attitude information, and the second control module controls the control magnet 10 to move along the current rotation trajectory until the real-time attitude information is in the within the target attitude range.
  • the magnet-controlled capsule system 1000 further includes a magnet attitude acquisition module, which continuously acquires real-time magnet attitude information of the control magnet 10, wherein the real-time magnet attitude information, the real-time attitude The information and the target attitude information are located in the same world coordinate system.
  • the second control module controls the control magnet 10 to rotate to an initial posture, wherein, in the initial posture, the magnetization direction of the control magnet 10 is oriented parallel or perpendicular to the world coordinate system. Z axis.
  • the second calculation module calculates the first unit vector corresponding to the real-time attitude information; the second calculation module calculates the second unit vector corresponding to the target attitude information; the second calculation module calculates the first The real-time angle difference between the unit vector and the second unit vector.
  • the magnetically controlled capsule system 1000 may also include computing devices such as computers, laptops, PDAs, and cloud servers. It may further include, but is not limited to, a processing module 40 and a storage module 30 .
  • computing devices such as computers, laptops, PDAs, and cloud servers. It may further include, but is not limited to, a processing module 40 and a storage module 30 .
  • a processing module 40 and a storage module 30 .
  • the schematic diagram is only an example of the magnetically controlled capsule system 1000 and does not constitute a limitation on the terminal equipment of the magnetically controlled capsule system 1000. It may include more or fewer components than shown in the figure, or a combination of certain components. Some components, or different components, for example, the magnetically controlled capsule system 1000 may also include input and output devices, network access devices, buses, etc.
  • the magnetically controlled capsule system 1000 can quantitatively control the movement process of the capsule 201, so that the capsule 201 accurately reaches the target position and/or target posture, and accurately controls the amount of movement of the capsule 201 through a closed-loop quantitative closed-loop control method. .
  • the magnetically controlled capsule system 1000 is particularly advanced in the scenario where the capsule 201 moves under the control of permanent magnets. Outstanding, it avoids the problem of slow operation of the permanent magnet due to untimely switching of speed and state, and the control process is highly robust.
  • the magnetically controlled capsule system 1000 can implement complex control logic and carry out derivative digestive tract inspection applications.
  • the magnetically controlled capsule system 1000 provides convenience for automated, intelligent inspection and derivative applications.
  • FIG. 10 it is a schematic module diagram of a magnetically controlled capsule system 1000 provided by an embodiment of the present invention.
  • the magnetic control capsule system 1000 also includes the above-mentioned magnetic control system 100, capsule positioning system 200, control magnet 10 and capsule 201, processing module 40, storage module 30, each module in the capsule 201, and the modules stored in the storage module 30 And a computer program that can be run on the processing module 40, such as the above-mentioned quantitative closed-loop control method program.
  • the processing module 40 executes the computer program, it implements the steps in each of the above control method embodiments, such as the steps shown in Figures 1 and 2 .
  • the magnetic source of the control magnet 10 is controlled by the servo motor and transmission mechanism to drive it to a designated position.
  • the transmission data of the servo motor is obtained.
  • the position of the control magnet 10 is obtained. Attitude angle status raw data. Position data is accurate to 1mm, and angle data is accurate to 1 degree.
  • the control magnet 10 is fixedly connected to the transmission mechanism.
  • the zero point is calibrated through photoelectric switches at some positions, such as some special vertical and horizontal angles. Then, by converting the driving amount of the servo motor that drives the movement of the control magnet 10, the control magnet can be accurately driven. 10 moves to the target position in the world coordinate system, and there is no need to determine its attitude angle by measuring the magnetic field direction of the control magnet 10 on site.
  • the capsule 201 may include a magnetic sensor 50, an acceleration sensor 60, a signal transmission module 70, a magnetic component (not shown) and a camera module 80.
  • the magnetic sensor 50, the acceleration sensor 60 and the magnetic component can be transmitted through an internal three-axis magnetic sensor.
  • the sensor 50, the three-axis acceleration sensor 60, the IMU sensor and multiple sets of external magnetic positioning devices work together to calculate the position and attitude of the capsule 201, and drive the movement of the capsule 201 by controlling the action of the magnet 10 on the magnetic parts.
  • the signal transmission module 70 transmits information to the external processing module 40 or server. After the external world drives the wireless capsule 201 to move to a designated position, the camera module 80 takes photos of the human body 400 and transmits them to the outside world through the signal output module, completing the internal photography. .
  • the control magnet 10 can be appropriately raised to weaken the suction force on the capsule 201, or lowered to increase the suction force on the capsule 201, and control the capsule 201 to switch between different situational states such as sinking to the bottom, floating on the liquid surface, and ceiling.
  • the magnetic control system 100 may also include a signal transmission module 20 and a communication bus 90 .
  • the signal transmission module 20 is used to send data to the processing module 40 or the server.
  • the signal transmission module 70 and the signal transmission module 20 can transmit data through wireless connections, such as Bluetooth, wifi, zigbee, etc., and the communication bus 90 is used to control the magnet. 10.
  • the communication bus 90 may include a channel to transmit information between the above-mentioned control magnet 10, the signal transmission module 20, the processing module 40 and the storage module 30. .
  • the present invention also proposes an electronic device, which includes a storage module 30 and a processing module 40.
  • the processing module 40 When the computer program is executed, the steps in the above-mentioned quantitative closed-loop control method can be realized, that is, the steps in any technical solution in the above-mentioned control method can be realized.
  • the electronic device may be part of the magnetically controlled capsule system 1000, a local terminal device, or part of a cloud server.
  • the processing module 40 may be a central processing unit or other general-purpose processor, the general-purpose processor may be a microprocessor, or the processor may be any conventional processor.
  • the processing module 40 is the control center of the magnetically controlled capsule system 1000, and uses various interfaces and lines to connect various parts of the entire magnetically controlled capsule system 1000.
  • the storage module 30 can be used to store the computer programs and/or modules.
  • the processing module 40 runs or executes the computer programs and/or modules stored in the storage module 30 and calls the computer programs and/or modules stored in the storage module 30.
  • the data is used to realize various functions of the magnetically controlled capsule system 1000.
  • the computer program may be divided into one or more modules/units, and the one or more modules/units are stored in the storage module 30 and executed by the processing module 40 to complete this invention.
  • the one or more modules/units may be a series of computer program instruction segments capable of completing specific functions. The instruction segments are used to describe the execution process of the computer program in the control method of the magnetically controlled capsule system 1000.
  • one embodiment of the present invention provides a readable storage medium that stores a computer program.
  • the computer program When the computer program is executed by the processing module 40, it can implement the steps in the above-mentioned control method of the magnetically controlled capsule system 1000, that is to say , implement the steps in any one of the technical solutions in the above-mentioned control method of the magnetically controlled capsule system 1000.
  • the integrated modules of the magnetic control capsule system 1000 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium.
  • the present invention can implement all or part of the processes in the methods of the above embodiments, and can also be completed by instructing relevant hardware through a computer program.
  • the computer program can be stored in a computer-readable storage medium, and the computer program can be stored in a computer-readable storage medium. When the program is executed by the processor, the steps of each of the above method embodiments can be implemented.

Landscapes

  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Surgery (AREA)
  • Biomedical Technology (AREA)
  • Medical Informatics (AREA)
  • Optics & Photonics (AREA)
  • Pathology (AREA)
  • Radiology & Medical Imaging (AREA)
  • Biophysics (AREA)
  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Gastroenterology & Hepatology (AREA)
  • Endoscopes (AREA)

Abstract

一种磁控胶囊系统（1000）及其定量闭环控制方法，方法包括：持续获取胶囊的实时位姿信息（101）；获取胶囊的目标位姿信息；根据目标位姿信息，确定目标位姿范围（102）；计算控制磁体的当前移动轨迹（103）；控制控制磁体沿当前移动轨迹运动（104）；使实时位姿信息在目标位姿范围内（105），继而定量、闭环地对胶囊的运动过程进行控制，精准控制胶囊的运动量。

Description

磁控胶囊系统及其定量闭环控制方法
本申请要求了申请日为2022年03月18日，申请号为202210272489.2，发明名称为“磁控胶囊系统及其定量闭环控制方法”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本发明涉及医疗设备技术领域，尤其涉及一种磁控胶囊系统及其定量闭环控制方法。
背景技术
体内设备定位技术，如无线胶囊、侵入式医疗器械等体内定位技术，受到越来越多的关注。磁控胶囊系统通过磁力驱动胶囊在体内运动，目前对胶囊的驱动，还需要通过有丰富经验的医师来完成。医师通过内置镜头拍摄消化道内壁的检查图像，确定胶囊的位置和姿态朝向，再通过外部控制磁体驱动胶囊继续运动到下一位置。
由于磁力的极度非线性、非均匀空间分布特点，消化道可形变环境以及摩擦力的影响，胶囊自旋导致难以依据图像判断真实方位，使得仅依靠图像视觉反馈信息，无法精准定量控制胶囊到达目标位置和目标姿态角度。现有的定位系统仅能起到辅助确认的作用，胶囊的下一步运动仍需要医师凭借经验判断，整个控制过程不够直观、精准。
尤其是在永磁体控制胶囊运动的过程中，由于永磁体无法像电磁铁一样高频地改变磁力大小和调整磁性方向，所以控制的速度慢，传递反馈的速度慢，很难根据反馈做如何进一步控制的判断，操作效率特别低，使用体验很不好。
发明内容
为解决上述现有技术问题中的至少其一，本发明的目的在于提供一种通过闭环的形式准确控制胶囊运动的磁控胶囊系统及其定量闭环控制方法。
为实现上述发明目的，本发明一实施方式提供一种磁控胶囊系统的定量闭环控制方法，包括如下步骤：
持续获取胶囊的实时位置信息；
获取所述胶囊的目标位置信息；
根据所述目标位置信息，确定目标位置范围；
根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；
控制所述控制磁体沿所述当前移动轨迹运动；
判断所述实时位置信息是否在所述目标范围内，若直至所述控制磁体停止运动，所述实时位置信息在所述目标位置范围外，则重复步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；控制所述控制磁体沿所述当前移动轨迹运动”，直至所述实时位置信息在所述目标位置范围内。
作为本发明的进一步改进，还包括步骤：
持续获取所述控制磁体的实时磁体位置信息，其中，所述实时磁体位置信息、所述实 时位置信息和所述目标位置信息均位于同一世界坐标系。
作为本发明的进一步改进，还包括步骤：
获取临界距离，其中，所述临界距离为所述胶囊在液体中悬浮时所述胶囊与所述控制磁体之间的距离；
根据所述实时磁体位置信息、所述实时位置信息和所述临界距离，计算所述胶囊的状态信息，其中，所述状态信息包括沉底状态、液面悬浮状态和吸顶状态。
作为本发明的进一步改进，其中，所述目标位置信息包括目标状态；
还包括步骤：
若所述状态信息与目标状态不一致，调节所述控制磁体的高度，直到所述胶囊处于目标状态；
调节所述控制磁体的朝向，以使所述胶囊的内窥镜朝向指定方向。
作为本发明的进一步改进，
当所述目标状态为沉底状态时，控制所述胶囊与所述控制磁体之间的距离大于所述临界距离与第一阈值之和；
当所述目标状态为吸顶状态时，控制所述胶囊与所述控制磁体之间的距离小于所述临界距离与第二阈值之和。
作为本发明的进一步改进，所述步骤“获取临界距离”包括：
根据临界距离算法或者根据测量数据获取所述临界距离；
其中，所述临界距离算法，是根据液面悬浮状态下，所述控制磁体和所述胶囊之间的吸力与所述胶囊受到的浮力之和等于所述胶囊的重力，计算所述控制磁体与所述胶囊之间的距离。
作为本发明的进一步改进，还包括步骤：
控制将所述控制磁体移动至初始位置，其中，于所述初始位置，所述控制磁体与所述胶囊在垂直于所述世界坐标系的XY平面的同一条直线上。
作为本发明的进一步改进，还包括步骤：
获取所述控制磁体的运动方式，所述运动方式包括平移方式和/或翻滚方式；
当所述控制磁体的运动方式为平移方式、且所述胶囊未运动时，切换所述控制磁体的运动方式为翻滚方式。
作为本发明的进一步改进，所述步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹”包括：
根据所述实时位置信息和所述目标位置信息，计算当前偏移量，其中，所述当前偏移量包括在所述世界坐标系中的X轴方向的X轴差值、以及在Y轴方向的Y轴差值；
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的X轴方向的第一差值，将所述第一差值与第一修正因子的乘积作为所述X轴差值；
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的Y轴方向的第二差 值，将所述第二差值与第二修正因子的乘积作为所述Y轴差值；
若所述控制磁体的运动方式为平移方式，所述当前移动轨迹为沿所述当前偏移量平移。
作为本发明的进一步改进，所述步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹”包括：
获取所述胶囊的长轴周长，其中，所述长轴周长为所述胶囊沿其长轴翻转一周对应的长度；
根据所述实时位置信息和所述目标位置信息，计算当前翻滚量，其中，所述当前翻滚量包括所述胶囊的翻滚圈数和翻滚方向；
若所述控制磁体的运动方式为翻滚方式，所述当前移动轨迹包括沿所述翻滚方向翻滚所述翻滚圈数。
作为本发明的进一步改进，其中，所述目标位置范围包括预设距离；
还包括步骤：
计算所述实时位置信息与所述目标位置信息在所述世界坐标系的XY平面上的投影的距离；
当所述距离小于等于所述预设距离，所述实时位置信息在所述目标位置范围内；
当所述距离大于所述预设距离，所述实时位置信息在所述目标位置范围外。
作为本发明的进一步改进，还包括步骤：
当所述实时位置信息在所述目标位置范围内时，或者，当所述实时磁体位置信息与所述实时位置信息对应的距离大于安全距离时；
控制所述控制磁体与所述胶囊在垂直于所述世界坐标系的XY平面的同一条直线上。
为实现上述发明目的之一，本发明一实施方式提供了一种磁控胶囊系统的定量闭环控制方法，包括如下步骤：
持续获取胶囊的实时姿态信息；
获取胶囊的目标姿态信息；
根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹；
控制所述控制磁体沿所述当前转动轨迹运动；
判断所述实时位置信息是否在所述目标范围内，若直至所述控制磁体停止运动，所述实时姿态信息在所述目标姿态范围外，则重复步骤“根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹；控制所述控制磁体沿所述当前转动轨迹运动”，直至所述实时姿态信息在所述目标姿态范围内。
作为本发明的进一步改进，持续获取所述控制磁体的实时磁体姿态信息，其中，所述实时磁体姿态信息、所述实时姿态信息和所述目标姿态信息均位于同一世界坐标系。
作为本发明的进一步改进，其中，所述当前转动轨迹包括当前转动量，所述当前转动量包括第一角度差和第二角度差；
控制所述控制磁体转动至初始姿态；
所述初始姿态包括与Z轴正向的第一夹角，所述目标姿态信息包括与Z轴正向的第二夹角，所述第一角度差为所述第二夹角与所述第一夹角差值与第三修正因子的乘积；
所述初始姿态包括在XY平面与Y轴正向的第三夹角，所述目标姿态信息包括在XY平面与Y轴正向的第四夹角，所述第二角度差为所述第四夹角与所述第三夹角的差值与第四修正因子的乘积。
作为本发明的进一步改进，其中，所述目标姿态范围包括预设角度差；
还包括步骤：
计算所述实时姿态信息对应的第一单位向量；
计算所述目标姿态信息对应的第二单位向量；
计算所述第一单位向量与所述第二单位向量的实时角度差；
当所述实时角度差小于等于所述预设角度差，所述实时姿态信息在所述目标姿态范围内；
当所述实时角度差大于所述预设角度差，所述实时姿态信息在所述目标姿态范围外。
为实现上述发明目的之一，本发明一实施方式提供了一种磁控胶囊系统，包括：第一获取模块、第一目标获取模块、第一计算模块、第一控制模块和第一判断模块；
所述第一获取模块，用于持续获取胶囊的实时位置信息；
所述第一目标获取模块，用于获取所述胶囊的目标位置信息，并根据所述目标位置信息，确定目标位置范围；
所述第一计算模块，用于根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；
所述第一控制模块，用于控制所述控制磁体沿所述当前移动轨迹运动；
所述第一判断模块，用于判断所述实时位置信息是否在所述目标范围内，当所述控制磁体停止运动，且所述实时位置信息在所述目标位置范围外，则重复“第一计算模块根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹，第一控制模块控制所述控制磁体沿所述当前移动轨迹运动”，直到所述实时位置信息在所述目标位置范围内。
为实现上述发明目的之一，本发明一实施方式提供了一种磁控胶囊系统，包括：第二获取模块、第二目标获取模块、第二计算模块、第二控制模块和第二判断模块；
所述第二获取模块，用于持续获取胶囊的实时姿态信息；
所述第二目标获取模块，用于获取胶囊的目标姿态信息；
所述第二计算模块，用于根据所述目标姿态信息，确定目标姿态范围和控制磁体的当前转动轨迹；
所述第二控制模块，用于控制所述控制磁体沿所述当前转动轨迹运动；
所述第二判断模块，用于判断所述实时位置信息是否在所述目标范围内，当所述控制 磁体停止运动，且所述实时姿态信息在所述目标姿态范围外，则重复“第二计算模块根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹，第二控制模块控制所述控制磁体沿所述当前转动轨迹运动”，直到所述实时姿态信息在所述目标姿态范围内。
为实现上述发明目的之一，本发明一实施方式提供了一种电子设备，包括存储模块和处理模块，所述存储模块存储有可在所述处理模块上运行的计算机程序，所述处理模块执行所述计算机程序时实现权利要求1所述的磁控胶囊系统的定量闭环控制方法中的步骤。
为实现上述发明目的之一，本发明一实施方式提供了一种可读存储介质，其存储有计算机程序，所述计算机程序被处理模块执行时实现权利要求1所述的磁控胶囊系统的定量闭环控制方法中的步骤。
与现有技术相比，本发明具有以下有益效果：该磁控胶囊系统的定量闭环控制方法能够定量地对胶囊的运动过程进行控制，使胶囊准确地到达目标位置和/或目标姿态，且通过闭环的定量闭环控制方法，精准控制胶囊的运动量。且该方法运用与永磁体控制的胶囊运动的场景下，避免了永磁体因速度、状态的切换不及时而操作慢的问题，控制过程的健壮性强，另外该方法可以实现复杂的控制逻辑，进行衍生的消化道检查应用，为磁控胶囊系统的自动化、智能化检查及衍生应用提供便利。
附图说明
图1是本发明其一实施例的磁控胶囊系统控制方法的流程图；
图2是本发明另一实施例的磁控胶囊系统控制方法的流程图；
图3是本发明一实施例的胶囊在人体内沉底且位置调节完后的结构示意图；
图4是本发明一实施例的胶囊在人体内沉底且姿态调节完后的结构示意图；
图5是本发明一实施例的控制磁体的磁力线分布及其垂直磁场相对强度示意图；
图6是本发明一实施例的胶囊在人体内受力平衡状态示意图；
图7是本发明一实施例的胶囊以平移方式运动的示意图；
图8是本发明一实施例的胶囊以翻滚方式运动的示意图；
图9是本发明一实施例的胶囊调整姿态过程的示意图；
图10是本发明一实施例的磁控胶囊系统的模块示意图；
其中，1000、磁控胶囊系统；100、磁控系统；200、胶囊定位系统；201、胶囊；300、床面；400、人体；10、控制磁体；20、信号传输模块；30、存储模块；40、处理模块；50、磁传感器；60、加速度传感器；70、信号输送模块；80、摄像模块；90、通信总线。
具体实施方式
以下将结合附图所示的具体实施方式对本发明进行详细描述。但这些实施方式并不限制本发明，本领域的普通技术人员根据这些实施方式所做出的结构、方法、或功能上的变换均包含在本发明的保护范围内。
应该理解，本文使用的例如“上”、“上方”、“下”、“下方”等表示空间相对位 置的术语是出于便于说明的目的来描述如附图中所示的一个单元或特征相对于另一个单元或特征的关系。空间相对位置的术语可以旨在包括设备在使用或工作中除了图中所示方位以外的不同方位。
本发明一实施例提供一种通过闭环的形式准确控制胶囊运动的磁控胶囊系统及其定量闭环控制方法，使胶囊准确地到达目标位置和/或目标姿态，且在永磁体控制胶囊运动的场景下具有更加显著的进步，为磁控胶囊系统的自动化、智能化检查及衍生应用提供便利。
本实施例的磁控胶囊系统1000，包括磁控系统100、胶囊定位系统200、控制磁体10和胶囊201，磁控系统100用于控制胶囊201的运动。胶囊定位系统200用于对胶囊201进行定位。胶囊201内部装有传感器模块，传感器模块包括用于检测磁场的磁传感器50(magnetic sensor)，磁传感器可以为：霍尔传感器、各向异性磁阻(AMR)传感器、巨磁阻(GMR)传感器、隧道磁阻(TMR)传感器等，控制磁体10包括用于发出磁场的磁源、用于控制磁源运动的伺服电机，所述磁源优选为永磁体。胶囊201即为胶囊内窥镜，其内部设有磁性件，与控制磁体10的磁源相互作用。通过磁源对磁性件的作用力，实现磁控系统100对胶囊201的位置和姿态的控制。
图1和图2为本申请一个实施方式的一种磁控胶囊系统1000的定量闭环控制方法的两种实施例，其中图1用于调整胶囊201的位置，图2用于调整胶囊201的姿态。图3和图4为本申请一个实施方式的磁控胶囊系统1000应用于人体400时的示意图，其中图3完成了对胶囊201位置的调整，图4完成了对胶囊201姿态的调整，胶囊201位于人体400内部，人体400平躺于床面300上，人体400外部设有包含控制磁体10的磁控系统100。在检查时，控制磁体10发出的磁场，控制人体400内的胶囊201运动。
这里，为清楚地表达本实施例中所描述的位置与方向，在本实施例中，定义人体400躺在床面300的上方，人体400的上方为正方向，人体400的下方为反方向，控制磁体10可以在人体400的各个方向控制胶囊201。以图3和图4为例，设定控制磁体10位于人体400的上方，另外浮力的方向向上，重力的方向向下。本申请中定义平行于纸面方向向左为左方向，平行于纸面方向向右为右方向。
下面先结合图1、图3、图5～8对胶囊201的位置调整进行论述，虽然本申请提供了如下述实施方式或流程图所述的方法操作步骤，但是基于常规或者无需创造性的劳动，所述方法在逻辑性上不存在必要因果关系的步骤中，这些步骤的执行顺序不限于本申请实施方式中所提供的执行顺序。
如图1所示，具体的磁控胶囊系统1000的定量闭环控制方法，包括如下步骤：
步骤101：持续获取胶囊201的实时位置信息。
同时，还可以持续获取所述控制磁体10的实时磁体位置信息，
其中，所述实时磁体位置信息、所述实时位置信息和下述目标位置信息均位于同一世界坐标系。
胶囊201的实时位置信息可以通过胶囊定位系统200获取，实时磁体位置信息可以通 过磁控系统100获取，将磁控系统100的坐标系与胶囊定位系统200的坐标系进行匹配，形成统一的世界坐标系。将胶囊定位系统200获取的胶囊位置信息坐标换算到世界坐标系中，将磁控系统100获取的磁体位置信息坐标也换算到世界坐标系中，从而可以对其统一调整。
胶囊定位系统200给出的6-DOF状态数据为[Cx,Cy,Cz,Ch,Cv,Cs]，实时位置信息采用其中的[Cx,Cy,Cz]，表示胶囊201在世界坐标系的XYZ坐标。此外，[Ch,Cv,Cs]用于表示胶囊201的姿态信息，将在下文对胶囊201姿态调整方法论述时进行详细描述。
磁控系统100给出的5-DOF状态数据表示为[Mx,My,Mz,Mh,Mv]，实时磁体位置采用其中的[Mx,My,Mz]，表示控制磁体10在世界坐标系的XYZ坐标。
另外为了减小移动胶囊201位置时，控制磁体10姿态的影响，下文在论述位置调整的过程中也会涉及到[Mh,Mv]，其中，[Mh,Mv]表示控制磁体10磁场方向N极的朝向角度，水平方位角Mh(取值范围[-180,+180]度)为磁体磁化方向矢量在XY平面投影矢量与Y轴正向的夹角，垂直倾斜角Mv(取值范围[0,+180]度)为控制磁体10磁化方向矢量与Z轴正向的夹角。
步骤102：获取胶囊201的目标位置信息，并根据所述目标位置信息，确定目标位置范围。目标位置信息可以表示为[Tx,Ty,Tz]，由于精准运动到该点位置效率不高，所以在胶囊201到达距离目标位置信息附近的一定范围内，即可视作已经运动到目标位置。
在执行步骤103之前，可以先将控制磁体10移动到初始位置，然后对胶囊201的状态进行判断和调整。
移动到初始位置，是控制所述控制磁体10移动至初始位置，其中，于所述初始位置，所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。也就是此时控制磁体10和胶囊201在X轴、Y轴上的坐标相同，这里的XY平面指的即是上文的世界坐标系。
并且，控制所述控制磁体10移动，将其磁化方向调整为竖直向上Mv＝0(或竖直向下Mv＝180)，使得控制磁体10位于胶囊201正上方时，胶囊201也保持竖直向上，此时Cv＝0(若竖直向下，则Cv＝180)。
另外为了避免控制磁体10在移动到胶囊201正上方的过程中，磁力吸引导致胶囊201发生位置移动，可以根据控制系统状态反馈信息，及时调整控制磁体10与胶囊201在Z轴方向上的距离，将该距离控制在安全距离范围内。
由于磁力的极度非线性、非均匀空间分布特点(参图5所示)，且控制磁体10在较远距离的非接触控制，尤其是永磁体不能快速地调整其磁力大小和方向，所以需要先对胶囊201的平衡状态进行判断，使其处于受力平衡的状态下进行后续的操作。
对胶囊201的状态进行判断和调整包括步骤：
获取临界距离，其中，所述临界距离为胶囊201在液体中悬浮时所述胶囊201与所述控制磁体10之间的距离；
根据所述实时磁体位置信息、所述实时位置信息和所述临界距离，计算胶囊201的状态信息，其中，所述状态信息包括沉底状态、液面悬浮状态和吸顶状态。
三种状态的受力分析可以参考图6所示，图6中从左至右，分别为胶囊201处于沉底状态、液面悬浮状态和吸顶状态时的受力情况，其中,G为胶囊201重力，Ff为胶囊201所受浮力,Fm为控制磁体10对胶囊201的吸力，N为消化道内壁对胶囊201的支持力。沉底状态也可参图2或3所示，消化道内包括上壁、下壁和液体，沉底状态时，胶囊201抵着下壁，吸顶状态时，胶囊201抵着上壁，液面悬浮状态时，控制磁体10的吸力与浮力之和等于胶囊201的重力。另外，在撤去吸力时，胶囊201会是沉底的状态。
其中，所述步骤“获取临界距离”包括：
根据临界距离算法或者根据测量数据获取所述临界距离。
临界距离算法，是根据液面悬浮状态下，所述控制磁体10和胶囊201之间的吸力与胶囊201受到的浮力之和等于胶囊201的重力，计算所述控制磁体10与胶囊201之间的距离。
更具体的公式方面，控制磁体10对正下方胶囊201的吸力可以表示为：
其中，M,m分别为控制磁体10、胶囊201内置磁铁的磁矩，r为控制磁体10与胶囊201内置磁铁的中心距离,μ0为真空磁导率。
控制磁体10对正下方胶囊201的吸力与胶囊201重力G和胶囊201所受浮力Ff平衡时，
Fm(Z0)+ρVg＝mcg
其中ρ为胶囊201所处介质液体密度，V为胶囊201体积，g为重力加速度常量，mc为胶囊201质量，Z0为所述临界距离。
结合上述公式，临界距离Z0的计算公式：
通过该公式可以计算出临界距离，当结合上述控制磁体10的初始位置，临界距离指的是Z轴方向上高度的距离，即为临界高度。
实际环境下，控制磁体10、胶囊201受充磁影响导致磁矩大小变化，胶囊201质量及体积受型号的变化影响，Z0根据实际的控制磁体10和胶囊201类型进行实验测量更为方便可靠。
控制磁体10中心与胶囊201的高度差dZ表示为：dZ≡Mz-Cz，其中，Mz为控制磁体10在Z轴方向上的坐标值，Cz为胶囊201在Z轴方向上的坐标值。
可以根据控制磁体10和胶囊201的状态数据实时计算获取该高度差。通过dZ判断胶囊201当前的状态，当dZ>Z0+δa，胶囊201处于沉底状态；当dZ<Z0-δa，胶囊201处于吸顶状态。
其中，冗余参量δa取值1～2cm，用于消除定位误差、消化道蠕动引起的噪声干扰。
另外，所述目标位置信息包括目标状态；
若所述状态信息与目标状态一致，控制磁体10的朝向正确，则无需调整；
若所述状态信息与目标状态不一致，调节所述控制磁体10的高度，直到胶囊201处于目标状态；
调节所述控制磁体10的朝向，以使胶囊201的镜头端朝向指定方向。
具体地，通过调整dZ的距离控制胶囊201转换为沉底、吸顶状态，驱动控制磁体10改变其上下方向的高度，当胶囊201当前处于沉底状态，需要改为吸顶状态时，调整dZ使其满足dZ<Z0-δb；当胶囊201当前处于吸顶状态，需要改为沉底状态时，调整dZ使其满足dZ>Z0+δb。
其中，冗余参量δb取值3cm，用于消除定位误差、消化道蠕动引起的噪声干扰，确保控制动作执行的成功率，使得胶囊201切换至目标状态。
另外，沉底与吸顶状态下，胶囊201依赖消化道的可变支持力、摩擦力实现控制磁体10磁力、胶囊201重力以及液体浮力的平衡。离开消化道壁的胶囊201无法在Z轴任意位置达到平衡，所以仅在液面附近有可能通过浮力的负反馈机制实现胶囊201在液面悬浮稳定。
以及，当所述目标状态为沉底状态时，控制所述胶囊201与所述控制磁体10之间的距离大于所述临界距离与第一阈值之和，第一阈值根据现场环境进行调整，当需要胶囊201沉 底时，胶囊201从底部向上拍摄，此时需要避免胶囊201被控制磁体10吸起，所以使控制磁体10距离胶囊201的距离尽量的远。
当所述目标状态为吸顶状态时，控制胶囊201与所述控制磁体10之间的距离小于所述临界距离与第二阈值之和，第二阈值根据现场环境进行调整，当需要胶囊201吸顶时，胶囊201从上方向下拍摄，此时需要避免胶囊201掉落，所以使控制磁体10距离胶囊201的距离尽量的近。
在步骤“调节所述控制磁体10的朝向，以使胶囊201的镜头端朝向指定方向”中，摄像模块80在胶囊201的一端，当胶囊201沉底时，需要摄像模块80向上拍摄，当胶囊201吸顶时，需要摄像模块80向下拍摄，所以根据胶囊201此时的状态，控制磁体10采用不同的磁极朝向。例如摄像模块80位于胶囊201的磁极N端，于沉底状态，控制磁体10的磁极S端朝向胶囊201，于吸顶状态，控制磁体10的磁极N端朝向胶囊201。
以及在执行步骤103，可以先执行以下步骤：
获取所述控制磁体10的运动方式，所述运动方式包括平移方式和/或翻滚方式。胶囊201可以以平移的方式直线移动，也可以以翻滚的形式滚动向前运动，平移方式可参图7所示，翻滚方式参图8所示。平移方式和/或翻滚方式可以人为选择，也可以自动选择，下文将两种情况区分展开论述。
平移方式运动
在倾斜坡度较小的近似XY平面内的胶囊位置移动，可以通过控制磁体10拖动胶囊201平移实现，且根据上文，已知胶囊201的实时位置信息为[Cx,Cy,Cz]，胶囊201的目标位置信息为[Tx,Ty,Tz]。由于胶囊201在Z轴方向上位置无法稳定连续控制，仅对XY平面内的胶囊位置进行控制操作。平移方式运动如图7所示。
步骤103：根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹。
根据所述实时位置信息和所述目标位置信息，计算当前偏移量，其中，所述当前偏移量包括在所述世界坐标系中的X轴方向的X轴差值、以及在Y轴方向的Y轴差值；
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的X轴方向的第一差值ΔCx，将所述第一差值与第一修正因子αx的乘积作为所述X轴差值ΔMx；第一差值的表达式为：△Cx＝Tx-Cx，X轴差值的表达式为：△Mx＝αx△Cx。
计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的Y轴方向的第二差 值ΔCy，将所述第二差值与第二修正因子αy的乘积作为所述Y轴差值ΔMy；第二差值的表达式为：△Cy＝Ty-Cy，Y轴差值的表达式为：△My＝αy△Cy。
其中，所述第一修正因子与所述第二修正因子的取值范围均为0到1之间，表达式为0<αx,αy<1，为方便计算，可以设定αx＝αy，一般选取αx,y＝0.7即可稳定、高效地反馈调整胶囊201到目标位置信息。
步骤104：控制所述控制磁体10沿所述当前移动轨迹运动。
若所述控制磁体10的运动方式为平移方式，所述当前移动轨迹为沿所述当前偏移量平移。
翻滚方式运动
胶囊201翻滚方式运动通常用于胶囊201无法被有效平移的情形，这类情形包括但不限于：控制磁体10离胶囊201较远无法提供足够大的牵引磁力；消化道壁坡度较大使得胶囊201无法有效地拖动平移；胶囊201移动方向存在无法有效越过的褶皱、障碍物等。翻滚方式运动如图8所示。
控制磁体10会与胶囊201反向转动，也就是说，当控制磁体10逆时针转动时，胶囊201会以顺时针翻滚。例如图8中，从第1张图至第5张图的运动状态变化，控制磁体10在向右运动的同时逆时针转动，理想地，胶囊201贴着下壁做顺时针的纯滑滚动。
步骤103：根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹。
获取胶囊201的长轴周长，其中，所述长轴周长为胶囊201沿其长轴翻转一周对应的长度；
根据所述实时位置信息和所述目标位置信息，计算当前翻滚量，其中，所述当前翻滚量包括胶囊201的翻滚方向和翻滚圈数；
其中，控制磁体10的水平转动使得水平方位角Mh＝Th，Th为上述的胶囊201待翻滚的目标水平方位角度。此时，控制磁体10的垂直转动面与Th方位重合，翻滚方向即为水平方位角度Th的夹角方向。
胶囊201外形近似为两端为球面的圆柱体，则胶囊201的长轴周长可以表示为：
L＝2H+(π-2)D，其中，H为胶囊201的长轴长度，D为胶囊直径。
胶囊201翻滚一周后，在XY平面的位置转移近似为：
根据上式求得的Δx、Δy，以及前述第一差值△Cx＝Tx-Cx和第二差值△Cy＝Ty-Cy，可以判断翻滚的量，在X轴方向和Y轴方向翻滚圈数可以分别表示为以及
而其中，所述翻滚圈数为整数，每圈对应的所述控制磁体10的转动量为360°，使胶囊201的位置转移若干个胶囊201的长轴周长，并保持原有胶囊201朝向姿态，即摄像模块80仍朝着原来的方向。
为了降低胶囊201在翻滚时由于胶囊重心偏离轴心的不对称性导致的自旋横向滚动，调整控制磁体10往翻滚方向前移一个胶囊201长轴周长距离L，提供牵引导向磁吸力。导向磁吸力沿预期翻滚方向，与胶囊201自旋横向滚动分力垂直，两者合力指向目标方向附近。并且在发生细微横向滚动时，导向磁吸力提供负反馈分力，削弱胶囊201横向分力矩的影响，引导胶囊201朝预期方向翻滚，并且在翻滚一周后位于控制磁体10正下方附近。
步骤104：控制所述控制磁体10沿所述当前移动轨迹运动。
若所述控制磁体10的运动方式为翻滚方式，所述当前移动轨迹为沿所述翻滚方向翻滚所述翻滚圈数运动。
另外，当所述控制磁体10的运动方式为平移方式，胶囊201未运动时，切换所述控制磁体10的运动方式为翻滚方式。
每次控制磁体10以平移方式驱动胶囊201的运动，设定最大修正循环次数(例如，不超过5次修正)，如果达到修正次数上限，仍然未满足阈值条件，则切换控制磁体10的运动方式，以翻滚方式驱动胶囊201的运动。
由于翻滚对应的摩擦力小于平移时的摩擦力，尤其小于胶囊201未运动时的静摩擦力，所以当平移运动方式无法有效移动胶囊201时，改为翻滚方式运动。
步骤105：判断所述实时位置信息是否在所述目标位置范围内；若直至所述控制磁体10停止运动，所述实时位置信息在所述目标位置范围外，则重复步骤103和步骤104“根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹；控制所述控制磁体10沿所述当前移动轨迹运动”，直至所述实时位置信息在所述目标位置范围内。
每次执行完一次胶囊201运动的动作后，更新胶囊201当前位置[Cx,Cy,Cz]，计算与目 标位置[Tx,Ty,Tz]在XY平面上是否到达目标位置范围内。若未到达目标位置范围内，则继续重复执行胶囊201平移和/或翻滚的动作；直到胶囊201位于目标位置范围内，控制胶囊位置的任务执行成功。
所述目标位置范围包括预设距离distth，distth一般取值5-10mm即可达到较好的控制精度和控制效率的平衡。
在步骤105中，还包括计算所述实时位置信息与所述目标位置信息在所述世界坐标系的XY平面上的投影的距离dist1；
该距离dist1的计算方法为：
其中，为实时位置信息在所述世界坐标系的向量表示，为目标位置信息在所述世界坐标系的向量表示，如图9所示。
当所述距离小于等于所述预设距离，即dist1≤distth1，所述实时位置信息在所述目标位置范围内；
当所述距离大于所述预设距离，即dist1>distth1，所述实时位置信息在所述目标位置范围外。
另外，将第一修正因子αx和第二修正因子αy的取值范围设置在0到1之间，可以避免发生振荡现象，防止胶囊201运动过量，例如在图7中防止胶囊201运动到T点的右边，避免出现一直修正不到位的问题。0<αx,αy<1相当于加入阻尼效应，可以确保振荡效果衰减，一定能收敛到目标，在运用本申请的方法的实验中，一般将上述步骤循环两次，即控制磁体10运动两次，即可将胶囊201移动到目标位置。
步骤106：当所述实时位置信息在所述目标位置范围内时，调整所述控制磁体10与胶囊201处于同一竖直方向，即在垂直于所述世界坐标系的XY平面的同一条直线上，此时[Mx,My]＝[Cx,Cy]。
在图7中的第3张图中，胶囊201运动到目标位置T时，控制磁体10可能与胶囊201不在同一竖直方向，为了便于在后续的姿态调整，将胶囊201从固定位置开始调整姿态，可以方便姿态位置的计算，所以在胶囊201调整到位时，将控制磁体10调整至与胶囊201位于同一竖直方向(如图3所示、或者如图7的第4张图所示)，是胶囊201运动到目标位置时，胶囊201和控制磁体10的理想姿态。
此时，磁化方向调整为竖直向上Mv＝0(或竖直向下Mv＝180)，使得控制磁体10位于胶 囊201正上方时，胶囊201也保持竖直向上，此时Cv＝0(若竖直向下，则Cv＝180)，也就是说，控制磁体10在胶囊201的正上方和/或正下方。
另外，当所述实时磁体位置信息与所述实时位置信息对应的距离大于安全距离时，控制所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。
当控制磁体10超出胶囊201的有效控制范围之后，驱动磁力将急剧衰减，此时将控制磁体10拉回到胶囊201正上方，重新开始位置调节的操作。必要时，需要调整控制磁体10距离胶囊201的高度到合适范围，以便提供足够的驱动磁力。
在胶囊201的位置移动完成后，进行胶囊201的朝向姿态角调整，将胶囊201镜头端指向特定方位。
下面结合图2、图4、图9对胶囊201的姿态调整的方法进行论述，包括如下步骤：
步骤201：持续获取胶囊201的实时姿态信息。
同时，持续获取所述控制磁体10的实时磁体姿态信息，其中，所述实时磁体姿态信息、所述实时姿态信息和下述目标姿态信息均位于同一世界坐标系。
胶囊定位系统200给出的6-DOF状态数据为[Cx,Cy,Cz,Ch,Cv,Cs]，其中，胶囊201朝向姿态角通过球坐标形式的状态参数[Ch,Cv]描述，Cv为垂直倾斜角，Ch为水平方位角。[Ch,Cv]表示胶囊201头部(镜头端)的朝向角度，水平方位角Ch(取值范围[-180,+180]度)为胶囊201头部朝向在XY平面投影矢量与Y轴正向的夹角，并按照顺时针方向增加(Y轴正向时Ch＝0，X轴正向时Ch＝90，Y轴负向时Ch＝±180，X轴负向时Ch＝-90)；垂直倾斜角Cv(取值范围[0,+180]度)为胶囊201头部朝向与Z轴正向的夹角。
Cs为胶囊201自旋角，胶囊201镜头拍摄图像正立时胶囊201自旋角Cs＝0，按照顺时针方向增加。本实施例朝向姿态角调整不涉及胶囊201自旋角Cs。
步骤202：获取胶囊201的目标姿态信息[Th,Tv]，并且根据所述目标姿态信息，计算目标姿态范围。
所述目标姿态范围包括预设角度差distth2，预设角度差distth2一般取值5-10度即可达到较好的控制精度和控制效率；
在开始步骤203或步骤204之前，控制所述控制磁体10转动至初始姿态，其中，于所述初始姿态，所述控制磁体10的磁化方向N极的朝向平行或垂直于所述世界坐标系的Z轴。
图3所示的实施例中，采用控制磁体10的磁化方向N极的朝向平行Z轴的方案，也就是上文所述的开始姿态调节之前的姿态。
步骤203：根据所述目标姿态信息，计算控制磁体10的当前转动轨迹。
所述当前转动轨迹包括当前转动量，所述当前转动量包括第一角度差和第二角度差，可参图9所示，将姿态的调节分解为两个角度上的调整；
所述初始姿态包括与Z轴正向的第一夹角Cv，所述目标姿态信息包括与Z轴正向的第二夹角Tv，所述第二夹角与所述第一夹角差值为△Cv＝Tv-Cv，所述第一角度差ΔMv为该差值与第三修正因子αv的乘积，表达式为：△Mv＝αv△Cv。
所述初始姿态包括在XY平面与Y轴正向的第三夹角Ch，所述目标姿态信息包括在XY平面与Y轴正向的第四夹角Th，所述第四夹角与所述第三夹角的差值为△Ch＝eff_ang(Th-Ch)，所述第二角度差ΔMh为该差值与第四修正因子αh的乘积，表达式为：△Mh＝αh△Ch。
其中，引入等效角度变化函数使得角度始终按照劣弧路径调整，可以提高角度调整的效率。等效角度变化函数定义为：
所述第三修正因子αv和所述第四修正因子αh的取值范围均为0到1之间，表达式为0<αh,αv<1，为方便计算，可以设定αh＝αv，一般选取αh,v＝0.8即可稳定、高效地反馈调整胶囊201到目标位置信息。
步骤204：控制所述控制磁体10沿所述当前转动轨迹运动。
步骤205：判断所述实时姿态信息是否在目标姿态范围内；若直至所述控制磁体10停止运动，所述实时姿态信息在所述目标姿态范围外，则重复步骤203和步骤204“根据所述目标姿态信息，计算目标姿态范围和控制磁体10的当前转动轨迹；控制所述控制磁体10沿所述当前转动轨迹运动”，直至所述实时姿态信息在所述目标姿态范围内，然后执行步骤206“结束调整控制磁体10”。
还包括步骤：
计算所述实时姿态信息对应的第一单位向量；
计算所述目标姿态信息对应的第二单位向量；
计算所述第一单位向量与所述第二单位向量的实时角度差；
其中，单位球面投影矢量计算公式为：
将实时姿态信息和目标姿态信息代入上述公式可以计算第一单位向量和第二单位向量，实时角度差dist2的计算公式如下：
当所述实时角度差小于等于所述预设角度差，也就是dist2≤distth2时，所述实时姿态信息在所述目标姿态范围内；
当所述实时角度差大于所述预设角度差，也就是dist2>distth2时，所述实时姿态信息在所述目标姿态范围外。
每次执行完一次胶囊201角度调整动作后，更新胶囊201的实时姿态信息[Ch,Cv]，根据上述的实时角度差计算实时姿态信息是否处于目标姿态范围内。若未满足条件，则继续重复执行胶囊201角度调整动作；若满足条件，则结束胶囊201角度调整任务，控制胶囊姿态的任务执行成功。
另外，与上述位置调整中的第一修正因子、第二修正因子原因相同的，0<αh,αv<1可以避免发生振荡现象，防止胶囊201运动过量，例如在图9中防止胶囊201运动越过P2点，避免出现一直修正不到位的问题。αh,αv相当于加入阻尼效应，可以确保振荡效果衰减，一定能收敛到目标，在运用本申请的方法的实验中，一般将上述步骤循环两次，即控制磁体10运动两次，即可将胶囊201移动到目标姿态。
通过上述的步骤，结合程序化的逻辑控制，将胶囊201控制路径转化为若干关键节点位置的目标状态控制，移动到关键节点后拍摄体内的照片，实现智能化程度较高的自动扫描检查功能。
该磁控胶囊系统1000的定量闭环控制方法能够定量地对胶囊201的运动过程进行控制， 使胶囊201准确地到达目标位置和/或目标姿态，且通过定量闭环控制方法，精准控制胶囊201的运动量。且该方法运用与永磁体控制的胶囊201运动的场景下，避免了永磁体因速度、状态的切换不及时而操作慢的问题，控制过程的健壮性强，另外该方法可以实现复杂的控制逻辑，进行衍生的消化道检查应用，为磁控胶囊系统1000的自动化、智能化检查及衍生应用提供便利。
在一个实施例中，提供了一种磁控胶囊系统1000。该磁控胶囊系统1000可以包括第一获取模块、第一目标获取模块、第一计算模块、第一控制模块和第一判断模块，各模块具体功能如下：
第一获取模块，用于持续获取胶囊201的实时位置信息；
第一目标获取模块，用于获取胶囊201的目标位置信息，并根据所述目标位置信息，确定目标位置范围；
第一计算模块，用于根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹；
第一控制模块，用于控制所述控制磁体10沿所述当前移动轨迹运动；
第一判断模块，用于判断所述实时位置信息是否在所述目标位置范围内；当所述控制磁体10停止运动，且所述实时位置信息在所述目标位置范围外，则重复“第一计算模块根据所述实时位置信息和所述目标位置信息，计算控制磁体10的当前移动轨迹，第一控制模块控制所述控制磁体10沿所述当前移动轨迹运动”，直到所述实时位置信息在所述目标位置范围内。
在一个实施例中，本实施例的磁控胶囊系统1000还包括磁体位置获取模块，所述磁体位置获取模块用于持续获取所述控制磁体10的实时磁体位置信息，其中，所述实时磁体位置信息、所述实时位置信息和所述目标位置信息均位于同一世界坐标系。
在一个实施例中，本实施例的磁控胶囊系统1000还包括临界距离获取模块和状态计算模块，所述临界距离获取模块获取临界距离，其中，所述临界距离为胶囊201在液体中悬浮时的胶囊201与所述控制磁体10之间的距离；
所述状态计算模块根据所述实时磁体位置信息、所述实时位置信息和所述临界距离，计算胶囊201的状态信息，其中，所述状态信息包括沉底状态、液面悬浮状态和吸顶状态。
在一个实施例中，本实施例的磁控胶囊系统1000还包括高度调节模块和朝向调节模块，所述目标位置信息包括目标状态；若所述状态信息与目标状态不一致，所述高度调节模块调节所述控制磁体10的高度，直到胶囊201处于目标状态；
所述朝向调节模块调节所述控制磁体10的朝向，以使胶囊201的内窥镜朝向指定方向。
在一个实施例中，当所述目标状态为沉底状态时，高度调节模块控制胶囊201与所述控制磁体10之间的距离大于所述临界距离与第一阈值之和；
当所述目标状态为吸顶状态时，高度调节模块控制胶囊201与所述控制磁体10之间的距离小于所述临界距离与第二阈值之和。
在一个实施例中，第一控制模块控制将所述控制磁体10移动至初始位置，其中，于所述初始位置，所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。
在一个实施例中，本实施例的磁控胶囊系统1000还包括运动方式获取模块，获取所述控制磁体10的运动方式，所述运动方式包括平移方式和/或翻滚方式。
在一个实施例中，第一计算模块根据所述实时位置信息和所述目标位置信息，计算当前偏移量。
在一个实施例中，第一计算模块计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的X轴方向的第一差值，将所述第一差值与第一修正因子的乘积作为所述X轴差值；
第一计算模块计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的Y轴方向的第二差值，将所述第二差值与第二修正因子的乘积作为所述Y轴差值。
在一个实施例中，第一获取模块获取胶囊201的长轴周长，其中，所述长轴周长为胶囊201沿其长轴翻转一周对应的长度；
第一计算模块根据所述实时位置信息和所述目标位置信息，计算当前翻滚量，其中，所述当前翻滚量包括胶囊201的翻滚圈数和翻滚方向。
在一个实施例中，所述翻滚圈数为整数，每圈对应的所述控制磁体10的转动量为360°。
在一个实施例中，所述目标位置范围包括预设距离；第一计算模块计算所述实时位置信息与所述目标位置信息在所述世界坐标系的XY平面上的投影的距离。
在一个实施例中，当所述实时位置信息在所述目标位置范围内时，第一控制模块控制所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。
在一个实施例中，当所述实时磁体位置信息与所述实时位置信息对应的距离大于安全距离时，第一控制模块控制所述控制磁体10与胶囊201在垂直于所述世界坐标系的XY平面的同一条直线上。
在一个实施例中，提供了一种磁控胶囊系统1000。该磁控胶囊系统1000可以包括第二 获取模块、第二目标获取模块、第二计算模块、第二控制模块和第二判断模块，各模块具体功能如下：
第二获取模块，用于持续获取胶囊201的实时姿态信息；
第二目标获取模块，用于获取胶囊201的目标姿态信息；
第二计算模块，用于根据所述目标姿态信息，确定目标姿态范围和控制磁体10的当前转动轨迹；
第二控制模块，用于控制所述控制磁体10沿所述当前转动轨迹运动；
第二判断模块，用于判断所述实时位置信息是否在所述目标位置范围内；当所述控制磁体10停止运动，且所述实时姿态信息在所述目标姿态范围外，则重复“第二计算模块根据所述目标姿态信息，计算目标姿态范围和控制磁体10的当前转动轨迹，第二控制模块控制所述控制磁体10沿所述当前转动轨迹运动”，直到所述实时姿态信息在所述目标姿态范围内。
在一个实施例中，磁控胶囊系统1000还包括磁体姿态获取模块，所述磁体姿态获取模块持续获取所述控制磁体10的实时磁体姿态信息，其中，所述实时磁体姿态信息、所述实时姿态信息和所述目标姿态信息均位于同一世界坐标系。
在一个实施例中，第二控制模块控制所述控制磁体10转动至初始姿态，其中，于所述初始姿态，所述控制磁体10的磁化方向N极的朝向平行或垂直于所述世界坐标系的Z轴。
在一个实施例中，第二计算模块计算所述实时姿态信息计对应的第一单位向量；第二计算模块计算所述目标姿态信息对应的第二单位向量；第二计算模块计算所述第一单位向量与所述第二单位向量的实时角度差。
所述磁控胶囊系统1000还可以包括计算机、笔记本、掌上电脑及云端服务器等计算设备。进一步可包括，但不仅限于，处理模块40、存储模块30。本领域技术人员可以理解，所述示意图仅仅是磁控胶囊系统1000的示例，并不构成对磁控胶囊系统1000终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述磁控胶囊系统1000还可以包括输入输出设备、网络接入设备、总线等。
需要说明的是，本发明实施例的磁控胶囊系统1000中未披露的细节，请参照本发明实施例的磁控胶囊系统1000的定量闭环控制方法中所披露的细节。
根据本发明的磁控胶囊系统1000能够定量地对胶囊201的运动过程进行控制，使胶囊201准确地到达目标位置和/或目标姿态，且通过闭环的定量闭环控制方法，精准控制胶囊201的运动量。且该磁控胶囊系统1000在永磁体控制的胶囊201运动的场景下的进步尤为 突出，避免了永磁体因速度、状态的切换不及时而操作慢的问题，控制过程的健壮性强，另外该磁控胶囊系统1000可以实现复杂的控制逻辑，进行衍生的消化道检查应用，为磁控胶囊系统1000的自动化、智能化检查及衍生应用提供便利。
如图10所示，是本发明一实施方式提供的磁控胶囊系统1000的模块示意图。磁控胶囊系统1000还包括上述的磁控系统100、胶囊定位系统200、控制磁体10和胶囊201、处理模块40、存储模块30、胶囊201内的各模块、以及存储在所述存储模块30中并可在所述处理模块40上运行的计算机程序，例如上述的定量闭环控制方法程序。所述处理模块40执行所述计算机程序时实现上述各个控制方法实施例中的步骤，例如图1和2所示的步骤。
控制磁体10的磁源通过伺服电机及传动机构控制驱动其运动至指定位置，通过磁控系统100的数据接口，获取伺服电机的传动数据，经过固定的比例转换公式，获取控制磁体10的位置、姿态角度状态原始数据。位置数据精确到1mm，角度数据精确到1度。
控制磁体10与传动机构固定连接，通过在一些位置的光电开关，如一些特殊垂直、水平角度标定零点，然后通过对驱动控制磁体10运动的伺服电机的驱动量的换算，可以精准地驱动控制磁体10在世界坐标系中运动到目标位置，无需通过现场测量控制磁体10磁场方向确定其姿态角度。
胶囊201可以包括磁传感器50、加速度传感器60、信号输送模块70、磁性件(未图示)和摄像模块80，磁传感器50、加速度传感器60、磁性件如上文所述，可以通过内部三轴磁传感器50、三轴加速度传感器60、IMU传感器和外部多组磁定位设备协同工作，计算出胶囊201所在的位置和姿态，通过控制磁体10对磁性件的作用驱动胶囊201运动。信号输送模块70将信息传输至外界的处理模块40或服务器中，外界驱动无线胶囊201运动到指定位置后，摄像模块80拍摄人体400内的照片通过信号输出模块传输至外界，完成对体内的拍摄。
控制磁体10可以适当拉高以减弱对胶囊201的吸力，或者降低以增大对胶囊201的吸力，控制胶囊201在沉底、液面悬浮、吸顶等不同情境状态中切换。
磁控系统100还可以包括信号传输模块20和通信总线90。信号传输模块20用于将数据发送至处理模块40或服务器，信号输送模块70和信号传输模块20可以通过无线连接的形式传输数据，如蓝牙、wifi、zigbee等，通信总线90用于将控制磁体10、信号传输模块20、处理模块40与存储模块30之间建立连接，通信总线90可包括一通路，在上述的控制磁体10、信号传输模块20、处理模块40与存储模块30之间传送信息。
另外，本发明还提出了一种电子设备，其包括存储模块30和处理模块40，处理模块40 执行所述计算机程序时可实现上述的定量闭环控制方法中的步骤，也就是说，实现上述控制方法中的任意一个技术方案中的步骤。
该电子设备可以是集成于磁控胶囊系统1000内的一部分、或者是本地的终端设备、还可以是云端服务器的一部分。
所述处理模块40可以是中央处理单元或其他通用处理器、通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。所述处理模块40是所述磁控胶囊系统1000的控制中心，利用各种接口和线路连接整个磁控胶囊系统1000的各个部分。
所述存储模块30可用于存储所述计算机程序和/或模块，所述处理模块40通过运行或执行存储在所述存储模块30内的计算机程序和/或模块，以及调用存储在存储模块30内的数据，实现所述磁控胶囊系统1000的各种功能。
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储模块30中，并由所述处理模块40执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述磁控胶囊系统1000控制方法中的执行过程。
进一步地，本发明一实施方式提供了一种可读存储介质，其存储有计算机程序，该计算机程序被处理模块40执行时可实现上述的磁控胶囊系统1000控制方法中的步骤，也就是说，实现上述磁控胶囊系统1000控制方法中的任意一个技术方案中的步骤。
所述磁控胶囊系统1000集成的模块如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。
应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施方式中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。
上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施方式的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施方式或变更均应包含在本发明的保护范围之内。

Claims (20)

  1. 一种磁控胶囊系统的定量闭环控制方法，其特征在于，包括如下步骤：
    持续获取胶囊的实时位置信息；
    获取所述胶囊的目标位置信息；
    根据所述目标位置信息，确定目标位置范围；
    根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；
    控制所述控制磁体沿所述当前移动轨迹运动；
    判断所述实时位置信息是否在所述目标范围内，若直至所述控制磁体停止运动，所述实时位置信息在所述目标位置范围外，则重复步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；控制所述控制磁体沿所述当前移动轨迹运动”，直至所述实时位置信息在所述目标位置范围内。
  2. 根据权利要求1所述的定量闭环控制方法，其特征在于，还包括步骤：
    持续获取所述控制磁体的实时磁体位置信息，其中，所述实时磁体位置信息、所述实时位置信息和所述目标位置信息均位于同一世界坐标系。
  3. 根据权利要求2所述的定量闭环控制方法，其特征在于，还包括步骤：
    获取临界距离，其中，所述临界距离为所述胶囊在液体中悬浮时所述胶囊与所述控制磁体之间的距离；
    根据所述实时磁体位置信息、所述实时位置信息和所述临界距离，计算所述胶囊的状态信息，其中，所述状态信息包括沉底状态、液面悬浮状态和吸顶状态。
  4. 根据权利要求3所述的定量闭环控制方法，其特征在于，其中，所述目标位置信息包括目标状态；
    还包括步骤：
    若所述状态信息与目标状态不一致，调节所述控制磁体的高度，直到所述胶囊处于目标状态；
    调节所述控制磁体的朝向，以使所述胶囊的内窥镜朝向指定方向。
  5. 根据权利要求4所述的定量闭环控制方法，其特征在于，
    当所述目标状态为沉底状态时，控制所述胶囊与所述控制磁体之间的距离大于所述临界距离与第一阈值之和；
    当所述目标状态为吸顶状态时，控制所述胶囊与所述控制磁体之间的距离小于所述临界距离与第二阈值之和。
  6. 根据权利要求3所述的定量闭环控制方法，其特征在于，所述步骤“获取临界距离”包括：
    根据临界距离算法或者根据测量数据获取所述临界距离；
    其中，所述临界距离算法，是根据液面悬浮状态下，所述控制磁体和所述胶囊之间的吸力与所述胶囊受到的浮力之和等于所述胶囊的重力，计算所述控制磁体与所述胶囊之间的距离。
  7. 根据权利要求2所述的定量闭环控制方法，其特征在于，还包括步骤：
    控制所述控制磁体移动至初始位置，其中，于所述初始位置，所述控制磁体与所述胶囊在垂直于所述世界坐标系的XY平面的同一条直线上。
  8. 根据权利要求2所述的定量闭环控制方法，其特征在于，还包括步骤：
    获取所述控制磁体的运动方式，所述运动方式包括平移方式和/或翻滚方式；
    当所述控制磁体的运动方式为平移方式、且所述胶囊未运动时，切换所述控制磁体的运动方式为翻滚方式。
  9. 根据权利要求8所述的定量闭环控制方法，其特征在于，所述步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹”包括：
    根据所述实时位置信息和所述目标位置信息，计算当前偏移量，其中，所述当前偏移量包括在所述世界坐标系中的X轴方向的X轴差值、以及在Y轴方向的Y轴差值；
    计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的X轴方向的第一差值，将所述第一差值与第一修正因子的乘积作为所述X轴差值；
    计算所述实时位置信息和所述目标位置信息在所述世界坐标系中的Y轴方向的第二差值，将所述第二差值与第二修正因子的乘积作为所述Y轴差值；
    若所述控制磁体的运动方式为平移方式，所述当前移动轨迹为沿所述当前偏移量平移。
  10. 根据权利要求8所述的定量闭环控制方法，其特征在于，所述步骤“根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹”包括：
    获取所述胶囊的长轴周长，其中，所述长轴周长为所述胶囊沿其长轴翻转一周对应的长度；
    根据所述实时位置信息和所述目标位置信息，计算当前翻滚量，其中，所述当前翻滚量包括所述胶囊的翻滚圈数和翻滚方向；
    若所述控制磁体的运动方式为翻滚方式，所述当前移动轨迹包括沿所述翻滚方向翻滚所述翻滚圈数。
  11. 根据权利要求2所述的定量闭环控制方法，其特征在于，其中，所述目标位置范围包括预设距离；
    还包括步骤：
    计算所述实时位置信息与所述目标位置信息在所述世界坐标系的XY平面上的投影的距离；
    当所述距离小于等于所述预设距离，所述实时位置信息在所述目标位置范围内；
    当所述距离大于所述预设距离，所述实时位置信息在所述目标位置范围外。
  12. 根据权利要求2所述的定量闭环控制方法，其特征在于，还包括步骤：
    当所述实时位置信息在所述目标位置范围内时，或者，当所述实时磁体位置信息与所述实时位置信息对应的距离大于安全距离时；
    控制所述控制磁体与所述胶囊在垂直于所述世界坐标系的XY平面的同一条直线上。
  13. 一种磁控胶囊系统的定量闭环控制方法，其特征在于，包括如下步骤：
    持续获取胶囊的实时姿态信息；
    获取胶囊的目标姿态信息；
    根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹；
    控制所述控制磁体沿所述当前转动轨迹运动；
    判断所述实时位置信息是否在所述目标范围内，若直至所述控制磁体停止运动，所述实时姿态信息在所述目标姿态范围外，则重复步骤“根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹；控制所述控制磁体沿所述当前转动轨迹运动”，直至所述实时姿态信息在所述目标姿态范围内。
  14. 根据权利要求13所述的定量闭环控制方法，其特征在于，持续获取所述控制磁体的实时磁体姿态信息，其中，所述实时磁体姿态信息、所述实时姿态信息和所述目标姿态信息均位于同一世界坐标系。
  15. 根据权利要求14所述的定量闭环控制方法，其特征在于，其中，所述当前转动轨迹包括当前转动量，所述当前转动量包括第一角度差和第二角度差；
    控制所述控制磁体转动至初始姿态；
    所述初始姿态包括与Z轴正向的第一夹角，所述目标姿态信息包括与Z轴正向的第二夹角，所述第一角度差为所述第二夹角与所述第一夹角差值与第三修正因子的乘积；
    所述初始姿态包括在XY平面与Y轴正向的第三夹角，所述目标姿态信息包括在XY平面与Y轴正向的第四夹角，所述第二角度差为所述第四夹角与所述第三夹角的差值与第四修正因子的乘积。
  16. 根据权利要求14所述的定量闭环控制方法，其特征在于，其中，所述目标姿态范围包括预设角度差；
    还包括步骤：
    计算所述实时姿态信息对应的第一单位向量；
    计算所述目标姿态信息对应的第二单位向量；
    计算所述第一单位向量与所述第二单位向量的实时角度差；
    当所述实时角度差小于等于所述预设角度差，所述实时姿态信息在所述目标姿态范围内；
    当所述实时角度差大于所述预设角度差，所述实时姿态信息在所述目标姿态范围外。
  17. 一种磁控胶囊系统，其特征在于，包括：第一获取模块、第一目标获取模块、第一计算模块、第一控制模块和第一判断模块；
    所述第一获取模块，用于持续获取胶囊的实时位置信息；
    所述第一目标获取模块，用于获取所述胶囊的目标位置信息，并根据所述目标位置信息，确定目标位置范围；
    所述第一计算模块，用于根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹；
    所述第一控制模块，用于控制所述控制磁体沿所述当前移动轨迹运动；
    所述第一判断模块，用于判断所述实时位置信息是否在所述目标范围内，当所述控制磁体停止运动，且所述实时位置信息在所述目标位置范围外，则重复“第一计算模块根据所述实时位置信息和所述目标位置信息，计算控制磁体的当前移动轨迹，第一控制模块控制所述控制磁体沿所述当前移动轨迹运动”，直到所述实时位置信息在所述目标位置范围内。
  18. 一种磁控胶囊系统，其特征在于，包括：第二获取模块、第二目标获取模块、第二计算模块、第二控制模块和第二判断模块；
    所述第二获取模块，用于持续获取胶囊的实时姿态信息；
    所述第二目标获取模块，用于获取胶囊的目标姿态信息；
    所述第二计算模块，用于根据所述目标姿态信息，确定目标姿态范围和控制磁体的当前转动轨迹；
    所述第二控制模块，用于控制所述控制磁体沿所述当前转动轨迹运动；
    所述第二判断模块，用于判断所述实时位置信息是否在所述目标范围内，当所述控制磁体停止运动，且所述实时姿态信息在所述目标姿态范围外，则重复“第二计算模块根据所述目标姿态信息，计算目标姿态范围和控制磁体的当前转动轨迹，第二控制模块控制所述控制磁体沿所述当前转动轨迹运动”，直到所述实时姿态信息在所述目标姿态范围内。
  19. 一种电子设备，其特征在于，包括存储模块和处理模块，所述存储模块存储有可在所述处理模块上运行的计算机程序，所述处理模块执行所述计算机程序时实现权利要求1所述的磁控胶囊系统的定量闭环控制方法中的步骤。
  20. 一种可读存储介质，其存储有计算机程序，其特征在于，所述计算机程序被处理模块执行时实现权利要求1所述的磁控胶囊系统的定量闭环控制方法中的步骤。
PCT/CN2023/082318 2022-03-18 2023-03-17 磁控胶囊系统及其定量闭环控制方法 WO2023174427A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2024555034A JP2025510641A (ja) 2022-03-18 2023-03-17 磁気制御カプセルシステムとその定量的閉ループ制御方法

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202210272489.2A CN116803330A (zh) 2022-03-18 2022-03-18 磁控胶囊系统及其定量闭环控制方法
CN202210272489.2 2022-03-18

Publications (1)

Publication Number Publication Date
WO2023174427A1 true WO2023174427A1 (zh) 2023-09-21

Family

ID=88022430

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2023/082318 WO2023174427A1 (zh) 2022-03-18 2023-03-17 磁控胶囊系统及其定量闭环控制方法

Country Status (3)

Country Link
JP (1) JP2025510641A (zh)
CN (1) CN116803330A (zh)
WO (1) WO2023174427A1 (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118263995A (zh) * 2024-03-28 2024-06-28 中山亿联智能科技有限公司 一种机顶盒磁吸充电控制方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170164816A1 (en) * 2015-01-06 2017-06-15 Olympus Corporation Guidance device and capsule medical device guidance system
US20170280983A1 (en) * 2015-04-17 2017-10-05 Olympus Corporation Capsule medical device guidance system
CN109044250A (zh) * 2018-08-28 2018-12-21 深圳市资福医疗技术有限公司 一种胶囊内窥镜运动控制方法、装置及终端设备
CN110236474A (zh) * 2019-06-04 2019-09-17 北京理工大学 一种主动式磁控胶囊机器人检测系统及检测方法
CN110575118A (zh) * 2019-09-11 2019-12-17 安翰科技(武汉)股份有限公司 胶囊内窥镜控制方法、系统，电子设备及可读存储介质
CN111035349A (zh) * 2020-03-11 2020-04-21 上海安翰医疗技术有限公司 胶囊内窥镜的姿态定位方法及胶囊内窥镜系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170164816A1 (en) * 2015-01-06 2017-06-15 Olympus Corporation Guidance device and capsule medical device guidance system
US20170280983A1 (en) * 2015-04-17 2017-10-05 Olympus Corporation Capsule medical device guidance system
CN109044250A (zh) * 2018-08-28 2018-12-21 深圳市资福医疗技术有限公司 一种胶囊内窥镜运动控制方法、装置及终端设备
CN110236474A (zh) * 2019-06-04 2019-09-17 北京理工大学 一种主动式磁控胶囊机器人检测系统及检测方法
CN110575118A (zh) * 2019-09-11 2019-12-17 安翰科技(武汉)股份有限公司 胶囊内窥镜控制方法、系统，电子设备及可读存储介质
CN111035349A (zh) * 2020-03-11 2020-04-21 上海安翰医疗技术有限公司 胶囊内窥镜的姿态定位方法及胶囊内窥镜系统

Also Published As

Publication number Publication date
CN116803330A (zh) 2023-09-26
JP2025510641A (ja) 2025-04-15

Similar Documents

Publication Publication Date Title
CN109676604B (zh) 机器人曲面运动定位方法及其运动定位系统
CN110967011B (zh) 一种定位方法、装置、设备及存储介质
US11612308B2 (en) Control method, control system, electronic device and readable storage medium for capsule endoscope
CN110977985A (zh) 一种定位的方法及装置
CN109658460A (zh) 一种机械臂末端相机手眼标定方法和系统
CN101612735B (zh) 移动机器人视觉系统防抖装置与防抖补偿控制方法
CN105487555A (zh) 一种无人机的悬停定位方法及装置
WO2023174427A1 (zh) 磁控胶囊系统及其定量闭环控制方法
CN108803683A (zh) 基于ZigBee无线传感网络的多摄像头追踪拍摄系统和方法
WO2023104115A1 (zh) 获取全景视频的方法、装置、系统、设备及存储介质
CN114035598B (zh) 一种多旋翼吊挂系统的视觉摆角检测与减摆方法
CN110238851B (zh) 一种移动机器人及其快速标定方法和系统
CN114536399A (zh) 基于多个位姿标识的误差检测方法及机器人系统
US20230225583A1 (en) System and method for controlling capsule endoscope
CN110207605A (zh) 一种基于机器视觉的金属结构变形的测量装置及方法
CN116797661A (zh) 一种瞳孔定位的方法以及相关装置
CN113298881B (zh) 单目相机-imu-机械臂的空间联合标定方法
CN103305410A (zh) 显微注射系统及坐标误差补偿和精确重复定位方法
CN113063416B (zh) 一种基于自适应参数互补滤波的机器人姿态融合方法
CN107678568B (zh) 一种通过移动终端模拟激光笔的方法及装置
CN117284500A (zh) 一种基于单目视觉和激光的盘绕式伸展臂位姿调整方法
CN207359075U (zh) 水下机器人非视距控制系统
CN105094377B (zh) 智能控制器的光标显示方法、设备和系统
CN112060083B (zh) 用于机械臂的双目立体视觉系统及其测量方法
WO2023174425A1 (zh) 磁控胶囊系统及其位姿标定表示方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23769929

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 2024555034

Country of ref document: JP

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 23769929

Country of ref document: EP

Kind code of ref document: A1