A control method for a robotic end effector

Abstract

The present invention discloses a control method for a robotic end effector, comprising: determining the target position of the replaced effector according to the length difference between the effectors before and after replacement; compensating for the gravity moment and friction moment of the effector according to the robotic dynamics model to control the movement of the effector; when the replaced effector moves to a position at a preset distance from the target position, using a PD control model with fixed gravity compensation to control it. The present invention can achieve a smooth stop during the dragging process of the effector, and can adjust the memory position according to different installed effectors to keep the position of the end of the effector consistent.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and particularly to a control method for the end effector of a robot. Background Art

[0002] Currently, minimally invasive surgery has basically replaced open surgery as the main direction of development in the field of surgical medicine. Compared with traditional open surgery, minimally invasive surgery has the advantages of less trauma, less pain, and faster recovery. With the development of robot technology, minimally invasive surgery based on laparoscopic surgical robots has gradually matured and been widely applied.

[0003] The doctor controls the end effector of the slave arm by operating the master hand to complete complex surgical operations. However, during the operation, it is inevitable to replace the end effector. At this time, the following problems will exist: ① After replacing the instrument, the doctor cannot accurately remember the target position before inserting and removing the instrument, and needs to adjust the position again, wasting time and energy; ② After reinstalling the instrument, if the dragged position is far from the previous target position, continuous adjustment is required, or the dragging force is too large, resulting in the instrument moving beyond the target position and damaging the organ; ③ Different instruments have different lengths. After replacing the instrument, the installation position is different from before. If adjusted according to the memory position, a large deviation may be caused.

[0004] In the existing control scheme, when the motor suddenly loses power enabling when dragging the instrument to the target position, it will give people the illusion of a motor failure, and the stop action is too sudden, affecting the doctor's operation experience. In addition, after the motor is in a non-use state, it needs to be enabled again to complete subsequent operations. This action needs to be repeated every time the instrument is replaced. Repeated enabling of the motor will undoubtedly reduce the service life of the motor. Summary of the Invention

[0005] Object of the Invention: Aiming at the above deficiencies, the present invention proposes a control method for the end effector of a robot, which can achieve a smooth stop during the dragging process of the instrument, make the continuous actions of movement and stop more coherent and compliant, and can adjust the memory position according to the different installed instruments to keep the end position of the instrument consistent.

[0006] Technical Solution:

[0007] A control method for the end effector of a robot, comprising:

[0008] Determining the target position of the instrument after replacement according to the length difference between the instruments before and after replacement;

[0009] Compensating the gravity moment and friction moment of the instrument according to the robot dynamics model to control the movement of the instrument; when the instrument after replacement moves to a position at a preset distance from the target position, a PD control model with fixed gravity compensation is used to control it.

[0010] The PD control model with fixed gravity compensation is specifically as follows:

[0011]

[0012] where τ is the target torque; q d is the desired position of the instrument, and G(q d ) is the gravity torque of the instrument at the target position; is the deviation between the desired speed and the current speed of the instrument; e is the deviation between the desired position and the current position of the instrument; Kp and Kd are the weight coefficients of the position change and the speed change of the PD control respectively.

[0013] The calculation methods of Kp and Kd are as follows:

[0014] Kp = ω 2 h

[0015] Kd = 2ωζh

[0016] where h is the inertia parameter of the end instrument, obtained according to the structural design of the instrument; ζ is the system damping coefficient; ω is the natural frequency, obtained by identifying different instrument motions.

[0017] The natural frequency obtained by identifying different instrument motions is specifically:

[0018] When the instrument moves to a position at a preset distance from the target position, the instrument is controlled to start decelerating to the target position at a preset speed, and the gravity torques of the instrument at the position at a preset distance from the target position and the target position are respectively obtained, and then substituted into the PD control model to solve for the natural frequency of the instrument.

[0019] Among the obtained numerical values of the natural frequency, the smaller value is selected as the natural frequency of the instrument.

[0020] The preset distance is within 5 mm.

[0021] An RFID device is provided on the instrument to read the parameter information of the instrument, and then the length difference of the instrument before and after replacement is obtained.

[0022] Advantageous effects: When the motor is enabled, the present invention simulates the appearance of an obstacle in front to prevent the instrument from continuing to move, and the stiffness of the obstacle is adjustable, so that the instrument can stop smoothly during the dragging process, and the continuous actions of movement and stop are more coherent and compliant. Moreover, the memory position can be adjusted according to different installed instruments to keep the position of the end of the instrument consistent. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 is a flowchart of the control method of the present invention;

[0024] Figure 2Schematic diagram of the target position of the end effector;

[0025] Figure 3 Comparison chart of the lengths of the end tools of different instruments;

[0026] Figure 4 Schematic diagram of the PD control model of the end effector;

[0027] Figure 5 Schematic diagram for data acquisition in the natural frequency identification process. Detailed implementation manners

[0028] The present invention will be further illustrated below in conjunction with the accompanying drawings and specific embodiments.

[0029] Figure 1 Flow chart of the control method of the present invention. As Figure 1 shown, during the surgical process, the doctor operates the master hand to control the movement of the end effector of the slave arm, and the control method of the robot end effector of the present invention is used to control the end effector when replacing the instrument, including the following steps:

[0030] (1) Record the position of the instrument before replacement, install the instrument after replacement, and determine the target position of the instrument after replacement according to the length difference between the instruments before and after replacement;

[0031] As Figure 2 、 Figure 3 shown, the position a of the instrument before replacement is obtained through the encoder. After replacing the instrument, the position is adjusted to b according to the length difference between the instruments before and after replacement, that is, the target position. Among them, the position deviation between a and b is the length difference between the instruments before and after replacement. Among them, an RFID device is provided on the instrument to read the parameter information of the instrument, and then the length difference between the instruments before and after replacement is obtained.

