Inverse kinematics control method for surgical robot

Abstract

The invention relates to the technical field of surgical robots, and discloses an inverse kinematics control method for a surgical robot, which comprises the following steps: establishing a kinematics model of the surgical robot, including the position and posture of an end effector, a joint angle and a Jacobian matrix, and defining an operability index; constructing a hierarchical optimization framework based on operation task priorities, decomposing an inverse kinematics problem into RCM constraint, end effector pose control, joint limiting constraint and operability maximization tasks, and sequencing according to the following priorities: the RCM constraint is the highest priority, the end effector pose control and the joint limiting constraint are the secondary priorities, and the operability maximization task is the highest priority; the maximization of the operability is the lowest priority; and sequentially solving the optimal joint speed of each priority task according to a priority sequence through a hierarchical quadratic programming framework, and ensuring that the solution of the low-priority task is optimized in the null space of the high-priority task by utilizing a null space projection matrix. The operability of the robot is maximized.

Description

Technical Field

[0001] The present invention relates to the technical field of surgical robots, and more particularly, to an inverse kinematics control method for surgical robots. Background Art

[0002] Robot-assisted Minimally Invasive Surgery (RMIS) reduces patient trauma and accelerates postoperative recovery through small incision operations. However, its core technical challenge lies in how to achieve high-precision and high-flexibility robot motion control in a restricted anatomical space. Inverse Kinematics (IK), as the core module of surgical robot motion planning, needs to simultaneously satisfy multiple task constraints (such as Remote Center of Motion (RCM) constraint, joint limits, end-effector trajectory tracking) and optimize motion flexibility.

[0003] However, most existing IK methods cannot simultaneously meet the multi-objective requirements such as Remote Center of Motion (RCM) constraint, joint limits, and manipulability optimization, and still have obvious deficiencies in dealing with RCM constraint, manipulability optimization, joint limits, and multi-task optimization. These problems limit the application of surgical robots in complex surgeries, especially in scenarios that require high precision and high flexibility. Summary of the Invention

[0004] The object of the present invention is to propose an inverse kinematics control method for surgical robots, which maximizes the manipulability of the robot while satisfying the remote center of motion constraint and joint limits.

[0005] To achieve the above object, the present invention proposes an inverse kinematics control method for surgical robots, including:

[0006] Step 1: Establish a kinematic model of the surgical robot, including the position and orientation of the end effector, joint angles, and Jacobian matrix; define a manipulability index for measuring the flexibility of the robot, and the manipulability index is calculated based on the Jacobian matrix;

[0007] Step 2: Construct a hierarchical optimization framework based on the surgical task priority, decompose the inverse kinematics problem into RCM constraint, end-effector pose control, joint limit constraint, and manipulability maximization tasks, and sort them according to the following priorities: RCM constraint is the highest priority, end-effector pose control and joint limit constraint are the secondary priorities, and manipulability maximization is the lowest priority;

[0008] Step 3: In the highest priority layer, through the calculation of the RCM constraint Jacobian matrix and residuals, ensure that the surgical instrument moves around the fixed incision point and minimize the incision point position error;

[0009] Step 4: In the secondary priority layer, based on the difference between the current pose and the target pose of the end effector, construct a pose control task, and drive the surgical instrument to move precisely along the preset trajectory by optimizing the residual of the end effector pose control task;

[0010] Step 5: Synchronously impose joint limit constraints in the secondary priority layer, and avoid the robot entering an unreachable configuration by restricting the upper and lower bounds of the joint velocity;

[0011] Step 6: In the lowest priority layer, based on the gradient projection method of the manipulability index, construct a manipulability maximization optimization task to optimize the robot joint velocity to maximize the motion flexibility;

[0012] Step 7: Under the hierarchical optimization framework, through the hierarchical quadratic programming framework, solve the optimal joint velocities of each priority task in sequence according to the priority order. During the solution process, use the null space projection matrix to ensure that the solution of the low-priority task is optimized in the null space of the high-priority task;

[0013] Step 8: Calculate and output the velocity commands of the robot joints in real time to achieve precise motion control of the surgical tool.

[0014] Optionally, the calculation formula of the manipulability index in Step 1 is:

[0015]

[0016] where: \(m(q)\) is the manipulability index; \(J\) is the spatial Jacobian matrix of the end effector; \(J^T\) T is the transpose matrix of the Jacobian matrix \(J\); \(\det(JJ^T)\) T is the determinant of the product of the Jacobian matrix \(J\) and its transpose \(J^T\), which is used to describe the manipulability of the robot and is a scalar value. When \(\det(JJ^T)\) T is close to zero, the robot may be close to a singularity, and at this time the flexibility of the robot decreases; when \(\det(JJ^T)\) T is large, the robot has high flexibility. T)

