A dynamic jacobian qp closed-loop control method and system for a micro-manipulator

Abstract

The application discloses a dynamic Jacobian-QP closed-loop control method and system for a micro manipulator. The method takes external position feedback as a control input, receives a current position and a target position of an end in each control cycle, and calculates a task space error. A desired end correction step is generated by a task space step generation module. A local input-output mapping at the current position is given by a dynamic Jacobian matrix field module. Then, a drive effective increment is solved under the current local Jacobian matrix and the current manipulator individual constraint. Finally, an actual drive command is output after compensation and safety limiting of an execution layer.

Description

Technical Field

[0001] This invention belongs to the technical fields of micro-robots, medical robots, ophthalmic microsurgery robots, visual servo control, robot adaptive control, precision motion control and constraint optimization control, specifically relating to a dynamic Jacobian matrix field-QP closed-loop control method and system for micro-manipulators. Background Technology

[0002] With the development of minimally invasive medicine, ophthalmic microsurgery, biomanipulation, and precision manufacturing, miniature robotic arms are increasingly being used in confined, fragile, and extremely low-tolerance operating environments. In ophthalmic microsurgery, instruments need to be inserted into the eye through scleral punctures to perform delicate procedures such as subretinal drug delivery, retinal vein cannulation, microneedle puncture, and membrane peeling within a very small space. These tasks not only require the robotic arm's end effector to be miniaturized, lightweight, and have a stable response, but also to achieve reliable target alignment, absolute positioning, and trajectory maintenance in microscopic vision, OCT, anatomical registration, or other external task coordinate systems.

[0003] In microrobot control, repeatability and absolute positioning accuracy correspond to different application requirements. Repeatability focuses on the consistency of the landing point when the same command is executed multiple times, mainly applicable to manual teleoperation or teach-and-play tasks. In such scenarios, the operator can rely on microscopic vision to observe and correct deviations in real time, and the system pays more attention to response consistency, low backlash, and operational stability. In contrast, when the task enters the semi-automatic or automatic control stage, the target position is usually derived from preoperative planning, image recognition, OCT target layer localization, anatomical registration, or offline trajectory. At this time, the control objective becomes to ensure that the end effector actually reaches the specified position in the external task coordinate system and maintains the trajectory shape. Repeatability alone is not enough to guarantee absolute alignment. The role of visual servoing is to introduce the target-end effector error obtained from microscopic vision, OCT, or other position measurements into the automatic control closed loop, enabling the controller to correct the drive command according to the actual error in each control cycle. Therefore, in this type of system, visual servoing is not simply an image detection module, but a closed-loop control link that realizes the absolute positioning, target alignment, and trajectory maintenance of the microrobot arm.

[0004] Existing micro-manipulators often employ geometric calibration models, fixed Jacobian matrices, fixed-gain PID controllers, or offline calibration parameters for control. While these methods can meet certain requirements in small-scale, single-operation, or manually remotely operated scenarios, they have limitations in larger workspaces, automatic trajectory tracking, cross-prototype reuse, or high-precision absolute alignment tasks. The main reason for this is that the small manufacturing scale of micro-manipulators makes the influence of compliant structures, laminated structures, micro-joints, guiding structures, transmission paths, and assembly errors on end-effector motion more significant. Even with the same nominal structure, significant differences can exist between different prototypes in geometric parameters, joint stiffness, friction states, drive efficiency, return characteristics, and local coupling relationships.

[0005] For compliant mechanisms, origami / SCM mechanisms, wire-driven mechanisms, tendon-driven mechanisms, piezoelectric-driven micro-motion platforms, and micro-continuous robots, the local input-output mapping between end-effector displacement and drive commands is typically not a globally constant matrix. Instead, it varies with the current position, mechanism configuration, compliant deformation, contact load, drive state, friction state, and historical motion state. Especially in large workspaces, complex trajectory phases, or frequent reversals, the same drive increment may produce different end-effector displacements at different positions. For wire-driven or tendon-driven micro-manipulators, cable tension, slack, stretching, friction, hysteresis, dead zone, return stroke, and reversal history further exacerbate this mapping uncertainty.

[0006] Therefore, if a fixed Jacobian matrix is ​​directly used for inverse solving, the controller is equivalent to assuming that the mapping between drive commands and end-effector displacement remains unchanged throughout the entire workspace, which can easily lead to absolute spatial trajectory deformation, local phase error, and end-effector offset. If a PID controller is directly used to map the end-effector error to the drive command, it is difficult to handle the spatial coupling of multi-input multi-output micro-manipulators. If the Jacobian is directly solved to obtain the drive increment, it usually only focuses on whether the end-effector one-step motion is close to the desired displacement, without explicitly considering the upper and lower limits of the actuators, the maximum increment per frame, speed limits, acceleration limits, travel boundaries, workspace boundaries, and the smoothness of commands between adjacent frames for different manipulators.

[0007] In summary, current technologies still lack a unified closed-loop control framework for the automated or semi-automated control of micro-robotic arms. This framework should possess the following characteristics: first, the ability to describe the dynamic Jacobian matrix field representing the local input-output mapping changes at different spatial locations; second, the ability to update the local Jacobian matrix online using end-effector feedback residuals; third, the ability to automatically adapt drive constraints and control commands to individual differences among different robotic arms; and fourth, the ability to unify visual servo feedback, dynamic Jacobian updates, constraint optimization solutions, and execution-level compensation into a single control link. Such methods are of great significance for improving the absolute alignment accuracy, trajectory consistency, and automatic control stability of medical robots and ophthalmic microsurgical robots. Summary of the Invention

[0008] The purpose of this invention is to solve the problems existing in the prior art and to provide a dynamic Jacobi-QP closed-loop control method and system for micro robotic arms.

[0009] The specific technical solution adopted in this invention is as follows:

[0010] In a first aspect, the present invention provides a dynamic Jacobi-QP closed-loop control method for a micro-manipulator, which performs the following steps in each control cycle:

[0011] S1. Receive the current position and target position of the end effector of the micro-robotic arm, and calculate the difference between the two as the task space error;

[0012] S2. Generate the desired end-point correction step size in the task space based on the task space error;

[0013] S3. Based on the current position, retrieve the local Jacobian matrix at the current position from the dynamic Jacobian matrix field; wherein, the dynamic Jacobian matrix field represents the mapping relationship between the drive increment and the end effector displacement of the micro-manipulator in the form of the sum of the reference Jacobian matrix and the spatial correction term; the spatial correction term is updated online based on the actual motion residual after the closed-loop execution of the previous control cycle; the actual motion residual is obtained by subtracting the predicted displacement of the effective drive increment of the previous control cycle from the difference of the end effector positions measured in two consecutive control cycles;

[0014] S4. Based on the expected end correction step size, the local Jacobian matrix, the effective driving increment of the previous control cycle, and the current set of individual constraints of the micro-manipulator, solve the quadratic programming problem to obtain the effective driving increment of the current control cycle.

[0015] S5. The effective incremental drive is converted into an actual drive command after being compensated and limited by the execution layer, and output to the driver of the micro-manipulator. Then, the next control cycle is entered, and closed-loop control is continued using the new end position feedback.

[0016] As a preferred embodiment of the first aspect above, in step S2, an error feedback control method is used to generate the desired end correction step size based on the task space error, and the desired end correction step size is subject to a modulus limit so that it does not exceed the preset maximum task space step size.

[0017] As a preferred embodiment of the first aspect above, the spatial correction term is represented as a weighted combination of spatial basis functions, obtained by summing the products of the values ​​of each spatial basis function at the current position and the corresponding parameter matrix; the spatial basis functions are any one of polynomial basis functions, radial basis functions, local linear model basis functions, spline basis functions, lookup table interpolation basis functions, neural network models, Gaussian process models, or local window least squares models.

