CN109974584A - A calibration system and calibration method for an auxiliary laser osteotomy robot - Google Patents

Abstract

The invention discloses a calibration system and a calibration method for an auxiliary laser osteotomy robot. The translation vector of the coordinate system relative to the coordinate system at the end of the manipulator, establishes the transfer matrix of the tool coordinate system relative to the coordinate system at the end of the manipulator, and obtains the transfer matrix of the tool coordinate system relative to the base coordinate system of the robot, so as to realize the tool calibration of the robot; solve the tool The coordinate system is relative to the translation vector of the tracking device coordinate system, and two sets of three-dimensional point sets at the origin of the tool coordinate system are obtained, and the transfer matrix of the robot base coordinate system relative to the camera coordinate system is solved to realize the hand-eye calibration of the robot.

Description

A calibration system and calibration method for an auxiliary laser osteotomy robot

technical field

The present disclosure relates to the field of robot calibration, in particular to a calibration system and a calibration method for an auxiliary laser osteotomy robot.

Background technique

In recent years, surgical robotics technology has been more and more widely used in clinical practice. The assisted laser osteotomy robot is an autonomous surgical robot that can help doctors to automatically complete the osteotomy according to the preoperative planning during the operation. Compared with traditional osteotomy surgical instruments, it has the advantages of precise controllability and less damage to surrounding tissues. Tool calibration and hand-eye calibration are important steps before the installation and operation of the auxiliary osteotomy robot, which directly determine the positioning accuracy and surgical effect of the robot.

During the research and development process, the inventor found that the traditional calibration method mostly relies on advanced measurement equipment, the calibration process is complicated, the cost of the measurement equipment is expensive, and the measurement accuracy largely depends on the accuracy of the measurement equipment, making it difficult to effectively apply in practical operations. . However, the existing calibration methods that rely on themselves, such as the "four-point method" and "six-point method", have complicated operation processes and poor stability, which cannot meet the requirements of clinical operations. On the other hand, in the assisted laser osteotomy robot system, the calibration of the laser tool is the calibration of the dangling point in the space of the effective laser ablation point, which belongs to the non-contact tool calibration, which increases the difficulty of system calibration.

Contents of the invention

In order to overcome the above-mentioned deficiencies of the prior art, the present disclosure provides a calibration system and a calibration method for an auxiliary laser osteotomy robot, which can automatically and effectively realize the tool and hand-eye calibration of the auxiliary laser osteotomy robot, and improve the calibration accuracy.

The technical solution adopted in the present disclosure is:

A calibration method for an auxiliary laser osteotomy robot, the method comprising the following steps:

Establish the coordinate system of the robot, the end of the arm, the camera, the tracking device, and the tool at the end of the arm;

Obtain the transfer matrix of the coordinate system of the end of the robot arm relative to the base coordinate system of the robot and the transfer matrix of the coordinate system of the tracking device relative to the camera coordinate system when the laser emitted by the tool is vertically irradiated to each plane of the calibration body and passes through the center of the calibration body;

Solve the translation vector of the tool coordinate system relative to the end coordinate system of the manipulator, and establish the transfer matrix of the tool coordinate system relative to the end coordinate system of the manipulator;

Multiply the transfer matrix of the coordinate system of the robot arm end relative to the robot base coordinate system and the transfer matrix of the tool coordinate system relative to the robot arm end coordinate system to obtain the transfer matrix of the tool coordinate system relative to the robot base coordinate system, and realize the tool of the robot. calibration;

Solve the translation vector of the tool coordinate system relative to the tracking device coordinate system, obtain two sets of three-dimensional point sets at the origin of the tool coordinate system, and solve the transfer matrix of the robot base coordinate system relative to the camera coordinate system to realize the hand-eye calibration of the robot.

As a further technical solution of the present disclosure, the method for obtaining the transfer matrix of the coordinate system of the end of the robot arm relative to the base coordinate system of the robot is:

Measure the distance from the tool to the center of the calibration body when the laser emitted by the tool at the end of the robotic arm strikes the first plane of the calibration body vertically and passes through the center of the calibration body;

When the measured distance is equal to the sum of the effective distance of laser ablation and the calibrated external ball radius, obtain the position and attitude information of the tool center point at the end of the manipulator relative to the robot base coordinate system at this time, and form the end coordinate system {E} of the manipulator The first transfer matrix relative to the robot base coordinate system {B}

Repeat the above steps until the second transition matrix of the robot arm end coordinate system {E} relative to the robot base coordinate system {B} is obtained third transition matrix and the fourth transition matrix

As a further technical solution of the present disclosure, the method for obtaining the transition matrix of the tracking device coordinate system relative to the camera coordinate system is:

Measure the distance from the tool to the center of the calibration body when the laser emitted by the tool at the end of the robotic arm strikes the first plane of the calibration body vertically and passes through the center of the calibration body;

