Inverse kinematics solution method for SSRMS configuration manipulator based on dual quaternion

Abstract

The SSRMS configuration mechanical arm inverse kinematics solving method based on dual quaternion belongs to the technical field of mechanical arm design. The method is as follows: a reference coordinate system and an end coordinate system are established, the lengths of the connecting rods of each joint of the mechanical arm are defined, the position coordinates of each joint are defined, the unit direction vectors of each joint are defined, the linear distances of each joint are defined, then the Plucker coordinates of each joint are obtained; the dual quaternion is used to calculate the pose of the end coordinate system relative to the reference coordinate system when the mechanical arm rotates; the inverse kinematics derivation of the first joint, the second joint, the sixth joint and the seventh joint is carried out respectively; the solving method is verified. The constraint relationship between the rotation angle values is established, the technical bottleneck that the real-time performance is difficult to consider in the traditional inverse kinematics solving is broken through, and the solving speed of the inverse kinematics is significantly improved while the solving accuracy is ensured.

Description

Technical Field

[0001] This invention relates to a method for solving the inverse kinematics of an SSRMS configuration robotic arm based on dual quaternions, belonging to the field of robotic arm design technology. Background Technology

[0002] SSRMS-configured robotic arms are a collective term for robotic arms that share the same degrees of freedom and joint arrangement characteristics as the space station's teleoperation system. Because they possess a redundant degree of freedom, these robotic arms can simultaneously track target pose and achieve complex constraints such as joint limitation avoidance and obstacle avoidance. Their physical offset design of the shoulder, elbow, and wrist joints, combined with the parallel configuration of these three consecutive joints, effectively prevents mechanical collisions with themselves during rotation, making them widely applicable in space missions. Typical applications include the Canadarm 2 robotic arm and ERA (European Robotic Arm) on the International Space Station, as well as the robotic arms in the experimental module and core module of the Chinese Space Station.

[0003] In remote operation scenarios on space stations, the robotic arm must meet real-time control requirements due to the constraints of the distance between the space station and the ground. This places stringent demands on the speed of inverse kinematics solution for a given target pose. Especially for SSRMS configuration robotic arms, the existence of multiple different robotic arm configurations (i.e., different joint angle sets) corresponding to the same target pose necessitates selecting the optimal configuration from multiple configurations that simultaneously satisfies constraints such as joint limit avoidance and obstacle avoidance, further exacerbating the demand for faster inverse kinematics solution.

[0004] In the prior art, 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 related robotic arm kinematic solutions, but do not propose a systematic solution for the speed optimization and multi-configuration selection problem of SSRMS configuration inverse kinematics solution. Summary of the Invention

[0005] To address the problems existing in the background technology, this invention provides a method for solving the inverse kinematics of an SSRMS configuration robotic arm based on dual quaternions.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: a method for solving the inverse kinematics of an SSRMS configuration robotic arm based on dual quaternions, the method comprising the following steps:

[0007] S1: Establish the reference coordinate system for the SSRMS configuration robotic arm and the end coordinate system Define the link length of each joint of the SSRMS configuration 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... ;

[0008] S2: Calculate the end-effector coordinate system during the rotational motion of the SSRMS configuration robotic arm using dual quaternions. Relative reference coordinate system position :

[0009] (1)

[0010] In formula (1):

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

[0012] The end-effector coordinate system when the SSRMS configuration robotic arm is in the zero position configuration Relative reference coordinate system The pose, where: , ;

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

[0014] S3: Inverse kinematics derivation based on dual quaternions when the first joint is fixed;

[0015] S3 includes the following steps:

[0016] S301: Constructing an equivalent six-DOF robotic arm: Rotation angle values ​​of the first joint of the SSRMS configuration robotic arm Given that the joint axes of the third joint, the fourth joint, and the fifth joint are continuously parallel to each other;

[0017] When the equivalent six-DOF robotic arm is in the zero-position configuration, the end-effector coordinate system... Relative reference coordinate system position for:

[0018] (2)

[0019] S302: The formula for calculating the forward kinematics of an equivalent six-DOF robotic arm is:

[0020] (3)

[0021] In formula (3):

[0022] These are the pose vector parameters;

[0023] S303: Rotation angle value of the first joint Given the given values, the dual quaternions of the joint axes of the equivalent six-DOF robotic arm can be obtained as follows:

[0024] (4)

[0025] The rotation angle value of the first joint can be obtained. Given, the dual quaternions for rotational transformations about the remaining joints:

[0026] (5)

[0027] S304: Solve for the rotation angle of the second joint. The rotation angle value of the sixth joint and the rotation angle value of the seventh joint ;

[0028] S30401: The third joint dual quaternion on the right side of retained expression (3) 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:

[0029] (6)

[0030] In equation (6):

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

[0032] (7)

[0033] In equation (7):

[0034] and Represents any number;

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

[0036] Denotes the Euclidean norm;

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

[0038] (8)

[0039] Then the plane The dual quaternion is:

[0040] (9)

[0041] In equation (9):

[0042] It is a column vector with all elements being 0;

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

[0044] (10)

[0045] In formula (10):

[0046] dual quaternions Another conjugate dual quaternion;

[0047] 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 it exists:

[0048] (11)

[0049] Combining equations (10) and (11), we obtain:

[0050] (12)

[0051] In equation (12):

[0052] Describing dual quaternions Another conjugate dual quaternion;

[0053] S30404: The sixth joint dual quaternion on the right side of retained expression (12) and the seventh joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0054] (13)

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

[0056] (14)

[0057] flat Rotate around the seventh joint to obtain a plane During the process, the joint axis of the seventh 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:

[0058] (15)

[0059] In equation (15):

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

[0061] For plane The normal vector;

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

[0063] Joint axis The direction vector;

[0064] Joint axis Line spacing;

[0065] Then we can obtain the dual quaternion of the sixth joint:

[0066] (16)

[0067] Combining equations (9) and (16), we can obtain:

[0068] (17)

[0069] S30405: Dual quaternions for the joint axis of the seventh joint. Combining equations (17) and (15), we obtain the joint axis dual quaternion of the seventh joint. With plane intersection for:

[0070] (18)

[0071] In equation (18):

[0072] Unused items;

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

[0074] (19)

[0075] Combining equations (19) and (15) yields the joint axis dual quaternion of the seventh joint. With plane intersection :

[0076] (20)

[0077] In equation (20):

[0078] ;

[0079] ;

[0080] in: , , , , , , ;

[0081] S30406: It can be seen that the intersection point Intersection If the point is the same, then the terms in equations (18) and (20) are equal, and we can obtain:

[0082] (twenty one)

[0083] (twenty two)

[0084] Combining equations (21) and (22), we can obtain the rotation angle value containing only the second joint. Relationship:

[0085] (twenty three)

[0086] In equation (23):

[0087] ;

[0088] ;

[0089] ;

[0090] S30407: Therefore, the rotation angle value of the second joint can be obtained. Two solutions:

[0091] (twenty four)

[0092] Substituting equation (24) into equation (21), we can obtain the rotation angle value of the sixth joint. Two solutions:

[0093] (25)

[0094] By combining equations (9) and (14), we can obtain the plane. The dual quaternion is:

[0095] (26)

[0096] The rotation angle value of the second joint Substituting into equation (19), we obtain the plane. The dual quaternion is:

[0097] (27)

[0098] According to equation (13), the plane and plane If all terms in the equations are equal, then by combining equations (26) and (27), we can obtain:

[0099] (28)

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

[0101] (29)

[0102] S305: Solve for the rotation angle of the fourth joint. The rotation angle value of the third joint And the rotation angle value in section five. ;

[0103] S30501: Combining equations (2) and (5), we can obtain:

[0104] (30)

[0105] By combining equations (6) and (30), we can obtain:

[0106] (31)

[0107] S30502: Dual quaternion of the third joint The fourth joint dual quaternion and the fifth joint dual quaternion Substitution formula (31):

[0108] (32)

[0109] (33)

[0110] (34)

[0111] (35)

[0112] By combining equations (32) and (33), we can obtain:

[0113] (36)

[0114] By combining equations (32) and (35), we can obtain:

[0115] (37)

[0116] (38)

[0117] Squaring both sides of equations (37) and (38) and summing them:

[0118] (39)

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

[0120] (40)

[0121] S30503: Use the sum-to-product formula in equations (37) and (38) to expand trigonometric functions:

[0122] (41)

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

[0124] (42)

[0125] By combining equations (36), (40), and (42), the rotation angle value of the fifth joint can be obtained. for:

[0126] (43)

[0127] S4: Inverse kinematics derivation based on dual quaternions when the second joint is fixed;

[0128] S4 includes the following steps:

[0129] S401: Rotation angle value of the second joint Given these values, we can obtain the dual quaternions for each joint axis of the equivalent six-DOF robotic arm:

[0130] (44)

[0131] S402: The formula for calculating the forward kinematics of an equivalent six-DOF robotic arm is:

[0132] (45)

[0133] The rotation angle value of the second joint can be obtained. Given the dual quaternions for rotational transformations around the remaining joints. for:

[0134] (46)

[0135] S403: Solve for the rotation angle of the first joint. The rotation angle value of the sixth joint and the rotation angle value of the seventh joint ;

[0136] S40301: The third joint dual quaternion on the right side of the retained expression (45) 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:

[0137] (47)

[0138] S40302: Assume there exists a point that simultaneously passes through the location of the second joint. and the location of the third 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:

[0139] (48)

[0140] Then the plane The dual quaternion is:

[0141] (49)

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

[0143] (50)

[0144] 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:

[0145] (51)

[0146] S40304: The sixth joint dual quaternion in retention formula (51) The quaternion of the seventh joint The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0147] (52)

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

[0149] (53)

[0150] S40305: Plane Rotate around the seventh joint to obtain a plane During the process, the joint axis of the seventh joint has a dual quaternion. With plane and plane The intersection points are the same as the intersection points. Then we can obtain the dual quaternion of the sixth joint:

[0151] (54)

[0152] By combining equations (54) and (49), we can obtain:

[0153] (55)

[0154] Given the joint axis dual quaternions of the seventh joint. Substituting equation (55) into the Clifford algebraic cross product (15) of the joint axis and the plane, we can obtain the intersection point. for:

[0155] (56)

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

[0157] (57)

[0158] By combining equations (45) and (57), the joint axis dual quaternion of the seventh joint is obtained. With plane intersection for:

[0159] (58)

[0160] In equation (58):

[0161] ;

[0162] ;

[0163] S40306: It can be seen that the intersection point Intersection If the point is the same, then the terms in equations (56) and (58) are equal, and we can obtain:

[0164] (59)

[0165] (60)

[0166] Combining equations (58) and (59), we can obtain the rotation angle value containing only the first joint. Relationship:

[0167] (61)

[0168] In equation (61):

[0169] ;

[0170] ;

[0171] ;

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

[0173] (62)

[0174] Substituting equation (62) into equation (59) yields the rotation angle value of the sixth joint. Two solutions:

[0175] (63)

[0176] By combining equations (53) and (57), we can obtain the plane. and plane The dual quaternion is:

[0177] (64)

[0178] (65)

[0179] According to equation (52), the plane and plane If all terms in the equation are equal, then we can obtain the following formula:

[0180] (66)

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

[0182] (67)

[0183] S404: Solve for the rotation angle of the fourth joint. The rotation angle value of the third joint And the rotation angle value of the fifth joint ;

[0184] S40401: Combining equations (44) and (45), we can obtain:

[0185] (68)

[0186] Combining equations (46) and (68), we obtain:

[0187] (69)

[0188] S40402: Obtain the rotation angle value of the first joint. The rotation angle value of the sixth joint and the rotation angle value of the seventh joint Then, substituting into equation (69), and following the solution process of equations (32)-(43), the rotation angle value of the fourth joint is obtained similarly. The rotation angle value of the third joint And the rotation angle value of the fifth joint .

