A method for adjusting the posture of the end of a robotic arm based on TCP coordinate system projection

Abstract

The present invention proposes a method for adjusting the posture of a manipulator end based on TCP coordinate system projection. The method comprises: in the optical navigator coordinate system, i.e., the first spatial coordinate system, determining the puncture path according to the coordinates of the first and second needle insertion points, and translating the path to obtain the z-axis vector of the first spatial coordinate system; determining the manipulator tool end coordinate system as the second spatial coordinate system; translating and rotating the first spatial coordinate system to make the z-axis vector of the first spatial coordinate system equal to the z-axis vector of the target second spatial coordinate system; based on the z-axis vector of the target second spatial coordinate system, determining the y and z-axis vectors of the z-axis of the target second spatial coordinate system in the third spatial coordinate system; based on the y and z-axis vectors of the target second spatial coordinate system, determining the x-axis vector of the x-axis of the target second spatial coordinate system in the third spatial coordinate system; based on the x, y, and z-axis vectors of the target second spatial coordinate system in the third spatial coordinate system, determining the rotation matrix of the target second spatial coordinate system.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and in particular to a method for adjusting the posture of a robot end based on TCP coordinate system projection. Background Art

[0002] In medical surgery, precise control of the robotic arm is crucial to achieving safe and effective puncture operations, especially when dealing with complex anatomical structures. The posture adjustment of the end of the robotic arm becomes a key link to ensure its precise movement along the predetermined path. With the continuous development of minimally invasive surgical technology, the requirements for the accuracy and efficiency of surgical robots are increasing. However, traditional posture adjustment methods usually rely on complex mathematical models and lengthy calculation processes, which not only leads to inefficient adjustments, but also increases the risk of introducing errors in high-precision surgical environments. Specifically, traditional methods often require multiple iterative calculations, resulting in delays in surgery, affecting the doctor's operating experience and the safety of patients. Therefore, there is an urgent need for a more efficient and accurate posture adjustment method to improve the response speed and stability of the robotic arm during surgery and ensure the smoothness and safety of the surgical process. Summary of the invention

[0003] In response to the challenges mentioned in the background technology section, the present invention provides an innovative method, a method for adjusting the posture of the end of the robotic arm tool based on TCP coordinate system projection, to adjust the posture of the end of the robotic arm tool so that the posture of the puncture needle during puncture completely coincides with the planned puncture path, thereby achieving precise puncture.

[0004] The embodiment of the present invention provides a method for adjusting the posture of a robot end based on TCP coordinate system projection, which adopts the following technical solutions:

[0005] 1) Determine the coordinate system of the optical navigator as the first spatial coordinate system, determine the puncture path by the three-dimensional spatial coordinates of the first needle insertion point and the second needle insertion point in the first spatial coordinate system, translate the puncture path until the second needle insertion point coincides with the origin of the first spatial coordinate system, and record the translated puncture path as the z-axis vector of the first spatial coordinate system;

[0006] 2) Determine the robot tool end coordinate system as the second space coordinate system;

[0007] 3) Determine that the robot base spatial coordinate system is the third spatial coordinate system, translate and rotate the first spatial coordinate system until the x-axis, y-axis and origin of the first spatial coordinate system and the third spatial coordinate system completely coincide, and the z-axis vector of the first spatial coordinate system at this time is the projection of the z-axis vector of the original first spatial coordinate system in the third spatial coordinate system, and use this projection as the z-axis vector of the target second spatial coordinate system, that is, the final target posture that the z-axis of the end of the robot tool needs to adjust;

[0008] 4) determining a z-axis vector of the z-axis of the target second space coordinate system in the third space coordinate system based on the z-axis of the first space coordinate system, the z-axis of the target second space coordinate system, and the third space coordinate system;

[0009] 5) determining the y-axis vector of the y-axis of the second spatial coordinate system of the target in the third spatial coordinate system based on the z-axis vector of the second spatial coordinate system of the target and the third spatial coordinate system;

[0010] 6) determining the x-axis vector of the x-axis of the second spatial coordinate system of the target in the third spatial coordinate system based on the y-axis and z-axis vectors of the second spatial coordinate system of the target and the third spatial coordinate system;

[0011] 7) Based on the x, y, and z axis vectors of the target second space coordinate system in the third space coordinate system, determine a rotation matrix of the target second space coordinate system with respect to the third space coordinate system.

[0012] Preferably, the z-axis vector of the first spatial coordinate system in step 1) is a vector from the first needle insertion point to the second needle insertion point translated onto the first spatial coordinate system.

[0013] Preferably, in step 2), the x-axis, y-axis and z-axis of the second spatial coordinate system are respectively parallel to the x-axis, y-axis and z-axis of the spatial coordinate system when no tool is installed at the end of the robot arm.

[0014] Preferably, the projection of the z-axis vector of the first spatial coordinate system and the target second spatial coordinate z-axis vector in the third spatial coordinate system in step 3) is to translate and rotate the z-axis vector of the first spatial coordinate system to the third spatial coordinate system and make the second needle insertion point coincide with the origin of the third spatial coordinate system, and then use the projection of the original z-axis vector of the first spatial coordinate system in the third spatial coordinate system as the z-axis vector of the second spatial coordinate system, that is, the final target posture that needs to be adjusted for the z-axis of the robot arm tool end coordinate system.

[0015] Preferably, in step 4), the z-axis vector of the target second space coordinate system z-axis in the third space coordinate system is calculated based on the angular relationship between the target second space coordinate system z-axis vector and the x-, y-, z-axis vectors of the third space coordinate system.

[0016] Preferably, in step 5), the y-axis vector of the y-axis of the target second space coordinate system in the third space coordinate system is determined according to two conditions: one is that the y-axis vector of the target second space coordinate system is perpendicular to the z-axis vector of the target second space coordinate system; the other is that the y-axis vector of the target second space coordinate system is parallel to the xoz plane in the third space coordinate system.

[0017] Preferably, in step 6), the x-axis vector of the x-axis of the target second space coordinate system in the third space coordinate system is determined according to three conditions: the x-axis vector of the x-axis of the target second space coordinate system in the third space coordinate system is determined according to three conditions: first, the y-axis vector of the target second space coordinate system is perpendicular to the x-axis vector of the target second space coordinate system; second, the z-axis vector of the target second space coordinate system is perpendicular to the x-axis vector of the target second space coordinate system; third, the unit vector coordinate system has the following mathematical equation: .

[0018] Preferably, in step 7), the rotation matrix of the target second space coordinate system with respect to the third space coordinate system can be obtained by transforming the projection of the x, y, z axis unit vectors in the target second space coordinate system in the third space coordinate system.

[0019] The method for adjusting the posture of the end of the robotic arm based on TCP coordinate system projection according to an embodiment of the present invention has at least the following beneficial effects: by projecting the posture of the end of the robotic arm with the predetermined puncture path, the required posture adjustment is quickly calculated, the real-time performance is improved, the calculation complexity is reduced, and the error accumulation is reduced, thereby improving the puncture accuracy and the success rate of the ablation surgery, and providing reliable support for minimally invasive medical applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0021] Figure 1 It is a flow chart of a method for adjusting the posture of a robot end based on TCP coordinate system projection provided according to an embodiment of the present invention.

[0022] Figure 2 It is a posture relationship diagram of the three-dimensional space coordinate system of the end of the puncture tool and the coordinate system of the robot arm base provided according to an example of the present invention.

[0023] Figure 3 It is a schematic diagram of the three-dimensional coordinate system of the robot arm tool end and the robot arm end TCP space provided according to an example of the present invention. DETAILED DESCRIPTION

[0024] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions 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 are within the scope of protection of the present invention.

[0025] The terms "first", "second" and the like in the specification and claims of the present invention and the above drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence.

[0026] Figure 1 This is a flowchart of a method for adjusting the posture of a robot end based on TCP coordinate system projection according to an embodiment of the present invention. This embodiment can be applied to the tool end of a medical surgical puncture robot to adjust the posture according to the planned puncture path. The method can be implemented by hardware and software of a space conversion relationship. The hardware of the space conversion relationship can use a six-axis robot and a puncture needle, and the software can use a ROS system.

[0027] like Figure 1 As shown, the method of this embodiment may specifically include: S110 determining the optical navigator coordinate system as a first spatial coordinate system, in the first spatial coordinate system, determining the puncture path by the three-dimensional spatial coordinates of the first needle entry point and the second needle entry point, and translating the puncture path until the second needle entry point coincides with the origin of the first spatial coordinate system, and recording the translated puncture path as the z-axis vector of the first spatial coordinate system.

[0028] In a specific implementation, a 3D reconstruction of the human abdominal CT image is performed according to the supporting 3D reconstruction software and two points are planned in the 3D human organ, wherein the first needle insertion point is the puncture needle insertion point on the skin surface, and the second needle insertion point is the human organ target point (such as a tumor). The first and second needle insertion points are recorded as points and Point , point A and point B form the z-axis vector of the first spatial coordinate system, denoted as vector , so the vector The mathematical expression is:

[0029] .

[0030] S120 determines that the robot arm tool end coordinate system is the second space coordinate system.

[0031] In specific implementation, Figure 3 As shown, the three-dimensional coordinate system of the robot end TCP It is the TCP three-dimensional coordinate system of the robot end preset when the robot leaves the factory. Figure 3 Medium Coordinate System The three-dimensional coordinate system of the end tool of the robot arm after the end tool is installed on the robot arm, where the coordinate system The direction and position can be set by software. Here, you need to The coordinate system is set as Figure 3 As shown, The x, y, and z axes of the coordinate system must be aligned with the of , , The purpose of the axis parallelism is to reduce the amount of calculation of the algorithm.

[0032] S130 determines that the spatial coordinate system of the robot arm base is the third spatial coordinate system, translates and rotates the first spatial coordinate system until the x-axis, y-axis and origin of the first spatial coordinate system and the third spatial coordinate system completely coincide, and the z-axis vector of the first spatial coordinate system at this time is the projection of the z-axis vector of the original first spatial coordinate system in the third spatial coordinate system, and this projection is used as the z-axis vector of the target second spatial coordinate system, that is, the final target posture that needs to be adjusted for the z-axis of the end of the robot arm tool.

[0033] In a specific implementation, the z-axis vector of the first spatial coordinate system (i.e., the vector obtained in step S110) ) is mapped to the third space coordinate system to obtain the vector .vector The angles with the x, y, and z axes in the third spatial coordinate system are respectively denoted as , , , since the vector That is, the target z-axis vector of the space coordinate system of the end of the robot tool, that is, the posture of the end of the robot tool needs to be rotated and the z-axis posture finally coincides with the vector Overlap, at this time, the x, y, and z axes of the second spatial coordinate system are parallel to the x, y, and z axes of the target second spatial coordinate system respectively. The angles with the x, y, and z axes in the third spatial coordinate system are respectively denoted as , , According to the above , , and , , By the following relationship:

[0034] , , ,

[0035] That is, the angles between the z-axis vector of the first spatial coordinate system and the z-axis vector of the target second spatial coordinate system and the x, y, and z axes in the third spatial coordinate system are equal, that is, vector AB is equal to the z-axis vector of the target second spatial coordinate system.

[0036] S140: Determine a z-axis vector of the z-axis of the target second space coordinate system in the third space coordinate system based on the z-axis of the first space coordinate system, the z-axis of the target second space coordinate system, and the third space coordinate system.

[0037] In specific implementation, Figure 2 As shown, the blue coordinate system is the third space coordinate system, denoted as , which can be expressed as: , where the vector ,vector ,vector are the x, y, z axis unit vectors in the third space coordinate system: , , .

[0038] Figure 2 The orange coordinate system is the second space coordinate system of the target, denoted as , which can be expressed as:

[0039] ,

[0040] where the vector is the z-axis unit vector in the second spatial coordinate system:

[0041] ,

[0042] Where α, β, and γ are vectors With vector ,vector ,vector The angle of Figure 2 The angles between z' and the x, y, and z axes are α, β, and γ, respectively. According to the dot product formula, α, β, and γ are:

[0043] (1) Calculate the dot product: , , ;

[0044] (2) Calculate the modulus length: , , , ;

[0045] (3) Calculate the angle: , , ;

[0046] According to the obtained angles α, β, and γ, we can know that: ;

[0047] According to the above equation, the z-axis unit vector in the second spatial coordinate system can be obtained The value of , that is, the z-axis vector under the first space coordinate.

[0048] S150: Determine the y-axis vector of the y-axis of the target second space coordinate system in the third space coordinate system based on the z-axis vector of the target second space coordinate system and the third space coordinate system.

[0049] In specific implementation, Figure 2 As shown, the orange three-dimensional coordinate system rotates around the z΄ axis to make the y axis parallel to the xoz plane. At this time, the unit vector in the second spatial coordinate system is It can be expressed as:

[0050] , and and vertical, so the second spatial coordinate system The following mathematical relationship is satisfied: , ;

[0051] From the above formula (1), we can know that:

[0052] ,

[0053] so for: ,Will Substituting into (2) we can get:

[0054] ,

[0055] Finally, it is concluded The value here , All take positive values:

[0056] .

[0057] S160: Determine the x-axis vector of the x-axis of the target second space coordinate system in the third space coordinate system based on the y-axis and z-axis vectors of the target second space coordinate system and the third space coordinate system.

[0058] In a specific implementation, the x-axis unit vector of the second spatial coordinate system is :

[0059] ;

[0060] Since the x, y, and z axes in the second spatial coordinate system are perpendicular to each other, the following mathematical equation holds true:

[0061] , , ;

[0062] According to the above obtained and Combining the above formulas (1), (2) and (3), we can get:

[0063] .

[0064] S170 determines a rotation matrix of the target second space coordinate system with respect to the third space coordinate system based on the x, y, and z axis vectors of the target second space coordinate system in the third space coordinate system.

[0065] In the specific implementation, since the rotation matrix is ​​an orthogonal matrix, its column vector is the unit vector on the coordinate axis after rotation. According to the vector obtained above , , Construct the rotation matrix R:

[0066] .

