Capsule robot magnetic suspension system and method and readable storage medium

Through the combination of permanent magnet electromagnetic joint drive and orthogonal guide module, combined with low-pass Kalman filter and visual feedback information, the active motion and positioning problems of capsule endoscopic robots are solved, achieving a larger range of motion and stable suspension effect.

CN118924217AActive Publication Date: 2024-11-12HARBIN INSTITUTE OF TECHNOLOGY (SHENZHEN) (INSTITUTE OF SCIENCE AND TECHNOLOGY INNOVATION HARBIN INSTITUTE OF TECHNOLOGY SHENZHEN)
View PDF 2 Cites 0 Cited by

Patent Information

Application Number
CN202410955317.4
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-17
Publication Date
2024-11-12
Estimated Expiration
2044-07-17

AI Technical Summary

Technical Problem

The existing capsule endoscopic robot lacks active motion control capabilities, and the magnetic levitation system driving method has accurate magnetic field strength and gradient limitations. The magnetic sensing positioning algorithm is limited by temperature drift and work space. The visual feedback control algorithm cannot independently control the nonlinear magnetic levitation system. .

Method used

Permanent magnet electromagnetic joint drive is adopted, combined with orthogonal guide module for magnetic levitation platform motion control, adding a low-pass Kalman filter to remove noise, and fusing visual feedback and magnetic sensing information for stable levitation and active control.

Benefits of technology

The active motion and sensing positioning functions of the capsule robot are realized, the range of motion is expanded, the suspension stability and positioning accuracy are improved, and the impact of external interference is reduced.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118924217A_ABST
    Figure CN118924217A_ABST
Patent Text Reader

Abstract

The invention provides a capsule robot magnetic suspension system and method based on magnetic sensing and visual feedback and a readable storage medium, the system comprises a magnetic suspension platform, an orthogonal guide rail module, a control processor and a capsule robot, the magnetic suspension platform is installed on the orthogonal guide rail module, and the control processor is installed on the control processor. The control processor is respectively connected with the magnetic suspension platform and the orthogonal guide rail module, the control processor can drive the magnetic suspension platform to generate a control electromagnetic field, and the control processor can drive the orthogonal guide rail module to move. The capsule robot has the beneficial effects that the functions of active motion control and sensing positioning of the capsule robot can be further achieved; the movement range of the capsule robot is expanded; the influence of external interference on the positioning problem of the capsule robot is expected to be improved; and stable suspension and active control of the capsule robot are realized.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to medical equipment, and in particular to a capsule robot magnetic suspension system and method based on magnetic sensing and visual feedback, and a readable storage medium. Background Art

[0002] Capsule endoscopy robot:

[0003] The capsule endoscope robot is mainly composed of image modules, communication modules and light modules. After the subject simply swallows the capsule, the capsule moves with the peristalsis of the human gastrointestinal tract, and the various modules in the capsule also start working. Through the capsule's communication module, such as Bluetooth, the gastrointestinal conditions collected by the image module are transmitted to an external computer, and the doctor can make a corresponding judgment on the patient's condition based on these images and give a reasonable treatment plan.

[0004] Capsule robot magnetic levitation system:

[0005] The magnetic levitation system of the capsule robot is a system that provides an external magnetic field to the capsule robot. It can perform active motion control of the capsule robot driven by magnetic levitation, which means that the capsule can stop at the lesion point or suspicious point of the gastrointestinal tract, which can greatly improve the accuracy of diagnosis and treatment. According to the source of the magnetic field generated by the magnetic levitation system, the magnetic drive method can be divided into electromagnetic drive and permanent magnet drive. Electromagnetic drive refers to the generation of a precise magnetic field by combining multiple pairs of electromagnetic coils in the capsule working space. Permanent magnet drive means that the control magnetic field of the capsule is derived from an external permanent magnet, and the movement of the permanent magnet is driven by a three-dimensional drive platform or a robotic arm to generate different magnetic forces and magnetic torques to drive the capsule to move.

[0006] Magnetic sensing positioning algorithm:

[0007] The magnetic sensor can sense the magnetic field strength at the current position. The magnetic sensor positioning algorithm refers to a positioning algorithm that obtains the relative position of the capsule robot and the sensor center by analyzing and calculating the sensor values.

[0008] Visual feedback control algorithm:

[0009] In the traditional robotic arm visual feedback control algorithm, the relative positions of the working plane of the robotic arm end effector and the camera in the machine-eye system are fixed, and the camera in the hand-eye system follows the movement of the robotic arm end effector. By analyzing the image information collected by the camera, the parameters of the image features can be obtained, and then the movement of the robotic arm or other actuators can be controlled.

[0010] The defects of the prior art are as follows:

[0011] 1. The existing capsule endoscope robot only has a single image acquisition function and cannot further realize the functions of active motion control and sensor positioning of the capsule robot.

[0012] 2. There are two problems with the existing magnetic levitation system of capsule robots. The first is that when using permanent magnet drive, the magnetic field can only be controlled by changing the position and posture of the external permanent magnet, and it is impossible to generate precise magnetic field strength and gradient, so there are some limitations in motion control. The second is that when using electromagnetic drive, the magnetic field gradient generated by the electromagnetic coil will seriously decay with distance, resulting in the generally weak driving ability of the electromagnetic drive system. The working space of the drive system is generally small, and the activity range of the capsule is severely limited.

[0013] 3. There are two problems with the existing magnetic sensor positioning algorithm. The first is that the sensor readings will drift with temperature during the operation of the magnetic sensor, resulting in insufficient accuracy of the positioning algorithm. The second is that the working space of the positioning algorithm is small. Once the working space is exceeded, the magnetic field strength of the capsule robot to the magnetic sensor decays rapidly, causing the readings to drop rapidly.

[0014] 4. The existing visual feedback control algorithm is only applicable to controlling commercial robotic arms with clear parameters and motion control models. The lower control frequency cannot independently control the electromagnetic coil output of the nonlinear magnetic levitation system. Summary of the invention

[0015] In order to solve the problems in the prior art, the present invention provides a capsule robot magnetic levitation system and method based on magnetic sensing and visual feedback, and a readable storage medium.

[0016] The invention objectives of the present invention are as follows:

[0017] 1. As an alternative to traditional gastroscopy, capsule endoscopy has broad prospects. However, most capsule endoscopes on the market have a single function and basically no ability to move autonomously. They can only move by the peristalsis of the gastrointestinal tract itself. This results in doctors being unable to make a more detailed assessment of the corresponding lesion due to limited imaging data at the lesion. The present invention aims to realize the active movement capability of the capsule endoscope.

[0018] 2. With respect to the magnetic levitation system of the capsule robot, the present invention aims to change its driving mode, adopt permanent magnet and electromagnetic combined drive, and use permanent magnets and electromagnetic coils to control the stable suspension of the capsule robot, thereby improving the suspension stability of the capsule, and driving the magnetic levitation platform to move through the orthogonal guide rail module, thereby controlling the active movement of the magnetic capsule and increasing the activity range of the capsule robot.

[0019] 3. With respect to the magnetic sensing positioning algorithm, the present invention aims to add a low-pass Kalman filter to the signal processing link, which can reduce or remove the high-frequency noise components in the measurement data to obtain a smoother signal and remove the interference caused by external electromagnetic interference and the temperature rise of the sensor.

