Method and device for determining position of parallel robot based on forward kinematics

By splitting the closed chain structure of the parallel robot into an open chain structure, and calculating the flange end position based on the position coordinates of the fourth joint and the standard DH parameters, the problem of complexity in the positive kinematics solution of the parallel robot is solved, and position calculation is improved speed and accuracy.

CN115488860BActive Publication Date: 2024-11-12BEIJING C H L ROBOTICS CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202211214966.6
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-09-30
Publication Date
2024-11-12
Estimated Expiration
2042-09-30

AI Technical Summary

Technical Problem

The positive kinematic solution of parallel robots is complicated and cannot accurately determine joint values, resulting in low position calculation speed and accuracy.

Method used

The closed chain structure formed by the parallel robot is divided into two open chain structures, and based on the position coordinates of the fourth joint and the standard DH parameters, the transformation matrix from the end of the flange to the world coordinate system is calculated to extract the flange The position coordinates of the end.

Benefits of technology

The speed and accuracy of the position calculation of parallel robots are improved. By splitting the closed chain structure into an open chain structure, the joint angle of the driven shaft is clarified, and the coupling phenomenon is reduced.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115488860B_ABST
    Figure CN115488860B_ABST
Patent Text Reader

Abstract

The embodiment of the present invention discloses a method and device for determining the position of a parallel robot based on forward kinematics. The method first divides the closed chain structure of the parallel robot into a first open chain structure and a second open chain structure; the first open chain structure and the second open chain structure are both formed by connecting various joints; secondly, based on the position and self-axis length of the third joint angle, and the position and self-axis length of the seventh joint angle, the position coordinates of the fourth joint angle in the basic coordinate system are calculated; finally, based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle, the joint angles of the first open chain structure, and the standard DH parameters of the parallel robot, the transformation matrix #imgabs0# is determined and the position coordinates corresponding to the flange end are extracted from the transformation matrix #imgabs1#. Thus, by splitting the closed chain structure into two open chain structures and determining the joint angle of the driven axis based on the position coordinates of the fourth joint in the basic coordinate system, the speed and accuracy of the parallel robot position calculation are improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of parallel robots, and in particular relates to a method and a device for determining the position of a parallel robot based on forward kinematics. Background Art

[0002] The kinematic analysis of serial robots or parallel robots includes forward kinematics and inverse kinematics. The definition of inverse kinematics is to solve several independent input motions when the position and posture information of the moving platform is known. For serial robots or parallel robots, the expressions of multiple input motions are independent and can be calculated in parallel and solved quickly. The forward kinematics problem is to solve the position and posture of the moving platform when multiple input motions are known.

[0003] For parallel robots, the inverse kinematics solution is often relatively clear. For example, the robot's joint values ​​can be effectively determined based on the shape of the platform. However, the forward kinematics solution may be quite complex and cannot accurately determine the robot's joint values ​​based on the shape of the platform. For example, regardless of whether it is a multi-degree-of-freedom robot or a low-degree-of-freedom robot, there is a certain degree of coupling in the mechanism, which means that the forward kinematics solution of the parallel robot may correspond to multiple platform configurations. Summary of the invention

[0004] The present invention provides a method and device for determining the position of a parallel robot based on forward kinematics. The method can improve the speed and accuracy of the position calculation of the parallel robot.

[0005] To achieve the above-mentioned purpose, according to the first aspect of an embodiment of the present application, a method for determining the position of a parallel robot based on forward kinematics is provided, the method comprising dividing a closed-chain structure formed by connecting a first joint, a second joint, a third joint, a fourth joint, a fifth joint, a sixth joint and a seventh joint of the parallel robot into a first open-chain structure and a second open-chain structure; the first open-chain structure and the second open-chain structure are both formed by connecting each joint; a first circle formed by taking the third joint angle position as the center and the third joint's own axial length as the radius is intersected with a second circle formed by taking the seventh joint angle position as the center and the seventh joint's own axial length as the radius, and the position coordinates of the fourth joint angle in the basic coordinate system are calculated; based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open-chain structure, and the standard DH parameters of the parallel robot, a transformation matrix from the flange end to the world coordinate system is determined. From the transformation matrix Extract the position coordinates corresponding to the flange end.

[0006] Optionally, when the first open-chain structure is formed by connecting the first joint, the second joint, the third joint, the fourth joint, and the fifth joint; the transformation matrix from the flange end to the world coordinate system is determined based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open-chain structure, and the standard DH parameters of the parallel robot. The transformation matrix includes: taking the position coordinates of the fourth joint angle in the basic coordinate system and the third joint angle as variables Establish the element correspondence and calculate the third joint angle θ3; obtain the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the fourth joint angle θ4, and the fifth joint angle θ5; based on the basic coordinate system, the world coordinate system, the first standard DH parameter of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the third joint angle θ3, the fourth joint angle θ4, and the fifth joint angle θ5, determine the transformation matrix from the flange end to the world coordinate system

[0007] Optionally, the transformation matrix from the flange end to the world coordinate system is determined based on the basic coordinate system, the world coordinate system, the first standard DH parameter of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the third joint angle θ3, the fourth joint angle θ4, and the fifth joint angle θ5. The method comprises: determining a transformation matrix from the first joint to the basic coordinate system based on the movement distance d1 of the first joint along the Z axis of the basic coordinate system Based on the second joint angle θ2 and the first standard DH parameter corresponding to the second joint, determine the transformation matrix from the second joint to the first joint Based on the third joint angle θ3 and the first standard DH parameter corresponding to the third joint, determine the transformation matrix from the third joint to the second joint Based on the fourth joint angle θ4 and the first standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the third joint: Based on the fifth joint angle θ5 and the first standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the first standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint Based on the transformation matrix from the base coordinate system to the world coordinate system The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system

[0008] Optionally, when the first open-chain structure is formed by connecting the first joint, the sixth joint, the seventh joint, the fourth joint, and the fifth joint; the transformation matrix from the flange end to the world coordinate system is determined based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open-chain structure, and the second standard DH parameters of the parallel robot. The transformation matrix includes: taking the position coordinates and the seventh joint angle as variables Establish an element correspondence relationship to obtain the seventh joint angle θ7; obtain the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the sixth joint angle θ6, the fourth joint angle θ4, and the fifth joint angle θ5; based on the basic coordinate system, the world coordinate system, the second standard DH parameters of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the sixth joint angle θ6, the seventh joint angle θ7, the fourth joint angle θ4, and the fifth joint angle θ5, determine the transformation matrix from the flange end to the world coordinate system

[0009] Optionally, based on the basic coordinate system, the world coordinate system, the second standard DH parameters of the parallel robot, the movement distance d1 of the first joint along the Z axis of the basic coordinate system, the sixth joint angle θ6, the seventh joint angle θ7, the fourth joint angle θ4, and the fifth joint angle θ5, the transformation matrix from the flange end to the world coordinate system is determined. The method comprises: determining a transformation matrix from the first joint to the basic coordinate system based on the movement distance d1 of the first joint along the Z axis of the basic coordinate system Based on the sixth joint angle θ6 and the second standard DH parameter corresponding to the sixth joint, determine the transformation matrix from the sixth joint to the first joint: Based on the seventh joint angle θ7 and the second standard DH parameter corresponding to the seventh joint, determine the transformation matrix from the seventh joint to the sixth joint: Based on the fourth joint angle θ4 and the second standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the seventh joint: Based on the fifth joint angle θ5 and the second standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the second standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint Based on the transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system

[0010] Optionally, the transformation matrix from the flange end to the world coordinate system Extract the position coordinates corresponding to the flange end, including: obtaining the transformation matrix from the flange end to the world coordinate system The transformation matrix The first three rows of elements are divided into a first column, a second column, a third column and a fourth column; the elements of the fourth column are determined as the position coordinates corresponding to the flange end.

[0011] To achieve the above-mentioned purpose, the second aspect of this embodiment also provides a device for determining the position of a parallel robot based on forward kinematics, and the device includes: a division module, which is used to divide the closed chain structure formed by the parallel robot connected by the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint and the seventh joint into a first open chain structure and a second open chain structure; the first open chain structure and the second open chain structure are both formed by connecting each joint; a calculation module, which is used to intersect a first circle formed by the third joint angle position as the center and the third joint's own axis length as the radius with a second circle formed by the seventh joint angle position as the center and the seventh joint's own axis length as the radius, and calculate the position coordinates of the fourth joint angle in the basic coordinate system; a determination module, which is used to determine the transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open chain structure, and the standard DH parameters of the parallel robot. Extraction module for extracting from the transformation matrix Extract the position coordinates corresponding to the flange end.

[0012] Optionally, when the first open-chain structure is formed by connecting the first joint, the second joint, the third joint, the fourth joint, and the fifth joint; the determination module includes: a calculation unit for transforming the position coordinates of the fourth joint angle in the basic coordinate system with the third joint angle as variables Establish an element correspondence relationship and calculate the third joint angle θ3; an acquisition unit is used to obtain the movement distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the fourth joint angle θ4, and the fifth joint angle θ5; a determination unit is used to determine the transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the first standard DH parameter of the parallel robot, the movement distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the third joint angle θ3, the fourth joint angle θ4, and the fifth joint angle θ5

[0013] Optionally, the determining unit includes: a first determining subunit, for determining a transformation matrix from the first joint to the basic coordinate system based on a moving distance d1 of the first joint along the Z axis of the basic coordinate system Based on the second joint angle θ2 and the first standard DH parameter corresponding to the second joint, determine the transformation matrix from the second joint to the first joint Based on the third joint angle θ3 and the first standard DH parameter corresponding to the third joint, determine the transformation matrix from the third joint to the second joint Based on the fourth joint angle θ4 and the first standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the third joint: Based on the fifth joint angle θ5 and the first standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the first standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint A second determining subunit is used to determine a transformation matrix from the base coordinate system to the world coordinate system. The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system

[0014] To achieve the above-mentioned purpose, according to a third aspect of an embodiment of the present application, a computer-readable medium is provided, on which a computer program is stored, and when the program is executed by a processor, the method described in the first aspect is implemented.

