CN115670660A - Calibration method, calibration device, surgical robot, electronic device and storage medium - Google Patents
Calibration method, calibration device, surgical robot, electronic device and storage medium Download PDFInfo
- Publication number
- CN115670660A CN115670660A CN202211385145.9A CN202211385145A CN115670660A CN 115670660 A CN115670660 A CN 115670660A CN 202211385145 A CN202211385145 A CN 202211385145A CN 115670660 A CN115670660 A CN 115670660A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- mechanical arm
- coordinate
- coordinate value
- unit vector
- 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
- 238000000034 method Methods 0.000 title claims abstract description 85
- 239000013598 vector Substances 0.000 claims description 174
- 239000011159 matrix material Substances 0.000 claims description 97
- 238000006243 chemical reaction Methods 0.000 claims description 91
- 230000009466 transformation Effects 0.000 claims description 50
- 238000004590 computer program Methods 0.000 claims description 18
- 238000013519 translation Methods 0.000 claims description 7
- 238000009434 installation Methods 0.000 claims description 5
- 230000008569 process Effects 0.000 description 14
- 238000010586 diagram Methods 0.000 description 10
- 238000004891 communication Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 5
- 238000013461 design Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 4
- 239000000700 radioactive tracer Substances 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000012937 correction Methods 0.000 description 2
- 230000014509 gene expression Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000000399 orthopedic effect Effects 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
Abstract
The embodiment of the invention provides a calibration method, a calibration device, a robot, electronic equipment and a storage medium, belonging to the field of surgical robots.
Description
Technical Field
The invention relates to the field of surgical robots, in particular to a calibration method, a calibration device, a robot, electronic equipment 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 as follows: the optical positioning instrument tracks a Tool tracer installed at the tail end of the mechanical arm, and obtains the pose of the tail end of the mechanical arm through the transformation relation between the Tool tracer and a Tool Center Point (TCP) coordinate system at the tail end of the mechanical arm, so that the mechanical arm is guided to move to the planned target pose.
The navigation and positioning of the surgical robot are completed, the transformation relation between the tool tracer and the TCP coordinate system at the tail end of the mechanical arm needs to be obtained in advance, and the process is called hand-eye calibration. However, the conventional hand-eye calibration method is very easy to cause large errors due to the fact that the acquisition equipment is shielded in the data acquisition process.
Disclosure of Invention
In view of the above, the present invention provides a calibration method, a calibration apparatus, a robot, an electronic device, and a storage medium, which can reduce calibration errors and solve the problem of large errors of the conventional calibration method.
In order to achieve the above purpose, the embodiment of the present invention adopts the following technical solutions:
in a first aspect, an embodiment of the present invention provides a calibration method for calibrating a surgical robot, where the surgical robot includes a mechanical arm, a mechanical arm intrinsic base, and a positioning camera, the method includes:
a reference array is arranged at the tail end of the mechanical arm; the reference array comprises a bracket arranged at the tail end of the mechanical arm and four positioning pieces arranged on the bracket, wherein the four positioning pieces are asymmetrically arranged;
establishing a reference array coordinate system by taking any three positioning pieces as reference;
controlling the tail end of the mechanical arm to be adjusted to O 1 A pose, acquiring a first unit vector and a second unit vector under the mechanical arm intrinsic TCP coordinate system, and recording a first coordinate value of an original point of the mechanical arm intrinsic TCP coordinate system under the mechanical arm intrinsic base coordinate systemA first conversion matrix from the mechanical arm intrinsic base coordinate system to the mechanical arm intrinsic TCP coordinate system, a conversion coordinate value of an origin of the reference array coordinate system under a positioning camera coordinate system, and a second conversion matrix from the positioning camera coordinate system to the reference array coordinate system; wherein the first unit vector and the second unit vector are mutually non-perpendicular;
keeping the original point of the mechanical arm intrinsic TCP coordinate system fixed under the mechanical arm intrinsic base coordinate system, controlling the tail end of the mechanical arm to rotate twice continuously by taking the first unit vector as an axis, taking the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center and a selected rotation method and a first angle, so that the tail end of the mechanical arm is adjusted to O 2 Pose and O 3 Position and pose respectively at O 2 Position and attitude in O 3 Under the position and the pose, recording a second coordinate value and a third coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system;
controlling the tail end of the mechanical arm to adjust back to O 1 And (3) keeping the original point of the mechanical arm intrinsic TCP coordinate system fixed under the mechanical arm intrinsic base coordinate system, controlling the tail end of the mechanical arm to rotate twice continuously by taking the second unit vector as an axis, taking the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center and a selected rotation method and a second angle, so that the tail end of the mechanical arm is sequentially adjusted to O 4 Position and attitude of O 5 Position and pose respectively at O 4 Position and attitude of O 5 Under the pose, recording a fourth coordinate value and a fifth coordinate value of the origin of the reference array coordinate system under the coordinate system of the positioning camera;
establishing a space rectangular coordinate system based on the first coordinate value, the first unit vector and the second unit vector, and acquiring the tail end of the mechanical arm as O 1 A third transformation matrix from the mechanical arm intrinsic TCP coordinate system to the space rectangular coordinate system in the pose;
calculating a fourth transformation matrix from the positioning camera coordinate system to the space rectangular coordinate system based on the first unit vector, the second unit vector, the transformed coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value;
and calculating a conversion matrix from the coordinate system of the positioning camera to the coordinate system of the mechanical arm intrinsic base and a conversion matrix from the mechanical arm intrinsic TCP coordinate system to the coordinate system of the reference array according to the first conversion matrix, the second conversion matrix, the third conversion matrix and the fourth conversion matrix.
Further, the step of calculating a fourth transformation matrix from the positioning camera coordinate system to the spatial rectangular coordinate system based on the first unit vector, the second unit vector, the transformed coordinate values, the second coordinate values, the third coordinate values, the fourth coordinate values, and the fifth coordinate values includes:
calculating the representation mode of the triaxial direction vector of the space rectangular coordinate system in the positioning camera coordinate system based on the first unit vector and the second unit vector;
fitting a fitting spherical surface formed by the transformation coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value to obtain a sphere center coordinate p of the fitting spherical surface c ；
And obtaining a fourth conversion matrix from the positioning camera coordinate system to the space rectangular coordinate system according to the representation mode of the three-axis direction vector of the space rectangular coordinate system under the positioning camera coordinate and the sphere center coordinate.
Further, the step of calculating, based on the first unit vector and the second unit vector, a representation manner of a triaxial direction vector of the spatial rectangular coordinate system in the positioning camera coordinate system includes:
converting the first unit vector into a representation in the coordinate system of the positioning camera to obtain a first conversion vector;
converting the second unit vector into a representation in the coordinate system of the positioning camera to obtain a second conversion vector;
and calculating the representation mode of the triaxial direction vector of the space rectangular coordinate system in the positioning camera coordinate system according to the first conversion vector and the second conversion vector.
Further, the step of fitting a fitting sphere formed by the transformed coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value, and the fifth coordinate value includes:
and performing spherical fitting on the transformed coordinate values, the second coordinate values, the third coordinate values, the fourth coordinate values and the fifth coordinate values by using a least square method to obtain a fitted sphere.
Further, the step of establishing a spatial rectangular coordinate system based on the first coordinate value, the first unit vector, and the second unit vector includes:
cross-multiplying the first unit vector by the second unit vector to obtain a cross-multiplied vector;
and establishing a space rectangular coordinate system by taking the first coordinate value as an origin, the first unit vector as an x axis and the cross multiplication vector as a y axis.
Further, the first conversion vector is:
a′ 1 ＝s 1 ×s 2 /|s 1 ×s 2 |
wherein s is 1 ＝p 2 -p′ 1 ，s 2 ＝p 3 -p 2 ，a′ 1 Denotes a first conversion vector, p' 1 Representing transformed coordinate values, p 2 Representing a second coordinate value, p 3 Representing a third coordinate value;
the second translation vector is:
a′ 2 ＝s′ 1 ×s′ 2 /|s′ 1 ×s′ 2 |
wherein, s' 1 ＝p 4 -p′ 1 ，s′ 2 ＝p 5 -p 4 ，a′ 2 Denotes a second conversion vector, p' 1 Representing transformed coordinate values, p 4 Represents a fourth coordinate value, p 5 Denotes the firstAnd five coordinate values.
In a second aspect, an embodiment of the present invention provides a calibration apparatus for calibrating a surgical robot, where the surgical robot includes a mechanical arm, a mechanical arm intrinsic base, and a positioning camera, and the apparatus includes an installation module, a first calibration module, and a second calibration module;
the mounting module is used for mounting a reference array at the tail end of the mechanical arm; the reference array comprises a bracket arranged at the tail end of the mechanical arm and four positioning pieces arranged on the bracket, wherein the four positioning pieces are asymmetrically arranged;
the first calibration module is used for establishing a reference array coordinate system by taking any three positioning parts as a reference;
the first calibration module is further used for controlling the tail end of the mechanical arm to be adjusted to O 1 A pose acquiring a first unit vector and a second unit vector under the mechanical arm intrinsic TCP coordinate system, and recording a first coordinate value of an origin of the mechanical arm intrinsic TCP coordinate system under the mechanical arm intrinsic base coordinate system, a first conversion matrix from the mechanical arm intrinsic base coordinate system to the mechanical arm intrinsic TCP coordinate system, a conversion coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system, and a second conversion matrix from the positioning camera coordinate system to the reference array coordinate system; wherein the first unit vector and the second unit vector are not perpendicular to each other;
the first calibration module is further configured to keep the original point of the intrinsic TCP coordinate system of the mechanical arm fixed under the intrinsic base coordinate system of the mechanical arm, and control the end of the mechanical arm to rotate twice continuously with the first unit vector as an axis, the original point of the intrinsic TCP coordinate system of the mechanical arm as a rotation center, and the selected rotation method and the first angle, so that the end of the mechanical arm is adjusted to O 2 Pose and O 3 Position and pose respectively at O 2 Position and orientation in O 3 Under the pose, recording a second coordinate value and a third coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system;
the first calibration module is also usedAdjusting back to O at the end of the mechanical arm 1 And (3) keeping the original point of the mechanical arm intrinsic TCP coordinate system fixed under the mechanical arm intrinsic base coordinate system, controlling the tail end of the mechanical arm to rotate twice continuously by taking the second unit vector as an axis, taking the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center and a selected rotation method and a second angle, so that the tail end of the mechanical arm is sequentially adjusted to O 4 Pose and O 5 Position and pose respectively at O 4 Pose and O 5 Under the pose, recording a fourth coordinate value and a fifth coordinate value of the origin of the reference array coordinate system under the coordinate system of the positioning camera;
the second calibration module is configured to establish a rectangular spatial coordinate system based on the first coordinate value, the first unit vector, and the second unit vector, and obtain that the end of the mechanical arm is O 1 A third transformation matrix from the mechanical arm intrinsic TCP coordinate system to the space rectangular coordinate system in the pose;
the second calibration module is configured to calculate a fourth transformation matrix from the positioning camera coordinate system to the spatial rectangular coordinate system based on the first unit vector, the second unit vector, the transformed coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value, and the fifth coordinate value;
the second calibration module is configured to calculate a transformation matrix from the positioning camera coordinate system to the mechanical arm intrinsic base coordinate system and a transformation matrix from the mechanical arm intrinsic TCP coordinate system to the reference array coordinate system according to the first transformation matrix, the second transformation matrix, the third transformation matrix and the fourth transformation matrix.
In a third aspect, an embodiment of the present invention provides a surgical robot, including a mechanical arm, a mechanical arm intrinsic base, a positioning camera, and a calibration device, where the calibration device is used to implement the calibration method according to the first aspect.
In a fourth aspect, an embodiment of the present invention provides an electronic device, which includes a processor and a memory, where the memory stores a computer program that can be executed by the processor, and the processor can execute the computer program to implement the calibration method according to the first aspect.
In a fifth aspect, an embodiment of the present invention provides a storage medium, on which a computer program is stored, where the computer program, when executed by a processor, implements the calibration method according to the first aspect.
According to the calibration method, the calibration device, the robot, the electronic device and the storage medium provided by the embodiment of the invention, under the condition that the original point of the TCP coordinate system of the mechanical arm is kept fixed under the coordinate system of the base intrinsic to the mechanical arm, the pose of the tail end of the mechanical arm is controlled to be adjusted to O 1 And adjusting the pose of the tail end of the mechanical arm based on the selected first unit vector and the second unit vector 2 Position and attitude of O 3 Position and attitude of O 4 Position and attitude of O 5 In the process of adjusting the pose at the tail end of the mechanical arm, the position of the origin of the TCP coordinate system intrinsic to the mechanical arm is kept unchanged all the time, the movement of the pose at the tail end of the mechanical arm can be limited to a small range to a certain extent, the probability that the positioning camera is shielded is greatly reduced, meanwhile, the first unit vector and the second unit vector can be flexibly selected, the probability that the positioning camera is shielded due to interference of the mechanical arm in a narrow space is further reduced, various data can be accurately acquired, the calibration accuracy can be improved, and errors can be reduced.
In order to make the aforementioned and other objects, features and advantages of the present invention comprehensible, preferred embodiments accompanied with figures are described in detail below.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required in the embodiments will be briefly described below, 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, and those skilled in the art can also obtain other related drawings based on the drawings without inventive efforts.
Fig. 1 is a block diagram illustrating a calibration system according to an embodiment of the present invention.
Fig. 2 shows one of the schematic structural diagrams of the surgical robot provided by the embodiment of the invention.
Fig. 3 shows a second schematic structural diagram of the surgical robot according to the embodiment of the present invention.
Fig. 4 is a block diagram illustrating a calibration apparatus according to an embodiment of the present invention.
Fig. 5 is a block diagram of an electronic device according to an embodiment of the present invention.
Reference numerals: 100-a calibration system; 110-a surgical robot; 120-a controller; 130-mechanical arm intrinsic base; 140-a robotic arm; 150-end of arm; 160-positioning camera; 170-a reference array; 180-a support; 190-positioning parts; 200-a calibration device; 210-mounting the module; 220-a first calibration module; 230-a second calibration module; 240-electronic device.
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.
It is noted that relational terms such as "first" and "second," and the like, may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrases "comprising one of 8230; \8230;" 8230; "does not exclude the presence of additional like elements in a process, method, article, or apparatus that comprises the element.
The navigation and positioning of the surgical robot are completed by obtaining the transformation relation between the tool tracer and the terminal coordinate system of the mechanical arm in advance, and the process is called hand-eye calibration.
The conventional hand-eye calibration method includes the following modes:
(1) The conversion relation between the reference array at the tail end of the mechanical arm and the coordinate system of the intrinsic TCP at the tail end of the mechanical arm can be measured in a hardware design drawing, the measured value is a design value, and the hand-eye calibration of the mechanical arm base and the positioning camera is carried out by using the value. Because the design value has an error with the actual value of the real object, the error is brought into the hand-eye calibration of the mechanical arm base and the positioning camera, and the result of the hand-eye calibration is used for correcting the coordinate system conversion relation of the end tool, the correction has a local type, and the correction is not applicable when the tail end posture of the mechanical arm is greatly changed.
(2) The method comprises the steps of firstly obtaining the rotation relation between a coordinate system of a positioning camera and a coordinate system of an intrinsic base of a mechanical arm, then fitting a spherical surface and a sphere center thereof in a mode of keeping an original point of the intrinsic TCP coordinate system of the mechanical arm still and rotating a reference array at the tail end of the mechanical arm, calculating the translation amount in the coordinate system conversion relation of a tail end tool by using the position of the sphere center, and obtaining the complete coordinate system conversion relation of the tail end tool by combining the previous rotation relation. According to the hand-eye calibration method, the tail end of the mechanical arm rotates around the tail end of the intrinsic TCP, so that a reference coordinate system is easily shielded, the reference array is difficult to capture by a positioning camera, various acquired data are wrong, and further calibration errors are large.
(3) In order to improve the "nine-point calibration method", the end of the robot needs to go 9 times, and the coverage space needs to be large, so as to ensure the global applicability of the calibration result. However, walking 9 times is cumbersome and time consuming, and the large range of movement is limited to operating rooms with small spaces.
Based on the above consideration, the embodiments of the present invention provide a calibration method, which can be completed in a narrow space, and can reduce the calibration error, thereby improving the problem of large error in the existing calibration method.
The calibration method provided by the embodiment of the present invention can be applied to the calibration system 100 shown in fig. 1, the calibration system 100 includes a surgical robot 110 and a controller 120, the surgical robot 110 can include a robot arm 140, a robot arm intrinsic base 130 and a positioning camera 160 shown in fig. 2, the robot arm 140 is disposed on the robot arm intrinsic base 130, the positioning camera 160 is disposed toward the robot arm 140, and the positioning camera 160 can be in communication connection with the controller 120 in a wired or wireless manner.
And a controller 120 for controlling the posture and movement of the robot arm 140.
The positioning camera 160 is used to position the robotic arm 140.
In a possible implementation manner, the embodiment of the present invention provides a calibration method for calibrating a surgical robot 110, where the surgical robot 110 may be the surgical robot 110 as shown in the figure, and the calibration method may include the following steps. In the present embodiment, the calibration method is applied to the controller 120 in fig. 1 for illustration.
S1, a reference array is arranged at the tail end of the mechanical arm.
In one possible embodiment, referring to FIG. 3, the reference array 170 may include a support frame 180 disposed at the end 150 of the robotic arm, and four positioning members 190 disposed on the support frame 180, the four positioning members 190 being asymmetrically disposed.
To enable the positioning camera 160 to more clearly acquire coordinate information of the reference array 170, the positioning member 190 may be provided as a reflective sheet.
And S2, establishing a reference array coordinate system by taking any three positioning pieces as reference.
S3, controlling the tail end of the mechanical arm to be adjusted to O 1 And (3) pose selecting a first unit vector and a second unit vector under the mechanical arm intrinsic TCP coordinate system, recording a first coordinate value of the origin of the mechanical arm intrinsic TCP coordinate system under the mechanical arm intrinsic base coordinate system, a first conversion matrix from the mechanical arm intrinsic base coordinate system to the mechanical arm intrinsic TCP coordinate system, a conversion coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system, and a second conversion matrix from the positioning camera coordinate system to the reference array coordinate system.
O 1 The pose can be flexibly selected according to the actual space conditions, that is, if the surgical robot 110 is in the operating room, the operator can select any pose as the O according to the setting conditions of the operating room (avoiding interference with other instruments and avoiding shielding by the positioning camera 160) 1 The position and orientation are not particularly limited in the present embodiment.
It is emphasized that the first unit vector and the second unit vector are not perpendicular to each other. And, the directions of the first unit vector and the second unit vector can be arbitrarily selected according to the actual situation in the calibration process. In the present embodiment, the directions of the first unit vector and the second unit vector are not particularly limited.
S4, keeping the original point of the mechanical arm intrinsic TCP coordinate system fixed under the mechanical arm intrinsic base coordinate system, controlling the tail end of the mechanical arm to rotate twice continuously by taking the first unit vector as an axis, taking the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center and a selected rotation method and a first angle, and enabling the tail end of the mechanical arm to be adjusted to O 2 Pose and O 3 Position and pose respectively at O 2 Position and attitude in O 3 Under the posture of the hand-held device,and recording a second coordinate value and a third coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system.
At O 2 Under the position and the pose, recording a second coordinate value of the origin of the reference array coordinate system under the coordinate system of the positioning camera at O 3 And recording a third coordinate value of the origin of the reference array coordinate system in the positioning camera coordinate system under the pose.
S5, controlling the tail end of the mechanical arm to adjust back to O 1 The pose is kept fixed under the coordinate system of the base of the mechanical arm by the original point of the TCP coordinate system of the mechanical arm, the tail end of the mechanical arm is controlled to rotate twice continuously by taking the second unit vector as an axis, the original point of the TCP coordinate system of the mechanical arm as a rotation center and a selected rotation method and a second angle, so that the tail end of the mechanical arm is adjusted to O 4 Pose and O 5 Position and pose respectively at O 4 Position and attitude of O 5 And under the position and the pose, recording a fourth coordinate value and a fifth coordinate value of the origin of the reference array coordinate system under the coordinate system of the positioning camera.
At O 4 In the pose, the fourth coordinate value of the origin of the reference array 170 coordinate system in the positioning camera 160 coordinate system is recorded, at O 5 In the pose, a fifth coordinate value of the origin of the reference array 170 coordinate system in the positioning camera 160 coordinate system is recorded.
S6, establishing a space rectangular coordinate system based on the first coordinate value, the first unit vector and the second unit vector, and obtaining the tail end O of the mechanical arm 1 And the mechanical arm at the pose is a third transformation matrix from the TCP coordinate system to a space rectangular coordinate system.
And S7, calculating a fourth conversion matrix from the positioning camera coordinate system to the space rectangular coordinate system based on the first unit vector, the second unit vector, the transformation coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value.
And S8, calculating a conversion matrix from the coordinate system of the positioning camera to the coordinate system of the mechanical arm intrinsic base and a conversion matrix from the mechanical arm intrinsic TCP coordinate system to the reference array coordinate system according to the first conversion matrix, the second conversion matrix, the third conversion matrix and the fourth conversion matrix.
It is emphasized that during steps S3-S5, the reference array 170 of the robotic arm tip 150 must be entirely visible to the positioning camera 160.
In the calibration process, the positioning camera 160 may position the mechanical arm 140 and the reference array 170 based on its own positioning function, so as to obtain a first coordinate value, a transformed coordinate value, a first transformation matrix, a second coordinate value, a third coordinate value, a fourth coordinate value, and a fifth coordinate value. And then transmitting the first coordinate value, the transformed coordinate value, the first transformation matrix, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value to the controller 120, and completing the calibration by the controller 120 by adopting the method of the steps S6-S8.
Compared with the traditional hand-eye calibration method of the surgical robot, in the calibration method, in the process of acquiring various data by adjusting the pose at the tail end of the mechanical arm, the original point position of the mechanical arm intrinsic TCP coordinate system is always kept unchanged, the movement of the pose at the tail end of the mechanical arm can be limited to a small range to a certain extent, the probability that the positioning camera is shielded is greatly reduced, meanwhile, the first unit vector and the second unit vector can be flexibly selected, the probability that the positioning camera is shielded due to interference of the mechanical arm in a narrow space is further reduced, and various data can be more accurately acquired, so that the calibration accuracy can be improved, and errors are reduced.
To avoid a large pose range of the robot arm tip 150 to some extent to obscure the reference array 170, the positioning camera 160 cannot capture the reference matrix. Thus, in practice, the first and second angles may be adjusted based on the visibility of the reference array 170 under the positioning camera 160.
In one possible embodiment, in particular, the first angle and the second angle may meet the following definition: 10 ° < m <180 °,10 ° < n <180 °, wherein m denotes a first angle and n denotes a second angle.
Further, the rotation method in steps S4 and S5 may be selected according to actual conditions, and may be, for example, a right-handed screw method or a left-handed screw method, and the present embodiment is not particularly limited. It should be emphasized, however, that the rotation method in steps S4 and S5 must be identical.
In the present embodiment, the mechanical arm intrinsic TCP coordinate system is represented by F tcp F for the mechanical arm intrinsic base coordinate system base Representing, positioning camera coordinate systems by F camera Expressed by reference to the array coordinate system F drf Express, mechanical arm intrinsic TCP coordinate system F tcp The origin of the mechanical arm is in a mechanical arm intrinsic base coordinate system F base First coordinate value of 1 To represent, reference array coordinate system F drf Is in the positioning camera coordinate system F camera P 'for conversion coordinate value of' 1 Expressing, mechanical arm intrinsic base coordinate system F base To mechanical arm intrinsic TCP coordinate system F tcp T for the first conversion matrix bTot Representing, positioning camera coordinate system F camera To the reference array coordinate system F drf For the second conversion matrix of cTod And (4) showing.
Mechanical arm intrinsic TCP coordinate system F tcp First unit vector of 1 Expressed as a, the second unit vector is represented by 2 And (4) showing. The first angle is denoted m, with reference to the array coordinate system F drf In a positioning camera coordinate system F camera Second coordinate value of 2 Identification, third coordinate value by p 3 And (4) showing. A second angle denoted by n, with reference to an array coordinate system F drf P for the fourth coordinate value of the origin in the coordinate system of the positioning camera 4 The fifth coordinate value is p 5 And (4) showing.
Space rectangular coordinate system F rot Express, mechanical arm intrinsic TCP coordinate system F tcp To space rectangular coordinate system F rot For the third conversion matrix T tTor Representing, positioning camera coordinate system F camera To space rectangular coordinate system F rot T for the fourth conversion matrix cTor Representing, positioning camera coordinate system F camera To the mechanical arm intrinsic base coordinate system F base T for conversion matrix cTob Express, mechanical arm intrinsic TCP coordinate system F tcp To the reference array coordinate system F drf T for conversion matrix tTod And (4) showing.
Further, with respect to step S6, establishing a spatial rectangular coordinate system may be achieved by the following steps.
And S61, cross-multiplying the first unit vector by the second unit vector to obtain a cross-multiplied vector.
And S62, establishing a space rectangular coordinate system by taking the first coordinate value as an origin, the first unit vector as an x axis and the cross multiplication vector as a y axis.
It should be understood that after the x-axis and the y-axis are determined, the z-axis of the spatial rectangular coordinate system can be determined.
Rectangular space coordinate system F rot The calculation formula of the direction vectors of the x-axis, the y-axis and the z-axis of (1) can be expressed as:
further, obtaining and establishing a space rectangular coordinate system F rot Thereafter, based on the principle of conversion between coordinate systems, the end 150 of the robot arm is O 1 In the pose, the mechanical arm 140 is internally provided with a third transformation matrix T from the TCP coordinate system to the space rectangular coordinate system tTor Can be expressed in the following form:
in a possible embodiment, step S7 may be further implemented as the following step.
And S71, calculating the representation mode of the triaxial direction vector of the space rectangular coordinate system under the coordinate of the positioning camera based on the first unit vector and the second unit vector.
And S72, fitting a fitting spherical surface formed by the transformed coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value to obtain a spherical center coordinate pc of the fitting spherical surface.
And S73, obtaining a fourth conversion matrix from the positioning camera coordinate system to the space rectangular coordinate system according to the representation mode of the three-axis direction vector of the space rectangular coordinate system under the positioning camera coordinate and the spherical center coordinate.
Further, in a possible embodiment, step S71 may be implemented by the following steps.
S711 converts the first unit vector into a representation in the coordinate system of the positioning camera to obtain a first converted vector.
S712, the second unit vector is converted into a representation in the positioning camera coordinate system, so as to obtain a second conversion vector.
And S713, calculating the expression mode of the triaxial direction vector of the space rectangular coordinate system under the coordinate of the positioning camera according to the first conversion vector and the second conversion vector.
The first amount of commutation can be defined by two vectors s perpendicular to it 1 And s 2 To represent, the vector s 1 And s 2 It can be understood that the camera coordinate system F is positioned camera The next two directional vectors. Similarly, the second conversion vector may be represented by two vectors s 'perpendicular thereto' 1 And s' 2 Is represented by vector s' 1 And s' 2 It can be understood that the camera coordinate system F is positioned camera The next two directional vectors.
In one possible embodiment, the first amount of commutation may be expressed as: a' 1 ＝s 1 ×s 2 /|s 1 ×s 2 L, wherein s 1 ＝p 2 -p′ 1 ，s 2 ＝p 3 -p 2 ，a′ 1 Representing a first translation vector.
The second translation vector may be represented as: a' 2 ＝s′ 1 ×s′ 2 /|s′ 1 ×s′ 2 L, wherein s' 1 ＝p 4 -p′ 1 ，s′ 2 ＝p 5 -p 4 ，a′ 2 Representing a second translation vector.
Based on a first conversion vector a' 1 And a second conversion vector a' 2 Rectangular coordinate system of space F rot The three-axis direction vector of the camera seat in the positioningMarker system F camera The following expressions may be used:
for step S72, in a possible implementation, a sphere fitting may be performed on the transformed coordinate values, the second coordinate values, the third coordinate values, the fourth coordinate values, and the fifth coordinate values by using a least square method to obtain a fitted sphere.
At this time, the sphere center coordinates of the fitting sphere can be expressed as:
for step S73, in one possible embodiment, the camera coordinate system F is located camera To space rectangular coordinate system F rot Fourth transformation matrix T cTor Can be expressed in the following form:
for step S8, in one possible implementation, the transformation matrix from the coordinate system of the positioning camera 160 to the coordinate system of the robot arm intrinsic base 130 and the transformation matrix from the coordinate system of the robot arm 140 intrinsic TCP to the coordinate system of the reference array 170 can be obtained according to the following transformation formulas.
The conversion formula is:
T cTob ＝T cTor ·(T tTor ) -1 ·(T bTot ) -1 ，T tTod ＝(T bTot ) -1 ·(T cTob ) -1 ·T cTod wherein, T cTob A transformation matrix T representing the coordinate system of the positioning camera to the intrinsic base coordinate system of the robot arm tTod Representing mechanical arm intrinsic TCP coordinate system to reference array seatA normalized transformation matrix.
Substituting the first conversion matrix, the second conversion matrix, the third conversion matrix and the fourth conversion matrix into the conversion formula to obtain a conversion matrix T from the coordinate system of the positioning camera to the coordinate system of the intrinsic base of the mechanical arm cTob And a transformation matrix T from the intrinsic TCP coordinate system of the mechanical arm to the reference array coordinate system tTod 。
According to the calibration method provided by the embodiment of the invention, four times of pose adjustment are carried out on the tail end of the mechanical arm, and based on various data collected in the pose adjustment process, a transformation matrix from a positioning camera coordinate system to a mechanical arm intrinsic base coordinate system and a transformation matrix from a mechanical arm intrinsic TCP coordinate system to a reference array coordinate system are calculated, and the two transformation matrices are global solutions, so that the problem that errors exist in the obtained transformation matrix due to the use of design parameters does not exist, additional calibration is not needed after calibration, and the calibration accuracy is higher. In addition, in the calibration process, the original point position of the mechanical arm intrinsic TCP coordinate system is always kept unchanged, namely, the movement of the pose of the tail end of the mechanical arm in the whole calibration process is limited within a small range, the space utilization rate is improved, the possibility that the positioning camera is shielded is reduced, and the calibration error can be reduced.
Based on the inventive concept of the above calibration method, in a possible implementation manner, referring to fig. 4, an embodiment of the present invention further provides a calibration apparatus 200, where the calibration apparatus 200 may be applied to the controller 120 in fig. 1, and the calibration apparatus 200 may include a mounting module 210, a first calibration module 220, and a second calibration module 230.
And a mounting module 210 for mounting the reference array at the end of the robot arm. The reference array comprises a support arranged at the tail end of the mechanical arm and four positioning pieces arranged on the support, and the four positioning pieces are arranged asymmetrically.
The first calibration module 220 is configured to establish a reference array coordinate system based on any three positioning elements.
The first calibration module 220 is further configured to control the end of the robot arm to adjust to O 1 Pose obtaining the first unit vector of the mechanical arm in the intrinsic TCP coordinate systemAnd a second unit vector, and recording a first coordinate value of the origin of the mechanical arm intrinsic TCP coordinate system under the mechanical arm intrinsic base coordinate system, a first conversion matrix from the mechanical arm intrinsic base coordinate system to the mechanical arm intrinsic TCP coordinate system, a conversion coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system, and a second conversion matrix from the positioning camera coordinate system to the reference array coordinate system. Wherein the first unit vector and the second unit vector are not perpendicular to each other.
The first calibration module 220 is further configured to keep the original point of the TCP coordinate system intrinsic to the mechanical arm fixed under the coordinate system of the base intrinsic to the mechanical arm, and control the end of the mechanical arm to rotate twice continuously by using the first unit vector as an axis and the original point of the TCP coordinate system intrinsic to the mechanical arm as a rotation center and by using the selected rotation method and the first angle, so that the end of the mechanical arm is sequentially adjusted to O 2 Pose and O 3 Position and pose respectively at O 2 Position and orientation in O 3 And under the pose, recording a second coordinate value and a third coordinate value of the origin of the reference array coordinate system under the coordinate system of the positioning camera.
The first calibration module 220 is further configured to control the end of the robot arm to adjust back to O 1 The pose is kept fixed under the coordinate system of the base of the mechanical arm by the original point of the TCP coordinate system of the mechanical arm, the tail end of the mechanical arm is controlled to rotate twice continuously by taking the second unit vector as an axis, the original point of the TCP coordinate system of the mechanical arm as a rotation center and a selected rotation method and a second angle, so that the tail end of the mechanical arm is adjusted to O 4 Pose and O 5 Position and pose respectively at O 4 Position and attitude of O 5 And recording a fourth coordinate value and a fifth coordinate value of the origin of the reference array coordinate system in the positioning camera coordinate system under the pose.
A second calibration module 230, configured to establish a rectangular spatial coordinate system based on the first coordinate value, the first unit vector, and the second unit vector, and obtain that the end of the mechanical arm is O 1 And the mechanical arm at the position and the pose is a third transformation matrix from the TCP coordinate system to the space rectangular coordinate system.
The second calibration module 230 is configured to calculate a fourth transformation matrix from the positioning camera coordinate system to the spatial rectangular coordinate system based on the first unit vector, the second unit vector, the transformed coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value, and the fifth coordinate value.
The second calibration module 230 is configured to calculate a transformation matrix from the coordinate system of the positioning camera to the coordinate system of the intrinsic base of the mechanical arm and a transformation matrix from the coordinate system of the intrinsic TCP of the mechanical arm to the coordinate system of the reference array according to the first transformation matrix, the second transformation matrix, the third transformation matrix and the fourth transformation matrix.
In the calibration device 200, through the cooperation of the installation module 210, the first calibration module 220 and the second calibration module 230, in the process of acquiring various data by adjusting the pose at the tail end of the mechanical arm, the original point position of the TCP coordinate system inherent in the mechanical arm is always kept unchanged, the movement of the pose at the tail end of the mechanical arm can be limited to a small range to a certain extent, the probability that the positioning camera is shielded is greatly reduced, meanwhile, the first unit vector and the second unit vector can be flexibly selected, the probability that the positioning camera is shielded due to interference caused by the mechanical arm in a narrow space is further reduced, and thus various data can be acquired more accurately, the calibration accuracy can be improved, and errors are reduced.
For specific limitations of the calibration apparatus 200, reference may be made to the above limitations of the calibration method, which are not described herein again. The modules in the calibration apparatus 200 may be implemented in whole or in part by software, hardware, and a combination thereof. The modules may be embedded in a hardware form or may be independent of a processor in the electronic device, or may be stored in a memory of the electronic device in a software form, so that the processor calls and executes operations corresponding to the modules.
In a possible implementation manner, the embodiment of the present invention further provides a robot, which includes a mechanical arm 140, a mechanical arm intrinsic base 130, a positioning camera 160, and a calibration apparatus, which is used to implement the calibration method provided in the foregoing implementation manner.
In one embodiment, an electronic device 240 is provided, and the electronic device 240 may be a terminal, and the internal structure thereof may be as shown in fig. 5. The electronic device 240 includes a processor, memory, a communication interface, a display screen, and an input device connected by a system bus. Wherein the processor of the electronic device 240 is configured to provide computing and control capabilities. The memory of the electronic device 240 includes a nonvolatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operating system and the computer program to run on the non-volatile storage medium. The communication interface of the electronic device 240 is used for performing wired or wireless communication with an external terminal, and the wireless communication may be implemented through WIFI, an operator network, near Field Communication (NFC), or other technologies. The computer program, when executed by a processor, implements the calibration method as provided in the above embodiments.
The structure shown in fig. 5 is a block diagram of only a part of the structure related to the solution of the present invention, and does not constitute a limitation to the electronic device 240 to which the solution of the present invention is applied, and a specific electronic device 240 may include more or less components than those shown in fig. 5, or combine some components, or have a different arrangement of components.
In one embodiment, the calibration apparatus 200 provided by the present invention can be implemented in a form of a computer program, and the computer program can be run on the electronic device 240 shown in fig. 5. The memory of the electronic device 240 may store various program modules that constitute the calibration apparatus 200, such as the installation module 210, the first calibration module 220, and the second calibration module 230 shown in fig. 4. The program modules constitute computer programs that cause the processor to perform the steps of the calibration method described in this specification.
For example, the electronic device 240 shown in fig. 5 may execute step S1 through the installation module 210 in the calibration apparatus 200 shown in fig. 4. The electronic device 240 may perform steps S0-S5 via the first calibration module 220. The electronic device 240 may perform steps S16-S8 via the second calibration module 230.
In one embodiment, an electronic device 240 is provided that includes a memory storing a computer program and a processor that implements the computer program when the processor executes the computer programThe following steps: a reference array is arranged at the tail end of the mechanical arm; establishing a reference array coordinate system by taking any three positioning pieces as reference; controlling the tail end of the mechanical arm to be adjusted to O 1 Selecting a first unit vector and a second unit vector under a mechanical arm intrinsic TCP coordinate system, recording a first coordinate value of an origin of the mechanical arm intrinsic TCP coordinate system under the mechanical arm intrinsic base coordinate system, a first conversion matrix from the mechanical arm intrinsic base coordinate system to the mechanical arm intrinsic TCP coordinate system, a conversion coordinate value of the origin of a reference array coordinate system under a positioning camera coordinate system, and a second conversion matrix from the positioning camera coordinate system to the reference array coordinate system; keeping the original point of the mechanical arm intrinsic TCP coordinate system fixed under the mechanical arm intrinsic base coordinate system, controlling the tail end of the mechanical arm to rotate twice continuously by taking the first unit vector as an axis, the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center and a selected rotation method and a first angle, and sequentially adjusting the tail end of the mechanical arm to O 2 Pose and O 3 Position and pose respectively at O 2 Position and orientation in O 3 Under the pose, recording a second coordinate value and a third coordinate value of the origin of the reference array coordinate system under the coordinate system of the positioning camera; control the tail end of the mechanical arm to adjust back to O 1 The pose is kept fixed under the coordinate system of the base of the mechanical arm by the original point of the TCP coordinate system of the mechanical arm, the tail end of the mechanical arm is controlled to rotate twice continuously by taking the second unit vector as an axis, the original point of the TCP coordinate system of the mechanical arm as a rotation center and a selected rotation method and a second angle, so that the tail end of the mechanical arm is adjusted to O 4 Pose and O 5 Position and pose respectively at O 4 Pose and O 5 Under the pose, recording a fourth coordinate value and a fifth coordinate value of the origin of the reference array coordinate system under the coordinate system of the positioning camera; establishing a space rectangular coordinate system based on the first coordinate value, the first unit vector and the second unit vector, and acquiring that the tail end of the mechanical arm is O 1 A third transformation matrix from the mechanical arm intrinsic TCP coordinate system to a space rectangular coordinate system in the pose; calculating the coordinate system of the positioning camera to the space based on the first unit vector, the second unit vector, the transformation coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate valueAnd a fourth transformation matrix of the rectangular coordinate system.
In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored, which when executed by a processor performs the steps of: a reference array is arranged at the tail end of the mechanical arm; establishing a reference array coordinate system by taking any three positioning pieces as reference; controlling the tail end of the mechanical arm to be adjusted to O 1 The pose system comprises a pose position selecting device, a first unit vector and a second unit vector under a mechanical arm intrinsic TCP coordinate system, a first coordinate value of an original point of the mechanical arm intrinsic TCP coordinate system under the mechanical arm intrinsic base coordinate system, a first conversion matrix from the mechanical arm intrinsic base coordinate system to the mechanical arm intrinsic TCP coordinate system, a conversion coordinate value of the original point of a reference array coordinate system under a positioning camera coordinate system, and a second conversion matrix from the positioning camera coordinate system to the reference array coordinate system; keeping the original point of the mechanical arm intrinsic TCP coordinate system fixed under the mechanical arm intrinsic base coordinate system, controlling the tail end of the mechanical arm to continuously rotate twice by taking the first unit vector as an axis, taking the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center and taking a selected rotation method and a first angle, and sequentially adjusting the tail end of the mechanical arm to be O 2 Position and attitude of O 3 Position and pose respectively at O 2 Position and orientation in O 3 Under the pose, recording a second coordinate value and a third coordinate value of the origin of the reference array coordinate system under the coordinate system of the positioning camera; control the tail end of the mechanical arm to adjust back to O 1 Position and pose, the original point of the mechanical arm intrinsic TCP coordinate system is kept fixed under the mechanical arm intrinsic base coordinate system, the tail end of the mechanical arm is controlled to rotate twice continuously by taking the second unit vector as an axis, taking the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center and a selected rotation method and a second angle, and the tail end of the mechanical arm is adjusted to O 4 Position and attitude of O 5 Position and pose respectively at O 4 Pose and O 5 Under the pose, recording a fourth coordinate value and a fifth coordinate value of the origin of the reference array coordinate system under the coordinate system of the positioning camera; establishing a space rectangular coordinate system based on the first coordinate value, the first unit vector and the second unit vector, and obtaining the tail end O of the mechanical arm 1 Mechanical arm intrinsic TCP coordinate system in pose to spaceA third transformation matrix of the angular coordinate system; and calculating a fourth conversion matrix from the positioning camera coordinate system to the space rectangular coordinate system based on the first unit vector, the second unit vector, the transformation coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value.
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 some 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, the functional modules in the embodiments 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, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present invention or a part thereof which substantially contributes to the prior art may be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device) 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 various media capable of storing program codes.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (10)
1. A calibration method is characterized by being used for calibration of a surgical robot, wherein the surgical robot comprises a mechanical arm, a mechanical arm intrinsic base and a positioning camera, and the method comprises the following steps:
a reference array is arranged at the tail end of the mechanical arm; the reference array comprises a bracket arranged at the tail end of the mechanical arm and four positioning pieces arranged on the bracket, wherein the four positioning pieces are asymmetrically arranged;
establishing a reference array coordinate system by taking any three positioning pieces as reference;
controlling the tail end of the mechanical arm to be adjusted to O 1 A pose selecting a first unit vector and a second unit vector under the mechanical arm intrinsic TCP coordinate system, recording a first coordinate value of an original point of the mechanical arm intrinsic TCP coordinate system under the mechanical arm intrinsic base coordinate system, a first conversion matrix from the mechanical arm intrinsic base coordinate system to the mechanical arm intrinsic TCP coordinate system, a conversion coordinate value of the original point of the reference array coordinate system under the positioning camera coordinate system, and a second conversion matrix from the positioning camera coordinate system to the reference array coordinate system; wherein the first unit vector and the second unit vector are not perpendicular to each other;
keeping the original point of the mechanical arm intrinsic TCP coordinate system fixed under the mechanical arm intrinsic base coordinate system, controlling the tail end of the mechanical arm to rotate twice continuously by taking the first unit vector as an axis, taking the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center and a selected rotation method and a first angle, so that the tail end of the mechanical arm is adjusted to O 2 Pose and O 3 Position and pose respectively at O 2 Position and orientation in O 3 Under the pose, recording a second coordinate value and a third coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system;
controlling the tail end of the mechanical arm to adjust back to O 1 And (3) posture, keeping the original point of the intrinsic TCP coordinate system of the mechanical arm fixed under the intrinsic base coordinate system of the mechanical arm, controlling the tail end of the mechanical arm to take the second unit vector as an axis, taking the original point of the intrinsic TCP coordinate system of the mechanical arm as a rotation center, continuously rotating twice by a selected rotation method and a second angle, and sequentially adjusting the tail end of the mechanical arm to O 4 Pose and O 5 Position and pose respectively at O 4 Pose and O 5 Under the position and the pose, recording a fourth coordinate value and a fifth coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system;
establishing a space rectangular coordinate system based on the first coordinate value, the first unit vector and the second unit vector, and acquiring the tail end of the mechanical arm as O 1 A third transformation matrix from the mechanical arm intrinsic TCP coordinate system to the space rectangular coordinate system in the pose;
calculating a fourth transformation matrix from the positioning camera coordinate system to the space rectangular coordinate system based on the first unit vector, the second unit vector, the transformed coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value;
and calculating a conversion matrix from the coordinate system of the positioning camera to the coordinate system of the mechanical arm intrinsic base and a conversion matrix from the coordinate system of the mechanical arm intrinsic TCP to the coordinate system of the reference array according to the first conversion matrix, the second conversion matrix, the third conversion matrix and the fourth conversion matrix.
2. The calibration method according to claim 1, wherein the step of calculating a fourth transformation matrix from the positioning camera coordinate system to the spatial rectangular coordinate system based on the first unit vector, the second unit vector, the transformed coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value comprises:
calculating the representation mode of the triaxial direction vector of the space rectangular coordinate system under the coordinate system of the positioning camera based on the first unit vector and the second unit vector;
fitting a fitting spherical surface formed by the transformation coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value to obtain a spherical center coordinate p of the fitting spherical surface c ；
And obtaining a fourth conversion matrix from the positioning camera coordinate system to the space rectangular coordinate system according to the representation mode of the three-axis direction vector of the space rectangular coordinate system under the positioning camera coordinate and the sphere center coordinate.
3. The calibration method according to claim 2, wherein the step of calculating, based on the first unit vector and the second unit vector, a representation of the three-axis direction vector of the rectangular spatial coordinate system in the positioning camera coordinate system includes:
converting the first unit vector into a representation in the coordinate system of the positioning camera to obtain a first conversion vector;
converting the second unit vector into a representation in the coordinate system of the positioning camera to obtain a second conversion vector;
and calculating the representation mode of the triaxial direction vector of the space rectangular coordinate system in the positioning camera coordinate system according to the first conversion vector and the second conversion vector.
4. The calibration method according to claim 2, wherein the step of fitting a fitting sphere formed by the transformed coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value and the fifth coordinate value comprises:
and performing spherical fitting on the transformed coordinate values, the second coordinate values, the third coordinate values, the fourth coordinate values and the fifth coordinate values by using a least square method to obtain a fitted sphere.
5. The calibration method according to claim 1 or 2, wherein the step of establishing a spatial rectangular coordinate system based on the first coordinate value, the first unit vector and the second unit vector comprises:
cross-multiplying the first unit vector by the second unit vector to obtain a cross-multiplied vector;
and establishing a space rectangular coordinate system by taking the first coordinate value as an origin, the first unit vector as an x axis and the cross multiplication vector as a y axis.
6. A calibration method according to claim 3, wherein the first translation vector is:
a′ 1 ＝s 1 ×s 2 /|s 1 ×s 2 |
wherein s is 1 ＝p 2 -p′ 1 ，s 2 ＝p 3 -p 2 ，a′ 1 Denotes a first conversion vector, p' 1 Representing transformed coordinate values, p 2 Representing a second coordinate value, p 3 Representing a third coordinate value;
the second translation vector is:
a′ 2 ＝s′ 1 ×s′ 2 /|s′ 1 ×s′ 2 |
wherein, s' 1 ＝p 4 -p′ 1 ，s′ 2 ＝p 5 -p 4 ，a′ 2 Denotes a second conversion vector, p' 1 Representing transformed coordinate values, p 4 Representing a fourth coordinate value, p 5 Indicating a fifth coordinate value.
7. The calibration device is characterized by being used for calibrating a surgical robot, wherein the surgical robot comprises a mechanical arm, a mechanical arm intrinsic base and a positioning camera, and the device comprises an installation module, a first calibration module and a second calibration module;
the mounting module is used for mounting a reference array at the tail end of the mechanical arm; the reference array comprises a bracket arranged at the tail end of the mechanical arm and four positioning pieces arranged on the bracket, wherein the four positioning pieces are asymmetrically arranged;
the first calibration module is used for establishing a reference array coordinate system by taking any three positioning parts as a reference;
the first calibration module is further used for controlling the tail end of the mechanical arm to be adjusted to O 1 A pose acquiring a first unit vector and a second unit vector under the mechanical arm intrinsic TCP coordinate system, and recording a first coordinate value of an origin of the mechanical arm intrinsic TCP coordinate system under the mechanical arm intrinsic base coordinate system, a first conversion matrix from the mechanical arm intrinsic base coordinate system to the mechanical arm intrinsic TCP coordinate system, a conversion coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system, and a second conversion matrix from the positioning camera coordinate system to the reference array coordinate system; wherein the first unit vector and the second unit vector are not perpendicular to each other;
the first calibration module is further configured to keep the original point of the mechanical arm intrinsic TCP coordinate system fixed under the mechanical arm intrinsic base coordinate system, and control the end of the mechanical arm to rotate twice continuously with the first unit vector as an axis, the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center, and the selected rotation method and the first angle, so that the end of the mechanical arm is adjusted to O sequentially 2 Pose and O 3 Position and pose respectively at O 2 Position and attitude in O 3 Recording the reference array coordinate system under the poseA second coordinate value and a third coordinate value of the origin point under the positioning camera coordinate system;
the first calibration module is further used for controlling the tail end of the mechanical arm to adjust back to the O state 1 And (3) keeping the original point of the mechanical arm intrinsic TCP coordinate system fixed under the mechanical arm intrinsic base coordinate system, controlling the tail end of the mechanical arm to rotate twice continuously by taking the second unit vector as an axis, taking the original point of the mechanical arm intrinsic TCP coordinate system as a rotation center and a selected rotation method and a second angle, so that the tail end of the mechanical arm is sequentially adjusted to O 4 Position and attitude of O 5 Position and pose respectively at O 4 Pose and O 5 Under the position and the pose, recording a fourth coordinate value and a fifth coordinate value of the origin of the reference array coordinate system under the positioning camera coordinate system;
the second calibration module is configured to establish a rectangular spatial coordinate system based on the first coordinate value, the first unit vector, and the second unit vector, and obtain that the end of the mechanical arm is O 1 A third transformation matrix from the mechanical arm intrinsic TCP coordinate system to the space rectangular coordinate system in the pose;
the second calibration module is configured to calculate a fourth transformation matrix from the positioning camera coordinate system to the spatial rectangular coordinate system based on the first unit vector, the second unit vector, the transformed coordinate value, the second coordinate value, the third coordinate value, the fourth coordinate value, and the fifth coordinate value;
the second calibration module is used for calculating a conversion matrix from the coordinate system of the positioning camera to the coordinate system of the mechanical arm intrinsic base and a conversion matrix from the coordinate system of the mechanical arm intrinsic TCP to the coordinate system of the reference array according to the first conversion matrix, the second conversion matrix, the third conversion matrix and the fourth conversion matrix.
8. A surgical robot, characterized by comprising a mechanical arm, a mechanical arm intrinsic base, a positioning camera and a calibration device for implementing the calibration method as claimed in any one of claims 1 to 6.
9. An electronic device, comprising a processor and a memory, the memory storing a computer program executable by the processor, the processor being configured to execute the computer program to implement the calibration method as claimed in any one of claims 1 to 6.
10. A storage medium having stored thereon a computer program, characterized in that the computer program, when being executed by a processor, carries out the calibration method as defined in any one of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211385145.9A CN115670660A (en) | 2022-11-07 | 2022-11-07 | Calibration method, calibration device, surgical robot, electronic device and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211385145.9A CN115670660A (en) | 2022-11-07 | 2022-11-07 | Calibration method, calibration device, surgical robot, electronic device and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115670660A true CN115670660A (en) | 2023-02-03 |
Family
ID=85050725
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211385145.9A Pending CN115670660A (en) | 2022-11-07 | 2022-11-07 | Calibration method, calibration device, surgical robot, electronic device and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115670660A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117958983A (en) * | 2024-03-29 | 2024-05-03 | 北京爱康宜诚医疗器材有限公司 | Zero point calibration method and device of surgical robot, storage medium and electronic equipment |
-
2022
- 2022-11-07 CN CN202211385145.9A patent/CN115670660A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117958983A (en) * | 2024-03-29 | 2024-05-03 | 北京爱康宜诚医疗器材有限公司 | Zero point calibration method and device of surgical robot, storage medium and electronic equipment |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2021128787A1 (en) | Positioning method and apparatus | |
| JP4191080B2 (en) | Measuring device | |
| CN110640747B (en) | Hand-eye calibration method and system for robot, electronic equipment and storage medium | |
| CN113524201B (en) | Active adjusting method and device for pose of mechanical arm, mechanical arm and readable storage medium | |
| Eck et al. | Precise haptic device co-location for visuo-haptic augmented reality | |
| CN113211445B (en) | Robot parameter calibration method, device, equipment and storage medium | |
| CN110977980A (en) | Mechanical arm real-time hand-eye calibration method and system based on optical position indicator | |
| CN115670660A (en) | Calibration method, calibration device, surgical robot, electronic device and storage medium | |
| CN110871434A (en) | Kinematics calibration method of parallel processing equipment | |
| WO2019119441A1 (en) | Method for controlling pan-tilt by means of somatosensory controller, and pan-tilt, somatosensory controller and system | |
| CN112603542A (en) | Hand-eye calibration method and device, electronic equipment and storage medium | |
| CN115042184A (en) | Robot hand-eye coordinate conversion method and device, computer equipment and storage medium | |
| CN116019562A (en) | Robot control system and method | |
| WO2023165355A1 (en) | Surgical platform positioning system, and pose information determining method and device | |
| JP2902285B2 (en) | 3D position control system | |
| CN113768535B (en) | Method, system and device for self-calibrating gesture of ultrasonic profiling probe for teleoperation | |
| CN113384347B (en) | Robot calibration method, device, equipment and storage medium | |
| Eck et al. | Comprehensive workspace calibration for visuo-haptic augmented reality | |
| CN118061206B (en) | Robot TCP calibration device, method, computer equipment and storage medium | |
| CN114211493B (en) | Remote control system and method for mechanical arm | |
| CN112589806B (en) | Robot pose information determination method, device, equipment and storage medium | |
| Li et al. | In-field self-calibration of robotic manipulator using stereo camera: Application to humanitarian demining robot | |
| CN118081776B (en) | Robot TCP point calibration method, device, medium and equipment of multipath sensor | |
| CN114791284B (en) | Calibration method and device of electronic compass in robot and robot | |
| US12090669B2 (en) | Off-line simulation system |
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 |