Magnetic control micro-robot multi-mode motion and path tracking control method

Abstract

The invention discloses a multi-mode motion and path tracking control method and system for a magnetic control micro-robot, and relates to the technical field of magnetic control micro-robots and intelligent control systems. Aiming at the technical pain points that an existing magnetic control micro-robot is single in motion mode, low in path tracking precision in an intestinal environment and difficult to reach a focus accurately, stable triggering and flexible switching of overturning and rolling dual-mode motion are achieved by regulating and controlling the relative angle between an external magnetic field plane and the length direction of the robot; the method comprises the following steps: respectively constructing kinetic models of two motions, and realizing path tracking closed-loop control by adopting a model prediction control algorithm adaptive to an overturning characteristic and combining visual real-time feedback. The obstacle crossing ability of the robot in the complex intestinal environment is effectively improved, the path tracking precision and the control stability are remarkably improved, it can be guaranteed that the robot accurately reaches the intestinal focus, and reliable technical support is provided for accurate diagnosis and treatment of intestinal diseases.

Description

Technical Field

[0001] This invention relates to the field of magnetically controlled microrobots and intelligent control systems, and in particular to a method for multimodal motion and path tracking control of a magnetically controlled microrobot. Background Technology

[0002] In the field of precision medical diagnosis and treatment, magnetically controlled microrobots have become an important research direction for targeted diagnosis and treatment of intestinal diseases due to their advantages such as wireless remote control, good biocompatibility, and adaptability to narrow physiological spaces. Their motion control performance directly determines the accuracy of diagnosis and treatment and the feasibility of clinical application. Among them, the realization of multimodal motion and the precise control of flipping motion are the core technical key points.

[0003] The closest existing technology to this invention mainly focuses on the motion control research of magnetically controlled microrobots. However, it has two major problems in practical applications, as follows: First, the motion mode is limited. Most existing magnetically controlled microrobots can only achieve a single motion mode and have not formed a stable triggering and flexible switching mechanism for rolling and flipping modes. This makes them unable to adapt to the passage requirements of complex unstructured environments such as intestinal mucosal folds and narrow segments, thus limiting the clinical application scenarios of the robots. Second, the control precision of flipping motion is insufficient. Existing technologies have not built a dynamic model that fits the actual scenario for the characteristics of flipping motion, have not considered the special case of rotation axis switching during flipping motion, and have not designed a path tracking control strategy adapted to flipping motion. This results in unstable posture and easy trajectory deviation during flipping motion, and the control precision is difficult to meet the requirements for accurate arrival of intestinal lesions, thus failing to support the clinical application of precise diagnosis and treatment of intestinal diseases. Summary of the Invention

[0004] To address these issues, this invention provides a multimodal motion and path tracking control method for a magnetically controlled microrobot. This method solves the problems of existing magnetically controlled microrobots having a single motion mode, being unable to adapt to the complex and unstructured environment of the intestine, having insufficient precision in flipping motion control, poor robustness and stability in path tracking, and difficulty in accurately reaching intestinal lesions.

[0005] To address the aforementioned technical problems, embodiments of the present invention provide a method for multimodal motion and path tracking control of a magnetically controlled microrobot, the method comprising the following steps:

[0006] S1: Build an external magnetic drive system. By adjusting the relative angle between the rotating plane of the external magnetic field and the length direction of the magnetically controlled microrobot, the system can stably trigger and flexibly switch between two modes of motion: flipping motion along the width direction and rolling motion along the length direction.

[0007] S2: Based on the structural characteristics and magnetic field principle of magnetically controlled microrobots, corresponding dynamic models are constructed for flipping and rolling motions respectively, and the correspondence between magnetic torque and robot motion posture and motion parameters is quantified;

[0008] S3: To address the issue of easy deviation in the flipping motion trajectory, a model predictive control algorithm adapted to the flipping motion characteristics is adopted. Combined with the robot motion parameters obtained from real-time visual feedback, a path tracking closed-loop control strategy is constructed to correct the robot's motion trajectory in real time and achieve accurate tracking of the preset path.

[0009] Preferably, in step S1, the specific method for implementing the flipping motion is as follows:

[0010] A magnetically controlled microrobot is placed along its length along the y-axis. First, a bias magnetic field is applied along the positive z-axis to form a "V" shape. Then, a rotating magnetic field in the xz plane is applied, with the plane of rotation perpendicular to the robot's length. The robot is then driven by magnetic torque to perform a stable flipping motion along its width. The vector expression for the rotating magnetic field is:

[0011] ;

[0012] In the formula, The strength of the rotating magnetic field. , , These represent the rotating magnetic field strengths along the x-axis, y-axis, and z-axis, respectively. The frequency of the rotating magnetic field. For time.

[0013] Preferably, in step S1, the specific method for implementing the rolling motion is as follows:

[0014] The magnetically controlled microrobot is placed along the x-axis along its length. First, a bias magnetic field is applied along the positive z-axis to make the robot form a "V" shape. Then, a rotating magnetic field with the same parameters as the flipping motion is applied in the xz plane so that the rotating plane of the external magnetic field is parallel to the length direction of the robot. The robot is driven to complete a stable rolling motion along its own length direction by the magnetic torque.

[0015] Preferably, in step S2, the specific method for constructing the flipping motion dynamics model is as follows:

[0016] The bilaterally symmetrical magnetically controlled microrobot is simplified into a cuboid model. Based on the switching characteristics of the rotation axis during the flipping motion, the flipping motion is divided into two working conditions: rotation around the vertex A of the "V" shape and rotation around the line P connecting the two endpoints of the "V" shape. Based on the parallel axis principle, the moment of inertia under the two working conditions is calculated respectively. Combined with the mechanical analysis of magnetic torque and gravitational torque, the dynamic equations corresponding to the two working conditions are constructed respectively, and the correspondence between the flipping angle, angular acceleration and magnetic torque is quantified.

[0017] Preferably, in step S2, the specific method for constructing the rolling motion dynamics model is as follows:

[0018] The magnetically controlled microrobot is equivalent to a circular arc structure, and the rolling process is divided into two stages: the contact point slides on the arc and rotates around the endpoint of the arc. For the working condition of the robot rolling around any contact point, the corresponding moment of inertia is calculated. Combined with the mechanical analysis of gravitational torque and magnetic torque, the dynamic equation corresponding to the rolling motion is constructed to accurately describe the changes in the robot's motion state during the rolling process.

[0019] Preferably, in step S3, the real-time visual feedback process includes:

[0020] The motion parameters of the magnetically controlled microrobot, including the center of mass position and yaw angle, are collected in real time through an externally integrated vision detection module. Based on the collected motion parameters, the deviation between the robot's actual motion trajectory and the preset reference path is calculated. Based on the deviation, the external magnetic control signal parameters are dynamically adjusted to correct the robot's motion trajectory in real time.

[0021] Preferably, in step S3, the prediction model construction process of the model predictive control algorithm adapted to the flipping motion characteristics is as follows:

[0022] The dynamic model of the robot's flipping motion is transformed into a state vector. Control input is Nonlinear control systems, in which and This indicates the coordinates of the center of mass of the magnetically controlled microrobot within its plane of motion. Indicates the robot's yaw angle. Indicates transpose. The angular velocity of the robot is used as a variable to adjust the robot's motion posture in the control system. At the reference point of the reference trajectory, the nonlinear system is expanded using Taylor series and discretized to construct augmented state variables. This transforms the discrete state-space equations into a predictive model, the expression of which is:

[0023] ;

[0024] ;

[0025] In the formula, for augmented state variables at time t, , , The state-space coefficient matrix, for Incremental control input at any time This is the system output.

[0026] Preferably, in the model predictive control algorithm, a quadratic objective function is designed based on the prediction time domain and the control time domain, and the expression of the objective function is:

[0027] ;

[0028] In the formula, In order to be in At any given moment, it is a comprehensive evaluation index of the system's control performance over a future period of time. To predict the time domain, To control the time domain, for Tracking deviation at any given moment for Control increment at any time, The state weight matrix is... To control the incremental weight matrix, For relaxation factor weights, It is a relaxation factor.

[0029] Preferably, when solving the objective function, constraints are applied to the control input and control increment, and the constraints are as follows:

[0030] ;

[0031] ;

[0032] In the formula, for Control input at any time, , These are the upper and lower limits of the control input quantity, respectively. , These are the upper and lower limits for controlling the increment, respectively;

[0033] By combining the objective function and constraints, the optimal control problem for trajectory tracking is transformed into a quadratic programming problem, yielding the optimal control increment sequence:

[0034] ;

[0035] In the formula, for The control increment sequence at each time step, , , They are respectively , , Incremental control input at any given time.

[0036] This invention also provides a multimodal motion and path tracking control system for a magnetically controlled microrobot. This system is used to implement the aforementioned multimodal motion and path tracking control method for a magnetically controlled microrobot, specifically including:

[0037] The multimodal motion triggering and switching module is used to build an external magnetic drive system. By adjusting the relative angle between the rotating plane of the external magnetic field and the length direction of the magnetically controlled microrobot, it can achieve stable triggering and flexible switching between two modes of robot motion: flipping motion along the width direction and rolling motion along the length direction.

[0038] The multimodal dynamics model building module is used to build corresponding dynamics models for flipping and rolling motions based on the structural characteristics and magnetic field action principle of magnetically controlled microrobots, and to quantify the correspondence between magnetic torque and robot motion posture and motion parameters.

[0039] The flipping motion path tracking closed-loop control module is designed to address the issue of easy deviation in flipping motion trajectories. It employs a model predictive control algorithm adapted to the characteristics of flipping motion, combined with real-time visual feedback of robot motion parameters, to construct a path tracking closed-loop control strategy, thereby correcting the robot's motion trajectory in real time and achieving accurate tracking of the preset path.

[0040] As can be seen from the above technical solutions, this invention application has the following beneficial effects:

[0041] (1) This invention achieves stable triggering and flexible switching of rolling and flipping motion modes by adjusting the relative angle between the rotating plane of the external magnetic field and the length direction of the magnetically controlled microrobot. The rolling mode enables the robot to move quickly over long distances, while the flipping mode can adapt to complex unstructured environments such as intestinal stenosis, mucosal folds, and irregular curved surfaces. This completely solves the core pain point of existing technologies, which have a single motion mode and cannot adapt to the complex physiological space of the intestine. It breaks through the limitations of clinical application scenarios for magnetically controlled microrobots and provides a reliable motion control basis for the efficient passage of robots in the confined space of the intestine.

[0042] (2) This invention addresses the dynamic switching characteristics of the rotation axis during flipping motion by constructing a flipping dynamics model with dual working conditions around the vertex and around the endpoint. Simultaneously, a model predictive control closed-loop strategy adapted to the flipping motion characteristics is designed, and dynamic trajectory correction is achieved by combining pose parameters obtained from real-time visual feedback. Experimental verification shows that the control strategy of this invention can achieve a maximum error of only 1.59mm throughout the parabolic path tracking process, with a root mean square error as low as 0.33mm. The longitudinal error remains stable within ±0.34mm for the first 6 seconds. Compared to traditional dual-closed-loop PID control, it significantly improves tracking accuracy and anti-interference robustness, solving the industry problem of unstable flipping motion posture and easy trajectory deviation in existing technologies. This ensures that the robot accurately reaches the intestinal lesion area, meeting the core accuracy requirements for precise diagnosis and treatment of intestinal diseases.

[0043] (3) This invention optimizes the quadratic objective function of model predictive control, applies hardware adaptation constraints to the control input and control increment, and smoothly adjusts the magnetic control parameters in real time with a rolling optimization strategy, effectively avoiding problems such as sudden changes in robot posture and stuttering. At the same time, the magnetic control signal output by the control is highly stable, with regular and constant magnetic field waveforms on the x-axis and z-axis, and no drastic changes in the magnetic field on the y-axis. This significantly reduces the risk of mechanical damage to the intestinal mucosa during robot movement. In addition, the dual-modal collaborative control system improves the motion control technology framework of the magnetically controlled microrobot, effectively promoting the application of this technology from laboratory research to clinical practice, and providing mature and reliable technical support for non-invasive targeted diagnosis and treatment of intestinal diseases. Attached Figure Description

[0044] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly described below. Referring to the drawings will make the features and advantages of the present invention clearer. The drawings are illustrative and should not be construed as limiting the present invention in any way. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort. Wherein:

[0045] Figure 1 This is a flowchart of a multimodal motion and path tracking control method for a magnetically controlled microrobot provided by the present invention;

[0046] Figure 2 This is a schematic diagram of the tumbling motion of the magnetically controlled microrobot in this invention;

[0047] Figure 3 This is a schematic diagram of the rolling motion of the magnetically controlled microrobot in this invention;

[0048] Figure 4 This is a schematic diagram of the tumbling motion dynamics model of the magnetically controlled microrobot in this invention, where (a) is the rotation case around point A and (b) is the rotation case around point P.

[0049] Figure 5 This is the rolling motion dynamic model of the magnetically controlled microrobot in this invention, where (a) and (b) represent the sliding of the robot's contact point with the ground on the arc, and (c) and (d) represent the robot's rotation around the left or right end point of the arc.

[0050] Figure 6 This is a schematic diagram of the magnetically controlled microrobot performing a tumbling motion on the platform in this invention;

[0051] Figure 7 This is a schematic diagram of the magnetically controlled microrobot performing rolling motion on the platform in this invention;

[0052] Figure 8 This is a schematic diagram of the parabolic path tracking of the flipping motion of the magnetically controlled microrobot in this invention, wherein (a) is a schematic diagram of the path tracking result, (b) is a curve of the longitudinal error changing with time, and (c) is a schematic diagram of the magnetic field changes in three directions.

[0053] Figure 9 This is a block diagram of a multimodal motion and path tracking control system for a magnetically controlled microrobot provided by the present invention. Detailed Implementation

[0054] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0055] This invention provides a multimodal motion and path tracking control method for a magnetically controlled microrobot. The core of this method involves using an external magnetic field to achieve stable triggering and flexible switching between the flipping and rolling dual-modal motions of the magnetically controlled microrobot. Combined with a model predictive control algorithm adapted to the flipping motion characteristics, closed-loop path tracking control is achieved. This addresses the problems of existing magnetically controlled microrobots having a single motion mode, being unable to adapt to the complex and unstructured environment of the intestine, and having insufficient precision in flipping motion control, poor robustness and stability in path tracking, making it difficult to accurately reach intestinal lesions. This provides reliable technical support for the precise diagnosis and treatment of intestinal diseases.

[0056] like Figure 1 As shown, this invention proposes a multimodal motion and path tracking control method for a magnetically controlled microrobot, which includes the following core steps:

[0057] S1: Build an external magnetic drive system. By adjusting the relative angle between the rotating plane of the external magnetic field and the length direction of the magnetically controlled microrobot, the system can stably trigger and flexibly switch between two modes of motion: flipping motion along the width direction and rolling motion along the length direction.

[0058] S2: Based on the structural characteristics and magnetic field principle of magnetically controlled microrobots, corresponding dynamic models are constructed for flipping and rolling motions respectively, and the correspondence between magnetic torque and robot motion posture and motion parameters is quantified;

[0059] S3: To address the issue of easy deviation in the flipping motion trajectory, a model predictive control algorithm adapted to the flipping motion characteristics is adopted. Combined with the robot motion parameters obtained from real-time visual feedback, a path tracking closed-loop control strategy is constructed to correct the robot's motion trajectory in real time and achieve accurate tracking of the preset path.

[0060] The specific implementation methods for each of the above steps are described in detail below:

[0061] In step S1, multimodal motion triggering and switching are performed. This step achieves independent triggering and flexible switching between the flipping and rolling motions of the magnetically controlled microrobot by adjusting the magnetic field parameters of the external magnetic drive system. The specific implementation methods of the two motion modes are as follows:

[0062] 1. Specific implementation of the flipping motion

[0063] When the plane of rotation of the applied magnetic field is perpendicular to the length direction of the magnetically controlled microrobot, the robot can achieve a flipping motion along its width direction. The specific implementation process is as follows: The magnetically controlled microrobot is placed along the y-axis along its length. First, a bias magnetic field is applied along the positive z-axis to form a "V" shape, improving the stability of subsequent flipping movements. Then, a rotating magnetic field is applied in the xz plane, with a magnetic field strength of [insert value here]. , frequency is Its vector expression is:

[0064] ;

[0065] In the formula, , , These represent the rotating magnetic field strengths along the x-axis, y-axis, and z-axis, respectively. For time.

[0066] Driven by this rotating magnetic field, the robot completes a stable flip along its own width direction (x-axis direction), such as... Figure 2As shown, the flipping motion can be divided into four consecutive stages: When the magnetic field vector is in the first quadrant, the robot rotates clockwise around the apex of the "V" shape and tilts towards the positive x-axis, forming a "V" shape with the opening facing right; when the magnetic field continues to rotate clockwise to the fourth quadrant, the contact point between the robot and the horizontal plane switches to the two ends of the "V" shape, forming a "V" shape with the opening facing down; when the magnetic field rotates to the third quadrant, the robot continues to rotate around the two ends of the "V" shape, forming a "V" shape with the opening facing left; when the magnetic field enters the second quadrant, the contact point between the robot and the ground switches back to the apex of the "V" shape, finally returning to the initial "V" shape with the opening facing up, completing one cycle of flipping motion.

[0067] 2. Specific implementation of rolling motion

[0068] When the plane of rotation of the applied magnetic field is parallel to the length direction of the magnetically controlled microrobot, the robot can achieve rolling motion along its length direction. The specific implementation process is as follows: the magnetically controlled microrobot is placed along the x-axis along its length direction. First, a bias magnetic field along the positive z-axis is applied to make the robot form a "V" shape. Then, a rotating magnetic field with the same parameters as the flipping motion is applied in the XOZ plane, and the robot is driven to complete a stable rolling motion along its own length direction (x-axis direction) by the magnetic torque.

[0069] like Figure 3 As shown, the entire rolling motion can be divided into four consecutive stages: When the magnetic field vector is in the first quadrant, the robot rotates clockwise around the apex of the "V" shape and tilts towards the positive x-axis. During the rotation, the fulcrum changes from the apex to the right end point. When the magnetic field vector is in the fourth quadrant, the robot continues to rotate clockwise. At this time, the contact point with the horizontal plane becomes the two ends of the "V" shape, with the opening of the "V" shape facing downwards. When the magnetic field vector rotates to the third quadrant, the robot rotates clockwise around the left end point of the "V" shape, becoming a configuration where the opening of the "V" shape faces left. When the magnetic field vector rotates in the second quadrant, the contact point between the robot and the ground becomes the apex of the "V" shape, and it returns to the "V" configuration with the opening facing upwards, completing one cycle of rolling motion.

[0070] Through the above-mentioned magnetic field regulation and configuration switching mechanism, the magnetically controlled microrobot can achieve flipping motion along the width direction and rolling motion along the length direction, enabling it to adapt to complex unstructured environments such as narrow intestines and sloping terrain. This effectively improves the robot's motion performance and environmental adaptability in confined spaces, providing a reliable motion control basis for the precise diagnosis and treatment of intestinal diseases.

[0071] In step S2, a multimodal dynamic model is constructed. Based on the structural characteristics of the magnetically controlled microrobot and the principle of magnetic field action, this step constructs dynamic models for both flipping and rolling motions, clarifying the relationship between magnetic torque and robot posture. This provides a solid theoretical foundation for subsequent path tracking control. The specific construction process is as follows:

[0072] 1. Construction of the dynamic model of the flipping motion

[0073] Because the robot's structure is bilaterally symmetrical, in its initial "V"-shaped opening facing upwards, the magnetic moments at both ends point outwards along the length direction, and the net magnetic moment points vertically upwards. Therefore, the robot is simplified to a cuboid model, with the following model parameters: mass... Magnetic moment single-side length The magnitude of the equivalent magnetic moment is equal to the single-sided magnetic moment. And the direction is along the length; where point A corresponds to the vertex of the "V" shape, point O is the centroid of the cuboid, and the distance between A and B is... The opening angle of the "V" shape is a fixed value (e.g.) ).

[0074] Based on the rotational characteristics of the flipping motion, such as Figure 4 As shown, the rotation axis of the magnetically controlled microrobot switches between a straight line passing through the vertex and perpendicular to the xz plane and the line connecting the two endpoints of the "V" shape. Based on this, the flipping motion is divided into two working conditions: rotation around point A and rotation around point P. Mechanical analysis is performed for each condition, and dynamic equations are constructed:

[0075] (1) Working condition of rotating around point A

[0076] When the robot rotates around point A, according to the parallel axis principle, the moment of inertia about point A is:

[0077] ;

[0078] External magnetic field With magnetic moment The included angle between them is The attitude angle of rotation about point A is The magnetic torque and gravitational torque acting on the robot are as follows:

[0079] ;

[0080] ;

[0081] The complete dynamic equation under this working condition is:

[0082] ;

[0083] In the formula, Let be the angular acceleration of the robot about the point of rotation, and be the attitude angle. The second derivative with respect to time, This is the acceleration due to gravity.

[0084] (2) Working condition of rotating about point P

[0085] When the robot rotates around point P (the line connecting the two ends of the "V" shape), according to the parallel axis principle, the moment of inertia about point P is:

[0086] ;

[0087] Let the attitude angle of the rotation about point P be . The magnetic torque and gravitational torque acting on the robot are as follows:

[0088] ;

[0089] ;

[0090] The complete dynamic equation under this working condition is:

[0091] .

[0092] In the formula, Let be the angular acceleration of the robot about the point of rotation, and be the attitude angle. The second derivative with respect to time, This is the acceleration due to gravity.

[0093] The dynamic equations for the two operating conditions described above can quantify the relationship between the flipping angle, angular acceleration, and magnetic torque, providing a theoretical basis for precise control of the flipping motion through the torque balance relationship.

[0094] 2. Construction of the Rolling Motion Dynamics Model

[0095] like Figure 5 As shown, the magnetically controlled microrobot is represented as an equivalent circular arc structure, where point O corresponds to the vertex of the "V" shape, and points A and B correspond to the two endpoints of the "V" shape. The radius of the equivalent circular arc is... , equivalent magnetic moment Pointing towards the center of the circle along the axis of symmetry The distance from the centroid C of the arc to the center of the circle The distance is:

[0096] ;

[0097] Setting model parameters: magnetic moment The angle with the vertical direction is ,magnetic field With magnetic moment The included angle is The contact point P is the center of the circle The angle between the line connecting the two points and the axis of symmetry is... The robot's rolling process is divided into two stages. In the first stage, the robot slides on the arc at its point of contact with the ground. In the second stage, the robot rotates around the left or right end of the arc.

