A six-degree-of-freedom humanoid robot arm inverse solution algorithm

Abstract

The application discloses a kind of six degrees of freedom humanoid robot arm inverse solution algorithm, comprising the following steps: S1: establish six-axis robot arm model and draw original joint coordinate system diagram.S2: the six-axis robot arm model is reversed, and virtual joint coordinate system diagram is established.S3: the virtual rotation angle of each joint of the six-axis robot arm model after reversal is solved, and the virtual rotation angle is corresponded with the rotation angle of each joint in original joint coordinate system diagram, as initial iteration angle is iteratively calculated.The application is reversed according to mechanical arm, and the inverse solution of the reversed mechanical arm is solved as iteration initial value according to the characteristics that the first three axes of mechanical arm intersect at a point, the selection of the iteration initial value is more close to the real rotation angle of joint compared with prior art, and the iteration number can be greatly reduced, and the operation efficiency is improved.

Description

Technical Field

[0001] This invention relates to the field of robot arm control technology, and in particular to an inverse kinematics algorithm for a six-degree-of-freedom humanoid robot arm. Background Technology

[0002] Inverse kinematics of robotic arms is a core technology in robot control. Its goal is to calculate the rotation angles of each joint of the robotic arm, given the spatial position and orientation of the end effector. For robotic arms with a spherical wrist structure, accurate analytical solutions for joint angles can be quickly derived using analytical methods. However, in practical applications, many six-axis robotic arms with non-spherical wrists exist, where the last three axes do not intersect at the same point, making it impossible to directly apply the mature inverse kinematics solutions used for spherical wrist robotic arms.

[0003] Current inverse kinematics (IK) solutions for six-axis robotic arms with non-spherical wrists are mainly divided into two categories: purely analytical methods and purely iterative methods. Purely analytical methods struggle to derive closed-form analytical solutions for some special non-spherical wrist robotic arms. Purely iterative methods rely on suitable initial iteration values ​​(currently, the initial state of the robotic arm is often chosen as the initial iteration value). If the initial value is not chosen appropriately, the iteration convergence speed may be extremely slow, or even fail to converge, resulting in low solution efficiency and difficulty in guaranteeing solution accuracy.

[0004] As shown in the figure, the rotation axes of the fourth, fifth, and sixth joints of the robotic arm do not intersect at a single point, therefore it does not belong to the spherical wrist robotic arm. However, the rotation axes of the first, second, and third joints do intersect at a single point. This invention mainly proposes a simplified inverse algorithm for this robotic arm structure. Summary of the Invention

[0005] To address the technical problem that existing inverse kinematics methods for six-axis robotic arms with non-spherical wrists suffer from unreasonable initial value selection, resulting in extremely slow iteration speeds or even failure to converge, this invention provides an inverse kinematics algorithm for a six-DOF humanoid robot arm to solve the aforementioned problems.

[0006] The technical solution adopted by this invention to solve its technical problem is: a six-degree-of-freedom humanoid robot arm inverse kinematics algorithm, comprising the following steps: S1: Create a six-axis robotic arm model and draw the original joint coordinate system diagram.

[0007] S2: Invert the six-axis robotic arm model to establish a virtual joint coordinate system diagram.

[0008] S3: Solve for the virtual rotation angles of each joint in the reversed six-axis robotic arm model, and correlate the virtual rotation angles with the rotation angles of each joint in the original joint coordinate system diagram.

[0009] S4: Calculate the homogeneous transformation matrix of the end joint based on the rotation angles of each joint in the original joint coordinate system diagram obtained from the solution. .

[0010] S5: Based on the homogeneous transformation matrix Calculate the position and attitude error vector of the end effector of a six-axis robotic arm. Position error and attitude error .

[0011] S6: If the position error and attitude error If all values ​​are less than the set threshold, the calculation ends; otherwise, the rotation angle of each joint is updated using an iterative formula, and steps S4 to S6 are repeated.

[0012] In an optional embodiment of the present invention, in step S2, the method for establishing the virtual joint coordinate system diagram is as follows: establishing a virtual base coordinate system that coincides with the origin of the end joint coordinate system in the original joint coordinate system diagram. A virtual tool coordinate system that coincides with the origin of the base coordinate system in the original joint coordinate system diagram. From the virtual base coordinate system To virtual tool coordinate system A virtual joint coordinate system diagram is established along the joint connection sequence of the six-axis robotic arm.

[0013] In an optional embodiment of the present invention, step S3, the method for solving the virtual rotation angles of each joint of the six-axis robotic arm model after reversal, includes the following steps: S31: Based on the original joint coordinate system diagram and the virtual joint coordinate system diagram, list the DH parameter table; S32: Calculate the homogeneous transformation matrix of each joint of the original six-axis robotic arm model and the inverted six-axis robotic arm model according to the DH parameter table. , , , , , According to the formula and formula ,get Solving the system of equations yields , and ,in, This represents the homogeneous transformation matrix of the virtual first coordinate system relative to the virtual base coordinate system after the six-axis robotic arm model is reversed. This represents the homogeneous transformation matrix of the virtual second coordinate system relative to the virtual first coordinate system after the six-axis robotic arm model is reversed. This represents the homogeneous transformation matrix of the virtual third coordinate system relative to the virtual second coordinate system after the six-axis robotic arm model is reversed. This represents the homogeneous transformation matrix of the virtual sixth coordinate system relative to the virtual base coordinate system after the six-axis robotic arm model is inverted. This represents the homogeneous transformation matrix of the virtual seventh coordinate system relative to the virtual sixth coordinate system after the six-axis robotic arm model is reversed. This represents the homogeneous transformation matrix of the original sixth coordinate system relative to the original base coordinate system in the original six-axis robotic arm model. This represents the rotation angle of the sixth joint, calculated based on the inverted six-axis robotic arm model. This represents the rotation angle of the fifth joint, calculated based on the inverted six-axis robotic arm model. This represents the rotation angle of the fourth joint, calculated based on the inverted six-axis robotic arm model. This represents the vertical length between the center of the second joint and the center of the third joint. This represents the vertical length between the center of the third joint and the center of the fourth joint. This represents the vertical length between the center of the fourth joint and the center of the fifth joint. This represents the vertical length between the center of the fifth joint and the center of the sixth joint. The homogeneous transformation matrix of the virtual fourth coordinate system relative to the virtual third coordinate system The fourth column, Represents a virtual sixth coordinate system The origin is in the virtual base coordinate system x-axis coordinates Represents a virtual sixth coordinate system The origin is in the virtual base coordinate system The y-axis coordinate, Represents a virtual sixth coordinate system The origin is in the virtual base coordinate system The z-axis coordinate.

[0014] S33: According to , and Solve , and ,in, This represents the rotation angle of the third joint, calculated based on the inverted six-axis robotic arm model. This represents the rotation angle of the second joint, calculated based on the inverted six-axis robotic arm model. This represents the rotation angle of the first joint, calculated based on the inverted six-axis robotic arm model.

[0015] In an optional embodiment of the present invention, the method for solving the system of equations in step S32 is as follows: S321: Order , , , The following system of transformed equations is obtained: ; S322: Solving the aforementioned system of deformed equations yields... ; S323: Solve , , .

[0016] In an optional embodiment of the present invention, the solution step S33 is as follows: According to the formula ,get ; According to the formula ,get , ,in, This represents the value in the second row and second column of the homogeneous transformation matrix between the virtual sixth coordinate system and the virtual third coordinate system after the six-axis robotic arm model has been reversed. This represents the value in the second row and first column of the homogeneous transformation matrix of the virtual fifth coordinate system relative to the virtual third coordinate system after the six-axis robotic arm model is reversed.

[0017] In an optional embodiment of the present invention, the iterative formula in step S6 is: ,in, The joint rotation angles used to calculate each parameter in step S5, It is the pseudo-inverse of the Jacobian matrix. This refers to the updated rotation angles of each joint.

[0018] In an optional embodiment of the present invention, the six-axis robotic arm model includes an original first coordinate system arranged sequentially according to the joint connection order. Original second coordinate system Original third coordinate system Original fourth coordinate system Original fifth coordinate system Original sixth coordinate system and located in the original first coordinate system The original base coordinate system in front The method for determining the origin and coordinate axes of each original coordinate system in the original joint coordinate system diagram is as follows: Original first coordinate system The origin is located at the connection between the first and second joints; the original base coordinate system The origin is located at the connection between the first joint and the original base; and the original base coordinate system The z-axis and x-axis are in relation to the original first coordinate system. The z-axis and x-axis are parallel to each other; The z-axis of each original coordinate system is the rotation axis of the corresponding joint. The x-axis of the original coordinate system corresponding to the nth joint is perpendicular to the z-axis of the original coordinate system corresponding to the nth joint and the z-axis of the original coordinate system corresponding to the (n+1)th joint, and intersects the z-axis of the original coordinate system corresponding to the nth joint and the (n+1)th joint simultaneously. The z-axis of the original coordinate systems corresponding to different joints do not coincide. Wherein, n is an odd number.

[0019] The z-axis and x-axis of the original coordinate system corresponding to the i-th joint intersect to form the origin of the original coordinate system corresponding to the i-th joint, where i is an integer.

[0020] In an optional embodiment of the present invention, the reversed six-axis robotic arm model includes a virtual first coordinate system arranged sequentially according to the reversed joint connection order. Virtual Second Coordinate System Virtual third coordinate system Virtual fourth coordinate system Virtual Fifth Coordinate System Virtual sixth coordinate system Located in the virtual first coordinate system Virtual base coordinate system in front and the virtual sixth coordinate system The virtual seventh coordinate system in the rear .

[0021] Virtual First Coordinate System The origin is located at the end of the sixth joint; virtual sixth coordinate system The origin is located at the connection between the first and second joints; virtual seventh coordinate system The origin is located at the connection between the first joint and the original base, in the virtual seventh coordinate system. The z-axis and x-axis are in relation to the virtual sixth coordinate system. The z-axis and x-axis are parallel; virtual base coordinate system The origin, z-axis, and x-axis are relative to the virtual first coordinate system. The origin, z-axis, and x-axis coincide.

[0022] In an optional embodiment of the present invention, the x-axis of the virtual coordinate system corresponding to the nth joint is perpendicular to the z-axis of the virtual coordinate system corresponding to the nth joint and the z-axis of the virtual coordinate system corresponding to the (n+1)th joint, and intersects with the z-axis of the virtual coordinate system corresponding to the nth joint and the z-axis of the virtual coordinate system corresponding to the (n+1)th joint simultaneously; the z-axis of the virtual coordinate systems corresponding to different joints do not coincide.

[0023] The beneficial effects of this invention are: (1) The present invention reverses the robotic arm and establishes constraint equations based on the characteristic that the three axes of the robotic arm intersect at a point, thereby solving the inverse solution of the reversed robotic arm as the initial value for iteration. The selection of the initial value for iteration is closer to the actual rotation angle of the joint than that of the prior art, which can greatly reduce the number of iterations and improve the computational efficiency.

[0024] (2) The present invention ensures the accuracy of the calculation results and improves the solution efficiency and precision by rationally designing the original joint coordinate system diagram and the virtual joint coordinate system diagram after the inverted robotic arm. Attached Figure Description

[0025] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0026] Figure 1 This is a flowchart of the inverse kinematics algorithm for the six-degree-of-freedom humanoid robot arm described in this invention; Figure 2 This is a flowchart of the iterative calculation process in the inverse kinematic algorithm for the six-degree-of-freedom humanoid robot arm described in this invention; Figure 3 This is a model diagram of the six-degree-of-freedom humanoid robot arm described in this invention; Figure 4 This is the original joint coordinate system diagram corresponding to the original six-axis robotic arm model; Figure 5 This is a diagram of the virtual joint coordinate system corresponding to the inverted six-axis robotic arm model.

[0027] In the diagram, 1 is the original base, 2 is the first joint, 3 is the second joint, 4 is the third joint, 5 is the fourth joint, 6 is the fifth joint, 7 is the sixth joint, 8 is the first link, 9 is the second link, 10 is the third link, 11 is the fourth link, 12 is the fifth link, and 13 is the sixth link. Detailed Implementation

[0028] Embodiments of the present invention are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0029] An inverse kinematics algorithm for a six-DOF humanoid robot arm, the inverse kinematics algorithm being used to solve... Figure 3 The rotation angles of each joint in the six-axis robotic arm shown are as follows.

[0030] like Figure 1 As shown, the inverse algorithm includes the following steps: S1: Create a six-axis robotic arm model and draw the original joint coordinate system diagram.

[0031] Six-axis robotic arm model such as Figure 3 As shown, the six-axis robotic arm, from the original base 1 to the end joint, includes the original base 1, the first joint 2, the second joint 3, the third joint 4, the fourth joint 5, the fifth joint 6, and the sixth joint 7, which are connected to each other. The first joint 2 is connected to the second joint 3 through the first link 8, the second joint 3 is connected to the third joint 4 through the second link 9, and so on. The third joint 4, the fourth joint 5, the fifth joint 6, and the sixth joint 7 are connected in sequence through the third link 10, the fourth link 11, and the fifth link 12.

[0032] The coordinate system is set according to the six-axis robotic arm model, including the original first coordinate system set sequentially according to the joint connection order. Original second coordinate system Original third coordinate system Original fourth coordinate system Original fifth coordinate system Original sixth coordinate system and located in the original first coordinate system The original base coordinate system in front The "front" refers to the direction towards the source of the torque, i.e., the direction closer to the original base 1.

[0033] like Figure 4 The diagram shown is a diagram of the original joint coordinate system. The method for determining the origin and coordinate axes of each original coordinate system in the diagram of the original joint coordinate system is as follows: Original first coordinate system The origin is located at the connection between the first joint 2 and the second joint 3 (specifically, the connection between the first link 8 and the second joint 3); original base coordinate system The origin is located at the connection between the first joint 2 and the original base 1; and the original base coordinate system The z-axis and x-axis are in relation to the original first coordinate system. The z-axis and x-axis are parallel. For the other original coordinate systems, the z-axis and x-axis are determined first, and then the origin is determined.

[0034] The z-axis of each original coordinate system is the rotation axis of the corresponding joint, and the x-axis of the original coordinate system corresponding to the nth joint (using...) (represented by) the z-axis perpendicular to the original coordinate system corresponding to the nth joint (using) (represented) and the z-axis of the original coordinate system corresponding to the (n+1)th joint (using) (represented by), and the z-axis of the original coordinate system corresponding to the nth joint and the z-axis of the original coordinate system corresponding to the (n+1)th joint intersect simultaneously; the z-axis of the original coordinate systems corresponding to different joints do not coincide, where n is an odd number; the z-axis of the original coordinate system corresponding to the i-th joint (using) (represented by) and the x-axis of the original coordinate system corresponding to the i-th joint (using) The intersection of the two coordinate systems forms the origin of the original coordinate system corresponding to the i-th joint, where i is an integer. The direction is from point to .

[0035] Specifically, in the original first coordinate system Origin, original second coordinate system z-axis (using (Indicates) direction and original first coordinate system z-axis (using Once the direction is determined (represented by...), to ensure the original first coordinate system... x-axis (using) (Indicates) simultaneously perpendicular and And with and intersect, It needs to pass through the original first coordinate system The origin of the original first coordinate system. The origin and the original second coordinate system The origins are the same, and then according to the original second coordinate system... x-axis (using) (Indicates) simultaneously perpendicular and the original third coordinate system z-axis (using (indicate), determine The direction.

[0036] Original third coordinate system The origin is set at the connection between the third link 10 and the fourth joint 5. Similarly, in the original third coordinate system... The origin, the original fourth coordinate system z-axis (using (Indicates) direction and original third coordinate system z-axis (using Once the direction is determined, the original fourth coordinate system can be calculated. The origin and x-axis direction (using) express).

[0037] Original fifth coordinate system The origin is set at the connection point between the fifth link 12 and the sixth joint 7. Similarly, in the original fifth coordinate system... The origin, the original sixth coordinate system z-axis (using (Indicates) direction and the original fifth coordinate system z-axis (using Once the direction is determined, the original sixth coordinate system can be calculated. The origin and x-axis direction (using) express).

[0038] S2: Invert the six-axis robotic arm model to establish a virtual joint coordinate system diagram.

[0039] The reversal refers to taking the end of the original six-axis robotic arm as the starting end, that is, the link connecting the sixth joint 7 (sixth link 13) as a virtual base, and taking the connection end between the first joint 2 and the original base 1 as the torque output end. The torque is transmitted from the sixth link 13 through the sixth joint 7, the fifth joint 6, the fourth joint 5, the third joint 4, the second joint 3, and the first joint 2 to the original base 1.

[0040] A virtual coordinate system is set based on the inverted six-axis robotic arm model, and the first virtual coordinate system is set sequentially according to the joint connection order after inversion. Virtual Second Coordinate System Virtual third coordinate system Virtual fourth coordinate system Virtual Fifth Coordinate System Virtual sixth coordinate system Located in the virtual first coordinate system Virtual base coordinate system in front and the virtual sixth coordinate system The virtual seventh coordinate system in the rear The so-called "virtual first coordinate system" "Forward" refers to the direction towards the source of torque, set according to the torque transmission direction of the inverted six-axis robotic arm model, i.e., the direction closer to the virtual base. "Virtual Sixth Coordinate System" "Rear" refers to the torque transmission direction along the reversed six-axis robotic arm model.

[0041] Therefore, the coordinate systems of the six-axis robotic arm model before and after the reversal have the following correspondence: Virtual First Coordinate System Corresponding to the original sixth coordinate system Virtual second coordinate system Corresponding to the original fifth coordinate system Virtual third coordinate system Corresponding to the original fourth coordinate system Virtual fourth coordinate system Corresponding to the original third coordinate system Virtual fifth coordinate system Corresponding to the original second coordinate system Virtual sixth coordinate system Corresponding to the original first coordinate system Virtual seventh coordinate system Corresponding to the original base coordinate system .

[0042] like Figure 5 The diagram shown is a virtual joint coordinate system diagram. The method for determining the origin and coordinate axes of each virtual coordinate system in the virtual joint coordinate system diagram is as follows: The z-axis of each virtual coordinate system is the rotation axis of the corresponding joint, and the x-axis of the virtual coordinate system corresponding to the nth joint (using...) (represented by) the z-axis perpendicular to the virtual coordinate system corresponding to the nth joint (using) (represented) and the z-axis of the virtual coordinate system corresponding to the (n+1)th joint (using) (represented by), and the z-axis of the virtual coordinate system corresponding to the nth joint and the z-axis of the virtual coordinate system corresponding to the (n+1)th joint intersect simultaneously; the z-axis of the virtual coordinate systems corresponding to different joints do not coincide.

[0043] Specifically, the virtual first coordinate system The origin is located at the end of the sixth joint 7, in the virtual first coordinate system. Origin, Virtual Second Coordinate System z-axis (using (Indicates) direction and virtual first coordinate system z-axis (using Once the direction is determined (represented by...), to ensure the virtual first coordinate system... x-axis (using) (Indicates) simultaneously perpendicular and And with and intersect, Need to pass through the virtual first coordinate system The origin, therefore the virtual first coordinate system The origin and the virtual second coordinate system The origins are the same, and then based on the virtual second coordinate system... x-axis (using) (Indicates) simultaneously perpendicular and virtual third coordinate system z-axis (using (indicate), determine The direction.

[0044] Virtual Third Coordinate System The origin is set at the connection between the third link 10 and the fourth joint 5. Similarly, in the virtual third coordinate system... Origin, Virtual Fourth Coordinate System z-axis (using (Representation) Direction and virtual third coordinate system z-axis (using Once the direction is determined, the virtual fourth coordinate system can be calculated. The origin and x-axis direction (using) express).

[0045] Virtual Fifth Coordinate System The origin is set at the connection between the first joint 2 and the second joint 3, specifically at the connection between the first link 8 and the second joint 3. Similarly, in the virtual fifth coordinate system... Origin, Virtual Sixth Coordinate System z-axis ( (Representation) Direction and virtual fifth coordinate system z-axis (using Once the direction is determined, the virtual sixth coordinate system can be calculated. The origin and x-axis direction (using) express).

[0046] Virtual Seventh Coordinate System The origin is located at the connection between the first joint 2 and the original base 1, in the virtual seventh coordinate system. z-axis (using (represented) and x-axis With the virtual sixth coordinate system The z-axis and x-axis are parallel; virtual base coordinate system Origin, z-axis (using) (represented) and the x-axis relative to the virtual first coordinate system The origin, z-axis, and x-axis coincide.

[0047] S3: Solve for the virtual rotation angles of each joint in the reversed six-axis robotic arm model, and correlate the virtual rotation angles with the rotation angles of each joint in the original joint coordinate system diagram.

[0048] The method for solving the virtual rotation angle includes the following steps: S31: List the DH parameter table based on the original joint coordinate system diagram and the virtual joint coordinate system diagram.

[0049] The DH parameter table contains parameters , , , The specific values ​​in the initial state are listed in Table 1 below, which is a table of DH parameters based on the original joint coordinate system diagram, and Table 2 below, which is a table of DH parameters based on the virtual joint coordinate system diagram.

[0050] Table 1

[0051] Table 2:

[0052] In Tables 1 and 2, Indicates along Direction, from arrive The distance. Indicates along Direction, from Go to The angle. Indicates along Direction, from arrive The distance. Indicates circling Direction, from Go to The angle. Indicates along Direction, from arrive The distance. Indicates along Direction, from Go to The angle. Indicates along Direction, from arrive The distance. Indicates circling Direction, from Go to The angle. This represents the vertical length between the center of the first joint 2 and the center of the second joint 3, which is the length of the first link 8. This indicates the vertical length between the center of the second joint 3 and the center of the third joint 4, which is the length of the second link 9. This indicates the vertical length between the center of the third joint 4 and the center of the fourth joint 5, which is the length of the third link 10. This indicates the vertical length between the center of the fourth joint 5 and the center of the fifth joint 6, which is the length of the fourth link 11. This indicates the vertical length between the center of the fifth joint 6 and the center of the sixth joint 7, which is the length of the fifth link 12.

[0053] S32: Calculate the homogeneous transformation matrix of each joint of the original six-axis robotic arm model and the reversed six-axis robotic arm model according to the DH parameter table.

[0054] For the original joint coordinate system diagram, the following relationship exists as shown in equation (1): (1) In the formula, This represents the homogeneous transformation matrix of the original first coordinate system relative to the original base coordinate system. This represents the homogeneous transformation matrix of the original second coordinate system relative to the original first coordinate system. This represents the homogeneous transformation matrix of the original third coordinate system relative to the original second coordinate system. This represents the homogeneous transformation matrix of the original fourth coordinate system relative to the original third coordinate system. This represents the homogeneous transformation matrix of the original fifth coordinate system relative to the original fourth coordinate system. This represents the homogeneous transformation matrix of the original sixth coordinate system relative to the original fifth coordinate system. This represents the homogeneous transformation matrix of the original sixth coordinate system relative to the original base coordinate system.

[0055] Given the spatial coordinates of the original end effector 、 、 Euler angles , , (in, Let x be the x-axis coordinate of the origin of the original sixth coordinate system in the original base coordinate system. This represents the y-axis coordinate of the origin of the original sixth coordinate system in the original base coordinate system. This represents the z-axis coordinate of the origin of the original sixth coordinate system in the original base coordinate system. The angle by which the end effector rotates about the x-axis of the original base coordinate system. Let y be the angle by which the end effector rotates about the y-axis of the original base coordinate system. The angle of rotation of the end effector around the z-axis of the original base coordinate system can be calculated by equation (2). .

[0056] (2)

[0058] For the virtual joint coordinate system diagram, the following relationship exists as shown in equation (3): (3) In the formula, This represents the homogeneous transformation matrix of the virtual first coordinate system relative to the virtual base coordinate system. This represents the homogeneous transformation matrix of the virtual second coordinate system relative to the virtual first coordinate system. Let represent the homogeneous transformation matrix of the virtual third coordinate system relative to the virtual second coordinate system. This represents the homogeneous transformation matrix of the virtual fourth coordinate system relative to the virtual third coordinate system. This represents the homogeneous transformation matrix of the virtual fifth coordinate system relative to the virtual fourth coordinate system. This represents the homogeneous transformation matrix of the virtual sixth coordinate system relative to the virtual fifth coordinate system. This represents the homogeneous transformation matrix of the virtual seventh coordinate system relative to the virtual sixth coordinate system. This represents the homogeneous transformation matrix of the virtual sixth coordinate system relative to the virtual base coordinate system. This represents the homogeneous transformation matrix of the virtual seventh coordinate system relative to the virtual base coordinate system.

[0059] Based on the correspondence between the original coordinate system and the virtual coordinate system, we obtain the following equation (4).

[0060] (4).

[0061] because Since it is a known quantity, therefore It is also a known quantity.

[0062] Let both sides of the equation (4) be multiplied on the right. And calculate using formula (3) As shown in formula (5).

[0063] (5)

[0064] in Given the quantities, we have the following formula (5): (6) Learned Since it is also a known quantity, it can be calculated. .

[0065] Due to the rotation axes of the last three joints of the reversed six-axis robotic arm model ( , and The two axes intersect at a point to form a spherical wrist. Based on the inverse solution method of the spherical wrist, the following formula (7) is obtained: (7) in, for The fourth column, , , , It can be calculated using the following formulas (8) to (11).

[0066] (8), (9), (10) (11).

[0067] In the formula, To determine the rotation angle of the sixth joint 7 based on the calculation of the six-axis reversing robotic arm, To determine the rotation angle of the fifth joint (6) based on the calculation of the reverse six-axis robotic arm, To determine the rotation angle of the fourth joint 5 based on the calculation of the reverse six-axis robotic arm, The rotation angle of the third joint 4 is calculated based on the reverse six-axis robotic arm.

[0068] Combining formulas (7) to (11), we obtain the following formula.

[0069] (12)

[0071] According to formula (12), we can obtain information about , and Solving the system of three linear equations in three variables yields the following results. , and The value of .

[0072] The solution method for this system of three linear equations includes the following steps: S321: Order , , , The following system of transformed equations is obtained: (13) S322: Solving the deformed system of equations shown in formula (13) yields the following equation (14).

[0073] (14)

[0074] S323: According to formula (14), we get: (15) Substitute equation (15) into equation (13) to solve. and We obtain the following equation (16).

[0075] (16)

[0076] S33: According to , and Solve , and ,in, This indicates the rotation angle of the second joint 3, calculated based on the inverted six-axis robotic arm model. This represents the rotation angle of the first joint 2, calculated based on the inverted six-axis robotic arm model.

[0077] The specific solution steps are as follows: Let equation (4) be multiplied on both sides. Thus, we obtain the following equation (17).

[0078] (17)

[0079] In the formula, Let represent the homogeneous transformation matrix of the virtual sixth coordinate system relative to the virtual third coordinate system. In equation (17), the angle and homogeneous transformation matrix All are known numbers, therefore Also a known number, it can be obtained from the fact that if matrices are equal, then corresponding positions in the matrices are equal.

[0080] (18)

[0081] Solving formula (18) yields As shown in the following formula.

[0082] (19)

[0083] In the formula, Representation matrix The value in the second row and second column. Representation matrix The value in the second row and first column.

[0084] Similarly, solve and Let each of the equal signs in equation (17) be multiplied on the right. We get the following equation (20).

[0085] (20)

[0086] in, This represents the homogeneous transformation matrix of the virtual fifth coordinate system relative to the virtual third coordinate system. It can be calculated using the following formula (21).

[0087] (twenty one)

[0088] In equation (20), angle , and homogeneous transformation matrix All are known numbers, therefore , Also known numbers, since the corresponding positions of the matrices are equal, we can obtain equations (22) and (23).

[0089] (twenty two) (twenty three) According to equation (22), we get As shown in equation (24), we obtain equation (23) as follows. As shown in equation (25) below.

[0090] (twenty four) (25) in, Representation matrix The value in the second row and first column. Representation matrix The value in the second row and second column. Representation matrix The value in the first row and third column, Representation matrix The value in the third row and third column.

[0091] The angles calculated above Corresponding to the original structure Because the coordinate axes of each virtual coordinate system deviate from the original coordinate system after the six-axis robotic arm reverses, this angle value is not the final value and needs to be used as the initial angle for iterative calculation. That is, the initial rotation angle of each joint. .

[0092] The following steps S4~S6 are as follows: Figure 2 The iterative process is shown.

[0093] S4: Calculate the homogeneous transformation matrix of the end joint based on the rotation angles of each joint in the original joint coordinate system diagram obtained from the solution. As shown in equation (26).

[0094] (26)

[0095] In the first calculation, all matrices on the right side of equation (26) are calculated using the initial angles of the above iteration. In subsequent iterations, the updated rotation angles of each joint are used.

[0096] S5: Based on the homogeneous transformation matrix Calculate the position and attitude error vector of the end effector of a six-axis robotic arm. Position error and attitude error .

[0097] For ease of description, the formula (2) will be... The results are simplified as follows: = (27) In formula (26) The results are simplified as follows: (28) because and All of these are known matrices. Based on the fact that the corresponding positions of the matrices are equal, we can obtain the values ​​of the parameters on the right side of the equal signs in formulas (27) and (28).

[0098] but (29)

[0099] In equation (29), , , , , , We obtain it through equations (30) to (35).

[0100] (30) (31) (32) (33) (34) (35) Position error and attitude error It is obtained through the following equations (36) and (37).

[0101] (36) (37) S6: If the position error and attitude error If all values ​​are less than the set threshold, the calculation ends; otherwise, the rotation angle of each joint is updated using an iterative formula, and steps S4 to S6 are repeated.

[0102] The iterative formula is as follows: (38) In equation (38), The joint rotation angles used to calculate each parameter in step S5, It is the pseudo-inverse of the Jacobian matrix. This refers to the updated rotation angles of each joint.

[0103] Simulation results show that the inverse algorithm described in this invention can reduce the number of iterations to less than 20, thereby greatly reducing the computational load on the computer.

[0104] In the description of this invention, the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance. It should be noted in the description of this invention that, unless otherwise explicitly specified and limited, the terms "connected" or "linked" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium.

[0105] In this specification, the illustrative expressions of the terms do not necessarily refer to the same embodiments. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments.

[0106] Based on the above-described preferred embodiments of the present invention, and through the foregoing description, those skilled in the art can make various changes and modifications without departing from the inventive concept. The technical scope of this invention is not limited to the contents of the specification, but must be determined according to the scope of the claims.

[0107] Symbol explanation: The original first coordinate system.

[0108] The original second coordinate system.

[0109] The original third coordinate system.

[0110] The original fourth coordinate system.

[0111] The original fifth coordinate system.

[0112] The original sixth coordinate system.

[0113] , original base coordinate system.

[0114] , the z-axis of the original coordinate system corresponding to the nth joint.

[0115] The z-axis of the original coordinate system corresponding to the (n+1)th joint.

[0116] , the x-axis of the original coordinate system corresponding to the nth joint.

[0117] The original i-th coordinate system The z-axis.

[0118] The original (i-1)th coordinate system The z-axis.

[0119] The original (i+1)th coordinate system The z-axis.

[0120] , where x is the x-axis of the original coordinate system corresponding to the i-th joint.

[0121] The original (i-1)th coordinate system x-axis Original first coordinate system The z-axis.

[0122] Original second coordinate system The z-axis.

[0123] The original third coordinate system The z-axis.

[0124] The original fourth coordinate system The z-axis.

[0125] The original fifth coordinate system The z-axis.

[0126] Original first coordinate system The x-axis.

[0127] Original second coordinate system The x-axis.

[0128] The original fourth coordinate system The x-axis.

[0129] Virtual first coordinate system.

[0130] A virtual second coordinate system.

[0131] A virtual third coordinate system.

[0132] A virtual fourth coordinate system.

[0133] A virtual fifth coordinate system.

[0134] Virtual sixth coordinate system.

[0135] Virtual base coordinate system.

[0136] Virtual seventh coordinate system.

[0137] The z-axis of the virtual coordinate system corresponding to the nth joint.

[0138] The z-axis of the virtual coordinate system corresponding to the (n+1)th joint.

[0139] Virtual base coordinate system The z-axis.

[0140] Virtual first coordinate system The z-axis.

[0141] Virtual second coordinate system The z-axis.

[0142] Virtual third coordinate system The z-axis.

[0143] Virtual fourth coordinate system The z-axis.

[0144] Virtual fifth coordinate system The z-axis.

[0145] Virtual sixth coordinate system The z-axis.

[0146] Virtual seventh coordinate system The z-axis.

[0147] , the x-axis of the virtual coordinate system corresponding to the nth joint.

[0148] Virtual first coordinate system The x-axis.

[0149] Virtual second coordinate system The x-axis.

[0150] Virtual fourth coordinate system The x-axis.

[0151] Virtual sixth coordinate system The x-axis.

[0152] Virtual sixth coordinate system The origin is in the virtual base coordinate system of x Axial coordinates Virtual sixth coordinate system The origin is in the virtual base coordinate system of y Axial coordinates Virtual sixth coordinate system The origin is in the virtual base coordinate system of z Axial coordinates ,along Direction, from arrive The distance.

[0153] ,along Direction, from Go to The angle.

[0154] ,along Direction, from arrive The distance.

[0155] , around Direction, from Go to The angle.

[0156] ,along Direction, from arrive The distance.

[0157] ,along Direction, from Go to The angle.

[0158] ,along Direction, from arrive The distance.

[0159] , around Direction, from Go to The angle.

[0160] A1 is the vertical length between the center of the first joint and the center of the second joint, which is the length of the first link.

[0161] A2, the vertical length between the center of the second joint and the center of the third joint, i.e., the length of the second link.

[0162] A3 is the vertical length between the center of the third joint and the center of the fourth joint, which is the length of the third link.

[0163] A4, the vertical length between the center of the fourth joint and the center of the fifth joint, i.e., the length of the fourth link.

[0164] A5 is the vertical length between the center of the fifth joint and the center of the sixth joint, which is the length of the fifth link.

[0165] , the homogeneous transformation matrix of the original first coordinate system relative to the original base coordinate system.

[0166] The homogeneous transformation matrix of the original second coordinate system relative to the original first coordinate system.

[0167] The homogeneous transformation matrix of the original third coordinate system relative to the original second coordinate system.

[0168] The homogeneous transformation matrix of the original fourth coordinate system relative to the original third coordinate system.

[0169] The homogeneous transformation matrix of the original fifth coordinate system relative to the original fourth coordinate system.

[0170] The homogeneous transformation matrix of the original sixth coordinate system relative to the original fifth coordinate system.

[0171] The homogeneous transformation matrix of the original sixth coordinate system relative to the original base coordinate system.

[0172] The homogeneous transformation matrix of the virtual first coordinate system relative to the virtual base coordinate system.

[0173] The homogeneous transformation matrix of the virtual second coordinate system relative to the virtual first coordinate system.

[0174] The homogeneous transformation matrix of the virtual third coordinate system relative to the virtual second coordinate system.

[0175] The homogeneous transformation matrix of the virtual fourth coordinate system relative to the virtual third coordinate system.

[0176] The homogeneous transformation matrix of the virtual fifth coordinate system relative to the virtual fourth coordinate system.

[0177] The homogeneous transformation matrix of the virtual sixth coordinate system relative to the virtual fifth coordinate system.

[0178] The homogeneous transformation matrix of the virtual seventh coordinate system relative to the virtual sixth coordinate system.

[0179] The homogeneous transformation matrix of the virtual sixth coordinate system relative to the virtual base coordinate system.

[0180] The homogeneous transformation matrix of the virtual seventh coordinate system relative to the virtual base coordinate system.

[0181] The x-axis coordinate of the origin of the original sixth coordinate system in the original base coordinate system.

[0182] The y-axis coordinate of the origin of the original sixth coordinate system in the original base coordinate system.

[0183] The z-axis coordinate of the origin of the original sixth coordinate system in the original base coordinate system.

[0184] The angle by which the end effector rotates about the x-axis of the original base coordinate system.

[0185] The angle by which the end effector rotates around the y-axis of the original base coordinate system.

[0186] The angle by which the end effector rotates around the z-axis of the original base coordinate system.

[0187] The rotation angle of the sixth joint is calculated based on the reverse six-axis robotic arm.

[0188] The rotation angle of the fifth joint is calculated based on the reverse six-axis robotic arm.

[0189] The rotation angle of the fourth joint is calculated based on the reverse six-axis robotic arm.

[0190] The rotation angle of the third joint is calculated based on the reverse six-axis robotic arm.

[0191] The rotation angle of the second joint is calculated based on the reversed six-axis robotic arm model.

[0192] The rotation angle of the first joint is calculated based on the reversed six-axis robotic arm model.

[0193] The homogeneous transformation matrix of the end joint is calculated based on the rotation angles of each joint obtained through iterative solving.

[0194] Position and attitude error vector Positional error.

[0195] , attitude error.

[0196] The error of the end joint position relative to the target position in the x-axis direction.

[0197] The error of the end joint position relative to the target position in the y-axis direction.

[0198] The error of the end joint position relative to the target position in the z-axis direction.

[0199] , , The attitude error of the end joint relative to the target attitude.

[0200] The rotation angles of each joint used in each parameter calculation step S5 are then calculated.

[0201] , the pseudo-inverse of the Jacobian matrix.

[0202] The updated rotation angles of each joint.

Claims

1. An inverse kinematics algorithm for a six-DOF humanoid robot arm, characterized in that, Includes the following steps: S1: Create a six-axis robotic arm model and draw the original joint coordinate system diagram; S2: Invert the six-axis robotic arm model and establish a virtual joint coordinate system diagram; S3: Solve for the virtual rotation angles of each joint in the reversed six-axis robotic arm model, and match the virtual rotation angles with the rotation angles of each joint in the original joint coordinate system diagram; S4: Calculate the homogeneous transformation matrix of the end joint based on the rotation angles of each joint in the original joint coordinate system diagram obtained from the solution. ; S5: Based on the homogeneous transformation matrix Calculate the position and attitude error vector of the end effector of a six-axis robotic arm. Position error and attitude error ; S6: If the position error and attitude error If all values ​​are less than the set threshold, the calculation ends; otherwise, the rotation angle of each joint is updated using an iterative formula, and steps S4 to S6 are repeated.

2. The inverse kinematics algorithm for a six-degree-of-freedom humanoid robot arm according to claim 1, characterized in that: In step S2, the method for establishing the virtual joint coordinate system diagram is as follows: A virtual base coordinate system is established that coincides with the origin of the end-joint coordinate system in the original joint coordinate system diagram. A virtual tool coordinate system that coincides with the origin of the base coordinate system in the original joint coordinate system diagram. From the virtual base coordinate system To virtual tool coordinate system A virtual joint coordinate system diagram is established along the joint connection sequence of the six-axis robotic arm.

3. The inverse kinematics algorithm for a six-degree-of-freedom humanoid robot arm according to claim 1, characterized in that: In step S3, the method for solving the virtual rotation angles of each joint of the six-axis robotic arm model after reversal includes the following steps: S31: Based on the original joint coordinate system diagram and the virtual joint coordinate system diagram, list the DH parameter table; S32: Calculate the homogeneous transformation matrix of each joint of the original six-axis robotic arm model and the inverted six-axis robotic arm model according to the DH parameter table. , , , , , According to the formula and formula ,get Solving the system of equations yields , and ,in, This represents the homogeneous transformation matrix of the virtual first coordinate system relative to the virtual base coordinate system after the six-axis robotic arm model is reversed. This represents the homogeneous transformation matrix of the virtual second coordinate system relative to the virtual first coordinate system after the six-axis robotic arm model is reversed. This represents the homogeneous transformation matrix of the virtual third coordinate system relative to the virtual second coordinate system after the six-axis robotic arm model is reversed. This represents the homogeneous transformation matrix of the virtual sixth coordinate system relative to the virtual base coordinate system after the six-axis robotic arm model is inverted. This represents the homogeneous transformation matrix of the virtual seventh coordinate system relative to the virtual sixth coordinate system after the six-axis robotic arm model is reversed. This represents the homogeneous transformation matrix of the original sixth coordinate system relative to the original base coordinate system in the original six-axis robotic arm model. This represents the rotation angle of the sixth joint, calculated based on the inverted six-axis robotic arm model. This represents the rotation angle of the fifth joint, calculated based on the inverted six-axis robotic arm model. This represents the rotation angle of the fourth joint, calculated based on the inverted six-axis robotic arm model. This represents the vertical length between the center of the second joint and the center of the third joint. This represents the vertical length between the center of the third joint and the center of the fourth joint. This represents the vertical length between the center of the fourth joint and the center of the fifth joint. This represents the vertical length between the center of the fifth joint and the center of the sixth joint. The homogeneous transformation matrix of the virtual fourth coordinate system relative to the virtual third coordinate system The fourth column, Represents a virtual sixth coordinate system The origin is in the virtual base coordinate system x-axis coordinates Represents a virtual sixth coordinate system The origin is in the virtual base coordinate system The y-axis coordinate, Represents a virtual sixth coordinate system The origin is in the virtual base coordinate system The z-axis coordinate. S33: According to , and Solve , and ,in, This represents the rotation angle of the third joint, calculated based on the inverted six-axis robotic arm model. This represents the rotation angle of the second joint, calculated based on the inverted six-axis robotic arm model. This represents the rotation angle of the first joint, calculated based on the inverted six-axis robotic arm model.

4. The inverse kinematics algorithm for a six-degree-of-freedom humanoid robot arm according to claim 3, characterized in that: In step S32, the solution method for the system of equations is as follows: S321: Order , , , The following system of transformed equations is obtained: ； S322: Solving the aforementioned system of deformed equations yields... ; S323: Solve , , .

5. The inverse kinematics algorithm for a six-DOF humanoid robot arm according to claim 3, characterized in that, The solution steps for step S33 are as follows: According to the formula ,get ; According to the formula ,get , ,in, This represents the value in the second row and second column of the homogeneous transformation matrix between the virtual sixth coordinate system and the virtual third coordinate system after the six-axis robotic arm model has been reversed. This represents the value in the second row and first column of the homogeneous transformation matrix of the virtual fifth coordinate system relative to the virtual third coordinate system after the six-axis robotic arm model is reversed.

6. The inverse kinematics algorithm for a six-degree-of-freedom humanoid robot arm according to claim 1, characterized in that: The iterative formula mentioned in step S6 is: ,in, The joint rotation angles used to calculate each parameter in step S5, It is the pseudo-inverse of the Jacobian matrix. This refers to the updated rotation angles of each joint.

7. The inverse kinematics algorithm for a six-degree-of-freedom humanoid robot arm according to claim 1, characterized in that: The six-axis robotic arm model includes an original first coordinate system set sequentially according to the joint connection order. Original second coordinate system Original third coordinate system Original fourth coordinate system Original fifth coordinate system Original sixth coordinate system and located in the original first coordinate system The original base coordinate system in front The method for determining the origin and coordinate axes of each original coordinate system in the original joint coordinate system diagram is as follows: Original first coordinate system The origin is located at the connection between the first joint and the second joint; Original base coordinate system The origin is located at the connection between the first joint and the original base; and the original base coordinate system The z-axis and x-axis are in relation to the original first coordinate system. The z-axis and x-axis are parallel to each other; The z-axis of each original coordinate system is the rotation axis of the corresponding joint. The x-axis of the original coordinate system corresponding to the nth joint is perpendicular to the z-axis of the original coordinate system corresponding to the nth joint and the z-axis of the original coordinate system corresponding to the (n+1)th joint, and intersects the z-axis of the original coordinate system corresponding to the nth joint and the (n+1)th joint simultaneously. The z-axis of the original coordinate systems corresponding to different joints do not coincide. Wherein, n is an odd number. The z-axis and x-axis of the original coordinate system corresponding to the i-th joint intersect to form the origin of the original coordinate system corresponding to the i-th joint, where i is an integer.

8. The inverse kinematics algorithm for a six-degree-of-freedom humanoid robot arm according to claim 7, characterized in that: The reversed six-axis robotic arm model includes a virtual first coordinate system set up sequentially according to the reversed joint connection order. Virtual Second Coordinate System Virtual third coordinate system Virtual fourth coordinate system Virtual Fifth Coordinate System Virtual sixth coordinate system Located in the virtual first coordinate system Virtual base coordinate system in front and the virtual sixth coordinate system The virtual seventh coordinate system in the rear ; Virtual First Coordinate System The origin is located at the end of the sixth joint; virtual sixth coordinate system The origin is located at the connection between the first and second joints; virtual seventh coordinate system The origin is located at the connection between the first joint and the original base, in the virtual seventh coordinate system. The z-axis and x-axis are in relation to the virtual sixth coordinate system. The z-axis and x-axis are parallel; virtual base coordinate system The origin, z-axis, and x-axis are relative to the virtual first coordinate system. The origin, z-axis, and x-axis coincide.

9. The inverse kinematics algorithm for a six-degree-of-freedom humanoid robot arm according to claim 8, characterized in that: The x-axis of the virtual coordinate system corresponding to the nth joint is perpendicular to the z-axis of the virtual coordinate system corresponding to the nth joint and the z-axis of the virtual coordinate system corresponding to the (n+1)th joint, and intersects with the z-axis of the virtual coordinate system corresponding to the nth joint and the (n+1)th joint simultaneously; the z-axis of the virtual coordinate systems corresponding to different joints do not coincide.

Images