Autonomous control method and system for in-vivo micro robot pose based on visual slam

Abstract

The application discloses an in-vivo micro robot posture autonomous regulation method and system based on visual SLAM, and the method comprises the following steps: S1, giving relevant parameters; S2, adopting visual SLAM to combine key frame short window optimization and geometric prior constraint to solve robot Euler angle posture sequence, and constructing visual servo mapping from image to angular velocity instruction through supervised learning; S3, establishing an optimization model fusing posture tracking accuracy, scanning coverage and multiple safety constraints, adopting genetic algorithm and Monte Carlo simulation to jointly solve an optimal control sequence, combining passable path dynamic programming, fusing feedforward compensation and global reference benchmark through a model predictive controller to realize rolling horizon closed-loop regulation. The system comprises three modules of in-vivo micro robot, visual perception and decision and in-vitro magnetic field array, and adopts a four-layer collaborative architecture. The application realizes high-precision and high-safety autonomous posture closed-loop control in a complex environment in the stomach without additional in-vitro positioning equipment.

Description

Technical Field

[0001] This invention relates to the field of robot posture control technology, specifically to a method and system for autonomous posture control of an in vivo microrobot based on visual SLAM. Background Technology

[0002] In recent years, with the increasing incidence of digestive tract diseases and the popularization of non-invasive diagnosis and treatment concepts, magnetically controlled intracorporeal microrobot endoscopy, as a core device for non-invasive gastrointestinal examination, has seen rapid development and widespread application in early cancer screening and clinical diagnosis of digestive tract diseases in my country due to its advantages of being painless, non-invasive, free from cross-infection, and requiring no anesthesia. As of 2024, the number of magnetically controlled intracorporeal microrobot endoscopes installed in my country exceeded 3,000 units, with over 5 million examinations completed. Its penetration rate in county-level hospitals has been increasing year by year, making it one of the core devices in my country's early gastric cancer screening system. Currently, the core control mode of magnetically controlled intracorporeal microrobots in clinical applications still relies primarily on manual intervention by the physician. This means that the physician controls the magnetic field through an external joystick to adjust the posture and position of the intracorporeal microrobot. This mode is highly dependent on the physician's experience and proficiency, and prolonged continuous operation can easily lead to visual and operational fatigue, resulting in insufficient scanning coverage of the gastric examination area and a high rate of missed diagnoses of suspicious lesions, making it difficult to meet the needs of large-scale clinical early screening applications.

[0003] The development of high-precision pose detection and autonomous posture control technologies has provided a novel solution to a series of pain points in the manual control of magnetically controlled intracellular microrobots. For pose detection of intracellular microrobots, existing technologies mainly fall into two categories: external positioning and visual pose estimation. External positioning schemes use external devices such as electromagnetic fields and X-rays to detect the pose of the intracellular microrobot, while visual pose estimation schemes use images captured by the intracellular microrobot's built-in camera to estimate the pose. For posture control of intracellular microrobots, existing technologies focus on predefined trajectory open-loop control and PID closed-loop feedback control, aiming to reduce reliance on manual operation by doctors. For intracellular microrobots, the core of achieving fully autonomous examination lies in high-precision and high-stability pose estimation and adaptive and high-safety posture control.

[0004] Reference 1, "Research Progress in Pose Detection Technology of Magnetically Controlled Capsule Endoscopy Robot" (Journal of Instrumentation, 2022, Vol. 43, No. 8, pp. 1-16), systematically reviews the mainstream pose detection schemes for magnetically controlled in vivo microrobots. It compares and analyzes the detection accuracy and applicable scenarios of electromagnetic positioning, optical positioning, and visual positioning schemes. The comparison shows that the pose calculation scheme based on monocular vision has the advantages of being radiation-free, having low hardware cost, and naturally integrating with the endoscopic imaging system, making it the optimal pose detection scheme for clinical application. However, the existing visual pose calculation schemes reviewed in this reference are not optimized for the special environment of the stomach, characterized by low texture, mucosal reflection, and non-rigid peristalsis of the gastric wall. They cannot solve the core problems of feature matching failure and pose calculation scale drift in complex dynamic environments. Reference 2, "Autonomous motion control of magnetic capsule endoscope for complete stomach coverage" (IEEE Transactions on Medical Robotics and Bionics, Vol. 3, No. 4, 2021, pp. 892-901), proposes an autonomous motion control strategy for a magnetically controlled capsule endoscope to achieve complete stomach coverage. Based on a predefined gastric scanning trajectory and external electromagnetic positioning feedback, it realizes autonomous motion control of the in vivo microrobot, achieving 98% coverage in a simulated gastric environment. However, this method relies on external electromagnetic positioning equipment for pose feedback, which not only increases the system hardware cost but also makes it difficult to meet the real-time attitude control requirements due to the transmission delay of positioning and control. Furthermore, the predefined trajectory open-loop control strategy cannot adaptively adjust control commands according to the real-time intragastric environment, making it difficult to cope with the problems of spin jitter of the in vivo microrobot and low target area alignment accuracy caused by dynamic disturbances within the stomach.

[0005] To achieve fully autonomous clinical examination using magnetically controlled in vivo microrobots, and to improve the stability of pose calculation and the adaptability of posture control in the complex dynamic environment of the stomach without the need for additional external positioning equipment, it is necessary to develop a posture control scheme for magnetically controlled in vivo microrobots that integrates visual pose calculation, autonomous command generation, and safe closed-loop drive. However, existing technologies cannot simultaneously achieve high-precision visual pose calculation without additional positioning equipment and adaptive, safe, and autonomous posture control, exhibiting significant limitations due to unstable pose feedback, poor control robustness, and high clinical safety risks. Therefore, developing a magnetically controlled in vivo microrobot system that requires no additional external positioning equipment, can adapt to the complex dynamic environment of the stomach, and achieves high-precision pose calculation and safe, stable, and autonomous posture control has become an urgent technical problem to be solved in this field. Furthermore, existing technologies do not deeply integrate gastric traversable path planning with model predictive control (MPC), failing to achieve real-time closed-loop optimal control under dynamic disturbances while ensuring full-domain scan coverage, further limiting the clinical application of fully autonomous magnetically controlled in vivo microrobot examinations. Summary of the Invention

[0006] The technical problem to be solved by this invention is to overcome the shortcomings of existing technologies, such as reliance on manual operation, low pose calculation accuracy, poor environmental adaptability, and insufficient control robustness of in vivo microrobots. This invention provides a method and system for autonomous posture control of in vivo microrobots based on visual SLAM. Through a unified three-step core process, it achieves fully autonomous, high-precision, and high-safety closed-loop posture control of in vivo microrobots in the complex environment of the stomach.

[0007] To solve the above problems, the present invention adopts the following technical solution:

[0008] First, this invention proposes a method for autonomous posture control of an in vivo microrobot based on visual SLAM, comprising the following steps:

[0009] S1. Given the constraints of the in vivo environment, the hardware parameters of the in vivo microrobot and magnetic control system, the control parameters of visual SLAM and supervised learning, the number of scheduling periods and the maximum number of iterations;

[0010] S2. Based on the intragastric images acquired by the in vivo microrobot, visual SLAM is used in combination with keyframe short window optimization and geometric prior constraints to solve the Euler angle pose sequence of the in vivo microrobot; at the same time, a supervised learning strategy is constructed with the mapping from the current frame image to the Euler angle angular velocity command as the learning objective to generate visual servo control commands for the in vivo microrobot.

[0011] S3. Based on the Euler angle and angular velocity command, calculate the coil driving current of the peripheral magnetic field array, drive the peripheral magnetic field array to control the attitude of the in-body microrobot, establish an optimized attitude control model for the in-body microrobot, and solve the optimized model based on a genetic algorithm to achieve closed-loop control of the attitude of the in-body microrobot.

[0012] Preferably, the optimization model in step S3 aims to maximize the gains in attitude tracking accuracy and gastric scan coverage, and minimize the penalty costs for attitude deviation and violations of safety constraints, and includes at least the following constraints:

[0013] (1) Attitude deviation probability constraint: The probability that the deviation between the actual Euler angle attitude of the in vivo microrobot and the target Euler angle attitude exceeds the preset maximum allowable deviation does not exceed the set value;

[0014] (2) Coil current amplitude constraint: The coil current in each scheduling period shall not exceed the preset safety threshold;

[0015] (3) Minimum attitude holding time constraint: The duration of the in vivo microrobot after attitude adjustment in both forward and reverse directions shall not be less than the corresponding minimum attitude holding time;

[0016] (4) Stomach wall safety distance constraint: The minimum distance between the in vivo microrobot and the stomach wall is kept within the preset safety distance range.

