Multibody simulation

Transcription

1 Multibody simulation Dynamics of a multibody system (Euler-Lagrange formulation) Dimitar Dimitrov Örebro University June 16, 2012 Main points covered Euler-Lagrange formulation manipulator inertia matrix 1/14

2 we derived the Newton-Euler equations of motion for a single rigid body f = m v c t = I c ω +ω I c ω kinetic energy of a rigid body T = 1 2 mvt c v c ωt I c ω Next, we focus on developing a dynamical model for a system of rigid bodies. Two popular approaches (that lead to the same set of equations) for doing this are Newton-Euler formulation Based on analysis of forces and moments due to constraints acting between links. Computationally attractive due to its recursive form. Euler-Lagrange formulation An energy-based approach to dynamics. It uses the kinetic and potential energy of a system as a starting point for the formulation of its equations of motion. Usually used for system analysis. 2/14

3 Newton-Euler Euler-Lagrange (a single particle) Newton s second law mÿ = f mg y kinetic energy T The l.h.s. can be written as mÿ = d dt (mẏ) = d dt ẏ (1 2 mẏ2 ) = d T dt ẏ potential energy P mg = P (mgy) = y y we define the Lagrangian L f mg x note that L = T P = 1 2 mẏ2 mgy L ẏ = T ẏ and L y = P y Euler-Lagrange equation d dt L ẏ L y = f 3/14

4 In general, for a manipulator system with n DoF with generalized coordinates q 1,...,q n, the Euler-Lagrange equations can be written as d L L = τ i, i = 1,...,n, dt q i q i where, τ i is the (generalized) force associated with q i. In the example with a single particle, y serves as the generalized coordinate, while f as the generalized force. five steps in order to formulate the equations of motion for a given system, we should perform the following steps 1 compute the kinetic energy T of the system 2 express T as a function of the generalized coordinates q 3 compute the potential energy P of the system 4 express P as a function of the generalized coordinates q 5 form the Lagrangian L and substitute in the Euler-Lagrange equations 4/14

5 Example (a single link manipulator) y Given a rigid body, attached to a revolute joint with axis of rotation z mg - weight of the link I c - inertia around an axis going through the CoM and parallel to the z axis r - distance from the joint to the CoM τ q x q - joint angle τ - joint torque r Problem I c formulate the equations of motion for the system using the Euler-Lagrange approach mg 5/14

6 Example (a single link manipulator) Step 1: compute the kinetic energy T of the system T = 1 2 m(ẋ2 +ẏ 2 )+ 1 2 I c q 2 Step 2: express T as a function of the generalized coordinate q The Cartesian coordinates x and y can be expressed as x = rsinq, y = rcosq. The kinetic energy becomes T = 1 2 m(r2 (cosq) 2 q 2 +r 2 (sinq) 2 q 2 )+ 1 2 I c q 2 = 1 2 mr2 q I c q 2 = 1 2 (I c +mr 2 ) q 2 = 1 2 I q2. I = I c +mr 2 is the inertia around the joint axis of rotation (recall the parallel axes theorem ). 6/14

7 Example (a single link manipulator) Step 3: compute the potential energy P of the system P = mgy Step 4: express P as a function of the generalized coordinate q P = mgrcosq Step 5: form L and substitute in the Euler-Lagrange equations L = T P = 1 2 I q2 +mgrcosq L q = I q d L dt q = I q L q = mgrsinq d L dt q L = τ = I q +mgrsinq q given τ and q we can simulate the behavior of the system if I c = 0, τ = 0, we obtain the equation of motion of an idealized pendulum q = g r sinq 7/14

8 Euler-Lagrange equations for a n DoF manipulator Step 1: compute the kinetic energy T of the system Let T i be the kinetic energy of link i For simplicity of notation we use T i = 1 2 m iv T i v i ωt i I iω i. v i to denote the linear velocity of the center of mass of link i ω i to denote the angular velocity of link i I i to denote the 3 3 inertia matrix of link i, calculated about its CoM and expressed in the world frame. The kinetic energy of the entire system is given by T = n T i. i=1 8/14

9 Step 2: express T as a function of the generalized coordinates q R n At each configuration q, the joint velocities q can be related to the linear velocity of the CoM of link i, using J vi R 3 n angular velocity of link i, using J ωi R 3 n v i = J vi (q) q, ω i = J ωi (q) q. Substituting v i and ω i in the kinetic energy for link i leads to T i = 1 2 m i q T J T v i J vi q qt J T ω i I i J ωi q = 1 [ ] 2 qt m i J T v i J vi +J T ω i I i J ωi q }{{} R n n For the whole system we obtain [ T = 1 n ( ) ] 2 qt m i J T v i J vi +J T ω i I i J ωi q = 1 2 qt H q i=1 }{{} H R n n 9/14

10 Manipulator inertia matrix n ) H(q) = (m i J T J vi vi +J T I ωi i J ωi i=1 H(q) = H(q) T (i.e., H is symmetric) x T Hx > 0 for all nonzero x R n (i.e., H(q) is positive definite) Step 3: compute the potential energy P of the system The potential energy of link i (P i ) can be computed by assuming that the mass of the entire object is concentrated at its CoM P i = m i g T r i. where g R 3 is the gravity vector, and r i is the position of the CoM of link i, both expressed in the world frame. For the whole system we have P = n P i = i=1 n m i g T r i. i=1 The potential energy P is a function of only the generalized coordinates q and not their derivatives. 10/14

11 Step 4: express P as a function of the generalized coordinates q This can be achieved by directly by using the forward geometric model. Hereafter, we will simple denote the potential energy using P(q). Step 5: form L and substitute in the Euler-Lagrange equations The Lagrangian becomes Hence, L = 1 2 qt H(q) q P(q). L q = 1 2 H q HT q = H q d L = H q +Ḣ q. dt q The explicit computation of Ḣ q and L/ q is quite involved, and we will not do it here. c(q, q) = Ḣ q L/ q is called the nonlinear term of the equations of motion and we will compute it in a straightforward way using the Newton-Euler formulation (next lecture). 11/14

12 Summary (Euler-Lagrange formulation) The equations of motion of a n DoF manipulator can be written as d L dt q L = τ = H(q) q +c(q, q) q the term H q denotes the generalized inertia forces H is only a function of q (and not q) c is in general a function of q and q (it is called the nonlinear term of the equations of motion) we can interpret c as forces/torques acting on the generalized coordinates (but not applied by the actuators) from the above equation it follows that if we apply τ = c the accelerations q will be equal to zero 12/14

13 Where are the constraints? When discussing the Euler-Lagrange formulation, the constraints due to the joints were not mentioned explicitly. For deriving the equations of motion we used Lagrangian = kinetic energy potential energy, where the total kinetic T and potential P energies were simply found by summing up T i and P i for the individual rigid bodies. How did we impose the conditions that the joints and links should stay together? Answer The constraints due to the joints are implicitly imposed when the energy of the system is expressed as a function of the generalized coordinates q. Hence, τ = H(q) q +c(q, q) implicitly accounts for the constraints due to the system structure. Such formulation is convenient because we do not need to explicitly deal with the constraints imposed by the joints. 13/14

14 τ q Given the joint torques τ (at a particular configuration q), the joint accelerations q can be computed using q = H(q) 1 (τ c(q, q)) numerical integration Given q, q and q, at time t, we can obtain q and q, at time t+ t by numerical integration ( q, q) t ( q,q) t+ t simulation (summary) given the state ( q,q) and control input τ at time t compute the joint accelerations q using the equations of motion numerically integrate q to obtain the state ( q,q) at time t+ t 14/14