A planning and control method for puncture robot

Abstract

The present invention discloses a puncture robot planning and control method, which belongs to the field of medical robot technology, including: establishing the connecting rod coordinate system of the puncture robot by improving the D-H method; calculating the transformation matrix between the coordinate systems of the puncture robot according to the D-H coordinate parameters, and multiplying the transformation matrix between the coordinate systems to obtain the conversion matrix from the base coordinate system of the puncture robot to the puncture needle coordinate system; calculating the forward kinematics equation of the puncture robot, defining the puncture angle and puncture length, and determining the direct relationship between the position of the puncture needle tip and the displacement of each motor sensor; using the geometric analysis method to solve the inverse kinematics for the displacement of each motor sensor. The present invention adopts the above-mentioned puncture robot planning and control method, which can improve the accuracy and efficiency of the biopsy process, reduce complications and patient discomfort, reduce the fatigue of doctors' operation, and improve the safety and reliability of biopsy surgery.

Description

Technical Field

[0001] The invention relates to the technical field of medical robots, and in particular to a puncture robot planning and control method. Background Art

[0002] With the continuous development of medical robot technology, the automation and accuracy of prostate biopsy have become the focus of attention. Traditional biopsy surgery has certain limitations, such as the influence of the surgeon's technical level on the biopsy results and human errors during the operation. Therefore, robot-assisted biopsy methods have become a trend that has received much research and attention.

[0003] Robot-assisted biopsy systems are usually designed with image guidance, path planning algorithms, and robot control. Image guidance technology provides real-time or previously acquired image information of the patient's anatomical structure, which helps determine the location, size, and depth of the biopsy target area. It is transmitted to the system through specific processing to help the robot operator locate and navigate during the biopsy process; the path planning algorithm considers factors such as the complexity of the anatomical structure, avoiding damage to surrounding tissues, and minimizing patient discomfort, and finds the best movement path for the robotic arm or needle; the robot control uses a series of motors and sensors to reach a specific target point or posture, with high precision and stability as the premise, which directly affects the success rate of the operation.

[0004] In the traditional image-guided method of percutaneous puncture of prostate tumors in the urinary system, manual puncture often has the problem of difficult and accurate manual positioning, resulting in the undesirable situation of "inaccurate puncture". Specifically, the results of manual puncture are easily affected by factors such as manual needle insertion accuracy and biopsy needle insertion posture restrictions, and the accuracy is low. Doctors are prone to fatigue during manual operation, the operation is complicated, the degree of standardization is not high, and patient satisfaction is low. Prostate puncture includes surgical methods such as systematic puncture and targeted puncture. For example, parallel puncture is used in systematic puncture, and the needle is inserted 12 times, resulting in multiple bleeding points, multiple complications, high pain, slow recovery, and easy infection. Summary of the invention

[0005] The purpose of the present invention is to provide a puncture robot planning and control method, which can improve the accuracy and efficiency of the biopsy process, reduce complications and patient discomfort, and reduce the fatigue of the doctor's operation, thereby improving the patient's treatment experience and improving the safety and reliability of the biopsy operation.

[0006] To achieve the above object, the present invention provides a puncture robot planning and control method, comprising the following steps:

[0007] S1. Establish the connecting rod coordinate system of the puncture robot by improving the DH method;

[0008] S2. Calculate the transformation matrix between the coordinate systems of the puncture robot according to the DH coordinate parameters of the puncture robot, and multiply the transformation matrices between the coordinate systems to obtain the transformation matrix from the puncture robot coordinate system to the puncture needle coordinate system;

[0009] S3, calculating the forward kinematics equation of the puncture robot, defining the puncture angle and puncture length, and finally determining the direct relationship between the position of the puncture needle tip and the displacement of each motor sensor;

[0010] S4. Use geometric analysis method to solve the inverse kinematics to calculate the displacement of each motor sensor.

[0011] Preferably, in step S1, the connecting rods are numbered starting from the fixed base, the fixed base is numbered as connecting rod 0, the first movable connecting rod is numbered as 1, and then the subsequent connecting rods are numbered in sequence;

[0012] A coordinate system is fixed on each connecting rod to describe the positional relationship between each connecting rod and the adjacent connecting rod. The coordinate system is named according to the connecting rod name, that is, the coordinate system fixed on connecting rod i is coordinate system {i};

[0013] The link coordinate system of the puncture robot is established according to the improved DH method, which includes three moving joints and two rotating joints.

[0014] Preferably, the homogeneous transformation matrix of the improved DH method is as follows:

[0015]

[0016] Among them, α i-1 is the connecting rod torsion angle, which is the rotation angle from axis to axis around the axis, and conforms to the right-hand rule; a i-1 is the length of the connecting rod, the distance between the axes of two adjacent joints, the length of the perpendicular line between the axes, a i-1 >0;d i is the connecting rod distance, is the directed distance between the axes; θ i is the connecting rod rotation angle, which rotates around the axis and the axis-to-axis rotation angle conforms to the right-hand rule; is the transformation matrix from coordinate system i-1 to coordinate system i; c represents cosine; s represents sin;

[0017] According to the coordinate parameters of the puncture robot, the transformation matrix between the coordinate systems of the puncture robot is calculated as follows:

[0018]

[0019] The transformation matrix from the puncture robot base coordinate system to the puncture needle coordinate system is obtained by multiplying the transformation matrices as follows:

[0020]

[0021] The general formula of the transformation matrix is ​​as follows:

[0022]

[0023] Where p=(p x , p v , p z ) is the position vector of the puncture needle tip; n, o, a vectors are the transformation matrices of the actuator end point x, y, z axis respectively; n = (n x , n y , n z ) is the normal vector, i.e., the X-axis fixed to the puncture needle coordinate system; o = (o x , o y ,o z ) is the Y axis, a=(a x , a y , a z ) is the Z axis; L represents the puncture length, that is, the distance from the front end point of the robot end guide to the puncture needle tip.

[0024] Preferably, in step S3, the forward kinematics equation of the puncture robot is obtained by combining formula (8) and formula (9):

[0025]

[0026] Preferably, in step S3, the displacement of the front-end mechanism motor sensor is set to d x1 , d y1 , the displacement of the rear-end mechanism motor sensor is d x2 , d y2 , the displacement of the base mechanism motor sensor is d z ; According to the coordinate system, θ 4 is the puncture deflection angle, θ 5 is the penetration pitch angle, and the specific expression is:

[0027]

[0028] Evaluate the algebraic values ​​in the forward kinematic equations:

[0029]

[0030] Define the displacement of each motor sensor of the robot in the previous state Calculate the displacement of each motor sensor:

[0031]

[0032] Combining formulas (10), (12), and (13), we can obtain the direct relationship between the needle tip position P(px, py, pz) and the displacement of each motor sensor:

[0033]

[0034] p=(p x , p v , p z ) is the position vector of the puncture needle tip; D is the fixed distance between the front-end mechanism and the rear-end mechanism of the robot.

[0035] Preferably, in step S4, the specific steps are as follows:

[0036] S41, assuming that the puncture needle tip is located at the target point, and the puncture needle tip position P (p x , p y , p z ), needle insertion point Q(q x ,q y ,q z ), the initial posture of the end effector A(A x0 , A y0 , A z0 ), B(B x0 , B y0 , B z0 ), assuming the final posture of the end effector (A x1 , A y1 , A z1 ), (B x1 , B y1 , B z1 );

[0037] S42. The equation of a two-point straight line in a known space is as follows:

[0038]

[0039] S43, substituting the two points PQ in step S41 into the straight line equation in step S42, the result is:

[0040]

[0041] The puncture angle is obtained from the two points PQ:

[0042]

[0043] Where Δx = q x -p x , Δy=q y -p y , Δz=q z -p z ;

[0044] S44. When the puncture length L is known, the value of the front end point of the guide in the Z-axis direction is equal to the Z coordinate value of the puncture needle tip minus the projection distance of the puncture length in the Z direction:

[0045] A z1 =p z -Lcθ 4 sθ 5 (18)

[0046] Where cθ4 represents cosθ 4 ; sθ5 represents sinθ 5 ;

[0047] B z1 =A z1 -D (19)

[0048] Substituting into equation (16) we get (A x1 , A y1 , A z1 ), (B x1 , B y1 , B z1 ) specific numerical solution;

[0049] S45. Comparison of the final posture with the initial posture yields:

[0050] Displacement in z-axis direction: d z =A z1 -A z0 ;

[0051] Displacement in the x-axis direction:

[0052] Front point: d x1 =A y1 -A y0 ; Back endpoint: d x2 =B y1 -B y0 ;

[0053] Displacement in the y-axis direction:

[0054] Front point: d y1 =A x1 -A x0 ; Back endpoint: d y2 =B x1 -B x0 .

[0055] Therefore, the present invention adopts the above-mentioned puncture robot planning and control method, which can improve the accuracy and efficiency of the biopsy process, reduce complications and patient discomfort, and reduce the doctor's operating fatigue, thereby improving the patient's treatment experience and improving the safety and reliability of the biopsy operation.

[0056] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 is the puncture robot link coordinate system

[0058] Figure 2 Define a graph for the puncture angle;

[0059] Figure 3 It is the geometric analysis diagram of inverse kinematics;

[0060] Figure 4 Schematic diagram of puncture length and angle. DETAILED DESCRIPTION

[0061] The technical solution of the present invention is further described below through the accompanying drawings and embodiments.

[0062] Unless otherwise defined, technical or scientific terms used in the present invention shall have the common meanings understood by one having ordinary skills in the field to which the present invention belongs.

[0063] Embodiment 1

[0064] The puncture robot has 7 degrees of freedom, with the end close to the patient as the front end and the end away from the patient as the back end. The front and back ends each have two motors in the horizontal and vertical directions to jointly control the end effector, giving it a certain pitch and deflection angle.

[0065] The present invention provides a puncture robot planning and control method, which specifically comprises the following steps:

[0066] S1. Establish the connecting rod coordinate system of the puncture robot by improving the DH method;

[0067] In step S1, the connecting rods are numbered starting from the fixed base, the fixed base is numbered as connecting rod 0, the first movable connecting rod is numbered as 1, and then the subsequent connecting rods are numbered in sequence;

[0068] A coordinate system is fixed on each connecting rod to describe the positional relationship between each connecting rod and the adjacent connecting rod. The coordinate system is named according to the connecting rod name, that is, the coordinate system fixed on connecting rod i is coordinate system {i};

[0069] The link coordinate system of the puncture robot is established according to the improved DH method, which includes three moving joints and two rotating joints. Figure 1 The state DH coordinate parameters are shown in Table 1.

[0070] Table 1 DH coordinate parameter table of puncture robot

[0071]

[0072]

[0073] S2. Calculate the transformation matrix between the coordinate systems of the puncture robot according to the DH coordinate parameters of the puncture robot, and multiply the transformation matrices between the coordinate systems to obtain the transformation matrix from the puncture robot coordinate system to the puncture needle coordinate system;

[0074] The general formula of the homogeneous transformation matrix of the improved DH method is as follows:

[0075]

[0076] Among them, α i-1 is the connecting rod torsion angle, which is the rotation angle from axis to axis around the axis, and conforms to the right-hand rule; a i-1 is the length of the connecting rod, the distance between the axes of two adjacent joints, the length of the perpendicular line between the axes, a i-1 >0;d i is the connecting rod distance, is the directed distance between the axes; θ i is the connecting rod rotation angle, which rotates around the axis and the axis-to-axis rotation angle conforms to the right-hand rule; is the transformation matrix from coordinate system i-1 to coordinate system i; c represents cosine; s represents sin;

[0077] According to the coordinate parameters of the puncture robot, the transformation matrix between the coordinate systems of the puncture robot is calculated as follows:

[0078]

[0079]

[0080] The transformation matrix from the puncture robot base coordinate system to the puncture needle coordinate system is obtained by multiplying the transformation matrices as follows:

[0081]

[0082] The general formula of the known transformation matrix is ​​as follows:

[0083]

[0084] Where p=(p x , p y , p z ) is the position vector of the puncture needle tip; n, o, a vectors are the transformation matrices of the actuator end point x, y, z axis respectively; n = (n x , n y , n z ) is the normal vector, i.e., the X-axis fixed to the puncture needle coordinate system; o = (o x , o y , o z ) is the Y axis, a=(a x , a y , a z) is the Z axis; L represents the puncture length, that is, the distance from the front end point of the robot end guide to the puncture needle tip.

[0085] S3, calculating the forward kinematics equation of the puncture robot, defining the puncture angle and puncture length, and finally determining the direct relationship between the position of the puncture needle tip and the displacement of each motor sensor;

[0086] In step S3, the forward kinematics equation of the puncture robot is obtained by combining formula (8) and formula (9):

[0087]

[0088] In step S3, the displacement of the front-end mechanism motor sensor is dx1, dy1, the displacement of the rear-end mechanism motor sensor is dx2, dy2, and the displacement of the base mechanism motor sensor is dz; according to the coordinate system, the puncture angle is as follows: Figure 2 As shown, θ4 is the puncture deflection angle, θ5 is the puncture pitch angle, and the specific expression is:

[0089]

[0090] Evaluate the algebraic values ​​in the forward kinematic equations:

[0091]

[0092] Specifically, define the displacement d*i of each motor sensor of the robot in the previous state, and calculate the displacement of each motor sensor:

[0093]

[0094] Combining formulas (10), (12), and (13), we can get the needle tip position P(p x , p y , p z ) and the direct relationship between the displacement of each motor sensor:

[0095]

[0096] p=(p x , p v , p z ) is the position vector of the puncture needle tip; D is the fixed distance between the front-end mechanism and the rear-end mechanism of the robot.

[0097] S4. Use the geometric analysis method to solve the inverse kinematics to calculate the displacement of each motor sensor. Figure 3 shown.

[0098] In step S4, the specific steps are as follows:

[0099] S41, assuming that the puncture needle tip is located at the target point, and the puncture needle tip position P (p x , p y , p z ), needle insertion point Q(q x ,q y ,q z ), the initial posture of the end effector A(A x0 , A y0 , A z0 ), B(B x0 , B y0 , B z0 ), assuming the final posture of the end effector (A x1 , A y1 , A z1 ), (B x1 , B y1 , B z1 );

[0100] S42. The equation of a two-point straight line in a known space is as follows:

[0101]

[0102] S43, substituting the two points PQ in step S41 into the straight line equation in step S42, the result is:

[0103]

[0104] The puncture angle is obtained from the two points PQ:

[0105]

[0106] Where Δx = q x -p x , Ay=q v -p y , Δz=q z -p z .

[0107] S44. When the puncture length L is known, the value of the front end point of the guide in the Z-axis direction is equal to the Z coordinate value of the puncture needle tip minus the projection distance of the puncture length in the Z direction:

[0108] A z1 =p z -Lcθ 4 sθ 5 (18)

[0109] Among them, cθ4 represents cosθ4; sθ5 represents sinθ5;

[0110] B z1 =A z1-D (19)

[0111] Substituting into equation (16) we get (A x1 , A y1 , A z1 ), (B x1 , B y1 , B z1 ) specific numerical solution;

[0112] S45. Comparison of the final posture with the initial posture yields:

[0113] Displacement in z-axis direction: dz = A z1 -A z0 ;

[0114] Displacement in the x-axis direction:

[0115] Front point: dx1 = A y1 -A y0 ; Back endpoint: dx2 = B y1 -B y0 ;

[0116] Displacement in the y-axis direction:

[0117] Front point: dy1 = A x1 -A x0 ; Back endpoint: dy2 = B x1 -B x0 .

[0118] Therefore, the present invention adopts the above-mentioned puncture robot planning and control method, which can improve the accuracy and efficiency of the biopsy process, reduce complications and patient discomfort, and reduce the doctor's operating fatigue, thereby improving the patient's treatment experience and improving the safety and reliability of the biopsy operation.

[0119] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A puncture robot planning and control method, characterized in that: The following steps are involved: S1. Establish the connecting rod coordinate system of the puncture robot by improving the DH method; specifically, the connecting rods are numbered in sequence starting from the fixed base, the fixed base is numbered as connecting rod 0, and the subsequent connecting rods are numbered in sequence, a coordinate system is fixed on each connecting rod, and the coordinate system is named according to the connecting rod name; S2. Calculate the transformation matrix between the coordinate systems of the puncture robot according to the DH coordinate parameters of the puncture robot, and multiply the transformation matrices between the coordinate systems to obtain the transformation matrix from the puncture robot coordinate system to the puncture needle coordinate system; S3, calculating the forward kinematics equation of the puncture robot, defining the puncture angle and puncture length, and finally determining the direct relationship between the position of the puncture needle tip and the displacement of each motor sensor; S4, using a geometric analytical method to solve the inverse kinematics to obtain the displacement of each motor sensor; The general formula of the homogeneous transformation matrix of the improved DH method is as follows: （1） in, is the connecting rod torsion angle, the rotation angle from axis to axis around the axis, which conforms to the right-hand rule; is the length of the connecting rod, the distance between the axes of two adjacent joints, and the length of the perpendicular line between the axes. ; is the connecting rod distance, is the directed distance between the axes; is the connecting rod rotation angle, which rotates around the axis and the axis-to-axis rotation angle conforms to the right-hand rule; For the coordinate system Convert to coordinate system The transformation matrix; c represents cos; s represents sin; According to the coordinate parameters of the puncture robot, the transformation matrix between the coordinate systems of the puncture robot is calculated as follows: （2） （3） （4） （5） （6） （7） The transformation matrix from the puncture robot base coordinate system to the puncture needle coordinate system is obtained by multiplying the transformation matrices as follows: （8） The general formula of the transformation matrix is ​​as follows: （9） in, is the position vector of the puncture needle; , , The vectors are the actuator end point posture , , The transformation matrix of the axis; is the normal vector, i.e., the X-axis fixed in the puncture needle coordinate system; is the Y axis, is the Z axis; Indicates the puncture length, that is, the distance from the front end point of the robot end guide to the puncture needle tip; In step S3, the forward kinematics equation of the puncture robot is obtained by combining formula (8) and formula (9): （10）； In step S3, the displacement of the front-end mechanism motor sensor is set to , , the displacement of the rear-end mechanism motor sensor is , , the displacement of the base mechanism motor sensor is ; Established according to the coordinate system, is the penetration pitch angle, and the specific expression is: （11） Evaluate the algebraic values ​​in the forward kinematic equations: （12） Define the displacement of each motor sensor of the robot in the previous state , calculate the displacement of each motor sensor: （13） Combining formulas (10), (12), and (13), we get the needle tip position: Direct relationship with the displacement of each motor sensor: （14） is the position vector of the puncture needle; is the fixed distance between the front and rear mechanisms of the robot; In step S4, the specific steps are as follows: S41, assuming that the puncture needle tip is located at the target point, and the puncture needle tip position is known , needle entry point , the initial posture of the end effector , , assuming the final posture of the end effector , ; S42. The equation of a two-point straight line in a known space is as follows: （15） S43, substituting the two points PQ in step S41 into the straight line equation in step S42, the result is: （16） The puncture angle is obtained from the two points PQ: （17） in, ; S44. When the puncture length L is known, the value of the front end point of the guide in the Z-axis direction is equal to the Z coordinate value of the puncture needle tip minus the projection distance of the puncture length in the Z direction: （18） in, express ; express ; （19） Substituting into equation (16) we can obtain , The specific numerical solution of ; S45. Comparison of the final posture with the initial posture yields: z Axial displacement: ; x Axial displacement: Front end point: ; Back endpoint: ; y Axial displacement: Front end point: ; Back endpoint: .

2. A puncture robot planning and control method according to claim 1, characterized in that: In step S1, the connecting rods are numbered starting from the fixed base, the fixed base is numbered as connecting rod 0, the first movable connecting rod is numbered as 1, and then the subsequent connecting rods are numbered in sequence; A coordinate system is fixed on each connecting rod to describe the positional relationship between each connecting rod and the adjacent connecting rod. The coordinate system is named according to the connecting rod name, that is, the connecting rod The upper fixed coordinate system is the coordinate system ; The link coordinate system of the puncture robot is established according to the improved DH method, which includes three moving joints and two rotating joints.

Images