A space multi-body system dynamics modeling method based on lagrange multiplier method

Abstract

The application discloses a kind of space multibody system dynamics modeling method based on Lagrange multiplier method, first establish the coordinate conversion matrix of local to global coordinate system of each component in space;With centroid coordinates and Euler angle definition active component pose, determine generalized coordinate;Position constraint equation is established to kinematic pair, after first and second derivative, obtain the numerical expression of velocity, acceleration constraint equation;Through the cross product property of space angular velocity matrix and vector and the symmetry of vector cross product, the velocity constraint equation is converted, and the numerical expression of Jacobian matrix and time partial derivative term is obtained;Based on the modified Lagrange multiplier method, the cross product term of angular velocity and angular momentum of space component rotation is considered, the dynamics equation is established and solved;The correlation law of Euler angle and space angular velocity is established, the conversion of both is realized.Therefore, the method of the application is suitable for various space multibody systems, solves the problem of existing modeling method when dealing with complex constraints, fills the gap of prior art.

Description

Technical Field

[0001] This invention relates to the field of space multibody system dynamics modeling technology, and in particular to a space multibody system dynamics modeling method based on the Lagrange multiplier method. Background Technology

[0002] Space multibody systems have wide applications in modern engineering, including robotics, aerospace equipment, and industrial automation equipment. The key characteristic of space multibody systems is their ability to perform complex motions and operations in three-dimensional space, involving multiple degrees of freedom and complex mechanical interactions. This complexity makes accurate dynamic modeling of space multibody systems a crucial step in design and control. The significance of dynamic modeling lies in its ability to predict the system's motion behavior, optimize design parameters, and improve control accuracy, thereby enhancing the overall performance and reliability of the system.

[0003] Traditional dynamic modeling methods include the Newton-Euler method, the Lagrange method, and d'Alembert's principle, each with its own advantages and disadvantages. The Newton-Euler method is intuitive and easy to understand, but it involves significant computation when dealing with complex mechanisms and constraints. The Lagrange method is suitable for multi-degree-of-freedom systems, but its derivative analysis is complex and relies on symbolic computation. D'Alembert's principle is more convenient for dealing with non-inertial frames of reference, but it also faces computational complexity issues.

[0004] Existing dynamic modeling methods often face problems such as high derivation complexity, low computational efficiency, and poor versatility when dealing with complex constraints in space multibody systems. As the application of space multibody systems continues to expand, the limitations of existing modeling methods are becoming increasingly apparent, especially when dealing with high degrees of freedom and complex constraints, where the systematic nature and solution efficiency of existing methods are insufficient to meet practical needs. Summary of the Invention

[0005] The purpose of this invention is to provide a dynamic modeling method for spatial multibody systems based on the Lagrange multiplier method, which mainly solves the dynamic system modeling problem of complex spatial multibody systems. In engineering practice, it can effectively deal with dynamic problems under multiple degrees of freedom and complex constraints in space. In academic research, this method expands the theoretical framework of dynamic modeling of spatial multibody systems by introducing a modified Lagrange multiplier method, providing new ideas and methods for the study of multibody system dynamics.

[0006] To achieve the above objectives, this invention provides a method for dynamic modeling of spatial multibody systems based on the Lagrange multiplier method, comprising the following steps:

[0007] S1. For a spatial multibody system, establish the coordinate transformation matrix R from the local coordinate system to the global coordinate system for each spatial component;

[0008] S2. In a multibody system in space, the pose of each moving component is defined by its centroid coordinates and Euler angles, which in turn determine the complete generalized coordinates of the system.

[0009] S3. Establish position constraint equations for the kinematic pairs in the space multibody system, respectively;

[0010] S4. Take the first and second derivatives of the position constraint equations to obtain the numerical expressions of the velocity constraint equations and the acceleration constraint equations, respectively.

[0011] S5. Based on the modified Lagrange multiplier method, establish and solve the dynamic equations of the space multibody system.

[0012] S6. Establish the correlation between the Euler angles and spatial angular velocity of each component, and realize the conversion between spatial angular velocity and component Euler angles.

[0013] Preferably, in S1, the spatial multibody system adopts the spatial RSSR mechanism model, and the relative attitude of the local coordinate system is described by XYZ type Euler angles. The corresponding coordinate transformation matrix R is defined by formula (1):

[0014]

[0015] Where matrix R represents the coordinate transformation matrix from the local coordinate system to the global coordinate system, α, β and γ represent the XYZ type Euler angles from the global coordinate system to the local coordinate system, c represents the cosine function and s represents the sinine function.

[0016] Preferably, in S2, the complete generalized coordinates of the spatial RSSR mechanism are defined by formula (2):

[0017]

[0018] The generalized coordinates q of each component j Defined by formula (3):

[0019] q j =[x j ,y j ,z j ,α j ,β j ,γ j ] T (3);

[0020] Where, x j y j , z j Let α represent the centroid coordinates of component j. j ,β j γ j This represents the XYZ type Euler angles of component j.

[0021] Preferably, in S3, the kinematic pairs of the spatial RSSR mechanism include two revolute pairs and two spherical pairs, and corresponding kinematic constraint equations are established for each of the four kinematic pairs;

[0022] For a revolute joint, the centroid coordinates represented by the local coordinate system should be consistent with its global coordinate system, and the position constraint equation of the revolute joint should be established based on the perpendicular relationship between the rotation direction vector of the revolute joint and two orthogonal vectors in the local and global coordinate systems.

[0023] For spherical pairs, the position constraint equations of the spherical pairs are established based on the fact that the connection point of the spherical pairs is the same point in different local coordinate systems.

[0024] Preferably, in S3, for components with redundant rotational degrees of freedom, additional angular constraint equations are added to obtain the complete position constraint equations for the spatial RSSR mechanism.

[0025] Preferably, S4 is as follows:

[0026] First, by differentiating the position constraint equations in S3 with respect to time, we obtain the velocity constraint equations:

[0027]

[0028] Among them, v O1 This represents the velocity vector of the crank's center of mass O1 in the global coordinate system. This represents the derivative of coordinate transformation matrix 1 with respect to time.

[0029] Secondly, based on the relationship between the spatial angular velocity matrix S and the coordinate transformation matrix R, and the relationship between the angular velocity vector ω and the spatial angular velocity matrix S:

[0030]

[0031] Where P is an arbitrary vector, the velocity constraint equation in formula (4) is further transformed into:

[0032]

[0033] The transformation of formulas (4) to (6) shows that the cross product of vectors can be transformed into the corresponding antisymmetric matrix, and the antisymmetric matrix can also be transformed into the cross product of the corresponding vectors.

[0034] Based on the symmetry of the vector cross product:

[0035] a×b=-b×a (7);

[0036] Therefore, the velocity constraint equation in formula (6) is further transformed into:

[0037]

[0038] Among them, T AO1 For R1· 1 r AO1 The cross product corresponds to the antisymmetric matrix, T m1 and T m2 They are vectors R1·m 1r and R1·m 2r Cross product of the corresponding antisymmetric matrix;

[0039] Then consider the generalized coordinates, generalized velocity, and generalized acceleration defined based on spatial angular velocity:

[0040]

[0041] Comparing equations (7) and (8), we obtain the Jacobian matrix Φ of the position constraint equation for the rotational joint A. q1 and the corresponding time partial derivative Φ t :

[0042] Finally, by differentiating the velocity constraint equation, we obtain the right-hand side terms of the acceleration constraint equation for the kinematic pair and the additional angle constraint equation, and further obtain the acceleration constraint equation corresponding to the position constraint equation.

[0043] Preferably, S5 is as follows:

[0044] Based on the Lagrange multiplier method, the dynamic equations of the space RSSR mechanism are established:

[0045]

[0046] Where M is the generalized mass matrix of the RSSR mechanism, Q e Let λ be the generalized force vector and λ be the Lagrange multiplier.

[0047] According to the momentum theorem and the angular momentum theorem for rigid bodies:

[0048]

[0049] Among them, F and M T Let I be the net external force and net external torque acting on the rigid body, respectively, and let I be the inertia matrix of the rigid body. Equation (10) shows that for the rotation problem of spatial components, the cross product of angular velocity ω and angular momentum I·ω needs to be considered.

[0050] Based on the modified Lagrange multiplier method, dynamic equations applicable to space multibody systems are established:

[0051]

[0052] Formula (11) adds a velocity cross product term compared to Formula (9); for a spatial RSSR mechanism, its generalized mass matrix M is defined as follows:

[0053]

[0054] Where, m j Let I be the translational mass of component j. j Let I be the inertia matrix of component j relative to its local coordinate system of centroid, which is generally a constant. pj Let be the inertia matrix of component j relative to its center of mass, which is related to the attitude of component j;

[0055] For a spatial RSSR mechanism, its generalized force vector is defined as shown in equation (13):

[0056] Q e =[0,0,-m1g,M x ,0,0,0,0,-m2g,0,0,0,0,0,-m3g,0,0,0] T (13);

[0057] In formula (13), the generalized force vector includes the gravity of the three components along the z-axis, and the driving torque M acting on the crank AB. x ;

[0058] For the dynamic equation defined by equation (11), the cross product of the generalized velocities is shown in equation (14):

[0059]

