Joint position calibration method of surgical robot

Abstract

The invention relates to the technical field of surgical robot control, and discloses a surgical robot joint position calibration method, which comprises the following steps: generating a multi-directional zigzag calibration track; the robot is controlled to move along the calibration track, robot state data and ground real joint position data are synchronously collected, and the robot state data comprise the current joint position and the electrode torque; based on the robot state data and the ground real joint position data, an error prediction model is trained, the input of the error prediction model is the current joint position and the motor torque in the robot state data, and the output of the error prediction model is the joint position error; and the trained error prediction model and a surgical robot control system work in parallel, errors output by the error prediction model in real time are superposed to the original joint position, and the robot joint position is corrected in real-time operation so as to improve the precision of the joint position. According to the invention, real-time, efficient, automatic and accurate calibration of the joint position of the surgical robot can be realized.

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: 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; 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 electrode 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; Step S4: The trained error prediction model works in parallel with the surgical robot control system, the error output by the error prediction model in real time is superimposed on the original joint position, and the robot joint position is corrected in real-time operation to improve the accuracy of the joint position.

[0007] Optionally, in step S1, the zigzag calibration trajectory is generated by: Define the initial trajectory matrix, whose direction is aligned with the axis of robot joint 1; 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 joint motion, r = J max -J min , J max Indicates the maximum allowable position of the joint, J min Represents the minimum allowed position of the joint, in degrees for rotational joints and millimeters for translational joints; k∈{2,3,4,5,6} is the sparsity coefficient used to control the trajectory density; n∈{1,2,3} is the trajectory direction dimension, including unidirectional, bidirectional, or tridirectional.

[0008] Optionally, in step S2, the synchronously collecting robot state data and ground truth joint position data includes: Obtain the original joint position in the robot state data by calculating through a motor encoder, and obtain the electrode 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.

[0009] Optionally, in step S3, the error prediction model includes a deep neural network 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 electrode torque; The hidden layer includes two fully connected layers, each fully connected layer has 100 neurons, and the activation function is Sigmoid; The dimension of the output layer is K, and the position errors of K joints of the robot are output; The loss function of the deep neural network model is: Where L is the mean square error, N is the number of training samples, is the predicted joint position error of the i-th sample, output by the DNN model, with a dimension of m; is the true joint position error of the i-th sample, and the calculation formula is: ,in is the true joint position of the ith sample measured by an external high-precision encoder, is the original joint position of the ith sample inferred from the motor encoder.

[0010] Optionally, in step S3, the error prediction model further includes a linear regression model, and its output error formula is: Where Δq j is the prediction error of the j-th joint, j=1,2…K; β j0 is the regression intercept term of the j-th joint; β ji is the regression coefficient of the i-th input feature of the j-th joint; x i is the i-th input feature, x i =[q raw ,τ], where q raw is the current joint position vector, τ is the motor torque vector, and m is the input feature dimension.

[0011] Optionally, 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 is the regression coefficient vector of the jth joint, and its vector is expressed as: β j =[β j0 ,β j1 ,β j2 ,…,β jm ] T ; X is the input feature matrix, Δq j is the error vector of the jth joint, and T represents the transpose.

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

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

[0014] Optionally, the calibrated joint positions are calculated using the following formula: q calibrated =q raw +Δq Among them, q calibrated is the calibrated joint position vector, q raw is the current joint position vector, and Δq is the joint error vector output by the model.

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

[0016] The beneficial effects of the present invention are: 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 electrode 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 original 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 improves 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.

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

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

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

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

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

[0022] The joint position calibration method of the surgical robot in this embodiment specifically includes the following steps: 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; 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.

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

[0024] In step S1, the zigzag calibration trajectory is generated by: Define the initial trajectory matrix, whose direction is aligned with the axis of robot joint 1; 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 joint motion, r = J max -J min , J max Indicates the maximum allowable position of the joint, J min Represents the minimum allowed position of the joint, in degrees for rotational joints and millimeters for translational joints; k∈{2,3,4,5,6} is the sparsity coefficient used to control the trajectory density; n∈{1,2,3} is the trajectory direction dimension, including unidirectional, bidirectional, or tridirectional.

[0025] Specifically, this step generates a multi-directional zigzag calibration trajectory. The specific process is as follows: (1) Define the initial trajectory matrix: 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, in the form of: Among them, q i (k) represents the position of the kth trajectory point at joint i, and its value range is normalized to [−0.5, 0.5][−0.5, 0.5].

[0026] (2) Perform trajectory transformation operation: The initial trajectory matrix is ​​translated and rotated by the homogeneous transformation matrix and the rotation matrix to generate calibration trajectories in different directions.

[0027] 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: Where T(⋅) is the translation matrix and R(J3,90°) represents a 90° rotation around the joint 3 axis.

[0028] 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: Where ⊙ represents element-by-element multiplication, which is used to adjust the trajectory range, and R(J3,45°) represents a 45° rotation around the axis of joint 3. Three-joint direction trajectory: Moving along the direction of joints 1-2-3, the trajectory is: Here, R(J2,45°) indicates a 45° rotation around the axis of joint 2, and R(J1,45°) indicates a 45° rotation around the axis of joint 1.

[0029] (3) Sparsity control: The trajectory sparsity Δs is calculated as follows: Where r = J max -J min , is the actual range of joint motion, and n is the trajectory direction dimension. For example, when k=2 and n=2 (bidirectional), the sparsity is small, the trajectory is denser, the calibration time is prolonged, and the error after model training is reduced. When k=6 and n=1 (unidirectional), the calibration time is shortened, which is suitable for fast initialization scenarios.

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

