A joint position calibration method for a surgical robot

Abstract

The present invention relates to the field of surgical robot control technology and discloses a method for calibrating the joint position of a surgical robot, comprising: generating a multi-directional zigzag calibration trajectory; controlling the robot to move along the calibration trajectory, synchronously collecting robot state data and ground-based true joint position data, the robot state data including the current joint position and electrode torque; training an error prediction model based on the robot state data and the ground-based true joint position data, the error prediction model input being the current joint position and motor torque in the robot state data, and outputting the joint position error; operating the trained error prediction model in parallel with the surgical robot control system, superimposing the error output by the error prediction model in real time onto the original joint position, and correcting the robot joint position in real-time operation to improve the accuracy of the joint position. The present invention can achieve real-time, efficient, automated, and precise calibration of the joint position of a surgical robot.

Description

Technical Field

[0001] The present invention relates to the technical field of surgical robot control, and more particularly, to a joint position calibration method for a surgical robot. Background Art

[0002] Surgical robotics has advanced rapidly in recent years and has become a vital tool in minimally invasive surgery. By precisely controlling the movement of surgical instruments, these robots significantly improve surgical precision and safety, reduce surgical trauma, and shorten patient recovery time. However, the widespread use of surgical robots also faces several technical challenges, the most critical of which is the precise control of joint position.

[0003] Joint position control in surgical robots is crucial to the success of surgical procedures. Accurate joint position information not only ensures accurate movement of surgical instruments but also supports the automation of complex surgical tasks such as tissue cutting, suturing, and manipulation. However, surgical robots face several practical operational challenges in practical applications. For example, load fluctuations during surgery, robot homing, and mechanical fatigue during extended operation can all affect joint position accuracy. These issues not only reduce the robot's operational precision but also potentially increase surgical risk.

[0004] Existing surgical robot joint position calibration methods mainly include model-based methods and vision-tracking-based methods. The model-based method relies on accurate modeling of the robot's mechanical structure and dynamics, but this method is difficult to accurately model when dealing with certain surgical robots with complex structures (such as robots with a large number of joints or cable-driven surgical robots, etc.), resulting in the inability to guarantee calibration accuracy; the vision-tracking-based method requires additional sensors, and the calibration time is too long, the efficiency is low, and it cannot meet real-time requirements. Summary of the Invention

[0005] The purpose of the present invention is to provide a joint position calibration method for a surgical robot, so as to achieve real-time, efficient, automated and precise calibration of the joint positions of the surgical robot.

[0006] To achieve the above objectives, the present invention proposes a joint position calibration method for a surgical robot, comprising:

[0007] Step S1: generating a multi-directional zigzag calibration trajectory covering the robot joint space; wherein the calibration trajectory covers a combination of single-joint, double-joint and triple-joint directions;

[0008] Step S2: Control the robot to move along the calibration trajectory, and synchronously collect robot state data and ground truth joint position data, wherein the robot state data includes current joint positions and motor torques;

[0009] Step S3: Based on the robot state data and the ground truth joint position data, an error prediction model is trained, wherein the input of the error prediction model is the current joint position and motor torque in the robot state data, and the output is the joint position error;

[0010] Step S4: The trained error prediction model works in parallel with the surgical robot control system, and the error output by the error prediction model in real time is superimposed on the current joint position, and the robot joint position is corrected in real-time operation to improve the accuracy of the joint position.

[0011] Optionally, in step S1, the zigzag calibration trajectory is generated by:

[0012] Define the initial trajectory matrix, whose direction is aligned with the axis of the robot shoulder joint;

[0013] The initial trajectory matrix is translated and rotated through the homogeneous transformation matrix and the rotation matrix to generate calibration trajectories in different directions;

[0014] The sparsity of the calibration trajectory is defined by the following formula:

[0015] , where Δ s is the trajectory sparsity; r is the range of motion of the joint, r = J max- J min , J max represents the maximum allowed position of the joint, J min Indicates the minimum allowable position of the joint, in degrees for rotational joints and millimeters for translational joints; k ∈{2,3,4,5,6} is the sparsity coefficient, which is used to control the trajectory density; n ∈{1,2,3} is the trajectory direction dimension, including unidirectional, bidirectional or tridirectional.

[0016] Optionally, in step S2, the synchronously collecting robot state data and ground truth joint position data includes:

[0017] Obtain the current joint position in the robot state data by calculating through a motor encoder, and obtain the motor torque in the robot state data by linear mapping of a motor current command;

[0018] The ground truth joint position data is obtained by measuring with an external high-precision encoder.

[0019] Optionally, in step S3, the error prediction model includes a deep neural network model;

[0020] The structure of the deep neural network model includes: an input layer, a hidden layer and an output layer;

[0021] The dimension of the input layer is m, and the input is m features in the robot state data, including the current joint position and motor torque;

[0022] The hidden layer includes two fully connected layers, each fully connected layer has 100 neurons, and the activation function is Sigmoid;

[0023] The dimension of the output layer is K, and the position errors of K joints of the robot are output;

[0024] The loss function of the deep neural network model is:

[0025] , where L is the mean square error, N is the number of training samples, For the i The predicted joint position error of each sample is output by the DNN model with a dimension of m; For the i The true joint position error of samples is calculated as follows: ,in The first i The true joint position of each sample, is the first i The current joint positions of samples.

[0026] Optionally, in step S3, the error prediction model further includes a linear regression model, and its output error formula is:

[0027] , where Δ q j For the j The prediction error of each joint, j=1,2…K; β j0 For the j The regression intercept term of each joint; β ji For the j The first joint i The regression coefficients of the input features; x i is the i-th input feature, x i =[ q raw , t],in q raw is the current joint position vector, t is the motor torque vector, and m is the input feature dimension.

[0028] Optionally, the parameters of the linear regression model are solved by the least squares method, and the calculation formula is:

[0029] β j =(X T X) -1 X T Δq j

[0030] Where j = 1, 2…K, β j For the j The regression coefficient vector of the joints is expressed as: β j =[ β j0 , β j1 , β j2 ,…, β jm ] T ; X is the input feature matrix, Δq j For the j The error vector of the joint, T Indicates transpose.

[0031] Optionally, the error prediction model is dynamically switched to a deep neural network model or a linear regression model for error prediction according to real-time requirements.

[0032] Optionally, in step S4, the frequency of the real-time correction is 800 Hz-1200 Hz.

[0033] Optionally, the calibrated joint positions are calculated using the following formula:

[0034] q calibrated = q raw +Δ q

[0035] in, q calibrated is the calibrated joint position vector, q raw is the current joint position vector, Δ q is the joint error vector output by the model.

[0036] Optionally, before executing step S1, the method further includes: initializing the surgical robot and executing a homing program, so that the robot explores joint limits and registers motor encoders to obtain initial joint position information.

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

[0038] The present invention first generates a multi-directional zigzag calibration trajectory covering the robot joint space, then controls the robot to move along the calibration trajectory, and synchronously collects robot state data and ground-truth joint position data, wherein the robot state data includes the current joint position and the motor torque, and then trains an error prediction model based on the robot state data and the ground-truth joint position data, and makes the trained error prediction model work in parallel with the surgical robot control system, and superimposes the error output by the error prediction model in real time to the current joint position, and corrects the robot joint position in real-time operation. The method trains the error prediction model, and the model learns the joint position error pattern, which can effectively compensate for the joint position deviation caused by factors such as load change and mechanical fatigue, and significantly improve the calibration accuracy of the surgical robot joint position. The calibration model of the present invention can work in parallel with the robot control system and correct the joint position in real-time operation, while significantly improving the calibration accuracy of the surgical robot joint position. At the same time, it greatly improves the calibration efficiency and real-time performance. Through precise joint position calibration, the surgical robot can perform surgical tasks more accurately, reduce human errors, and improve the safety and success rate of surgery.

