DYNAMICS OF SERIAL ROBOTIC MANIPULATORS

Transcription

1 DYNAMICS OF SERIAL ROBOTIC MANIPULATORS

2 NOMENCLATURE AND BASIC DEFINITION We consider here a mechanical system composed of r rigid bodies and denote: M i 6x6 inertia dyads of the ith body. Wi 6 x 6 angular-velocity dyad of the ith body. M i I i O W O m i 1 i Ω i O i = 1, 2. r O O Where: 1 is 3x3 identity matrix O is 3x3 zero matrix Ω i is the angular velocity matrix of the ith body I i is the inertia matrix of the ith body

3 NOMENCLATURE AND BASIC Furthermore we define: DEFINITION w w i working wrench exerted on the ith body w c i nonworking constraint wrench exerted on the ith body t i twist of the ith body μ i 6-D momentum screw w W i = n i W w c i = n i Where : f i W C f i C n i moment acting on ith body f i force acting on ith body t i = ω i c i μ i = I iω i m i c i

4 NEWTON-EULER EQUATIONS OF THE ITH RIGID BODY Clearly, from the definitions of M i, μ i and t i we have μ i = M i t i Deriving, we obtain: μ i = M i t i + W i μ i = M i t i + W i M i t i Recalling the Newton-Euler equations for a rigid body: I i ω i = ω i I i ω i + n w i + n c i m i c i = f W C i + f i which can be written in compact form using the foregoing definitions, we obtain the Newton-Euler equations of the ith body in the form: M i t i = W i M i t i + w W C i + w i

5 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS The Euler-Lagrange dynamical equations of a mechanical system are now recalled, as pertaining to serial manipulators. Thus, the mechanical system at hand has n degrees of freedom, its n independent generalized coordinates being the n joint variables, which are stored in the n-dimensional vector θ. We thus have: d T dt θ T θ = φ where T is a scalar function denoting the kinetic energy of the system and is the n-dimensional vector of generalized force.

6 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS We can decompose into two parts, p and n, the former arising from V and termed the conservative force of the system; the latter is the nonconservative force. That is: φ p V θ The above Euler-Lagrange equations thus becoming: d L dt θ L θ = φ n Where L is the Lagrangian of the system, defined as: L T V

7 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS Moreover, the kinetic energy of the system is given by the sum of all the kinetic energies of r links. The kinetic energy of the rigid body in term of generalized variables is: T = r i T i = r 1 i 2 t i T M i t i whereas the vector of nonconservative generalized forces is given by: φ n A D θ θ As denotes A the active power and the D power dissipated in the system

8 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS The wrench w ie is decomposed into two parts: w ia active wrench supplied by the actuators and w id dissipative wrench arises from viscous and Coulomb friction Thus, the power supplied to the ith link is readily computed as: i A = (w i A ) T t i i D = (w i D ) T t i Similar to the kinetic energy, then, the power supplied to the overall system is simply the sum of the individual powers supplied to each link, and expressed as: A = 1 r A i D = 1 r D i

9 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS Further definitions are now introduced: t manipulator twist µ manipulator momentum w C manipulator constraint wrench w A manipulator active wrench w D manipulator dissipative wrench t = t 1 t 2... t n μ = μ 1 μ 2... μ n w C = w 1 C w 2 C... w n C w A = w 1 A w 2 A... w n A w D = w 1 D w 2 D... w n D

10 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS Additionally, the 6nx6n matrices of manipulator mass M and manipulator angular velocity W are also introduced below: M = diag M 1, M 2,, M n W = diag (W 1, W 2,, W n ) From this definitions we have: μ = Mt μ = M t + WMt

11 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS With the foregoing definitions, then, the kinetic energy of the manipulator takes on a simple form, namely, T = 1 2 tt Mt = 1 2 tt μ Which is a quadratic form in the system twist. Since the twist, on the other hand, is a linear function of the vector θ of joint rates, the kinetic energy turns out to be a quadratic form in the vector of joint rates.

12 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS Moreover, we will assume that this form is homogeneous in θ : T = 1 2 θt I(θ) θ Notice that the above assumption implies that the base of the robot is fixed to an inertial base, and hence, when all joints are locked, the kinetic energy of the robot vanishes. It is apparent that: I θ = 2 (T) θ 2

13 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS Furthermore, the Euler-Lagrange equations can be written in the form: d T dt θ T θ + V θ = φ n The partial derivatives appearing in the foregoing equation take the forms derived below: dt d θ = I(θ) θ d T and hence: = I θ θ + I(θ, θ) θ dt θ

14 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS We express the kinetic energy in the form: T = 1 2 p θ, θ T θ Where p(θ, θ)is the generalized momentum of the manipulator, defined as: p θ, θ = I(θ) θ Hence: T θ = 1 2 p θ T θ

15 THE EULER-LAGRANGE EQUATIONS OF SERIAL MANIPULATORS The Euler-Lagrange equations thus taking on the alternative form: I θ θ + I θ, θ 1 2 I θ θ T θ + V θ = φ n

16 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT Consider the manipulator of Figure with links designed so that their mass centers C 1, C 2, C 3 are located at the midpoints of segments O 1 O 2, O 2 O 3, and O 3 P,respectively. Moreover, the ith link has a mass m i and a centroidal moment of inertia in a direction normal to the plane of motion I i, while the joints are actuated by motors delivering torques τ 1, τ 2, and τ 3 the lubricant of the joints producing dissipative torques that we will neglect in this model. Under the assumption that gravity acts in the direction of Y, find the associated Euler-Lagrange equations.

17 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT SOLUTION: All vectors introduced below are 2-D, the scalar angular velocities of the links ωi, for i =1, 2, 3, being: ω 1 = θ 1 ω 2 = θ 1 + θ 2 ω 3 = θ 1 + θ 2 + θ 3

18 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT Moreover, the velocities of the mass centers are: c 1 = 1 2 θ 1Ea 1 c 2 = θ 1 Ea θ 1 + θ 2 Ea 2 c 3 = θ 1 Ea 1 + θ 1 + θ 2 Ea θ 1 + θ 2 + The kinetic energy then becoming: 3 T = 1 (m 2 i 1 c i 2 + I i ω 2 i) θ 3 Ea 3

19 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT The squared magnitudes of the mass-center velocities are now computed using the expressions derived above. After simplifications, these yield c 1 2 = 1 4 a 1 2 θ 1 2 c 2 2 = a 1 2 θ a θ θ 1 θ 2 + θ 2 + a1 a 2 cos θ 2 θ 1 + θ 1 θ 2

20 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT The kinetic energy of the whole manipulator thus becomes: T = 1 2 (I 11θ I12 θ 1 θ 2 + 2I 23 θ 1 θ 3 + I 22 θ I33 θ 3 2 ) with coefficients I ij, for i = 1, 2, 3, and j = i to 3 being the distinct entries of the 3 3 matrix of generalized inertia of the system. These entries are given below: I 11 = I 1 + I 2 + I m 1a m 2 a a a 1 a 2 c 2 + m 3 a a a a 1 a 3 c 2 + a 1 a 3 c 23 + a 2 a 3 c 3 I 12 = I 2 + I m a a 1 a 2 c 2 + m 3 2a a a 1 a 2 c 2 + a 1 a 3 c a 2 a 3 c 3 I 13 = I a a 1 a 3 c 23 + a 2 a 3 c 3

21 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT I 22 = I 2 + I m 2a m 3 a a a 2 a 3 c 3 I 23 = I m 3 a a 2 a 3 c 3 I 33 = I m 3a 3 2 where: c i = cos θ i e c ij = cos θ i + θ j Furthermore, the potential energy of the manipulator is computed as the sum of the individual link potential energies : V = 1 2 m 1ga 1 senθ 1 + m 2 g a 1 senθ a 2sen θ 1 + θ 2 + m 3 g a 1 senθ 1 + a 2 sen θ 1 + θ a 3sen θ 1 + θ 2 + θ 3

22 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT While the total power delivered to the manipulator takes the form: = τ 1 θ 1 + τ 2 θ 2 + τ 3 θ 3 We now proceed to compute the various terms: I 11 = m 2 a 1 a 2 s 2 θ 2 m 3 2a 1 a 2 s 2 θ 2 + a 1 a 3 s 23 θ 2 + θ 3 + a 2 a 3 s 3 θ 3 I 22 = 1 2 m 2a 1 a 2 s 2 θ 2 m 3 2a 1 a 2 s 2 θ 2 + a 1 a 3 s 23 θ 2 + θ 3 + a 2 a 3 s 3 θ 3 I 12 = 1 2 m 3 a 1 a 3 s 23 θ 2 + θ 3 + a 2 a 3 s 3 θ 3 I 22 = m 3 a 2 a 3 s 3 θ 3 I 23 = 1 2 m 3a 2 a 3 s 3 θ 3 I 33 = 0

23 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT We have then: I 11 θ 1 + I 12 θ 2 + I 13 θ 3 I θ = h = I 12 θ 1 + I 22 θ 2 + I 23 θ 3 I 13 θ 1 + I 23 θ 2 + I 33 θ 3 whose components are readily calculated as: h 1 = m 2 a 1 a 2 s 2 + m 3 2a 1 a 2 s 2 + a 1 a 3 s 23 θ 1 θ 2 m 3 a 1 a 3 s 23 + a 2 a 3 s 3 θ 1 θ m 2a 1 a 2 s 2 + m 3 2a 1 a 2 s 2 + a 1 a 3 s 23 θ 2 2 m 3 a 1 a 3 s 23 θ 2 θ m 3 a 1 a 3 s 23 θ 3 2 h 2 = 1 2 m 2a 1 a 2 s 2 + m 3 2a 1 a 2 s 2 + a 1 a 3 s 23 θ 1 θ m 3 a 1 a 3 s 3 + a 2 a 3 s 3 θ 1 θ 3 m 3 a 2 a 3 s 3 θ 2 θ m 3a 2 a 3 s 3 θ 3 2 h 3 = 1 2 m 3a 1 a 3 s 23 θ 1 θ m 3 a 1 a 3 s 23 + a 2 a 3 s 3 θ 1 θ m 3a 2 a 3 s 3 θ 2 θ 3

24 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT I θ = θ I its entries being denoted by I ij. This matrix, in component form, is given by : I = 0 I 11,2 θ 1 + I 12,2 θ 2 + I 13,2 θ 3 I 11,3 θ 1 + I 12,3 θ 2 + I 13,3 θ 3 0 I 12,2 θ 1 + I 22,2 θ 2 + I 13,2 θ 3 I 12,3 θ 1 + I 22,3 θ 2 + I 23,3 θ 3 0 I 13,2 θ 1 + I 23,2 θ 2 + I 33,2 θ 3 I 13,3 θ 1 + I 23,3 θ 2 + I 33,3 θ 3 with the shorthand notation I ij,k indicating the partial derivative of I ij with respect to θ k. As the reader can verify, these entries are given as: I 11 = 0

25 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT I 12 = m 2 a 1 a 2 s 2 + m 3 2a 1 a 2 s 2 + a 1 a 3 s 23 θ m 2a 1 a 2 s 2 + m 3 2a 1 a 2 s 2 + a 1 a 3 s 23 θ m 3a 1 a 3 s 23 θ 3 I 13 = m 3 a 1 a 3 s 23 + a 2 a 3 s 3 θ m 3 a 1 a 3 s a 2 a 3 s 3 θ m 3 a 1 a 3 s 23 + a 2 a 3 s 3 θ 3 = 0 I 21 I 22 = 1 2 m 2a 1 a 2 s 2 + m 3 (2a 1 a 2 s 2 + a 1 a 3 s 23 )] θ 1 I 23 = 1 2 m 3 a 1 a 2 s a 2 a 3 s 3 θ 1 m 3 a 2 a 3 s 3 θ m 3a 2 a 3 s 3 θ 3 = 0 I 31 I 32 = 1 2 m 3a 1 a 3 s 23 θ 1 I 33 = 1 2 m 3 a 1 a 3 s 23 + a 2 a 3 s 3 θ m 3a 2 a 3 s 3 θ 2

26 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT Now, we define the 3-dimensional vector γ below: γ = its three components being: γ 1 = 0 I θ θ γ 2 = m 2 a 1 a 2 s 2 + m 3 2a 1 a 2 s 2 + a 1 a 3 s 23 θ 1 2 [m 2 a 1 a 2 s 2 + m 3 2a 1 a 2 s 2 + a 1 a 3 s 23 ]θ 1 2 θ 2 2 m 3 a 1 a 3 s 23 θ 1 2 θ 3 2 γ 3 = m 3 a 1 a 2 s 23 + a 2 a 3 s 3 θ 1 2 m 3 a 1 a 3 s a 2 a 3 s a 2 a 3 s 3 θ 1 2 θ 2 2 m 3 a 1 a 3 s 23 + a 2 a 3 s 3 θ 1 θ 3 m 3 a 2 a 3 s 3 θ 3 2 m 3 a 2 a 3 s 3 θ 2 θ 3 T θ

27 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT We now turn to the computation of the partial derivatives of the potential energy: V = 1 θ 1 2 m 1ga 1 c 1 + m 2 g a 1 c a 2c 12 + m 3 g a 1 c 1 + a 2 c a 2c 123 V = 1 θ 2 2 m 2ga 2 + m 3 g a 2 c a 3c 123 V = 1 θ 3 2 m 3ga 3 c 123

28 EULER-LAGRANGE EQUATIONS OF A PLANAR ROBOT The Euler-Lagrange equations thus reduce to: I 11 θ 1 + I 12 θ 2 + I 13 θ 3 + h γ m 1ga 1 c 1 + m 2 g a 1 c a 2c 12 + m 3 g a 1 c 1 + a 2 c a 2c 123 = τ 1 I 12 θ 1 + I 12 θ 2 + I 23 θ 3 + h γ m 2ga 2 + m 3 g a 2 c a 3c 123 = τ 2 I 13 θ 1 + I 23 θ 2 + I 33 θ 3 + h γ m 3ga 3 c 123 = τ 3