[0060] For a spatial RSSR mechanism, the generalized velocity vector comprises the velocity vectors of three components, and the cross product of each component's velocity vector contains the translational velocity v. j The cross product and rotational angular velocity ω j The cross product of the three-dimensional vectors is transformed into a corresponding antisymmetric matrix. The cross product of the 18×1 generalized velocity vector is transformed into a diagonal matrix.

[0061] For a component with an irregular spatial shape, the translational mass matrix M in the generalized mass matrix pj Essentially, it is an identity matrix, with the rotational inertia matrix I. pj Essentially a symmetric matrix, the cross product of the generalized velocity vectors is as follows:

[0062]

[0063] Therefore, the velocity cross product term added in formula (11) does not contribute to the translational term, but contributes the same cross product term as in formula (10) to the rotational term.

[0064] Based on the modified dynamic equations and acceleration constraint equations, the matrix form of the dynamic differential equations is obtained, and the second-order differential equations are transformed into first-order differential equations. The first-order differential equations are obtained by integration using the fourth-order Runge-Kutta method, and finally the generalized coordinates, generalized velocities, and generalized accelerations of the corresponding components are obtained.

[0065] Therefore, the present invention employs the above-mentioned method for dynamic modeling of spatial multibody systems based on the Lagrange multiplier method, and the beneficial effects are as follows:

[0066] (1) Based on the constraint characteristics of spatial kinematic pairs, this invention establishes the standard position constraint equations of spatial kinematic pairs using generalized coordinates in Euler angle form;

[0067] (2) This invention uses the properties of spatial angular velocity matrix and vector cross product to numerically express velocity constraint equation and acceleration constraint equation, thereby extracting elements such as Jacobian matrix necessary for dynamic modeling;

[0068] (3) This invention adds a cross product term of generalized velocity and generalized momentum, establishes a dynamic modeling process applicable to space multibody systems, and verifies the method with a typical space RSSR mechanism.

[0069] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0070] Figure 1 This is a schematic diagram of a spatial RSSR mechanism model, an embodiment of the spatial multibody system dynamics modeling method based on the Lagrange multiplier method of the present invention.

[0071] Figure 2 This is an embodiment of the present invention, which describes the definition of the local coordinate system of different components of a spatial RSSR mechanism based on the Lagrange multiplier method for dynamic modeling of spatial multibody systems.

[0072] Figure 3 This invention provides a numerical representation of the Jacobian matrix of the motion constraint equation of the rotation pair A, according to an embodiment of a spatial multibody system dynamics modeling method based on the Lagrange multiplier method.

[0073] Figure 4 This is a flowchart illustrating the dynamic modeling process of a space RSSR mechanism, an embodiment of the spatial multibody system dynamic modeling method based on the Lagrange multiplier method of the present invention.

[0074] Figure 5This is a schematic diagram comparing the application and results of the dynamic modeling method of the present invention based on the Lagrange multiplier method for spatial multibody system dynamics modeling. In the diagram, subscript 1 represents the dynamic calculation result based on formula (25), subscript 2 represents the calculation result of Adams simulation, and subscript 3 represents the dynamic calculation result based on formula (23). (a) is the curve of the Euler angle of the component changing with time; (b) is the curve of the coordinate of the centroid of the component changing with time; (c) is the curve of the constraint force of the spherical pair changing with time; and (d) is the spatial motion trajectory of the centroid from C to D. Detailed Implementation

[0075] The technical solution of the present invention will be further described below with reference to the accompanying drawings and embodiments.

[0076] Unless otherwise defined, the technical or scientific terms used in this invention shall have the ordinary meaning as understood by one of ordinary skill in the art to which this invention pertains.

[0077] A dynamic modeling method for space multibody systems based on the Lagrange multiplier method is proposed, primarily for dynamic modeling and behavioral analysis of complex space multibody systems. The purpose of this invention is to establish a systematic dynamic modeling method applicable to space multibody systems, and to achieve numerical expression of various parameters in the dynamic modeling, thereby enabling accurate and efficient solutions to the dynamic behavior of space multibody systems. This provides methodological support for the control and structural optimization of multibody system dynamic behavior, and includes the following steps:

[0078] S1, such as Figure 1 As shown, the spatial multibody system adopts the spatial RSSR mechanism model, and its commonly used motion form is crank-rocker motion. Among them, the crank AB is hinged at point A and rotates about the x-axis in space; the connecting rod BC performs a compound motion in space; and the rocker CD is hinged at point D and rotates about the y-axis in space. Figure 1 The crank rotates counterclockwise around point A, and the corresponding rocker arm rotates clockwise around point D.

[0079] For this spatial RSSR mechanism, there are 6 structural parameters: the length L1 of crank AB, the length L2 of connecting rod BC, the length L3 of rocker CD, and the x-coordinate L of point A. x The y-coordinate of point D is L y The z-coordinate of point D is L z There are two angle parameters: the angle α1 between the driving member AB and the xOy plane, and the angle β3 between the driven member CD and the yOz plane.

[0080] For this spatial RSSR mechanism, there are 6 mass parameters: the mass m1 and moment of inertia I1 of crank AB, the mass m2 and moment of inertia I2 of connecting rod BC, and the mass m3 and moment of inertia I3 of rocker CD. Here, the moments of inertia I1, I2 and I3 refer to the inertia matrix of each component relative to its local coordinate system of the center of mass, which is a symmetric matrix.

[0081] For a multibody system in space, in order to describe the pose of each component, it is necessary to establish a coordinate transformation matrix R from the local coordinate system to the global coordinate system of each component in space; Figure 2 A local coordinate system is defined for each component, and the relative attitude of the local coordinate system can be described by Euler angles. Euler angles include 12 spatial attitude description types, represented by ZXZ and XYZ types. Different types of Euler angles do not affect the dynamic modeling method. In this embodiment, the relative attitude of the local coordinate system is described by XYZ type Euler angles, and the corresponding coordinate transformation matrix R is defined by formula (1):

[0082]

[0083] Where matrix R represents the coordinate transformation matrix from the local coordinate system to the global coordinate system, α, β and γ represent the XYZ type Euler angles from the global coordinate system to the local coordinate system, c represents the cosine function and s represents the sinine function.

[0084] S2. In a multibody system in space, the pose of each moving component is defined by its centroid coordinates and Euler angles, thereby determining the complete generalized coordinates of the system.

[0085] The RSSR mechanism has a total of 3 moving parts (crank, connecting rod, and rocker arm), and its complete generalized coordinates are defined by formula (2):

[0086]

[0087] The generalized coordinates q of each component j Defined by formula (3):

[0088] q j =[x j ,y j ,z j ,α j ,β j ,γ j ] T (3);

[0089] Where, x j y j , z j Let α represent the centroid coordinates of component j. j ,β j γ jThis represents the XYZ type Euler angles of component j.

[0090] S3. Establish position constraint equations for the kinematic pairs in the space multibody system, respectively;

[0091] The kinematic pairs of the spatial RSSR mechanism include two revolute joints and two spherical joints. Corresponding kinematic constraint equations are established for each of the four kinematic pairs.

[0092] For the revolute joint of crank AB at point A, on the one hand, the centroid coordinates represented by the local coordinate system 1 should be consistent with its global coordinates; on the other hand, the rotation direction vector of the revolute joint is perpendicular to two orthogonal vectors in the local and global coordinate systems. Therefore, the position constraint equation for the revolute joint A is defined by formula (4):

[0093]

[0094] Where, r O1 Let r be the position vector of the crank's center of mass O1 in the global coordinate system. A Let r be the position vector of the rotation sub-point A in the global coordinate system. A =[L x ,0,0] T R1 is the coordinate transformation matrix from local coordinate system 1 to global coordinate system. 1 r AO1 Let O1 be the position vector of the centroid O1 of component 1 relative to point A in local coordinate system 1. 1 r AO1 =[0,L1 / 2,0] T ;n 1g Let n be the rotation direction vector in the global coordinate system. 1r Let be the rotation direction vector in the local coordinate system. For the revolute joint A, both are [1, 0, 0]. T ;m 1r and m 2r In the local coordinate system, n 1r Two perpendicular direction vectors can be obtained by considering n. 1r The results, obtained through Schmidt orthogonalization, are currently represented as [0, 1.0, 0]. T and [0,0,1.0] T .

[0095] For the spherical pair of crank AB and connecting rod BC at point B, based on the fact that point B is the same point in local coordinate system 1 and local coordinate system 2, the position constraint equation of the spherical pair B is defined by formula (5):

[0096]

[0097] Where, r O2Let O2 be the position vector of the link's centroid O2 in the global coordinate system, and R2 be the coordinate transformation matrix from local coordinate system 2 to the global coordinate system. 1 r B and 2 r B Let be the position vectors of point B in local coordinate system 1 and local coordinate system 2, respectively. 1 r B =[0,L1 / 2,0] T , 2 r B =[L2 / 2,0,0] T .

[0098] For the spherical joint of link BC and rocker CD at point C, based on the idea of ​​the position constraint equation of spherical joint B, the position constraint equation of spherical joint C is defined by formula (6):

[0099]

[0100] Where, r O3 Let O3 be the position vector of the joystick's centroid O3 in the global coordinate system, and R3 be the coordinate transformation matrix from the local coordinate system 3 to the global coordinate system. 2 r C and 3 r C Let be the position vectors of point C in local coordinate system 2 and local coordinate system 3, respectively. 2 r C =[-L2 / 2,0,0] T , 3 r C =[0,0,L3 / 2] T .

[0101] For the revolute joint of the rocker CD at point D, based on the same idea as the position constraint equation of revolute joint A, the position constraint equation of revolute joint D is defined by formula (7):

[0102]

[0103] in, 3 r DO3 Let O3 be the position vector of the center of mass of the rocker component relative to point D in the local coordinate system. 1 r DO3 =[0,0,L3 / 2] T ;n 1g Let n be the rotation direction vector in the global coordinate system. 1r Let be the rotation direction vector in the local coordinate system. For the revolute joint D, both are [0, 1.0, 0]. T ;m 1r and m 2rIn the local coordinate system, n 1r Two perpendicular direction vectors can be obtained by considering n. 1r The values ​​obtained by Schmidt orthogonalization are currently represented as [1.0, 0.0, 0]. T and [0,0,1.0] T .

[0104] For the RSSR mechanism, link BC also has a redundant rotational degree of freedom (rotation about the x2 axis). To constrain this degree of freedom, an additional angular constraint about the link is added, as shown in formula (8):

[0105]

[0106] Where α2 is the Euler angle of link BC in the x-direction.

[0107] Combining the relevant content of formulas (4) to (8), the complete position constraint equation of the spatial RSSR mechanism is obtained as shown in formula (9):

[0108]

[0109] Given the crank AB rotation angle α1, the remaining 17 position coordinates of the three components can be obtained based on the position constraint equation of formula (9).

[0110] S4. By taking the first and second derivatives of the position constraint equation in formula (9), numerical expressions for the velocity constraint equation and the acceleration constraint equation can be obtained, respectively. However, the derivation of the formula by direct differentiation is extremely difficult, and existing approaches mostly start from the symbolic function, resulting in low numerical computation efficiency of the dynamic model.

[0111] Based on this problem, this invention proposes a differentiation analysis method for constraint equations. Taking the revolute joint A as an example, firstly, the position constraint equation in formula (4) of S3 is differentiated with respect to time to obtain the velocity constraint equation as shown in formula (10):

[0112]

[0113] Among them, v O1 This represents the velocity vector of the crank's center of mass O1 in the global coordinate system. This represents the derivative of coordinate transformation matrix 1 with respect to time.

[0114] Secondly, based on the relationship between the spatial angular velocity matrix S and the coordinate transformation matrix R, and the relationship between the angular velocity vector ω and the spatial angular velocity matrix S:

[0115]

[0116] Where P is an arbitrary vector, therefore, the velocity constraint equation in formula (10) can be further transformed:

[0117]

[0118] The transformation from formula (10) to formula (12) shows that the cross product of vectors can be transformed into the corresponding antisymmetric matrix, and the antisymmetric matrix can also be transformed into the cross product of the corresponding vectors.

[0119] Based on the symmetry of the vector cross product:

[0120] a×b=-b×a (13);

[0121] Therefore, the velocity constraint equation in formula (12) can be further transformed:

[0122]

[0123] Among them, T AO1 For R1· 1 r AO1 The cross product corresponds to the antisymmetric matrix, T m1 and T m2 They are vectors R1·m 1r and R1·m 2r The cross product corresponds to the antisymmetric matrix.

[0124] Then consider the generalized coordinates, generalized velocity, and generalized acceleration defined based on spatial angular velocity:

[0125]

[0126] Comparing equations (14) and (15), we can obtain the Jacobian matrix Φ of the position constraint equation for the rotational joint A. q1 and the corresponding time partial derivative Φ t :

[0127]

[0128] Similarly, the Jacobian matrix and time partial derivatives of the other three kinematic pair constraint equations and the additional angle constraint equations can be obtained, and combined to obtain the complete Jacobian matrix Φ. q and time partial derivative Φ t :

[0129]

[0130] At this point, the velocity constraint equation corresponding to the position constraint equation of formula (9) is as shown in formula (18):

[0131]

[0132] In Formula (18), the generalized coordinates and generalized velocities are defined by spatial angular velocity (Formula (15)).

[0133] Further differentiating the velocity constraint equation corresponding to formula (12), we obtain the acceleration constraint equation for the rotating joint A:

[0134]

[0135] Based on formula (19), the right-hand side of the acceleration equation can be extracted:

[0136]

[0137] Similarly, the right-hand side terms of the other three kinematic pair acceleration constraint equations and the additional angular constraint acceleration equation can be obtained, and combined to obtain the complete right-hand side term γ of the acceleration equation. C As shown in formula (21):

[0138]

[0139] At this point, the acceleration equation corresponding to the position constraint equation of formula (9) is as shown in formula (22):

[0140]

[0141] Similarly, the generalized coordinates and generalized acceleration here are defined based on spatial angular velocity (Equation (15)). Figure 3 The numerical expression of the Jacobian matrix of the motion constraint equation of the rotational joint A is given, including the differentiation and transformation from equation (9) to equation (20).

[0142] S5. Based on the modified Lagrange multiplier method, establish and solve the dynamic equations of the spatial multibody system, specifically:

[0143] Based on the Lagrange multiplier method, the dynamic equations of the space RSSR mechanism can be established:

[0144]

[0145] Where M is the generalized mass matrix of the RSSR mechanism, Q e Let λ be the generalized force vector and λ be the Lagrange multiplier.

[0146] However, formula (23) is inaccurate for spatial multibody systems, therefore, according to the momentum theorem and angular momentum theorem for rigid bodies:

[0147]

[0148] Among them, F and M TLet I be the net external force and net external torque acting on the rigid body, respectively, and let I be the inertia matrix of the rigid body. Equation (24) shows that for the rotation problem of spatial components, the cross product of angular velocity ω and angular momentum I·ω needs to be considered.

[0149] Based on the modified Lagrange multiplier method, dynamic equations applicable to space multibody systems are established:

[0150]

[0151] Formula (25) adds a velocity cross product term compared to Formula (23); for the spatial RSSR mechanism, its generalized mass matrix M is defined as follows:

[0152]

[0153] Where, m j Let I be the translational mass of component j. j Let I be the inertia matrix of component j relative to its local coordinate system of centroid, which is generally a constant. pj Let be the inertia matrix of component j relative to its center of mass, which is related to the attitude of component j.

[0154] For a spatial RSSR mechanism, its generalized force vector is defined as shown in equation (27):

[0155] Q e =[0,0,-m1g,M x ,0,0,0,0,-m2g,0,0,0,0,0,-m3g,0,0,0] T (27);

[0156] In formula (27), the generalized force vector includes the gravity of the three components along the z-axis, and the driving torque M acting on the crank AB. x .

[0157] For the dynamic equation defined by equation (25), the cross product of the generalized velocities is shown in equation (28):

[0158]

[0159] For a spatial RSSR mechanism, the generalized velocity vector comprises the velocity vectors of three components, and the cross product of each component's velocity vector contains the translational velocity v. j The cross product and rotational angular velocity ω j The cross product of three-dimensional vectors can be transformed into a corresponding antisymmetric matrix. The cross product of 18×1 generalized velocity vectors can be transformed into a diagonal matrix.

[0160] For a component with an irregular spatial shape, the translational mass matrix M in the generalized mass matrix pjEssentially, it is an identity matrix, with the rotational inertia matrix I. pj Essentially a symmetric matrix, the cross product of the generalized velocity vectors is as follows:

[0161]

[0162] Therefore, the velocity cross product term added in formula (25) does not contribute to the translational term, but contributes the same cross product term as in formula (24) to the rotational term.

[0163] Based on the modified dynamic equation of formula (25) and the acceleration constraint equation corresponding to formula (22), the matrix form of the dynamic differential equation is obtained:

[0164]

[0165] The Jacobian matrix Φ in formula (30) q And the right-hand side term γ of the acceleration equation C All values ​​have been expressed numerically, and the efficiency of numerical solutions is significantly improved compared to symbolic function solutions.

[0166] For the second-order differential equation of formula (30), it can be transformed into a first-order differential equation by reducing its order, and the variable y is defined as follows:

[0167]

[0168] The first derivative of variable y is:

[0169]

[0170] At this point, the second-order differential equation of formula (30) is transformed into the corresponding first-order differential equation:

[0171]

[0172] The first-order differential equation corresponding to formula (33) can be integrated by the fourth-order Runge-Kutta method to obtain the generalized coordinates, generalized velocity and generalized acceleration of the corresponding component, thus realizing the dynamic solution.

[0173] S6. Establish the correlation between the Euler angles and spatial angular velocity of each component, and realize the conversion between spatial angular velocity and component Euler angles.

[0174] The velocity constraint equation (Equation (18)), acceleration constraint equation (Equation (22)), and dynamic differential equation (Equation (30)) are all defined based on the new generalized coordinates (Equation (15)), while the position constraint equation (Equation (9)) is defined based on the original generalized coordinates (Equation (3)). Comparing the generalized coordinates of Equation (3) and Equation (15), it is necessary to establish the correlation between the Euler angles α, β, γ and the spatial angular velocity ω of each component, based on the definition of the spatial angular velocity matrix S:

[0175]