[0039] 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

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

[0041] Figure 1 A diagram showing the steps of a joint position calibration method for a surgical robot according to an embodiment of the present invention. DETAILED DESCRIPTION

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

[0043] like Figure 1 As shown, this embodiment provides a joint position calibration method for a surgical robot. This embodiment uses a surgical robot with seven joints as an example to illustrate the method. Joint 1 (J1, a revolute joint, unit: degrees) is the shoulder joint, used to control the left and right swing of the robotic arm; Joint 2 (J2, a revolute joint, unit: degrees) is the elbow joint, used to control the up and down pitch of the robotic arm; Joint 3 (J3, a translational joint, unit: millimeters) is the insertion joint, used to control the advance and retreat (translational motion) of the surgical instrument; Joints 4-7 (J4-J7) are the end instrument joints, used to control the instrument's posture and grip. Joint 4 (J4, a revolute joint) is used for instrument roll, Joint 5 (J5, a revolute joint) is used for wrist pitch, and Joints 6 and 7 (J6 and J7, revolute joints) are used for gripper opening and closing. The position and movement of Joints 1, 2, and 3 directly affect the spatial position of the end effector. Therefore, their precise control and calibration are particularly important in surgical robots. This embodiment's method primarily calibrates these three joints; the calibration method for the remaining joints is similar.

[0044] The joint position calibration method of the surgical robot in this embodiment specifically includes the following steps:

[0045] Step S1: generating a multi-directional zigzag calibration trajectory covering the robot joint space; wherein the calibration trajectory covers a combination of single-joint, double-joint and triple-joint directions;

[0046] Before executing step S1, the process also includes: initializing the surgical robot and executing a homing program, so that the robot explores the joint limits and registers the motor encoders to obtain initial joint position information.

[0047] Specifically, when the surgical robot is powered on, it first performs a homing procedure, which allows the robot to explore the joint limits and register the motor encoders. This process ensures that the initial state of the joint positions is known, providing a basis for subsequent calibration.

[0048] In step S1, the zigzag calibration trajectory is generated by:

[0049] Define the initial trajectory matrix, whose direction is aligned with the axis of robot joint 1;

[0050] The initial trajectory matrix is translated and rotated through the homogeneous transformation matrix and the rotation matrix to generate calibration trajectories in different directions;

[0051] The sparsity of the calibration trajectory is defined by the following formula:

[0052]

[0053] Among them, Δ s is the trajectory sparsity; r is the range of motion of the joint, r = J max- J min , J max represents the maximum allowed position of the joint, J min Indicates the minimum allowable position of the joint, in degrees for rotational joints and millimeters for translational joints; k ∈{2,3,4,5,6} is the sparsity coefficient, which is used to control the trajectory density; n ∈{1,2,3} is the trajectory direction dimension, including unidirectional, bidirectional or tridirectional.

[0054] Specifically, this step generates a multi-directional zigzag calibration trajectory. The specific process is as follows:

[0055] (1) Define the initial trajectory matrix:

[0056] Define the initial trajectory matrix T1 as a zigzag path in the normalized joint space, with the direction aligned with the axis of joint 1, and its form is:

[0057]

[0058] in, q i (k) Indicates the k Trajectory points at the joint i The position is normalized to the range of [−0.5, 0.5][−0.5, 0.5].

[0059] (2) Perform trajectory transformation operation:

[0060] The initial trajectory matrix is translated and rotated by the homogeneous transformation matrix and the rotation matrix to generate calibration trajectories in different directions.

[0061] Single joint direction trajectory: Move only along the direction of joint 1, joint 2, or joint 3. For example, the trajectory in the direction of joint 2 is:

[0062]

[0063] in, T (⋅) is the translation matrix, R ( J 3,90°) means a 90° rotation around the joint's 3 axis.

[0064] Double joint direction trajectory: Move along the direction of joint 1-2, joint 2-3 or joint 1-3. For example, the trajectory in the direction of joint 1-2:

[0065]

[0066] Among them, ⊙ represents element-by-element multiplication, which is used to adjust the trajectory range. R ( J 3,45°) means a 45° rotation around the joint's 3 axis.

[0067] Three-joint direction trajectory: Move along the direction of joints 1-2-3, the trajectory is:

[0068]

[0069] in, R ( J 2,45°) means a 45° rotation around the axis of joint 2. R ( J 1,45°) means a 45° rotation around the axis of joint 1.

[0070] (3) Sparsity control:

[0071] Trajectory sparsity Δ s Calculated by the following formula:

[0072]

[0073] in, r = J max - J min is the actual range of motion of the joint, n is the trajectory direction dimension. For example, when k =2, n =2 (bidirectional), the sparsity is smaller, the trajectory is denser, the calibration time is extended, and the error after model training is reduced; when k =6, n =1 (unidirectional), the calibration time is shortened, which is suitable for fast initialization scenarios.

[0074] The calibration trajectory in this step is defined in joint space rather than Cartesian space. Lower sparsity yields better coverage of the joint space, but also increases execution time. The direction and sparsity of the zigzag calibration trajectory can be adjusted based on the surgical robot's workspace to ensure uniform coverage of the workspace.

[0075] Step S2: Control the robot to move along the calibration trajectory, and synchronously collect robot state data and ground truth joint position data, wherein the robot state data includes current joint positions and motor torques;

[0076] In this step, the current joint position in the robot state data is obtained by calculating the motor encoder, and the motor torque in the robot state data is obtained by linear mapping the motor current command; the ground true joint position data is obtained by measuring with an external high-precision encoder.

[0077] Specifically, the ground truth joint position data can be collected by installing high-precision rotary encoders and linear encoders on the robot's mechanical arm. The robot status data can be collected in real time by the robot control system. In the specific implementation process, the current joint position is obtained through the robot control system. q raw (including 7 joint positions) and motor torque t In this embodiment, the motor torque t This is obtained through linear mapping of motor current commands and includes nine torque parameters. The first seven torque parameters correspond to the drive motors of the seven joints (J1-J7). The additional two torque parameters are reserved for the system or are redundant drive parameters (such as tool gripping force compensation and friction compensation). External encoder data is synchronously recorded via a custom system node and aligned with the robot status data using timestamps.

[0078] The constructed training data pairs are in the form of { x i ,Δ q i}, where: input (7 joint positions + 9 motor torques), output (Errors of only the first three positioning joints), which is the label value.

[0079] Step S3: Based on the robot state data and the ground truth joint position data, an error prediction model is trained, wherein the input of the error prediction model is the current joint position and motor torque in the robot state data, and the output is the joint position error;

[0080] In this step, the error prediction model includes a deep neural network model (DNN) and a linear regression model. The error prediction model dynamically switches to a deep neural network model or a linear regression model for error prediction according to real-time requirements.

[0081] The structure of the deep neural network model includes: an input layer, a hidden layer and an output layer;

[0082] The dimension of the input layer is m, and the input is m features in the robot state data, including the current joint positions and motor torques. In this embodiment, m=16, corresponding to 7 joint positions and 9 motor torques in the robot state.

[0083] The hidden layer includes two fully connected layers, each fully connected layer has 100 neurons, and the activation function is Sigmoid;

[0084] The dimension of the output layer is K, which outputs the position errors of K joints of the robot. In this embodiment, K=3, which corresponds to the position errors Δ of joints 1, 2, and 3. q 1,Δ q 2,Δ q 3.

[0085] The training parameters of the deep neural network model include:

[0086] Loss function: Mean Square Error (MSE), the formula is:

[0087]