[0017] Preferably, solving the in vivo microrobot attitude control optimization model specifically includes the following steps:

[0018] S3.1 Set the genetic algorithm parameters, including: population size Npop, crossover rate Pc, mutation rate Pm and maximum number of generations Gmax;

[0019] S3.2 Randomly generate an initial population consisting of Npop chromosomes; the chromosomes in the initial population consist of m code points. If the code point i is 1, it means that the magnetic field array is in the attitude adjustment state during the i-th scheduling period; if it is -1, it means that the magnetic field array is in the attitude maintenance state during the i-th scheduling period.

[0020] S3.3. Initialize the generation index g to 0;

[0021] S3.4 Let g = g + 1, calculate the fitness of the chromosome in the g-th generation, and initialize the chromosome index k to 1;

[0022] S3.5. Decode the k-th chromosome in the current population to determine the attitude adjustment sequence of the magnetic field array within the scheduling day; if the in vivo microrobot does not meet the shortest attitude maintenance time constraint, calculate the fitness of the k-th chromosome according to the preset penalty function. , and jump to step S3.8;

[0023] If the in-vivo micro-robot meets the shortest attitude holding time constraint, calculate the initial planned coil current of the magnetic field array in each scheduling period according to the attitude adjustment sequence within the magnetic field array scheduling day, and continue to execute step S3.6;

[0024] S3.6. Use the Monte Carlo simulation method to simulate the operating conditions of the in-vivo micro-robot within the scheduling day, and iteratively update the planned coil current of the magnetic field array according to the simulation results to obtain the optimal planned control sequence of the magnetic field array;

[0025] S3.7. According to the optimal planned control sequence of the magnetic field array, simulate the operating conditions of the in-vivo micro-robot within the scheduling day, calculate the attitude tracking accuracy and gastric cavity scanning coverage rate of the in-vivo micro-robot, and calculate the fitness of the k-th chromosome according to the preset fitness function :

[0026] S3.8. Determine whether the fitness calculation of all chromosomes in the current population is completed, that is, determine whether k is equal to Npop; if k < Npop, let k = k + 1, and jump to step S3.5; otherwise, continue to execute step S3.9;

[0027] S3.9. Determine whether the maximum number of generations of evolution has been reached; if g = Gmax, execute step S3.10; otherwise, based on the fitness, perform replication, crossover and mutation operations on the current population, update the chromosome population, and jump to step S3.4;

[0028] S3.10. Output the control scheme of the in-vivo micro-robot corresponding to the chromosome with the highest fitness in the current population as the optimal scheme, and end the algorithm process.

[0029] Preferably, step S3.2 specifically includes the following steps:

[0030] S3.2.1. Set the initial working state of the magnetic field array to the attitude adjustment state, and set the value of the first code bit in the chromosome to 1;

[0031] S3.2.2. Randomly generate a length of the attitude adjustment period, and initialize the first several code bits of the chromosome to 1;

[0032] S3.2.3. Determine whether the remaining length of the chromosome is less than the preset shortest attitude holding time. If so, initialize the remaining code bits to -1 and end the initialization of the current chromosome; if not, randomly generate a length of the attitude holding period, and initialize the corresponding number of subsequent code bits of the chromosome to -1;

[0033] S3.2.4. Determine again whether the remaining length of the chromosome is less than the preset minimum attitude adjustment time. If yes, initialize all remaining code points to 1 and end the initialization of the current chromosome. If no, randomly generate an attitude adjustment time period length, initialize the corresponding number of code points of the subsequent chromosome to 1, and return to step S3.2.3.

[0034] S3.2.5 Repeat the above steps until a complete chromosome is generated and an initial population is formed.

[0035] Preferably, step S3.5, which involves calculating the initial planned coil current of the magnetic field array during each scheduling period based on the attitude adjustment sequence of the magnetic field array scheduling day, specifically includes the following steps:

[0036] S3.5.1 Decode the kth chromosome in the current population to determine the working status of the magnetic field array in each scheduling period;

[0037] S3.5.2 Statistically analyze the continuous working periods experienced by the magnetic field array within the scheduling day, and determine the start and end scheduling periods of each continuous working period;

[0038] S3.5.3 Calculate the initial planned coil current for each continuous working period in sequence:

[0039] If the current continuous working period is an attitude adjustment period, the initial planned coil current for each scheduling period is calculated based on the target Euler angle attitude and the preset magnetic field-attitude conversion coefficient for that period.

[0040] If the current continuous working period is the attitude maintenance period, then the initial planned coil current for each scheduling period is set to the constant current value required for attitude maintenance.

[0041] Preferably, step S3.6 involves using Monte Carlo simulation to simulate the daily operation of the in vivo microrobot and iteratively correcting the planned coil current of the magnetic field array based on the simulation results to obtain the optimal planned control sequence for the magnetic field array. This specifically includes the following steps:

[0042] S3.6.1 Initialize the real-time control performance evaluation indicators of the in vivo microrobot, including posture tracking accuracy, gastric cavity scan coverage and safety boundary fit.

[0043] S3.6.2. Determine the pose state of the in vivo microrobot in the initial period based on the working sequence of the magnetic field array determined by chromosome decoding.

[0044] S3.6.3. Based on the intragastric image of the current time period, determine the feature distribution and pose calculation constraint parameters of visual SLAM, and use Gaussian distribution to describe the random fluctuation of the actual pose around its SLAM solution value.

[0045] S3.6.4 Generate random pose deviations that follow the above Gaussian distribution as real-time disturbances for the current time period;

[0046] S3.6.5. Based on the random pose deviation of the current time period, determine the real-time coil current of the magnetic field array, and calculate the attitude tracking benefit and attitude deviation penalty cost.

[0047] S3.6.6 Update the attitude tracking accuracy and gastric cavity scan coverage indicators, and update the safety boundary fit indicator when the gastric wall safety distance constraint is met;

[0048] S3.6.7. Based on the visual SLAM model, calculate the real-time pose and Euler angles of the microrobot in the body during the current time period.

[0049] S3.6.8. Enter the next scheduling period and repeat S3.6.3 to S3.6.7 until one simulation run of all scheduling periods is completed;

[0050] S3.6.9 Repeat steps S3.6.2 to S3.6.8 until the preset number of Monte Carlo simulations is reached;

[0051] S3.6.10 Determine whether the control performance indicators of each scheduling period meet the clinical requirements. If all meet the requirements, end the simulation. If there are scheduling periods that do not meet the requirements, correct the planned coil current of these periods according to the preset correction coefficient, and return to step S3.6.1 to re-simulate until all periods meet the clinical requirements or the maximum number of corrections is reached.

[0052] Preferably, the target Euler angle pose is generated based on the passable path in the stomach cavity, specifically including the following steps:

[0053] Based on the 3D point cloud map of the stomach wall output by visual SLAM, statistical filtering and voxel downsampling are performed to construct a 3D grid map of the stomach cavity that matches the size of the in vivo microrobot.

[0054] Based on the preset minimum safe distance of the stomach wall, the stomach wall point cloud is subjected to morphological dilation processing, the obstacle area is marked in the grid map, and the free passable space of the stomach cavity is extracted.

[0055] Using the current pose of the in vivo microrobot calculated by visual SLAM as the starting point of the path, and the preset key pose sequence of the whole gastric cavity without blind spots as the ending point of the path, the improved RRT* algorithm is used to generate a globally passable path, and the target Euler angle pose sequence corresponding to each node on the path is calculated.

[0056] Based on real-time acquired intragastric images and visual SLAM pose calculation results, the dynamic disturbance of gastric wall peristalsis is perceived. A local obstacle avoidance correction algorithm is used to correct the global passable path in real time, and the passable path and the corresponding target Euler angle pose sequence are dynamically updated.

[0057] Preferably, a model predictive controller is used to achieve closed-loop control of the posture of the in vivo microrobot, specifically including the following steps:

[0058] A discretized state-space model of the attitude dynamics of an in vivo microrobot is established, with the three-axis Euler angles and Euler angle angular velocities of the in vivo microrobot as state variables and the three-axis Euler angle angular velocity control commands as control variables.

[0059] The prediction time domain and control time domain of model predictive control are defined. The target Euler angle attitude sequence is used as the tracking reference value, and the optimization objective is to maximize the overall control benefit. A finite time domain open-loop optimization objective function is constructed.

[0060] The attitude deviation constraint, coil current constraint, shortest attitude holding time constraint, and stomach wall safety distance constraint are transformed into inequality constraints for the model predictive control optimization problem.

[0061] The constrained quadratic programming problem is solved by rolling the solution to obtain the optimal angular velocity control sequence in the control time domain. The first control variable in the sequence is taken as the final control command and sent to the external magnetic field array.