[0176] Therefore, based on formula (34), the relationship between the spatial angular velocity ω and the Euler angles α, β, γ is as follows:

[0177]

[0178] At this point, the relationship between spatial angular acceleration and Euler angles is as follows:

[0179]

[0180] Based on formulas (35) and (36), the conversion between spatial angular velocity ω and component Euler angles α, β, γ can be realized. Figure 4 The dynamic modeling process for a spatial RSSR mechanism includes solving the kinematic model, constructing the dynamic model, converting spatial angular velocity to Euler angles, and performing dynamic numerical integration. Figure 5 The application and comparison results of this dynamic modeling method are shown. The curve of subscript 1 completely coincides with the curve of subscript 2, and the curve of subscript 3 deviates from these two curves. This indicates that the solution of the modified dynamic equation (formula (25)) is completely matched with the Adams simulation, while the unmodified dynamic equation (formula (20)) will accumulate errors during long-term numerical calculation.

[0181] In summary, this invention systematically organizes the standard position constraint equations of spatial kinematic pairs, addressing existing dynamic modeling methods. By introducing spatial angular velocity, it achieves differentiation of the position constraint equations, thereby enabling numerical expression of key elements such as the Jacobian matrix. Furthermore, considering the rotational characteristics of spatial multibody systems, it modifies the application of the Lagrange multiplier method in spatial multibody systems by adding a cross product term between generalized velocity and generalized momentum. Through the correlation between spatial angular velocity and Euler angles, it achieves the conversion from solving for spatial angular velocity to solving for Euler angles. This method has a systematic and standardized modeling process for complex spatial multibody systems, and its solution efficiency is significantly improved compared to the solution using symbolic functions. Comparison with Adams simulation results verifies the correctness of this method.

[0182] Therefore, this invention employs the aforementioned method for dynamic modeling of spatial multibody systems based on the Lagrange multiplier method. By constructing the motion constraint equations of the spatial multibody system and leveraging the conversion relationship between spatial angular velocity and Euler angles, it achieves a numerical expression of the Jacobian matrix of the constraint equations. Based on the modified Lagrange multiplier method, it establishes dynamic equations exemplified by spatial RSSR mechanisms. This method is not only highly systematic and has relatively simple formula derivation, but also boasts high numerical computation efficiency and is applicable to various types of spatial multibody systems.

[0183] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A method for dynamic modeling of spatial multibody systems based on the Lagrange multiplier method, characterized in that, Includes the following steps: S1. For a spatial multibody system, establish the coordinate transformation matrix from the local coordinate system to the global coordinate system for each spatial component. R ; S2. In a multibody system in space, the pose of each moving component is defined by its centroid coordinates and Euler angles, which in turn determine the complete generalized coordinates of the system. S3. Establish position constraint equations for the kinematic pairs in the space multibody system, respectively; S4. Take the first and second derivatives of the position constraint equations to obtain the numerical expressions of the velocity constraint equations and the acceleration constraint equations, respectively. S5. Based on the modified Lagrange multiplier method, establish and solve the dynamic equations of the space multibody system. S6. Establish the correlation between the Euler angles and spatial angular velocity of each component, and realize the conversion between spatial angular velocity and component Euler angles; S5 specifically refers to: Based on the Lagrange multiplier method, the dynamic equations of the space RSSR mechanism are established: （9）； in, The generalized mass matrix of the RSSR mechanism. It is a generalized force vector. For Lagrange multipliers; According to the momentum theorem and the angular momentum theorem for rigid bodies: （10）； in, and These are the net external force and net external torque acting on the rigid body, respectively. Let be the inertia matrix of the rigid body; Equation (10) shows that, for the rotation problem of spatial components, angular velocity needs to be considered. With angular momentum The cross product of terms; Based on the modified Lagrange multiplier method, dynamic equations applicable to space multibody systems are established: （11）； Formula (11) adds a velocity cross product term compared to Formula (9); for spatial RSSR mechanisms, its generalized mass matrix M The definition is as follows: （12）； in, For components Translational mass, For components The inertia matrix relative to its local coordinate system of centroid is a constant. For components The inertia matrix relative to its center of mass, and the component It is related to posture; For a spatial RSSR mechanism, its generalized force vector is defined as shown in formula (13): （13）； In formula (13), the generalized force vector contains three along... z The weight of the components along the axial direction, and the driving torque acting on the crank AB to rotate. ; For the dynamic equation defined by equation (11), the cross product of the generalized velocities is shown in equation (14): （14）； For a spatial RSSR mechanism, the generalized velocity vector comprises the velocity vectors of three components, and the cross product of the velocity vectors of each component contains the translational velocity. cross product and rotational angular velocity The cross product of the three-dimensional vectors is transformed into a corresponding antisymmetric matrix. The cross product of the 18×1 generalized velocity vector is transformed into a diagonal matrix. For a component with an irregular spatial shape, the translational mass matrix in the generalized mass matrix is... Essentially, it is an identity matrix, the rotational inertia matrix. Essentially a symmetric matrix, the cross product of the generalized velocity vectors is as follows: （15）； Therefore, the velocity cross product term added in formula (11) is a cross product term that does not contribute to the translational term, but contributes the same as that in formula (10) to the rotational term; Based on the modified dynamic equations and acceleration constraint equations, the matrix form of the dynamic differential equations is obtained, and the second-order differential equations are transformed into first-order differential equations. The first-order differential equations are obtained by integration using the fourth-order Runge-Kutta method, and finally the generalized coordinates, generalized velocities, and generalized accelerations of the corresponding components are obtained.

2. The method for dynamic modeling of spatial multibody systems based on the Lagrange multiplier method according to claim 1, characterized in that, In S1, the spatial multibody system adopts the spatial RSSR mechanism model, and the relative attitude of the local coordinate system is described by XYZ type Euler angles, with the corresponding coordinate transformation matrix. Defined by formula (1): （1）； Among them, matrix This represents the coordinate transformation matrix from the local coordinate system to the global coordinate system. , and Represents the XYZ type Euler angles from the global coordinate system to the local coordinate system. Represents the cosine function. This represents the sin function.

3. The method for dynamic modeling of spatial multibody systems based on the Lagrange multiplier method according to claim 2, characterized in that, In S2, the complete generalized coordinates of the spatial RSSR mechanism are defined by formula (2): （2）； Generalized coordinates of each component Defined by formula (3): （3）； in, Representing components The coordinates of the centroid, Representing components The XYZ type Euler angles.

4. The method for dynamic modeling of spatial multibody systems based on the Lagrange multiplier method according to claim 3, characterized in that, In S3, the kinematic pairs of the spatial RSSR mechanism include two revolute pairs and two spherical pairs. Corresponding kinematic constraint equations are established for each of the four kinematic pairs. For a revolute joint, the centroid coordinates represented by the local coordinate system should be consistent with its global coordinate system, and the position constraint equation of the revolute joint should be established based on the perpendicular relationship between the rotation direction vector of the revolute joint and two orthogonal vectors in the local and global coordinate systems. For spherical pairs, the position constraint equations of the spherical pairs are established based on the fact that the connection point of the spherical pairs is the same point in different local coordinate systems.

5. The method for dynamic modeling of spatial multibody systems based on the Lagrange multiplier method according to claim 4, characterized in that, In S3, for components with redundant rotational degrees of freedom, additional angular constraint equations are added to obtain the complete position constraint equations for the spatial RSSR mechanism.

6. The method for dynamic modeling of spatial multibody systems based on the Lagrange multiplier method according to claim 5, characterized in that, S4 specifically refers to: First, by differentiating the position constraint equations in S3 with respect to time, we obtain the velocity constraint equations: （4）； in, Indicates the crank center of mass Velocity vector in the global coordinate system Let represent the derivative of coordinate transformation matrix 1 with respect to time. Indicates the centroid of component 1 relative point A Position vector in local coordinate system 1, Represents the rotation direction vector in the global coordinate system. Let be the rotation direction vector in the local coordinate system. and In the local coordinate system, the coordinates are related to... Two perpendicular direction vectors, through the Schmidt orthogonalization yields the following; Secondly, based on the spatial angular velocity matrix With coordinate transformation matrix Relationship, angular velocity vector With spatial angular velocity matrix Relationship: （5）； in, Since is an arbitrary vector, the velocity constraint equation in formula (4) is further transformed into: （6）； The transformation of formula (4) to formula (6) shows that the cross product of vectors can be transformed into the corresponding antisymmetric matrix, and the antisymmetric matrix can also be transformed into the cross product of the corresponding vectors. Based on the symmetry of the vector cross product: （7）； Therefore, the velocity constraint equation in formula (6) is further transformed into: （7）； in, for The cross product corresponds to the antisymmetric matrix. and They are vectors and Cross product of the corresponding antisymmetric matrix; Consider generalized coordinates, generalized velocities, and generalized accelerations defined based on spatial angular velocities: （8）； Comparing formulas (7) and (8), we obtain the revolute joint. A Jacobian matrix of position constraint equations and the corresponding time partial derivatives : Finally, by differentiating the velocity constraint equation, we obtain the right-hand side terms of the acceleration constraint equation for the kinematic pair and the additional angle constraint equation, and further obtain the acceleration constraint equation corresponding to the position constraint equation.

Images