Magnetic driven enteroscope robot vision servo control method based on jacobian matrix estimation

Abstract

The application discloses a magnetic-driven enteroscope robot visual servo control method based on Jacobian matrix estimation, which comprises the following steps: S1: through image processing technology, target feature image pixel coordinates in the enteroscope field of view are extracted; S2: a magnetic-driven enteroscope robot system kinematics model is established, and an interactive Jacobian matrix is adaptively estimated on line by using a gradient neural network according to the target feature image pixel coordinates; S3: according to the interactive Jacobian matrix, a visual servo controller is designed by using a zeroization neural network, a quadratic programming control framework is constructed in combination with mechanical arm joint position and speed limit constraints, and an optimal joint speed instruction is output to drive the mechanical arm, so that accurate visual servo control of the enteroscope on the target image feature is realized. The magnetic-driven enteroscope robot visual servo control method based on Jacobian matrix estimation can estimate the interactive Jacobian matrix on line to overcome model uncertainty, satisfy joint limit constraints, and realize safe and accurate visual servo control.

Description

Technical Field

[0001] This invention relates to the field of medical robot control, and more specifically to a visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation. Background Technology

[0002] A magnetically driven colonoscopy robot system is a medical device that uses a large external magnet to drive and control the movement of a colonoscope with small magnets embedded at its end, enabling non-invasive and flexible intestinal examinations. Integrating visual servo control into the magnetically driven colonoscopy robot allows for various automated functions such as automatic polyp tracking and autonomous navigation, significantly reducing the difficulty of operating the colonoscopy robot and the workload for doctors.

[0003] Traditional visual servo control heavily relies on accurate system kinematic models, particularly the interactive Jacobian matrix. This matrix encompasses the complete mapping from the motion of the driving magnet to changes in image pixels, including magnetic field actuation, coordinate transformation, and camera projection models. However, in magnetically driven colonoscopy robot systems, magnetic field coupling exhibits strong nonlinearity, and camera intrinsic parameters may contain uncalibrated errors, making it difficult to obtain an accurate interactive Jacobian matrix. Using an inaccurate model reduces control precision and can even lead to task failure.

[0004] Furthermore, traditional visual servoing control algorithms suffer from slow convergence speed and insufficient anti-interference capability. In the complex and dynamic intestinal environment, it is difficult to guarantee fast and stable visual servoing convergence. Simultaneously, to ensure the safe operation of the robot, the joint positions and speeds of the robotic arm must be limited within physically permissible ranges during the visual servoing control of the magnetically driven colonoscopy robot. Existing methods rarely integrate these hard constraints with the visual servoing task, which involves model uncertainties, thus limiting their safety and practicality in complex clinical environments.

[0005] Therefore, there is an urgent need for a visual servo control method for magnetically driven colonoscopy robot systems, which can estimate unknown or uncertain system interaction Jacobian matrices online, strictly meet the limit constraints of the robot arm joint position and velocity, and achieve fast and stable visual servo convergence. Summary of the Invention

[0006] To overcome the shortcomings of the prior art, the present invention aims to provide a visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation. This method can estimate the interactive Jacobi matrix online to overcome model uncertainty and meet joint limit constraints, thereby achieving safe and accurate visual servo control.

[0007] To achieve the objective of this invention, the following solution is adopted: A visual servo control method for a magnetically driven colonoscopy robot based on Jacobian matrix estimation includes the following steps: S1: Extract the pixel coordinates of the target feature image in the colonoscopy field of view using image processing technology; S2: Establish a kinematic model of the magnetically driven colonoscopy robot system, and use a gradient neural network to online adaptively estimate the interactive Jacobian matrix based on the pixel coordinates of the target feature image; S3: Based on the interactive Jacobian matrix, a visual servo controller is designed using a null neural network. Combined with the joint position and speed limit constraints of the robotic arm, a quadratic programming control framework is constructed to output the optimal joint speed command to drive the robotic arm and achieve precise visual servo control of the colonoscope on the target image features.

[0008] Further, in step S2, the system kinematic model includes the robotic arm Jacobian matrix, the magnetic drive Jacobian matrix, the image Jacobian matrix, and the coordinate system rotation matrix; The mapping relationships from joint velocity to the spatial velocity of the driving magnet, from the spatial velocity of the driving magnet to the velocity of the colonoscope, and from the velocity of the colonoscope to the velocity of the image pixels are established sequentially, and the kinematic equations of the system are obtained by combining them: in, Image pixel speed, For joint velocity, For the Jacobian matrix of the robotic arm, It is an interactive Jacobian matrix.

[0009] Further, in step S2, the gradient neural network adaptively estimates the interactive Jacobian matrix online in the following way: The sum of squared errors between the actual value of the image pixel velocity and the estimated value calculated based on the estimation matrix is ​​defined as a scalar error function. Based on the gradient descent principle, the estimated matrix is ​​continuously updated along the negative gradient direction of the scalar error function, and the estimated matrix is ​​converged to the true interactive Jacobian matrix through iteration.

[0010] Furthermore, in step S3, the visual servo controller employing a null neural network design defines image errors. ,in Let s be the desired feature location and s be the current image feature location. Construct the error dynamics equation: in, The value represents the rate of change of image error; λ is a positive real number representing the convergence speed parameter. This represents an array containing multiple activation functions; This represents the rate of change of the desired feature location; This represents the current pixel velocity of the image; the equation causes the image error to converge exponentially to zero over time.

[0011] Furthermore, each activation function in the activation function array is: Where κ1, κ2, and κ3 are positive real numbers representing coefficients for adjusting the convergence rate, a is a real number greater than 0 and less than 1, and b and c are powers determined by natural numbers.

[0012] Further, in step S3, the estimated value of the interactive Jacobian matrix is ​​combined with the robotic arm Jacobian matrix to form the system Jacobian matrix, and the rate of change of the desired feature position is combined with the error feedback term to form the control input vector. An equation is then established in which the system Jacobian matrix multiplied by the joint velocity equals the control input vector: Where A represents the system Jacobian matrix, x represents the joint velocity, and b represents the control input vector, this equation serves as an equality constraint in the quadratic programming framework.

[0013] Furthermore, in step S3, the position and speed limit constraints of the robotic arm joints include the lower and upper limits of the angles of each joint, as well as the lower and upper limits of the angular velocities of each joint. All joints must simultaneously satisfy the constraints of the lower and upper limits of the angles and the constraints of the lower and upper limits of the angular velocities during the movement.

[0014] Furthermore, in step S3, an integration function is constructed to uniformly represent the joint position and velocity constraints of the robotic arm at the joint velocity level, thereby obtaining the upper and lower bounds of the joint velocity: Further simplification into inequality constraints: Where x represents the joint velocity. and Let C and d represent the lower and upper bounds of the joint velocity after position constraint transformation, respectively. Let C represent the constraint matrix and d represent the constraint vector.

[0015] Furthermore, in step S3, the quadratic programming control framework is constructed as follows: Where W represents the positive definite weight matrix, A represents the system Jacobian matrix, and b represents the control input vector. The optimal joint speed command is obtained by solving this problem online in each control cycle.

[0016] Furthermore, in step S3, the quadratic programming control framework is solved online in each control cycle, and the optimal joint speed command that simultaneously satisfies the joint position and speed limit constraints of the robotic arm is output to drive the robotic arm, thereby realizing precise visual servo control of the target image features under the condition of model uncertainty in the interactive Jacobian matrix.

[0017] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. This invention innovatively solves the model uncertainty problem in the visual servo control of a magnetically driven colonoscope, significantly improving the system's robustness and practicality. Traditional visual servo control heavily relies on accurate system kinematic models, especially the interactive Jacobian matrix, which integrates magnetic field drive, coordinate transformation, and camera projection. Accurate acquisition of this matrix is ​​difficult and susceptible to interference. This invention introduces a gradient neural network, utilizing real-time measurable joint velocities and image feature velocities to perform online adaptive estimation of the interactive Jacobian matrix. This eliminates the need for tedious pre-calibration of complex magnetic coupling models and camera intrinsic parameters, fundamentally overcoming the risk of decreased control accuracy or even failure caused by model uncertainty, enabling the system to adapt to real clinical environments.

[0018] 2. This invention combines a null-negative neural network to design a visual servo controller with fast convergence and noise resistance, ensuring both speed and robustness of control. Traditional visual servo control algorithms suffer from slow convergence speed and insufficient anti-interference capability, making it difficult to meet the control requirements of complex and dynamic intestinal environments. This invention uses a null-negative neural network to design a visual servo controller, constructing an error dynamics equation to bring the image error exponential to zero, achieving an adjustable and faster convergence speed than traditional proportional control. Simultaneously, the designed activation function further enhances the system's ability to suppress external disturbances, giving the entire control system stronger robustness and adaptability when facing unknown disturbances such as model errors and measurement noise.

