A teleoperation zero-force control method

Abstract

The present invention discloses a zero-force control method for teleoperation, which includes: acquiring the real-time pose of the end of the master hand and mapping it to the end of the slave arm, calculating the expected positions of the joints of the slave arm, and controlling the movement of the joints of the slave arm; acquiring the limit range corresponding to each joint of the current instrument for the slave arm, acquiring the positions of the joints of the slave arm in real time, judging whether they are within the limit range, and performing impedance control on the joints not within the limit range, calculating the expected torques of the joints of the master hand and performing control. During the operation process of the present invention, the doctor can not only feel the collision force of the mechanical limit of the instrument, but also the master hand will not have unexpected movements by the doctor, the surgical process will not be interrupted, the operation time is shortened, and at the same time, the operation experience of the doctor is greatly improved, and the immersion of teleoperation is stronger.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and in particular to a zero-force control method for teleoperation. Background Art

[0002] At present, 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 small trauma, less pain, and quick recovery. With the development of robot technology, minimally invasive surgery based on laparoscopic surgical robots has gradually matured and been widely applied.

[0003] The laparoscopic surgical robot has a master-slave teleoperation structure, including a master hand and a slave arm. The doctor controls the movement of the end effector of the slave arm and the endoscope by operating the master hand. The basic process of master-slave operation is as follows: after the poses of the end effector of the master hand and the end effector of the slave arm are matched, the doctor holds the end effector of the master hand to perform the operation, and the controller maps the pose of the end effector of the master hand to the end effector of the slave arm in real time, and the end effector reproduces the pose of the master hand in real time.

[0004] During the surgical operation, the doctor will continuously grasp the end effector of the master hand and change the pose of the end of the master hand. Since the movement range of the doctor's hand is large, the movement range of the master hand is usually set large to make the doctor's operation comfortable. However, due to the mechanical limit of the end effector of the slave arm, its movement space is small. Therefore, when the doctor operates the master hand to make the end effector of the slave arm exceed its movement space, the end effector will stop moving, resulting in the interruption of the operation, greatly prolonging the operation time, and at the same time making the doctor's operation experience worse and the immersion of teleoperation worse. Summary of the Invention

[0005] Object of the Invention: Based on this need, it is necessary to invent a zero-force control method for teleoperation that conforms to the human operation intuition, does not interrupt the surgical process, shortens the operation time, and has a stronger immersion in teleoperation.

[0006] Technical Solution:

[0007] A zero-force control method for teleoperation includes:

[0008] Obtaining the real-time pose of the end of the master hand and mapping it to the end of the slave arm, calculating the expected positions of the joints of the slave arm, and controlling the movement of the joints of the slave arm;

[0009] Obtaining the limit range of each joint of the slave arm corresponding to the current instrument, obtaining the positions of the joints of the slave arm in real time, judging whether they are within the limit range, and performing impedance control on the joints not within the limit range, calculating the expected torques of the joints of the master hand and performing control.

[0010] The impedance control is specifically:

[0011] Construct an impedance controller to obtain the impedance force τ of the corresponding joint as follows:

[0012] τ = K p *(q m - q t ) + K d *(0 - v t )

[0013] where K p , K d are the stiffness gain matrix and damping matrix of the impedance controller respectively; v t is the current speed of the joint, obtained by reading from the motor encoder of the joint; q m is the desired position of the joint, and q t is the current position of the joint.

[0014] K p = mw 2 , K d = 2kmw; where m is the inertia of the master hand, w is the natural frequency, and k is the damping ratio.

[0015] The acquisition of the desired position of the joint is specifically as follows:

[0016] If the joint position obtained in real time is greater than the maximum value of its working limit range, set its desired position to this maximum value;

[0017] If the joint position obtained in real time is less than the minimum value of its working limit range, set its desired position to this minimum value.

[0018] The calculation of the desired torque of each joint of the master hand is specifically as follows:

[0019] Calculate the Jacobian matrix of the corresponding joint according to the positions of each joint of the slave arm, and combine the impedance force in the impedance control to calculate the Cartesian space torque of each joint of the slave arm, and map it to the master hand;

[0020] Calculate the Jacobian matrix of the corresponding joint according to the current positions of each joint of the master hand, and combine the gravitational torque of each joint of the master hand calculated by the inverse dynamics of the robot to calculate the desired torque of each joint of the master hand.

[0021] The acquisition of the real-time pose of the end of the master hand and mapping it to the end of the slave arm is specifically as follows: Calculate the pose of the current end of the master hand relative to its base in real time through the forward kinematics of the robot, and map it to the end of the slave arm to calculate the desired pose of the end of the slave arm relative to its base.

[0022] Establish coordinate systems with the same direction at the base of the master hand and the base of the slave arm respectively, and obtain the mapping relationship between the end of the master hand and the end of the slave arm accordingly.

[0023] The specific method for obtaining the limit ranges of the joints of the slave arm corresponding to the current instrument is as follows:

[0024] According to the type of the instrument at the end of the slave arm, obtain the maximum and minimum values of the joints of the slave arm when the current instrument moves to its limit position, and use them as the limit range for the operation of the current instrument.

[0025] Design a buffer value e, and calculate the maximum limit Q max = q max - e and the minimum limit Q min = q min + e, to obtain the limit range for the operation of the current instrument.

[0026] The value range of the buffer value e is (0, 10°].

[0027] The e = 5°.

[0028] An RFID device is provided on the instrument to read the parameter information of the instrument, and the parameter information includes the type of the current instrument and its limit position.

[0029] Beneficial effects: By simulating a virtual collision environment to calculate the corresponding virtual collision force, and converting the virtual collision force to be sent by the master hand end motor, the doctor can not only feel the collision force of the mechanical limit of the instrument, but also the master hand will not move in an unexpected way for the doctor, the surgical process will not be interrupted, the surgical time is shortened, and at the same time, the operation experience of the doctor is greatly improved, and the immersion of teleoperation is stronger. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 is the control flow chart of the present invention;

[0031] Figure 2 is a schematic diagram of the coordinate systems corresponding to the master hand and the slave arm;

[0032] Figure 3 is a schematic diagram of the motion relationship between the slave arm and the master hand.

[0033] Among them, 1 is the master hand, and 2 is the slave arm. DETAILED DESCRIPTION OF THE INVENTION

[0034] The following further clarifies the present invention in conjunction with the drawings and specific embodiments.

[0035] Figure 1 is the flow chart of the present invention, Figure 1 as shown, the following steps:

[0036] (1) Determine the mapping relationship between the end of the master hand and the end of the slave arm.

[0037] Specifically, kinematic modeling is performed on the robotic arms of the master hand and the slave arm respectively according to the DH coordinate method to obtain the poses of their respective ends relative to their respective bases; in the present invention, base coordinate systems with the same direction are established on the bases of the master hand and the slave arm respectively, and thus the mapping relationship between the end of the master hand and the end of the slave arm can be obtained.

[0038] (2) Calculate the desired positions of the joints of the slave arm according to the real-time pose of the end of the master hand, and perform control accordingly.

[0039] When the doctor operates the end of the master hand, the controller calculates in real time the pose of the current end of the master hand relative to its base through forward kinematics of the robot, and maps it to the end of the slave arm, and calculates the desired pose of the end of the slave arm relative to its base. Among them, the position information and the attitude information adopt the methods of incremental mapping and absolute mapping respectively; the desired positions of the joints of the slave arm are obtained through inverse kinematics and sent to its servo motors to achieve the teleoperation function of the slave arm continuously following the doctor's operation.

[0040] (3) Determine the limit range of the joints of the slave arm corresponding to the current instrument.

[0041] According to the type of the end instrument of the slave arm, obtain the maximum and minimum values of the joints of the slave arm when the current instrument moves to its limit position, and record them as q max and q min respectively; among them, an RFID device is provided on the instrument to read the parameter information of the instrument, and the parameter information includes the type of the current instrument and its limit position; the present invention designs a buffer value e, and accordingly obtains the maximum limit Q max and the minimum limit Q min of the movement of the joints of the slave arm, and the calculation is as follows:

[0042] Q max = q max - e

[0043] Q min = q min + e

[0044] Among them, the value range of the buffer value e is (0, 10°], and the present invention preferably selects e = 5°.

[0045] (4) Real-time obtain the positions of the joints of the slave arm through the motor encoder, and judge whether they are within the limit range determined in step (3). If all joints are within the limit range, continue to control the movement of the joints of the slave arm; otherwise, perform impedance control on the joints that are not within the limit range to simulate the collision force of touching the virtual wall of the instrument;

[0046] Specifically:

[0047] If the real-time obtained joint position is greater than Q max, then set its desired position as Q max ;

[0048] If the joint position obtained in real time is less than Q min , then set its desired position as Q min ;

[0049] In the zero-force dragging of the present invention, when the joint reaches the desired position, its speed is exactly 0. Then, the impedance force τ (i.e., the joint torque) of the joint in the constructed impedance controller is:

[0050] τ = K p *(q m - q t ) + K d *(0 - v t )

[0051] Wherein, K p , K d are respectively the stiffness gain matrix and the damping matrix of the impedance controller; v t is the current speed of the joint, obtained by reading from the motor encoder; q m is the desired position of the joint, and q t is the current position of the joint.

[0052] Since the impedance force is to simulate the collision force of the virtual wall of the instrument, and the position of the virtual wall is the maximum limit and the minimum limit of the joint obtained in step (3), the impedance force is finally executed by the motor of the master hand, and the pose of the master hand will be mapped to the slave arm. Therefore, the impedance force should meet the self-stabilization condition as follows:

[0053] K p = mw 2

[0054] K d = 2kmw

[0055] Wherein, m is the inertia of the master hand; w is the natural frequency, obtained by parameter adjustment; k is the damping ratio, and the value in the present invention is 1.

[0056] (5) Calculate the Cartesian space torque of each joint of the slave arm relative to its base.

[0057] According to the positions of each joint of the slave arm obtained in step (4), calculate the Jacobian matrix J1, and combine the impedance force τ of the joint to calculate the Cartesian space torque τ1 = J1 -T *τ, as Figure 3 shown, J1 -T is the reverse transpose matrix of the Jacobian matrix J1.

[0058] (6) Calculate the expected torque of each joint of the master hand relative to its base, and perform control accordingly.

[0059] Since the directions of the base coordinate systems of the master hand and the slave arm are the same, map the Cartesian space torque τ1 of each joint of the slave arm relative to its base obtained in step (5) to the master hand. Calculate the gravitational torque G of each joint of the master hand through the inverse dynamics of the robot, and calculate the corresponding Jacobian matrix J2 and its transpose matrix J2 based on the current positions of each joint of the master hand. T Thus, calculate the expected torque τ of each joint of the master hand. d The calculation is as follows:

[0060] τ d = J2 T *τ1 + G

[0061] The present invention adds a force feedback technology for the instrument to the ordinary teleoperation technology, enabling the doctor to sense the mechanical limit of the instrument in real time; calculates the gravity of the master hand as a compensation torque through the inverse dynamics method to make the master hand in a zero-force state; establishes an impedance model at the mechanical limit of the end of the slave arm, calculates the corresponding virtual collision force by simulating a virtual collision environment, and converts the virtual collision force to the master hand side motor for transmission. At this time, the poses of the master hand and the instrument are in one-to-one correspondence, and the calculation coefficient of the virtual collision force satisfies the self-stabilization condition. Therefore, at this time, the doctor can not only feel the collision force of the mechanical limit of the instrument, but also the master hand will not move in an unexpected way for the doctor. The present invention does not interrupt the surgical process, shortens the surgical time, and at the same time greatly improves the doctor's operation experience, and the immersion of the teleoperation is stronger.

[0062] 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 scope of the technical concept 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 teleoperation zero-force control method, characterized in that, Including: Obtain the real-time pose of the end of the master hand and map it to the end of the slave arm, calculate the expected positions of the joints of the slave arm, and control the movements of the joints of the slave arm. According to the type of the current instrument at the end of the slave arm, obtain the maximum value q of each joint of the slave arm when the current instrument moves to its limit position max and the minimum value q min . Design a buffer value e, and calculate the maximum value Q of the movement of each joint of the slave arm max = q max - e and the minimum value Q min = q min + e, obtain the limit range of each joint of the slave arm corresponding to the current instrument, obtain the positions of each joint of the slave arm in real time, judge whether they are within the limit range, and perform impedance control on the joints not within the limit range, calculate the expected torque of each joint of the master hand and perform control; The impedance control specifically is: Construct an impedance controller, and obtain the impedance force τ of the corresponding joint as: τ = K p *(q m - q t ) + K d *(0 - v t ) Among them, K p and K d are the stiffness gain matrix and damping matrix of the impedance controller respectively; v t is the current velocity of this joint, obtained by reading from the motor encoder of this joint; q m is the desired position of this joint, and q t is the current position of this joint; Among them, the obtaining of the expected position of the joint specifically is: If the joint position obtained in real time is greater than the maximum value of its limit range, set its expected position to this maximum value. If the joint position obtained in real time is less than the minimum value of its limit range, set its expected position to this minimum value. The calculation of the expected torque of each joint of the master hand specifically is: Calculate the Jacobian matrix of the corresponding joint according to the positions of the joints of the slave arm, combine the impedance force in the impedance control to calculate the Cartesian space torque of the joints of the slave arm, and map it to the master hand. Calculate the Jacobian matrix of the corresponding joint according to the current positions of the joints of the master hand, combine the gravity torque of the joints of the master hand obtained by robot inverse dynamics, and calculate the expected torque of the joints of the master hand.

2. The teleoperation zero-force control method according to claim 1, characterized in that, K p = mw 2 , K d = 2kmw; where m is the inertia of the dominant hand, w is the natural frequency, and k is the damping ratio.

3. The teleoperation zero-force control method according to claim 1, wherein The obtaining of the real-time pose of the end of the master hand and mapping it to the end of the slave arm specifically is: Calculate the pose of the current end of the master hand relative to the base of the master hand in real time through robot forward kinematics, map it to the end of the slave arm, and calculate the expected pose of the end of the slave arm relative to the base of the slave arm.

4. The teleoperation zero-force control method according to claim 3, characterized in that Establish coordinate systems with the same direction at the base of the master hand and the base of the slave arm respectively, and obtain the mapping relationship between the end of the master hand and the end of the slave arm accordingly.

5. The teleoperation zero-force control method according to claim 1, characterized in that The value range of the buffer value e is (0, 10°].

6. The teleoperation zero-force control method according to claim 5, characterized in that, The e = 5°.

7. The teleoperation zero-force control method according to claim 1, characterized in that An RFID device is provided on the instrument for reading the parameter information of the instrument, and the parameter information includes the type of the current instrument and its limit position.

Images