[0088] Where L is the mean square error, N is the number of training samples, For the i The predicted joint position error of each sample is output by the DNN model with a dimension of m; For the i The true joint position error of samples is calculated as follows: ,in The first i The true joint position of each sample, is the first i The current joint positions of samples.

[0089] Optimizer: Adam, learning rate 0.001, training for 200 epochs, batch size 1024.

[0090] Regularization: L2 weight decay (coefficient 0.0005) to prevent overfitting.

[0091] Furthermore, the linear regression model is trained as follows:

[0092] The output error formula of the linear regression model is:

[0093]

[0094] Among them, Δ q j For the j The prediction error of each joint, j=1,2,3; β j0 For the j The regression intercept term of the joint; β ji For the j The first joint i The regression coefficients of the input features; x i is the i-th input feature, x i =[ q raw , t ],in q raw is the current joint position vector, t is the motor torque vector, and m is the input feature dimension.

[0095] The parameters of the linear regression model are solved by the least squares method, and the calculation formula is:

[0096] β j =(X T X) -1 X T Δq j

[0097] Among them, j=1,2,3, β j For the j The regression coefficient vector of the joints is expressed as: β j =[ β j0 , β j1 , β j2 ,…, β jm ] T ;

[0098] T represents transpose, X is the input feature matrix, and its form is:

[0099]

[0100] The first column is all 1, corresponding to the intercept term β j0 ; The following 16 columns are input features x i =[ q raw , t ].

[0101] Δq j For the j The true error vector of the joint is given by N The error composition of the samples is:

[0102] .

[0103] The goal of the least squares method is to minimize the sum of squares of the prediction errors, that is:

[0104]

[0105] untie β j is such that the predicted value X βj Projected to the true error Δq in the input feature space j The optimal approximation of .

[0106] In this method, the DNN model captures the complex error characteristics of the robot's multiple joints through nonlinear mapping, resulting in more stable performance under long-term loads, but with slower inference speed. The linear regression model, on the other hand, offers higher computational efficiency (0.38ms) and is suitable for real-time control, but with slightly lower accuracy (the error is approximately 0.07° higher than the DNN), making it suitable for latency-sensitive scenarios. Therefore, the error prediction model can dynamically switch between a deep neural network model and a linear regression model for error prediction and calibration based on real-time requirements.

[0107] Step S4: The trained error prediction model works in parallel with the surgical robot control system, and the error output by the error prediction model in real time is superimposed on the current joint position, and the robot joint position is corrected in real-time operation to improve the accuracy of the joint position.

[0108] In this step, the frequency of real-time correction is preferably 800 Hz -1200 Hz. The calibrated joint position is calculated using the following formula:

[0109] q calibrated = q raw +Δ q

[0110] in, q calibrated is the calibrated joint position vector, qraw is the current joint position vector, Δ q is the joint error vector output by the model.

[0111] Specifically, this step uses the trained calibration model in parallel with the robot control system to correct the robot's joint positions in real time to improve joint position accuracy. When the calibration model outputs joint position errors, the corrected joint positions are obtained by adding these errors to the robot's current joint positions.

[0112] Through the above steps, this method can efficiently calibrate the joint positions of the surgical robot, improve the accuracy and reliability of the surgical robot, and provide better technical support for minimally invasive surgery.