[0015] Compared with the prior art, an embodiment of the present invention provides a method and device for determining the posture of a parallel robot based on inverse kinematics, the method comprising: first, dividing a closed chain structure of the parallel robot formed by connecting a first joint, a second joint, a third joint, a fourth joint, a fifth joint, a sixth joint and a seventh joint into a first open chain structure and a second open chain structure; the first open chain structure and the second open chain structure are both formed by connecting each joint; secondly, intersecting a first circle formed by taking the third joint angle position as the center and the third joint's own axial length as the radius with a second circle formed by taking the seventh joint angle position as the center and the seventh joint's own axial length as the radius, and calculating the position coordinates of the fourth joint angle in the basic coordinate system; thereafter, determining the transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open chain structure, and the standard DH parameters of the parallel robot. Finally, from the transformation matrix The position coordinates corresponding to the flange end are extracted from the . Thus, by splitting the closed chain structure into two open chain structures and determining the joint angle of the driven axis based on the position coordinates of the fourth joint in the basic coordinate system, the speed and accuracy of the parallel robot position calculation are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Hereinafter, some specific embodiments of the present invention will be described in detail in an exemplary and non-limiting manner with reference to the accompanying drawings. The same reference numerals in the accompanying drawings indicate the same or similar components or parts. It should be understood by those skilled in the art that these drawings are not necessarily drawn to scale. In the accompanying drawings:

[0017] Figure 1 A schematic flow chart of a method for determining the position of a parallel robot based on forward kinematics provided in one embodiment of the present invention;

[0018] Figure 2 The transformation matrix from the flange end to the world coordinate system is determined based on the first case in one embodiment of the present invention. Schematic diagram of the process;

[0019] Figure 3 The transformation matrix from the flange end to the world coordinate system is determined based on the second case in one embodiment of the present invention. Schematic diagram of the process;

[0020] Figure 4 A schematic diagram of the structure of the first standard DH parameter of the parallel robot provided by one embodiment of the present invention;

[0021] Figure 5 A schematic diagram of the structure of the second standard DH parameters of the parallel robot provided by an embodiment of the present invention;

[0022] Figure 6 A schematic diagram of the structure of a parallel robot provided by an embodiment of the present invention;

[0023] Figure 7 A schematic diagram of the structure of a device for determining the position of a parallel robot based on forward kinematics provided in one embodiment of the present invention. DETAILED DESCRIPTION

[0024] In order to make the purpose, features and advantages of the present invention more obvious and easy to understand, the technical solutions 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 those skilled in the art without creative work are within the scope of protection of the present invention.

[0025] like Figure 1 As shown, it is a flow chart of a method for determining the position of a parallel robot based on forward kinematics provided by an embodiment of the present invention; Figure 4 , which is a schematic diagram of the structure of the first standard DH parameters of a parallel robot provided by an embodiment of the present invention; Figure 5 A schematic diagram of the structure of the second standard DH parameters of the parallel robot provided in one embodiment of the present invention; Figure 6 , which is a schematic diagram of the structure of a parallel robot provided in one embodiment of the present invention.

[0026] A method for determining the position of a parallel robot based on forward kinematics, the method comprising at least the following steps:

[0027] S101, dividing a closed chain structure formed by connecting a first joint, a second joint, a third joint, a fourth joint, a fifth joint, a sixth joint, and a seventh joint of the parallel robot into a first open chain structure and a second open chain structure; the first open chain structure and the second open chain structure are both formed by connecting the joints;

[0028] S102, intersecting a first circle formed by the third joint angle position as a center and the third joint axis length as a radius with a second circle formed by the seventh joint angle position as a center and the seventh joint axis length as a radius, and calculating the position coordinates of the fourth joint angle in the basic coordinate system;

[0029] S103, determining a transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open-chain structure, and the standard DH parameters of the parallel robot;

[0030] S104, extracting the position coordinates corresponding to the flange end from the transformation matrix from the flange end to the world coordinate system.

[0031] In S101, Figure 5 As shown, for example: the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint and the seventh joint of the parallel robot are combined to form a closed chain structure. In the closed chain structure, the first open chain structure formed by the first joint, the second joint, the third joint, the fourth joint and the fifth joint are connected in sequence, and the second open chain structure formed by the sixth joint and the seventh joint are connected in sequence. In the closed chain structure, the first open chain structure formed by the first joint, the sixth joint, the seventh joint, the fourth joint and the fifth joint are connected in sequence, and the second open chain structure formed by the second joint and the third joint are connected in sequence. In the closed chain structure, the first open chain structure is the main axis, and the second open chain structure is the secondary axis.

[0032] Therefore, splitting the closed-chain structure into two open-chain structures is beneficial to improving the speed of position calculation of the parallel robot.

[0033] It should be noted that the third joint and the seventh joint are driven axes; the first joint, the second joint, the fourth joint, the fifth joint and the sixth joint are active axes.

[0034] In S102, the first standard DH parameter is as follows Figure 4 As shown, where θ dh,x (°), d dh,x ,d ah,x with a dh,x The x in (°) represents the number of the joint axis, for example, x=1, 2, 3, 4, 5, 6, 7.