[0019] 3. This invention achieves safe and optimal control under strict joint physical constraints. Existing methods often handle visual servo control and joint limit constraints separately, or use simple truncation methods, which can easily lead to robotic arm tremors or even task failure. This invention models the visual servo task and the hard constraints of joint position and velocity as a unified quadratic programming problem. By solving this problem online, it can output the optimal joint velocity command while satisfying all safety constraints. This integrated processing framework ensures that the movement of the magnetically driven colonoscopy robot remains within a safe physical range when performing automated examinations and biopsies, significantly improving the safety of clinical operations. Attached Figure Description

[0020] Figure 1 This is a flowchart of a visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation, as described in an embodiment of the present invention. Figure 2 This is an overview diagram of the magnetically driven colonoscopy robot system in an embodiment of the present invention; Figure 3 This is a kinematic model diagram of the magnetically driven colonoscopy robot system in an embodiment of the present invention; Figure 4 This is a simulation environment diagram of the magnetically driven colonoscopy robot in an embodiment of the present invention; Figure 5 This is a schematic diagram of the motion trajectory of feature points on the camera plane of the magnetically driven colonoscopy robot in an embodiment of the present invention; wherein, the feature points start from the initial position and eventually reach the center of the camera plane; Figure 6 This is a diagram illustrating the error convergence process of the visual servo control of the magnetically driven colonoscopy robot in an embodiment of the present invention. Figure 7 This is a diagram showing the joint angle changes of the simulated UR12e robotic arm in an embodiment of the present invention; Figure 8 This is a graph showing the rate of change of joint angles of the simulated UR12e robotic arm in an embodiment of the present invention. Detailed Implementation

[0021] The present invention will now be further described in conjunction with the accompanying drawings and specific embodiments. It should be noted that, without conflict, the various embodiments or technical features described below can be arbitrarily combined to form new embodiments.

[0022] This invention provides a visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation. This method uses a gradient neural network to adaptively estimate the unknown interactive Jacobi matrix online, uses a null neural network to design the visual servo controller, and combines the limit constraints of the robotic arm joints to construct the visual servo control scheme of the magnetically driven colonoscopy robot as a quadratic programming problem. This enables fast, safe, and robust visual servo control of the magnetically driven colonoscopy robot under the condition of model uncertainty.

[0023] like Figure 1-8 As shown in the figure, the visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to an embodiment of the present invention includes the following steps: S1: Extract the pixel coordinates of the target feature image in the colonoscopy field of view using image processing technology.

[0024] In this embodiment, image processing technology is used to extract, such as Figure 2 The image shown is composed of pixel coordinates of the target feature image in the colonoscopy field of view of the magnetically driven colonoscopy robot system. .

[0025] S2: Establish a kinematic model of the magnetically driven colonoscopy robot system, and use a gradient neural network to adaptively estimate the interactive Jacobian matrix online based on the pixel coordinates of the target feature image.

[0026] In this embodiment, the specific details of the kinematic modeling and online estimation of the interaction Jacobian matrix of the magnetically driven colonoscopy robot system are as follows: Specifically, system kinematic modeling: such as Figure 3 As shown, a complete mapping from the robot joint space to the image feature space is established. The spatial velocity of the driving magnet mounted at the end of the robotic arm is also shown. With the joint speed of the robotic arm The relationship is: in It is the Jacobian matrix of the robotic arm. This represents the number of degrees of freedom of the robotic arm. Based on the principle of magnetic drive, the colonoscope moves... With the movement of the driving magnet The mapping relationship is as follows: in This is the magnetic drive Jacobian matrix. The pixel velocity of the target point in the image plane. With colonoscopy speed The relationship is: in It is the image Jacobian matrix. This is the coordinate system rotation matrix. The combined kinematic equations of the system are: in It is an interactive Jacobian matrix.

[0027] Specifically, the interactive Jacobian matrix estimation involves using a gradient neural network to adaptively estimate the unknown interactive Jacobian matrix online. Define a scalar estimation error function: in for The estimated matrix. Based on the gradient descent principle, the estimated matrix... The update law can be designed as in To adjust the positive real number of the update rate, the kinematic model of the robotic arm is... Substituting into the above equation, we get: S3: Based on the interactive Jacobian matrix, a visual servo controller is designed using a null neural network. Combined with the joint position and speed limit constraints of the robotic arm, a quadratic programming control framework is constructed to output the optimal joint speed command to drive the robotic arm and achieve precise visual servo control of the colonoscope on the target image features.

