Puncture robot positioning control method and system, electronic equipment and storage medium

Abstract

The invention discloses a puncture robot positioning control method and system, electronic equipment and a storage medium, and the method comprises the steps: obtaining the pose of a terminal navigator of a puncture robot under a first coordinate system which is the coordinate system of optical tracking equipment; the pose of the tail end navigator under a second coordinate system is obtained, and the second coordinate system is a robot base coordinate system; obtaining a conversion relation between the first coordinate system and the second coordinate system; acquiring the positions of the needle entering point and the target point in the first coordinate system, and converting the positions into positions in a second coordinate system; acquiring a target posture and a target position of the puncture needle; joint movement information is obtained based on the target posture and the target position; taking the joint motion information as motion control information of the puncture robot; the process is repeated in the moving process, and the pose of the puncture needle is adjusted. The technical effects that motion planning is carried out before the puncture robot moves, loop iteration is carried out on the motion planning in the motion process, and the motion precision is improved are achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of medical equipment, and in particular to a puncture robot positioning control method, system, electronic equipment and storage medium. Background Art

[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 terminal puncture needle of the miniaturized parallel puncture robot is controlled to reach the accurate position and posture. When the puncture needle punctures downward along the sleeve, it reaches the target point via the needle entry point.

[0003] In the control system, the optical tracking device is separated from the puncture robot. The optical tracking device is fixed relative to the base of the robot, and the movement of the robot has no effect on the position and posture of the optical tracking device. However, in the actual use environment, the robot or the optical tracking device will vibrate due to the environment, which will affect the movement accuracy of the robot. Summary of the invention

[0004] The main purpose of the present invention is to provide a puncture robot positioning control method to solve the problem in the related art that the optical tracking device is easily affected by the environment when guiding the robot movement, resulting in reduced robot movement accuracy.

[0005] In order to achieve the above object, the present invention provides a puncture robot positioning control method, comprising:

[0006] Step 100: acquiring the position and posture of the terminal navigator of the puncture robot in a first coordinate system, where the first coordinate system is a coordinate system of an optical tracking device, and the optical tracking device is used to track the terminal navigator;

[0007] Step 200, obtaining the position and posture of the terminal navigator in a second coordinate system, where the second coordinate system is a robot base coordinate system of the puncture robot;

[0008] Step 300: Acquire a conversion relationship between the first coordinate system and the second coordinate system, wherein the conversion relationship is obtained according to the position and posture of the terminal navigator in the first coordinate system and the position and posture of the terminal navigator in the second coordinate system;

[0009] 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 into positions in the second coordinate system based on the conversion relationship;

[0010] Step 500: acquiring a target posture and a target position of the puncture needle based on the positions of the needle insertion point and the target point in the second coordinate system;

[0011] Step 600: Calculate and obtain joint motion information of the puncture robot based on the target posture and the target position, use the joint motion information as motion control information of the puncture robot, and control the motion 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, obtaining the position and posture of the terminal navigator in the second coordinate system in step 200 includes:

[0014] According to the joint position information of the puncture robot, forward kinematics is used to obtain the position and posture of the end navigator in the second coordinate system.

[0015] Further, the step 300 of obtaining the conversion relationship between the first coordinate system and the second coordinate system includes:

[0016] Using homogeneous transformation matrix NDI T cal represents the position and posture of the terminal navigator in the first coordinate system;

[0017] Using homogeneous transformation matrix base T cal represents the position and posture of the terminal navigator in the second coordinate system;

[0018] According to the homogeneous transformation matrix NDI T cal and the homogeneous transformation matrix base T cal The relationship between the calculation of the homogeneous transformation matrix base T NDI , the conversion relationship is the homogeneous transformation matrix base T NDI .

[0019] Further, after the positions of the needle entry point and the target point in the first coordinate system are converted into positions in the second coordinate system in step 400, the method further includes:

[0020] The accessibility of the puncture robot is judged and pre-positioned so that the needle entry point and the target point are within the accessible space of the puncture robot.

[0021] Furthermore, in step 600, the joint motion information of the puncture robot is obtained by using inverse kinematics solution based on the target posture and the target position, including:

[0022] Step 6001, obtaining the homogeneous transformation matrix expression T0 of the TCP posture of the puncture needle of the puncture robot;

[0023] Step 6002, obtaining the rotation matrix R when the puncture needle is in the target posture;

[0024] Step 6003, combining the homogeneous transformation matrix expression T0 and the rotation matrix R, solving the homogeneous transformation matrix expression T0 to obtain the homogeneous transformation matrix T of the TCP posture of the puncture needle;

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

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

[0027] Get the vector from the target to the needle entry point The vector Based on the positions of the target point and the needle insertion point in the second coordinate system;

[0028] Get the value when the puncture needle is located at the vector When the straight line is located, 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, solving the homogeneous transformation matrix expression T0 to obtain the homogeneous transformation matrix T of the TCP posture of the puncture needle, includes:

[0031] Assume the position coordinates of TCP are (P x , P y , P z ), then the corresponding position matrix P = (P x ,P y ,P z ) T

[0032] Combining the rotation matrix R and the homogeneous transformation matrix expression T0, the TCP position coordinates (P x , P y , P z );

[0033] Based on the rotation matrix R and the TCP position coordinates (P x , P y , P z ) is solved to obtain the homogeneous transformation matrix T of the TCP posture of the puncture needle.

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

[0035] A puncture robot, the puncture robot comprising a robot body, an end effector, an end navigator and a puncture needle, the end navigator being fixedly mounted on the end effector, and the puncture needle being fixedly mounted on the end effector;

[0036] An optical tracking device, wherein the optical tracking device is used to track the position of the terminal navigator;

[0037] A control end, wherein the control end is used to execute the above-mentioned puncture robot positioning control method.

[0038] According to another aspect of the present application, there is provided an electronic device, including:

[0039] a memory storing execution instructions; and

[0040] A processor executes the execution instructions stored in the memory, so that the processor executes the above-mentioned puncture robot positioning control method.

[0041] According to another aspect of the present application, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a processor, the processor executes the above-mentioned puncture robot positioning control method.

[0042] In the embodiment of the present invention, the position and posture of the terminal navigator of the puncture robot in a first coordinate system is obtained, the first coordinate system is the coordinate system of the optical tracking device, and the optical tracking device is used to track the terminal navigator; the position and posture of the terminal navigator in a second coordinate system is obtained, and the second coordinate system is the robot base coordinate system of the puncture robot; the conversion relationship between the first coordinate system and the second coordinate system is obtained, and the conversion relationship is obtained according to the position and posture of the terminal navigator in the first coordinate system and the position and posture of the terminal navigator in the second coordinate system; the positions of the needle entry point and the target point in the first coordinate system are obtained, and based on the conversion relationship, the positions of the needle entry point and the target point in the first coordinate system are converted into the positions in the second coordinate system; based on the positions of the needle entry point and the target in the second coordinate system, the target posture and target position of the puncture needle are obtained; the joint motion of the puncture robot is obtained by solving the target posture and target position. information; using the joint motion information as the motion control information of the puncture robot, and controlling the movement of the puncture robot; repeating the above in the process of the puncture robot's movement to adjust the posture of the puncture needle, so as to achieve the purpose of solving the joint motion information of the puncture robot according to the target posture and target position after the puncture robot is pre-positioned, and obtaining the position of the terminal navigator in two coordinate systems in real time during the movement of the puncture robot based on the solved joint motion information, and substituting it into the solving process to adjust the posture of the puncture needle, thereby realizing the technical effect of performing motion planning before the puncture robot moves, iterating the motion planning based on the latest position during the movement, and improving the motion accuracy of the puncture robot, thereby solving the problem in the related technology that the optical tracking device is easily affected by the environment when guiding the robot movement, resulting in reduced robot motion accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The accompanying drawings constituting a part of the present invention are used to provide a further understanding of the present invention, so that other features, purposes and advantages of the present invention become more obvious. The accompanying drawings of the exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the accompanying drawings:

[0044] Figure 1 4 is a flow chart of a positioning control method for a puncture robot according to an embodiment of the present invention. DETAILED DESCRIPTION

[0045] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0046] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the numbers used in this way can be interchanged under appropriate circumstances to facilitate the embodiments of the present invention described herein.