[0018] As a preferred embodiment of the first aspect above, the method for online updating the parameters of the spatial correction term is as follows: each parameter matrix is ​​expanded into a parameter vector, and the actual motion residual is represented as a linear parameter model composed of the parameter vector, the observation matrix, and the observation noise. The observation matrix is ​​constructed from the spatial basis function values ​​at the starting position of the motion and the effective driving increment of the previous control cycle. Then, Kalman filtering is used to recursively update the parameter vector, wherein the parameter vector is modeled as a random walk process during the Kalman filtering process, and the covariance update adopts the Joseph form.

[0019] As a preferred embodiment of the first aspect, the online update of the spatial correction term is performed only when the update gating conditions are met; otherwise, the update of the current control cycle is skipped and the previous valid parameter is retained. The update gating conditions include: the effective driving increment is greater than the minimum effective excitation threshold, the end position observation is valid, the actual motion residual does not exceed the abnormal threshold, the current control cycle is not in the stable holding phase, and the current control cycle does not trigger the execution layer compensation pulse.

[0020] As a preferred embodiment of the first aspect mentioned above, in step S3, a rationality check is also required on the candidate Jacobian matrix obtained after online update. If the candidate Jacobian matrix passes the rationality check, the candidate Jacobian matrix is ​​adopted, or the spatial correction term is weighted by a preset fusion coefficient and summed with the reference Jacobian matrix to obtain the local Jacobian matrix. If it fails, the update result is rejected, and the reference Jacobian matrix or the valid Jacobian matrix that passed the rationality check in the previous control cycle is adopted as the local Jacobian matrix.

[0021] As a preferred embodiment of the first aspect above, in S4, the quadratic programming problem is solved by minimizing the objective function, which is obtained by weighting the following three terms: a tracking term that makes the product of the local Jacobian matrix and the effective driving increment approximate the desired end correction step size, a regularization term that suppresses the magnitude of the effective driving increment, and a smoothing term that suppresses abrupt changes in the effective driving increment between adjacent control cycles.

[0022] More preferably, the set of individual constraints includes at least one of the following: upper and lower limits of the actuator of the micromanipulator, maximum drive increment per frame, speed limit, acceleration limit, joint limit, workspace boundary, contact safety boundary, and task phase safety boundary;

[0023] More preferably, the micro-manipulator has three driving inputs, and the individual constraint set is the upper and lower bound box constraints of each driving input; the quadratic programming problem is solved using the three-dimensional box-constrained quadratic programming (box-QP) active set enumeration method: each driving variable is placed in one of three states: lower bound, free, or upper bound; all active constraint combinations are enumerated; under each combination, the free variables are analytically solved; and the one that minimizes the cost of the objective function is selected from all feasible candidate solutions as the effective driving increment.

[0024] As a preferred embodiment of the first aspect above, the execution layer compensation includes one or more of dead zone compensation, take-up compensation, anti-jamming pulse compensation, micro-jitter compensation, hysteresis compensation, directional hysteresis compensation, static friction compensation, piezoelectric hysteresis compensation, and compliant mechanism microstepping compensation; the safety limit includes at least one of speed limit, acceleration limit, travel safety limit, and mission safety boundary limit.

[0025] As a preferred embodiment of the first aspect, it further includes at least one of dynamic trajectory phase management and stability determination; the dynamic trajectory phase management specifically involves: when performing a dynamic trajectory task, if the norm of the task space error exceeds a first error threshold, the phase advancement of the target trajectory is paused or slowed down; when the norm of the task space error is lower than a second error threshold and remains below a preset number of frames, the phase advancement of the target trajectory continues, wherein the second error threshold is less than the first error threshold; the stability determination specifically involves: if the norm of the task space error is less than a stability threshold for a preset number of consecutive frames, the current target point positioning is determined to be complete.

[0026] In a second aspect, the present invention provides a dynamic Jacobi-QP closed-loop control system for a micro-manipulator, comprising functional modules constructed by a computer program, the functional modules being configured to implement the dynamic Jacobi-QP closed-loop control method for a micro-manipulator as described in any of the first aspects above.

[0027] Compared with the prior art, the present invention has the following advantages:

[0028] First, this invention expands the control object of the micro-robotic arm from a fixed Jacobian matrix to a spatially varying dynamic Jacobian matrix field. Through Describing the local input-output mapping changes at different spatial locations can reduce the absolute spatial trajectory distortion and local phase error caused by a fixed Jacobian matrix.

[0029] Second, this invention utilizes the online update of the actual motion residuals after closed-loop execution. This method enables the control system to adapt to different spatial locations, different mechanism states, and local mapping differences between different robotic arms. It can compensate for model deviations caused by manufacturing errors, assembly errors, differences in compliance characteristics, differences in friction conditions, differences in transmission efficiency, and differences in cable / tendon drive states.

[0030] Third, this invention solves for the driving increment under the current individual constraints of the robotic arm through QP. The QP module does not independently undertake the function of identifying unknown models, but rather solves for stable, smooth, and constrained driving commands under the current local Jacobian matrix and the current set of individual constraints. As a result, the same control framework does not need to directly copy the fixed control parameters of a certain robotic arm, but can automatically generate adapted commands based on the current local model, driving range, and safety boundaries of the robotic arm.

[0031] Fourth, this invention limits the PID or other error feedback controller to a task space step size generator, rather than a direct driver controller. This design avoids the problems of direct PID neglecting multi-input multi-output coupling and spatial Jacobian variation, making the control structure more suitable for various types of micro robotic arms such as micro serial arms, micro parallel arms, compliant mechanisms, line-driven mechanisms, tendon-driven mechanisms, and piezoelectric micro-motion platforms.

[0032] Fifth, this invention uses QP to simultaneously process end-point tracking, drive command amplitude, adjacent frame command smoothing, and execution constraints, enabling the system to maintain stable execution even after target point switching, control phase switching, dynamic trajectory advancement, and local Jacobian updates.

[0033] Sixth, this invention can combine execution layer compensation mechanisms such as deadzone, take-up, anti-stick, dither, and hysteresis compensation to improve the controllability of micro-execution systems within small error ranges and reduce the risk of small instructions being swallowed by friction, dead zones, backtracking, or jamming.

[0034] Seventh, this invention is applicable to scenarios in medical robots and ophthalmic microsurgical robots where high requirements are placed on micrometer-level absolute positioning, target alignment, and trajectory maintenance. For purely teleoperation tasks, this invention can improve response consistency and closed-loop stability; for semi-automatic or automatic control tasks, this invention can further improve absolute spatial positioning accuracy and trajectory consistency. Attached Figure Description

[0035] Figure 1 A schematic diagram illustrating the steps of a dynamic Jacobi-QP closed-loop control method for a micro-manipulator;

[0036] Figure 2A schematic diagram of the module composition of a dynamic Jacobi-QP closed-loop control system for a micro robotic arm;

[0037] Figure 3 This is a schematic diagram of the structure of a computer electronic device;

[0038] Figure 4 This is a diagram showing the nine-point closed-loop positioning result in an embodiment of the present invention.

[0039] Figure 5 This is a nine-point closed-loop error convergence curve diagram in an embodiment of the present invention;

[0040] Figure 6 This is a statistical chart of the final error and convergence time of the nine-point closed loop in an embodiment of the present invention;

[0041] Figure 7 This is a diagram showing the distribution of the final error and convergence time of the static circular trajectory as a function of phase in an embodiment of the present invention.