When the measured distance is equal to the sum of the effective distance of laser ablation and the radius of the circumscribed sphere of the calibration body, the position and attitude information of the tracking device coordinate system relative to the camera coordinate system is obtained, and the tracking device coordinate system {D} relative to the camera coordinate system{D} is formed. The first transition matrix of C}

Repeat the above steps until the second transition matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C} is obtained third transition matrix and the fourth transition matrix

As a further technical solution of the present disclosure, the solution method of the translation vector of the tool coordinate system relative to the end coordinate system of the mechanical arm is:

Establish the conversion relation of the tool coordinate system {T}, the robot end coordinate system {E}, and the robot base coordinate system {B};

The transfer matrix of the robot arm end coordinate system {E} relative to the robot base coordinate system {B} Substitute into the conversion relation obtained above to obtain the incompatible equation system;

Using the singular value decomposition method to solve the optimal least squares solution of the incompatible equation system, the translation vector of the tool coordinate system {T} relative to the robot end coordinate system {E} is obtained

Combined with the principle of invariance of the rotation matrix of the translation relationship between the tool coordinate system {T} and the end coordinate system of the manipulator {E}, the transfer matrix of the tool coordinate system {T} relative to the coordinate system of the end of the manipulator {E} is obtained

The transition matrix of the tool coordinate system {T} relative to the robot end coordinate system {E} The transfer matrix of the robot arm end coordinate system {E} relative to the robot base coordinate system {B} Multiply to get the transfer matrix of the tool coordinate system {T} relative to the robot base coordinate system {B}

According to the transfer matrix of the tool coordinate system {T} relative to the robot base coordinate system {B} Adjust the position and attitude information of the tool center point at the end of the robot arm relative to the robot base coordinate system to realize the tool calibration of the robot system.

As a further technical solution of the present disclosure, the method for solving the translation vector of the tool coordinate system relative to the tracking device coordinate system is:

Establish the conversion relation of the tool coordinate system {T}, the tracking device coordinate system {D}, and the camera coordinate system {C};

The transfer matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C} Substituting into the conversion relation obtained above, the incompatible equations are obtained;

Using the singular value decomposition method to solve the optimal least squares solution of the incompatible equation system, the translation vector of the tool coordinate system {T} relative to the tracking device coordinate system {D} is obtained

As a further technical solution of the present disclosure, the step of acquiring two sets of three-dimensional point sets at the origin of the tool coordinate system includes:

according to the translation vector of the tool coordinate system {T} relative to the tracking device coordinate system {D} Translate the origin of the tracking coordinate system to the origin of the tool coordinate system;

Obtain two sets of three-dimensional point sets with the origin of the tool coordinate system located at any position in space under the robot base coordinate system and the camera coordinate system.

As a further technical solution of the present disclosure, the step of solving the transition matrix of the robot base coordinate system relative to the camera coordinate system includes:

Define the function F between the rotation matrix of the robot base coordinate system and the camera coordinate system and the origin of the tool coordinate system between two sets of three-dimensional point sets under the robot base coordinate system and the camera coordinate system;

Use the least squares method based on singular value decomposition to solve the maximum value of the function F, and obtain the rotation matrix of the robot coordinate system relative to the camera coordinate system;

According to the transformation mapping relationship of the origin of the tool coordinate system from the robot base coordinate system to the camera coordinate system, calculate the translation component of the robot base coordinate system relative to the camera coordinate system;

Based on the rotation matrix of the robot base coordinate system relative to the camera coordinate system and the translation component of the robot base coordinate system relative to the camera coordinate system, the transfer matrix of the robot base coordinate system and the camera coordinate system is obtained;

According to the transfer matrix of the robot base coordinate system and the camera coordinate system, the tool installed at the end of the manipulator is controlled to move according to the set trajectory, and the pose information of the tool relative to the camera coordinate system is obtained at the same time to realize the hand-eye calibration of the system.

A calibration system for an auxiliary laser osteotomy robot, the system includes a robot system, a vision system, a calibration device and a processor;

The robotic system is used for manipulating the robotic arm so that the tool emits laser light vertically to each plane of the calibration device and passes through the center of the calibration body to obtain the transfer matrix of the coordinate system at the end of the robotic arm relative to the base coordinate system of the robot, and upload it to the processor ;

The vision system is used to obtain the transfer matrix of the coordinate system of the tracking device relative to the coordinate system of the camera when the laser emitted by the tool is vertically irradiated on each plane of the calibration body and passes through the center of the calibration device, and uploaded to the processor;

The processor includes a transfer matrix establishment module, a robot tool calibration module and a robot hand-eye calibration module; wherein:

The transfer matrix establishment module is used to judge whether the distance from the tool to the center of the outer ball of the calibration device satisfies the sum of the effective distance of laser ablation and the radius of the outer ball of the calibration device, and if so, obtain the coordinate system of the end of the robot arm relative to the base coordinate of the robot The transfer matrix of the system and the transfer matrix of the tracking device coordinate system relative to the camera coordinate system;

