Surgical robot compliance control method and system

Abstract

The invention discloses a compliance control method and system for a surgical robot, and belongs to the field of robot control algorithms, and the method comprises the steps: obtaining the position information, speed information and current information of a robot joint module; performing gravity compensation according to the position information; friction force compensation is carried out according to the speed information; friction torque residual error compensation is carried out through an RBF neural network; and the starting torque of the joint module is optimized. According to the robot compliance control method, the system can be simplified and the cost can be reduced while the control effect is ensured; according to the friction torque residual compensation after the RBF neural network is added, the torque compensation of the joint module is more uniform, the external force required during dragging operation is smaller, and overcompensation is avoided.

Description

Technical Field

[0001] The present invention belongs to the field of robot control algorithms, and particularly relates to a compliant control method and system for a surgical robot. Background Art

[0002] Traditional robot teaching mainly relies on the operation of a teaching box, requiring the operator to have certain professional knowledge. By debugging each action of the robot, robot teaching is completed. This teaching method has high requirements for operators, and the implementation process is complex and inefficient. By using a control method to compensate for the gravity and friction force on each joint of the robot, the robot is approximately in a state of not being affected by force, so that people can easily drag each joint of the robot. Compliant control can simplify the operation of robot teaching. Compared with teaching by a teaching box, it does not require the operator to have relevant professional knowledge, and the operation is simple and efficient.

[0003] CN115781687A discloses a sensorless robot compliant control method. According to force analysis, the mapping relationship between the encoder change amount of the robot and the external force being dragged is calculated, so as to calculate the external force on the joint in combination with the collected encoder change amount; a admittance control model is constructed to calculate the target torque of the joint and control the joint movement. CN116292593A discloses a ball socket joint, a discrete continuum and a minimally invasive surgical robot, applying the ball socket joint to the discrete continuum to improve the connection stability between the joints of the continuum and improve the bending compliance and bending flexibility of the continuum. Existing compliant control methods compensate for friction through a static friction model or a dynamic friction model. However, the static friction model is difficult to accurately model the friction force, and the dynamic friction model is too complex and difficult to accurately identify the parameters. Summary of the Invention

[0004] The technical problem to be solved by the present invention is: in the process of friction force modeling for the compliant control of a surgical robot, how to optimize the starting torque to reduce the starting torque during compliant control, so that the dragging is smoother.

[0005] To achieve the above object, the technical solution adopted by the present invention is:

[0006] A compliant control method for a surgical robot, comprising the following steps:

[0007] Step 1: Obtain the position information, speed information and current information of the joint module;

[0008] Step 2: Perform gravity compensation according to the position information fed back by the joint module;

[0009] Step 3: Perform friction compensation according to the speed information fed back by the joint module;

[0010] Step 4: Compensate the frictional torque residual through a neural network based on the speed information and position information fed back by the joint module;

[0011] Step 5: Optimize the starting torque of the joint module according to the gravity compensation, friction compensation, and frictional torque residual compensation results in Steps 2, 3, and 4.

[0012] In the aforementioned compliant control method for a surgical robot, in Step 2, within a motion cycle of the joint module, the speed magnitudes in the first half cycle and the second half cycle are the same but in opposite directions. Taking the speed jump point as the axis, the sum of the symmetrically measured currents on both sides is the gravity compensation current. The expression of the gravity compensation model is:

[0013] where q is the position of the joint module, is the gravity compensation current, and a, b, and c are the first fitting coefficient, the second fitting coefficient, and the third fitting coefficient respectively obtained by fitting the gravity compensation current measurement data.

[0014] In the aforementioned compliant control method for a surgical robot, in Step 3, taking the speed jump point as the axis, the difference between the symmetric currents on both sides is the friction compensation current. Taking the root mean square of the friction compensation currents measured at the same speed, the obtained value is the friction compensation current at that speed. According to the measurement data of the friction compensation current, the friction compensation model is obtained, and the expression is:

[0015]

[0016] where is is the friction compensation, the maximum static friction, is the Coulomb friction, m is the viscous frictional torque proportionality coefficient, is the switching speed, is the rotational angular velocity of the joint module, is the direction of the speed.

[0017] In the aforementioned compliant control method for a surgical robot, in Step 4, the expression of the frictional torque residual compensation obtained using an RBF neural network based on the speed information and position information of the joint module is:

[0018]

[0019] where, F RBF is the output frictional torque residual compensation, are the network weights, is the output of the nth hidden layer node.

[0020] The aforementioned compliant control method for a surgical robot. In step five, the total expression of the output torque is:

[0021]

[0022] where q is the position of the joint module, is the speed of the joint module, K is the friction torque scaling coefficient, F RBF is the friction torque residual compensation, is the friction compensation, is the gravity compensation, is the output torque.

[0023] The aforementioned compliant control method for a surgical robot. In step five, define state = 0 or 1. state = 0 represents the stationary state, and state = 1 represents the motion state;

[0024] When , drag normally according to the compliant control, that is, perform gravity compensation, friction compensation, and RBF friction torque residual compensation;

[0025] When , set the torque according to the state of state and the difference between the position of the joint module and the position in the previous cycle. When the joint module was stationary in the previous cycle and , the output torque at startup is:

[0026]

[0027] where V T is the given speed threshold, is the given encoder front and back state difference threshold, is the maximum static friction, represents the current moment the position feedback by the joint module;

[0028] Otherwise, limit the output torque at startup to only perform gravity compensation, that is , so that the joint module remains stable at a speed of 0 without vibration.

[0029] The aforementioned compliant control method for a surgical robot. In step three,

[0030] The expression of friction compensation in the low-speed state is:

[0031]

[0032] The expression of friction compensation in the high-speed state is:

[0033]

[0034] is the maximum static friction force in the low-speed state, is the Coulomb friction force in the low-speed state, is the viscous friction torque proportionality coefficient in the low-speed state, is the switching speed in the low-speed state;

[0035] is the maximum static friction force in the high-speed state, is the Coulomb friction force in the high-speed state, is the viscous friction torque proportionality coefficient in the high-speed state, is the switching speed in the high-speed state.

[0036] A computer system includes a memory, a processor, and a computer program stored on the memory. The processor executes the computer program to implement the steps of the above method.

[0037] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, it implements the steps of the above method.

[0038] Beneficial effects achieved by the present invention: After adding the friction torque residual compensation to the robot compliant control method of the present invention, the torque compensation is more uniform, the external force required for dragging is smaller, and over-compensation does not occur. Compared with the zero-force control method using joint torque sensors in the prior art, the robot compliant control method of the present invention can simplify the system and reduce costs while ensuring the control effect. Description of the Drawings

[0039] Figure 1 is the flow chart of the surgical robot compliant control method in Embodiment 1 of the present invention;

[0040] Figure 2 is the position, current, and current data diagram of the joint module in Embodiment 1 of the present invention;

[0041] Figure 3 is the position, speed, current, and current preprocessing data diagram of the joint module at a certain speed in Embodiment 1 of the present invention;

[0042] Figure 4(a) is the comparison diagram of the current compensation results without adding neural network compensation at a certain speed in Embodiment 1 of the present invention;

[0043] Figure 4(b) is the comparison diagram of the current compensation results with adding neural network compensation at a certain speed in Embodiment 1 of the present invention;

[0044] Figure 5 is the gravity compensation current diagram in Embodiment 1 of the present invention;

[0045] Figure 6 It is the friction compensation current diagram in Embodiment 2 of the present invention. Specific Embodiment

[0046] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative work shall fall within the protection scope of the present invention.

[0047] Embodiment 1

[0048] See Figure 1 , this embodiment provides a compliant control method for a surgical robot, including the following steps:

[0049] Step 1: Obtain the position information, speed information, and current information of the joint module.

[0050] Use the position sensor, speed sensor, and current sensor integrated in the joint module to measure and feedback the position information, speed information, and current information of the joint module; as Figure 2 shown is the position, speed, and current data diagram of the joint module obtained by testing, where column 1 is the measured position information of the joint module, column 2 is the measured speed information of the joint module, column 3 is the measured current information of the joint module, and the joint module performs a quasi-periodic reciprocating motion.

[0051] The measurement data obtained in this embodiment are all tested on the joint module of zero-difference cloud control.

[0052] Step 2: Perform gravity compensation according to the position information fed back by the joint module.