[0042] Figure 8 This is a statistical chart of the final error and segmented convergence time of the static circular trajectory in an embodiment of the present invention. Detailed Implementation

[0043] To make the above-mentioned objects, features, and advantages of the present invention more readily understood, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Many specific details are set forth in the following description to provide a thorough understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar modifications without departing from the spirit of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below. The technical features in the various embodiments of the present invention can be combined accordingly without mutual conflict.

[0044] This invention provides a dynamic Jacobi-QP closed-loop control method for micro-robotic arms. This method achieves high-precision absolute positioning and trajectory maintenance of the micro-robotic arm in the external task coordinate system through visual servo feedback, online updating of the dynamic Jacobi matrix field, and constraint optimization. The core technical problems solved by this method can be summarized as follows:

[0045] First, to address the problem that relying solely on repeatability in automated or semi-automated tasks is insufficient to guarantee absolute spatial target alignment for micro-robotic arms, a closed-loop control method based on position feedback is provided. This method enables the controller to automatically correct drive commands based on the error between the current position of the end effector and the target position, thereby improving the positioning accuracy and trajectory maintenance capability of the micro-robotic arm in the task coordinate system.

[0046] Second, to address the problem of inconsistent local input-output mapping of micro-robotic arms at different spatial positions and the difficulty of describing the entire workspace with a fixed Jacobian matrix, a dynamic Jacobian matrix field modeling and updating method is provided, which enables the controller to correct the local Jacobian matrix based on the current position and actual motion residuals.

[0047] Third, to address the problem that control parameters are difficult to directly replicate between different micro-robotic arms due to differences in manufacturing errors, assembly errors, compliance characteristics, friction states, drive efficiency, and safety boundaries, an optimized control method combining local Jacobian matrices and individual constraints is provided, enabling the same control framework to adapt to the drive range, motion boundaries, and command smoothing requirements of different machines.

[0048] Fourth, to address the difficulty of direct PID control in handling multi-input multi-output coupling and spatial Jacobian variations in micro-robotic arms, a hierarchical control method is provided that uses a PID or other error feedback controller as a task space step size generator, rather than directly driving a command generator.

[0049] Fifth, to address the problem that the direct inverse solution of the Jacobian matrix cannot simultaneously consider driver limiting, maximum increment per frame, smoothing of adjacent frame commands, velocity / acceleration constraints, and workspace safety boundaries, a quadratic programming-based method for solving the driver increment is provided, which makes a trade-off between end-tracking accuracy, driver command amplitude, smoothing of adjacent frames, and individual execution constraints.

[0050] Sixth, to address the non-ideal execution layer issues that may exist in micro-execution systems, such as small instruction dead zones, friction, hysteresis, backtracking, jamming, and micro-oscillations, an optional execution layer compensation mechanism is provided to improve the controllability and stability of the micro-manipulator within a small error range.

[0051] Seventh, to address the issues of excessively large local phase error peaks, error accumulation at direction reversal points, and trajectory shape deformation during dynamic trajectory execution, a trajectory phase pause or deceleration mechanism based on an error threshold is provided. This allows the system to prioritize restoring closed-loop positioning when the error is too large before continuing to advance the target trajectory.

[0052] like Figure 1 As shown, in a preferred embodiment of the present invention, the dynamic Jacobi-QP closed-loop control method for a micro-manipulator executes the following steps S1 to S5 in each control cycle:

[0053] S1. Receive the current position and target position of the end effector of the micro-robotic arm, and calculate the difference between the two as the task space error.

[0054] It should be noted that this invention does not limit the specific method of obtaining the end-effector position. The current end-effector position can be provided by microscopic vision, OCT, optical tracking, electromagnetic tracking, encoder fusion, force / position fusion, or other external measurement systems.

[0055] It should be noted that the task space of this invention can be a two-dimensional planar position, a three-dimensional spatial position, or an extended task space including attitude. This error serves as a closed-loop control object, used for subsequent task space step size generation, stability determination, dynamic trajectory phase management, and, when necessary, Jacobian update determination.

[0056] S2. The task space step size generation module generates the desired end correction step size in the task space based on the task space error.

[0057] It should be noted that the expected end-effector correction step size represents the displacement increment that the end-effector needs to generate in the task space within the current control cycle. This module only outputs the task space motion and does not directly output the driver command. This invention adopts this hierarchical output method, which avoids the multi-input multi-output coupling and spatial Jacobian variation problems that are difficult to handle when directly mapping the end-effector error to the driver command using the proportional-integral-derivative (PID) method, thus decoupling the error correction from the driver solution.

[0058] In one embodiment of the present invention, the task space step size generation module uses an error feedback control method to generate the desired end-point correction step size. The error feedback control law includes at least two of a proportional term, a differential term, and an integral term, and the generated desired end-point correction step size is subject to a magnitude limit to ensure that it does not exceed the maximum task space step size allowed in the current control stage. Furthermore, based on the magnitude of the task space error or the task state, the control process can be divided into at least two stages: a coarse positioning stage, a fine positioning stage, a stabilization stage, and a dynamic trajectory stage. Different control stages employ at least one of a different maximum task space step size, integral gain, and stability criterion. For example, a larger step size upper limit can be used in the coarse positioning stage with a larger error to achieve rapid convergence, while a smaller step size upper limit can be used in the fine positioning and stabilization stages with a smaller error to ensure positioning accuracy and stabilization capability.

[0059] S3. The dynamic Jacobian matrix field modeling module retrieves the local Jacobian matrix at the current position from the dynamic Jacobian matrix field based on the current position. The dynamic Jacobian matrix field represents the mapping relationship between the drive increment and the end effector displacement of the micro-manipulator in the form of the sum of the reference Jacobian matrix and the spatial correction term. The spatial correction term is updated online by the dynamic Jacobian matrix field online update module based on the actual motion residual after the closed-loop execution of the previous control cycle. The actual motion residual is obtained by subtracting the predicted displacement of the effective drive increment of the previous control cycle from the difference of the end effector positions measured in two consecutive control cycles.

[0060] The actual motion residuals obtained in this step reflect local mapping errors that cannot be explained by the fixed reference Jacobian at the current position and current mechanism state. Due to objective limitations, if a fixed Jacobian matrix is ​​directly used for inverse kinematics, the controller is essentially assuming that the mapping between drive commands and end-effector displacement remains constant throughout the workspace, which easily leads to absolute spatial trajectory deformation, local phase errors, and end-effector offset. This embodiment decomposes the Jacobian matrix into a reference Jacobian matrix and a position-dependent spatial correction term: the reference Jacobian matrix remains fixed, and the spatial correction term obtains position-related deviations through online learning. The two are then superimposed to obtain the local Jacobian matrix at the current working point.

[0061] In one embodiment of the present invention, the spatial correction term is represented as a weighted combination of spatial basis functions, which is obtained by summing the product of the value of each spatial basis function at the current position and the corresponding parameter matrix; the spatial basis function is any one of polynomial basis function, radial basis function, local linear model basis function, spline basis function, lookup table interpolation basis function, neural network model, Gaussian process model or local window least squares model.

[0062] In one embodiment of the present invention, the method for online updating the parameters of the spatial correction term is as follows: each parameter matrix is ​​expanded into a parameter vector, and the actual motion residual is represented as a linear parameter model composed of the parameter vector, the observation matrix, and the observation noise. The observation matrix is ​​constructed from the spatial basis function values ​​at the starting position of the current motion and the effective driving increment of the previous control cycle. Then, the parameter vector is recursively updated using any one of Kalman filtering, recursive least squares (RLS), extended Kalman filtering (EKF), unscented Kalman filtering (UKF), sliding window least squares, or offline-online hybrid update methods. Preferably, Kalman filtering is used, in which case the parameter vector is modeled as a random walk process, and the covariance update adopts the Joseph form to maintain the numerical stability of the covariance matrix.

[0063] In one embodiment of the present invention, the online update of the spatial correction term is performed only when the update gating condition is met; otherwise, the update of the current control cycle is skipped and the previous valid parameter is retained to avoid unreliable data contaminating the dynamic Jacobian matrix field. The update gating condition includes: the effective driving increment is greater than the minimum effective excitation threshold, the end position observation is valid, the actual motion residual does not exceed the abnormal threshold, the current state is not in the stable holding phase, and the current control cycle does not trigger the execution layer compensation pulse.

[0064] In embodiments of the present invention, after online updates are completed, a rationality check is performed on the candidate Jacobian matrix obtained after the online update to avoid abnormal control commands caused by ill-conditioned matrices, noise-driven models, or abnormal local updates. The specific rationality check includes: the condition number of the candidate Jacobian matrix is ​​less than a condition number threshold, the minimum singular value is greater than a singular value threshold, and the deviation norm of the candidate Jacobian matrix relative to the benchmark Jacobian matrix is ​​less than a deviation threshold. Based on the check results, the following operations are performed: if the candidate Jacobian matrix passes the rationality check, it is adopted; or the spatial correction term is weighted by a preset fusion coefficient and summed with the benchmark Jacobian matrix to obtain the local Jacobian matrix. If it fails, the update result is rejected, and the benchmark Jacobian matrix or the valid Jacobian matrix that passed the rationality check in the previous control cycle is adopted as the local Jacobian matrix. Through this check and rollback mechanism, the local Jacobian matrix entering subsequent solutions always has sufficient numerical stability and physical rationality.

[0065] S4. Based on the expected end correction step size, the local Jacobian matrix, the effective drive increment of the previous control cycle, and the current set of individual constraints of the micro-manipulator, solve the quadratic programming problem to obtain the effective drive increment of the current control cycle.

[0066] In the solution process, this invention requires a trade-off between end-point tracking accuracy, drive command amplitude, smoothness between adjacent frames, and individual execution constraints to obtain the optimal solution. Directly solving the Jacobian algorithm to calculate the drive increment typically only focuses on whether the end-point motion approaches the desired displacement, without explicitly considering the driver's upper and lower limits, maximum increment per frame, velocity limits, acceleration limits, travel boundaries, workspace boundaries, and smoothness of commands between adjacent frames. Therefore, this step explicitly incorporates these factors into the solution through constraint optimization, outputting a stable, smooth, and constrained effective drive increment.

[0067] In one embodiment of the present invention, the quadratic programming problem is solved by minimizing the objective function, which is obtained by weighting the following three terms: a tracking term that makes the product of the local Jacobian matrix and the driving effective increment approximate the desired end correction step size, a regularization term that suppresses the magnitude of the driving effective increment, and a smoothing term that suppresses abrupt changes in the driving effective increment between adjacent control cycles.

[0068] In one embodiment of the present invention, the individual constraint set includes at least one of the following: the upper and lower limits of the actuator of the micro-manipulator, the maximum drive increment per frame, the speed limit, the acceleration limit, the joint limit, the workspace boundary, the contact safety boundary, and the task phase safety boundary, so that the same control framework can be adapted to different prototypes and will not be fixed on the control parameters of a certain prototype.

[0069] In one embodiment of the present invention, the micro-manipulator has three driving inputs, and the individual constraint set consists of upper and lower bound box constraints for each driving input. The quadratic programming problem is solved using a three-dimensional box-constrained quadratic programming (box-QP) active set enumeration method: each driving variable is placed in one of three states—lower bound, free, or upper bound—and all active constraint combinations are enumerated. Under each combination, the free variables are analytically solved, and the solution that minimizes the objective function cost is selected from all feasible candidate solutions as the effective driving increment. This solution method completes the analytical solution within all active constraint combinations without calling an external iterative solver.

[0070] Of course, in other embodiments of the present invention, other solvers can also be used to solve the objective function, such as three-dimensional box-QP active set enumeration, general active set method, interior point method, projection gradient method, coordinate descent method, OSQP-type solvers or other quadratic programming solvers.

[0071] S5. The effective incremental drive is converted into an actual drive command after being compensated and limited by the execution layer, and output to the driver of the micro-manipulator. Then, the next control cycle is entered, and closed-loop control is continued using the new end position feedback.

[0072] This step can improve the controllability and stability of the micro-actuator within a small error range. Because real-world micro-actuators may suffer from dead zones, backlash, friction, hysteresis, cable slack, jamming of compliant mechanisms, or invalid micro-commands, these issues can lead to small commands being swallowed up by friction, dead zones, backlash, or jamming if left unaddressed. Therefore, this invention improves the effectiveness of small command execution through execution layer compensation and safety limiting, ensuring that command amplitude, speed, acceleration, and stroke remain within safe ranges, thereby maintaining a stable closed loop.

[0073] In one embodiment of the present invention, the execution layer compensation includes one or more of the following compensation methods: dead zone compensation; take-up compensation, i.e., when a drive direction reversal is detected, a corresponding dead zone compensation amount is added to the drive command, and the direction reversal is triggered only after confirmation by hysteresis of multiple consecutive frames; anti-jamming pulse compensation; micro-jitter compensation; hysteresis compensation; directional hysteresis compensation; static friction compensation; piezoelectric hysteresis compensation; compliant mechanism micro-stepping compensation. Additionally, the safety limit includes at least one of speed limit, acceleration limit, travel safety limit, and task safety boundary limitation.

[0074] In one embodiment of the present invention, a dynamic trajectory phase management module is further included. Specifically, when performing a dynamic trajectory task, if the norm of the task space error exceeds a first error threshold, the phase advancement of the target trajectory is paused or slowed down, so that the closed-loop control prioritizes the recovery of positioning. When the norm of the task space error is lower than a second error threshold and is maintained for a preset number of frames, the phase advancement of the target trajectory continues. The second error threshold is less than the first error threshold, so as to reduce the error peak in the dynamic trajectory. This is particularly suitable for trajectory areas with direction reversal, curvature change, large local Jacobian change, or strong individual execution constraints.

[0075] The aforementioned dynamic trajectory phase management module can pause, decelerate, or resume target trajectory phase advancement based on the magnitude of the end-point error, thereby reducing peak errors in the dynamic trajectory. Therefore, this module can be used for circular trajectories, elliptical trajectories, figure-eight trajectories, surgical needle insertion trajectories, blood vessel following trajectories, or other continuous path tasks.

[0076] In one embodiment of the present invention, a stability determination module is further included, specifically: if the norm of the task space error is less than a stable threshold for a consecutive preset number of frames, the current target point is determined to be located, so as to avoid misjudgment caused by single-frame measurement noise or instantaneous entry into the threshold.

[0077] In summary, this invention improves the target alignment, absolute positioning, and trajectory maintenance capabilities of a micro-manipulator in the external task coordinate system through the synergy of task space error feedback, online updating of the dynamic Jacobian matrix field, rationality checking and conservative fusion, quadratic programming solution under individual constraints, execution layer compensation and safety limiting, and trajectory phase management and stability determination. It is applicable to a variety of different devices and scenarios. A brief introduction to the applicable devices and scenarios of this invention follows.

[0078] This invention is applicable to various types of micro robotic arms, including but not limited to micro serial robotic arms, micro parallel robotic arms, compliant mechanism robotic arms, origami / SCM micro robotic arms, wire-driven robotic arms, tendon-driven robotic arms, piezoelectric-driven micro-motion platforms, micro continuum robots, micro surgical end effectors, and medical robot end micro-manipulation modules.

[0079] This invention is particularly applicable to microrobot systems requiring automated or semi-automated control, especially in applications where target alignment, absolute positioning, and trajectory maintenance are needed in external task coordinate systems, microscopic vision coordinate systems, OCT coordinate systems, anatomical registration coordinate systems, or other absolute spatial coordinate systems. Representative applications include subretinal drug delivery, microneedle puncture, retinal vein cannulation, membrane peeling, microscopic exploration, minimally invasive surgical end-effector positioning, biological cell manipulation, and other precision operations in confined spaces within ophthalmic microsurgical robots.

[0080] For purely teleoperated microrobots, operators can correct deviations in real time using microscopes or image feedback. In this case, the system emphasizes repeatability, response consistency, and operational stability. However, for automated, semi-automated, image-guided, trajectory planning, and cross-prototype reuse tasks, repeatability alone is insufficient to guarantee the alignment accuracy and trajectory consistency of the end effector in absolute space. This invention primarily addresses the latter type of task, namely, improving the automatic positioning and trajectory control capabilities of microrobots in absolute space through visual servo feedback, dynamic Jacobian matrix field updates, and QP constraint optimization.

[0081] To enable those skilled in the art to better understand the technical solution, the implementation process and technical effects of the methods described in S1 to S5 above will be specifically demonstrated below through an embodiment.

[0082] Example

[0083] This embodiment provides a dynamic Jacobi-QP closed-loop control system for a micro-robotic arm that implements the methods described in S1 to S5 above. In this system, the various functional modules cooperate with each other to execute steps S1 to S5 in each control cycle. The specific implementation of the entire system is described in detail below.

[0084] The overall control flow of this system is as follows:

[0085] The system uses external position feedback as control input, receiving the current position and target position of the end effector in each control cycle and calculating the task space error. The task space step size generation module generates the expected end effector correction step size. The dynamic Jacobian matrix field module (including the dynamic Jacobian matrix field modeling module, the dynamic Jacobian matrix field online update module, and the Jacobian matrix rationality check module) provides the local input-output mapping at the current position. The constraint optimization solution module solves the effective driving increment under the current local Jacobian matrix and the current individual constraints of the robotic arm. Finally, after execution layer compensation and safety limiting, the actual driving command is output.

[0086] Specifically, corresponding to any first The control cycle is defined as follows: The current position of the end point in each control cycle is: ;

[0087] The target location is: ;

[0088] in, It represents the dimension of the task space, which can be a two-dimensional planar position, a three-dimensional spatial position, or an extended task space that includes attitude.

[0089] The control error is: ;

[0090] The task space step size generation module is based on Generate the desired end-point correction step size: ;

[0091] The Dynamic Jacobian Matrix Field module provides the current local Jacobian matrix: ;

[0092] The Quadratic Programming (QP) module is based on , Previous frame drives increment Given the current individual constraints of the robotic arm, solve for the current effective increment of the drive: ;

[0093] The execution layer compensation module will Convert to actual sent commands: ;

[0094] Therefore, the overall data link in the system is as follows:

[0095]

[0096] like Figure 2 As shown, the functional modules used in this system include a position input and error definition module, a task space step size generation module, a dynamic Jacobian matrix field modeling module, a dynamic Jacobian matrix field online update module, a Jacobian matrix rationality check module, a QP individualized driven incremental solution module, an execution layer compensation and command output module, a dynamic trajectory phase management module, and a stability determination module.

[0097] Furthermore, it should be noted that each of the above functional modules is essentially a computer program module or software functional module. For example... Figure 3 As shown, the system operates based on a computer electronic device, which includes a memory and a processor;

[0098] The memory is used to store computer programs;

[0099] The processor is used to call various functional modules to implement the dynamic Jacobi-QP closed-loop control method for micro-robotic arms described in S1 to S5 when executing the computer program.

[0100] The following is a detailed description of the internal processes of each module.

[0101] 1. Position Input and Error Definition Module

[0102] This module is only used to define the position variables and error variables received by the control system, and does not limit the position detection, recognition, or reconstruction methods. For three-dimensional tasks, the... The current position at the end of the frame is denoted as:

[0103]

[0104] The target location is denoted as:

[0105]

[0106] For two-dimensional tasks, the following can be considered:

[0107]

[0108] For tasks involving pose, Expanded into a position-attitude combination vector.

[0109] Position error is defined as: ;

[0110] The error norm is: ;

[0111] This error is used for subsequent task space step size generation, stability determination, dynamic trajectory phase management, and Jacobian update determination when necessary.

[0112] 2. Task Space Step Generation Module

[0113] The task space step size generation module is used to generate steps based on errors. Calculate the desired correction step size at the end of the current control cycle. This module does not directly output driver commands, but only outputs the desired motion in the task space. (Step size adjustment) The generation method is as follows:

[0114]

[0115] in, This indicates vector magnitude limiting, keeping the direction unchanged and only scaling the magnitude to no more than the upper limit;

[0116] For the error integral term:

[0117]

[0118] , , These are proportional, differential, and staged integral gains, respectively; upper limit. This represents the maximum allowed task space step size for the current stage. The entire stage can be divided into coarse positioning, fine positioning, stabilization, and dynamic trajectory stages based on the error magnitude or task status. In this embodiment, the division is based on the error magnitude: error magnitude greater than 50µm is the coarse positioning stage, 20–50µm is the fine positioning stage, 10–20µm is the fine positioning stage, and no greater than 10µm is the stabilization stage. Different step size limits, integral gains, and stability criteria can be used for different stages.

[0119] Of course, in other embodiments, this module may also employ PD control, nonlinear feedback control, model prediction step size generation, sliding mode control, or other error feedback methods, as long as its output is the task space correction step size. .

[0120] 3. Dynamic Jacobian Matrix Field Modeling Module

[0121] The incremental drive and end-effector displacement of a micro-manipulator typically satisfy a locally linear approximation relationship:

[0122]

[0123] in, To drive effective increments, This represents the mechanism state or implicit state that affects the local mapping, such as compliant deformation, friction state, load state, driving state, historical motion state, or individual prototype differences. In this embodiment, the implicit state is not modeled separately; its influence is implicitly expressed through directional partitioning and spatial basis function correction terms.

[0124] To describe the local mapping changes at different spatial locations, this invention employs a dynamic Jacobian matrix field:

[0125]

[0126] in, The reference Jacobian matrix or initial calibration Jacobian matrix is ​​obtained from offline calibration (it can be loaded from the calibration file or applied to each joint individually while the robotic arm is stationary). (Position excitation, measurement of end displacement, and sequential solution, or multi-position least squares fitting) This is the spatial correction term for the current location. The spatial correction term can be expressed as:

[0127]

[0128] in, For spatial basis functions, in this embodiment, quadratic polynomial basis functions are used. , where x, y, and z are the end position coordinates normalized with the reference position as the origin; This is the corresponding parameter matrix.

[0129] In this embodiment, the spatial correction term is divided into six directional partitions according to the dominant direction of the driving increment. The dominant direction is the direction corresponding to the component with the largest absolute value among all components of the driving increment, i.e. Six directions; each direction partition independently maintains a set of parameters for spatial correction terms, i.e., the parameter matrix corresponding to each spatial basis function. During a query, the dominant direction of the effective incremental drive in the current control cycle is used to determine the relevant direction partition. Based on the parameters maintained by that direction partition, the query is performed according to... The local Jacobian matrix at the current position is reconstructed; if the effective update count of a certain corridor has not yet reached the preset threshold (e.g., 6 times), the correction parameters of that corridor are not included in the query, and the benchmark Jacobian matrix is ​​directly used as the local Jacobian matrix.

[0130] This module is used to solve the problem that the Jacobian matrix of the same robotic arm is not fixed at different spatial positions. It is also used to express the local mapping differences between different robotic arms caused by differences in manufacturing, assembly, compliance, friction and transmission.

[0131] 4. Dynamic Jacobian Matrix Field Online Update Module

[0132] This module is used to correct the dynamic Jacobian matrix field online based on the actual motion results after closed-loop execution. Let the effective increment of the drive output from the previous control cycle be... The end position measured in two consecutive frames is and The actual end displacement is:

[0133]

[0134] Benchmark Jacobian Matrix The predicted displacement for this motion is:

[0135]

[0136] The difference between the two is defined as the model residual:

[0137]

[0138] This residual reflects local mapping errors in the fixed reference Jacobian that cannot be explained by the current position and current mechanism state. The system interprets this residual using a spatial correction term:

[0139]

[0140] The dynamic Jacobian correction term is expressed as:

[0141]

[0142] in, For spatial basis functions, This corresponds to the parameter matrix. Expand all parameter matrices into parameter vectors:

[0143]

[0144] The residual observations can then be written as a linear parametric model:

[0145]

[0146] in, From the starting position of this movement Spatial basis functions at and the incremental growth driven by the previous cycle structure, To observe noise. If the task space dimension is... The driving dimension is Then we can define:

[0147]

[0148] In this embodiment, the parameter vector is updated recursively using Kalman filtering. The dynamic Jacobian parameters are modeled as a random walk process:

[0149]

[0150] in, The process noise has a covariance of Observation noise The covariance is . No. The Kalman update process for each control cycle is as follows:

[0151]

[0152]

[0153]

[0154]

[0155]

[0156]

[0157] in, To observe and predict residuals, To observe the covariance of the predicted residuals, For Kalman gain, This represents the updated parameter covariance. Updating the covariance using the Joseph form helps maintain the numerical stability of the covariance matrix. After the parameter update, the spatial correction term is reconstructed based on the new parameter vector:

[0158]

[0159] And the candidate Jacobian matrix is ​​obtained:

[0160]

[0161] To avoid unreliable data contaminating the dynamic Jacobian matrix field, this module sets update gating conditions. Parameter updates are only performed when all of the following conditions are met; otherwise, the update for this cycle is skipped and the previous valid parameters are retained. The conditions to be met are as follows: the effective drive increment is greater than the minimum effective excitation threshold (a smaller value can be taken for the near-term fine positioning); the end-position observation is valid (i.e., frame age does not exceed the preset threshold, pose quality is not lower than the preset threshold, and pairing delay does not exceed the preset threshold); the current control cycle is not in the stable holding interval or the near-term fine positioning interval; the current control cycle has not triggered the execution layer compensation pulse; and the current control cycle is not in the commutation cooling period. After the parameter update is completed, the update results must be verified: if the updated residual exceeds the preset threshold (which can be an absolute threshold or a proportional threshold relative to the residual before the update), or if the condition number of the updated Jacobian matrix exceeds the preset threshold, or if its deviation norm relative to the baseline Jacobian matrix exceeds the preset threshold, then the update is rolled back to the parameters before the update and the current update is abandoned.

[0162] Through the above online updates, the system can continuously correct the local input-output mapping using closed-loop motion residuals, so that the dynamic Jacobian matrix field gradually adapts to the actual motion characteristics of different spatial positions and different individual robotic arms.

[0163] 5. Jacobian Matrix Reasonableness Check Module

[0164] To prevent abnormal observations or erroneous parameter updates from disrupting control stability, this invention includes a Jacobian matrix rationality check module after dynamic Jacobian updates. The candidate Jacobian matrix is:

[0165]

[0166] The system checks the candidate Jacobian matrix from both numerical stability and physical plausibility perspectives. In this embodiment, the candidate Jacobian matrix must simultaneously satisfy the following conditions:

[0167]

[0168] in, For condition numbers, For the smallest singular value, The Frobenius norm is used. The condition number and minimum singular value are used to prevent Jacobian matrices that are close to singular or ill-conditioned from entering the control; the deviation magnitude constraint is used to prevent single anomalous residuals from causing excessive non-physical corrections in the local model.

[0169] If the candidate Jacobian matrix passes the check, the controller either adopts the candidate matrix or uses a conservative fusion method.

[0170]

[0171] in, To fix the fusion coefficient, the intensity of the online correction term is controlled. In this embodiment, the fusion coefficient is adjusted online: while ensuring the condition number constraint and deviation norm constraint, ... The maximum feasible fusion coefficient is determined by binary search within the interval, and different upper limits of the fusion coefficient are used in different control stages.

[0172] If the candidate Jacobian matrix fails the check, the update result is rejected, and the baseline Jacobian matrix or the valid Jacobian matrix from the previous frame that passed the check is used, i.e.:

[0173]

[0174] or:

[0175]

[0176] This module is used to ensure that the Jacobian matrix entering the subsequent QP solution has sufficient numerical stability and physical rationality, thereby avoiding abnormal control commands caused by ill-conditioned matrices, noise-driven models or abnormal local updates.

[0177] 6. QP Individualized Incremental Solver Module

[0178] This module is used to solve for the effective incremental drive under the current local Jacobian matrix and the current individual constraints of the robot arm. QP does not independently perform the function of identifying unknown mechanism parameters, but rather solves for stable, smooth, and constrained drive commands suitable for the current robot arm based on the given dynamic Jacobian matrix field and individual constraint parameters, or those that have been updated online.

[0179] Let the current individual robot arm number be The current local Jacobian matrix is: The current set of individual constraints is: The constraint set may include upper and lower limits of the drive range, maximum drive increment per frame, speed limit, acceleration limit, joint travel limit, workspace boundary, and task safety boundary. In this embodiment, the individual constraint set includes: maximum drive increment per frame, upper and lower limits of the drive, command speed limit, command acceleration limit, and joint travel limit.

[0180] The QP solution is in the form of:

[0181]

[0182] The constraints are:

[0183]

[0184] The first term is used to make the end effector motion approach the desired task space step size; the second term is used to suppress excessively large drive commands; the third term is used to suppress abrupt changes in commands between adjacent frames; the constraint set is used to describe the drive capability and safety boundaries of the current robotic arm individual. In this embodiment, the coefficients... Take 0.02, coefficient Let's take 0.05. For a micro-manipulator with three drive inputs, This can be simplified to a box constraint:

[0185]

[0186] In the formula, This represents the maximum driving increment per frame at the current stage.

[0187] At this point, a 3D box-QP active set enumeration method can be used. Each driving variable can be in one of three states: lower bound, free, or upper bound. The system enumerates all combinations of active constraints, analytically solves for the free variables under each combination, and selects the minimum cost solution from all feasible candidate solutions as the current effective driving increment. If no solution is feasible for any combination, a zero increment is output as the backoff solution. Through this module, the same control framework does not need to directly copy the fixed commands or fixed gains of a particular robotic arm, but automatically calculates the driving increment under the local model and individual constraints of the current robotic arm.

[0188] 7. Execution Layer Compensation and Command Output Module

[0189] QP output To ensure efficient incremental driving, practical micro-execution systems may suffer from issues such as dead zones, backlash, friction, hysteresis, cable slack, compliant mechanism jamming, piezoelectric hysteresis, or invalid micro-commands. Therefore, this embodiment incorporates execution layer compensation:

[0190]

[0191] in, This may include one or more of the following compensations:

[0192] 1. Deadzone compensation;

