Inverse kinematics solving method for six-degree-of-freedom bias type series mechanical arm based on dual quaternion

Abstract

The invention discloses an inverse kinematics solving method for a six-degree-of-freedom bias type series mechanical arm based on dual quaternions, and belongs to the technical field of mechanical arm design. The method comprises the following steps that the six-degree-of-freedom offset type series mechanical arms are classified, and the six-degree-of-freedom offset type series mechanical arms comprise the I-type six-degree-of-freedom offset type series mechanical arms and the II-type six-degree-of-freedom offset type series mechanical arms; respectively establishing a reference coordinate system and a tail end coordinate system, and defining the connecting rod length of each joint of the mechanical arm, the position coordinate of each joint, the unit direction vector of each joint and the line distance of each joint to obtain the Pluecker coordinate of each joint; the I-class six-degree-of-freedom bias type series mechanical arm and the II-class six-degree-of-freedom bias type series mechanical arm are respectively subjected to inverse kinematics analysis and solution; and verifying the solving method. According to the method, efficient solving of inverse kinematics is achieved, the real-time requirement for the inverse kinematics solving speed in an industrial scene is met, and the control performance and application efficiency of the mechanical arm in a high-precision operation scene are improved.

Description

Technical Field

[0001] This invention relates to a method for solving the inverse kinematics of a six-degree-of-freedom biased serial robotic arm based on dual quaternions, belonging to the field of robotic arm design technology. Background Technology

[0002] Six-degree-of-freedom serial robotic arms are common mechanical structures in industrial applications. Due to their ability to move flexibly in any position within the workspace, they are widely used in industrial scenarios such as welding, assembly, and material handling.

[0003] To meet the demands of high-precision operations, these robotic arms typically require analytical calculation of joint angles for precise control. A six-DOF tandem robotic arm with three consecutive parallel joints is a commonly used design in the industry. However, existing technologies have significant shortcomings when performing inverse kinematics solutions for a six-DOF tandem robotic arm with three consecutive parallel joints. For example, the inverse kinematics solution algorithms for similar robotic arms disclosed in Reference 1 (Liu Yang, Li Mengfei, Feng Weichao, et al. A kinematic reconstruction method for single joint faults of SSRMS configuration robotic arms: 202410133265.2 [P]. 2024-06-14) and Reference 2 (Selig J M. Robot Kinematics and Flags [C]. Geometric Algebra with Applications in Science and Engineering, 2001. Springer) involve a large number of algebraic operations, resulting in excessively long solution times and low computational efficiency. These algorithms are difficult to meet the strict requirements for inverse kinematics solution speed in real-time control scenarios of robotic arms, thus restricting their practical application effectiveness in high-time-efficiency industrial scenarios. Summary of the Invention

[0004] To address the problems existing in the background technology, this invention provides a method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions, the method comprising the following steps:

[0006] S1: Classification of six-DOF biased serial robotic arms, including:

[0007] A type I six-degree-of-freedom offset serial robotic arm in which the joint axes of the second joint, the third joint, and the fourth joint are continuously parallel to each other.

[0008] A type II six-degree-of-freedom offset serial robotic arm in which the joint axes of the third joint, the fourth joint, and the fifth joint are continuously parallel to each other.

[0009] S2: Establish separate reference coordinate systems for the Type I six-DOF offset serial manipulator and the Type II six-DOF offset serial manipulator. and the end coordinate system Define the link length of each joint of the robotic arm as... The position coordinates of each joint in the reference coordinate system are: The unit direction vector of each joint is The linear spacing of each joint is Then the Plück coordinates of each joint, i.e., the joint axes, are... ;

[0010] S3: Inverse kinematics analysis and solution of a type I six-DOF biased serial manipulator;

[0011] S3 includes the following steps:

[0012] S301: Forward kinematic analysis of a Class I six-DOF biased serial robotic arm:

[0013] Calculating the end-effector coordinate system during the rotational motion of a type I six-DOF biased tandem robotic arm using dual quaternions Relative reference coordinate system position :

[0014] (1)

[0015] In formula (1):

[0016] ,in: These are the pose vector parameters;

[0017] These are the rotation angle values ​​for each joint;

[0018] The end-effector coordinate system of a Class I six-DOF biased serial robotic arm in its zero-position configuration Relative reference coordinate system The pose; where: , ;

[0019] Let be the dual quaternions for each joint rotation, where: As a dual unit;

[0020] S302: Inverse kinematics derivation of a type I six-DOF biased serial manipulator.

[0021] S302 includes the following steps:

[0022] S30201: Solve for the rotation angle of the first joint of a type I six-DOF offset tandem robotic arm. The rotation angle value of the fifth joint and the rotation angle value of the sixth joint ;

[0023] S30201 includes the following steps:

[0024] S3020101: The second joint dual quaternion on the right side of retained form (1) Third joint dual quaternion and the fourth joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0025] (2)

[0026] In formula (2):

[0027] dual quaternions The conjugate dual quaternions of , and the following relation exists:

[0028] (3)

[0029] In formula (3):

[0030] and Represents any number;

[0031] and Represents an arbitrary three-dimensional vector;

[0032] This represents a three-dimensional vector [0 0 0] whose elements are all 0;

[0033] Denotes the Euclidean norm;

[0034] S3020102: Assume there exists a point that simultaneously passes through the location of the first joint. and the location of the second joint And the direction vector of the joint axis of the second joint Vertical plane Then we can obtain a plane. To the reference coordinate system vertical distance from the origin for:

[0035] (4)

[0036] Then the plane The dual quaternion is:

[0037] (5)

[0038] S3020103: Plane When rotating around the fourth joint, the third joint, and the second joint in sequence, according to the property of plane rotation around the axis in equation (2), we can obtain:

[0039] (6)

[0040] In equation (6):

[0041] dual quaternions Another conjugate dual quaternion;

[0042] Due to the plane The plane is always perpendicular to the joint axes of the second, third, and fourth joints. After rotation around these three joints, the plane... The relationship remains unchanged, meaning the following expression exists:

[0043] (7)

[0044] Combining equations (6) and (7), we can obtain:

[0045] (8)

[0046] S3020104: The fifth joint dual quaternion on the right side of retained expression (8) The quaternion of the sixth joint The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0047] (9)

[0048] In equation (9):

[0049] Describing dual quaternions Another conjugate dual quaternion;

[0050] flat After rotating around the fifth joint, a plane is obtained. ,flat After rotating around the sixth joint, a plane is obtained. ,flat With plane The following relationship exists:

[0051] (10)

[0052] flat After rotating around the sixth joint, a plane is obtained. During the process, the joint axis of the sixth joint has a dual quaternion. With plane and plane The intersection points are the same as the intersection points. The intersection point is obtained by using the Clifford algebraic cross product of the joint axis and the plane. coordinate:

[0053] (11)

[0054] In equation (11):

[0055] This is the outer product of Clifford's algebra;

[0056] For plane The normal vector;

[0057] For plane Distance to the origin of the coordinate system;

[0058] Joint axis The direction vector;

[0059] Joint axis Line spacing;

[0060] Then we can obtain the dual quaternion of the fifth joint:

[0061] (12)

[0062] By combining equations (5) and (12), we can obtain:

[0063] (13)

[0064] S3020105: Dualize the joint axis of the sixth joint with a quaternion. Combining equations (11) and (13), we obtain the joint axis dual quaternion of the sixth joint. With plane intersection for:

[0065] (14)

[0066] In equation (14):

[0067] Unused items;

[0068] flat The plane is obtained by rigid body transformation on the left side of equation (9). :

[0069] (15)

[0070] Combining equations (11) and (15) yields the joint axis dual quaternion of the sixth joint. With plane intersection :

[0071] (16)

[0072] In equation (16):

[0073] ;

[0074] ;

[0075] in: , , , , , , ;

[0076] S3020106: It can be seen that the intersection point... Intersection If the point is the same, then the terms in equations (14) and (16) are equal, and we can obtain:

[0077] (17)

[0078] (18)

[0079] Combining equations (17) and (18), we can obtain the rotation angle value containing only the first joint. Relationship:

[0080] (19)

[0081] In equation (19):

[0082] , , ;

[0083] Therefore, the rotation angle value of the first joint can be obtained. Two solutions:

[0084] (20)

[0085] Substituting equation (20) into equation (17) yields the rotation angle value of the fifth joint. Two solutions:

[0086] (twenty one)

[0087] S3020107: Combining equations (5) and (10), we can obtain the plane. The dual quaternion is:

[0088] (twenty two)

[0089] The rotation angle value of the first joint Substituting into equation (15) yields the plane The dual quaternion is:

[0090] (twenty three)

[0091] According to equation (9), the plane and plane If all terms in the equations are equal, then by combining equations (22) and (23), we can obtain:

[0092] (twenty four)

[0093] therefore, When it is time, we can obtain:

[0094] (25)

[0095] S30202: Solve for the rotation angle of the second joint of a type I six-DOF offset tandem robotic arm. The rotation angle value of the third joint and the rotation angle value of the fourth joint .

[0096] S30202 includes the following steps:

[0097] S3020201: The rotation angle value of the first joint. The rotation angle value of the fifth joint and the rotation angle value of the sixth joint Substitute into formula (2):

[0098] (26)

[0099] S3020202: Dual quaternion of the second joint Third joint dual quaternion and the fourth joint dual quaternion Substituting into equation (26), we get:

[0100] (27)

[0101] (28)

[0102] (29)

[0103] (30)

[0104] By combining equations (27) and (28), we can obtain:

[0105] (31)

[0106] By combining equations (27) and (30), we can obtain:

[0107] (32)

[0108] (33)

[0109] Squaring both sides of equations (32) and (33) and summing them:

[0110] (34)

[0111] Therefore, the rotation angle value of the third joint for:

[0112] (35)

[0113] S3020203: Use the sum-to-product formula in equations (32) and (33) to expand trigonometric functions:

[0114] (36)

[0115] The rotation angle value of the second joint can be obtained. for:

[0116] (37)

[0117] By combining equations (31), (35), and (37), the rotation angle value of the fourth joint can be obtained. for:

[0118] (38)

[0119] S4: Inverse kinematics analysis and solution of a type II six-degree-of-freedom offset serial manipulator;

[0120] S4 includes the following steps:

[0121] S401: Forward kinematic analysis of a type II six-DOF biased serial robotic arm:

[0122] Calculating the end-effector coordinate system during the rotational motion of a type II six-DOF biased tandem robotic arm using dual quaternions Relative reference coordinate system position :

[0123] (39)

[0124] In equation (39):

[0125] The end-effector coordinate system of a Type II six-DOF offset serial robotic arm in its zero-position configuration Relative reference coordinate system The position;

[0126] For each joint rotation, there are dual quaternions;

[0127] S402: Inverse kinematics derivation of a type II six-DOF biased serial manipulator.

[0128] S402 includes the following steps:

[0129] S40201: Solve for the rotation angle of the first joint of a type II six-DOF offset tandem robotic arm. The rotation angle value of the second joint and the rotation angle value of the sixth joint. ;

[0130] S40201 includes the following steps:

[0131] S4020101: The third joint dual quaternion on the right side of the retained expression (39) The fourth joint dual quaternion and the fifth joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0132] (40)

[0133] S4020102: Assume there exists a point that simultaneously passes through the location of the first joint. and the location of the second joint And the direction vector of the joint axis of the third joint Vertical plane Then we can obtain a plane. To the reference coordinate system vertical distance from the origin for:

[0134] (41)

[0135] Then the plane The dual quaternion is:

[0136] (42)

[0137] S4020103: Plane When rotating around the fifth, fourth, and third joints in sequence, according to the property of planar rotation around an axis in equation (40), we can obtain:

[0138] (43)

[0139] Due to the plane The plane is always perpendicular to the joint axes of the third, fourth, and fifth joints. After rotation around these three joints, the plane... The relationship remains unchanged, meaning the following expression exists:

[0140] (44)

[0141] Combining equations (43) and (44), we obtain:

[0142] (45)

[0143] S4020104: The first joint dual quaternion in equation (45) Second joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, while the remaining terms are retained on the right side, resulting in:

[0144] (46)

[0145] flat After rotating around the second joint, a plane is obtained. ,flat After rotating around the first joint, a plane is obtained. ,flat With plane The following relationship exists:

[0146] (47)

[0147] According to the plane Second joint dual quaternion Obtain a plane for:

[0148] (48)

[0149] S4020105: Plane After rotating around the first joint, a plane is obtained. During the process, the joint axis of the first joint is a dual quaternion. With plane and plane The intersection points are the same as the intersection points. The intersection point is obtained by using the Clifford algebraic cross product of the joint axis and the plane. coordinate:

[0150] (49)

[0151] flat The plane is obtained by rigid body transformation on the right side of equation (46). :

[0152] (50)

[0153] Obtain the joint axis dual quaternion of the first joint. With plane intersection :

[0154] (51)

[0155] In equation (51):

[0156] ;

[0157] ;

[0158] S4020106: It can be seen that the plane... With plane If they are on the same plane, then the joint axis of the first joint has a dual quaternion. Since the intersection point with the two planes is the same point, and the terms in equations (49) and (51) are equal, we can obtain:

[0159] (52)

[0160] (53)

[0161] Combining equations (52) and (53), we can obtain the rotation angle value containing only the sixth joint. Relationship:

[0162] (54)

[0163] In equation (54):

[0164] ;

[0165] ;

[0166] ;

[0167] Therefore, the rotation angle value of the sixth joint can be obtained. Two solutions:

[0168] (55)

[0169] Substituting equation (55) into equation (52) yields the rotation angle value of the second joint. Two solutions:

[0170] (56)

[0171] S4020107: According to equation (47), the plane can be obtained The dual quaternion is:

[0172] (57)

[0173] According to equation (50), the plane can be obtained The dual quaternion is:

[0174] (58)

[0175] Due to the plane With plane Since they are on the same plane, the terms of equations (57) and (58) are equal, and the rotation angle value of the first joint can be obtained. for:

[0176] (59)

[0177] S40202: Solve for the rotation angle of the third joint of a type II six-DOF offset tandem robotic arm. The rotation angle value of the fourth joint and the rotation angle value of the fifth joint .

[0178] S40202 includes the following steps:

[0179] S4020201: According to equation (40), we can obtain:

[0180] (60)

[0181] S4020202: The rotation angle value of the first joint. The rotation angle value of the second joint and the rotation angle value of the sixth joint Substituting into equation (60), we get:

[0182] (61)

[0183] (62)

[0184] (63)

[0185] (64)

[0186] Combining equations (61) and (62), we can obtain:

[0187] (65)

[0188] By combining equations (61) and (64), we can obtain:

[0189] (66)

[0190] (67)

[0191] Squaring both sides of equations (66) and (67) and summing them:

[0192] (68)

[0193] Therefore, the rotation angle value of the fourth joint for:

[0194] (69)

[0195] S4020203: Use the sum-to-product formula in equations (66) and (67) to expand trigonometric functions:

[0196] (70)

[0197] The rotation angle value of the third joint can be obtained. for:

[0198] (71)

[0199] By combining equations (65), (69), and (71), the rotation angle value of the fifth joint can be obtained. for:

[0200] (72)

[0201] S5: Verify the solution method.

[0202] Compared with the prior art, the beneficial effects of the present invention are:

[0203] This invention employs an analytical method combining dual quaternions and Clifford algebras, taking into account the characteristics of the robotic arm configuration (positional classification of three consecutive parallel joints), to perform inverse kinematics modeling for both Type I and Type II six-DOF offset serial robotic arms. Dual quaternions are used to express planar rigid body transformations and joint rotation characteristics, while Clifford algebraic outer product operations are used to accurately solve for the coordinates of the intersection points of the line and the plane. The constraint relationships and analytical expressions between joint angles are derived, achieving efficient inverse kinematics solutions. Compared to existing technologies, taking the solution process of a Type I six-DOF offset serial robotic arm as an example, this invention improves the computational efficiency by 43.17% compared to the inverse kinematics solution method in Reference 1 and by 73.27% compared to the inverse kinematics solution method in Reference 2. This significantly meets the real-time requirements for inverse kinematics solution speed in industrial scenarios and enhances the control performance and application efficiency of robotic arms in high-precision operation scenarios. Attached Figure Description

[0204] Figure 1 This is the zero-position configuration diagram of the Type I six-degree-of-freedom offset serial robotic arm of the present invention;

[0205] Figure 2 This is the zero-position configuration diagram of the Type II six-degree-of-freedom offset serial robotic arm of the present invention;

[0206] Figure 3 The planar aspect of the Type I six-degree-of-freedom offset serial robotic arm of the present invention A schematic diagram;

[0207] Figure 4 The planar aspect of the Type I six-degree-of-freedom offset serial robotic arm of the present invention With plane A schematic diagram;

[0208] Figure 5 This is a schematic diagram of the inverse kinematics solution sequence for the Type I six-degree-of-freedom biased serial manipulator of the present invention;

[0209] Figure 6 This is a schematic diagram of the inverse kinematics solution sequence for the Type II six-degree-of-freedom offset serial manipulator of the present invention;

[0210] Figure 7This is a comparison chart of the computation time of the inverse kinematics solution method of this invention with other inverse kinematics solution methods. Detailed Implementation

[0211] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the invention, not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0212] A method for solving the inverse kinematics of a six-DOF biased serial manipulator based on dual quaternions, the method comprising the following steps:

[0213] S1: Based on the positions of three consecutive parallel joints within the six joints of the robotic arm, six-degree-of-freedom offset serial robotic arms are classified, including:

[0214] A type I six-DOF offset serial robotic arm in which the joint axes of the second, third, and fourth joints are continuously parallel to each other, such as... Figure 1 As shown, its shoulder joint has yaw and pitch degrees of freedom, its elbow joint has pitch degrees of freedom, and its wrist joint has yaw, pitch, and roll degrees of freedom.

[0215] A type II six-DOF offset serial robotic arm in which the joint axes of the third, fourth, and fifth joints are continuously parallel to each other, such as... Figure 2 As shown, its shoulder joint has yaw, pitch and roll degrees of freedom, its elbow joint has pitch degree of freedom, and its wrist joint has yaw and pitch degrees of freedom.

[0216] S2: Establish separate reference coordinate systems for the Type I six-DOF offset serial manipulator and the Type II six-DOF offset serial manipulator. and the end coordinate system Define the link length of each joint of the robotic arm as... The position coordinates of each joint in the reference coordinate system are: The unit direction vector of each joint is The linear spacing of each joint is Then the Plück coordinates of each joint, i.e., the joint axes, are... ;

[0217] Table 1. Plück coordinates of joints in a type I six-DOF offset tandem robotic arm.

[0218]

[0219] Table 2. Plück coordinates of joints in a Type II six-DOF offset tandem robotic arm.

[0220]

[0221] In the table: express The lengths of the two numbers are the same, and the rest are similar.

[0222] S3: Inverse kinematics analysis and solution of a type I six-DOF biased serial manipulator;

[0223] S3 includes the following steps:

[0224] S301: Forward kinematic analysis of a Class I six-DOF biased serial robotic arm:

[0225] Calculating the end-effector coordinate system during the rotational motion of a type I six-DOF biased tandem robotic arm using dual quaternions Relative reference coordinate system position To solve for the known terms in the inverse kinematics:

[0226] (1)

[0227] In formula (1):

[0228] ,in: These are the pose vector parameters;

[0229] These are the rotation angle values ​​for each joint;

[0230] The six-DOF biased serial robotic arm is in a zero-position configuration. Time end coordinate system Relative reference coordinate system The pose; where: , ;

[0231] Let be the dual quaternions for each joint rotation, where: As a dual unit;

[0232] Table 3. Dual quaternions for the joint rotation of a type I six-DOF biased serial manipulator.

[0233]

[0234] S302: Inverse kinematics derivation of a type I six-DOF biased serial manipulator.

[0235] S302 includes the following steps:

[0236] Since the joint axes of the second, third, and fourth joints of a type I six-DOF offset serial manipulator are continuously parallel to each other, according to Pieper's theorem, given the end-effector coordinate system... Relative reference coordinate system position Each joint of a type I six-DOF biased serial manipulator has an analytical solution.

[0237] S30201: Solve for the rotation angle of the first joint of a type I six-DOF offset tandem robotic arm. The rotation angle value of the fifth joint and the rotation angle value of the sixth joint ;

[0238] S30201 includes the following steps:

[0239] S3020101: The second joint dual quaternion on the right side of retained form (1) Third joint dual quaternion and the fourth joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0240] (2)

[0241] In formula (2):

[0242] dual quaternions The conjugate dual quaternions of , and the following relation exists:

[0243] (3)

[0244] In formula (3):

[0245] and Represents any number;

[0246] and Represents an arbitrary three-dimensional vector;

[0247] This represents a three-dimensional vector [0 0 0] whose elements are all 0;

[0248] Denotes the Euclidean norm;

[0249] S3020102: As Figure 3 As shown, assume there exists a point where the first joint is located simultaneously. and the location of the second joint And the direction vector of the joint axis of the second joint Vertical plane Then we can obtain a plane. To the reference coordinate system vertical distance from the origin for:

[0250] (4)

[0251] Then the plane The dual quaternion is:

[0252] (5)

[0253] Because the joint axes of the second, third, and fourth joints of the Type I six-DOF offset manipulator are continuously parallel to each other, the plane... Simultaneously perpendicular to the joint axis of the second joint, the joint axis of the third joint, and the joint axis of the fourth joint, therefore, even the rotation angle value of the second joint... The rotation angle value of the third joint and the rotation angle value of the fourth joint Changes occur in the joint axes of the second, third, and fourth joints relative to the plane. The positional relationship will not change, that is, it will always remain vertical, and equation (5) will not change.

[0254] S3020103: Plane When rotating around the fourth joint, the third joint, and the second joint in sequence, according to the property of plane rotation around the axis in equation (2), we can obtain:

[0255] (6)

[0256] In formula (6):

[0257] dual quaternions Another conjugate dual quaternion;

[0258] Due to the plane The plane is always perpendicular to the joint axes of the second, third, and fourth joints. After rotation around these three joints, the plane... The relationship remains unchanged, meaning the following expression exists:

[0259] (7)

[0260] Combining equations (6) and (7), we can obtain:

[0261] (8)

[0262] S3020104: The fifth joint dual quaternion on the right side of retained expression (8) The quaternion of the sixth joint The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0263] (9)

[0264] In equation (9):

[0265] Describing dual quaternions Another conjugate dual quaternion;

[0266] In rigid body transformations, any plane, after rotating about an axis by a certain angle, still retains its planar structure. According to equation (9), the plane... After rotating around the fifth joint, a plane is obtained. ,flat After rotating around the sixth joint, a plane is obtained. ,like Figure 4 As shown, plane With plane The following relationship exists:

[0267] (10)

[0268] exist Figure 4 In the middle, plane After rotating around the sixth joint, a plane is obtained. During the process, the joint axis of the sixth joint has a dual quaternion. With plane and plane The intersection points are the same as the intersection points. The intersection point is obtained by using the Clifford algebraic cross product of the joint axis and the plane. coordinate:

[0269] (11)

[0270] In equation (11):

[0271] This is the outer product of Clifford's algebra;

[0272] For plane The normal vector;

[0273] For plane Distance to the origin of the coordinate system;

[0274] Joint axis The direction vector;

[0275] Joint axis Line spacing;

[0276] Then we can obtain the dual quaternion of the fifth joint:

[0277] (12)

[0278] By combining equations (5) and (12), we can obtain:

[0279] (13)

[0280] S3020105: Dualize the joint axis of the sixth joint with a quaternion. Combining equations (11) and (13), we obtain the joint axis dual quaternion of the sixth joint. With plane intersection for:

[0281] (14)

[0282] In equation (14):

[0283] For unused terms, no specific functional expression is given for ease of explanation;

[0284] flat The plane is obtained by rigid body transformation on the left side of equation (9). :

[0285] (15)

[0286] Combining equations (11) and (15) yields the joint axis dual quaternion of the sixth joint. With plane intersection :

[0287] (16)

[0288] In equation (16):

[0289] ;

[0290] ;

[0291] in: , , , , , , ;

[0292] S3020106: Based on the above analysis, the intersection point... Intersection If the point is the same, then the terms in equations (14) and (16) are equal, and we can obtain:

[0293] (17)

[0294] (18)

[0295] Combining equations (17) and (18), we can obtain the rotation angle value containing only the first joint. Relationship:

[0296] (19)

[0297] In equation (19):

[0298] , , ;

[0299] Therefore, the rotation angle value of the first joint can be obtained. Two solutions:

[0300] (20)

[0301] Substituting equation (20) into equation (17) yields the rotation angle value of the fifth joint. Two solutions:

[0302] (twenty one)

[0303] S3020107: Combining equations (5) and (10), we can obtain the plane. The dual quaternion is:

[0304] (twenty two)

[0305] The rotation angle value of the first joint Substituting into equation (15) yields the plane The dual quaternion is:

[0306] (twenty three)

[0307] In equation (23), the pose is in the zero position configuration. Conjugate dual quaternions of pose First joint dual quaternion and plane All are known terms, therefore, the plane The third and fourth components , It is also a known item.

[0308] According to equation (9), the plane and plane If all terms in the equations are equal, then by combining equations (22) and (23), we can obtain:

[0309] (twenty four)

[0310] therefore, ( At that time, the rotation angle value of the sixth joint When the value is usually 0), we get:

[0311] (25)

[0312] S30202: Solve for the rotation angle of the second joint of a type I six-DOF offset tandem robotic arm. The rotation angle value of the third joint and the rotation angle value of the fourth joint .

[0313] S30202 includes the following steps:

[0314] S3020201: The rotation angle value of the first joint. The rotation angle value of the fifth joint and the rotation angle value of the sixth joint Substitute into formula (2):

[0315] (26)

[0316] Due to the first joint dual quaternion , pose Pose in zero-position configuration Fifth joint dual quaternion and the sixth joint dual quaternion All of these are known terms, therefore we can obtain the left side. ;

[0317] S3020202: Dual quaternion of the second joint Third joint dual quaternion and the fourth joint dual quaternion Substituting into equation (26), we get:

[0318] (27)

[0319] (28)

[0320] (29)

[0321] (30)

[0322] Due to the fact that in equation (26) , Each item is equal, and Given the terms, by combining equations (27) and (28), we can obtain:

[0323] (31)

[0324] By combining equations (27) and (30), we can obtain:

[0325] (32)

[0326] (33)

[0327] Squaring both sides of equations (32) and (33) and summing them:

[0328] (34)

[0329] Therefore, the rotation angle value of the third joint for:

[0330] (35)

[0331] S3020203: Use the sum-to-product formula in equations (32) and (33) to expand trigonometric functions:

[0332] (36)

[0333] The rotation angle value of the second joint can be obtained. for:

[0334] (37)

[0335] By combining equations (31), (35), and (37), the rotation angle value of the fourth joint can be obtained. for:

[0336] (38)

[0337] Based on the above analysis, the pose can be obtained. Given the analytical solution for the remaining rotation angle values, where the rotation angle value of the first joint is... The rotation angle value of the third joint The rotation angle value of the fifth joint There are two solutions for each rotation angle. The order of solving for each rotation angle value is as follows: Figure 5 As shown.

[0338] S4: Inverse kinematics analysis and solution of a type II six-degree-of-freedom offset serial manipulator;

[0339] S4 includes the following steps:

[0340] S401: Forward kinematic analysis of a type II six-DOF biased serial robotic arm:

[0341] Calculating the end-effector coordinate system during the rotational motion of a type II six-DOF biased tandem robotic arm using dual quaternions Relative reference coordinate system position To solve for the known terms in the inverse kinematics:

[0342] (39)

[0343] In equation (39):

[0344] The Type II six-DOF biased serial robotic arm is in a zero-position configuration. Time end coordinate system Relative reference coordinate system The position;

[0345] For each joint rotation, there are dual quaternions;

[0346] Table 4. Dual Quaternions for Joint Rotation of a Type II Six-DOF Offset Serial Robotic Arm

[0347]

[0348] S402: Inverse kinematics derivation of a type II six-DOF biased serial manipulator.

[0349] S402 includes the following steps:

[0350] S40201: Solve for the rotation angle of the first joint of a type II six-DOF offset tandem robotic arm. The rotation angle value of the second joint and the rotation angle value of the sixth joint. ;

[0351] S40201 includes the following steps:

[0352] S4020101: The third joint dual quaternion on the right side of the retained expression (39) The fourth joint dual quaternion and the fifth joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0353] (40)

[0354] S4020102: Assume there exists a point that simultaneously passes through the location of the first joint. and the location of the second joint And the direction vector of the joint axis of the third joint Vertical plane Then we can obtain a plane. To the reference coordinate system vertical distance from the origin for:

[0355] (41)

[0356] Then the plane The dual quaternion is:

[0357] (42)

[0358] Based on the configuration characteristics of the equivalent robotic arm, it can be seen that the joint axes of the third joint, the fourth joint, and the fifth joint are continuously parallel to each other, therefore the plane... Simultaneously perpendicular to the joint axis of the third joint, the joint axis of the fourth joint, and the joint axis of the fifth joint, therefore, even the rotation angle value of the third joint... The rotation angle value of the fourth joint and the rotation angle value of the fifth joint Changes occur in the joint axes of the third, fourth, and fifth joints relative to the plane. The positional relationship will not change, that is, it will always remain vertical, and equation (42) will not change.

[0359] S4020103: Plane When rotating around the fifth, fourth, and third joints in sequence, according to the property of planar rotation around an axis in equation (40), we can obtain:

[0360] (43)

[0361] Due to the plane The plane is always perpendicular to the joint axes of the third, fourth, and fifth joints. After rotation around these three joints, the plane... The relationship remains unchanged, meaning the following expression exists:

[0362] (44)

[0363] Combining equations (43) and (44), we obtain:

[0364] (45)

[0365] S4020104: The first joint dual quaternion in equation (45) Second joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, while the remaining terms are retained on the right side, resulting in:

[0366] (46)

[0367] In rigid body transformations, any plane, after rotating about an axis by a certain angle, still retains its planar structure. According to equation (46), the plane... After rotating around the second joint, a plane is obtained. ,flat After rotating around the first joint, a plane is obtained. ,flat With plane The following relationship exists:

[0368] (47)

[0369] According to the plane Second joint dual quaternion Obtain a plane for:

[0370] (48)

[0371] S4020105: Plane After rotating around the first joint, a plane is obtained. During the process, the joint axis of the first joint is a dual quaternion. With plane and plane The intersection points are the same as the intersection points. The intersection point is obtained by using the Clifford algebraic cross product of the joint axis and the plane. coordinate:

[0372] (49)

[0373] flat The plane is obtained by rigid body transformation on the right side of equation (46). :

[0374] (50)

[0375] Obtain the joint axis dual quaternion of the first joint. With plane intersection :

[0376] (51)

[0377] In equation (51):

[0378] ;

[0379] ;

[0380] S4020106: Based on the above analysis, it can be seen that the plane... With plane If they are on the same plane, then the joint axis of the first joint has a dual quaternion. Since the intersection point with the two planes is the same point, and the terms in equations (49) and (51) are equal, we can obtain:

[0381] (52)

[0382] (53)

[0383] Combining equations (52) and (53), we can obtain the rotation angle value containing only the sixth joint. Relationship:

[0384] (54)

[0385] In equation (54):

[0386] ;

[0387] ;

[0388] ;

[0389] Therefore, the rotation angle value of the sixth joint can be obtained. Two solutions:

[0390] (55)

[0391] Substituting equation (55) into equation (52) yields the rotation angle value of the second joint. Two solutions:

[0392] (56)

[0393] S4020107: According to equation (47), the plane can be obtained The dual quaternion is:

[0394] (57)

[0395] According to equation (50), the plane can be obtained The dual quaternion is:

[0396] (58)

[0397] Due to the plane With plane Since they are on the same plane, the terms of equations (57) and (58) are equal, and the rotation angle value of the first joint can be obtained. for:

[0398] (59)

[0399] S40202: Solve for the rotation angle of the third joint of a type II six-DOF offset tandem robotic arm. The rotation angle value of the fourth joint and the rotation angle value of the fifth joint .

[0400] S40202 includes the following steps:

[0401] S4020201: According to equation (40), we can obtain:

[0402] (60)

[0403] S4020202: The rotation angle value of the first joint. The rotation angle value of the second joint and the rotation angle value of the sixth joint Substituting into equation (60), we get:

[0404] (61)

[0405] (62)

[0406] (63)

[0407] (64)

[0408] Due to the fact that in equation (60) , Each item is equal, and Given the terms, by combining equations (61) and (62), we can obtain:

[0409] (65)

[0410] By combining equations (61) and (64), we can obtain:

[0411] (66)

[0412] (67)

[0413] Squaring both sides of equations (66) and (67) and summing them:

[0414] (68)

[0415] Therefore, the rotation angle value of the fourth joint for:

[0416] (69)

[0417] S4020203: Use the sum-to-product formula in equations (66) and (67) to expand trigonometric functions:

[0418] (70)

[0419] The rotation angle value of the third joint can be obtained. for:

[0420] (71)

[0421] By combining equations (65), (69), and (71), the rotation angle value of the fifth joint can be obtained. for:

[0422] (72)

[0423] Based on the above analysis, the pose can be obtained. Given the analytical solution for the remaining rotation angle values, where the rotation angle value of the second joint is... The rotation angle value of the fourth joint The rotation angle value of the sixth joint There are two solutions for each rotation angle. The order of solving for each rotation angle value is as follows: Figure 6 As shown.

[0424] S5: Verify the solution method.

[0425] To illustrate the effectiveness of the inverse kinematics solution method proposed in this invention, the inverse kinematics solution process of a type I six-degree-of-freedom biased serial manipulator is taken as an example, and it is compared with existing inverse kinematics solution methods in references 1 and 2.

[0426] The specific comparison steps are as follows:

[0427] S501: In MATLAB, use the random function unifrnd to randomly generate 10,000 sets of robotic arm configurations (each configuration represents a set of joint rotation angle values), and substitute them into the forward kinematics calculation formula (1) of the robotic arm to obtain the end pose of each configuration. ;

[0428] S502: in end-effector pose As initial conditions, three methods were used to solve the inverse kinematics, and the total time for solving 10,000 sets of inverse kinematics with different mechanism configurations was statistically analyzed. The timing method used was the tic-toc timing function built into MATLAB, with the time unit being milliseconds (ms); the computing platform was MATLAB 2021a; the computer performance parameters were Intel(R) Core(TM) i3-10100 CPU @ 3.60GHz, 16.0 GB RAM.

[0429] S503: To demonstrate the validity of the solution results, each inverse kinematics solution method was repeated 10 times, and the calculation results are as follows: Figure 7 As shown, the computation time and average time for each solution method are presented in Tables 5 and 6, respectively. Using the computation times of Reference 1 and Reference 2 as comparative references, the percentage of solution time for the method proposed in this invention compared to the two methods mentioned above is calculated using the following formula:

[0430] (73)

[0431] In equation (73):

[0432] This indicates the time taken by the inverse kinematics solution method in this invention;

[0433] This indicates the time taken by the inverse kinematics solution method in Reference 1 or Reference 2.

[0434] Table 5 Solution time for different inverse kinematics methods

[0435]

[0436] Table 6. Average solution time for different inverse kinematics methods

[0437]

[0438] Comprehensive analysis Figure 7 As shown in Table 5, the inverse kinematics solution method in Reference 2 requires the longest computation time, followed by the inverse kinematics solution method in Reference 1, while the inverse kinematics solution method in this invention requires the shortest computation time. In each calculation, the solution time of the algorithm in this invention is less than that of the other two algorithms.

[0439] Comprehensive analysis Figure 7 As shown in Table 6, the average solution time for 10 inverse kinematics calculations is reduced by 43.17% compared to Method 1 and by 73.27% compared to Method 2. Therefore, the computational efficiency of the inverse kinematics solution method in this invention is greatly improved.

[0440] Fundamental properties of dual quaternions:

[0441] The basic knowledge about dual quaternions mentioned above can be found in relevant materials; this article provides some references. The relationship between rigid body transformations and dual quaternions in 3D space can be found in the reference (Daniilidis K. Hand-eye calibration using dual quaternions [J]. International Journal of Robotics Research, 1999, 18(3): 286-298); the dual quaternion representations and transformations of points, lines, and planes can be found in the reference (Radavelli LA, De Pieri ER, Martins D, et al. Points, Lines, Screws and Planes in DualQuaternions Kinematics [M]. Advances in Robot Kinematics. Cham; Springer International Publishing. 2014: 285-293); the conjugate of dual quaternions and its properties can be found in the reference (Bayro-Corrochano E. Modeling the 3D kinematics of the eye in the geometricalgebra framework [J]. Pattern Recognition, 2003, 36(12): 2993-3012).

[0442] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of the equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0443] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions, characterized in that: The method includes the following steps: S1: Classification of six-DOF biased serial robotic arms, including: A type I six-degree-of-freedom offset serial robotic arm in which the joint axes of the second joint, the third joint, and the fourth joint are continuously parallel to each other. A type II six-degree-of-freedom offset serial robotic arm in which the joint axes of the third joint, the fourth joint, and the fifth joint are continuously parallel to each other. S2: Establish separate reference coordinate systems for the Type I six-DOF offset serial manipulator and the Type II six-DOF offset serial manipulator. and the end coordinate system Define the link length of each joint of the robotic arm as The position coordinates of each joint in the reference coordinate system are: The unit direction vector of each joint is The linear spacing of each joint is Then the Plück coordinates of each joint, i.e., the joint axes, are... ; S3: Inverse kinematics analysis and solution of a type I six-DOF biased serial manipulator; S4: Inverse kinematics analysis and solution of a type II six-degree-of-freedom offset serial manipulator; S5: Verify the solution method.

2. The method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions according to claim 1, characterized in that: S3 includes the following steps: S301: Forward kinematic analysis of a Class I six-DOF biased serial robotic arm: Calculating the end-effector coordinate system during the rotational motion of a type I six-DOF biased tandem robotic arm using dual quaternions Relative reference coordinate system position : (1) In formula (1): ,in: These are the pose vector parameters; These are the rotation angle values ​​for each joint; The end-effector coordinate system of a Class I six-DOF biased serial robotic arm in its zero-position configuration Relative reference coordinate system The pose; where: , ; Let be the dual quaternions for each joint rotation, where: As a dual unit; S302: Inverse kinematics derivation of a type I six-DOF biased serial manipulator.

3. The method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions according to claim 2, characterized in that: S302 includes the following steps: S30201: Solve for the rotation angle of the first joint of a type I six-DOF offset tandem robotic arm. The rotation angle value of the fifth joint and the rotation angle value of the sixth joint ; S30202: Solve for the rotation angle of the second joint of a type I six-DOF offset tandem robotic arm. The rotation angle value of the third joint and the rotation angle value of the fourth joint .

4. The method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions according to claim 3, characterized in that: S30201 includes the following steps: S3020101: The second joint dual quaternion on the right side of retained form (1) Third joint dual quaternion and the fourth joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in: (2) In formula (2): dual quaternions The conjugate dual quaternions of , and the following relation exists: (3) In formula (3): and Represents any number; and Represents an arbitrary three-dimensional vector; This represents a three-dimensional vector [0 0 0] whose elements are all 0; Denotes the Euclidean norm; S3020102: Assume there exists a point that simultaneously passes through the location of the first joint. and the location of the second joint And the direction vector of the joint axis of the second joint Vertical plane Then we can obtain a plane. To the reference coordinate system vertical distance from the origin for: (4) Then the plane The dual quaternion is: (5) S3020103: Plane When rotating around the fourth joint, the third joint, and the second joint in sequence, according to the property of plane rotation around the axis in equation (2), we can obtain: (6) In equation (6): dual quaternions Another conjugate dual quaternion; Due to the plane The plane is always perpendicular to the joint axes of the second, third, and fourth joints. After rotation around these three joints, the plane... The relationship remains unchanged, meaning the following expression exists: (7) Combining equations (6) and (7), we can obtain: (8) S3020104: The fifth joint dual quaternion on the right side of retained expression (8) The quaternion of the sixth joint The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in: (9) In equation (9): Describing dual quaternions Another conjugate dual quaternion; flat After rotating around the fifth joint, a plane is obtained. ,flat After rotating around the sixth joint, a plane is obtained. ,flat With plane The following relationship exists: (10) flat After rotating around the sixth joint, a plane is obtained. During the process, the joint axis of the sixth joint has a dual quaternion. With plane and plane The intersection points are the same as the intersection points. The intersection point is obtained by using the Clifford algebraic cross product of the joint axis and the plane. coordinate: (11) In equation (11): This is the outer product of Clifford's algebra; For plane The normal vector; For plane Distance to the origin of the coordinate system; Joint axis The direction vector; Joint axis Line spacing; Then we can obtain the dual quaternion of the fifth joint: (12) By combining equations (5) and (12), we can obtain: (13) S3020105: Dualize the joint axis of the sixth joint with a quaternion. Combining equations (11) and (13), we obtain the joint axis dual quaternion of the sixth joint. With plane intersection for: (14) In equation (14): Unused items; flat The plane is obtained by rigid body transformation on the left side of equation (9). : (15) Combining equations (11) and (15) yields the joint axis dual quaternion of the sixth joint. With plane intersection : (16) In equation (16): ； ； in: , , , , , , ; S3020106: It can be seen that the intersection point... Intersection If the point is the same, then the terms in equations (14) and (16) are equal, and we can obtain: (17) (18) Combining equations (17) and (18), we can obtain the rotation angle value containing only the first joint. Relationship: (19) In equation (19): ， ， ； Therefore, the rotation angle value of the first joint can be obtained. Two solutions: (20) Substituting equation (20) into equation (17) yields the rotation angle value of the fifth joint. Two solutions: (21) S3020107: Combining equations (5) and (10), we can obtain the plane. The dual quaternion is: (22) The rotation angle value of the first joint Substituting into equation (15) yields the plane The dual quaternion is: (23) According to equation (9), the plane and plane If all terms in the equations are equal, then by combining equations (22) and (23), we can obtain: (24) therefore, When it is time, we can obtain: (25)。 5. The method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions according to claim 4, characterized in that: S30202 includes the following steps: S3020201: The rotation angle value of the first joint. The rotation angle value of the fifth joint and the rotation angle value of the sixth joint Substitute into formula (2): (26) S3020202: Dual quaternion of the second joint Third joint dual quaternion and the fourth joint dual quaternion Substituting into equation (26), we get: (27) (28) (29) (30) By combining equations (27) and (28), we can obtain: (31) By combining equations (27) and (30), we can obtain: (32) (33) Squaring both sides of equations (32) and (33) and summing them: (34) Therefore, the rotation angle value of the third joint for: (35) S3020203: Use the sum-to-product formula in equations (32) and (33) to expand trigonometric functions: (36) The rotation angle value of the second joint can be obtained. for: (37) By combining equations (31), (35), and (37), the rotation angle value of the fourth joint can be obtained. for: (38)。 6. The method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions according to claim 1, characterized in that: S4 includes the following steps: S401: Forward kinematic analysis of a type II six-DOF biased serial robotic arm: Calculating the end-effector coordinate system during the rotational motion of a type II six-DOF biased tandem robotic arm using dual quaternions Relative reference coordinate system position : (39) In equation (39): The end-effector coordinate system of a Type II six-DOF offset serial robotic arm in its zero-position configuration Relative reference coordinate system The position; For each joint rotation, there are dual quaternions; S402: Inverse kinematics derivation of a type II six-DOF biased serial manipulator.

7. The method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions according to claim 6, characterized in that: S402 includes the following steps: S40201: Solve for the rotation angle of the first joint of a type II six-DOF offset tandem robotic arm. The rotation angle value of the second joint and the rotation angle value of the sixth joint. ; S40202: Solve for the rotation angle of the third joint of a type II six-DOF offset tandem robotic arm. The rotation angle value of the fourth joint And the rotation angle value of the fifth joint .

8. The method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions according to claim 7, characterized in that: S40201 includes the following steps: S4020101: The third joint dual quaternion on the right side of the retained expression (39) The fourth joint dual quaternion and the fifth joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in: (40) S4020102: Assume there exists a point that simultaneously passes through the location of the first joint. and the location of the second joint And the direction vector of the joint axis of the third joint Vertical plane Then we can obtain a plane. To the reference coordinate system vertical distance from the origin for: (41) Then the plane The dual quaternion is: (42) S4020103: Plane When rotating around the fifth, fourth, and third joints in sequence, according to the property of planar rotation around an axis in equation (40), we can obtain: (43) Due to the plane The plane is always perpendicular to the joint axes of the third, fourth, and fifth joints. After rotation around these three joints, the plane... The relationship remains unchanged, meaning the following expression exists: (44) Combining equations (43) and (44), we obtain: (45) S4020104: The first joint dual quaternion in equation (45) Second joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, while the remaining terms are retained on the right side, resulting in: (46) flat After rotating around the second joint, a plane is obtained. ,flat After rotating around the first joint, a plane is obtained. ,flat With plane The following relationship exists: (47) According to the plane Second joint dual quaternion Obtain a plane for: (48) S4020105: Plane After rotating around the first joint, a plane is obtained. During the process, the joint axis of the first joint is a dual quaternion. With plane and plane The intersection points are the same as the intersection points. The intersection point is obtained by using the Clifford algebraic cross product of the joint axis and the plane. coordinate: (49) flat The plane is obtained by rigid body transformation on the right side of equation (46). : (50) Obtain the joint axis dual quaternion of the first joint. With plane intersection : (51) In equation (51): ； ； S4020106: It can be seen that the plane... With plane If they are on the same plane, then the joint axis of the first joint has a dual quaternion. Since the intersection point with the two planes is the same point, and the terms in equations (49) and (51) are equal, we can obtain: (52) (53) Combining equations (52) and (53), we can obtain the rotation angle value containing only the sixth joint. Relationship: (54) In equation (54): ； ； ； Therefore, the rotation angle value of the sixth joint can be obtained. Two solutions: (55) Substituting equation (55) into equation (52) yields the rotation angle value of the second joint. Two solutions: (56) S4020107: According to equation (47), the plane can be obtained The dual quaternion is: (57) According to equation (50), the plane can be obtained The dual quaternion is: (58) Due to the plane With plane Since they are on the same plane, the terms of equations (57) and (58) are equal, and the rotation angle value of the first joint can be obtained. for: (59)。 9. The method for solving the inverse kinematics of a six-degree-of-freedom biased serial manipulator based on dual quaternions according to claim 8, characterized in that: S40202 includes the following steps: S4020201: According to equation (40), we can obtain: (60) S4020202: The rotation angle value of the first joint. The rotation angle value of the second joint and the rotation angle value of the sixth joint Substituting into equation (60), we get: (61) (62) (63) (64) Combining equations (61) and (62), we can obtain: (65) By combining equations (61) and (64), we can obtain: (66) (67) Squaring both sides of equations (66) and (67) and summing them: (68) Therefore, the rotation angle value of the fourth joint for: (69) S4020203: Use the sum-to-product formula in equations (66) and (67) to expand trigonometric functions: (70) The rotation angle value of the third joint can be obtained. for: (71) By combining equations (65), (69), and (71), the rotation angle value of the fifth joint can be obtained. for: (72)。

Images