[0053] An unframed torque motor is used in the joint module. The force output by the joint module is mainly used to overcome gravity and friction. There is a good linear relationship between the output torque of the motor and the motor current. In this embodiment, the gravity compensation and friction compensation of the joint module are realized by controlling the compensation current of the motor.

[0054] As Figure 3 shown, the data measured when the speed of the joint module is 5.4 degrees / second, where column 1 is the measured position information of the joint module, column 2 is the measured speed information of the joint module, column 3 is the measured current information of the joint module, and column 4 is the current information after passing through a low-pass filter.

[0055] The low-pass filter is a Butterworth low-pass filter, which has a simple structure and is easy to implement. After filtering, it can effectively remove the interference of high-frequency signals and improve the accuracy of the measured current.

[0056] It is actually tested that the gravity compensation value has the same change trend as the gravity compensation current, that is, gravity compensation can be achieved according to the magnitude of the gravity compensation current.

[0057] In a motion cycle of the joint module, the speed magnitudes in the first half cycle and the second half cycle are the same but the directions are opposite. The measured current of the joint module also changes with the change of speed. Taking the speed jump point as the axis, the sum of the symmetric currents on the left and right sides is the gravity compensation current. The expression of the gravity compensation model is:

[0058] where q is the position of the joint module, is the gravity compensation current, and a, b, and c are the first fitting coefficient, the second fitting coefficient, and the third fitting coefficient respectively obtained by fitting the gravity compensation current measurement data.

[0059] As Figure 5 shown, the gravity compensation current obtained according to the gravity compensation model has the same change trend as the actually measured current, and the similarity is high, indicating that the gravity compensation model used in the present invention has high accuracy.

[0060] Step 3: Perform friction compensation according to the speed information fed back by the joint module.

[0061] The force output by the joint module mainly overcomes gravity and friction. It is approximately considered that friction is an odd function of speed. Referring to Figure 3 shown, in a motion cycle of the joint module, the speed magnitudes in the first half cycle and the second half cycle are the same but the directions are opposite. The measured current of the joint module also changes with the change of speed. Taking the speed jump point as the axis, the difference between the symmetric currents on the left and right sides is the friction compensation current. Taking the root mean square of the friction compensation currents measured at the same speed, the obtained value is the friction compensation current at that speed. Then, according to the measurement data of the friction compensation current, the friction compensation model can be obtained, and the expression is:

[0062]

[0063] where, is the friction compensation, is the maximum static friction, is the Coulomb friction, m is the viscous friction torque proportionality coefficient, is the switching speed, is the joint module speed, is the direction of the speed.

[0064] Through friction compensation, the limitation of friction on the movement of the joint module can be effectively overcome, and the compliance and control accuracy of the joint module operation can be improved.

[0065] Step 4: Compensate the frictional torque residual through a neural network based on the speed information and position information fed back by the joint module.

[0066] In the frictional force compensation in Step 3, it is approximately considered that the frictional force is a function of speed. However, in practice, the frictional force effects at different positions are not exactly the same and are related to equipment assembly, machining errors, etc. To compensate for the errors generated during system modeling and parameter identification, in this embodiment, a Radial Basis Function (RBF) neural network is used to compensate for the residual. The expression for compensating the frictional torque residual obtained using the RBF neural network based on the speed information and position information of the joint module is:

[0067]

[0068] where, F RBF is the compensated output of the frictional torque residual, is the network weight, is the output of the nth hidden layer node. As shown in Fig. 4(a), it is a comparison diagram of the actual current and the compensated current without adding neural network compensation. As shown in Fig. 4(b), it is a comparison diagram of the actual current and the compensated current after adding neural network compensation. It can be seen that the compensated current after adding the RBF neural network is closer to the actual current. Using the RBF neural network to compensate for the frictional force residual can improve the control accuracy.

[0069] It should be noted that the neural network model that can be used in the present invention is not limited to the RBF neural network, and other well-known neural networks can also complete the compensation of the frictional torque residual.

[0070] Step 5: Optimize the starting torque of the joint module according to the results of gravity compensation, frictional force compensation, and frictional torque residual compensation in Step 2, Step 3, and Step 4.

[0071] Define state = 0 or 1, where state = 0 represents the stationary state and state = 1 represents the moving state.

[0072] The change condition of the state is:

[0073]

[0074] state = 0

[0075]

[0076] state = 1

[0077] represents the position fed back by the joint module at the current moment and Indicates the position feedback by the joint module at the previous moment, Indicates logical AND, Take the absolute value.

[0078] For joints without force sensors, the drag torque at the start-up moment has always been a difficulty faced, and it is also one of the advantages of joints with force sensors. Therefore, the present invention optimizes the output torque of the joint module at the start-up moment.

[0079] When At this time, drag normally according to compliant control, that is, perform gravity compensation, friction compensation, and RBF friction torque residual compensation;

[0080] When At this time, the torque is set according to the state and the difference between the position of the joint module and the position in the previous cycle. When the joint module was stationary in the previous cycle and At this time, the output torque at start-up is:

[0081]

[0082] Among them, V T Is the given speed threshold, Is the given difference threshold of the encoder front and back states, Is the maximum static friction;

[0083] Otherwise, limit that the output torque at start-up only performs gravity compensation, that is , so that the joint module can remain stable even when the speed is 0 and no vibration will occur.

[0084] In summary, the total expression of the output torque of the compliant control method is:

[0085]

[0086] Among them, q is the position of the joint module, Is the speed of the joint module, K is the friction torque scaling coefficient, F RBF Is the friction torque residual compensation, Is the friction compensation, Is the gravity compensation, Is the output torque.

[0087] Embodiment 2

[0088] This embodiment provides a compliant control method for a surgical robot, including the following steps:

[0089] Step 1: Obtain the position information, speed information, and current information of the joint module;

[0090] Step 2: Perform gravity compensation according to the position information feedback by the joint module;

[0091] Step 3: Perform friction compensation based on the speed information fed back by the joint module;

[0092] Step 4: Compensate for the friction torque residual through a neural network based on the speed information and position information fed back by the joint module;

[0093] Step 5: Optimize the starting torque of the joint module according to the gravity compensation, friction compensation, and friction torque residual compensation results of Step 2, Step 3, and Step 4.

[0094] In Step 3, taking the speed jump point as the axis, the friction compensation current is obtained by subtracting the symmetric currents on the left and right sides. The root mean square of the measured friction compensation currents at the same speed is calculated, and the resulting value is the friction compensation current at that speed. Based on the measurement data of the friction compensation current, a friction compensation model is obtained, and its expression is:

[0095]

[0096] where, is the friction compensation, the maximum static friction, is the Coulomb friction, m is the proportionality coefficient of the viscous friction torque, is the switching speed, is the angular velocity of the joint module rotation, is the direction of the speed.

[0097] In reality, the maximum static friction, Coulomb friction, and proportionality coefficient of the viscous friction torque will change due to the change in speed. As shown in Figure 6 the relationship between the measured friction compensation current and speed is shown. Therefore, it is necessary to correct the above friction compensation expression according to the actual situation. In a possible embodiment, the speed of the joint module lower than 1.5 degrees / second is regarded as the low-speed state, and higher than 1.5 degrees / second is regarded as the high-speed state.

[0098] Refer to Figure 6 as shown, it can be obtained that the expression of the friction compensation in the low-speed state is:

[0099]

[0100] The expression of the friction compensation in the high-speed state is:

[0101] .

[0102] is the maximum static friction in the low-speed state, is the Coulomb friction in the low-speed state, is the proportionality coefficient of the viscous friction torque in the low-speed state, is the switching speed in the low-speed state;

[0103] is the maximum static friction in the high-speed state, is the Coulomb friction in the high-speed state, is the proportionality coefficient of the viscous friction torque in the high-speed state, is the switching speed in the high-speed state.

[0104] The corrected friction compensation model is more consistent with the actual current, which can effectively improve the accuracy of compliant control.

[0105] The specific demarcation line between high speed and low speed needs to be determined according to the actual joint module structure and test environment, and is not strictly limited to a certain fixed value.

[0106] The robot compliant control method of the present invention can effectively overcome the adverse effects of gravity and friction on robot operation, realize the compliance of the joint module. Compared with the traditional zero-force control method using joint torque sensors, this method can simplify the system and reduce costs while ensuring the control effect. After adding the RBF neural network friction torque residual compensation, the torque compensation of the joint module is more uniform, the external force required for dragging is smaller, and over-compensation will not occur.

[0107] Example 3

[0108] A computer system includes a memory, a processor, and a computer program stored on the memory, and the processor executes the computer program to implement the steps of the above method.

[0109] Example 4

[0110] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, it implements the steps of the above method.

[0111] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, and the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the specified functions in one process Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0112] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to operate in a particular manner, such that the instructions stored in the computer-readable memory produce a manufacture including an instruction device that implements the functions specified in one or more of the processes and / or blocks Figure 1 of one or more of the processes and / or blocks Figure 1 specified in the flowchart.

[0113] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable apparatus provide steps for implementing the functions specified in one or more of the processes and / or blocks Figure 1 of one or more of the processes and / or blocks Figure 1 specified in the flowchart.

[0114] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the technical principle of the present invention, several improvements and modifications can be made, and these improvements and modifications should also be regarded as the protection scope of the present invention.

Claims

1. A compliant control method for a surgical robot, characterized in that: The following steps are involved: Step 1: Obtain the position information, speed information and current information of the joint module; Step 2: Perform gravity compensation based on the position information fed back by the joint module; Step 3: Compensate for friction according to the speed information fed back by the joint module; Step 4: Compensate the friction torque residual through a neural network based on the speed information and position information fed back by the joint module; Step 5: According to the gravity compensation, friction compensation and friction torque residual compensation results of steps 2, 3 and 4, the starting torque of the joint module is optimized.

2. A surgical robot compliance control method according to claim 1, characterized in that: In step 2, in one motion cycle of the joint module, the speeds of the first half cycle and the second half cycle are the same in magnitude and opposite in direction. With the speed jump point as the axis, the symmetrical measurement currents on the left and right sides are added to form the gravity compensation current. The expression of the gravity compensation model is: Among them, q is the position of the joint module, is the gravity compensation current, a, b, and c are the fitting coefficient 1, fitting coefficient 2, and fitting coefficient 3 respectively obtained by fitting the gravity compensation current measurement data.

3. A surgical robot compliance control method according to claim 1, characterized in that: In step 3, the friction compensation current is obtained by subtracting the symmetrical currents on the left and right sides with the speed jump point as the axis. The friction compensation current measured at the same speed is taken as the root mean square, and the obtained value is the friction compensation current at the speed. The friction compensation model is obtained based on the measurement data of the friction compensation current, and the expression is: Among them, To compensate for friction, Maximum static friction, is the Coulomb friction force, m is the proportional coefficient of the viscous friction torque, is the switching speed, is the angular velocity of the joint module, is the direction of velocity.

4. A surgical robot compliance control method according to claim 1, characterized in that: In step 4, the friction torque residual compensation expression obtained by using the RBF neural network according to the speed information and position information of the joint module is: Among them, F RBF is the output friction torque residual compensation, is the network weight, Output of the nth hidden layer node.

5. A surgical robot compliance control method according to claim 1, characterized in that: In step 5, the total expression of the output torque is: Among them, q is the position of the joint module, is the joint module speed, K is the friction torque scaling factor, F RBF is the friction torque residual compensation, To compensate for friction, For gravity compensation, is the output torque.

6. A surgical robot compliance control method according to claim 5, characterized in that: In step 5, define state=0 or 1, where state=0 indicates a stationary state and state=1 indicates a moving state; when When , drag normally according to the soft control, that is, gravity compensation, friction compensation, and RBF friction torque residual compensation; when When the joint module is stationary in the previous cycle and When , the output torque at startup is: Among them, V T is a given speed threshold, is the threshold of the difference between the states before and after a given encoder, is the maximum static friction, Indicates the current time Position of joint module feedback; Otherwise, the output torque at startup is limited to gravity compensation only, that is, , so that the joint module remains stable and does not vibrate when the speed is 0.

7. A surgical robot compliance control method according to claim 3, characterized in that: In step three, The expression of friction compensation at low speed is: The expression of friction compensation at high speed is: is the maximum static friction at low speed, The Coulomb friction force at low speed is is the proportional coefficient of viscous friction torque at low speed, Switch speed when in low speed state; The maximum static friction at high speed, Coulomb friction force at high speed, is the proportional coefficient of viscous friction torque at high speed, Switch speed when in high speed state.

8. A computer system, characterized in that: The invention comprises a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images