[0020] 4. Regarding the visual feedback control algorithm, the present invention aims to fuse the visual feedback with the information of the magnetic sensor, so that the visual information can be used to identify deviations in a larger range, while the information of the magnetic sensor can be used to identify deviations in a smaller range. The feedback link of the visual information is selected according to the size of the deviation to achieve stable suspension and active control of the capsule robot.

[0021] The present invention provides a capsule robot magnetic levitation system based on magnetic sensing and visual feedback, comprising a magnetic levitation platform, an orthogonal guide rail module, a control processor and a capsule robot, wherein the magnetic levitation platform is installed on the orthogonal guide rail module, the control processor is respectively connected to the magnetic levitation platform and the orthogonal guide rail module, the control processor can drive the magnetic levitation platform to generate a control electromagnetic field, the control processor can drive the orthogonal guide rail module to move, the capsule robot can maintain stable suspension at the center position of the magnetic levitation platform under the action of the control electromagnetic field generated by the magnetic levitation platform, when active motion is required to track the feature point of the lesion, the control processor controls the orthogonal guide rail module to move at a uniform and stable speed in the direction of the feature point, driving the magnetic levitation platform to move synchronously, and the capsule robot tracks the feature point under the action of the control electromagnetic field generated by the magnetic levitation platform to complete the active motion.

[0022] As a further improvement of the present invention, the magnetic levitation platform includes a magnetic sensor module and a magnetic levitation coil module, the control processor can drive the magnetic levitation coil module to generate a control electromagnetic field, and the magnetic sensor module can detect the position change of the capsule robot and transmit it to the control processor.

[0023] As a further improvement of the present invention, the magnetic sensor module and the magnetic levitation coil module are both centrally symmetrically arranged, the ideal suspension control position of the capsule robot is located at the center of the magnetic levitation platform, and the magnetic sensor module is arranged at the center of the magnetic levitation platform, above the magnetic levitation coil module and below the capsule robot.

[0024] As a further improvement of the present invention, the control processor includes an image processing module, a guide rail control module, a signal processing module and a coil control module. The signal output end of the magnetic sensing module is connected to the signal input end of the signal processing module, the signal output end of the signal processing module is connected to the signal input end of the coil control module, the signal output end of the coil control module is connected to the signal input end of the magnetic levitation coil module, the signal output end of the magnetic levitation coil module is connected to the signal input end of the capsule robot, the signal output end of the image processing module is connected to the signal input end of the guide rail control module, the signal output end of the guide rail control module is connected to the signal input end of the orthogonal guide rail module, and the signal output end of the orthogonal guide rail module is connected to the signal input end of the magnetic levitation coil module.

[0025] As a further improvement of the present invention, the signal processing module adds a low-pass Kalman filter after amplifying the initial signal to remove high-frequency noise components in the measurement data to obtain a smoother signal.

[0026] As a further improvement of the present invention, the coil control module uses the input of the signal processing module with a higher frequency as the deviation subject and the input of the image processing module with a lower frequency as the integral term based on the input of the image processing module and the signal processing module, and outputs current to the magnetic levitation coil module through a PID controller.

[0027] As a further improvement of the present invention, the capsule robot magnetic levitation system also includes a camera module, which is installed on the head of the capsule robot. The signal output end of the camera module is connected to the signal input end of the image processing module. The optical axis of the micro endoscope of the camera module coincides with the main axis of the capsule robot. The camera module is equipped with an LED lamp, which can provide a light source in the digestive tract environment.

[0028] As a further improvement of the present invention, the image processing module performs image processing and feature extraction on the image obtained by the camera module, and performs the following steps on the image: Gaussian filtering—RBG channel binarization—Canny edge detection—contour area curvature screening, to obtain a characteristic contour of the lesion feature point, calculate the image moment of the characteristic contour, and estimate the position and posture characteristics of the camera module through the obtained geometric moment and central moment;

[0029] As a further improvement of the present invention, the guide rail control module controls the orthogonal guide rail module through the output of a pulse frequency according to the position deviation between the capsule robot and the lesion feature point obtained by the image processing module.

[0030] As a further improvement of the present invention, there are two arrangements of the image processing module, guide rail control module, signal processing module and coil control module. The first arrangement is that the image processing module, guide rail control module, signal processing module and coil control module are all concentrated in a central control PC, and the central control PC controls the orthogonal guide rail module and the magnetic levitation coil module through a serial port; the second arrangement is that the image processing module is located in the central control PC, the guide rail control module is located in the guide rail control single-chip microcomputer to control the orthogonal guide rail module, and the signal processing module and coil control module are located in the coil control single-chip microcomputer to control the magnetic levitation coil module.

[0031] As a further improvement of the present invention, the orthogonal guide rail module is composed of two orthogonal linear modules, and the orthogonal guide rail module is fixedly connected to the magnetic suspension platform;

[0032] As a further improvement of the present invention, the magnetic sensing module adopts two groups of Hall sensors arranged in opposite directions and symmetrically.

[0033] As a further improvement of the present invention, the magnetic levitation coil module includes a magnetic levitation permanent magnet and two groups of orthogonal electromagnetic coils. The magnetic levitation permanent magnet can provide a vertical levitation magnetic force, always generate a vertical levitation magnetic field during operation, and generate a magnetic field in the horizontal plane that causes the capsule robot to deviate from the center position. The electromagnetic coil can provide a control magnetic field that constrains the capsule robot to the center position of the magnetic levitation platform and can be controlled by the coil control module.

[0034] As a further improvement of the present invention, the capsule robot is provided with a built-in permanent magnet, which can obtain magnetic force and magnetic torque in an external control magnetic field; the capsule robot is provided with an IMU sensor module, which can feedback the position and posture of the capsule robot.

[0035] The present invention also provides a capsule robot magnetic suspension method based on magnetic sensing and visual feedback, provides the capsule robot magnetic suspension system based on magnetic sensing and visual feedback, and performs a capsule stable suspension control process based on the capsule robot magnetic suspension system, including:

[0036] When the deviation between the capsule robot and the feature point is less than or equal to the threshold, it is considered that the center of the magnetic levitation platform has reached the ideal position. The ideal position of the capsule robot is the center of the magnetic levitation platform. The coil control module outputs PWM to the magnetic levitation coil module according to the position deviation. The magnetic levitation coil module generates a control electromagnetic field to keep the capsule robot stably suspended. The position change of the capsule robot will change the original image obtained by the camera module and the Hall signal obtained by the magnetic sensor module. The image processing module and the signal processing module are used to analyze them respectively to obtain the current position of the capsule robot. The position deviation is reduced through the circular control path to realize feedback control of the static suspension of the capsule.

[0037] As a further improvement of the present invention, the capsule stable suspension control process includes:

[0038] When the deviation between the capsule robot and the feature point is less than or equal to the threshold, it is considered that the center of the magnetic levitation platform is close enough to the feature point. At this time, the magnetic levitation coil module constrains the capsule robot near the center of the magnetic levitation platform and eliminates the remaining deviation from the feature point, so that the optical axis of the endoscope camera of the camera module is aligned with the feature point;