The robot tool calibration module is used to solve the translation vector of the tool coordinate system relative to the coordinate system of the end of the robot arm, and establish a transfer matrix of the tool coordinate system relative to the coordinate system of the end of the robot arm; the coordinate system of the end of the robot arm is relative to the base coordinate of the robot. The transfer matrix of the system is multiplied by the transfer matrix of the tool coordinate system relative to the coordinate system of the end of the robot arm, and the transfer matrix of the tool coordinate system relative to the robot base coordinate system is obtained to realize the tool calibration of the robot;

The robot hand-eye calibration module is used to solve the translation vector of the tool coordinate system relative to the tracking device coordinate system, obtain two sets of three-dimensional point sets of the origin of the tool coordinate system according to the translation vector of the tool coordinate system relative to the tracking device coordinate system, and solve The transfer matrix of the robot base coordinate system relative to the camera coordinate system realizes the hand-eye calibration of the robot.

As a further technical solution of the present disclosure, the calibration device is a regular tetrahedral marker body, a tracking device is provided at the four vertex corners of the regular tetrahedral marker body, and the center of the four planes of the regular tetrahedral marker body is provided with light-transmitting small holes.

Through the above technical solutions, the beneficial effects of the present disclosure are:

(1) In the present disclosure, combined with the system characteristics of the laser osteotomy robot, a calibration device is designed specifically for the effective laser ablation point, and at the same time, the calibration of the non-contact tool in the robot coordinate system and the camera coordinate system is realized, which can greatly The calibration accuracy of the robot is improved; and the calibration device is simple and easy to operate, which greatly reduces the cost;

(2) The calibration method proposed by the present disclosure overcomes the problems of complex operation process and poor stability of the traditional method, improves the automation degree of the calibration process, and reduces unnecessary errors such as manual operation;

(3) The present disclosure is applicable to a robot system for assisting laser osteotomy surgery, which can effectively and accurately realize tool calibration and hand-eye calibration, and improve the degree of automation of the robot.

Description of drawings

The accompanying drawings, which constitute a part of the present disclosure, are used to provide further understanding of the present disclosure, and the exemplary embodiments of the present disclosure and their descriptions are used to explain the present application and do not constitute an improper limitation of the present disclosure.

Fig. 1 is a structural diagram of a positive tetrahedral marker in Embodiment 1;

Fig. 2 is the flow chart of the calibration method of the assisted laser osteotomy robot in the second embodiment;

Fig. 3 is a schematic diagram of the structure of the three-coordinate system of the embodiment.

Detailed ways

The present disclosure will be further described below in conjunction with the accompanying drawings and embodiments.

It should be noted that the following detailed description is exemplary and intended to provide further explanation of the present disclosure. Unless defined otherwise, all technical and scientific terms used in this disclosure have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure belongs.

It should be noted that the terminology used here is only for describing specific implementations, and is not intended to limit the exemplary implementations according to the present application. As used herein, unless the context clearly dictates otherwise, the singular is intended to include the plural, and it should also be understood that when the terms "comprising" and/or "comprising" are used in this specification, they mean There are features, steps, operations, means, components and/or combinations thereof.

Glossary:

(1) TCP, Tool Center Point, tool center point.

Embodiment one

This embodiment provides a calibration system for an assisted laser osteotomy robot, the calibration device includes a robot system, a vision system, a calibration device and a processor, and the robot system adopts the existing technical structure of an assisted laser osteotomy robot system , which mainly includes a six-degree-of-freedom robot arm and a laser tool installed at the end of the robot arm. The vision system includes an infrared stereo camera, and the calibration device is a positive four-sided marker.

Please refer to FIG. 1 , the calibration device is a regular tetrahedral marker body, the four vertices of the regular tetrahedral marker body are respectively installed with tracking devices for infrared reflective balls, and the four planes of the regular tetrahedral marker body are respectively transparent It is made of material, and the center calibration points of the four planes of the marking tool are respectively provided with light-passing holes, so that the laser can pass through the light-passing holes on a plane and irradiate the opposite corners perpendicular to the plane. By controlling the distance from the laser emission position to the vertex angle opposite to a plane of the marking tool, it is ensured that the effective laser ablation points of multiple sets of calibration points coincide with the center of the regular tetrahedron marking body.

The regular tetrahedral marking body of this embodiment has a special symmetrical structure of a regular tetrahedron, which can realize uniform distribution of calibration points and adjust the attitude of the robot to the greatest extent, thereby improving the accuracy of self-calibration.

Control the robot to operate the mechanical arm so that the laser emitted by the laser source is irradiated from the light hole at the center of the first plane of the marking tool to the vertex opposite to the plane, so that the laser illuminates the plane vertically and passes through the center of the marking tool .

