CN114795486A - Intraoperative real-time robot hand-eye calibration method and system based on probe - Google Patents

Intraoperative real-time robot hand-eye calibration method and system based on probe Download PDF

Info

Publication number
CN114795486A
CN114795486A CN202210644055.0A CN202210644055A CN114795486A CN 114795486 A CN114795486 A CN 114795486A CN 202210644055 A CN202210644055 A CN 202210644055A CN 114795486 A CN114795486 A CN 114795486A
Authority
CN
China
Prior art keywords
relative
pose
probe
mechanical arm
camera
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210644055.0A
Other languages
Chinese (zh)
Inventor
刘芳德
杨良著
吕存策
郎君伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Huxi Yunbaisheng Technology Co ltd
Original Assignee
Hangzhou Huxi Yunbaisheng Technology Co ltd
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 Hangzhou Huxi Yunbaisheng Technology Co ltd filed Critical Hangzhou Huxi Yunbaisheng Technology Co ltd
Priority to CN202210644055.0A priority Critical patent/CN114795486A/en
Publication of CN114795486A publication Critical patent/CN114795486A/en
Pending legal-status Critical Current

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/30—Surgical robots
    • 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/10—Computer-aided planning, simulation or modelling of surgical operations
    • 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/70—Manipulators specially adapted for use in surgery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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
    • A61B2034/2046—Tracking techniques
    • A61B2034/2065—Tracking using image or pattern recognition
    • 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
    • A61B2034/2068—Surgical navigation systems; Devices for tracking or guiding surgical instruments, e.g. for frameless stereotaxis using pointers, e.g. pointers having reference marks for determining coordinates of body points
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Health & Medical Sciences (AREA)
  • Engineering & Computer Science (AREA)
  • Surgery (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Medical Informatics (AREA)
  • General Health & Medical Sciences (AREA)
  • Biomedical Technology (AREA)
  • Heart & Thoracic Surgery (AREA)
  • Nuclear Medicine, Radiotherapy & Molecular Imaging (AREA)
  • Molecular Biology (AREA)
  • Animal Behavior & Ethology (AREA)
  • Robotics (AREA)
  • Public Health (AREA)
  • Veterinary Medicine (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Manipulator (AREA)

Abstract

The embodiment of the disclosure provides a method and a system for calibrating hands and eyes of an intraoperative real-time robot based on a probe, belonging to the technical field of data processing of surgical robots, wherein the method comprises the following steps: acquiring a relative pose A of the tail end of the mechanical arm relative to a mechanical arm base and a relative pose B of a probe of the tail end of the mechanical arm relative to a camera; establishing a pose relation of the probe relative to the mechanical arm base through a preset calibration equation; calculating and solving the calibration equation to obtain a relative pose X of the probe relative to the tail end of the mechanical arm and a relative pose Z of the camera relative to a base of the mechanical arm; and determining real-time 5-degree-of-freedom pose information of the probe based on the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm. Through the processing scheme disclosed by the invention, the operation precision of the surgical robot is improved.

Description

Intraoperative real-time robot hand-eye calibration method and system based on probe
Technical Field
The disclosure relates to the technical field of surgical robot data processing, in particular to a probe-based intraoperative real-time robot hand-eye calibration method and system.
Background
The calibration of the robot eyes is the basis of the control method of the vision guide robot. The hand-eye calibration method can map the object identified by the sensor to the coordinate system space of the mechanical arm so as to realize the accurate grabbing and operation of the target by the mechanical arm. However, the existing hand-eye calibration technology has many defects in medical surgery, and mainly has the following problems:
1) in actual surgery, doctors usually change the surgical instruments or move the camera many times due to the requirements of hygiene and safety.
Every time when changing the instrument or adjusting the camera, the size and the installation position of the instrument are difficult to ensure to be accurate and consistent, so the surgical robot needs to be calibrated again. At present, in the general hand-eye calibration methods, special reference objects are needed, and a calibration plate in a checkered cross pattern form is generally used, but the methods cannot be used in the operation. In clinical practice, calibration is most conveniently performed using surgical tools, with needle-like instruments (e.g., probes, needles, non-deformable catheters, cotton-tipped swabs, etc.) being the most common. The use of such tools as references is extremely convenient in the clinic.
2) Because the size, length and placement position of the probe are unknown, the position of the axis of the probe can only be reliably and accurately measured from the sensor (5 degrees of freedom). In the existing calibration algorithm, the complete posture (6 degrees of freedom) of the target needs to be measured from the sensor.
Aiming at the situation, the invention develops a new calibration solving algorithm, converts the complete matrix solving problem into the optimization problem on the plum cluster subspace with only 5 degrees of freedom, thereby realizing the calibration method.
Based on the prior art, the invention discloses a real-time hand-eye calibration method based on probe operation. The probe replaces a checkerboard calibration plate, 5 degrees of freedom are calculated and solved, and finally the size and length of the probe, the relative pose of the probe relative to the tail end of the mechanical arm and the relative pose of the camera relative to the mechanical arm base are obtained, so that the surgical robot can be adjusted in real time in an operation, and high accuracy is kept.
Disclosure of Invention
In view of the above, embodiments of the present disclosure provide a method and system for real-time intraoperative robot hand-eye calibration based on a probe, so as to at least partially solve the problems in the prior art.
In a first aspect, an embodiment of the present disclosure provides a probe-based intraoperative real-time robot hand-eye calibration method, including:
acquiring a relative pose A of the tail end of the mechanical arm relative to a mechanical arm base and a relative pose B of a probe of the tail end of the mechanical arm relative to a camera, wherein A is a known 6-degree-of-freedom pose of the tail end of the mechanical arm relative to the mechanical arm base, and B is a known 5-degree-of-freedom pose of the probe relative to the camera;
establishing a pose relation of the probe relative to the mechanical arm base through a preset calibration equation;
calculating and solving the calibration equation to obtain a relative pose X of the probe relative to the tail end of the mechanical arm and a relative pose Z of the camera relative to a base of the mechanical arm, wherein X is an unknown pose of 5 degrees of freedom of the probe relative to the tail end of the mechanical arm; z is an unknown 6-degree-of-freedom pose of the camera relative to the mechanical arm base;
and determining the real-time 5-degree-of-freedom pose information of the probe based on the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm.
According to a specific implementation manner of the embodiment of the present disclosure, before acquiring the relative pose a of the end of the mechanical arm with respect to the base of the mechanical arm and the relative pose a of the probe of the end of the mechanical arm with respect to the camera, the method further includes:
four parameters are preset A, B, X, Z, where A is the known 6 degree of freedom pose of the robot arm tip relative to the robot arm base, B is the known 5 degree of freedom pose of the probe relative to the camera, X is the unknown 5 degree of freedom pose of the probe relative to the robot arm tip, and Z is the unknown 6 degree of freedom pose of the camera relative to the robot arm base.
According to a specific implementation manner of the embodiment of the disclosure, the acquiring the relative pose a of the tail end of the mechanical arm relative to the base of the mechanical arm and the relative pose a of the probe of the tail end of the mechanical arm relative to the camera includes:
and acquiring data of the camera to obtain the relative pose B of the probe relative to the camera.
According to a specific implementation manner of the embodiment of the present disclosure, the establishing, through a preset calibration equation, a pose relationship of the probe with respect to the mechanical arm base includes:
calculating the pose of the probe relative to the base of the mechanical arm by using an AX method and a ZB method according to the setting of the parameters to obtain
Equation [ 1 ]: AX ═ ZB.
According to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain the relative pose X of the probe with respect to the end of the mechanical arm and the relative pose Z of the camera with respect to the base of the mechanical arm includes:
assuming that the 6 degree-of-freedom variables for both a and B are known, AX ═ ZB is expressed in the form:
Figure BDA0003683360450000031
further, equation [ 2 ] is obtained: r A R X ＝R Z R B And equation [ 3 ] R A t X +t A ＝R z t B +t Z 。
According to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain a relative pose X of the probe with respect to the end of the mechanical arm and a relative pose Z of the camera with respect to the base of the mechanical arm further includes:
by disregarding the attitude components of X and B in the Z axisThe calibration method is expressed in the form of incomplete lie algebra se (3), and the equation (2) and R in the equation (3) are expressed X And R B Replacement into a 3-dimensional vector, denoted as r X ,r B ，
In fact
Figure BDA0003683360450000032
Thus, a new equation form is obtained:
equation [ 4 ]: r A r X ＝R Z r B And equation [ 5 ]: r A t X +t A ＝R Z t B +t Z 。
According to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain a relative pose X of the probe with respect to the end of the mechanical arm and a relative pose Z of the camera with respect to the base of the mechanical arm further includes:
suppose Z is an unknown parameter in 6 degrees of freedom, an
Z ═ exp (Z) and Z ═ p (p) z ，θ z ) Wherein p and theta denote a relative displacement and a relative angular displacement, respectively,
let r be l ＝R A r X ，r r ＝R Z r B ，Δr＝r l -r r ＝R A r X -R Z r B ；
t l ＝R A t X +t A ，t r ＝R Z t B +t Z ，Δ t ＝t l -t r ＝R A t X +t A -R Z t B -t Z
For sampling point A 1 ,A 2 ,A 3 …, and B corresponds to 1 ,B 2 ,B 3 …, optimizing to obtain X, Z by using a formula [ 1 ], wherein,
formula [ 1 ]:
Figure BDA0003683360450000041
according to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain a relative pose X of the probe with respect to the end of the mechanical arm and a relative pose Z of the camera with respect to the base of the mechanical arm further includes:
a set of initial values X of X and Z is given to the formula [ 1 ] (0) ，Z (0) Then setting a regression model
Formula [ 2 ]:
Figure BDA0003683360450000042
wherein J (Δ r) and J (Δ t) are Jacobian matrices of Δ r and Δ t to X and Z, respectively.
Repeating the iteration by using the formula (2) to obtain X (0) ，Z (0) ，X (1) ，Z (1) ，X (2) ，Z (2) … further result in
Figure BDA0003683360450000043
According to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain a relative pose X of the probe with respect to the end of the mechanical arm and a relative pose Z of the camera with respect to the base of the mechanical arm further includes:
in the specific solving process, the formula (2) is developed into
Figure BDA0003683360450000044
Wherein the content of the first and second substances,
Figure BDA0003683360450000045
an antisymmetric matrix corresponding to the vector (lie algebra) t is represented; log (R) is the form in which lie groups R are converted into lie algebras; j is a function of l Is the left Jacobian of a 3-dimensional lie algebra and has:
Figure BDA0003683360450000051
Figure BDA0003683360450000052
wherein
Figure BDA0003683360450000053
a r Is a unit vector of r, i.e. a r ＝r/θ 0 Solved by a new formula, X (k+1) And Z (k+1) And further obtaining X and Z, namely the real-time poses of the probe and the camera.
In a second aspect, an embodiment of the present disclosure provides a real-time intraoperative robot hand-eye calibration system based on a probe, including:
the system comprises an acquisition module, a camera and a control module, wherein the acquisition module is used for acquiring a relative pose A of the tail end of the mechanical arm relative to a mechanical arm base and a relative pose B of a probe of the tail end of the mechanical arm relative to the camera, A is a known 6-degree-of-freedom pose of the tail end of the mechanical arm relative to the mechanical arm base, and B is a known 5-degree-of-freedom pose of the probe relative to the camera;
the establishing module is used for establishing a pose relation of the probe relative to the mechanical arm base through a preset calibration equation;
the calculation module is used for calculating and solving the calibration equation to obtain a relative pose X of the probe relative to the tail end of the mechanical arm and a relative pose Z of the camera relative to a base of the mechanical arm, wherein X is an unknown pose of 5 degrees of freedom of the probe relative to the tail end of the mechanical arm; z is an unknown 6-degree-of-freedom pose of the camera relative to the mechanical arm base;
and the determining module is used for determining the real-time 5-degree-of-freedom pose information of the probe based on the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm.
In a third aspect, the disclosed embodiments also provide a non-transitory computer-readable storage medium storing computer instructions for causing a computer to execute the probe-based intraoperative real-time robotic hand-eye calibration method in the first aspect or any implementation manner of the first aspect.
In a fourth aspect, the disclosed embodiments also provide a computer program product comprising a computer program stored on a non-transitory computer readable storage medium, the computer program comprising program instructions that, when executed by a computer, cause the computer to perform the probe-based intraoperative real-time robotic eye calibration method of the first aspect or any of the implementations of the first aspect.
The intraoperative real-time robot hand-eye calibration scheme based on the probe in the embodiment of the disclosure comprises the steps of obtaining a relative pose A of the tail end of a mechanical arm relative to a mechanical arm base and a relative pose B of the probe at the tail end of the mechanical arm relative to a camera, wherein A is a known 6-degree-of-freedom pose of the tail end of the mechanical arm relative to the mechanical arm base, and B is a known 5-degree-of-freedom pose of the probe relative to the camera; establishing a pose relation of the probe relative to the mechanical arm base through a preset calibration equation; calculating and solving the calibration equation to obtain a relative pose X of the probe relative to the tail end of the mechanical arm and a relative pose Z of the camera relative to a base of the mechanical arm, wherein X is an unknown pose of 5 degrees of freedom of the probe relative to the tail end of the mechanical arm; z is an unknown 6-degree-of-freedom pose of the camera relative to the mechanical arm base; and determining real-time 5-degree-of-freedom pose information of the probe based on the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm. Through the processing scheme disclosed by the invention, a new calibration solving algorithm is developed, and a complete matrix solving problem is converted into an optimization problem on a plum group subspace with only 5 degrees of freedom, so that the calibration method is realized; meanwhile, the probe replaces a checkerboard calibration plate, and the size, the length, the relative pose of the probe relative to the tail end of the mechanical arm and the relative pose of the camera relative to the base of the mechanical arm are finally obtained by calculating and solving the 5 degrees of freedom, so that the surgical robot can be adjusted in real time in the operation, and the high precision is kept.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, the drawings needed to be used in the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only some embodiments of the present disclosure, and it is obvious for those skilled in the art that other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a schematic diagram of a degree of freedom of a probe according to an embodiment of the present disclosure;
fig. 2 is a schematic structural diagram of a probe robot system provided in an embodiment of the present disclosure;
fig. 3 is a flowchart of calibrating a hand-eye of a robot according to an embodiment of the disclosure.
Detailed Description
The embodiments of the present disclosure are described in detail below with reference to the accompanying drawings.
The embodiments of the present disclosure are described below with specific examples, and other advantages and effects of the present disclosure will be readily apparent to those skilled in the art from the disclosure in the specification. It is to be understood that the described embodiments are merely illustrative of some, and not restrictive, of the embodiments of the disclosure. The disclosure may be embodied or carried out in various other specific embodiments, and various modifications and changes may be made in the details within the description without departing from the spirit of the disclosure. It is to be noted that the features in the following embodiments and examples may be combined with each other without conflict. All other embodiments, which can be derived by a person skilled in the art from the embodiments disclosed herein without inventive step, are intended to be within the scope of the present disclosure.
It is noted that various aspects of the embodiments are described below within the scope of the appended claims. It should be apparent that the aspects described herein may be embodied in a wide variety of forms and that any specific structure and/or function described herein is merely illustrative. Based on the disclosure, one skilled in the art should appreciate that one aspect described herein may be implemented independently of any other aspects and that two or more of these aspects may be combined in various ways. For example, an apparatus may be implemented and/or a method practiced using any number of the aspects set forth herein. In addition, such an apparatus may be implemented and/or such a method may be practiced using other structure and/or functionality in addition to or other than one or more of the aspects set forth herein.
It should be noted that the drawings provided in the following embodiments are only for illustrating the basic idea of the present disclosure, and the drawings only show the components related to the present disclosure rather than the number, shape and size of the components in actual implementation, and the type, amount and ratio of the components in actual implementation may be changed arbitrarily, and the layout of the components may be more complicated.
In addition, in the following description, specific details are provided to provide a thorough understanding of the examples. However, it will be understood by those skilled in the art that the aspects may be practiced without these specific details.
The embodiment of the disclosure provides a probe-based intraoperative real-time robot hand-eye calibration method. The real-time intra-operative robot hand-eye calibration method based on the probe provided by the embodiment can be executed by a computing device, the computing device can be implemented as software, or implemented as a combination of software and hardware, and the computing device can be integrally arranged in a server, a client and the like.
Referring to fig. 1, 2 and 3, the real-time intraoperative robot hand-eye calibration method based on a probe in the embodiment of the present disclosure may include the following steps:
s101, acquiring a relative pose A of the tail end of the mechanical arm relative to a mechanical arm base and a relative pose B of a probe at the tail end of the mechanical arm relative to a camera, wherein A is a known 6-degree-of-freedom pose of the tail end of the mechanical arm relative to the mechanical arm base, and B is a known 5-degree-of-freedom pose of the probe relative to the camera;
s102, establishing a pose relation of the probe relative to the mechanical arm base through a preset calibration equation;
s103, calculating and solving the calibration equation to obtain a relative pose X of the probe relative to the tail end of the mechanical arm and a relative pose Z of the camera relative to a base of the mechanical arm, wherein X is an unknown 5-degree-of-freedom pose of the probe relative to the tail end of the mechanical arm; z is an unknown 6-degree-of-freedom pose of the camera relative to the mechanical arm base;
and S104, determining real-time 5-degree-of-freedom pose information of the probe based on the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm.
Specifically, the method for intraoperative real-time hand-eye calibration by using the probe mainly comprises the following contents:
1) the real-time hand-eye calibration technology of the intraoperative probe comprises the following steps:
the probe is used for replacing the existing chessboard marking plate, and the accurate pose of the probe can be obtained in real time in the operation. The degree of freedom of the needle in the present invention is shown in fig. 1. As shown in FIG. 1, let (x, y, z) and (nx, n) y ,n z ) Respectively the position and attitude of the probe relative to the end of the arm (or camera). Wherein, under the local coordinate taking the probe as a reference system, the direction of the probe is the z-axis.
When the probe is used in surgery, only the position (x, y, z) of the tip of the needle, and the axis n of the needle, can be measured, and the spin angle of the probe in the z-axis cannot be measured. So in the calculation process, the pose of the probe relative to the end of the mechanical arm (or camera) has only 5 degrees of freedom, where n is wound z Is an unknown variable.
2) After the probe is used for calibrating different positions, calculation solution is carried out through 5 degrees of freedom:
the relative pose A of the mechanical arm is changed for multiple times, and the relative pose B of the probe relative to the camera is obtained through the camera, so that a series of sampling points A are obtained 1 ,A 2 ,A 3 …, and B corresponds to 1 ,B 2 ,B 3 …. The 5 degrees of freedom are solved computationally according to the method proposed in the present invention by establishing the equation AX ═ ZB (where X and B both have only 5 degrees of freedom).
3) And finally acquiring the size and length of the probe, the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm after calculation and solution.
Referring to fig. 2, the overall system to which the present application relates consists of 3 parts:
1. mechanical arm: the high-performance multi-axis mechanical arm is used for operation and control processing;
2. and (3) probe: the pose is fixed on the tail end of the mechanical arm and changes along with the motion of the mechanical arm, so that the pose is calibrated on line in real time;
3. a camera: the camera is fixed in a space outside the mechanical arm body, and the pose of the camera and the world coordinate system is kept unchanged and used for detecting the pose of the probe;
a flow chart of the calibration calculation method is shown in fig. 3, and mainly includes the following steps:
1. setting parameters
Four parameters were set A, B, X, Z:
a: known pose (6 degrees of freedom) of the robot arm tip relative to the robot arm base;
b: the pose of the known probe with respect to the camera (only 5 degrees of freedom);
x: unknown pose of the probe relative to the end of the arm (only 5 degrees of freedom);
z: unknown pose of the camera with respect to the robot arm base (6 degrees of freedom);
2. data acquisition
1) Setting a relative pose A of the tail end of the mechanical arm relative to a mechanical arm base;
2) acquiring the relative pose B of the probe relative to the camera through data acquisition of the camera;
3. calibration equation
Similar to a general hand-eye calibration method, the position and posture of the probe relative to the mechanical arm base are calculated by using an AX method and a ZB method according to the setting of parameters to obtain
Equation [ 1 ]: AX ═ ZB
Of course, the equation [ 1 ] cannot be solved directly due to the lack of the degree of freedom parameter of B.
4. Optimization solving method for plum group subspace
Through the calculation and solution of the equation (1), the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm are obtained, and the specific solution process is as follows:
converting equation [ 1 ] AX to ZB into a solvable equation form:
generally, AX ═ ZB will take the form where the 6 degree of freedom variables for a and B are known:
Figure BDA0003683360450000101
further, equation [ 2 ] is obtained: r A R X ＝R Z R B And equation [ 3 ] R A t X +t A ＝R z t B +t Z ；
Lie group differential molecular space of probe attitude
However, since the attitude components of X and B on the Z axis are not considered in the present invention, it is expressed in the form of (incomplete) lie algebra se (3). R in the above two equations X And R B Replacement into a 3-dimensional vector, denoted as r X ,r B 。
In fact
Figure BDA0003683360450000102
Thus, a new equation form can be obtained:
equation [ 4 ]: r A r X ＝R Z r B
And
equation [ 5 ]: r A t X +t A ＝R Z t B + t Z
Next, one solves r in the two equations above X ,t X And Z.
Third, optimization solving method for plum group subspace
Before solving the equation, some variables are set.
Z is an unknown parameter of 6 degrees of freedom, an
Z ═ exp (Z) and Z ═ p (p) z ，θ z )
Where p and θ represent the relative displacement and the relative angular displacement, respectively.
Let r be l ＝R A r X ，r r ＝R Z r B ，Δr＝r l -r r ＝R A r X -R Z r B ；
t l ＝R A t X +t A ，t r ＝R Z t B +t Z ，Δt＝t l -t r ＝R A t X +t A -R Z t B -t Z
For sampling point A 1 ，A 2 ，A 3 ,.., and B, respectively 1 ，B 2 ，B 3 ,., using a formula [ 1 ], carrying out optimization processing to obtain X, Z.
Formula [ 1 ]:
Figure BDA0003683360450000111
solving method for nonlinear iterative numerical value
The solution of the equation [ 1 ] is not an analytical solution, but an estimation value obtained by using a gaussian-newton iterative method, and the solution process is as follows.
First, a set of initial values X of X and Z is given (0) ，Z (0) Then setting a regression model
Formula [ 2 ]:
Figure BDA0003683360450000112
wherein J (Δ r) and J (Δ t) are Jacobian matrices of Δ r and Δ t to X and Z, respectively.
Then, the formula [ 2 ] can be used for repeated iteration to obtain X (0) ，Z (0) ，X (1) ，Z (1) ，X (2) ，Z (2) … further result in
Figure BDA0003683360450000113
Subspace Jacobi calculating method
In the present problem, J (Δ r) and J (Δ t) are both very complex jacobian matrices. In the concrete solving process, the formula (2) can become into a formula after being expanded
Figure BDA0003683360450000121
Wherein the content of the first and second substances,
Figure BDA0003683360450000122
an antisymmetric matrix corresponding to the vector (lie algebra) t is represented; log (R) is the form in which lie groups R are converted into lie algebras; j is a function of l Is the left Jacobian of a 3-dimensional lie algebra and has:
Figure BDA0003683360450000123
Figure BDA0003683360450000124
wherein
Figure BDA0003683360450000125
a r Is a unit vector of r, i.e. a r -r/θ 0 Then, X can be solved by a new formula (k+1) And Z (k+1) And further obtaining X and Z, namely the poses of the probe and the camera.
According to a specific implementation manner of the embodiment of the present disclosure, before acquiring the relative pose a of the end of the mechanical arm with respect to the base of the mechanical arm and the relative pose a of the probe of the end of the mechanical arm with respect to the camera, the method further includes:
four parameters are preset A, B, X, Z, where A is the known 6 degree of freedom pose of the robot arm tip relative to the robot arm base, B is the known 5 degree of freedom pose of the probe relative to the camera, X is the unknown 5 degree of freedom pose of the probe relative to the robot arm tip, and Z is the unknown 6 degree of freedom pose of the camera relative to the robot arm base.
According to a specific implementation manner of the embodiment of the present disclosure, the acquiring a relative pose a of the end of the mechanical arm with respect to the base of the mechanical arm and a relative pose of the probe of the end of the mechanical arm with respect to the camera includes:
and acquiring data of the camera to obtain the relative pose B of the probe relative to the camera.
According to a specific implementation manner of the embodiment of the present disclosure, the establishing, through a preset calibration equation, a pose relationship of the probe with respect to the mechanical arm base includes:
calculating the pose of the probe relative to the base of the mechanical arm by using an AX method and a ZB method according to the setting of the parameters to obtain
Equation [ 1 ]: AX ═ ZB.
According to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain the relative pose X of the probe with respect to the end of the mechanical arm and the relative pose Z of the camera with respect to the base of the mechanical arm includes:
assuming that the 6 degree-of-freedom variables for both a and B are known, AX ═ ZB is expressed in the form:
Figure BDA0003683360450000131
further, equation [ 2 ] is obtained: r A R X ＝R Z R B And equation [ 3 ] R A t X +t A ＝R z t B +t Z 。
According to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain a relative pose X of the probe with respect to the end of the mechanical arm and a relative pose Z of the camera with respect to the base of the mechanical arm further includes:
the attitude components of X and B on the Z axis are not considered, the calibration method is expressed in the form of incomplete lie algebra se (3), and the equation [ 2 ] and R in the equation [ 3 ] are used X And R B Replacement into a 3-dimensional vector, denoted as r X ,r B ，
In fact
Figure BDA0003683360450000132
Thus, a new equation form is obtained:
equation [ 4 ]: r A r X ＝R Z r B And equation [ 5 ]: r A t X +t A ＝R Z t B +t Z 。
According to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain the relative pose X of the probe with respect to the end of the mechanical arm and the relative pose Z of the camera with respect to the base of the mechanical arm further includes:
suppose Z is an unknown parameter in 6 degrees of freedom, an
Z ═ exp (Z) and Z ═ p (p) z ，θ z )
Wherein p and theta denote a relative displacement and a relative angular displacement, respectively,
let r be l ＝R A r X ，r r ＝R Z r B ，Δr＝r l -r r ＝R A r X -R Z r B ；
t l ＝R A t X +t A ，t r ＝R Z t B +t Z ，Δt＝t l -t r ＝R A t X +t A -R Z t B -t Z
For sampling point A 1 ，A 2 ，A 3 ,.., and B, respectively 1 ，B 2 ，B 3 ,., optimizing by using a formula [ 1 ] to obtain X, Z, wherein,
formula [ 1 ]:
Figure BDA0003683360450000141
according to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain a relative pose X of the probe with respect to the end of the mechanical arm and a relative pose Z of the camera with respect to the base of the mechanical arm further includes:
a set of initial values X of X and Z is given to the formula [ 1 ] (0) ，Z (0 ) Then setting a regression model
Formula [ 2 ]:
Figure BDA0003683360450000142
wherein J (Δ r) and J (Δ t) are Jacobian matrices of Δ t and Δ t, respectively, versus X and Z.
Repeating the iteration by using the formula (2) to obtain X (0) ，Z (0) ，X (1) ，Z (1) ，X (2) ，Z (2) … further result in
Figure BDA0003683360450000143
According to a specific implementation manner of the embodiment of the present disclosure, the calculating and solving the calibration equation to obtain a relative pose X of the probe with respect to the end of the mechanical arm and a relative pose Z of the camera with respect to the base of the mechanical arm further includes:
in the concrete solving process, the formula (2) is developed into
Figure BDA0003683360450000144
Wherein the content of the first and second substances,
Figure BDA0003683360450000145
an antisymmetric matrix corresponding to the vector (lie algebra) t is represented; log (R) is the conversion of Liqun R to LiquanA form of a number; j is a function of l Is the left Jacobian of a 3-dimensional lie algebra and has:
Figure BDA0003683360450000151
Figure BDA0003683360450000152
wherein
Figure BDA0003683360450000153
a r Is a unit vector of r, i.e. a r ＝r/θ 0
Solved by a new formula, X (k+1) And Z (k+1) And further obtaining X and Z, namely the real-time poses of the probe and the camera.
Corresponding to the above method embodiments, the present application further provides a real-time intraoperative robot hand-eye calibration system based on a probe, including:
the system comprises an acquisition module, a camera and a control module, wherein the acquisition module is used for acquiring a relative pose A of the tail end of the mechanical arm relative to a mechanical arm base and a relative pose B of a probe of the tail end of the mechanical arm relative to the camera, A is a known 6-degree-of-freedom pose of the tail end of the mechanical arm relative to the mechanical arm base, and B is a known 5-degree-of-freedom pose of the probe relative to the camera;
the establishing module is used for establishing a pose relation of the probe relative to the mechanical arm base through a preset calibration equation;
the calculation module is used for calculating and solving the calibration equation to obtain a relative pose X of the probe relative to the tail end of the mechanical arm and a relative pose Z of the camera relative to a base of the mechanical arm, wherein X is an unknown pose of 5 degrees of freedom of the probe relative to the tail end of the mechanical arm; z is an unknown 6-degree-of-freedom pose of the camera relative to the mechanical arm base;
and the determining module is used for determining the real-time 5-degree-of-freedom pose information of the probe based on the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm.
The flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems that perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
The units described in the embodiments of the present disclosure may be implemented by software or hardware. Where the name of a unit does not in some cases constitute a limitation of the unit itself, for example, the first retrieving unit may also be described as a "unit for retrieving at least two internet protocol addresses".
It should be understood that portions of the present disclosure may be implemented in hardware, software, firmware, or a combination thereof.
The above description is only for the specific embodiments of the present disclosure, but the scope of the present disclosure is not limited thereto, and any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope of the present disclosure should be covered within the scope of the present disclosure. Therefore, the protection scope of the present disclosure shall be subject to the protection scope of the claims.

Claims (10)

1. A real-time intraoperative robot hand-eye calibration method based on a probe is characterized by comprising the following steps:
acquiring a relative pose A of the tail end of the mechanical arm relative to a mechanical arm base and a relative pose B of a probe of the tail end of the mechanical arm relative to a camera, wherein A is a known 6-degree-of-freedom pose of the tail end of the mechanical arm relative to the mechanical arm base, and B is a known 5-degree-of-freedom pose of the probe relative to the camera;
establishing a pose relation of the probe relative to the mechanical arm base through a preset calibration equation;
calculating and solving the calibration equation to obtain a relative pose X of the probe relative to the tail end of the mechanical arm and a relative pose Z of the camera relative to a base of the mechanical arm, wherein X is an unknown pose of 5 degrees of freedom of the probe relative to the tail end of the mechanical arm; z is an unknown 6-degree-of-freedom pose of the camera relative to the mechanical arm base;
and determining real-time 5-degree-of-freedom pose information of the probe based on the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm.
2. The method of claim 1, wherein prior to acquiring the relative pose a of the robot arm tip with respect to the robot arm base and the relative pose a of the probe of the robot arm tip with respect to the camera, the method further comprises:
four parameters are preset A, B, X, Z, where A is the known 6 degree of freedom pose of the robot arm tip relative to the robot arm base, B is the known 5 degree of freedom pose of the probe relative to the camera, X is the unknown 5 degree of freedom pose of the probe relative to the robot arm tip, and Z is the unknown 6 degree of freedom pose of the camera relative to the robot arm base.
3. The method of claim 2, wherein the acquiring the relative pose a of the robot arm tip with respect to the robot arm base and the relative pose a of the probe of the robot arm tip with respect to the camera comprises:
and acquiring data of the camera to obtain the relative pose B of the probe relative to the camera.
4. The method according to claim 3, wherein the establishing of the pose relationship of the probe relative to the mechanical arm base through a preset calibration equation comprises:
calculating the pose of the probe relative to the base of the mechanical arm by using an AX method and a ZB method according to the setting of the parameters to obtain
Equation [ 1 ]: AX ═ ZB.
5. The method of claim 1, wherein the calculating and solving the calibration equation to obtain the relative pose X of the probe with respect to the end of the mechanical arm and the relative pose Z of the camera with respect to the base of the mechanical arm comprises:
assuming that the 6 degree-of-freedom variables for both a and B are known, AX ═ ZB is expressed in the form:
Figure FDA0003683360440000021
further, equation [ 2 ] is obtained: r A R X ＝R Z R B And equation [ 3 ] R A t X +t A ＝R z t B +t Z 。
6. The method of claim 5, wherein the computationally solving the calibration equations to obtain a relative pose X of the probe with respect to the end of the arm and a relative pose Z of the camera with respect to the base of the arm, further comprises:
the attitude components of X and B on the Z axis are not considered, the calibration method is expressed in the form of incomplete lie algebra se (3), and the equation [ 2 ] and R in the equation [ 3 ] are used X And R B Replacement into a 3-dimensional vector, denoted as r X ，r B ，
In fact
Figure FDA0003683360440000022
Thus, a new equation form is obtained:
equation [ 4 ]: r A r X ＝R Z r B And equation [ 5 ]: r A t X +t A ＝R Z t B +t Z 。
7. The method of claim 6, wherein the calculating and solving the calibration equation to obtain the relative pose X of the probe with respect to the end of the mechanical arm and the relative pose Z of the camera with respect to the base of the mechanical arm further comprises:
suppose Z is an unknown parameter in 6 degrees of freedom, an
Z ═ exp (Z) and Z ═ p (p) z ，θ z )
Wherein p and theta denote a relative displacement and a relative angular displacement, respectively,
let r be l ＝R A r X ，r r ＝R Z r B ，Δr＝r l -r r ＝R A r X -R Z r B ；
t l ＝R A t X +t A ，t r ＝R Z t B +t Z ，Δt＝t l -t r ＝R A t X +t A -R Z t B -t Z
For sampling point A 1 ，A 2 ，A 3 ,.., and B, respectively 1 ，B 2 ，B 3 ,., optimizing by using a formula [ 1 ] to obtain X, Z, wherein,
formula [ 1 ]:
Figure FDA0003683360440000031
8. the method of claim 7, wherein the computationally solving the calibration equations to obtain a relative pose X of the probe with respect to the end of the arm and a relative pose Z of the camera with respect to the base of the arm, further comprises:
a set of initial values X of X and Z is given to the formula [ 1 ] (0) ，Z (0) Then setting a regression model
Formula [ 2 ]:
Figure FDA0003683360440000032
wherein J (Δ r) and J (Δ t) are Jacobian matrices of Δ r and Δ t to X and Z, respectively.
Repeating the iteration by using the formula (2) to obtain X (0) ，Z (0) ，X (1) ，Z (1) ，X (2) ，Z (2) … further result in
Figure FDA0003683360440000033
9. The method of claim 8, wherein the computationally solving the calibration equations to obtain a relative pose X of the probe with respect to the end of the arm and a relative pose Z of the camera with respect to the base of the arm, further comprises:
in the concrete solving process, the formula (2) is developed into
Figure FDA0003683360440000034
Wherein the content of the first and second substances,
Figure FDA0003683360440000035
an antisymmetric matrix corresponding to the vector (lie algebra) t is represented; log (R) is the form in which lie groups R are converted into lie algebras; j is a function of l Is the left Jacobian of a 3-dimensional lie algebra and has:
Figure FDA0003683360440000036
Figure FDA0003683360440000037
wherein
Figure FDA0003683360440000038
a r Is a unit vector of r, i.e. a r ＝r/θ 0 Solved by a new formula, X (k+1) And z (k+1) And further obtaining X and Z, namely the real-time poses of the probe and the camera.
10. A real-time robot hand eye calibration system in art based on probe, its characterized in that includes:
the system comprises an acquisition module, a camera and a control module, wherein the acquisition module is used for acquiring a relative pose A of the tail end of the mechanical arm relative to a mechanical arm base and a relative pose B of a probe of the tail end of the mechanical arm relative to the camera, A is a known 6-degree-of-freedom pose of the tail end of the mechanical arm relative to the mechanical arm base, and B is a known 5-degree-of-freedom pose of the probe relative to the camera;
the establishing module is used for establishing a pose relation of the probe relative to the mechanical arm base through a preset calibration equation;
the calculation module is used for calculating and solving the calibration equation to obtain a relative pose X of the probe relative to the tail end of the mechanical arm and a relative pose Z of the camera relative to a base of the mechanical arm, wherein X is an unknown pose of 5 degrees of freedom of the probe relative to the tail end of the mechanical arm; z is an unknown 6-degree-of-freedom pose of the camera relative to the mechanical arm base;
and the determining module is used for determining the real-time 5-degree-of-freedom pose information of the probe based on the relative pose X of the probe relative to the tail end of the mechanical arm and the relative pose Z of the camera relative to the base of the mechanical arm.
CN202210644055.0A 2022-06-08 2022-06-08 Intraoperative real-time robot hand-eye calibration method and system based on probe Pending CN114795486A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210644055.0A CN114795486A (en) 2022-06-08 2022-06-08 Intraoperative real-time robot hand-eye calibration method and system based on probe

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210644055.0A CN114795486A (en) 2022-06-08 2022-06-08 Intraoperative real-time robot hand-eye calibration method and system based on probe

Publications (1)

Publication Number Publication Date
CN114795486A true CN114795486A (en) 2022-07-29

Family

ID=82522071

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210644055.0A Pending CN114795486A (en) 2022-06-08 2022-06-08 Intraoperative real-time robot hand-eye calibration method and system based on probe

Country Status (1)

Country Link
CN (1) CN114795486A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117103286A (en) * 2023-10-25 2023-11-24 杭州汇萃智能科技有限公司 Manipulator eye calibration method and system and readable storage medium

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117103286A (en) * 2023-10-25 2023-11-24 杭州汇萃智能科技有限公司 Manipulator eye calibration method and system and readable storage medium
CN117103286B (en) * 2023-10-25 2024-03-19 杭州汇萃智能科技有限公司 Manipulator eye calibration method and system and readable storage medium

Similar Documents

Publication Publication Date Title
CN110640747B (en) Hand-eye calibration method and system for robot, electronic equipment and storage medium
CN105232155B (en) Operating robot adjusts system
CN109512516B (en) Robot interface positioning determination system and method
CN108324373B (en) Accurate positioning implementation method of puncture surgery robot based on electromagnetic positioning system
CN110815206B (en) Kinematics calibration method for Stewart parallel robot
CN111618859B (en) Method for feeding back mechanical arm high-precision force under static or low-speed working condition
JP7258516B2 (en) Calibration method and gripping system
DE102011114333B4 (en) A method of registering an X-ray volume with a position sensing system using a registration phantom
CN111216138A (en) Robot calibration method, robot calibration system and readable storage medium
CN114795486A (en) Intraoperative real-time robot hand-eye calibration method and system based on probe
CN114343847A (en) Hand-eye calibration method of surgical robot based on optical positioning system
CN111452048A (en) Calibration method and device for relative spatial position relationship of multiple robots
CN110871434A (en) Kinematics calibration method of parallel processing equipment
CN114216456A (en) Attitude measurement method based on IMU and robot body parameter fusion
Santolaria et al. Self-alignment of on-board measurement sensors for robot kinematic calibration
CN114310880A (en) Mechanical arm calibration method and device
CN113974835A (en) Operation robot motion control method based on telecentric motionless point constraint
CN112603542A (en) Hand-eye calibration method and device, electronic equipment and storage medium
DE102019102927B4 (en) Method and device for determining dimensional and/or geometric properties of a measurement object
CN109866224B (en) Robot jacobian matrix calculation method, device and storage medium
JP5378908B2 (en) Robot accuracy adjustment method and robot
CN110900608B (en) Robot kinematics calibration method based on optimal measurement configuration selection
CN113052913A (en) High-precision calibration method for transfer pose of two-stage combined vision measurement system
Furutani et al. Parameter calibration for non-cartesian CMM
CN116942314A (en) Positioning method and system for mixing optical positioning and mechanical positioning

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination