Kinematic modeling method for parallel five-connecting-rod mechanism

Abstract

The invention discloses a kinematics modeling method for a parallel five-connecting-rod mechanism, which comprises the following steps of: performing kinematics positive solution through a trigonometric function relationship of an included angle between a connecting rod and a joint in the kinematics positive solution and inverse solution modeling process of a dual-drive parallel five-connecting-rod mechanism, namely, obtaining a tail end position coordinate (x, y) by knowing the rotating angles theta1 and theta2 of a driving motor; and carrying out inverse kinematics solution, namely, solving the rotation angles theta1 and theta2 of the driving motor according to the known tail end position coordinates (x, y). According to the method, a pure geometric trigonometric function solving method is adopted, the constraint relation between parameters of the mechanism can be reflected, multiple solutions and singular solutions are avoided, the unique solution of kinematics forward and inverse solutions is achieved, and a theoretical basis is provided for development of a control system when the mechanism is applied to a numerical control machine tool.

Description

Technical Field

[0001] The present invention relates to the technical field of connecting rod mechanisms, and more particularly to a kinematic modeling method for a parallel five-connecting rod mechanism. Background Art

[0002] Five-bar linkages are widely used in CNC machine tools with specific functions, gripping robot arms, and walking robot leg structures. Especially when used in CNC machine tools, compared with the traditional serial CNC machine tool configuration, the end position of the parallel five-bar linkage must be determined by inverse solution of the kinematic model to obtain the rotation angle of the driving joint, and the end position is controlled by controlling the rotation angle of the driving motor through the control system.

[0003] Among the existing kinematic modeling methods of parallel five-bar linkage, some adopt the method of simplifying the five-bar structure into a 2-DOF series structure, such as Figure 1 As shown in the figure, the DH method is used to perform kinematic analysis on the series structure. However, the kinematic model established by this method does not include the constraint relationship between the parallel joints, and there may be multiple solutions for the forward solution and the inverse solution. There is also the closed vector method, which still has multiple solutions, such as Figure 2 Points C and C' in.

[0004] Therefore, providing a kinematic modeling method for a parallel five-bar linkage mechanism is an urgent problem to be solved by those skilled in the art. Summary of the invention

[0005] In view of this, the present invention provides a kinematic modeling method for a parallel five-bar linkage mechanism to achieve a unique solution for the forward and inverse kinematic solutions of the parallel five-bar linkage mechanism.

[0006] In order to achieve the above object, the present invention adopts the following technical solution:

[0007] A kinematic modeling method for a parallel five-bar linkage mechanism, wherein the parallel five-bar linkage mechanism comprises two active swivel joints, two passive swivel joints and an end actuator mounting component, wherein the two active swivel joints are driven by two driving motors respectively; the two passive swivel joints and the two active swivel joints, as well as the two passive swivel joints and the end actuator mounting component are hinged by connecting rods; the rotation centers of the two active swivel joints are set to A and B respectively, the rotation centers of the two passive swivel joints are set to C and D respectively, the center of the end actuator mounting component is set to E, the connecting rods between the two passive swivel joints and the two active swivel joints are set to AC and BD respectively, and the connecting rods between the two passive swivel joints and the end actuator mounting component are set to CE and DE respectively, including:

[0008] Kinematic inverse solution modeling steps:

[0009] 1) Set the midpoint of the line connecting A and B to O, and the lengths of OA and OB to L 3 ; Establish a coordinate system with O as the origin, set the position coordinates of E to (x, y), connect EA, EB, and EO as auxiliary lines, and the lengths of AC and BD are both L 1 , the lengths of EC and ED are both L 2 ; Set the rotation angle of AC to θ 1 , the rotation angle of BD is θ 2 ; Set the angle between EO and OA to α, set the angle between EO and OB to β, set the angle between EA and AC to χ, set the angle between EB and BD to δ, set the angle between EA and OA to θ 11 , set the angle between EB and OB to θ 22 ;

[0010] 2) Given (x, y), solve for θ 1 and θ 2 ,θ 1 and θ 2 The calculation formula is as follows:

[0011] θ 1 =π-θ 11 -x

[0012] θ 2 =π-θ 22 -δ;

[0013] Kinematics forward solution modeling steps:

[0014] 1) Set the midpoint of the line connecting A and B to O, and the lengths of OA and OB to L 3 ; Establish a coordinate system with O as the origin and set the position coordinate of E to (x E ,y E ), connect CD as an auxiliary line, the length of CD is L CD , the lengths of EC and ED are both L 2 ; Set the rotation angle of AC to θ 1 , the rotation angle of BD is θ 2 ; Set the angle between EC and ED to θ E , set the angle between EC and CD to θ C1 , set the angle between CD and the horizontal line to θ C2 , set the angle between ED and CD to θ D1 ;