[0032] (2) When the instrument moves to a position at a preset distance from the target position, a PD control model with fixed gravity compensation is used to control the instrument;

[0033] In the embodiment of the present invention, the preset distance is within 5 mm, preferably 5 mm. When the instrument moves to a position 5 mm away from the target position, it means that it moves to the vicinity of the target position;

[0034] After replacing the instrument, the doctor drags the instrument to start moving. Before the instrument moves to the vicinity of the target position, the instrument is controlled by compensating for the gravity moment and friction moment according to the robot dynamics model;

[0035] When the instrument moves to the vicinity of its target position, the friction moment is relatively small compared to the gravity moment and can be ignored. Therefore, the PD control model of the present invention adds gravity compensation in the PD control, and the target torque τ is specifically as follows:

[0036]

[0037] Wherein, q is the current position of the instrument, which is obtained by collecting through an encoder; G(q) is the gravitational moment of the instrument at the position q; is the speed deviation, is the desired speed of the instrument, which is 0; is the current speed of the instrument; e is the position deviation, e = q d -q, q d is the desired position of the instrument; Kp and Kd are the weight coefficients of the position change amount and the speed change amount of the PD control respectively;

[0038] On the surface, the gravity compensation term G(q) changes with the current position of the instrument. However, in actual control, this control method is only adopted near the target position. Therefore, the gravity compensation term G(q) can be considered as a constant value. Thus, G(q) = G(q d ) = G; where G is the gravitational moment of the instrument at the target position;

[0039] The entire PD control model is as Figure 4 shown, that is:

[0040]

[0041] When the instrument reaches the target position, the position deviation e and the speed deviation are both 0. Then, the target moment τ at this time is the gravitational moment of the instrument at the target position. In this way, the instrument can maintain balance at the target position;

[0042] Among them, the calculation methods of Kp and Kd are as follows:

[0043] Kp = ω 2 h

[0044] Kd = 2ωζh

[0045] Among them, h is the inertia parameter of the end instrument, which is obtained according to the structural design of the instrument; ζ is the system damping coefficient. In this embodiment, ζ = 1, that is, the response is critically damped and can provide the fastest non-oscillatory response; ω is the natural frequency;

[0046] The present invention identifies the natural frequency ω of each instrument according to the motion of different instruments, thereby completing the construction of the PD control model, which is calculated by the following method:

[0047] Collect the position of the instrument in real time through the encoder. When the instrument moves to a position that is a preset distance x from the target position, control the instrument to start decelerating at a preset speed v until it reaches the target position, and the speed reduces to 0 after reaching the target position; calculate the torque value τ1 at the position where the instrument is x away from the target position and the gravitational torque G at the target position based on the current value of the motor.

[0048] As Figure 5 shown, set the moving speed of the instrument at the position x away from the target position as v, and the speed reduces to 0 after reaching the target position, that is, the position deviation is x and the speed deviation is -v. Generally, v takes the maximum speed of the instrument. Substitute the above formula to get:

[0049] xhω 2 -2hwv + G = τ1

[0050] ω can be obtained:

[0051]

[0052] Furthermore, through actual tests, it is obtained that in the case where the natural frequency ω is too large, due to some external forces, such as the friction between the instrument and the slide rail, the instrument will oscillate back and forth when it is dragged to the target position, so this value is discarded. Finally, the natural frequency ω is determined as:

[0053]

[0054] Construct the PD control model of the corresponding instrument according to the obtained natural frequency ω, and control the instrument accordingly.

[0055] The present invention realizes the position limiting function based on the motor torque loop, compensates for its gravitational torque and frictional torque before the instrument moves near the target position. When the instrument moves near the target position, it simulates the appearance of an obstacle in front in the motor enabled state to prevent the instrument from continuing to move, which can achieve a smooth stop during the dragging process of the instrument, improve the doctor's operation experience, and can adjust the target position according to different installed instruments to keep the position of the instrument end consistent.

[0056] The preferred embodiments of the present invention have been described in detail above, but the present invention is not limited to the specific details in the above embodiments. Within the technical concept scope of the present invention, various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical solutions of the present invention, and these equivalent transformations all belong to the protection scope of the present invention.

Claims

1. A control method for a robot end effector, characterized in that: Including: Determine the target position of the instrument after replacement according to the length difference of the instrument before and after replacement; Compensate for the gravity moment and friction moment of the instrument according to the robot dynamics model to control the movement of the instrument; when the instrument after replacement moves to a position at a preset distance from the target position, use a PD control model with fixed gravity compensation to control it; The PD control model with fixed gravity compensation is specifically: ； Among them, is the target torque; is the desired position of the instrument, , is the gravitational torque of the instrument at the target position; is the deviation between the desired speed and the current speed of the instrument; is the deviation between the desired position and the current position of the instrument, 、 are the weight coefficients of the position change and the speed change of the PD control model respectively; , , where is the inertial parameter of the instrument, obtained according to the structural design of the instrument; is the system damping coefficient; is the natural frequency, obtained by identifying the motion of different instruments; When the instrument moves to a position at a preset distance from the target position x the controller operates the instrument to start decelerating to the target position at a preset speed v and calculates the torque value at the position where the instrument is located based on the current value of the motor x to obtain: ​ ； It can be solved that: ； Take the smaller value to obtain: 。 2. The control method of the robotic end effector according to claim 1, characterized in that: The preset distance is within 5 mm.

3. The control method of the robot end effector according to claim 1, characterized in that: An RFID device is provided on the instrument to read the parameter information of the instrument, so as to obtain the length difference of the instrument before and after replacement.

Images