The robot system is used to manipulate the mechanical arm so that the tool emits laser light to vertically irradiate each plane of the calibration body and pass through the center of the calibration body, and obtain the position and attitude information of the end tcp of the mechanical arm relative to the robot base coordinate system to form the end of the mechanical arm. The transfer matrix of the coordinate system relative to the robot base coordinate system is uploaded to the processor.

The vision system is used to obtain the position and attitude information of the tracking device relative to the camera coordinate system when the laser emitted by the tool is vertically irradiated on each plane of the calibration body and passes through the center of the calibration body, and forms the coordinate system of the tracking device relative to the camera coordinate system. transition matrix and upload it to the processor.

The processor, the processor includes a transfer matrix establishment module, a robot tool calibration module and a robot hand-eye calibration module; wherein:

The transfer matrix establishment module is used to judge whether the distance from the tool to the center of the circumscribed sphere of the calibration body satisfies the sum of the effective distance of laser ablation and the radius of the circumscribed sphere of the calibration body. The transfer matrix of the system and the transfer matrix of the tracking device coordinate system relative to the camera coordinate system;

The robot tool calibration module is used to solve the translation vector of the tool coordinate system relative to the coordinate system of the end of the robot arm, and establish a transfer matrix of the tool coordinate system relative to the coordinate system of the end of the robot arm; the coordinate system of the end of the robot arm is relative to the base coordinate of the robot. The transfer matrix of the system is multiplied by the transfer matrix of the tool coordinate system relative to the coordinate system of the end of the robot arm, and the transfer matrix of the tool coordinate system relative to the robot base coordinate system is obtained to realize the tool calibration of the robot;

The robot hand-eye calibration module is used to solve the translation vector of the tool coordinate system relative to the tracking device coordinate system. According to the translation vector of the tool coordinate system relative to the tracking device coordinate system, two sets of three-dimensional point sets of the origin of the tool coordinate system are obtained, and the solution is obtained. The transfer matrix of the robot base coordinate system relative to the camera coordinate system realizes the hand-eye calibration of the robot.

In this embodiment, the robot system is specifically configured as:

For the first calibration point at the center of the first plane of the calibration body, the laser emitted by the end tool is passed through the small hole in the center of the first plane of the positive four-sided calibration body through the mechanical arm of the robot system, and the distance d measured by the laser ranging satisfies Relationship: d=l+r, obtain the position and attitude information of the tcp at the end of the manipulator relative to the robot base coordinate system at this time, and form the transfer matrix of the end coordinate system {E} of the manipulator relative to the robot base coordinate system {B}

As above, for the other three calibration points, the position and attitude information of the end tool relative to the robot base coordinate system is obtained through the robot system, and the transfer matrix of the robot arm end coordinate system {E} relative to the robot base coordinate system {B} is obtained.

Specifically, the vision system is specifically configured to:

For the first calibration point at the center of the first plane of the calibration body, the laser emitted by the end tool is made to pass through the small hole in the center of the first plane of the regular four-sided calibration body through the robotic arm of the robot system, and the distance d measured by the laser ranging satisfies Relation: d=l+r, obtain the position and attitude information of the tracking device relative to the camera coordinate system at this time, and form the transition matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C}

As above, for the other three calibration points, the position and attitude information of the tracking device relative to the camera coordinate system is obtained through the infrared stereo camera of the vision system, and the transfer matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C} is obtained

Specifically, the robot tool calibration module is specifically configured as:

The transfer matrix of the robot arm end coordinate system {E} of the four marked points obtained by the robot system relative to the robot base coordinate system {B} Substitute into the transformation relationship for the calibration of the robot tool to obtain an incompatible equation system, and use the singular value decomposition (SVD) method to solve the most probable approximate solution of the equation system in the sense of the least square method, and obtain the tool coordinate system {T} relative to the mechanical Translation vector of the arm end coordinate system {E}

Combined with the invariant principle of the rotation matrix of the translation relationship, based on the translation vector of the tool coordinate system {T} relative to the end coordinate system {E} of the manipulator Get the transfer matrix of the tool coordinate system {T} relative to the end coordinate system {E} of the manipulator

The transfer matrix of the robot arm end coordinate system {E} relative to the robot base coordinate system {B} Transfer matrix from the tool coordinate system {T} to the robot end coordinate system {E} Multiply to get the transfer matrix of the tool coordinate system {T} relative to the robot base coordinate system {B} Realize the tool calibration of the robot.

The robot hand-eye calibration module is specifically configured as:

The transition matrix of the tracking device coordinate system {D} of the four marker points obtained by the robot system relative to the camera coordinate system {C} Substitute into the transformation relationship for the calibration of the robot tool to obtain an incompatible equation system, and use the singular value decomposition (SVD) method to solve the most probable approximate solution of the equation system in the sense of the least square method, and obtain the tool coordinate system {T} relative to the tracking translation vector of device coordinate system {D}

according to the translation vector of the tool coordinate system {T} relative to the tracking device coordinate system {D} The origin of the tracking device acquired by the infrared stereo camera is translated to the origin of the tool coordinate system, so that the camera can directly read the position information of the origin of the tool coordinate system.