[0047] In the present invention, the directions or positional relationships indicated by the terms "upper", "lower", "inner", etc. are based on the directions or positional relationships shown in the drawings. These terms are mainly used to better describe the present invention and its embodiments, and are not used to limit the indicated devices, elements or components to have a specific direction, or to be constructed and operated in a specific direction.

[0048] In addition, some of the above terms may be used to express other meanings in addition to indicating a direction or position relationship. For example, the term "on" may also be used to express a certain dependency or connection relationship in some cases. For those skilled in the art, the specific meanings of these terms in the present invention can be understood according to specific circumstances.

[0049] In addition, the terms "disposed", "provided with", "connected", "fixed" and the like should be understood in a broad sense. 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, elements or components. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0050] In addition, the term "plurality" shall mean two or more.

[0051] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

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

[0053] A puncture robot, the puncture robot comprising a robot body, an end effector, an end navigator and a puncture needle, the end navigator being fixedly mounted on the end effector, and the puncture needle being fixedly mounted on the end effector;

[0054] An optical tracking device is used to track the position of the end navigator.

[0055] Before positioning and controlling the puncture robot, a pre-positioning is performed, and the puncture robot and the optical tracking device are arranged at corresponding positions according to the surgical requirements. The positioning control method applied to the positioning control system includes:

[0056] Step 100: Acquire the position and posture of the terminal navigator of the puncture robot in a first coordinate system, where the first coordinate system is a coordinate system of an optical tracking device, and the optical tracking device is used to track the terminal navigator.

[0057] In a specific implementation, the end navigator includes at least three identification points fixed on the end effector, and the optical tracking device includes a visual sensor, and the spatial position of the end navigator in the first coordinate system can be acquired through the optical tracking device.

[0058] Step 200: Acquire the position and posture of the terminal navigator in a second coordinate system, where the second coordinate system is the robot base coordinate system of the puncture robot.

[0059] In a specific implementation, since the end navigator is fixed on the end effector, and the end effector is driven by the puncture robot to move, the movement amount of the driving end or the movement amount of each joint on the puncture robot will directly determine the position and posture of the end effector. Therefore, the forward kinematics method can be used to solve the position and posture of the end navigator 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 by a device capable of recording position information, such as an encoder configured on the joint.

[0060] Step 300: Acquire a conversion relationship between the first coordinate system and the second coordinate system, wherein the conversion relationship is obtained according to a posture of the terminal navigator in the first coordinate system and a posture of the terminal navigator in the second coordinate system.

[0061] In a specific implementation, since the first coordinate system and the second coordinate system belong to 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), and the needle insertion point and the target point need to be given in the first coordinate system (i.e., the optical tracking device coordinate system), and the spatial position of the end navigator obtained by the optical tracking device in the subsequent motion process is also recorded in the first coordinate system. Therefore, when the relative position of the puncture robot and the optical tracking device is fixed, it is necessary to obtain the conversion relationship between the first coordinate system and the second coordinate system. In this embodiment, the conversion relationship is obtained according to the posture of the end navigator in the first coordinate system and the posture of the end navigator in the second coordinate system.

[0062] Specifically, in this embodiment, the terminal navigator has a posture expression in the first coordinate system and also has a posture expression in the second coordinate system. The conversion relationship between the first coordinate system and the second coordinate system can be solved by calculating the two posture expressions.

[0063] Step 400: Acquire the positions of the needle entry point and the target point in the first coordinate system, and convert the positions of the needle entry point and the target point in the first coordinate system into positions in the second coordinate system based on the conversion relationship.

[0064] In a specific embodiment, the positions of the needle entry point and the target point are given in the first coordinate system, and then the spatial positions of the needle entry point and the target point in the second coordinate system are calculated based on the conversion 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 entry point and the target point in the second coordinate system, the target posture and target position of the puncture needle are obtained. In a specific embodiment, since the puncture needle needs to reach the target point via the needle entry point, the target posture and target position of the puncture needle can be determined after the positions of the needle entry 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: Calculate and obtain joint motion information of the puncture robot based on the target posture and the target position, use the joint motion information as motion control information of the puncture robot, and control the motion of the puncture robot;

[0067] In a specific embodiment, when the target posture and target position of the puncture needle are known, inverse kinematics can be used to obtain the joint motion information of the puncture robot, which at least includes the motion amount of each joint on the puncture robot, such as the elongation of the motor or the rotation amount of the motor, etc.

[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 obtaining the joint motion information of the puncture robot, the motion of each joint of the puncture robot can be controlled, thereby controlling the motion of the end effector and the puncture needle. To ensure the motion accuracy of the puncture needle, in this embodiment, when the puncture robot moves according to the calculated joint operation information, the position between the puncture robot and the needle insertion point may change slightly due to environmental changes such as vibration, thereby causing the puncture robot to be unable to accurately complete the puncture.

[0070] Therefore, it is necessary to adjust the posture of the puncture needle in real time according to the actual position, target posture and target position of the puncture needle during the movement. When the position of the puncture robot changes, the relative position between the puncture robot and the optical tracking device may also change, so the conversion relationship between the first coordinate system and the second coordinate system will change. When the position of the needle insertion point changes, the target posture and target position of the puncture needle will also change.

[0071] To this end, during the movement of the puncture needle, this embodiment needs to repeat steps 100 to 600 to adjust the position of the puncture needle. Specifically, in this embodiment, the position of the terminal navigator in the first coordinate system and the position in the second coordinate system are obtained in real time, and a new conversion relationship is obtained. Then, based on the new conversion relationship, the needle insertion point and the target point are converted to the second coordinate system to obtain a new target posture and target position of the puncture needle. Then, based on the new target posture and target position, the joint motion information is solved to control the joints of the puncture robot to move, thereby ensuring the accuracy of the puncture robot movement.

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

[0073] In summary, the present invention achieves the purpose of calculating the joint motion information of the puncture robot according to the target posture and target position after the puncture robot is pre-positioned, and obtaining the position of the terminal navigator in two coordinate systems in real time during the movement of the puncture robot based on the calculated joint motion information, and substituting it into the solution process to adjust the posture of the puncture needle, thereby realizing the technical effect of performing motion planning before the puncture robot moves, iterating the motion planning based on the latest position during the movement, and improving the motion accuracy of the puncture robot, thereby solving the problem in the related art that the visual sensor is easily affected by the environment when guiding the robot movement, resulting in reduced robot motion accuracy.

[0074] Furthermore, since the position and posture of the terminal navigator is acquired in real time during the movement, the problem of incomplete or inaccurate data acquired by the optical tracking device due to occlusion of the terminal navigator during the early path calculation can be avoided.

[0075] Furthermore, obtaining the position and posture of the terminal navigator in the second coordinate system in step 200 includes:

[0076] According to the joint position information of the puncture robot, forward kinematics is used to obtain the position and posture of the end navigator in the second coordinate system.

[0077] Specifically, it should be noted that forward kinematics can calculate the position and posture of the robot end through known joint angles and lengths. Depending on the type of puncture robot, the solution process of forward kinematics is different. Generally speaking, it is necessary to perform geometric analysis on the puncture robot or establish a DH model, or to combine geometric analysis with the DH modeling method for solution. In one embodiment, for a puncture robot combined with series and parallel connection, a combination of geometric analysis and DH modeling method is used when performing kinematic solution. Specifically, the position of the key points on the puncture robot in the second coordinate system, the relationship between the rotation angle of the rotary joint and the drive end, etc. can be obtained through geometric analysis. Combined with the results obtained by geometric analysis, a DH model can be established, and then the DH parameter table is determined based on the DH model, and the homogeneous transformation matrix is ​​used based on the DH parameter table to express the position and posture of the terminal navigator.

[0078] Further, the step 300 of obtaining the conversion relationship between the first coordinate system and the second coordinate system includes:

[0079] Using homogeneous transformation matrix NDI T cal represents the position and posture of the terminal navigator in the first coordinate system;

[0080] Using homogeneous transformation matrix base T cal represents the position and posture of the terminal navigator in the second coordinate system;

[0081] According to the homogeneous transformation matrix NDI T cal and the homogeneous transformation matrix base T cal The relationship between the calculation of the homogeneous transformation matrix base T NDI , the conversion relationship is the homogeneous transformation matrix base T NDI .

[0082] Specifically, in this embodiment, the position of the terminal navigator in the first coordinate system can be read by the optical tracking device and expressed as: NDI T cal According to the robot's forward kinematics, the position of the end navigator in the second coordinate system can be calculated from the current robot motor length or joint position, which can be expressed as 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 is the homogeneous transformation matrix of the first coordinate system in the second coordinate system.

[0085] The needle insertion point is NDI p insert =(x2,y2,z2), the target point is NDI p target =(x1,y1,z1), according to the above conversion relationship:

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

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

[0088] Further, after the positions of the needle entry point and the target point in the first coordinate system are converted into positions in the second coordinate system in step 400, the method further includes:

[0089] The accessibility of the puncture robot is judged and pre-positioned so that the needle entry point and the target point are within the accessible space of the puncture robot.

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

[0091] In one embodiment, the step 600 of obtaining the joint motion information of the puncture robot by using inverse kinematics solution based on the target posture and the target position includes:

[0092] Step 6001, obtain the homogeneous transformation matrix expression T0 of the puncture needle TCP posture of the puncture robot. In a specific embodiment, the homogeneous transformation matrix expression T0 of the puncture needle TCP posture can be obtained by solving the puncture robot in a forward kinematics manner. Depending on the structure of the puncture robot, the homogeneous transformation matrix expression of the puncture needle TCP posture has different expression forms.

[0093] In one embodiment, the puncture robot adopts the puncture robot disclosed in patent CN117338435B. Assume that the coordinates of the puncture needle TCP position in the second coordinate system are (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 posture in the second coordinate system is obtained as follows:

[0094]

[0095] in,

[0096] θ wb- is the rotation angle of the rear end of the universal joint at the lower layer, θ wf- is the rotation angle of the front end of the universal joint at the lower layer;

[0097]

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

[0099] a1=-y 2- ;

[0100] a3=l g2 ;

[0101] For the above-mentioned puncture robot, the central intersection of the rear end 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 central points of the rear end bearings of the four linear motion components, the positive direction is toward the extension direction of the end bracket, the z-axis is perpendicular to the horizontal plane upward, and the y-axis is determined by the right-hand rule.

[0102] Assume that the center points of the two bearings at the front end of the lower motion platform are O 2- The center points of the two bearings at the front end of the upper motion platform are O 2+ .O 2- The position coordinates (x 2- ,y 2- , z 2- ), O 2+ The position coordinates (x 2+ ,y2+ , z 2+ ), can be obtained through geometric relations.

[0103] l h : The distance between the center points of the rear end 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 end 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 rotation axis of the front end of the universal joint at the lower layer is also equal to O 2+ Length to the center point of the rotation axis of the front end of the universal joint on the upper layer;

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

[0106] It should be noted that the above expression of the homogeneous transformation matrix for the TCP posture of the puncture needle is not restrictive, and those skilled in the art can calculate the corresponding homogeneous transformation matrix expression according to the actual puncture robot structure.

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

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

[0109] Get the vector from the target to the needle entry point The vector Based on the positions of the target point and the needle insertion point in the second coordinate system;

[0110] Get the value when the puncture needle is located at the vector When the straight line is located, 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 to Since the end motion of the puncture robot mentioned above only rotates around the x-axis and the y-axis, let the first rotation angle around the x-axis be α, and then the rotation angle around the y-axis be β. When the straight line is located, the following relationship holds:

[0113]

[0114]

[0115] Then the rotation matrix R of the target posture is expressed as:

[0116]

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

[0118] Combining the rotation matrix R and the homogeneous transformation matrix expression T0, the TCP position coordinates (P x , P y , P z );

[0119] Based on the rotation matrix R and the TCP position coordinates (P x , P y , P z ) is solved to obtain the homogeneous transformation matrix T of the TCP posture of the puncture needle.

[0120] Specifically, after 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 using inverse kinematics solution.

[0125] On the other hand, when solving the TCP position coordinates (P x , P y , P z Specifically, for the puncture robot in the above embodiment, when the puncture needle reaches the target posture, the puncture needle punctures downward along the z-axis, passing through the needle insertion point to reach the target point, so the TCP posture of the puncture needle is consistent with the needle insertion point. NDI p insert , Target NDI p target Collinear, that is:

[0126]

[0127] Therefore, the TCP position coordinates (P x , P y , P z ) for verification.

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

[0129] A puncture robot, the puncture robot comprising a robot body, an end effector, an end navigator and a puncture needle, the end navigator being fixedly mounted on the end effector, and the puncture needle being fixedly mounted on the end effector;

[0130] An optical tracking device, wherein the optical tracking device is used to track the position of the terminal navigator;

[0131] A control end, wherein the control end is used to execute the above-mentioned puncture robot positioning control method.

[0132] According to another aspect of the present application, there is provided an electronic device, including:

[0133] a memory storing execution instructions; and

[0134] A processor executes the execution instructions stored in the memory, so that the processor executes the above-mentioned puncture robot positioning control method.

[0135] According to another aspect of the present application, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a processor, the processor executes the above-mentioned puncture robot positioning control method.

[0136] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Within the spirit and principle of the present invention, any modification, equivalent replacement, improvement, etc. made shall be included in the protection scope of the present invention.

Claims

1. A puncture robot positioning control method, characterized in that: include: Step 100: acquiring the position and posture of the terminal navigator of the puncture robot in a first coordinate system, where the first coordinate system is a coordinate system of an optical tracking device, and the optical tracking device is used to track the terminal navigator; Step 200, obtaining the position and posture of the terminal navigator in a second coordinate system, where the second coordinate system is a robot base coordinate system of the puncture robot; Step 300: Acquire a conversion relationship between the first coordinate system and the second coordinate system, wherein the conversion relationship is obtained according to the position and posture of the terminal navigator in the first coordinate system and the position and posture of the terminal navigator in 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 into positions in the second coordinate system based on the conversion relationship; Step 500: acquiring a target posture and a target position of the puncture needle based on the positions of the needle insertion point and the target point in the second coordinate system; Step 600: Calculate and obtain joint motion information of the puncture robot based on the target posture and the target position, use the joint motion information as motion control information of the puncture robot, and control the motion of the puncture robot; Step 700: Repeat steps 100-600 during the movement of the puncture robot to adjust the position of the puncture needle.

2. The puncture robot positioning control method according to claim 1, characterized in that: The step 200 of obtaining the position and posture of the end navigator in the second coordinate system includes: According to the joint position information of the puncture robot, forward kinematics is used to obtain the position and posture of the end navigator in the second coordinate system.

3. The puncture robot positioning control method according to claim 1, characterized in that: The step 300 of obtaining the conversion relationship between the first coordinate system and the second coordinate system includes: Using homogeneous transformation matrix NDI T cal represents the position and posture of the terminal navigator in the first coordinate system; Using homogeneous transformation matrix base T cal represents the position and posture of the terminal navigator in the second coordinate system; According to the homogeneous transformation matrix NDI T cal and the homogeneous transformation matrix base T cal The conversion relationship is calculated based on the relationship between base T NDI .

4. The puncture robot positioning control method according to claim 1, characterized in that: After the positions of the needle insertion point and the target point in the first coordinate system are converted into positions in the second coordinate system in step 400, the method further includes: The accessibility of the puncture robot is judged and pre-positioned so that the needle entry point and the target point are within the accessible space of the puncture robot.

5. The puncture robot positioning control method according to claim 3, characterized in that: In step 600, the joint motion information of the puncture robot is obtained by using inverse kinematics solution based on the target posture and the target position, including: Step 6001, obtaining the homogeneous transformation matrix expression T0 of the TCP posture of the puncture needle of the puncture robot; Step 6002, obtaining the rotation matrix R when the puncture needle is in the target posture; Step 6003, combining the homogeneous transformation matrix expression T0 and the rotation matrix R, solving the homogeneous transformation matrix expression T0 to obtain the homogeneous transformation matrix T of the TCP posture of the puncture needle; Step 6004: Based on the homogeneous transformation matrix T, the joint motion information is obtained by using inverse kinematics solution.

6. The puncture robot positioning control method according to claim 5, characterized in that: Step 6002, obtaining the rotation matrix R when the puncture needle is in the target posture, including: Get the vector from the target to the needle entry point The vector Based on the positions of the target point and the needle insertion point in the second coordinate system; Get the value when the puncture needle is located at the vector When the straight line is located, the rotation angle of each joint on the puncture robot around the rotation axis; The rotation matrix R is obtained based on the rotation angle.

7. The puncture robot positioning control method according to claim 6, characterized in that: Step 6003, combining the homogeneous transformation matrix expression T0 and the rotation matrix R, solving the homogeneous transformation matrix expression T0 to obtain the homogeneous transformation matrix T of the TCP posture of the puncture needle, including: Assume the position coordinates of TCP are (P x , P y , P z ), then the corresponding position matrix P = (P x ,P y ,P z ) T ; Combining the rotation matrix R and the homogeneous transformation matrix expression T0, the TCP position coordinates (P x , P y , P z ); Based on the rotation matrix R and the TCP position coordinates (P x , P y , P z ) is solved to obtain the homogeneous transformation matrix T of the TCP posture of the puncture needle.

8. A puncture robot positioning control system, characterized in that: include: A puncture robot, the puncture robot comprising a robot body, an end effector, an end navigator and a puncture needle, the end navigator being fixedly mounted on the end effector, and the puncture needle being fixedly mounted on the end effector; An optical tracking device, wherein the optical tracking device is used to track the position of the terminal navigator; A control end, wherein the control end is used to execute the puncture robot positioning control method according to any one of claims 1 to 7.

9. An electronic device, characterized in that: include: A memory storing execution instructions; as well as A processor, wherein the processor executes the execution instruction stored in the memory, so that the processor executes the puncture robot positioning control method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, wherein the computer program includes program instructions, and when the program instructions are executed by a processor, the processor executes the puncture robot positioning control method according to any one of claims 1 to 7.

Images