[0098] When the robot rolls around any contact point P, its moment of inertia about the contact point is:

[0099] ;

[0100] ;

[0101] in Let C be the distance from the point of contact to the centroid C.

[0102] Under this condition, the gravitational torque and magnetic torque can be expressed as follows:

[0103] ;

[0104] ;

[0105] The dynamic equation corresponding to the rolling motion is:

[0106] ;

[0107] In the formula, Angular acceleration, i.e. The second derivative with respect to time reflects the rate of change of the robot's magnetic moment attitude angle.

[0108] The above dynamic equations can accurately describe the changes in the robot's motion state during rolling, providing theoretical support for the stable control of rolling motion.

[0109] In step S3, closed-loop control for tracking the flipping motion path is performed. This step addresses the issue of the robot's trajectory easily deviating during flipping motion by designing a closed-loop control strategy. A model predictive control algorithm is employed and adjusted according to the characteristics of the flipping motion. A high-precision path tracking model is built, and real-time trajectory correction is achieved by combining it with real-time visual feedback. The specific implementation process is as follows:

[0110] 1. Real-time visual feedback

[0111] Through an externally integrated vision detection module, key motion parameters such as the center of mass position and yaw angle of the magnetically controlled microrobot are collected in real time. Based on the collected parameters, the deviation between the robot's actual motion trajectory and the preset reference path is accurately calculated. The deviation is corrected in real time, and the external magnetic control signal parameters are dynamically adjusted to correct the robot's motion trajectory in real time, thereby ensuring the accuracy of path tracking.

[0112] 2. Model predictive control algorithm adapted to flipping motion characteristics

[0113] (1) Construction of the prediction model

[0114] The kinematic model of the robot's flipping motion is considered as a state vector. and control input are The control system, in which and This indicates the coordinates of the center of mass of the magnetically controlled microrobot within its plane of motion. Indicates the robot's yaw angle. Indicates transpose. The angular velocity of the robot is used as a variable to adjust the robot's motion posture in the control system; the general nonlinear expression of this system is:

[0115] ;

[0116] in, State vector The first derivative with respect to time, i.e. , representing the changes in the robot's speed and angular velocity; It is a nonlinear function that represents the nonlinear mapping relationship between the system state and the control input, and is determined by the robot's kinematic model.

[0117] First, determine the reference trajectory for the robot's flipping motion. Assume that the reference state corresponding to each sampling point on the reference trajectory is... , Furthermore, both the reference state and the reference input satisfy the above robot kinematics model, the expression of which is:

[0118] .

[0119] At the reference point, the nonlinear expression of the system is expanded using a Taylor series, ignoring second-order and higher-order terms. The expanded expression is then discretized by subtraction to obtain the discrete state-space equation. Subsequently, the augmented state variables are reconstructed. The original discrete state-space equations are transformed into new state-space equations, thereby obtaining a prediction model for the robot's flipping motion, as detailed below:

[0120] ;

[0121] ;

[0122] In the formula, for augmented state variables at time t, , , The state-space coefficient matrix, for Incremental control input at any time This is the system output quantity; Let be the state error vector, representing the deviation between the actual state and the reference state at the current moment; The control input error vector represents the deviation between the actual control input and the reference control input at the previous moment.

[0123] Define the prediction time domain as Control time domain is By predicting the future The state of the step system, for the future The control input of each step is optimized, and the optimization relationship can be expressed as:

[0124] ;

[0125] In the formula, This is the deviation matrix between the predicted sequence and the reference trajectory sequence of the system's future output. , The prediction coefficient matrix is ​​derived based on the state-space coefficient matrix. This is the sequence of control increments within the future control time domain.

[0126] (2) Design of the objective function

[0127] When designing the objective function, two core requirements must be met: first, to ensure the future performance of the system. The objective is twofold: first, to approximate the actual state of the robot as closely as possible to the reference state, ensuring accurate trajectory tracking; and second, to keep the changes in control input gradual, guaranteeing the smoothness of the robot's flipping motion. Based on this, a quadratic function is used as the objective function to solve for the optimal solution. The objective function expression is:

[0128] ;

[0129] In the formula, In order to be in At any given moment, it is a comprehensive evaluation index of the system's control performance over a future period of time. To predict the time domain, To control the time domain, for Tracking deviation at any given moment for Control increment at any time, The state weight matrix is... To control the incremental weight matrix, For relaxation factor weights, This is a relaxation factor used to ensure that the optimization problem has a feasible solution.

[0130] (3) Setting constraints and finding the optimal solution

[0131] To ensure that control commands conform to the hardware's execution capabilities and that control signals are smooth, constraints must be imposed on the control input and control increment. The constraints are as follows:

[0132] ;

[0133] ;

[0134] In the formula, for Control input at any time, , These are the upper and lower limits of the control input quantity, respectively. , These are the upper and lower limits for controlling the increment, respectively.

[0135] By combining the objective function and constraints, the optimal control problem for trajectory tracking is transformed into a quadratic programming problem, which can be solved to obtain the future... The optimal control increment sequence for each step:

[0136] ;

[0137] In the formula, for The control increment sequence at each time step, , , They are respectively , , Incremental control input at any given time.

[0138] (4) Implementation of rolling optimization strategy

[0139] A rolling optimization strategy using model predictive control is employed, selecting only the first term of the optimal control increment sequence for calculation. The amount of control at any given time, discarding the rest There are several control variables, and the calculation expression for each control variable is:

[0140] ;

[0141] The control input at the current moment is substituted into the kinematic model for solution to obtain the predicted trajectory of the system. At the next sampling moment, the above prediction, optimization and solution process is repeated. Based on the newly collected robot state parameters, prediction and optimization are performed again. The process is iterated in a loop, and finally the optimal control for tracking the robot's flipping motion trajectory is achieved, ensuring that the robot can accurately and smoothly track the preset reference trajectory.

