Surgical robot joint space compliance control method, system and device based on dynamic compensation

Abstract

The invention provides a surgical robot joint space compliance control method, system and device based on dynamic compensation. The method comprises the steps that dynamic parameters of a robot are identified based on composite learning; performing dynamic force compensation on the robot based on an identification result; constructing a telecentric motion constraint kinematic model; constructing a null space compliance control strategy; calculating a final control moment of the robot based on composite learning force compensation, a task space target tracking strategy and a null space compliance control strategy; and controlling a mechanical arm to complete configuration transformation and endoscope view adjustment by utilizing the final control moment. The final control torque is calculated by utilizing composite learning force compensation, a task space target tracking strategy and a null space compliance control strategy so as to drive the mechanical arm to move. Configuration adjustment of the robot can be achieved on the premise that the visual field of the endoscope is not affected, and the situation that the robot destroys an expected puncture point and exceeds an expected joint movement range in the interaction process is avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of surgical robots, and in particular to a method, system and device for spatial compliance control of surgical robot joints based on dynamic compensation. Background Art

[0002] As an essential tool in modern medicine, surgical robots have been widely used in a variety of complex surgeries. Their precision, stability, and flexibility ensure the success of both minimally invasive and high-risk surgeries. Zero-space compliance control is a key technology in surgical robotic systems. It enables the robot to achieve spatial compliance and force interaction performance in its joints while simultaneously completing its primary task using redundant degrees of freedom.

[0003] In practical applications, joint spatial compliance control not only requires the robot to accurately perform the primary task but also supports the surgeon's dynamic adjustment of the robot's joint configuration. For example, during surgery, the surgeon may need to apply appropriate external forces to adjust the robot arm's configuration to optimize its posture and range of operation without affecting the surgical procedure. This compliance allows the surgical robot to better adapt to complex surgical environments and provide the ability to interact naturally with the surgeon. However, achieving joint compliance requires precise dynamic modeling and parameter identification, and traditional control methods often struggle to simultaneously meet both accuracy and real-time requirements. Summary of the Invention

[0004] In response to the shortcomings of the existing technology, the present invention provides a method, system and device for flexible control of the joint space of a surgical robot based on dynamic compensation. The present invention enables the robot to adjust its configuration without affecting the field of view of the endoscope, and has the function of preventing the robot from destroying the expected puncture point and exceeding the expected joint movement range during the interaction process.

[0005] The technical solution of the present invention is: a method for spatial compliance control of surgical robot joints based on dynamic compensation, comprising the following steps:

[0006] S1) Efficient identification of robot dynamic parameters based on composite learning;

[0007] S2), performing dynamic force compensation on the robot based on the identification results;

[0008] S3), constructing a telecentric motion constraint kinematic model;

[0009] S4) Constructing a zero-space compliant control strategy to achieve robot configuration adjustment without affecting the endoscope field of view;

[0010] S5), calculating the final control torque of the robot based on the composite learning force compensation, the task space target tracking strategy and the zero space compliant control strategy;

[0011] S6) transmits the final control torque of the robot to the lower computer controller, driving the robotic arm to complete the configuration transformation and endoscope field adjustment under force interaction.

[0012] Preferably, in step S1), efficient identification of the robot's mechanical arm dynamic parameters is achieved based on composite learning, specifically comprising the following steps:

[0013] S11), the dynamic model pass The form is simplified to By combining multiple variants of φ with the input

[0014]

[0015] Where, is a regression matrix containing known dynamic relationships; θ represents the joint angle position of the robot; represents the joint angular velocity of the robot; v represents the auxiliary variable; represents the derivative of the auxiliary variable; φ base represents the regression matrix without considering friction; φ fric represents the regression matrix considering friction;

[0016] S12), by introducing a low-pass filter L a (s) Constructing acceleration-free dynamics model;

[0017] S13), defining the joint space tracking error and the filter tracking error, and combining the dynamic model to obtain the open-loop error dynamic equation;

[0018] e=θ d -θ;

[0019]

[0020] Where, e is the joint space tracking error; e s is the filter tracking error; M(θ) represents the inertia matrix;

[0021] represents the centripetal and Coriolis force matrices; θ d is the desired joint angle position of the robot; represents the desired joint angular velocity of the robot; θ represents the joint angular position of the robot; represents the joint angular velocity of the robot; is the joint velocity tracking error; Λ is a positive definite diagonal matrix; represents the derivative of the filtered tracking error;

[0022] S14), designing a control law, and substituting the control law into the open-loop error dynamics equation to obtain a closed-loop error dynamics model; wherein the control law is:

[0023]

[0024] The closed-loop error dynamics model is:

[0025]

[0026] Where, τ is the control torque of the robot; K c represents a positive definite diagonal gain matrix; represents the dynamic parameters of the robot; φ represents abbreviation of;

[0027] S15), combining low-pass filter to build a composite learning law, by filtering the torque τ f and the regression matrix φ f The integral calculation of , defines the excitation matrix Θ(t), the standard moment prediction error ∈, and the generalized moment prediction error ξ(t), namely:

[0028]