[0193] 2. Take-up compensation;

[0194] 3. Dither micro-jitter compensation;

[0195] 4. Directional hysteresis compensation;

[0196] 5. Delay compensation;

[0197] 6. Speed ​​limit;

[0198] 7. Acceleration limiting;

[0199] 8. Trip safety limits;

[0200] 9. Mission safety boundary constraints.

[0201] In this embodiment, the execution layer compensation includes three types: dead zone compensation, take-up compensation, and directional hysteresis compensation; while the safety limiting includes four types: velocity limiting, acceleration limiting, travel safety limiting, and task safety boundary limiting. Specifically, take-up compensation is implemented as follows: maintaining the directional dead zone state of each driver (positive dead zone, negative dead zone, and current directional state); when the expected drive increment is lower than the noise threshold, it is processed as pass-through and compensation is not triggered; when a drive direction reversal is detected, the corresponding directional dead zone compensation amount is added to the drive command; directional reversal is only triggered after confirmation by multiple consecutive frames of symbolic hysteresis, meaning the new direction must be maintained for a preset number of consecutive frames to be considered a valid reversal; after confirming the reversal, the directional state is updated, and take-up is marked as activated for update gating judgment.

[0202] In this embodiment, the speed limit is that the command speed does not exceed The acceleration limit is that the commanded acceleration does not exceed The safe travel limit is that the angles of each joint are maintained centered on the nominal position. Within the scope; mission safety boundary limits are set separately according to mission phase.

[0203] For a wire-driven robotic arm, this module can be specified as follows:

[0204]

[0205] For piezoelectrically driven micro-motion platforms, this module can be specified as hysteresis compensation, voltage limiting, and micro-step compensation. For magnetically driven microrobots, this module can be specified as constraints on magnetic field amplitude, direction, and rate of change.

[0206] 8. Dynamic trajectory phase management module

[0207] For dynamic trajectory tasks, this invention can set trajectory phase advancement management. If the terminal error exceeds a preset threshold, the target trajectory phase advancement is paused or slowed down, allowing closed-loop control to prioritize restoring positioning; once the error recovers to a lower threshold and remains there, the target trajectory advancement continues.

[0208] The triggering condition is: ;

[0209] The recovery conditions are: ;

[0210] This module is used to reduce error peaks in dynamic trajectories, and is especially suitable for trajectory areas with direction reversal, curvature changes, large local Jacobian changes, or strong individual execution constraints.

[0211] 9. Stability determination module

[0212] For point-to-point positioning or discrete waypoint control, this embodiment includes a stability determination module. If the current error is less than a set threshold N for multiple consecutive frames, the current target point is determined to be reached.

[0213] Let the stability threshold be... The number of consecutive frames is Stable count is ,but:

[0214]

[0215] like: If the target point is reached, it is determined that the current target point has been reached. In this embodiment, the stability threshold ε is set to 10µm, and the consecutive frame number threshold N is set to 15 frames.

[0216] This module is used to avoid misjudgments caused by single-frame measurement noise or instantaneous threshold entry.

[0217] Based on the above nine functional modules, the complete execution steps of this invention are as follows:

[0218] The first step is to receive the end position of the current control cycle. and target location Calculation error:

[0219]

[0220] The second step is to determine the error. The expected step size of the task space generated at the current task stage .

[0221] The third step is based on the current location. Query the dynamic Jacobian matrix field (which has been obtained through online updates after the closed-loop execution of the previous control cycle) to obtain candidate local Jacobian matrices. .

[0222] The fourth step is to perform a rationality check on the candidate local Jacobian matrices to obtain the control Jacobian matrix. .

[0223] Fifth step, according to , Previous frame drives increment and current individual constraints of the robotic arm The effective increment of the current driver is solved by QP. .

[0224] Step 6, Input the execution layer compensation module to obtain the actual command. .

[0225] Step 7: Execute the command and enter the next control cycle, continuing closed-loop control using the new end-position feedback.

[0226] Step 8: When the update gating conditions are met, after closed-loop execution and before querying the next control cycle, adjust the actual end displacement... displacement predicted by the model The residuals between them are used to update the parameters of the spatial correction term, thereby updating the spatial correction term. This updates the dynamic Jacobian matrix field.

[0227] The above steps one through eight are executed repeatedly until the target point meets the stability criteria or the dynamic trajectory task is completed.

[0228] Furthermore, to verify the effectiveness of the adaptive closed-loop control system in this embodiment, two closed-loop positioning experiments were conducted on a prototype of a three-axis miniature serially driven robotic arm. This prototype is formed by multiple miniature linearly driven basic units connected in series. The end-effector position is obtained by an external measurement system. After receiving the current end-effector position and the target position, the controller outputs drive commands according to the link described in steps one through eight above: "task space step size generation - dynamic Jacobian matrix field - QP solution - execution layer compensation". The specific procedures and results of the two closed-loop positioning experiments are as follows:

[0229] Experiment 1: Nine-point closed-loop positioning experiment

[0230] This experiment sets up 9 target points in a 3D workspace, and visits 9 waypoints sequentially in each experiment. To avoid the influence of occasional startup states on the statistical results, five consecutive experiments (R4–R8) are selected as representative data, containing a total of [data missing]. One waypoint. Success is determined by the end-point error entering the 15µm threshold and remaining stable for 15 frames.

[0231] like Figure 4 As shown, the nine-point closed-loop positioning result diagram is displayed. The diagram shows the closed-loop positioning result of the three-axis micro-serialized robotic arm at nine target points. The upper part shows the relationship between the target point and the final position in the XY and XZ projections; the lower part shows the distribution of the residual error after convergence in the XY and XZ error planes. The dashed circle represents the 15µm threshold.

[0232] like Figure 5 As shown, the error convergence curves for each waypoint in five nine-point closed-loop experiments (R4–R8) are illustrated. The horizontal axis represents the relative time within the current waypoint, and the vertical axis represents the terminal error. Logarithmic coordinates are used to simultaneously display the initial error at the millimeter level and the final threshold at 15µm. This figure illustrates the entire process of closed-loop control from coarse convergence to fine positioning.

[0233] like Figure 6 As shown, the graph illustrates the final error and convergence time statistics of the nine-waypoint closed loop. It displays the final error distribution at the nine waypoints, the convergence time distribution reaching the 15µm stability threshold, and the segmented convergence time statistics. This graph demonstrates that closed-loop control can not only reach the vicinity of the target point but also quantitatively assess convergence accuracy and speed.

[0234] The experimental results show that the median final error for successful samples was 8.20 µm, and the 90th percentile final error was 12.81 µm; the median total convergence time to reach and maintain a stable error of 15 µm was 7.14 s. This demonstrates that closed-loop control can stably compress the end-point positioning error to the micrometer level.

[0235] Experiment 2: Static Circular Trajectory Waypoint Closed-Loop Experiment

[0236] To verify the stability of this method under a large number of continuous waypoints, a static circular trajectory experiment with a radius of 2 mm was conducted. In this experiment, the circular trajectory was discretized into 180 phase points, each phase point serving as an independent waypoint. The closed-loop controller converged before moving to the next point. In the XZ plane, five consecutive, completely successful laps (L1–L5) were selected, containing a total of [data missing]. One waypoint.

[0237] like Figure 7The figure shows the distribution of final error and convergence time as a function of phase in a static circular trajectory with a radius of 2 mm in the XZ plane. The figure displays the distribution of final error and convergence time as a function of phase for 900 waypoints. Gray hollow dots represent samples of each phase in each loop, the black solid line represents the phase median, and the dashed line represents the 90th percentile. This figure is used to illustrate the distribution characteristics of closed-loop error and convergence time in the trajectory phase under a large number of waypoints.

[0238] like Figure 8 As shown, the figure displays the final error and piecewise convergence time statistics of the static circular trajectory. The figure shows the box plot of the final error for 5 consecutive loops of the static circle in the XZ plane, spanning 900 waypoints, as well as the distribution statistics of the total settling time from the starting point to 100µm, from 100µm to 30µm, and from 30µm to 15µm and maintaining stability. This figure is used to illustrate the stable convergence capability of the present invention under a large number of consecutive waypoints.

[0239] In summary, the experimental results show that all 900 waypoints converged successfully in this experiment. The median final error was 11.51 µm, the 90th percentile final error was 14.09 µm, and the maximum final error was 15.00 µm. The median total settling time was 6.92 s, and the range from 25% to 75% was 1.96–10.78 s.

[0240] Therefore, the static circle experiment further demonstrates that the closed-loop control method can not only complete the localization of a small number of discrete points, but also maintain stable convergence under a large number of waypoint conditions.

[0241] The embodiments described above are merely some preferred implementations of the present invention and are not intended to limit the invention. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the invention. Therefore, all technical solutions obtained through equivalent substitution or transformation fall within the protection scope of the present invention.

Claims

1. A dynamic Jacobi-QP closed-loop control method for a micro-manipulator, characterized in that, The following steps are performed in each control cycle: S1. Receive the current position and target position of the end effector of the micro-robotic arm, and calculate the difference between the two as the task space error; S2. Generate the desired end-point correction step size in the task space based on the task space error; S3. Based on the current position, retrieve the local Jacobian matrix at the current position from the dynamic Jacobian matrix field; wherein, the dynamic Jacobian matrix field represents the mapping relationship between the drive increment and the end effector displacement of the micro-manipulator in the form of the sum of the reference Jacobian matrix and the spatial correction term; the spatial correction term is updated online based on the actual motion residual after the closed-loop execution of the previous control cycle; the actual motion residual is obtained by subtracting the predicted displacement of the effective drive increment of the previous control cycle from the difference of the end effector positions measured in two consecutive control cycles; S4. Based on the expected end correction step size, the local Jacobian matrix, the effective driving increment of the previous control cycle, and the current set of individual constraints of the micro-manipulator, solve the quadratic programming problem to obtain the effective driving increment of the current control cycle. S5. The effective incremental drive is converted into an actual drive command after being compensated and limited by the execution layer, and output to the driver of the micro-manipulator. Then, the next control cycle is entered, and closed-loop control is continued using the new end position feedback.

2. The dynamic Jacobi-QP closed-loop control method for a micro-manipulator according to claim 1, characterized in that, In step S2, an error feedback control method is used to generate the desired end correction step size based on the task space error, and the desired end correction step size is subject to a modulus limit so that it does not exceed the preset maximum task space step size.

3. The dynamic Jacobi-QP closed-loop control method for a micro-manipulator according to claim 1, characterized in that, The spatial correction term is represented as a weighted combination of spatial basis functions, obtained by summing the products of the values ​​of each spatial basis function at the current position and the corresponding parameter matrix; the spatial basis functions are any one of the following: polynomial basis functions, radial basis functions, local linear models, spline models, lookup table interpolation models, neural network models, Gaussian process models, or local window least squares models.

4. The dynamic Jacobi-QP closed-loop control method for a micro-manipulator according to claim 3, characterized in that, The method for online updating the parameters of the spatial correction term is as follows: each parameter matrix is ​​expanded into a parameter vector, and the actual motion residual is represented as a linear parameter model composed of the parameter vector, the observation matrix, and the observation noise. The observation matrix is ​​constructed from the spatial basis function values ​​at the starting position of the motion and the effective driving increment of the previous control cycle. Then, Kalman filtering is used to recursively update the parameter vector, wherein the parameter vector is modeled as a random walk process during the Kalman filtering process, and the covariance update adopts the Joseph form.

5. The dynamic Jacobi-QP closed-loop control method for a micro-manipulator according to claim 1, characterized in that, The online update of the spatial correction term is only performed when the update gating conditions are met; otherwise, the update of the current control cycle is skipped and the previous valid parameter is retained. The update gating conditions include: the effective driving increment is greater than the minimum effective excitation threshold, the end position observation is valid, the actual motion residual does not exceed the abnormal threshold, the current control cycle is not in the stable holding phase, and the current control cycle does not trigger the execution layer compensation pulse.

6. The dynamic Jacobi-QP closed-loop control method for a micro-manipulator according to claim 1, characterized in that, In step S3, a rationality check is also required on the candidate Jacobian matrix obtained after online update. If the candidate Jacobian matrix passes the rationality check, the candidate Jacobian matrix is ​​adopted, or the spatial correction term is weighted according to the preset fusion coefficient and summed with the reference Jacobian matrix to obtain the local Jacobian matrix. If the update fails, the result is rejected, and the baseline Jacobian matrix or the valid Jacobian matrix that passed the rationality check in the previous control cycle is used as the local Jacobian matrix.

7. The dynamic Jacobi-QP closed-loop control method for a micro-manipulator according to claim 1, characterized in that, In S4, the quadratic programming problem is solved by minimizing the objective function, which is obtained by weighting the following three terms: a tracking term that makes the product of the local Jacobian matrix and the driving effective increment approximate the desired end correction step size, a regularization term that suppresses the magnitude of the driving effective increment, and a smoothing term that suppresses abrupt changes in the driving effective increment between adjacent control cycles. Preferably, the set of individual constraints includes at least one of the following: upper and lower limits of the actuator of the micromanipulator, maximum drive increment per frame, speed limit, acceleration limit, joint limit, workspace boundary, contact safety boundary, and task phase safety boundary; Preferably, the micro-robotic arm has three driving inputs, and the individual constraint set is the upper and lower bound box constraints of each driving input; the quadratic programming problem is solved using the three-dimensional box-constrained quadratic programming (box-QP) active set enumeration method: each driving variable is placed in one of three states: lower bound, free, or upper bound; all active constraint combinations are enumerated; under each combination, the free variables are analytically solved; and the solution that minimizes the objective function cost is selected from all feasible candidate solutions as the effective driving increment.

8. The dynamic Jacobi-QP closed-loop control method for a micro-manipulator according to claim 1, characterized in that, The execution layer compensation includes one or more of the following: dead zone compensation, take-up compensation, anti-jamming pulse compensation, micro-jitter compensation, hysteresis compensation, directional hysteresis compensation, static friction compensation, piezoelectric hysteresis compensation, and compliant mechanism microstepping compensation; the safety limit includes at least one of the following: speed limit, acceleration limit, travel safety limit, and mission safety boundary limit.

9. The dynamic Jacobi-QP closed-loop control method for a micro-manipulator according to claim 1, characterized in that, It also includes at least one of dynamic trajectory phase management and stability determination; the dynamic trajectory phase management specifically involves: when performing a dynamic trajectory task, if the norm of the task space error exceeds a first error threshold, the phase advancement of the target trajectory is paused or slowed down; when the norm of the task space error is lower than a second error threshold and remains below a preset number of frames, the phase advancement of the target trajectory continues, wherein the second error threshold is less than the first error threshold; the stability determination specifically involves: if the norm of the task space error is less than a stability threshold for a preset number of frames, the current target point positioning is determined to be complete.

10. A dynamic Jacobi-QP closed-loop control system for a micro-robotic arm, characterized in that, It includes functional modules constructed by computer programs, said functional modules being configured to implement the dynamic Jacobi-QP closed-loop control method for a micro-manipulator as described in any one of claims 1 to 9.

Images