[0142] The technical solution of the present invention will be further verified and explained through specific embodiments below:

[0143] Example 1: Flipping motion of a magnetically controlled microrobot

[0144] This embodiment describes the actual implementation process and effect of the magnetically controlled microrobot's flipping motion. The experimental conditions are set as follows: the microrobot is placed along the y-axis, the magnitude of the rotating magnetic field is 15mT, and the magnetic field frequency is 1Hz.

[0145] During the experiment, a vertically upward (positive z-axis direction) magnetic field with continuously increasing amplitude was first applied to the robot, which steadily increased to 15 mT within 0.35 s, causing the robot to bend upward and form a stable "V" shape. Subsequently, a clockwise rotating magnetic field with an amplitude of 15 mT and a frequency of 1 Hz was applied in the zx plane, driving the robot to complete the flipping motion in the positive x-axis direction.

[0146] Experimental results are as follows Figure 6 As shown, the entire flipping process is divided into four consecutive stages: from 0.35s to 0.6s, the magnetic field rotates from the positive z-axis to the positive x-axis, and the robot tilts to the left; from 0.6s to 0.85s, the magnetic field rotates to the negative z-axis, and the robot rolls into an inverted "V" shape; from 0.85s to 1.1s, the magnetic field rotates to the negative x-axis, and the robot changes to an open-to-right posture; from 1.1s to 1.35s, the magnetic field rotates back to the positive z-axis, and the robot resets to its initial "V" shape, completing one full flipping cycle. Testing showed that the robot's flipping speed in this embodiment is stable at 15.51 mm / s; and when the robot is initially placed along the x-axis, applying a corresponding rotating magnetic field to the yz plane can drive it to flip in the y-axis direction, verifying the versatility of the flipping motion triggering method.

[0147] Example 2: Rolling motion of a magnetically controlled microrobot

[0148] This embodiment specifically describes the actual implementation process and effect of the rolling motion of the magnetically controlled microrobot. The experimental conditions are set as follows: the microrobot is placed along the x-axis, the magnitude of the rotating magnetic field is 15mT, and the magnetic field frequency is 2Hz.

[0149] During the experiment, a vertically upward (positive z-axis direction) magnetic field with continuously increasing amplitude was first applied to the robot, which steadily rose to 15 mT within 0.2 s, causing the robot to bend upward and form a stable "V" shape. Subsequently, a clockwise rotating magnetic field with an amplitude of 15 mT and a frequency of 2 Hz was applied in the zx plane, driving the robot to complete the rolling motion in the positive x-axis direction.

[0150] Experimental results are as follows Figure 7 As shown, the entire rolling process is divided into the following stages: From 0.2s to 0.4s, the robot rolls in a "V" shape towards the x-axis, with the left side gradually moving downwards towards the ground and becoming the fulcrum, while the right side moves upwards continuously; from 0.4s to 0.5s, the robot uses the right side as the fulcrum and the left side moves downwards continuously, forming an inverted "V" shape; from 0.5s to 0.55s, the robot begins to use the left side as the fulcrum and the right side rotates upwards, finally returning to the original "V" shape configuration in 0.7s. In this embodiment, the robot completes one cycle of rolling within 0.5s, moving a distance of 27.16mm at a speed of 54.32mm / s; when the micro-robot is placed along the y-axis, a rotating magnetic field in the yz plane can be applied to drive the robot to roll in the y-axis direction, verifying the universality of the rolling motion triggering method.

[0151] Example 3: Closed-loop control for tracking the flipping motion path of a magnetically controlled microrobot

[0152] This embodiment specifically employs Model Predictive Control (MPC) to perform closed-loop path tracking control on the flipping motion of the magnetically controlled microrobot. The path tracking accuracy and motion stability of this closed-loop control strategy are verified through experiments. The specific experimental conditions are as follows: the applied magnetic field amplitude is 15mT, the magnetic field frequency is 4Hz, and the path tracking target is a parabolic path. During the experiment, the above-mentioned Model Predictive Control strategy is used to perform closed-loop control on the flipping motion of the magnetically controlled microrobot to achieve accurate tracking of the preset parabolic path.

[0153] The path tracking results are as follows Figure 8 As shown in Figure (a), the red curve represents the robot's actual motion trajectory, and the blue curve represents the preset reference parabolic path. It can be seen from the figure that the two have a high degree of overall overlap. In the latter half of the reference parabolic path, where the curvature changes significantly, the robot needs to frequently adjust its posture to conform to the reference trajectory due to the influence of the model's predicted step size and control input constraints. This results in a slight deviation in the actual motion trajectory, but the amount of deviation is within the preset acceptable range and does not affect the overall tracking effect.

[0154] The corresponding longitudinal error as a function of time curve is as follows: Figure 8As shown in Figure (b), during the first 6 seconds of trajectory tracking, the longitudinal error remained within ±0.34 mm with minimal fluctuations. In the later stages of tracking, when the robot's actual trajectory deviated from the reference parabolic path, the model predictive control strategy took effect in real time, driving the robot to continuously correct itself. This achieved a closed-loop adjustment, correcting downward deviations upwards and vice versa, ultimately allowing the robot to accurately reach the endpoint of the parabolic path in 16.16 seconds. Statistical analysis of the experimental data showed that the maximum error during this path tracking process was 1.59 mm, and the root mean square error was only 0.33 mm, demonstrating excellent overall tracking accuracy.

[0155] The changes in the magnetic field in the three directions are as follows: Figure 8 As shown in (c), the magnetic field along the x-axis exhibits a standard sine wave, and the magnetic field along the z-axis exhibits a standard cosine wave, with both frequencies remaining stable. The magnetic field along the y-axis remains generally stable without significant abrupt changes, ensuring the smooth and controllable nature of the robot's flipping motion and path tracking process. This verifies the effectiveness and reliability of using model predictive control to achieve closed-loop control of the magnetically controlled microrobot's flipping motion path tracking. Simultaneously, a parallel comparative experiment was conducted using a dual-closed-loop PID control algorithm as a comparison. The experimental results show that the model predictive control strategy adopted in this invention has significant advantages in tracking accuracy, robustness, and control output stability, effectively ensuring the robot accurately reaches the intestinal lesion area and meeting the practical application needs of precision diagnosis and treatment of intestinal diseases.

[0156] Example 4: Multimodal Motion and Path Tracking Control System for Magnetically Controlled Microrobots

[0157] This embodiment provides a multimodal motion and path tracking control system for a magnetically controlled microrobot. This system is used to implement the multimodal motion and path tracking control methods for magnetically controlled microrobots described in embodiments 1 to 3 above, such as... Figure 9 As shown, the system specifically includes:

[0158] The multimodal motion triggering and switching module 100 is used to build an external magnetic drive system. By adjusting the relative angle between the rotating plane of the external magnetic field and the length direction of the magnetically controlled microrobot, it can realize the stable triggering and flexible switching of two modes of robot motion: flipping motion along the width direction and rolling motion along the length direction.

[0159] The multimodal dynamics model building module 200 is used to build corresponding dynamics models for flipping motion and rolling motion based on the structural characteristics and magnetic field action principle of the magnetically controlled microrobot, and to quantify the correspondence between magnetic torque and robot motion posture and motion parameters.

[0160] The 300 flip motion path tracking closed-loop control module is designed to address the issue of easy deviation in flip motion trajectory. It employs a model predictive control algorithm adapted to the characteristics of flip motion, combined with real-time visual feedback of robot motion parameters, to construct a path tracking closed-loop control strategy, correct the robot's motion trajectory in real time, and achieve accurate tracking of the preset path.

[0161] The specific implementation methods of each functional module in this embodiment are completely consistent with the corresponding steps in the aforementioned method embodiment, and can achieve the same technical effects as the method embodiment. Therefore, they will not be described again here.

[0162] Example 5: Magnetically Controlled Microrobot

[0163] This embodiment provides a magnetically controlled microrobot, which is controlled by the multimodal motion and path tracking control method of the magnetically controlled microrobot described in any one of embodiments 1 to 3.

[0164] The magnetically controlled microrobot has a "V"-shaped configuration. Driven by an external magnetic field, when the plane of rotation containing the magnetic field is parallel to the robot's length direction, the robot performs a rolling motion mode along its own length. When the plane of rotation containing the magnetic field is perpendicular to the robot's length direction, the robot performs a flipping motion mode along its own width direction. The robot adjusts the magnetic field parameters in real time through an external magnetic control system. In the flipping motion mode, it performs closed-loop path tracking control based on model predictive control to correct the movement trajectory, achieving precise movement and targeted lesion arrival in the complex intestinal environment.

[0165] Example 6: Electronic Equipment

[0166] This invention provides an electronic device, which includes a processor, a memory, and a bus system. The processor and the memory are connected through the bus system. The memory is used to store instructions, and the processor is used to execute the instructions stored in the memory to implement the multimodal motion and path tracking control method for a magnetically controlled microrobot as described in any one of embodiments 1 to 3.

[0167] It should be noted that the processor in this embodiment can be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), an off-the-shelf programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. The general-purpose processor can be a microprocessor or any conventional processor. The memory can include read-only memory and random access memory, and provides instructions and data to the processor. A portion of the memory may also include non-volatile random access memory.

[0168] Example 7: Computer Storage Medium

[0169] This invention provides a computer storage medium storing a computer software product, which includes several instructions to cause a computer device to execute all or part of the steps of the magnetically controlled microrobot multimodal motion and path tracking control method described in any one of embodiments 1 to 3 of this invention.

[0170] The aforementioned storage media include, but are not limited to: USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, optical disks, and other media capable of storing program code.

[0171] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0172] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0173] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The functions specified in one or more boxes. These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable apparatus for implementing the process. Figure 1One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0174] Obviously, the above embodiments are merely illustrative examples for clear explanation and are not intended to limit the implementation. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all possible implementations here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this invention.

Claims

1. A method for multimodal motion and path tracking control of a magnetically controlled microrobot, characterized in that, Includes the following steps: S1: Build an external magnetic drive system. By adjusting the relative angle between the rotating plane of the external magnetic field and the length direction of the magnetically controlled microrobot, the system can stably trigger and flexibly switch between two modes of motion: flipping motion along the width direction and rolling motion along the length direction. S2: Based on the structural characteristics and magnetic field principle of magnetically controlled microrobots, corresponding dynamic models are constructed for flipping and rolling motions respectively, and the correspondence between magnetic torque and robot motion posture and motion parameters is quantified; S3: To address the issue of easy deviation in the flipping motion trajectory, a model predictive control algorithm adapted to the flipping motion characteristics is adopted. Combined with the robot motion parameters fed back in real time from the vision, a path tracking closed-loop control strategy is constructed to correct the robot's motion trajectory in real time and achieve accurate tracking of the preset path.

2. The multimodal motion and path tracking control method for a magnetically controlled microrobot according to claim 1, characterized in that, In step S1, the specific method for implementing the flipping motion is as follows: A magnetically controlled microrobot is placed along its length along the y-axis. First, a bias magnetic field is applied along the positive z-axis to form a "V" shape. Then, a rotating magnetic field in the xz plane is applied, with the plane of rotation perpendicular to the robot's length. The robot is then driven by magnetic torque to perform a stable flipping motion along its width. The vector expression for the rotating magnetic field is: ； In the formula, The strength of the rotating magnetic field. , , These represent the rotating magnetic field strengths along the x-axis, y-axis, and z-axis, respectively. The frequency of the rotating magnetic field. For time.

