Inverse kinematics control method for surgical robots

Abstract

The present invention relates to the field of surgical robot technology and discloses a surgical robot inverse kinematics control method, comprising: establishing a kinematic model of the surgical robot, including the position and posture of the end effector, joint angles, and Jacobian matrix, and defining a maneuverability index; constructing a hierarchical optimization framework based on surgical task priorities, decomposing the inverse kinematics problem into RCM constraints, end effector posture control, joint limit constraints, and maneuverability maximization tasks, and sorting 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; using a hierarchical quadratic programming framework, solving the optimal joint velocity of each priority task in order of priority, and using a 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. The present invention maximizes the maneuverability of the robot.

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 a surgical robot. Background Art

[0002] Robot-assisted minimally invasive surgery (RMIS) reduces patient trauma and accelerates postoperative recovery through small incisions. However, a core technical challenge lies in achieving high-precision and highly flexible robotic motion control within confined anatomical spaces. Inverse kinematics (IK), a core module for surgical robot motion planning, must simultaneously satisfy multiple constraints (such as remote center of motion (RCM) constraints, joint limits, and end-point trajectory tracking) while optimizing motion flexibility.

[0003] However, most existing IK methods are unable to simultaneously meet multiple objectives such as remote center of motion (RCM) constraints, joint restrictions, and operability optimization, and still have obvious shortcomings when dealing with RCM constraints, operability optimization, joint restrictions, and multi-task optimization. These problems limit the application of surgical robots in complex surgeries, especially in scenarios requiring high precision and high flexibility. Summary of the Invention

[0004] The purpose of this invention is to propose an inverse kinematics control method for a surgical robot, which can maximize the maneuverability of the robot while satisfying the remote motion center constraints and joint restrictions.

[0005] To achieve the above objectives, the present invention proposes an inverse kinematics control method for a surgical robot, comprising:

[0006] Step 1: Establish a kinematic model of the surgical robot, including the position and posture of the end effector, joint angles, and Jacobian matrix; define a maneuverability index to measure the flexibility of the robot, which is calculated based on the Jacobian matrix;

[0007] 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 rank 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;

[0008] 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;

[0009] Step 4: In the secondary priority layer, a posture control task is constructed based on the difference between the current posture of the end effector and the target posture. The surgical instrument is driven to move precisely along the preset trajectory by optimizing the residual of the end effector posture control task.

[0010] 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 speeds.

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

[0012] 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. 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.

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

[0014] Optionally, the calculation formula of the operability index in step 1 is:

[0015]

[0016] 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 singular point, at which point the robot's flexibility decreases; when det(JJ T) When larger, the robot has higher flexibility.

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

[0018]

[0019] in: is the Jacobian matrix of task i, describing the mapping from task space to 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, 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; C p is the inequality constraint matrix used to include hard limits defining joint limits; n is the number of tasks.

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

[0021]

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

[0023] 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 p r is the reference point position vector on the surgical instrument axis, 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.

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

[0025]

[0026] Among them: e ee is the residual error of the end effector posture control task; T desired 、T currentare the homogeneous transformation matrices of the desired end-effector pose and the actual end-effector pose; 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.

[0027] Optionally, the joint limit constraint in step 5 is defined by the following inequality:

[0028]

[0029] 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 a velocity constraint. is the joint velocity vector.

[0030] Optionally, the task of maximizing operability in step 6 is implemented by the following optimization problem:

[0031]

[0032] 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.

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

[0034] and

[0035] in: is the optimal joint velocity vector of the current priority layer p; is the optimal joint velocity vector of 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 level p-1); is the Jacobian matrix J p-1 The damped pseudo-inverse matrix of .

[0036] Optionally, the null space projection matrix N p-1 The construction process includes the following steps:

[0037] 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;

[0038] The pseudo-inverse of the Jacobian matrix is calculated using the following formula:

[0039]

[0040] in, For J p-1 is the transpose of ; λ is the damping coefficient, which is used to avoid numerical singularities.

[0041] Optionally, the method further includes:

[0042] 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.

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

[0044] The present invention decomposes the inverse kinematics problem into three levels: RCM constraint (highest priority), end-effector posture control and joint limit constraint (second priority), and operability optimization (lowest priority). The hierarchical quadratic programming (HQP) framework is used for global optimization. The RCM constraint is used as a hard task to ensure that the surgical instrument moves strictly around the fixed incision point to avoid tissue damage during the operation. In the second priority layer, the posture control task is constructed based on the difference between the current posture of the end effector and the target posture. The surgical instrument is driven to move precisely along the preset trajectory by optimizing the residual of the end effector posture control task. Under the premise of ensuring safety, the submillimeter precision requirements of minimally invasive surgery can be met. Low-priority operability optimization tasks maximize 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 speed of each priority task is solved in sequence 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, which can achieve efficient and stable real-time solution; thus, the present invention achieves the maximization of the robot's flexibility and motion performance while meeting the surgical safety and precision requirements. The method of the present invention can be adapted to platforms such as the da Vinci surgical robot, providing a safer, more accurate and flexible motion control solution for minimally invasive surgery.

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

