Inverse kinematics closed-form solving method of six-degree-of-freedom multi-link structure

Abstract

The invention discloses an inverse kinematics closed-form solving method of a six-degree-of-freedom multi-connecting-rod structure, and the method aims at a specific six-degree-of-freedom multi-connecting-rod mechanism, and comprises the steps: giving an expected pose, firstly solving joint angles q1, then solving q2 and q4, then solving q3, then solving q5 and q6, and finally screening solutions of q1 to q6 again by adopting the limiting condition of each joint, so as to obtain the inverse kinematics closed-form solving method of the six-degree-of-freedom multi-connecting-rod structure. Then, permutation and combination are conducted on the screened solutions, the solutions are substituted into a forward kinematics equation of a six-degree-of-freedom multi-connecting-rod structure, and a group of joint angles meeting the requirement that a homogeneous transformation matrix from a tail end connecting rod coordinate system to a base coordinate system is equal to the expected pose are found. According to the method, the calculation time of inverse kinematics is shortened, and the real-time performance of motion control is improved.

Description

Technical Field

[0001] The present invention relates to the field of robot kinematics, and particularly to a closed-form solution method for inverse kinematics of a six-degree-of-freedom multi-link structure. Background Art

[0002] In the field of robotics, the solution of inverse kinematics (IK) of a multi-link system is one of the core problems in robot control. As the name implies, inverse kinematics is the inverse process of forward kinematics. It is the process of finding the joint angles q given the pose ( B p, B p, B R) of a certain link (mainly the link where the end effector is located) in the robot coordinate system Σ. However, different from the forward kinematics that can obtain a unique closed-form solution, the inverse kinematics problem is very complex. There may be multiple solutions or even infinitely many solutions (for example, when there is kinematic redundancy), or there may be no feasible solution (for example, when the pose of the end effector is not within the workspace of the multi-link system). Even if there is a feasible solution, due to the complexity of the equations, it is not always possible to find a closed-form solution. The calculation of a closed-form solution either requires finding the important equations containing unknowns through algebraic intuition or finding the key points in terms of structure through geometric intuition, so as to express the position or orientation as a function of fewer unknowns. For example, if the last three joints (wrist) form an orthogonal spherical wrist (such as PUMA560), the solution can be decomposed. First, solve the first three joints, and then solve the last three joints to obtain a closed-form solution.

[0003] Therefore, for complex multi-link systems, the inverse kinematics problem is usually solved by numerical methods. For example, Zhang Tie et al. from South China University of Technology (CN119407781A) proposed an inverse kinematics solution method for a seven-degree-of-freedom robotic arm based on the Damped Least Squares (DLS). This method introduces a damping factor to optimize the pseudo-inverse of the Jacobian matrix, improving the numerical stability of inverse kinematics solution. Lian Wenkang et al. from China National Institute of Automation of China South Industries Corporation (CN116304512B) proposed an inverse kinematics solution method for a robot leg. This method is aimed at a leg structure with a coupled-driven parallelogram parallel joint and a hip joint where the three axes do not intersect at a point. It decouples the position and attitude at the ankle joint: for the position, using the initial position of the motor as the iteration initial value, and adopting a numerical method to solve the inverse kinematics; for the attitude, directly obtaining the ankle joint angle relationship by a geometric method, so as to decouple the position and attitude and perform fast solution.

[0004] In summary, in the inverse kinematics solution method of the multi-link system, it is difficult to find a closed-form solution, which has poor generality and often depends on a specific joint structure; the numerical solution has stronger generality and does not depend on the joint structure, but has problems such as low computational efficiency, poor real-time performance, and numerical instability. Summary of the Invention

[0005] Aiming at the deficiencies of the prior art, the present invention analyzes the forward kinematics equation of a six-degree-of-freedom multi-link system, finds a closed-form solution for the inverse kinematics applicable to the multi-link system, and proposes a closed-form solution method for the inverse kinematics of a six-degree-of-freedom multi-link structure, which can accelerate the calculation time of the inverse kinematics and improve the real-time performance.

[0006] A closed-form solution method for the inverse kinematics of a six-degree-of-freedom multi-link structure, wherein the rotation axes of the six links of the six-degree-of-freedom multi-link structure are the z-axis, x-axis, y-axis, y-axis, y-axis, and x-axis respectively, and the lengths are L1 to L6; the position offset of the current joint relative to the previous joint satisfies condition 1 or condition 2:

[0007] Condition 1: The position offsets are successively: (0, L2, -L1), (0, L3, 0), (0, L4, 0), (0, 0, -L5), (0, 0, -L6), (0, 0, 0);

[0008] Condition 2: The position offsets are successively: (0, -L2, -L1), (0, -L3, 0), (0, -L4, 0), (0, 0, -L5), (0, 0, -L6), (0, 0, 0);

[0009] The solution method includes the following steps:

[0010] S1: Given the desired pose of the end link of the six-degree-of-freedom multi-link structure, when the position offset satisfies condition 1, directly execute S3; when the position offset satisfies condition 2, first execute S2; the desired pose is expressed as:

[0011]

[0012] S2: Transform the desired pose according to the following rules:

[0013] x → x, y → -y, z → z, α → -α, β → β, γ → -γ

[0014] where α, β, γ are the Tait-Bryan Euler angles in the Z-Y-X intrinsic rotation order, representing the roll angle, pitch angle, and yaw angle around the x, y, and z axes respectively;

[0015] S3: First calculate the joint angle q1 of the corresponding joint of the link coordinate system 1, and classify and discuss according to whether R 3,1 is equal to 0: If R 3,1= 0, then q1 = arctan(R 2,1 / R 1,1 ); Otherwise, solve the following transcendental equation to obtain q1:

[0016]

[0017] S4: Solve the following transcendental equation according to q1 to obtain the joint angle q2 of the corresponding joint of Link Coordinate System 2:

[0018] a2cos(q2) + b2sin(q2) = c2

[0019] a2 = ycos(q1) - L2cos(q1) - xsin(q1) - L3

[0020] b2 = L1 + z

[0021] c2 = L4

[0022] Substitute q1 into the following equation to calculate the joint angle q4 of the corresponding joint of Link Coordinate System 4:

[0023]

[0024] S5: Solve the following transcendental equation according to q1 and q4 to obtain the joint angle q3 of the corresponding joint of Link Coordinate System 3:

[0025] a3cos(q3) + b3sin(q3) = c3

[0026] a3 = L6sin(q4)

[0027] b3 = L5 + L6cos(q4)

[0028] c3 = -(y - L2)sin(q1) - xcos(q1)

[0029] S6: Substitute q2, q3, and q4 into the following formula to calculate the joint angle q5 of the corresponding joint of Link Coordinate System 5:

[0030] q5 = -arcsin(R 3,1 / cos(q2)) - q3 - q4

[0031] Solve the transcendental equation according to q2 to obtain the joint angle q6 of the corresponding joint of Link Coordinate System 6:

[0032] R 3,2 cos(q6) - R 3,3 sin(q6) = sin(q2)

[0033] S7: Using the limit conditions of each joint, rescreen the solutions of q1 to q6, then permute and combine the screened solutions, and substitute them into the forward kinematics equation of the six-degree-of-freedom multi-link structure to find a set of joint angles that satisfy the equality between the homogeneous transformation matrix from the end-effector link coordinate system to the base coordinate system and the expected pose.

[0034] Further, the six-degree-of-freedom multi-link structure is the six-degree-of-freedom multi-link system of the leg of the Wukong-IV humanoid robot.

[0035] Further, the forward kinematics equation is specifically:

[0036] T des = B T6(q) = B T1(q1)· 1 T2(q2)· 2 T3(q3)· 3 T4(q4)· 4 T5(q5)· 5 T6(q6)

[0037] where T des is the homogeneous transformation matrix representing the expected pose of the end-effector link coordinate system Σ6; B T6(q) represents the homogeneous transformation matrix from the end-effector link coordinate system Σ6 to the base coordinate system Σ B ; B T1(q1) represents the homogeneous transformation matrix from the link coordinate system Σ1 to the base coordinate system Σ B ; i T i+1 (q i+1 ) represents the homogeneous transformation matrix from the link coordinate system Σ i+1 to the link coordinate system Σ i ; q i represents the joint angle of the joint corresponding to the coordinate system Σ i .

[0038] An inverse kinematics closed-form solution device for a six-degree-of-freedom multi-link structure, used to implement the inverse kinematics closed-form solution method of the six-degree-of-freedom multi-link structure. The device includes a six-degree-of-freedom multi-link structure judgment unit, a pose transformation unit, six joint angle solution units, and a closed-form solution output unit;

[0039] The six-degree-of-freedom multi-link structure judgment unit is used to receive the expected pose T des, determine whether the rotation axes of the six links of the six-degree-of-freedom multi-link structure satisfy the condition of being the z-axis, x-axis, y-axis, y-axis, y-axis, and x-axis in sequence. If not, directly exit; if so, continue to determine whether the position offset of the current joint relative to the previous joint satisfies Condition 1. If it satisfies, execute the joint angle q1 solving unit; if not, determine whether it satisfies Condition 2. If it satisfies, execute the pose transformation unit; otherwise, directly exit; the specific Condition 1 and Condition 2 are as follows:

[0040] Condition 1: The position offsets are in sequence: (0, L2, -L1), (0, L3, 0), (0, L4, 0), (0, 0, -L5), (0, 0, -L6), (0, 0, 0);

[0041] Condition 2: The position offsets are in sequence: (0, -L2, -L1), (0, -L3, 0), (0, -L4, 0), (0, 0, -L5), (0, 0, -L6), (0, 0, 0);

[0042] The pose transformation unit is used to transform the expected pose of the six-degree-of-freedom multi-link structure that satisfies Condition 2 according to the following rules: x→x, y→-y, z→z, α→-α, β→β, γ→-γ, and then execute the joint angle q1 solving unit;

[0043] The joint angle q1 solving unit is used to perform classification and solution according to whether R 3,1 is equal to 0. If R 3,1 = 0, then q1 = arctan(R 2,1 / R 1,1 ); otherwise, solve the following transcendental equation to obtain q1:

[0044]

[0045] The joint angle q2 solving unit is used to solve the following transcendental equation to obtain q2 according to q1:

[0046] a2cos(q2)+b2sin(q2) = c2

[0047] a2 = ycos(q1)-L2cos(q1)-xsin(q1)-L3

[0048] b2 = L1+z

[0049] c2 = L4

[0050] The joint angle q4 solving unit is used to substitute q1 into the following equation to calculate q4:

[0051]

[0052] The joint angle q3 solving unit is used to solve the transcendental equation as follows to obtain q3 based on q1 and q4:

[0053] a3cos(q3) + b3sin(q3) = c3

[0054] a3 = L6sin(q4)

[0055] b3 = L5 + L6cos(q4)

[0056] c3 = -(y - L2)sin(q1) - xcos(q1)

[0057] The joint angle q5 solving unit is used to substitute q2, q3, and q4 into the following formula to calculate the joint angle q5 of the corresponding joint of the fifth link coordinate system:

[0058] q5 = -arcsin(R 3,1 / cos(q2)) - q3 - q4

[0059] The joint angle q6 solving unit is used to solve the following transcendental equation to obtain q6:

[0060] R 3,2 cos(q6) - R 3,3 sin(q6) = sin(q2)

[0061] The closed-form solution output unit is used to re-screen the solutions of q1 to q6 based on the limit conditions of each joint, then perform permutations and combinations on the screened solutions, substitute them into the forward kinematics equation of the six-degree-of-freedom multi-link structure, and output a set of joint angles that satisfy the homogeneous transformation matrix from the end link coordinate system to the base coordinate system being equal to the expected pose.

[0062] The beneficial effects of the present invention are as follows:

[0063] The inverse kinematics closed-form solution method of the six-degree-of-freedom multi-link structure of the present invention finds the inverse kinematics closed-form solution of the multi-link system, avoids the problems of low calculation efficiency, poor real-time performance, and numerical instability of the general numerical solution method, speeds up the calculation time of inverse kinematics, and improves the real-time performance of motion control. Description of the Drawings

[0064] Figure 1 It is a schematic diagram of the joint structure of the embodiment of the present invention.

[0065] Figure 2 It is a flowchart of the inverse kinematics closed-form solution method of the six-degree-of-freedom multi-link structure of the embodiment of the present invention.

[0066] Figure 3 It is a side view of the joint structure of the embodiment of the present invention. Detailed Embodiments

[0067] The present invention will be described in detail below with reference to the accompanying drawings and preferred embodiments. The objectives and effects of the present invention will become more apparent. It should be understood that the specific embodiments described herein are merely used to explain the present invention and are not intended to limit the present invention.

[0068] The six-degree-of-freedom multi-link structure of this embodiment is as Figure 1 shown, which is the multi-link system of the leg of the Wukong-IV humanoid robot and has six degrees of freedom. In this embodiment, the left leg is selected, and the coordinate system names, joint names, rotation axes, and position offsets relative to the previous joint of the left leg from the base coordinate system Σ B to the end-link coordinate system are as follows:

[0069] Σ1: HipZ, z-axis, (0, L2, -L1)

[0070] Σ2: HipX, x-axis, (0, L3, 0)

[0071] Σ3: HipY, y-axis, (0, L4, 0)

[0072] Σ4: Knee, y-axis, (0, 0, -L5)

[0073] Σ5: AnkleY, y-axis, (0, 0, -L6)

[0074] Σ6: AnkleX, x-axis, (0, 0, 0)

[0075] The closed-form solution method for the inverse kinematics of the six-degree-of-freedom multi-link structure of this embodiment includes the following steps:

[0076] S1. Using the symbolic math toolbox of MATLAB and utilizing the homogeneous transformation matrix (Homogeneous Transformations Matrix) and its chain rule, obtain the forward kinematics equation:

[0077] T des = B T6(q) = B T1(q1)· 1 T2(q2)· 2 T3(q3)· 3 T4(q4)· 4 T5(q5)· 5 T6(q6) (1)

[0078] where, T des is the homogeneous transformation matrix representing the desired pose of the end-link coordinate system Σ6; B T6(q) represents the homogeneous transformation matrix from the end-link coordinate system Σ6 to the base coordinate system Σ B ;B T1(q1) represents the homogeneous transformation matrix from the link coordinate system Σ1 to the base coordinate system Σ B ; i T i+1 (q i+1 ) represents the homogeneous transformation matrix from the link coordinate system Σ i+1 to the link coordinate system Σ i ; q i represents the joint angle of the joint corresponding to the coordinate system Σ i .

[0079] S2. Note that the structures of Wukong-IV's left and right legs are symmetric. Here, taking the left leg as an example, the inverse kinematic solution of the right leg can be obtained by mirror symmetry of the solution of the left leg. Therefore, first determine whether it is the left leg. If it is the right leg, the desired pose is transformed according to the following rules:

[0080] x → x, y → -y, z → z

[0081] α → -α, β → β, γ → -γ

[0082] where x, y, z are the positions of the end-effector link coordinate system Σ6; α, β, γ are the attitudes of the end-effector link coordinate system Σ6, which are the Tait–Bryan angles in the order of intrinsic rotations of Z-Y-X, representing the roll, pitch, and yaw angles around the x, y, and z axes respectively.

[0083] S3. First, calculate q1. Multiply both sides of the forward kinematic equation (1) by to obtain:

[0084]

[0085] where the left side of the equation is:

[0086]

[0087] x′ = xcos(q1) - L2sin(q1) + ysin(q1)

[0088] y′ = ycos(q1) - L2cos(q1) - xsin(q1)

[0089] z′ = L1 + z

[0090] The right side of the equation is:

[0091]

[0092] R′ 2,2= cos(q2)cos(q6) - cos(q3 + q4 + q5)sin(q2)sin(q6)

[0093] R′ 2,3 = -cos(q2)sin(q6) - cos(q3 + q4 + q5)cos(q6)sin(q2)

[0094] R′ 3,2 = cos(q6)sin(q2) + cos(q3 + q4 + q5)cos(q2)sin(q6)

[0095] R′ 3,3 = cos(q3 + q4 + q5)cos(q2)cos(q6) - sin(q2)sin(q6)

[0096] x′ = -L6 sin(q3 + q4) - L5sin(q3)

[0097] y′ = L3 + L4 cos(q2) + sin(q2)[L5 cos(q3) + L6(cos(q3 + q4))]

[0098] z′ = L4sin(q2) - cos(q2)[L5cos(q3) + L6(cos(q3 + q4))]

[0099] Wherein, L3 to L6 respectively represent the lengths of the third, fourth, fifth, and sixth connecting rods;

[0100] Observing the matrix elements (2, 1) and (3, 1) on both sides of Equation (2) gives:

[0101] R 2,1 cos(q1) - R 1,1 sin(q1) = sin(q3 + q4 + q5)sin(q2) (3)

[0102] R 3,1 = -sin(q3 + q4 + q5)cos(q2) (4)

[0103] The following is a discussion in different cases:

[0104] Case 1: When R 3,1 = 0:

[0105] sin(q3 + q4 + q5)cos(q2) = 0 (5)

[0106] Generally, the hip roll joint satisfies the limit Therefore, cos(q2) ≠ 0 and sin(q3 + q4 + q5) = 0.

[0107] Substituting into Equation (3) gives:

[0108] R 2,1 cos(q1) - R 1,1 sin(q1) = 0 (6)

[0109] Assume R 1,1 = 0, substituting into Equation (6), we get R 2,1 cos(q1) = 0. Since generally, the hip yaw joint satisfies the limit Therefore, cos(q1) ≠ 0, R 2,1 = 0. Thus It is contradictory to the orthogonal property of the rotation matrix and the original assumption is not valid, so R 1,1 ≠ 0.

[0110] Dividing both sides of Equation (6) by R 1,1 cos(q1) gives: Thus

[0111] Case 2: When R 3,1 ≠ 0:

[0112] Dividing Equation (3) by Equation (4) gives:

[0113]

[0114] Observing the matrix elements (2, 4) and (3, 4) on both sides of Equation (2) gives:

[0115] ycos(q1) - L2 cos(q1) - xsin(q1) = L3 + L4 cos(q2) + sin(q2)[L5 cos(q3) + L6(cos(q3 + q4))] (8)

[0116] L1 + z = L4sin(q2) - cos(q2)[L5cos(q3) + L6(cos(q3 + q4))] (9)

[0117] where L1 and L2 respectively represent the lengths of the first and second connecting rods;

[0118] From Equation (9), it can be seen that when L5 cos(q3) + L6(cos(q3 + q4)) = 0, z = -L1 + L4sin(q2), that is, the vertical height of the left foot is equal to the hip pitch joint, as Figure 3 shown. During normal movement, this situation will not occur. In the subsequent proof, it is assumed that z < -L1 + L4sin(q2), so:

[0119] L5 cos(q3)+L6(cos(q3+q4))≠0

[0120] By transposing equations (8) and (9) and then dividing them term by term, we get:

[0121]

[0122] By cross - multiplying, we get:

[0123] cos(q2)(ycos(q1)-L2 cos(q1)-xsin(q1)-L3)=L4 - sin(q2)(L1+z) (11)

[0124] By combining equations (7) and (8), q2 can be eliminated to obtain a transcendental equation containing only q1. The specific process is as follows:

[0125] First, since and cos(q2)>0, so:

[0126]

[0127] Dividing both sides of equation (11) by cos(q2) and substituting equations (7) and (12), we get:

[0128]

[0129] Equation (13) is a transcendental equation containing only q1, which can be transformed into a polynomial equation through variable substitution. Squaring both sides and arranging, we get:

[0130]

[0131] Expanding and arranging, we get:

[0132]

[0133] Let Then Substituting into equation (15) and multiplying both sides by 1 + u 2 we get:

[0134] 4au 2 +b(1 - u 2 ) 2 +2cu(1 - u 2 )+2du(1 + u 2 )+e(1 - u 2 )(1 + u 2 )+f(1 + u 2 ) 2 =0 (16) Expanding and arranging, we get:

[0135] Au4 +Bu 3 +Cu 2 +Du + E = 0 (17)

[0136] A = b - e + f

[0137] B = -2c + 2d

[0138] C = 4a - 2b + 2f

[0139] D = 2c + 2d

[0140] E = b + e + f

[0141] Solve the quartic equation to obtain q1 = 2 arctan(u). Note that a quartic equation has at most four real solutions. Generally, the hip yaw joint limit is There may be more than one solution that satisfies this condition, and further judgment is required after calculating other joint angles.

[0142] S4. Substitute q1 into equation (8) to obtain a transcendental equation containing only q2:

[0143] a2cos(q2) + b2sin(q2) = c2 (18)

[0144] a2 = ycos(q1) - L2cos(q1) - xsin(q1) - L3

[0145] b2 = L1 + z

[0146] c2 = L4

[0147] The solution method of equation (18) is similar to that of equation (15). Let Then Substitute into equation (18), and multiply both sides by After rearrangement, we get:

[0148]

[0149] Equation (19) may have two solutions. Generally, the hip roll joint satisfies the limit There may be more than one solution that satisfies this condition, and further judgment is required after calculating other joint angles.

[0150] S5. Calculate q4 according to q1. Observe the matrix elements (1, 4), (2, 4), (3, 4) on both sides of equation (2):

[0151] x' = (y - L2)sin(q1) + xcos(q1) = -L5sin(q3) - L6sin(q3 + q4) (20)

[0152] y′ = (y - L2)cos(q1) - xsin(q1)

[0153] = L3 + L4cos(q2) + sin(q2)[L5cos(q3) + L6(cos(q3 + q4))] (21)

[0154] z′ = L1 + z = L4sin(q2) - cos(q2)[L5cos(q3) + L6(cos(q3 + q4))] (22)

[0155]

[0156] Generally, the knee joint satisfies the limit q4 ∈ (0, π), and there is no multiple solution.

[0157] S6. Substitute q1 and q4 into Equation (20) to obtain a transcendental equation containing only q3:

[0158] a3cos(q3) + b3sin(q3) = c3 (25)

[0159] a3 = L6sin(q4)

[0160] b3 = L5 + L6cos(q4)

[0161] c3 = -(y - L2)sin(q1) - xcos(q1)

[0162] The solution method is the same as that of Equation (18), so it will not be elaborated here. Equation (25) may also have two solutions. Generally, the hip pitch joint satisfies the limit There may be more than one solution that satisfies this condition, and further judgment is required after calculating other joint angles.

[0163] S7. Substitute q2, q3, and q4 into Equation (4) to calculate q5:

[0164]

[0165] S8. Finally, calculate q6. Multiply both sides of the forward kinematic equation (1) by the inverse matrix 5 of the homogeneous transformation matrix T6(q6) from the link coordinate system Σ6 to the link coordinate system Σ5

[0166]

[0167] Among them, the left side of the equation is:

[0168]

[0169] The right side of the equation is:

[0170]

[0171] The partial elements of the matrix are as follows:

[0172] R″ 1.1 = cos(q4 + q5)[cos(q1)cos(q3) - sin(q1)sin(q2)sin(q3)] - sin(q4 + q5)[cos(q1)sin(q3) + cos(q3)sin(q1)sin(q2)]

[0173] R″ 1.3 = sin(q4 + q5)[cos(q1)cos(q3) - sin(q1)sin(q2)sin(q3)] + cos(q4 + q5)[cos(q1)sin(q3) + cos(q3)sin(q1)sin(q2)]

[0174] R″ 2.1 = cos(q4 + q5)[cos(q3)sin(q1) + cos(q1)sin(q2)sin(q3)] - sin(q4 + q5)[sin(q1)sin(q3) - cos(q1)cos(q3)sin(q2)]

[0175] R″ 2，3 = sin(q4 + q5)[cos(q3)sin(q1) + cos(q1)sin(q2)sin(q3)] + cos(q4 + q5)[sin(q1)sin(q3) - cos(q1)cos(q3)sin(q2)]

[0176] x″ = -[L5 cos(q1) + L6 cos(q4)][cos(q1)sin(q3) + cos(q3)sin(q1)sin(q2)] - L6sin(q4)[cos(q1)cos(q3) - sin(q1)sin(q2)sin(q3)] - L3 sin(q1) - L4 cos(q2)sin(q1)

[0177] y″ = L2 - [L5 + L6 cos(q4)][sin(q1)sin(q3) - cos(q1)cos(q3)sin(q2)] - L6 sin(q4)[cos(q3)sin(q1) + cos(q1)sin(q2)sin(q3)] + L3 cos(q1) + L4 cos(q1)cos(q2)

[0178] z″ = -L1 + L4 sin(q2) - L5 cos(q2)cos(q3) - L6 cos(q2)cos(q3 + q4)

[0179] From the matrix element (3, 2) on both sides of Equation (27), a transcendental equation containing only q6 is obtained:

[0180] R 3，2 cos(q6) - R 3，3 sin(q6) = sin(q2) (28)

[0181] The solution method is the same as that of Equation (18) and will not be elaborated here. Equation (28) may also have two solutions. Generally, the ankle roll joint satisfies the limit There may be more than one solution that satisfies this condition, and it needs to be judged by forward kinematics in the next step.

[0182] The transcendental equation of S9 and q1 has 4 solutions, and the transcendental equations of q2, q3, and q6 each have 2 solutions. The solutions that satisfy the joint limits are not unique. After permutation and combination, traverse and substitute them into the forward kinematics equation (1) to find the set of joint angles that satisfy T des = B T6(q).

[0183] To prove the effect of this embodiment, the damping least squares method was also used to solve the six-degree-of-freedom multi-link system of the left leg of the Wukong-IV humanoid robot in this embodiment. The joint angles and time consumption obtained by the method of this embodiment and the damping least squares method are shown in Table 1.

[0184] It can be seen from Table 1 that the time consumption of the closed-form solution method in this embodiment is significantly shortened, and the solution speed is increased by about 85%, approaching one order of magnitude.

[0185] Table 1 Data such as joint angles and time consumption obtained by the method of this embodiment and the damping least squares method

[0186]

[0187] Those of ordinary skill in the art can understand that the above are only preferred examples of the invention and are not used to limit the invention. Although the invention has been described in detail with reference to the foregoing examples, for those skilled in the art, they can still modify the technical solutions recorded in the foregoing examples, or perform equivalent replacements for some of the technical features. Any modifications, equivalent replacements, etc. made within the spirit and principle of the invention shall be included within the protection scope of the invention.

Claims

1. A closed-form solution method for inverse kinematics of a six-degree-of-freedom multi-link structure, characterized in that, The rotation axes of the six links of the six-degree-of-freedom multi-link structure are the z-axis, x-axis, y-axis, y-axis, y-axis, and x-axis respectively, and the lengths are L1 to L6; the position offset of the current joint relative to the previous joint satisfies Condition 1 or Condition 2: Condition 1: The position offsets are successively: (0, L2, -L1), (0, L3, 0), (0, L4, 0), (0, 0, -L5), (0, 0, -L6), (0, 0, 0); Condition 2: The position offsets are successively: (0, -L2, -L1), (0, -L3, 0), (0, -L4, 0), (0, 0, -L5), (0, 0, -L6), (0, 0, 0); The solution method includes the following steps: S1: Given the desired pose of the end link of the six-degree-of-freedom multi-link structure, when the position offset satisfies Condition 1, directly execute S3; when the position offset satisfies Condition 2, first execute S2; the desired pose is expressed as: S2: Transform the desired pose according to the following rules: x→x, y→-y, z→z, α→-α, β→β, γ→-γ where α, β, and γ are the Tait-Bryan Euler angles in the Z-Y-X intrinsic rotation order, representing the roll angle, pitch angle, and yaw angle around the x, y, and z axes respectively; S3: First, calculate the joint angle q1 of the joint corresponding to the link coordinate system 1. Classify and discuss according to whether R 3,1 is equal to 0: If R 3,1 = 0, then q1 = arctan(R 2,1 / R 1,1 ); otherwise, solve the following transcendental equation to obtain q1: S4: Solve the following transcendental equation according to q1 to obtain the joint angle q2 of the corresponding joint of Link Coordinate System 2: a2cos(q2)+b2 sin(q2)=c2 a2=ycos(q1)-L2cos(q1)-xsin(q1)-L3 b2=L1+z c2=L4 Substitute q1 into the following equation to calculate the joint angle q4 of the corresponding joint of Link Coordinate System 4: S5: According to q1 and q4, solve the following transcendental equation to obtain the joint angle q3 of the corresponding joint of Link Coordinate System 3: a3cos(q3)+b3sin(q3)=c3 a3=L6sin(q4) b3=L5+L6cos(q4) c3=-(y-L2)sin(q1)-xcos(q1) S6: Substitute q2, q3, and q4 into the following formula to calculate the joint angle q5 of the corresponding joint of Link Coordinate System 5: q5 = -arcsin(R 3,1 / cos(q2)) - q3 - q4 Solve the transcendental equation according to q2 to obtain the joint angle q6 of the corresponding joint of Link Coordinate System 6: R 3,2 cos(θ6) - R 3,3 sin(θ6) = sin(θ2) S7: Use the limit conditions of each joint to screen the solutions of q1 to q6 again, and then arrange and combine the screened solutions, substitute them into the forward kinematics equation of the six-degree-of-freedom multi-link structure, and find a set of joint angles that satisfy the homogeneous transformation matrix from the end link coordinate system to the base coordinate system to be equal to the desired pose.

2. The closed-form solution method for inverse kinematics of the six-degree-of-freedom multi-link structure according to claim 1, wherein, The six-degree-of-freedom multi-link structure is the six-degree-of-freedom multi-link system of the leg of the Wukong-IV humanoid robot.

3. The closed-form solution method for the inverse kinematics of the six-degree-of-freedom multi-link structure according to claim 1, characterized in that, The specific forward kinematics equation is: T des = B T6(q) = B T1(q1)· 1 T2(q2)· 2 T3(q3)· 3 T4(q4)· 4 T5(q5)· 5 T6(q6) Among them, T des is the homogeneous transformation matrix representing the desired pose of the end-effector coordinate system Σ6; B T6(q) represents the homogeneous transformation matrix from the end-effector coordinate system Σ6 to the base coordinate system Σ B ; B T1(q1) represents the homogeneous transformation matrix from the link coordinate system Σ1 to the base coordinate system Σ B ; i T i+1 (q i+1 ) represents the homogeneous transformation matrix from the link coordinate system Σ i+1 to the link coordinate system Σ i ; q i represents the joint angle of the joint corresponding to the coordinate system Σ i .

4. An inverse kinematics closed-form solution device for a six-degree-of-freedom multi-link structure, characterized in that, The device for realizing the inverse kinematics closed-form solution method of the six-degree-of-freedom multi-link structure described in Claim 1 includes a six-degree-of-freedom multi-link structure judgment unit, a pose transformation unit, six joint angle solving units, and a closed-form solution output unit; The six-degree-of-freedom multi-link structure judgment unit is used to receive the expected pose T input by the user des , and determine whether the rotation axes of the six links of the six-degree-of-freedom multi-link structure satisfy the conditions of being the z-axis, x-axis, y-axis, y-axis, y-axis, and x-axis in sequence. If not, directly exit; if satisfied, continue to determine whether the position offset of the current joint relative to the previous joint satisfies Condition 1. If satisfied, execute the joint angle q1 solving unit; if not, determine whether it satisfies Condition 2. If satisfied, execute the pose transformation unit; otherwise, directly exit; The specific conditions of Condition 1 and Condition 2 are as follows: Condition 1: The position offsets are successively: (0, L2, -L1), (0, L3, 0), (0, L4, 0), (0, 0, -L5), (0, 0, -L6), (0, 0, 0); Condition 2: The position offsets are successively: (0, -L2, -L1), (0, -L3, 0), (0, -L4, 0), (0, 0, -L5), (0, 0, -L6), (0, 0, 0); The pose transformation unit is used to transform the desired pose of the six - degree - of - freedom multi - link structure that meets Condition 2 according to the following rules: x→x, y→ - y, z→z, α→ - α, β→β, γ→ - γ, and then execute the joint angle q1 solving unit; The joint angle q1 solving unit is used to perform classification and solution according to whether R 3,1 is equal to 0. If R 3,1 = 0, then q1 = arctan(R 2,1 / R 1,1 ); otherwise, solve the following transcendental equation to obtain q1: The joint angle q2 solving unit is used to solve the following transcendental equation for q2 based on q1: a2cos(q2)+b2 sin(q2)=c2 a2=ycos(q1)-L2cos(q1)-xsin(q1)-L3 b2=L1+z c2=L4 The joint angle q4 solving unit is used to calculate q4 by substituting q1 into the following equation: The joint angle q3 solving unit is used to solve the following transcendental equation for q3 based on q1 and q4: a3 cos(q3)+b3 sin(q3)=c3 a3=L6sin(q4) b3=L5+L6 cos(q4) c3=-(y - L2)sin(q1)-xcos(q1) The joint angle q5 solving unit is used to calculate the joint angle q5 of the corresponding joint of link coordinate system five by substituting q2, q3, and q4 into the following formula: q5 = - arcsin(R 3,1 / cos(q2)) - q3 - q4 The joint angle q6 solving unit is used to solve the following transcendental equation for q6: R 3,2 cos(θ6) - R 3,3 sin(θ6) = sin(θ2) The closed - form solution output unit is used to re - screen the solutions of q1~q6 based on the limit conditions of each joint, then perform permutation and combination on the screened solutions, substitute them into the forward kinematics equation of the six - degree - of - freedom multi - link structure, and output a set of joint angles that satisfy the homogeneous transformation matrix from the end - effector link coordinate system to the base coordinate system being equal to the desired pose.

Images