[0035] When the first open-chain structure is a structure formed by sequentially connecting the first joint, the second joint, the third joint, the fourth joint and the fifth joint, the first standard DH parameter of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the fourth joint angle θ4, the fifth joint angle θ5 and the sixth joint angle θ6 are obtained; then, based on the first standard DH parameter and the moving distance d1 corresponding to the first joint of the parallel robot, the transformation matrix is ​​determined. Based on the first standard DH parameter and the second joint angle θ2 corresponding to the second joint of the parallel robot, determine the transformation matrix Based on the first standard DH parameter corresponding to the third joint of the parallel robot and the third joint angle θ3 as a variable, determine the transformation matrix containing the θ3 variable Based on the first standard DH parameter corresponding to the fourth joint of the parallel robot and the fourth joint angle θ4, the transformation matrix is ​​determined in, Represents the transformation matrix from the first joint to the base coordinate system; represents the transformation matrix from the second joint to the first joint, represents the transformation matrix from the third joint to the second joint, Represents the transformation matrix from the fourth joint to the third joint. Finally, based on the transformation matrix Transformation Matrix The transformation matrix containing the θ3 variable And the transformation matrix Determine the transformation matrix containing the θ3 variable

[0036] For example: the transformation matrix containing the θ3 variable The specific calculation formula is shown in the following formula (1):

[0037]

[0038] When the first open-chain structure is a structure formed by connecting the first joint, the sixth joint, the seventh joint, the fourth joint and the fifth joint, the second standard DH parameter of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the fourth joint angle θ4, the fifth joint angle θ5 and the sixth joint angle θ6 are obtained; then, based on the second standard DH parameter and the moving distance d1 corresponding to the first joint of the parallel robot, the transformation matrix is ​​determined. Based on the second standard parameter corresponding to the sixth joint of the parallel robot and the sixth joint angle θ6, the transformation matrix is ​​determined Based on the second standard parameter corresponding to the seventh joint of the parallel robot and the seventh joint angle θ7 as a variable, determine the transformation matrix containing the θ7 variable Based on the second standard parameter corresponding to the fourth joint of the parallel robot and the fourth joint angle θ4, the transformation matrix is ​​determined in, represents the transformation matrix from the sixth joint to the first joint, represents the transformation matrix from the seventh joint to the sixth joint, Represents the transformation matrix from the fourth joint to the seventh joint; finally based on the transformation matrix Transformation Matrix The transformation matrix containing the θ7 variable And the transformation matrix Determine the transformation matrix containing the θ7 variable

[0039] Transformation Matrix The specific calculation formula is shown in the following formula (2):

[0040]

[0041] Assume that the position coordinates of the fourth joint in the base coordinate system are (x, y, z); from the transformation matrix containing the θ3 variable The fourth column element is extracted as follows:

[0042]