Operate the robotic arm to make the laser tool randomly pick points in space, and obtain the point set P _i (i=1,...n) with the origin of the tool coordinate system in the robot coordinate system and the point set P' _i in the camera coordinate system .

The hand-eye calibration is realized by the least squares solution fitting of two sets of three-dimensional point sets.

Embodiment two

This embodiment provides a calibration method for an assisted laser osteotomy robot, which is implemented based on the above-mentioned calibration device for an assisted laser osteotomy robot.

Please refer to accompanying drawing 2, the calibration method of described auxiliary laser osteotomy robot comprises the following steps:

S101, setting a calibration body and a tracking device for an auxiliary laser osteotomy robot, and adjusting a laser emission distance.

Let the robot base coordinate system be {B}, the robot end coordinate system be {E}, the camera coordinate system be {C}, the tracking device coordinate system be {D}, and the laser tool coordinate system be {T} (hereinafter referred to as tool coordinate system), {T} is the coordinate system established with the effective point of laser ablation as the origin, please refer to Figure 3.

Specifically, in the step 101, the specific implementation of adjusting the laser emission distance is as follows:

Measure the distance that the laser emitted by the end tool is irradiated from the central clear hole of a plane of the calibration body to the vertex angle opposite to the plane. Let the distance measured by the end laser tool be d, the effective distance l of laser ablation and the radius r of the circumscribed sphere of the calibration body, satisfy the relationship: d=l+r. Determine whether the measured distance is equal to d, if not, control the manipulator to adjust the distance of the laser emitted by the end tool to satisfy d.

S102: Acquire data information of four calibration points on the calibration body, including the transfer matrix of the coordinate system {E} of the end of the robot arm relative to the base coordinate system {B} of the robot The transition matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C}

Specifically, for the first calibration point at the center of the first plane of the calibration body, the laser beam emitted by the end tool is passed through the small hole in the center of the first plane of the positive four-sided calibration body through the mechanical arm of the robot system, and the measured laser distance The distance d satisfies the relationship: d=l+r, obtain the position and attitude information of the end tcp of the manipulator relative to the robot base coordinate system at this time, and obtain the transfer of the end coordinate system {E} of the manipulator relative to the robot base coordinate system {B} matrix Obtain the position and attitude information of the tracking device relative to the camera coordinate system through the infrared stereo camera of the vision system, and obtain the transfer matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C}

As above, for the other three calibration points, obtain the position and attitude information of the end tool relative to the robot base coordinate system through the robot system, and obtain the transfer matrix of the end coordinate system {E} of the manipulator relative to the robot base coordinate system {B} Obtain the position and attitude information of the tracking device relative to the camera coordinate system through the infrared stereo camera of the vision system, and obtain the transfer matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C}

S103, solving the translation vector of the tool coordinate system {T} relative to the end coordinate system {E} of the manipulator and the translation vector of the tool coordinate system {T} relative to the tracker coordinate system {D}

Specifically, the transfer matrix of the end coordinate system {E} of the robot arm relative to the robot base coordinate system {B} of the four marked points obtained by the robot system Substituting the conversion relation of robot tool calibration, the incompatible equations are obtained, and the most probable solution of the equations in the sense of the least square method is solved by singular value decomposition (SVD), and the tool coordinate system {T} relative to the mechanical Translation vector of the arm end coordinate system {E}

Specifically, in the step 103, the translation vector of the tool coordinate system {T} relative to the robot arm end coordinate system {E} is solved The specific process is as follows:

The transfer matrix of the tool coordinate system {T} relative to the robot base coordinate system {B} is expressed as Since the laser tool and tracking device of the robot are fixedly connected to the end link of the manipulator of the robot, the pose relationship of the tool coordinate system {T} relative to the end link coordinate system {E} is constant, that is, the rotation component and translation vector constant.

The transfer matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C} is expressed as Since the calibration tool and the tracking device are also in a fixed relationship, the transfer matrix of the tool coordinate system {T} relative to the tracking device coordinate system {D} is constant, that is, the rotation component and translation vector constant.

The default tool coordinate system {T} is obtained by translation of the coordinate system of the end of the manipulator {E}, then the pose of the tool coordinate system is the same as that of the coordinate system of the end of the manipulator. To complete the calibration of the robot tool coordinate system, it is only required to take the translation vector of the transfer matrix between the tool coordinate system and the coordinate system of the end of the robot arm

The tool coordinate system {T}, the robot end coordinate system {E}, and the robot base coordinate system {B} coordinate systems satisfy the following transformation relations:

The transformation matrix from the coordinate system of the robot arm end of the ith calibration point obtained in step 102 to the robot base coordinate system Substitute into formula (1) to get Written in block form as:

Let the 4th column calculated on both sides of the equation be equal, then we get:

For each calibration point, the position vector of the origin of the tool coordinate system relative to the robot base coordinate system {B} constant. Then, for all calibration points there are:

Written in matrix form as:

The above formula is an incompatible equation system, and the translation vector can be obtained The best least squares solution for .

Specifically, the singular value decomposition method (SVD) is used to solve the most probable solution of the above-mentioned incompatible equations (5) in the sense of the least square method, and the specific implementation process is as follows:

make Perform SVD decomposition on A = UΛV ^T , that is, the generalized inverse matrix A ⁺ =VΛ ⁺ U ^T of A can be obtained, where

Finally, use A ⁺ to find the least squares solution of the incompatible equations, namely

The transition matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C} for the 4 markers obtained by the vision system Using the same method as above, solve the translation vector of the tool coordinate system {T} relative to the tracking device coordinate system {D} It will not be repeated in this application.

S104, combined with the principle of invariance of the rotation matrix of the translation relationship, based on the translation vector of the tool coordinate system {T} relative to the coordinate system {E} of the end of the robot arm Obtain the transition matrix of the tool coordinate system {T} relative to the robot arm end coordinate system {E}

In this embodiment, the tool coordinate system {T} is obtained by translation of the coordinate system {E} of the end of the manipulator, then the posture of the tool coordinate system is the same as that of the coordinate system of the end of the manipulator. Combined with the invariant principle of the rotation matrix of the translation relationship, the transfer matrix of the tool coordinate system {T} relative to the robot arm end coordinate system {E} can be obtained

S105, the transfer matrix of the coordinate system {E} of the end of the robot arm relative to the base coordinate system {B} of the robot Transfer matrix from the tool coordinate system {T} to the robot end coordinate system {E} Multiply to get the transfer matrix of the tool coordinate system {T} relative to the robot base coordinate system {B} The robot system is based on the transfer matrix of the tool coordinate system {T} relative to the robot base coordinate system {B} Adjust the position and attitude information of the tool center point at the end of the robot arm relative to the robot base coordinate system to realize the tool calibration of the robot system.

Specifically, the transition matrix of the tool coordinate system {T} relative to the robot base coordinate system {B} The calculation formula is:

The robot system obtains the transfer matrix of the tool coordinate system {T} relative to the robot base coordinate system {B} Operate the robotic arm to adjust the position and attitude of the end tool to realize tool calibration of the robot.

S106. Obtain two sets of three-dimensional information of the origin of the tool coordinate system.

Specifically, in the step 106, the specific implementation process of acquiring two sets of three-dimensional information of the origin of the tool coordinate system is as follows:

according to the translation vector of the tool coordinate system {T} relative to the tracking device coordinate system {D} The origin of the tracking device acquired by the infrared stereo camera is translated to the origin of the tool coordinate system, so that the camera can directly read the position information of the origin of the tool coordinate system.

Operate the robotic arm to make the laser tool randomly pick points in space, and obtain the three-dimensional information P _i (i=1,...n) of the tool coordinate system origin in the robot coordinate system and the point set P' _i in the camera coordinate system.

S107, realizing hand-eye calibration through least square solution fitting of two sets of three-dimensional point sets.

Specifically, in the step 107, the specific process of realizing hand-eye calibration through the least square solution fitting of two sets of three-dimensional point sets is as follows:

(1) The two sets of three-dimensional information about the origin of the tool coordinate system obtained in step 106 are respectively the point set P _i under the robot coordinate system and the point set P' _i under the camera coordinate system, and the two sets of point sets satisfy the following relationship:

P' _i =RP _i +t+n _i (7)

Among them, R is the rotation matrix of the robot coordinate system and the camera coordinate system, t is the translation vector, and n _i is the noise vector.

Get R and t such that:

(2) Suppose the rotation matrix of the least squares solution is The translation vector is The centers of the two sets of points are

Assume Then {P' _i } and {P" _i } have the same center, that is, P'=P".

make then there are

(3) Requiring minδ ² is equivalent to seeking define function

in

Perform SVD decomposition on H=UΛV ^T , let X=VUT ⁽ 3×3 orthogonal matrix), then we have

XH＝VU ^T UΛV ^T ＝VΛV ^T (13)

For any 3×3 orthogonal matrix B, there is Trace(XH)≥Trace(BXH).

Among them, if det(X)=+1, then If det(X)=-1, then the algorithm is not applicable (this happens very rarely). Among all 3×3 orthogonal matrices, X maximizes F, that is, δ ² is the smallest.

(4) According to the SVD method and the properties of the orthogonal matrix, the Then the translation component can be obtained as

The transfer matrix of the robot base coordinate system and the camera coordinate system is obtained as:

The robot system is based on the transfer matrix of the robot base coordinate system {B} relative to the camera coordinate system {C} When the robot system controls the tool installed at the end of the robotic arm to move according to the set trajectory, at the same time, the camera of the vision system acquires the pose information of the tool and feeds it back to the robot system in real time to realize the hand-eye calibration of the robot system.