[0015] 2) Known θ 1 and θ 2 , solve (x E ,y E ), x E and EThe calculation formula is as follows:

[0016] x E =x C +L 2 ·cos(θ C1 +θ C2 )

[0017] y E =y C +L 2 ·sin(θ C1 +θ C2 ).

[0018] Furthermore, in the kinematic inverse modeling step,

[0019] According to the triangle cosine theorem, the calculation formula for χ is:

[0020]

[0021] θ 11 The calculation formula is:

[0022]

[0023] In the above two formulas, L AE is the length of EA, L OE is the length of EO;

[0024] Among them, L AE The calculation formula is:

[0025]

[0026] L OE The calculation formula is:

[0027]

[0028] The calculation formula of α is:

[0029] α=π-β

[0030] The calculation formula of β is:

[0031]

[0032] Substituting the above equations into θ 1 =π-θ 11 -χ, we can find θ 1 The relationship with (x,y) is:

[0033]

[0034] Similarly, according to the triangle cosine theorem, the calculation formula for δ is:

[0035]

[0036] θ 22 The calculation formula is:

[0037]

[0038] In the above two formulas, L BE is the length of EB;

[0039] L BE The calculation formula is:

[0040]

[0041] Substitute θ 2 =π-θ 22 -δ can be obtained:

[0042]

[0043] Furthermore, in the kinematic modeling step, according to the triangle sine theorem, θ C1 The calculation formula is:

[0044]

[0045] According to the cosine theorem, θ E The calculation formula is:

[0046]

[0047] Among them, L CD The calculation formula is:

[0048]

[0049] Δx is the difference between the horizontal coordinates of C and D, Δy is the difference between the vertical coordinates of C and D, and the calculation formulas are:

[0050] Δx=x D -x C

[0051] Δy=y D -y C

[0052] The horizontal coordinate x of C C The calculation formula is:

[0053] x C =-L 3 -L 1 ·cos(θ 1 )

[0054] The horizontal coordinate x of D D The calculation formula is:

[0055] x D =L 3 +L 1 ·cos(θ 2 )

[0056] The y coordinate of C C The calculation formula is:

[0057] y C =L 1 ·sin(θ 1 )

[0058] The y coordinate of D C The calculation formula is:

[0059] y D =L 1 ·sin(θ 2 )

[0060] θ C2 The calculation formula is:

[0061]

[0062] Substituting the above formula into x E =x C +L 2 ·cos(θ C1 +θ C2 ), y E =y C +L 2 ·sin(θ C1 +θ C2 ), we can get the horizontal and vertical coordinate values ​​of E (x E ,y E ):

[0063]

[0064] It can be seen that the present invention provides a kinematic modeling method for a parallel five-bar linkage mechanism. Compared with the prior art, the present invention has the following beneficial effects:

[0065] The solution method using pure geometric trigonometric functions can reflect the constraint relationship between the various parameters of the mechanism, avoid the emergence of multiple solutions and singular solutions, and achieve the unique solution of the kinematic forward and inverse solutions, providing a theoretical basis for the control system development when the mechanism is used in CNC machine tools. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0067] Figure 1 The accompanying drawing is a schematic structural diagram of a 2-DOF series structure in the prior art;

[0068] Figure 2 The accompanying drawing shows a closed vector polygon formed by using a closed vector method in the prior art;

[0069] Figure 3 The accompanying drawing is a schematic structural diagram of a parallel five-link mechanism provided by the present invention;

[0070] Figure 4 The accompanying drawing is a schematic diagram of a parallel five-bar linkage mechanism formed by kinematic inverse modeling provided by the present invention;

[0071] Figure 5 The accompanying drawing is a schematic diagram of a parallel five-bar linkage formed by kinematic forward solution modeling provided by the present invention. DETAILED DESCRIPTION

[0072] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0073] like Figure 3 As shown, an embodiment of the present invention discloses a kinematic modeling method of a parallel five-bar linkage mechanism, wherein the parallel five-bar linkage mechanism includes two active swivel joints 1, two passive swivel joints 2 and an end actuator mounting component 3, the two active swivel joints 1 are driven by two driving motors respectively; the two passive swivel joints 2 and the two active swivel joints 1, as well as the two passive swivel joints 2 and the end actuator mounting component 3 are hinged through connecting rods 4; the rotation centers of the two active swivel joints 1 are set to A and B respectively, the rotation centers of the two passive swivel joints 2 are set to C and D respectively, the center of the end actuator mounting component 3 is set to E, the connecting rods 4 between the two passive swivel joints 2 and the two active swivel joints 1 are set to AC and BD respectively, and the connecting rods 4 between the two passive swivel joints 2 and the end actuator mounting component 3 are set to CE and DE respectively, including:

[0074] Kinematic inverse solution modeling steps:

[0075] 1) If Figure 4 As shown, the midpoint of the line connecting A and B is set to O, and the lengths of OA and OB are both L. 3 ; Establish a coordinate system with O as the origin, set the position coordinates of E to (x, y), connect EA, EB, and EO as auxiliary lines, and the lengths of AC and BD are both L 1 , the lengths of EC and ED are both L 2 ; Set the rotation angle of AC to θ 1 , the rotation angle of BD is θ 2 ; Set the angle between EO and OA to α, set the angle between EO and OB to β, set the angle between EA and AC to χ, set the angle between EB and BD to δ, set the angle between EA and OA to θ 11 , set the angle between EB and OB to θ 22 ;

[0076] 2) Given (x, y), solve for θ 1 and θ 2 ,θ 1 and θ 2 The calculation formula is as follows:

[0077] θ 1 =π-θ 11 -x

[0078] θ 2 =π-θ 22 -δ;

[0079] According to the triangle cosine theorem, the calculation formula for χ is:

[0080]

[0081] θ 11 The calculation formula is:

[0082]

[0083] In the above two formulas, L AE is the length of EA, L OE is the length of EO;

[0084] Among them, L AE The calculation formula is:

[0085]

[0086] L OE The calculation formula is:

[0087]

[0088] The calculation formula of α is:

[0089] α=π-β

[0090] The calculation formula of β is:

[0091]

[0092] Substituting the above equations into θ 1 =π-θ 11 -χ, we can find θ 1 The relationship with (x,y) is:

[0093]

[0094] Similarly, according to the triangle cosine theorem, the calculation formula for δ is:

[0095]

[0096] θ 22 The calculation formula is:

[0097]

[0098] In the above two formulas, L BE is the length of EB;

[0099] L BE The calculation formula is:

[0100]

[0101] Substitute θ 2 =π-θ 22 -δ can be obtained:

[0102]

[0103] Kinematics forward solution modeling steps:

[0104] 1) If Figure 5 As shown, the midpoint of the line connecting A and B is set to O, and the lengths of OA and OB are both L. 3 ; Establish a coordinate system with O as the origin and set the position coordinate of E to (x E ,y E ), connect CD as an auxiliary line, the length of CD is L CD , the lengths of EC and ED are both L 2 ; Set the rotation angle of AC to θ 1 , the rotation angle of BD is θ 2 ; Set the angle between EC and ED to θ E , set the angle between EC and CD to θ C1 , set the angle between CD and the horizontal line to θ C2, set the angle between ED and CD to θ D1 ;

[0105] 2) Known θ 1 and θ 2 , solve (x E ,y E ), x E and E The calculation formula is as follows:

[0106] x E =x C +L 2 ·cos(θ C1 +θ C2 )

[0107] y E =y C +L 2 ·sin(θ C1 +θ C2 );

[0108] According to the triangle sine theorem, θ C1 The calculation formula is:

[0109]

[0110] According to the cosine theorem, θ E The calculation formula is:

[0111]

[0112] Among them, L CD The calculation formula is:

[0113]

[0114] Δx is the difference between the horizontal coordinates of C and D, Δy is the difference between the vertical coordinates of C and D, and the calculation formulas are:

[0115] Δx=x D -x C

[0116] Δy=y D -y C

[0117] The horizontal coordinate x of C C The calculation formula is:

[0118] x C =-L 3 -L 1 ·cos(θ 1 )

[0119] The horizontal coordinate x of DD The calculation formula is:

[0120] x D =L 3 +L 1 ·cos(θ 2 )

[0121] The y coordinate of C C The calculation formula is:

[0122] y C =L 1 ·sin(θ 1 )

[0123] The y coordinate of D C The calculation formula is:

[0124] y D =L 1 ·sin(θ 2 )

[0125] θ C2 The calculation formula is:

[0126]

[0127] Substituting the above formula into x E =x C +L 2 ·cos(θ C1 +θ C2 ), y E =y C +L 2 ·sin(θ C1 +θ C2 ), we can get the horizontal and vertical coordinate values ​​of E (x E ,y E ):

[0128]

[0129] In the kinematics forward solution and inverse solution modeling process of the dual-drive parallel five-bar linkage, the present invention performs kinematics forward solution through the trigonometric function relationship between the angles between the connecting rods and the joints, that is, the rotation angle θ of the drive motor is known. 1 and θ 2 , solve the end position coordinates (x, y), and perform inverse kinematics, that is, given the end position coordinates (x, y), solve the drive motor rotation angle θ 1 and θ 2The solution adopts the solution method of pure geometric trigonometric functions, which can reflect the constraint relationship between the parameters of the mechanism, avoid the emergence of multiple solutions and singular solutions, and realize the unique solution of the kinematics forward and inverse solutions, providing a theoretical basis for the control system development when the mechanism is used in CNC machine tools.

[0130] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0131] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A kinematic modeling method for a parallel five-bar linkage mechanism, wherein the parallel five-bar linkage mechanism comprises two active swivel joints, two passive swivel joints and an end actuator mounting component, wherein the two active swivel joints are driven by two driving motors respectively; the two passive swivel joints and the two active swivel joints, as well as the two passive swivel joints and the end actuator mounting component are hinged by connecting rods; the rotation centers of the two active swivel joints are set to A and B respectively, the rotation centers of the two passive swivel joints are set to C and D respectively, the center of the end actuator mounting component is set to E, the connecting rods between the two passive swivel joints and the two active swivel joints are set to AC and BD respectively, and the connecting rods between the two passive swivel joints and the end actuator mounting component are set to CE and DE respectively, characterized in that: include: Kinematic inverse solution modeling steps: 1) Set the midpoint of the line connecting A and B to O, and the lengths of OA and OB to L3; establish a coordinate system with O as the origin, set the position coordinates of E to (x, y), connect EA, EB, and EO as auxiliary lines, the lengths of AC and BD to L1, and the lengths of EC and ED to L2; set the rotation angle of AC to θ1, and the rotation angle of BD to θ2; set the angle between EO and OA to α, the angle between EO and OB to β, the angle between EA and AC to χ, the angle between EB and BD to δ, and the angle between EA and OA to θ 11 , set the angle between EB and OB to θ 22 ; 2) Given (x, y), solve for θ1 and θ2. The calculation formulas for θ1 and θ2 are as follows: θ1=π-θ 11 -x θ2=π-θ 22 -d; Kinematics forward solution modeling steps: 1) Set the midpoint of the line connecting A and B to O, and the lengths of OA and OB to L3; establish a coordinate system with O as the origin, and set the position coordinates of E to (x E ,y E ), connect CD as an auxiliary line, the length of CD is L CD , the lengths of EC and ED are both L2; the rotation angle of AC is set to θ1, the rotation angle of BD is set to θ2; the angle between EC and ED is set to θ E , set the angle between EC and CD to θ C1 , set the angle between CD and the horizontal line to θ C2 , set the angle between ED and CD to θ D1 ; 2) Given θ1 and θ2, solve (x E ,y E ), x E and E The calculation formula is as follows: x E =x C +L2·cos(θ C1 +θ C2 ) and E =and C +L2·sin(θ C1 +θ C2 )。 2. The kinematic modeling method of a parallel five-bar linkage according to claim 1, characterized in that: In the inverse kinematics modeling step, According to the triangle cosine theorem, the calculation formula for χ is: θ 11 The calculation formula is: In the above two formulas, L AE is the length of EA, L OE is the length of EO; Among them, L AE The calculation formula is: L OE The calculation formula is: The calculation formula of α is: α＝π-β The calculation formula of β is: Substitute the above equations into θ1=π-θ 11 -χ, the relationship between θ1 and (x, y) can be obtained as: Similarly, according to the triangle cosine theorem, the calculation formula for δ is: θ 22 The calculation formula is: In the above two formulas, L BE is the length of EB; L BE The calculation formula is: Substitute θ2 = π-θ 22 -d can be obtained:

3. The kinematic modeling method of a parallel five-bar linkage according to claim 1, characterized in that: In the kinematic modeling step, according to the triangle sine theorem, θ C1 The calculation formula is: According to the cosine theorem, θ E The calculation formula is: Among them, L CD The calculation formula is: Δx is the difference between the horizontal coordinates of C and D, Δy is the difference between the vertical coordinates of C and D, and the calculation formulas are: Δx=x D -x C Δy=y D -y C The horizontal coordinate x of C C The calculation formula is: x C =-L3-L1·cos(θ1) The horizontal coordinate x of D D The calculation formula is: x D =L3+L1·cos(θ2) The y coordinate of C C The calculation formula is: and C =L1·sin(θ1) The y coordinate of D C The calculation formula is: and D =L1·sin(θ2) θ C2 The calculation formula is: Substituting the above formula into x E =x C +L2·cos(θ C1 +θ C2 ), y E =y C +L2·sin(θ C1 +θ C2 ), we can get the horizontal and vertical coordinate values ​​of E (x E ,y E ):

Images