[0039] Taking the feature point as the reference, the ideal position of the capsule robot is the position when the main axis is aligned with the feature point, and the position deviation is obtained by subtracting it from the current position. The coil control module receives the position deviation, takes the deviation e(t) obtained by the signal processing module as the main body, and the deviation E(t) obtained by the image processing module as the integral term, and outputs the PWM parameter n(t) through the PID controller;

[0040]

[0041] Where:

[0042] e(t)——the error amount obtained by the signal processing module;

[0043] E(t)——the error amount obtained by the image processing module;

[0044] K P ——Proportional term coefficient of the controller;

[0045] K I1 ——Integral coefficient of magnetic error;

[0046] K I2 ——Visual error term coefficient;

[0047] K D ——differential term coefficient of the controller;

[0048] The magnetic levitation coil module changes the direction of the voltage through the H bridge according to the sign of the PWM parameter n(t), and changes the magnitude of the voltage through the DC power supply controller according to the magnitude of the PWM parameter n(t). The control voltage is applied to two groups of orthogonal electromagnetic coils, and the capsule robot is constrained at the center position in the horizontal direction by generating a control electromagnetic field. Since the vertical levitation force is a function of the position, when the horizontal position remains unchanged, the capsule robot can be subjected to a stable vertical levitation force, which is balanced with gravity at the designed height, and the three-dimensional position of the capsule robot can be determined.

[0049] When disturbed by the outside world, the position of the capsule robot changes, which in turn changes the original image obtained by the camera module and the Hall signal obtained by the magnetic sensor module. The original image is processed and feature extracted by the image processing module. First, the noise is removed by Gaussian filtering, and then the color area of ​​the feature point is found by RBG channel binarization. Then, Canny edge detection is performed to obtain the possible feature point contours. Finally, the contour area and curvature are screened to obtain the feature contours of the lesion feature points. The geometric moment and central moment are calculated based on the feature contours, and the position and direction of the camera module are estimated to obtain the current position of the capsule robot.

[0050] For an image with a pixel size of M×N, convert it into a grayscale image, and its (p+q)-order geometric moment is defined as follows:

[0051]

[0052] Where:

[0053] (x, y) – coordinates of the pixel;

[0054] g(x, y)——the gray value at the corresponding coordinate;

[0055] The area of ​​the characteristic contour m 00 It is expressed as the zero-order moment, which represents the vertical distance between the camera module and the feature contour.

[0056]

[0057] The centroid of the feature profile is determined by the zero-order moment m 00 and the first-order moment m 10 and m 01 Calculate, which represents the horizontal position change between the camera module and the feature contour,

[0058]

[0059] After obtaining the barycentric coordinates, calculate the (p+q)-order center distance as follows:

[0060]

[0061] Calculate the eigenvector with the maximum eigenvalue of the covariance matrix C of the feature profile, which represents the angle θ of the major axis of the feature profile and is used to correct the axial rotation of the original image obtained by the camera module;

[0062] Covariance matrix C:

[0063]

[0064] Angle θ of the major axis of the feature profile:

[0065]

[0066] The Hall signal passes through the signal processing module, and the voltage is first adjusted to a range that can be read by the ADC interface through an amplifier circuit, and then a low-pass Kalman filter is performed to remove high-frequency noise in the measurement data;

[0067] A first-order lag filter is used, which works based on the following difference equation:

[0068] y(t)=(1-α)·x(t)+α·y(t-1)

[0069] Where:

[0070] y(t) is the output signal after filtering;

[0071] x(t) is the input signal;

[0072] y(t-1) is the output signal of the previous time period;

[0073] α is the filter time constant;

[0074] During the control process, the Hall signal is linearly related to the magnetic field strength at the magnetic sensor module. Since the capsule robot has a built-in permanent magnet, the different positions of the built-in permanent magnet will affect the magnetic field strength at the magnetic sensor module. The current position of the capsule robot is obtained based on the relationship between the position of the capsule robot and the magnetic field strength.

[0075]

[0076] Where:

[0077] m is the magnetic moment of the permanent magnet built into the capsule robot;

[0078] is the direction vector of the relative position between the capsule robot and the magnetic sensor module;

[0079] B sensor is the magnetic field strength at the magnetic sensor module;

[0080] B constThe magnetic field strength provided for the magnetic levitation permanent magnet;

[0081] B coil (I) is the magnetic field strength provided by the magnetic levitation coil module, and I is the coil current;

[0082] The current position of the capsule robot is input into the summer, and the position deviation is continuously reduced through the loop control path to achieve stable suspension of the capsule robot.

[0083] As a further improvement of the present invention, when the deviation between the capsule robot and the feature point in the image space is less than or equal to 3 pixels, it is considered that the center of the magnetic suspension platform has reached the ideal position.

[0084] As a further improvement of the present invention, a capsule active motion control process based on the capsule robot magnetic levitation system includes:

[0085] When the deviation between the capsule robot and the feature point is greater than the threshold, the guide rail control module outputs PWM to the orthogonal guide rail module according to the feature point position deviation. The orthogonal guide rail module drives the magnetic levitation platform to move synchronously, drives the center of the magnetic levitation platform to actively move close to the feature point, and performs the capsule stable suspension control process to keep the capsule robot stably suspended at the center of the magnetic levitation platform, thereby making the position of the capsule robot close to the feature point. The capsule robot approaching the feature point will affect the original image of the camera module, thereby changing the image processing module to obtain the current relative position of the feature point, and reduce the position deviation through the loop control path to achieve feedback control of the active motion of the capsule.

[0086] As a further improvement of the present invention, the capsule active motion control process includes:

[0087] When the deviation between the capsule robot and the feature point is greater than the threshold, the center of the magnetic levitation platform is not close enough to the feature point. At this time, the orthogonal guide module needs to make the magnetic levitation platform track the movement of the feature point, and the coil control module fixed on the magnetic levitation platform constrains the capsule robot to move synchronously, so that the main axis of the capsule robot tracks the movement of the feature point, reducing the relative position deviation of the feature point in the image space;

[0088] The ideal relative position of the feature point in the image space is located at the center of the image, and the feature point position deviation can be obtained by subtracting it from the current feature point position. The guide rail control module calculates the position deviation of the magnetic suspension translation in the physical space based on the feature point position deviation. According to the needs of positioning accuracy and operation stability, the discrete control output PWM of a fixed moving distance is used to send to the orthogonal guide rail module, and the continuous control process is divided into multiple discrete visual positioning cycles. Within a cycle, the direction of the guide rail movement is determined according to the direction of the offset. Each movement sends a pulse wave to the guide rail in the direction of movement, so that the guide rail moves the minimum distance, and the orthogonal guide rail module can be operated stably and smoothly.

[0089]

[0090] Where:

[0091] s is the lead of the guide rail;

[0092] N is the number of PWM pulses sent to the guide rail servo motor;

[0093] N0 is the number of pulse subdivisions required for the guide servo motor to make one revolution;

[0094] The orthogonal guide rail module drives the magnetic levitation platform to move synchronously, and drives the center of the magnetic levitation platform to actively move close to the feature point;