[0043] From formula (3), we can know that the position coordinates of the fourth joint in the basic coordinate system are ( in: represents the first row of elements in formula (3), represents the second row of elements in formula (3), Represents the third row of elements in formula (3).

[0044] Therefore, the following equation system (4) is established:

[0045]

[0046] After that, after subtracting all the numbers without θ3 from x and y, the sum of the squares is The specific formula is shown in formula (5):

[0047]

[0048] Similarly, from the transformation matrix containing the θ7 variable The position coordinates of the fourth joint in the basic coordinate system are extracted as Therefore, the following equation system (6) is established:

[0049]

[0050] Formula (6) is simplified to the following formula (7):

[0051]

[0052] Solve equations (5) and (7) to obtain the intersection point (x, y); thus, obtain the position coordinates (x, y, z) of the fourth joint in the basic coordinate system.

[0053] In S103, in the first case, the first open chain structure formed by connecting the first joint, the second joint, the third joint, the fourth joint, and the fifth joint is used as the main axis, and the second open chain structure formed by connecting the sixth joint and the seventh joint is used as the secondary axis. The standard parameters of the parallel robot are the first standard DH parameters. Figure 2In the schematic diagram of the second standard DH parameters shown, the flange is used as the dividing line for explanation respectively, and the joints located before the flange are: the second joint is derived from the first joint and the DH parameter corresponding to the second joint is calculated with reference to the first joint, the third joint is derived from the second joint and the DH parameter corresponding to the third joint is calculated with reference to the second joint, the fourth joint is derived from the third joint and the DH parameter corresponding to the fourth joint is calculated with reference to the third joint, and the fifth joint is derived from the fourth joint and the DH parameter corresponding to the fifth joint is calculated with reference to the fourth joint.

[0054] Exemplarily, first, according to the obtained position coordinates (x, y, z), combined with formula (4), the third joint angle θ3 is solved; secondly, based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the first joint angle, the second joint angle, the third joint angle, the fourth joint angle, the fifth joint angle, and the first standard DH parameters of the parallel robot, the transformation matrix from the flange end to the world coordinate system is determined.

[0055] The second case: the first open-chain structure formed by connecting the first joint, the sixth joint, the seventh joint, the fourth joint and the fifth joint is the main axis, and the second open-chain structure formed by connecting the second joint and the third joint is the secondary axis. The standard parameters of the parallel robot are the second standard DH parameters.

[0056] exist Figure 3 In the schematic diagram of the second standard DH parameters shown, the flange is used as the dividing line for explanation respectively. The joints located before the flange are: the sixth joint is derived from the first joint and the DH parameter corresponding to the sixth joint is calculated with reference to the first joint, the seventh joint is derived from the sixth joint and the DH parameter corresponding to the seventh joint is calculated with reference to the sixth joint, the fourth joint is derived from the seventh joint and the DH parameter corresponding to the fourth joint is calculated with reference to the seventh joint, and the fifth joint is derived from the fourth joint and the DH parameter corresponding to the fifth joint is calculated with reference to the fourth joint.

[0057] Exemplarily, first, according to the obtained position coordinates (x, y, z), combined with formula (6), the seventh joint angle θ7 is solved; secondly, based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the first joint angle, the sixth joint angle, the seventh joint angle, the fourth joint angle, the fifth joint angle, and the second standard parameters of the parallel robot, the transformation matrix from the flange end to the world coordinate system is determined.

[0058] In S104, the transformation matrix from the flange end to the world coordinate system is obtained. Transform the matrix The first three rows of elements are divided into the first column, the second column, the third column and the fourth column; the elements in the fourth column are determined as the position coordinates corresponding to the flange end.

[0059] Therefore, this embodiment disassembles the closed-chain structure into two open-chain structures, and determines the position coordinates of the fourth joint in the basic coordinate system based on the intersection of the third joint and the seventh joint, which is beneficial to the solution of the driven axis and improves the accuracy and efficiency of the parallel robot position calculation.

[0060] like Figure 2 As shown, it is a transformation matrix for determining the flange end to the world coordinate system based on the first case in one embodiment of the present invention. In a preferred implementation of this embodiment, when the first open-chain structure is formed by connecting the first joint, the second joint, the third joint, the fourth joint, and the fifth joint; determine the transformation matrix from the flange end to the world coordinate system At least the following steps are included:

[0061] S201, establishing an element correspondence relationship of a transformation matrix from a fourth joint to a basic coordinate system with the position coordinates and the third joint angle as variables, and calculating the third joint angle;

[0062] S202, obtaining a moving distance of the first joint along the Z axis of the basic coordinate system, a second joint angle, a fourth joint angle, and a fifth joint angle;

[0063] S203, determine the transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the first standard DH parameters of the parallel robot, the moving distance of the first joint along the Z axis of the basic coordinate system, the second joint angle, the third joint angle, the fourth joint angle, and the fifth joint angle.

[0064] Specifically, based on the movement distance d1 of the first joint along the Z axis of the basic coordinate system, the transformation matrix from the first joint to the basic coordinate system is determined: Based on the second joint angle θ2 and the first standard DH parameter corresponding to the second joint, determine the transformation matrix from the second joint to the first joint Based on the third joint angle θ3 and the first standard DH parameter corresponding to the third joint, determine the transformation matrix from the third joint to the second joint Based on the fourth joint angle θ4 and the first standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the third joint: Based on the fifth joint angle θ5 and the first standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the first standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint Based on the transformation matrix from the base coordinate system to the world coordinate system The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system

[0065] For example: Transformation Matrix The calculation formula is shown in formula (8):

[0066]

[0067] Therefore, this embodiment uses the first open chain structure formed by the first joint, the second joint, the third joint, the fourth joint and the fifth joint as the main axis, and determines the transformation matrix from the flange end to the world coordinate system based on the second standard DH parameter corresponding to the main axis. The accuracy and efficiency of parallel robot position calculation are improved.

[0068] like Figure 3 As shown, the transformation matrix from the flange end to the world coordinate system is determined based on the second case in one embodiment of the present invention. In a preferred implementation of this embodiment, when the first open-chain structure is formed by connecting the first joint, the sixth joint, the seventh joint, the fourth joint, and the fifth joint; determine the transformation matrix from the flange end to the world coordinate system At least the following steps are included:

[0069] S301, establishing an element correspondence relationship of a transformation matrix from the fourth joint to the basic coordinate system with the position coordinates and the seventh joint angle as variables to obtain the seventh joint angle;

[0070] S302, obtaining a moving distance of the first joint along the Z axis of the basic coordinate system, a sixth joint angle, a fourth joint angle, and a fifth joint angle;

[0071] S303, based on the basic coordinate system, the world coordinate system, the second standard DH parameters of the parallel robot, the movement distance of the first joint along the Z axis of the basic coordinate system, the sixth joint angle, the seventh joint angle, the fourth joint angle, and the fifth joint angle, determine the transformation matrix from the flange end to the world coordinate system.

[0072] In S303, based on the movement distance d1 of the first joint along the Z axis of the basic coordinate system, a transformation matrix from the first joint to the basic coordinate system is determined. Based on the sixth joint angle θ6 and the second standard DH parameter corresponding to the sixth joint, determine the transformation matrix from the sixth joint to the first joint: Based on the seventh joint angle θ7 and the second standard DH parameter corresponding to the seventh joint, determine the transformation matrix from the seventh joint to the sixth joint: Based on the fourth joint angle θ4 and the second standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the seventh joint: Based on the fifth joint angle θ5 and the second standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the second standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint Based on the transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system

[0073] For example: Transformation Matrix The calculation formula is shown in formula (9):

[0074]

[0075] Therefore, this embodiment uses the first open chain structure formed by the first joint, the sixth joint, the seventh joint, the fourth joint and the fifth joint as the main axis, and determines the transformation matrix from the flange end to the world coordinate system based on the second standard DH parameter corresponding to the main axis. The accuracy and efficiency of parallel robot position calculation are improved.

[0076] It should be noted that although θ appears in both the first standard DH parameter and the second standard parameter dh,x (°), d dh,x 、a dh,x (mm), but the spindle structures are different, so the specific values ​​corresponding to the parameters with the same expression form in the first standard DH parameters and the second standard DH parameters are different.

[0077] It should be understood that in various embodiments of the present invention, the size of the serial numbers of the above-mentioned processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0078] like Figure 7As shown, it is a structural schematic diagram of a device for determining the position of a parallel robot based on forward kinematics provided by an embodiment of the present invention. A device for determining the position of a parallel robot based on forward kinematics, the device 700 specifically includes: a division module 701, used to divide the closed chain structure formed by the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint and the seventh joint of the parallel robot into a first open chain structure and a second open chain structure; the first open chain structure and the second open chain structure are both formed by the connection of each joint; a calculation module 702, used to intersect the first circle formed by the third joint angle position as the center and the third joint's own axis length as the radius with the second circle formed by the seventh joint angle position as the center and the seventh joint's own axis length as the radius, and calculate the position coordinates of the fourth joint angle in the basic coordinate system; a determination module 703, used to determine the transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open chain structure, and the standard DH parameters of the parallel robot. Extraction module 704, for extracting from the transformation matrix Extract the position coordinates corresponding to the flange end.