[0062] At the next scheduling moment, based on the real-time pose feedback of the in vivo microrobot output by visual SLAM, the state variables and reference sequence are updated, and the above steps are repeated to achieve closed-loop attitude control with rolling time domain optimization.

[0063] The optimal control sequence output by model predictive control is used as the final execution instruction, the angular velocity instruction output by supervised learning visual servoing is used as a feedforward compensation term and incorporated into the optimization objective function, and the global plan sequence solved by the genetic algorithm is used as the reference benchmark for the prediction time domain of model predictive control.

[0064] Meanwhile, this invention proposes an in vivo microrobot posture autonomous control system based on visual SLAM, comprising:

[0065] The in-body micro-robot module, which incorporates a micro-camera, an illumination unit, a wireless transmission unit, and a cylindrical permanent magnet, is used to acquire gastroscopic images in real time and transmit the images to the visual perception and decision-making module via the wireless transmission unit. It can perform multi-degree-of-freedom posture adjustments under the drive of an external magnetic field.

[0066] The visual perception and decision-making module is used to receive images inside the stomach. It has a built-in lightweight visual SLAM processing unit, a supervised learning visual servo unit, and an instruction safety verification unit. It is used to combine keyframe short window optimization and stomach geometric prior constraints to solve the Euler angle posture sequence of the microrobot inside the body, output angular velocity control instructions through the supervised learning model, and complete the safety verification of the instructions.

[0067] The external magnetic field array control module includes multiple sets of electromagnetic coils, power drive circuits, and a lower-level control unit. It is used to receive angular velocity control commands, calculate them into coil drive currents, generate controlled magnetic fields and magnetic torques, and realize closed-loop attitude control of the in vivo microrobot.

[0068] Preferably, the system adopts a four-layer collaborative architecture: front-end image acquisition and preprocessing—mid-level visual SLAM pose calculation—upper-level instruction generation and visual servoing—lower-level magnetic control drive and closed-loop feedback, wherein:

[0069] The front-end image acquisition and preprocessing submodule is integrated into the in vivo microrobot module and is used to complete real-time acquisition, noise reduction, anti-glare removal and contrast enhancement preprocessing of images inside the stomach.

[0070] The mid-level visual SLAM pose calculation submodule is integrated into the visual perception and decision-making module. It is used to complete the stable calculation of the six-degree-of-freedom pose of the in vivo microrobot in the gastric environment and provide pose feedback.

[0071] The upper-level instruction generation and visual servoing submodule is integrated into the visual perception and decision-making module and is used to complete the generation, fusion and security verification of angular velocity control instructions.

[0072] The underlying magnetic control drive submodule is integrated into the external magnetic field array control module. It is used to complete the precise output of coil current and magnetic field drive, and realize the closed-loop control of the posture of the in vivo micro-robot.

[0073] The present invention adopts the above technical solution and has the following technical effects compared with the prior art:

[0074] (1) This invention introduces keyframe short window optimization and gastric cavity geometric prior constraints in visual SLAM. Through local sliding window nonlinear optimization and gastric size boundary constraints, it effectively suppresses scale drift and pose accumulation errors of monocular vision in low-texture, low-light gastric environment. No additional external positioning equipment is required, which greatly reduces the system hardware cost while ensuring the pose calculation accuracy.

[0075] (2) This invention constructs a supervised learning mapping model from the current frame image to the Euler angle angular velocity command, realizing end-to-end visual servoing from a single frame to angular velocity, avoiding the delay of feature matching and Jacobian matrix calculation in traditional visual servoing; at the same time, the supervised learning output is integrated as a feedforward compensation term into the model predictive control (MPC) optimization target, and together with the global sequence planned by the genetic algorithm, it forms a multi-level control architecture of feedforward, feedback and reference, which significantly improves the response speed of attitude adjustment and the alignment accuracy of the target area, and effectively suppresses the spin jitter of the microrobot in the body caused by dynamic disturbances such as stomach wall peristalsis and reflective interference.

[0076] (3) Based on the real-time three-dimensional point cloud map output by visual SLAM, this invention combines gastric wall expansion processing and improved path planning algorithm to dynamically generate passable paths in the gastric cavity and corresponding target Euler angle posture sequences. Through MPC, the control instructions are optimized by rolling under multiple constraints such as the shortest posture holding time, coil current limit, and safe distance from the gastric wall. This ensures that the microrobot in the body can perform full-domain scanning without blind spots in the gastric cavity while always maintaining a clinically safe distance from the gastric wall, achieving a balance between high coverage and high safety, and meeting the clinical safety requirements of digestive tract examination. Attached Figure Description

[0077] Figure 1 This invention relates to a method for autonomous posture control of in vivo microrobots based on visual SLAM.

[0078] Figure 2 This is a flowchart of the front-end and back-end processing of the visual SLAM processing unit involved in the present invention. Detailed Implementation

[0079] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0080] Example 1: This example proposes a method for autonomous posture control of an in vivo microrobot based on visual SLAM, which is divided into the following steps: Figure 1 The three core steps shown are described in detail below:

[0081] S1. Given the constraints of the in vivo environment, the hardware parameters of the in vivo microrobot and magnetic control system, the control parameters of visual SLAM and supervised learning, the number of scheduling periods and the maximum number of iterations;

[0082] S2. Based on the intragastric images acquired by the in vivo microrobot, visual SLAM is used in combination with keyframe short window optimization and geometric prior constraints to solve the Euler angle pose sequence of the in vivo microrobot; at the same time, a supervised learning strategy is constructed with the mapping from the current frame image to the Euler angle angular velocity command as the learning objective to generate visual servo control commands for the in vivo microrobot.

[0083] S3. Based on the Euler angle and angular velocity command, calculate the coil driving current of the peripheral magnetic field array, drive the peripheral magnetic field array to control the attitude of the in-body microrobot, establish an optimized attitude control model for the in-body microrobot, and solve the optimized model based on a genetic algorithm to achieve closed-loop control of the attitude of the in-body microrobot.

[0084] For in vivo microrobot gastric examination systems, the key to improving posture control performance during diagnosis and treatment lies in achieving high-precision alignment of the target area, smooth and stable posture tracking, and blind-spot-free scanning of the entire gastric region while ensuring clinical safety constraints. Based on this, this invention proposes an in vivo microrobot visual closed-loop posture autonomous control model, with a core multi-dimensional comprehensive evaluation system. Specific evaluation indicators and safety constraints are as follows:

[0085] (1) Attitude deviation probability constraint: The probability that the deviation between the actual Euler angle attitude of the in vivo microrobot and the target Euler angle attitude exceeds the preset maximum allowable deviation does not exceed the set value;

[0086] (2) Coil current amplitude constraint: The coil current in each scheduling period shall not exceed the preset safety threshold;

[0087] (3) Minimum attitude holding time constraint: The duration of the in vivo microrobot after attitude adjustment in both forward and reverse directions shall not be less than the corresponding minimum attitude holding time;

[0088] (4) Stomach wall safety distance constraint: The minimum distance between the in vivo microrobot and the stomach wall is kept within the preset safety distance range.

[0089] like Figure 2 As shown, the front-end and back-end processing steps of the visual SLAM processing unit in step S2 are as follows:

[0090] S2.1: Set the visual SLAM running parameters, including: upper and lower limits of the number of feature points extracted per frame, feature matching threshold, key frame selection threshold, sliding window length, geometric prior constraint weight, and upper limit of the number of nonlinear optimization iterations;

[0091] S2.2: Initialize the visual SLAM system, including: camera intrinsic parameter multi-coordinate system extrinsic parameter calibration, local and global map initialization, feature extractor initialization, and sliding window optimizer initialization;

[0092] S2.3: The scheduling time frame index t is initialized to 1, that is, t=1;

[0093] S2.4: Let t=t+1, obtain the preprocessed gastroscopy image of the t-th frame, and enter the front-end processing unit;

[0094] S2.5: Perform ORB feature extraction and descriptor calculation on the current frame image, complete the feature matching of adjacent frames, and use the RANSAC algorithm to remove mismatched points;

[0095] S2.6: Determine whether the system has completed map initialization; if map initialization has not been completed, solve the homography matrix / fundamental matrix to complete the initial pose estimation and map initialization, and continue to execute step S2.7; if map initialization has been completed, track the constructed local map, solve the initial pose of the current frame by minimizing the reprojection error, and continue to execute step S2.7.