[0189] S5: Inverse kinematics derivation based on dual quaternions when the sixth joint is fixed;

[0190] S5 includes the following steps:

[0191] S501: The formula for calculating the forward kinematics of an equivalent six-DOF robotic arm is:

[0192] (70)

[0193] The rotation angle value of the sixth joint can be obtained. Given the dual quaternions for rotational transformations around the remaining joints. for:

[0194] (71)

[0195] Dual quaternions for the joint axes of an equivalent six-DOF robotic arm:

[0196] (72)

[0197] S502: Solve for the rotation angle of the first joint. The rotation angle value of the second joint and the rotation angle value of the seventh joint ;

[0198] S50201: The third joint dual quaternion on the right side of the retained expression (70) 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:

[0199] (73)

[0200] S50202: Assume there exists a point that simultaneously passes through the location of the second joint. and the location of the third 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:

[0201] (74)

[0202] Then the plane The dual quaternion is:

[0203] (75)

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

[0205] (76)

[0206] 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:

[0207] (77)

[0208] (78)

[0209] S50204: The first joint dual quaternion in equation (78) Second joint dual quaternion The relevant terms are moved 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:

[0210] (79)

[0211] 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:

[0212] (80)

[0213] S50205: Plane Rotate around the first joint to obtain a plane 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. ;

[0214] By combining equations (75) and (71), we can obtain:

[0215] (81)

[0216] The intersection point can be obtained by using the Clifford algebraic cross product of the joint axis and the plane (15). for:

[0217] (82)

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

[0219] (83)

[0220] According to equation (15), the joint axis dual quaternion of the first joint is obtained. With plane intersection for:

[0221] (84)

[0222] In equation (84):

[0223] ;

[0224]

[0225] S50206: It can be seen that the terms in equations (82) and (84) are equal, so we can obtain:

[0226] (85)

[0227] (86)

[0228] Combining equations (85) and (86), we obtain:

[0229] (87)

[0230] In equation (87):

[0231] ;

[0232] ;

[0233] ;

[0234] S50207: Therefore, the rotation angle value of the seventh joint can be obtained. Two solutions:

[0235] (88)

[0236] Substituting equation (88) into equation (85) yields the rotation angle value of the second joint. Two solutions:

[0237] (89)

[0238] A plane can be obtained The dual quaternion is:

[0239] (90)

[0240] Substituting equation (88) into equation (83) yields the plane. The dual quaternion is:

[0241] (91)

[0242] Since all terms in equations (90) and (91) are equal, we can obtain:

[0243] (92)

[0244] S503: Solve for the rotation angle of the fourth joint. The rotation angle value of the third joint And the rotation angle value of the fifth joint ;

[0245] S50301: Rotation angle value of the sixth joint The change only affects the position of the seventh joint, which can be obtained according to equation (71):

[0246] (93)

[0247] By combining equations (73) and (93), we can obtain:

[0248] (94)

[0249] S50302: Obtain the rotation angle value of the first joint. The rotation angle value of the second joint and the rotation angle value of the seventh joint Then, substituting into equation (94), and following the solution process of equations (32)-(43), the rotation angle value of the fourth joint is obtained similarly. The rotation angle value of the third joint And the rotation angle value of the fifth joint .

[0250] S6: Inverse kinematics derivation based on dual quaternions when the seventh joint is fixed;

[0251] S6 includes the following steps:

[0252] S601: End coordinate system Relative reference coordinate system The pose is:

[0253] (95)

[0254] Therefore, the forward kinematics calculation formula for the equivalent six-degree-of-freedom robotic arm can be obtained as follows:

[0255] (96)

[0256] The dual quaternions can be obtained when rotational transformations occur around the remaining joints. for:

[0257] (97)

[0258] The dual quaternions of the joint axes of the equivalent six-DOF robotic arm can be obtained. for:

[0259] (98)

[0260] S602: Solve for the rotation angle of the first joint. The rotation angle value of the second joint and the rotation angle value of the sixth joint ;

[0261] S60201: The third joint dual quaternion on the right side of the retained expression (96) 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:

[0262] (99)

[0263] S60202: Assume there exists a point that simultaneously passes through the location of the second joint. and the location of the third 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:

[0264] (100)

[0265] Then the plane The dual quaternion is:

[0266] (101)

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

[0268] (102)

[0269] 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:

[0270] (103)

[0271] (104)

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

[0273] (105)

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

[0275] (106)

[0276] S60205: According to equations (101) and (97), we can obtain:

[0277] (107)

[0278] S60206: Plane Rotate around the first joint to obtain a plane 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. Substituting into the Clifford algebraic cross product of the joint axis and the plane (15), we can obtain the intersection point. for:

[0279] (108)

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

[0281] (109)

[0282] The joint axis dual quaternion of the first joint With plane intersection for:

[0283] (110)

[0284] In equation (110):

[0285] ;

[0286] ;

[0287] S60207: According to equation (105), the plane With plane Since they are on the same plane, the intersection point Intersection If the terms in equations (108) and (110) are equal at the same point, then we can obtain:

[0288] (111)

[0289] (112)

[0290] By combining equations (111) and (112), we can obtain:

[0291] (113)

[0292] In equation (113):

[0293] ;

[0294] ;

[0295] ;

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

[0297] (114)

[0298] Substituting equation (114) into equation (85) yields the rotation angle value of the second joint. Two solutions:

[0299] (115)

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

[0301] (116)

[0302] According to equation (109), the plane can be obtained. The dual quaternion is:

[0303] (117)

[0304] Due to the plane and plane If all terms in the equation are equal, then we can obtain:

[0305] (118)

[0306] S603: Solve for the rotation angle of the fourth joint. The rotation angle value of the third joint And the rotation angle value of the fifth joint ;

[0307] S60301: Due to the rotation angle value of the seventh joint Since the change does not affect the position of the other joints, we can conclude that:

[0308] (119)

[0309] Combining equations (89), (87), and (119), we obtain:

[0310] (120)

[0311] S60302: Obtain 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 Then, substituting into equation (120), and following the solution process of equations (32)-(43), the rotation angle value of the fourth joint is obtained similarly. The rotation angle value of the third joint and the rotation angle value of the fifth joint .

[0312] S7: Verify the solution method.

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

[0314] This invention utilizes the compact representation of rotational motion using dual quaternions and combines Clifford's algebraic outer product operation to accurately obtain the coordinates of the intersection points of the joint axes and the plane, establishing constraint relationships between the rotation angles of the first, second, sixth, and seventh joints. Simultaneously, based on the configuration feature of three consecutive parallel joints in the SSRMS configuration, analytical expressions for the rotation angles of the third, fourth, and fifth joints are derived. This overcomes the technical bottleneck of balancing multi-configuration optimization and real-time performance in traditional inverse kinematics solutions. While ensuring solution accuracy, it significantly improves the speed of inverse kinematics solutions, effectively meeting the real-time control requirements of the robotic arm in remote operation scenarios on space stations, including rapid response to target pose and real-time control of complex constraints such as joint limit avoidance and obstacle avoidance. This provides key technical support for the efficient application of SSRMS configuration robotic arms in space missions. Attached Figure Description

[0315] Figure 1 This is a zero-position configuration diagram of the SSRMS configuration robotic arm of the present invention;

[0316] Figure 2This is the zero-position configuration diagram of the equivalent six-degree-of-freedom robot arm when the rotation angle value of the first joint of the SSRMS configuration robot arm of the present invention is given;

[0317] Figure 3 When the rotation angle value of the first joint of the SSRMS configuration robotic arm of the present invention is given, the plane is... A schematic diagram;

[0318] Figure 4 When the rotation angle value of the first joint of the SSRMS configuration robotic arm of the present invention is given, the plane is... With plane A schematic diagram;

[0319] Figure 5 This is the zero-position configuration diagram of the equivalent six-degree-of-freedom manipulator when the rotation angle value of the second joint of the SSRMS configuration manipulator of the present invention is given;

[0320] Figure 6 The plane when the rotation angle value of the second joint of the SSRMS configuration robotic arm of the present invention is given. A schematic diagram;

[0321] Figure 7 The plane when the rotation angle value of the second joint of the SSRMS configuration robotic arm of the present invention is given. With plane A schematic diagram;

[0322] Figure 8 This is a schematic diagram of the inverse kinematics solution sequence when the rotation angle value of the first joint or the rotation angle value of the second joint of the SSRMS configuration robotic arm of the present invention is given.

[0323] Figure 9 This is the zero-position configuration diagram of the equivalent six-degree-of-freedom manipulator when the rotation angle value of the sixth joint of the SSRMS configuration manipulator of the present invention is given;

[0324] Figure 10 This is the zero-position configuration diagram of the equivalent six-degree-of-freedom manipulator when the rotation angle value of the seventh joint of the SSRMS configuration manipulator of the present invention is given;

[0325] Figure 11 This is a schematic diagram of the inverse kinematics solution sequence when the rotation angle value of the sixth joint or the rotation angle value of the seventh joint of the SSRMS configuration robotic arm of the present invention is given.

[0326] Figure 12 This 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

[0327] 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.

[0328] A method for solving the inverse kinematics of an SSRMS configuration robotic arm based on dual quaternions, the method comprising the following steps:

[0329] S1: Establish the reference coordinate system for the SSRMS configuration robotic arm and the end coordinate system Define the link length of each joint of the SSRMS configuration 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... ;

[0330] Table 1. Plück coordinates of joints in the SSRMS-configured robotic arm

[0331]

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

[0333] S2: Calculate the end-effector coordinate system during the rotational motion of the SSRMS configuration robotic arm using dual quaternions. Relative reference coordinate system position :

[0334] (1)

[0335] In formula (1):

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

[0337] The SSRMS-configured robotic arm is in a zero-position configuration. Time end coordinate system Relative reference coordinate system The pose, where: , ;

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

[0339] Table 2 Dual Quaternions of Joint Rotation Axes in SSRMS-Configured Robotic Arms

[0340]

[0341] When the rotation angle values ​​of any one of the first, second, sixth, and seventh joints of the SSRMS configuration robotic arm are given, the SSRMS configuration robotic arm can be equivalent to an equivalent six-degree-of-freedom robotic arm with continuously parallel joint axes of the third, fourth, and fifth joints. According to Pieper's theorem, given the end-effector coordinate system... Relative reference coordinate system position Each joint of the equivalent six-degree-of-freedom robotic arm has an analytical solution.

[0342] S3: Inverse kinematics derivation based on dual quaternions when the first joint is fixed;

[0343] S3 includes the following steps:

[0344] S301: Constructing an equivalent six-DOF robotic arm: such as Figure 2 As shown, the rotation angle value of the first joint of the SSRMS configuration robotic arm Given that the joint axes of the third joint, the fourth joint, and the fifth joint are continuously parallel to each other;

[0345] Then the equivalent six-degree-of-freedom robotic arm is in the zero-position configuration (the rest... - When both are 0, the final coordinate system Relative reference coordinate system position (The known terms when solving the inverse kinematics) are:

[0346] (2)

[0347] S302: The formula for calculating the forward kinematics of an equivalent six-DOF robotic arm is:

[0348] (3)

[0349] In formula (3):

[0350] These are the pose vector parameters;

[0351] S303: According to Figure 2From the configuration characteristics of an equivalent six-DOF manipulator, it is known that after the first joint rotates, the positions of all other joints change. Therefore, given the rotation angle θ1 of the first joint, the dual quaternions of the joint axes of the equivalent six-DOF manipulator can be obtained as follows:

[0352] (4)

[0353] Table 3. Dual quaternions of the equivalent six-DOF robotic arm joint positions when the first joint rotation angle value is given.

[0354]

[0355] The rotation angle value of the first joint can be obtained. Given, the dual quaternions for rotational transformations about the remaining joints:

[0356] (5)

[0357] Table 4. Dual quaternions of the rotation axes of each joint of the equivalent six-DOF robotic arm when the rotation angle of the first joint is given.

[0358]

[0359] S304: Solve for the rotation angle of the second joint. The rotation angle value of the sixth joint and the rotation angle value of the seventh joint ;

[0360] S30401: The third joint dual quaternion on the right side of retained expression (3) 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:

[0361] (6)

[0362] In formula (6):

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

[0364] (7)

[0365] In equation (7):

[0366] and Represents any number;

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

[0368] Denotes the Euclidean norm;

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

[0370] (8)

[0371] When the first or second joint rotates, its position coordinates remain unchanged, then the plane The dual quaternion is:

[0372] (9)

[0373] In equation (9):

[0374] It is a column vector with all elements being 0;

[0375] Based on the configuration characteristics of the equivalent six-degree-of-freedom 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, and 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 (9) will not change.

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

[0377] (10)

[0378] In formula (10):

[0379] dual quaternions Another conjugate dual quaternion;

[0380] 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 it exists:

[0381] (11)

[0382] Combining equations (10) and (11), we obtain:

[0383] (12)

[0384] In equation (12):

[0385] Describing dual quaternions Another conjugate dual quaternion;

[0386] S30404: The sixth joint dual quaternion on the right side of retained expression (12) and the seventh joint dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0387] (13)

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

[0389] (14)

[0390] exist Figure 4 In the middle, plane Rotate around the seventh joint to obtain a plane During the process, the joint axis of the seventh 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:

[0391] (15)

[0392] In equation (15):

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

[0394] For plane The normal vector;

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

[0396] Joint axis The direction vector;

[0397] Joint axis Line spacing;

[0398] According to Table 6, the dual quaternion of the sixth joint can be obtained as follows:

[0399] (16)

[0400] Combining equations (9) and (16), we can obtain:

[0401] (17)

[0402] S30405: Dual quaternions for the joint axis of the seventh joint. Combining equations (17) and (15), we obtain the joint axis dual quaternion of the seventh joint. With plane intersection for:

[0403] (18)

[0404] In equation (18):

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

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

[0407] (19)

[0408] Combining equations (19) and (15) yields the joint axis dual quaternion of the seventh joint. With plane intersection :

[0409] (20)

[0410] In equation (20):

[0411] ;

[0412] ;

[0413] in: , , , , , , ;

[0414] S30406: Based on the above analysis, the intersection point... Intersection If the point is the same, then the terms in equations (18) and (20) are equal, and we can obtain:

[0415] (twenty one)

[0416] (twenty two)

[0417] Combining equations (21) and (22), we can obtain the rotation angle value containing only the second joint. Relationship:

[0418] (twenty three)

[0419] In equation (23):

[0420] ;

[0421] ;

[0422] ;

[0423] S30407: Therefore, the rotation angle value of the second joint can be obtained. Two solutions:

[0424] (twenty four)

[0425] Substituting equation (24) into equation (21), we can obtain the rotation angle value of the sixth joint. Two solutions:

[0426] (25)

[0427] By combining equations (9) and (14), we can obtain the plane. The dual quaternion is:

[0428] (26)

[0429] The rotation angle value of the second joint Substituting into equation (19), we obtain the plane. The dual quaternion is:

[0430] (27)

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

[0432] According to equation (13), the plane and plane If all terms in the equations are equal, then by combining equations (26) and (27), we can obtain:

[0433] (28)

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

[0435] (29)

[0436] S305: Solve for the rotation angle of the fourth joint. The rotation angle value of the third joint And the rotation angle value in section five. ;

[0437] S30501: Combining equations (2) and (5), we can obtain:

[0438] (30)

[0439] By combining equations (6) and (30), we can obtain:

[0440] (31)

[0441] S30502: Dual quaternion of the third joint The fourth joint dual quaternion and the fifth joint dual quaternion Substitution formula (31):

[0442] (32)

[0443] (33)

[0444] (34)

[0445] (35)

[0446] Due to the fact that in equation (31) , Each item is equal, and Given the terms, by combining equations (32) and (33), we can obtain:

[0447] (36)

[0448] By combining equations (32) and (35), we can obtain:

[0449] (37)

[0450] (38)

[0451] Squaring both sides of equations (37) and (38) and summing them:

[0452] (39)

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

[0454] (40)

[0455] S30503: Use the sum-to-product formula in equations (37) and (38) to expand trigonometric functions:

[0456] (41)

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

[0458] (42)

[0459] By combining equations (36), (40), and (42), the rotation angle value of the fifth joint can be obtained. for:

[0460] (43)

[0461] Based on the above analysis, the rotation angle value of the first joint of the SSRMS configuration robotic arm 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.

[0462] S4: Inverse kinematics derivation based on dual quaternions when the second joint is fixed;

[0463] S4 includes the following steps:

[0464] S401: According to As can be seen from the configuration characteristics of a moderately efficient six-DOF robotic arm, the first joint is not affected by the rotation angle of the second joint. The third, fourth, fifth, sixth, and seventh joints are all affected by the rotation angle of the second joint. The influence of this. Therefore, the rotation angle value of the second joint. Given these values, we can obtain the dual quaternions for each joint axis of the equivalent six-DOF robotic arm:

[0465] (44)

[0466] Table 5. Dual quaternions of the equivalent six-DOF robotic arm joint positions when the second joint rotation angle value is given.

[0467]

[0468] S402: The formula for calculating the forward kinematics of an equivalent six-DOF robotic arm is:

[0469] (45)

[0470] The rotation angle value of the second joint can be obtained. Given the dual quaternions for rotational transformations around the remaining joints. for:

[0471] (46)

[0472] Table 6. Dual Quaternions of the Rotation Axes of Each Joint of the Equivalent Six-DOF Robotic Arm When the Rotation Angle Value of the Second Joint is Given

[0473]

[0474] S403: Solve for the rotation angle of the first joint. The rotation angle value of the sixth joint and the rotation angle value of the seventh joint ;

[0475] S40301: The third joint dual quaternion on the right side of the retained expression (45) 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:

[0476] (47)

[0477] S40302: Assume there exists a point that simultaneously passes through the location of the second joint. and the location of the third joint And the direction vector of the joint axis of the third joint Vertical plane ,like Figure 6 As shown, the plane can be obtained. To the reference coordinate system vertical distance from the origin for:

[0478] (48)

[0479] Then the plane The dual quaternion is:

[0480] (49)

[0481] 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 (49) will not change.

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

[0483] (50)

[0484] 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:

[0485] (51)

[0486] S40304: The sixth joint dual quaternion in retention formula (51) The quaternion of the seventh joint The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in:

[0487] (52)

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

[0489] (53)

[0490] S40305: Plane Rotate around the seventh joint to obtain a plane During the process, the joint axis of the seventh joint has a dual quaternion. With plane and plane The intersection points are the same as the intersection points. ,like Figure 7 As shown, the dual quaternion of the sixth joint can be obtained according to Table 6:

[0491] (54)

[0492] By combining equations (54) and (49), we can obtain:

[0493] (55)

[0494] Based on Table 5, the joint axis dual quaternions of the seventh joint are known. Substituting equation (55) into the Clifford algebraic cross product (15) of the joint axis and the plane, we can obtain the intersection point. for:

[0495] (56)

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

[0497] (57)

[0498] By combining equations (45) and (57), the joint axis dual quaternion of the seventh joint is obtained. With plane intersection for:

[0499] (58)

[0500] In equation (58):

[0501] ;

[0502] ;

[0503] S40306: Based on the above analysis, the intersection point... Intersection If the point is the same, then the terms in equations (56) and (58) are equal, and we can obtain:

[0504] (59)

[0505] (60)

[0506] Combining equations (58) and (59), we can obtain the rotation angle value containing only the first joint. Relationship:

[0507] (61)

[0508] In equation (61):

[0509] ;

[0510] ;

[0511] ;

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

[0513] (62)

[0514] Substituting equation (62) into equation (59) yields the rotation angle value of the sixth joint. Two solutions:

[0515] (63)

[0516] By combining equations (53) and (57), we can obtain the plane. and plane The dual quaternion is:

[0517] (64)

[0518] (65)

[0519] In equations (64) and (65), the pose is in the zero-position configuration. , pose First joint dual quaternion and plane All are known terms, therefore, the plane The second, third, and fourth components , , It is also a known item.

[0520] According to equation (52), the plane and plane If all terms in the equation are equal, then we can obtain the following formula:

[0521] (66)

[0522] therefore, ( When the angle value θ7 of joint 7 is usually taken as 0, we can obtain:

[0523] (67)

[0524] S404: Solve for the rotation angle of the fourth joint. The rotation angle value of the third joint And the rotation angle value of the fifth joint ;

[0525] S40401: Combining equations (44) and (45), we can obtain:

[0526] (68)

[0527] Combining equations (46) and (68), we obtain:

[0528] (69)

[0529] S40402: Obtain the rotation angle value of the first joint. The rotation angle value of the sixth joint and the rotation angle value of the seventh joint Then, substituting into equation (69), and following the solution process of equations (32)-(43), the rotation angle value of the fourth joint is obtained similarly. The rotation angle value of the third joint And the rotation angle value of the fifth joint ;

[0530] Based on the above analysis, the rotation angle value of the second joint of the SSRMS configuration robotic arm can be obtained. Given the analytical solutions for the remaining joint angles, where the rotation angle value of the first joint is... Rotation angle values ​​of the four joints and the rotation angle value of the sixth joint There are two solutions. The SSRMS configuration robotic arm has a given rotation angle value for the first joint. Or the rotation angle value of the second joint When solving for the rotation angles of the remaining joints, the order is as follows: Figure 8 As shown.

[0531] S5: Inverse kinematics derivation based on dual quaternions when the sixth joint is fixed;

[0532] S5 includes the following steps:

[0533] S501: According to Figure 9 As can be seen from the configuration characteristics of a moderately efficient six-DOF robotic arm, only the position of the seventh joint is affected by the rotation angle of the sixth joint. Due to the influence of [various factors], the formula for calculating the forward kinematics of the equivalent six-degree-of-freedom robotic arm is:

[0534] (70)

[0535] The rotation angle value of the sixth joint can be obtained. Given the dual quaternions for rotational transformations around the remaining joints. for:

[0536] (71)

[0537] Dual quaternions for the joint axes of an equivalent six-DOF robotic arm:

[0538] (72)

[0539] S502: Solve for the rotation angle of the first joint. The rotation angle value of the second joint and the rotation angle value of the seventh joint ;

[0540] S50201: The third joint dual quaternion on the right side of the retained expression (70) 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:

[0541] (73)

[0542] S50202: Assume there exists a point that simultaneously passes through the location of the second joint. and the location of the third 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:

[0543] (74)

[0544] Then the plane The dual quaternion is:

[0545] (75)

[0546] 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 (75) will not change.

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

[0548] (76)

[0549] 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:

[0550] (77)

[0551] (78)

[0552] S50204: The first joint dual quaternion in equation (78) Second joint dual quaternion The relevant terms are moved 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:

[0553] (79)

[0554] In rigid body transformations, any plane, after rotating about an axis by a certain angle, still retains its planar structure. According to equation (79), 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:

[0555] (80)

[0556] S50205: Plane Rotate around the first joint to obtain a plane 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. ;

[0557] By combining equations (75) and (71), we can obtain:

[0558] (81)

[0559] The intersection point can be obtained by using the Clifford algebraic cross product of the joint axis and the plane (15). for:

[0560] (82)

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

[0562] (83)

[0563] According to equation (15), the joint axis dual quaternion of the first joint is obtained. With plane intersection for:

[0564] (84)

[0565] In equation (84):

[0566] ;

[0567]

[0568] S50206: Based on the above analysis, it can be seen that the terms in equations (82) and (84) are equal, and we can obtain:

[0569] (85)

[0570] (86)

[0571] Combining equations (85) and (86), we obtain:

[0572] (87)

[0573] In equation (87):

[0574] ;

[0575] ;

[0576] ;

[0577] S50207: Therefore, the rotation angle value of the seventh joint can be obtained. Two solutions:

[0578] (88)

[0579] Substituting equation (88) into equation (85) yields the rotation angle value of the second joint. Two solutions:

[0580] (89)

[0581] A plane can be obtained The dual quaternion is:

[0582] (90)

[0583] Substituting equation (88) into equation (83) yields the plane. The dual quaternion is:

[0584] (91)

[0585] Since all terms in equations (90) and (91) are equal, we can obtain:

[0586] (92)

[0587] S503: Solve for the rotation angle of the fourth joint. The rotation angle value of the third joint and the rotation angle value of the fifth joint ;

[0588] S50301: According to Figure 9 Based on the configuration characteristics of a moderately efficient six-DOF robotic arm, the rotation angle value of the sixth joint is... The change only affects the position of the seventh joint, which can be obtained according to equation (71):

[0589] (93)

[0590] By combining equations (73) and (93), we can obtain:

[0591] (94)

[0592] S50302: Obtain the rotation angle value of the first joint. The rotation angle value of the second joint and the rotation angle value of the seventh joint Then, substituting into equation (94), and following the solution process of equations (32)-(43), the rotation angle value of the fourth joint is obtained similarly. The rotation angle value of the third joint and the rotation angle value of the fifth joint ;

[0593] Based on the above analysis, the rotation angle value of the sixth joint of the SSRMS configuration robotic arm can be obtained. Given the analytical solutions for the remaining joint angles, where the rotation angle value of the second joint is... Rotation angle values ​​of the four joints and the rotation angle value of the seventh joint There are two solutions.

[0594] S6: Inverse kinematics derivation based on dual quaternions when the seventh joint is fixed;

[0595] S6 includes the following steps:

[0596] S601: According to Figure 10 Based on the configuration characteristics of a moderately efficient six-DOF robotic arm, the rotation angle value of the seventh joint is... When a change occurs, it can be understood as the end coordinate system. Around itself X e The axis rotates, but the other joints are unaffected; at this time, the end-effector coordinate system... Relative reference coordinate system The pose is:

[0597] (95)

[0598] Therefore, the forward kinematics calculation formula for the equivalent six-degree-of-freedom robotic arm can be obtained as follows:

[0599] (96)

[0600] The dual quaternions can be obtained when rotational transformations occur around the remaining joints. for:

[0601] (97)

[0602] The dual quaternions of the joint axes of the equivalent six-DOF robotic arm can be obtained. for:

[0603] (98)

[0604] S602: Solve for the rotation angle of the first joint. The rotation angle value of the second joint and the rotation angle value of the sixth joint ;

[0605] S60201: The third joint dual quaternion on the right side of the retained expression (96) 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:

[0606] (99)

[0607] S60202: Assume there exists a point that simultaneously passes through the location of the second joint. and the location of the third 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:

[0608] (100)

[0609] Then the plane The dual quaternion is:

[0610] (101)

[0611] 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 (101) will not change.

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

[0613] (102)

[0614] 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:

[0615] (103)

[0616] (104)

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

[0618] (105)

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

[0620] (106)

[0621] S60205: According to equations (101) and (97), we can obtain:

[0622] (107)

[0623] S60206: Plane Rotate around the first joint to obtain a plane 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. Substituting into the Clifford algebraic cross product of the joint axis and the plane (15), we can obtain the intersection point. for:

[0624] (108)

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

[0626] (109)

[0627] The joint axis dual quaternion of the first joint With plane intersection for:

[0628] (110)

[0629] In equation (110):

[0630] ;

[0631] ;

[0632] S60207: According to equation (105), the plane With plane Since they are on the same plane, the intersection point Intersection If the terms in equations (108) and (110) are equal at the same point, then we can obtain:

[0633] (111)

[0634] (112)

[0635] By combining equations (111) and (112), we can obtain:

[0636] (113)

[0637] In equation (113):

[0638] ;

[0639] ;

[0640] ;

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

[0642] (114)

[0643] Substituting equation (114) into equation (85) yields the rotation angle value of the second joint. Two solutions:

[0644] (115)

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

[0646] (116)

[0647] According to equation (109), the plane can be obtained. The dual quaternion is:

[0648] (117)

[0649] Due to the plane and plane If all terms in the equation are equal, then we can obtain:

[0650] (118)

[0651] S603: Solve for the rotation angle of the fourth joint. The rotation angle value of the third joint And the rotation angle value of the fifth joint ;

[0652] S60301: Due to the rotation angle value of the seventh joint Since the change does not affect the position of the other joints, we can conclude that:

[0653] (119)

[0654] Combining equations (89), (87), and (119), we obtain:

[0655] (120)

[0656] S60302: Obtain 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 Then, substituting into equation (120), and following the solution process of equations (32)-(43), the rotation angle value of the fourth joint is obtained similarly. The rotation angle value of the third joint And the rotation angle value of the fifth joint ;

[0657] Based on the above analysis, the rotation angle value of the seventh joint of the SSRMS configuration robotic arm can be obtained. Given the analytical solutions for the remaining joint angles, where the rotation angle value of the second joint is... Rotation angle values ​​of the four joints and the rotation angle value of the sixth joint There are two solutions. The SSRMS configuration robotic arm has a given rotation angle value for the sixth joint. Or the rotation angle value of the seventh joint When solving for the rotation angles of the remaining joints, the order is as follows: Figure 11 As shown.

[0658] S7: Verify the solution method.

[0659] To demonstrate the effectiveness of the inverse kinematics solution algorithm for the SSRMS configuration robotic arm proposed in this invention, given the rotation angle value of the second joint... Taking the solution of the remaining joint angles as an example, we compare it with the existing inverse kinematics solution methods in references 1 and 2.

[0660] The specific comparison steps are as follows:

[0661] S701: In MATLAB, use the random function unifrnd to randomly generate 10,000 sets of robotic arm configurations (each configuration represents a set of joint angles), and substitute them into equation (1) to obtain the end pose of each configuration.

[0662] S502: Rotation angle values ​​based on end pose and each configuration. As initial conditions, inverse kinematics were solved using three methods (Method 1 in Reference 1, Method 2 in Reference 2, and the inverse kinematics solution algorithm in this invention), and the total solution time for 10,000 sets of inverse kinematics with different mechanism types was statistically analyzed. The timing method used was the built-in MATLAB tic-toc timing function, 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.0GB RAM.

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

[0664] (121)

[0665] In equation (121):

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

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

[0668] Table 7 Solution time for different inverse kinematics methods

[0669]

[0670] Table 8. Average solution time for different inverse kinematics methods

[0671]

[0672] Comprehensive analysis Figure 12 As shown in Table 7, the inverse kinematics solution algorithm in Reference 2 requires the longest computation time, followed by the inverse kinematics solution algorithm in Reference 1, while the inverse kinematics solution algorithm 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.

[0673] Comprehensive analysis Figure 12 As shown in Table 8, 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 algorithm in this invention is greatly improved.

[0674] Fundamental properties of dual quaternions:

[0675] 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).

[0676] 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.

[0677] 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 an SSRMS configuration robotic arm based on dual quaternions, characterized in that: Includes the following steps: S1: Establish the reference coordinate system for the SSRMS configuration robotic arm and the end coordinate system Define the link length of each joint of the SSRMS configuration 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... ; S2: Calculating the rotational motion of the SSRMS configuration robotic arm using dual quaternions relatively position : (1) These are the rotation angle values ​​for each joint; When the SSRMS configuration robotic arm is in the zero position configuration relatively The pose, where: , ; Let be the dual quaternions for each joint rotation, where: As a dual unit; S3: Inverse kinematics derivation based on dual quaternions when the first joint is fixed; S301: Constructing an equivalent six-DOF robotic arm: Rotation angle values ​​of the first joint of the SSRMS configuration robotic arm Given that the joint axes of the third joint, the fourth joint, and the fifth joint are continuously parallel to each other; When the equivalent six-DOF robotic arm is in the zero-position configuration, relatively position for: (2) S302: The formula for calculating the forward kinematics of an equivalent six-DOF robotic arm is: (3) These are the pose vector parameters. ; S303: Given θ1, the dual quaternions of the joint axes of the equivalent six-DOF robotic arm are: (4) achievable Given, the dual quaternions for rotational transformations about the remaining joints: (5) S304: Retained form (3) on the right side , as well as The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in: (6) for The conjugate dual quaternions of , and the following relation exists: (7) and Represents any number; and Represents an arbitrary three-dimensional vector; Denotes the Euclidean norm; Assume there exists a point where the second joint is located simultaneously. and the location of the third joint And the direction vector of the joint axis of the third joint Vertical plane Then a plane can be obtained. arrive vertical distance from the origin for: (8) but The dual quaternion is: (9) It is a column vector with all elements being 0; When rotating around the fifth, fourth, and third joints in sequence, according to the property of planar rotation around an axis in equation (6), we can obtain the following equation: (10) for Another conjugate dual quaternion; because The joint axes of the third, fourth, and fifth joints remain perpendicular, and after rotation around these three joints, The relationship remains unchanged, meaning it exists: (11) Combining equations (10) and (11), we obtain: (12) In equation (12): express Another conjugate dual quaternion; The right side of retained form (12) as well as The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in: (13) After rotating around the sixth joint, a plane is obtained. , After rotating around the seventh joint, a plane is obtained. , and The following relationship exists: (14) get During the process, and and The intersection points are the same as the intersection points. The result is obtained using the Clifford algebraic outer product of the joint axis and the plane. coordinate: (15) 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 sixth joint: (16) The same applies to the rest; Combining equations (9) and (16), we can obtain: (17) Will Combining equations (17) and (15), we obtain for: (18) Unused items; The plane is obtained by rigid body transformation on the left side of equation (13). : (19) Combining equations (19) and (15), we obtain : (20) ； ； in: , , , , , , ; It can be seen that, and If the point is the same, then the terms in equations (18) and (20) are equal, and we can obtain: (21) (22) Combining equations (21) and (22), we can obtain only those containing Relationship: (23) ； ； ； Therefore, we can obtain Two solutions: (24) Substituting equation (24) into equation (21), we get Two solutions: (25) By combining equations (9) and (14), we can obtain The dual quaternion is: (26) Will Substituting into equation (19), we get The dual quaternion is: (27) According to equation (13), and If all terms in the equations are equal, then by combining equations (26) and (27), we can obtain: (28) therefore, When it is time, we can obtain: (29) S305: Combining equations (2) and (5), we can obtain: (30) By combining equations (6) and (30), we can obtain: (31) Will , as well as Substitution formula (31): (32) (33) (34) (35) By combining equations (32) and (33), we can obtain: (36) By combining equations (32) and (35), we can obtain: (37) (38) Squaring both sides of equations (37) and (38) and summing them: (39) therefore, for: (40) In equations (37) and (38), trigonometric function expansions are performed using the sum-to-product formula: (41) achievable for: (42) By combining equations (36), (40), and (42), we can obtain... for: (43) S4: Inverse kinematics derivation based on dual quaternions when the second joint is fixed; S5: Inverse kinematics derivation based on dual quaternions when the sixth joint is fixed; S6: Inverse kinematics derivation based on dual quaternions when the seventh joint is fixed; S7: Verify the solution method.

Images