[0017] Optionally, the hierarchical optimization framework in Step 2 is defined by the following global optimization formula:

[0018]

[0019] where: is the Jacobian matrix of task \(i\), which describes the mapping from the task space to the joint space; is the residual vector of task \(i\), which represents the deviation between the current state and the target; is the weight coefficient of task i, used to adjust the task priority, where the task weight of RCM is K t1 , the weight of the end - effector pose control task is K t2 , the weight of the manipulability task is K t3 ; is the residual gain coefficient of task i, used to scale the influence of the residual term; is the joint velocity vector; W is the slack variable, used to handle the violation of the inequality constraint; K d 、K w are the gain coefficients of the damping term and the slack variable, used to prevent numerical instability; C p is the inequality constraint matrix, used to include the hard limits defining the joint limits; n is the number of tasks.

[0020] Optionally, the Jacobian matrix and the residual of the RCM constraint described in step 3 are calculated by the following formula:

[0021]

[0022] e rcm =||p trocar -p rcm ||

[0023] where: J rcm is the Jacobian matrix of the RCM constraint, e rcm is the RCM residual; is the unit direction vector of the surgical instrument axis; is the transpose of; p r is the position vector of the reference point on the surgical instrument axis, used to construct the geometric relationship of the RCM constraint; is the transpose of p r ; J pre is the Jacobian matrix of the instrument tip position; is the partial derivative of the instrument axis direction with respect to the joint position; I is the 3×3 identity matrix, used for projection calculation; P trocar is the position of the fixed incision point; P rcm is the actual position of the surgical instrument.

[0024] Optionally, the residual of the end - effector pose control task described in step 4 is calculated by the following Lie group mapping:

[0025]

[0026] where: e ee is the residual of the end - effector pose control task; T desired 、T currentHomogeneous transformation matrices for the desired and actual end - effector poses respectively; T desired , T current ∈SE(3), where SE(3) represents the special Euclidean group in three - dimensional space, used to describe the position and orientation of the robot end - effector, and contains all possible combinations of three - dimensional translations and rotations; is the inverse matrix of T desired ; log 6 represents mapping the pose error to a six - dimensional vector in the Lie algebra space, including translational and rotational components.

[0027] Optionally, the joint limit constraints described in step 5 are defined by the following inequalities:

[0028]

[0029] where: q - , q + are the lower and upper bound vectors of the joint position respectively; q is the current joint position; δt is the control cycle time, used to convert the position constraint into a velocity constraint, is the joint velocity vector.

[0030] Optionally, the task of maximizing the manipulability described in step 6 is achieved through the following optimization problem:

[0031]

[0032] where: is the gradient vector of the manipulability index m(q), calculated in real - time by the numerical difference method; is transpose; △t is the discretization time step, used to project the gradient into the joint velocity space; is the joint velocity vector, and m(q) is the manipulability index.

[0033] Optionally, the hierarchical quadratic programming framework described in step 7 is solved by the following iterative formula:

[0034] and

[0035] where: is the optimal joint velocity vector of the current priority layer p; is the optimal joint velocity vector of the priority layer p - 1; I is the identity matrix; N p-1 is the null - space projection matrix of the layer task (priority layer p - 1); J p-1 is the Jacobian matrix of the high - level task (priority layer p - 1); is the Jacobian matrix J p-1The damping pseudo-inverse matrix.

[0036] Optionally, the null space projection matrix N p-1 is constructed through the following steps:

[0037] Perform singular value decomposition on the high-level task Jacobian matrix J p-1 and eliminate the directions with singular values less than the threshold ∈ = 1×10 -6 .

[0038] Calculate the pseudo-inverse of the Jacobian matrix using the following formula:

[0039]

[0040] where is the transpose of J p-1 ; λ is the damping coefficient used to avoid numerical singularity.

[0041] Optionally, the method further includes:

[0042] Step 9: Monitor the manipulability index m(q) in real time. If it is lower than the preset threshold, dynamically adjust the task weights, and increase the weight K t2 of the end effector pose control task to twice the original value to prioritize ensuring the trajectory tracking accuracy.

[0043] The beneficial effects of the present invention are as follows:

[0044] By decomposing the inverse kinematics problem into three levels: RCM constraint (highest priority), end effector pose control and joint limit constraint (secondary priority), and manipulability optimization (lowest priority), and using a hierarchical quadratic programming (HQP) framework for global optimization, the RCM constraint is regarded as a hard task to ensure that the surgical instrument moves strictly around the fixed incision point, avoiding intraoperative tissue damage; in the secondary priority layer, based on the difference between the current pose and the target pose of the end effector, a pose control task is constructed, and the residual of the end effector pose control task is optimized to drive the surgical instrument to move precisely along the preset trajectory. On the premise of ensuring safety, it can meet the sub-millimeter accuracy requirements of minimally invasive surgery; the lowest priority manipulability optimization task maximizes the robot's flexibility through the gradient projection method, significantly enhancing the operation ability in complex anatomical environments; through the hierarchical quadratic programming framework, the optimal joint velocities of each priority task are solved sequentially according to the priority order. During the solution process, the null space projection matrix is used to ensure that the solution of the low-priority task is optimized in the null space of the high-priority task, enabling efficient and stable real-time solution; thus, the present invention realizes maximizing the flexibility and motion performance of the robot while meeting the requirements of surgical safety and accuracy. The method of the present invention can be adapted to platforms such as the da Vinci surgical robot, providing a safer, more precise and flexible motion control solution for minimally invasive surgery.

[0045] The system of the present invention has other characteristics and advantages, which will be apparent from the accompanying drawings incorporated herein and the subsequent detailed description, or will be described in detail in the accompanying drawings incorporated herein and the subsequent detailed description, and these accompanying drawings and detailed description are jointly used to explain the specific principles of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] By describing the exemplary embodiments of the present invention in more detail in conjunction with the accompanying drawings, the above and other objects, features and advantages of the present invention will become more apparent. In the exemplary embodiments of the present invention, the same reference numerals generally represent the same components.

[0047] Figure 1 The flowchart showing the steps of an inverse kinematics control method for a surgical robot according to the present invention is shown. DETAILED DESCRIPTION

[0048] Aiming at the problem that the existing IK methods cannot simultaneously meet the multi-objective requirements such as RCM constraint, joint limit, and manipulability optimization, the present invention proposes an inverse kinematics control method for a surgical robot. The core of the present invention lies in maximizing the motion flexibility of the robot on the premise of ensuring surgical safety based on the hierarchical quadratic programming (HQP) framework through strict priority division and null space projection mechanism.

[0049] The technical principle is as follows:

[0050] 1. Task Hierarchy and Hard Priority Guarantee: The surgical tasks are divided into RCM constraint (highest priority), end-effector pose tracking (secondary priority), and manipulability optimization (lowest priority) according to clinical importance. The HQP framework is used to ensure that high-priority tasks are absolutely prioritized, and low-priority tasks are only optimized in their null space.

[0051] 2. Programmable RCM Constraint: By dynamically calculating the RCM Jacobian matrix and the residual, the traditional mechanical RCM is replaced to improve intraoperative adaptability.

[0052] 3. Gradient Projection Optimization: Using the gradient information of the manipulability index, the motion flexibility is optimized in real time in the redundant degrees of freedom to avoid singular configurations.

[0053] The present invention will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to make the present invention more thorough and complete, and to fully convey the scope of the present invention to those skilled in the art.

[0054] As Figure 1As shown in the figure, a method for inverse kinematics control of a surgical robot according to an embodiment of the present invention includes the following steps:

[0055] Step 1: Establish a kinematic model of the surgical robot, including the position and orientation of the end effector, joint angles, and the Jacobian matrix; define a manipulability index for measuring the flexibility of the robot, and the manipulability index is calculated based on the Jacobian matrix;

[0056] Specifically, in this step, a kinematic model is established. The kinematic model of the surgical robot includes the position and orientation of the end effector, joint angles, and the Jacobian matrix J. The Jacobian matrix describes the relationship between the position and orientation of the end effector and the joint angles.

[0057] In this step, the manipulability index is defined as:

[0058]

[0059] Where:

[0060] m(q): The manipulability index, which is an index for measuring the motion flexibility of the robot under the current joint configuration q, reflects the ability of its end effector to generate speed and force in the task space. The larger its value, the farther the robot is from the singular configuration, and the stronger the motion flexibility and manipulation ability; when the value approaches zero, the robot is close to the singular state and the motion ability is limited;

[0061] The spatial Jacobian matrix of the end effector, which is a 6-dimensional column vector, n q is the degree of freedom of the surgical robot joint, and R represents a real number matrix;

[0062] det(JJ T ): The determinant of the product of the Jacobian matrix J and its transpose J T is used to describe the manipulability of the robot and is a scalar value, reflecting the hypervolume (square of the volume) spanned by the Jacobian matrix, characterizing the "effective workspace" of the robot's motion ability; when det(JJ T ) approaches zero, the robot may be close to the singular point, and at this time the flexibility of the robot decreases; when det(JJ T) is large, the robot has high flexibility.

[0063] Step 2: Construct a hierarchical optimization framework based on the surgical task priority, decompose the inverse kinematics problem into RCM constraints, end effector pose control, joint limit constraints, and manipulability maximization tasks, and sort them according to the following priorities: RCM constraints are the highest priority, end effector pose control and joint limit constraints are the secondary priorities, and manipulability maximization is the lowest priority;

[0064] Specifically, in this step, a hierarchical optimization framework is constructed. First, the tasks are decomposed as follows:

[0065] Highest-priority task: RCM constraint to ensure that the instrument moves around the fixed incision point;

[0066] Second-priority task: End-effector pose control and joint limit constraints;

[0067] Lowest-priority task: Maximization of manipulability.

[0068] Then, a global optimization problem is modeled, and the hierarchical optimization framework is defined by the following global optimization formula:

[0069]

[0070]

[0071] Where:

[0072] The Jacobian matrix of task i, which describes the mapping from the task space to the joint space. m is the dimension of the position and orientation of the end effector of the surgical robot, and n q is the degree of freedom of the joints of the surgical robot. R represents the real number matrix;

[0073] The residual vector of task i, which represents the deviation between the current state and the target;

[0074] The weight coefficient of task i, which is used to adjust the task priority. The task weight of RCM is K t1 , the task weight of end-effector pose control is K t2 , and the task weight of manipulability is K t3 ; Preferably, in this embodiment, it is set that: the task weight K t1 of the remote center of motion = 1.0, the task weight K t2 of end-effector pose control = 1.0, and the task weight K t3 of manipulability = 0.01;

[0075] The residual gain coefficient of task i, which is used to scale the influence of the residual term;

[0076] The joint velocity vector;

[0077] The slack variable, which is used to handle the violation of the inequality constraint;

[0078] The damping term and the gain coefficient of the slack variable to prevent numerical instability; Preferably, in this embodiment, the damping gain K is setd = 1×10 -9 , slack variable gain K w = 1×10 -5 ;

[0079] Inequality constraint matrix, used to define hard constraints such as joint limits.

[0080] Through the hierarchical optimization framework, the priority of the RCM constraint is strictly higher than that of other tasks, ensuring that the incision point error ≤ 0.007 mm (simulation result).

[0081] Step 3: In the highest priority layer, by calculating the RCM constraint Jacobian matrix and the residual, ensure that the surgical instrument moves around the fixed incision point to minimize the incision point position error;

[0082] Specifically, first define the RCM residual calculation as:

[0083] e rcm = ||p trocar - p rcm ||

[0084] Where:

[0085] e rcm is the RCM residual;

[0086] Position of the fixed incision point;

[0087] Actual position of the surgical instrument.

[0088] Then, construct the RCM Jacobian matrix:

[0089]

[0090] e rcm = ||p trocar - p rcm ||

[0091] Where:

[0092] J rcm is the Jacobian matrix of the RCM constraint;

[0093] Unit direction vector of the surgical instrument axis, ps is the instrument axis vector;

[0094] Position vector of the reference point on the surgical instrument axis, used to construct the geometric relationship of the RCM constraint;

[0095] Jacobian matrix of the instrument tip position;

[0096] Partial derivative of the instrument axis direction with respect to the joint position, used to describe the influence of joint movement on the axis direction;

[0097] 3×3 identity matrix, used for projection calculation.

[0098] Calculate the partial derivative of the instrument axis direction with respect to the joint position When doing so, the central difference method is used for numerical approximation. The specific formula is as follows:

[0099]

[0100] Where:

[0101] is the current joint position vector, with dimension n q is the degree of freedom of the robot joint; for example: for a 7-degree-of-freedom surgical robot, q = [q 1 , q 2 , …, q 7 T ;

[0102] The differential step size of the j-th joint, used for numerical approximation of the partial derivative. Its value is preferably: Δq j = 1×10 -6 rad, balancing calculation accuracy and numerical stability;

[0103] The unit direction vector of the instrument axis, calculated from forward kinematics, and its calculation formula is:

[0104]

[0105] Where, is the instrument tip position; is the instrument base position (reference point);

[0106] The partial derivative of the instrument axis direction with respect to the j-th joint position, describing the influence of joint movement on the axis direction, which is a three-dimensional vector corresponding to the x, y, z direction components.

[0107] In the specific implementation process, first apply a positive perturbation q j +Δq j to the j-th joint, and calculate the axis direction after the perturbation Then apply a negative perturbation q j -Δq j ​, calculate the perturbed axis direction Then calculate the partial derivatives using the central difference formula For each joint j = 1, 2, …, n q Independently calculate the partial derivatives to generate the complete Jacobian matrix block:

[0108] The implementation process and principle of this step are as follows:

[0109] The Jacobian matrix J of the RCM constraint rcm Consists of two parts, corresponding to the dynamic adjustment of the instrument position and the axis direction respectively:

[0110] (1) Position projection term:

[0111]

[0112] Among them, is the 3×3 identity matrix, which is used for projection calculation: project the instrument tip velocity onto the plane perpendicular to the axis to restrict its rotation around the incision point; is the projection matrix of the instrument axis direction, is the unit vector.

[0113] (2) Axis direction compensation term:

[0114]

[0115] Among them, represents the position vector of a fixed reference point on the instrument axis (such as the instrument base point), which is used to calculate the coupling effect of the axis direction change on the joint velocity; is the scalar projection, representing the displacement of the reference point along the axis direction.

[0116] Through the geometric association of p r The RCM constraint can adapt to the small changes in the instrument axis direction in real time, reducing the error fluctuation by 50%; the introduction of the identity matrix I avoids the singularity problem and ensures the numerical stability of the projection operation. The programmable RCM constraint enables the robot to dynamically adapt to the intraoperative incision point offset error controlled at the sub-millimeter level.

[0117] Step 4: In the secondary priority layer, based on the difference between the current pose and the target pose of the end effector, construct a pose control task, and drive the surgical instrument to move precisely along the preset trajectory through the optimization of the residual of the end effector pose control task;

[0118] Specifically, this step performs end effector pose control, and the residual of the end effector pose control task is calculated through the following Lie group mapping:

[0119]

[0120] Wherein:

[0121] e ee : The residual of the end - effector pose control task;

[0122] T desired , T current ∈ SE(3): The homogeneous transformation matrices of the desired end - effector pose and the actual end - effector pose respectively; SE(3) represents the special Euclidean group in three - dimensional space, which is used to describe the position and orientation of the robot end - effector and contains all possible combinations of three - dimensional translations and rotations;

[0123] T desired The inverse matrix of;

[0124] log 6 is The operation, representing the mapping from the Lie group to the Lie algebra, outputs a six - dimensional vector [ω, v], where is the rotation component, is the translation component; is the Lie algebra of SE(3).

[0125] The optimization objective is:

[0126]

[0127] Wherein, is the spatial Jacobian matrix of the end - effector, which is updated in real - time by the recursive Newton - Euler algorithm (RNEA).

[0128] By adopting the Lie group mapping to avoid Euler - angle singularities, the pose tracking error ≤ 1.1×10 -6 (simulation data).

[0129] Step 5: Synchronously impose joint limit constraints in the secondary priority layer. By restricting the upper and lower bounds of the joint velocity, the robot is prevented from entering an unreachable configuration;

[0130] Specifically, the joint limit constraints described in this step are defined by the following inequalities:

[0131]

[0132] Wherein:

[0133] The lower and upper bound vectors of the joint position;

[0134] The current joint position;

[0135] The control cycle time is used to convert the position constraint into a velocity constraint. In this embodiment, the preferred control cycle time δt = 1 ms.

[0136] The joint velocity is strictly restricted within the physically feasible range by the joint limit constraint to avoid damage to the mechanical structure.

[0137] Step 6: In the lowest-priority layer, based on the gradient projection method of the manipulability index, construct an optimization task for maximizing manipulability, and optimize the robot joint velocity to maximize the motion flexibility;

[0138] Specifically, for the maximization optimization of manipulability in this step, first calculate the manipulability index m(q):

[0139]

[0140] where is the spatial Jacobian matrix of the end effector; det(JJ T ) is the determinant of the product of the Jacobian matrix J and its transpose J T , which is used to describe the manipulability of the robot and is a scalar value; when det(JJ T ) is close to zero, the robot may be close to a singular point, and at this time the flexibility of the robot decreases; when det(JJ T) ) is relatively large, the robot has higher flexibility.

[0141] Then, perform gradient projection optimization. The maximization task of manipulability is achieved through the following optimization problem:

[0142]

[0143] where:

[0144] The gradient vector of the manipulability index m(q), which is used to guide the optimization direction of the joint velocity , so that the robot moves towards a more flexible configuration. It is calculated in real time by the numerical difference method, and the preferred difference step size Δq = 1×10 -6 rad; is the transpose of ;

[0145] The discretized time step, which is used to project the gradient into the joint velocity space. The preferred value is Δt = 1 ms, which is synchronized with the controller;

[0146] m(q) represents the manipulability index and is calculated by .

[0147] Calculate the gradient of the manipulability index m(q) by the central difference method The specific formula is as follows:

[0148]

[0149] Where:

[0150] represents the current joint position vector, with dimension n q is the joint degree of freedom of the robot. For example, for a 7-degree-of-freedom surgical robot, q = [q 1 , q 2 , …, q 7 T .

[0151] is the differential step size of the j-th joint, used for numerical approximation of partial derivatives; its value is preferably: Δq j = 1×10 -6 rad, which can balance calculation accuracy and numerical stability;

[0152] is the partial derivative of the manipulability index with respect to the position of the j-th joint,

[0153] Gradient vector

[0154] In this step, the motion flexibility of the robot is maximized by optimizing m(q), ensuring that the posture can be efficiently adjusted in complex surgical scenarios (such as operations in narrow cavities), avoiding being trapped in singular configurations. In the simulation of (7 degrees of freedom + 5 degrees of freedom tool), the manipulability is increased by 146%, significantly enhancing the operation ability in narrow cavities.

[0155] Step 7: Under the hierarchical optimization framework, through the hierarchical quadratic programming framework, the optimal joint velocities of each priority task are solved in order of priority. During the solution process, the null space projection matrix is used to ensure that the solutions of low-priority tasks are optimized in the null space of high-priority tasks;

[0156] Specifically, this step performs hierarchical quadratic programming (HQP) solution, and the specific process includes:

[0157] (1) Construction of the null space projection matrix:

[0158]

[0159] Where, n q ×n q dimensional identity matrix;

[0160] Null space projection matrix of the high-level task (priority layer p - 1);

[0161] ​ The Jacobian matrix of the high-level task (priority level p-1), where m is the task dimension;

[0162] The Jacobian matrix J p-1 The damped pseudo-inverse matrix of.

[0163] Then, iterative solution is performed:

[0164]

[0165] Where:

[0166] The optimal joint velocity vector of the current priority level p, where n q is the joint degree of freedom;

[0167] The optimal joint velocity vector of priority level p-1;

[0168] The null space projection matrix N mentioned above p-1 The construction process includes the following steps:

[0169] Perform singular value decomposition on the high-level task Jacobian matrix J p-1 and eliminate the directions with singular values less than the threshold ∈ = 1×10 -6 ;

[0170] Calculate the pseudo-inverse of the Jacobian matrix using the damping coefficient λ:

[0171]

[0172] Where, is the transpose of J p-1 ; λ is the damping coefficient used to avoid numerical singularity, preferably λ = 1×10 -4 .

[0173] Preferably, the OSQP quadratic programming solver is used for solution, and the single solution time can be ≤1ms, meeting the real-time control requirements.

[0174] Step 8: Calculate and output the velocity command of the robot joint in real time to achieve precise motion control of the surgical tool.

[0175] In this embodiment, the method further includes:

[0176] Step 9: Monitor the manipulability index m(q) in real time. If it is lower than the preset threshold, dynamically adjust the task weight, and increase the weight K t2 of the end effector pose control task to twice the original value to give priority to ensuring the trajectory tracking accuracy.

[0177] Specifically, during the precise motion control of the surgical tool, the manipulability index m(q) is monitored in real time. When it is lower than the preset threshold, the task weights are dynamically adjusted. For example, for a 7-degree-of-freedom surgical robot, if at a certain moment m(q) = 0.5, it indicates that it is in a state of high flexibility. If m(q) is lower than m min = 0.1, then it is determined to be close to the singular configuration. Then the weight update is executed. For example, the task weight K t2 of the end-effector pose is increased from 1.0 to 2.0; the task weight K t1 of the RCM task remains unchanged at 1.0.

[0178] By dynamically adjusting the task weights, the trajectory tracking accuracy is preferentially guaranteed in the singular region, and the error fluctuation is reduced.

[0179] In summary, an inverse kinematics control method for a surgical robot according to an embodiment of the present invention realizes the maximization of the robot's manipulability while satisfying the remote center of motion (RCM) constraint and joint limits through a hierarchical quadratic programming framework. At the same time, compared with the prior art, the present invention also has the following technical effects:

[0180] 1. Improve surgical safety

[0181] Strict satisfaction of the RCM constraint: By setting the RCM constraint as the highest-priority task, the present invention ensures that the surgical tool always rotates around the incision point, avoiding tissue damage. This improvement significantly improves the safety of the surgery and reduces the risk of surgical complications.

[0182] Strict compliance with joint limits: By introducing joint limits into the optimization problem, the present invention ensures that the robot joint angles always remain within the physically feasible range, avoiding joint overload and damage, and further improving the safety of the surgery.

[0183] 2. Improve robot flexibility

[0184] Maximization of manipulability: By optimizing the manipulability index m = det(JJT), the present invention significantly improves the flexibility of the robot. In a complex surgical environment, the robot can more flexibly adjust its posture, avoid singular points, and thus achieve more efficient surgical operations.

[0185] Multi-task optimization: Through the hierarchical quadratic programming framework, the present invention can simultaneously handle multiple tasks, including end-effector pose control, RCM constraint, and manipulability maximization. This multi-task optimization method enables the robot to achieve higher flexibility and motion performance while meeting the surgical safety and precision requirements.

[0186] 3. Achieve efficient real-time control

[0187] Fast solution ability: The present invention adopts an efficient quadratic programming solver (such as OSQP), which can solve complex optimization problems in a short time and meet the requirements of real-time control. Experiments show that the average calculation time of the present invention is less than 1 ms with RCM constraints and less than 0.3 ms without RCM constraints, significantly superior to the prior art.

[0188] Real-time feedback and adjustment: Combining the position and attitude information of the surgical tool obtained in real time by the sensor, the present invention can adjust the joint speed command in real time to ensure the accuracy and real-time performance of the surgical operation.

[0189] 4. Improve surgical accuracy

[0190] Precise control of the end effector attitude: The present invention optimizes the attitude control task of the end effector to ensure the precise position and attitude of the surgical tool. This improvement significantly improves the surgical accuracy, enabling the robot to perform more delicate operations in a complex surgical environment.

[0191] Reduce human error: By means of an automated and optimized inverse kinematics control method, the present invention reduces the errors caused by human operation and improves the overall quality of the surgery.

[0192] The embodiments of the present invention have been described above. The above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations are obvious to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments.

Claims

1. A surgical robot inverse kinematics control method, characterized in that: include: Step 1: Establish the kinematic model of the surgical robot, including the position and posture of the end effector, joint angles, and Jacobian matrix; A maneuverability index is defined, which is used to measure the flexibility of the robot, and the maneuverability index is calculated based on the Jacobian matrix; Step 2: Construct a hierarchical optimization framework based on surgical task priorities, decompose the inverse kinematics problem into RCM constraints, end effector posture control, joint limit constraints, and maneuverability maximization tasks, and sort them according to the following priorities: RCM constraints are the highest priority, end effector posture control and joint limit constraints are the second priority, and maneuverability maximization is the lowest priority; Step 3: In the highest priority layer, the RCM constrained Jacobian matrix and residual calculation are used to ensure that the surgical instrument moves around the fixed incision point and minimize the incision point position error; Step 4: In the secondary priority layer, based on the difference between the current posture of the end effector and the target posture, a posture control task is constructed, and the surgical instrument is driven to move precisely along the preset trajectory by optimizing the residual of the end effector posture control task; Step 5: Simultaneously apply joint limit constraints in the secondary priority layer to prevent the robot from entering an unreachable configuration by limiting the upper and lower bounds of the joint speed; Step 6: In the lowest priority layer, based on the gradient projection method of the maneuverability index, a maneuverability maximization optimization task is constructed to optimize the robot joint speed to maximize motion flexibility; Step 7: Under the hierarchical optimization framework, the optimal joint velocity of each priority task is solved in order of priority through the hierarchical quadratic programming framework. In the solution process, the null space projection matrix is ​​used to ensure that the solution of the low priority task is optimized in the null space of the high priority task; Step 8: Calculate and output the velocity instructions of the robot joints in real time to achieve precise motion control of surgical tools.

2. The inverse kinematics control method of a surgical robot according to claim 1, characterized in that: The calculation formula of the operability index in step 1 is: Where: m(q) is the maneuverability index; J is the spatial Jacobian matrix of the end effector; J T is the transposed matrix of the Jacobian matrix J; det(JJ T ) is the Jacobian matrix J and its transpose J T The determinant of the product of is used to describe the maneuverability of the robot. It is a scalar value. When det(JJ T ) is close to zero, the robot may be close to a singularity point, at which point the robot's flexibility decreases; when det(JJ T) When larger, the robot has higher flexibility.

3. The inverse kinematics control method for a surgical robot according to claim 2, characterized in that: The hierarchical optimization framework in step 2 is defined by the following global optimization formula: in: is the Jacobian matrix of task i, describing the mapping from task space to joint space; is the residual vector of task i, indicating the deviation between the current state and the target; is the weight coefficient of task i, which is used to adjust the task priority, where the task weight of RCM is K t1 , the weight of the terminal posture control task is K t2 , the weight of the operability task is K t3 ; is the residual gain coefficient of task i, which is used to scale the influence of the residual term; is the joint velocity vector; W is the slack variable used to handle the violation of the inequality constraint; K d , K w is the gain coefficient of the damping term and the relaxation variable, used to prevent numerical instability; is the inequality constraint matrix used to include hard limits defining joint limits; n is the number of tasks.

4. The inverse kinematics control method for a surgical robot according to claim 3, characterized in that: The Jacobian matrix and residual of the RCM constraint described in step 3 are calculated by the following formula: e rcm =||p trocar -p rcm || Among them: J rcm is the Jacobian matrix of the RCM constraint, e rcm is the RCM residual; is the unit direction vector of the surgical instrument axis; for The transpose of r is the reference point position vector on the axis of the surgical instrument, which is used to construct the geometric relationship of the RCM constraint; For p r The transpose of J pre is the Jacobian matrix of the instrument tip position; is the partial derivative of the instrument axis direction with respect to the joint position; I is a 3×3 unit matrix used for projection calculation; P trocar is the position of the fixed incision point; P rcm is the actual position of the surgical instrument.

5. The inverse kinematics control method for a surgical robot according to claim 4, characterized in that: The residual of the end effector posture control task described in step 4 is calculated by the following Lie group mapping: Where: e ee is the residual error of the end effector posture control task; T desired , T current are the homogeneous transformation matrices of the desired end-effector posture and the actual end-effector posture; T desired , T current ∈SE(3), SE(3) represents a special Euclidean group in three-dimensional space, which is used to describe the position and posture of the robot end effector and includes all possible combinations of three-dimensional translation and rotation; T desired The inverse matrix of ; log6 represents mapping the pose error to a six-dimensional vector in the Lie algebraic space, including translation and rotation components.

6. The inverse kinematics control method for a surgical robot according to claim 5, characterized in that: The joint limit constraints described in step 5 are defined by the following inequality: Where: q - ,q + are the lower and upper limit vectors of the joint position respectively; q is the current joint position; δt is the control cycle time, which is used to convert the position constraint into the velocity constraint. is the joint velocity vector.

7. The inverse kinematics control method for a surgical robot according to claim 6, characterized in that: The task of maximizing operability described in step 6 is achieved through the following optimization problem: in: is the gradient vector of the maneuverability index m(q), which is calculated in real time by numerical difference method; for The transpose of ; △t is the discretized time step, which is used to project the gradient into the joint velocity space; is the joint velocity vector, and m(q) is the maneuverability index.

8. The inverse kinematics control method for a surgical robot according to claim 7, characterized in that: The hierarchical quadratic programming framework described in step 7 is solved by the following iterative formula: and in: is the optimal joint velocity vector of the current priority layer p; is the optimal joint velocity vector of priority level p-1; I is the identity matrix; N p-1 is the null space projection matrix of the layer task (priority layer p-1); J p-1 is the Jacobian matrix of the high-level task (priority level p-1); is the Jacobian matrix J p-1 The damped pseudo-inverse matrix of .

9. The inverse kinematics control method for a surgical robot according to claim 8, characterized in that: The null space projection matrix N p-1 The construction process includes the following steps: For high-level tasks, the Jacobian matrix J p-1 Perform singular value decomposition and remove singular values ​​less than the threshold ∈ = 1×10 -6 direction; The pseudo-inverse of the Jacobian matrix is ​​calculated using the following formula: in, For J p-1 is the transpose of ; λ is the damping coefficient, which is used to avoid numerical singularities.

10. The inverse kinematics control method for a surgical robot according to claim 9, characterized in that: The method further comprises: Step 9: Monitor the maneuverability index m(q) in real time. If it is lower than the preset threshold, dynamically adjust the task weight and increase the weight K of the end effector posture control task to t2 Increase to twice the original value to prioritize trajectory tracking accuracy.

Images