[0029] Where t represents the current time; s a represents the length of the interval integral; s represents the complex Laplace operator; represents the expanded output vector; t e Represents φ f (t) the moment when the interval incentive conditions are first met;

[0030] S16) Construct a composite learning law, namely:

[0031]

[0032] Where Γ is the positive definite diagonal learning rate matrix; k a 、k b 、k c is the weight coefficient; Represents the dynamic parameters of the robot.

[0033] As a preferred embodiment, in step S12), by introducing a low-pass filter L a (s) = s / (s+a) to filter the moment τ and compare it with the filtered regression matrix Combined with the above, the influence of acceleration term is eliminated and the dynamic model is simplified to:

[0034]

[0035] Where, τ f (t) represents the torque after filtering; W represents the unknown parameter vector; s represents the complex Laplace operator; a represents the cutoff frequency of the filter; L a (s) represents the low-pass filter; τ is the control torque of the robot; is the regression matrix after filtering; T is the transpose operation.

[0036] Preferably, in step S2), the robot is dynamically compensated using a linearized dynamic equation, wherein the linearized dynamic equation is expressed as:

[0037]

[0038] Where, is the regression matrix after filtering; The dynamic parameters of the robot; M(θ) represents the inertia matrix; represents the joint angular acceleration of the robot; represents the centripetal force and Coriolis force matrix; represents the friction force matrix; G(θ) represents the gravity matrix.

[0039] Preferably, in step S3), constructing a telecentric motion constraint kinematic model specifically includes the following steps:

[0040] S31) Define the telecentric motion point P c The point where the endoscope and the telecentric motion point coincide is P rcm , the target point P to be followed t ;

[0041] S32), known telecentric motion point P c The three-dimensional position of , the telecentric motion error is defined as:

[0042] e F =P c -P rcm ;

[0043] S33) To ensure that the endoscope field of view always follows the target point P t , calculate the target unit vector Right now:

[0044]

[0045] Among them, the axis direction of the endoscope is n t , and the target unit vector With the current vector n t The angular error between e ;

[0046] S33), the target unit vector is expressed as a target attitude quaternion, that is:

[0047]

[0048] Where f represents the function that converts the target unit vector into a quaternion; Represents the target attitude quaternion; the superscript F represents the base coordinate system; the subscripts des and T represent the expectation and target respectively;

[0049] S34) Calculate the quaternion required to rotate to the target endoscope direction based on the target posture quaternion and the current posture quaternion Right now:

[0050]

[0051] Where, is the current attitude quaternion; Quaternion representing the target pose.

[0052] Preferably, in step S4), the control torque is projected into the null space by a null space projector to adjust the joint configuration without affecting the telecentric motion constraint and the endoscope field of view, wherein the null space projector is represented as:

[0053]

[0054] Where, Λ(θ curr ) represents the calculation of the null space projection based on the current joint position; Λ represents the function of the null space projection; θ curr Indicates the current joint position; I 7×7 represents the identity matrix; J T 、 denote the transpose and pseudo-inverse of the Jacobian matrix respectively;

[0055] As a preference, in step S4), by using the zero space error e ψ Control the robot elbow movement within the desired range [ψ min ,ψ max ], that is:

[0056]

[0057] Where, θ(ψ min ) represents the joint position where the elbow joint angle is minimum; θ(ψ max ) represents the joint position when the elbow joint angle is maximum; ψ min , ψ maxRepresent the maximum and minimum elbow joint angles respectively; the zero space error e ψ It represents the deviation of the current joint configuration from the expected value. Feedback control can limit the range of motion of the elbow.

[0058] As a preference, in step S4), by designing the force control law of the null space, by adjusting the stiffness matrix K p and the damping matrix K d To achieve compliant control, ensure the stability and safety of the robot's motion range, and at the same time maintain the stability and accuracy of the endoscope's field of view to the greatest extent, thereby achieving configuration adjustment and compliant control of the robot in zero space; the force control law of the zero space is expressed as:

[0059]

[0060] Where, T x represents the force control vector in the null space; Indicates the current joint speed; K p is the adjustment stiffness matrix; K d is the damping matrix; e ψ Zero spatial error.

[0061] As a preference, in step S5), the final control torque τ of the robot a The calculation is as follows:

[0062]

[0063] Where, T L and T x denote the force control vectors in the task space and null space respectively; Represents the dynamic parameters of the robot; is the regression matrix after filtering.

[0064] As an example, in step S5), the force control vector T of the task space L Expressed as:

[0065]

[0066] Where, J T represents the transpose of the Jacobian matrix; K p To adjust the stiffness matrix, K d is the damping matrix; e x is the Cartesian error, e x =[e F ,θ E ] T ;e F is the telecentric motion error; θ E Represents the target quaternion The first three components of Indicates the current Cartesian velocity.

[0067] Preferably, the present invention further provides a surgical robot joint space compliance control system based on dynamic compensation, comprising:

[0068] Parameter identification module, used to efficiently identify the robot's dynamic parameters;

[0069] Dynamic force compensation module, which performs dynamic force compensation on the robot based on the identification results;

[0070] Constraint building module, used to build telecentric motion constraint kinematic model;