[0028] In this embodiment, the specific details of the visual servo controller design and the construction of the quadratic programming control framework are as follows: Specifically, the design of the visual servo controller: Define image error as ,in Let be the desired feature location. A nullified neural network design is used to converge the error exponent to zero, specifically expressed as: in, Positive real numbers used to adjust the convergence rate; For functions containing multiple activation functions An array of [data]. To accelerate the visual servo convergence speed of the magnetically driven colonoscopy robot and improve its robustness to external disturbances, the activation function is designed as follows: in, To adjust the positive real number of the convergence rate, , , , It is a natural number.

[0029] Specifically, the construction of the quadratic programming control scheme: The above contains the estimated interaction matrix System kinematic model Substituting the above nullified neural network controller into the equation, we can further simplify to obtain: in, Let Jacobian matrix be the system's Jacobian matrix. For the joint movement speed of the robotic arm, To control the input. To ensure safety, joint positions. and speed Physical limits must be met: Construct an integration function to uniformly represent the above constraints at the joint velocity level: in, in, For soft constraint adjustment parameters, typical parameter settings are: It can be fine-tuned according to the actual parameters of the robotic arm to ensure the smoothness and safety of joint movements. Further refinement yields: in, Based on the above constraints, the joint velocity problem is constructed as a quadratic programming problem. The objective is to find the optimal joint velocity while satisfying the constraints, thereby minimizing the control error. The quadratic programming control scheme is constructed as follows: in, It is a positive definite weight matrix.

[0030] like Figure 4 As shown, the magnetically driven colonoscopy robot system was verified for visual servo control using the above method in a simulation environment.

[0031] like Figure 5 As shown, in the simulation experiment, the feature points start from the initial pixel position, and under the drive of the controller, their motion trajectories can smoothly and quickly converge to the center of the camera plane (i.e., the desired position). This demonstrates that the method can still achieve accurate visual servo alignment even when the model is learning online.

[0032] like Figure 6 As shown, in the simulation experiment, the pixel error of the visual servo control starts from the initial state and converges to zero over time.

[0033] like Figure 7 and Figure 8 As shown, the angles of each joint of the robotic arm during the simulation are displayed. and angular velocity The curves show the changes in angle and angular velocity of all joints. It can be seen that throughout the entire task execution process, the angles and angular velocities of all joints are strictly limited within the preset safety boundaries. This confirms that the quadratic programming control framework can effectively handle joint constraint limitations and ensure the safe operation of the robot.

[0034] The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation in this invention has the following advantages: 1. This invention innovatively solves the problem of model uncertainty, improving the robustness and practicality of the system. Traditional visual servo control heavily relies on accurate system kinematic models, especially the interactive Jacobian matrix integrating magnetic field drive, coordinate transformation, and camera projection, which is difficult to obtain accurately and is easily disturbed. This invention introduces gradient neural networks into the field of magnetically driven colonoscope control for the first time. By designing an update law for the estimation matrix, the estimated Jacobian matrix can converge to the true value using only real-time measurable joint velocities and image feature velocities. This method eliminates the need for tedious pre-calibration of complex magnetic coupling models and camera intrinsic parameters, fundamentally overcoming the risk of decreased control accuracy or even failure caused by model uncertainty, enabling the system to adapt to real clinical environments.

[0035] 2. This invention employs a null-neural network design to create a fast-converging and robust noise-resistant visual servo controller, ensuring rapid convergence and strong anti-interference capabilities. In the controller design, this invention utilizes a null-neural network design formula to construct error dynamics. This design theoretically guarantees that the pixel error exponentially converges to zero, with an adjustable and faster convergence speed than traditional proportional control. Simultaneously, the null-neural network solver used to solve the quadratic rule problem also guarantees convergence. The combination of gradient neural networks and null-neural networks gives the entire control system stronger robustness and adaptability when facing unknown disturbances such as model errors and measurement noise.

[0036] 3. This invention achieves safe and optimal control under strict joint physical constraints. Existing methods often handle visual servo control and joint limit constraints separately, or use simple truncation methods, which may lead to jitter or task failure. This invention unifies the visual servo task with model uncertainties and the hard constraints of joint position and velocity into a quadratic programming problem. By solving this quadratic programming problem, the optimal joint velocity command can be calculated online while satisfying all safety constraints. This integrated framework ensures that the robot's movement remains within a safe physical range when performing automated biopsy tasks, significantly improving operational safety.

[0037] The above is a detailed description of the preferred embodiments of the present invention. However, the present invention is not limited to the embodiments described. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention. All such equivalent modifications or substitutions are included within the scope defined by the claims of this application.