[0046] The above and other objects, features and advantages of the present invention will become more apparent through a more detailed description of exemplary embodiments of the present invention with reference to the accompanying drawings, in which like reference numerals generally represent like components.

[0047] Figure 1 A flowchart of 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 method cannot simultaneously meet the multi-objective requirements such as RCM constraints, joint restrictions and operability optimization, the present invention proposes an inverse kinematics control method for surgical robots. The core of the present invention is to maximize the robot's motion flexibility while ensuring surgical safety through strict priority division and zero space projection mechanism based on the hierarchical quadratic programming (HQP) framework.

[0049] The technical principles are as follows:

[0050] 1. Task stratification and hard priority guarantee: Surgical tasks are divided into RCM constraints (highest priority), end-point posture tracking (second priority), and operability optimization (lowest priority) according to clinical importance. The HQP framework ensures that high-priority tasks are given absolute priority, and low-priority tasks are optimized only in their null space.

[0051] 2. Programmable RCM constraints: By dynamically calculating the RCM Jacobian matrix and residuals, it replaces the traditional mechanical RCM and improves intraoperative adaptability.

[0052] 3. Gradient projection optimization: Utilize the gradient information of the maneuverability index to optimize motion flexibility in redundant degrees of freedom in real time and avoid singular configurations.

[0053] The present invention will now be described in more detail with reference to the accompanying drawings. While preferred embodiments of the present invention are shown in the accompanying drawings, it should be understood that the present invention may be implemented in various forms and is not limited to the embodiments set forth herein. Rather, 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] like Figure 1As shown, a surgical robot inverse kinematics control method 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 posture of the end effector, joint angles, and Jacobian matrix; define a maneuverability index to measure the flexibility of the robot, which is calculated based on the Jacobian matrix;

[0056] Specifically, this step establishes a kinematic model, which includes the position and posture of the end effector, joint angles, and the Jacobian matrix J. The Jacobian matrix describes the relationship between the position and posture of the end effector and the joint angles.

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

[0058]

[0059] in:

[0060] m(q): Maneuverability index, which is an indicator that measures the robot's mobility under the current joint configuration q. It 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 its mobility and maneuverability are. When the value approaches zero, the robot is close to the singular state and its mobility is limited.

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

[0062] det(JJ T ): 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 that reflects the hypervolume (the square of the volume) spanned by the Jacobian matrix and represents the "effective workspace" of the robot's motion capability. When det(JJ T ) is close to zero, the robot may be close to a singular point, at which point the robot's flexibility decreases; when det(JJ T) When larger, the robot has higher flexibility.

[0063] 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 rank 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;

[0064] Specifically, this step builds a hierarchical optimization framework and first decomposes the tasks:

[0065] Highest priority: RCM constraints, ensuring instrument movement around fixed incision points;

[0066] Second priority tasks: end pose control and joint limit constraints;

[0067] Lowest priority task: Maximize operability.

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

[0069]

[0070]

[0071] in:

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

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

[0074] The weight coefficient of task i 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 ; Preferably, in this embodiment, the task weight K of the remote motion center is set t1 =1.0, the weight of the terminal posture control task K t2 =1.0, operability task weight K t3 =0.01;

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

[0076] Joint velocity vector;

[0077] Slack variables, used to handle violations of inequality constraints;

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

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

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

[0081] 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;

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

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

[0084] in:

[0085] e rcm is the RCM residual;

[0086] Fix the location of the incision point;

[0087] The actual location of the surgical instruments.

[0088] Then, construct the RCM Jacobian matrix:

[0089]

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

[0091] in:

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

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

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

[0095] Jacobian matrix of the instrument tip position;

[0096] The partial derivative of the instrument axis direction with respect to the joint position is used to describe the effect of joint motion on the axis direction;

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

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

[0099]

[0100] in:

[0101] is the current joint position vector, dimension n q is the robot joint degree of freedom; for example, for a 7-DOF surgical robot, q = [q1,q2,…,q7] T ;

[0102] The differential step size of the jth joint is used to numerically approximate the partial derivative. Its value is preferably: Δq j =1×10 -6 rad, balance between computational accuracy and numerical stability;

[0103] The unit direction vector of the instrument axis is calculated by forward kinematics and is calculated as follows:

[0104]

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

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

[0107] In the specific implementation process, firstly, a positive perturbation q is applied to the j-th joint. j +Δq j , calculate the axis direction after disturbance Then apply a negative perturbation q to the j-th joint j -Δq j , calculate the axis direction after disturbance Then calculate the partial derivatives using the central difference formula For each joint j=1,2,…,n q Compute the partial derivatives independently, generating the complete Jacobian block:

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

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