[0071] Zero-space compliance control module, used to adjust the robot configuration without affecting the endoscope's field of view;

[0072] The control torque calculation module calculates the final control torque of the robot using composite learning force compensation, task space target tracking, and zero space compliant control strategy;

[0073] The robot control module uses the final control torque of the robot to control the robotic arm to complete configuration transformation and endoscope field adjustment under force interaction.

[0074] Preferably, the present invention further provides a surgical robot joint space compliance control based on dynamic compensation, comprising:

[0075] at least one processor; and a memory communicatively connected to the at least one processor; wherein,

[0076] The memory stores computer program instructions that can be executed by the at least one processor. The computer program instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned surgical robot joint space compliance control method.

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

[0078] 1. The present invention enables the robot to adjust its configuration without affecting the endoscope's field of view, thus preventing the robot from damaging the intended puncture point and exceeding the intended range of joint motion during interaction.

[0079] 2. The present invention uses a composite learning method to perform offline dynamic compensation on the robot. Based on the telecentric motion constraint, the main task control torque T can be obtained. L Based on the zero-space compliance control, the secondary task control torque T can be obtained x,By constructing a robot multi-task force control scheme including composite learning force compensation, task space target tracking strategy and zero space compliant control strategy, the final control torque is calculated, and finally the control torque is used to drive the movement of the robotic arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] Figure 1 Schematic diagram of the process of the present invention;

[0081] Figure 2 Schematic diagram of the structure of the Franka Emika Panda robotic arm according to an embodiment of the present invention;

[0082] Figure 3 Schematic diagram of telecentric motion constraint according to an embodiment of the present invention;

[0083] Figure 4 This is a graph showing changes in the prediction error norm of the Franka Emika Panda robotic arm during parameter identification using a composite learning method according to an embodiment of the present invention;

[0084] Figure 5 This is a graph showing changes in the joint tracking error norm of the Franka Emika Panda robotic arm during parameter identification using a composite learning method according to an embodiment of the present invention;

[0085] Figure 6 This is a graph showing changes in the estimated parameter norm of the Franka Emika Panda robotic arm during parameter identification using a composite learning method according to an embodiment of the present invention;

[0086] Figure 7 This is a diagram showing changes in the interaction torque experienced by the Franka Emika Panda robotic arm during human-machine interaction according to an embodiment of the present invention;

[0087] Figure 8 This is a diagram showing changes in telecentric motion error of the Franka Emika Panda robotic arm under external force during human-machine interaction according to an embodiment of the present invention;

[0088] Figure 9 This is a diagram showing changes in joint angles of the Franka Emika Panda robotic arm under external force during human-machine interaction according to an embodiment of the present invention;

[0089] Figure 10 This is a graph showing pixel error changes during target tracking by a simulated Franka Emika Panda robotic arm according to an embodiment of the present invention.

[0090] Figure 11This is a diagram showing changes in telecentric motion error during target tracking by a simulated Franka Emika Panda robotic arm according to an embodiment of the present invention.

[0091] In the figure, 1 is the first driving rod; 2 is the second driving rod; 3 is the third driving rod; 4 is the fourth driving rod; 5 is the fifth driving rod; 6 is the sixth driving rod; 7 is the seventh driving rod; 8 is the force sensor; and 9 is the endoscope. DETAILED DESCRIPTION

[0092] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings:

[0093] Example 1

[0094] like Figure 1 As shown, this embodiment provides a method for spatial compliance control of a surgical robot joint based on dynamic compensation, comprising the following steps:

[0095] S1) Implementing efficient identification of the robot's dynamic parameters based on composite learning; specifically including the following steps:

[0096] S11), the dynamic model pass The form is simplified to By combining multiple variants of φ with the input

[0097]

[0098] Where, is a regression matrix containing known dynamic relationships; θ represents the joint angle position of the robot; represents the joint angular velocity of the robot; v represents the auxiliary variable; represents the derivative of the auxiliary variable; φ base represents the regression matrix without considering friction; φ fric represents the regression matrix considering friction;

[0099] S12) To solve the problem of difficult acceleration measurement and noise influence in robot dynamics, a low-pass filter L is introduced. a (s) Constructing acceleration-free dynamics model;

[0100] By introducing a low-pass filter L a (s) = s / (s+a) to filter the moment τ and compare it with the filtered regression matrix Combined with the above, the influence of acceleration term is eliminated and the dynamic model is simplified to:

[0101]

[0102] Where, τ f (t) represents the torque after filtering; W represents the unknown parameter vector; s represents the complex Laplace operator; a represents the cutoff frequency; L a (s) represents the low-pass filter; τ is the control torque of the robot; is the regression matrix after filtering; T is the transpose operation.

[0103] S13), in order to achieve efficient parameter identification and control, define the joint space tracking error and the filter tracking error, and combine the dynamic model to obtain the open-loop error dynamic equation;

[0104] e=θ d -θ;

[0105]

[0106] Where, e is the joint position tracking error; e s is the filter tracking error; M(θ) represents the inertia matrix;

[0107] Denote the centripetal and Coriolis force matrices; θ d is the desired joint angle position of the robot; represents the desired joint angular velocity of the robot; Λ is a positive definite diagonal matrix; θ is the joint angular position of the robot; is the joint velocity tracking error; represents the derivative of the filtered tracking error;

[0108] S14), designing a control law, and substituting the control law into the open-loop error dynamics equation to obtain a closed-loop error dynamics model; wherein the control law is:

[0109]

[0110] The closed-loop error dynamics model is:

[0111]

[0112] Where, τ is the control torque of the robot; K c represents a positive definite diagonal gain matrix; represents the dynamic parameters of the robot; φ represents abbreviation of;

[0113] S15), combining low-pass filter to build a composite learning law, by filtering the torque τ f and the regression matrix φ fThe integral calculation of , defines the excitation matrix Θ(t), the standard moment prediction error ∈, and the generalized moment prediction error ξ(t), namely:

[0114]

[0115]

[0116] Where t represents the current time; s a represents the length of the interval integral; s represents the complex Laplace operator; represents the expanded output vector; t e Represents φ f (t) the moment when the interval incentive conditions are first met;

[0117] S16) Construct a composite learning law, namely:

[0118]

[0119] Where Γ is the positive definite diagonal learning rate matrix; k a 、k b 、k c is the weight coefficient; Represents the dynamic parameters of the robot.

[0120] S2), performing dynamic force compensation on the robot based on the identification results;

[0121] This embodiment uses a linearized dynamic equation to perform dynamic force compensation on the robot, wherein the linearized dynamic equation is expressed as:

[0122]

[0123] Where, is the regression matrix after filtering; The dynamic parameters of the robot; M(θ) represents the inertia matrix; represents the joint angular acceleration of the robot; represents the centripetal force and Coriolis force matrix; represents the friction force matrix; G(θ) represents the gravity matrix.

[0124] S3), constructing a telecentric motion constraint kinematic model; Figure 3 As shown, the specific steps include:

[0125] S31) Define the telecentric motion point P c The point where the endoscope and the telecentric motion point coincide is P rcm , the target point P to be followed t ;

[0126] S32), known telecentric motion point Pc The three-dimensional position of , the telecentric motion error is defined as:

[0127] e F =P c -P rcm ;

[0128] S33) To ensure that the endoscope field of view always follows the target point P t , calculate the target unit vector Right now:

[0129]

[0130] Among them, the axis direction of the endoscope is n t , and the target unit vector With the current vector n t The angular error between e ;

[0131] S33), the target unit vector is expressed as a target attitude quaternion, that is:

[0132]

[0133] Where f represents the function that converts the target unit vector into a quaternion; Represents the target attitude quaternion; the superscript F represents the base coordinate system; the subscripts des and T represent the expectation and target respectively;

[0134] S34) Calculate the quaternion required to rotate to the target endoscope direction based on the target posture quaternion and the current posture quaternion Right now:

[0135]

[0136] Where, is the current attitude quaternion; Quaternion representing the target pose.

[0137] S4) Constructing a zero-space compliant control strategy to achieve robot configuration adjustment without affecting the endoscope field of view;

[0138] The control torque is projected into the null space by a null space projector, so as to adjust the joint configuration without affecting the telecentric motion constraint and the endoscope field of view. The null space projector is represented as:

[0139]

[0140] Where, Λ(θ curr ) represents the calculation of the null space projection based on the current joint position; Λ represents the function of the null space projection; θ currIndicates the current joint position; I 7×7 represents the identity matrix; J T 、 denote the transpose and pseudo-inverse of the Jacobian matrix respectively;

[0141] By the zero space error e ψ Control the robot elbow movement within the desired range [ψ min ,ψ max ], that is:

[0142]

[0143] Where, θ(ψ min ) represents the joint position where the elbow joint angle is minimum; θ(ψ max ) represents the joint position when the elbow joint angle is maximum; ψ min , ψ max Represent the maximum and minimum elbow joint angles respectively; the zero space error e ψ It represents the deviation of the current joint configuration from the expected value. Feedback control can limit the range of motion of the elbow.

[0144] By designing the force control law of the null space and adjusting the stiffness matrix K p and the damping matrix K d To achieve compliant control, ensure the stability and safety of the robot's motion range, and at the same time maintain the stability and accuracy of the endoscope's field of view to the greatest extent, thereby achieving configuration adjustment and compliant control of the robot in zero space; the force control law of the zero space is expressed as:

[0145]

[0146] Where, T x represents the force control vector in the null space; Indicates the current joint speed; K p is the adjustment stiffness matrix; K d is the damping matrix; e ψ Zero spatial error.

[0147] S5) Calculate the final control torque τ of the robot based on the composite learning force compensation, task space target tracking strategy and zero space compliant control strategy a ;Right now:

[0148]

[0149] Where, T L and T x denote the force control vectors in the task space and null space respectively; Represents the dynamic parameters of the robot; is the regression matrix after filtering.

[0150] Among them, the force control vector T of the task space L Expressed as:

[0151]