[0096] S2.7: Perform keyframe filtering judgment to determine whether the current frame meets the keyframe filtering conditions; if it does not meet the filtering conditions, it is determined to be an invalid frame, the current frame is discarded, and the process jumps to step S2.4; if it meets the filtering conditions, it is determined to be a valid keyframe, the keyframe and initial pose are output, the process enters the backend processing unit, and step S2.8 is continued.

[0097] S2.8: Insert valid keyframes into the sliding window to complete local map construction and update, generate new map points through triangulation, and filter and remove invalid map points with insufficient tracking rate;

[0098] S2.9: Based on the sliding window, short-window nonlinear optimization is performed with the core objective of minimizing visual reprojection error. Prior geometric constraints on gastric cavity size are added to correct the scale drift of pose calculation.

[0099] S2.10: Perform local loop closure detection and filter candidate frames for loop closure; if a valid loop closure is detected, perform global pose graph optimization to complete global scale and pose drift correction, and continue to step S2.11; if no valid loop closure is detected, directly execute step S2.11.

[0100] S2.11: Output the optimized six-DOF pose data of the in-body microrobot in the current frame, extract the three-axis Euler angles simultaneously, and determine whether the processing of all frames in the current scheduling cycle has been completed, i.e., determine whether t is equal to the total number of scheduling periods; if t is less than the total number of scheduling periods, jump to step S2.4; otherwise, output the full-cycle pose sequence and complete the SLAM solution process.

[0101] The multi-coordinate system extrinsic parameter calibration and system initialization steps described in step S2.2 are as follows:

[0102] S2.0.1: Complete the camera intrinsic parameter calibration, obtain the camera focal length, principal point coordinates, radial and tangential distortion coefficients, and construct the camera imaging model;

[0103] This embodiment uses Zhang's calibration method to complete the intrinsic parameter calibration of a monocular camera. The specific process is as follows: print a checkerboard calibration board, control the built-in camera of the micro-robot to acquire ≥20 clear images of the calibration board in different poses; extract the pixel coordinates of the checkerboard corner points in each image, and combine them with the world coordinates of the calibration board to solve for the camera intrinsic parameter matrix K and distortion coefficients; minimize the reprojection error through nonlinear optimization, optimize the intrinsic parameters and distortion coefficients, and finally obtain the camera intrinsic parameter matrix:

[0104]

[0105] In the formula, , For camera , Normalized focal length along the axial direction, , The image principal point pixel coordinates; distortion coefficients include , , The three radial distortion coefficients and , Two tangential distortion coefficients are used to construct a camera pinhole imaging model and a distortion correction model.

[0106] Exemplary parameters in this embodiment: Camera resolution 400×400, focal length ==400 pixels, principal point coordinates =200、 =200 pixels, distortion coefficient =-0.32、 =0.11、 =-0.02、 =-0.005、 =0.003.

[0107] S2.0.2: Complete hand-eye calibration and solve for the extrinsic parameters of the camera coordinate system and the body coordinate system of the microrobot inside the robot, including the rotation matrix and translation vector;

[0108] This embodiment uses an eye-in-hand calibration model. The body coordinate system of the in-body microrobot is {Body}, the camera coordinate system is {Cam}, and the extrinsic hand-eye parameters to be solved are... That is, the homogeneous transformation matrix from the camera coordinate system to the body coordinate system of the microrobot inside the body, expressed as:

[0109]

[0110] In the formula, It is a 3×3 rotation matrix. It is a 3×1 translation vector.

[0111] The calibration process is as follows: fix the position of the calibration plate, control the microrobot inside the body to complete ≥15 different pose transformations under the drive of an external magnetic field, and simultaneously record the transformation matrix of the camera coordinate system relative to the world coordinate system of the calibration plate under each pose. And the transformation matrix of the body coordinate system of the microrobot within the body relative to the magnetic field world coordinate system. Based on hand-eye calibration equation ,in The initial values ​​of the hand-eye extrinsic parameters are obtained by using the Tsai-Lenz algorithm. Then, the reprojection error is optimized by nonlinear least squares method to obtain the final hand-eye calibration extrinsic parameters, realizing the conversion from camera pose to the pose of the in-body micro-robot.

[0112] S2.0.3: Complete the magnetic coordinate system calibration, solve the transformation relationship between the body coordinate system of the microrobot inside the body and the magnetic field world coordinate system, and achieve coordinate system consistency of the pose data;

[0113] In this embodiment, the magnetic field world coordinate system is {Mag}, and the extrinsic parameter for the magnetic coordinate system transformation to be solved is BodyMag T, which is the homogeneous transformation matrix from the body coordinate system of the microrobot to the magnetic field world coordinate system. The calibration process is as follows:

[0114] (1) Establish the world coordinate system reference of the magnetic field array, with the center of the magnetic field array as the origin, the array plane as the XY plane, and the positive direction of the Z axis perpendicular to the array plane upward, and determine the magnetic field world coordinate system {Mag}.

[0115] (2) Fix the in vivo microrobot on a high-precision three-axis turntable and place it at the center of the magnetic field array workspace. Control the turntable to drive the in vivo microrobot to complete ≥20 sets of known posture transformations. Record the transformation matrix of the in vivo microrobot body coordinate system relative to the turntable reference coordinate system under each posture. At the same time, solve the pose of the in vivo microrobot in the magnetic field world coordinate system through the current-magnetic torque mapping relationship of the magnetic field array.

[0116] (3) Based on multiple sets of pose pairing data, the singular value decomposition (SVD) method is used to solve the point set registration problem to obtain the initial value of BodyMag T. Then, nonlinear optimization is completed by minimizing the pose solution error, and finally the transformation extrinsic parameters between the body coordinate system of the micro-robot and the magnetic field world coordinate system are obtained.

[0117] (4) Combine the hand-eye extrinsic parameters solved by S2.0.2 to complete the full-link coordinate transformation from the camera coordinate system to the body coordinate system of the micro-robot inside the body and then to the magnetic field world coordinate system, so as to realize the system-wide coordinate unification of pose data.

[0118] S2.0.4: Initialize the local and global maps, clear map point and keyframe history data, and initialize the sliding window optimizer and loop closure detection module.

[0119] Exemplary parameters in this embodiment: the number of keyframes within the sliding window is fixed at 8 frames, loop closure detection uses the bag-of-words (BoW) model, the feature dictionary uses the ORB feature dictionary, the loop closure detection similarity threshold is set to 0.75, the nonlinear optimizer uses the g2o general graph optimization library, and the maximum number of iterations is set to 20.

[0120] For in vivo microrobots, the core of improving clinical diagnostic effectiveness is to achieve a closed-loop system of precise pose perception, autonomous servo command generation, and safe and smooth posture control. Based on this, this embodiment proposes an in vivo microrobot posture control optimization model, with the highest Euler angle posture tracking accuracy and the largest gastric cavity scanning coverage as the core optimization objectives. The specific expression for maximizing the overall control benefit is shown in formula (1):

[0121] (1)

[0122] In formula (1), t is the scheduling period index (t=1, 2, …, m); m is the number of scheduling periods; F is the comprehensive benefit of the microrobot control in the body; The gain in attitude tracking accuracy of the in vivo microrobot during the scheduling period t. The benefit of gastric cavity scan coverage for in vivo microrobots during scheduling period t; The cost of penalizing the posture deviation of the in vivo microrobot during the scheduling period t; The penalty cost for violating safety constraints by in vivo microrobots during the scheduling day;

[0123] In formula (1), the attitude tracking accuracy gain of the in vivo microrobot during the scheduling period t is... Specifically, as shown in formula (2):

[0124] (2)

[0125] In formula (2), Weights for attitude tracking rewards; The actual Euler angle pose of the microrobot within the body during the scheduling period t; ΔT represents the target Euler angle pose of the microrobot within the body during the scheduling period t; ΔT is the length of the scheduling period.

[0126] In formula (1), the gain of gastric cavity scan coverage of the in vivo microrobot during the scheduling period t is... Specifically, as shown in formula (3):

[0127] (3)

[0128] In formula (3), Weighting based on scan coverage gain; The newly added effective scanning area of ​​the gastric cavity during the scheduling period t;

[0129] In formula (1), the posture deviation penalty cost of the in vivo microrobot during the scheduling period t is... Specifically, as shown in formula (4):

[0130] (4)

[0131] In formula (4), The attitude deviation penalty coefficient for scheduling period t; The planned posture of the in vivo microrobot during the scheduling period t is shown in formula (5):

[0132] (5)

[0133] In formula (5), The planned posture of the in vivo microrobot during the scheduling period t is determined jointly by the visual SLAM solution and the control optimization model. The Euler angle pose calculated by visual SLAM for the scheduling period t; The planned attitude adjustment of the magnetic field array during the scheduling period t is represented by a positive value, which indicates a positive attitude adjustment of the in vivo microrobot, and a negative value indicates a negative attitude adjustment of the in vivo microrobot. The planned attitude is iteratively updated based on the target Euler angle attitude.