[0113] 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 method for calibrating joint positions of a surgical robot, characterized in that: include: Step S1: Generate a multi-directional zigzag calibration trajectory to cover the robot joint space; wherein the calibration trajectory covers a combination of single-joint, double-joint and triple-joint directions; Step S2: Control the robot to move along the calibration trajectory, and synchronously collect robot state data and ground truth joint position data, wherein the robot state data includes current joint positions and motor torques; Step S3: Based on the robot state data and the ground truth joint position data, an error prediction model is trained, wherein the input of the error prediction model is the current joint position and motor torque in the robot state data, and the output is the joint position error; The error prediction model includes a deep neural network model and a linear regression model. The structure of the deep neural network model includes: an input layer, a hidden layer, and an output layer. The dimension of the input layer is m, and the input is m features in the robot state data, including the current joint position and motor torque. The hidden layer includes two fully connected layers, each with 100 neurons and a Sigmoid activation function. The dimension of the output layer is K, and it outputs the position errors of K joints of the robot. The training parameters of the deep neural network model include: Loss function: Mean Square Error (MSE), the formula is: , where L is the mean square error, N is the number of training samples, For the i The predicted joint position error of each sample is output by the DNN model with a dimension of m; For the i The true joint position error of samples is calculated as follows: ,in The first i The true joint position of each sample, is the first i The current joint position of each sample; The linear regression model is trained as follows: The output error formula of the linear regression model is: , where Δ q j For the j The prediction error of each joint, j=1,2,3; β j0 For the j The regression intercept term of the joint; β ji For the j The first joint i The regression coefficients of the input features; x i is the i-th input feature, x i =[ q raw , τ ],in q raw is the current joint position vector, τ is the motor torque vector, m is the input feature dimension; Step S4: The trained error prediction model works in parallel with the surgical robot control system, and the joint position error output by the error prediction model in real time is superimposed on the current joint position, and the robot joint position is corrected in real-time operation to improve the accuracy of the joint position.

2. The joint position calibration method of a surgical robot according to claim 1, characterized in that: In step S1, the zigzag calibration trajectory is generated by: Define the initial trajectory matrix, whose direction is aligned with the axis of the robot shoulder joint; The initial trajectory matrix is translated and rotated through the homogeneous transformation matrix and the rotation matrix to generate calibration trajectories in different directions; The sparsity of the calibration trajectory is defined by the following formula: , where Δ s is the trajectory sparsity; r is the range of motion of the joint, r = J max- J min , J max represents the maximum allowed position of the joint, J min Indicates the minimum allowable position of the joint, in degrees for rotational joints and millimeters for translational joints; k ∈{2,3,4,5,6} is the sparsity coefficient, which is used to control the trajectory density; n ∈{1,2,3} is the trajectory direction dimension, including unidirectional, bidirectional or tridirectional.

3. The method for calibrating joint positions of a surgical robot according to claim 1, wherein: In step S2, the synchronous acquisition of robot state data and ground truth joint position data includes: Obtain the current joint position in the robot state data by calculating through a motor encoder, and obtain the motor torque in the robot state data by linear mapping of a motor current command; The ground truth joint position data is obtained by measuring with an external high-precision encoder.

4. The method for calibrating joint positions of a surgical robot according to claim 1, wherein: The parameters of the linear regression model are solved by the least squares method, and the calculation formula is: β j =(X T X) -1 X T Δq j Where j = 1, 2…K, β j For the j The regression coefficient vector of the joints is expressed as: β j =[ β j0 , β j1 , β j2 ,…, β jm ] T ; X is the input feature matrix, Δq j For the j The error vector of the joint, T Indicates transpose.

5. The method for calibrating joint positions of a surgical robot according to claim 1, wherein: The error prediction model is dynamically switched to a deep neural network model or a linear regression model for error prediction according to real-time requirements.

6. The method for calibrating joint positions of a surgical robot according to claim 1, wherein: In step S4, the frequency of real-time correction is 800 Hz-1200 Hz.

7. The joint position calibration method of a surgical robot according to claim 1, characterized in that: The calibrated joint positions are calculated using the following formula: q calibrated = q raw +D q in, q calibrated is the calibrated joint position vector, q raw is the current joint position vector, Δ q is the joint error vector output by the model.

8. The method for calibrating joint positions of a surgical robot according to claim 1, wherein: Before executing step S1, the process also includes: initializing the surgical robot and executing a homing program, so that the robot explores the joint limits and registers the motor encoders to obtain initial joint position information.

Images