Capsule robot magnetic levitation system and method, and readable storage medium

Abstract

The present invention provides a capsule robot magnetic levitation system and method based on magnetic sensing and visual feedback, as well as a readable storage medium. The system includes a magnetic levitation platform, an orthogonal guide rail module, a control processor, and a capsule robot. The magnetic levitation platform is mounted on the orthogonal guide rail module, and 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, and the control processor can drive the orthogonal guide rail module to move. The present invention has the following beneficial effects: it can further realize the active motion control and sensor positioning functions of the capsule robot; expand the capsule robot's range of motion; is expected to improve the impact of external interference on the capsule robot's positioning problems; and achieve stable suspension and active control of the capsule robot.

Description

Technical Field

[0001] The present invention relates to medical equipment, and in particular to a capsule robot magnetic levitation 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 primarily consists of an imaging module, a communication module, and a light module. After the patient simply swallows the capsule, it moves with the peristaltic movements of the gastrointestinal tract, simultaneously activating the various modules within. The capsule's communication module, such as Bluetooth, transmits gastrointestinal images captured by the imaging module to an external computer. Based on these images, the doctor can make a diagnosis and prescribe an appropriate treatment plan.

[0004] Capsule robot magnetic levitation system:

[0005] The magnetic levitation system for a capsule robot provides an external magnetic field to the capsule robot. This system can actively control the motion of the capsule robot driven by magnetic levitation, which means that the capsule can stop at lesions or suspicious points in the gastrointestinal tract, greatly improving the accuracy of diagnosis and treatment. Depending on 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's working space. Permanent magnet drive refers to the control magnetic field of the capsule derived from an external permanent magnet. 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 location. The magnetic sensing 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 traditional robotic arm visual feedback control algorithms, the work plane of the robot's end effector (in the machine-eye system) and the camera are fixed relative to each other, while the camera in the hand-eye system follows the movement of the robot's end effector. By analyzing the image information captured by the camera, parameters of the image features can be derived, which are then used to control the movement of the robot arm or other actuators.

