+

CN115281838B - Robot posture accuracy testing method and auxiliary device - Google Patents

Robot posture accuracy testing method and auxiliary device Download PDF

Info

Publication number
CN115281838B
CN115281838B CN202210984660.2A CN202210984660A CN115281838B CN 115281838 B CN115281838 B CN 115281838B CN 202210984660 A CN202210984660 A CN 202210984660A CN 115281838 B CN115281838 B CN 115281838B
Authority
CN
China
Prior art keywords
laparoscope
instrument
positioning
coordinate system
tool
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
Application number
CN202210984660.2A
Other languages
Chinese (zh)
Other versions
CN115281838A (en
Inventor
赵佳伟
陈霖
裴炎义
文洋
王伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Sagebot Intelligent Medical Equipment Co Ltd
Original Assignee
Harbin Sagebot Intelligent Medical Equipment Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Harbin Sagebot Intelligent Medical Equipment Co Ltd filed Critical Harbin Sagebot Intelligent Medical Equipment Co Ltd
Priority to CN202210984660.2A priority Critical patent/CN115281838B/en
Publication of CN115281838A publication Critical patent/CN115281838A/en
Application granted granted Critical
Publication of CN115281838B publication Critical patent/CN115281838B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B90/00—Instruments, implements or accessories specially adapted for surgery or diagnosis and not covered by any of the groups A61B1/00 - A61B50/00, e.g. for luxation treatment or for protecting wound edges
    • A61B90/06—Measuring instruments not otherwise provided for
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • 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
    • A61B2034/301—Surgical robots for introducing or steering flexible instruments inserted into the body, e.g. catheters or endoscopes
    • 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
    • A61B2034/302—Surgical robots specifically adapted for manipulations within body cavities, e.g. within abdominal or thoracic cavities

Landscapes

  • Health & Medical Sciences (AREA)
  • Engineering & Computer Science (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Surgery (AREA)
  • Remote Sensing (AREA)
  • General Health & Medical Sciences (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Veterinary Medicine (AREA)
  • Public Health (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Medical Informatics (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • Automation & Control Theory (AREA)
  • Pathology (AREA)
  • Oral & Maxillofacial Surgery (AREA)
  • Robotics (AREA)
  • Endoscopes (AREA)

Abstract

本发明涉及自动化医疗设备测试技术领域，公开机器人位姿精度测试方法及辅助装置。该测试方法包括如下步骤：步骤S1：将腹腔镜工装和器械工装分别固定于待测机器人的第一手臂和第二手臂上；步骤S2：分别获取腹腔镜工装以及器械工装在中间坐标系下的第一齐次位姿矩阵TElb和第二齐次位姿矩阵TIns；步骤S3：计算器械工装在腹腔镜工装的坐标系下的实际器械位姿矩阵Ta，Ta＝TIns ‑1TElb；获取待测机器人处于当前形位下器械工装在腹腔镜工装的坐标系下的理论位姿矩阵Td；步骤S4：比较器械工装在腹腔镜工装的坐标系下的实际器械位姿矩阵Ta与理论位姿矩阵Td，以获取待测机器人的位姿精度。所述测试方法的通用性强且测试精度高。

The present invention relates to the technical field of automated medical equipment testing, and discloses a robot posture accuracy testing method and an auxiliary device. The testing method comprises the following steps: step S1: fixing a laparoscope tool and an instrument tool on the first arm and the second arm of a robot to be tested respectively; step S2: obtaining the first homogeneous posture matrix T Elb and the second homogeneous posture matrix T Ins of the laparoscope tool and the instrument tool in the intermediate coordinate system respectively; step S3: calculating the actual instrument posture matrix T a of the instrument tool in the coordinate system of the laparoscope tool, T a =T Ins ‑1 T Elb ; obtaining the theoretical posture matrix T d of the instrument tool in the coordinate system of the laparoscope tool when the robot to be tested is in the current shape and position; step S4: comparing the actual instrument posture matrix T a of the instrument tool in the coordinate system of the laparoscope tool with the theoretical posture matrix T d to obtain the posture accuracy of the robot to be tested. The testing method has strong versatility and high testing accuracy.

Description

Robot pose precision testing method and auxiliary device
Technical Field
The invention relates to the technical field of automatic medical equipment testing, in particular to a method for testing the pose precision of a robot and an auxiliary device.
Background
With the rapid development of medical technology, micro-wound surgery is gradually popularized, and the micro-wound surgery is surgery performed by using modern medical instruments such as laparoscopes, thoracoscopes and related equipment, and has the advantages of small wound, light pain, rapid recovery and the like. In laparoscopic surgery, two mechanical arms of a robot simultaneously enter a target position in a human body, a camera mounted on one mechanical arm is used for acquiring coordinates of the target position in a three-dimensional space, and a robot control system controls the action of an executing instrument mounted on the other mechanical arm through coordinate information acquired by the camera. Therefore, ensuring that the robot has higher pose accuracy is a key factor for improving the success rate of laparoscopic surgery.
The method for testing the pose accuracy of the robot in the prior art comprises the steps of firstly obtaining actual coordinates of a camera and an executing instrument through joint axes on two mechanical arms of the robot to be tested, obtaining theoretical coordinates of the camera and the executing instrument according to joint rotation angles of the two mechanical arms, and finally determining the pose accuracy of the robot to be tested by comparing differences between the actual coordinates and the theoretical coordinates. In addition, by adopting the testing method, an operator needs to mark a proper joint axis on the mechanical arm of the robot to be tested each time and rotate the proper joint axis to a testing position, so that the workload is larger and the universality is poor.
Therefore, it is needed to propose a method for testing the pose accuracy of a robot to solve the above problems.
Disclosure of Invention
Based on the above, the robot pose precision testing method provided by the invention has the advantages of strong universality and higher testing precision, and can reduce the workload of operators.
The auxiliary device provided by the invention is suitable for the method for testing the pose precision of the robot, can be suitable for testing the pose precision of various robots to be tested, and has strong universality and high testing precision.
In order to achieve the above purpose, the invention adopts the following technical scheme:
the invention provides a method for testing the pose precision of a robot, which comprises the following steps:
Step S1, fixing a laparoscope fixture on a first arm of a robot to be tested, and fixing an instrument fixture on a second arm of the robot to be tested;
S2, respectively acquiring a first homogeneous pose matrix T Elb of the laparoscopic tool under an intermediate coordinate system and a second homogeneous pose matrix T Ins of the instrument tool under the intermediate coordinate system by using optical measurement equipment, wherein the intermediate coordinate system is a coordinate system under the optical measurement equipment;
S3, calculating an actual instrument pose matrix T a of the instrument tool under a coordinate system of the laparoscope tool, wherein T a＝TIns -1TElb;
acquiring a theoretical pose matrix T d of the instrument tool under the coordinate system of the laparoscope tool when the robot to be tested is in the current shape;
And S4, comparing an actual instrument pose matrix T a of the instrument tool with a theoretical pose matrix T d of the instrument tool in a coordinate system of the laparoscope tool so as to acquire the pose precision of the robot to be detected.
As a preferred scheme of the robot pose accuracy testing method, the first homogeneous pose matrix T Elb is:
Wherein [ n x,ny,nz]T ] is a unit vector of the laparoscopic tool in the X direction under the intermediate coordinate system [ O x,oy,oz]T ] is the unit vector of the laparoscopic tool in the Y direction under the intermediate coordinate system[ A x,ay,az]T ] is the unit vector of the laparoscopic tool in the Z direction under the intermediate coordinate systemAnd [ x H,yH,zH]T ] is the coordinate of the endoscope coordinate origin H of the laparoscopic tool in the intermediate coordinate system.
As a preferred scheme of the robot pose accuracy testing method, when the laparoscope is a 0-degree laparoscope, the specific steps of obtaining the first unified pose matrix T Elb are as follows:
s21, respectively acquiring point coordinates of a first endoscope positioning point A, a second endoscope positioning point B, a third endoscope positioning point C and an endoscope coordinate origin H on the laparoscope tool by using the optical measurement equipment, wherein the point coordinates are respectively marked as ：A(xA,yA,zA)、B(xB,yB,zB)、C(xC,yC,zC) and H (x H,yH,zH) under the intermediate coordinate system, the connecting line direction of the first endoscope positioning point A and the second endoscope positioning point B is parallel to a Y axis, and the connecting line direction of the second endoscope positioning point B and the third endoscope positioning point C is parallel to a Z axis;
step S22, calculating a unit vector of the laparoscopic tool in the Y direction under the intermediate coordinate system Wherein,
Calculating a unit vector of the laparoscopic tool in the Z direction under the intermediate coordinate systemWherein,
Step S23, calculating a unit vector of the laparoscopic tool in the X direction under the intermediate coordinate system
As a preferable scheme of the robot pose precision testing method, when the laparoscope is a +30 DEG laparoscope or a-30 DEG laparoscope, the specific steps for obtaining the first unified pose matrix T Elb are as follows:
S21', respectively acquiring point coordinates of a fourth endoscope positioning point D, a fifth endoscope positioning point E, a sixth endoscope positioning point F, a seventh endoscope positioning point G and an endoscope coordinate origin H on the laparoscopic tool by utilizing the optical measurement equipment under the intermediate coordinate system, wherein the point coordinates are respectively marked as ：D(xD,yD,zD)、E(xE,yE,zE)、F(xF,yF,zC)、G(xG,yG,zG) and H (X H,yH,zH), the connecting line direction of the fourth endoscope positioning point D and the fifth endoscope positioning point E is parallel to a Y axis, and the connecting line direction of the sixth endoscope positioning point F and the seventh endoscope positioning point G is parallel to an X axis;
step S22', calculating a unit vector of the laparoscopic tool in the Y direction under the intermediate coordinate system Wherein,
Calculating a unit vector of the laparoscopic tool in the X direction under the intermediate coordinate systemWherein,
Step S23', calculating a unit vector of the laparoscopic tool in the Z direction under the intermediate coordinate system
As a preferred scheme of the robot pose accuracy testing method, the second homogeneous pose matrix T Ins is:
Wherein [ n' x,n'y,n'z]T ] is a unit vector of the instrument tool in the X direction under the intermediate coordinate system [ O' x,o'y,o'z]T ] is the unit vector of the instrument tool in the Y direction under the intermediate coordinate system[ A' x,a'y,a'z]T ] is the unit vector of the instrument tool in the Z direction under the intermediate coordinate systemAnd [ x P,yP,zP]T ] is the coordinate of the instrument coordinate origin P of the instrument tool under the intermediate coordinate system.
As a preferred scheme of the robot pose accuracy testing method, the specific steps of obtaining the second homogeneous pose matrix T Ins are as follows:
S21', respectively acquiring point coordinates of a first instrument positioning point I, a second instrument positioning point J, a third instrument positioning point K, a fourth instrument positioning point L and an instrument coordinate origin H on the instrument tool by utilizing the optical measurement equipment, wherein the point coordinates are respectively marked as ：I(xI,yI,zI)、J(xJ,yJ,zJ)、K(xK,yK,zK)、L(xL,yL,zL) and P (x P,yP,zP) under the intermediate coordinate system, and the connecting line of the first instrument positioning point I and the second instrument positioning point J is parallel to the connecting line of the third instrument positioning point K and the fourth instrument positioning point L and is parallel to a YZ plane;
Step S22' calculating a unit vector of the instrument tool in the X direction under the intermediate coordinate system Wherein the method comprises the steps of
Calculating point coordinates of a midpoint P of a connecting line of the first instrument positioning point I and the third instrument positioning point K and a midpoint Q of a connecting line of the second instrument positioning point J and the fourth instrument positioning point L, wherein the point coordinates are respectively recorded as P (x P,yP,zP)、Q(xQ,yQ,zQ);
calculating a unit vector of the instrument tool in the Z direction under the intermediate coordinate system Wherein,
Step S23' calculating a unit vector of the instrument tool in the Y direction under the intermediate coordinate system
The auxiliary device provided by the invention is applied to the robot pose precision testing method according to any scheme, and comprises the following steps:
The laparoscopic tool is provided with a plurality of laparoscopic locating points, any two laparoscopic locating points are connected to form a laparoscopic locating detection line, at least two of the laparoscopic locating detection lines are not parallel, and the optical measurement equipment can acquire a first homogeneous pose matrix T Elb of the laparoscopic tool under an intermediate coordinate system through the laparoscopic locating points on the non-parallel laparoscopic locating detection line;
The equipment tool is provided with a plurality of equipment positioning points, any two equipment positioning points are connected to form an equipment positioning detection line, at least two of the equipment positioning detection lines are not parallel, and the optical measurement equipment can acquire a second homogeneous pose matrix T Ins of the equipment tool under an intermediate coordinate system through the equipment positioning points on the equipment positioning detection line which are not parallel.
As a preferred aspect of the auxiliary device, the laparoscopic tool includes:
The mirror body comprises a first part and a second part which are arranged at an included angle of 150 degrees;
The number of the mirror rods is two, namely a first mirror rod and a second mirror rod, the end face of the first mirror rod is perpendicular to the axis direction of the first mirror rod, the included angle between the end face of the second mirror rod and the axis of the second mirror rod is 150 degrees, and the first part can be selectively and detachably connected with the first mirror rod or the second mirror rod;
the endoscope body and the endoscope rod are provided with the endoscope positioning points.
The auxiliary device comprises a first cavity mirror detection surface, a second cavity mirror detection surface and a third cavity mirror detection surface which are sequentially connected, wherein a fourth cavity mirror detection surface is arranged on the second part, a first cavity mirror positioning hole, a second cavity mirror positioning hole, a third cavity mirror positioning hole, a fourth cavity mirror positioning hole, a fifth cavity mirror positioning hole and a sixth cavity mirror positioning hole are arranged on the mirror body, the first cavity mirror positioning hole, the second cavity mirror positioning hole and the third cavity mirror positioning hole are all positioned on the second cavity mirror detection surface and are not collinear, the fourth cavity mirror positioning hole is a through hole and penetrates through the first cavity mirror detection surface and the third cavity mirror detection surface, the axis direction of the fourth cavity mirror positioning hole is parallel to the connecting line direction of the first cavity mirror positioning hole and the second cavity mirror positioning hole, the fifth cavity mirror positioning hole and the sixth cavity mirror positioning hole are both positioned on the second cavity mirror detection surface, and the seventh cavity mirror positioning hole is arranged on one side of the fifth cavity mirror detection surface.
As the preferred scheme of auxiliary device, the first portion still includes sixth chamber mirror detection face, sixth chamber mirror detection face with the second chamber mirror detection face is parallel, just the both ends of sixth chamber mirror detection face are connected respectively first chamber mirror detection face with third chamber mirror detection face, be provided with non-collinear eighth chamber mirror locating hole, ninth chamber mirror locating hole and tenth chamber mirror locating hole on the sixth chamber mirror detection face.
As a preferred embodiment of the auxiliary device, the distance between the second and third endoscope positioning holes is greater than 40mm, and/or
The distance between the fifth endoscope positioning hole and the sixth endoscope positioning hole is larger than 40mm.
As auxiliary device's preferred scheme, the apparatus frock includes support piece, first pincers body and second pincers body, first pincers body with the second pincers body all rotate set up in on the support piece, first pincers body with all be provided with on the second pincers body the apparatus setpoint.
As an optimal scheme of the auxiliary device, a first instrument detection surface is arranged on the first clamp body, a second instrument detection surface is arranged on the second clamp body, the first instrument detection surface is parallel to the second instrument detection surface, a first instrument positioning hole and a second instrument positioning hole are formed in the first instrument detection surface, and a third instrument positioning hole and a fourth instrument positioning hole are formed in the second instrument detection surface.
Preferably, the distance between the first instrument positioning hole and the second instrument positioning hole is larger than 40mm, and/or
The spacing between the third instrument positioning hole and the fourth instrument positioning hole is greater than 40mm.
As an auxiliary device, the laparoscopic tool is provided with a first positioning piece which can be matched with a first positioning hole on a first arm of the robot to be tested, and/or
The instrument tool is provided with a second locating piece which can be matched with a second locating hole on a second arm of the robot to be detected.
The beneficial effects of the invention are as follows:
According to the robot pose precision testing method, the laparoscopic tool and the instrument tool are respectively fixed on the first arm and the second arm of the robot to be tested, when the robot to be tested is tested, only the actual instrument pose matrix T a of the instrument tool under the coordinate system of the laparoscopic tool is needed, then the actual instrument pose matrix T a is different from the theoretical pose matrix T d of the instrument tool under the coordinate system of the laparoscopic tool under the current shape of the robot to be tested, the pose precision of the robot to be tested can be obtained, the testing process is simple, the corresponding coordinate system is not needed to be established by utilizing the joint axes on the two arms of the robot to be tested, the problem of low testing precision caused by structural errors of the robot to be tested can be avoided, and therefore the testing precision of the testing method is guaranteed.
The auxiliary device for the pose precision test of the robot, provided by the invention, comprises a laparoscope tool and an instrument tool, can be suitable for pose precision test of various robots to be tested, has strong universality, and can avoid the problem of low test precision caused by structural errors of the robots to be tested during test, thereby improving the accuracy of test results.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following description will briefly explain the drawings needed in the description of the embodiments of the present invention, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and other drawings may be obtained according to the contents of the embodiments of the present invention and these drawings without inventive effort for those skilled in the art.
Fig. 1 is a flowchart of a method for testing the pose accuracy of a robot according to an embodiment of the present invention;
fig. 2 is a schematic structural diagram of a laparoscopic tool according to an embodiment of the present invention;
FIG. 3 is a schematic structural diagram II of a laparoscopic tool provided by an embodiment of the present invention;
FIG. 4 is a right side view of a laparoscopic tool provided by an embodiment of the present invention;
FIG. 5 is a left side view of a laparoscopic tool provided by an embodiment of the present invention;
FIG. 6 is a rear view of a laparoscopic tool provided by an embodiment of the present invention;
FIG. 7 is a schematic structural view of an instrument tool according to an embodiment of the present invention;
FIG. 8 is a schematic view of an instrument tool provided in an embodiment of the present invention in one state;
Fig. 9 is a schematic structural view of an instrument tool provided in an embodiment of the present invention in another state.
In the figure:
1-a lens body, 11-a first part, 111-a first lens detection surface, 1111-a fourth lens positioning hole, 112-a second lens detection surface, 1121-a first lens positioning hole, 1122-a second lens positioning hole, 1123-a third lens positioning hole, 113-a third lens detection surface, 114-a sixth lens detection surface, 1141-an eighth lens positioning hole, 1142-a ninth lens positioning hole, 1143-a tenth lens positioning hole, 115-a first positioning member, 12-a second part, 121-a fourth lens detection surface, 1211-a fifth lens positioning hole and 1212-a sixth lens positioning hole;
2-lens rod, 21-fifth lens detection surface, 211-seventh lens positioning hole;
3-a first clamp body, 31-a first instrument detection surface, 311-a first instrument positioning hole and 312-a second instrument positioning hole;
4-second forceps body, 41-second instrument detection surface, 411-third instrument positioning hole and 412-fourth instrument positioning hole;
5-support.
Detailed Description
The invention is described in further detail below with reference to the drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the invention and are not limiting thereof. It should be further noted that, for convenience of description, only some, but not all of the structures related to the present invention are shown in the drawings.
In the description of the present invention, unless explicitly stated or limited otherwise, the terms "connected," "connected," and "fixed" are to be construed broadly, and may, for example, be fixedly connected, detachably connected, or integrally formed, mechanically connected, electrically connected, directly connected, indirectly connected through an intervening medium, or in communication between two elements or in an interaction relationship between two elements. The specific meaning of the above terms in the present invention will be understood in specific cases by those of ordinary skill in the art.
In the present invention, unless expressly stated or limited otherwise, a first feature "above" or "below" a second feature may include both the first and second features being in direct contact, as well as the first and second features not being in direct contact but being in contact with each other through additional features therebetween. Moreover, a first feature being "above," "over" and "on" a second feature includes the first feature being directly above and obliquely above the second feature, or simply indicating that the first feature is higher in level than the second feature. The first feature being "under", "below" and "beneath" the second feature includes the first feature being directly under and obliquely below the second feature, or simply means that the first feature is less level than the second feature.
In the description of the present embodiment, the terms "upper", "lower", "left", "right", and the like are orientation or positional relationships based on those shown in the drawings, merely for convenience of description and simplicity of operation, and do not indicate or imply that the apparatus or elements referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus should not be construed as limiting the invention. Furthermore, the terms "first," "second," and the like, are used merely for distinguishing between descriptions and not for distinguishing between them.
The laparoscopic surgical robot generally includes a robot body and first and second arms movably provided on the robot body, and when performing a laparoscopic surgical operation, a laparoscope and a surgical instrument are generally fixed on the first and second arms, respectively, wherein the laparoscope corresponds to a 'human eye', the surgical instrument corresponds to a 'human hand', and a control system of the robot body corresponds to a 'human brain', and if the pose accuracy of the robot is sufficiently high, the actual position of the surgical instrument fed back to the robot control system by the laparoscope and the theoretical position of the surgical instrument controlled by the robot control system should be kept consistent, so that the pose accuracy of the laparoscopic surgical robot can be evaluated by comparing the difference between the actual position and the theoretical position of the surgical instrument.
Based on the above principle, as shown in fig. 1, the method for testing the pose accuracy of the robot provided in this embodiment includes the following steps:
step S1, fixing a laparoscope fixture on a first arm of a robot to be tested, and fixing an instrument fixture on a second arm of the robot to be tested;
Step S2, respectively acquiring a first homogeneous pose matrix T Elb of the laparoscopic tool under an intermediate coordinate system and a second homogeneous pose matrix T Ins of the instrument tool under the intermediate coordinate system by using optical measurement equipment, wherein the intermediate coordinate system is a coordinate system under the optical measurement equipment;
S3, calculating an actual instrument pose matrix T a of the instrument tool under a coordinate system of the laparoscope tool, wherein T a＝TIns -1TElb;
acquiring a theoretical pose matrix T d of an instrument tool of the robot to be tested in the current shape and position in a coordinate system of a laparoscope tool;
And S4, comparing an actual instrument pose matrix T a of the instrument tool with a theoretical pose matrix T d of the instrument tool in a coordinate system of the laparoscope tool to acquire pose accuracy of the robot to be detected.
According to the robot pose precision testing method, the laparoscopic tool and the instrument tool are respectively fixed on the first arm and the second arm of the robot to be tested, when the robot to be tested is tested, only the actual instrument pose matrix T a of the instrument tool under the coordinate system of the laparoscopic tool is needed to be obtained, then the actual instrument pose matrix T a is different from the theoretical pose matrix T d of the instrument tool under the coordinate system of the laparoscopic tool of the robot to be tested under the current shape, the pose precision of the robot to be tested can be obtained, the testing process is simple, the corresponding coordinate system is not needed to be established by utilizing the joint axes on the two arms of the robot to be tested, the problem of low testing precision caused by structural errors of the robot to be tested can be avoided, and therefore the testing precision of the testing method is guaranteed.
For easy understanding, the specific flow of the robot pose accuracy testing method will be described in detail with reference to the specific structure of the auxiliary device for the robot pose accuracy testing method shown in fig. 2 to 9.
As shown in fig. 2-5, a plurality of endoscope positioning points are arranged on the laparoscopic tool, any two endoscope positioning points are connected to form an endoscope positioning detection line, at least two of the plurality of endoscope positioning detection lines are not parallel, and the optical measurement equipment can acquire a first homogeneous pose matrix T Elb of the laparoscopic tool under an intermediate coordinate system through the endoscope positioning points on the non-parallel endoscope positioning detection lines.
Specifically, the laparoscopic tool comprises a laparoscopic body 1 and a laparoscopic rod 2, wherein the laparoscopic body 1 comprises a first part 11 and a second part 12 which are arranged at an included angle, the first part 11 is connected with the laparoscopic rod 2, the first part 11 comprises a first endoscopic detection surface 111, a second endoscopic detection surface 112 and a third endoscopic detection surface 113 which are sequentially connected, a fourth endoscopic detection surface 121 is arranged on the second part 12, a first endoscopic positioning hole 1121, a second endoscopic positioning hole 1122, a third endoscopic positioning hole 1123, a fourth endoscopic positioning hole 1111, a fifth endoscopic positioning hole 1211 and a sixth endoscopic positioning hole 1212 are arranged on the laparoscopic body 1, the first endoscopic positioning hole 1121, the second endoscopic positioning hole 1122 and the third endoscopic positioning hole 1123 are all positioned on the second endoscopic detection surface 112 and are not collinear, the fourth endoscopic positioning hole 1111 is a through hole and penetrates through the first endoscopic detection surface 111 and the third endoscopic detection surface 113, the axial direction of the fourth endoscopic positioning hole 1111 is parallel to the first endoscopic positioning hole 1211 and the fifth endoscopic positioning hole 1211, and the fifth endoscopic positioning hole 21 is arranged on the side of the fifth endoscopic rod 21 and is far from the fifth endoscopic positioning hole 1212.
Further, the first portion 11 further includes a sixth cavity mirror detecting surface 114, the sixth cavity mirror detecting surface 114 is parallel to the second cavity mirror detecting surface 112, two ends of the sixth cavity mirror detecting surface 114 are respectively connected to the first cavity mirror detecting surface 111 and the third cavity mirror detecting surface 113, and an eighth cavity mirror positioning hole 1141, a ninth cavity mirror positioning hole 1142 and a tenth cavity mirror positioning hole 1143 which are not collinear are formed in the sixth cavity mirror detecting surface 114.
For convenience of description, it is now defined that an intersection point between the first lumen locating hole 1121 and the second lumen detecting surface 112 is a first lumen locating point a, an intersection point between the second lumen locating hole 1122 and the second lumen detecting surface 112 is a second lumen locating point B, an intersection point between the third lumen locating hole 1123 and the second lumen detecting surface 112 is a third lumen locating point C, an intersection point between the fourth lumen locating hole 1111 and the first lumen detecting surface 111 is a fourth lumen locating point D, an intersection point between the fourth lumen locating hole 1111 and the third lumen detecting surface 113 is a fifth lumen locating point E, an intersection point between the fifth lumen locating hole 1211 and the fourth lumen detecting surface 121 is a sixth lumen locating point F, an intersection point between the sixth lumen locating hole 1212 and the fourth lumen detecting surface 121 is a seventh lumen locating point G, an intersection point between the eighth lumen locating hole 1141 and the sixth lumen detecting surface 114 is an eighth lumen locating point a', an intersection point between the ninth lumen locating hole 1111 and the sixth lumen locating surface 114 is a seventh lumen locating point C, an intersection point between the ninth lumen locating hole 1141 and the sixth lumen locating surface 114 is a seventh lumen locating point C, and a fifth lumen locating point C, an intersection point between the fifth lumen locating point C and the sixth lumen locating point 114 is a seventh lumen locating point C, and a fifth lumen locating point C, and a point C between the fifth lumen locating point 114 is a fifth lumen locating point C. Wherein, the connection line AB is parallel to the connection line A 'B', and the connection line BC is parallel to the connection line B 'C'.
There are two types of laparoscopes on the market, the first is a 0-degree laparoscope, the fifth laparoscopic detection surface 21 of the laparoscopic rod 2 is perpendicular to the second laparoscopic detection surface 112, the second is a +30-degree laparoscope or a-30-degree laparoscope, and the inclination angle of the fifth laparoscopic detection surface 21 of the laparoscopic rod 2 in the vertical direction is 30 degrees.
In order to improve the versatility of the auxiliary device, in the present embodiment, the included angle between the fourth cavity mirror detecting surface 121 and the second cavity mirror detecting surface 112 is 150 °, the number of the mirror rods 2 is two, namely, the first mirror rod and the second mirror rod, the fifth cavity mirror detecting surface 21 of the first mirror rod is perpendicular to the axial direction of the first mirror rod, the included angle between the fifth cavity mirror detecting surface 21 of the second mirror rod and the axial direction of the second mirror rod is 150 °, and the first portion 11 can be selectively detachably connected to the first mirror rod or the second mirror rod. When the robot to be tested of the 0 DEG laparoscope is required to be tested, an operator can connect the first mirror rod to the first part 11, when the robot to be tested of the +30 DEG laparoscope is required to be tested, the operator can connect the second mirror rod to the first part 11 and enable the fifth mirror detection surface 21 of the second mirror rod to be parallel to the fourth mirror detection surface 121, and when the robot to be tested of the-30 DEG laparoscope is required to be tested, the operator can connect the second mirror rod to the first part 11 and enable the fifth mirror detection surface 21 of the second mirror rod to form an included angle with the fourth mirror detection surface 121 and enable the fifth mirror detection surface 21 of the second mirror rod and the fourth mirror detection surface 121 to be symmetrical relative to the axis of the second mirror rod. By adopting the arrangement mode, the universality of the auxiliary device can be improved, the production cost can be reduced, and the auxiliary device is convenient to store.
Optionally, the first portion 11 is provided with a threaded hole, the mirror rod 2 is provided with an external thread, and the mirror rod 2 can be screwed into the threaded hole of the first portion 11, so that detachable connection of the mirror rod 2 and the threaded hole is realized, processing is facilitated, and connection is fastened. Of course, in other embodiments, the first portion 11 and the lens rod 2 may be detachably connected by a clamping connection or other manners, which is not limited in this embodiment. In addition, the laparoscope tool is arranged into a split structure, so that the respective processing precision of the lens body 1 and the lens rod 2 can be ensured, and the accuracy of a test result is further ensured.
Optionally, the first portion 11 is provided with a first positioning element 115, and the first positioning element 115 can be matched with a first positioning hole on a first arm of the robot to be tested, so as to realize accurate positioning between the laparoscopic tool and the first arm.
In this embodiment, the first homogeneous pose matrix T Elb of the laparoscopic tool in the intermediate coordinate system is:
Wherein, [ n x,ny,nz]T ] is a unit vector of the laparoscopic tool in the X direction under the intermediate coordinate system [ O x,oy,oz]T ] is the unit vector of the laparoscopic tool in the Y direction under the intermediate coordinate system[ A x,ay,az]T ] is the unit vector of the laparoscopic tool in the Z direction under the intermediate coordinate systemAnd [ x H,yH,zH]T ] is the coordinate of the endoscope coordinate origin H of the laparoscopic tool in the intermediate coordinate system.
As shown in fig. 2, when the laparoscopic tool is a 0 ° laparoscope, the laparoscopic tool is first fixed on a first arm of a robot to be measured, at this time, it is required to ensure that the second laparoscopic detection surface 112 faces the optical measurement device, the second laparoscopic detection surface 112 is parallel to the YZ plane in the intermediate coordinate system, the AB connection line is parallel to the Y axis in the intermediate coordinate system, and the BC connection line is parallel to the Z axis in the intermediate coordinate system. The intermediate coordinate system is the coordinate system under the optical measuring equipment.
At this time, the specific steps of obtaining the first alignment pose matrix T Elb are:
s21, respectively acquiring point coordinates of a first endoscope positioning point A, a second endoscope positioning point B, a third endoscope positioning point C and an endoscope coordinate origin H on a laparoscope tool under an intermediate coordinate system by using optical measurement equipment, wherein the point coordinates are respectively marked as ：A(xA,yA,zA)、B(xB,yB,zB)、C(xC,yC,zC) and H (x H,yH,zH);
Step S22, calculating a unit vector of the laparoscopic tool in the Y direction under the intermediate coordinate system Wherein,
Calculating unit vector of laparoscopic tool in Z direction under intermediate coordinate systemWherein,
Step S23, calculating a unit vector of the laparoscopic tool in the X direction under the intermediate coordinate system
As shown in fig. 3, when the laparoscopic tool is a +30° laparoscope, the laparoscopic tool is first fixed on a first arm of a robot to be measured, and the sixth laparoscopic detection surface 114 is required to be oriented to the optical measurement device, so as to ensure that the fifth laparoscopic detection surface 21 is within the photographing range of the optical measurement device, at this time, the fourth laparoscopic detection surface 121 is parallel to the XY plane in the intermediate coordinate system, the DE connecting line is parallel to the Y axis in the intermediate coordinate system, and the FG connecting line is parallel to the X axis in the intermediate coordinate system. When the laparoscopic tool is a 30 ° laparoscope, the laparoscopic tool is first fixed on a first arm of a robot to be measured, and the second laparoscopic detection surface 112 is required to face the optical measurement device, so as to ensure that the fifth laparoscopic detection surface 21 is within the shooting range of the optical measurement device, at this time, the fourth laparoscopic detection surface 121 is parallel to the XY plane in the intermediate coordinate system, the DE connecting line is parallel to the Y axis in the intermediate coordinate system, and the FG connecting line is parallel to the X axis in the intermediate coordinate system.
At this time, the specific steps of obtaining the first alignment pose matrix T Elb are:
S21', respectively acquiring point coordinates of a fourth endoscope positioning point D, a fifth endoscope positioning point E, a sixth endoscope positioning point F, a seventh endoscope positioning point G and an endoscope coordinate origin H on a laparoscope tool under an intermediate coordinate system by using optical measurement equipment, wherein the point coordinates are respectively marked as ：D(xD,yD,zD)、E(xE,yE,zE)、F(xF,yF,zC)、G(xG,yG,zG) and H (x H,yH,zH);
Step S22', calculating a unit vector of the laparoscopic tool in the Y direction under the intermediate coordinate system Wherein,
Calculating unit vector of laparoscopic tool in X direction under intermediate coordinate systemWherein,
Step S23', calculating a unit vector of the laparoscopic tool in the Z direction under the intermediate coordinate system
It should be noted that, the eighth, ninth and tenth positioning holes 1141, 1142, 1143 provided on the sixth lens detecting surface 114 may be used as spare positioning holes for the first, second and third lens positioning holes 1121, 1122, 1123, and may be calculated when the optical measuring device is inconvenient to obtain the coordinates of the points of the positioning points of the respective lenses on the second lens detecting surface 112AndIndirect acquisition ofAndThe test efficiency is higher.
As shown in fig. 7-9, a plurality of instrument positioning points are arranged on the instrument tool, any two instrument positioning points are connected to form an instrument positioning detection line, at least two of the plurality of instrument positioning detection lines are not parallel, and the optical measurement device can acquire a second homogeneous pose matrix T Ins of the instrument tool under an intermediate coordinate system through the instrument positioning points on the non-parallel instrument positioning detection lines.
Specifically, the instrument tool comprises a supporting piece 5, a first clamp body 3 and a second clamp body 4, wherein the first clamp body 3 and the second clamp body 4 are both rotatably arranged on the supporting piece 5, a first instrument detection surface 31 is arranged on the first clamp body 3, a second instrument detection surface 41 is arranged on the second clamp body 4, the first instrument detection surface 31 and the second instrument detection surface 41 are parallel, a first instrument positioning hole 311 and a second instrument positioning hole 312 are arranged on the first instrument detection surface 31, and a third instrument positioning hole 411 and a fourth instrument positioning hole 412 are arranged on the second instrument detection surface 41.
Further, a pin shaft is fixed on the supporting piece 5, and the first clamp body 3 and the second clamp body 4 are both rotatably connected to the pin shaft.
Optionally, a second positioning piece is arranged on the supporting piece 5, and the second positioning piece can be matched with a second positioning hole on a second arm of the robot to be tested, so that accurate positioning between the instrument tool and the second arm is realized.
For convenience of description, it will be defined that an intersection point between the first instrument positioning hole 311 and the first instrument detection surface 31 is a first instrument positioning point I, an intersection point between the second instrument positioning hole 312 and the first instrument detection surface 31 is a second instrument positioning point J, an intersection point between the third instrument positioning hole 411 and the second instrument detection surface 41 is a third instrument positioning point K, and an intersection point between the fourth instrument positioning hole 412 and the second instrument detection surface 41 is a fourth instrument positioning point L.
In this embodiment, the second homogeneous pose matrix T Ins of the instrument tool in the intermediate coordinate system is:
wherein [ n' x,n'y,n'z]T ] is the unit vector of the instrument in the X direction under the intermediate coordinate system [ O' x,o'y,o'z]T ] is the unit vector of the instrument in the Y direction under the intermediate coordinate system[ A' x,a'y,a'z]T ] is the unit vector of the instrument in the Z direction under the intermediate coordinate systemAnd x P,yP,zP]T is the coordinate of the instrument coordinate origin P of the instrument tool in the intermediate coordinate system.
After the instrument fixture is mounted on the second arm of the robot to be tested, the first clamp body 3 and the second clamp body 4 are driven to rotate around the pin shaft by a preset angle so that the first clamp body 3 and the second clamp body 4 are arranged in an included angle, wherein the preset angle at which the first clamp body 3 and the second clamp body 4 rotate around the pin shaft is not limited in the embodiment, and the first clamp body 3 and the second clamp body 4 are not parallel only during testing. At this time, the planes of the first instrument detection surface 31 and the second instrument detection surface 41 are parallel to the YZ plane in the intermediate coordinate system, and the first instrument positioning point I, the second instrument positioning point J, the third instrument positioning point K, and the fourth instrument positioning point L are symmetrically arranged about the bisector of the connecting line IJ and the connecting line KL.
Specifically, the specific steps of obtaining the second homogeneous pose matrix T Ins are as follows:
S21', respectively acquiring point coordinates of a first instrument positioning point I, a second instrument positioning point J, a third instrument positioning point K, a fourth instrument positioning point L and an instrument coordinate origin H on an instrument tool under an intermediate coordinate system by utilizing optical measurement equipment, wherein the point coordinates are respectively marked as ：I(xI,yI,zI)、J(xJ,yJ,zJ)、K(xK,yK,zK)、L(xL,yL,zL) and P (x P,yP,zP);
Step S22' calculating a unit vector of the instrument tool in the X direction under the intermediate coordinate system Wherein the method comprises the steps of
Calculating point coordinates of a midpoint P of a connecting line of the first instrument positioning point I and the third instrument positioning point K and a midpoint Q of a connecting line of the second instrument positioning point J and the fourth instrument positioning point L, wherein the point coordinates are respectively recorded as P (x P,yP,zP)、Q(xQ,yQ,zQ);
Calculating unit vector of instrument tool in Z direction under intermediate coordinate system Wherein,
Step S23' calculating a unit vector of the instrument tool in the Y-direction under the intermediate coordinate system
It will be appreciated that when the spacing between two points is small, the line connecting the two points may be drawn in multiple lines, and there is a large error in the direction determined by the two ends. To ensure accuracy of the test results, the spacing between the second and third endoscope positioning holes 1122 and 1123 is greater than 40mm, the spacing between the fifth and sixth endoscope positioning holes 1211 and 1212 is greater than 40mm, the spacing between the first and second instrument positioning holes 311 and 312 is greater than 40mm, and the spacing between the third and fourth instrument positioning holes 411 and 412 is greater than 40mm.
Further, the method for obtaining the theoretical pose matrix T d of the instrument tool of the robot to be measured in the current shape and the coordinate system of the laparoscope tool specifically comprises the steps of reading each joint angle theta i of a second arm of the robot to be measured in the current shape and then introducing the second arm into a kinematic model of an arm system, namely T d＝F(θi), wherein i=1, 2. The kinematic model of the arm system is the prior art, and this embodiment will not be described in detail.
Further, after the actual instrument pose matrix T a and the theoretical pose matrix T d of the instrument tool in the coordinate system of the laparoscope tool are obtained, the specific method for obtaining the pose precision of the robot to be measured is as follows:
Step S41, calculating a homogeneous pose transformation matrix T e between the pose matrix T a of the actual instrument and the theoretical pose matrix T d, wherein,
Step S42, according to the precision matrixAnd acquiring the pose angle of the robot to be tested.
It should be noted that, in the homogeneous pose transformation matrix T e between the actual instrument pose matrix T a and the theoretical pose matrix T d, the first three rows and three columns are the pose states of the robot to be measured. The method for acquiring the pose angle of the robot to be tested according to the precision matrix T is not described in detail in this embodiment, where the pose angle includes, but is not limited to, euler angles, shaft angles, euler parameters, quaternions, and the like, and may be selected according to actual test conditions, which is not limited in this embodiment.
The robot pose precision testing method provided by the embodiment can simplify the establishment flow of a laparoscopic coordinate system and an instrument coordinate system, so that the testing efficiency of the pose precision of the laparoscopic surgical robot is improved, the problem of low testing precision caused by structural errors of the robot to be tested can be avoided, the establishment precision of the laparoscopic coordinate system and the instrument coordinate system is guaranteed, the measurement coordinate systems corresponding to a 0-degree laparoscope, +30-degree laparoscope and a-30-degree laparoscope on the market can be compatible through the arrangement of the two lens rods 2, and the mode of describing the pose precision of the mechanical arm of the robot to be tested by using the homogeneous pose transformation matrix T e of the theoretical pose and the actual pose of an instrument tool is more visual and accurate.
Note that the above is only a preferred embodiment of the present invention and the technical principle applied. It will be understood by those skilled in the art that the present invention is not limited to the particular embodiments described herein, but is capable of various obvious changes, rearrangements and substitutions as will now become apparent to those skilled in the art without departing from the scope of the invention. Therefore, while the invention has been described in connection with the above embodiments, the invention is not limited to the embodiments, but may be embodied in many other equivalent forms without departing from the spirit or scope of the invention, which is set forth in the following claims.

Claims (15)

1.机器人位姿精度测试方法，其特征在于，包括如下步骤：1. A robot posture accuracy testing method, characterized in that it comprises the following steps: 步骤S1：将腹腔镜工装固定于待测机器人的第一手臂上，将器械工装固定于所述待测机器人的第二手臂上；Step S1: fixing the laparoscope tooling on the first arm of the robot to be tested, and fixing the instrument tooling on the second arm of the robot to be tested; 步骤S2：利用光学测量设备分别获取所述腹腔镜工装在中间坐标系下的第一齐次位姿矩阵TElb以及所述器械工装在所述中间坐标系下的第二齐次位姿矩阵TIns，所述中间坐标系为所述光学测量设备下的坐标系；Step S2: using an optical measuring device to respectively obtain a first homogeneous pose matrix T Elb of the laparoscope tool in an intermediate coordinate system and a second homogeneous pose matrix T Ins of the instrument tool in the intermediate coordinate system, wherein the intermediate coordinate system is a coordinate system of the optical measuring device; 步骤S3：计算所述器械工装在所述腹腔镜工装的坐标系下的实际器械位姿矩阵Ta，其中，Ta＝TIns -1TElb；Step S3: Calculate the actual instrument posture matrix Ta of the instrument tool in the coordinate system of the laparoscope tool, where Ta = T Ins -1 T Elb ; 获取所述待测机器人处于当前形位下所述器械工装在所述腹腔镜工装的坐标系下的理论位姿矩阵Td；Obtaining a theoretical posture matrix T d of the instrument fixture in the coordinate system of the laparoscope fixture when the robot to be tested is in the current position; 步骤S4：比较所述器械工装在所述腹腔镜工装的坐标系下的实际器械位姿矩阵Ta与理论位姿矩阵Td，以获取所述待测机器人的位姿精度。Step S4: Compare the actual instrument pose matrix Ta of the instrument fixture in the coordinate system of the laparoscope fixture with the theoretical pose matrix Td to obtain the pose accuracy of the robot to be tested. 2.根据权利要求1所述的机器人位姿精度测试方法，其特征在于，所述第一齐次位姿矩阵TElb为：2. The robot posture accuracy testing method according to claim 1, characterized in that the first homogeneous posture matrix T Elb is: 其中，[nx，ny，nz]T为所述腹腔镜工装在所述中间坐标系下X方向的单位向量[ox，oy，oz]T为所述腹腔镜工装在所述中间坐标系下Y方向的单位向量[ax，ay，az]T为所述腹腔镜工装在所述中间坐标系下Z方向的单位向量[xH，yH，zH]T为所述腹腔镜工装的腔镜坐标原点H在所述中间坐标系下的坐标。Wherein, [ nx , ny , nz ] T is the unit vector of the laparoscope tool in the X direction in the intermediate coordinate system [o x , o y , o z ] T is the unit vector of the laparoscope tool in the Y direction in the intermediate coordinate system [ ax , ay , az ] T is the unit vector of the laparoscope tool in the Z direction in the intermediate coordinate system [x H , y H , z H ] T is the coordinate of the laparoscope coordinate origin H of the laparoscope tool in the intermediate coordinate system. 3.根据权利要求2所述的机器人位姿精度测试方法，其特征在于，当所述腹腔镜为0°腹腔镜，获取所述第一齐次位姿矩阵TElb的具体步骤为：3. The robot posture accuracy testing method according to claim 2 is characterized in that, when the laparoscope is a 0° laparoscope, the specific steps of obtaining the first homogeneous posture matrix T Elb are: S21：利用所述光学测量设备分别获取所述腹腔镜工装上的第一腔镜定位点A、第二腔镜定位点B、第三腔镜定位点C以及腔镜坐标原点H在所述中间坐标系下的点坐标，分别记为：A(xA，yA，zA)、B(xB，yB，zB)、C(xC，yC，zC)和H(xH，yH，zH)，其中，所述第一腔镜定位点A和所述第二腔镜定位点B的连线方向与Y轴平行，所述第二腔镜定位点B和所述第三腔镜定位点C的连线方向与Z轴平行；S21: using the optical measurement device to respectively obtain the point coordinates of the first laparoscope positioning point A, the second laparoscope positioning point B, the third laparoscope positioning point C and the laparoscope coordinate origin H on the laparoscope tooling in the intermediate coordinate system, which are respectively recorded as: A( xA , yA , zA ), B( xB , yB , zB ), C( xC , yC , zC ) and H( xH , yH , zH ), wherein the direction of the line connecting the first laparoscope positioning point A and the second laparoscope positioning point B is parallel to the Y axis, and the direction of the line connecting the second laparoscope positioning point B and the third laparoscope positioning point C is parallel to the Z axis; 步骤S22：计算所述腹腔镜工装在所述中间坐标系下Y方向上的单位向量其中， Step S22: Calculate the unit vector of the laparoscope tool in the Y direction in the intermediate coordinate system in, 计算所述腹腔镜工装在所述中间坐标系下Z方向上的单位向量其中， Calculate the unit vector of the laparoscope tool in the Z direction in the intermediate coordinate system in, 步骤S23：计算所述腹腔镜工装在所述中间坐标系下X方向上的单位向量 Step S23: Calculate the unit vector of the laparoscope tool in the X direction in the intermediate coordinate system 4.根据权利要求2所述的机器人位姿精度测试方法，其特征在于，当所述腹腔镜为+30°腹腔镜或者-30°腹腔镜，获取所述第一齐次位姿矩阵TElb的具体步骤为：4. The robot posture accuracy testing method according to claim 2, characterized in that when the laparoscope is a +30° laparoscope or a -30° laparoscope, the specific steps of obtaining the first homogeneous posture matrix T Elb are: S21'：利用所述光学测量设备分别获取所述腹腔镜工装上的第四腔镜定位点D、第五腔镜定位点E、第六腔镜定位点F、第七腔镜定位点G以及腔镜坐标原点H在所述中间坐标系下的点坐标，分别记为：D(xD，yD，zD)、E(xE，yE，zE)、F(xF，yF，zC)、G(xG，yG，zG)和H(xH，yH，zH)，其中，所述第四腔镜定位点D和所述第五腔镜定位点E的连线方向与Y轴平行，所述第六腔镜定位点F和所述第七腔镜定位点G的连线方向与X轴平行；S21': using the optical measurement device to respectively obtain the point coordinates of the fourth laparoscope positioning point D, the fifth laparoscope positioning point E, the sixth laparoscope positioning point F, the seventh laparoscope positioning point G and the laparoscope coordinate origin H on the laparoscope tooling in the intermediate coordinate system, which are respectively recorded as: D( xD , yD , zD ), E( xE , yE , zE ), F( xF , yF , zC ), G( xG , yG , zG ) and H( xH , yH , zH ), wherein the direction of the line connecting the fourth laparoscope positioning point D and the fifth laparoscope positioning point E is parallel to the Y axis, and the direction of the line connecting the sixth laparoscope positioning point F and the seventh laparoscope positioning point G is parallel to the X axis; 步骤S22'：计算所述腹腔镜工装在所述中间坐标系下Y方向上的单位向量其中， Step S22': Calculate the unit vector of the laparoscope tool in the Y direction in the intermediate coordinate system in, 计算所述腹腔镜工装在所述中间坐标系下X方向上的单位向量其中， Calculate the unit vector of the laparoscope tool in the X direction in the intermediate coordinate system in, 步骤S23'：计算所述腹腔镜工装在所述中间坐标系下Z方向上的单位向量 Step S23': Calculate the unit vector of the laparoscope tool in the Z direction in the intermediate coordinate system 5.根据权利要求1所述的机器人位姿精度测试方法，其特征在于，所述第二齐次位姿矩阵TIns为：5. The robot posture accuracy testing method according to claim 1, characterized in that the second homogeneous posture matrix T Ins is: 其中，[n'x，n'y，n'z]T为所述器械工装在所述中间坐标系下X方向的单位向量[o'x，o'y，o'z]T为所述器械工装在所述中间坐标系下Y方向的单位向量[a'x，a'y，a'z]T为所述器械工装在所述中间坐标系下Z方向的单位向量[xP，yP，zP]T为所述器械工装的器械坐标原点P在所述中间坐标系下的坐标。Wherein, [ n'x , n'y , n'z ] T is the unit vector of the tool in the X direction in the intermediate coordinate system [o' x , o' y , o' z ] T is the unit vector of the instrument fixture in the Y direction in the intermediate coordinate system [a' x , a' y , a' z ] T is the unit vector of the tool in the Z direction in the intermediate coordinate system [x P , y P , z P ] T is the coordinate of the instrument coordinate origin P of the instrument tooling in the intermediate coordinate system. 6.根据权利要求5所述的机器人位姿精度测试方法，其特征在于，获取所述第二齐次位姿矩阵TIns的具体步骤为：6. The robot posture accuracy testing method according to claim 5, characterized in that the specific steps of obtaining the second homogeneous posture matrix T Ins are: S21”：利用所述光学测量设备分别获取所述器械工装上的第一器械定位点I、第二器械定位点J、第三器械定位点K、第四器械定位点L以及器械坐标原点H在所述中间坐标系下的点坐标，分别记为：I(xI，yI，zI)、J(xJ，yJ，zJ)、K(xK，yK，zK)、L(xL，yL，zL)和P(xP，yP，zP)，其中，所述第一器械定位点I和所述第二器械定位点J的连线与所述第三器械定位点K和所述第四器械定位点L的连线平行且平行于YZ平面；S21″: using the optical measuring device to respectively obtain the point coordinates of the first instrument positioning point I, the second instrument positioning point J, the third instrument positioning point K, the fourth instrument positioning point L and the instrument coordinate origin H on the instrument tooling in the intermediate coordinate system, which are respectively recorded as: I(x I , y I , z I ), J(x J , y J , z J ), K(x K , y K , z K ), L(x L , y L , z L ) and P(x P , y P , z P ), wherein the line connecting the first instrument positioning point I and the second instrument positioning point J is parallel to the line connecting the third instrument positioning point K and the fourth instrument positioning point L and is parallel to the YZ plane; 步骤S22”：计算所述器械工装在所述中间坐标系下X方向上的单位向量其中 Step S22": Calculate the unit vector of the tool in the X direction in the intermediate coordinate system in 计算所述第一器械定位点I和所述第三器械定位点K连线的中点P以及所述第二器械定位点J和所述第四器械定位点L连线的中点Q的点坐标，分别记为：P(xP，yP，zP)、Q(xQ，yQ，zQ)；Calculate the point coordinates of the midpoint P of the line connecting the first instrument positioning point I and the third instrument positioning point K and the midpoint Q of the line connecting the second instrument positioning point J and the fourth instrument positioning point L, and record them as: P(x P , y P , z P ) and Q(x Q , y Q , z Q ) respectively; 计算所述器械工装在所述中间坐标系下Z方向上的单位向量其中， Calculate the unit vector of the tool in the Z direction in the intermediate coordinate system in, 步骤S23”：计算所述器械工装在所述中间坐标系下Y方向上的单位向量 Step S23": Calculate the unit vector of the tool in the Y direction in the intermediate coordinate system 7.辅助装置，应用于如权利要求1-6中任一项所述的机器人位姿精度测试方法，其特征在于，所述辅助装置包括：7. An auxiliary device, applied to the robot posture accuracy testing method according to any one of claims 1 to 6, characterized in that the auxiliary device comprises: 腹腔镜工装，其上设置有多个腔镜定位点，任意两个所述腔镜定位点相连接形成腔镜定位检测线，多条所述腔镜定位检测线中的至少两个不平行，光学测量设备能够通过不平行的所述腔镜定位检测线上的腔镜定位点获取所述腹腔镜工装在中间坐标系下的第一齐次位姿矩阵TElb；A laparoscope tooling is provided with a plurality of laparoscope positioning points, any two of the laparoscope positioning points are connected to form a laparoscope positioning detection line, at least two of the plurality of laparoscope positioning detection lines are not parallel, and an optical measurement device can obtain a first homogeneous pose matrix T Elb of the laparoscope tooling in an intermediate coordinate system through the laparoscope positioning points on the non-parallel laparoscope positioning detection lines; 器械工装，其上设置有多个器械定位点，任意两个所述器械定位点相连接形成器械定位检测线，多条所述器械定位检测线中的至少两个不平行，光学测量设备能够通过不平行的所述器械定位检测线上的器械定位点获取所述器械工装在中间坐标系下的第二齐次位姿矩阵TIns。An instrument fixture is provided with a plurality of instrument positioning points, any two of the instrument positioning points are connected to form an instrument positioning detection line, at least two of the plurality of instrument positioning detection lines are not parallel, and an optical measurement device can obtain a second homogeneous pose matrix T Ins of the instrument fixture in an intermediate coordinate system through the instrument positioning points on the non-parallel instrument positioning detection lines. 8.根据权利要求7所述的辅助装置，其特征在于，所述腹腔镜工装包括：8. The auxiliary device according to claim 7, characterized in that the laparoscopic tooling comprises: 镜体(1)，包括呈150°夹角设置的第一部(11)和第二部(12)；The mirror body (1) comprises a first portion (11) and a second portion (12) arranged at an angle of 150°; 镜杆(2)，所述镜杆(2)的数量为两个，分别为第一镜杆和第二镜杆，所述第一镜杆的端面与所述第一镜杆的轴线方向垂直，所述第二镜杆的端面与所述第二镜杆的轴线之间的夹角为150°，所述第一部(11)能够选择性地可拆卸连接于所述第一镜杆或者所述第二镜杆；A mirror rod (2), wherein the number of the mirror rods (2) is two, namely a first mirror rod and a second mirror rod, the end face of the first mirror rod is perpendicular to the axis direction of the first mirror rod, the angle between the end face of the second mirror rod and the axis of the second mirror rod is 150°, and the first part (11) can be selectively detachably connected to the first mirror rod or the second mirror rod; 所述镜体(1)和所述镜杆(2)上均设置有所述腔镜定位点。The mirror body (1) and the mirror rod (2) are both provided with the cavity mirror positioning points. 9.根据权利要求8所述的辅助装置，其特征在于，所述第一部(11)包括依次连接的第一腔镜检测面(111)、第二腔镜检测面(112)和第三腔镜检测面(113)，所述第二部(12)上设置有第四腔镜检测面(121)；所述镜体(1)上设置有第一腔镜定位孔(1121)、第二腔镜定位孔(1122)、第三腔镜定位孔(1123)、第四腔镜定位孔(1111)、第五腔镜定位孔(1211)和第六腔镜定位孔(1212)，其中，所述第一腔镜定位孔(1121)、所述第二腔镜定位孔(1122)和所述第三腔镜定位孔(1123)均位于所述第二腔镜检测面(112)上，且不共线；所述第四腔镜定位孔(1111)为通孔，并贯穿所述第一腔镜检测面(111)和所述第三腔镜检测面(113)设置，所述第四腔镜定位孔(1111)的轴线方向与所述第一腔镜定位孔(1121)和所述第二腔镜定位孔(1122)的连线方向平行；所述第五腔镜定位孔(1211)和所述第六腔镜定位孔(1212)均位于所述第四腔镜检测面(121)上；所述镜杆(2)远离所述镜体(1)的一侧设置有第五腔镜检测面(21)，所述第五腔镜检测面(21)上设置有第七腔镜定位孔(211)。9. The auxiliary device according to claim 8, characterized in that the first part (11) comprises a first laparoscope detection surface (111), a second laparoscope detection surface (112) and a third laparoscope detection surface (113) connected in sequence, and the second part (12) is provided with a fourth laparoscope detection surface (121); the mirror body (1) is provided with a first laparoscope positioning hole (1121), a second laparoscope positioning hole (1122), a third laparoscope positioning hole (1123), a fourth laparoscope positioning hole (1111), a fifth laparoscope positioning hole (1211) and a sixth laparoscope positioning hole (1212), wherein the first laparoscope positioning hole (1121), the second laparoscope positioning hole (1122) and the third laparoscope positioning hole (1123) are all The fourth laparoscope positioning hole (1111) is a through hole and is arranged to penetrate the first laparoscope detection surface (111) and the third laparoscope detection surface (113), and the axial direction of the fourth laparoscope positioning hole (1111) is parallel to the direction of the connection line between the first laparoscope positioning hole (1121) and the second laparoscope positioning hole (1122); the fifth laparoscope positioning hole (1211) and the sixth laparoscope positioning hole (1212) are both located on the fourth laparoscope detection surface (121); a fifth laparoscope detection surface (21) is arranged on the side of the mirror rod (2) away from the mirror body (1), and a seventh laparoscope positioning hole (211) is arranged on the fifth laparoscope detection surface (21). 10.根据权利要求9所述的辅助装置，其特征在于，所述第一部(11)还包括第六腔镜检测面(114)，所述第六腔镜检测面(114)与所述第二腔镜检测面(112)平行，且所述第六腔镜检测面(114)的两端分别连接所述第一腔镜检测面(111)和所述第三腔镜检测面(113)，所述第六腔镜检测面(114)上设置有不共线的第八腔镜定位孔(1141)、第九腔镜定位孔(1142)和第十腔镜定位孔(1143)。10. The auxiliary device according to claim 9 is characterized in that the first part (11) also includes a sixth laparoscope detection surface (114), the sixth laparoscope detection surface (114) is parallel to the second laparoscope detection surface (112), and the two ends of the sixth laparoscope detection surface (114) are respectively connected to the first laparoscope detection surface (111) and the third laparoscope detection surface (113), and the sixth laparoscope detection surface (114) is provided with an eighth laparoscope positioning hole (1141), a ninth laparoscope positioning hole (1142) and a tenth laparoscope positioning hole (1143) which are not co-linear. 11.根据权利要求9所述的辅助装置，其特征在于，所述第二腔镜定位孔(1122)和所述第三腔镜定位孔(1123)之间的间距大于40mm；和/或11. The auxiliary device according to claim 9, characterized in that the distance between the second laparoscope positioning hole (1122) and the third laparoscope positioning hole (1123) is greater than 40 mm; and/or 所述第五腔镜定位孔(1211)和所述第六腔镜定位孔(1212)之间的间距大于40mm。The distance between the fifth laparoscope positioning hole (1211) and the sixth laparoscope positioning hole (1212) is greater than 40 mm. 12.根据权利要求7所述的辅助装置，其特征在于，所述器械工装包括支撑件(5)、第一钳体(3)和第二钳体(4)，所述第一钳体(3)和所述第二钳体(4)均转动设置于所述支撑件(5)上，所述第一钳体(3)和所述第二钳体(4)上均设置有所述器械定位点。12. The auxiliary device according to claim 7 is characterized in that the instrument tooling comprises a support member (5), a first pliers body (3) and a second pliers body (4), the first pliers body (3) and the second pliers body (4) are both rotatably arranged on the support member (5), and the instrument positioning point is arranged on the first pliers body (3) and the second pliers body (4). 13.根据权利要求12所述的辅助装置，其特征在于，所述第一钳体(3)上设置有第一器械检测面(31)，所述第二钳体(4)上设置有第二器械检测面(41)，所述第一器械检测面(31)和所述第二器械检测面(41)平行；所述第一器械检测面(31)上设置有第一器械定位孔(311)和第二器械定位孔(312)，所述第二器械检测面(41)上设置有第三器械定位孔(411)和第四器械定位孔(412)。13. The auxiliary device according to claim 12 is characterized in that a first instrument detection surface (31) is provided on the first pliers body (3), a second instrument detection surface (41) is provided on the second pliers body (4), and the first instrument detection surface (31) and the second instrument detection surface (41) are parallel; a first instrument positioning hole (311) and a second instrument positioning hole (312) are provided on the first instrument detection surface (31), and a third instrument positioning hole (411) and a fourth instrument positioning hole (412) are provided on the second instrument detection surface (41). 14.根据权利要求13所述的辅助装置，其特征在于，所述第一器械定位孔(311)和所述第二器械定位孔(312)之间的间距大于40mm；和/或14. The auxiliary device according to claim 13, characterized in that the distance between the first instrument positioning hole (311) and the second instrument positioning hole (312) is greater than 40 mm; and/or 所述第三器械定位孔(411)和所述第四器械定位孔(412)之间的间距大于40mm。The distance between the third instrument positioning hole (411) and the fourth instrument positioning hole (412) is greater than 40 mm. 15.根据权利要求7-14中任一项所述的辅助装置，其特征在于，所述腹腔镜工装上设置有第一定位件(115)，所述第一定位件(115)能与待测机器人的第一手臂上的第一定位孔相配合；和/或15. The auxiliary device according to any one of claims 7 to 14, characterized in that a first positioning member (115) is provided on the laparoscope tooling, and the first positioning member (115) can cooperate with a first positioning hole on the first arm of the robot to be tested; and/or 所述器械工装上设置有第二定位件，所述第二定位件能与所述待测机器人的第二手臂上的第二定位孔相配合。The instrument tooling is provided with a second positioning piece, and the second positioning piece can be matched with a second positioning hole on the second arm of the robot to be tested.
CN202210984660.2A 2022-08-17 2022-08-17 Robot posture accuracy testing method and auxiliary device Active CN115281838B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210984660.2A CN115281838B (en) 2022-08-17 2022-08-17 Robot posture accuracy testing method and auxiliary device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210984660.2A CN115281838B (en) 2022-08-17 2022-08-17 Robot posture accuracy testing method and auxiliary device

Publications (2)

Publication Number Publication Date
CN115281838A CN115281838A (en) 2022-11-04
CN115281838B true CN115281838B (en) 2024-12-24

Family

ID=83829908

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210984660.2A Active CN115281838B (en) 2022-08-17 2022-08-17 Robot posture accuracy testing method and auxiliary device

Country Status (1)

Country Link
CN (1) CN115281838B (en)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110559083A (en) * 2019-09-10 2019-12-13 深圳市精锋医疗科技有限公司 surgical robot and control method and control device for tail end instrument of surgical robot
CN111870346A (en) * 2020-07-31 2020-11-03 北京理工大学 Space registration method, device and electronic device for robot and imaging equipment

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020005370A1 (en) * 2018-06-27 2020-01-02 Auris Health, Inc. Systems and techniques for providing multiple perspectives during medical procedures
CN113876433B (en) * 2020-07-01 2025-08-05 北京术锐机器人股份有限公司 Robot system and control method
CN111956329B (en) * 2020-08-12 2022-04-26 中国科学院深圳先进技术研究院 A dual-arm robot calibration method, system, terminal and storage medium
CN112006779B (en) * 2020-09-27 2024-05-03 安徽埃克索医疗机器人有限公司 Precision detection method of surgical navigation system
CN113100933A (en) * 2021-04-06 2021-07-13 德智鸿(上海)机器人有限责任公司 Operation scene configuration method and device, computer equipment and storage medium
CN113349939B (en) * 2021-07-12 2023-03-21 哈尔滨思哲睿智能医疗设备股份有限公司 Performance test method and system for passive active hand type master-slave control surgical robot

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110559083A (en) * 2019-09-10 2019-12-13 深圳市精锋医疗科技有限公司 surgical robot and control method and control device for tail end instrument of surgical robot
CN111870346A (en) * 2020-07-31 2020-11-03 北京理工大学 Space registration method, device and electronic device for robot and imaging equipment

Also Published As

Publication number Publication date
CN115281838A (en) 2022-11-04

Similar Documents

Publication Publication Date Title
JP7237483B2 (en) Robot system control method, control program, recording medium, control device, robot system, article manufacturing method
CN105848552B (en) Method for measuring distance based on endoscope and endoscopic system
CN110370286B (en) Recognition method of fixed-axis motion rigid body space position based on industrial robot and monocular camera
JP4021413B2 (en) Measuring device
JP6025386B2 (en) Image measuring apparatus, image measuring method, and image measuring program
JP2009269110A (en) Assembly equipment
CN112754616B (en) Ultrasonic positioning puncture system and storage medium
CN110325329A (en) The method that the executor for diagnosing and/or treating effector system is calibrated
CN113442169A (en) Method and device for calibrating hands and eyes of robot, computer equipment and readable storage medium
CN100579479C (en) Puncture hole positioning device for endoscope-operated surgical robot
US12183038B2 (en) Camera calibration using fiducial markers on surgical tools
CN113349939B (en) Performance test method and system for passive active hand type master-slave control surgical robot
US10650536B2 (en) Systems and methods for tracker characterization and verification
WO2022141153A1 (en) Ultrasonic positioning puncture system and storage medium
CN116785001B (en) Method, system and storage medium for calibrating implantation operation tool
CN113208731A (en) Binocular vision system-based hand and eye calibration method for surgical puncture robot
US11813027B2 (en) System and method for positioning a surgical tool
WO2023040632A1 (en) Computer-readable storage medium, alignment method and system, surgical robot system, and electronic device
CN115281838B (en) Robot posture accuracy testing method and auxiliary device
US20230252681A1 (en) Method of medical calibration
CN106695796B (en) Portable laser scanning survey arm hand and eye calibrating method based on RANSAC
WO2025085633A1 (en) Systems and methods for improved stereotactic localization
CN113598954B (en) System, method, equipment and medium for calibrating end effector of mechanical arm
CN108057584A (en) A kind of automatic butt system
JP2022047374A (en) Surgical navigation system, medical imaging system with surgical navigation function, and registration method of medical images for surgical navigation

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
GR01 Patent grant
GR01 Patent grant
点击 这是indexloc提供的php浏览器服务，不要输入任何密码和下载