CN114770459B - Main control arm motion control method - Google Patents
Main control arm motion control method Download PDFInfo
- Publication number
- CN114770459B CN114770459B CN202210547901.7A CN202210547901A CN114770459B CN 114770459 B CN114770459 B CN 114770459B CN 202210547901 A CN202210547901 A CN 202210547901A CN 114770459 B CN114770459 B CN 114770459B
- Authority
- CN
- China
- Prior art keywords
- joint
- speed
- control arm
- expected
- expected speed
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 55
- 238000000034 method Methods 0.000 title claims abstract description 36
- 239000011159 matrix material Substances 0.000 claims description 63
- 238000004364 calculation method Methods 0.000 claims description 25
- 238000013507 mapping Methods 0.000 claims description 18
- 230000005484 gravity Effects 0.000 claims description 9
- 208000028752 abnormal posture Diseases 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 abstract description 3
- 230000036544 posture Effects 0.000 description 17
- 230000000694 effects Effects 0.000 description 10
- 238000002324 minimally invasive surgery Methods 0.000 description 8
- 238000005457 optimization Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000002357 laparoscopic surgery Methods 0.000 description 5
- 210000000707 wrist Anatomy 0.000 description 5
- 238000012545 processing Methods 0.000 description 3
- 238000001356 surgical procedure Methods 0.000 description 3
- 238000012084 abdominal surgery Methods 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 210000004247 hand Anatomy 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 230000000284 resting effect Effects 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J3/00—Manipulators of master-slave type, i.e. both controlling unit and controlled unit perform corresponding spatial movements
- B25J3/04—Manipulators of master-slave type, i.e. both controlling unit and controlled unit perform corresponding spatial movements involving servo mechanisms
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
- A61B34/30—Surgical robots
- A61B34/37—Master-slave robots
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J3/00—Manipulators of master-slave type, i.e. both controlling unit and controlled unit perform corresponding spatial movements
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
- B25J9/04—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type by rotating at least one arm, excluding the head movement itself, e.g. cylindrical coordinate type or polar coordinate type
- B25J9/046—Revolute coordinate type
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1679—Programme controls characterised by the tasks executed
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Surgery (AREA)
- Life Sciences & Earth Sciences (AREA)
- Molecular Biology (AREA)
- Heart & Thoracic Surgery (AREA)
- Medical Informatics (AREA)
- Biomedical Technology (AREA)
- Animal Behavior & Ethology (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Automation & Control Theory (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Manipulator (AREA)
Abstract
The embodiment of the application provides a master control arm motion control method, which comprises the following steps: a master control arm movement control method, comprising: obtaining a first expected speed of each joint according to the current speed and the current position of each joint of the main control arm; obtaining a second expected speed of each joint according to a cost function and the current position of each joint of the main control arm, wherein the cost function is a function of the posture change quantity of one or more joints of the main control arm; calculating a third expected speed of each joint according to the first expected speed and the second expected speed, and obtaining an expected position of each joint according to the third expected speed of each joint; and controlling each joint of the master control arm to move according to the third expected speed and the expected position of each joint. The application solves the problem that the main hand is easy to reach the odd pose, reduces the operation difficulty of the main hand, and is beneficial to improving the safety and the operation efficiency of the laparoscopic minimally invasive operation.
Description
Technical Field
The application relates to the technical field of master control arm control, in particular to a master control arm motion control method.
Background
Minimally invasive surgery refers to surgery performed using endoscopes such as laparoscopes and thoracoscopes, and related devices. Compared with the traditional open surgery, the minimally invasive surgery has the advantages of small wound, light pain, quick recovery and the like. With the development of robotics, laparoscopic surgical robotic systems are becoming increasingly popular for use in minimally invasive surgery.
The laparoscopic surgery robot system comprises a master hand and a slave hand, wherein the master hand is provided with a master control arm and a clamping end connected to the master control arm, the master control arm comprises a plurality of joints and connecting rods, and the slave hand is provided with a manipulator arm and an endoscope and surgical instrument arranged on the manipulator arm. The laparoscopic surgery robot system is a master-slave type operation system, and an operator can control surgical instruments and a laparoscope on a slave hand to move through the clamping end of the master hand. In the process of operating the main hand, when a plurality of connecting rods of the clamping end are close to be parallel, the main hand can reach an odd-position posture, and an operator is difficult to pitch. In general, the master-slave connection between the master hand and the slave hand is disconnected, and the master hand is reset and then the master-slave connection is reestablished, however, the method can cause operation suspension, reduce operation continuity and increase operation risk.
Disclosure of Invention
The application provides a main control arm motion control method for solving the technical problem of poor main control effect.
The application provides a master control arm motion control method, which comprises the following steps:
obtaining a first expected speed of each joint according to the current speed and the current position of each joint of the main control arm;
obtaining a second expected speed of each joint according to a cost function and the current position of each joint of the main control arm, wherein the cost function is a function of the posture change quantity of one or more joints of the main control arm;
calculating a third expected speed of each joint according to the first expected speed and the second expected speed, and obtaining an expected position of each joint according to the third expected speed of each joint;
and controlling each joint of the master control arm to move according to the third expected speed and the expected position of each joint.
In some embodiments, controlling movement of each joint of the master control arm according to the third desired velocity and desired position of each joint comprises:
calculating the gravity feedforward of each joint by a reverse dynamics method;
calculating a target moment according to the third expected speed, the expected position, the current speed, the current position and the gravity feed-forward of each joint;
and controlling each joint to move according to the target moment.
In some embodiments, the cost function is calculated by:
wherein i is the joint number of each joint, C i (q) represents the cost function of joint i, q i (t) represents the current position of joint i, q imax Represents the maximum limit of the joint, q imin Representing the joint minimum limit of the joint i.
In some embodiments, deriving the second desired velocity of each joint from the cost function and the current position of each joint of the master control arm comprises:
acquiring the current position of each joint of the main control arm, and calculating a Jacobian matrix according to the current position;
calculating a zero space mapping matrix according to the Jacobian matrix and the pseudo-inverse matrix thereof;
and constructing a cost function, calculating to obtain a negative gradient of the cost function, and obtaining a second expected speed of each joint according to the negative gradient and a zero space mapping matrix, wherein the cost function is a function of the posture change quantity of one or more joints of the main control arm.
In some embodiments, constructing a cost function, calculating a negative gradient of the cost function, and obtaining a second desired velocity for each joint from the negative gradient and a zero-space mapping matrix, comprising:
and constructing a cost function, calculating to obtain a negative gradient of the cost function, and obtaining a second expected speed of each joint according to the negative gradient, the zero space mapping matrix and the step length.
In some embodiments, the step size is calculated in real time according to an operation speed of an operator, and the calculation formula includes:wherein x1 is the step length, w is the end angular velocity corresponding to the main control arm, and w 0 At a preset angular velocity x 0 The corresponding end angular velocity of the main control arm is w 0 Step size at time.
In some embodiments, calculating a third desired velocity for each joint from the first desired velocity and the second desired velocity comprises:
and carrying out weighted calculation on the first expected speed and the second expected speed to obtain a third expected speed of each joint, wherein a calculation formula comprises:
dq d ＝k1*dq1+k2*dq2
wherein dq is d For the third desired speed, k1 is a weight coefficient of the first desired speed, and k2 is a weight coefficient of the second desired speed.
In some embodiments, k1, k2 are both constants.
In some embodiments, k2 is a constant, and k1 is calculated in real time according to an operation speed of an operator, and the calculation formula includes: k1 =k 0 *(w/w 0 ) Wherein w is the end angular velocity corresponding to the main control arm, and w 0 For a preset angular velocity k 0 At the end of the main control arm, the angular velocity corresponding to k1 is w 0 And (5) taking the value.
In some embodiments, deriving the first desired velocity for each joint from the current velocity and the current position of each joint of the master control arm includes:
acquiring the current position and the current speed of each joint of the main control arm, and calculating a Jacobian matrix and a pseudo-inverse matrix thereof according to the current position;
calculating the corresponding tail end angular velocity of each joint according to the current velocity of each joint and the Jacobian matrix;
and calculating the first expected speed of each joint according to the tail end angular speed of each joint and the Jacobian pseudo-inverse matrix.
The main control arm motion control method provided by the application has the beneficial effects that:
according to the application, the first expected speed of each joint is calculated according to the current speed and the current position of each joint, so that the first expected speed can reflect the movement trend of the main control arm, and the movement of the main control arm is controlled according to the first expected speed, so that the labor-saving effect can be achieved; because the cost function is a function of the gesture variable quantity of the joint, the second expected speed obtained according to the negative gradient of the cost function and the current position of each joint is related to the gesture variable quantity of the joint, the control of the movement of the master control arm according to the second expected speed can achieve the effect of reducing the gesture variable quantity of the joint, the third expected speed is obtained according to the first expected speed and the second expected speed, the expected position is further obtained, and the third expected speed and the expected position are used as input conditions for controlling the movement of the master control arm, so that the effect of saving labor and reducing the gesture variable quantity of the joint can be achieved for the operation of the master control arm, the master control arm is prevented from reaching the odd pose, the working available space of the master arm is enlarged, and the safety and the operation efficiency of the laparoscopic minimally invasive surgery are improved.
Drawings
In order to more clearly illustrate the technical solution of the present application, the drawings that are needed in the embodiments will be briefly described below, and it will be obvious to those skilled in the art that other drawings can be obtained from these drawings without inventive effort.
A schematic system architecture of a laparoscopic surgical robot is schematically shown in fig. 1;
an odd pose schematic of the master hand is shown schematically in fig. 2;
another singular pose schematic of the master hand is exemplarily shown in fig. 3;
a schematic diagram of the principle of master control arm motion control is schematically shown in fig. 4;
a schematic flow chart of a master control arm motion control method is exemplarily shown in fig. 5;
FIG. 6 is a flow chart illustrating a method of calculating a desired speed of a primary task;
FIG. 7 is a flow chart illustrating a method of calculating a secondary task desired speed;
a schematic diagram of the operation of the master control arm is shown schematically in fig. 8.
Detailed Description
For the purposes of making the objects and embodiments of the present application more apparent, an exemplary embodiment of the present application will be described in detail below with reference to the accompanying drawings in which exemplary embodiments of the present application are illustrated, it being apparent that the exemplary embodiments described are only some, but not all, of the embodiments of the present application.
It should be noted that the brief description of the terminology in the present application is for the purpose of facilitating understanding of the embodiments described below only and is not intended to limit the embodiments of the present application. Unless otherwise indicated, these terms should be construed in their ordinary and customary meaning.
The terms first, second, third and the like in the description and in the claims and in the above-described figures are used for distinguishing between similar or similar objects or entities and not necessarily for describing a particular sequential or chronological order, unless otherwise indicated. It is to be understood that the terms so used are interchangeable under appropriate circumstances.
The terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a product or apparatus that comprises a list of elements is not necessarily limited to all elements explicitly listed, but may include other elements not expressly listed or inherent to such product or apparatus.
The laparoscopic surgery robot is a novel medical device for an operator to perform minimally invasive surgery, and at present, a system architecture of the laparoscopic surgery robot with wider application can be seen in fig. 1, and the laparoscopic surgery robot comprises an operation platform, a bedside mechanical arm system and an imaging system. The operation platform comprises a display device, a main hand and a pedal, wherein the main hand comprises a main control arm and a clamping end connected to the main control arm, the main control arm comprises a plurality of joints and connecting rods, and an operator operates the clamping end to enable the clamping end to move in a space; the bedside mechanical arm system comprises a manipulator arm, wherein a laparoscope and surgical instruments can be installed on the manipulator arm, and the master hand and the manipulator arm can be controlled to be connected with and disconnected from each other through pedals on an operation platform; the imaging system comprises an image processing device and a laparoscope, wherein the image processing device is in communication connection with the laparoscope and the display device.
In some embodiments, the surgical instrument may be mounted to the manipulator arm prior to performing the minimally invasive abdominal surgery, and the operator may sit on the manipulator platform with the hands resting on the gripping ends of the master hand. In the process of performing the laparoscopic minimally invasive surgery, an operator observes images shot by the laparoscope through a display device (such as a stereoscopic eyepiece), and controls surgical instruments and the laparoscope above the operating table to move through an operation master hand, so that surgical contents are completed, and the image processing equipment can transmit the images shot by the laparoscope to the display device in real time, so that the operator can conveniently determine the next operation according to the shot images in the surgical process.
In some embodiments, during minimally invasive abdominal surgery, when the plurality of links of the master hand are nearly parallel, the master hand may reach an odd ectopic pose, which is difficult for the operator to perform a pitching operation. Two exemplary singular poses can be seen in fig. 2 and 3.
Referring to fig. 2, a schematic diagram of a main hand reaching an odd-abnormal pose is shown, the main hand includes a joint J4, a connecting rod 401, a joint J5, a connecting rod 501, a joint J6, a connecting rod 601, a joint J7 and a clamping end 701, which are sequentially connected, and a fingerstall F is arranged on the clamping end 701. The joints J4, J5, J6, and J7 are provided with rotation axes, respectively, the rotation axis of the joint J5 may be referred to as a pitch axis, the rotation axis of the joint J6 may be referred to as a yaw axis, and the rotation axis of the joint J7 may be referred to as a yaw axis.
As shown in fig. 2, the angle between the rotation axis of the joint J7 and the rotation axis of the joint J5 is α, when α is 0 degrees, the end of the clamping end 701 faces the joint J5, and when α is 180 degrees, the end of the clamping end 701 faces away from the joint J5, wherein the end of the clamping end 701 refers to the end of the clamping end 701 facing away from the joint J7.
When α is small, the transverse roller of the joint J7 and the pitch axis of the joint J5 are approximately parallel, at this time, the inward bending angle of the hand of the operator along the wrist is large, if the operator wants to perform the pitch operation, the operator needs to control the joint J5 to rotate, however, at this time, the wrist of the operator is in a posture where it is difficult to apply force, and the difficulty of controlling the joint J5 to rotate is high.
As shown in fig. 3, when α is large, the lateral roller of the joint J7 and the pitch axis of the joint J5 are also approximately parallel, and at this time, the hand of the operator is bent outward along the wrist to a large angle, and if the operator wants to perform the pitch operation, the operator needs to control the joint J5 to rotate, however, at this time, the wrist of the operator is also in a position where it is difficult to apply force, and the difficulty in controlling the joint J5 to rotate is high.
In order to solve the technical problem of high operation difficulty of a main hand, the embodiment of the application provides a motion control method of a main control arm, which controls the motion of each joint of the main hand by setting a main task and a secondary task, utilizes the main task to provide assistance for the motion of each joint, enables an operator to drag each joint in a labor-saving manner, utilizes the secondary task to control each joint to move, enables a redundant joint J4 to assist other joints to move, prevents the main hand from reaching an odd and abnormal pose, and reduces the operation difficulty of the main hand.
Referring to fig. 4, a schematic diagram of a master control arm motion control according to an embodiment of the present application is provided. As shown in fig. 4, motion control for one joint may include common control of primary and secondary tasks.
In some embodiments, the input of the primary task may include the current position q and the current speed dq of each joint. The Jacobian matrix J can be calculated according to the current position q, and the Jacobian pseudo-inverse matrix inv (J) can be obtained by calculating the pseudo-inverse matrix of the Jacobian matrix J. The end angular velocity omega corresponding to each joint can be calculated through the current velocity dq and the Jacobian matrix J. According to the tail end angular velocity omega and the Jacobian pseudo inverse matrix inv (J) corresponding to the joints, the joint expected speed dq1 of each joint corresponding to the main task can be calculated, wherein the joint expected speed dq1 can be called as a first expected speed, and the first expected speed is the main task expected speed.
In some embodiments, the inputs to the subtask may include a cost function C (q), a step size x1, and a current location q. The cost function C (q) represents the variation of the current posture and zero position of one or more joints, the step length x1 is a function which is inversely related to the end angular velocity corresponding to the main hand, and is an optimized speed unit of the secondary task and used for representing the control speed of the secondary task on joint motion. The step size x1 can default to 1 and can be adjusted, and the larger the step size, the shorter the process of optimizing the subtask, i.e. the faster the joint will complete the motion.
The Jacobian matrix J can be calculated according to the current position q of each joint, and the Jacobian pseudo-inverse matrix inv (J) can be obtained by calculating the pseudo-inverse matrix of the Jacobian matrix J. The zero-space mapping matrix N (q) can be obtained according to the Jacobian matrix J and the Jacobian pseudo-inverse matrix inv (J). The joint expected speed dq2 of each joint corresponding to the secondary task can be calculated according to the cost function C (q), the step length x1 and the Jacobian pseudo-inverse matrix inv (J), wherein the joint expected speed dq2 can be called as a second expected speed, and the second expected speed is the secondary task expected speed.
In some embodiments, the common control of the primary task and the secondary task refers to calculating the desired speed dq of each joint based on the desired joint speed dq1 corresponding to the primary task and the desired joint speed dq2 corresponding to the secondary task d The desired speed dq d May be referred to as a third desired speed. When the third expected speed is calculated, the joint expected speed dq1 corresponding to the primary task has a weight, the weight is a weight coefficient k1, and the joint expected speed dq2 corresponding to the secondary task has a weight, the weight is a weight coefficient k2. Obtaining the desired position q of each joint by integrating the third desired velocity d . The third expected speed dq of each joint d Desired position q d The gravity feedforward G, the current position q and the current speed dq are input into a joint controller, the joint controller can calculate the target joint moment of each joint, and each joint is controlled to move according to the target joint moment, so that the movement control of the main control arm is realized.
According to the control principle, the main task can calculate the first expected speed according to the current position and the current speed of the joint, and the movement of the joint is controlled according to the first expected speed, so that an operator can control the joint more labor-saving; the secondary task takes the zero position posture of the joint as an optimization target, and can control the movement amplitude of the joint to be in a smaller range, so that the master hand can be prevented from reaching the odd-abnormal posture. The joint control can achieve the effects of saving labor and avoiding odd ectopic postures through the combined action of the main task and the secondary task.
For further description of the control method of the movement of the master control arm provided by the embodiment of the present application, fig. 5 schematically shows a flow chart of a control method of the movement of the master control arm, and as shown in fig. 5, the control method may include the following steps:
step S101: and obtaining a first expected speed of each joint according to the current speed and the current position of each joint of the main control arm.
In some embodiments, for each joint of the master control arm, a current speed and a current position of each joint may be obtained, and a first desired speed is calculated according to the current speed and the current position, respectively, and a method for calculating the first desired speed may refer to fig. 6, including the following steps:
step S1011: and acquiring the current position and the current speed of each joint of the main control arm, and calculating a Jacobian matrix and a pseudo-inverse matrix thereof according to the current position.
In some embodiments, the current speed dq and current position q of each joint of the master control arm can be read from the motor encoder. The current speed dq may be the angular speed of the current motion of the joint, and the current position q may be the angle of the current rotation of the joint.
In some embodiments, the jacobian matrix J can be derived from the current position of each joint of the master control arm. The calculation method of the jacobian matrix J can refer to the prior art and will not be described in detail here.
In some embodiments, for the master control arm shown in FIG. 2, the Jacobian matrix J is calculated as follows:
J＝[z 4 z 5 z 6 z 7 ] (1)
(1) Wherein z is 4 Representing the vector coordinates, z, of the axis of rotation of joint J4 in the base coordinate system 5 Representing the vector coordinates, z, of the axis of rotation of joint J5 in the base coordinate system 6 Indicating that the axis of rotation of joint J6 is inVector coordinates, z in the base coordinate system 7 The vector coordinates of the axis of rotation of joint J7 in the base coordinate system are shown.
In some embodiments, after obtaining the jacobian matrix J of the master control arm, a pseudo-inverse of the jacobian matrix J, which may be referred to simply as the jacobian pseudo-inverse, may be calculated.
Step S1012: and calculating the corresponding tail end angular velocity of each joint according to the current velocity of each joint and the Jacobian matrix.
In some embodiments, the end angular velocity ω is calculated as follows:
ω＝dq*J (2)
step S1013: and calculating the first expected speed of each joint according to the tail end angular speed of each joint and the Jacobian pseudo-inverse matrix.
In some embodiments, the first desired velocity of each joint may be calculated from the jacobian pseudo-inverse and the terminal angular velocity of each joint as follows:
dq1＝ω*inv(J) (3)
step S102: and obtaining a second expected speed of each joint according to a cost function and the current position of each joint of the main control arm, wherein the cost function is a function of the posture change quantity of one or more joints of the main control arm.
In some embodiments, the method of the second desired speed may be seen in fig. 7, including the steps of:
step S1021: and acquiring the current position of each joint of the main control arm, and calculating a Jacobian matrix according to the current position.
In some embodiments, the current position q of the joint may be read from the motor encoder.
In some embodiments, the calculation of the first desired speed and the second desired speed may be performed in parallel, or the calculation of the first desired speed may be performed first, then the calculation of the second desired speed may be performed, or the calculation of the second desired speed may be performed first, then the calculation of the first desired speed may be performed.
In some embodiments, the calculation formula of the jacobian matrix can be referred to as formula (1), and will not be described herein.
In some embodiments, after the Jacobian matrix J of the first master control arm is obtained, a pseudo-inverse of the Jacobian matrix J inv (J) may be calculated.
Step S1022: and calculating a zero space mapping matrix according to the Jacobian matrix and the pseudo-inverse matrix thereof.
In some embodiments, the zero-space mapping matrix may be calculated from the pseudo-inverse of the jacobian matrix J. The calculation formula of the zero space mapping matrix is as follows:
N(q)＝I-inv(J)*J (4)
(4) Where N (q) represents a zero-space mapping matrix and I represents an identity matrix.
Step S1023: and constructing a cost function, calculating to obtain a negative gradient of the cost function, and obtaining a second expected speed of each joint according to the negative gradient and a zero space mapping matrix, wherein the cost function is a function of the posture change quantity of one or more joints of the main control arm.
In some embodiments, the master control arm includes a plurality of joints, such as joints J4, J5, J6, and J7 in FIG. 2, a cost function may be set according to the pose change amount of one or more of the joints, the cost function may reduce the pose change amount of the joint as an optimization target, and the master hand may be prevented from reaching the odd pose.
In some embodiments, the calculation formula for the cost function includes:
(5) Wherein q is i (t) represents the current position of the joint i, i being the joint number, C i (q) represents the cost function of joint i, q i The difference between (t) and the zero position of the joint i is the posture change quantity of the joint i, q imax And q imin Respectively representing the maximum limit and the minimum limit of the joint i, wherein the joint maximum limit is the maximum rotation angle of the rotation shaft of the joint, the joint minimum limit is the minimum rotation angle of the rotation shaft of the joint, and q imax And q imin The difference in (2) may be referred to as the amount of articulation limit. Different joints, q imax May be different, q imin The scores may also be different, q at different times during the minimally invasive procedure i (t) may also be different.
It should be noted that, if the equation (5) is a calculation formula of a cost function of a single joint, and the cost function is set according to a plurality of joints, a matrix with a cost function C (q) of n×1 may be set, where N represents the number of joints of the master hand, and each column of elements corresponds to one joint.
In some embodiments, for the master hand shown in fig. 2 and 3, to avoid the master hand reaching an odd pose, the joint that is needed to control the amount of pose change is joint J6, and thus, only the cost function of joint J6 may be calculated. At this time, C 6 (q) may represent a cost function of joint J6, and q (t) may range from 0 to 180. q 6max To limit the maximum of the joint J6, q 6max Can be 180 degrees, q 6min Is the minimum limit of the joint J6, q 6min May be 0 degrees. The cost function of the master control arm is C (q), C (q) = [0, C 6 (q),0]。
In some embodiments, to reduce the value of the cost function, a negative gradient of the cost function may be calculatedThe calculation formula is as follows:
in some embodiments, the second desired speed dq2 corresponding to the cost function may be calculated from the zero space mapping matrix and the negative gradient of the cost function, where the second desired speed dq2 is calculated as follows:
in some embodiments, the second expected speed dq2 corresponding to the cost function may also be calculated according to the negative gradient and the step size of the zero space mapping matrix and the cost function, and the calculation formula of the second expected speed dq2 is as follows:
(8) Wherein x1 represents a step length corresponding to the subtask and is used for controlling the optimizing speed of the subtask.
In some embodiments, the speed of subtask optimization may be controlled by adjusting the step size of the subtasks. The adjustment formula is as follows:
(9) Wherein x1 is the step size, w is the terminal angular velocity of the joint, i.e. the operating speed of the operator, w 0 Is a preset angular velocity, w 0 The value of (2) can be adjusted according to actual needs. X is x 0 For an end angular velocity w of w 0 Step size at time. According to the formula (9), the online speed regulation of the step length of the subtask can be realized. When the operation speed of an operator to a main hand is high, the optimization speed of the secondary task can be adjusted to be relatively low by setting a small step length so as to ensure the operation safety, and when the operator does not operate or the operation speed is low, the optimization speed of the secondary task can be adjusted to be relatively high by setting a large step length so as not to influence the operation safety, and the method is also beneficial to avoiding the main hand from reaching an odd pose.
Step S103: and calculating a third expected speed of each joint according to the first expected speed and the second expected speed, and obtaining an expected position of each joint according to the third expected speed of each joint.
In some embodiments, a third desired speed dq may be calculated from the first and second desired speeds of each joint d The calculation formula is as follows:
dq d ＝k1*dq1+k2*dq2 (10)
(10) Where k1 is a first weight coefficient, and k2 is a second weight coefficient, and represents a first desired speed.
In some embodiments, k1 and k2 may be constants determined according to the operator's operating feel, where k1 may be set to a first constant and k2 may be a second constant, which may be different.
In some embodiments, k2 may be a constant and k1 may be a function of operator operating speed ω, calculated as follows:
k1＝k 0 *(w/w 0 ) (11)
(11) Wherein k is 0 To when the end angular velocity w is w 0 And k1 is a value. According to formula (11), k1 may be adjusted online. When the operation speed of the joint of the main hand by the operator is increased, k1 is increased, so that the posture change of the main hand is attached to the operation of the operator as much as possible, the misjudgment of the movement of the main hand by the operator is avoided, and the operation safety is ensured. When the operation speed of the operator to the joints of the main hand becomes smaller, k1 is reduced, so that the posture change of the main hand is more flexible, the main hand is prevented from reaching singular points, and the operation continuity is ensured.
In some embodiments, the second desired speed is calculated by equation (7), and the gesture change amount in the cost function can be limited to a threshold range by adjusting the first weight coefficient and the second weight coefficient, so as to achieve the effect of controlling the gesture change amount of the operated joint to make the gesture change amount not exceed a preset range, where the threshold range can be set according to practical situations, for example, the threshold range can be in a range of-5 to +5 degrees. If the zero position of the joint J is 90 degrees and the posture change amount is in the range of-5 to +5 degrees, as shown in fig. 8, if the zero position of the joint J6 is 90 degrees, the current position of the joint J6 needs to be controlled in the range of 85 to 95 degrees. In some embodiments, the third desired velocity of each joint is integrated to obtain the desired position q of each joint d 。
In some embodiments, the second desired speed is calculated by the formula (8) and the formula (9), and the gesture change amount in the cost function can be limited within the threshold range by adjusting the first weight coefficient, the second weight coefficient and the step length, so as to achieve the effect of controlling the gesture change amount of the operated joint, and enabling the gesture change amount not to exceed the preset range.
Step S104: and controlling each joint of the master control arm to move according to the third expected speed and the expected position of each joint.
In some embodiments, the mass center position and the mass parameter of each joint connecting rod of the main control arm can be obtained in real time, and the gravity feed-forward G of each joint of the main hand is calculated through a reverse dynamics method.
In some embodiments, after the third desired speed, the desired position, the current speed, the current position, and the gravity feed-forward of each joint are input to the joint controller, the joint controller may calculate the target moment by using the PD controller or the PI controller and the like and using a servo algorithm, and the master hand controls each joint to move according to the target moment, where the calculation formula of the target moment τ is as follows:
τ＝Kp(q d -q)+Kd(dq d -dq)+G (12)
(12) Wherein Kp is a weight coefficient of a position change amount, the position change amount is a difference value between a desired position and a current position of the joint, kd is a weight coefficient of a speed change amount, the speed change amount is a difference value between a third desired speed and the current speed of the joint, and Kp and Kd can be set in a PD controller (proportional-derivative controller) or a PI controller (proportional-integral controller) or other controllers.
In some embodiments, for the master hand shown in fig. 2, when the operator operates the gripping end of the master hand in the rotation direction of the joint J6, the calculated target joint moments of the joint J5 and the joint J7 may be 0, the target joint moment of the joint J6 may be 0, and the target joint moment of the joint J4 may be other than 0, so that the joint J4 may move in the rotation direction of the joint J6, which may be the clockwise rotation direction or the counterclockwise rotation direction, according to the method shown in fig. 5.
According to the motion control method provided in the above embodiment, the motion of the joint of the main hand shown in fig. 2 or fig. 3 is controlled, and an exemplary control effect can be seen from fig. 8, which is a schematic diagram of the working state of the main hand, in fig. 8, the rotation angle of the joint J6 is 90 degrees, and the corresponding kinematic angle is 0 degrees, that is, the joint J6 is in the zero position posture. As shown in fig. 8, if the operator manipulates the joint J6 to attempt to control the joint J6 to rotate clockwise, at this time, the joint controller performs a main task to calculate a first desired speed of the joint J6 according to the current speed and the current position of the joint J6, for example, the desired speed is +6, wherein +represents clockwise and counterclockwise, and the calculated first desired speed of the joint J4 may be 0. If the joint J6 is directly controlled to move according to the expected speed, the movement of the joint J6 can enable the master hand to achieve an odd pose, a cost function is set to be a function corresponding to the pose change quantity of the joint J6 through the access of a secondary task, a zero space mapping matrix is obtained through calculation of the pseudo-inverse of the Jacobian matrix, the expected speed of each joint under the optimal target of the secondary task in the current movement state is obtained according to the negative gradient of the zero space mapping matrix and the cost function and the step length of the secondary task, and as the joint J6 is provided with a redundant joint, namely the joint J4, the second expected speed of the joint J6 can be calculated to be-6, the second expected speed of the joint J4 can be +6, after the first expected speed and the second expected speed are integrated, the movement of the joint J4 is achieved, the motion target of the joint J6 is achieved, the optimal target of the secondary task is achieved, namely the master hand completes the clockwise selection operation, the joint J6 can be kept in the pose, and the connecting rod of the joint J6 and the joint J5 can be kept perpendicular, so that the master hand achieves the odd pose.
As shown in FIG. 8, when the joint J4 assists the joint J6 to move, the joint J6 keeps a zero position posture, at this time, the wrist of the operator is in a vertical state with the rotation axis of the joint J5, the operation space of the operator is enlarged, the joint J5 can be conveniently operated to rotate, the master hand posture is not required to be readjusted by disconnecting the master-slave connection, the continuity of the laparoscopic minimally invasive surgery is ensured, the surgical procedure is facilitated to be quickened, and the pain of the patient is relieved.
In some embodiments, the optimization degree of the subtask on the joint motion can be adjusted by adjusting the first weight coefficient and the step length, so that the motion amplitude of the joint is within the threshold range, and the adjustment manner of the first weight coefficient and the step length can be seen from the above description and is not repeated here. For example, the threshold range may be-5 to 5 in degrees, wherein the angle of the threshold range is a kinematic angle, i.e., the amplitude of motion is 0 degrees when the joint is in the zero position.
In some embodiments, the primary task is performed based on operator manipulation, and when the operator does not manipulate the primary hand, only secondary tasks, gravity feed forward, current position and current speed inputs are entered into the joint controller, with the pose set in advance to the cost function prior to operator manipulationAnd at the minimum position, the subtask cannot be executed, and the controller only balances the gravity at the moment, so that the zero-force control function is completed. When an operator operates, the main task and the secondary task simultaneously play roles, and at the moment, the gesture joint of the joint controller can finish not only a dragging task but also an optimizing task.
According to the embodiment, the primary task is arranged to provide assistance for the movement of the joints, the secondary task is arranged to control the posture change quantity of the joints, the primary task and the secondary task are jointly executed, so that the operation of an operator can correspond to the movement of the redundant joints of the main control arm, the secondary task can offset the movement of at least one joint in the primary task, the movement effect of the offset-moved joint in the primary task is achieved through the movement of the redundant joints, the technical problem that the master hand can easily reach odd and abnormal postures is solved, the labor-saving effect is achieved, the movement range of the joint controller is favorably enlarged, the inertia of the joint movement is reduced, the influence of friction force is reduced, the operation hand feeling of the joints is improved, the operation flow is accelerated, and the pain of a patient is relieved.
Since the foregoing embodiments are all described in other modes by reference to the above, the same parts are provided between different embodiments, and the same and similar parts are provided between the embodiments in the present specification. And will not be described in detail herein.
The above embodiments of the present application do not limit the scope of the present application.
Claims (7)
1. A master control arm movement control method, comprising:
obtaining a first expected speed of each joint according to the current speed and the current position of each joint of the main control arm, wherein the first expected speed is a main task expected speed, the main task is used for providing assistance for the movement of each joint, and the calculation method of the first expected speed comprises the following steps: acquiring the current position and the current speed of each joint of the main control arm, and calculating a Jacobian matrix and a pseudo-inverse matrix thereof according to the current position; calculating the corresponding tail end angular velocity of each joint according to the current velocity of each joint and the Jacobian matrix; according to the end angular velocity of each joint and the Jacobi pseudo-inverse matrix, calculating to obtain a first expected velocity of each joint;
obtaining a second expected speed of each joint according to a cost function and the current position of each joint of the main control arm, wherein the cost function is a function of the posture change quantity of one or more joints of the main control arm, the second expected speed is a secondary task expected speed, the secondary task is used for assisting the movement of each joint and avoiding reaching an odd-abnormal posture, and a calculation formula of the cost function comprises:q i (t) represents the current position of the joint i, i being the joint number, C i (q) represents the cost function of joint i, q imax And q imin The calculation method of the second expected speed comprises the following steps of: calculating a zero space mapping matrix according to the Jacobian matrix and the pseudo-inverse matrix thereof; obtaining the second desired speed according to the zero-space mapping matrix and the negative gradient of the cost function;
a third expected speed of each joint is calculated according to the first expected speed and the second expected speed, and the expected position of each joint is obtained according to the third expected speed of each joint, wherein the third expected speed is the expected speed for jointly controlling the movement of each joint through the main task and the secondary task, and the calculating method of the third expected speed comprises the following steps: weighting the first expected speed and the second expected speed to obtain the third expected speed;
and controlling each joint of the master control arm to move according to the third expected speed and the expected position of each joint.
2. The master control arm movement control method according to claim 1, wherein controlling each joint of the master control arm to move according to the third desired speed and the desired position of each joint, comprises:
calculating the gravity feedforward of each joint by a reverse dynamics method;
calculating a target moment according to the third expected speed, the expected position, the current speed, the current position and the gravity feed-forward of each joint;
and controlling each joint to move according to the target moment.
3. The method according to claim 1, wherein the obtaining the second desired velocity from the negative gradient of the zero-space mapping matrix and the cost function includes: and obtaining a second expected speed of each joint according to the negative gradient, the zero space mapping matrix and the step length, wherein the step length represents the control speed of the subtask on the joint movement.
4. The master control arm movement control method according to claim 3, wherein the step size is calculated in real time according to an operation speed of an operator, and the calculation formula includes:wherein x1 is the step length, w is the end angular velocity corresponding to the main control arm, and w 0 At a preset angular velocity x 0 The corresponding end angular velocity of the main control arm is w 0 Step size at time.
5. The master control arm motion control method according to claim 1, wherein the calculation formula of the third desired speed includes:
dq d ＝k1*dq1+k2*dq2
wherein dq is d For the third desired speed, dq1 is the first desired speed, dq2 is the second desired speed, k1 is a weight coefficient of the first desired speed, and k2 is a weight coefficient of the second desired speed.
6. The master control arm motion control method according to claim 5, wherein k1 and k2 are each a constant.
7. The master control arm movement control method according to claim 5, wherein k2 is a constant, and k1 is calculated in real time according to an operation speed of an operator, and the calculation formula includes: k1 =k 0 *(w/w 0 ) Wherein w is the end angular velocity corresponding to the main control arm, and w 0 For a preset angular velocity k 0 At the end of the main control arm, the angular velocity corresponding to k1 is w 0 And (5) taking the value.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210547901.7A CN114770459B (en) | 2022-05-18 | 2022-05-18 | Main control arm motion control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210547901.7A CN114770459B (en) | 2022-05-18 | 2022-05-18 | Main control arm motion control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114770459A CN114770459A (en) | 2022-07-22 |
| CN114770459B true CN114770459B (en) | 2023-12-08 |
Family
ID=82408359
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210547901.7A Active CN114770459B (en) | 2022-05-18 | 2022-05-18 | Main control arm motion control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114770459B (en) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018224038A1 (en) * | 2017-06-09 | 2018-12-13 | 微创（上海）医疗机器人有限公司 | Medical robot and control method thereof |
| CN109159151A (en) * | 2018-10-23 | 2019-01-08 | 北京无线电测量研究所 | A kind of mechanical arm space tracking tracking dynamic compensation method and system |
| CN110893118A (en) * | 2018-09-12 | 2020-03-20 | 微创（上海）医疗机器人有限公司 | Surgical robot system and method for controlling movement of robot arm |
| WO2020116792A1 (en) * | 2018-12-05 | 2020-06-11 | 주식회사 미래컴퍼니 | Method and system for remotely controlling surgical slave arm |
| CN113305843A (en) * | 2021-05-28 | 2021-08-27 | 深圳亿嘉和科技研发有限公司 | Zero-force control method for mechanical arm |
| WO2021230136A1 (en) * | 2020-05-12 | 2021-11-18 | ファナック株式会社 | Robot control device |
| WO2022007358A1 (en) * | 2020-07-08 | 2022-01-13 | 深圳市优必选科技股份有限公司 | Impedance control method and apparatus, impedance controller, and robot |
| CN113977602A (en) * | 2021-10-27 | 2022-01-28 | 华南理工大学 | Force feedback tail end holder admittance control method |
| CN114102603A (en) * | 2021-12-13 | 2022-03-01 | 南京佗道医疗科技有限公司 | Zero-force dragging method based on Cartesian space |
| CN114454180A (en) * | 2022-03-22 | 2022-05-10 | 深圳市优必选科技股份有限公司 | Motion control method and device of mobile robot and mobile robot |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111538949B (en) * | 2020-07-10 | 2020-10-16 | 深圳市优必选科技股份有限公司 | Redundant robot inverse kinematics solving method and device and redundant robot |
-
2022
- 2022-05-18 CN CN202210547901.7A patent/CN114770459B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018224038A1 (en) * | 2017-06-09 | 2018-12-13 | 微创（上海）医疗机器人有限公司 | Medical robot and control method thereof |
| CN110893118A (en) * | 2018-09-12 | 2020-03-20 | 微创（上海）医疗机器人有限公司 | Surgical robot system and method for controlling movement of robot arm |
| CN109159151A (en) * | 2018-10-23 | 2019-01-08 | 北京无线电测量研究所 | A kind of mechanical arm space tracking tracking dynamic compensation method and system |
| WO2020116792A1 (en) * | 2018-12-05 | 2020-06-11 | 주식회사 미래컴퍼니 | Method and system for remotely controlling surgical slave arm |
| WO2021230136A1 (en) * | 2020-05-12 | 2021-11-18 | ファナック株式会社 | Robot control device |
| WO2022007358A1 (en) * | 2020-07-08 | 2022-01-13 | 深圳市优必选科技股份有限公司 | Impedance control method and apparatus, impedance controller, and robot |
| CN113305843A (en) * | 2021-05-28 | 2021-08-27 | 深圳亿嘉和科技研发有限公司 | Zero-force control method for mechanical arm |
| CN113977602A (en) * | 2021-10-27 | 2022-01-28 | 华南理工大学 | Force feedback tail end holder admittance control method |
| CN114102603A (en) * | 2021-12-13 | 2022-03-01 | 南京佗道医疗科技有限公司 | Zero-force dragging method based on Cartesian space |
| CN114454180A (en) * | 2022-03-22 | 2022-05-10 | 深圳市优必选科技股份有限公司 | Motion control method and device of mobile robot and mobile robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114770459A (en) | 2022-07-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102252641B1 (en) | Command shaping to dampen vibrations in mode transitions | |
| JP5612971B2 (en) | Master-slave manipulator | |
| US11529202B2 (en) | Systems and methods for controlling a camera position in a surgical robotic system | |
| EP3119319B1 (en) | Tele-operative surgical systems and methods of control at joint limits using inverse kinematics | |
| EP1973021B1 (en) | Master-slave manipulator system | |
| US10188471B2 (en) | Tele-operative surgical systems and methods of control at joint limits using inverse kinematics | |
| US10765485B2 (en) | Medical support arm device and method of controlling medical support arm device | |
| Carignan et al. | Development of an exoskeleton haptic interface for virtual task training | |
| JP6421171B2 (en) | System and method for following a path using zero space | |
| EP2617530B1 (en) | Master operation input device and master-slave manipulator | |
| KR102146708B1 (en) | Systems and methods for avoiding collisions between manipulator arms using a null-space | |
| JP2020000904A (en) | System and methods for managing multiple null-space objectives and sli behaviors | |
| US20190125462A1 (en) | Multi-input robotic surgical system control scheme | |
| Lee et al. | Modeling and control of robotic surgical platform for single-port access surgery | |
| WO2017169649A1 (en) | Medical observation device, drive control method, medical observation system, and support arm device | |
| JP2024516939A (en) | Apparatus, computer-implemented method and computer program product | |
| Ren et al. | A master-slave control system with workspaces isomerism for teleoperation of a snake robot | |
| CN114770459B (en) | Main control arm motion control method | |
| US20230131269A1 (en) | Continuum robot control system and control method, and program | |
| Ding et al. | Choice of handedness and automated suturing for anthropomorphic dual-arm surgical robots | |
| Kapoor et al. | Telemanipulation of snake-like robots for minimally invasive surgery of the upper airway | |
| JP7153335B2 (en) | Surgery support device | |
| CN117442347A (en) | Remote control method for underdrive mechanism | |
| JP4839036B2 (en) | Medical control device | |
| Wang | Design, modeling and control of a minimally invasive surgical platform |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB02 | Change of applicant information |
Address after: 210012 building 3, No. 34, Dazhou Road, Yuhuatai District, Nanjing, Jiangsu Province Applicant after: Tuodao Medical Technology Co.,Ltd. Address before: 210012 building 3, No. 34, Dazhou Road, Yuhuatai District, Nanjing, Jiangsu Province Applicant before: Nanjing Tuodao Medical Technology Co.,Ltd. |
|
| CB02 | Change of applicant information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |