CN114863417A - High-precision master-slave pose registration method for surgical robot - Google Patents
High-precision master-slave pose registration method for surgical robot Download PDFInfo
- 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
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
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),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;
(iv) ComputingP 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,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
(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,
(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 thatP 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) computingWherein the content of the first and second substances,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,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
(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,
(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),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;
(iv) ComputingP 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,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
(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 distancesThe 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 thatP 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) computingWherein the content of the first and second substances,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,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
(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),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
(iv) ComputingP 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,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
(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,
(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 thatP 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) computingWherein the content of the first and second substances,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,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
(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).
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)
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 |
-
2022
- 2022-04-24 CN CN202210447243.4A patent/CN114863417A/en active Pending
Cited By (3)
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 |