[0152] Where, J T represents the transpose of the Jacobian matrix; K p To adjust the stiffness matrix, K d is the damping matrix; e x is the Cartesian error, e x =[e F ,θ E ] T ;e F is the telecentric motion error; θ E Represents the target quaternion The first three components of Indicates the current Cartesian velocity.

[0153] S6) The final control torque τ of the robot a The signal is transmitted to the lower controller to drive the robotic arm to complete the configuration transformation and endoscope field adjustment under force interaction.

[0154] This embodiment uses the structure of the Franka Emika Panda robotic arm. Figure 2 As shown, from Figure 4 As can be seen from the figure, the change in the prediction error norm of the Franka Emika Panda robot arm during the composite learning parameter identification process converges to 9.4 after running the excitation trajectory, which is much smaller than the initial value of 120. This shows that the composite learning parameter identification can converge.

[0155] like Figure 5 As shown in the figure, the curve shows the change in the tracking error norm of the Franka Emika Panda robot arm during the composite learning parameter identification process. After running the excitation trajectory, it converged to 0.01, which is much smaller than the initial value of 0.022. This shows that the composite learning parameter identification can converge.

[0156] like Figure 6 As shown, the curve shows the change in the estimated parameter norm of the Franka Emika Panda robot arm during the composite learning parameter identification process. After running the excitation trajectory, it converges to a specific result, which shows that the composite learning parameter identification can converge.

[0157] like Figure 7 As shown, where τ e1 ,τ e2 ,τ e3 ,τ e4 ,τe5 ,τ e6 ,τ e7 These represent the external forces acting on the first, second, third, fourth, fifth, sixth, and seventh drive rods 1, 2, 3, 4, 5, 6, and 7 of the Franka Emika Panda robot arm during human-robot interaction in joint space. During the interactive task, a human pushes the robot's four elbow joints, first pushing it to its left limit, then applying brief external forces to that limit multiple times. The human then pushes the robot to its right limit, applying brief external forces to that limit multiple times, and finally returning it to its initial position.

[0158] like Figure 8 The curve shows the distance between the tool axis at the end of the Franka Emika Panda robot arm and the telecentric motion point during human-machine interaction in the joint space. When the human applies external force to push the robot joint, the telecentric motion error is less than 2×10 -3 Meters, indicating that the robot arm can satisfy the telecentric motion constraint during movement.

[0159] like Figure 9 As shown, θ1, θ2, θ3, θ4, θ5, θ6, and θ7 respectively represent the changes in the joint angle positions of the first driving rod 1, the second driving rod 2, the third driving rod 3, the fourth driving rod 4, the fifth driving rod 5, the sixth driving rod 6, and the seventh driving rod 7 of the Franka Emika Panda robot arm during human-computer interaction in the joint space.

[0160] The upper limit of the third joint angle is set to 0.15 radians, and the lower limit of the third joint angle is set to -0.15 radians. During the execution of the task, the third joint angle can be guaranteed to change within the set range, indicating the effectiveness of the present invention in processing the zero-space joint limit constraints.

[0161] like Figure 10 As shown, the curve shows the change of pixel error in the process of simulating the Franka Emika Panda robot arm tracking the target position. After the robot movement is completed, the pixel error converges to 0, indicating the effectiveness of the present invention in adjusting the endoscope field of view according to the target position.

[0162] like Figure 11 As shown in the figure, the curve shows the distance between the tool axis at the end of the robot arm and the telecentric motion point during the process of tracking the target position of the simulated Franka Emika Panda robot arm. During the process of tracking the target position, the telecentric motion error is less than 10×10 -3 Meters, indicating that the robot arm can satisfy the telecentric motion constraint during movement.

[0163] Example 2

[0164] This embodiment provides a surgical robot joint spatial compliance control system based on dynamic compensation, including:

[0165] Parameter identification module, used to efficiently identify the robot's dynamic parameters;

[0166] Dynamic force compensation module, which performs dynamic force compensation on the robot based on the identification results;

[0167] Constraint building module, used to build telecentric motion constraint kinematic model;

[0168] Zero-space compliance control module, used to adjust the robot configuration without affecting the endoscope's field of view;

[0169] The control torque calculation module calculates the final control torque of the robot using composite learning force compensation, task space target tracking, and zero space compliant control strategy;

[0170] The robot control module uses the final control torque of the robot to control the robotic arm to complete configuration transformation and endoscope field adjustment under force interaction.

[0171] As a preferred embodiment of the present invention, the parameter identification module efficiently identifies the dynamic parameters of the robot's manipulator arm, specifically comprising the following steps:

[0172] S11), the dynamic model pass The form is simplified to By combining multiple variants of φ with the input

[0173]

[0174] Where, is a regression matrix containing known dynamic relationships; θ represents the joint angle position of the robot; represents the joint angular velocity of the robot; v represents the auxiliary variable; represents the derivative of the auxiliary variable; φ base represents the regression matrix without considering friction; φ fric represents the regression matrix considering friction;

[0175] S12) To solve the problem of difficult acceleration measurement and noise influence in robot dynamics, a low-pass filter L is introduced. a (s) Constructing acceleration-free dynamics model;

[0176] By introducing a low-pass filter L a(s) = s / (s+a) to filter the moment τ and compare it with the filtered regression matrix Combined with the above, the influence of acceleration term is eliminated and the dynamic model is simplified to:

[0177]

[0178] Where, τ f (t) represents the torque after filtering; W represents the unknown parameter vector; s represents the complex Laplace operator; a represents the cutoff frequency; L a (s) represents the low-pass filter; τ is the control torque of the robot; is the regression matrix after filtering; T is the transpose operation.

[0179] S13), in order to achieve efficient parameter identification and control, define the joint space tracking error and the filter tracking error, and combine the dynamic model to obtain the open-loop error dynamic equation;

[0180] e=θ d -θ;

[0181]

[0182] Where, e is the joint space tracking error; e s is the filter tracking error; M(θ) represents the inertia matrix;

[0183] represents the centripetal and Coriolis force matrices; θ d is the desired joint angle position of the robot; represents the desired joint angular velocity of the robot; θ is the joint angular position of the robot; is; Λ is a positive definite diagonal matrix; represents the derivative of the filtered tracking error;

[0184] S14), designing a control law, and substituting the control law into the open-loop error dynamics equation to obtain a closed-loop error dynamics model; wherein the control law is:

[0185]

[0186] The closed-loop error dynamics model is:

[0187]

[0188] Where, τ is the control torque of the robot; K c represents a positive definite diagonal gain matrix; represents the dynamic parameters of the robot; φ represents abbreviation of;

[0189] S15), combining low-pass filter to build a composite learning law, by filtering the torque τ f and the regression matrix φ f The integral calculation of , defines the excitation matrix Θ(t), the standard moment prediction error ∈, and the generalized moment prediction error ξ(t), namely:

[0190]

[0191] Where t represents the current time; s a represents the length of the interval integral; s represents the complex Laplace operator; represents the expanded output vector; t e Represents φ f (t) the moment when the interval incentive conditions are first met;

[0192] S16) Construct a composite learning law, namely:

[0193]

[0194] Where Γ is the positive definite diagonal learning rate matrix; k a 、k b 、k c is the weight coefficient; Represents the dynamic parameters of the robot.

[0195] As a preferred embodiment of the present invention, the dynamic force compensation module uses a linearized dynamic equation to perform dynamic force compensation on the robot, wherein the linearized dynamic equation is expressed as follows:

[0196]

[0197] Where, is the regression matrix after filtering; The dynamic parameters of the robot; M(θ) represents the inertia matrix; represents the joint angular acceleration of the robot; represents the centripetal force and Coriolis force matrix; represents the friction force matrix; G(θ) represents the gravity matrix.

[0198] As a preferred embodiment of the present invention, the constraint construction module constructs a telecentric motion constraint kinematic model, which specifically includes the following steps:

[0199] S31) Define the telecentric motion point P c The point where the endoscope and the telecentric motion point coincide is P rcm , the target point P to be followed t ;

[0200] S32), known telecentric motion point P c The three-dimensional position of , the telecentric motion error is defined as:

[0201] e F =P c -P rcm ;

[0202] S33) To ensure that the endoscope field of view always follows the target point P t , calculate the target unit vector Right now:

[0203]

[0204] Among them, the axis direction of the endoscope is n t , and the target unit vector With the current vector n t The angular error between e ;

[0205] S33), the target unit vector is expressed as a target attitude quaternion, that is:

[0206]

[0207] Where f represents the function that converts the target unit vector into a quaternion; Represents the target attitude quaternion; the superscript F represents the base coordinate system; the subscripts des and T represent the expectation and target respectively;

[0208] S34) Calculate the quaternion required to rotate to the target endoscope direction based on the target posture quaternion and the current posture quaternion Right now:

[0209]

[0210] Where, is the current attitude quaternion; Quaternion representing the target pose.

[0211] As a preferred embodiment of this invention, the null space compliance control module projects the control torque into the null space through a null space projector, thereby adjusting the joint configuration without affecting the telecentric motion constraint and the endoscope field of view. The null space projector is represented as:

[0212]

[0213] Where, Λ(θ curr ) represents the calculation of the null space projection based on the current joint position; Λ represents the function of the null space projection; θ curr Indicates the current joint position; I 7×7represents the identity matrix; J T 、 denote the transpose and pseudo-inverse of the Jacobian matrix respectively;

[0214] The zero space compliance control module is based on the zero space error e ψ Control the robot elbow movement within the desired range [ψ min ,ψ max ], that is:

[0215]

[0216] Where, θ(ψ min ) represents the joint position where the elbow joint angle is minimum; θ(ψ max ) represents the joint position when the elbow joint angle is maximum; ψ min , ψ max Represent the maximum and minimum elbow joint angles respectively; the zero space error e ψ It represents the deviation of the current joint configuration from the expected value. Feedback control can limit the range of motion of the elbow.

[0217] The zero space compliance control module designs the force control law of the zero space and adjusts the stiffness matrix K p and the damping matrix K d To achieve compliant control, ensure the stability and safety of the robot's motion range, and at the same time maintain the stability and accuracy of the endoscope's field of view to the greatest extent, thereby achieving configuration adjustment and compliant control of the robot in zero space; the force control law of the zero space is expressed as:

[0218]

[0219] Where, T x represents the force control vector in the null space; Indicates the current joint speed; K p is the adjustment stiffness matrix; K d is the damping matrix; e ψ Zero spatial error.

[0220] As preferred in this embodiment, the control torque calculation module calculates the final control torque τ of the robot based on the composite learning force compensation, task space target tracking strategy and zero space compliance control strategy a ;Right now:

[0221]

[0222] Where, T L and T x denote the force control vectors in the task space and null space respectively; Represents the dynamic parameters of the robot; is the regression matrix after filtering.

[0223] Among them, the force control vector T of the task space L Expressed as:

[0224]

[0225] Where, J T represents the transpose of the Jacobian matrix; K p To adjust the stiffness matrix, K d is the damping matrix; e x is the Cartesian error, e x =[e F ,θ E ] T ;e F is the telecentric motion error; θ E Represents the target quaternion The first three components of Indicates the current Cartesian velocity.

[0226] Example 3

[0227] This embodiment provides a surgical robot joint spatial compliance control based on dynamic compensation, including:

[0228] at least one processor; and a memory communicatively connected to the at least one processor; wherein,

[0229] The memory stores computer program instructions that can be executed by the at least one processor. The computer program instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned surgical robot joint space compliance control method.

[0230] Any reference to memory, storage, database, or other medium used in this embodiment may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM).

[0231] The processor can be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc.

[0232] Computer-readable media may include any entity or device capable of carrying computer program code, recording media, USB flash drives, removable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signals, telecommunications signals, and software distribution media. It should be noted that the content of computer-readable media may be appropriately expanded or reduced based on the requirements of legislation and patent practice within a jurisdiction. For example, in some jurisdictions, based on legislation and patent practice, computer-readable media do not include electric carrier signals and telecommunications signals.

[0233] The above embodiments and descriptions are only for explaining the principles and best embodiments of the present invention. Without departing from the spirit and scope of the present invention, the present invention may be subject to various changes and improvements, which shall fall within the scope of the invention to be protected.

Claims

1. A method for spatial compliance control of surgical robot joints based on dynamic compensation, characterized in that: The steps include: S1) Identify the robot's dynamic parameters based on composite learning; S2), performing dynamic force compensation on the robot based on the identification results; S3), constructing a telecentric motion constraint kinematic model; S4) Constructing a zero-space compliant control strategy to achieve robot configuration adjustment without affecting the endoscope field of view; S5), calculating the final control torque of the robot based on the composite learning force compensation, the task space target tracking strategy and the zero space compliant control strategy; S6) transmits the final control torque of the robot to the lower computer controller, driving the robotic arm to complete the configuration transformation and endoscope field adjustment under force interaction.

2. The method for spatially compliant control of surgical robot joints based on dynamic compensation according to claim 1, characterized in that: In step S1), the dynamic parameters of the robot's manipulator are identified based on composite learning, specifically including the following steps: S11), the dynamic model pass The form is simplified to By combining multiple variants of φ with the input Where, is a regression matrix containing known dynamic relationships; θ represents the joint angle position of the robot; represents the joint angular velocity of the robot; v represents the auxiliary variable; represents the derivative of the auxiliary variable; φ base represents the regression matrix without considering friction; φ fric represents the regression matrix considering friction; S12), by introducing a low-pass filter L a (s) Construct an acceleration-free dynamic model, namely: Where, τ f (t) represents the filtered torque; W represents the unknown parameter vector; s represents the complex Laplace operator; L a (s) represents the low-pass filter; τ is the control torque of the robot; is the regression matrix after filtering; T is the transpose operation; S13), defining the joint space tracking error and the filter tracking error, and combining the dynamic model to obtain the open-loop error dynamic equation; Where, e is the joint position tracking error; e s is the filter tracking error; M(θ) represents the inertia matrix; represents the centripetal and Coriolis force matrices; θ d is the desired joint angle position of the robot; represents the desired joint angular velocity of the robot; θ represents the joint angular position of the robot; represents the joint angular velocity of the robot; is the joint velocity tracking error; Λ is a positive definite diagonal matrix; represents the derivative of the filtered tracking error; S14), designing a control law, and obtaining a closed-loop error dynamics model by substituting the control law into the open-loop error dynamics equation; wherein the control law is: The closed-loop error dynamics model is: Where, τ is the control torque of the robot; K c represents a positive definite diagonal gain matrix; represents the dynamic parameters of the robot; φ represents abbreviation of; S15), combining low-pass filter to build a composite learning law, by filtering the torque τ f and the regression matrix φ f The integral calculation of defines the excitation matrix Θ(t), the standard moment prediction error ∈, and the generalized moment prediction error ξ(t), namely: Where t represents the current time; s a represents the length of the interval integral; s represents the complex Laplace operator; represents the expanded output vector; t e Represents φ f (t) the moment when the interval incentive conditions are first met; S16) Construct a composite learning law, namely: Where Γ is a positive definite diagonal learning rate matrix; k a 、k b 、k c is the weight coefficient; Represents the dynamic parameters of the robot.

3. The method for spatially compliant control of surgical robot joints based on dynamic compensation according to claim 2, characterized in that: In step S2), the robot is subjected to dynamic force compensation using a linearized dynamic equation, wherein the linearized dynamic equation is expressed as: Where, is the regression matrix after filtering; The dynamic parameters of the robot; M(θ) represents the inertia matrix; represents the joint angular acceleration of the robot; represents the centripetal force and Coriolis force matrix; F(θ) represents the friction force matrix; G(θ) represents the gravity matrix.

4. The method for spatially compliant control of surgical robot joints based on dynamic compensation according to claim 1, characterized in that: In step S3), a telecentric motion constraint kinematic model is constructed, which specifically includes the following steps: S31) Define the telecentric motion point P c The point where the endoscope and the telecentric motion point coincide is P rcm , the target point P to be followed t ; S32), according to the telecentric motion point P c The three-dimensional position of defines the telecentric motion error as: e F =P c -P rcm ; S33) To ensure that the endoscope field of view always follows the target point P t , calculate the target unit vector Right now: Among them, the axis direction of the endoscope is n t , and the target unit vector With the current vector n t The angular error between e ; S33), the target unit vector is expressed as a target attitude quaternion, that is: Where f represents the function that converts the target unit vector into a quaternion; Represents the target attitude quaternion; the superscript F represents the base coordinate system; the subscripts des and T represent the expectation and target respectively; S34) Calculate the quaternion required to rotate to the target endoscope direction based on the target posture quaternion and the current posture quaternion Right now: Where, is the current attitude quaternion; Quaternion representing the target pose.

5. The method for spatial compliance control of surgical robot joints based on dynamic compensation according to claim 4, characterized in that: In step S4), the control torque is projected into the null space by a null space projector to adjust the joint configuration without affecting the telecentric motion constraint and the endoscope field of view, wherein the null space projector is represented as: Where, Λ(θ curr ) represents the calculation of the null space projection based on the current joint position; Λ represents the function of the null space projection; θ curr Indicates the current joint position; I 7×7 represents the identity matrix; J T 、 denote the transpose and pseudo-inverse of the Jacobian matrix, respectively.

6. The method for spatial compliance control of surgical robot joints based on dynamic compensation according to claim 5, characterized in that: In step S4), the zero space error e ψ Control the robot elbow movement within the desired range [ψ min ,ψ max ], that is: Where, θ(ψ min ) represents the joint position where the elbow joint angle is minimum; θ(ψ max ) represents the joint position when the elbow joint angle is maximum; ψ min , ψ max Represent the maximum and minimum elbow joint angles respectively; the zero space error e ψ It represents the deviation of the current joint configuration from the expected value. Feedback control can limit the range of motion of the elbow.

7. The method for spatially compliant control of surgical robot joints based on dynamic compensation according to claim 6, characterized in that: In step S4), by designing the force control law of the null space, the stiffness matrix K is adjusted. p and the damping matrix K d To achieve compliant control, ensure the stability and safety of the robot's motion range, and at the same time maintain the stability and accuracy of the endoscope's field of view to the greatest extent, thereby achieving configuration adjustment and compliant control of the robot in zero space; the force control law of the zero space is expressed as: Where, T x represents the force control vector in the null space; Indicates the current joint speed; K p is the adjustment stiffness matrix; K d is the damping matrix; e ψ Zero spatial error.

8. The method for spatial compliance control of surgical robot joints based on dynamic compensation according to claim 7, characterized in that: In step S5), the final control torque τ of the robot a The calculation is as follows: Where, T L and T x denote the force control vectors in the task space and null space respectively; Represents the dynamic parameters of the robot; is the regression matrix after filtering; Among them, the force control vector T of the task space L Expressed as: Where, J T represents the transpose of the Jacobian matrix; K p To adjust the stiffness matrix, K d is the damping matrix; e x is the Cartesian error, e x =[e F ,θ E ] T ;e F is the telecentric motion error; θ E Represents the target quaternion The first three components of Indicates the current Cartesian velocity.

9. A surgical robot joint space compliance control system based on dynamic compensation, characterized in that: The system utilizes the method according to any one of claims 1 to 8 to realize compliant control of the joint space of the surgical robot, and the system comprises: Parameter identification module, used to efficiently identify the robot's dynamic parameters; Dynamic force compensation module, which performs dynamic force compensation on the robot based on the identification results; Constraint building module, used to build telecentric motion constraint kinematic model; Zero-space compliance control module, used to adjust the robot configuration without affecting the endoscope's field of view; The control torque calculation module calculates the final control torque of the robot using composite learning force compensation, task space target tracking, and zero space compliant control strategy; The robot control module uses the final control torque of the robot to control the robotic arm to complete configuration transformation and endoscope field adjustment under force interaction.

10. A surgical robot joint space compliance control based on dynamic compensation, characterized in that: include: at least one processor; and a memory communicatively connected to the at least one processor; wherein, The memory stores computer program instructions that can be executed by the at least one processor, and the computer program instructions are executed by the at least one processor so that the at least one processor can execute the surgical robot joint space compliance control method described in any one of claims 1-8.

Images