Claims

1. A visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation, characterized in that, Includes the following steps: S1: Extract the pixel coordinates of the target feature image in the colonoscopy field of view using image processing technology; S2: Establish a kinematic model of the magnetically driven colonoscopy robot system, and use a gradient neural network to online adaptively estimate the interactive Jacobian matrix based on the pixel coordinates of the target feature image; S3: Based on the interactive Jacobian matrix, a visual servo controller is designed using a null neural network. Combined with the joint position and speed limit constraints of the robotic arm, a quadratic programming control framework is constructed to output the optimal joint speed command to drive the robotic arm and achieve precise visual servo control of the colonoscope on the target image features.

2. The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to claim 1, characterized in that, In step S2, the system kinematic model includes the robotic arm Jacobian matrix, the magnetic drive Jacobian matrix, the image Jacobian matrix, and the coordinate system rotation matrix; The mapping relationships from joint velocity to the spatial velocity of the driving magnet, from the spatial velocity of the driving magnet to the velocity of the colonoscope, and from the velocity of the colonoscope to the velocity of the image pixels are established sequentially, and the kinematic equations of the system are obtained by combining them: in, Image pixel speed, For joint velocity, For the Jacobian matrix of the robotic arm, It is an interactive Jacobian matrix.

3. The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to claim 1, characterized in that, In step S2, the gradient neural network adaptively estimates the interactive Jacobian matrix online as follows: The sum of squared errors between the actual value of the image pixel velocity and the estimated value calculated based on the estimation matrix is ​​defined as a scalar error function. Based on the gradient descent principle, the estimated matrix is ​​continuously updated along the negative gradient direction of the scalar error function, and the estimated matrix is ​​converged to the true interactive Jacobian matrix through iteration.

4. The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to claim 1, characterized in that, In step S3, the visual servo controller using a zero-negation neural network design defines image errors. ,in Let s be the desired feature location and s be the current image feature location. Construct the error dynamics equation: in, The value represents the rate of change of image error; λ is a positive real number representing the convergence speed parameter. This represents an array containing multiple activation functions; This represents the rate of change of the desired feature location; This represents the current pixel velocity of the image; the equation causes the image error to converge exponentially to zero over time.

5. The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to claim 4, characterized in that, Each activation function in the activation function array is: Where κ1, κ2, and κ3 are positive real numbers representing coefficients for adjusting the convergence rate, a is a real number greater than 0 and less than 1, and b and c are powers determined by natural numbers.

6. The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to claim 1, characterized in that, In step S3, the estimated value of the interactive Jacobian matrix is ​​combined with the robotic arm Jacobian matrix to form the system Jacobian matrix, and the rate of change of the desired feature position is combined with the error feedback term to form the control input vector. An equation is then established in which the system Jacobian matrix multiplied by the joint velocity equals the control input vector. Where A represents the system Jacobian matrix, x represents the joint velocity, and b represents the control input vector, this equation serves as an equality constraint in the quadratic programming framework.

7. The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to claim 1, characterized in that, In step S3, the position and speed limit constraints of the robotic arm joints include the lower and upper limits of the angles of each joint, as well as the lower and upper limits of the angular velocities of each joint. All joints must simultaneously satisfy the constraints of the lower and upper limits of the angles and the constraints of the lower and upper limits of the angular velocities during the movement.

8. The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to claim 7, characterized in that, In step S3, an integration function is constructed to uniformly represent the joint position and velocity constraints of the robotic arm at the joint velocity level, thereby obtaining the upper and lower bounds of the joint velocity: Further simplification into inequality constraints: Where x represents the joint velocity. and Let C and d represent the lower and upper bounds of the joint velocity after position constraint transformation, respectively. Let C represent the constraint matrix and d represent the constraint vector.

9. The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to claim 8, characterized in that, In step S3, the quadratic programming control framework is constructed as follows: Where W represents the positive definite weight matrix, A represents the system Jacobian matrix, and b represents the control input vector. The optimal joint speed command is obtained by solving this problem online in each control cycle.

10. The visual servo control method for a magnetically driven colonoscopy robot based on Jacobi matrix estimation according to claim 1, characterized in that, In step S3, the quadratic programming control framework is solved online in each control cycle, and the optimal joint speed command that simultaneously satisfies the joint position and speed limit constraints of the robotic arm is output to drive the robotic arm, thereby realizing precise visual servo control of the target image features under the condition of model uncertainty in the interactive Jacobian matrix.

Images