[0031] 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 electrode torques; In this step, the original joint position in the robot state data is obtained by calculating the motor encoder, and the electrode 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.

[0032] Specifically, the ground truth joint position data can be collected by installing high-precision rotary encoders and linear encoders on the robot's manipulator. The robot state data can be collected in real time by the robot control system. In the specific implementation process, the original joint position q is obtained by the robot control system. raw(including 7 joint positions) and motor torque τ. In this embodiment, motor torque τ is obtained by linearly mapping the motor current command. It contains 9 torque parameters. The first 7 torque parameters correspond to the drive motors of the 7 joints (J1-J7). The additional 2 torque parameters are reserved or redundant drive parameters (such as tool grip force compensation and friction compensation). External encoder data is synchronously recorded through a custom system node and aligned with the robot status data using timestamps.

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

[0034] 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; 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.

[0035] 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 electrode torque; in this embodiment, m=16, corresponding to 7 joint positions and 9 motor torques in the robot state.

[0036] The hidden layer includes two fully connected layers, each fully connected layer has 100 neurons, and the activation function is Sigmoid; 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 output position errors Δq1, Δq2, and Δq3 of joints 1, 2, and 3.

[0037] 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, is the predicted joint position error of the i-th sample, output by the DNN model, with dimension m; is the true joint position error of the i-th sample, and the calculation formula is: ,in is the true joint position of the ith sample measured by an external high-precision encoder, is the original joint position of the ith sample inferred from the motor encoder.

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

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

[0040] Furthermore, the linear regression model is trained as follows: The output error formula of the linear regression model is: Where Δq j is the prediction error of the j-th joint, j=1,2,3; β j0 is the regression intercept term of the j-th joint; β ji is the regression coefficient of the i-th input feature of the j-th joint; x i is the i-th input feature, x i =[q raw ,τ], where q raw is the current joint position vector, τ is the motor torque vector, and m is the input feature dimension.

[0041] 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, 3, β j is the regression coefficient vector of the jth joint, and its vector is expressed as: β j =[β j0 ,β j1 ,β j2 ,…,β jm ] T ; T represents transpose, X is the input feature matrix, and its form is: The first column is all 1, corresponding to the intercept term β j0 ; The subsequent 16 columns are input features x i =[q raw ,τ].

[0042] Δq j is the true error vector of the j-th joint, which is composed of the errors of N samples, namely: .

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

[0044] Solution β 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 .

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

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

[0047] 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: q calibrated =q raw +Δq Among them, q calibrated is the calibrated joint position vector, q raw is the current joint position vector, and Δq is the joint error vector output by the model.

[0048] Specifically, this step involves running the trained calibration model in parallel with the robot control system to calibrate the robot's joint positions in real time to improve joint position accuracy. When the calibration model outputs joint position errors, these errors are added to the robot's original joint positions to generate the corrected joint positions.

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

[0050] 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 covering the robot joint space; 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 real joint position data, wherein the robot state data includes current joint positions and electrode torques; Step S3: Based on the robot state data and the ground true 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; Step S4: The trained error prediction model works in parallel with the surgical robot control system, the joint position error output by the error prediction model in real time is superimposed on the original 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 method for generating the zigzag calibration trajectory is: Define the initial trajectory matrix, whose direction is aligned with the axis of robot joint 1; 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 joint motion, r = J max -J min , J max represents the maximum allowable position of the joint, J min Represents the minimum allowed position of the joint, in degrees for rotational joints and millimeters for translational joints; k∈{2,3,4,5,6} is the sparsity coefficient used to control the trajectory density; n∈{1,2,3} is the trajectory direction dimension, including unidirectional, bidirectional or tridirectional.

3. The joint position calibration method of a surgical robot according to claim 1, characterized in that: In step S2, the synchronous acquisition of robot state data and ground truth joint position data includes: Obtain the original joint position in the robot state data by calculating through a motor encoder, and obtain the electrode 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, characterized in that: In step S3, the error prediction model includes a deep neural network 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 electrode torque; The hidden layer includes 2 fully connected layers, each fully connected layer has 100 neurons, and the activation function is Sigmoid; The dimension of the output layer is K, and the position errors of K joints of the robot are output; The loss function of the deep neural network model is: Among them, L is the mean square error, N is the number of training samples, is the predicted joint position error of the i-th sample, output by the DNN model, with a dimension of m; is the true joint position error of the i-th sample, and the calculation formula is: ,in is the true joint position of the ith sample measured by an external high-precision encoder, is the original joint position of the ith sample inferred from the motor encoder.

5. The method for calibrating joint positions of a surgical robot according to claim 4, characterized in that: In step S3, the error prediction model also includes a linear regression model, and its output error formula is: Among them, Δq j is the prediction error of the jth joint, j=1,2…K; β j0 is the regression intercept term of the jth joint; β ji is the regression coefficient of the i-th input feature of the j-th joint; x i is the i-th input feature, x i =[q raw ,τ], where q raw is the current joint position vector, τ is the motor torque vector, and m is the input feature dimension.

6. The method for calibrating joint positions of a surgical robot according to claim 5, characterized in that: 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 is the regression coefficient vector of the jth joint, and its vector is expressed as: β j =[β j0 ,β j1 ,β j2 ,…,β jm ] T ; X is the input feature matrix, Δq j is the error vector of the jth joint, and T represents the transpose.

7. The method for calibrating joint positions of a surgical robot according to claim 6, characterized in that: 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.

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

9. The method for calibrating joint positions 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 +Δq Among them, q calibrated is the calibrated joint position vector, q raw is the current joint position vector, and Δq is the joint error vector output by the model.

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

Images