CN115813556A - Surgical robot calibration method and device, surgical robot and storage medium - Google Patents
Surgical robot calibration method and device, surgical robot and storage medium Download PDFInfo
- Publication number
- CN115813556A CN115813556A CN202211686989.7A CN202211686989A CN115813556A CN 115813556 A CN115813556 A CN 115813556A CN 202211686989 A CN202211686989 A CN 202211686989A CN 115813556 A CN115813556 A CN 115813556A
- Authority
- CN
- China
- Prior art keywords
- pose
- surgical robot
- coordinate system
- translation
- calibration
- 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
Images
Landscapes
- Manipulator (AREA)
Abstract
The invention relates to the field of robot calibration, and discloses a surgical robot calibration method, a surgical robot calibration device, a surgical robot and a storage medium, wherein the method comprises the following steps: controlling the surgical robot to move according to a preset calibration route, so that the surgical robot sequentially reaches each calibration pose; the calibration poses comprise an initial pose, a first translation pose, a second translation pose and at least two rotation poses; shooting mark points on the surgical robot through a camera device, and recording pose data of the surgical robot in each calibration pose; calculating a first rotation matrix from the base coordinate system to the camera coordinate system according to the initial pose, the first translation pose and the second translation pose; and calculating a first translation matrix from the base coordinate system to the camera coordinate system and a second rotation matrix and a second translation matrix from the flange coordinate system of the surgical robot to the reference coordinate system of the surgical robot according to the first rotation matrix and the at least two rotation poses.
Description
Technical Field
The invention relates to the field of robot calibration, in particular to a surgical robot calibration method and device, a surgical robot and a storage medium.
Background
The surgical robot has the advantages of high positioning precision and good repeatability, and is widely applied to image navigation type orthopedic clinical surgery. The navigation and positioning principle of the surgical robot is briefly described as follows: the positioning camera tracks the mark points arranged on the mechanical arm, and obtains the actual spatial pose of the tail end of the mechanical arm through the transformation relation between a reference coordinate system and a TCP coordinate system (a flange coordinate system at the tail end of the mechanical arm) at the tail end of the mechanical arm, so that the mechanical arm is guided to move to the planned target pose. The pose of the mechanical arm end intrinsic TCP relative to the mechanical arm intrinsic base coordinate system can be directly obtained from a control system of the mechanical arm, the conversion relation between the mechanical arm end reference array coordinate system and the mechanical arm end intrinsic TCP coordinate system needs to be obtained through calibration, and the accuracy of the calibration result directly influences the positioning precision of the mechanical arm assisted surgery.
The conversion relationship between the robot arm end reference array coordinate system and the robot arm end intrinsic TCP coordinate system (end tool coordinate system conversion relationship for short) can be obtained by the following method: the mechanical arm end reference array and the mechanical arm end intrinsic TCP are in rigid connection, the coordinate system conversion relation of the mechanical arm end reference array and the mechanical arm end intrinsic TCP can be measured in a hardware design drawing, and the measurement value is a design value. The value is used for calibrating the mechanical arm base and the hand eye of the positioning camera. On the basis of the hand-eye calibration, the probe is used for acquiring the coordinates of the verification point on the mechanical arm tail end reference array, and the coordinates are used for correcting the conversion relation of the tail end tool coordinate system. The disadvantages are as follows: an error exists between a design value and an assembled object, the error is brought into hand-eye calibration of a mechanical arm base and a positioning camera, and the result of the hand-eye calibration is used for correcting the coordinate system conversion relation of the end tool, so that the correction has locality, and when the tail end posture of the mechanical arm is greatly changed, the correction is not applicable.
Disclosure of Invention
In a first aspect, the present application provides a calibration method for a surgical robot, including:
controlling the surgical robot to move according to a preset calibration route, so that the surgical robot sequentially reaches each calibration pose; the calibration poses comprise an initial pose, a first translation pose, a second translation pose and at least two rotation poses;
shooting mark points on the surgical robot through a camera device, and recording pose data of the surgical robot in each calibration pose;
calculating a first rotation matrix from a base coordinate system of the surgical robot to a camera coordinate system of the camera device according to the initial pose, the first translation pose and the second translation pose;
and calculating a first translation matrix from the base coordinate system to the camera coordinate system and a second rotation matrix and a second translation matrix from a flange coordinate system of the surgical robot to a reference coordinate system of the surgical robot according to the first rotation matrix and the at least two rotation poses.
Further, the controlling the surgical robot to move according to a preset calibration route, so that the surgical robot reaches each calibration pose in sequence, includes:
controlling the surgical robot to translate along the X-axis direction of the base coordinate system to the first translation pose;
controlling the surgical robot to translate along the Y-axis direction of the base coordinate system to the second translation pose;
and controlling the surgical robot to perform at least two movements including rotation in sequence, so that the surgical robot forms the at least two rotation poses.
Further, the shooting the mark points on the surgical robot by the camera device, and recording pose data of the surgical robot at each calibration pose, includes:
when the surgical robot is positioned at the calibration pose, the camera shooting device shoots the marking points on the surgical robot and obtains the translation vector from the camera coordinate system to the reference coordinate system;
and when the surgical robot is positioned at the rotary pose, the rotary change and the translation vector from the base coordinate system to the flange coordinate system are obtained through the control parameters of the surgical robot.
Further, the calculating a first rotation matrix from the base coordinate system to the camera coordinate system according to the initial pose, the first translation pose, and the second translation pose includes:
obtaining a first vector according to the first translation pose and the initial pose, and obtaining a second vector according to the second translation pose and the initial pose;
and calculating coordinate values of the coordinate system vector (x, y, z) of the base coordinate system in the camera coordinate system according to the first vector and the second vector, so as to determine the first rotation matrix.
Further, the first vector and the second vector expression are:
s 1 ＝t c1 -t c0 ；
s 2 ＝t c2 -t c0 ；
in the formula, s 1 Is said first vector, s 2 Is said second vector, t c0 Pose data for the initial pose, t c1 Pose data, t, for the first translation pose c2 Pose data for the second translation pose;
and a coordinate value calculation expression of the coordinate system vector of the flange coordinate system under the camera coordinate system is as follows:
the first rotation matrix expression is:
R＝(x’,y’,z’)。
further, the calculating the first translation matrix and the second translation matrix includes:
establishing a conversion equation according to a conversion relation among the base coordinate system, the optical camera coordinate system, the reference coordinate system and the flange coordinate system;
and respectively substituting each rotation pose into the conversion equation to solve to obtain the first translation matrix and the second translation matrix.
Further, the calculating the second rotation matrix includes:
and after the first translation matrix and the second translation matrix are obtained through calculation, the second rotation matrix is obtained through calculation according to the transformation equation and pose data of any target pose.
In a second aspect, the present application further provides a calibration apparatus for a surgical robot, including:
the control module is used for controlling the surgical robot to move according to a preset calibration route so that the surgical robot reaches each calibration pose in sequence; the calibration poses comprise an initial pose, a first translation pose, a second translation pose and at least two rotation poses;
the acquisition module is used for shooting mark points on the surgical robot through a camera device and recording pose data of the surgical robot in each calibration pose;
a first calculation module, configured to calculate a first rotation matrix from a base coordinate system of the surgical robot to a camera coordinate system of the imaging device according to the initial pose, the first translation pose, and the second translation pose;
and the second calculation module is used for calculating a first translation matrix from the base coordinate system to the camera coordinate system and a second rotation matrix and a second translation matrix from the flange coordinate system of the surgical robot to the reference coordinate system of the surgical robot according to the first rotation matrix and the at least two rotation poses.
In a third aspect, the present application further provides a surgical robot arm, including a processor and a memory, where the memory stores a computer program, and the computer program executes the calibration method of the surgical robot when running on the processor.
In a fourth aspect, the present application further provides a readable storage medium storing a computer program, which when executed on a processor performs the surgical robot calibration method.
The invention discloses a method and a device for calibrating a surgical robot, the surgical robot and a storage medium, wherein the method comprises the following steps: controlling the surgical robot to move according to a preset calibration route, so that the surgical robot sequentially reaches each calibration pose; the calibration poses comprise an initial pose, a first translation pose, a second translation pose and at least two rotation poses; shooting mark points on the surgical robot through a camera device, and recording pose data of the surgical robot in each calibration pose; calculating a first rotation matrix from the base coordinate system to the camera coordinate system according to the initial pose, the first translation pose and the second translation pose; and calculating a first translation matrix from the base coordinate system to the camera coordinate system, and a second rotation matrix and a second translation matrix from the flange coordinate system of the surgical robot to the reference coordinate system of the surgical robot according to the first rotation matrix and the at least two rotation poses. In the calculation process, a design value is not needed, so that the calculation result is obtained from an actual measurement value and is not interfered by an error between the design value and an actual value of the surgical robot, and the calibration result is not accurate. The accuracy of the calibration result is increased.
Drawings
In order to more clearly illustrate the technical solution of the present invention, the drawings required to be used in the embodiments will be briefly described below, and it should be understood that the following drawings only illustrate some embodiments of the present invention, and therefore should not be considered as limiting the scope of the present invention. Like components are numbered similarly in the various figures.
Fig. 1 is a schematic flow chart illustrating a calibration method of a surgical manipulator according to an embodiment of the present disclosure;
FIG. 2 is a schematic view illustrating a calibration scenario of a surgical robotic arm according to an embodiment of the present application;
FIG. 3 is a diagram illustrating a coordinate system transformation relationship according to an embodiment of the present application;
fig. 4 shows a schematic structural diagram of a calibration device of a surgical manipulator according to an embodiment of the present application.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments.
The components of embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present invention, presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present invention without making any creative effort, shall fall within the protection scope of the present invention.
Hereinafter, the terms "including", "having", and their derivatives, which may be used in various embodiments of the present invention, are only intended to indicate specific features, numbers, steps, operations, elements, components, or combinations of the foregoing, and should not be construed as first excluding the existence of, or adding to, one or more other features, numbers, steps, operations, elements, components, or combinations of the foregoing.
Furthermore, the terms "first," "second," "third," and the like are used solely to distinguish one from another and are not to be construed as indicating or implying relative importance.
Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which various embodiments of the present invention belong. The terms (such as those defined in commonly used dictionaries) should be interpreted as having a meaning that is consistent with their contextual meaning in the relevant art and will not be interpreted in an idealized or overly formal sense unless expressly so defined herein in various embodiments of the present invention.
The technical scheme of the application is applied to calibration of the surgical robot, namely, before the surgical robot is actually used, the conversion relation among a base coordinate system, a reference coordinate system, a flange coordinate system and an external camera coordinate system on the surgical robot is calibrated. And acquiring and calculating the pose data of each mechanical arm to obtain a transformation matrix from the base coordinate system to the camera coordinate system. And a transformation matrix of the flange coordinate system to the reference coordinate system.
The robot is provided with a mark point for the camera device to capture, the mark point is charged by a special mark device and can be a reflective ball or a reflective plate and other articles which are easy to capture and identify by the camera device. The reference coordinate system is a coordinate system established on the basis of the marking points.
The base coordinate system is a coordinate system established on the basis of the robot base, and the flange coordinate system is a coordinate system established on the basis of the tail end of the robot. The camera coordinate system is a coordinate system established by the position of the camera device. The base coordinate system and the camera coordinate system are stationary due to the working environment, and only the flange coordinate system and the reference coordinate system move.
It is understood that the above-mentioned optical camera directly captures the spatial coordinate data of the mark point, so that the transformation matrix from the camera coordinate system to the reference coordinate system can be directly obtained through the measurement of the optical camera. Similarly, the robot motion parameters are known per se, so that the transformation matrix from the base coordinate system to the flange coordinate system can be obtained directly from the control parameters of the robot as long as the starting coordinates of the end flange are determined. The calibration of the present application is to obtain a transformation matrix from the base coordinate system to the camera coordinate system. And a transformation matrix of the flange coordinate system to the reference coordinate system. The conversion matrix is composed of a rotation matrix and a translation vector, and the conversion matrix can be obtained only by acquiring the corresponding rotation matrix and translation vector.
The technical solution of the present application will be described below with specific examples.
Example 1
As shown in fig. 1, the calibration method of the surgical robot of the present application includes the following steps:
and S100, controlling the surgical robot to move according to a preset calibration route, so that the surgical robot sequentially reaches each calibration pose.
In this embodiment, in order to perform calibration, the surgical robot is controlled to move, and the surgical robot is stopped after moving for a certain distance or rotating for a certain angle, so that the pose of the surgical robot is fixed, and the pose when the robot stops is the calibration pose.
The calibration pose of the embodiment includes an initial pose, a first translation pose, a second translation pose, and at least two rotation poses.
The initial pose is the default position that the robot returns to when not working, and the position is set by the self-definition of a program. For example, at the initial position, the flange of the surgical robot coincides with the origin of the polar coordinate system. The translation pose is a pose formed by controlling the robot to perform translation movement, for example, a first translation pose is obtained by controlling the surgical robot to translate a certain distance on the X axis, and a second translation pose is obtained by controlling the surgical robot to translate on the Y axis. It can be understood that the translation pose is obtained first after the initial pose, and the rotation pose is obtained only after the translation is finished.
The rotary poses need to be at least two and are completely different, and the rotary poses need to move including rotary operation of the surgical robot, so that the poses of the surgical robot are changed due to rotation. The number of the rotary poses can be properly added for calibrating the precision.
And S200, shooting mark points on the surgical robot through a camera device, and recording pose data of the surgical robot in each calibration pose.
The camera device may be a video camera or a camera or the like for photographing the surgical robot and acquiring coordinates of the marker points. The present embodiment is described with reference to a camera as an example.
As shown in fig. 2, for the calibration scene in this embodiment, the calibration device further includes a camera 200 in addition to the surgical robot 100 to be calibrated, the surgical robot 100 forms various poses by means of translation and rotation, and the camera 200 obtains spatial coordinates of the surgical robot by shooting the marker points 110 on the surgical robot 100.
The pose data comprises translation vectors and rotation changes, can be embodied in a matrix form, and can represent the pose conditions of the surgical mechanical arm under different coordinate systems.
For convenience of explanation, in the present embodiment, the following definitions are made, and the base coordinate system of the surgical robot is denoted by F base Flange coordinate system is marked as F flange Reference coordinate system is marked as F endRF The camera coordinate system is marked as F camera 。F base To F flange Is denoted as R bi The translation vector is denoted t bi 。F camera To F endRF Is denoted as R ci The translation vector is denoted t ci . The two pairs of rotation change and translation vectors can be obtained directly through shooting of a camera or in a motion system of the surgical robot, and the rotation change and translation vectors are known quantities as long as the pose of the surgical robot is determined.
When the surgical robot is in any calibration pose, F can be acquired by shooting through the camera camera To F endRF Translation vector t of ci T is t because the camera 200 directly photographs the mark point 110 ci Can be directly acquired. And i of the subscripts in the translation vectors and the rotation matrix represents the number of the calibration pose. The numbers may be generated in the order of recording the nominal pose, e.g. the initial pose recorded first, then the translation vector acquired at that time is t c0 Then in turn the first and second translational poses, then i grows to 1 and 2 in turn. The rotation positions recorded afterwards also have their index i increasing in this order, for example i equals 3 and 4 in order if there are two rotation positions, or 5,6,7 in order if there are more rotation positions, etc.
When the surgical robot is in the rotation pose, F is acquired in addition to the translation vector described above base To F flange Rotational change of (R) bi And a translation vector t bi 。
That isThat is, in the first and second translational positions described above, only F is recorded camera To F endRF Translation vector t of ci For the rotation pose, F is additionally recorded base To F flange Of (3) a rotation matrix R bi And a translation vector t bi 。
It can be understood that the obtained rotation matrixes and the obtained translation vectors are real-time measurement values, and do not need to participate in design values of the mechanical arm, so that subsequent calculation errors caused by errors of the design values are avoided. The above design values refer to size parameters of the respective positional components and the like when designing the surgical robot. Such as the length of the robot arm, or the coordinate position of the marking point on the robot, etc.
In this embodiment, after the pose data of the initial pose and the two translation poses are acquired, the pose data of the four rotation poses is acquired as an example, and then the pose data of 7 poses is acquired in total by adding the initial pose and the two translation poses.
Step S300, calculating a first rotation matrix from the base coordinate system to the camera coordinate system according to the initial pose, the first translation pose and the second translation pose.
The first rotation matrix from the base coordinate system to the camera coordinate system can be calculated by the initial pose, the first translation pose and the second translation pose, and the specific calculation process is as follows.
Firstly, a first vector is obtained according to the first translation pose and the initial pose, and a second vector is obtained according to the second translation pose and the initial position.
Wherein the first vector and the second vector expression are:
s 1 ＝t c1 -t c0 ；
s 2 ＝t c2 -t c0 ；
in the formula, s 1 Is said first vector, s 2 Is said second vector, t c0 Pose data for the initial pose (i.e. F) camera To F endRF Translation vector of) t c1 Is a stand forPose data of the first translation pose, t c2 Pose data for the second translation pose.
And calculating to obtain coordinate values (x ', y ', z ') of the coordinate system vector (x, y, z) of the base coordinate system in the camera coordinate system according to the first vector and the second vector.
It can be understood that the first vector and the second vector represent the relative position relationship between the surgical robot and the initial position after two translations, and since the whole translation is performed without the rotation transformation, the data can be used to find the first rotation matrix R from the base coordinate system to the camera coordinate system.
Wherein, (x ', y ', z '), the calculation expression of which is:
it will be appreciated that each of the above x ', y ', z ' terms can be represented in the form of a vector, i.e.:
then, according to the conversion relationship between the spatial coordinate systems, the first rotation matrix R = (x ', y ', z '), that is:
the first rotation matrix may be verified by left-multiplying the spatial coordinates of the base coordinate system by R.
Step S400, calculating a first translation matrix from the base coordinate system to the camera coordinate system, and a second rotation matrix and a second translation matrix from the flange coordinate system of the surgical robot to the reference coordinate system of the surgical robot according to the first rotation matrix and the at least two rotation poses.
After the first rotation matrix is obtained, the rotation matrix can be relied on to furtherObtaining a first translation matrix t and a second rotation matrix R which need to be solved next e And a second translation matrix t e 。
As shown in FIG. 3, is F base ，F flange ，F endRF ，F cmnera The coordinate system conversion relation closed loop between the two can obtain the corresponding conversion equation according to the conversion relation closed loop:
[R bi ，t bi ]·[R e ，t e ]＝[R，t] -1 ·[R ci ，t ci ]the equation is simplified to obtain:
R bi R e ＝R -1 R ci ……(1)，
R bi t e +t bi ＝R -1 t ci -R -1 t……(2)
from formula (2): r bi t e +R -1 t＝R -1 t ci -t bi ……(3)
The first rotation matrix R is obtained in step S300, and only the translation vector t remains as the unknown quantity e And t.
Recording:
t m ＝R -1 t，
t i ＝R -1 t ci -t bi
then (3) becomes
R bi t e +t m ＝t i ……(4)
t m Because of the unknown quantity t, t m Are themselves unknown constants. t is t i In each rotational position, t ci And t bi Is known and can therefore be calculated directly by bringing into the pose data of the rotational pose, and R bi A known quantity in each rotational position.
When the subscript i corresponding to the four rotation poses ranges from 3,4,5 and 6, the t can be calculated m 、t i 、R bi And t e Expressed in the following form:
t in the formula ex 、t ey 、t ez Representing the flange coordinate system F flange Next, the end of arm reference frame F endRF Coordinate value of the origin, t mx 、t my 、t mz 、t ix 、t iy 、t iz Are vector values of corresponding variables respectively, and are used for form parameters of a subsequent least square method. R bi Each value in the matrix is a known value obtained from the control parameters of the surgical robot at the corresponding rotational pose.
The formula (4) can be changed to
The above formula is equivalent to:
substituting the pose data of the rotation pose into the pose data to obtain:
assuming that the first matrix on the left of the equation above is a, the second column vector on the left of the equation is x, and the column vector on the right of the equation is b, the above equation can be written as Ax = b, and using least squares the solution for x can be found as: x = (A) T A) -1 A T b。
It should be noted that the premise that x has a solution is that a is a column full rank matrix, and since a contains data of 4 poses, the number of rows of a exceeds the number of columns, and as long as the rotation matrices of any two poses are not equal, a can satisfy the column full rank, so that as long as at least two rotation poses are ensured to be different, x can be kept to have a solution.
Therefore, the second translation vector t e And t m Has been solved for:
wherein x (0), x (1), x (2), x (3), x (4) and x (5) each represent t ex 、t 。y 、t ez 、tmx、t my 、t mz 。
And because of t m ＝R -1 t So the first translational vector t can also already be calculated:
t＝R.t m 。
once the above solution is obtained, it is possible to obtain a solution by combining any of the groups R bi ，R ci Re can be obtained by substituting the data into the formula (1). The calculation expression is as follows:
since there are 4 rotation positions in this embodiment, the value of i is 3,4,5,6. If the number of the rotary poses is only two, the value of i is 3,4. If more, then expand in turn.
According to the calculation process, the design value is not used in the whole process of the mechanical arm calibration, and the accuracy of the rotation matrix and the translation vector obtained by the calibration is higher than that obtained by using the design value to participate in the calculation.
The first rotation matrix R, the first translation vector t and the second rotation matrix R are obtained e Second translation vector t e Then, it is equal to obtain F camera To F base And F flange To F endRF And F flange To F endRF And (4) converting the matrix to finish calibration. By means of the two conversion matrixes, in actual work, the mark points on the surgical robot are shot through the camera, the mark points can be converted into coordinate points of the flange at the tail end of the surgical robot, and therefore the coordinate points can be converted into coordinate points of the flange at the tail end of the surgical robotThereby realizing the tracking of the tail end of the surgical robot.
The target method in this embodiment obtains required data by designing a specific movement mode of the surgical robot, and then calculates a first rotation matrix R, a first translational vector t, and a second rotation matrix R, respectively e Second translation vector t e And then the solution of the two unknown transformation matrixes is completed, so that the calibration is completed. In the whole calibration process, the design value of the mechanical arm is not needed, and the four quantities can be calculated only by data shot by the optical camera during calibration and motion parameters of the robot during motion, so that errors between the design value and an actual value are avoided, and the accuracy of a calibration result is improved.
Example 2
As shown in fig. 4, the present application also provides a surgical robot calibration apparatus, including:
the control module 10 is configured to control the surgical robot to move according to a preset calibration route, so that the surgical robot sequentially reaches each calibration pose; the calibration poses comprise an initial pose, a first translation pose, a second translation pose and at least two rotation poses;
the acquisition module 20 is configured to shoot the marker points on the surgical robot through the camera device, and record pose data of the surgical robot at each calibration pose;
a first calculation module 30, configured to calculate a first rotation matrix from the base coordinate system of the surgical robot to the camera coordinate system of the image capturing device according to the initial pose, the first translation pose, and the second translation pose;
a second calculation module 40, configured to calculate a first translation matrix from the base coordinate system to the camera coordinate system, and a second rotation matrix and a second translation matrix from the flange coordinate system of the surgical robot to the reference coordinate system of the surgical robot according to the first rotation matrix and the at least two rotation poses.
The application also provides a surgical mechanical arm, which comprises a processor and a memory, wherein the memory stores a computer program, and the computer program executes the calibration method of the surgical robot when running on the processor.
The present application further provides a readable storage medium storing a computer program which, when run on a processor, performs the surgical robot calibration method.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method can be implemented in other ways. The apparatus embodiments described above are merely illustrative and, for example, the flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present invention. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In addition, each functional module or unit in each embodiment of the present invention may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
The functions may be stored in a computer-readable storage medium if they are implemented in the form of software functional modules and sold or used as separate products. Based on such understanding, the technical solution of the present invention or a part of the technical solution that contributes to the prior art in essence can be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a smart phone, a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention.
Claims (10)
1. A surgical robot calibration method is characterized by comprising the following steps:
controlling the surgical robot to move according to a preset calibration route, so that the surgical robot sequentially reaches each calibration pose; the calibration poses comprise an initial pose, a first translation pose, a second translation pose and at least two rotation poses;
shooting mark points on the surgical robot through a camera device, and recording pose data of the surgical robot in each calibration pose;
calculating a first rotation matrix from a base coordinate system of the surgical robot to a camera coordinate system of the camera device according to the initial pose, the first translation pose and the second translation pose;
and calculating a first translation matrix from the base coordinate system to the camera coordinate system and a second rotation matrix and a second translation matrix from a flange coordinate system of the surgical robot to a reference coordinate system of the surgical robot according to the first rotation matrix and the at least two rotation poses.
2. The surgical robot calibration method according to claim 1, wherein the controlling of the surgical robot to move according to a preset calibration route so that the surgical robot reaches each calibration pose in turn comprises:
controlling the surgical robot to translate along the X-axis direction of the base coordinate system to the first translation pose;
controlling the surgical robot to translate along the Y-axis direction of the base coordinate system to the second translation pose;
and controlling the surgical robot to perform at least two movements including rotation in sequence, so that the surgical robot forms the at least two rotation poses.
3. The calibration method for the surgical robot according to claim 1, wherein the capturing the mark points on the surgical robot by the camera device and recording the pose data of the surgical robot at each calibration pose comprises:
when the surgical robot is positioned at the calibration pose, the mark points on the surgical robot are shot through the camera device, and the translation vector from the camera coordinate system to the reference coordinate system is obtained;
and when the surgical robot is positioned at the rotary pose, the rotary change and the translation vector from the base coordinate system to the flange coordinate system are obtained through the control parameters of the surgical robot.
4. The surgical robot calibration method of claim 1, wherein said calculating a first rotation matrix of the base coordinate system to the camera coordinate system from the initial pose, the first translational pose, and the second translational pose comprises:
obtaining a first vector according to the first translation pose and the initial pose, and obtaining a second vector according to the second translation pose and the initial pose;
and calculating coordinate values of a coordinate system vector (x, y, z) of the base coordinate system in the camera coordinate system according to the first vector and the second vector, so as to determine the first rotation matrix.
5. A surgical robot calibration method according to claim 4, wherein the first and second vector expressions are:
s 1 ＝t c1 -t c0 ；
s 2 ＝t c2 -t c0 ；
in the formula, s 1 Is said first vector, s 2 Is said second vector, t c0 Pose data for the initial pose, t c1 Pose data, t, for the first translation pose c2 Pose data for the second translation pose;
and the coordinate value calculation expression of the coordinate system vector of the flange coordinate system under the camera coordinate system is as follows:
the first rotation matrix expression is:
R＝(x’,y’,z’)。
6. the surgical robot calibration method of claim 1, wherein the calculating the first translation matrix and the second translation matrix comprises:
establishing a conversion equation according to a conversion relation among the base coordinate system, the optical camera coordinate system, the reference coordinate system and the flange coordinate system;
and respectively substituting each rotation pose into the conversion equation to solve to obtain the first translation matrix and the second translation matrix.
7. The surgical robot calibration method of claim 6, wherein the calculating the second rotation matrix comprises:
and after the first translation matrix and the second translation matrix are obtained through calculation, the second rotation matrix is obtained through calculation according to the transformation equation and pose data of any target pose.
8. A surgical robot calibration device, comprising:
the control module is used for controlling the surgical robot to move according to a preset calibration route so that the surgical robot reaches each calibration pose in sequence; the calibration poses comprise an initial pose, a first translation pose, a second translation pose and at least two rotation poses;
the acquisition module is used for shooting mark points on the surgical robot through a camera device and recording pose data of the surgical robot in each calibration pose;
a first calculation module, configured to calculate a first rotation matrix from a base coordinate system of the surgical robot to a camera coordinate system of the imaging device according to the initial pose, the first translation pose, and the second translation pose;
and the second calculation module is used for calculating a first translation matrix from the base coordinate system to the camera coordinate system and a second rotation matrix and a second translation matrix from the flange coordinate system of the surgical robot to the reference coordinate system of the surgical robot according to the first rotation matrix and the at least two rotation poses.
9. A surgical robotic arm comprising a processor and a memory, the memory storing a computer program which, when run on the processor, performs the surgical robot calibration method of any of claims 1 to 7.
10. A readable storage medium, characterized in that it stores a computer program which, when run on a processor, performs the surgical robot calibration method of any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211686989.7A CN115813556A (en) | 2022-12-27 | 2022-12-27 | Surgical robot calibration method and device, surgical robot and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211686989.7A CN115813556A (en) | 2022-12-27 | 2022-12-27 | Surgical robot calibration method and device, surgical robot and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115813556A true CN115813556A (en) | 2023-03-21 |
Family
ID=85518649
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211686989.7A Pending CN115813556A (en) | 2022-12-27 | 2022-12-27 | Surgical robot calibration method and device, surgical robot and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115813556A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117067222A (en) * | 2023-10-16 | 2023-11-17 | 苏州康多机器人有限公司 | Pose early warning method and device of surgical robot, surgical robot and medium |
-
2022
- 2022-12-27 CN CN202211686989.7A patent/CN115813556A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117067222A (en) * | 2023-10-16 | 2023-11-17 | 苏州康多机器人有限公司 | Pose early warning method and device of surgical robot, surgical robot and medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113532311B (en) | Point cloud splicing method, device, equipment and storage equipment | |
| CN106457562B (en) | Method and robot system for calibration machine people | |
| CN110919658B (en) | Robot calibration method based on vision and multi-coordinate system closed-loop conversion | |
| CN107598977B (en) | Method and system for realizing automatic robot teaching by using vision and laser range finder | |
| CN108489395A (en) | Vision measurement system structural parameters calibration and affine coordinate system construction method and system | |
| CN113601503B (en) | Hand-eye calibration method, device, computer equipment and storage medium | |
| WO2012127845A1 (en) | Robot control apparatus, robot control method, program, and recording medium | |
| CN106426172A (en) | Calibration method and system for industrial robot tool coordinate system | |
| CN110136204B (en) | Sound film dome assembly system based on calibration of machine tool position of bilateral telecentric lens camera | |
| CN109807937B (en) | Robot hand-eye calibration method based on natural scene | |
| JP7185860B2 (en) | Calibration method for a multi-axis movable vision system | |
| CN111390901A (en) | Automatic calibration method and calibration device for mechanical arm | |
| CN115187658B (en) | Multi-camera visual large target positioning method, system and equipment | |
| CN114918926B (en) | Mechanical arm visual registration method and device, control terminal and storage medium | |
| JP4960941B2 (en) | Camera calibration device for zoom lens-equipped camera of broadcast virtual studio, method and program thereof | |
| CN114310901B (en) | Coordinate system calibration method, device, system and medium for robot | |
| CN111915685B (en) | Zoom camera calibration method | |
| CN115813556A (en) | Surgical robot calibration method and device, surgical robot and storage medium | |
| CN114445506A (en) | Camera calibration processing method, device, equipment and storage medium | |
| CN112229323A (en) | Six-degree-of-freedom measurement method of checkerboard cooperative target based on monocular vision of mobile phone and application of six-degree-of-freedom measurement method | |
| CN113524201B (en) | Active adjusting method and device for pose of mechanical arm, mechanical arm and readable storage medium | |
| CN115439633A (en) | Calibration method and device and electronic equipment | |
| CN114918928B (en) | Method and device for accurately positioning surgical mechanical arm, control terminal and storage medium | |
| CN116394254A (en) | Zero calibration method and device for robot and computer storage medium | |
| CN108345463B (en) | Three-dimensional measuring method and device based on robot, robot and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |