CN111430022A - Puncture algorithm of puncture surgical robot - Google Patents
Puncture algorithm of puncture surgical robot Download PDFInfo
- Publication number
- CN111430022A CN111430022A CN202010201350.XA CN202010201350A CN111430022A CN 111430022 A CN111430022 A CN 111430022A CN 202010201350 A CN202010201350 A CN 202010201350A CN 111430022 A CN111430022 A CN 111430022A
- Authority
- CN
- China
- Prior art keywords
- puncture
- algorithm
- stage
- joint
- coordinate system
- 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.)
- Pending
Links
- 238000004364 calculation method Methods 0.000 claims abstract description 26
- 238000000034 method Methods 0.000 claims abstract description 24
- 239000012636 effector Substances 0.000 claims abstract description 11
- 230000009466 transformation Effects 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 8
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 238000001356 surgical procedure Methods 0.000 claims description 4
- 238000002679 ablation Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G16—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR SPECIFIC APPLICATION FIELDS
- G16H—HEALTHCARE INFORMATICS, i.e. INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR THE HANDLING OR PROCESSING OF MEDICAL OR HEALTHCARE DATA
- G16H40/00—ICT specially adapted for the management or administration of healthcare resources or facilities; ICT specially adapted for the management or operation of medical equipment or devices
- G16H40/60—ICT specially adapted for the management or administration of healthcare resources or facilities; ICT specially adapted for the management or operation of medical equipment or devices for the operation of medical equipment or devices
- G16H40/63—ICT specially adapted for the management or administration of healthcare resources or facilities; ICT specially adapted for the management or operation of medical equipment or devices for the operation of medical equipment or devices for local operation
-
- A—HUMAN NECESSITIES
- A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
- A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
- A61B17/00—Surgical instruments, devices or methods, e.g. tourniquets
- A61B17/34—Trocars; Puncturing needles
-
- 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
Landscapes
- Health & Medical Sciences (AREA)
- Engineering & Computer Science (AREA)
- Biomedical Technology (AREA)
- Surgery (AREA)
- Life Sciences & Earth Sciences (AREA)
- Public Health (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Molecular Biology (AREA)
- Animal Behavior & Ethology (AREA)
- Heart & Thoracic Surgery (AREA)
- Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
- Veterinary Medicine (AREA)
- Epidemiology (AREA)
- Pathology (AREA)
- Primary Health Care (AREA)
- General Business, Economics & Management (AREA)
- Business, Economics & Management (AREA)
- Robotics (AREA)
- Manipulator (AREA)
Abstract
The invention relates to a puncture algorithm of a puncture surgical robot, which comprises the following steps: the algorithm comprises 2 stages: the first stage is a positive kinematics analysis and calculation stage, and the position and the posture of the end effector relative to a fixed coordinate system are obtained by using the stage through joint angles; the second stage is an inverse kinematics analysis and calculation stage, and the position of each joint in the joint space is solved by using the stage according to the known position and posture of the end effector relative to the fixed coordinate system. The invention obtains the working space of the robot by a numerical method, randomly samples each joint, obtains the space position of the tail end according to positive kinematics, combines the space positions to form a rough working space range, and performs compensation movement on the tail end by the rear end moving platform on the premise of meeting the requirement of rotating around a far end motion center, and the algorithm is simple and accurate.
Description
Technical Field
The invention relates to the field of puncture surgical robots, in particular to a puncture algorithm of a puncture surgical robot.
Background
The kinematics researches the motion characteristics of the robot and does not consider the force generated by the motion of the robot. In order to conveniently represent the complex geometric shape of the robot, a connecting rod coordinate system needs to be arranged on each connecting rod of the robot, and then the relation PI among the connecting rod coordinate systems needs to be researched. While controlling the motion of each joint, the robot end effector can move to a designated position and posture in a working space (Cartesian space) and complete a series of working tasks. However, since the joint space of the robot control and the cartesian space of the end effector are not uniform, we need to establish a connection in these two space coordinates by kinematic analysis. Therefore, a puncture surgical robot puncture algorithm which is simple in calculation and simplified in solving mode is urgently needed.
Disclosure of Invention
The invention aims to provide a puncture surgical robot puncture algorithm which is simple in calculation and simplified in solving mode.
In order to solve the problems, the invention provides a puncture algorithm of a puncture surgical robot, which comprises 2 stages: the first stage is a positive kinematics analysis and calculation stage, and the position and the posture of the end effector relative to a fixed coordinate system are obtained by using the stage through joint angles; the second stage is an inverse kinematics analysis and calculation stage, and the position of each joint in the joint space is solved by using the stage according to the known position and posture of the end effector relative to the fixed coordinate system.
The method has the advantages that modeling is carried out by adopting a positive kinematics analysis and calculation stage method, the method is different from a common D-H parameter method in the sequence of rotation and translation during space transformation, the method is more visual and simpler, and in an inverse kinematics analysis and calculation stage, the solution process can be greatly simplified and calculation can be carried out more quickly and accurately due to the existence of three joints forming a Cartesian system.
Further, the positive kinematics analysis and calculation stage comprises a D-H parameter method for motion modeling, and a transformation matrix of the terminal needle point coordinate system relative to the inertial coordinate system is obtained through the motion modeling method.
Furthermore, the inverse kinematics analysis and calculation stage comprises an algebraic algorithm, the algebraic algorithm is used for calculating, so that the angle of the rotary joint is obtained through the pitching angle and the yawing angle, and then the displacement of the mobile joint is solved according to the space coordinates.
Further, the inverse kinematics analysis and calculation stage further comprises a geometric algorithm.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a schematic diagram of a spatial transformation relationship of a robotic puncture algorithm for puncture surgery according to the present invention;
FIG. 2 is a schematic view of the D-H parameters between adjacent links of a robotic puncture algorithm for a puncture procedure of the present invention;
FIG. 3 shows D-H link parameters of joints of a puncture ablation robot according to a puncture algorithm of the puncture surgical robot;
fig. 4 is a diagram of the range of motion of each joint of the puncture surgical robot puncture algorithm according to the present invention.
Detailed Description
The present invention will be further described in detail with reference to the following specific examples:
the invention aims to provide a puncture algorithm device of a puncture surgical robot, which is simple to operate, stable in work and high in efficiency.
As shown in fig. 1, the problem to be solved by the present invention is to provide a puncture surgical robot puncture algorithm with simple calculation and simplified solution.
In order to solve the problems, the invention provides a puncture algorithm of a puncture surgical robot, which comprises 2 stages: the first stage is a positive kinematics analysis and calculation stage, and the position and the posture of the end effector relative to a fixed coordinate system are obtained by using the stage through joint angles; the second stage is an inverse kinematics analysis and calculation stage, and the position of each joint in the joint space is solved by using the stage according to the known position and posture of the end effector relative to the fixed coordinate system.
The method has the advantages that modeling is carried out by adopting a positive kinematics analysis and calculation stage method, the method is different from a common D-H parameter method in the sequence of rotation and translation during space transformation, the method is more visual and simpler, and in an inverse kinematics analysis and calculation stage, the solution process can be greatly simplified and calculation can be carried out more quickly and accurately due to the existence of three joints forming a Cartesian system.
Further, the positive kinematics analysis and calculation stage comprises a D-H parameter method for motion modeling, and a transformation matrix of the terminal needle point coordinate system relative to the inertial coordinate system is obtained through the motion modeling method.
As shown in FIGS. 2 and 3, in practice, the spatial transformation of the joint i-1 to i is usually described by the following four parameters αi-1Is the connecting rod torsion angle, ai-1Is the length of the connecting rod, diIs the offset distance theta of the connecting rodiThe relationship of each link parameter is defined for the link angle.
Establishing a coordinate system of each joint on the puncture ablation surgical robot according to the method, appointing a0＝0,α0＝0,θ00 and assuming that if joint 1 is a mobile joint, q1 is variable, called the joint variable, denoted specifically as q* 1While defining q* 1The variable with star is the variable quantity of the joint, L2-312 mm, L3-84 mm, L4-530 mm, LL 4-80 mm, nx-223 mm and nz-186.5 mm through adjacent rotation matrixes of an improved D-H parameter table, a general formula and a D-H parameter table are substituted into each parameter, and a series of transformation matrixes can be obtained, and the transformation matrixes of a terminal needle point coordinate system relative to an inertial coordinate system, namely positive motion matrixes can be obtained through continuous multiplication of the matrixesThe expression of science is:
furthermore, the inverse kinematics analysis and calculation stage comprises an algebraic algorithm, the algebraic algorithm is used for calculating, so that the angle of the rotary joint is obtained through the pitching angle and the yawing angle, and then the displacement of the mobile joint is solved according to the space coordinates.
In actual practice, we can derive a transformation matrix of the end effector coordinate system {6} relative to the base coordinate system {0} based on the parametric modeling method, as shown in FIG. 4
Wherein the rotation matrix is
The translation matrix is
From this we can see the rotation matrix of the rotating joint coordinate system 4 with respect to the base coordinate system 0
Is a constant matrix, is only related to the initial structure of the robot, and is not related to the first three joint variables q1, q2 and q 3. This is because the first three joints are all translation joints, and the movement of the joints will not be possibleThe method has the advantages that the attitude of a joint coordinate system is influenced, the calculation of inverse kinematics is greatly simplified, the angle of a rotary joint is calculated only through the pitching angle and the yawing angle, and then the displacement of a mobile joint is calculated according to space coordinates. Assuming that the position and posture of the needle tip relative to the base coordinate system is T
Calculating according to positive kinematics analysis, the joint angles q4 and q5 can be obtained by solving
After q4 and q5 joint variables are obtained through solving, the spatial positions generated by the rotary joints are only required to be compensated by the translation joints, and then q1, q2 and q3 can be obtained.
Further, the inverse kinematics analysis and calculation stage further comprises a geometric algorithm.
In actual operation, the working space of the robot is obtained through a numerical method, each joint is randomly sampled, the space position of the tail end is obtained according to positive kinematics, the space position and the tail end are combined together to form a rough working space range, the rear-end moving platform performs compensation movement on the robot on the premise that the requirement of rotating around a far-end motion center is met, and the algorithm is simple and accurate.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (4)
1. A puncture surgical robot puncture algorithm is characterized by comprising 2 stages: the first stage is a positive kinematics analysis and calculation stage, and the position and the posture of the end effector relative to a fixed coordinate system are obtained by using the stage through joint angles; the second stage is an inverse kinematics analysis and calculation stage, and the position of each joint in the joint space is solved by using the stage according to the known position and posture of the end effector relative to the fixed coordinate system.
2. The robotic puncture algorithm of puncture surgery according to claim 1, wherein: and the positive kinematics analysis and calculation stage comprises a D-H parameter method for motion modeling, and a transformation matrix of the terminal needle point coordinate system relative to the inertial coordinate system is obtained through the motion modeling method.
3. The robotic puncture algorithm of puncture surgery according to claim 1, wherein: the inverse kinematics analysis and calculation stage comprises an algebraic algorithm, the algebraic algorithm is used for calculating, the angle of the rotary joint is obtained through the pitching and deflecting angles, and then the displacement of the mobile joint is solved according to the space coordinates.
4. The robotic puncture algorithm of puncture surgery according to claim 1, wherein: the inverse kinematics analysis computation phase also includes a geometric algorithm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010201350.XA CN111430022A (en) | 2020-03-20 | 2020-03-20 | Puncture algorithm of puncture surgical robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010201350.XA CN111430022A (en) | 2020-03-20 | 2020-03-20 | Puncture algorithm of puncture surgical robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111430022A true CN111430022A (en) | 2020-07-17 |
Family
ID=71553474
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010201350.XA Pending CN111430022A (en) | 2020-03-20 | 2020-03-20 | Puncture algorithm of puncture surgical robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111430022A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113977602A (en) * | 2021-10-27 | 2022-01-28 | 华南理工大学 | Force feedback tail end holder admittance control method |
| CN114176902A (en) * | 2021-01-18 | 2022-03-15 | 广州市微眸医疗器械有限公司 | Robot automatic compensation motion control method |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107856014A (en) * | 2017-11-08 | 2018-03-30 | 浙江工业大学 | Mechanical arm pose control method based on gesture recognition |
| CN108673505A (en) * | 2018-05-28 | 2018-10-19 | 南昌大学 | A kind of mechanical arm tail end precise motion control method |
| CN109129469A (en) * | 2018-08-01 | 2019-01-04 | 珠海格力电器股份有限公司 | Mechanical arm kinematics inverse solution method and device and mechanical arm |
-
2020
- 2020-03-20 CN CN202010201350.XA patent/CN111430022A/en active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107856014A (en) * | 2017-11-08 | 2018-03-30 | 浙江工业大学 | Mechanical arm pose control method based on gesture recognition |
| CN108673505A (en) * | 2018-05-28 | 2018-10-19 | 南昌大学 | A kind of mechanical arm tail end precise motion control method |
| CN109129469A (en) * | 2018-08-01 | 2019-01-04 | 珠海格力电器股份有限公司 | Mechanical arm kinematics inverse solution method and device and mechanical arm |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114176902A (en) * | 2021-01-18 | 2022-03-15 | 广州市微眸医疗器械有限公司 | Robot automatic compensation motion control method |
| CN113977602A (en) * | 2021-10-27 | 2022-01-28 | 华南理工大学 | Force feedback tail end holder admittance control method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10682758B2 (en) | Numerical method for obtaining the inverse kinematics of six-degree-of-freedom serial robot with an offset wrist | |
| CN107901041A (en) | A kind of robot vision servo control method based on image blend square | |
| CN109895101A (en) | A kind of articulated manipulator inverse kinematics numerical value unique solution acquiring method | |
| TWI704039B (en) | Mechanical arm singular point control method and system | |
| CN107116542B (en) | A kind of six joint industrial robot passes through the control method and system of posture singular point | |
| CN109702751A (en) | A kind of location class of seven freedom series connection mechanical arm is against solution method | |
| JP5580850B2 (en) | Fast grip contact calculation for serial robots | |
| EP3511127B1 (en) | Controller for robot and inverse transform method for robot | |
| CN113545851B (en) | Control method, system, equipment and storage medium for reconstructing instrument surgical field center | |
| CN110695988A (en) | Method and system for cooperative motion of double mechanical arms | |
| CN106844951B (en) | Method and system for solving inverse kinematics of super-redundant robot based on segmented geometric method | |
| CN111430022A (en) | Puncture algorithm of puncture surgical robot | |
| CN111230866B (en) | Calculation method for real-time pose of six-axis robot tail end following target object | |
| CN111791234A (en) | Anti-collision control algorithm for working positions of multiple robots in narrow space | |
| Corinaldi et al. | Singularity-free path-planning of dexterous pointing tasks for a class of spherical parallel mechanisms | |
| CN115284293A (en) | Space station mechanical arm multi-mode path planning system and method suitable for complex tasks | |
| CN109129469B (en) | Mechanical arm kinematics inverse solution method and device and mechanical arm | |
| Siciliano et al. | Kinematics | |
| CN113334381A (en) | Movable decoupling continuum robot control method | |
| CN112917479B (en) | Approximate pose calculation method and device of five-axis robot and storage medium | |
| CN113084797B (en) | Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition | |
| JP4745921B2 (en) | Control method of welding robot | |
| CN111488654A (en) | Robot kinematics solving method based on global coordinate system recursive regression mode | |
| Karadimos et al. | Virtual reality simulation of a robotic laparoscopic surgical system | |
| Qizhi et al. | On the kinematics analysis and motion planning of the manipulator of a mobile robot |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20200717 |
|
| RJ01 | Rejection of invention patent application after publication |