CN114863417A - High-precision master-slave pose registration method for surgical robot - Google Patents

High-precision master-slave pose registration method for surgical robot Download PDF

Info

Publication number
CN114863417A
CN114863417A CN202210447243.4A CN202210447243A CN114863417A CN 114863417 A CN114863417 A CN 114863417A CN 202210447243 A CN202210447243 A CN 202210447243A CN 114863417 A CN114863417 A CN 114863417A
Authority
CN
China
Prior art keywords
point
catheter
slave
pose
registration
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
CN202210447243.4A
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.)
Shanghai University of Engineering Science
Original Assignee
Shanghai University of Engineering Science
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 Shanghai University of Engineering Science filed Critical Shanghai University of Engineering Science
Priority to CN202210447243.4A priority Critical patent/CN114863417A/en
Publication of CN114863417A publication Critical patent/CN114863417A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/60—Type of objects
    • G06V20/64—Three-dimensional objects
    • G06V20/653—Three-dimensional objects by matching three-dimensional models, e.g. conformal mapping of Riemann surfaces
    • 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
    • A61B34/37—Master-slave 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/70—Manipulators specially adapted for use in surgery
    • A61B34/76—Manipulators having means for providing feel, e.g. force or tactile feedback
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V2201/00—Indexing scheme relating to image or video recognition or understanding
    • G06V2201/03—Recognition of patterns in medical or anatomical images
    • G06V2201/034—Recognition of patterns in medical or anatomical images of medical instruments

Landscapes

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

Abstract

The invention relates to a high-precision master-slave pose registration method for a surgical robot. The process of realizing the dynamic accurate registration of the catheter pose based on the multi-point contact force measurement comprises the following steps: based on the measurement force, the deformation displacement of the contact point of the conduit is solved, an optimal conversion matrix is calculated through the relative distance between the master hand conduit and the slave hand conduit, and based on a closest point iterative algorithm, a rotation matrix and a translation vector between a master hand conduit model and a slave hand conduit model are found, so that the two matching data meet the optimal matching under certain measurement. The invention realizes real-time and accurate human tactile representation of the vascular intervention robot through model registration, can monitor the operation safety from two aspects of calculation of a force tactile rendering model and contact force measurement, and improves the transparency of the system.

Description

High-precision master-slave pose registration method for surgical robot
Technical Field
The invention belongs to the technical field of catheter modeling and force touch rendering, and relates to a high-precision master-slave pose registration method for a surgical robot.
Background
The operation robot is used as the assistance of a doctor, so that the doctor can operate at a remote main hand end, the influence of X-rays on the doctor is prevented, and a patient can be treated by a specialist doctor in time under special conditions of remote areas or battlefield operations and the like. With the development of communication technologies such as 5G and the like, remote medical technologies are further improved, and the lack of human tactile feedback of the current medical machine becomes an important problem restricting the popularization of the medical machine.
The virtual reality of the main hand end of the vascular interventional operation robot truly reflects the pose of the slave hand end catheter to the virtual end by establishing a space three-dimensional coordinate system. The flexible catheter is in multipoint dynamic line contact with the vessel, and the bending torsion of the flexible catheter is difficult to accurately model and register. And the stress monitoring based on physical model calculation is difficult to avoid modeling errors. Affecting the modeling control effect.
Model registration based on pose measurement in the current blood vessel intervention robot human tactile representation technology is difficult to accurately simulate the dynamic process of catheter-blood vessel collision stress. The catheter and the blood vessel are in dynamic multipoint line contact, and the model registration accuracy of the bending and torsional deformation of the catheter is difficult to ensure.
Therefore, the research on the high-precision master-slave pose registration method of the surgical robot has extremely important significance.
Disclosure of Invention
The invention aims to improve the registration precision of a flexible catheter and a blood vessel multipoint dynamic line contact model and provides a high-precision master-slave pose registration method of a surgical robot. The method is based on frame difference image recognition to measure the position and the posture of the soft section at the front end of the catheter, so as to realize the initial registration of the model; based on multipoint contact force measurement, a closest point iterative algorithm with force touch information error minimization as a target is designed, and accurate catheter-vessel multipoint dynamic line contact rendering dynamic model registration is achieved.
In order to achieve the purpose, the technical scheme adopted by the invention is as follows:
a high-precision surgical robot master-slave pose registration method is characterized in that preliminary registration of catheter poses (namely positions and postures) is achieved on the basis of image recognition measurement, and dynamic accurate registration of the catheter poses is achieved on the basis of multipoint contact force measurement;
the process of realizing the dynamic accurate registration of the catheter pose based on the multipoint contact force measurement comprises the following steps: based on the measurement force, the deformation displacement of the contact point of the conduit is solved, and based on the relative distance between the master hand conduit and the slave hand conduit and the closest point iterative algorithm (ICP), namely based on the optimal matching method of the least square method, the rotation matrix and the translation vector between the master hand conduit model and the slave hand conduit model are found, so that the two matching data meet the optimal matching under certain measurement.
As a preferred technical scheme:
according to the high-precision master-slave pose registration method for the surgical robot, the primary registration process comprises the following steps: the method comprises the steps of establishing a catheter model on a master hand end by using a Cosserat elastic rod theory, measuring the pose change of a front soft section of the catheter at a slave hand end through image recognition, solving a transformation matrix of a coordinate system of the master hand end catheter model and a coordinate system of the slave hand end catheter model based on a D-H coordinate transformation method, realizing the association of the master hand end catheter model and the slave hand end catheter model, and realizing the primary registration of the pose of the catheter by using a rotation matrix and a translation vector.
In the primary registration stage, an ICP algorithm is commonly used to match catheter position data output by a monocular camera with a center line corresponding to a catheter path in a virtual three-dimensional image coordinate system to obtain an initial rotation matrix R in the process that a catheter enters an aortic vessel to select a target bifurcation vessel to reach a target by using an ICP algorithm k And translation vector T k The essence is to find the accurate corresponding points of the master point set and the slave point set and continuously iterate the rotation matrix and the translation vector of the two point sets; during preliminary registration, the rotation matrix and the translation vector are obtained through the following procedures:
(i) obtaining central line data under a main hand end virtual three-dimensional image coordinate system based on a preoperative three-dimensional virtual image coordinate space, namely a main hand end catheter model point set Q, Q ═ Q i |Q i ∈R 3 ,i＝1,2,...,n}(，Q i Representing a particular point, R, in the master hand endpoint set 3 Representing three-dimensional space coordinates, wherein n represents the number of point collection points; real-time acquisition of catheter from hand end under actual coordinate system through position and posture sensorPath data is set of points P, P ═ P from the hand catheter model i |P i ∈R 3 ,i＝1,2,...,n}(，P i Representing a particular point, R, concentrated from the hand end point 3 Representing three-dimensional space coordinates, wherein n represents the number of point collection points;
(ii) with P i k Represents P i The (k) th iteration of (a),
Figure BDA0003611977540000022
represents P i k The closest point in Q, i.e. the point and Q 1 Is on a straight line Q 1 Q n Length of projection line on and P in P i k And P 1 Is on a straight line P 1 P n The length of the projected line on is closest;
(iii) calculating a rotation matrix R k And translation vector T k So that
Figure BDA0003611977540000025
(iv) Computing
Figure BDA0003611977540000026
P k+1 For data of point set P at the k +1 th iteration, P i k+1 For a point to concentrate a specific point P i At the data of the (k + 1) th iteration,
Figure BDA0003611977540000028
is R k Rotation matrix of the ith point, T i k Is T k The translation vector of the ith point;
(v) calculating the average value of the position error of each point in the master hand catheter model point set and the slave hand catheter model point set
Figure BDA00036119775400000210
(vi) Judge d | | k+1 -d k Whether | is less than τ is true, d k And d k+1 The position error averages of the k-th iteration and the k + 1-th iteration respectively, tau is an iteration threshold,if so, outputting R at the last iteration k And T k As the rotation matrix and translation vector of the preliminary registration; otherwise, returning to the step (iii).
The high-precision master-slave pose registration method for the surgical robot comprises the following specific steps of acquiring catheter path data under a hand-end actual coordinate system in real time through a pose sensor:
(i1) taking the whole transparent simulated blood vessel and placing the whole transparent simulated blood vessel in the shooting range of the monocular camera;
(i2) wrapping the front end of the catheter with a piece of paper marked with stripes A and B; before wrapping, the stripe A and the stripe B are connected into a V shape; the plane of the wrapped stripe A is vertical to the central axis of the conduit, and the stripe A is positioned at the front end of the stripe B;
(i3) placing the catheter in a simulated blood vessel for contact motion, capturing a stripe A and a stripe B at the speed of acquiring 20 frames of images per second by a monocular camera, converting the images into a gray image after filtering, and detecting and reading pixel coordinates of central points of the stripe A and the stripe B by a frame difference method;
(i4) calibrating the proportional relation between the pixel coordinates and the coordinates in the actual scene through the length and width of the blood vessels in the actual scene and the length and width of the blood vessels in the image, and converting the pixel coordinates of the central points of the stripes A and the stripes B into actual coordinates through the proportional relation;
(i5) the translation freedom degree displacement L of the front end of the catheter can be obtained through the conversion of the actual coordinate of the central point of the stripe A, the rotation displacement theta is obtained through the distance d between the actual coordinate of the central point of the stripe A and the actual coordinate of the central point of the stripe B,
Figure BDA0003611977540000031
(i6) and obtaining the motion coordinate of the catheter through the translation freedom degree displacement L and the rotation displacement theta, namely obtaining the catheter path data under the actual coordinate system of the hand end.
In the high-precision master-slave pose registration method for the surgical robot, in step (i1), the shooting range of the monocular camera is 640 × 480 pixels, the resolution is 1920 × 1080, and the maximum frame rate is 30 FPS.
The high-precision surgical robot master-slave pose registration method is similar to an ICP (inductively coupled plasma) algorithm, the accurate registration also needs to acquire a conversion matrix, and different from the primary registration, in the accurate registration stage, in the dynamic motion process of a catheter, the influence of the contact stress of the catheter on the effectiveness of the master-slave registration is considered, namely the motion of the catheter is divided into free motion and contact motion, and the error of the contact force of a master hand and a slave hand needs to be considered in the target function of the accurate registration; during free motion, a master-slave registration error mainly exists between the actual pose of the slave hand end and the virtual pose of the master hand end, and an error measurement function of the system is a ratio of the deviation between the actual pose of the slave hand end and the virtual pose of the master hand end divided by the actual pose of the slave hand end; when the alignment is accurate, the rotation matrix and the translation vector are obtained through the following process:
(a) calculating a rotation matrix R i And translation vector T i So that
Figure BDA0003611977540000032
P i And Q i Respectively from the position of the catheter path at the hand end and the position of the corresponding point of the center line of the blood vessel at the main hand end, R i Is the rotation matrix of the ith point in R, T i Is the translation vector of the ith point, n is the number of points in the model point set, Err (R, T) is the error of the pose of the master hand catheter and the pose of the slave hand catheter, wherein:
Δx i ＝F k /k；
k N1 Δz 1 +k N2 Δz 2 +L+k N(m-1) Δz m-1 +k Nm Δz m ＝F k ；
in the formula,. DELTA.x i Is the amount of deformation of the ith contact point; f k The unit N is the stress of the spring mass point; k is the elastic coefficient in N/m; k is a radical of Ni The elastic coefficient of the spring in the normal direction of the ith mass point is shown in the unit of N/m, i is 1 … m; Δ z i The deformation of the ith mass point in the normal direction is represented by the unit m, i is 1 … m;
(b) computing
Figure BDA0003611977540000041
Wherein the content of the first and second substances,
Figure BDA0003611977540000042
and T i k Respectively represent R of the k-th iteration i And T i ，P k+1 For data of point set P at the k +1 th iteration, P i k+1 For a point to concentrate a specific point P i At the data of the (k + 1) th iteration,
Figure BDA0003611977540000045
is R k Rotation matrix of the ith point, T i k Is T k The translation vector of the ith point;
(c) calculating the average value of the position error of each point in the master hand catheter model point set and the slave hand catheter model point set
Figure BDA0003611977540000047
(d) Judge d | | k+1 -d k Whether | is less than τ is true, d k Is a sum of d k+1 Respectively the average value of the position errors of the k and k +1 iterations, tau is an iteration threshold, and if the average value is positive, R in the last iteration is output i And T i As a rotation matrix and translation vector for accurate registration; otherwise, returning to the step (a).
Advantageous effects
The invention realizes real-time and accurate human tactile representation of the vascular intervention machine through model registration and pose prediction, improves the transparency of the system, and mainly has the following characteristics:
(1) based on multipoint contact force measurement, a model registration method which takes minimization of force and touch reproduction errors as a target is designed, and high-precision dynamic pose registration of the vascular interventional surgical robot is realized; the invention can enrich and develop dynamic modeling theory, and promote the human tactile feedback technology of the vascular intervention machine to be applied clinically;
(2) carrying out model registration by taking minimization of the relative distance error between the master hand and the slave hand as a target to realize accurate and dynamic model registration of multipoint dynamic line contact modeling; the invention can monitor the safety from two aspects of force touch rendering model calculation and contact force measurement, and improves the transparency and the safety of the surgical robot system.
Drawings
FIG. 1 is a Cosserat elastic rod model;
FIG. 2 is a schematic diagram of a catheter pose measurement based on a frame difference method;
FIG. 3 is a schematic of a guidewire collision and contact force measurement based on an optical fiber and conductive rubber sensor; wherein, 1-copper electrode, 2-conductive rubber, 3-conduit, 4-optical fiber pressure sensor;
FIG. 4 is a schematic diagram of a closest point selection strategy in a registration process;
FIG. 5 is a force analysis graph based on a mass-spring model;
FIG. 6 is a diagram illustrating relative distances between corresponding points;
FIG. 7 is a comparison of the position of each path point for the fine registration of the conventional ICP method and the present invention; wherein, (a) is a master-slave catheter path which is not registered, (b) is a master-slave catheter path which is registered by a traditional ICP method, and (c) is a master-slave catheter path which is precisely registered;
fig. 8 is a comparison of registration error for conventional ICP and the inventive fine registration; wherein, (a) is the conventional ICP registration error, and (b) is the fine registration error of the invention;
fig. 9 is a comparison of the registered catheter path data and the true path.
Detailed Description
The invention will be further illustrated with reference to specific embodiments. It should be understood that these examples are for illustrative purposes only and are not intended to limit the scope of the present invention. Further, it should be understood that various changes or modifications of the present invention may be made by those skilled in the art after reading the teaching of the present invention, and such equivalents may fall within the scope of the present invention as defined in the appended claims.
A high-precision surgical robot master-slave pose registration method specifically comprises the following steps:
(1) the method comprises the steps of achieving preliminary registration of catheter poses based on image recognition measurement;
at the main handThe catheter model is established by the Cosserat elastic rod theory (as shown in figure 1, n is the discrete node number of the catheter model, and the discrete node x 0 ,x 1 ,x 2 ...x n Divide the whole line into a plurality of line segments e 0 ,e 1 ,e 2 ...e n-1 Wherein e is i-1 ＝x i -x i-1 ) Measuring the pose change of a front-end flexible section of the catheter at the slave hand end through image recognition, solving a transformation matrix of a coordinate system of a master hand-end catheter model and a coordinate system of a slave hand-end catheter model based on a D-H coordinate transformation method, realizing the association of the master hand-end catheter model and the slave hand-end catheter model, and realizing the primary registration of the catheter pose through a rotation matrix and a translation vector;
the rotation matrix and the translation vector are obtained by the following process:
(i) obtaining central line data under a main hand end virtual three-dimensional image coordinate system based on a preoperative three-dimensional virtual image coordinate space, namely a main hand end catheter model point set Q, Q ═ Q i |Q i ∈R 3 ,i＝1,2,...,n}(，Q i Representing a particular point, R, in the master hand endpoint set 3 Representing three-dimensional space coordinates, wherein n represents the number of point collection points; real-time acquisition of catheter path data under a slave-hand-end actual coordinate system through a pose sensor, namely a slave-hand-end catheter model point set P, P ═ P { (P {) i |P i ∈R 3 ,i＝1,2,...,n}(，P i Representing a particular point, R, concentrated from the hand end point 3 Representing three-dimensional space coordinates, wherein n represents the number of point collection points;
the method comprises the following specific steps of acquiring the catheter path data under the actual hand coordinate system in real time through a pose sensor:
(i1) taking the whole transparent simulated blood vessel and placing the whole transparent simulated blood vessel in the shooting range of a monocular camera; the shooting range of the monocular camera is 640 multiplied by 480 pixels, the resolution is 1920 multiplied by 1080, and the maximum frame frequency is 30 FPS;
(i2) as shown in fig. 2, the catheter tip is wrapped with a sheet of paper labeled with stripe a and stripe B; before wrapping, the stripe A and the stripe B are connected into a V shape; the plane of the wrapped stripe A is vertical to the central axis of the conduit, and the stripe A is positioned at the front end of the stripe B;
(i3) placing the catheter in a simulated blood vessel for contact motion, capturing a stripe A and a stripe B at the speed of acquiring 20 frames of images per second by a monocular camera, converting the images into a gray image after filtering, and detecting and reading pixel coordinates of central points of the stripe A and the stripe B by a frame difference method;
(i4) calibrating the proportional relation between the pixel coordinates and the coordinates in the actual scene through the length and width of the blood vessels in the actual scene and the length and width of the blood vessels in the image, and converting the pixel coordinates of the central points of the stripes A and the stripes B into actual coordinates through the proportional relation;
(i5) the translation freedom degree displacement L of the front end of the catheter can be obtained through the conversion of the actual coordinate of the central point of the stripe A, the rotation displacement theta is obtained through the distance d between the actual coordinate of the central point of the stripe A and the actual coordinate of the central point of the stripe B,
Figure BDA0003611977540000061
(i6) obtaining the motion coordinate of the catheter through the translation freedom degree displacement L and the rotation displacement theta, namely obtaining the catheter path data under the actual coordinate system of the hand end;
(ii) with P i k Represents P i The (k) th iteration of (a),
Figure BDA0003611977540000063
is represented by P i k The closest point in Q, i.e. the point and Q 1 Is on a straight line Q 1 Q n Length of projection line on and P in P i k And P 1 Is on a straight line P 1 P n The length of the projected line on is closest; as shown in fig. 4;
(iii) calculating a rotation matrix R k And translation vector T k So that
Figure BDA0003611977540000066
(iv) Computing
Figure BDA0003611977540000067
P k+1 Is a pointData of set P at k +1 th iteration, P i k+ 1 is a point-concentrated specific point P i At the data of the (k + 1) th iteration,
Figure BDA0003611977540000069
is R k Rotation matrix of the ith point, T i k Is T k The translation vector of the ith point;
(v) calculating the average value of the position error of each point in the master hand-end catheter model point set and the slave hand-end catheter model point set
Figure BDA00036119775400000611
(vi) Judge d | | k+1 -d k Whether < τ (τ taken to be 1mm) holds, d k And d k+1 Respectively the average value of the position errors of the k th iteration and the k +1 th iteration, if so, the R of the last iteration is output k And T k As the rotation matrix and translation vector of the preliminary registration; otherwise, returning to the step (iii);
(2) the dynamic accurate registration of the pose of the catheter is realized based on the multipoint contact force measurement;
based on a closest point iterative algorithm (ICP), finding a rotation matrix and a translation vector between a master hand end catheter model and a slave hand end catheter model, so that the two matched data meet the optimal matching under certain measurement;
compared with preliminary registration, accurate registration is based on measurement force, and deformation displacement delta x of contact point of conduit is solved i Calculating the optimal transformation matrix by the concept of relative distance, and FIG. 6 is a diagram illustrating the relative distance between the catheter path point and the corresponding point of the blood vessel centerline, where point a and point b are the collision contact points of the catheter and the blood vessel wall, Δ x 1 And Δ x 2 D is the amount of deformation under the contact force 1 And d 2 The distances between the points a, b and their corresponding points, respectively, using the relative distances
Figure BDA0003611977540000071
The registration accuracy can be measured, and the position deviation matching caused by stress deformation can be reducedThe influence of the precision; and (3) precisely registering the objective function, considering the stress deformation, and solving a transformation matrix by iteratively minimizing the relative distance between corresponding points of the master hand and the slave hand.
As shown in fig. 3, the contact stress of the front end of the guide tube 5 is measured by using the optical fiber pressure sensor 6 installed at the front end of the guide tube, and the contact stress of the side wall of the guide tube 5 is measured by using the piezoresistive effect of the conductive rubber 4 coated on the side wall of the guide tube; the copper electrode 3 is powered; the diameter of a probe of the optical fiber pressure sensor is 0.25mm, the precision is +/-2 mmHg, the resolution is 0.5mmHg, the outer diameter is 1mm, the length is 4m, the surface is coated with PTFE (polytetrafluoroethylene), and the highest sampling frequency is 250Hz (namely the sampling frequency is not more than 250 Hz); the optical fiber pressure sensor adopts white light interference propagation, is not interfered by electromagnetic signals, radio frequency signals, magnetic resonance signals and other microwave signals in an application environment, can well keep the original precision and repeatability, can be easily measured and used repeatedly, has electromagnetic compatibility, and can meet the requirements of universal blood vessel intervention: the shape of the blood vessel is soft enough, has no harm to human body, and can adapt to the complexity of the shape of the blood vessel of human body; the size is small, and the catheter is convenient to integrate with a catheter; high sensitivity; the conductive rubber is silicon rubber with conductive particles uniformly dispersed inside; under the action of pressure, the conductive particles in the conductive rubber are contacted to generate a piezoresistive effect; the conductive rubber has a simple and reliable structure, can fully ensure the real-time performance and the accuracy of force measurement, and can meet the requirement of universal blood vessel intervention: the shape of the blood vessel is soft enough, has no harm to human body, and can adapt to the complexity of the shape of the blood vessel of human body; the size is small, and the catheter is convenient to integrate with a catheter; high sensitivity;
the rotation matrix and the translation vector are obtained by the following process:
(a) calculating a rotation matrix R i And translation vector T i So that
Figure BDA0003611977540000072
P i And Q i Respectively from the position of the catheter path at the hand end and the position of the corresponding point of the center line of the blood vessel at the main hand end, R i Is the rotation matrix of the ith point in R, T i Is the translation vector of the ith point, n is the number of model points, Err (R, T) is the error of the pose of the master hand catheter and the pose of the slave hand catheter, where Δ x i Is the amount of deformation, Δ x, of the ith contact point i The method is calculated based on the force measurement at the hand end, and the deformation behavior of the vessel wall in an area generated by taking a contact point as a center is considered to be related to adjacent springs, namely the total deformation displacement under the action of the adjacent springs needs to be solved, and the force analysis is shown in fig. 5;
F k ＝k N1 Δz 1 +k T1 Δr 1 sinθ 1 ＝f N1 +f T1 sinθ 1 ；
wherein f is T1 Representing tangential force, f N1 Denotes normal force, k N1 Denotes the spring constant, Δ r, of the spring in the direction of the particle normal 1 Representing the amount of deflection, Δ z, of the spring between adjacent particles in the tangential direction 1 Representing the amount of deformation of the particle in the normal direction, theta 1 The included angle between the connecting line of adjacent particles and the horizontal direction; for the i spring particles:
f N1 +f N2 +L+f N(i-1) +f Ni ＝F k -f Ti sinθ i ；
let i be m, sin θ i When the value is equal to 0, then
k N1 Δz 1 +k N2 Δz 2 +L+k N(m-1) Δz m-1 +k Nm Δz m ＝F k ；
Wherein k is Ni The elastic coefficient of the spring in the normal direction of the ith mass point is shown in the unit of N/m, i is 1 … m; Δ z i The deformation of the ith mass point in the normal direction is represented by the unit m, i is 1 … m; total amount of deformation displacement Δ x in the region of each contact point i The expression of (a) is as follows:
Δx i ＝F k /k；
wherein, F k The unit N is the stress of the spring mass point; k is the elastic coefficient in N/m;
(b) computing
Figure BDA0003611977540000081
Wherein the content of the first and second substances,
Figure BDA0003611977540000082
and T i k Respectively represent R of the k-th iteration i And T i ，P k+1 For data of point set P at the k +1 th iteration, P i k+1 For a point to concentrate a specific point P i At the data of the (k + 1) th iteration,
Figure BDA0003611977540000085
is R k Rotation matrix of the ith point, T i k Is T k The translation vector of the ith point;
(c) calculating the average value of the position error of each point in the master hand catheter model point set and the slave hand catheter model point set
Figure BDA0003611977540000087
(d) Judge d | | k+1 -d k Whether | is less than τ is true, d k Is a sum of d k+1 Respectively the average value of the position errors of the k th iteration and the k +1 th iteration, if so, the R of the last iteration is output i And T i As a rotation matrix and translation vector for accurate registration; otherwise, returning to the step (a).
During the experiment, an operator holds the omega.7 handle at the main hand end to push, pull and turn a knob, and inserts the catheter which is positioned at the tail end of the UR5 robot from the hand end into a blood vessel, and drives the catheter to reach a target focus from an aorta blood vessel through a bifurcation blood vessel. And meanwhile, the position data of the front end of the catheter, which are acquired by the hand monocular camera and the force sensor, are identified, the path of the catheter is acquired, and then the registration processing is carried out on the path of the catheter and the path of the catheter at the main hand. Fig. 7 shows position data of each path point after the conventional ICP registration method and the high-precision pose registration method of the present invention are used, and the accuracy of the path positions of the master-slave catheters obtained after the high-precision pose registration method of the present invention is registered is significantly higher than that of the conventional ICP registration method.
The registration error corresponding to the calculated catheter path point is shown in fig. 8, in the registration process, the error is gradually reduced, the error of the initial iteration is maximum, the error reduction amplitude is also maximum, and the amplitude finally tends to be stable along with the increase of the iteration times. The traditional ICP registration method does not consider the influence of stress, and the root mean square error of final registration reaches 6.72 mm. The high-precision pose registration method reduces registration errors. With the iteration times in the registration process increasing, the obtained conversion matrix is more and more accurate, the corresponding registration error is smaller and smaller, and finally the obtained conversion matrix is stabilized at about 1.02 mm.
Fig. 9 shows the real path and the post-registration path, and it can be seen from the figure that, according to the registration result, the master hand completes the real-time accurate display of the catheter position, and the precision of the fine registration of the present invention can meet the requirement of the interventional operation.

Claims (6)

1. A high-precision master-slave pose registration method for a surgical robot is characterized by comprising the following steps: the method comprises the steps of firstly realizing preliminary registration of the pose of a catheter based on image recognition measurement, and then realizing dynamic accurate registration of the pose of the catheter based on multipoint contact force measurement;
the process of realizing the dynamic accurate registration of the catheter pose based on the multipoint contact force measurement comprises the following steps: and solving the deformation displacement of the contact point of the conduit based on the measurement force, and finding a rotation matrix and a translation vector between the master hand end conduit model and the slave hand end conduit model based on a closest point iterative algorithm so that the two matching data meet the optimal matching under certain measurement.
2. The high-precision master-slave pose registration method for the surgical robot according to claim 1, wherein the primary registration process comprises the following steps: the method comprises the steps of establishing a catheter model on a master hand end by using a Cosserat elastic rod theory, measuring the pose change of a front soft section of the catheter at a slave hand end through image recognition, solving a transformation matrix of a coordinate system of the master hand end catheter model and a coordinate system of the slave hand end catheter model based on a D-H coordinate transformation method, realizing the association of the master hand end catheter model and the slave hand end catheter model, and realizing the primary registration of the pose of the catheter by using a rotation matrix and a translation vector.
3. The high-precision master-slave pose registration method for the surgical robot according to claim 2, wherein the rotation matrix and the translation vector are obtained by the following procedures during the initial registration:
(i) obtaining central line data under a main hand end virtual three-dimensional image coordinate system based on a preoperative three-dimensional virtual image coordinate space, namely a main hand end catheter model point set Q, Q ═ Q i |Q i ∈R 3 ,i＝1,2,...,n}(，Q i Representing a particular point, R, in the master hand endpoint set 3 Representing three-dimensional space coordinates, wherein n represents the number of point collection points; real-time acquisition of catheter path data under a slave-hand-end actual coordinate system through a pose sensor, namely a slave-hand-end catheter model point set P, P ═ P { (P {) i |P i ∈R 3 ,i＝1,2,...,n}(，P i Representing a particular point, R, concentrated from the hand end point 3 Representing three-dimensional space coordinates, wherein n represents the number of point collection points;
(ii) with P i k Represents P i The (k) th iteration of (a),
Figure FDA0003611977530000011
represents P i k The closest point in Q, i.e. the point and Q 1 Is on a straight line Q 1 Q n Length of projection line on and P in P i k And P 1 Is on a straight line P 1 P n The length of the projected line on is closest;
(iii) calculating a rotation matrix R of the point set Q and the point set P k And translation vector T k So that
Figure FDA0003611977530000012
(iv) Computing
Figure FDA0003611977530000013
P k+1 For data of point set P at the k +1 th iteration, P i k+1 For a point to concentrate a specific point P i Number of iterations at k +1According to the above-mentioned technical scheme,
Figure FDA0003611977530000014
is R k Rotation matrix of the ith point, T i k Is T k The translation vector of the ith point;
(v) calculating the average value of the position error of each point in the master hand catheter model point set and the slave hand catheter model point set
Figure FDA0003611977530000015
(vi) Judge d | | k+1 -d k Whether | is less than τ is true, d k And d k+1 Respectively the average value of the position errors of the k and k +1 iterations, tau is an iteration threshold, and if the average value is positive, R in the last iteration is output k And T k As the rotation matrix and translation vector of the preliminary registration; otherwise, returning to the step (iii).
4. The high-precision master-slave pose registration method for the surgical robot according to claim 3, wherein the real-time acquisition of the catheter path data from the hand-end actual coordinate system by the pose sensor comprises the following steps:
(i1) taking the whole transparent simulated blood vessel and placing the whole transparent simulated blood vessel in the shooting range of the monocular camera;
(i2) wrapping the front end of the catheter with a piece of paper marked with stripes A and B; before wrapping, the stripe A and the stripe B are connected into a V shape; the plane of the wrapped stripe A is vertical to the central axis of the catheter, and the stripe A is positioned at the front end of the stripe B;
(i3) placing the catheter in a simulated blood vessel for contact motion, capturing a stripe A and a stripe B at the speed of acquiring 20 frames of images per second by a monocular camera, converting the images into a gray image after filtering, and detecting and reading pixel coordinates of central points of the stripe A and the stripe B by a frame difference method;
(i4) calibrating the proportional relation between the pixel coordinates and the coordinates in the actual scene through the length and the width of the blood vessels in the actual scene and the length and the width of the blood vessels in the image, and converting the pixel coordinates of the central points of the stripes A and the stripes B into actual coordinates through the proportional relation;
(i5) the translation freedom degree displacement L of the front end of the catheter can be obtained through the conversion of the actual coordinate of the central point of the stripe A, the rotation displacement theta is obtained through the distance d between the actual coordinate of the central point of the stripe A and the actual coordinate of the central point of the stripe B,
Figure FDA0003611977530000021
(i6) and obtaining the motion coordinate of the catheter through the translation freedom degree displacement L and the rotation displacement theta, namely obtaining the catheter path data under the actual coordinate system of the hand end.
5. The high-precision surgical robot master-slave pose registration method according to claim 4, wherein in step (i1), the shooting range of the monocular camera is 640 × 480 pixels, the resolution is 1920 × 1080, and the maximum frame rate is 30 FPS.
6. The high-precision master-slave pose registration method for the surgical robot according to claim 4, wherein the rotation matrix and the translation vector are obtained by the following procedures during precise registration:
(a) calculating a rotation matrix R i And translation vector T i So that
Figure FDA0003611977530000022
P i And Q i Respectively from the position of the catheter path at the hand end and the position of the corresponding point of the center line of the blood vessel at the main hand end, R i Is the rotation matrix of the ith point in R, T i Is the translation vector of the ith point, n is the number of points in the model point set, Err (R, T) is the error of the pose of the master hand catheter and the pose of the slave hand catheter, wherein:
Δx i ＝F k /k；
k N1 Δz 1 +k N2 Δz 2 +L+k N(m-1) Δz m-1 +k Nm Δz m ＝F k ；
in the formula,. DELTA.x i Is the amount of deformation of the ith contact point; f k The unit N is the stress of the spring mass point; k is the elastic coefficient in N/m; k is a radical of Ni The elastic coefficient of the spring in the normal direction of the ith mass point is shown in the unit of N/m, i is 1 … m; Δ z i The deformation of the ith mass point in the normal direction is represented by the unit m, i is 1 … m;
(b) computing
Figure FDA0003611977530000031
Wherein the content of the first and second substances,
Figure FDA0003611977530000032
and T i k Respectively represent R of the k-th iteration i And T i ，P k+1 For data of point set P at the k +1 th iteration, P i k+1 For a point to concentrate a specific point P i At the data of the (k + 1) th iteration,
Figure FDA0003611977530000033
is R k Rotation matrix of the ith point, T i k Is T k The translation vector of the ith point;
(c) calculating the average value of the position error of each point in the master hand catheter model point set and the slave hand catheter model point set
Figure FDA0003611977530000034
(d) Judge d | | k+1 -d k Whether | is less than τ is true, d k Is a sum of d k+1 Respectively the average value of the position errors of the k and k +1 iterations, tau is an iteration threshold, and if the average value is positive, R in the last iteration is output i And T i As a rotation matrix and translation vector for accurate registration; otherwise, returning to the step (a).
CN202210447243.4A 2022-04-24 2022-04-24 High-precision master-slave pose registration method for surgical robot Pending CN114863417A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210447243.4A CN114863417A (en) 2022-04-24 2022-04-24 High-precision master-slave pose registration method for surgical robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210447243.4A CN114863417A (en) 2022-04-24 2022-04-24 High-precision master-slave pose registration method for surgical robot

Publications (1)

Publication Number Publication Date
CN114863417A true CN114863417A (en) 2022-08-05

Family

ID=82633442

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210447243.4A Pending CN114863417A (en) 2022-04-24 2022-04-24 High-precision master-slave pose registration method for surgical robot

Country Status (1)

Country Link
CN (1) CN114863417A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115363774A (en) * 2022-08-22 2022-11-22 武汉理工大学 Fusion force shape sensing interventional catheter surgical robot, control system and method
CN115778554A (en) * 2023-01-09 2023-03-14 深圳市精锋医疗科技股份有限公司 Catheter robot, registration method thereof and readable storage medium

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115363774A (en) * 2022-08-22 2022-11-22 武汉理工大学 Fusion force shape sensing interventional catheter surgical robot, control system and method
CN115363774B (en) * 2022-08-22 2024-05-10 武汉理工大学 Interventional catheter surgical robot with fusion force sensing function, control system and method
CN115778554A (en) * 2023-01-09 2023-03-14 深圳市精锋医疗科技股份有限公司 Catheter robot, registration method thereof and readable storage medium

Similar Documents

Publication Publication Date Title
CN114863417A (en) High-precision master-slave pose registration method for surgical robot
US11678813B2 (en) Systems and methods for deformation compensation using shape sensing
US11504187B2 (en) Systems and methods for localizing, tracking and/or controlling medical instruments
US8271068B2 (en) Method for dynamic road mapping
Lobaton et al. Continuous shape estimation of continuum robots using X-ray images
JP2020513904A (en) Device for flexible device insertion
CN102835965B (en) Optical pressure measuring
CN109414287A (en) For generating the method and system of the smoothed image of elongate medical device
Brij Koolwal et al. An ultrasound-based localization algorithm for catheter ablation guidance in the left atrium
JP6436442B2 (en) Photoacoustic apparatus and image processing method
Wang et al. Robotic ultrasound: View planning, tracking, and automatic acquisition of transesophageal echocardiography
Vandini et al. Vision-based intraoperative shape sensing of concentric tube robots
CN113842172B (en) Pharyngeal rear wall visual touch recognition device based on template matching and arithmetic averaging
CN113229941A (en) Interventional robot contactless teleoperation system based on augmented reality and calibration method
Deng et al. Learning robotic ultrasound scanning skills via human demonstrations and guided explorations
CN115089212A (en) Three-dimensional vision-guided automatic neck ultrasonic scanning method and system for mechanical arm
Meng et al. New method for geometric calibration and distortion correction of conventional C-arm
Kaya et al. Visual needle tip tracking in 2D US guided robotic interventions
CN111477318B (en) Virtual ultrasonic probe tracking method for remote control
Ma et al. A novel robotic system for ultrasound-guided peripheral vascular localization
CN114536331B (en) Method for determining external stress of deformable mechanical arm based on association identification and robot system
Zhao et al. Needle tip pose estimation for ultrasound-guided steerable flexible needle with a complicated trajectory in soft tissue
Boctor et al. Ultrasound self-calibration
CN115067993A (en) Ultrasonic probe control method and device, terminal equipment and storage medium
Mao et al. Direct 3d ultrasound fusion for transesophageal echocardiography

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