A method and system for kinematic inverse solution of series-parallel hybrid mechanism

Abstract

The application relates to the technical field of series-parallel hybrid mechanism, in particular to a series-parallel hybrid mechanism kinematics inverse solution method and system, which comprises the following steps: acquiring the position and attitude matrix of an end effector of a hybrid mechanism; calculating the direction line segment of the intersection point of the end attitude of a lower platform spherical pair hinge point in a parallel mechanism and two rotating shafts in a serial mechanism; calculating the direction line segment between two end points of an upper platform hinge point in the parallel mechanism; solving the accompanying angle of the translational motion of the upper platform of the parallel mechanism, and obtaining a parallel mechanism attitude transformation matrix according to Z-Y-X Euler angles; obtaining the lengths of each branch chain according to the relationship between the parallel mechanism attitude transformation matrix and each hinge point; calculating the attitude change matrix of the serial mechanism; and solving the angles of the two rotating shafts of the serial mechanism according to the inverse solution of the serial mechanism. The application solves the limitation of single control of the series or parallel structure.

Description

Technical Field

[0001] This invention relates to the field of series-parallel hybrid mechanisms, and in particular to a method and system for inverse kinematics solution of series-parallel hybrid mechanisms. Background Technology

[0002] Currently, the main five-axis machining equipment on the market and used in engineering has a linkage relationship with serial properties. Its system can be divided into X-axis motion system, Y-axis motion system, Z-axis motion system, and A-pendulum around the X-axis and C-pendulum around the Z-axis. A and C are installed on the X-axis and the X-axis is installed on the Y-axis. Alternatively, it can be a serial five-axis structure with a combination relationship. It can perform complex surface engraving on multiple spatial surfaces after completing the workpiece clamping once.

[0003] Its disadvantages are:

[0004] 1. Poor motion accuracy. Due to the use of a multi-stage series structure, the error exists in the form of multi-axis progressive accumulation, with the end effector being the extreme point of error accumulation. At the same time, the series structure is mostly a cantilever beam structure, which is subjected to large unidirectional forces. Large forces will cause large material deformation, resulting in large errors that accumulate to the end effector and thus poor accuracy.

[0005] 2. The structural equipment is heavy and bulky. The series structure is mostly a cantilever beam structure. To enhance its structural rigidity, the entire structural volume of the equipment needs to be strengthened. Moreover, most of the structures occupy the overall space of the equipment, which increases the weight and volume of the equipment.

[0006] 3. The equipment has a large moment of inertia. Due to the increase in volume and weight caused by the reinforcement of structural rigidity, the overall volume and weight of the moving parts have increased. In order to meet the needs of movement and processing, the driving power needs to be increased, which leads to the wear of the moving parts and thus increases the economic investment.

[0007] There is also a parallel structure processing equipment, which is characterized by multiple branches connecting to a single end motion platform. Under the action of the coupled motion of each branch, the end motion platform forms the same motion pattern as the serial mechanism.

[0008] Its disadvantages are:

[0009] 1. The range of motion is small. The motion form of the parallel mechanism is a multi-branch coupling form, and each branch is mutually constrained. In order to avoid structural interference between branches, its range of motion is small.

[0010] 2. The control is complex. The motion of the parallel mechanism is a multi-branch coupling form, which is prone to motion singularities and multiple solutions, making its control more complex. In addition, the non-intuitive nature of the branch motion and the end motion increases the difficulty.

[0011] 3. Low space utilization: Due to motion interference, the movement range of each branch cannot be fully utilized, and huge gaps are formed between each branch. Summary of the Invention

[0012] In view of the shortcomings of the prior art, the present invention solves the limitation of single control of series or parallel structures.

[0013] The technical solution adopted in this invention is: a method for inverse kinematics solution of a series-parallel hybrid mechanism, comprising:

[0014] Step 1: Obtain the position and attitude matrix of the end effector of the hybrid mechanism;

[0015] Furthermore, the attitude matrix formula for the hybrid mechanism is:

[0016]

[0017] Where ε is the rotation angle along the X-axis, δ is the rotation angle along the Y-axis, and R g11 To R g33 Let R be the attitude matrix. g The elements in.

[0018] Step 2: Calculate the direction line segment connecting the endpoint A of the line connecting the center hinge points A1 and A3 of the lower platform ball joint in the parallel mechanism with the intersection point K of the end attitudes of the two rotating shafts in the series mechanism.

[0019] Furthermore, direction segments The formula is:

[0020]

[0021] in, f represents the position of the end effector of the hybrid mechanism; The distance between them Let K be the directional line segment between the end attitude intersection point K and the endpoint I, where I is the end execution point of the hybrid mechanism.

[0022] Step 3: Calculate the direction line segment between point B, the endpoint of the line connecting hinge points B1 and B3 on the upper platform in the parallel mechanism, and point A, the endpoint of the line connecting the center hinge points A1 and A3 on the lower platform in the parallel mechanism.

[0023] Furthermore, direction segments for:

[0024]

[0025] Among them, T BK (1,4),T BK (2,4),T BK(3,4) represents the direction line segment. matrix T BK The elements of the corresponding row and column in R g13 R g23 R g33 Let R be the attitude matrix. g The element in the coordinate system is X, where X is the point P of the end effector of the hybrid mechanism relative to the coordinate system. A O A X A Y A Z represents the distance traveled along the x-axis in the coordinate system; Y represents the distance of point P of the end effector of the hybrid mechanism relative to the coordinate system. A O A X A Y A Z represents the distance traveled along the y-axis in the Z-axis; Z is the distance of point P of the end effector of the hybrid mechanism relative to the coordinate system. A O A X A Y A The distance traveled along the z-axis in Z.

[0026] Step 4: Based on the inverse solution of the 3SPR parallel mechanism, solve for the accompanying angles α and β of the translational motion of the upper platform of the parallel mechanism, and derive the attitude transformation matrix R of the parallel mechanism based on the ZYX Euler angles. p ;

[0027] Furthermore, the formula for the attitude transformation matrix of the parallel mechanism is:

[0028]

[0029] Where, n x n y n z o x o y o z a x a y and a z Let R be the attitude matrix. p The elements in.

[0030] Step 5: Based on the attitude transformation matrix R of the parallel mechanism p The length of each branch is derived from the relationship between each hinge point;

[0031] Furthermore, the length of the branch is:

[0032]

[0033] in,

[0034]

[0035]

[0036] a1 and a2 are position constants of points B1 and B2, respectively, and B1 and B2 are the centers of the hinge points of the revolute joint; b1 and b2 are position constants of points A1 and A2, respectively, and A1 and A2 are the centers of the hinge points of the spherical joint.

[0037] Step 6: Calculate the attitude change matrix R of the cascaded mechanism. s ;

[0038] Furthermore, the formula for the attitude change matrix of the serial mechanism is:

[0039]

[0040] Among them, T 11 =v 13 sinθ C sinθ F -v 11 w 13 cosθ C -v 13 w 11 cosθ C cosθ F ;

[0041] T 12 = -cosθ F Sθ C -w 11 cosθ C sinθ F ;T 13 =0;

[0042] T 14 =f1+f3cosθ C +f4w 11 cosθ C +f5w 13 cosθ C -f1v 11 sinθ C sinθ F -

[0043] f6v 13 sinθ C sinθ F -f1v 13 w 13 cosθ C +

[0044] f6v 11 w 13 cosθ C +f1v 11 w11 cosθ C cosθ F +f6v 13 w 11 cosθ C cosθ F ;

[0045] T 21 =-v 11 w 13 sinθ C -v 13 cosθ C sinθ F -v 13 w 11 cosθ F sinθ C ;

[0046] T 22 =cosθ C cosθ F -w 11 sinθ C sinθ F ;T 23 =0;

[0047] T 24 =

[0048] f3sinθ C +f4w 11 sinθ C +f5w 13 sinθ C -f1v 13 w 13 sinθ C +f6v 11 w 13 sinθ C +

[0049] f1v 11 cosθ C sinθ F +f6v 13 cosθ C sinθ F +f1v 11 w 11 cosθ F sinθ C +

[0050] f6v 13 w 11 cosθ F sinθ C ;

[0051] T 31 =v 13 w 13 cosθ F -v 11 w 11 ;T 32 =w 13 sinθ F ;T 33 =0.

[0052] Step 7: Based on the inverse solution of the series mechanism, solve for the angle θ of the rotation driven by the two shafts of the series mechanism. F and θ C ;

[0053] Furthermore, the formula for the angle of rotation driven by the second rotating shaft of the series mechanism is:

[0054]

[0055] In the formula,

[0056] Furthermore, the formula for the angle of rotation driven by the first rotating shaft of the series mechanism is:

[0057] θ C1 =2arctan(A1)

[0058] θ C2 =2arctan(A2) (38)

[0059] in:

[0060]

[0061]

[0062] In the formula, a = R s22 +cosθ F b = 2w 11 sinθ F c = R s22 -cosθ F .

[0063] A kinematic inverse solution system for a series-parallel hybrid mechanism includes: a memory for storing instructions executable by a processor; and a processor for executing instructions for a kinematic inverse solution method for the series-parallel hybrid mechanism.

[0064] The beneficial effects of this invention are:

[0065] 1. High motion accuracy: Because the parallel mechanism with average error is used as the translation mechanism which is prone to errors, the errors generated by translation are averaged on each branch. The serial mechanism also uses two-stage serial connection to minimize the cumulative error. In addition, the motion core of this invention is a closed solution algorithm. As long as the solver and drive can move out of the calculated data, the accuracy of walking can be guaranteed.

[0066] 2. Strong load capacity. Also, because a parallel mechanism is used as the translation mechanism and a series mechanism is located on it, the main load is transferred to the parallel mechanism. Moreover, the force-bearing components of the parallel mechanism are all two-force members, and the load direction is along the axis of the member. There are no weak structures such as cantilever beams.

[0067] 3. Flexible movement: Due to the use of a hybrid mechanism, the end-effector posture movement adopts a series mechanism. Since the series mechanism has no coupling connection in the single-axis movement, the range of motion is large and unrestricted, and it can achieve posture angles that cannot be achieved by parallel mechanisms, thus making the movement flexible. Attached Figure Description

[0068] Figure 1 This is a flowchart of the inverse kinematic solution of the serial-parallel hybrid mechanism of the present invention;

[0069] Figure 2 This is a schematic diagram of a 3SPR parallel structure;

[0070] Figure 3 This is a simplified diagram of the structural constraints and coordinate system establishment of the 3SPR parallel mechanism;

[0071] Figure 4 This is a schematic diagram of the connecting rod between the first and second rotating shafts of the 2R series mechanism.

[0072] Figure 5 This is a simplified diagram of the structural constraints and coordinate system establishment of the 2R series mechanism;

[0073] Figure 6 This is a simplified diagram of the hybrid mechanism structure;

[0074] Figure 7 This is a simplified diagram of the constraints and coordinate system establishment of the hybrid mechanism;

[0075] Figure 8 This is a schematic diagram of the hybrid mechanism operating in MATLAB;

[0076] Figure 9 It involves creating a 3D digital model of a hybrid mechanism in SolidWorks. Detailed Implementation

[0077] The present invention will be further described below with reference to the accompanying drawings and embodiments. The drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0078] like Figure 2 The 3SPR parallel structure is constructed, and the inverse solution of the 3SPR parallel mechanism is built. The positions x, y, z of the 3SPR parallel mechanism platform are used as inputs, and the parameters l1, l2, l3 of the three linear drives are used as outputs.

[0079] The 3SPR mechanism is characterized by consisting of two platforms, an upper platform and a chain connecting the two platforms. The lower platform, also known as the stationary platform, is generally fixed relative to the ground, while the upper platform, also known as the moving platform, moves relative to the stationary platform under the drive of the chain. The three chains are identical in structure, and each chain, from the stationary platform to the moving platform, consists of a ball joint (S), a translational joint (P), and a revolute joint (R). The revolute joints are fixed in relative position, and the translational joints are the kinematic pairs where the drive is located. The movement of the translational joints will cause the movement of the moving platform.

[0080] like Figure 3 As shown, the motion patterns produced by the moving platform vary depending on the arrangement of the rotating joint axes. A coordinate system is established with the center of the rotating joint as the coordinate system, and the X-axis is along the axial direction. The arrangement of rotating joint 1 is X... c1 The arrangement of rotary joint 2 is X c2 The arrangement of rotary joint 3 is X c3 And X c1 ⊥A1B1,X c2 ⊥A2B2,X c3 ⊥A3B3,X c2 ⊥X c1 ,X c2 ⊥X c3 ,X c1 ||X c3 .

[0081] therefore:

[0082] The direction vector of revolute 1 in the coordinate system of the moving platform is represented as follows: B C1 = [1, 0, 0,] T (1)

[0083] The direction vector of the revolute 2 in the coordinate system of the moving platform is represented as follows: B C2 = [0, 1, 0,] T (2)

[0084] The direction vector of revolute 3 in the coordinate system of the moving platform is represented as follows: B C3 = [-1, 0, 0,] T (3)

[0085] The centers of the hinge points of the revolute joint are B1, B2, and B3, respectively. A coordinate system for the moving platform is established with the center of edge B1B3. B O B X B Y BZ; The centers of the hinge points of the spherical joint are A1, A2, and A3, respectively. Establish a static platform coordinate system with the center of edge A1A3. A O A X A Y B Z;

[0086] Point B1 is represented in the coordinate system of the moving platform as follows: B B1 = [0, -a1, 0, ] T (4)

[0087] Point B2 is represented in the coordinate system of the moving platform as follows: B B2 = [a2, 0, 0,] T (5)

[0088] Point B3 is represented in the coordinate system of the moving platform as follows: B B3 = [0, a1, 0, ] T (6)

[0089] The representation of point A1 in the static platform coordinate system is: A A1 = [0, -b1, 0, ] T (7)

[0090] The representation of point A2 in the static platform coordinate system is: A A2 = [b2, 0, 0,] T (8)

[0091] The representation of point A3 in the static platform coordinate system is: A A3 = [0, b1, 0, ] T (9)

[0092] The coordinate system of the moving platform in the coordinate system of the static platform is represented as follows:

[0093] From ZYX Euler angles, we know:

[0094]

[0095] The revolute joint on the stationary platform is represented as follows:

[0096]

[0097]

[0098]

[0099] because A C i ⊥BA i B i Therefore A C i ⊥A iB, where i = 1, 2, 3;

[0100]

[0101]

[0102] From the structural constraints,

[0103]

[0104]

[0105]

[0106] have to:

[0107]

[0108] (A)-(C) yields b1(cosβsinγ)=0, since b1 is a structural constant, b1≠0;

[0109] When cosβ=0 sin(β)=1, substituting into (1) gives z=0, which is inconsistent with reality. Therefore, sinγ=0, that is, γ=0, so we get:

[0110]

[0111] Solving for:

[0112]

[0113] The above-derived relationships show that when the 3SPR mechanism performs translational motions x, y, and z on the moving platform, it will generate two accompanying rotational motions: rotation around the X-axis and rotation around the Y-axis.

[0114] Substitute the solution from the above equation into... In the middle, then Known;

[0115] Branch vectors:

[0116] Branch length:

[0117] Since the drive is a translational joint, the length of the branch can be determined given the position of the moving platform relative to the stationary platform. By controlling the drive and thus the length of the branch, the moving platform can reach the desired position through the coupled motion of each branch.

[0118] Construct the inverse solution of the 2R series mechanism, using the attitude matrix T at the end. BK As input, output rotation drive θC and θ F ;

[0119] like Figure 4 The 2R mechanism consists of two rotating and connecting rods that are connected end to end in sequence. The structural feature of the 2R mechanism is that the second rotating shaft is arranged at a certain angle to the first rotating shaft 1 through the connecting rod, which is non-orthogonal. The end effector is installed on the second rotating shaft 2, and the drive is located on the two rotating joints respectively. All kinematic joints and connecting rods are located on the same plane. The end effector coincides with the first rotating shaft 1 at a certain position (zero position).

[0120] like Figure 5 A simplified diagram of the structural constraints and coordinate system of the 2R series mechanism is established, and the coordinate system of the series mechanism is established sequentially according to the connection order of the connecting rods and rotating shafts.

[0121] Establish the coordinate system of the moving platform with the center of edge B1B3. B O B X B Y B Z, B Z is perpendicular to the moving platform. B X points to the first axis of rotation. B Y is used to deduce its direction using the right-hand rule.

[0122] Establish a coordinate system at the revolute joint of the first rotation axis 1. C O C X C Y C Z, C Z represents the direction of the axis of the rotary joint. B X and B The direction of Y changes as the drive rotates.

[0123] With connecting rod Establish a coordinate system at the connection point with the first rotation axis 1 D O D X D Y D Z, D The Z-direction coincides with the direction of one of the rotation axes. B Along the connecting rod in the X direction direction, D Y is used to deduce its direction using the right-hand rule.

[0124] With connecting rod With connecting rod Establish a coordinate system at the connection point E O E X E Y E Z, E Z is perpendicular to the connecting rod. E X along the link E Y is used to deduce its direction using the right-hand rule.

[0125] Establish a coordinate system at the revolute joint of the second rotation axis 2. F O F X F Y F Z, F Z represents the direction of the axis of the rotary joint. F X and F The direction of Y changes as the drive rotates.

[0126] With connecting rod Establish a coordinate system at the connection point with the second rotation axis 2 G O G X G Y G Z, G Along the connecting rod in the Z direction direction, G The X direction is perpendicular to the connecting rod. G Y is used to deduce its direction using the right-hand rule.

[0127] With connecting rod With end effector Establish a coordinate system at the connection point H O H X H Y H Z, H Z is in the same direction as the end effector. E X along the link E Y is used to deduce its direction using the right-hand rule.

[0128] Establish a coordinate system at the intersection of the first axis of rotation 1 and the second axis of rotation 2. K O K X K Y K Z、 K Z is in the same direction as the end effector. K X along the link and H Z in the same direction, K Y is used to deduce its direction using the right-hand rule.

[0129] Establish a coordinate system with the end effector's end point. I O I X I Y I Z、 I Z is in the same direction as the end effector. I X along the link and I Z in the same direction, IY is used to deduce its direction using the right-hand rule.

[0130] at the same time, and The included angle is and The included angle is

[0131] Using the DH method to transform each coordinate system, and expressing them using ZYX Euler angles, we have:

[0132] coordinate system C O C X C Y C Z in coordinate system B O B X B Y B The homogeneous coordinates under Z are represented as follows:

[0133]

[0134] Where f1 is The length.

[0135] coordinate system D O D X D Y D Z in coordinate system C O C X C Y C The homogeneous coordinates under Z are represented as follows:

[0136]

[0137] In the formula, θ C f2 is the angle of rotation of the first axis of rotation. Length;

[0138] coordinate system E O E X E Y E Z in coordinate system D O D X D Y D The homogeneous coordinates under Z are represented as follows:

[0139]

[0140] In the formula, f3 is Length;

[0141] coordinate systemF O F X F Y F Z in coordinate system EO E X E Y E The homogeneous coordinates under Z are represented as follows:

[0142]

[0143] In the formula, f4 is Length;

[0144] coordinate system G O G X G Y G Z in coordinate system F O F X F Y F The homogeneous coordinates under Z are represented as follows:

[0145]

[0146] In the formula, θ F f5 is the angle of rotation of the second axis of rotation. Length;

[0147] coordinate system H O H X H Y H Z in coordinate system G O G X G Y G The homogeneous coordinates under Z are represented as follows:

[0148]

[0149]

[0150] In the formula, f6 is Length;

[0151] coordinate system K O K X K Y K Z in coordinate system H O H X H Y H The homogeneous coordinates under Z are represented as follows:

[0152]

[0153] In the formula, f7 is Length;

[0154] coordinate system K O K X K Y K Z in coordinate system B O B X B Y B The homogeneous coordinates under Z are represented as follows:

[0155]

[0156] In the formula:

[0157] T 11 =v 13 sinθ C sinθ F -v 11 w 13 cosθ C -v 13 w 11 cosθ C cosθ F ;

[0158] T 12 = -cosθ F Sθ C -w 11 cosθ C sinθ F ;

[0159] T 13 =0;

[0160] T 14 =f1+f3cosθ C +f4w 11 cosθ C +f5w 13 cosθ C -f1v 11 sinθ C sinθ F -f6v 13 sinθ C sinθ F -f1v 13 w 13 cosθ C +f6v 11 w 13 cosθ C +f1v 11 w 11 cosθ C cosθ F +f6v13 w 11 cosθ C cosθ F ;

[0161] T 21 =-v 11 w 13 sinθ C -v 13 cosθ C sinθ F -v 13 w 11 cosθ F sinθ C ;

[0162] T 22 =cosθ C cosθ F -w 11 sinθ C sinθ F ;

[0163] T 23 =0;

[0164] T 24 =

[0165] f3sinθ C +f4w 11 sinθ C +f5w 13 sinθ C -f1v 13 w 13 sinθ C +f6v 11 w 13 sinθ C +

[0166] f1v 11 cosθ C sinθ F +f6v 13 cosθ C sinθ F +f1v 11 w 11 cosθ F sinθ C +

[0167] f6v 13 w 11 cosθ F sinθ C ;

[0168] T 31=v 13 w 13 cosθ F -v 11 w 11 ;

[0169] T 32 =w 13 sinθ F ;

[0170] T 33 =0;

[0171] T 34 =f2+f5w 11 -f4w 13 -f1(v 13 w 11 +v 11 w 13 cosθ F )+f6(v 11 w 11 -v 13 w 13 cosθ F );

[0172] T 41 =0;

[0173] T 42 =0;

[0174] T 43 =0;

[0175] T 44 =1;

[0176] From T 31 =v 13 w 13 cosθ F -v 11 w 11 it is known that:

[0177]

[0178] it is obtained that:

[0179]

[0180] From T 22 =cosθ C cosθ F -w 11 sin C sinθ F and the half-angle formula, it is known that:

[0181]

[0182] make have:

[0183] (T 22 +cosθ F A 2 +(2w 11 sinθ F )A+(T 22 -cosθ F ) = 0

[0184] have to:

[0185]

[0186]

[0187] Where a = T 22 +cosθ F b = 2w 11 sinθ F c = T 22 -cosθ F ;

[0188] Solving for:

[0189] θ C1 =2arctan(A1)

[0190] θ C2 =2arctan(A2) (26)

[0191] The above calculations show that when the end attitude (the attitude of the intersection point K) is known, the rotation angles of the first and second axes can be calculated, where the first axis has two values.

[0192] like Figure 1 As shown, a method for inverse kinematics solution of a series-parallel hybrid mechanism includes:

[0193] like Figure 6 This is a simplified diagram of a hybrid mechanism. The inverse solution of the hybrid mechanism, constructed using a 3SPR parallel mechanism and a 2R series mechanism, is given by the end positions X, Y, and Z of the hybrid mechanism and its orientation R. X R Y The input and output of the hybrid mechanism consists of three linear drives (l1, l2, l3) and two rotary drives (θ). C θ F ;

[0194] Given the position and orientation of the end effector of the hybrid mechanism, determine the parameters of the five drives.

[0195] Step 1: Obtain the position of the end effector of the hybrid mechanism. and attitude matrix R = R g ;

[0196] Where X represents point P of the end effector of the hybrid mechanism relative to the coordinate system. A O A X A Y A Z represents the distance traveled along the x-axis in the coordinate system; Y represents the distance of point P of the end effector of the hybrid mechanism relative to the coordinate system. A O A X A Y A Z represents the distance traveled along the y-axis in the Z-axis; Z is the distance of point P of the end effector of the hybrid mechanism relative to the coordinate system. A O A X A Y A The distance traveled along the z-axis in Z.

[0197] Specifically as follows:

[0198] like Figure 7 As shown, a coordinate system for the 3SPR+2R hybrid mechanism is established. The coordinate system is known to be relative to the end point I. A O A X A Y B The position of Z is The end effector is a rotation of ε angle around the X-axis and a rotation of δ angle around the Y-axis.

[0199] Given the position of the endpoint I relative to the coordinate system A O A X A Y B The position of Z is The end effector rotates about the X-axis by an angle ε called R. X (ε), rotated about the Y-axis by an angle δ R Y (δ), the formulas are as follows:

[0200]

[0201] Where ε is the rotation angle along the X-axis and δ is the rotation angle along the Y-axis.

[0202] Then the end effector attitude matrix R g for:

[0203]

[0204] Step 2: Calculate the direction line segment of the line connecting the lower platform hinge point A1 and hinge point A3 in the parallel mechanism and the intersection point K of the two rotating shafts in the series mechanism.

[0205] Among them, the position of the end effector of the hybrid mechanism With direction line segment The relationship is:

[0206]

[0207] It is concluded that

[0208] Where f is The distance between them, the direction line segment between the end attitude intersection point K and the endpoint I, where I is the end execution point of the hybrid mechanism, that is, the movement of the point during translational or rotational motion;

[0209] vector exist A O A X A Y B The representation under Z is:

[0210]

[0211] Among them, R g11 To R g33 The attitude matrix R of the end effector g The elements in.

[0212] Step 3: Due to the direction line segment of the line connecting the upper platform hinge point B1 and hinge point B3 in the parallel mechanism and the intersection point K of the two rotating shafts in the series mechanism... fixed;

[0213] Therefore, it can be based on The conclusion is in, It is the line segment pointing from endpoint A to endpoint B.

[0214] Direction line segment for:

[0215]

[0216] Among them, T BK (1,4),T BK (2,4),T BK (3,4) represents the direction line segment. matrix T BK The elements in the corresponding row and column.

[0217] Step 4: Based on the inverse solution of the 3SPR parallel mechanism, solve for the accompanying angles α and β of the translational motion of the upper platform of the parallel mechanism, and derive the attitude transformation matrix R of the parallel mechanism based on the ZYX Euler angles. p ;

[0218] The position of the moving platform in the coordinate system of the static platform is:

[0219]

[0220] Substitute equation (32) into equation (14) to solve for the accompanying angles α and β, and substitute α and β into the ZYX Euler angle formula (10) to solve for the attitude transformation matrix of the parallel mechanism;

[0221]

[0222] Where, n x n y n z o x o y o z a x a y and a z Let R be the attitude matrix. p The elements in.

[0223] Step 5: Based on the attitude matrix R p The lengths l1, l2, and l3 of each branch are derived from the relationship between each hinge point;

[0224] Substituting (31) into (15), we obtain the branch vector as follows:

[0225]

[0226]

[0227]

[0228] Where a1 and a2 are position constants of points B1 and B2, and B1 and B2 are the centers of the hinge points of the revolute joint; b1 and b2 are position constants of points A1 and A2, and A1 and A2 are the centers of the hinge points of the spherical joint.

[0229] The length of the branch is:

[0230]

[0231] Where L1(1) represents the first element of vector L1, L1(2) represents the second element of vector L1, and L1(3) represents the third element of vector L1.

[0232] Step Six: Through R g =R p R s Calculate the attitude change matrix R of the cascade mechanism. s =R p -1 R g Attitude matrix R s coordinate systemK O K X K Y K Z relative to the coordinate system B O B X B Y B The Z-pose;

[0233] Due to the attitude R of the moving platform relative to the static platform p Given that the attitude of the end effector is related to the attitudes of the series mechanism and the parallel mechanism, the following relationship exists:

[0234] R g =R p R s

[0235] The attitude change matrix of the cascaded mechanism is:

[0236] R s =R p -1 R g (35)

[0237] In the formula, R g Given the initial conditions, R p Solve from equation (33);

[0238] Therefore, the coordinate system can be calculated. k O k X k Y k Z relative to the coordinate system B O B X B Y B The attitude transformation of Z R s The formula is:

[0239]

[0240] Step 7: Based on the inverse solution of the series mechanism, solve for the angle θ of the driving rotation of the series mechanism. C θ F ;

[0241] From the reverse solution of the series mechanism (25)(26), we know that the angle of rotation driven by the second rotating shaft of the series mechanism is:

[0242]

[0243] In the formula,

[0244] The angle of rotation driven by the first rotating shaft of the series mechanism is:

[0245] θ C1=2arctan(A1)

[0246] θ C2 =2arctan(A2) (38)

[0247] in:

[0248]

[0249]

[0250] In the formula, a = R s22 +cosθ F b = 2w 11 sinθ F c = R s22 -cosθ F ;

[0251] At this point, the reverse solution of the 3SPR+2R series-parallel hybrid mechanism has been fully solved, and the position parameters of the five actuators can be obtained by using the known position and attitude of the end effector.

[0252] Simulation and verification:

[0253] The above derivation process is implemented in MATLAB. By inputting the relative position and rotation angle of the end effector I, the length of each branch and the rotation angle of the revolute joint are solved, and the position, attitude and parameters of each drive are recorded during the operation.

[0254] The main code is as follows:

[0255]

[0256]

[0257]

[0258] like Figure 8 As shown, run the above program in MATLAB.

[0259] Similarly, to verify the correctness of the simulation, a 3D digital model was created in SolidWorks and imported into Adams, as follows: Figure 9 As shown, simulation verification was performed, with the same position and attitude measurements as in MATLAB, and the parameters of each drive were recorded.

[0260] Table 1 shows the position input values ​​of the end effector, the solution values ​​obtained from the inverse kinematics solution in MATLAB, and the measured values ​​in Adams. The error does not exceed 0.1 of the measured value.

[0261] Table 1: Comparison of end effector position input values, MATLAB inverse kinematics solution, and measured values ​​in Adams

[0262]

[0263]

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

Claims

1. A method for inverse kinematics solution of a series-parallel hybrid mechanism, characterized in that, include: Step 1: Obtain the position and attitude matrix of the end effector of the hybrid mechanism; Step 2: Calculate the direction line segment of the endpoint of the lower platform ball joint hinge point in the parallel mechanism and the intersection point of the end attitudes of the two rotating shafts in the series mechanism; Step 3: Calculate the direction line segment between the two endpoints of the upper platform hinge point in the parallel mechanism; Step 4: Solve for the accompanying angle of the translational motion of the upper platform of the parallel mechanism, and according to... ZYX Euler angles are used to derive the attitude transformation matrix of the parallel mechanism; Step 5: Determine the length of each branch based on the relationship between the attitude transformation matrix of the parallel mechanism and each hinge point; The formula for calculating the length of each branch is: （34） in, , ， , , for , The position constant of the point. , The center of the hinge point of the revolute joint; , for , The position constant of the point. , The center of the ball joint hinge; Step 6: Calculate the attitude change matrix of the cascade mechanism; Step 7: Based on the inverse solution of the series mechanism, solve for the rotation angles of the first and second rotating shafts of the series mechanism.

2. The inverse kinematics solution method for a series-parallel hybrid mechanism according to claim 1, characterized in that, The attitude matrix formula for a hybrid mechanism is as follows: = = = (28) in, for Axis rotation angle, for Y Axis rotation angle, to pose matrix The elements in.

3. The inverse kinematics solution method for a series-parallel hybrid mechanism according to claim 1, characterized in that, The formula for calculating the direction line segment at the intersection point in step two is: （29） in, The position of the end effector of the hybrid mechanism; for The distance between them End attitude intersection point K and the finish line I directional line segments, I It serves as the final execution point for the hybrid mechanism.

4. The inverse kinematics solution method for a series-parallel hybrid mechanism according to claim 1, characterized in that, The formula for the direction line segment between the two endpoints in step three is: = = （31） in, , , Direction line segment matrix The corresponding row and column elements in the text. , , pose matrix The elements in X For the end effector of the hybrid mechanism, point P is relative to the coordinate system. Middle x The distance the axis moves; Y For the end effector of the hybrid mechanism, point P is relative to the coordinate system. Middle y The distance the axis moves; Z For the end effector of the hybrid mechanism, point P is relative to the coordinate system. Middle z The distance traveled along the axis.

5. The inverse kinematics solution method for a series-parallel hybrid mechanism according to claim 1, characterized in that, The formula for the attitude transformation matrix of the parallel mechanism in step four is: = （33） in, , , , , , , , and The attitude transformation matrix The elements in.

6. The inverse kinematics solution method for a series-parallel hybrid mechanism according to claim 1, characterized in that, The formula for the attitude change matrix of the series mechanism is: = （36） in, ; = ; ; + + + ； ； ； ； + ； ; ; ; for Length; for Length; for Length; for Length; for Length; for Length; and The included angle is ; and The included angle is .

7. The inverse kinematics solution method for a series-parallel hybrid mechanism according to claim 6, characterized in that, The formula for the angle of rotation driven by the second rotating shaft of the series mechanism is: （37） In the formula, ; for Length; for Length; for Length; for Length; for Length; for The length.

8. The inverse kinematics solution method for a series-parallel hybrid mechanism according to claim 7, characterized in that, The formula for the angle of rotation driven by the first rotating shaft of the series mechanism is: （38） in: In the formula, .

9. A kinematic inverse solution system for a series-parallel hybrid mechanism, characterized in that, include: Memory is used to store instructions that can be executed by the processor; A processor for executing instructions to implement the inverse kinematics solution method for a serial-parallel hybrid mechanism as described in any one of claims 1-8.

Images