[0134] In formula (1), The penalty cost for violating the safety constraints of the in vivo microrobot is estimated based on the safe distance between the in vivo microrobot and the stomach wall and the safe threshold of the coil current, as shown in formula (6):

[0135] (6)

[0136] In formula (6), Weighting for security penalties; , These are binary variables representing violations of safety distance and exceeding coil current limits, respectively. A value of 1 indicates that the in vivo microrobot violates the minimum safe distance constraint from the stomach wall during the scheduling period t. A value of 1 indicates that the coil current of the magnetic field array exceeds the safety threshold during the scheduling period t.

[0137] The constraints in the in vivo microrobot attitude control optimization model are as follows:

[0138] In vivo microrobot posture deviation probability constraint:

[0139] (7)

[0140] In formula (7), This represents the maximum permissible deviation in the posture of the in vivo microrobot. express The probability of an event occurring is given by the left side of the inequality, which represents the probability of the microrobot's posture deviation exceeding the limit; p represents the probability that the microrobot's posture deviation exceeds the maximum allowable deviation. This chance constraint is transformed into a hard constraint condition that can be solved by a genetic algorithm based on the posture deviation statistics obtained from Monte Carlo simulation.

[0141] Magnetic field array coil current amplitude constraint:

[0142] (8)

[0143] In formula (8), and These are the maximum forward and reverse current thresholds for the magnetic field array coils, respectively.

[0144] The minimum pose hold time constraint for in vivo microrobots:

[0145] (9)

[0146] (10)

[0147] In formulas (9) and (10), and These represent the duration of the forward and reverse postures maintained by the microrobot within the body, respectively. and The minimum posture holding time of the in vivo microrobot is represented by formulas (11) and (12), respectively.

[0148] (11)

[0149] (12)

[0150] In formulas (11) and (12), The angle for adjusting the maximum posture of a microrobot in vivo in a single operation; This represents the maximum permissible angular velocity for a microrobot within the body.

[0151] Safety distance constraints for in vivo microrobots on the stomach wall:

[0152] (13)

[0153] In formula (13), The minimum distance between the in vivo microrobot and the stomach wall during the scheduling period t can be calculated using formula (14):

[0154] (14)

[0155] In formula (14), The spatial position of the in vivo microrobot calculated by visual SLAM during the scheduling period t; The point cloud coordinates of the three-dimensional map of the stomach wall during the scheduling period t; and These represent the minimum and maximum safe distances allowed for the microrobots inside the body, respectively.

[0156] The solution steps for the in vivo microrobot attitude control optimization model described in step S3 are as follows:

[0157] S3.1: Set the genetic algorithm parameters, including: population size Npop, crossover rate Pc, mutation rate Pm, and maximum number of generations Gmax;

[0158] S3.2: Randomly generate an initial population consisting of Npop chromosomes; the chromosomes in the initial population consist of m code points, where code point i is "1", indicating that the magnetic field array is in attitude adjustment state during the i-th scheduling period; and code point i is "-1", indicating that the magnetic field array is in attitude maintenance state during the i-th scheduling period (i=1, 2, …, m).

[0159] S3.3: The generation index g is initialized to 0, that is, g=0;

[0160] S3.4: Let g = g + 1, calculate the fitness of the chromosome in the g-th generation, and initialize the chromosome index k to 1, that is, let k = 1;

[0161] S3.5: Decode the k-th chromosome in the current population to determine the attitude adjustment sequence of the magnetic field array during the scheduling day;

[0162] If the in vivo microrobot does not meet the minimum posture maintenance time constraints given by formulas (9) and (10), then the fitness of the kth chromosome is calculated according to formula (15). Then proceed to step S3.8;

[0163] (15)

[0164] If the in vivo microrobot satisfies the minimum attitude maintenance time constraints given by formulas (9) and (10), then the initial planned coil current of the magnetic field array in each scheduling period is calculated based on the attitude adjustment sequence of the magnetic field array scheduling day.

[0165] Continue with step S3.6;

[0166] S3.6: Use the Monte Carlo simulation method to simulate the operating conditions of the in-vivo micro-robot within the scheduling day, and iteratively update the planned coil current of the magnetic field array according to the simulation results to obtain the optimal planned control sequence of the magnetic field array;

[0167] S3.7: According to the optimal planned control sequence of the magnetic field array, simulate the operating conditions of the in-vivo micro-robot within the scheduling day, calculate the attitude tracking accuracy and gastric cavity scanning coverage rate of the in-vivo micro-robot, and calculate the fitness of the k-th chromosome according to formula (16) :

[0168] (16)

[0169] In formula (16), is the penalty coefficient given in advance;

[0170] S3.8: Determine whether the fitness calculation of all chromosomes in the current population is completed, that is, determine whether k is equal to Npop; if k < Npop, then set k = k + 1 and jump to step S3.5; otherwise, continue to execute the next step S3.9;

[0171] S3.9: Determine whether the maximum number of generations of evolution is reached, that is, determine whether the generation index g is equal to Gmax; if g = Gmax, then continue to execute step S3.10; otherwise, based on the fitness, perform replication, crossover, and mutation operations on the current population, update the chromosome population, and jump to step S3.4;

[0172] S3.10: Output the control scheme of the in-vivo micro-robot corresponding to the chromosome with the highest fitness in the current population as the optimal scheme, and end the algorithm process.

[0173] The chromosome population initialization step described in step S3.2 is specifically as follows:

[0174] S3.2.1: Randomly determine the initial working state of the magnetic field array. For the convenience of description, assume that the initial state of the magnetic field array is the attitude adjustment state, that is, in the chromosome, the first binary code bit takes the value of "1"; the index n is initialized to 1, that is, set n = 1;

[0175] S3.2.2: Randomly generate an integer uniformly distributed in the interval , and initialize the first to the -th code bits of the chromosome to "1", that is, from scheduling period 1 to scheduling period , the magnetic field array is in the attitude adjustment state;

[0176] S3.2.3: If the remaining length of the chromosome Less than the shortest posture holding time of the in vivo microrobot If the remaining code points of the chromosome are initialized to "-1", jump to S3.2.5; otherwise, randomly generate an interval. integers that follow a uniform distribution , the first chromosome To the All code bits are set to "-1", meaning they are used from the scheduling period. to The magnetic field arrays are all in an attitude-maintaining state; let n = n + 1;

[0177] S3.2.4: If the remaining length of the chromosome Less than the shortest posture holding time of the in vivo microrobot If the condition is met, all remaining code points on the chromosome are initialized to "1", and the process jumps to S3.2.5; otherwise, a random interval is generated. integers that follow a uniform distribution Next, the first chromosome To the All code bits are set to "1", meaning from the scheduling period to The magnetic field arrays are all in attitude adjustment state; let n=n+1, and execute S3.2.3;

[0178] S3.2.5: Repeat the above steps until the entire initial chromosome population is generated.

[0179] The steps in step S3.5, which involve decoding the k-th chromosome in the current population to determine the initial planned coil current of the magnetic field array in each scheduling period, are as follows:

[0180] S3.5.1: Decode the kth chromosome in the current population to determine the working status of the magnetic field array in each scheduling period;

[0181] S3.5.2: Calculate the number of consecutive working periods experienced by the magnetic field array within a scheduling day; assume that the magnetic field array experiences a total of J consecutive working periods within a scheduling day, and the j-th consecutive working period consists of Tc,j scheduling periods, with the initial scheduling period index being tst,j and the final scheduling period index being tend,j (j=1,2,…,J);

[0182] S3.5.3: Calculate the initial planned coil current of the magnetic field array in each scheduling period during the J consecutive working periods; if the j-th consecutive working period is an attitude adjustment period, then the initial planned coil current for each scheduling period is:

[0183] (17)

[0184] If the j-th consecutive working period is an attitude maintenance period, then the initial planned coil current for each scheduling period is:

[0185] (18)

[0186] In formulas (17) and (18), Let j be the target Euler angle attitude during the j-th consecutive working time period. If j=1, then Let j be the initial target posture of the in vivo microrobot throughout the entire scheduling day; if j > 1, and the j-th consecutive working period is a posture adjustment period, then Let j be the target attitude corresponding to the reference path; if j > 1, and the j-th consecutive working period is the attitude maintenance period, then ; This refers to the magnetic field-attitude conversion coefficient. The constant coil current required to maintain attitude.

[0187] Step S3.6, which involves simulating the intraday operation of the in vivo microrobot using Monte Carlo simulation and iteratively updating the planned coil current of the magnetic field array based on the simulation results to obtain the optimal planned control sequence for the magnetic field array, is as follows:

[0188] S3.6.1: Initialize all evaluation metrics for measuring the real-time control performance of the in vivo microrobot to zero, i.e.:

[0189] (19)

[0190] In formula (19), , and The values ​​of the three real-time evaluation metrics, namely, Pose Tracking Accuracy (PTA), Cavity Scanning Rate (CSR), and Safety Boundary Degree (SBD), for the in vivo microrobot during the scheduling period t are:

[0191] S3.6.2: Initialize the scheduling period index t to 1; obtain the working state of the in vivo microrobot in the initial period according to the working sequence of the magnetic field array determined by chromosome decoding; if the in vivo microrobot is in the posture adjustment state in the initial period, set its initial pose as the reference pose for visual SLAM solution; if the in vivo microrobot is in the posture holding state in the initial period, set its initial pose as the target pose optimized in the previous cycle.

[0192] S3.6.3: Based on the intragastric image at time t, determine the visual SLAM feature distribution parameters and pose calculation constraint parameters; In this invention, a Gaussian distribution is used to describe the random fluctuation characteristics of the actual pose near its SLAM solution value. The probability density function f(x) and cumulative probability distribution function F(x) of this distribution are shown in the following equations:

[0193] (20)

[0194] (twenty one)

[0195] The pose fluctuation characteristics are closely related to the motion speed. Therefore, when the motion speed is different, the shape parameters μ and σ in the Gaussian distribution should take different values.

[0196] S3.6.4: Generate random pose deviations that follow a Gaussian distribution, i.e.:

[0197] (twenty two)

[0198] In formula (22), The random pose deviation during the scheduling period t; The pose calculated by visual SLAM for the scheduling period t; function is the inverse function of the cumulative probability distribution function of the Gaussian distribution; c is a random number that follows a uniform distribution in the interval [0,1].

[0199] S3.6.5: Treat the random pose deviation generated in step S3.6.4 as real-time interference, and determine the real-time coil current I of the magnetic field array in the current time period according to equation (23). t The attitude tracking benefit and attitude deviation penalty cost are determined according to equations (2) and (3) respectively.

[0200] (twenty three)

[0201] S3.6.6: Based on the calculation results of step S3.6.5, update the values ​​of indicators PTA and CSR during the scheduling period t using the following formula:

[0202] (twenty four)

[0203] In formula (24), nsim is the number of Monte Carlo simulations given in advance;

[0204] Regarding the indicator SBD, if the in vivo microrobot meets the safe distance constraint from the stomach wall during this period, the indicator is updated according to the following formula:

[0205] (25)

[0206] S3.6.7: Calculate the real-time pose and Euler angle orientation of the microrobot in the body at the current time period according to the visual SLAM model;

[0207] S3.6.8: Let t = t + 1, and repeat steps S3.6.3 to S3.6.7 until t = m;

[0208] S3.6.9: Repeat steps S3.6.2 to S3.6.8 until the maximum number of Monte Carlo simulations nsim is reached;

[0209] S3.6.10: If the control performance indicators of the in vivo microrobot meet the clinical requirements in all time periods, i.e., satisfy the constraints given by formula (6), the simulation ends; if the clinical requirements are not met in some time periods, it indicates that the planned coil current of the magnetic field array is set too high in these time periods. In this case, the planned coil current of these time periods can be corrected by the following formula:

[0210] (26)

[0211] In formula (26), γ is the planned coil current of the magnetic field array after correction at time period t; γ is the pre-defined correction coefficient; Ω is the set of scheduling time periods that do not meet clinical requirements; when correcting the planned coil current of time periods that do not meet requirements, attention should be paid to the range of values ​​for the planned coil current; if the magnetic field array is in reverse adjustment state, the range of its planned coil current is... Conversely, if the magnetic field array is in a positive adjustment state, then the range of its planned coil current values ​​is: After the planned coil current is corrected, jump to S3.6.1 to continue the simulation; the maximum number of corrections is set to 20. If the constraint is still not met after exceeding the upper limit, it is determined to be an infeasible solution.

[0212] The target Euler angle pose is generated based on the traversable path in the stomach cavity, and the method for obtaining the traversable path is as follows:

[0213] S7.1: Based on the 3D point cloud map of the stomach wall output by visual SLAM, statistical filtering and voxel downsampling are performed to construct a 3D grid map of the stomach cavity, and the grid resolution is matched with the size of the micro-robot in the body.

[0214] S7.2: Based on the minimum safe distance from the stomach wall Morphological dilation was performed on the gastric wall point cloud, and obstacle areas were marked in the raster map to extract the free passage space of the gastric cavity.

[0215] S7.3: Using the current pose of the in vivo microrobot calculated by visual SLAM as the starting point of the path, and the preset key pose sequence of the whole gastric cavity without blind spots as the ending point of the path, the improved RRT* algorithm is used to perform global path planning, generate a globally passable path, and the target Euler angle pose sequence of the corresponding nodes of the path.

[0216] The steps to improve the RRT* algorithm are as follows:

[0217] Spatial initialization: Based on the three-dimensional grid map of the gastric cavity, the free and passable area is defined as the effective sampling space. The current pose of the microrobot in the body is used as the root node to initialize the path search random tree.

[0218] Targeted sampling: A target bias sampling mechanism is introduced to prioritize sampling in key pose regions of the gastric cavity during scanning, while selecting the node in the random tree that is closest to the sampling point as the extended parent node;

[0219] Safety expansion: The expansion step size is dynamically adjusted in combination with the stomach wall safety distance constraint to generate candidate new nodes and complete collision verification. After the verification is passed, the new node is added to the random tree.

[0220] Path optimization: Optimize node connectivity through neighborhood rewiring to reduce global path cost and ensure path convergence to near-optimal solution;

[0221] Attitude calculation: When the random tree expands to the target pose neighborhood, a globally passable path is generated by backtracking from the endpoint to the starting point, and the target Euler angle attitude sequence corresponding to each node of the path is calculated simultaneously.

[0222] S7.4: Based on real-time acquired intragastric images and SLAM pose calculation results, the dynamic disturbance of gastric wall peristalsis is sensed, and the artificial potential field method is used to perform local obstacle avoidance correction on the global passable path. The passable path and the corresponding target Euler angle attitude sequence are updated in real time and output to the control loop.

[0223] This embodiment uses a model predictive control (MPC) controller to achieve closed-loop control of the posture of an in vivo microrobot. The specific implementation steps of the MPC controller are as follows:

[0224] S8.1: Establish a discretized state-space model of the attitude dynamics of the in vivo microrobot, using the Euler angles and Euler angle angular velocities of the three axes of the in vivo microrobot as state variables:

[0225] The control variable is a three-axis Euler angle and angular velocity control command. Construct discrete state equations:

[0226]

[0227] In the formula, A is the state matrix and B is the control input matrix, which is pre-calibrated based on the rotational inertia, magnetic torque coefficient and damping coefficient of the microrobot body.

[0228] S8.2: Set the MPC prediction time domain Control Time Domain The target Euler angle attitude sequence corresponding to the passable path is used as the tracking reference value. With the goal of maximizing the overall control benefit as in formula (1), a finite-time open-loop optimization objective function is constructed.

[0229] S8.3: The attitude deviation constraints, coil current constraints, shortest attitude holding time constraints, and stomach wall safety distance constraints in formulas (7)-(13) are transformed into inequality constraints for the MPC optimization problem.

[0230] S8.4: Rolling solution of constrained quadratic programming problem to obtain the optimal angular velocity control sequence in the control time domain, and take the first control quantity of the sequence as the final control command to be issued to the external magnetic field array;

[0231] S8.5: At the next scheduling moment, based on the real-time pose feedback of the in vivo microrobot output by visual SLAM, update the state variables and reference sequence, repeat steps S8.2-S8.4, and realize closed-loop attitude control with rolling time domain optimization.

[0232] S8.6: The optimal control sequence output by the model predictive control is issued as the final execution command; the angular velocity command output by the supervised learning visual servo unit is used as a feedforward compensation term and incorporated into the MPC optimization objective function; the global plan sequence obtained by the genetic algorithm is used as the reference benchmark for the MPC prediction time domain.