3. The multimodal motion and path tracking control method for a magnetically controlled microrobot according to claim 1, characterized in that, In step S1, the specific method for implementing the rolling motion is as follows: The magnetically controlled microrobot is placed along the x-axis along its length. First, a bias magnetic field is applied along the positive z-axis to make the robot form a "V" shape. Then, a rotating magnetic field with the same parameters as the flipping motion is applied in the xz plane so that the rotating plane of the external magnetic field is parallel to the length direction of the robot. The robot is driven to complete a stable rolling motion along its own length direction by the magnetic torque.

4. The multimodal motion and path tracking control method for a magnetically controlled microrobot according to claim 1, characterized in that, In step S2, the specific method for constructing the flipping motion dynamics model is as follows: The bilaterally symmetrical magnetically controlled microrobot is simplified into a cuboid model. Based on the switching characteristics of the rotation axis during the flipping motion, the flipping motion is divided into two working conditions: rotation around the vertex A of the "V" shape and rotation around the line P connecting the two endpoints of the "V" shape. Based on the parallel axis principle, the moment of inertia under the two working conditions is calculated respectively. Combined with the mechanical analysis of magnetic torque and gravitational torque, the dynamic equations corresponding to the two working conditions are constructed respectively, and the correspondence between the flipping angle, angular acceleration and magnetic torque is quantified.

5. The multimodal motion and path tracking control method for a magnetically controlled microrobot according to claim 1, characterized in that, In step S2, the specific method for constructing the rolling motion dynamics model is as follows: The magnetically controlled microrobot is equivalent to a circular arc structure, and the rolling process is divided into two stages: the contact point slides on the arc and rotates around the endpoint of the arc. For the working condition of the robot rolling around any contact point, the corresponding moment of inertia is calculated. Combined with the mechanical analysis of gravitational torque and magnetic torque, the dynamic equation corresponding to the rolling motion is constructed to accurately describe the changes in the robot's motion state during the rolling process.

6. The multimodal motion and path tracking control method for a magnetically controlled microrobot according to claim 1, characterized in that, In step S3, the real-time visual feedback process includes: The motion parameters of the magnetically controlled microrobot, including the center of mass position and yaw angle, are collected in real time through an externally integrated vision detection module. Based on the collected motion parameters, the deviation between the robot's actual motion trajectory and the preset reference path is calculated. Based on the deviation, the external magnetic control signal parameters are dynamically adjusted to correct the robot's motion trajectory in real time.

7. The multimodal motion and path tracking control method for a magnetically controlled microrobot according to claim 1, characterized in that, In step S3, the prediction model construction process of the model predictive control algorithm adapted to the flipping motion characteristics is as follows: The dynamic model of the robot's flipping motion is transformed into a state vector. Control input is Nonlinear control systems, in which and This indicates the coordinates of the center of mass of the magnetically controlled microrobot within its plane of motion. Indicates the robot's yaw angle. Indicates transpose. The angular velocity of the robot is used as a variable to adjust the robot's motion posture in the control system. At the reference point of the reference trajectory, the nonlinear system is expanded using Taylor series and discretized to construct augmented state variables. This transforms the discrete state-space equations into a predictive model, the expression of which is: ； ； In the formula, for augmented state variables at time t, , , The state-space coefficient matrix, for Incremental control input at any time This is the system output.

8. The multimodal motion and path tracking control method for a magnetically controlled microrobot according to claim 7, characterized in that, In the model predictive control algorithm, a quadratic objective function is designed based on the prediction time domain and the control time domain. The expression of the objective function is: ； In the formula, In order to be in At any given moment, it is a comprehensive evaluation index of the system's control performance over a future period of time. To predict the time domain, To control the time domain, for Tracking deviation at any given moment for Control increment at any time, The state weight matrix is... To control the incremental weight matrix, For relaxation factor weights, It is a relaxation factor.

9. The multimodal motion and path tracking control method for a magnetically controlled microrobot according to claim 8, characterized in that, When solving the objective function, constraints are imposed on the control input and control increment. The constraints are as follows: ； ； In the formula, for Control input at any time, , These are the upper and lower limits of the control input quantity, respectively. , These are the upper and lower limits for controlling the increment, respectively; Combining the objective function and constraints, the optimal control problem for trajectory tracking is transformed into a quadratic programming problem, yielding the optimal control increment sequence: ； In the formula, for The control increment sequence at each time step, , , They are respectively , , Incremental control input at any given time.

10. A multimodal motion and path tracking control system for a magnetically controlled microrobot, characterized in that, The system is used to implement the multimodal motion and path tracking control method for a magnetically controlled microrobot as described in any one of claims 1 to 9, specifically including: The multimodal motion triggering and switching module is used to build an external magnetic drive system. By adjusting the relative angle between the rotating plane of the external magnetic field and the length direction of the magnetically controlled microrobot, it can achieve stable triggering and flexible switching between two modes of robot motion: flipping motion along the width direction and rolling motion along the length direction. The multimodal dynamics model building module is used to build corresponding dynamics models for flipping and rolling motions based on the structural characteristics and magnetic field action principle of magnetically controlled microrobots, and to quantify the correspondence between magnetic torque and robot motion posture and motion parameters. The flipping motion path tracking closed-loop control module is designed to address the issue of easy deviation in flipping motion trajectories. It employs a model predictive control algorithm adapted to the characteristics of flipping motion, combined with real-time visual feedback of robot motion parameters, to construct a path tracking closed-loop control strategy, thereby correcting the robot's motion trajectory in real time and achieving accurate tracking of the preset path.

Images