[0079] In an optional embodiment, when the first open-chain structure is formed by connecting the first joint, the second joint, the third joint, the fourth joint, and the fifth joint; the determination module includes: a calculation unit for transforming the position coordinates of the fourth joint angle in the basic coordinate system with the third joint angle as a variable Establish an element correspondence relationship and calculate the third joint angle θ3; an acquisition unit is used to obtain the movement distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the fourth joint angle θ4, and the fifth joint angle θ5; a determination unit is used to determine the transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the first standard DH parameter of the parallel robot, the movement distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the third joint angle θ3, the fourth joint angle θ4, and the fifth joint angle θ5

[0080] In an optional embodiment, the determining unit includes: a first determining subunit, for determining a transformation matrix from the first joint to the basic coordinate system based on a moving distance d1 of the first joint along the Z axis of the basic coordinate system Based on the second joint angle θ2 and the first standard DH parameter corresponding to the second joint, determine the transformation matrix from the second joint to the first joint Based on the third joint angle θ3 and the first standard DH parameter corresponding to the third joint, determine the transformation matrix from the third joint to the second joint Based on the fourth joint angle θ4 and the first standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the third joint: Based on the fifth joint angle θ5 and the first standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the first standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint A second determining subunit is used to determine a transformation matrix from the base coordinate system to the world coordinate system. The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system

[0081] In an optional embodiment, when the first open-chain structure is formed by connecting the first joint, the sixth joint, the seventh joint, the fourth joint, and the fifth joint; the determination module includes:

[0082] The calculation unit is also used to transform the position coordinates and the seventh joint angle as variables into a transformation matrix Establish an element correspondence relationship and calculate the seventh joint angle θ7; the acquisition unit is also used to obtain the movement distance d1 of the first joint along the Z axis of the basic coordinate system, the sixth joint angle θ6, the fourth joint angle θ4, and the fifth joint angle θ5; the determination unit is also used to determine the transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the second standard DH parameters of the parallel robot, the movement distance d1 of the first joint along the Z axis of the basic coordinate system, the sixth joint angle θ6, the seventh joint angle θ7, the fourth joint angle θ4, and the fifth joint angle θ5

[0083] In an optional embodiment, the determining unit includes: the first determining subunit is further used to determine the transformation matrix from the first joint to the basic coordinate system based on the movement distance d1 of the first joint along the Z axis of the basic coordinate system Based on the sixth joint angle θ6 and the second standard DH parameter corresponding to the sixth joint, determine the transformation matrix from the sixth joint to the first joint: Based on the seventh joint angle θ7 and the second standard DH parameter corresponding to the seventh joint, determine the transformation matrix from the seventh joint to the sixth joint: Based on the fourth joint angle θ4 and the second standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the seventh joint: Based on the fifth joint angle θ5 and the second standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the second standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint The second determining subunit is further configured to, based on the transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system

[0084] In an optional embodiment, the extraction module includes: an acquisition unit for acquiring a transformation matrix from the flange end to the world coordinate system A partitioning unit is used to convert the transformation matrix The first three rows of elements are divided into a first column, a second column, a third column and a fourth column by column; and a determination unit is used to determine the elements of the fourth column as position coordinates corresponding to the flange end.

