WO2022032964A1

WO2022032964A1 - Dual-arm robot calibration method, system, terminal, and storage medium

Info

Publication number: WO2022032964A1
Application number: PCT/CN2020/139303
Authority: WO
Inventors: 熊璟; 徐常福; 夏泽洋
Original assignee: 中国科学院深圳先进技术研究院
Priority date: 2020-08-12
Filing date: 2020-12-25
Publication date: 2022-02-17
Also published as: CN111956329B; CN111956329A

Abstract

A dual-arm robot calibration method, a system, a terminal (50) and a storage medium. The method comprises: establishing a needle tip position calibration model, and calibrating the needle tip position of a puncture needle tube according to the model; acquiring a needle tip ultrasonic image of the puncture needle tube by means of an ultrasonic probe, the ultrasonic probe and the puncture needle tube being respectively fixed on end effectors of left and right mechanical arms of a dual-arm robot; establishing, according to the needle tip position and the needle tip ultrasonic image, a position registration model of the needle tip of the puncture needle tube and the needle tip in needle tip the ultrasonic image in a robot-based coordinate system; and solving the position registration model by using an iterative closest point algorithm, obtaining a coordinate transformation matrix from an ultrasonic image coordinate system to a robot end flange coordinate system, and calibrating the ultrasonic probe according to the coordinate transformation matrix. There is no need to use a third-party locator and model, improving the calibration accuracy of the dual-arm robot.

Description

A dual-arm robot calibration method, system, terminal and storage medium

technical field

The present application belongs to the technical field of robot calibration, and particularly relates to a calibration method, system, terminal and storage medium for a dual-arm robot.

Background technique

In recent years, with the rapid development of robot technology and image processing technology, the use of robots to assist or replace traditional manual medical surgery has achieved good scientific research results and application prospects. In particular, the dual-arm robotic system can not only control the ultrasonic probe and the puncture needle tube separately, but also flexibly control the needle insertion angle and posture in a small surgical space.

In the prior art, commonly used robot calibration methods include:

1. The ultrasonic probe calibration method using Locator and Phantom. In this method, the markers corresponding to the locator (such as the opto-magnetic locator) are fixed on the ultrasonic probe and the model, and then the equation expression between the ultrasonic image and the actual physical position of the corresponding model is established according to the geometric parameters of the model designed in advance. , and finally the coordinate transformation matrix between the ultrasonic image coordinate system and the coordinate system fixed on the ultrasonic probe locator is obtained by iterative approximation solution or closed-form equation system.

2. Ultrasonic probe calibration method using robot operation and plane calibration device. The method uses a robot to operate the ultrasonic probe to scan the plane calibration device, and then establishes a combination of equations for the calibration between the ultrasonic image and the actual physical position of the corresponding plane calibration device, and then uses the equation to solve the relationship between the ultrasonic image coordinate system and the robot manipulator coordinate system. Coordinate transformation matrix.

To sum up, the existing robot calibration methods usually require the use of third-party trackers or mechanism models, and these devices have certain errors in their positioning and size, so it is difficult to ensure the puncture accuracy of the robot. At the same time, the existing methods are mainly used for calibration of freed-hand ultrasound-guided puncture and single-arm robot calibration of ultrasound-guided puncture, but have not been well applied in the calibration of dual-arm robots.

SUMMARY OF THE INVENTION

The present application provides a method, system, terminal and storage medium for calibrating a dual-arm robot, aiming to solve one of the above-mentioned technical problems in the prior art at least to a certain extent.

In order to solve the above problems, the application provides the following technical solutions:

A dual-arm robot calibration method, comprising:

establishing a needle tip position calibration model, and calibrating the needle tip position of the puncture needle tube according to the model;

Obtain the needle tip ultrasound image of the puncture needle tube through an ultrasonic probe; wherein, the ultrasonic probe and the puncture needle tube are respectively fixed on the end effectors of the left and right mechanical arms of the dual-arm robot;

According to the needle tip position calibration result and the needle tip ultrasound image, a position registration model of the needle tip of the puncture needle tube and the needle tip in the needle tip ultrasound image under the robot base coordinate system is established;

The iterative closest point algorithm is used to solve the position registration model, and a coordinate transformation matrix from the ultrasound image coordinate system to the robot end flange coordinate system is obtained, and the ultrasound probe is calibrated according to the coordinate transformation matrix.

The technical solution adopted in the embodiment of the present application further includes: the needle tip position calibration includes:

Install a top on the end flange of any mechanical arm of the dual-arm robot;

Controlling the top to reach a preset reference point, and calculating the position of the reference point in the robot base coordinate system;

Remove the top, and install a puncture needle end effector on the end flange of any mechanical arm of the dual-arm robot;

Controlling the needle tip of the puncture needle to reach a preset reference point, and recording the pose matrix of the end flange in the robot coordinate system;

establishing an equation expression that the position of the needle tip of the puncture needle is equal to the position of the preset reference point;

The equation expression is solved, and the coordinates of the needle tip of the puncture needle tube in the robot end flange coordinate system are calibrated.

The technical solution adopted in the embodiment of the present application further includes: the position calculation formula of the preset reference point in the robot base coordinate system is:

^R P _Rtip = ^R T _ER · ^ER P _Mtip

In the above formula, ^ER P _Mtip is the coordinate from the top to the end flange, and ^R T _ER is the pose of the end flange in the robot base coordinate system;

The coordinates of the needle tip of the puncture needle in the robot end flange coordinate system are:

^ER P _Ntip = inv( ^R T _ER ) · ^R P _Rtip

In the above formula, inv() represents the inversion operation of the matrix.

The technical solution adopted in the embodiment of the present application further includes: the acquiring the needle tip ultrasound image of the puncture needle tube through the ultrasound probe includes:

The poses of the flanges at the ends of the left and right robotic arms of the dual-arm robot are recorded, and the position of the needle tip in the image coordinate system in the needle tip ultrasound image is marked.

The technical solution adopted in the embodiment of the present application further includes: the establishment of a position registration model of the needle tip of the puncture needle tube and the needle tip in the ultrasound image of the needle tip under the robot base coordinate system according to the position of the needle tip and the ultrasound image of the needle tip includes: :

Based on the position calibration result of the needle tip, the position of the needle tip in the image coordinate system in the ultrasound image of the needle tip, and the poses of the end flanges of the left and right robotic arms of the dual-arm robot, the base coordinates of the needle tip of the puncture needle tube in the robot are established The registration model whose position under the system is equal to the position of the needle tip in the robot base coordinate system in the ultrasound image of the needle tip:

inv( ^R T _EL ) ^R P _Ntip = ^EL T _I · ^I P _Ntip

In the above formula, ^R T _EL is the pose of the end flange of the robotic arm on which the ultrasonic probe end effector is installed on the dual-arm robot, ^R P _Ntip is the coordinate transformation matrix from the needle tip coordinate system to the robot base coordinate system, and ^R P _Ntip = ^R T _ER · ^ER P _Ntip , _{IP Ntip} ^is the position of the needle tip in the image coordinate system in the needle tip ultrasound image, and ^EL T _I is the coordinate transformation matrix from the ultrasound image coordinate system to the robot end flange coordinate system.

The technical solution adopted in the embodiment of the present application further includes: establishing a needle tip position calibration model, and after calibrating the needle tip position of the puncture needle tube according to the model, the method further includes:

A needle tip posture calibration model is established, and the model calibrates the needle tip posture of the puncture needle tube based on the needle tip position calibration result.

The technical solution adopted in the embodiment of the present application further includes: the calibration of the needle tip posture of the puncture needle tube based on the needle tip position calibration result includes:

Control the needle tip of the puncture needle to reach the first arbitrary point, define this point as the origin of the needle tip coordinate system, and calculate the position of the point in the robot base coordinate system according to the needle tip position calibration result, and record the two arms at this time. The attitude matrix of the robot arm end flange in the robot coordinate system;

Controlling the needle tip of the puncture needle to reach a second arbitrary point, defining the point as a point on the y-axis of the needle tip coordinate system, and calculating the position of the point in the robot base coordinate system according to the needle tip position calibration result;

Establish a vector equation system in the y-axis direction of the needle tip coordinate system, and solve the y-axis direction vector according to the matrix operation operation;

Controlling the needle tip of the puncture needle to reach the third arbitrary point, defining this point as a point on the z-axis of the needle-tip coordinate system, and obtaining the z-axis direction vector;

Adopt the cross-multiplication of the y-axis direction vector and the z-axis direction vector to obtain the x-axis direction vector, and then obtain a new z-axis direction vector through the cross-multiplication of the x-axis direction vector and the y-axis direction vector;

Combining the x-axis direction vector, the y-axis direction vector and the new z-axis direction vector in sequence, the attitude matrix of the needle tip of the puncture needle tube is obtained.

The technical solution adopted in the embodiment of the present application further includes: the establishing a position registration model of the needle tip of the puncture needle tube and the needle tip in the ultrasound image of the needle tip under the robot base coordinate system further includes:

Based on the calibration result of the needle tip position of the puncture needle, the attitude calibration result, the position of the needle tip in the ultrasound image of the needle tip, and the poses of the flanges of the left and right arm ends of the dual-arm robot, the position of the needle tip in the ultrasound image of the needle tip of the puncture needle tube and the needle tip in the ultrasound image of the needle tip are established. The position registration model in the robot base coordinate system:

inv( ^R T _EL · ^EL T _Ptip ) · ^R P _Ntip = ^Ptip T _I · ^I P _Ntip

In the above formula, ^EL T _Ptip is the coordinate transformation matrix from any fixed needle tip on the ultrasound probe to the flange at the end of the robotic arm where the ultrasound probe is installed, and ^Ptip T _I is the coordinate transformation matrix from the ultrasound image coordinate system to the coordinate system of the robot end flange.

Another technical solution adopted in the embodiment of the present application is: a dual-arm robot calibration system, including a needle tip calibration module and an ultrasonic probe calibration module;

The needle tip calibration module is used to establish a needle tip position calibration model, and calibrate the needle tip position of the puncture needle tube according to the model;

The ultrasonic probe calibration module includes:

Ultrasonic image acquisition sub-module: used for acquiring the needle tip ultrasonic image of the puncture needle tube through an ultrasonic probe; wherein, the ultrasonic probe and the puncture needle tube are respectively fixed on the end effectors of the left and right mechanical arms of the dual-arm robot;

Needle tip position registration sub-module: used to establish a position registration model of the calibrated puncture needle tube needle tip and the needle tip in the needle tip ultrasound image under the robot base coordinate system according to the needle tip position calibration result and the needle tip ultrasound image ;

Coordinate calculation sub-module: used to solve the position registration model by using the iterative closest point algorithm, obtain the coordinate transformation matrix from the ultrasound image coordinate system to the robot end flange coordinate system, and calibrate the ultrasound probe according to the coordinate transformation matrix

Another technical solution adopted by the embodiments of the present application is: a terminal, the terminal includes a processor and a memory coupled to the processor, wherein,

The memory stores program instructions for implementing the dual-arm robot calibration method;

The processor is configured to execute the program instructions stored in the memory to control the calibration of the dual-arm robot.

Another technical solution adopted by the embodiments of the present application is: a storage medium storing program instructions executable by a processor, where the program instructions are used to execute the dual-arm robot calibration method.

Compared with the prior art, the beneficial effect of the embodiment of the present application is that the dual-arm robot calibration method, system, terminal and storage medium of the embodiment of the present application can demarcate the position of the needle tip of the puncture needle tube by establishing a point-to-point needle tip position calibration model, Then, based on the calibration result and the needle tip ultrasound image scanned by the ultrasound probe, a registration model between the calibrated needle tip position and the needle tip position in the needle tip ultrasound image is established. Finally, the iterative closest point registration algorithm is used to calculate the ultrasound image coordinate system to the robot end method. The coordinate transformation matrix of the blue coordinate system realizes the high-precision calibration of the needle tip of the puncture needle and the ultrasonic probe of the dual-arm robot. The present application does not require the use of third-party locators and models. By fusing the mechanism and ultrasonic images of the dual-arm robot, the calibration of the needle tip of the puncture needle and the ultrasonic probe is realized, and the calibration accuracy of the dual-arm robot is improved.

Description of drawings

1 is a flowchart of a dual-arm robot calibration method according to a first embodiment of the present application;

2 is a flowchart of a method for calibrating a needle tip position of a needle tube according to the first embodiment of the application;

3 is a flowchart of a dual-arm robot calibration method according to a second embodiment of the present application;

4 is a flowchart of a method for calibrating a needle tip attitude of a needle tube according to a second embodiment of the present application;

Figure 5 shows the application of the embodiment of the present application to a dual-arm robot puncture system based on ultrasonic guidance, wherein (a) is to control the tip to reach a specified reference point, (b) is to calibrate the needle tip of the puncture needle in the robot end flange coordinate system (c) is the position coordinate of the calibrated ultrasonic probe tip in the robot end flange coordinate system, (d) is to control the ultrasonic probe to scan the puncture needle tip;

6 is a schematic structural diagram of a dual-arm robot calibration system according to an embodiment of the application;

FIG. 7 is a schematic structural diagram of a terminal according to an embodiment of the present application;

FIG. 8 is a schematic structural diagram of a storage medium according to an embodiment of the present application.

detailed description

In order to make the purpose, technical solutions and advantages of the present application more clearly understood, the present application will be described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, but not to limit the present application.

Please refer to FIG. 1 , which is a flowchart of a method for calibrating a dual-arm robot according to the first embodiment of the present application. The dual-arm robot calibration method according to the first embodiment of the present application includes two parts: puncturing needle tip position calibration and ultrasonic probe calibration, and specifically includes the following steps:

Step 100: establishing a needle tip position calibration model, and calibrating the needle tip position of the puncture needle;

In this step, the embodiment of the present application utilizes the characteristics of the dual-arm robot itself that the dual-arm high-precision positioning and the dual-arm position coordinates are in the same robot base coordinate system to establish a needle tip position calibration model, and calibrate the needle tip position. Please also refer to FIG. 2 , which is a flowchart of a method for calibrating a needle tip position of a needle tube according to an embodiment of the invention, which specifically includes the following steps:

Step 101: Design and process a high-precision top, and install it on the end flange of either arm of the dual-arm robot;

Step 102: control the top to reach the preset reference point through the dual-arm robot, and calculate the position of the reference point in the robot base coordinate system; the calculation formula is:

^R P _Rtip = ^R T _ER · ^ER P _Mtip (1)

In formula (1), ^ER P _Mtip is the coordinates from the top to the end flange, which can be obtained according to the preset top parameters; ^R T _ER is the pose of the end flange in the robot base coordinate system, which can be found in the robot operating system. read directly.

Step 103: Remove the tip, and install the end effector of the puncture needle tube on the end flange of any mechanical arm of the dual-arm robot;

Step 104: control the needle tip of the puncture needle to reach the preset reference point through the dual-arm robot, and record the pose matrix of the end flange in the robot coordinate system;

Step 105: establish an equation that the position of the needle tip of the puncture needle is equal to the position of the reference point;

Step 106: Solve the equation expression, and calibrate the coordinates ^ER P _Ntip of the needle tip of the puncture needle in the robot end flange coordinate system:

^ER P _Ntip = inv( ^R T _ER ) · ^R P _Rtip (2)

In formula (2), inv( ) represents the inversion operation of the matrix.

Based on the above steps, the position calibration of the needle tip of the puncture needle is completed.

Step 110: Fix the ultrasonic probe and the puncture needle tube on the end effectors of the left and right mechanical arms of the dual-arm robot respectively, and control the ultrasonic probe to scan the needle tip of the puncture needle tube through the dual-arm robot to obtain an ultrasonic image of the needle tip, and record the dual-arm robot respectively. The poses ^R T _EL , ^R T _ER of the left and right arm end flanges;

It will be appreciated that a tracker may also be fixed on the ultrasound probe or puncture needle tube instead of the end flange position.

Step 120: mark the position _{IP Ntip} ^of the needle tip in the image coordinate system in the ultrasound image of the needle tip;

Step 130: Based on the calibration result of the needle tip position of the puncture needle tube, the needle tip position in the ultrasound image of the needle tip, and the positions of the flanges of the left and right arm end flanges of the dual-arm robot, establish the position of the needle tip of the puncture needle tube and the needle tip in the ultrasound image of the needle tip in the robot base coordinate system registration model;

In the first embodiment of the present application, on the basis of the calibration results of the needle tip position of the needle tube, the needle tip position in the needle tip ultrasound image and the positions of the flanges of the left and right arm ends of the dual-arm robot are combined to establish the needle tip of the puncture needle tube and the needle tip ultrasound in the robot base coordinate system. The position registration model of the needle tip in the robot base coordinate system in the image, its equation expression is:

inv( ^R T _EL ) ^R P _Ntip = ^EL T _I ^I P _Ntip (3)

In formula (3), ^R P _Ntip is the coordinate transformation matrix from the needle tip coordinate system of the needle tube to the robot base coordinate system, and its calculation formula is ^R P _Ntip = ^R T _ER · ^ER P _Ntip , and ^EL T _I is the ultrasound image coordinate system to the robot. The coordinate transformation matrix of the end flange coordinate system.

Step 140: using the iterative closest point algorithm to solve the position registration model, obtain the coordinate transformation matrix ^ELTI from the _ultrasound image coordinate system to the robot end flange coordinate system, and calibrate the ultrasound probe according to the coordinate transformation matrix;

The calibration of the ultrasound probe is completed through steps 110 to 140 .

Based on the above, the dual-arm robot calibration method according to the first embodiment of the present application utilizes the characteristics of the dual-arm robot itself that the dual-arm high-precision positioning and the dual-arm position coordinates are in the same robot base coordinate system, and firstly establishes a point-to-point needle tip position calibration Model, calibrate the position of the needle tip of the puncture needle, and then establish the position registration model of the needle tip of the puncture needle tube and the needle tip in the ultrasound image of the needle tip in the robot base coordinate system based on the calibration result and the ultrasound image of the needle tip scanned by the ultrasonic probe. The point registration algorithm calculates the coordinate transformation matrix from the ultrasonic image coordinate system to the robot end flange coordinate system, and realizes the high-precision calibration of the needle tip of the puncture needle and the ultrasonic probe of the dual-arm robot.

Please refer to FIG. 3 , which is a flowchart of a method for calibrating a dual-arm robot according to the second embodiment of the present application. The dual-arm robot calibration method according to the second embodiment of the present application includes two parts: puncturing needle tip position and attitude calibration and ultrasonic probe calibration, and specifically includes the following steps:

Step 200: establishing a needle tip position calibration model, and calibrating the needle tip position of the puncture needle tube;

In this step, the method for calibrating the position of the needle tip is the same as that in the first embodiment, which will not be repeated here.

Step 210 : establishing a needle tip attitude calibration model, and calibrating the needle tip attitude of the puncture needle tube;

In this step, please refer to FIG. 4 together, which is a flowchart of the method for calibrating the attitude of the needle tip of the needle tube according to the second embodiment of the present application, which specifically includes the following steps:

Step 211 : control the needle tip of the puncture needle to reach the first arbitrary point, define this point as the origin (P _o ) of the needle tip coordinate system, and calculate the position ^R of the point (P _o ) in the robot base coordinate system according to the calibration result of the needle tip position P _oTip , at the same time record the attitude matrix of the flange at the end of the robotic arm of the dual-arm robot in the robot coordinate system

Step 212 : control the needle tip of the puncture needle to reach the second arbitrary point, define this point as the point (P _y ) on the y-axis of the needle tip coordinate system, and calculate the point (P _y ) under the robot base coordinate system according to the calibration result of the needle tip position The position of ^R P _yTip ;

Step 213: Establish a vector equation system in the y-axis direction of the needle tip coordinate system, and solve the y-axis direction vector according to the matrix operation operation. The vector equation system is:

In formula (4), _ny is the y-axis direction vector, and Δy is the distance between ^R P _oTip and ^R P _yTip ;

And solve the y-axis direction vector as:

Step 214: control the needle tip of the puncture needle to reach the third arbitrary point, define the point as a point on the z-axis of the needle-tip coordinate system, and obtain the z-axis direction vector n _z ;

The calculation method of the z-axis direction vector is the same as that of the y-axis direction vector, which will not be repeated here.

Step 215 : obtain the x-axis direction vector n _x by cross-multiplying the y-axis direction vector and the z-axis direction vector, and then obtain a new z-axis direction vector through the cross-multiplication of the x-axis direction vector and the y-axis direction vector;

Step 216: Combine the x-axis direction vector, the y-axis direction vector and the new z-axis direction vector in turn to obtain the attitude matrix ^R R _Tip of the needle tip of the puncture needle:

^R R _Tip = [n _x ,n _y ,n _z ] (6)

Through the above steps 200 and 210, the calibration of the position and posture of the needle tip of the puncture needle is completed.

Step 220: Fix the ultrasonic probe and the puncture needle tube on the end effectors of the left and right mechanical arms of the dual-arm robot respectively, and control the ultrasonic probe to scan the needle tip of the puncture needle tube through the dual-arm robot to obtain an ultrasonic image of the needle tip, and record the dual-arm robot respectively. The poses ^R T _EL , ^R T _ER of the left and right arm end flanges;

Step 230: Mark the position _{IP Ntip} ^of the needle tip in the image coordinate system in the needle tip ultrasound image;

Step 240: Based on the calibration results of the needle tip position and posture of the puncture needle tube, the needle tip position in the needle tip ultrasound image, and the poses of the flanges at the left and right arm ends of the dual-arm robot, establish that the needle tip of the puncture needle tube and the needle tip in the ultrasound image of the needle tip are in the robot base coordinate system The position registration model of ;

In the second embodiment of the present application, on the basis of the calibration results of the position and attitude of the needle tip of the puncture needle tube, the needle tip of the puncture needle tube is established in the robot base coordinate system by combining the needle tip position in the ultrasound image of the needle tip and the positions of the flanges of the left and right arm ends of the dual-arm robot. The registration model whose position is equal to the position of the needle tip in the ultrasound image of the needle tip in the robot base coordinate system, its equation expression is:

inv( ^R T _EL · ^EL T _Ptip ) · ^R P _Ntip = ^Ptip T _I · ^I P _Ntip (7)

In the above formula, ^EL T _Ptip is the coordinate transformation matrix from any fixed needle tip on the ultrasound probe to the flange at the end of the robotic arm where the ultrasound probe is installed ^. T _I is the coordinate transformation matrix from the ultrasound image coordinate system to the robot end flange coordinate system.

Step 250: Solve the registration model by using the iterative closest point algorithm, obtain a coordinate transformation matrix ^Ptip T _I from the ultrasound image coordinate system to the robot end flange coordinate system, and calibrate the ultrasound probe according to the coordinate transformation matrix;

The calibration of the ultrasound probe is completed through steps 220 to 250 .

Based on the above, the dual-arm robot calibration method of the second embodiment of the present application further establishes a needle tip attitude calibration model on the basis of the needle tip position calibration, and calibrates the needle tip attitude of the puncture needle tube, and based on the needle tip position and attitude calibration results of the puncture needle tube, Combined with the needle tip position in the needle tip ultrasound image and the pose of the left and right arm end flanges of the dual-arm robot, a registration model is established in which the position of the needle tip of the puncture needle in the robot base coordinate system is equal to the position of the needle tip in the needle tip ultrasound image under the robot base coordinate system. , so as to realize the calibration of the needle tip of the puncture needle and the ultrasonic probe of the dual-arm robot with high precision.

The embodiment of the present application does not need to use a third-party locator and model, and realizes the calibration of the needle tip of the puncture needle and the ultrasonic probe by integrating the mechanism of the dual-arm robot and the ultrasonic image, thereby improving the calibration accuracy of the dual-arm robot.

In order to verify the feasibility and effectiveness of the embodiments of the present application, an experimental test is carried out by taking the application of the present application in a dual-arm robotic puncture system based on ultrasound guidance as an example. Specifically as shown in Figure 5, where (a) is to control the tip to reach the specified reference point, (b) is to calibrate the position coordinates of the needle tip of the puncture needle in the robot end flange coordinate system, (c) is to calibrate the ultrasonic probe at the end of the robot. The position coordinate calibration in the coordinate system of the flange at the end of the robot, (d) is to control the ultrasonic probe to scan the needle tip of the puncture needle. The experimental results show that the calibration accuracy of the embodiment of the present application can reach 0.6467±0.3099 mm, which realizes the high-precision calibration of the needle tip of the puncture needle tube and the ultrasonic probe.

Please refer to FIG. 6 , which is a schematic structural diagram of a dual-arm robot calibration system according to an embodiment of the present application. The dual-arm robot calibration system of the embodiment of the present application includes a needle tip calibration module and an ultrasonic probe calibration module, wherein the needle tip calibration module includes:

Needle tip position calibration sub-module: used to establish a needle tip position calibration model, and calibrate the needle tip position of the puncture needle tube; wherein, the embodiment of the present application utilizes the dual-arm robot itself and the dual-arm high-precision positioning and the dual-arm position coordinates are in the same robot base. The characteristics of the coordinate system are used to establish the needle tip position calibration model, and the needle tip position is calibrated. Specific calibration methods include:

1. Design and process a high-precision top, and install it on the end flange of either arm of the dual-arm robot;

2. Control the top to reach the preset reference point through the dual-arm robot, and calculate the position of the reference point in the robot base coordinate system; the calculation formula is:

^R P _Rtip = ^R T _ER · ^ER P _Mtip (1)

3. Remove the tip, and install the end effector of the puncture needle tube on the end flange of either arm of the dual-arm robot;

4. Control the needle tip of the puncture needle to reach the preset reference point through the dual-arm robot, and record the pose matrix of the end flange in the robot coordinate system;

5. Establish an equation expression that the position of the needle tip of the puncture needle is equal to the position of the reference point;

6. Solve the equation expression, and calibrate the coordinates ^ER P _Ntip of the needle tip of the puncture needle in the coordinate system of the robot end flange:

^ER P _Ntip = inv( ^R T _ER ) · ^R P _Rtip (2)

In formula (2), inv( ) represents the inversion operation of the matrix.

Needle tip attitude calibration sub-module: used to establish a needle tip attitude calibration model to calibrate the needle tip attitude of the puncture needle; wherein, the needle tip attitude calibration method includes:

1. Control the needle tip of the puncture needle to reach the first arbitrary point, define this point as the origin of the needle tip coordinate system (P _o ), and calculate the position ^R P of the point (P _o ) in the robot base coordinate system according to the calibration result of the needle tip position _oTip , at the same time record the attitude matrix of the flange at the end of the robotic arm of the dual-arm robot in the robot coordinate system

2. Control the needle tip of the puncture needle to reach the second arbitrary point, define this point as the point (P _y ) on the y-axis of the needle tip coordinate system, and calculate the point (P _y ) in the robot base coordinate system according to the calibration result of the needle tip position. position ^R P _yTip ;

3. Establish a vector equation system in the y-axis direction of the needle tip coordinate system, and solve the y-axis direction vector according to the matrix operation. The vector equation system is:

And solve the y-axis direction vector as:

4. Control the needle tip of the puncture needle to reach the third arbitrary point, define this point as the point on the z-axis of the needle-tip coordinate system, and obtain the z-axis direction vector;

5. Use the cross product of the y-axis direction vector and the z-axis direction vector to obtain the x-axis direction vector, and then obtain the new z-axis direction vector through the cross-multiplication of the x-axis direction vector and the y-axis direction vector;

6. Combine the x-axis direction vector, the y-axis direction vector and the new z-axis direction vector in turn to obtain the attitude matrix ^R R _Tip of the needle tip of the puncture needle:

^R R _Tip = [n _x ,n _y ,n _z ] (6)

The ultrasound probe calibration module includes:

Ultrasound image acquisition sub-module: used to control the ultrasound probe to scan the needle tip of the puncture needle to obtain the needle tip ultrasound image, mark the position of the needle tip in the image coordinate system _{IP Ntip} ⁱⁿ the needle tip ultrasound image, and record the left and right arm end methods of the dual-arm robot respectively. The poses ^R T _EL and ^R T _ER of Lan are respectively fixed on the end effectors of the left and right robotic arms of the dual-arm robot. It will be appreciated that a tracker may also be fixed on the ultrasound probe or puncture needle tube instead of the end flange position.

Needle tip position registration sub-module: It is used to establish the base coordinates of the needle tip in the puncture needle tube and the needle tip in the ultrasound image of the needle tip on the robot based on the needle tip position calibration result, the needle tip position in the needle tip ultrasound image, and the poses of the left and right arm end flanges of the dual-arm robot The position registration model under the system;

In the embodiment of the present application, on the basis of the calibration results of the needle tip position of the needle tube, combined with the needle tip position in the needle tip ultrasound image and the poses of the flanges of the left and right arm ends of the dual-arm robot, the needle tip of the puncture needle tube and the needle tip in the needle tip ultrasound image are established in the robot base coordinate system The registration model with equal lower positions, its equation expression is:

inv( ^R T _EL ) ^R P _Ntip = ^EL T _I ^I P _Ntip (3)

In the embodiment of the present application, the needle tip position registration sub-module is also used to establish the needle tip of the puncture needle tube based on the position and attitude calibration results of the needle tip of the puncture needle tube, the needle tip position in the needle tip ultrasound image, and the positions of the flanges of the left and right arm ends of the dual-arm robot The equation is expressed as:

inv( ^R T _EL · ^EL T _Ptip ) · ^R P _Ntip = ^Ptip T _I · ^I P _Ntip (7)

Coordinate calculation sub-module: based on formula (3) or (7), the iterative closest point algorithm is used to solve the position registration model, and the coordinate transformation matrix ^EL T _I from the ultrasonic image coordinate system to the robot end flange coordinate system is obtained. or ^Ptip T _I , the ultrasound probe is calibrated according to the coordinate transformation matrix.

Please refer to FIG. 7 , which is a schematic structural diagram of a terminal according to an embodiment of the present application. The terminal 50 includes a processor 51 and a memory 52 coupled to the processor 51 .

The memory 52 stores program instructions for implementing the above-mentioned dual-arm robot calibration method.

The processor 51 is configured to execute program instructions stored in the memory 52 to control the calibration of the dual-arm robot.

The processor 51 may also be referred to as a CPU (Central Processing Unit, central processing unit). The processor 51 may be an integrated circuit chip with signal processing capability. The processor 51 may also be a general purpose processor, digital signal processor (DSP), application specific integrated circuit (ASIC), off-the-shelf programmable gate array (FPGA) or other programmable logic device, discrete gate or transistor logic device, discrete hardware component . A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.

Please refer to FIG. 8 , which is a schematic structural diagram of a storage medium according to an embodiment of the present application. The storage medium of this embodiment of the present application stores a program file 61 capable of implementing all the above methods, wherein the program file 61 may be stored in the above-mentioned storage medium in the form of a software product, and includes several instructions to make a computer device (which may It is a personal computer, a server, or a network device, etc.) or a processor (processor) that executes all or part of the steps of the methods of the various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, Read-Only Memory (ROM, Read-Only Memory), Random Access Memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program codes , or terminal devices such as computers, servers, mobile phones, and tablets.

The above description of the disclosed embodiments enables any person skilled in the art to make or use the present application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined in this application may be implemented in other embodiments without departing from the spirit or scope of this application. Therefore, this application is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

A method for calibrating a dual-arm robot, comprising:

establishing a needle tip position calibration model, and calibrating the needle tip position of the puncture needle tube according to the model;

Obtain the needle tip ultrasound image of the puncture needle tube through an ultrasonic probe; wherein, the ultrasonic probe and the puncture needle tube are respectively fixed on the end effectors of the left and right mechanical arms of the dual-arm robot;

According to the needle tip position calibration result and the needle tip ultrasound image, a position registration model of the needle tip of the puncture needle tube and the needle tip in the needle tip ultrasound image under the robot base coordinate system is established;

The iterative closest point algorithm is used to solve the position registration model, and a coordinate transformation matrix from the ultrasonic image coordinate system to the robot end flange coordinate system is obtained, and the ultrasonic probe is calibrated according to the coordinate transformation matrix.
The method for calibrating a dual-arm robot according to claim 1, wherein the needle tip position calibration comprises:

Install a top on the end flange of any mechanical arm of the dual-arm robot;

Controlling the top to reach a preset reference point, and calculating the position of the reference point in the robot base coordinate system;

Remove the top, and install a puncture needle end effector on the end flange of any mechanical arm of the dual-arm robot;

Controlling the needle tip of the puncture needle to reach a preset reference point, and recording the pose matrix of the end flange in the robot coordinate system;

establishing an equation expression that the position of the needle tip of the puncture needle is equal to the position of the preset reference point;

The equation expression is solved, and the coordinates of the needle tip of the puncture needle tube in the robot end flange coordinate system are calibrated.
The method for calibrating a dual-arm robot according to claim 2, wherein the formula for calculating the position of the preset reference point in the robot base coordinate system is:

R P Rtip = R T ER · ER P Mtip

In the above formula, ER P Mtip is the coordinate from the top to the end flange, and R T ER is the pose of the end flange in the robot base coordinate system;

The coordinates of the needle tip of the puncture needle in the robot end flange coordinate system are:

ER P Ntip = inv( R T ER ) · R P Rtip

In the above formula, inv( ) represents the inversion operation of the matrix.
The method for calibrating a dual-arm robot according to claim 3, wherein the acquiring the ultrasound image of the needle tip of the puncture needle via an ultrasound probe comprises:

The poses of the flanges at the ends of the left and right robotic arms of the dual-arm robot are recorded, and the position of the needle tip in the image coordinate system in the needle tip ultrasound image is marked.
The method for calibrating a dual-arm robot according to claim 4, wherein the needle tip of the puncture needle is established according to the needle tip position and the needle tip ultrasound image and the needle tip in the needle tip ultrasound image is in the robot base coordinate system The location registration model includes:

The base coordinates of the puncture needle tip in the robot are established based on the position calibration result of the needle tip, the position of the needle tip in the image coordinate system in the ultrasound image of the needle tip, and the poses of the end flanges of the left and right robotic arms of the dual-arm robot The registration model whose position under the system is equal to the position of the needle tip in the robot base coordinate system in the ultrasound image of the needle tip:

inv( R T EL ) R P Ntip = EL T I · I P Ntip

In the above formula, R T EL is the pose of the end flange of the robotic arm on which the ultrasonic probe end effector is installed on the dual-arm robot, R P Ntip is the coordinate transformation matrix from the needle tip coordinate system to the robot base coordinate system, and R P Ntip = R T ER · ER P Ntip , IP Ntip is the position of the needle tip in the image coordinate system in the needle tip ultrasound image, and EL T I is the coordinate transformation matrix from the ultrasound image coordinate system to the robot end flange coordinate system.
The method for calibrating a dual-arm robot according to claim 1, wherein the establishing a needle tip position calibration model and calibrating the needle tip position of the puncture needle tube according to the model further comprises:

A needle tip posture calibration model is established, and the model calibrates the needle tip posture of the puncture needle tube based on the needle tip position calibration result.
The method for calibrating a dual-arm robot according to claim 6, wherein the calibrating the needle tip posture of the puncture needle tube based on the needle tip position calibration result comprises:

Control the needle tip of the puncture needle to reach the first arbitrary point, define this point as the origin of the needle tip coordinate system, and calculate the position of this point in the robot base coordinate system according to the needle tip position calibration result, and record the arms at this time. The attitude matrix of the robot arm end flange in the robot coordinate system;

Controlling the needle tip of the puncture needle to reach the second arbitrary point, defining the point as a point on the y-axis of the needle tip coordinate system, and calculating the position of the point in the robot base coordinate system according to the needle tip position calibration result;

Establish a vector equation system in the y-axis direction of the needle tip coordinate system, and solve the y-axis direction vector according to the matrix operation operation;

Controlling the needle tip of the puncture needle to reach the third arbitrary point, defining this point as a point on the z-axis of the needle-tip coordinate system, and obtaining the z-axis direction vector;

Adopt the cross-multiplication of the y-axis direction vector and the z-axis direction vector to obtain the x-axis direction vector, and then obtain a new z-axis direction vector through the cross-multiplication of the x-axis direction vector and the y-axis direction vector;

Combining the x-axis direction vector, the y-axis direction vector and the new z-axis direction vector in sequence, the attitude matrix of the needle tip of the puncture needle tube is obtained.
The method for calibrating a dual-arm robot according to claim 7, wherein the establishing a position registration model of the needle tip of the puncture needle tube and the needle tip in the ultrasound image of the needle tip under the robot base coordinate system further comprises:

Based on the calibration result of the needle tip position of the puncture needle, the attitude calibration result, the position of the needle tip in the ultrasound image of the needle tip, and the postures of the flanges of the left and right arm ends of the dual-arm robot, the position of the needle tip in the ultrasound image of the needle tip of the puncture needle tube and the needle tip in the ultrasound image of the needle tip are established. The position registration model in the robot base coordinate system:

inv( R T EL · EL T Ptip ) · R P Ntip = Ptip T I · I P Ntip

In the above formula, EL T Ptip is the coordinate transformation matrix from any fixed needle tip on the ultrasound probe to the flange at the end of the robotic arm where the ultrasound probe is installed, and Ptip T I is the coordinate transformation matrix from the ultrasound image coordinate system to the coordinate system of the robot end flange.
A dual-arm robot calibration system, characterized in that it includes a needle tip calibration module and an ultrasonic probe calibration module;

The needle tip calibration module is used to establish a needle tip position calibration model, and calibrate the needle tip position of the puncture needle tube according to the model;

The ultrasonic probe calibration module includes:

Ultrasound image acquisition sub-module: used for acquiring the needle tip ultrasound image of the puncture needle tube through an ultrasonic probe; wherein, the ultrasonic probe and the puncture needle tube are respectively fixed on the end effectors of the left and right mechanical arms of the dual-arm robot;

Needle tip position registration sub-module: used to establish a position registration model of the calibrated puncture needle tube needle tip and the needle tip in the needle tip ultrasound image under the robot base coordinate system according to the needle tip position calibration result and the needle tip ultrasound image ;

Coordinate calculation sub-module: used to solve the position registration model by using the iterative closest point algorithm, obtain the coordinate transformation matrix from the ultrasound image coordinate system to the robot end flange coordinate system, and calibrate the ultrasound probe according to the coordinate transformation matrix .
A terminal, characterized in that the terminal includes a processor and a memory coupled to the processor, wherein,

The memory stores program instructions for realizing the dual-arm robot calibration method according to any one of claims 1-8;

The processor is configured to execute the program instructions stored in the memory to control the calibration of the dual-arm robot.
A storage medium, characterized in that it stores program instructions executable by a processor, and the program instructions are used to execute the calibration method for a dual-arm robot according to any one of claims 1 to 8.