[0110] (1) Position projection term:

[0111]

[0112] in, It is a 3×3 identity matrix used for projection calculations: the instrument tip velocity is projected onto a plane perpendicular to the axis, constraining its rotation around the incision point; is the projection matrix in the direction of the instrument axis, is a unit vector.

[0113] (2) Axis direction compensation item:

[0114]

[0115] in, 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 axis direction change on joint velocity; It is a scalar projection, which represents the displacement of the reference point along the axis.

[0116] By p r The RCM constraint, based on the geometric associations of the robot, allows real-time adaptation to minute changes in the instrument axis orientation, reducing error fluctuations by 50%. The introduction of the identity matrix I avoids singularity issues and ensures numerical stability of projection operations. Programmable RCM constraints enable the robot to dynamically adapt to intraoperative incision point offset errors, controlling them to submillimeter levels.

[0117] Step 4: In the secondary priority layer, a posture control task is constructed based on the difference between the current posture of the end effector and the target posture. The surgical instrument is driven to move precisely along the preset trajectory by optimizing the residual of the end effector posture control task.

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

[0119]

[0120] in:

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

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

[0123] T desired The inverse matrix of

[0124] log6 is Operation, representing the mapping from Lie group to 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 goal is:

[0126]

[0127] in, 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 Lie group mapping to avoid Euler angle singularity, the pose tracking error is ≤1.1×10 -6 (Simulated data).

[0129] 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 speeds.

[0130] Specifically, the joint limit constraint in this step is defined by the following inequality:

[0131]

[0132] in:

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

[0134] Current joint position;

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

[0136] Joint limit constraints are used to strictly limit the joint speed to a physically feasible range to avoid damage to the mechanical structure.

[0137] Step 6: In the lowest priority layer, a maneuverability maximization optimization task is constructed based on the gradient projection method of the maneuverability index to optimize the robot joint speed to maximize motion flexibility;

[0138] Specifically, this step performs the optimization of maximizing operability, and first calculates the operability index m(q):

[0139]

[0140] in, is the spatial Jacobian matrix of the end effector; 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 and is a scalar value; when det(JJ T ) is close to zero, the robot may be close to a singular point, at which point the robot's flexibility decreases; when det(JJ T) When larger, the robot has higher flexibility.

[0141] Then, gradient projection optimization is performed, and the task of maximizing operability is achieved through the following optimization problem:

[0142]

[0143] in:

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

[0145] Discretize the time step used to project the gradient into the joint velocity space, preferably Δt = 1ms, synchronized with the controller;

[0146] m(q) represents the operability index, which is expressed by calculate.

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

[0148]

[0149] in:

[0150] Represents the current joint position vector, dimension n q is the joint degree of freedom of the robot. For example, for a 7-DOF surgical robot, q = [q1,q2,…,q7] T .

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

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

[0153] Gradient vector

[0154] This step maximizes the robot's motion flexibility by optimizing m(q), ensuring efficient posture adjustment in complex surgical scenarios (such as narrow cavity operations) and avoiding falling into strange configurations. In the (7-degree-of-freedom + 5-degree-of-freedom tool) simulation, maneuverability increased by 146%, significantly enhancing the ability to operate in narrow cavities.

[0155] 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. 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.

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

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

[0158]

[0159] in, n q ×n q -dimensional identity matrix;

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

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

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

[0163] Then, perform iterative solution:

[0164]

[0165] in:

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

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

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

[0169] 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;

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

[0171]

[0172] in, For J p-1 The transpose of λ is the damping coefficient, which is used to avoid numerical singularity, and the preferred value is λ = 1 × 10 -4 .

[0173] The OSQP quadratic programming solver is preferably used for solving the problem, which can achieve a single solution time of ≤1ms and meet the real-time control requirements.

[0174] Step 8: Calculate and output the velocity instructions of the robot joints 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 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.

[0177] Specifically, during the precise motion control of surgical tools, the maneuverability index m(q) is monitored in real time. When it is lower than the preset threshold, the task weight is dynamically adjusted. For example, for a 7-DOF surgical robot, if m(q) = 0.5 at a certain moment, it means that it is in a state of high flexibility. If m(q) is lower than mmin = 0.1, it is determined to be close to a singular configuration. Then perform weight update, for example, the end pose task weight K t2 Increased from 1.0 to 2.0; RCM task weight K t1 Keep 1.0 unchanged.

[0178] By dynamically adjusting task weights, trajectory tracking accuracy is prioritized in singular areas, reducing error fluctuations.

[0179] In summary, the inverse kinematics control method for a surgical robot according to an embodiment of the present invention achieves maximum robot operability while satisfying remote center of motion (RCM) constraints and joint restrictions through a hierarchical quadratic programming framework. Compared with the prior art, the present invention also has the following technical effects:

[0180] 1. Improve surgical safety

[0181] Strictly satisfying RCM constraints: By setting RCM constraints as the highest priority, the present invention ensures that surgical tools always rotate around the incision point, avoiding tissue damage. This improvement significantly improves surgical safety and reduces the risk of surgical complications.

[0182] Strict adherence to joint constraints: By introducing joint constraints 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] Maximizing operability: This invention significantly improves the robot's flexibility by optimizing the operability index m = det(JJT). In complex surgical environments, the robot can more flexibly adjust its posture and avoid singularities, thereby achieving more efficient surgical operations.

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

[0186] 3. Achieve efficient real-time control

[0187] Fast solution capability: This method uses an efficient quadratic programming solver (such as OSQP) to solve complex optimization problems in a short time, meeting the requirements of real-time control. Experiments show that the average computation time of this method is less than 1ms with RCM constraints and less than 0.3ms without RCM constraints, significantly outperforming existing technologies.

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

[0189] 4. Improve surgical precision

[0190] Precise End-Effector Posture Control: This invention optimizes the end-effector posture control task to ensure the precise position and posture of surgical tools. This improvement significantly improves surgical precision, enabling the robot to perform more delicate operations in complex surgical environments.

[0191] Reducing human errors: Through automated and optimized inverse kinematics control methods, the present invention reduces errors caused by human operation and improves the overall quality of surgery.

[0192] While various embodiments of the present invention have been described above, the above description is intended to be illustrative, not exhaustive, and not limited to the disclosed embodiments. Many modifications and variations will be apparent to those skilled 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 a 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 to measure the flexibility of the robot, wherein 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 rank 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 constraint 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, a posture control task is constructed based on the difference between the current posture of the end effector and the target posture. 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 speeds. Step 6: In the lowest priority layer, a maneuverability maximization optimization task is constructed based on the gradient projection method of the maneuverability index 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. 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. Step 8: Calculate and output the velocity instructions of the robot joints in real time to achieve precise motion control of the surgical tool.

2. The inverse kinematics control method for a surgical robot according to claim 1, characterized in that: The calculation formula of the operability index in step 1 is: ， in: m ( q ) is the operability index; J is the spatial Jacobian matrix of the end effector; J T is the Jacobian matrix J The transposed matrix of det( JJ T ) is the Jacobian matrix J 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 singular 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, wherein: The hierarchical optimization framework in step 2 is defined by the following global optimization formula: ； in: For the task i The Jacobian matrix describes the mapping from task space to joint space; For the task i The residual vector represents the deviation between the current state and the target; For the task i The weight coefficient 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 ; For the task i The residual gain coefficient is used to scale the impact 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, which is 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, wherein: The Jacobian matrix and residual of the RCM constraint described in step 3 are calculated by the following formula: ； in: 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 is the reference point position vector on the surgical instrument axis, which is used to construct the geometric relationship of the RCM constraint; for 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 identity matrix used for projection calculation; P trocar To fix the position of the 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 pose control task described in step 4 is calculated by the following Lie group mapping: ； in: e ee is the residual of the end-effector pose control task; T desired 、 T current are the homogeneous transformation matrices of the desired end-effector pose and the actual end-effector pose respectively; T desired , T current ∈ SE (3), SE (3) A special Euclidean group in three-dimensional space used to describe the position and posture of the robot end effector, which includes all possible combinations of three-dimensional translations and rotations; for 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 inequalities: ； in: q - 、 q + are the lower and upper limit vectors of the joint position respectively; q is the current joint position; δt To control the cycle time, it is used to convert the position constraint into the speed 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: ▽m(q) Operability Index m ( q ) is calculated in real time by numerical difference method; ▽m(q) T for ▽m(q) The transpose of △t is the discretized time step used to project the gradient into the joint velocity space; is the joint velocity vector, m ( q ) is the operability index.

8. The inverse kinematics control method for a surgical robot according to claim 7, wherein: The hierarchical quadratic programming framework described in step 7 is solved by the following iterative formula: ； in: The current priority level p The optimal joint velocity vector of ; Priority layer p- 1 optimal joint velocity vector; I is the identity matrix; N p-1 Priority layer p- 1's null space projection matrix; J p-1 Priority layer p- 1's Jacobian matrix; 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, wherein: The null space projection matrix N p-1 The construction process includes the following steps: Priority layer p- Jacobian matrix of 1 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 The transpose of λ is the damping coefficient, 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: Real-time monitoring of operability index m ( q ), if it is lower than the preset threshold, the task weight is dynamically adjusted to increase the weight of the end effector posture control task K t2 Increase to twice the original value to prioritize trajectory tracking accuracy.

Images