[0085] The above device can execute the method for determining the position of a parallel robot based on forward kinematics provided in an embodiment of the present invention, and has the corresponding functional modules and beneficial effects of executing the method for determining the position of a parallel robot based on forward kinematics. For technical details not fully described in this embodiment, please refer to the method for determining the position of a parallel robot based on forward kinematics provided in an embodiment of the present invention.

[0086] The present invention also provides an electronic device, comprising: a processor; a memory for storing executable instructions of the processor; the processor is used to read the executable instructions from the memory and execute the instructions to implement the method for determining the position of a parallel robot based on forward kinematics described in the present invention.

[0087] In addition to the above-mentioned methods and devices, an embodiment of the present application may also be a computer program product, which includes computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the method according to various embodiments of the present application described in the above-mentioned "Exemplary Method" section of this specification.

[0088] The computer program product may be written in any combination of one or more programming languages ​​to write program codes for performing the operations of the embodiments of the present application, including object-oriented programming languages, such as Java, C++, etc., and conventional procedural programming languages, such as "C" language or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as an independent software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0089] In addition, an embodiment of the present application may also be a computer-readable storage medium on which computer program instructions are stored. When the computer program instructions are executed by a processor, the processor executes the steps of the method according to the following embodiments of the present application described in the above "Exemplary Method" section of this specification.

[0090] The computer readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can include, for example, but is not limited to, a system, device or device of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0091] The basic principles of the present application are described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, effects, etc. mentioned in the present application are only examples and not limitations, and it cannot be considered that these advantages, strengths, effects, etc. are required by each embodiment of the present application. In addition, the specific details disclosed above are only for the purpose of illustration and ease of understanding, not for limitation, and the above details do not limit the present application to being implemented by adopting the above specific details.

[0092] The block diagrams of the devices, apparatuses, equipment, and systems involved in this application are only illustrative examples and are not intended to require or imply that they must be connected, arranged, and configured in the manner shown in the block diagram. As will be appreciated by those skilled in the art, these devices, apparatuses, equipment, and systems can be connected, arranged, and configured in any manner. Words such as "including", "comprising", "having", etc. are open words, referring to "including but not limited to", and can be used interchangeably with them. The words "or" and "and" used here refer to the words "and / or" and can be used interchangeably with them, unless the context clearly indicates otherwise. The words "such as" used here refer to the phrase "such as but not limited to", and can be used interchangeably with them.

[0093] It should also be noted that in the apparatus, device and method of the present application, each component or each step can be decomposed and / or recombined. Such decomposition and / or recombination should be regarded as equivalent solutions of the present application.

[0094] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use the present application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to other aspects without departing from the scope of the present application. Therefore, the present application is not intended to be limited to the aspects shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

[0095] The above description has been given for the purpose of illustration and description. In addition, this description is not intended to limit the embodiments of the present application to the forms disclosed herein. Although multiple example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, changes, additions and sub-combinations thereof.

[0096] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine different embodiments or examples described in this specification and the features of different embodiments or examples, unless they are contradictory.

[0097] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0098] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A method for determining the position of a parallel robot based on forward kinematics, characterized in that: include: Dividing a closed chain structure formed by connecting the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint and the seventh joint of the parallel robot into a first open chain structure and a second open chain structure; The first open-chain structure and the second open-chain structure are both formed by connecting various joints; The first circle formed by the third joint angle position as the center and the third joint axis length as the radius is intersected with the second circle formed by the seventh joint angle position as the center and the seventh joint axis length as the radius, and the position coordinates of the fourth joint angle in the basic coordinate system are calculated; Based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open-chain structure, and the standard DH parameters of the parallel robot, determine the transformation matrix from the flange end to the world coordinate system From the transformation matrix Extract the position coordinates corresponding to the flange end.

2. The method according to claim 1, characterized in that: When the first open-chain structure is formed by connecting the first joint, the second joint, the third joint, the fourth joint, and the fifth joint; the transformation matrix from the flange end to the world coordinate system is determined based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open-chain structure, and the standard DH parameters of the parallel robot. include: The transformation matrix with the position coordinates of the fourth joint angle in the basic coordinate system and the third joint angle as variables Establish the corresponding relationship between the elements and calculate the third joint angle θ3; Obtain the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the fourth joint angle θ4, and the fifth joint angle θ5; Based on the basic coordinate system, the world coordinate system, the first standard DH parameter of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the third joint angle θ3, the fourth joint angle θ4, and the fifth joint angle θ5, determine the transformation matrix from the flange end to the world coordinate system 3. The method according to claim 2, characterized in that The transformation matrix from the flange end to the world coordinate system is determined based on the basic coordinate system, the world coordinate system, the first standard DH parameter of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the third joint angle θ3, the fourth joint angle θ4, and the fifth joint angle θ5. include: Based on the movement distance d1 of the first joint along the Z axis of the base coordinate system, determine the transformation matrix from the first joint to the base coordinate system Based on the second joint angle θ2 and the first standard DH parameter corresponding to the second joint, determine the transformation matrix from the second joint to the first joint Based on the third joint angle θ3 and the first standard DH parameter corresponding to the third joint, determine the transformation matrix from the third joint to the second joint Based on the fourth joint angle θ4 and the first standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the third joint: Based on the fifth joint angle θ5 and the first standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the first standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint Based on the transformation matrix from the base coordinate system to the world coordinate system The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system 4. The method according to claim 1, characterized in that: When the first open-chain structure is formed by connecting the first joint, the sixth joint, the seventh joint, the fourth joint, and the fifth joint; the transformation matrix from the flange end to the world coordinate system is determined based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open-chain structure, and the standard DH parameters of the parallel robot. include: The transformation matrix with the position coordinates and the seventh joint angle as variables Establish the element correspondence and obtain the seventh joint angle θ7; Obtain the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the sixth joint angle θ6, the fourth joint angle θ4, and the fifth joint angle θ5; Based on the basic coordinate system, the world coordinate system, the second standard DH parameters of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the sixth joint angle θ6, the seventh joint angle θ7, the fourth joint angle θ4, and the fifth joint angle θ5, determine the transformation matrix from the flange end to the world coordinate system 5. The method according to claim 4, characterized in that The transformation matrix from the flange end to the world coordinate system is determined based on the basic coordinate system, the world coordinate system, the second standard DH parameters of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the sixth joint angle θ6, the seventh joint angle θ7, the fourth joint angle θ4, and the fifth joint angle θ5. include: Based on the movement distance d1 of the first joint along the Z axis of the base coordinate system, determine the transformation matrix from the first joint to the base coordinate system Based on the sixth joint angle θ6 and the second standard DH parameter corresponding to the sixth joint, determine the transformation matrix from the sixth joint to the first joint: Based on the seventh joint angle θ7 and the second standard DH parameter corresponding to the seventh joint, determine the transformation matrix from the seventh joint to the sixth joint: Based on the fourth joint angle θ4 and the second standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the seventh joint: Based on the fifth joint angle θ5 and the second standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the second standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint Based on the transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system 6. The method according to claim 1, characterized in that The transformation matrix from the flange end to the world coordinate system Extract the position coordinates corresponding to the flange end, including: Get the transformation matrix from the flange end to the world coordinate system The transformation matrix The first three rows of elements are divided into the first column, the second column, the third column and the fourth column; The elements in the fourth column are determined as the position coordinates corresponding to the flange end.

7. A device for determining the position of a parallel robot based on forward kinematics, characterized in that: include: A division module, used for dividing a closed chain structure formed by connecting the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint and the seventh joint of the parallel robot into a first open chain structure and a second open chain structure; The first open-chain structure and the second open-chain structure are both formed by connecting various joints; A calculation module, used for intersecting a first circle formed by taking the third joint angle position as the center and the third joint axis length as the radius with a second circle formed by taking the seventh joint angle position as the center and the seventh joint axis length as the radius, and calculating the position coordinates of the fourth joint angle in the basic coordinate system; A determination module is used to determine the transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the position coordinates of the fourth joint angle in the basic coordinate system, the joint angles of the first open-chain structure, and the standard DH parameters of the parallel robot. Extraction module for extracting from the transformation matrix Extract the position coordinates corresponding to the flange end.

8. The device according to claim 7, characterized in that When the first open-chain structure is formed by connecting the first joint, the second joint, the third joint, the fourth joint, and the fifth joint; the determining module includes: A calculation unit, for taking the position coordinates of the fourth joint angle in the base coordinate system and the third joint angle as a transformation matrix of the variables Establish the corresponding relationship between the elements and calculate the third joint angle θ3; An acquisition unit, used for acquiring a moving distance d1 of the first joint along the Z axis of the basic coordinate system, a second joint angle θ2, a fourth joint angle θ4, and a fifth joint angle θ5; a determination unit, for determining a transformation matrix from the flange end to the world coordinate system based on the basic coordinate system, the world coordinate system, the first standard DH parameter of the parallel robot, the moving distance d1 of the first joint along the Z axis of the basic coordinate system, the second joint angle θ2, the third joint angle θ3, the fourth joint angle θ4, and the fifth joint angle θ5 9. The device according to claim 8, characterized in that The determining unit comprises: The first determination subunit is used to determine the transformation matrix from the first joint to the basic coordinate system based on the movement distance d1 of the first joint along the Z axis of the basic coordinate system Based on the second joint angle θ2 and the first standard DH parameter corresponding to the second joint, determine the transformation matrix from the second joint to the first joint Based on the third joint angle θ3 and the first standard DH parameter corresponding to the third joint, determine the transformation matrix from the third joint to the second joint Based on the fourth joint angle θ4 and the first standard DH parameter corresponding to the fourth joint, determine the transformation matrix from the fourth joint to the third joint: Based on the fifth joint angle θ5 and the first standard DH parameter corresponding to the fifth joint, determine the transformation matrix from the fifth joint to the fourth joint Based on the first standard DH parameters corresponding to the flange, determine the transformation matrix from the flange end to the fifth joint A second determining subunit is used to determine a transformation matrix from the base coordinate system to the world coordinate system. The transformation matrix The transformation matrix The transformation matrix The transformation matrix The transformation matrix And the transformation matrix Determine the transformation matrix from the flange end to the world coordinate system 10. A computer readable medium having a computer program stored thereon, wherein when the program is executed by a processor, the method according to any one of claims 1 to 6 is implemented.