[0233] As can be seen from the above embodiments, this invention constructs a full-link autonomous control system consisting of "visual SLAM steady-state pose calculation—supervised learning visual servoing—genetic algorithm global planning—Monte Carlo safety verification—MPC rolling optimization closed-loop control." By deeply integrating the fast feedforward response of supervised learning, the global optimization capability of genetic algorithms, and the multi-constraint rolling optimization of MPC, the system achieves accurate tracking and smooth control of Euler angle pose in the complex environment of low texture, weak illumination, and dynamic peristaltic disturbances within the stomach, while ensuring that the in vivo microrobot maintains a clinically safe distance from the stomach wall. Compared with traditional manual operation modes and predefined trajectory open-loop control schemes, this invention does not require additional external positioning equipment. It forms a complete closed loop from image acquisition to control command issuance, significantly reducing the dependence on physician experience. While ensuring full coverage of the gastric cavity, it effectively suppresses the spin jitter of the in vivo microrobot and the alignment deviation of the target area, providing a highly robust and safe system-level solution for fully autonomous clinical examination of in vivo microrobots.

[0234] Example 2: This example proposes an in vivo microrobot posture autonomous control system based on visual SLAM, including:

[0235] The in-body micro-robot module is equipped with a built-in micro-camera, lighting unit, wireless transmission unit and cylindrical permanent magnet. It is used to acquire gastroscopy images in real time and send the images to the visual perception and decision-making module through the wireless transmission unit. It can complete multi-degree-of-freedom posture adjustment under the drive of an external magnetic field.

[0236] The visual perception and decision-making module is used to receive images inside the stomach. It has a built-in lightweight visual SLAM processing unit, a supervised learning visual servo unit, and an instruction safety verification unit. It is used to combine keyframe short window optimization and stomach geometric prior constraints to solve the Euler angle posture sequence of the microrobot inside the body, output angular velocity control instructions through the supervised learning model, and complete the safety verification of the instructions.

[0237] The external magnetic field array control module includes multiple sets of electromagnetic coils, power drive circuits, and a lower-level control unit. It is used to receive angular velocity control commands, calculate them into coil drive currents, generate controlled magnetic fields and magnetic torques, and realize closed-loop attitude control of the in vivo microrobot.

[0238] The system adopts a four-layer collaborative architecture: front-end image acquisition and preprocessing, mid-layer visual SLAM pose calculation, upper-layer instruction generation and visual servoing, and lower-layer magnetic control drive and closed-loop feedback.

[0239] The front-end image acquisition and preprocessing submodule is integrated into the in vivo microrobot module and is used to complete real-time acquisition, noise reduction, anti-glare removal and contrast enhancement preprocessing of images inside the stomach.

[0240] The mid-level visual SLAM pose calculation submodule is integrated into the visual perception and decision-making module. It is used to complete the stable calculation of the six-degree-of-freedom pose of the in vivo microrobot in the gastric environment and provide pose feedback.

[0241] The upper-level instruction generation and visual servoing submodule is integrated into the visual perception and decision-making module and is used to complete the generation, fusion and security verification of angular velocity control instructions.

[0242] The underlying magnetic control drive submodule is integrated into the external magnetic field array control module. It is used to complete the precise output of coil current and magnetic field drive, and realize the closed-loop control of the posture of the in vivo micro-robot.

[0243] This invention achieves high-precision and robust autonomous posture control of an in vivo microrobot in the complex dynamic environment of the stomach by fusing visual SLAM pose calculation with deep closed-loop driven by a magnetic array, without the need for additional external positioning equipment. Under the premise of meeting the safety constraints of clinical diagnosis and treatment, it significantly reduces the dependence of the operation of the in vivo microrobot on the doctor's experience, improves the alignment accuracy of the target area, the stability of posture tracking, and the coverage of the examination area in the stomach, and provides core technical support for the fully autonomous clinical examination of the in vivo microrobot.

[0244] It should be noted that the processing flow of Embodiment 2 corresponds to the specific steps of the method provided in Embodiment 1 of the present invention, and has the corresponding functional modules and beneficial effects of the method. Technical details not described in detail in this embodiment can be found in the method provided in Embodiment 1 of the present invention.

[0245] The program code used to implement the methods of this application may be written in any combination of one or more programming languages. This program code may be provided to a processor or controller of a general-purpose computer, special-purpose computer, or other programmable data processing device, such that when executed by the processor or controller, the functions / operations specified in the flowcharts and / or block diagrams are implemented. The program code may be executed entirely on a machine, partially on a machine, as a standalone software package partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0246] The specific implementation schemes described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific implementation schemes of the present invention and are not intended to limit the scope of the present invention. Any equivalent changes and modifications made by those skilled in the art without departing from the concept and principles of the present invention should fall within the scope of protection of the present invention.

Claims

1. A method for autonomous posture control of an in vivo microrobot based on visual SLAM, characterized in that, Includes the following steps: S1. Given the constraints of the in vivo environment, the hardware parameters of the in vivo microrobot and magnetic control system, the control parameters of visual SLAM and supervised learning, the number of scheduling periods and the maximum number of iterations; S2. Based on the intragastric images acquired by the in vivo microrobot, visual SLAM is used in combination with keyframe short window optimization and geometric prior constraints to solve the Euler angle pose sequence of the in vivo microrobot; at the same time, a supervised learning strategy is constructed with the mapping from the current frame image to the Euler angle angular velocity command as the learning objective to generate visual servo control commands for the in vivo microrobot. S3. Based on the Euler angle and angular velocity command, calculate the coil driving current of the peripheral magnetic field array, drive the peripheral magnetic field array to control the attitude of the in-body microrobot, establish an optimized attitude control model for the in-body microrobot, and solve the optimized model based on a genetic algorithm to achieve closed-loop control of the attitude of the in-body microrobot.

2. The method according to claim 1, characterized in that, The optimization model described in step S3 aims to maximize the gains in attitude tracking accuracy and gastric scan coverage, and minimize the penalty costs for attitude deviation and safety constraint violations, and includes at least the following constraints: (1) Attitude deviation probability constraint: The probability that the deviation between the actual Euler angle attitude of the in vivo microrobot and the target Euler angle attitude exceeds the preset maximum allowable deviation does not exceed the set value; (2) Coil current amplitude constraint: The coil current in each scheduling period shall not exceed the preset safety threshold; (3) Minimum attitude holding time constraint: The duration of the in vivo microrobot after attitude adjustment in both forward and reverse directions shall not be less than the corresponding minimum attitude holding time; (4) Stomach wall safety distance constraint: The minimum distance between the in vivo microrobot and the stomach wall is kept within the preset safety distance range.

3. The method according to claim 2, characterized in that, Solving the in vivo microrobot attitude control optimization model specifically includes the following steps: S3.1 Set the genetic algorithm parameters, including: population size Npop, crossover rate Pc, mutation rate Pm and maximum number of generations Gmax; S3.2 Randomly generate an initial population consisting of Npop chromosomes; the chromosomes in the initial population consist of m code points. If the code point i is 1, it means that the magnetic field array is in the attitude adjustment state during the i-th scheduling period; if it is -1, it means that the magnetic field array is in the attitude maintenance state during the i-th scheduling period. S3.

3. Initialize the generation index g to 0; S3.4 Let g = g + 1, calculate the fitness of the chromosome in the g-th generation, and initialize the chromosome index k to 1; S3.

5. Decode the k-th chromosome in the current population to determine the attitude adjustment sequence of the magnetic field array within the scheduling day; if the in vivo microrobot does not meet the shortest attitude maintenance time constraint, calculate the fitness of the k-th chromosome according to the preset penalty function. Then proceed to step S3.8; If the in vivo microrobot meets the minimum attitude maintenance time constraint, then calculate the initial planned coil current of the magnetic field array in each scheduling period according to the attitude adjustment sequence of the magnetic field array scheduling day, and continue to execute step S3.6; S3.

6. The Monte Carlo simulation method is used to simulate the daily operation of the in vivo microrobot. Based on the simulation results, the planned coil current of the magnetic field array is iteratively updated to obtain the optimal planned control sequence of the magnetic field array. S3.

7. Based on the optimal planned control sequence of the magnetic field array, simulate the operation of the in vivo microrobot during the scheduling day, calculate the attitude tracking accuracy and gastric cavity scan coverage of the in vivo microrobot, and calculate the fitness of the k-th chromosome according to the preset fitness function. : S3.

8. Determine whether the fitness calculation of all chromosomes in the current population is completed, that is, determine whether k is equal to Npop; if k < Npop, then set k = k + 1 and jump to step S3.5; otherwise, continue to execute step S3.9; S3.

9. Determine whether the maximum number of generations of evolution has been reached; if g = Gmax, then execute step S3.10; otherwise, based on the fitness, perform replication, crossover and mutation operations on the current population, update the chromosome population, and jump to step S3.4; S3.

10. Output the control scheme of the in-vivo micro-robot corresponding to the chromosome with the highest fitness in the current population as the optimal scheme, and end the algorithm process.

