CN114770459A - Main control arm motion control method - Google Patents
Main control arm motion control method Download PDFInfo
- Publication number
- CN114770459A CN114770459A CN202210547901.7A CN202210547901A CN114770459A CN 114770459 A CN114770459 A CN 114770459A CN 202210547901 A CN202210547901 A CN 202210547901A CN 114770459 A CN114770459 A CN 114770459A
- Authority
- CN
- China
- Prior art keywords
- joint
- control arm
- speed
- cost function
- calculating
- 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.)
- Granted
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 53
- 238000000034 method Methods 0.000 title claims abstract description 40
- 239000011159 matrix material Substances 0.000 claims description 66
- 238000004364 calculation method Methods 0.000 claims description 20
- 238000013507 mapping Methods 0.000 claims description 18
- 230000005484 gravity Effects 0.000 claims description 9
- 238000002324 minimally invasive surgery Methods 0.000 abstract description 8
- 238000005457 optimization Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 9
- 238000002357 laparoscopic surgery Methods 0.000 description 5
- 210000000707 wrist Anatomy 0.000 description 5
- 238000012545 processing Methods 0.000 description 3
- 206010034719 Personality change Diseases 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 238000001356 surgical procedure Methods 0.000 description 2
- YCKRFDGAMUMZLT-UHFFFAOYSA-N Fluorine atom Chemical group [F] YCKRFDGAMUMZLT-UHFFFAOYSA-N 0.000 description 1
- 210000000683 abdominal cavity Anatomy 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 229910052731 fluorine Inorganic materials 0.000 description 1
- 239000011737 fluorine Substances 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
Images
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)
- Surgery (AREA)
- General Health & Medical Sciences (AREA)
- Life Sciences & Earth Sciences (AREA)
- Automation & Control Theory (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Mathematical Physics (AREA)
- Physics & Mathematics (AREA)
- Biomedical Technology (AREA)
- Heart & Thoracic Surgery (AREA)
- Medical Informatics (AREA)
- Molecular Biology (AREA)
- Animal Behavior & Ethology (AREA)
- Public Health (AREA)
- Veterinary Medicine (AREA)
- Manipulator (AREA)
Abstract
The embodiment of the application provides a method for controlling the motion of a main control arm, which comprises the following steps: a master control arm motion 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 variation of one or more joints of the main control arm; calculating to obtain 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 main control arm to move according to the third expected speed and the expected position of each joint. The problem that the main hand easily reaches the singular position and posture is solved, the operation difficulty of the main hand is reduced, and the safety and the operation efficiency of the laparoscopic minimally invasive surgery are improved.
Description
Technical Field
The application relates to the technical field of main control arm control, in particular to a method for controlling the motion of a main control arm.
Background
Minimally invasive surgery refers to surgery performed using endoscopes and related devices such as laparoscopes and thoracoscopes. Compared with the traditional open type operation, the minimally invasive surgery has the advantages of small wound, light pain, quick recovery and the like. With the development of robot technology, laparoscopic surgical robot systems are increasingly used in minimally invasive surgery.
The laparoscopic surgical 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 a connecting rod, and the slave hand is provided with an operating arm, an endoscope and a surgical instrument which are installed on the operating arm. The laparoscopic surgical robot system is a master-slave operating system, and an operator can control the surgical instruments and the laparoscope on a slave hand to move through a clamping end of a master hand. In the process of operating the main hand, when the plurality of connecting rods of the clamping end are close to be parallel, the main hand can reach a singular pose, and the operator is difficult to perform pitching operation. Usually, the master-slave connection between the master hand and the slave hand needs to be disconnected at this time, and the master hand is reset and then the master-slave connection is reestablished, however, the method can cause the operation to be suspended, the continuity of the operation is reduced, and the operation risk is increased.
Disclosure of Invention
In order to solve the technical problem that the control effect of a master hand is poor, the application provides a motion control method of a master control arm.
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 variation of one or more joints of the main control arm;
calculating to obtain 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 the desired position of each joint comprises:
calculating the gravity feedforward of each joint by an inverse dynamics method;
calculating a target moment according to the third expected speed, the expected position, the current speed, the current position and the gravity feedforward 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 serial number of each joint, Ci(q) represents the cost function of the joint i, qi(t) represents the current position of the joint i, qimaxRepresents the maximum joint limit of the joint i, qiminRepresents the joint minimum limit of the joint i.
In some embodiments, deriving a second desired velocity for 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 master control arm, and calculating a Jacobian matrix according to the current position;
calculating a null 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 attitude variation 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 null-space mapping matrix comprises:
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 null space mapping matrix and the step length.
In some embodiments, the step length is calculated in real time according to the operating speed of the operator, and the calculation formula includes:
wherein x1 is the step length, w is the terminal angular velocity corresponding to the master control arm, and w0For a predetermined angular velocity, x0The angular velocity of the tail end corresponding to the main control arm is w0Step size of time.
In some embodiments, calculating a third desired velocity for each joint from the first desired velocity and the second desired velocity comprises:
and performing weighted calculation on the first expected speed and the second expected speed to obtain a third expected speed of each joint, wherein the calculation formula comprises:
dqd＝k1*dq1+k2*dq2
wherein dq isdFor the third desired speed, k1 is a weight coefficient for the first desired speed, and k2 is a weight coefficient for the second desired speed.
In some embodiments, k1, k2 are both constants.
In some embodiments, k2 is a constant, k1 is calculated in real time according to the operation speed of the operator, and the calculation formula comprises: k1 ═ k0*(w/w0) Wherein w is the terminal angular velocity corresponding to the master control arm, w0To preset angular velocity, k0K1 at the end of the master control arm with an angular velocity of w0Get whenThe value is obtained.
In some embodiments, deriving a first desired velocity for each joint from the current velocity and the current position of each joint of the master control arm comprises:
acquiring the current position and the current speed of each joint of the master control arm, and calculating a Jacobian matrix and a pseudo-inverse matrix thereof according to the current position;
calculating the terminal angular velocity corresponding to each joint according to the current velocity and the Jacobian matrix of each joint;
and calculating to obtain a first expected speed of each joint according to the terminal angular speed of each joint and the Jacobian pseudo-inverse matrix.
The motion control method of the main control arm has the advantages that:
according to the method, 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 main control arm is controlled to move according to the first expected speed, so that the labor-saving effect can be achieved; the cost function is a function of the posture variation of the joints, so that a second expected speed obtained according to the negative gradient of the cost function and the current position of each joint is associated with the posture variation of the joint, the main control arm is controlled to move according to the second expected speed to achieve the effect of reducing the posture variation of the joint, a third expected speed is obtained according to the first expected speed and the second expected speed, the expected position is further obtained, the third expected speed and the expected position are used as input conditions for controlling the movement of the main control arm, the effects of saving labor and reducing the posture variation of the joint can be achieved for the operation of the main control arm, the main hand is prevented from achieving a singular posture, the working available space of the main hand is enlarged, and the safety and the operation efficiency of the minimally invasive laparoscopic surgery are improved.
Drawings
In order to more clearly describe the technical solution of the present application, the drawings required to be used in the embodiments will be briefly described below, and it is obvious for those skilled in the art to obtain other drawings without inventive labor.
Fig. 1 is a schematic diagram illustrating a system architecture of a laparoscopic surgical robot;
fig. 2 shows an exemplary singular pose diagram of a master hand;
FIG. 3 illustrates another singular pose diagram of a master hand;
FIG. 4 is a schematic diagram illustrating the principle of master control arm motion control;
FIG. 5 is a flow diagram illustrating a master control arm motion control method;
FIG. 6 is a flow chart diagram illustrating a method of calculating the desired speed of the primary task;
FIG. 7 is a flow chart illustrating a method for calculating the desired speed of the subtask;
fig. 8 is a schematic diagram illustrating an operation state of a master control arm.
Detailed Description
To make the purpose and embodiments of the present application clearer, the following will clearly and completely describe the exemplary embodiments of the present application with reference to the attached drawings in the exemplary embodiments of the present application, and it is obvious that the described exemplary embodiments are only a part of the embodiments of the present application, and not all of the embodiments.
It should be noted that the brief descriptions of the terms in the present application are only for the convenience of understanding the embodiments described below, and are not intended to limit the embodiments of the present application. These terms should be understood in their ordinary and customary meaning unless otherwise indicated.
The terms "first," "second," "third," and the like in the description and claims of this application and in the foregoing drawings are used for distinguishing between similar or analogous objects or entities and are not necessarily intended to limit the order or sequence in which they are presented unless otherwise indicated. It is to be understood that the terms so used are interchangeable under appropriate circumstances.
The terms "comprises" and "comprising," as well as any variations thereof, are intended to cover a non-exclusive inclusion, such that a product or device that comprises a list of elements is not necessarily limited to all of the elements explicitly listed, but may include other elements not expressly listed or inherent to such product or device.
The laparoscopic surgery robot is a new medical device for an operator to perform minimally invasive surgery, and at present, the system architecture of the laparoscopic surgery robot with wider application can be seen in fig. 1, and 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 a connecting rod, and an operator operates the clamping end to move in a space; the bedside mechanical arm system comprises an operating arm, a laparoscope and surgical instruments can be installed on the operating arm, and the master hand and the operating arm can be controlled to establish and break master-slave connection through pedals on an operating 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 is in communication connection with a display device.
In some embodiments, the surgical instrument may be mounted to the manipulator arm prior to performing minimally invasive laparoscopic surgery, and the operator may sit upright on the manipulator platform and place the hand on the gripping end of the main hand. In the process of carrying out the abdominal cavity minimally invasive surgery, an operator observes images shot by the laparoscope through a display device (such as a three-dimensional eyepiece), and controls surgical instruments above an operating table and the laparoscope to move by operating a master hand, so that the 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 determine the next operation according to the shot images in the surgical process.
In some embodiments, during minimally invasive laparoscopic surgery, when the multiple links of the main hand are close to parallel, the main hand can reach a singular pose and the operator can have difficulty performing pitching operations. Two exemplary singular poses can be seen in fig. 2 and 3.
Referring to fig. 2, the main hand is a schematic diagram of a main hand reaching a singular pose, and 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 connected in sequence, and a finger sleeve F is arranged on the clamping end 701. The joint J4, the joint J5, the joint J6, and the joint 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 roll axis.
As shown in fig. 2, the rotation axis of the joint J7 and the rotation axis of the joint J5 form an angle α, when α is 0 degrees, the end of the clamping end 701 faces the joint J5, 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 is the end of the clamping end 701 facing away from the joint J7.
When α is small, the roll axis of the joint J7 and the pitch axis of the joint J5 are approximately parallel, the hand of the operator bends inward along the wrist at a large angle, and if the operator wants to perform the pitch operation, the joint J5 needs to be controlled to rotate, but at this time, the wrist of the operator is in a posture in which the force is hard to be exerted, and the difficulty in controlling the joint J5 to rotate is high.
As shown in fig. 3, when α is large, the roll axis of the joint J7 and the pitch axis of the joint J5 are also in a substantially parallel state, and in this case, the hand of the operator bends outward at a large angle along the wrist, and if the operator wants to perform the pitch operation, the joint J5 needs to be controlled to rotate, but in this case, the wrist of the operator is also in a posture in which it is difficult to exert a force, and it is difficult to control the joint J5 to rotate.
In order to solve the technical problem of high operation difficulty of a master hand, the embodiment of the application provides a motion control method of a master control arm, the method controls the motion of each joint of the master hand by setting a master task and a slave task, the master task is used for providing assistance for the motion of each joint, an operator can drag each joint in a labor-saving manner, the slave task is used for controlling each joint to move, the redundant joint J4 assists other joints to move, the master hand is prevented from reaching a singular pose, and the operation difficulty of the master hand is reduced.
Referring to fig. 4, a schematic diagram of a motion control of a master control arm according to an embodiment of the present application is provided. As shown in FIG. 4, motion control for one joint may include joint control of a primary task and a secondary task.
In some embodiments, the inputs to the primary task may include the current position q and current velocity dq of each joint. And calculating a Jacobian matrix J according to the current position q, and calculating a pseudo-inverse matrix of the Jacobian matrix J to obtain a Jacobian pseudo-inverse matrix inv (J). And calculating the terminal angular speed omega corresponding to each joint according to the current speed dq and the Jacobian matrix J. According to the terminal angular velocity ω corresponding to the joint and the Jacobian pseudo-inverse matrix inv (J), the joint desired velocity dq1 of each joint corresponding to the main task can be calculated, wherein the joint desired velocity dq1 can be called a first desired velocity, and the first desired velocity is the main task desired velocity.
In some embodiments, the inputs to the subtasks may include a cost function C (q), a step size x1, and a current position q. Wherein the cost function c (q) represents the variation of the current posture of one or more joints and the zero position, and the step length x1 is a function which is inversely related to the terminal angular velocity corresponding to the primary hand, and is an optimized velocity unit of the secondary task and is used for representing the control velocity of the secondary task on the joint motion. The step size x1 can be defaulted to 1 and can be adjusted, the larger the step size the shorter the sub-task optimization process will be, i.e. the faster the joint will complete the motion.
Calculating a Jacobian matrix J according to the current position q of each joint, and calculating a pseudo-inverse matrix of the Jacobian matrix J to obtain a Jacobian pseudo-inverse matrix inv (J). The null-space mapping matrix n (q) is obtained from the jacobian matrix J and the jacobian pseudo-inverse matrix inv (J). According to the cost function C (q), the step length x1 and the Jacobian pseudo-inverse matrix inv (J), the joint desired speed dq2 of each joint corresponding to the subtask can be calculated, wherein the joint desired speed dq2 can be called a second desired speed, and the second desired speed is the subtask desired speed.
In some embodiments, the common control of the primary task and the secondary task means that the desired velocity dq1 of each joint is calculated from the desired velocity dq1 of the joint corresponding to the primary task and the desired velocity dq2 of the joint corresponding to the secondary taskdThe desired speed dqdMay be referred to as a third desired speed. Wherein, when calculating the third desired velocity, the joint desired velocity dq1 corresponding to the main task has a weightThe desired joint velocity dq2 for the subtask has a weight of a weight coefficient k1, which is a weight coefficient k 2. Obtaining the desired position q of each joint by integrating the third desired velocityd. The third desired speed dq of each jointdDesired position qdThe gravity feedforward G, the current position q and the current speed dq are input into a joint controller, the target joint torque of each joint can be calculated through the joint controller, and each joint is controlled to move according to the target joint torque, so that the motion control of the main control arm is realized.
According to the control principle, the main task can calculate a first expected speed according to the current position and the current speed of the joint, and the joint can be controlled by an operator in a labor-saving manner by controlling the movement of the joint according to the first expected speed; the secondary task takes the zero posture of the joint as an optimization target, and can control the motion amplitude of the joint within a smaller range, so that the main hand can be prevented from reaching a singular posture. Through the combined action of the main task and the secondary task, the joint can be controlled to achieve the effects of saving labor and avoiding singular poses.
To further describe the control method for controlling the motion of the master control arm provided in the embodiment of the present application, fig. 5 schematically illustrates a flowchart of the control method for controlling the motion 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 velocity and a current position of each joint may be obtained, and a first desired velocity may be calculated according to the current velocity and the current position, and the calculation method of the first desired velocity may be as shown in fig. 6, and includes 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 velocity dq and current position q of each joint of the master control arm may be read from the motor encoder. The current velocity dq may be the angular velocity at which the joint is currently moving and the current position q may be the angle at which the joint is currently rotating.
In some embodiments, the Jacobian matrix J may be derived from the current position of the joints of the master control arm. The calculation method of the jacobian matrix J can refer to the prior art and is not 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＝[z4 z5 z6 z7] (1)
(1) in the formula, z4Representing the vector coordinate, z, of the axis of rotation of the joint J4 in the base coordinate system5Representing the vector coordinate, z, of the axis of rotation of the joint J5 in the base coordinate system6Representing the vector coordinate, z, of the axis of rotation of the joint J6 in the base coordinate system7The vector coordinates of the axis of rotation of joint J7 in the base coordinate system are shown.
In some embodiments, after the Jacobian matrix J of the master control arm is obtained, a pseudo-inverse Jacobian matrix J, which may be referred to simply as the Jacobian pseudo-inverse, may be calculated.
Step S1012: and calculating the terminal angular velocity corresponding to each joint according to the current velocity and the Jacobian matrix of each joint.
In some embodiments, the tip angular velocity ω is calculated as follows:
ω＝dq*J (2)
step S1013: and calculating to obtain a first expected speed of each joint according to the terminal angular speed of each joint and the Jacobian pseudo-inverse matrix.
In some embodiments, the first desired velocity for each joint may be calculated from the jacobian pseudo-inverse matrix and the tip angular velocity for 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 variation of one or more joints of the main control arm.
In some embodiments, a 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 master 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 a motor encoder.
In some embodiments, the calculation of the first desired velocity and the second desired velocity may be performed in parallel, or the calculation of the first desired velocity may be performed first and then the calculation of the second desired velocity may be performed, or the calculation of the second desired velocity may be performed first and then the calculation of the first desired velocity may be performed.
In some embodiments, the formula for calculating the jacobian matrix can be referred to as formula (1), and is not described herein.
In some embodiments, after obtaining the Jacobian matrix J of the first master control arm, the pseudo-inverse matrix inv (J) of the Jacobian matrix J may be calculated.
Step S1022: and calculating a null space mapping matrix according to the Jacobian matrix and the pseudo-inverse matrix thereof.
In some embodiments, the null-space mapping matrix may be computed from the pseudo-inverse of the Jacobian matrix J. The calculation formula of the null space mapping matrix is as follows:
N(q)＝I-inv(J)*J (4)
(4) in the formula, n (q) represents a null 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 attitude variation 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, and a cost function may be set according to the posture change amount of one or more joints, and the cost function takes the posture change amount of the joints to be reduced as an optimization target, so that the master hand can be prevented from reaching the singular posture.
In some embodiments, the formula for calculating the cost function includes:
(5) in the formula, qi(t) represents the current position of the joint i, i is the joint number, Ci(q) represents the cost function of the joint i, qiThe difference value between (t) and the zero position of the joint i is the posture variation of the joint i, and q isimaxAnd q isiminRespectively representing the maximum limit and the minimum limit of a joint i, wherein the maximum limit of the joint is the maximum rotation angle of a rotation shaft of the joint, the minimum limit of the joint is the minimum rotation angle of the rotation shaft of the joint, and qimaxAnd q isiminThe difference in (b) may be referred to as the joint constraint. Different joints, qimaxMay not be the same, qiminThe difference may be different, q is different at different time of the minimally invasive surgery processi(t) may be different.
Equation (5) is a formula for calculating a cost function for a single joint, and if the cost function is provided for a plurality of joints, the cost function c (q) may be provided as a matrix of N × 1, 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 a singular pose, the joint needed to control the pose change amount is joint J6, and therefore, only the cost function for joint J6 may be calculated. At this time, C6(q) may represent a cost function of joint J6, and q (t) may range from 0 to 180. q. q of6maxMaximum limit for joint J6, q6maxCan be 180 degrees, q6minMinimum limit for joint J6, q6minMay be 0 degrees. The cost function of the master control arm is C (q), C (q) ═ 0,0, C6(q),0]。
In some embodiments, to reduce the value of the cost function, a negative gradient of the cost function may be calculated
The calculation formula is as follows:
in some embodiments, the second desired velocity dq2 corresponding to the cost function may be calculated according to the null-space mapping matrix and the negative gradient of the cost function, and at this time, the calculation formula of the second desired velocity dq2 is as follows:
in some embodiments, the second desired speed dq2 corresponding to the cost function can be calculated according to the negative gradient and the step size of the null-space mapping matrix and the cost function, and in this case, the calculation formula of the second desired speed dq2 is as follows:
(8) in the formula, x1 represents a step size corresponding to the subtask and is used for controlling the optimization speed of the subtask.
In some embodiments, the speed of subtask optimization may be controlled by adjusting the step size of the subtask. The adjustment formula is as follows:
(9) where x1 is the step size, w is the terminal angular velocity of the joint, i.e. the operator's operating speed, w0To preset angular velocity, w0The value of (b) can be adjusted according to actual needs. x is a radical of a fluorine atom0At the end of an angular velocity w of w0Step size of time. According to the formula (9), the online speed regulation of the step length of the secondary task can be realized. When the operation speed of the operator to the 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 the operator does not operateWhen 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, the operation safety is not influenced, and the main hand can be prevented from reaching a singular pose.
Step S103: and calculating to obtain 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 velocity dq may be calculated from the first and second desired velocities of each jointdThe calculation formula is as follows:
dqd＝k1*dq1+k2*dq2 (10)
(10) where k1 is a first weight coefficient indicating the first desired velocity, and k2 is a second weight coefficient indicating the second desired velocity.
In some embodiments, k1 and k2 may be constants determined according to the operation hand feeling of the operator, wherein k1 may be set as a first constant, k2 may be set as a second constant, and the first constant and the second constant may be different.
In some embodiments, k2 may be a constant and k1 may be a function of the operator-manipulated speed ω, as calculated by:
k1＝k0*(w/w0) (11)
(11) in the formula, k0When the angular velocity at the end is w0The value of k 1. According to equation (11), k1 can be adjusted online. When the operation speed of the operator on the joint of the main hand is increased, k1 is increased, so that the posture change of the main hand is matched with the operation of the operator as much as possible, the misjudgment of the motion of the main hand by the operator is avoided, and the operation safety is ensured. When the operation speed of the operator on the joint of the main hand is reduced, k1 is reduced, so that the posture change of the main hand is more flexible, the main hand is prevented from reaching singularity, and the operation continuity is guaranteed.
In some embodiments, the second desired velocity is calculated by equation (7), and the attitude change amount in the cost function can be limited within the threshold range by adjusting the first weight coefficient and the second weight coefficientThe posture variation of the operated joint is controlled to be less than a preset range, wherein the threshold range can be set according to the actual situation, for example, the threshold range can be in the range of-5 to +5 degrees. If the zero position of the joint J is an angle of 90 degrees and the attitude change amount is in the range of-5 to +5, as shown in fig. 8, if the zero position of the joint J6 is a rotation angle of the rotation shaft of the joint J6 of 90 degrees, the current position of the joint J6 needs to be controlled to be 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 jointd。
In some embodiments, the second desired velocity is calculated by formula (8) and formula (9), and the posture change amount in the cost function can be limited within a 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 posture change amount of the operated joint so that the posture change amount does not exceed a preset range.
Step S104: and controlling each joint of the main 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 an inverse dynamics method.
In some embodiments, after the third desired velocity, the desired position, the current velocity, the current position, and the gravity of each joint are fed forward and input to the joint controller, the joint controller may use a PD controller or a PI controller to calculate a target moment by using a servo algorithm, and the master hand controls each joint to move according to the target moment, where the target moment τ is calculated as follows:
τ＝Kp(qd-q)+Kd(dqd-dq)+G (12)
(12) where Kp is a weight coefficient of a position change amount, Kd is a difference between a desired position of the joint and a current position, Kd is a weight coefficient of a velocity change amount, and the velocity change amount is a difference between a third desired velocity of the joint and a current velocity, and both Kp and Kd may be set by a controller such as a PD controller (proportional-derivative controller) or a PI controller (proportional-integral controller).
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 joint J6, the calculated target joint moments for joint J5 and joint J7 may be 0, the target joint moment for joint J6 may be 0, and the target joint moment for joint J4 may not be 0, according to the method shown in fig. 5, so that joint J4 may move in the rotation direction of joint J6, which may be a clockwise rotation direction or a counterclockwise rotation direction.
According to the motion control method provided by the above embodiment, the motion of the joint of the master hand shown in fig. 2 or fig. 3 is controlled, and an exemplary control effect can be seen in fig. 8, which is a schematic view of an operating state of the master 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 a zero position. As shown in fig. 8, if the operator operates the joint J6 to attempt to control the joint J6 to rotate clockwise, the joint controller performs the main task by calculating a first desired velocity of the joint J6, e.g., +6, where + represents clockwise and counter-clockwise represents-based on the current velocity and current position of the joint J6, and the calculated first desired velocity of the joint J4 may be 0. If the movement of the joint J6 is directly controlled according to the expected speed, the movement of the joint J6 can enable a main hand to reach a singular pose, the method sets a cost function as a function corresponding to the posture variation of the joint J6 through the access of a secondary task, obtains a zero-space mapping matrix through calculating the pseudo-inverse of a Jacobian matrix, obtains the expected speed of each joint under the optimization target of the secondary task in the current movement state according to the negative gradient of the zero-space mapping matrix and the cost function and the step length of the secondary task, and because 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, and after the first expected speed and the second expected speed are integrated, the movement of the joint J4 not only realizes the movement target of the joint J6, but also realizes the optimization target of the secondary task, namely, the main hand finishes clockwise selection operation, the joint J6 can be kept in a zero-position posture, and the connecting rod of the joint J6 is vertical to the connecting rod of the joint J5, so that the main hand is prevented from reaching a singular posture.
As shown in fig. 8, when the joint J4 assists the joint J6 to move, the joint J6 maintains a zero-position posture, at this time, the wrist of the operator is perpendicular to 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 posture of the master hand does not need to be readjusted by disconnecting the master-slave connection, the continuity of the laparoscopic minimally invasive surgery is guaranteed, the surgical procedure is accelerated, and the pain of a patient is relieved.
In some embodiments, the optimization strength of the subtask on the joint motion may 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 may be as described above and is not described herein again. Illustratively, the threshold range may be-5 to 5 in degrees, where the angle of the threshold range is a kinematic angle, that is, the motion amplitude is 0 degree when the joint is at a zero position.
In some embodiments, the primary task is performed based on an operator's manipulation, and when the operator does not manipulate the primary hand, only inputs of the secondary task, gravity feed forward, current position, and current velocity are provided in the joint controller, and the attitude is set in advance in the cost function before the operator manipulates
And at the minimum position, the secondary task cannot be executed, and the controller only balances gravity at the moment to complete the function of zero-force control. When an operator operates, the primary task and the secondary task play roles simultaneously, and at the moment, the attitude joint of the joint controller can complete not only a dragging task but also an optimization task.
It can be seen from the above embodiments that, in the present application, assistance is provided for the movement of joints by setting the primary task, the posture variation of the joints is controlled by setting the secondary task, and the operation of the operator can correspond to the movement of redundant joints of the main control arm by executing the primary task and the secondary task together, and the secondary task can cancel the movement of at least one joint in the primary task, and the movement effect of the joint whose movement is canceled in the primary task is achieved by the movement of the redundant joints, thereby solving the technical problem that the primary hand can easily reach singular poses, and taking into account the labor saving effect, being beneficial to increasing the movement range of the joint controller, reducing the inertia of joint movement, alleviating the influence of friction force, improving the operation hand feeling of the joints, accelerating the operation flow, and alleviating the pain of patients.
Since the above embodiments are all described by referring to and combining with other embodiments, the same portions are provided between different embodiments, and the same and similar portions between the various embodiments in this specification may be referred to each other. 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 (10)
1. A master control arm motion 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 variation of one or more joints of the main control arm;
calculating to obtain 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.
2. The master control arm motion control method of claim 1, wherein controlling each joint of the master control arm to move according to a third desired velocity and a desired position of each joint comprises:
calculating the gravity feedforward of each joint by an inverse dynamics method;
calculating a target moment according to the third expected speed, the expected position, the current speed, the current position and the gravity feedforward of each joint;
and controlling each joint to move according to the target moment.
3. The master control arm motion control method of claim 1, wherein the cost function is calculated by:
wherein i is the joint number of each joint, Ci(q) represents the cost function of the joint i, qi(t) represents the current position of the joint i, qimaxRepresents the maximum joint limit of the joint i, qiminRepresenting the joint minimum limit of the joint i.
4. The master control arm motion control method of claim 1, wherein deriving a second desired velocity for 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 master control arm, and calculating a Jacobian matrix according to the current position;
calculating a null 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 attitude variation of one or more joints of the main control arm.
5. The method of claim 4, wherein constructing a cost function, calculating a negative gradient of the cost function, and obtaining a second desired velocity for each joint according to the negative gradient and a null-space mapping matrix comprises:
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.
6. The master control arm motion control method according to claim 5, wherein the step size is calculated in real time according to an operation speed of an operator, and a calculation formula includes:
wherein x1 is the step length, w is the terminal angular velocity corresponding to the master control arm, and w0For a predetermined angular velocity, x0The angular velocity of the tail end corresponding to the main control arm is w0Step size of time.
7. The master control arm motion control method of claim 1, wherein calculating a third desired velocity for each joint from the first desired velocity and the second desired velocity comprises:
and performing weighted calculation on the first expected speed and the second expected speed to obtain a third expected speed of each joint, wherein the calculation formula comprises:
dqd＝k1*dq1+k2*dq2
wherein dq isdFor the third desired speed, k1 is a weighting factor for the first desired speed and k2 is a weighting factor for the second desired speed.
8. The master control arm motion control method of claim 7, wherein k1, k2 are both constants.
9. The master control arm movement control method of claim 7, wherein k2 is a constant, k1 is calculated in real time according to the operation speed of the operator, and the calculation formula comprises: k1 ═ k0*(w/w0) Wherein w is the terminal angular velocity corresponding to the master control arm, w0To preset angular velocity, k0For k1 corresponding to the main control armHas an end angular velocity of w0The value of time.
10. The master control arm motion control method of claim 1, wherein obtaining the first desired velocity for each joint from the current velocity and the current position of each joint of the master control arm comprises:
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 terminal angular velocity corresponding to each joint according to the current velocity and the Jacobian matrix of each joint;
and calculating to obtain a first expected speed of each joint according to the terminal angular speed of each joint and the Jacobian pseudo-inverse matrix.
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 true CN114770459A (en) | 2022-07-22 |
| CN114770459B 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 (11)
| 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 |
| US20220009096A1 (en) * | 2020-07-10 | 2022-01-13 | Ubtech Robotics Corp Ltd | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same |
| 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 |
-
2022
- 2022-05-18 CN CN202210547901.7A patent/CN114770459B/en active Active
Patent Citations (11)
| 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 |
| US20220009096A1 (en) * | 2020-07-10 | 2022-01-13 | Ubtech Robotics Corp Ltd | Inverse kinematics solving method for redundant robot and redundant robot and computer readable storage medium using the same |
| 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 |
|---|---|
| CN114770459B (en) | 2023-12-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11173598B2 (en) | System and methods for managing multiple null-space objectives and SLI behaviors | |
| US10617480B2 (en) | Tele-operative surgical systems and methods of control at joint limits using inverse kinematics | |
| KR102252641B1 (en) | Command shaping to dampen vibrations in mode transitions | |
| US10765485B2 (en) | Medical support arm device and method of controlling medical support arm device | |
| US10188471B2 (en) | Tele-operative surgical systems and methods of control at joint limits using inverse kinematics | |
| JP6421171B2 (en) | System and method for following a path using zero space | |
| US9510911B2 (en) | System and methods for managing multiple null-space objectives and SLI behaviors | |
| KR102109594B1 (en) | Systems and methods for cancellation of joint motion using the null-space | |
| KR102146708B1 (en) | Systems and methods for avoiding collisions between manipulator arms using a null-space | |
| JP7251669B2 (en) | Control system, control method, and arm system | |
| US20190125462A1 (en) | Multi-input robotic surgical system control scheme | |
| JP2016512733A (en) | System and method using zero space to anisotropically enhance manipulator joint motion | |
| WO2017169649A1 (en) | Medical observation device, drive control method, medical observation system, and support arm device | |
| WO2016152046A1 (en) | Medical support arm device and method of controlling medical support arm device | |
| CN114770459B (en) | Main control arm motion control method | |
| CN118319502B (en) | Master-slave motion control method of surgical robot and related equipment | |
| CN117442347A (en) | Remote control method for underdrive mechanism | |
| CN118902357A (en) | Endoscope motion control method, system, electronic device and storage medium |
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 | ||
| 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. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |