Puncture robot positioning and control methods, systems, electronic devices and storage media

Abstract

This invention discloses a positioning and control method, system, electronic device, and storage medium for a puncture robot, comprising: acquiring the pose of the end effector of the puncture robot in a first coordinate system, wherein the first coordinate system is the coordinate system of the optical tracking device; acquiring the pose of the end effector in a second coordinate system, wherein the second coordinate system is the robot base coordinate system; acquiring the transformation relationship between the first and second coordinate systems; acquiring the positions of the needle insertion point and the target point in the first coordinate system and converting them to positions in the second coordinate system; acquiring the target posture and target position of the puncture needle; obtaining joint motion information based on the target posture and target position; using the joint motion information as motion control information for the puncture robot; and repeating the above process during movement to adjust the pose of the puncture needle. This invention achieves the technical effect of performing motion planning before the puncture robot moves and iteratively refining the motion planning during movement, thereby improving motion accuracy.

Description

Technical Field

[0001] This invention relates to the field of medical device technology, and more specifically, to a puncture robot positioning and control method, system, electronic device, and storage medium. Background Technology

[0002] The positioning control of the puncture robot based on optical tracking is based on the known needle entry point and target point given by image registration. Under the guidance of the optical tracking device, the end puncture needle of the miniaturized parallel puncture robot is controlled to reach the accurate position and posture. When the puncture needle is puncturing downward along the sleeve, it reaches the target point through the needle entry point.

[0003] In the control system, the optical tracking device and the puncture robot are separate. The optical tracking device is fixed relative to the robot's base, and the robot's movement does not affect the position and orientation of the optical tracking device. However, in actual use environments, the robot or optical tracking device may vibrate due to environmental factors, affecting the robot's motion accuracy. Summary of the Invention

[0004] The main objective of this invention is to provide a positioning and control method for a puncture robot, so as to solve the problem that the accuracy of robot movement is easily reduced due to environmental influences when optical tracking devices guide robot movement in related technologies.

[0005] To achieve the above objectives, the present invention provides a positioning and control method for a puncture robot, comprising:

[0006] Step 100: Obtain the pose of the end effector of the puncture robot in the first coordinate system, where the first coordinate system is the coordinate system of the optical tracking device, and the optical tracking device is used to track the end effector.

[0007] Step 200: Obtain the pose of the end effector in the second coordinate system, where the second coordinate system is the robot base coordinate system of the puncture robot;

[0008] Step 300: Obtain the transformation relationship between the first coordinate system and the second coordinate system. The transformation relationship is obtained based on the pose of the end-point navigator in the first coordinate system and the pose of the end-point navigator in the second coordinate system.

[0009] Step 400: Obtain the positions of the needle insertion point and the target point in the first coordinate system, and convert the positions of the needle insertion point and the target point in the first coordinate system to the positions in the second coordinate system based on the transformation relationship;

[0010] Step 500: Based on the positions of the needle insertion point and the target point in the second coordinate system, obtain the target posture and target position of the puncture needle;

[0011] Step 600: Based on the target posture and the target position, calculate the joint motion information of the puncture robot, use the joint motion information as the motion control information of the puncture robot, and control the movement of the puncture robot.

[0012] Step 700: Repeat steps 100-600 during the movement of the puncture robot to adjust the position of the puncture needle.

[0013] Furthermore, step 200, obtaining the pose of the end effector in the second coordinate system, includes:

[0014] Based on the joint position information of the puncture robot, the pose of the end effector in the second coordinate system is obtained using forward kinematics.

[0015] Furthermore, obtaining the transformation relationship between the first coordinate system and the second coordinate system in step 300 includes:

[0016] Homogeneous transformation matrix NDI T cal This represents the pose of the end-point navigator in the first coordinate system;

[0017] Homogeneous transformation matrix base T cal This indicates the pose of the end-point navigator in the second coordinate system;

[0018] According to the homogeneous transformation matrix NDI T cal and the homogeneous transformation matrix base T cal Calculate the homogeneous transformation matrix based on the relationship between them. base T NDI The transformation relationship is the homogeneous transformation matrix. base T NDI .

[0019] Furthermore, after converting the positions of the needle insertion point and the target point in the first coordinate system to their positions in the second coordinate system in step 400, the method further includes:

[0020] The puncture robot is assessed for accessibility and pre-positioned to ensure that the needle insertion point and target point are within the reachable space of the puncture robot.

[0021] Furthermore, step 600, which involves obtaining the joint motion information of the puncture robot using inverse kinematics based on the target pose and the target position, includes:

[0022] Step 6001: Obtain the homogeneous transformation matrix expression T0 of the TCP pose of the puncture needle of the puncture robot;

[0023] Step 6002: Obtain the rotation matrix R of the puncture needle when it is in the target posture;

[0024] Step 6003: Combine the homogeneous transformation matrix expression T0 and the rotation matrix R to solve the homogeneous transformation matrix expression T0 to obtain the homogeneous transformation matrix T of the puncture needle TCP pose;

[0025] Step 6004: Based on the homogeneous transformation matrix T, the joint motion information is obtained by inverse kinematics calculation.

[0026] Further, step 6002, obtaining the rotation matrix R of the puncture needle in the target posture, includes:

[0027] Obtain the vector from the target point to the needle insertion point. The vector This is obtained based on the positions of the target point and needle insertion point in the second coordinate system;

[0028] Obtain when the puncture needle is located at the vector When the line is in the middle, the rotation angle of each joint on the puncture robot around the rotation axis;

[0029] The rotation matrix R is obtained based on the rotation angle.

[0030] Further, step 6003, combining the homogeneous transformation matrix expression T0 and the rotation matrix R, and solving the homogeneous transformation matrix expression T0 to obtain the homogeneous transformation matrix T of the puncture needle TCP pose, includes:

[0031] Let the TCP location coordinates be (P) x P y P z If ), then the corresponding position matrix P = (P x ,P y ,P z ) T

[0032] By combining the rotation matrix R and the homogeneous transformation matrix T0, the TCP position coordinates (P) can be solved. x P y P z );

[0033] Based on the rotation matrix R and the TCP position coordinates (P) x P y P z The homogeneous transformation matrix T of the TCP pose of the puncture needle is obtained by solving the problem.

[0034] According to another aspect of this application, a puncture robot positioning and control system is provided, comprising:

[0035] A puncture robot, comprising a robot body, an end effector, an end navigator, and a puncture needle, wherein the end navigator is fixed to the end effector and the puncture needle is fixed to the end effector;

[0036] An optical tracking device for tracking the position of the end-point navigator;

[0037] The control terminal is used to execute the above-described puncture robot positioning and control method.

[0038] According to another aspect of this application, an electronic device is provided, comprising:

[0039] Memory, the memory storing execution instructions; and

[0040] The processor executes the execution instructions stored in the memory, causing the processor to perform the above-described puncture robot positioning and control method.

[0041] According to another aspect of this application, a computer-readable storage medium is provided, the computer-readable storage medium storing a computer program, the computer program including program instructions, which, when executed by a processor, cause the processor to perform the above-described puncture robot positioning and control method.

[0042] In this embodiment of the invention, the pose of the end effector of the puncture robot is obtained in a first coordinate system, which is the coordinate system of the optical tracking device used to track the end effector; the pose of the end effector is obtained in a second coordinate system, which is the robot base coordinate system of the puncture robot; the transformation relationship between the first and second coordinate systems is obtained, which is derived from the pose of the end effector in the first and second coordinate systems; the positions of the needle insertion point and the target point are obtained in the first coordinate system, and the positions of the needle insertion point and the target point in the first coordinate system are converted to the positions in the second coordinate system based on the transformation relationship; based on the positions of the needle insertion point and the target point in the second coordinate system, the target pose and target position of the puncture needle are obtained; and the joint motion of the puncture robot is calculated based on the target pose and target position. Information; joint motion information is used as motion control information for the puncture robot, and the robot's movement is controlled accordingly; the above process is repeated during the robot's movement to adjust the puncture needle's pose. This achieves the goal of calculating the robot's joint motion information based on the target posture and position after the robot is pre-positioned, and acquiring the end effector's position in two coordinate systems in real time during the robot's movement based on the calculated joint motion information, and substituting this information into the calculation process to adjust the puncture needle's pose. This achieves the technical effect of performing motion planning before the robot moves, and iterating the motion planning based on the latest position during the movement, thereby improving the robot's motion accuracy. This also solves the problem in related technologies where optical tracking devices are easily affected by the environment, leading to a decrease in robot motion accuracy. Attached Figure Description

[0043] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention, making other features, objects, and advantages of the invention more apparent. The illustrative embodiments of the invention illustrated in the drawings and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings:

[0044] Figure 1 This is a flowchart illustrating the positioning and control method for a puncture robot according to an embodiment of the present invention. Detailed Implementation

[0045] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0046] It should be noted that the terms "first," "second," etc., used in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of the invention described herein.

[0047] In this invention, the terms "upper," "lower," "inner," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. These terms are primarily for the purpose of better describing the invention and its embodiments, and are not intended to limit the indicated device, element, or component to having a specific orientation, or to be constructed and operated in a specific orientation.

[0048] Furthermore, in addition to indicating direction or positional relationship, some of the aforementioned terms may also have other meanings. For example, the term "above" may also be used in certain situations to indicate a dependency or connection. Those skilled in the art can understand the specific meaning of these terms in this invention based on the specific circumstances.

[0049] Furthermore, the terms "set up," "equipped with," "connected," and "fixed" should be interpreted broadly. For example, "connected" can be a fixed connection, a detachable connection, or an integral structure; it can be a mechanical connection or an electrical connection; it can be a direct connection or an indirect connection through an intermediate medium, or it can be an internal connection between two devices, components, or parts. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0050] In addition, the term "multiple" should mean two or more.

[0051] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0052] To solve related technical problems, such as Figure 1 As shown, this embodiment of the invention provides a positioning control method for a puncture robot, which is applied in a positioning control system for a puncture robot. In this embodiment, the positioning control system includes:

[0053] A puncture robot, comprising a robot body, an end effector, an end navigator, and a puncture needle, wherein the end navigator is fixed to the end effector and the puncture needle is fixed to the end effector;

[0054] An optical tracking device for tracking the position of the end navigator.

[0055] Before positioning and controlling the puncture robot, pre-positioning is performed, placing the puncture robot and optical tracking device in the appropriate positions according to the surgical requirements. The positioning control methods applied to this positioning control system include:

[0056] Step 100: Obtain the pose of the end effector of the puncture robot in the first coordinate system, where the first coordinate system is the coordinate system of the optical tracking device, and the optical tracking device is used to track the end effector.

[0057] In a specific implementation, the end navigator includes at least three marker points fixed on the end effector, and the optical tracking device includes a vision sensor, which can acquire the spatial position of the end navigator in a first coordinate system.

[0058] Step 200: Obtain the pose of the end effector in the second coordinate system, which is the robot base coordinate system of the puncture robot.

[0059] In a specific implementation, since the end effector is fixed to the end effector, and the end effector is driven by the puncture robot, the motion of the drive end or the motion of each joint on the puncture robot directly determines the pose of the end effector. Therefore, forward kinematics can be used to calculate the pose of the end effector in the second coordinate system based on the joint position information of each joint on the puncture robot. In one implementation, the joint position information can be obtained through a device such as an encoder configured on the joint that can record position information.

[0060] Step 300: Obtain the transformation relationship between the first coordinate system and the second coordinate system. The transformation relationship is obtained based on the pose of the end navigator in the first coordinate system and the pose of the end navigator in the second coordinate system.

[0061] In a specific implementation, since the first and second coordinate systems are different coordinate systems, the motion control of the puncture robot needs to be performed in the second coordinate system (i.e., the robot base coordinate system), while the needle insertion point and target point need to be given in the first coordinate system (i.e., the optical tracking device coordinate system). Furthermore, the spatial position of the end effector acquired by the optical tracking device during subsequent movement is also recorded in the first coordinate system. Therefore, when the relative positions of the puncture robot and the optical tracking device are fixed, it is necessary to obtain the transformation relationship between the first and second coordinate systems. In this embodiment, this transformation relationship is obtained based on the pose of the end effector in the first coordinate system and the pose of the end effector in the second coordinate system.

[0062] Specifically, in this embodiment, the end-point navigator has a pose expression in the first coordinate system and also a pose expression in the second coordinate system. The transformation relationship between the first and second coordinate systems can be obtained by calculating the two pose expressions.

[0063] Step 400: Obtain the positions of the needle insertion point and the target point in the first coordinate system, and convert the positions of the needle insertion point and the target point in the first coordinate system to the positions in the second coordinate system based on the transformation relationship.

[0064] In a specific implementation, the positions of the needle insertion point and the target point are given in the first coordinate system, and then the spatial positions of the needle insertion point and the target point in the second coordinate system are calculated based on the transformation relationship between the first coordinate system and the second coordinate system obtained in step 300.

[0065] Step 500: Based on the positions of the needle insertion point and the target point in the second coordinate system, obtain the target posture and target position of the puncture needle. In a specific implementation, since the puncture needle needs to reach the target point via the needle insertion point, the target posture and target position of the puncture needle can be determined once the positions of the needle insertion point and the target point in the second coordinate system are determined. It can be understood that the target posture and target position of the puncture needle are the target posture and target position of the puncture needle in the second coordinate system.

[0066] Step 600: Based on the target posture and the target position, calculate the joint motion information of the puncture robot, use the joint motion information as the motion control information of the puncture robot, and control the movement of the puncture robot.

[0067] In a specific implementation, once the target posture and target position of the puncture needle are known, inverse kinematics calculation can be used to obtain the joint motion information of the puncture robot. This joint motion information includes at least the motion of each joint on the puncture robot, such as the extension or rotation of the motor.

[0068] Step 700: Repeat steps 100-600 during the movement of the puncture robot to adjust the position of the puncture needle.

[0069] In a specific implementation, after acquiring the joint motion information of the puncture robot, the movement of each joint of the puncture robot can be controlled, thereby controlling the movement of the end effector and the puncture needle. To ensure the accuracy of the puncture needle movement, in this embodiment, during the process of the puncture robot moving according to the calculated joint movement information, environmental changes such as vibration may cause slight changes in the position between the puncture robot and the needle insertion point, which may result in the puncture robot being unable to complete the puncture accurately.

[0070] Therefore, the puncture needle's pose needs to be adjusted in real time during the movement based on its actual position, target posture, and target position. When the position of the puncture robot changes, the relative position between the puncture robot and the optical tracking device may also change, thus altering the transformation relationship between the first and second coordinate systems. Furthermore, when the needle insertion point changes, the target posture and target position of the puncture needle will also change.

[0071] Therefore, during the movement of the puncture needle, this embodiment needs to repeat steps 100 to 600 to adjust the puncture needle's pose. Specifically, in this embodiment, the position of the end effector in the first coordinate system and the position in the second coordinate system are acquired in real time, and a new transformation relationship is obtained. Then, based on the new transformation relationship, the needle insertion point and the target point are transformed into the second coordinate system to obtain the new target pose and target position of the puncture needle. Finally, based on the new target pose and target position, the joint motion information is calculated to control the joints of the puncture robot to move, thereby ensuring the precise movement of the puncture robot.

[0072] The time interval between repeating steps 100 to 600 can be determined based on the response and execution speed of the puncture robot. When the response and execution speed are slow, the data from the optical tracking device can be read once every 2 seconds and substituted into the above steps to control the motion of the puncture robot.

[0073] In summary, this invention achieves the goal of pre-positioning the puncture robot, calculating the joint motion information of the puncture robot based on the target posture and target position, and during the puncture robot's movement based on the calculated joint motion information, acquiring the position of the end effector in two coordinate systems in real time and substituting it into the calculation process to adjust the pose of the puncture needle. This realizes the technical effect of performing motion planning before the puncture robot moves and iteratively refining the motion planning based on the latest position during the movement, thereby improving the motion accuracy of the puncture robot. It also solves the problem in related technologies where visual sensors are easily affected by the environment when guiding robot movement, leading to a decrease in robot motion accuracy.

[0074] Furthermore, since the pose of the end-point navigator is acquired in real time during the movement, the problem of incomplete or inaccurate data obtained by the optical tracking device due to occlusion of the end-point navigator during the early path calculation can be avoided.

[0075] Furthermore, step 200, obtaining the pose of the end effector in the second coordinate system, includes:

[0076] Based on the joint position information of the puncture robot, the pose of the end effector in the second coordinate system is obtained using forward kinematics.

[0077] Specifically, it should be noted that forward kinematics can calculate the position and orientation of a robot's end effector using known joint angles and lengths. The solution process for forward kinematics varies depending on the type of puncture robot. Generally, it requires geometric analysis or the establishment of a Hierarchical Harmonic Drive (DH) model, or a combination of geometric analysis and DH modeling. In one implementation, for a series-parallel combined puncture robot, a combination of geometric analysis and DH modeling is used for kinematic calculation. Specifically, geometric analysis can obtain the positions of key points on the puncture robot in the second coordinate system, as well as the relationship between the rotation angles of the rotary joints and the drive end effector. The results from the geometric analysis are then used to establish a DH model, and a DH parameter table is determined based on the DH model. Finally, a homogeneous transformation matrix is ​​used to express the pose of the end effector navigator based on the DH parameter table.

[0078] Furthermore, obtaining the transformation relationship between the first coordinate system and the second coordinate system in step 300 includes:

[0079] Homogeneous transformation matrix NDI T cal This represents the pose of the end-point navigator in the first coordinate system;

[0080] Homogeneous transformation matrix base T cal This indicates the pose of the end-point navigator in the second coordinate system;

[0081] According to the homogeneous transformation matrix NDI T cal and the homogeneous transformation matrix base T cal Calculate the homogeneous transformation matrix based on the relationship between them. base T NDI The transformation relationship is the homogeneous transformation matrix. base T NDI .

[0082] Specifically, in this embodiment, the pose of the end-effector in the first coordinate system can be read by the optical tracking device, and represented by a homogeneous transformation matrix as follows: NDI T cal Based on the robot's forward kinematics, the pose of the end effector in the second coordinate system can be calculated from the current robot motor length or joint position, and expressed by a homogeneous transformation matrix. base T cal The transformation relationship between the first coordinate system and the second coordinate system can be calculated through the relationship between the two:

[0083] base T NDI = base T cal *(NDI T cal ) -1

[0084] in, base T NDI Let be the homogeneous transformation matrix of the first coordinate system in the second coordinate system.

[0085] Let the needle insertion point be... NDI p insert = (x2, y2, z2), target point is NDI p target = (x1, y1, z1), according to the above transformation relationship, we can obtain:

[0086] base p insert = base T NDI * NDI p insert

[0087] bese p target = base T NDI * NDI p target

[0088] Furthermore, after converting the positions of the needle insertion point and the target point in the first coordinate system to their positions in the second coordinate system in step 400, the method further includes:

[0089] The puncture robot is assessed for accessibility and pre-positioned to ensure that the needle insertion point and target point are within the reachable space of the puncture robot.

[0090] Specifically, in this embodiment, after obtaining the positions of the needle insertion point and the target point in the second coordinate system, it can be determined whether the current puncture robot can reach the needle insertion point and the target point. If not, the position of the puncture robot needs to be adjusted. After adjustment, when the origin of the second coordinate system changes, the positions of the needle insertion point and the target point in the new second coordinate system need to be obtained again, and the method of obtaining them is as described in steps 100 to 300.

[0091] In one implementation, step 600, which involves obtaining the joint motion information of the puncture robot using inverse kinematics based on the target pose and the target position, includes:

[0092] Step 6001: Obtain the homogeneous transformation matrix expression T0 of the puncture needle TCP pose of the puncture robot. In a specific implementation, the homogeneous transformation matrix expression T0 of the puncture needle TCP pose can be obtained by solving the puncture robot using forward kinematics. Depending on the structure of the puncture robot, the homogeneous transformation matrix expression of the puncture needle TCP pose has different forms.

[0093] In one embodiment, the puncture robot is the puncture robot disclosed in patent CN117338435B. Let the coordinates of the puncture needle TCP position in the second coordinate system be (P...). x P y P z Based on the forward kinematics of the puncture robot, the homogeneous transformation matrix expression T0 of the puncture needle TCP pose in the second coordinate system is obtained as follows:

[0094]

[0095] in,

[0096] θ wb- θ is the rotation angle of the rear end of the lower universal joint. wf- This refers to the rotation angle of the front end of the lower universal joint;

[0097]

[0098] d2=x 2- +l g1 ;

[0099] a1=-y 2- ;

[0100] a3=l g2 ;

[0101] For the aforementioned puncture robot, the center intersection of the rear bearings of the four linear motion components (i.e., the intersection of the hinge points) is set as the origin O0 of the third coordinate system. The x-axis is perpendicular to the plane formed by the center points of the rear bearings of the four linear motion components, with its positive direction pointing towards the extension direction of the end support. The z-axis is perpendicular to the horizontal plane and upwards, and the y-axis is determined by the right-hand rule.

[0102] Let O be the center point of the two bearings located at the front end of the lower motion platform. 2- The center point of the two bearings at the front end of the upper motion platform is O. 2+ O 2- Position coordinates (x) 2- y 2- , z 2- ), O 2+ Position coordinates (x) 2+ y2+ , z 2+ ), can be obtained through geometric relationships.

[0103] l h The distance between the center points of the rear fixed bearings of the two linear motion components located in the lower motion platform is equal to the distance between the center points of the rear fixed bearings of the two linear motion components located in the upper motion platform.

[0104] l g1 :O 2- The length to the center point of the rotating shaft at the front end of the lower universal joint is also equal to O. 2+ The length to the center point of the front rotating shaft of the upper universal joint;

[0105] l g2 The length from the center point of the front rotating shaft of the lower universal joint to the TCP position of the puncture needle is also equal to the length from the center point of the front rotating shaft of the upper universal joint to the TCP position of the puncture needle.

[0106] It should be noted that the above expression for the homogeneous transformation matrix of the TCP pose of the puncture needle is not restrictive. Those skilled in the art can calculate the corresponding homogeneous transformation matrix expression based on the actual puncture robot structure.

[0107] Step 6002: Obtain the rotation matrix R of the puncture needle when it is in the target pose. In a specific implementation, it is known that the homogeneous transformation matrix T0 of the puncture needle TCP pose includes a rotation matrix and a position matrix. Therefore, the rotation matrix needs to be obtained when solving for the homogeneous transformation matrix T0. In this embodiment, it is necessary to obtain the rotation matrix R of the puncture needle when it is in the target pose.

[0108] Specifically, obtaining the rotation matrix R of the puncture needle in the target posture includes:

[0109] Obtain the vector from the target point to the needle insertion point. The vector This is obtained based on the positions of the target point and needle insertion point in the second coordinate system;

[0110] Obtain when the puncture needle is located at the vector When the line is in the middle, the rotation angle of each joint on the puncture robot around the rotation axis;

[0111] The rotation matrix R is obtained based on the rotation angle.

[0112] In this embodiment, the vector from the target point to the needle insertion point is set as Since the end effector of the aforementioned puncture robot only rotates around the x-axis and y-axis, let the first rotation around the x-axis be α, and the subsequent rotation around the y-axis be β. When the puncture needle is located in the target orientation at the vector... When the line lies within the line, the following relationship holds:

[0113]

[0114]

[0115] The rotation matrix R of the target attitude is then expressed as:

[0116]

[0117] Step 6003: Combine the homogeneous transformation matrix expression T0 and the rotation matrix R to solve the homogeneous transformation matrix expression T0 to obtain the homogeneous transformation matrix T of the puncture needle TCP pose. In a specific implementation, let the TCP position coordinates be (P... x P y P z If ), then the corresponding pose matrix P = (P x ,P y ,P z ) T ;

[0118] By combining the rotation matrix R and the homogeneous transformation matrix T0, the TCP position coordinates (P) can be solved. x P y P z );

[0119] Based on the rotation matrix R and the TCP position coordinates (P) x P y P z The homogeneous transformation matrix T of the TCP pose of the puncture needle is obtained by solving the problem.

[0120] Specifically, once the rotation matrix R is determined, it can be solved to obtain:

[0121] p x =a3R 11 +d2

[0122] p y =a3R 21 -a3R 33 -a1

[0123] p z =a3R 31 +d1

[0124] Step 6004: Based on the homogeneous transformation matrix T, the joint motion information is obtained by inverse kinematics calculation.

[0125] On the other hand, after solving for the TCP position coordinates (P) x P y P z This allows for position verification. Specifically, for the puncture robot in the above embodiments, when the puncture needle reaches the target pose, the puncture needle punctures downward along the z-axis, passing through the insertion point to reach the target point. Therefore, the TCP pose of the puncture needle and the insertion point are related. NDI p insert Target NDI p target Collinear, that is:

[0126]

[0127] Therefore, the TCP position coordinates (P) obtained from the solution can be used to determine the TCP position coordinates. x P y P z Verification will be performed.

[0128] According to another aspect of this application, a puncture robot positioning and control system is provided, comprising:

[0129] A puncture robot, comprising a robot body, an end effector, an end navigator, and a puncture needle, wherein the end navigator is fixed to the end effector and the puncture needle is fixed to the end effector;

[0130] An optical tracking device for tracking the position of the end-point navigator;

[0131] The control terminal is used to execute the above-described puncture robot positioning and control method.

[0132] According to another aspect of this application, an electronic device is provided, comprising:

[0133] Memory, the memory storing execution instructions; and

[0134] The processor executes the execution instructions stored in the memory, causing the processor to perform the above-described puncture robot positioning and control method.

[0135] According to another aspect of this application, a computer-readable storage medium is provided, the computer-readable storage medium storing a computer program, the computer program including program instructions, which, when executed by a processor, cause the processor to perform the above-described puncture robot positioning and control method.

[0136] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the invention by those skilled in the art. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the invention should be included within the scope of protection of the invention.

Claims

1. A puncture robot positioning control system characterized by, Comprising: a puncture robot, the puncture robot comprising a robot body, an end effector, an end navigator fixed on the end effector, and a puncture needle fixed on the end effector; an optical tracking device for tracking the position of the end navigator; a control end for performing the following operations: Step 100, obtaining the pose of the end navigator of the puncture robot in a first coordinate system, the first coordinate system being the coordinate system of the optical tracking device for tracking the end navigator; Step 200, obtaining the pose of the end navigator in a second coordinate system, the second coordinate system being the robot base coordinate system of the puncture robot; Step 300, obtaining the conversion relationship of the first coordinate system and the second coordinate system, the conversion relationship being obtained according to the pose of the end navigator in the first coordinate system and the pose of the end navigator in the second coordinate system, comprising: Using a homogeneous transformation matrix denotes the pose of the end navigator in the first coordinate system; Using a homogeneous transformation matrix represents a pose of the end navigator in the second coordinate system; According to a relationship between the first coordinate system and the second coordinate system ; Step 400, obtaining the positions of the needle entry point and the target point in the first coordinate system, and converting the positions of the needle entry point and the target point in the first coordinate system to the positions in the second coordinate system based on the conversion relationship; Step 500, based on the positions of the needle entry point and the target point in the second coordinate system, obtaining the target attitude and the target position of the puncture needle; Step 600, based on the target attitude and the target position, calculating the joint motion information of the puncture robot, taking the joint motion information as the motion control information of the puncture robot, and controlling the motion of the puncture robot; Step 700, repeating steps 100-600 during the motion of the puncture robot to adjust the pose of the puncture needle.

2. The puncture robotic positioning control system of claim 1, wherein, The obtaining of the pose of the end navigator in the second coordinate system in step 200 comprises: According to the joint position information of the puncture robot, the pose of the end navigator in the second coordinate system is obtained by forward kinematics.

3. The puncture robotic positioning control system of claim 1, wherein, After converting the positions of the needle entry point and the target point in the first coordinate system to the positions in the second coordinate system in step 400, it further comprises: Performing reachability judgment and pre-positioning on the puncture robot to make the needle entry point and the target point located in the reachable space of the puncture robot.

4. The puncture robotic positioning control system of claim 3, wherein, The obtaining of the joint motion information of the puncture robot based on the target attitude and the target position in step 600 comprises: Step 6001, obtaining a homogeneous transformation matrix expression of a puncture needle TCP pose of a puncture robot ; Step 6002, obtaining a rotation matrix when the puncture needle is in the target pose ; Step 6003, simultaneously expressing the homogeneous transformation matrix and the rotation matrix , the homogeneous transformation matrix solving the homogeneous transformation matrix of the puncture needle TCP pose ; Step 6004, obtaining the joint motion information based on the homogeneous transformation matrix , and obtaining the joint motion information by inverse kinematics solution.

5. The puncture robotic positioning control system of claim 4, wherein, Step 6002, obtaining a rotation matrix when the puncture needle is in the target pose comprising: obtaining a vector presented by the target point to the needle entry point , the vector is obtained based on positions of the target point and the needle entry point in the second coordinate system obtaining the rotation angle of each joint of the puncture robot around the rotation axis when the puncture needle is located on the straight line of the vector ​ obtaining the rotation matrix based on the rotation angle .

6. The puncture robotic positioning control system of claim 5, wherein, Step 6003, obtaining the homogeneous transformation matrix expression and the rotation matrix , the homogeneous transformation matrix expression solving the homogeneous transformation matrix of the puncture needle TCP pose , comprising: Let TCP position coordinates be Then the corresponding position matrix is ; simultaneously with the rotation matrix and the homogeneous transformation matrix expression solving TCP position coordinates ; based on the rotation matrix and the TCP position coordinates solving the homogeneous transformation matrix of the puncture needle TCP pose .

7. An electronic device, comprising: Comprising: a memory storing execution instructions; and a processor executing the execution instructions stored in the memory, so that the processor performs the operations performed by the control end as claimed in claim 1.

8. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, the computer program comprising program instructions, the program instructions being executed by the processor to perform the operations performed by the control end as claimed in claim 1.

Images