SSRMS configuration mechanical arm inverse kinematics solving method based on dual quaternion

Abstract

The invention discloses a dual quaternion-based SSRMS configuration mechanical arm inverse kinematics solving method, and belongs to the technical field of mechanical arm design. The method comprises the following steps that a reference coordinate system and a tail end coordinate system are established, 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 are defined, and then the Pluecker coordinate of each joint is obtained; the pose of the tail end coordinate system relative to the reference coordinate system when the mechanical arm rotates is calculated through the dual quaternion; inverse kinematics derivation is conducted when the first joint, the second joint, the sixth joint and the seventh joint are fixed; and verifying the solving method. According to the method, the constraint relation between the rotation angle values is established, the technical bottleneck that multi-configuration optimization and real-time performance are difficult to consider in traditional inverse kinematics solution is broken through, and the inverse kinematics solution speed is remarkably increased while the solution precision is guaranteed.

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 formula (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 The change does not affect the position of the other joints, therefore:

[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 joint positions of the equivalent six-DOF robotic arm 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 The change does not affect the position of the other joints, therefore:

[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 Figure 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 Figure 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 inverse kinematics solution of SSRMS configuration manipulator based on dual quaternion, characterized in that: The method comprises the following steps: S1: Establish the reference coordinate system of SSRMS configuration manipulator and the end coordinate system , define the link length of each joint of SSRMS configuration manipulator as , the position coordinates of each joint in the reference coordinate system are , the unit direction vector of each joint is , the wire distance of each joint is , then the Plucker coordinates of each joint, i.e. the joint axis, are ; S2: Calculate the end coordinate system of SSRMS configuration manipulator rotating motion by using dual quaternion Relative reference coordinate system Pose : (1) In formula (1): is a rotation angle value of each joint; end effector coordinate frame of the SSRMS configuration manipulator in a zero position configuration relative reference coordinate frame pose of the end effector coordinate frame with respect to the relative reference coordinate frame, wherein: , ; is the dual quaternion for each joint rotation, where: is the dual unit; S3: Inverse kinematics derivation based on dual quaternion when the first joint is fixed; S4: Inverse kinematics derivation based on dual quaternion when the second joint is fixed; S5: Inverse kinematics derivation based on dual quaternion when the sixth joint is fixed; S6: Inverse kinematics derivation based on dual quaternion when the seventh joint is fixed; S7: Verification of the solving method is performed.

2. The inverse kinematics solving method for SSRMS configuration manipulator based on dual quaternion according to claim 1, characterized in that: The S3 comprises the following steps: S301: Construct an equivalent six-degree-of-freedom mechanical arm: the rotation angle value of the first joint of the SSRMS configuration mechanical arm At a given time, the joint axis of the third joint, the joint axis of the fourth joint, and the joint axis of the fifth joint are successively parallel to each other; The pose of the end coordinate system of the equivalent six-degree-of-freedom robot arm relative to the reference coordinate system is: when the equivalent six-degree-of-freedom robot arm is in a zero position configuration is: ​ (2) S302: The forward kinematics calculation formula of the equivalent six-degree-of-freedom manipulator is: (3) In formula (3): is a pose vector parameter; S303: The rotation angle value of the first joint Given the above, the dual quaternion of each joint axis of the equivalent six-degree-of-freedom robot arm is: (4) The rotation angle value of the first joint can be obtained Given the rotation transformation around the remaining joints, the dual quaternion is given by: (5) S304: Solve the rotation angle value of the second joint , the rotation angle value of the sixth joint , and the rotation angle value of the seventh joint ; S30401: the relevant term of the third joint pair dual quaternion on the right side of equation (3) , the fourth joint pair dual quaternion , and the fifth joint pair dual quaternion , and the rest of the terms are transferred to the left side of the equation using the conjugate property of dual quaternions, then we get: (6) In formula (6): is the conjugate dual quaternion of the dual quaternion and the following relation holds: (7) In formula (7): and denotes any number; and denotes any three-dimensional vector; denotes the Euclidean norm; 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: (8) Then the plane The dual quaternion of the plane is (9) In formula (9): is a column vector with elements all equal to 0; S30403: plane When the plane is rotated sequentially about the fifth joint, the fourth joint and the third joint, the following equation is obtained from the property of the rotation of the plane about the axis in equation (6): (10) In formula (10): For dual quaternions Another conjugate dual quaternion of the dual quaternion Since the plane is always perpendicular to the joint axis of the third joint, the joint axis of the fourth joint and the joint axis of the fifth joint, the plane does not change after the rotation about the three joints, i.e. the relationship holds: (11) By combining formula (10) and formula (11), the following can be obtained: (12) In formula (12): representing a dual quaternion another conjugate dual quaternion of S30404: the sixth joint pair dual quaternion on the right side of the reserved equation (12) and the seventh joint pair dual quaternion The relevant terms are transferred to the left side of the equation using the conjugate property of dual quaternions, resulting in: (13) plane plane obtained after rotation about the sixth joint , plane plane obtained after rotation about the seventh joint , plane plane has the following relationship: (14) plane rotation about the seventh joint yields a plane during the process, the joint axis of the seventh joint to the dual quaternion is the intersection point of the plane and the plane is the intersection point of the plane the intersection point is obtained using the Clifford algebra outer product of the joint axis and the plane coordinates: (15) In formula (15): Clifford algebra exterior product; is the normal vector to the plane is the normal vector to the plane is a plane distance to the origin of the local coordinate system; the direction vector of the joint axis of the joint axis axis of the joint line distance; Then the sixth joint dual quaternion can be obtained: (16) By combining formula (9) and formula (16), the following can be obtained: (17) 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: (18) In formula (18): is an unused item; plane The plane is obtained from the rigid body transformation of the left side of equation (13) : (19) Solving equations (19) and (15) simultaneously gives the joint axis dual quaternions of the seventh joint the intersection of the plane with the plane : (20) In formula (20): ； ； wherein: , , , , , , ; S30406: it is known that the intersection with the intersection point, then each of the terms in equation (18) and equation (20) are equal, respectively, and we have: (21) (22) The simultaneous equations (21) and (22) can be solved to obtain the value of the rotation angle of the second joint only the relationship: (23) In formula (23): ； ； ； S30407: thus the rotation angle value of the second joint is obtained two solutions: (24) Substituting equation (24) into equation (21), the rotation angle value of the sixth joint can be obtained two solutions: (25) The dual quaternion of the plane of the equations (9) and (14) is: is: (26) The rotation angle value of the second joint is substituted into formula (19) The dual quaternion of the plane is: (27) According to equation (13), the planes and are equal, then equations (26) and (27) can be obtained simultaneously. (28) Thus, Thus, (29) S305: Solve the rotation angle value of the fourth joint , the rotation angle value of the third joint , and the rotation angle value of the fifth joint ; S30501: By combining formula (2) and formula (5), the following can be obtained: (30) By combining formula (6) and formula (30), the following can be obtained: (31) S30502: a third joint dual quaternion , a fourth joint dual quaternion , and a fifth joint dual quaternion into equation (31): (32) (33) (34) (35) By combining formula (32) and formula (33), the following can be obtained: (36) By combining formula (32)-(35), the following can be obtained: (37) (38) Take square of both sides of formula (37) and formula (38) and sum them up: (39) Thus, the rotation angle value of the fourth joint is : (40) S30503: In formula (37) and formula (38), the trigonometric function expansion is performed by using the sum and difference product formula: (41) The rotation angle value of the third joint can be obtained is: (42) Simultaneously, the rotation angle value of the fifth joint can be obtained according to equation (36), equation (40) and equation (42) is: (43)。 3. The inverse kinematics solving method for SSRMS manipulator based on dual quaternion according to claim 2, characterized in that: The S4 comprises the following steps: S401: A rotation angle value of the second joint Given the above, the dual quaternions of the joint axes of the equivalent six-DOF manipulator are obtained: (44) S402: The forward kinematics calculation formula of the equivalent six-degree-of-freedom manipulator is: (45) The rotation angle value of the second joint can be obtained The dual quaternion when a rotation transformation occurs around the remaining joints given the posterior is: (46) S403: Solve 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 ; S40301: the relevant term of the third joint pair dual quaternion on the right side of equation (45) , the fourth joint pair dual quaternion and the fifth joint pair dual quaternion are transferred to the left side of the equation by using the conjugate property of the dual quaternion, and then we have: (47) 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: (48) Then the plane The dual quaternion of is (49) S40303: plane When the plane is rotated sequentially about the fifth joint, the fourth joint, and the third joint, according to the property of the rotation of the plane about an axis in equation (47), we have (50) Since the plane is always perpendicular to the joint axis of the third joint, the joint axis of the fourth joint and the joint axis of the fifth joint, the plane does not change after the rotation about the three joints, i.e. the relationship holds: (51) S40304: the relevant term of the sixth joint pair dual quaternion in formula (51) and the seventh joint pair dual quaternion is transferred to the left side of the formula using the conjugate property of the dual quaternion, and the remaining terms are obtained: (52) plane plane obtained after rotation about the sixth joint , plane plane obtained after rotation about the seventh joint , plane there is a relationship between the plane ​ (53) S40305: plane Rotation about the seventh joint yields a plane During the process, the joint axis of the seventh joint is the intersection point of the plane and the plane and the intersection point of the plane is the intersection point Then the dual quaternion of the sixth joint can be obtained: (54) By combining formula (54) and formula (49), the following can be obtained: (55) Dual quaternions of joint axes of the seventh joint Substituting equation (55) into the Clifford algebra outer product equation (15) for the joint axis and plane gives the intersection point as: (56) plane The plane is obtained from the rigid body transformation of the left side of equation (52) : (57) Substituting equation (45) into equation (57) gives the joint axis dual quaternion of the seventh joint the intersection of the plane with the line is: (58) In formula (58): ； ； S40306: It is known that the intersection point is the same point, each of the terms in equation (56) and equation (58) is equal, and the following can be obtained: (59) (60) The simultaneous equations (58) and (59) can be solved to obtain the rotation angle values of the first joint only the relationship: (61) In formula (61): ； ； ； S40307: thus the rotation angle value of the first joint can be obtained two solutions: (62) Substituting equation (62) into equation (59) gives the value of the rotation angle of the sixth joint two solutions: (63) The dual quaternions of the plane and the plane are (64) (65) According to equation (52), the planes and are equal, respectively, then the equation is obtained: (66) Thus, Thus, (67) S404: Solve the rotation angle value of the fourth joint , the rotation angle value of the third joint , and the rotation angle value of the fifth joint ; S40401: By combining formula (44) and formula (45), the following can be obtained: (68) By combining formula (46) and formula (68), the following can be obtained: (69) 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 , the rotation angle value of the fourth joint , the rotation angle value of the third joint , and the rotation angle value of the fifth joint are obtained by substituting the above values into formula (69) and following the solving process of formulae (32)-(43).

4. The dual quaternion-based inverse kinematics solving method for SSRMS configuration robotic arms according to claim 3, wherein: The S5 comprises the following steps: S501: The forward kinematics calculation formula of the equivalent six-degree-of-freedom manipulator is: (70) The rotation angle value of the sixth joint can be obtained The dual quaternion when a rotation transformation occurs around the remaining joints given the posterior is: (71) The dual quaternion of each joint axis of the equivalent six-degree-of-freedom manipulator: (72) S502: Solve 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 ; S50201: the relevant terms of the third joint pair dual quaternion on the right side of equation (70) , the fourth joint pair dual quaternion and the fifth joint pair dual quaternion are transferred to the left side of the equation using the conjugate property of dual quaternions, and then we have: (73) S50202: Assuming that there exists a plane which is perpendicular to the direction vector of the joint axis of the third joint and which passes through the position of the second joint and the position of the third joint , then the perpendicular distance of the plane to the origin of the reference coordinate system is obtained as follows: ​​​​​ (74) Then the plane The dual quaternion of the plane is (75) S50203: plane Upon sequentially rotating about the fifth joint, the fourth joint, and the third joint, according to the property of the plane rotating about an axis in equation (73), we have: (76) Since the plane is always perpendicular to the joint axis of the third joint, the joint axis of the fourth joint and the joint axis of the fifth joint, the plane does not change after the rotation about the three joints, i.e. the relationship holds: (77) (78) S50204: The relevant terms of the first joint dual quaternion and the second joint dual quaternion are transferred to the left side of the equation using the conjugate property of dual quaternions, and the remaining terms remain on the right side of the equation, resulting in: (79) plane plane obtained after rotation about the second joint , plane plane obtained after rotation about the first joint , plane there is a relationship between the plane ​ (80) S50205: plane Rotating about the first joint results in a plane During the process, the joint axis of the first joint is the intersection point of the plane and the plane is the intersection point ; By combining formula (75) and formula (71), the following can be obtained: (81) According to the Clifford algebra outer product (15) of the joint axis and the plane, the intersection point is: (82) plane The plane is obtained from the rigid body transformation of the left side of equation (79) : (83) According to equation (15), the joint axis dual quaternion of the first joint is obtained the intersection of the plane with the line is: (84) In formula (84): ； S50206: It can be known that each term in formula (82) and formula (84) is equal, and the following can be obtained: (85) (86) By combining formula (85) and formula (86), the following can be obtained: (87) In formula (87): ； ； ； S50207: Thus the rotation angle value of the seventh joint can be obtained two solutions: (88) Substituting equation (88) into equation (85) gives the value of the rotation angle of the second joint two solutions: (89) available planes The dual quaternion of is: (90) Substituting equation (88) into equation (83) gives the plane The dual quaternion of the plane (91) Since each term in formula (90) and formula (91) is equal, the following can be obtained: (92) S503: Solve the rotation angle value of the fourth joint , the rotation angle value of the third joint , and the rotation angle value of the fifth joint ; S50301: Rotational angle value of the sixth joint The change only affects the position of the seventh joint, which can be obtained according to equation (71): (93) By combining formula (73) and formula (93), the following can be obtained: (94) 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 , the rotation angle value of the fourth joint , the rotation angle value of the third joint , and the rotation angle value of the fifth joint are obtained according to the solving process of formulas (32)-(43) by substituting formula (94).

5. The dual quaternion-based inverse kinematics solving method for SSRMS configuration robotic arms according to claim 4, wherein: The S6 comprises the following steps: S601: end coordinate system relative reference coordinate system the pose of the end coordinate system (95) Therefore, the forward kinematics calculation formula of the equivalent six-degree-of-freedom manipulator is obtained as follows: (96) dual quaternion obtainable when a rotational transformation about the remaining joints occurs is: (97) Dual quaternions of joint axes of available equivalent six-degree-of-freedom robotic arms are: (98) S602: Solve 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 ; S60201: the relevant term of the third joint pair dual quaternion on the right side of equation (96) , the fourth joint pair dual quaternion and the fifth joint pair dual quaternion are transferred to the left side of the equation by using the conjugate property of dual quaternions, and then we have (99) 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: (100) Then the plane The dual quaternion of is (101) S60203: plane Upon sequentially rotating about the fifth joint, the fourth joint, and the third joint, according to the property of the plane rotating about an axis in equation (99), we have: (102) Since the plane is always perpendicular to the joint axis of the third joint, the joint axis of the fourth joint and the joint axis of the fifth joint, the plane does not change after the rotation about the three joints, i.e. the relationship holds: (103) (104) S60204: Relate the first joint dual quaternion and the second joint dual quaternion to the left side of equation (104) using the conjugate property of dual quaternions, leaving the remaining terms on the right side, which gives: (105) plane plane obtained after rotation about the sixth joint , plane plane obtained after rotation about the first joint , plane plane has the following relationship: (106) S60205: According to formula (101) and formula (97), the following can be obtained: (107) S60206: plane Rotation about the first joint yields a plane During the process, the joint axis of the first joint has the same intersection point with the plane and the plane The intersection point of the plane Substitute the Clifford algebra outer product formula (15) of the joint axis and the plane, the intersection point is: (108) plane The plane is obtained from the rigid body transformation of the right side of the equation of formula (105) : (109) Joint axis dual quaternion of first joint intersection with plane of intersection is: (110) In formula (110): ； ； S60207: According to formula (105), the plane is the same plane as the plane , so the intersection point is the same point as the intersection point , so each term in formula (108) and formula (110) is equal, respectively, and we have: (111) (112) By combining formula (111) and formula (112), the following can be obtained: (113) In formula (113): ； ； ； S60208: Thus the rotation angle value of the sixth joint can be obtained two solutions: (114) Substituting equation (114) into equation (85) gives the value of the rotation angle of the second joint two solutions: (115) The dual quaternion of the plane according to the formula (106) is: ​ (116) The dual quaternion of the plane according to the formula (109) is: ​ (117) Since the plane and the plane are equal, we have (118) S603: Solve the rotation angle value of the fourth joint , the rotation angle value of the third joint , and the rotation angle value of the fifth joint ; S60301: Since the rotation angle value of the seventh joint does not affect the positions of the remaining joints, it is obtained that (119) By combining formula (89), formula (87) and formula (119), the following can be obtained: (120) 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 , the rotation angle value of the fourth joint , the rotation angle value of the third joint , and the rotation angle value of the fifth joint .

Images