4. The method according to claim 3, characterized in that, Step S3.2 specifically includes the following steps: S3.2.

1. Set the initial working state of the magnetic field array to the attitude adjustment state, and set the value of the first code bit in the chromosome to 1; S3.2.

2. Randomly generate a length of the attitude adjustment period, and initialize the first several code bits of the chromosome to 1; S3.2.

3. Determine whether the remaining length of the chromosome is less than the preset shortest attitude holding time. If so, initialize the remaining code bits to -1 and end the initialization of the current chromosome; if not, randomly generate a length of the attitude holding period, and initialize the subsequent corresponding number of code bits of the chromosome to -1; S3.2.

4. Again, determine whether the remaining length of the chromosome is less than the preset shortest attitude adjustment time. If so, initialize the remaining code bits to 1 and end the initialization of the current chromosome; if not, randomly generate a length of the attitude adjustment period, and initialize the subsequent corresponding number of code bits of the chromosome to 1, and return to step S3.2.3; S3.2.

5. Repeat the above steps until a complete chromosome is generated and an initial population is formed.

5. The method according to claim 3, characterized in that, The specific steps for calculating the initial planned coil current of the magnetic field array in each scheduling period according to the attitude adjustment sequence within the magnetic field array scheduling day in step S3.5 are as follows: S3.5.

1. Decode the k-th chromosome in the current population to determine the working state of the magnetic field array in each scheduling period; S3.5.

2. Count the continuous working periods experienced by the magnetic field array within the scheduling day, and determine the start and end scheduling periods of each continuous working period; S3.5.

3. Calculate the initial planned coil current of each continuous working period in turn: If the current continuous working period is an attitude adjustment period, calculate the initial planned coil current of each scheduling period according to the target Euler angle attitude of this period and the preset magnetic field-attitude conversion coefficient; If the current continuous working period is an attitude holding period, set the initial planned coil current of each scheduling period to the constant current value required for attitude holding.

6. The method according to claim 3, characterized in that, The specific steps for simulating the operating conditions of the in-vivo micro-robot within the scheduling day by using the Monte Carlo simulation method, and iteratively correcting the planned coil current of the magnetic field array according to the simulation results to obtain the optimal planned control sequence of the magnetic field array are as follows: S3.6.

1. Initialize the real-time control performance evaluation indexes of the in-vivo micro-robot, including attitude tracking accuracy, gastric cavity scanning coverage rate and safety boundary fitting degree; S3.6.

2. Determine the pose state of the in vivo microrobot in the initial period based on the working sequence of the magnetic field array determined by chromosome decoding. S3.6.

3. Based on the intragastric image of the current time period, determine the feature distribution and pose calculation constraint parameters of visual SLAM, and use Gaussian distribution to describe the random fluctuation of the actual pose around its SLAM solution value. S3.6.4 Generate random pose deviations that follow the above Gaussian distribution as real-time disturbances for the current time period; S3.6.

5. Based on the random pose deviation of the current time period, determine the real-time coil current of the magnetic field array, and calculate the attitude tracking benefit and attitude deviation penalty cost. S3.6.6 Update the attitude tracking accuracy and gastric cavity scan coverage indicators, and update the safety boundary fit indicator when the gastric wall safety distance constraint is met; S3.6.

7. Based on the visual SLAM model, calculate the real-time pose and Euler angles of the microrobot in the body during the current time period. S3.6.

8. Enter the next scheduling period and repeat S3.6.3 to S3.6.7 until one simulation run of all scheduling periods is completed; S3.6.9 Repeat steps S3.6.2 to S3.6.8 until the preset number of Monte Carlo simulations is reached; S3.6.10 Determine whether the control performance indicators of each scheduling period meet the clinical requirements. If all meet the requirements, end the simulation. If there are scheduling periods that do not meet the requirements, correct the planned coil current of these periods according to the preset correction coefficient, and return to step S3.6.1 to re-simulate until all periods meet the clinical requirements or the maximum number of corrections is reached.

7. The method according to claim 2, characterized in that, The target Euler angle pose is generated based on the accessible path in the stomach cavity, specifically including the following steps: Based on the 3D point cloud map of the stomach wall output by visual SLAM, statistical filtering and voxel downsampling are performed to construct a 3D grid map of the stomach cavity that matches the size of the in vivo microrobot. Based on the preset minimum safe distance of the stomach wall, the stomach wall point cloud is subjected to morphological dilation processing, the obstacle area is marked in the grid map, and the free passable space of the stomach cavity is extracted. Using the current pose of the in vivo microrobot calculated by visual SLAM as the starting point of the path, and the preset key pose sequence of the whole gastric cavity without blind spots as the ending point of the path, the improved RRT* algorithm is used to generate a globally passable path, and the target Euler angle pose sequence corresponding to each node on the path is calculated. Based on real-time acquired intragastric images and visual SLAM pose calculation results, the dynamic disturbance of gastric wall peristalsis is perceived. A local obstacle avoidance correction algorithm is used to correct the global passable path in real time, and the passable path and the corresponding target Euler angle pose sequence are dynamically updated.

8. The method according to claim 2, characterized in that, A model predictive controller is used to achieve closed-loop control of the posture of an in vivo microrobot, specifically including the following steps: A discretized state-space model of the attitude dynamics of an in vivo microrobot is established, with the three-axis Euler angles and Euler angle angular velocities of the in vivo microrobot as state variables and the three-axis Euler angle angular velocity control commands as control variables. The prediction time domain and control time domain of model predictive control are defined. The target Euler angle attitude sequence is used as the tracking reference value, and the optimization objective is to maximize the overall control benefit. A finite time domain open-loop optimization objective function is constructed. The attitude deviation constraint, coil current constraint, shortest attitude holding time constraint, and stomach wall safety distance constraint are transformed into inequality constraints for the model predictive control optimization problem. The constrained quadratic programming problem is solved by rolling the solution to obtain the optimal angular velocity control sequence in the control time domain. The first control variable in the sequence is taken as the final control command and sent to the external magnetic field array. At the next scheduling moment, based on the real-time pose feedback of the in vivo microrobot output by visual SLAM, the state variables and reference sequence are updated, and the above steps are repeated to achieve closed-loop attitude control with rolling time domain optimization. The optimal control sequence output by model predictive control is used as the final execution instruction, the angular velocity instruction output by supervised learning visual servoing is used as a feedforward compensation term and incorporated into the optimization objective function, and the global plan sequence solved by the genetic algorithm is used as the reference benchmark for the prediction time domain of model predictive control.

9. A vision-SLAM-based in vivo microrobot posture autonomous control system, characterized in that, include: The in-body micro-robot module, which incorporates a micro-camera, an illumination unit, a wireless transmission unit, and a cylindrical permanent magnet, is used to acquire gastroscopic images in real time and transmit the images to the visual perception and decision-making module via the wireless transmission unit. It can perform multi-degree-of-freedom posture adjustments under the drive of an external magnetic field. The visual perception and decision-making module is used to receive images inside the stomach. It has a built-in lightweight visual SLAM processing unit, a supervised learning visual servo unit, and an instruction safety verification unit. It is used to combine keyframe short window optimization and stomach geometric prior constraints to solve the Euler angle posture sequence of the microrobot inside the body, output angular velocity control instructions through the supervised learning model, and complete the safety verification of the instructions. The external magnetic field array control module includes multiple sets of electromagnetic coils, power drive circuits, and a lower-level control unit. It is used to receive angular velocity control commands, calculate them into coil drive currents, generate controlled magnetic fields and magnetic torques, and realize closed-loop attitude control of the in vivo microrobot.

10. The system according to claim 9, characterized in that, The system adopts a four-layer collaborative architecture: front-end image acquisition and preprocessing, mid-layer visual SLAM pose calculation, upper-layer instruction generation and visual servoing, and lower-layer magnetic control drive and closed-loop feedback. The front-end image acquisition and preprocessing submodule is integrated into the in vivo microrobot module and is used to complete real-time acquisition, noise reduction, anti-glare removal and contrast enhancement preprocessing of images inside the stomach. The mid-level visual SLAM pose calculation submodule is integrated into the visual perception and decision-making module. It is used to complete the stable calculation of the six-degree-of-freedom pose of the in vivo microrobot in the gastric environment and provide pose feedback. The upper-level instruction generation and visual servoing submodule is integrated into the visual perception and decision-making module and is used to complete the generation, fusion and security verification of angular velocity control instructions. The underlying magnetic control drive submodule is integrated into the external magnetic field array control module. It is used to complete the precise output of coil current and magnetic field drive, and realize the closed-loop control of the posture of the in vivo micro-robot.

Images