WO2022198615A1 - Calibration method and system for dual-arm robot puncture system - Google Patents

Calibration method and system for dual-arm robot puncture system Download PDF

Info

Publication number
WO2022198615A1
WO2022198615A1 PCT/CN2021/083163 CN2021083163W WO2022198615A1 WO 2022198615 A1 WO2022198615 A1 WO 2022198615A1 CN 2021083163 W CN2021083163 W CN 2021083163W WO 2022198615 A1 WO2022198615 A1 WO 2022198615A1
Authority
WO
WIPO (PCT)
Prior art keywords
dual
parameter
arm robot
plane
puncture needle
Prior art date
Application number
PCT/CN2021/083163
Other languages
French (fr)
Chinese (zh)
Inventor
熊璟
李强云
夏泽洋
Original Assignee
中国科学院深圳先进技术研究院
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 中国科学院深圳先进技术研究院 filed Critical 中国科学院深圳先进技术研究院
Priority to PCT/CN2021/083163 priority Critical patent/WO2022198615A1/en
Publication of WO2022198615A1 publication Critical patent/WO2022198615A1/en

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/20—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis
    • A—HUMAN NECESSITIES
    • A61—MEDICAL OR VETERINARY SCIENCE; HYGIENE
    • A61B—DIAGNOSIS; SURGERY; IDENTIFICATION
    • A61B34/00—Computer-aided surgery; Manipulators or robots specially adapted for use in surgery
    • A61B34/30—Surgical robots

Definitions

  • the present application belongs to the technical field of medical image processing, and in particular relates to a calibration method and system for a dual-arm robotic puncture system.
  • the dual-arm robotic puncture system with multi-degree-of-freedom ends can not only control the ultrasonic probe and the puncture needle separately, and flexibly control the needle insertion angle and posture in a small surgical space; Accurate image positioning and good visual effects.
  • the calibration scheme of the dual-arm robot puncture system with multi-degree-of-freedom end mainly includes:
  • the method installs positioning sensors on the left and right end flanges of the robot respectively, and completes the calibration of the positioning sensors through the robot hand-eye calibration; then controls the robot to scan the calibration template with the ultrasonic probe in different postures, and establishes the relationship between the ultrasonic image and the calibration template through the positioning sensor.
  • the coordinate transformation matrix between the ultrasound image coordinate system and the positioning sensor coordinate system is obtained.
  • 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 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.
  • the above-mentioned calibration methods are usually designed for the dual-arm robotic puncture system with a zero-degree-of-freedom end, and it is difficult to efficiently calibrate the dual-arm robotic puncture system with a multi-degree-of-freedom end.
  • the existing calibration methods often require third-party tracking devices such as positioning sensors, cameras, and models. These devices have certain errors in their positioning and size, and are costly.
  • the present application provides a method and system for calibrating a dual-arm robotic puncture system, aiming to solve one of the above-mentioned technical problems in the prior art at least to a certain extent.
  • a method for calibrating a dual-arm robot puncture system comprising:
  • the parameters to be calibrated include the first initial parameter of the puncture needle, the first translation parameter, and the second initial parameter of the ultrasonic probe parameter, second translation parameter and rotation parameter;
  • Acquire two mutually perpendicular S-plane ultrasound images and T-plane ultrasound images by using the ultrasound probe use the S-plane ultrasound images and T-plane ultrasound images to track the motion trajectories of the ultrasound probe and the puncture needle respectively, and use the trajectory fitting to track the movement trajectories of the ultrasound probe and puncture needle.
  • the first translation parameter, the second translation parameter and the rotation parameter are calibrated.
  • the kinematic analysis of the dual-arm robot puncturing system includes:
  • R P(d n ) R P(0)+v n ⁇ d n
  • B T R is the homogeneous coordinate transformation matrix from ⁇ R ⁇ to ⁇ B ⁇
  • ⁇ R ⁇ represents the end flange coordinate system of the right arm of the dual-arm robot
  • ⁇ B ⁇ is the base coordinate system of the dual-arm robot
  • R P(d n ) is the coordinate of the needle tip in ⁇ R ⁇ when the puncture needle is translated by dn
  • v n is the unit direction vector in ⁇ R ⁇ when the puncture needle is translated, that is, the first translation parameter.
  • the kinematic analysis of the dual-arm robot puncturing system includes:
  • n n(0)+d p ⁇ v p
  • B T L is the homogeneous coordinate transformation matrix from ⁇ L ⁇ to ⁇ B ⁇ , ⁇ L ⁇ represents the end flange coordinate system of the left arm of the dual-arm robot;
  • B P(d p / ⁇ ) is the translation of the ultrasonic probe d p or rotation ⁇ , the position vector of the ultrasound probe in ⁇ B ⁇ .
  • L T ST (d p / ⁇ ) is the set composed of L T S (d p / ⁇ ) and L T T (d p / ⁇ ), when the ultrasound probe is translated by d p or rotated by ⁇ , ⁇ S ⁇ or ⁇ T
  • the homogeneous coordinate transformation matrix from ⁇ to ⁇ L ⁇ ; ⁇ S ⁇ and ⁇ T ⁇ correspond to the S-plane ultrasound image coordinate system and the T-plane ultrasound image coordinate system of the ultrasound probe respectively
  • ST P is the set composed of SP and T P , Represents the pixel coordinates in the S-plane ultrasound image or the T-plane ultrasound image
  • R(n, ⁇ ) is the rotation transformation matrix of the coordinate system when the coordinate system rotates around the axis n by an angle of ⁇
  • n and c are the method of rotating the plane of the ultrasound probe respectively vector and rotation center coordinates
  • n(0) and C(0) are respectively the unit normal vector and rotation center coordinates of the rotation plane in ⁇ L ⁇ when the ultrasonic probe
  • the technical solution adopted in the embodiment of the present application further includes: the use of the multi-point method to calibrate the first initial parameter of the puncture needle is specifically:
  • the technical solution adopted in the embodiment of the present application further includes: the calibration of the second initial parameter of the ultrasonic probe according to the biplane ultrasonic imaging is specifically:
  • the calibrating the first translation parameter includes:
  • the calibrating the second translation parameter includes:
  • the calibrating the rotation parameter includes:
  • the coordinate vector L P(i) of the ultrasonic probe's rotation trajectory point under ⁇ L ⁇ is obtained, and a circle fitting in three-dimensional space is performed on the trajectory point.
  • the rotation parameters (n(0), c(0)) of the ultrasonic probe are obtained:
  • a dual-arm robot puncturing system calibration system comprising:
  • Kinematic analysis module used for kinematic analysis of the dual-arm robot puncture system to obtain parameters to be calibrated of the dual-arm robot puncture system, the parameters to be calibrated include the first initial parameter of the puncture needle, the first translation parameter, and second initial parameters, second translation parameters and rotation parameters of the ultrasound probe;
  • the first parameter calibration module is used to calibrate the first initial parameter of the puncture needle by using the multi-point method, and perform bi-plane ultrasonic imaging on the tip of the puncture needle, and the ultrasonic imaging is performed according to the bi-plane ultrasonic imaging.
  • the second initial parameter of the probe is calibrated;
  • the second parameter calibration module is used to obtain two mutually perpendicular S-plane ultrasonic images and T-plane ultrasonic images by using the ultrasonic probe, and use the S-plane ultrasonic images and T-plane ultrasonic images to track the movements of the ultrasonic probe and the puncture needle respectively track, and calibrate the first translation parameter, the second translation parameter and the rotation parameter through track fitting.
  • the beneficial effects of the embodiments of the present application are: the method and system for calibrating a dual-arm robotic puncture system according to the embodiments of the present application can provide two vertical ultrasound planes by utilizing the high-precision positioning performance of the dual-arm robot and the transrectal ultrasound probe.
  • the characteristics of the multi-degree-of-freedom ultrasonic probe and multi-degree-of-freedom needle tube are analyzed to obtain the parameters to be calibrated for the puncture needle and the ultrasonic probe.
  • the puncture needle and the parameter calibration of the transrectal ultrasound probe makes the calibration of the puncture needle and the ultrasound probe into a closed loop, which can efficiently calibrate the puncture needle and the transrectal ultrasound probe of the dual-arm robotic puncture system at the same time without the use of third-party tracking equipment. Conducive to cost savings.
  • FIG. 1 is a flowchart of a method for calibrating a dual-arm robotic puncture system according to an embodiment of the present application
  • FIG. 2 is a schematic structural diagram of a dual-arm robotic puncture system with a multi-degree-of-freedom end
  • FIG. 3 is a schematic diagram of a geometric model of a transrectal ultrasound probe
  • FIG. 4 is a schematic diagram of setting a coordinate system of a dual-arm robotic puncture system according to an embodiment of the application
  • FIG. 5 is a frame diagram of parameter calibration to be calibrated of the dual-arm robotic puncture system according to an embodiment of the application
  • FIG. 6 is a schematic structural diagram of a calibration system of a dual-arm robotic puncture system according to an embodiment of the present application.
  • the method for calibrating a dual-arm robot puncture system in the embodiment of the present application can provide two vertical ultrasound planes by using the high-precision positioning performance of the dual-arm robot and the transrectal ultrasound probe without using a third-party tracking device.
  • the parameters to be calibrated are divided into two groups. Initial parameters and motion parameters, and then complete the calibration of the initial parameters and motion parameters based on biplane ultrasound images.
  • FIG. 1 is a flowchart of a method for calibrating a dual-arm robotic puncture system according to an embodiment of the present application.
  • the method for calibrating a dual-arm robotic puncture system according to an embodiment of the present application includes the following steps:
  • S10 Perform kinematics analysis on the dual-arm robot puncture system to obtain parameters to be calibrated for the dual-arm robot puncture system, and divide the to-be-calibrated parameters into initial parameters and motion parameters;
  • FIG. 2 is a schematic structural diagram of the dual-arm robotic puncture system. It includes a dual-arm robot, an ultrasound instrument with a transrectal ultrasound probe, and two end effectors; wherein, the two end effectors are respectively installed on the end flanges of the left and right arms of the dual-arm robot, and the end of the right arm is
  • the actuator is equipped with a puncture needle that can translate and rotate about itself.
  • the left arm end effector is fitted with a transrectal ultrasound probe that translates and rotates around itself. Each end effector has an initial position, and each movement starts from the initial position, and then resets to the initial position after the operation is completed.
  • the geometric model of the transrectal ultrasound probe is shown in Figure 3, which provides two mutually perpendicular S planes and T planes.
  • FIG. 4 is a schematic diagram of the coordinate system setting of the dual-arm robot puncturing system, wherein ⁇ L ⁇ and ⁇ R ⁇ represent the end flange coordinate systems of the left and right arms respectively; ⁇ S ⁇ and ⁇ T ⁇ respectively Corresponding to the S-plane ultrasound image coordinate system and the T-plane ultrasound image coordinate system provided by the transrectal ultrasound probe; ⁇ B ⁇ is the base coordinate system of the dual-arm robot (ie, the world coordinate system of the entire system).
  • the kinematics analysis performed on the dual-arm robotic puncture system includes two parts: the kinematics analysis of the puncture needle and the kinematics analysis of the ultrasonic probe, and specifically includes:
  • S11 Perform kinematic analysis on the puncture needle to obtain the to-be-calibrated parameters of the puncture needle;
  • the to-be-calibrated parameters of the puncture needle include a first initial parameter R P(0) and a first translation parameter v n ;
  • the puncture needle rotates around itself without changing its position, and the position of the puncture needle ( B P(d n )) is determined by its initial position ( R P(0) ) and the translation distance (d n ). Its kinematic equation is as follows:
  • B T R is the homogeneous coordinate transformation matrix from ⁇ R ⁇ to ⁇ B ⁇ , which is provided by the robot operating system (ROS);
  • R P(d n ) is the time when the puncture needle is translated d n , the coordinates of the needle tip in ⁇ R ⁇ ;
  • v n is the unit direction vector of the puncture needle when it translates in ⁇ R ⁇ , and is the parameter to be calibrated.
  • S12 Perform kinematic analysis on the ultrasonic probe to obtain parameters to be calibrated for the ultrasonic probe.
  • the parameters to be calibrated for the ultrasonic probe include a second initial parameter L T ST (0,0), a second translation parameter v p and a rotation parameter (n( 0), c(0));
  • n n(0)+d p ⁇ v p
  • B T L is the homogeneous coordinate transformation matrix from ⁇ L ⁇ to ⁇ B ⁇ , which is provided by ROS;
  • L T ST (d p / ⁇ ) is L T S (d p / ⁇ ) and L
  • ST P is SP and The set composed of T P represents the pixel coordinates in the S-plane ultrasound image or the T-plane ultrasound image;
  • R(n, ⁇ ) is the rotation transformation matrix of the coordinate system when the coordinate system rotates about the axis n by ⁇ ;
  • n and c are the ultrasound The normal vector and rotation center coordinates of the probe rotation plane;
  • n(0) and C(0) are the unit normal vector and rotation center coordinates of the rotation plane in ⁇ L ⁇ when the ultrasonic probe is at the initial position,
  • the obtained parameters to be calibrated include a first initial parameter, a first translation parameter, a second initial parameter, a second translation parameter and a rotation parameter, wherein the first translation parameter, the second translation parameter and the rotation parameter
  • the parameters are motion parameters, as shown in Table 1 below:
  • the first initial parameter R P(0) and the second initial parameter L T ST (0,0) are required to perform kinematic modeling; when the ultrasonic probe and the puncture needle are in the initial position
  • the first translation parameter v n and the second translation parameter v p are also required to perform kinematic modeling during translation; when the ultrasonic probe rotates, the rotation parameters (n(0), c(0) are also required. )) before the kinematics modeling during rotation can be performed.
  • the calibration process of the first initial parameter and the second initial parameter is as follows:
  • the first initial parameter R P(0) of the puncture needle is the coordinates of the needle tip under ⁇ R ⁇ when the puncture needle is at the initial position.
  • the embodiment of the present application adopts the multi-point method to calibrate the initial parameter R P(0).
  • the specific calibration process is as follows: first, reset the puncture needle to the initial position; then control the robot to let the needle tip touch a fixed reference point in different postures , record the parameters B T R (i) of the robot under different postures; finally, solve the least squares of the following equation to obtain the first initial parameter R P(0) of the puncture needle:
  • the second initial parameter L T ST (0,0) of the ultrasound probe is the set of parameters L T S (0,0) and L T T (0,0), which is the image coordinate system ( ⁇ S ⁇ and ⁇ T ⁇ ) to ⁇ L ⁇ homogeneous coordinate transformation matrix.
  • the biplane ultrasonic imaging of the needle tip is used to calibrate the second initial parameter L T ST (0,0).
  • the specific calibration process is as follows: first, reset the puncture needle and the ultrasonic probe to the initial position; then control the robot to scan the calibrated needle tip with the ultrasonic probe in different postures, and record the parameters of the robot under different postures ( B T L (i), B T R (i)); finally, the second initial parameter L T ST (0,0) of the ultrasound probe is obtained by solving the least squares of the following equation:
  • S30 Use the transrectal ultrasound probe of the dual-arm robotic puncture system to acquire two mutually perpendicular S-plane ultrasound images and T-plane ultrasound images, and use the S-plane ultrasound images and the T-plane ultrasound images to track the motion trajectories of the ultrasound probe and the puncture needle, respectively,
  • the motion parameters of the dual-arm robot puncture system were calibrated by trajectory fitting;
  • the first translation parameter v n is the unit direction vector of the translation of the puncture needle under ⁇ R ⁇ .
  • the ultrasonic S plane is used to track the translation trajectory of the puncture needle, and v n is calibrated by direct fitting in the three-dimensional space.
  • the specific calibration process is as follows: first, control the robot to make the puncture needle and the ultrasonic probe nearly parallel, so that the S plane can scan the needle tip; then, keep the robot posture unchanged, and control the motor to make the puncture needle start to translate, and save different translation distances Finally, according to the following equation (6), the trajectory points of the translation of the puncture needle tip under ⁇ R ⁇ are obtained, and the trajectory points are fitted with straight lines in the three-dimensional space to obtain the first point of the puncture needle.
  • Translation parameter v n
  • R P(i) B T -1 R ⁇ B T L ⁇ L T S (0,0) ⁇ S P(i) (6)
  • the second translation parameter v p is the unit direction vector of the translation of the ultrasound probe under ⁇ L ⁇ .
  • the ultrasonic S plane is used to track the translation trajectory of the ultrasonic probe, and vp is calibrated by direct fitting in the three-dimensional space.
  • the specific calibration process is as follows: first, control the robot to make the puncture needle and the ultrasonic probe nearly parallel, so that the S plane can scan the needle tip; then, keep the robot posture unchanged, control the motor, make the ultrasonic probe start to translate, and save the data at different translation distances.
  • the rotation parameters (n(0), c(0)) are the normal vector of the rotation plane and the coordinates of the rotation center when the ultrasonic probe is rotated.
  • the embodiment of the present application uses the T plane to track the rotation trajectory of the ultrasonic probe, and then completes the calibration of the rotation parameters (n(0), c(0)) through circle fitting in the three-dimensional space.
  • the specific calibration process is as follows: first, control the robot to make the puncture needle and the ultrasonic probe nearly parallel, and let the T plane intersect the needle; then, keep the robot posture unchanged, control the motor, make the ultrasonic probe start to rotate, and save the rotation angle of the ultrasonic probe.
  • the method for calibrating a dual-arm robot puncture system in the embodiment of the present application utilizes the high-precision positioning performance of the dual-arm robot and the feature that the transrectal ultrasound probe can provide two vertical ultrasound planes, and performs kinematics on the multi-degree-of-freedom ultrasound probe and the multi-degree-of-freedom needle tube.
  • the parameters to be calibrated for the puncture needle and the ultrasonic probe are obtained, and the parameters of the puncture needle and the transrectal ultrasonic probe are calibrated simultaneously based on the two vertical ultrasonic planes provided by the transrectal ultrasonic probe.
  • the embodiment of the present application integrates the mechanism of the dual-arm robot and the ultrasonic image, so that the calibration of the puncture needle and the calibration of the ultrasonic probe become a closed loop, and the puncture needle and the transrectal ultrasonic probe of the dual-arm robot puncture system can be calibrated efficiently at the same time, without the need for The use of third-party tracking equipment is conducive to saving costs.
  • the following embodiments are experimentally tested on an existing dual-arm robotic puncture system guided by a transrectal ultrasound probe. After the calibration of the whole system is completed, the needle tip of the dual-arm robotic puncture system in different motion states (initial state, translation and rotation) is scanned by the ultrasonic probe. Calculate the needle tip coordinates under the needle tube calibration and the needle tip coordinates under the probe calibration respectively, and take the distance between the two as the positioning error.
  • the experimental results show that the embodiment of the present application can efficiently calibrate the dual-arm robot puncture system, and the calibration accuracy is high.
  • FIG. 6 is a schematic structural diagram of a calibration system of a dual-arm robotic puncture system according to an embodiment of the present application.
  • the dual-arm robotic puncture system calibration system 40 in the embodiment of the present application includes:
  • Kinematics analysis module 41 used for kinematic analysis of the dual-arm robot puncture system to obtain parameters to be calibrated for the dual-arm robot puncture system, and to divide the to-be-calibrated parameters into initial parameters and motion parameters;
  • the first parameter calibration module 42 is used to calibrate the first initial parameters of the puncture needle by using the multi-point method based on the results of the kinematic analysis, and to perform biplane ultrasonic imaging on the tip of the puncture needle, and to calibrate the ultrasonic probe according to the biplane ultrasonic imaging.
  • the second initial parameter is calibrated;
  • the second parameter calibration module 43 is used to obtain two mutually perpendicular S-plane ultrasound images and T-plane ultrasound images using the transrectal ultrasound probe of the dual-arm robotic puncture system, and use the S-plane ultrasound images and the T-plane ultrasound images to track the ultrasound probe respectively and the motion trajectory of the puncture needle, and calibrate the motion parameters of the dual-arm robot puncture system by trajectory fitting.

Landscapes

  • Health & Medical Sciences (AREA)
  • Surgery (AREA)
  • Engineering & Computer Science (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Biomedical Technology (AREA)
  • Robotics (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Medical Informatics (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • General Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Ultra Sonic Daignosis Equipment (AREA)

Abstract

A calibration method and system for a dual-arm robot puncture system. The calibration method comprises: performing kinematics analysis on a dual-arm robot puncture system, so as to obtain parameters to be calibrated of the dual-arm robot puncture system; calibrating a first initial parameter of a puncture needle by using a multi-point method, performing biplanar ultrasonic imaging on the tip of the puncture needle, and calibrating a second initial parameter of an ultrasonic probe according to biplanar ultrasonic imaging; and acquiring, by using the ultrasonic probe, an S-plane ultrasonic image and a T-plane ultrasonic image perpendicular to each other, respectively tracking the motion trajectories of the ultrasonic probe and the puncture needle by using the S-plane ultrasonic image and the T-plane ultrasonic image, and calibrating a first translation parameter, a second translation parameter and a rotation parameter by means of trajectory fitting. By means of the method, a puncture needle and a transrectal ultrasonic probe of a dual-arm robot puncture system can be effectively calibrated at the same time, without the need of a third-party tracking device.

Description

一种双臂机器人穿刺系统标定方法及系统A method and system for calibrating a dual-arm robot puncture system 技术领域technical field
本申请属于医学图像处理技术领域，特别涉及一种双臂机器人穿刺系统标定方法及系统。The present application belongs to the technical field of medical image processing, and in particular relates to a calibration method and system for a dual-arm robotic puncture system.
背景技术Background technique
近年来，随着机器人技术和医学图像处理技术的高速发展，使得影像引导的机器人在医学领域取得了优秀的成果。特别是具有多自由度末端的双臂机器人穿刺系统，该系统不仅能分开控制超声探头与穿刺针，实现在狭小的手术空间内灵活地控制入针角度与姿态；而且能提供三维影像，实现高精度的影像定位与良好的视觉效果。In recent years, with the rapid development of robotics and medical image processing technology, image-guided robots have achieved excellent results in the medical field. In particular, the dual-arm robotic puncture system with multi-degree-of-freedom ends can not only control the ultrasonic probe and the puncture needle separately, and flexibly control the needle insertion angle and posture in a small surgical space; Accurate image positioning and good visual effects.
目前，多自由度末端的双臂机器人穿刺系统标定方案主要包括：At present, the calibration scheme of the dual-arm robot puncture system with multi-degree-of-freedom end mainly includes:
一、采用定位传感器和标定模板进行标定。该方法在机器人的左右末端法兰分别安装定位传感器，通过机器人手眼标定，完成定位传感器的标定；然后控制机器人以不同的姿态让超声探头扫描标定模板，通过定位传感器建立超声图像和标定模板之间的坐标变换等式，通过解最小二乘问题，得到超声图像坐标系至定位传感器坐标系间的坐标变换矩阵。1. Use positioning sensor and calibration template for calibration. The method installs positioning sensors on the left and right end flanges of the robot respectively, and completes the calibration of the positioning sensors through the robot hand-eye calibration; then controls the robot to scan the calibration template with the ultrasonic probe in different postures, and establishes the relationship between the ultrasonic image and the calibration template through the positioning sensor. By solving the least squares problem, the coordinate transformation matrix between the ultrasound image coordinate system and the positioning sensor coordinate system is obtained.
二、使用机器人操作和平面校准装置的超声探头标定方法。该方法通过使用机器人操作超声探头扫描平面校准装置，进而建立超声图像和对应平面校准装置实际物理位置之间标定的方程式组合，然后采用方程式求解出超声图像坐标系到机器人操作器坐标系之间的坐标变换矩阵。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 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.
然而，综上所述的标定方法通常面向零自由度末端的双臂机器人穿刺系统进行设计，难以高效地完成对多自由度末端的双臂机器人穿刺系统进行标定。同时，现有标定方法常需要借助定位传感器、相机、模型等第三方跟踪设备， 这些设备本身的定位和尺寸具有一定误差，且成本较高。However, the above-mentioned calibration methods are usually designed for the dual-arm robotic puncture system with a zero-degree-of-freedom end, and it is difficult to efficiently calibrate the dual-arm robotic puncture system with a multi-degree-of-freedom end. At the same time, the existing calibration methods often require third-party tracking devices such as positioning sensors, cameras, and models. These devices have certain errors in their positioning and size, and are costly.
发明内容SUMMARY OF THE INVENTION
本申请提供了一种双臂机器人穿刺系统标定方法及系统，旨在至少在一定程度上解决现有技术中的上述技术问题之一。The present application provides a method and system for calibrating a dual-arm robotic puncture system, 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 method for calibrating a dual-arm robot puncture system, comprising:
对双臂机器人穿刺系统进行运动学分析，得到所述双臂机器人穿刺系统的待标定参数；所述待标定参数包括穿刺针的第一初始参数、第一平移参数，以及超声探头的第二初始参数、第二平移参数和旋转参数；Perform kinematic analysis on the dual-arm robot puncture system to obtain parameters to be calibrated for the dual-arm robot puncture system; the parameters to be calibrated include the first initial parameter of the puncture needle, the first translation parameter, and the second initial parameter of the ultrasonic probe parameter, second translation parameter and rotation parameter;
使用多点法对所述穿刺针的第一初始参数进行标定，并对所述穿刺针的针尖进行双平面超声成像，根据所述双平面超声成像对所述超声探头的第二初始参数进行标定；Using a multi-point method to calibrate the first initial parameter of the puncture needle, perform biplane ultrasonic imaging on the tip of the puncture needle, and calibrate the second initial parameter of the ultrasonic probe according to the biplane ultrasonic imaging ;
利用所述超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用所述S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，并通过轨迹拟合对所述第一平移参数、第二平移参数和旋转参数进行标定。Acquire two mutually perpendicular S-plane ultrasound images and T-plane ultrasound images by using the ultrasound probe, use the S-plane ultrasound images and T-plane ultrasound images to track the motion trajectories of the ultrasound probe and the puncture needle respectively, and use the trajectory fitting to track the movement trajectories of the ultrasound probe and puncture needle. The first translation parameter, the second translation parameter and the rotation parameter are calibrated.
本申请实施例采取的技术方案还包括：所述对双臂机器人穿刺系统进行运动学分析包括：The technical solution adopted in the embodiment of the present application further includes: the kinematic analysis of the dual-arm robot puncturing system includes:
对所述穿刺针进行运动学分析；所述穿刺针绕自身旋转不改变自身的位置，所述穿刺针的位置 BP(d n)由其初始位置 RP(0)和平移距离d n决定，所述穿刺针的运动学等式为： Kinematic analysis is performed on the puncture needle; the puncture needle rotates around itself without changing its position, and the position B P(d n ) of the puncture needle is determined by its initial position R P(0) and the translation distance d n , the kinematic equation of the puncture needle is:
BP(d n)＝ BT R· RP(d n) B P(d n ) = B T R · R P(d n )
RP(d n)＝ RP(0)+v n·d n R P(d n )= R P(0)+v n ·d n
上式中, BT R为{R}到{B}的齐次坐标变换矩阵，{R}表示双臂机器人右臂的末端法兰坐标系，{B}为双臂机器人的基坐标系； RP(d n)为穿刺针平移d n时，其针尖在{R}中的坐标；v n为穿刺针平移时在{R}中的单位方向向量，即第一平移参数。 In the above formula, B T R is the homogeneous coordinate transformation matrix from {R} to {B}, {R} represents the end flange coordinate system of the right arm of the dual-arm robot, and {B} is the base coordinate system of the dual-arm robot; R P(d n ) is the coordinate of the needle tip in {R} when the puncture needle is translated by dn; v n is the unit direction vector in {R} when the puncture needle is translated, that is, the first translation parameter.
本申请实施例采取的技术方案还包括：所述对双臂机器人穿刺系统进行运动学分析包括：The technical solution adopted in the embodiment of the present application further includes: the kinematic analysis of the dual-arm robot puncturing system includes:
对所述超声探头进行运动学分析；所述超声探头绕自身旋转时，其位置由其旋转角度θ决定；所述超声探头平移时，其位置由平移距离d p决定；所述超声探头的运动学等式为： Kinematic analysis is performed on the ultrasonic probe; when the ultrasonic probe rotates around itself, its position is determined by its rotation angle θ; when the ultrasonic probe is translated, its position is determined by the translation distance dp ; the movement of the ultrasonic probe The learning equation is:
BP(d p/θ)＝ BT L· LT ST(d p/θ)· STP B P(d p /θ) = B T L · L T ST (d p /θ) · ST P
Figure PCTCN2021083163-appb-000001
Figure PCTCN2021083163-appb-000001
Figure PCTCN2021083163-appb-000002
Figure PCTCN2021083163-appb-000002
n＝n(0)+d p·v p n=n(0)+d p ·v p
c＝c(0)+d p·v p c=c(0)+d p ·v p
上式中， BT L为{L}到{B}的齐次坐标变换矩阵，{L}表示双臂机器人左臂的末端法兰坐标系； BP(d p/θ)为超声探头平移d p或旋转θ时，超声探头在{B}中的位置向量。 LT ST(d p/θ)为 LT S(d p/θ)和 LT T(d p/θ)组成的集合，为超声探头平移d p或旋转θ时，{S}或{T}到{L}的齐次坐标变换矩阵；{S}和{T}分别对应超声探头的S平面超声图像坐标系和T平面超声图像坐标系； STP为 SP和 TP组成的集合，表示S平面超声图像或T平面超声图像中的像素坐标；R(n,θ)为坐标系绕轴n旋转θ角度时，坐标系的旋转变换矩阵；n和c分别为超声探头旋转平面的法向 量和旋转中心坐标；n(0)和C(0)分别为超声探头处于初始位置时，在{L}中旋转平面的单位法向量和旋转中心坐标，即旋转参数；v p为超声探头在{L}中平移的单位方向向量，即第二平移参数。 In the above formula, B T L is the homogeneous coordinate transformation matrix from {L} to {B}, {L} represents the end flange coordinate system of the left arm of the dual-arm robot; B P(d p /θ) is the translation of the ultrasonic probe d p or rotation θ, the position vector of the ultrasound probe in {B}. L T ST (d p /θ) is the set composed of L T S (d p /θ) and L T T (d p /θ), when the ultrasound probe is translated by d p or rotated by θ, {S} or {T The homogeneous coordinate transformation matrix from } to {L}; {S} and {T} correspond to the S-plane ultrasound image coordinate system and the T-plane ultrasound image coordinate system of the ultrasound probe respectively; ST P is the set composed of SP and T P , Represents the pixel coordinates in the S-plane ultrasound image or the T-plane ultrasound image; R(n, θ) is the rotation transformation matrix of the coordinate system when the coordinate system rotates around the axis n by an angle of θ; n and c are the method of rotating the plane of the ultrasound probe respectively vector and rotation center coordinates; n(0) and C(0) are respectively the unit normal vector and rotation center coordinates of the rotation plane in {L} when the ultrasonic probe is at the initial position, namely the rotation parameters; v p is the ultrasonic probe in the The unit direction vector of translation in {L}, that is, the second translation parameter.
本申请实施例采取的技术方案还包括：所述使用多点法对所述穿刺针的第一初始参数进行标定具体为：The technical solution adopted in the embodiment of the present application further includes: the use of the multi-point method to calibrate the first initial parameter of the puncture needle is specifically:
将所述穿刺针复位至初始位置；reset the puncture needle to the initial position;
控制所述双臂机器人以不同的姿态让针尖碰一个固定参考点，记录所述双臂机器人不同姿态下的参数 BT R(i)； Control the dual-arm robot to let the needle tip touch a fixed reference point with different attitudes, and record the parameters B T R (i) of the dual-arm robot under different attitudes;
基于所述参数 BT R(i)，对下述等式解最小二乘，得到所述穿刺针的第一初始参数 RP(0)： Based on the parameter B T R (i), the least squares are solved for the following equation to obtain the first initial parameter R P(0) of the puncture needle:
BT R(i-1)· RP(0)＝ BT R(i)· RP(0),i＝1...n B T R (i-1) · R P(0) = B T R (i) · R P(0), i=1...n
本申请实施例采取的技术方案还包括：所述根据所述双平面超声成像对所述超声探头的第二初始参数进行标定具体为：The technical solution adopted in the embodiment of the present application further includes: the calibration of the second initial parameter of the ultrasonic probe according to the biplane ultrasonic imaging is specifically:
将所述穿刺针和超声探头复位至初始位置；resetting the puncture needle and the ultrasonic probe to the initial position;
控制所述双臂机器人以不同的姿态让所述超声探头扫描标定好的针尖，记录所述双臂机器人不同姿态下的参数( BT L(i), BT R(i))； Control the dual-arm robot to scan the calibrated needle tip with the ultrasonic probe in different postures, and record the parameters of the dual-arm robot under different postures ( B T L (i), B T R (i));
基于所述参数( BT L(i), BT R(i))，对下述等式解最小二乘，得到所述超声探头的第二初始参数 LT ST(0,0)： Based on the parameters ( B T L (i), B T R (i)), least squares are solved for the following equation to obtain the second initial parameter L T ST (0,0) of the ultrasonic probe:
Figure PCTCN2021083163-appb-000003
Figure PCTCN2021083163-appb-000003
本申请实施例采取的技术方案还包括：所述对所述第一平移参数进行标定包括：The technical solution adopted in the embodiment of the present application further includes: the calibrating the first translation parameter includes:
控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让所述S平面能够扫描到针尖；Controlling the dual-arm robot to make the puncture needle and the ultrasonic probe nearly parallel, so that the S plane can scan the needle tip;
保持所述双臂机器人姿态不变，并控制电机，使所述穿刺针开始平移，并保存不同平移距离下的S平面超声图像；Keep the posture of the dual-arm robot unchanged, and control the motor to make the puncture needle start to translate, and save the S-plane ultrasound images under different translation distances;
基于所述S平面超声图像，根据下述等式，求得所述针尖在{ER}下平移的轨迹点坐标 RP(i)，对所述轨迹点做三维空间中的直线拟合，得到所述穿刺针的第一平移参数v n： Based on the S-plane ultrasound image, according to the following equation, the trajectory point coordinates R P(i) of the needle tip translation under {ER} are obtained, and a straight line in the three-dimensional space is fitted to the trajectory points to obtain The first translation parameter v n of the puncture needle:
RP(i)＝ BT -1 R· BT L· LT S(0,0)· SP(i) R P(i) = B T -1 R · B T L · L T S (0,0) · S P(i)
本申请实施例采取的技术方案还包括：所述对所述第二平移参数进行标定包括：The technical solution adopted in the embodiment of the present application further includes: the calibrating the second translation parameter includes:
控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让S平面能扫描到针尖；Controlling the dual-arm robot to make the puncture needle and the ultrasonic probe nearly parallel, so that the S plane can scan the needle tip;
保持所述双臂机器人姿态不变，控制电机，使所述超声探头开始平移，并保存不同平移距离下的S平面超声图像；Keep the posture of the dual-arm robot unchanged, control the motor, make the ultrasonic probe start to translate, and save the S-plane ultrasonic images under different translation distances;
基于所述S平面超声图像，根据下述等式，求得所述超声探头在{EL}下平移轨迹点的坐标向量 LP(i)，对所述轨迹点做三维空间中的直线拟合，求得所述超声探头的第二平移参数v p： Based on the S-plane ultrasound image, according to the following equation, obtain the coordinate vector L P(i) of the ultrasound probe's translation trajectory point under {EL}, and perform a straight line fitting in the three-dimensional space on the trajectory point , obtain the second translation parameter v p of the ultrasonic probe:
LP(i)＝ LT S(0,0)· SP(i) L P(i) = L T S (0,0) · S P(i)
本申请实施例采取的技术方案还包括：所述对所述旋转参数进行标定包括：The technical solution adopted in the embodiment of the present application further includes: the calibrating the rotation parameter includes:
控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让T平面与穿刺针相交；Controlling the dual-arm robot to make the puncture needle and the ultrasonic probe nearly parallel, and let the T plane intersect the puncture needle;
保持所述双臂机器人姿态不变，控制电机，使所述超声探头开始旋转，并保存不同旋转角度下的T平面超声图像；Keep the posture of the dual-arm robot unchanged, control the motor, make the ultrasonic probe start to rotate, and save the T-plane ultrasonic images under different rotation angles;
基于所述T平面超声图像，根据下述等式，求得所述超声探头在{L}下旋 转轨迹点的坐标向量 LP(i)，对所述轨迹点做三维空间中的圆拟合，得到所述超声探头的旋转参数(n(0),c(0))： Based on the T-plane ultrasonic image, according to the following equation, the coordinate vector L P(i) of the ultrasonic probe's rotation trajectory point under {L} is obtained, and a circle fitting in three-dimensional space is performed on the trajectory point. , the rotation parameters (n(0), c(0)) of the ultrasonic probe are obtained:
LP(i)＝ LT T(0,0)· TP(i) L P(i) = L T T (0,0) · T P(i)
本申请实施例采取的另一技术方案为：一种双臂机器人穿刺系统标定系统，包括：Another technical solution adopted in the embodiment of the present application is: a dual-arm robot puncturing system calibration system, comprising:
运动学分析模块：用于对双臂机器人穿刺系统进行运动学分析，得到所述双臂机器人穿刺系统的待标定参数，所述待标定参数包括穿刺针的第一初始参数、第一平移参数、以及超声探头的第二初始参数、第二平移参数和旋转参数；Kinematic analysis module: used for kinematic analysis of the dual-arm robot puncture system to obtain parameters to be calibrated of the dual-arm robot puncture system, the parameters to be calibrated include the first initial parameter of the puncture needle, the first translation parameter, and second initial parameters, second translation parameters and rotation parameters of the ultrasound probe;
第一参数标定模块：用于使用多点法对所述穿刺针的第一初始参数进行标定，并对所述穿刺针的针尖进行双平面超声成像，根据所述双平面超声成像对所述超声探头的第二初始参数进行标定；The first parameter calibration module is used to calibrate the first initial parameter of the puncture needle by using the multi-point method, and perform bi-plane ultrasonic imaging on the tip of the puncture needle, and the ultrasonic imaging is performed according to the bi-plane ultrasonic imaging. The second initial parameter of the probe is calibrated;
第二参数标定模块：用于利用所述超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用所述S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，并通过轨迹拟合对所述第一平移参数、第二平移参数和旋转参数进行标定。The second parameter calibration module is used to obtain two mutually perpendicular S-plane ultrasonic images and T-plane ultrasonic images by using the ultrasonic probe, and use the S-plane ultrasonic images and T-plane ultrasonic images to track the movements of the ultrasonic probe and the puncture needle respectively track, and calibrate the first translation parameter, the second translation parameter and the rotation parameter through track fitting.
相对于现有技术，本申请实施例产生的有益效果在于：本申请实施例的双臂机器人穿刺系统标定方法及系统利用双臂机器人高精度定位性能和经直肠超声探头能够提供两个垂直超声平面的特点，对多自由度的超声探头和多自由度针管进行运动学分析，得到穿刺针和超声探头的待标定参数，并基于经直肠超声探头提供的两个垂直超声平面同时实现对穿刺针和经直肠超声探头的参数标定，使得穿刺针管标定和超声探头标定成为一个闭环，能够高效地对双臂机器人穿刺系统的穿刺针和经直肠超声探头同时进行标定，且无需采用第三方跟踪设备，有利于节约成本。Compared with the prior art, the beneficial effects of the embodiments of the present application are: the method and system for calibrating a dual-arm robotic puncture system according to the embodiments of the present application can provide two vertical ultrasound planes by utilizing the high-precision positioning performance of the dual-arm robot and the transrectal ultrasound probe. The characteristics of the multi-degree-of-freedom ultrasonic probe and multi-degree-of-freedom needle tube are analyzed to obtain the parameters to be calibrated for the puncture needle and the ultrasonic probe. Based on the two vertical ultrasonic planes provided by the transrectal ultrasonic probe, the puncture needle and The parameter calibration of the transrectal ultrasound probe makes the calibration of the puncture needle and the ultrasound probe into a closed loop, which can efficiently calibrate the puncture needle and the transrectal ultrasound probe of the dual-arm robotic puncture system at the same time without the use of third-party tracking equipment. Conducive to cost savings.
附图说明Description of drawings
图1是本申请实施例的双臂机器人穿刺系统标定方法的流程图；1 is a flowchart of a method for calibrating a dual-arm robotic puncture system according to an embodiment of the present application;
图2为具有多自由度末端的双臂机器人穿刺系统的结构示意图；2 is a schematic structural diagram of a dual-arm robotic puncture system with a multi-degree-of-freedom end;
图3为经直肠超声探头的几何模型示意图；3 is a schematic diagram of a geometric model of a transrectal ultrasound probe;
图4为本申请实施例的双臂机器人穿刺系统的坐标系设置示意图；4 is a schematic diagram of setting a coordinate system of a dual-arm robotic puncture system according to an embodiment of the application;
图5为本申请实施例的双臂机器人穿刺系统的待标定参数标定框架图；FIG. 5 is a frame diagram of parameter calibration to be calibrated of the dual-arm robotic puncture system according to an embodiment of the application;
图6为本申请实施例的双臂机器人穿刺系统标定系统结构示意图。FIG. 6 is a schematic structural diagram of a calibration system of a dual-arm robotic puncture system according to an embodiment of the present application.
具体实施方式Detailed ways
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请。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.
针对现有技术的不足，本申请实施例的双臂机器人穿刺系统标定方法在不利用第三方跟踪设备的情况下，利用双臂机器人高精度定位性能和经直肠超声探头能够提供两个垂直超声平面的特点，建立了一种基于双平面超声图像的标定方法，对多自由度的超声探头和多自由度针管进行运动学分析，根据超声探头和穿刺针的不同运动状态，将待标定参数分为初始参数和运动参数，然后基于双平面超声图像完成初始参数和运动参数的标定。In view of the deficiencies of the prior art, the method for calibrating a dual-arm robot puncture system in the embodiment of the present application can provide two vertical ultrasound planes by using the high-precision positioning performance of the dual-arm robot and the transrectal ultrasound probe without using a third-party tracking device. According to the different motion states of the ultrasonic probe and the puncture needle, the parameters to be calibrated are divided into two groups. Initial parameters and motion parameters, and then complete the calibration of the initial parameters and motion parameters based on biplane ultrasound images.
具体地，请参阅图1，是本申请实施例的双臂机器人穿刺系统标定方法的流程图。本申请实施例的双臂机器人穿刺系统标定方法包括以下步骤：Specifically, please refer to FIG. 1 , which is a flowchart of a method for calibrating a dual-arm robotic puncture system according to an embodiment of the present application. The method for calibrating a dual-arm robotic puncture system according to an embodiment of the present application includes the following steps:
S10：对双臂机器人穿刺系统进行运动学分析，得到双臂机器人穿刺系统的待标定参数，将待标定参数划分为初始参数和运动参数；S10: Perform kinematics analysis on the dual-arm robot puncture system to obtain parameters to be calibrated for the dual-arm robot puncture system, and divide the to-be-calibrated parameters into initial parameters and motion parameters;
本步骤中，请一并参阅图2，为双臂机器人穿刺系统的结构示意图。其包括一个双臂机器人、一台带有经直肠超声探头的超声仪以及两个末端执行器；其中，两个末端执行器分别安装在双臂机器人的左右臂的末端法兰上，右臂末 端执行器装配有穿刺针，穿刺针可绕自身平移和旋转。左臂末端执行器装配有经直肠超声探头，经直肠超声探头可绕自身平移和旋转。每个末端执行器都设有初始位置，每次运动由初始位置开始，操作完成后再复位至初始位置。经直肠超声探头的几何模型如图3所示,它提供了两个相互垂直的S平面和T平面。In this step, please refer to FIG. 2 , which is a schematic structural diagram of the dual-arm robotic puncture system. It includes a dual-arm robot, an ultrasound instrument with a transrectal ultrasound probe, and two end effectors; wherein, the two end effectors are respectively installed on the end flanges of the left and right arms of the dual-arm robot, and the end of the right arm is The actuator is equipped with a puncture needle that can translate and rotate about itself. The left arm end effector is fitted with a transrectal ultrasound probe that translates and rotates around itself. Each end effector has an initial position, and each movement starts from the initial position, and then resets to the initial position after the operation is completed. The geometric model of the transrectal ultrasound probe is shown in Figure 3, which provides two mutually perpendicular S planes and T planes.
进一步地，请参阅图4，为双臂机器人穿刺系统的坐标系设置示意图，其中，{L}和{R}分别表示左、右臂的末端法兰坐标系；{S}和{T}分别对应经直肠超声探头提供的S平面超声图像坐标系和T平面超声图像坐标系；{B}为双臂机器人的基坐标系(即整个系统的世界坐标系)。本申请实施例中，对双臂机器人穿刺系统进行的运动学分析包括穿刺针的运动学分析和超声探头的运动学分析两个部分，具体包括：Further, please refer to FIG. 4, which is a schematic diagram of the coordinate system setting of the dual-arm robot puncturing system, wherein {L} and {R} represent the end flange coordinate systems of the left and right arms respectively; {S} and {T} respectively Corresponding to the S-plane ultrasound image coordinate system and the T-plane ultrasound image coordinate system provided by the transrectal ultrasound probe; {B} is the base coordinate system of the dual-arm robot (ie, the world coordinate system of the entire system). In the embodiment of the present application, the kinematics analysis performed on the dual-arm robotic puncture system includes two parts: the kinematics analysis of the puncture needle and the kinematics analysis of the ultrasonic probe, and specifically includes:
S11：对穿刺针进行运动学分析，得到穿刺针的待标定参数；穿刺针的待标定参数包括第一初始参数 RP(0)和第一平移参数v n； S11: Perform kinematic analysis on the puncture needle to obtain the to-be-calibrated parameters of the puncture needle; the to-be-calibrated parameters of the puncture needle include a first initial parameter R P(0) and a first translation parameter v n ;
穿刺针绕自身旋转不改变自身的位置，穿刺针的位置( BP(d n))由其初始位置( RP(0))和平移距离(d n)决定。其运动学等式如下： The puncture needle rotates around itself without changing its position, and the position of the puncture needle ( B P(d n )) is determined by its initial position ( R P(0) ) and the translation distance (d n ). Its kinematic equation is as follows:
BP(d n)＝ BT R· RP(d n)   (1) B P(d n ) = B T R · R P(d n ) (1)
RP(d n)＝ RP(0)+v n·d n   (2) R P(d n ) = R P(0)+v n ·d n (2)
式(1)、(2)中, BT R为{R}到{B}的齐次坐标变换矩阵，由机器人操作系统(ROS)提供； RP(d n)为穿刺针平移d n时，其针尖在{R}中的坐标；v n为穿刺针在{R}中平移时的单位方向向量，为待标定参数。 In formulas (1) and (2), B T R is the homogeneous coordinate transformation matrix from {R} to {B}, which is provided by the robot operating system (ROS); R P(d n ) is the time when the puncture needle is translated d n , the coordinates of the needle tip in {R}; v n is the unit direction vector of the puncture needle when it translates in {R}, and is the parameter to be calibrated.
S12：对超声探头进行运动学分析，得到超声探头的待标定参数，超声探头的待标定参数包括第二初始参数 LT ST(0,0)、第二平移参数v p和旋转参数(n(0),c(0))； S12: Perform kinematic analysis on the ultrasonic probe to obtain parameters to be calibrated for the ultrasonic probe. The parameters to be calibrated for the ultrasonic probe include a second initial parameter L T ST (0,0), a second translation parameter v p and a rotation parameter (n( 0), c(0));
当超声探头绕自身旋转时，超声探头的位置( BP(d p/θ))由初始位置和旋 转角度(θ)决定；当超声探头平移时，超声探头的位置由初始位置和平移距离(d p)决定。其运动学等式如下： When the ultrasonic probe rotates around itself, the position of the ultrasonic probe ( B P(d p /θ)) is determined by the initial position and the rotation angle (θ); when the ultrasonic probe is translated, the position of the ultrasonic probe is determined by the initial position and the translation distance ( d p ) decision. Its kinematic equation is as follows:
BP(d p/θ)＝ BT L· LT ST(d p/θ)· STP B P(d p /θ) = B T L · L T ST (d p /θ) · ST P
Figure PCTCN2021083163-appb-000004
Figure PCTCN2021083163-appb-000004
Figure PCTCN2021083163-appb-000005
Figure PCTCN2021083163-appb-000005
n＝n(0)+d p·v p n=n(0)+d p ·v p
c＝c(0)+d p·v p   (3) c=c(0)+ dp · vp (3)
式(3)中， BT L为{L}到{B}的齐次坐标变换矩阵，由ROS提供； LT ST(d p/θ)为为 LT S(d p/θ)和 LT T(d p/θ)组成的集合，表示超声探头平移d p或旋转θ时，图像坐标系{S}或{T}到{L}的齐次坐标变换矩阵； STP为 SP和 TP组成的集合，表示S平面超声图像或T平面超声图像中的像素坐标；R(n,θ)为坐标系绕轴n旋转θ时，坐标系的旋转变换矩阵；n和c分别为超声探头旋转平面的法向量和旋转中心坐标；n(0)和C(0)分别为超声探头处于初始位置时，在{L}中旋转平面的单位法向量和旋转中心坐标，为待标定参数；v p为超声探头在{L}中平移的单位方向向量，为待标定参数。 In formula (3), B T L is the homogeneous coordinate transformation matrix from {L} to {B}, which is provided by ROS; L T ST (d p /θ) is L T S (d p /θ) and L The set consisting of T T (d p /θ), which represents the homogeneous coordinate transformation matrix of the image coordinate system {S} or {T} to {L} when the ultrasound probe is translated by d p or rotated by θ; ST P is SP and The set composed of T P represents the pixel coordinates in the S-plane ultrasound image or the T-plane ultrasound image; R(n, θ) is the rotation transformation matrix of the coordinate system when the coordinate system rotates about the axis n by θ; n and c are the ultrasound The normal vector and rotation center coordinates of the probe rotation plane; n(0) and C(0) are the unit normal vector and rotation center coordinates of the rotation plane in {L} when the ultrasonic probe is at the initial position, and are the parameters to be calibrated; v p is the unit direction vector of the ultrasonic probe translation in {L}, and is the parameter to be calibrated.
根据上述运动学分析结果，得到的待标定参数包括穿第一初始参数、第一平移参数、第二初始参数、第二平移参数和旋转参数，其中，第一平移参数、第二平移参数以及旋转参数为运动参数，具体如下表1所示：According to the above kinematic analysis results, the obtained parameters to be calibrated include a first initial parameter, a first translation parameter, a second initial parameter, a second translation parameter and a rotation parameter, wherein the first translation parameter, the second translation parameter and the rotation parameter The parameters are motion parameters, as shown in Table 1 below:
表1待标定参数分类Table 1 Classification of parameters to be calibrated
Figure PCTCN2021083163-appb-000006
Figure PCTCN2021083163-appb-000006
Figure PCTCN2021083163-appb-000007
Figure PCTCN2021083163-appb-000007
当超声探头和穿刺针都处于初始位置时，需求得第一初始参数 RP(0)和第二初始参数 LT ST(0,0)才可进行运动学建模；当超声探头和穿刺针平移时，还需求得第一平移参数v n和第二平移参数v p才可进行平移时的运动学建模；当超声探头旋转时，还需求得旋转参数(n(0),c(0))才可进行旋转时的运动学建模。 When both the ultrasonic probe and the puncture needle are in the initial position, the first initial parameter R P(0) and the second initial parameter L T ST (0,0) are required to perform kinematic modeling; when the ultrasonic probe and the puncture needle are in the initial position When translating, the first translation parameter v n and the second translation parameter v p are also required to perform kinematic modeling during translation; when the ultrasonic probe rotates, the rotation parameters (n(0), c(0) are also required. )) before the kinematics modeling during rotation can be performed.
S20：基于运动学分析结果，使用多点法对穿刺针的第一初始参数进行标定，并对穿刺针的针尖进行双平面超声成像，根据双平面超声成像对超声探头的第二初始参数进行标定；S20: Based on the kinematic analysis result, use the multi-point method to calibrate the first initial parameter of the puncture needle, perform biplane ultrasonic imaging on the tip of the puncture needle, and calibrate the second initial parameter of the ultrasonic probe according to the biplane ultrasonic imaging ;
本步骤中，第一初始参数和第二初始参数的标定过程具体如下：In this step, the calibration process of the first initial parameter and the second initial parameter is as follows:
穿刺针的第一初始参数 RP(0)为穿刺针在初始位置时，针尖在{R}下的坐标。本申请实施例采用多点法对初始参数 RP(0)进行标定，具体标定过程为：首先，让穿刺针复位至初始位置；然后控制机器人以不同的姿态让针尖去碰一个固定的参考点，记录下机器人不同姿态下的参数 BT R(i)；最后对下述等式解最小二乘，得到穿刺针的第一初始参数 RP(0)： The first initial parameter R P(0) of the puncture needle is the coordinates of the needle tip under {R} when the puncture needle is at the initial position. The embodiment of the present application adopts the multi-point method to calibrate the initial parameter R P(0). The specific calibration process is as follows: first, reset the puncture needle to the initial position; then control the robot to let the needle tip touch a fixed reference point in different postures , record the parameters B T R (i) of the robot under different postures; finally, solve the least squares of the following equation to obtain the first initial parameter R P(0) of the puncture needle:
BT R(i-1)· RP(0)＝ BT R(i)· RP(0),i＝1...n   (4) B T R (i-1) · R P(0) = B T R (i) · R P(0), i = 1...n (4)
超声探头的第二初始参数 LT ST(0,0)是参数 LT S(0,0)和 LT T(0,0)的集合，为超声探头处于初始位置时，图像坐标系({S}和{T})到{L}的齐次坐标变换矩阵。本申请实施例采用针尖的双平面超声成像对第二初始参数 LT ST(0,0)进行标定。具体标定过程为：首先，让穿刺针和超声探头复位至初始位置；然后控制机器人以不同的姿态让超声探头扫描标定好的针尖，记录机器人不同姿态下的参数( BT L(i), BT R(i))；最后，通过对下述等式解最小二乘，得到超声探头的第二初始参数 LT ST(0,0)： The second initial parameter L T ST (0,0) of the ultrasound probe is the set of parameters L T S (0,0) and L T T (0,0), which is the image coordinate system ({ S} and {T}) to {L} homogeneous coordinate transformation matrix. In this embodiment of the present application, the biplane ultrasonic imaging of the needle tip is used to calibrate the second initial parameter L T ST (0,0). The specific calibration process is as follows: first, reset the puncture needle and the ultrasonic probe to the initial position; then control the robot to scan the calibrated needle tip with the ultrasonic probe in different postures, and record the parameters of the robot under different postures ( B T L (i), B T R (i)); finally, the second initial parameter L T ST (0,0) of the ultrasound probe is obtained by solving the least squares of the following equation:
Figure PCTCN2021083163-appb-000008
Figure PCTCN2021083163-appb-000008
S30：利用双臂机器人穿刺系统的经直肠超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，通过轨迹拟合对双臂机器人穿刺系统的运动参数进行标定；S30: Use the transrectal ultrasound probe of the dual-arm robotic puncture system to acquire two mutually perpendicular S-plane ultrasound images and T-plane ultrasound images, and use the S-plane ultrasound images and the T-plane ultrasound images to track the motion trajectories of the ultrasound probe and the puncture needle, respectively, The motion parameters of the dual-arm robot puncture system were calibrated by trajectory fitting;
本步骤中，双臂机器人穿刺系统的待标定参数标定过程如图5所示，运动参数的标定过程具体包括：In this step, the calibration process of the parameters to be calibrated of the dual-arm robot puncture system is shown in Figure 5, and the calibration process of the motion parameters specifically includes:
第一平移参数v n为穿刺针在{R}下平移的单位方向向量。本申请实施例采用超声S平面跟踪穿刺针的平移轨迹，并通过三维空间的直接拟合对v n进行标定。具体标定过程为：首先，控制机器人，使穿刺针和超声探头接近平行，让S平面能扫描到针尖；然后，保持机器人姿态不变，并控制电机，使穿刺针开始平移，并保存不同平移距离下的S平面超声图像；最后，根据下述等式(6)，求得穿刺针尖在{R}下平移的轨迹点，对轨迹点做三维空间中的直线拟合，得到穿刺针的第一平移参数v n： The first translation parameter v n is the unit direction vector of the translation of the puncture needle under {R}. In the embodiment of the present application, the ultrasonic S plane is used to track the translation trajectory of the puncture needle, and v n is calibrated by direct fitting in the three-dimensional space. The specific calibration process is as follows: first, control the robot to make the puncture needle and the ultrasonic probe nearly parallel, so that the S plane can scan the needle tip; then, keep the robot posture unchanged, and control the motor to make the puncture needle start to translate, and save different translation distances Finally, according to the following equation (6), the trajectory points of the translation of the puncture needle tip under {R} are obtained, and the trajectory points are fitted with straight lines in the three-dimensional space to obtain the first point of the puncture needle. Translation parameter v n :
RP(i)＝ BT -1 R· BT L· LT S(0,0)· SP(i)    (6) R P(i) = B T -1 R · B T L · L T S (0,0) · S P(i) (6)
第二平移参数v p为超声探头在{L}下平移的单位方向向量。本申请实施例采用超声S平面跟踪超声探头的平移轨迹，并通过三维空间的直接拟合对v p进行标定。具体标定过程为：首先，控制机器人，使穿刺针和超声探头接近平行，让S平面能扫描到针尖；然后，保持机器人姿态不变，控制电机，使超声探头开始平移，保存不同平移距离下的S平面超声图像；最后，根据下述等式(7)，求得超声探头在{L}下平移轨迹点的坐标向量 LP(i)，对轨迹点做三维空间中的直线拟合，求得超声探头的第二平移参数v p： The second translation parameter v p is the unit direction vector of the translation of the ultrasound probe under {L}. In the embodiment of the present application, the ultrasonic S plane is used to track the translation trajectory of the ultrasonic probe, and vp is calibrated by direct fitting in the three-dimensional space. The specific calibration process is as follows: first, control the robot to make the puncture needle and the ultrasonic probe nearly parallel, so that the S plane can scan the needle tip; then, keep the robot posture unchanged, control the motor, make the ultrasonic probe start to translate, and save the data at different translation distances. S-plane ultrasound image; finally, according to the following equation (7), the coordinate vector L P(i) of the ultrasound probe’s translation trajectory point under {L} is obtained, and a straight line in the three-dimensional space is fitted to the trajectory point to find Obtain the second translation parameter v p of the ultrasound probe:
LP(i)＝ LT S(0,0)· SP(i)    (7) L P(i) = L T S (0,0) · S P(i) (7)
旋转参数(n(0),c(0))为超声探头旋转时，旋转平面的法向量和旋转中心坐标。本申请实施例采用T平面跟踪超声探头的旋转轨迹，然后通过三维空间的圆拟合，完成对旋转参数(n(0),c(0))的标定。具体标定过程为：首先，控制机器人，使穿刺针和超声探头接近平行，让T平面与针相交；然后，保持机器人姿态不变，控制电机，使超声探头开始旋转，并保存不同旋转角度下的T平面超声图像；最后，根据下述等式(8)，求得超声探头在{L}下旋转轨迹点的坐标向量 LP(i)，对轨迹点做三维空间中的圆拟合，得到超声探头的旋转参数(n(0),c(0))： The rotation parameters (n(0), c(0)) are the normal vector of the rotation plane and the coordinates of the rotation center when the ultrasonic probe is rotated. The embodiment of the present application uses the T plane to track the rotation trajectory of the ultrasonic probe, and then completes the calibration of the rotation parameters (n(0), c(0)) through circle fitting in the three-dimensional space. The specific calibration process is as follows: first, control the robot to make the puncture needle and the ultrasonic probe nearly parallel, and let the T plane intersect the needle; then, keep the robot posture unchanged, control the motor, make the ultrasonic probe start to rotate, and save the rotation angle of the ultrasonic probe. T-plane ultrasonic image; finally, according to the following equation (8), the coordinate vector L P(i) of the ultrasonic probe’s rotating trajectory point under {L} is obtained, and the trajectory point is fitted with a circle in the three-dimensional space to obtain Rotation parameters of the ultrasound probe (n(0), c(0)):
LP(i)＝ LT T(0,0)· TP(i)   (8) L P(i) = L T T (0,0) · T P(i) (8)
本申请实施例的双臂机器人穿刺系统标定方法利用双臂机器人高精度定位性能和经直肠超声探头能够提供两个垂直超声平面的特点，对多自由度的超声探头和多自由度针管进行运动学分析，得到穿刺针和超声探头的待标定参数，并基于经直肠超声探头提供的两个垂直超声平面同时实现对穿刺针和经直肠超声探头的参数标定。本申请实施例融合了双臂机器人的机构和超声图像，使得穿刺针管标定和超声探头标定成为一个闭环，能够高效地对双臂机器人穿刺系统的穿刺针和经直肠超声探头同时进行标定，且无需采用第三方跟踪设备，有利于节约成本。The method for calibrating a dual-arm robot puncture system in the embodiment of the present application utilizes the high-precision positioning performance of the dual-arm robot and the feature that the transrectal ultrasound probe can provide two vertical ultrasound planes, and performs kinematics on the multi-degree-of-freedom ultrasound probe and the multi-degree-of-freedom needle tube. Through analysis, the parameters to be calibrated for the puncture needle and the ultrasonic probe are obtained, and the parameters of the puncture needle and the transrectal ultrasonic probe are calibrated simultaneously based on the two vertical ultrasonic planes provided by the transrectal ultrasonic probe. The embodiment of the present application integrates the mechanism of the dual-arm robot and the ultrasonic image, so that the calibration of the puncture needle and the calibration of the ultrasonic probe become a closed loop, and the puncture needle and the transrectal ultrasonic probe of the dual-arm robot puncture system can be calibrated efficiently at the same time, without the need for The use of third-party tracking equipment is conducive to saving costs.
为了验证本申请实施例的可行性和有效性，以下实施例通过在已有的经直肠超声探头引导的双臂机器人穿刺系统上进行了实验测试。在整个系统完成标定后，通过超声探头对双臂机器人穿刺系统在不同的运动状态(初始状态、平移和旋转)下的针尖进行扫描。分别计算针管标定下的针尖坐标以及探头标定下的针尖坐标，以两者的距离为定位误差,实验得到的不同运动状态下的针尖定位误差均可达到1mm以内。实验结果表明，本申请实施例能够高效的对双臂 机器人穿刺系统进行标定，且标定精度较高。In order to verify the feasibility and effectiveness of the embodiments of the present application, the following embodiments are experimentally tested on an existing dual-arm robotic puncture system guided by a transrectal ultrasound probe. After the calibration of the whole system is completed, the needle tip of the dual-arm robotic puncture system in different motion states (initial state, translation and rotation) is scanned by the ultrasonic probe. Calculate the needle tip coordinates under the needle tube calibration and the needle tip coordinates under the probe calibration respectively, and take the distance between the two as the positioning error. The experimental results show that the embodiment of the present application can efficiently calibrate the dual-arm robot puncture system, and the calibration accuracy is high.
请参阅图6，为本申请实施例的双臂机器人穿刺系统标定系统结构示意图。本申请实施例的双臂机器人穿刺系统标定系统40包括：Please refer to FIG. 6 , which is a schematic structural diagram of a calibration system of a dual-arm robotic puncture system according to an embodiment of the present application. The dual-arm robotic puncture system calibration system 40 in the embodiment of the present application includes:
运动学分析模块41：用于对双臂机器人穿刺系统进行运动学分析，得到双臂机器人穿刺系统的待标定参数，将待标定参数划分为初始参数和运动参数；Kinematics analysis module 41: used for kinematic analysis of the dual-arm robot puncture system to obtain parameters to be calibrated for the dual-arm robot puncture system, and to divide the to-be-calibrated parameters into initial parameters and motion parameters;
第一参数标定模块42：用于基于运动学分析结果，使用多点法对穿刺针的第一初始参数进行标定，并对穿刺针的针尖进行双平面超声成像，根据双平面超声成像对超声探头的第二初始参数进行标定；The first parameter calibration module 42 is used to calibrate the first initial parameters of the puncture needle by using the multi-point method based on the results of the kinematic analysis, and to perform biplane ultrasonic imaging on the tip of the puncture needle, and to calibrate the ultrasonic probe according to the biplane ultrasonic imaging. The second initial parameter is calibrated;
第二参数标定模块43：用于利用双臂机器人穿刺系统的经直肠超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，通过轨迹拟合对双臂机器人穿刺系统的运动参数进行标定。The second parameter calibration module 43 is used to obtain two mutually perpendicular S-plane ultrasound images and T-plane ultrasound images using the transrectal ultrasound probe of the dual-arm robotic puncture system, and use the S-plane ultrasound images and the T-plane ultrasound images to track the ultrasound probe respectively and the motion trajectory of the puncture needle, and calibrate the motion parameters of the dual-arm robot puncture system by trajectory fitting.
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本发明中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本发明所示的这些实施例，而是要符合与本发明所公开的原理和新颖特点相一致的最宽的范围。The above description of the disclosed embodiments enables any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined in this invention may be implemented in other embodiments without departing from the spirit or scope of this invention. Thus, the present invention is not intended to be limited to the embodiments of the present invention shown, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims (9)

  1. 一种双臂机器人穿刺系统标定方法，其特征在于，包括：A method for calibrating a dual-arm robot puncture system, comprising:
    对双臂机器人穿刺系统进行运动学分析，得到所述双臂机器人穿刺系统的待标定参数；所述待标定参数包括穿刺针的第一初始参数、第一平移参数，以及超声探头的第二初始参数、第二平移参数和旋转参数；Perform kinematic analysis on the dual-arm robot puncture system to obtain parameters to be calibrated for the dual-arm robot puncture system; the parameters to be calibrated include the first initial parameter of the puncture needle, the first translation parameter, and the second initial parameter of the ultrasonic probe parameter, second translation parameter and rotation parameter;
    使用多点法对所述穿刺针的第一初始参数进行标定，并对所述穿刺针的针尖进行双平面超声成像，根据所述双平面超声成像对所述超声探头的第二初始参数进行标定；Using a multi-point method to calibrate the first initial parameter of the puncture needle, perform biplane ultrasonic imaging on the tip of the puncture needle, and calibrate the second initial parameter of the ultrasonic probe according to the biplane ultrasonic imaging ;
    利用所述超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用所述S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，并通过轨迹拟合对所述第一平移参数、第二平移参数和旋转参数进行标定。Acquire two mutually perpendicular S-plane ultrasound images and T-plane ultrasound images by using the ultrasound probe, use the S-plane ultrasound images and T-plane ultrasound images to track the motion trajectories of the ultrasound probe and the puncture needle respectively, and use the trajectory fitting to track the movement trajectories of the ultrasound probe and puncture needle. The first translation parameter, the second translation parameter and the rotation parameter are calibrated.
  2. 根据权利要求1所述的双臂机器人穿刺系统标定方法，其特征在于，所述对双臂机器人穿刺系统进行运动学分析包括：The method for calibrating a dual-arm robot puncture system according to claim 1, wherein the kinematic analysis of the dual-arm robot puncture system comprises:
    对所述穿刺针进行运动学分析；所述穿刺针绕自身旋转不改变自身的位置，所述穿刺针的位置 BP(d n)由其初始位置 RP(0)和平移距离d n决定，所述穿刺针的运动学等式为： Kinematic analysis is performed on the puncture needle; the puncture needle rotates around itself without changing its position, and the position B P(d n ) of the puncture needle is determined by its initial position R P(0) and the translation distance d n , the kinematic equation of the puncture needle is:
    BP(d n)＝ BT R· RP(d n) B P(d n ) = B T R · R P(d n )
    RP(d n)＝ RP(0)+v n·d n R P(d n )= R P(0)+v n ·d n
    上式中, BT R为{R}到{B}的齐次坐标变换矩阵，{R}表示双臂机器人右臂的末端法兰坐标系，{B}为双臂机器人的基坐标系； RP(d n)为穿刺针平移d n时，其针尖在{R}中的坐标；v n为穿刺针平移时在{R}中的单位方向向量，即第一平移参数。 In the above formula, B T R is the homogeneous coordinate transformation matrix from {R} to {B}, {R} represents the end flange coordinate system of the right arm of the dual-arm robot, and {B} is the base coordinate system of the dual-arm robot; R P(d n ) is the coordinate of the needle tip in {R} when the puncture needle is translated by dn; v n is the unit direction vector in {R} when the puncture needle is translated, that is, the first translation parameter.
  3. 根据权利要求2所述的双臂机器人穿刺系统标定方法，其特征在于，所述对双臂机器人穿刺系统进行运动学分析包括：The method for calibrating a dual-arm robot puncture system according to claim 2, wherein the kinematic analysis of the dual-arm robot puncture system comprises:
    对所述超声探头进行运动学分析；所述超声探头绕自身旋转时，其位置由其旋转角度θ决定；所述超声探头平移时，其位置由平移距离d p决定；所述超声探头的运动学等式为： Kinematic analysis is performed on the ultrasonic probe; when the ultrasonic probe rotates around itself, its position is determined by its rotation angle θ; when the ultrasonic probe is translated, its position is determined by the translation distance dp ; the movement of the ultrasonic probe The learning equation is:
    BP(d p/θ)＝ BT L· LT ST(d p/θ)· STP B P(d p /θ) = B T L · L T ST (d p /θ) · ST P
    Figure PCTCN2021083163-appb-100001
    Figure PCTCN2021083163-appb-100001
    Figure PCTCN2021083163-appb-100002
    Figure PCTCN2021083163-appb-100002
    n＝n(0)+d p·v p n=n(0)+d p ·v p
    c＝c(0)+d p·v p c=c(0)+d p ·v p
    上式中， BT L为{L}到{B}的齐次坐标变换矩阵，{L}表示双臂机器人左臂的末端法兰坐标系； BP(d p/θ)为超声探头平移d p或旋转θ时，超声探头在{B}中的位置向量， LT ST(d p/θ)为 LT S(d p/θ)和 LT T(d p/θ)组成的集合，为超声探头平移d p或旋转θ时，{S}或{T}到{L}的齐次坐标变换矩阵；{S}和{T}分别对应超声探头的S平面超声图像坐标系和T平面超声图像坐标系； STP为 SP和 TP组成的集合，表示S平面超声图像或T平面超声图像中的像素坐标；R(n,θ)为坐标系绕轴n旋转θ角度时，坐标系的旋转变换矩阵；n和c分别为超声探头旋转平面的法向量和旋转中心坐标；n(0)和C(0)分别为超声探头处于初始位置时，在{L}中旋转平面的单位法向量和旋转中心坐标，即旋转参数；v p为超声探头在{L}中平移的单位方向向量，即第二平移参数。 In the above formula, B T L is the homogeneous coordinate transformation matrix from {L} to {B}, {L} represents the end flange coordinate system of the left arm of the dual-arm robot; B P(d p /θ) is the translation of the ultrasonic probe The position vector of the ultrasound probe in {B} when d p or rotated by θ, L T ST (d p /θ) is the set of L T S (d p /θ) and L T T (d p /θ) , is the homogeneous coordinate transformation matrix from {S} or {T} to {L} when the ultrasound probe is translated by d p or rotated by θ; {S} and {T} correspond to the S-plane ultrasound image coordinate system and T of the ultrasound probe, respectively Planar ultrasound image coordinate system; ST P is the set composed of SP and T P, representing the pixel coordinates in the S -plane ultrasound image or the T-plane ultrasound image; R(n, θ) is when the coordinate system rotates around the axis n by an angle of θ, The rotation transformation matrix of the coordinate system; n and c are the normal vector and the rotation center coordinate of the ultrasonic probe rotation plane respectively; n(0) and C(0) are respectively the rotation plane of the ultrasonic probe in the initial position, in {L}. The unit normal vector and the coordinates of the rotation center, namely the rotation parameter; v p is the unit direction vector of the ultrasound probe translating in {L}, that is, the second translation parameter.
  4. 根据权利要求2所述的双臂机器人穿刺系统标定方法，其特征在于，所述使用多点法对所述穿刺针的第一初始参数进行标定具体为：The method for calibrating a dual-arm robot puncture system according to claim 2, wherein the use of a multi-point method to calibrate the first initial parameter of the puncture needle is specifically:
    将所述穿刺针复位至初始位置；reset the puncture needle to the initial position;
    控制所述双臂机器人以不同的姿态让针尖碰一个固定参考点，记录所述双臂机器人不同姿态下的参数 BT R(i)； Control the dual-arm robot to let the needle tip touch a fixed reference point with different attitudes, and record the parameters B T R (i) of the dual-arm robot under different attitudes;
    基于所述参数 BT R(i)，对下述等式解最小二乘，得到所述穿刺针的第一初始参数 RP(0)： Based on the parameter B T R (i), the least squares are solved for the following equation to obtain the first initial parameter R P(0) of the puncture needle:
    BT R(i-1)· RP(0)＝ BT R(i)· RP(0),i＝1...n。 B T R (i-1) · R P(0) = B T R (i) · R P(0), i=1...n.
  5. 根据权利要求3所述的双臂机器人穿刺系统标定方法，其特征在于，所述根据所述双平面超声成像对所述超声探头的第二初始参数进行标定具体为：The method for calibrating a dual-arm robot puncture system according to claim 3, wherein the calibration of the second initial parameter of the ultrasonic probe according to the biplane ultrasonic imaging is specifically:
    将所述穿刺针和超声探头复位至初始位置；resetting the puncture needle and the ultrasonic probe to the initial position;
    控制所述双臂机器人以不同的姿态让所述超声探头扫描标定好的针尖，记录所述双臂机器人不同姿态下的参数( BT L(i), BT R(i))； Control the dual-arm robot to scan the calibrated needle tip with the ultrasonic probe in different postures, and record the parameters of the dual-arm robot under different postures ( B T L (i), B T R (i));
    基于所述参数( BT L(i), BT R(i))，对下述等式解最小二乘，得到所述超声探头的第二初始参数 LT ST(0,0)： Based on the parameters ( B T L (i), B T R (i)), least squares are solved for the following equation to obtain the second initial parameter L T ST (0,0) of the ultrasonic probe:
    Figure PCTCN2021083163-appb-100003
    Figure PCTCN2021083163-appb-100003
  6. 根据权利要求5所述的双臂机器人穿刺系统标定方法，其特征在于，所述对所述第一平移参数进行标定包括：The method for calibrating a dual-arm robot puncture system according to claim 5, wherein the calibrating the first translation parameter comprises:
    控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让所述S平面能够扫描到针尖；Controlling the dual-arm robot to make the puncture needle and the ultrasonic probe nearly parallel, so that the S plane can scan the needle tip;
    保持所述双臂机器人姿态不变，并控制电机，使所述穿刺针开始平移，并保存不同平移距离下的S平面超声图像；Keep the posture of the dual-arm robot unchanged, and control the motor to make the puncture needle start to translate, and save the S-plane ultrasound images under different translation distances;
    基于所述S平面超声图像，根据下述等式，求得所述针尖在{R}下平移的轨迹点坐标 RP(i)，对所述轨迹点做三维空间中的直线拟合，得到所述穿刺针的第一平移参数v n： Based on the S-plane ultrasound image, according to the following equation, the trajectory point coordinates R P(i) of the needle tip translation under {R} are obtained, and a straight line in three-dimensional space is fitted to the trajectory points to obtain The first translation parameter v n of the puncture needle:
    RP(i)＝ BT -1 R· BT L· LT S(0,0)· SP(i)。 R P(i) = B T -1 R · B T L · L T S (0,0) · S P(i).
  7. 根据权利要求6所述的双臂机器人穿刺系统标定方法，其特征在于，所述对所述第二平移参数进行标定包括：The method for calibrating a dual-arm robot puncture system according to claim 6, wherein the calibrating the second translation parameter comprises:
    控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让S平面能扫描到针尖；Controlling the dual-arm robot to make the puncture needle and the ultrasonic probe nearly parallel, so that the S plane can scan the needle tip;
    保持所述双臂机器人姿态不变，控制电机，使所述超声探头开始平移，并保存不同平移距离下的S平面超声图像；Keep the posture of the dual-arm robot unchanged, control the motor, make the ultrasonic probe start to translate, and save the S-plane ultrasonic images under different translation distances;
    基于所述S平面超声图像，根据下述等式，求得所述超声探头在{L}下平移轨迹点的坐标向量 LP(i)，对所述轨迹点做三维空间中的直线拟合，求得所述超声探头的第二平移参数v p： Based on the S-plane ultrasonic image, according to the following equation, the coordinate vector L P(i) of the trajectory point of the ultrasonic probe's translation under {L} is obtained, and the trajectory point is fitted with a straight line in the three-dimensional space , obtain the second translation parameter v p of the ultrasonic probe:
    LP(i)＝ LT S(0,0)· SP(i)。 L P(i) = L T S (0,0) · S P(i).
  8. 根据权利要求7所述的双臂机器人穿刺系统标定方法，其特征在于，所述对所述旋转参数进行标定包括：The method for calibrating a dual-arm robot puncture system according to claim 7, wherein the calibrating the rotation parameter comprises:
    控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让T平面与穿刺针相交；Controlling the dual-arm robot to make the puncture needle and the ultrasonic probe nearly parallel, and let the T plane intersect the puncture needle;
    保持所述双臂机器人姿态不变，控制电机，使所述超声探头开始旋转，并保存不同旋转角度下的T平面超声图像；Keep the posture of the dual-arm robot unchanged, control the motor, make the ultrasonic probe start to rotate, and save the T-plane ultrasonic images under different rotation angles;
    基于所述T平面超声图像，根据下述等式，求得所述超声探头在{L}下旋转轨迹点的坐标向量 LP(i)，对所述轨迹点做三维空间中的圆拟合，得到所述超声探头的旋转参数(n(0),c(0))： Based on the T-plane ultrasonic image, according to the following equation, the coordinate vector L P(i) of the ultrasonic probe's rotation trajectory point under {L} is obtained, and a circle fitting in three-dimensional space is performed on the trajectory point. , the rotation parameters (n(0), c(0)) of the ultrasonic probe are obtained:
    LP(i)＝ LT T(0,0)· TP(i)。 L P(i) = L T T (0,0) · T P(i).
  9. 一种双臂机器人穿刺系统标定系统，其特征在于，包括：A dual-arm robot puncturing system calibration system, characterized in that it includes:
    运动学分析模块：用于对双臂机器人穿刺系统进行运动学分析，得到所述 双臂机器人穿刺系统的待标定参数，所述待标定参数包括穿刺针的第一初始参数、第一平移参数、以及超声探头的第二初始参数、第二平移参数和旋转参数；Kinematic analysis module: used for kinematic analysis of the dual-arm robot puncture system to obtain parameters to be calibrated of the dual-arm robot puncture system, the parameters to be calibrated include the first initial parameter of the puncture needle, the first translation parameter, and second initial parameters, second translation parameters and rotation parameters of the ultrasound probe;
    第一参数标定模块：用于使用多点法对所述穿刺针的第一初始参数进行标定，并对所述穿刺针的针尖进行双平面超声成像，根据所述双平面超声成像对所述超声探头的第二初始参数进行标定；The first parameter calibration module is used to calibrate the first initial parameter of the puncture needle by using the multi-point method, and perform bi-plane ultrasonic imaging on the tip of the puncture needle, and the ultrasonic imaging is performed according to the bi-plane ultrasonic imaging. The second initial parameter of the probe is calibrated;
    第二参数标定模块：用于利用所述超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用所述S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，并通过轨迹拟合对所述第一平移参数、第二平移参数和旋转参数进行标定。The second parameter calibration module is used to obtain two mutually perpendicular S-plane ultrasonic images and T-plane ultrasonic images by using the ultrasonic probe, and use the S-plane ultrasonic images and T-plane ultrasonic images to track the movements of the ultrasonic probe and the puncture needle respectively track, and calibrate the first translation parameter, the second translation parameter and the rotation parameter through track fitting.
PCT/CN2021/083163 2021-03-26 2021-03-26 Calibration method and system for dual-arm robot puncture system WO2022198615A1 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
PCT/CN2021/083163 WO2022198615A1 (en) 2021-03-26 2021-03-26 Calibration method and system for dual-arm robot puncture system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/CN2021/083163 WO2022198615A1 (en) 2021-03-26 2021-03-26 Calibration method and system for dual-arm robot puncture system

Publications (1)

Publication Number Publication Date
WO2022198615A1 true WO2022198615A1 (en) 2022-09-29

Family

ID=83396243

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/083163 WO2022198615A1 (en) 2021-03-26 2021-03-26 Calibration method and system for dual-arm robot puncture system

Country Status (1)

Country Link
WO (1) WO2022198615A1 (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117204951A (en) * 2023-09-22 2023-12-12 上海睿触科技有限公司 Operation positioning navigation equipment based on X-rays and positioning realization method thereof

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1678920A (en) * 2002-08-29 2005-10-05 皇家飞利浦电子股份有限公司 Biplane ultrasonic imaging with icon depicting the mutual plane orientation
CN102057296A (en) * 2008-06-05 2011-05-11 皇家飞利浦电子股份有限公司 Extended field of view ultrasonic imaging with a two dimensional array probe
CN102811666A (en) * 2010-03-19 2012-12-05 皇家飞利浦电子股份有限公司 Automatic positioning of imaging plane in ultrasonic imaging
CN103412337A (en) * 2013-07-16 2013-11-27 复旦大学 Dual-independent rotating magnetic bar electromagnetic tracking-based position tracking method and electromagnetic tracking system
US20150297177A1 (en) * 2014-04-17 2015-10-22 The Johns Hopkins University Robot assisted ultrasound system
CN106344153A (en) * 2016-08-23 2017-01-25 深圳先进技术研究院 Automatic tracking device and method for flexible puncture needle tip
CN111437011A (en) * 2020-03-30 2020-07-24 中国科学院深圳先进技术研究院 Puncture surgery robot system
CN111956329A (en) * 2020-08-12 2020-11-20 中国科学院深圳先进技术研究院 Calibration method, system, terminal and storage medium for double-arm robot
CN112168349A (en) * 2020-08-31 2021-01-05 同济大学 Needle tracking and path display method of venipuncture robot

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1678920A (en) * 2002-08-29 2005-10-05 皇家飞利浦电子股份有限公司 Biplane ultrasonic imaging with icon depicting the mutual plane orientation
CN102057296A (en) * 2008-06-05 2011-05-11 皇家飞利浦电子股份有限公司 Extended field of view ultrasonic imaging with a two dimensional array probe
CN102811666A (en) * 2010-03-19 2012-12-05 皇家飞利浦电子股份有限公司 Automatic positioning of imaging plane in ultrasonic imaging
CN103412337A (en) * 2013-07-16 2013-11-27 复旦大学 Dual-independent rotating magnetic bar electromagnetic tracking-based position tracking method and electromagnetic tracking system
US20150297177A1 (en) * 2014-04-17 2015-10-22 The Johns Hopkins University Robot assisted ultrasound system
CN106344153A (en) * 2016-08-23 2017-01-25 深圳先进技术研究院 Automatic tracking device and method for flexible puncture needle tip
CN111437011A (en) * 2020-03-30 2020-07-24 中国科学院深圳先进技术研究院 Puncture surgery robot system
CN111956329A (en) * 2020-08-12 2020-11-20 中国科学院深圳先进技术研究院 Calibration method, system, terminal and storage medium for double-arm robot
CN112168349A (en) * 2020-08-31 2021-01-05 同济大学 Needle tracking and path display method of venipuncture robot

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117204951A (en) * 2023-09-22 2023-12-12 上海睿触科技有限公司 Operation positioning navigation equipment based on X-rays and positioning realization method thereof
CN117204951B (en) * 2023-09-22 2024-04-30 上海睿触科技有限公司 Operation positioning navigation equipment based on X-rays and positioning realization method thereof

Similar Documents

Publication Publication Date Title
CN113133832B (en) Calibration method and system for double-arm robot puncture system
WO2022032964A1 (en) Dual-arm robot calibration method, system, terminal, and storage medium
CN111331592B (en) Mechanical arm tool center point correcting device and method and mechanical arm system
CN110355754B (en) Robot hand-eye system, control method, device and storage medium
JP4191080B2 (en) Measuring device
CN111801198B (en) Hand-eye calibration method, system and computer storage medium
CN111012506B (en) Robot-assisted puncture surgery end tool center calibration method based on stereoscopic vision
WO2022141153A1 (en) Ultrasonic positioning puncture system and storage medium
CN112754616B (en) Ultrasonic positioning puncture system and storage medium
CN109129466A (en) A kind of active vision device and its control method for stereotaxis robot
CN113974835B (en) Telecentric motionless point constraint-based surgical robot motion control method
CN114343847B (en) Hand-eye calibration method of surgical robot based on optical positioning system
WO2022198615A1 (en) Calibration method and system for dual-arm robot puncture system
CN111759463A (en) Method for improving positioning precision of surgical mechanical arm
CN111360812B (en) Industrial robot DH parameter calibration method and calibration device based on camera vision
WO2023046074A1 (en) Autonomous learning method for mechanical arm control method
CN116035705A (en) Integral kinematic space positioning method for surgical robot
CN111477318B (en) Virtual ultrasonic probe tracking method for remote control
CN114589682A (en) Iteration method for automatic calibration of robot hand and eye
CN115431278B (en) Robot calibration method, system and storage medium based on VTK feature point transformation
CN117338422B (en) Space registration and kinematics solver control method, system and device
WO2024007900A1 (en) Method and apparatus for adjusting redundant degree of freedom, and computer device and mechanical arm
Yoshida et al. Image plane positioning by pneumatic actuators for ultrasound guidance
CN113878583B (en) Underwater mechanical arm control method and system
Qian et al. Development of a flexible endoscopic robot with autonomous tracking control ability using machine vision and deep learning

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21932240

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21932240

Country of ref document: EP

Kind code of ref document: A1