[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, which cannot 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 attenuate 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 range of activity of the capsule is severely restricted.

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

[0014] 4. Existing visual feedback control algorithms are only suitable for 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 objects of the invention are as follows:

[0017] As an alternative to traditional gastroscopy, capsule endoscopy holds great promise. However, most commercially available capsule endoscopes are limited in functionality and lack autonomous motion, relying solely on the gastrointestinal tract's natural peristalsis for movement. This results in limited imaging data at the site of the lesion, hindering detailed assessment of the lesion. This present invention aims to enable active motion in capsule endoscopes.

[0018] 2. Regarding the magnetic levitation system of the capsule robot, the present invention aims to change its driving mode and adopt a permanent magnet and electromagnetic combined drive. At the same time, permanent magnets and electromagnetic coils are used to control the stable suspension of the capsule robot, thereby improving the suspension stability of the capsule. The magnetic levitation platform is driven by the orthogonal guide rail module to control the active movement of the magnetic capsule and increase the range of activity of the capsule robot.

[0019] 3. Regarding 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 interference caused by external electromagnetic interference and sensor temperature rise.

[0020] 4. Regarding the visual feedback control algorithm, the present invention aims to fuse visual feedback with information from magnetic sensors, enabling visual information to be used to identify deviations in a larger range, while magnetic sensor information is used to identify deviations in a smaller range. The feedback link for 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 magnetic levitation system for a capsule robot 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, and 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, and the control processor can drive the orthogonal guide rail module to move. Under the action of the control electromagnetic field generated by the magnetic levitation platform, the capsule robot can maintain stable suspension at the center position of the magnetic levitation platform. When active movement is required to track a lesion feature point, 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. Under the action of the control electromagnetic field generated by the magnetic levitation platform, the capsule robot tracks the feature point and completes active movement.

[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. 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 sensing module and the magnetic levitation coil module are 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, and the magnetic sensing 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 higher frequency as the deviation subject and the input of the image processing module with 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 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.

[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 the central control PC, and the central control PC controls the orthogonal guide rail module and the magnetic levitation coil module through the 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 microcontroller to control the orthogonal guide rail module, and the signal processing module and coil control module are located in the coil control microcontroller 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 levitation 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 levitation method based on magnetic sensing and visual feedback, provides the capsule robot magnetic levitation system based on magnetic sensing and visual feedback, and performs a capsule stable levitation control process based on the capsule robot magnetic levitation 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 keeps the capsule robot stably suspended by generating a control electromagnetic field. 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, the center of the magnetic levitation platform is considered to be 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] Based on the feature point, the ideal position of the capsule robot is the position when the main axis is aligned with the feature point. The position deviation is obtained by subtracting it from the current position. The coil control module receives the position deviation and uses the deviation e(t) obtained by the signal processing module as the main term and the deviation E(t) obtained by the image processing module as the integral term. The PID controller outputs the PWM parameter n(t).

[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 coefficient;

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

[0048] The magnetic levitation coil module changes the direction of the voltage through an H-bridge according to the sign of the PWM parameter n(t), and changes the voltage magnitude through a DC power supply controller according to the magnitude of the PWM parameter n(t). The control voltage is applied to two sets of orthogonal electromagnetic coils, generating a controlled electromagnetic field that constrains the capsule robot horizontally to its center position. Because the vertical levitation force is a function of position, when the horizontal position remains unchanged, the capsule robot can be subjected to a stable vertical levitation force, which balances gravity at the designed height and can determine the capsule robot's three-dimensional position.

[0049] When disturbed by external factors, 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, Gaussian filtering is used to remove noise. Then, the RBG channel is binarized to find the color area of the feature point. Then, Canny edge detection is performed to obtain the possible feature point contours. Finally, the contour area and curvature are screened to obtain the characteristic contour of the lesion feature point. The geometric moment and central moment are calculated based on the characteristic contour, and the position and direction of the camera module are estimated, thereby obtaining 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)——the 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 center of gravity of the feature profile is determined by the zero-order moment m 00 and first-order moment m 10 and m 01 Calculation, 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 largest eigenvalue in the covariance matrix C of the feature profile, which represents the angle θ of the long 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 the 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 sensing module;

[0080] B constThe magnetic field strength provided to 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 levitation 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, and then change the image processing module to obtain the current relative position of the feature point. The position deviation is reduced through the circular control path, thereby realizing 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 rail module needs to make the magnetic levitation platform track the movement of the feature point. 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 a feature point in image space is at the center of the image. By subtracting the current feature point position from the center, we can obtain the feature point position deviation. The guide rail control module calculates the position deviation of the magnetic levitation translation in physical space based on the feature point position deviation. Based on the requirements of positioning accuracy and operational stability, it uses discrete control with a fixed movement distance to output PWM to the orthogonal guide rail module, splitting the continuous control process into multiple discrete visual positioning cycles. Within a cycle, the direction of the guide rail movement is determined based on the direction of the offset. Each movement sends a pulse wave to the guide rail in the required direction of movement, so that the guide rail moves the minimum distance, achieving stable and smooth operation of the orthogonal guide rail module.

[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 pulse subdivision number required for the guide servo motor to make one revolution;

[0094] The orthogonal guide rail module drives the magnetic levitation platform to move synchronously, driving 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 of 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. The position deviation is then 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, achieving the effect of the capsule robot actively moving close to the feature point;

[0096] When the capsule robot approaches a feature point, it will affect the original image of the camera module, thereby changing the current relative position of the feature point obtained by the image processing module. 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 and the position deviation of the capsule robot are reduced, thus realizing the active movement of the capsule robot tracking 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 response to the current situation where existing capsule endoscopy robots only have a single image acquisition function, the present invention proposes a new capsule robot solution, which includes a magnetic levitation platform, an orthogonal guide rail module, a control processor, and a capsule robot. This can further realize the active motion control and sensor positioning functions of the capsule robot;

[0102] 2. Compared with the existing capsule robot magnetic levitation system, the permanent magnet electromagnetic combined drive proposed in this invention can effectively avoid the shortcomings of using permanent magnets or electromagnets alone, can generate precise magnetic field strength and gradient, and drive the magnetic levitation platform through the orthogonal guide module, thereby expanding the range of motion of the capsule robot;

[0103] 3. In order to address the external electromagnetic interference and sensor temperature rise interference problems of the existing magnetic sensing 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 capsule robot positioning problem.

[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 realizing 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 following briefly introduces the drawings required for use in the description of the embodiments. 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 any creative work.

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

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

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

[0109] Figure 4 This is a flow chart of capsule active motion control of a capsule robot magnetic levitation method based on magnetic sensing and visual feedback of the present invention. 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 orientations or positional relationships based on the orientations 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" and the like are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, features defined as "first", "second" and the like 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 expressly specified or limited, the terms "mounted," "connected," and "connected" should be understood broadly. For example, they may refer to fixed connections, detachable connections, or integral connections; they may refer to direct connections, indirect connections through an intermediate medium, or internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0113] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0114] like Figures 1 to 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 stably and uniformly 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 primarily consists of a magnetic sensor module 105 and a magnetic levitation coil module 108. These modules are arranged symmetrically, with the centers of each module coinciding with the center of the platform. The capsule robot's ideal levitation control position is located at the center of the platform. The magnetic sensor module 105 is located at the center of the platform, above the magnetic levitation coil module 108 and below the capsule robot 109.

[0117] The control processor primarily consists of an image processing module 102, a track control module 103, a signal processing module 106, and a coil control module 107. Two possible configurations are available. In the first, all modules are centralized on a central control PC, which then controls the orthogonal track module 104 and magnetic levitation coil module 108 via a serial port. In the second, the image processing module 102 resides on the central control PC, the track control module 103 resides on the track control microcontroller (MCU) controlling the orthogonal track module 104, and the signal processing module 106 and coil control module 107 reside on the coil control MCU controlling the magnetic levitation coil module 108.

[0118] Camera module 101 is fixed to the head of capsule robot 109. The optical axis of the camera module's microendoscope coincides with the main axis of the capsule robot. It is fixed by an interference fit or adhesive dispensing to ensure that the image captured by camera module 101 reflects the position and posture characteristics of capsule robot 109. Camera module 101 is equipped with an LED light 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, obtains the characteristic contour of the lesion feature point, calculates the image moment of the characteristic contour, and estimates 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 sensing 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 components.

[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 with a lower frequency as the integral term, and outputs current to the magnetic levitation coil module 108 through the PID controller.

[0125] The magnetic levitation coil module 108 includes a permanent magnetic levitation magnet 1081 and two sets of orthogonal electromagnetic coils 1082. The permanent magnetic levitation magnet 1081 provides vertical levitation magnetic force and cannot be controlled. It generates a vertical levitation magnetic field both before and after operation. On the horizontal plane, it generates a magnetic field that deflects the capsule robot 109 from its center position. The magnetic levitation coil 1082 provides a control magnetic field that constrains the capsule robot 109 to the center of the magnetic levitation platform and can be controlled by the coil control module 107.

[0126] 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, the other functional modules can be supplemented with an IMU sensor module to provide feedback on the position and posture of capsule robot 109, thereby achieving more precise positioning. As a further improvement of the present invention, the other functional modules can be supplemented with a minimally invasive surgery function module for sampling or drug administration.

[0127] The present invention also provides a capsule robot magnetic levitation method based on magnetic sensing and visual feedback, provides the capsule robot magnetic levitation system based on magnetic sensing and visual feedback, and performs a capsule stable levitation control process based on the capsule robot magnetic levitation 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 capsule's static suspension.

[0129] The capsule stable suspension control process 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 capsule robot 109 is the position when the main axis is aligned with the feature point. Subtracting this from the current position yields the position deviation. The coil control module 107 receives the position deviation and uses the deviation e(t) obtained by the signal processing module 106 as the principal component and the deviation E(t) obtained by the image processing module 102 as the integral term. This output is the PWM parameter n(t) via a PID controller.

[0132]

[0133] Where,

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

[0135] E(t) — 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 coefficient;

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

[0140] The magnetic levitation coil module 108 changes the direction of the voltage via an H-bridge according to the sign of the PWM parameter n(t). The DC power supply controller then changes the voltage magnitude according to the magnitude of the PWM parameter n(t). This control voltage is applied to two sets of orthogonal electromagnetic coils, generating a control magnetic field that constrains the capsule robot 109 horizontally to its center position. Because the vertical levitation force is a function of position, when the horizontal position remains unchanged, the capsule robot 109 experiences a stable vertical levitation force, balancing gravity at the designed height and allowing the capsule robot 109's three-dimensional position to be determined.

[0141] When subject to external interference, the position of capsule robot 109 shifts, altering the original image obtained by camera module 101 and the Hall effect signal generated by magnetic sensor module 105. The original image undergoes image processing and feature extraction by image processing module 102. First, noise is removed through Gaussian filtering. Then, RBG channel binarization is performed to identify the color regions of the feature points. Canny edge detection is then performed to obtain possible feature point contours. Finally, contour area and curvature screening are performed to obtain the characteristic contours of the lesion feature points. Based on the characteristic contours, geometric moments and central moments are calculated to estimate the position and orientation of camera module 101, thereby determining the current position of the capsule robot.

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

[0143]

[0144] Where,

[0145] (x, y)——the 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 as the zero-order moment, which represents the vertical distance between the camera module 101 and the characteristic contour,

[0148]

[0149] The center of gravity of the feature profile can be obtained by the zero-order moment m 00 and first-order moment m 10 and m 01 Calculate, which represents the horizontal position change between the camera module 101 and the feature 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 for digital quantity to 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 operates based on the following difference equation:

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

[0161] Where,

[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 sensing 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 sensing 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] Where,

[0169] m is the magnetic moment of the permanent magnet built into 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 sensing module 105;

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

[0173] B coil (I) is the magnetic field strength provided by the magnetic levitation 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 the loop 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. 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 to keep the capsule robot 109 stably suspended at the center of the magnetic levitation platform, thereby making 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, thereby changing the image processing module 102 to obtain the current relative position of the feature point. The position deviation is reduced through the circular control path, thereby realizing feedback control of the active motion of the capsule.

[0177] During the capsule's stable levitation control process, the critical point at which the orthogonal guide rail module 104 experiences overshoot oscillation can be automatically used as the threshold for determining the deviation from the feature point, rather than a fixed threshold set empirically. When the orthogonal guide rail module 104 reaches the critical point, it indicates that the guide rail's motion can no longer eliminate the deviation from the feature point. The remaining deviation from the feature point can then be eliminated using the magnetic levitation 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 levitation 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 the 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 the feature points and reduces the relative position deviation of the feature points in the image space.

[0181] The ideal relative position of a feature point in image space is at the center of the image. Subtracting this from the current feature point position yields the feature point position deviation. The guide rail control module 103 calculates the position deviation of the magnetic levitation translation in physical space based on the feature point position deviation. Based on the requirements for positioning accuracy and operational stability, it uses discrete control with a fixed movement distance to output PWM to the orthogonal guide rail module 104. The continuous control process is broken down into multiple discrete visual positioning cycles. Within each cycle, the direction of the guide rail movement is determined based on the offset direction. Each movement sends a pulse wave to the guide rail in the desired direction, ensuring the minimum movement distance and achieving stable and smooth operation of the orthogonal guide rail module 104.

[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 pulse subdivision number 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 levitation platform to move synchronously, and drives the center of the magnetic levitation platform to actively move close to the feature point.

[0188] The relative motion of the magnetic levitation platform is equivalent to external interference with the stable suspension control of capsule robot 109, causing its position to deviate from the center of the magnetic levitation platform. In this case, magnetic sensor feedback is required to stabilize the capsule suspension control, ensuring that capsule robot 109 remains stably suspended at the center of the magnetic levitation platform. The position changes of capsule robot 109 are detected by magnetic sensor module 105, processed by signal processing module 106 to obtain the current position, and then the position deviation is input into coil control module 107. Coil control module 107 uses a PID controller to output PWM parameters to magnetic levitation coil module 108 based on the deviation obtained by signal processing module 106. Magnetic levitation coil module 108 generates a control magnetic field, confining the capsule robot to the center of the magnetic levitation platform, achieving the effect of actively moving capsule robot 109 toward the feature point.

[0189] When capsule robot 109 approaches a feature point, it affects the original image from camera module 101, which in turn changes the current relative position of the feature point obtained by image processing module 102. This input to the summer can reduce the relative position deviation of the feature point. By looping the control path, the relative position deviation of the feature point and the position deviation of capsule robot 109 are reduced, enabling capsule robot 109 to actively track the feature point while maintaining a stable hovering state.

[0190] In the capsule's active motion control process, the critical point at which the orthogonal guide rail module 104 experiences overshoot oscillation can be automatically used as a threshold for determining the deviation of feature points, rather than a fixed threshold set based on experience. When the orthogonal guide rail module 104 reaches the critical point, it indicates that the guide rail's motion can no longer eliminate the deviation from the feature point. The remaining deviation from the feature point can be eliminated using the magnetic levitation coil module 108.

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

[0192] 1. The magnetic levitation system adopts permanent magnet and electromagnetic combined 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 visual feedback with information from magnetic sensors, select the feedback link for visual information based on the size of the deviation, and achieve stable suspension and active control of the capsule robot.

[0196] The above is a further detailed description of the present invention in conjunction with specific preferred embodiments, and the specific implementation of the present invention should not be considered to be limited to these descriptions. For those skilled in the art of the present invention, without departing from the concept of the present invention, several simple deductions or substitutions can be made, which should be considered to fall within the scope of protection of the present invention.

Claims

1. A capsule robot magnetic levitation system based on magnetic sensing and visual feedback, characterized by: The device comprises 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 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 active movement; 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. 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 centrally symmetrically arranged. The ideal levitation 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. The control processor includes an image processing module, a guide rail control module, a signal processing module and a coil control module; 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 sets of Hall sensors arranged in opposite directions; The magnetic levitation coil module includes a magnetic levitation permanent magnet and two sets of orthogonal electromagnetic coils. The magnetic levitation permanent magnet can provide a vertical levitation magnetic force, always generating a vertical levitation magnetic field during operation, and generating 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.

2. The capsule robot magnetic levitation system based on magnetic sensing and visual feedback according to claim 1 is characterized in that: 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.

3. The capsule robot magnetic levitation system based on magnetic sensing and visual feedback according to claim 2, 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 image processing module and the signal processing module 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 levitation system further includes a camera module, which is mounted on the head of the capsule robot. A signal output end of the camera module is connected to a 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 capable of providing 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, and 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.

4. A method for magnetic levitation of a capsule robot based on magnetic sensing and visual feedback, characterized by: A capsule robot magnetic levitation system based on magnetic sensing and visual feedback according to any one of claims 1 to 3 is provided, and a capsule stable levitation control process based on the capsule robot magnetic levitation system comprises: 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 keeps the capsule robot stably suspended by generating a control electromagnetic field. 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.

5. The capsule robot magnetic levitation method based on magnetic sensing and visual feedback according to claim 4 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, the center of the magnetic levitation platform is considered to be 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. Based on the feature point, the ideal position of the capsule robot is the position when the main axis is aligned with the feature point. The position deviation is obtained by subtracting it from the current position. The coil control module receives the position deviation and uses the deviation e(t) obtained by the signal processing module as the main term and the deviation E(t) obtained by the image processing module as the integral term. The PID controller outputs the PWM parameter n(t). 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 coefficient; K D ——differential term coefficient of the controller; The magnetic levitation coil module changes the direction of the voltage through an H-bridge according to the sign of the PWM parameter n(t), and changes the voltage magnitude through a DC power supply controller according to the magnitude of the PWM parameter n(t). The control voltage is applied to two sets of orthogonal electromagnetic coils, generating a controlled electromagnetic field that constrains the capsule robot horizontally to its center position. Because the vertical levitation force is a function of position, when the horizontal position remains unchanged, the capsule robot can be subjected to a stable vertical levitation force, which balances gravity at the designed height and can determine the capsule robot's three-dimensional position. When disturbed by external factors, 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, Gaussian filtering is used to remove noise. Then, the RBG channel is binarized to find the color area of the feature point. Then, Canny edge detection is performed to obtain the possible feature point contours. Finally, the contour area and curvature are screened to obtain the characteristic contour of the lesion feature point. The geometric moment and central moment are calculated based on the characteristic contour, and the position and direction of the camera module are estimated, thereby obtaining 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)——the 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 center of gravity of the feature profile is determined by the zero-order moment m 00 and first-order moment m 10 and m 01 Calculation, represents the horizontal position change between the camera module and the feature contour, After obtaining the center of gravity coordinates, calculate the p+q order center distance as follows: Calculate the eigenvector with the largest eigenvalue in the covariance matrix C of the feature profile, which represents the angle θ of the long 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 the 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 sensing module; B const The magnetic field strength provided to 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 orthogonal guide module's motion state shows overshoot oscillation is automatically used as the threshold for judging the size of the feature point deviation, rather than a fixed threshold set based on 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.

6. The capsule robot magnetic levitation method based on magnetic sensing and visual feedback according to claim 4, characterized in that: The capsule robot 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, and then change the image processing module to obtain the current relative position of the feature point. The position deviation is reduced through the circular control path, thereby realizing feedback control of the active motion of the capsule.

7. 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 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 rail module needs to make the magnetic levitation platform track the movement of the feature point. 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. The difference between the ideal relative position of the feature point and the current feature point position can be used to obtain the feature point position deviation. The guide rail control module calculates the position deviation of the magnetic levitation translation in the physical space based on the feature point position deviation. According to the needs of positioning accuracy and operation stability, a discrete control output PWM of a fixed moving distance is used to output PWM 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 required movement direction, so that the guide rail moves the minimum distance, thereby achieving stable and smooth operation of the orthogonal guide rail module. 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 pulse subdivision number required for the guide servo motor to make one revolution; The orthogonal guide rail module drives the magnetic levitation platform to move synchronously, driving 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 of 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. The position deviation is then 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, achieving the effect of the capsule robot actively moving close to the feature point; When the capsule robot approaches a feature point, it affects the original image of the camera module, which in turn changes the current relative position of the feature point obtained by the image processing module. 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 and the position deviation of the capsule robot are reduced, thus achieving the active motion of the capsule robot tracking the feature point while maintaining a stable suspension state. During the control process, the critical point where the orthogonal guide module's motion state shows overshoot oscillation is automatically used as the threshold for judging the size of the feature point deviation, rather than a fixed threshold set based on 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. 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 4 to 7 when executed by a processor.

Images