[0095] The relative motion of the magnetic levitation platform is equivalent to applying external interference to the stable suspension control of the capsule robot, causing the position of the capsule robot to deviate from the center of the magnetic levitation platform. At this time, it is necessary to perform stable suspension control on the capsule robot through magnetic sensor feedback to keep the capsule robot stably suspended at the center of the magnetic levitation platform. The position change of the capsule robot is detected by the magnetic sensor module, and the current position is obtained through processing by the signal processing module. Then, the position deviation is input into the coil control module. The coil control module outputs PWM parameters to the magnetic levitation coil module through the PID controller according to the deviation obtained by the signal processing module. The magnetic levitation coil module generates a control electromagnetic field to constrain the capsule robot to the center of the magnetic levitation platform, so as to achieve the effect of active movement of the capsule robot approaching the feature point.

[0096] The capsule robot's approach to the feature point will affect the original image of the camera module, and then change the image processing module to obtain the current relative position of the feature point. The input summer can reduce the relative position deviation of the feature point. By looping the control path, the relative position deviation of the feature point is reduced, and the position deviation of the capsule robot is reduced, so that the capsule robot can actively track the feature point while maintaining a stable suspension state.

[0097] As a further improvement of the present invention, when the deviation between the capsule robot and the feature point in the image space is greater than 3 pixels, the guide rail control module outputs PWM to the orthogonal guide rail module according to the feature point position deviation.

[0098] As a further improvement of the present invention, during the control process, the critical point where the motion state of the orthogonal guide rail module shows overshoot oscillation is automatically used as the threshold for judging the deviation size of the feature point, rather than a fixed threshold set according to experience. When the orthogonal guide rail module is at the critical point, it means that the motion of the guide rail can no longer eliminate the deviation from the feature point, and the remaining deviation from the feature point can be eliminated only by the magnetic levitation coil module.

[0099] The present invention also provides a readable storage medium, in which execution instructions are stored. When the execution instructions are executed by a processor, they are used to implement the method.

[0100] The beneficial effects of the present invention are:

[0101] 1. In view of the fact that the existing capsule endoscope robot only has a single image acquisition function, the present invention proposes a new capsule robot solution, which includes a magnetic suspension platform, an orthogonal guide rail module, a control processor and a capsule robot, and can further realize the functions of active motion control and sensor positioning of the capsule robot;

[0102] 2. With respect to the existing capsule robot magnetic levitation system, the permanent magnet electromagnetic joint drive proposed in the present invention can effectively avoid the disadvantages of using permanent magnets or electromagnets alone, can generate precise magnetic field strength and gradient, and drive the magnetic levitation platform to move through the orthogonal guide rail module, thereby expanding the range of activities of the capsule robot;

[0103] 3. In order to solve the problems of external electromagnetic interference and sensor temperature rise interference in the existing magnetic sensor positioning algorithm, adding a low-pass Kalman filter in the signal processing link can reduce or remove the high-frequency noise components in the measurement data to obtain a smoother signal, which is expected to improve the impact of external interference on the positioning of the capsule robot.

[0104] 4. In order to solve the problem of nonlinear model control of the existing visual feedback control algorithm, model-free control is used to fuse the visual feedback with the information of the magnetic sensor. The feedback link of the visual information is selected according to the size of the deviation. In a larger range of deviations, active motion control is performed, and in a smaller range of deviations, the stable suspension effect of the capsule robot is improved, thereby achieving stable suspension and active control of the capsule robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0105] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other solutions can be obtained based on these drawings without paying creative work.

[0106] Figure 1 It is a principle block diagram of a capsule robot magnetic suspension system based on magnetic sensing and visual feedback of the present invention;

[0107] Figure 2 It is a schematic diagram of the decomposed structure of a capsule robot magnetic suspension system based on magnetic sensing and visual feedback of the present invention;

[0108] Figure 3This is a capsule stable suspension control flow chart of a capsule robot magnetic suspension method based on magnetic sensing and visual feedback of the present invention;

[0109] Figure 4 The present invention discloses a capsule active motion control flow chart of a capsule robot magnetic levitation method based on magnetic sensing and visual feedback. DETAILED DESCRIPTION

[0110] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0111] In the description of the present invention, it should be understood that the terms "center", "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside" and the like indicate positions or positional relationships based on the positions or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the scope of protection of the present invention. In addition, the terms "first", "second", etc. are only used for descriptive purposes, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, features defined as "first", "second", etc. may explicitly or implicitly include one or more of the features. In the description of the present invention, unless otherwise specified, "multiple" means two or more.

[0112] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a direct connection, or an indirect connection through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood by specific circumstances.

[0113] The present invention will be further described below in conjunction with the accompanying drawings and specific implementation methods.

[0114] like Figure 1 to Figure 2 As shown, a capsule robot magnetic levitation system based on magnetic sensing and visual feedback includes a magnetic levitation platform, an orthogonal guide rail module (also called a two-dimensional guide rail) 103, a control processor and a capsule robot (referred to as capsule) 109.

[0115] When the capsule robot's magnetic levitation system is powered on, the capsule robot 109 can maintain stable suspension at the center of the magnetic levitation platform under the action of the control electromagnetic field generated by the magnetic levitation coil module 108. When active movement is required to track the lesion feature point, the orthogonal guide rail module 103 moves in a uniform and stable manner in the direction of the feature point, driving the magnetic levitation coil module 108 on the magnetic levitation platform to move synchronously. The capsule robot 109 tracks the feature point under the action of the control electromagnetic field, completing the active movement of the magnetic levitation system.

[0116] The magnetic levitation platform is mainly composed of a magnetic sensor module 105 and a magnetic levitation coil module 108. The magnetic sensor module 105 and the magnetic levitation coil module 108 are arranged symmetrically, and the center of each module coincides with the center of the magnetic levitation platform. The ideal suspension control position of the capsule robot is located at the center of the magnetic levitation platform. The magnetic sensor module 105 is arranged at the center of the magnetic levitation platform, above the magnetic levitation coil module 108 and below the capsule robot 109.

[0117] The control processor is mainly composed of an image processing module 102, a guide rail control module 103, a signal processing module 106, and a coil control module 107, and there are two arrangements. The first arrangement is that all modules are centralized to a central control PC, and then the orthogonal guide rail module 104 and the magnetic levitation coil module 108 are controlled through a serial port. The second arrangement is that the image processing module 102 is located in the central control PC, the guide rail control module 103 is located in the guide rail control microcontroller to control the orthogonal guide rail module 104, and the signal processing module 106 and the coil control module 107 are located in the coil control microcontroller to control the magnetic levitation coil module 108.

[0118] The camera module 101 is fixed on the head of the capsule robot 109. The optical axis of the micro endoscope of the camera module coincides with the main axis of the capsule robot. The camera module 101 is fixed by interference fit or glue dispensing to ensure that the image obtained by the camera module 101 can reflect the position and posture characteristics of the capsule robot 109. The camera module 101 is equipped with an LED lamp to provide light in the digestive tract environment.

[0119] The image processing module 102 performs image processing and feature extraction on the image obtained by the camera module 101, and performs the following operations on the image: Gaussian filtering - RBG channel binarization - Canny edge detection - contour area curvature screening, to obtain the characteristic contour of the lesion feature points, calculate the image moment of the characteristic contour, and estimate the position and posture characteristics of the camera module 101 through the obtained geometric moment and central moment.

[0120] The guide rail control module 103 controls the orthogonal guide rail module 104 by outputting a pulse frequency according to the position deviation between the capsule robot 109 and the lesion feature point obtained by the image processing module 102 .

[0121] The orthogonal guide rail module 104 is composed of two orthogonal linear modules, and the guide rail moving platform is fixed to the magnetic suspension platform.

[0122] The magnetic sensor module 105 uses two sets of Hall sensors arranged in opposite directions and symmetrically, which can eliminate the influence of temperature drift of the sensors and other circuit elements.

[0123] The signal processing module 106 amplifies the initial signal and then adds a low-pass Kalman filter to remove high-frequency noise components in the measurement data to obtain a smoother signal.

[0124] The coil control module 107 uses the input of the image processing module 102 and the signal processing module 106 as the deviation subject and the input of the image processing module 102 as the integral term, and outputs current to the magnetic suspension coil module 108 through the PID controller.

[0125] The magnetic suspension coil module 108 includes a magnetic suspension permanent magnet 1081 and two groups of orthogonal electromagnetic coils 1082. The magnetic suspension permanent magnet 1081 is used to provide a vertical suspension magnetic force, which cannot be controlled. It always generates a vertical suspension magnetic field before and after operation, and generates a magnetic field on the horizontal plane that causes the capsule robot 109 to deviate from the center position. The magnetic suspension coil 1082 is used to provide a control magnetic field that constrains the capsule robot 109 at the center position of the magnetic suspension platform, and can be controlled by the coil control module 107.

[0126] The capsule robot 109 includes a built-in permanent magnet, a camera module 101 and other functional modules. The built-in permanent magnet is used to obtain magnetic force and magnetic torque in an external control magnetic field, and the camera module is used to transmit images of the digestive tract. As a further improvement of the present invention, other functional modules can add an IMU sensor module to feedback the position and posture of the capsule robot 109, which can have a more accurate positioning effect. As a further improvement of the present invention, other functional modules can add a minimally invasive surgery function module to perform sampling or medication functions.

[0127] The present invention also provides a capsule robot magnetic suspension method based on magnetic sensing and visual feedback, provides the capsule robot magnetic suspension system based on magnetic sensing and visual feedback, and performs a capsule stable suspension control process based on the capsule robot magnetic suspension system, including:

[0128] When the deviation between the capsule robot 109 and the feature point is less than or equal to the threshold, it is considered that the center of the magnetic levitation platform has reached the ideal position. The ideal position of the capsule robot 109 is the center of the magnetic levitation platform. The coil control module 107 outputs PWM to the magnetic levitation coil module 108 according to the position deviation. The magnetic levitation coil module 108 keeps the capsule robot 109 stably suspended by generating a control electromagnetic field. The position change of the capsule robot 109 will change the original image obtained by the camera module 101 and the Hall signal obtained by the magnetic sensor module. The image processing module 102 and the signal processing module 106 are used to analyze them respectively to obtain the current position of the capsule robot. The position deviation is reduced through a circular control path to realize feedback control of the static suspension of the capsule.

[0129] The specific process of capsule stable suspension control is as follows:

[0130] like Figure 3 As shown, when the deviation between the capsule robot 109 and the feature point is small (less than or equal to 3 pixels in the image space), it can be considered that the center of the magnetic levitation platform is close enough to the feature point. At this time, the magnetic levitation coil module 108 can constrain the capsule robot 109 near the center of the magnetic levitation platform and eliminate the remaining deviation from the feature point, so that the optical axis of the endoscopic camera of the camera module 101 is aligned with the feature point.

[0131] Based on the feature point, the ideal position of the capsule robot 109 is the position when the main axis is aligned with the feature point, and the position deviation can be obtained by subtracting it from the current position. The coil control module 107 receives the position deviation, takes the deviation e(t) obtained by the signal processing module 106 as the main body, and the deviation E(t) obtained by the image processing module 102 as the integral term, and outputs the PWM parameter n(t) through the PID controller.

[0132]

[0133] In the formula,

[0134] e(t)——the error amount obtained by the signal processing module 106;

[0135] E(t)——the error amount obtained by the image processing module 102;

[0136] K P ——Proportional term coefficient of the controller;

[0137] K I1 ——Integral coefficient of magnetic error;

[0138] K I2 ——Visual error term coefficient;

[0139] K D ——differential term coefficient of the controller;

[0140] The magnetic suspension coil module 108 changes the direction of the voltage through the H bridge according to the sign of the PWM parameter n(t), changes the magnitude of the voltage through the DC power supply controller according to the magnitude of the PWM parameter n(t), and applies the control voltage to two groups of orthogonal electromagnetic coils, thereby generating a control magnetic field to constrain the capsule robot 109 in the horizontal direction to the center position. Since the vertical suspension force is a function of the position, when the horizontal position remains unchanged, the capsule robot 109 can be subjected to a stable vertical suspension force, which is balanced with gravity at the designed height, and the three-dimensional position of the capsule robot 109 can be determined.

[0141] When disturbed by the outside world, the position of the capsule robot 109 will change, thereby changing the original image obtained by the camera module 101 and the Hall signal obtained by the magnetic sensor module 105. The original image is processed and feature extracted by the image processing module 102. First, the noise is removed by Gaussian filtering, then the color area of ​​the feature point is found by RBG channel binarization, and then Canny edge detection is performed to obtain the possible feature point contour, and finally the contour area and curvature are screened to obtain the feature contour of the lesion feature point. The geometric moment and central moment are calculated based on the feature contour, and the position and direction of the camera module 101 are estimated to obtain the current position of the capsule robot.

[0142] For an image with a pixel size of M×N, it is converted into a grayscale image, and its (p+q)-order geometric moment is defined as follows.

[0143]

[0144] In the formula,

[0145] (x, y) – coordinates of the pixel;

[0146] g(x, y)——the gray value at the corresponding coordinate;

[0147] The area of ​​the characteristic contour m 00 It can be expressed by the zero-order moment, which represents the vertical distance between the camera module 101 and the characteristic contour,

[0148]

[0149] The centroid of the feature profile can be obtained by the zero-order moment m 00 and the first-order moment m 10 and m 01 Calculate, representing the horizontal position change between the camera module 101 and the characteristic contour,

[0150]

[0151] After obtaining the barycentric coordinates, the (p+q)-order center distance can be calculated as shown below.

[0152]

[0153] The eigenvector with the maximum eigenvalue of the covariance matrix C of the characteristic profile is calculated, which represents the angle θ of the long axis of the characteristic profile and is used to correct the axial rotation of the original image obtained by the camera module 101 .

[0154] Covariance matrix C:

[0155]

[0156] Angle θ of the major axis of the feature profile:

[0157]

[0158] The Hall signal passes through the signal processing module 106, and the voltage is first adjusted to a suitable range that can be read by the ADC interface through an amplifier circuit, and then a low-pass Kalman filter is performed to remove high-frequency noise in the measurement data.

[0159] Consider a first-order lag filter, which works based on the following difference equation:

[0160] y(t)=(1-α)·x(t)+α·y(t-1)

[0161] In the formula,

[0162] y(t) is the output signal after filtering;

[0163] x(t) is the input signal;

[0164] y(t-1) is the output signal of the previous time period;

[0165] α is the filter time constant;

[0166] During the control process, the Hall signal is linearly related to the magnetic field strength at the magnetic sensor module 105. Since the capsule robot 109 has a built-in permanent magnet, the built-in permanent magnet in different positions will affect the magnetic field strength at the magnetic sensor module 105. The current position of the capsule robot 109 can be obtained based on the relationship between the position of the capsule robot 109 and the magnetic field strength.

[0167]

[0168] In the formula,

[0169] m is the magnetic moment of the built-in permanent magnet of the capsule robot 109;

[0170] is the direction vector of the relative position between the capsule robot 109 and the magnetic sensor module 105;

[0171] B sensor is the magnetic field strength at the magnetic sensor module 105;

[0172] B const The magnetic field strength provided for the magnetic levitation permanent magnet;

[0173] B coil (I) is the magnetic field strength provided by the magnetic suspension coil module 108, and I is the coil current;

[0174] The current position of the capsule robot 109 is input into the summer, and the position deviation can be continuously reduced through a cyclic control path, thereby achieving stable suspension of the capsule robot 109 .

[0175] The capsule active motion control process based on the capsule robot magnetic levitation system includes:

[0176] When the deviation between the capsule robot 109 and the feature point is greater than the threshold, the guide rail control module 103 outputs PWM to the orthogonal guide rail module 104 according to the feature point position deviation, and the orthogonal guide rail module 104 drives the magnetic levitation platform to move synchronously, drives the center of the magnetic levitation platform to actively move close to the feature point, and performs the capsule stable suspension control process, so that the capsule robot 109 maintains stable suspension at the center of the magnetic levitation platform, and then makes the position of the capsule robot 109 close to the feature point. The capsule robot 109 approaching the feature point will affect the original image of the camera module, and then change the image processing module 102 to obtain the current relative position of the feature point, and reduce the position deviation through the loop control path, so as to realize the feedback control of the active motion of the capsule.

[0177] In the capsule stable suspension control process, the critical point where the motion state of the orthogonal guide rail module 104 overshoots and oscillates can be automatically used as the threshold for judging the deviation of the feature point during the control process, rather than a fixed threshold set based on experience. When the orthogonal guide rail module 104 is at the critical point, it means that the motion of the guide rail can no longer eliminate the deviation from the feature point, and the remaining deviation from the feature point can be eliminated only by the magnetic suspension coil module 108.

[0178] The capsule active motion control process is as follows:

[0179] like Figure 4 As shown, when the deviation between the capsule robot 109 and the feature point is large (greater than 3 pixels in the image space), the center of the magnetic suspension platform is not close enough to the feature point. At this time, the orthogonal guide rail module 104.

[0180] It is necessary to make the magnetic levitation platform track the movement of feature points, and the coil control module 107 fixed on the magnetic levitation platform constrains the capsule robot 109 to move synchronously, so that the main axis of the capsule robot 109 tracks the movement of feature points and reduces the relative position deviation of feature points in the image space.

[0181] The ideal relative position of the feature point in the image space is located at the center of the image, and the feature point position deviation can be obtained by subtracting it from the current feature point position. The guide rail control module 103 calculates the position deviation of the magnetic suspension translation in the physical space based on the feature point position deviation, and outputs PWM to the orthogonal guide rail module 104 using discrete control with a fixed moving distance according to the needs of positioning accuracy and operation stability. The continuous control process is divided into multiple discrete visual positioning cycles, and the direction of the guide rail movement is determined according to the direction of the offset within a cycle. Each movement sends a pulse wave to the guide rail in the direction of movement, so that the guide rail moves the minimum distance, and the orthogonal guide rail module 104 can be operated stably and smoothly.

[0182]

[0183] In the formula

[0184] s is the lead of the guide rail, which can be selected as 75mm;

[0185] N is the number of PWM pulses sent to the guide rail servo motor;

[0186] N0 is the number of pulse subdivisions required for the guide servo motor to make one revolution, which can be set to 1600;

[0187] The orthogonal guide rail module 104 drives the magnetic suspension platform to move synchronously, and drives the center of the magnetic suspension platform to actively move close to the feature point.

[0188] The relative motion of the magnetic levitation platform is equivalent to applying external interference to the stable suspension control of the capsule robot 109, causing the position of the capsule robot 109 to deviate from the center of the magnetic levitation platform. At this time, it is necessary to perform stable suspension control on the capsule through magnetic sensor feedback so that the capsule robot 109 can maintain stable suspension at the center of the magnetic levitation platform. The position change of the position of the capsule robot 109 will be detected by the magnetic sensor module 105, and the current position will be obtained through the signal processing module 106, and then the position deviation will be input into the coil control module 107. The coil control module 107 outputs PWM parameters to the magnetic levitation coil module 108 through the PID controller according to the deviation obtained by the signal processing module 106. The magnetic levitation coil module 108 generates a control magnetic field to constrain the capsule robot to the center of the magnetic levitation platform, so as to achieve the effect of the capsule robot 109 actively moving close to the feature point.

[0189] The capsule robot 109 approaches the feature point, which affects the original image of the camera module 101, thereby changing the current relative position of the feature point obtained by the image processing module 102, and the input summator can reduce the relative position deviation of the feature point. By looping the control path, the relative position deviation of the feature point is reduced, and the position deviation of the capsule robot 109 is reduced, so that the capsule robot 109 can track the active movement of the feature point while maintaining a stable suspension state.

[0190] In the capsule active motion control process, the critical point where the motion state of the orthogonal rail module 104 overshoots and oscillates can be automatically used as the threshold for judging the deviation of the feature point during the control process, rather than a fixed threshold set based on experience. When the orthogonal rail module 104 is at the critical point, it means that the motion of the rail can no longer eliminate the deviation from the feature point, and the remaining deviation from the feature point can be eliminated only by the magnetic suspension coil module 108.

[0191] Compared with the prior art, the present invention provides a capsule robot magnetic levitation system and method based on magnetic sensing and visual feedback, and a readable storage medium, which has the following technical innovations:

[0192] 1. The magnetic levitation system adopts permanent magnet and electromagnetic joint drive, and uses a control processor to accurately drive the electromagnetic coil.

[0193] 2. The driving platform is driven by a two-dimensional guide rail to realize active motion control of the capsule robot.

[0194] 3. Adding a low-pass Kalman filter to the magnetic positioning signal processing link can reduce high-frequency noise.

[0195] 4. Fuse the visual feedback with the information of the magnetic sensor, select the feedback link of the visual information according to the size of the deviation, and realize the stable suspension and active control of the capsule robot.

[0196] The above contents are further detailed descriptions of the present invention in combination with specific preferred embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. For ordinary technicians in the technical field to which the present invention belongs, several simple deductions or substitutions can be made without departing from the concept of the present invention, which should be regarded as falling within the protection scope of the present invention.

Claims

1. A capsule robot magnetic levitation system based on magnetic sensing and visual feedback, characterized in that: It includes a magnetic levitation platform, an orthogonal guide rail module, a control processor and a capsule robot, wherein the magnetic levitation platform is installed on the orthogonal guide rail module, the control processor is connected to the magnetic levitation platform and the orthogonal guide rail module respectively, the control processor can drive the magnetic levitation platform to generate a control electromagnetic field, the control processor can drive the orthogonal guide rail module to move, and the capsule robot can maintain stable suspension at the center position of the magnetic levitation platform under the action of the control electromagnetic field generated by the magnetic levitation platform. When active movement is required to track the feature point of the lesion, the control processor controls the orthogonal guide rail module to move in a uniform and stable manner in the direction of the feature point, driving the magnetic levitation platform to move synchronously, and the capsule robot tracks the feature point under the action of the control electromagnetic field generated by the magnetic levitation platform to complete the active movement.

2. The capsule robot magnetic levitation system based on magnetic sensing and visual feedback according to claim 1 is characterized in that: The magnetic levitation platform includes a magnetic sensor module and a magnetic levitation coil module, the control processor can drive the magnetic levitation coil module to generate a control electromagnetic field, and the magnetic sensor module can detect the position change of the capsule robot and transmit it to the control processor; The magnetic sensor module and the magnetic levitation coil module are both arranged in a centrally symmetrical manner. The ideal suspension control position of the capsule robot is located at the center of the magnetic levitation platform. The magnetic sensor module is arranged at the center of the magnetic levitation platform, above the magnetic levitation coil module and below the capsule robot.

3. The capsule robot magnetic levitation system based on magnetic sensing and visual feedback according to claim 2 is characterized in that: The control processor includes an image processing module, a guide rail control module, a signal processing module and a coil control module. The signal output end of the magnetic sensing module is connected to the signal input end of the signal processing module, the signal output end of the signal processing module is connected to the signal input end of the coil control module, the signal output end of the coil control module is connected to the signal input end of the magnetic levitation coil module, the signal output end of the magnetic levitation coil module is connected to the signal input end of the capsule robot, the signal output end of the image processing module is connected to the signal input end of the guide rail control module, the signal output end of the guide rail control module is connected to the signal input end of the orthogonal guide rail module, and the signal output end of the orthogonal guide rail module is connected to the signal input end of the magnetic levitation coil module.

4. The capsule robot magnetic levitation system based on magnetic sensing and visual feedback according to claim 3 is characterized in that: The signal processing module amplifies the initial signal and then adds a low-pass Kalman filter to remove high-frequency noise components in the measurement data to obtain a smoother signal; The coil control module uses the input of the signal processing module with a higher frequency as the deviation subject and the input of the image processing module with a lower frequency as the integral term according to the input of the image processing module and the signal processing module, and outputs current to the magnetic suspension coil module through a PID controller; The capsule robot magnetic suspension system further includes a camera module, which is mounted on the head of the capsule robot, wherein a signal output end of the camera module is connected to a signal input end of the image processing module, an optical axis of a micro endoscope of the camera module coincides with a main axis of the capsule robot, and the camera module is equipped with an LED lamp, which can provide a light source in the digestive tract environment; The image processing module performs image processing and feature extraction on the image obtained by the camera module, and performs the following steps on the image: Gaussian filtering, RBG channel binarization, Canny edge detection, contour area curvature screening, to obtain the characteristic contour of the lesion feature point, calculate the image moment of the characteristic contour, and estimate the position and posture characteristics of the camera module through the obtained geometric moment and central moment; The guide rail control module controls the orthogonal guide rail module by outputting a pulse frequency according to the position deviation between the capsule robot and the lesion feature point obtained by the image processing module.

5. The capsule robot magnetic levitation system based on magnetic sensing and visual feedback according to claim 2 is characterized in that: The orthogonal guide rail module is composed of two orthogonal linear modules, and the orthogonal guide rail module is fixedly connected to the magnetic suspension platform; The magnetic sensing module uses two groups of Hall sensors arranged symmetrically in opposite directions; The magnetic levitation coil module includes a magnetic levitation permanent magnet and two groups of orthogonal electromagnetic coils. The magnetic levitation permanent magnet can provide a vertical levitation magnetic force, always generates a vertical levitation magnetic field during operation, and generates a magnetic field on the horizontal plane that causes the capsule robot to deviate from the center position. The electromagnetic coil can provide a control magnetic field that constrains the capsule robot to the center position of the magnetic levitation platform and can be controlled by the coil control module; The capsule robot is provided with a built-in permanent magnet, which can obtain magnetic force and magnetic torque in an external control magnetic field; the capsule robot is provided with an IMU sensor module, which can feedback the position and posture of the capsule robot.

6. A capsule robot magnetic levitation method based on magnetic sensing and visual feedback, characterized in that: A capsule robot magnetic suspension system based on magnetic sensing and visual feedback according to any one of claims 1 to 5 is provided, and a capsule stable suspension control process is performed based on the capsule robot magnetic suspension system, comprising: When the deviation between the capsule robot and the feature point is less than or equal to the threshold, it is considered that the center of the magnetic levitation platform has reached the ideal position. The ideal position of the capsule robot is the center of the magnetic levitation platform. The coil control module outputs PWM to the magnetic levitation coil module according to the position deviation. The magnetic levitation coil module generates a control electromagnetic field to keep the capsule robot stably suspended. The position change of the capsule robot will change the original image obtained by the camera module and the Hall signal obtained by the magnetic sensor module. The image processing module and the signal processing module are used to analyze them respectively to obtain the current position of the capsule robot. The position deviation is reduced through the circular control path to realize feedback control of the static suspension of the capsule.