[0067] Substitute the above rotation matrix into the rotation matrix part of the robot arm control quaternion and then run the robot arm. Then the final posture of the robot arm tool end will coincide with the planned path posture.

[0068] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope defined in the claims.

Claims

1. A method for adjusting the posture of a robot end based on TCP coordinate system projection, characterized in that: The following steps are involved: 1) Determine the coordinate system of the optical navigator as the first spatial coordinate system, determine the puncture path by the three-dimensional spatial coordinates of the first needle insertion point and the second needle insertion point in the first spatial coordinate system, translate the puncture path until the second needle insertion point coincides with the origin of the first spatial coordinate system, and record the translated puncture path as the z-axis vector of the first spatial coordinate system; 2) Determine the coordinates of the end of the robot tool as the second space coordinate system; 3) Determine that the robot base spatial coordinate system is the third spatial coordinate system, translate and rotate the first spatial coordinate system until the x-axis, y-axis and origin of the first spatial coordinate system and the third spatial coordinate system completely coincide, and the z-axis vector of the first spatial coordinate system at this time is the projection of the z-axis vector of the original first spatial coordinate system in the third spatial coordinate system, and use this projection as the z-axis vector of the target second spatial coordinate system, that is, the final target posture that the z-axis of the end of the robot tool needs to adjust; 4) determining a z-axis vector of the z-axis of the target second space coordinate system in the third space coordinate system based on the z-axis of the first space coordinate system, the z-axis of the target second space coordinate system, and the third space coordinate system; 5) determining the y-axis vector of the y-axis of the second spatial coordinate system of the target in the third spatial coordinate system based on the z-axis vector of the second spatial coordinate system of the target and the third spatial coordinate system; 6) determining the x-axis vector of the x-axis of the second spatial coordinate system of the target in the third spatial coordinate system based on the y-axis and z-axis vectors of the second spatial coordinate system of the target and the third spatial coordinate system; 7) Based on the x, y, and z axis vectors of the target second space coordinate system in the third space coordinate system, determine a rotation matrix of the target second space coordinate system with respect to the third space coordinate system.

2. The method for adjusting the posture of a robot end based on TCP coordinate system projection according to claim 1, characterized in that: The z-axis vector of the first spatial coordinate system mentioned in step 1) is a vector from the first needle insertion point to the second needle insertion point.

3. The method for adjusting the posture of a robot end based on TCP coordinate system projection according to claim 1, characterized in that: In step 2), the second spatial coordinate system is the result of translation of the robot end coordinate system, that is, the second spatial coordinate system is parallel to the x, y, and z axes of the robot end coordinate system.

4. The method for adjusting the posture of a robot end based on TCP coordinate system projection according to claim 1, characterized in that: The projection of the z-axis vector of the first spatial coordinate system described in step 3) to the third spatial coordinate system is to translate and rotate the z-axis vector of the first spatial coordinate system to the third spatial coordinate system and make the second needle insertion point coincide with the origin of the third spatial coordinate system, and then use the projection of the original z-axis vector of the first spatial coordinate system to the above-mentioned third spatial coordinate system as the z-axis vector of the target second spatial coordinate system, that is, the final target posture that needs to be adjusted for the z-axis of the robot arm tool end coordinate system.

5. The method for adjusting the posture of a robot end based on TCP coordinate system projection according to claim 1, characterized in that: In step 4), the z-axis vector of the target second spatial coordinate system z-axis in the third spatial coordinate system is obtained based on the angle relationship between the target second spatial coordinate system z-axis vector and the x, y, z-axis vectors of the third spatial coordinate system.

6. The method for adjusting the posture of a robot end based on TCP coordinate system projection according to claim 1, characterized in that: In step 5), the y-axis vector of the y-axis of the target second space coordinate system in the third space coordinate system is determined according to two conditions: one is that the y-axis vector of the target second space coordinate system is perpendicular to the z-axis vector of the target second space coordinate system; the other is that the y-axis vector of the target second space coordinate system is parallel to the xoz plane in the third space coordinate system.

7. The method for adjusting the posture of a robot end based on TCP coordinate system projection according to claim 1, characterized in that: In step 6), the x-axis vector of the x-axis of the target second space coordinate system in the third space coordinate system is determined according to three conditions: first, the y-axis vector of the target second space coordinate system is perpendicular to the x-axis vector of the target second space coordinate system; second, the z-axis vector of the target second space coordinate system is perpendicular to the x-axis vector of the target second space coordinate system; third, the unit vector coordinate system has the following mathematical equation: .

8. The method for adjusting the posture of a robot end based on TCP coordinate system projection according to claim 1, characterized in that: In step 7), the rotation matrix of the target second space coordinate system with respect to the third space coordinate system can be obtained by transforming the projection of the x, y, z axis unit vectors in the target second space coordinate system in the third space coordinate system.

Images