The calibration method proposed in this embodiment overcomes the problems of complex operation process and poor stability of the traditional method, improves the automation degree of the calibration process, and reduces unnecessary errors such as manual operation. The calibration in the camera coordinate system can improve the calibration accuracy of the robot; and the calibration device is simple and easy to operate, which greatly reduces the cost; it is suitable for the auxiliary laser osteotomy robot system, and can effectively and accurately realize the tool in the robot coordinate system and the camera coordinate system. Under the calibration, the degree of automation of the robot is improved.

Although the specific implementation of the present disclosure has been described above in conjunction with the accompanying drawings, it does not limit the protection scope of the present disclosure. Those skilled in the art should understand that on the basis of the technical solutions of the present disclosure, those skilled in the art do not need to pay creative work Various modifications or variations that can be made are still within the protection scope of the present disclosure.

Claims (9)

1. A calibration method of an auxiliary laser osteotomy robot, characterized in that, comprising the following steps: Establish the coordinate system of the robot, the end of the manipulator, the camera, the tracking device, and the tool at the end of the manipulator; Obtain the transfer matrix of the coordinate system at the end of the manipulator relative to the base coordinate system of the robot and the transfer matrix of the coordinate system of the tracking device relative to the camera coordinate system when the laser emitted by the tool is irradiated vertically on each plane of the calibration body and passes through the center of the calibration body; Solve the translation vector of the tool coordinate system relative to the coordinate system of the end of the robot arm, and establish the transfer matrix of the tool coordinate system relative to the coordinate system of the end of the robot arm; Multiply the transfer matrix of the end coordinate system of the manipulator relative to the base coordinate system of the robot by the transfer matrix of the tool coordinate system relative to the end coordinate system of the manipulator to obtain the transfer matrix of the tool coordinate system relative to the base coordinate system of the robot, and realize the tool coordinate system of the robot calibration; Solve the translation vector of the tool coordinate system relative to the tracking device coordinate system, obtain two sets of three-dimensional point sets at the origin of the tool coordinate system, solve the transfer matrix of the robot base coordinate system relative to the camera coordinate system, and realize the hand-eye calibration of the robot. 2. the calibration method of auxiliary laser osteotomy robot according to claim 1, is characterized in that, the acquisition method of described mechanical arm end coordinate system with respect to the transfer matrix of robot base coordinate system is: Measure the distance from the tool to the center of the circumscribed sphere of the calibration body when the laser emitted by the tool at the end of the manipulator is vertically irradiated to the first plane of the calibration body and passes through the center of the calibration body; When the measured distance is equal to the sum of the effective distance of laser ablation and the calibrated external ball radius, obtain the position and attitude information of the tool center point at the end of the manipulator relative to the robot base coordinate system at this time, and form the end coordinate system {E} of the manipulator The first transfer matrix relative to the robot base coordinate system {B} Repeat the above steps until the second transfer matrix of the robot end coordinate system {E} relative to the robot base coordinate system {B} is obtained third transfer matrix and the fourth transition matrix 3. The calibration method of the auxiliary laser osteotomy robot according to claim 1, wherein the method for obtaining the transfer matrix of the coordinate system of the tracking device relative to the coordinate system of the camera is: Measure the distance from the tool to the center of the circumscribed sphere of the calibration body when the laser emitted by the tool at the end of the manipulator is vertically irradiated to the first plane of the calibration body and passes through the center of the calibration body; When the measured distance is equal to the sum of the effective distance of laser ablation and the radius of the circumscribed sphere of the calibration body, the position and attitude information of the tracking device coordinate system relative to the camera coordinate system is obtained, and the tracking device coordinate system {D} relative to the camera coordinate system{D} is formed. The first transition matrix of C} Repeat the above steps until the second transition matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C} is obtained third transition matrix and the fourth transition matrix 4. the calibration method of the auxiliary laser osteotomy robot according to claim 1, is characterized in that, the solution method of the translation vector of described tool coordinate system with respect to the coordinate system of mechanical arm end is: Establish the conversion relation of the tool coordinate system {T}, the robot end coordinate system {E}, and the robot base coordinate system {B}; The transfer matrix of the robot end coordinate system {E} relative to the robot base coordinate system {B} Substituting into the conversion relation obtained above, the incompatible equations are obtained; Using the singular value decomposition method to solve the optimal least squares solution of the incompatible equation system, the translation vector of the tool coordinate system {T} relative to the robot end coordinate system {E} is obtained Combining the principle of invariance of the rotation matrix of the translation relationship between the tool coordinate system {T} and the end coordinate system {E} of the manipulator, the transfer matrix of the tool coordinate system {T} relative to the end coordinate system {E} of the manipulator is obtained The transition matrix of the tool coordinate system {T} relative to the robot end coordinate system {E} The transfer matrix of the robot arm end coordinate system {E} relative to the robot base coordinate system {B} Multiply to get the transfer matrix of the tool coordinate system {T} relative to the robot base coordinate system {B} According to the transfer matrix of the tool coordinate system {T} relative to the robot base coordinate system {B} Adjust the position and attitude information of the tool center point at the end of the robot arm relative to the robot base coordinate system to realize the tool calibration of the robot system. 5. The calibration method of the auxiliary laser osteotomy robot according to claim 1, wherein the method for solving the translation vector of the tool coordinate system relative to the tracking device coordinate system is: Establish the conversion relation of the tool coordinate system {T}, the tracking device coordinate system {D}, and the camera coordinate system {C}; The transition matrix of the tracking device coordinate system {D} relative to the camera coordinate system {C} Substitute into the conversion relation obtained above to obtain the incompatible equation system; Using the singular value decomposition method to solve the optimal least squares solution of the incompatible equation system, the translation vector of the tool coordinate system {T} relative to the tracking device coordinate system {D} is obtained 6. the calibration method of auxiliary laser osteotomy robot according to claim 1, is characterized in that, the step of two groups of three-dimensional point sets of described acquisition tool coordinate system origin comprises: according to the translation vector of the tool coordinate system {T} relative to the tracking device coordinate system {D} Translate the origin of the tracking coordinate system to the origin of the tool coordinate system; Obtain two sets of three-dimensional point sets with the origin of the tool coordinate system located at any position in space under the robot base coordinate system and the camera coordinate system. 7. The calibration method of an auxiliary laser osteotomy robot according to claim 1, wherein the step of solving the transfer matrix of the robot base coordinate system relative to the camera coordinate system comprises: Define the function F between the rotation matrix of the robot base coordinate system and the camera coordinate system and the origin of the tool coordinate system between two sets of three-dimensional point sets under the robot base coordinate system and the camera coordinate system; The maximum value of the function F is solved by the least square method based on singular value decomposition, and the rotation matrix of the robot coordinate system relative to the camera coordinate system is obtained; According to the transformation mapping relationship of the origin of the tool coordinate system from the robot base coordinate system to the camera coordinate system, calculate the translation component of the robot base coordinate system relative to the camera coordinate system; Based on the rotation matrix of the robot base coordinate system relative to the camera coordinate system and the translation component of the robot base coordinate system relative to the camera coordinate system, the transfer matrix of the robot base coordinate system and the camera coordinate system is obtained; According to the transfer matrix of the robot base coordinate system and the camera coordinate system, the tool installed at the end of the manipulator is controlled to move according to a certain trajectory, and the position and orientation information of the tool relative to the camera coordinate system is obtained at the same time to realize the hand-eye calibration of the robot system. 8. A calibration system for an auxiliary laser osteotomy robot, characterized in that it includes a robot system, a vision system, a calibration device and a processor; The robotic system is used for manipulating the robotic arm so that the tool emits laser light vertically to each plane of the calibration device and passes through the center of the calibration body to obtain the transfer matrix of the coordinate system at the end of the robotic arm relative to the base coordinate system of the robot, and upload it to the processor ; The vision system is used to obtain the transition matrix of the coordinate system of the tracking device relative to the coordinate system of the camera when the laser emitted by the tool is vertically irradiated to each plane of the calibration body and passes through the center of the calibration device, and uploaded to the processor; The processor includes a transfer matrix establishment module, a robot tool calibration module and a robot hand-eye calibration module; wherein: The transfer matrix building module is used to judge whether the distance from the tool to the center of the circumscribed sphere of the calibration device satisfies the sum of the effective distance of laser ablation and the radius of the circumscribed sphere of the calibration device. The transfer matrix of the system and the transfer matrix of the tracking device coordinate system relative to the camera coordinate system; The robot tool calibration module is used to solve the translation vector of the tool coordinate system relative to the coordinate system of the end of the robot arm, and establish a transfer matrix of the tool coordinate system relative to the coordinate system of the end of the robot arm; the coordinate system of the end of the robot arm is relative to the base coordinate of the robot. The transfer matrix of the system is multiplied by the transfer matrix of the tool coordinate system relative to the coordinate system of the end of the robot arm, and the transfer matrix of the tool coordinate system relative to the robot base coordinate system is obtained to realize the tool calibration of the robot; The robot hand-eye calibration module is used to solve the translation vector of the tool coordinate system relative to the tracking device coordinate system. According to the translation vector of the tool coordinate system relative to the tracking device coordinate system, two sets of three-dimensional point sets of the origin of the tool coordinate system are obtained, and the solution is obtained. The transfer matrix of the robot base coordinate system relative to the camera coordinate system realizes the hand-eye calibration of the robot. 9 . The calibration system of an auxiliary laser osteotomy robot according to claim 8 , wherein the calibration device is a regular tetrahedral marker body, and a tracking device is provided at four apex corners of the regular tetrahedral marker body, and the The center of the four planes of the regular tetrahedral marker body is provided with a small hole through which light passes.