7. The capsule robot magnetic levitation method based on magnetic sensing and visual feedback according to claim 6 is characterized in that: The capsule stable suspension control process includes: When the deviation between the capsule robot and the feature point is less than or equal to the threshold, it is considered that the center of the magnetic levitation platform is close enough to the feature point. At this time, the magnetic levitation coil module constrains the capsule robot near the center of the magnetic levitation platform and eliminates the remaining deviation from the feature point, so that the optical axis of the endoscope camera of the camera module is aligned with the feature point; Taking the feature point as the reference, the ideal position of the capsule robot is the position when the main axis is aligned with the feature point, and the position deviation is obtained by subtracting it from the current position. The coil control module receives the position deviation, takes the deviation e(t) obtained by the signal processing module as the main body, and the deviation E(t) obtained by the image processing module as the integral term, and outputs the PWM parameter n(t) through the PID controller; Where: e(t)——the error amount obtained by the signal processing module; E(t)——the error amount obtained by the image processing module; K P ——Proportional term coefficient of the controller; K I1 ——Integral coefficient of magnetic error; K I2 ——Visual error term coefficient; K D ——differential term coefficient of the controller; The magnetic levitation coil module changes the direction of the voltage through the H bridge according to the sign of the PWM parameter n(t), and changes the magnitude of the voltage through the DC power supply controller according to the magnitude of the PWM parameter n(t). The control voltage is applied to two groups of orthogonal electromagnetic coils, and the capsule robot is constrained at the center position in the horizontal direction by generating a control electromagnetic field. Since the vertical levitation force is a function of the position, when the horizontal position remains unchanged, the capsule robot can be subjected to a stable vertical levitation force, which is balanced with gravity at the designed height, and the three-dimensional position of the capsule robot can be determined. When disturbed by the outside world, the position of the capsule robot changes, which in turn changes the original image obtained by the camera module and the Hall signal obtained by the magnetic sensor module. The original image is processed and feature extracted by the image processing module. First, the noise is removed by Gaussian filtering, and then the color area of ​​the feature point is found by RBG channel binarization. Then, Canny edge detection is performed to obtain the possible feature point contours. Finally, the contour area and curvature are screened to obtain the feature contours of the lesion feature points. The geometric moment and central moment are calculated based on the feature contours, and the position and direction of the camera module are estimated to obtain the current position of the capsule robot. For an image with a pixel size of M×N, convert it into a grayscale image, and its (p+q)-order geometric moment is defined as follows: Where: (x, y) – coordinates of the pixel; g(x, y)——the gray value at the corresponding coordinate; The area of ​​the characteristic contour m 00 It is expressed as the zero-order moment, which represents the vertical distance between the camera module and the feature contour. The centroid of the feature profile is determined by the zero-order moment m 00 and the first-order moment m 10 and m 01 Calculate, which represents the horizontal position change between the camera module and the feature contour, After obtaining the barycentric coordinates, calculate the (p+q)-order center distance as follows: Calculate the eigenvector with the maximum eigenvalue of the covariance matrix C of the feature profile, which represents the angle θ of the major axis of the feature profile and is used to correct the axial rotation of the original image obtained by the camera module; Covariance matrix C: Angle θ of the major axis of the feature profile: The Hall signal passes through the signal processing module, and the voltage is first adjusted to a range that can be read by the ADC interface through an amplifier circuit, and then a low-pass Kalman filter is performed to remove high-frequency noise in the measurement data; A first-order lag filter is used, which works based on the following difference equation: y(t)=(1-α)·x(t)+α·y(t-1) Where: y(t) is the output signal after filtering; x(t) is the input signal; y(t-1) is the output signal of the previous time period; α is the filter time constant; During the control process, the Hall signal is linearly related to the magnetic field strength at the magnetic sensor module. Since the capsule robot has a built-in permanent magnet, the different positions of the built-in permanent magnet will affect the magnetic field strength at the magnetic sensor module. The current position of the capsule robot is obtained based on the relationship between the position of the capsule robot and the magnetic field strength. Where: m is the magnetic moment of the permanent magnet built into the capsule robot; is the direction vector of the relative position between the capsule robot and the magnetic sensor module; B sensor is the magnetic field strength at the magnetic sensor module; B const The magnetic field strength provided for the magnetic levitation permanent magnet; B coil (I) is the magnetic field strength provided by the magnetic levitation coil module, and I is the coil current; The current position of the capsule robot is input into the summator, and the position deviation is continuously reduced through the loop control path to achieve stable suspension of the capsule robot; During the control process, the critical point where the motion state of the orthogonal guide module shows overshoot oscillation is automatically used as the threshold for judging the deviation size of the feature point, rather than a fixed threshold set according to experience. When the orthogonal guide module is at the critical point, it means that the motion of the orthogonal guide module can no longer eliminate the deviation from the feature point, and the remaining deviation from the feature point can be eliminated only by the magnetic levitation coil module.

8. The capsule robot magnetic levitation method based on magnetic sensing and visual feedback according to claim 6, characterized in that: The capsule active motion control process based on the capsule robot magnetic levitation system includes: When the deviation between the capsule robot and the feature point is greater than the threshold, the guide rail control module outputs PWM to the orthogonal guide rail module according to the feature point position deviation. The orthogonal guide rail module drives the magnetic levitation platform to move synchronously, drives the center of the magnetic levitation platform to actively move close to the feature point, and performs the capsule stable suspension control process to keep the capsule robot stably suspended at the center of the magnetic levitation platform, thereby making the position of the capsule robot close to the feature point. The capsule robot approaching the feature point will affect the original image of the camera module, thereby changing the image processing module to obtain the current relative position of the feature point, and reduce the position deviation through the loop control path to achieve feedback control of the active motion of the capsule.

9. The capsule robot magnetic levitation method based on magnetic sensing and visual feedback according to claim 8, characterized in that: The capsule active motion control process includes: When the deviation between the capsule robot and the feature point is greater than the threshold, the center of the magnetic levitation platform is not close enough to the feature point. At this time, the orthogonal guide module needs to make the magnetic levitation platform track the movement of the feature point, and the coil control module fixed on the magnetic levitation platform constrains the capsule robot to move synchronously, so that the main axis of the capsule robot tracks the movement of the feature point, reducing the relative position deviation of the feature point in the image space; The ideal relative position of the feature point in the image space is located at the center of the image, and the feature point position deviation can be obtained by subtracting it from the current feature point position. The guide rail control module calculates the position deviation of the magnetic suspension translation in the physical space based on the feature point position deviation. According to the needs of positioning accuracy and operation stability, the discrete control output PWM of a fixed moving distance is used to send to the orthogonal guide rail module, and the continuous control process is divided into multiple discrete visual positioning cycles. Within a cycle, the direction of the guide rail movement is determined according to the direction of the offset. Each movement sends a pulse wave to the guide rail in the direction of movement, so that the guide rail moves the minimum distance, and the orthogonal guide rail module can be operated stably and smoothly. Where: s is the lead of the guide rail; N is the number of PWM pulses sent to the guide rail servo motor; N0 is the number of pulse subdivisions required for the guide servo motor to make one revolution; The orthogonal guide rail module drives the magnetic levitation platform to move synchronously, and drives the center of the magnetic levitation platform to actively move close to the feature point; The relative motion of the magnetic levitation platform is equivalent to applying external interference to the stable suspension control of the capsule robot, causing the position of the capsule robot to deviate from the center of the magnetic levitation platform. At this time, it is necessary to perform stable suspension control on the capsule robot through magnetic sensor feedback to keep the capsule robot stably suspended at the center of the magnetic levitation platform. The position change of the capsule robot is detected by the magnetic sensor module, and the current position is obtained through processing by the signal processing module. Then, the position deviation is input into the coil control module. The coil control module outputs PWM parameters to the magnetic levitation coil module through the PID controller according to the deviation obtained by the signal processing module. The magnetic levitation coil module generates a control electromagnetic field to constrain the capsule robot to the center of the magnetic levitation platform, so as to achieve the effect of active movement of the capsule robot approaching the feature point. The capsule robot's approach to the feature point will affect the original image of the camera module, and then change the image processing module to obtain the current relative position of the feature point. The input summer can reduce the relative position deviation of the feature point. By looping the control path, the relative position deviation of the feature point is reduced, and the position deviation of the capsule robot is reduced, so that the capsule robot can actively track the feature point while maintaining a stable suspension state. During the control process, the critical point where the motion state of the orthogonal guide module shows overshoot oscillation is automatically used as the threshold for judging the deviation size of the feature point, rather than a fixed threshold set according to experience. When the orthogonal guide module is at the critical point, it means that the motion of the orthogonal guide module can no longer eliminate the deviation from the feature point, and the remaining deviation from the feature point can be eliminated only by the magnetic levitation coil module.

10. A readable storage medium, characterized in that: The readable storage medium stores execution instructions, which are used to implement the method according to any one of claims 6 to 9 when executed by a processor.

Citation Information

Patent Citations

  • Capsule type endoscope magnetic control system

    CN107773205A

  • External control device of magnetic control capsule robot

    CN115844313A