Lecture Note 12: Dynamics of Open Chains: Lagrangian Formulation

Transcription

1 ECE5463: Introduction to Robotics Lecture Note 12: Dynamics of Open Chains: Lagrangian Formulation Prof. Wei Zhang Department of Electrical and Computer Engineering Ohio State University Columbus, Ohio, USA Spring 2018 Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 1 / 20

2 Outline Introduction Euler-Lagrange Equations Lagrangian Formulation of Open-Chain Dynamics Outline Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 2 / 20

3 From Single Rigid Body to Open Chains Recall Newton-Euler Equation for a single rigid body: F b = G b Vb [ad Vb ] T (G b V b ) Open chains consist of multiple rigid links connected through joints Dynamics of adjacent links are coupled. We are concerned with modeling multi-body dynamics subject to constraints. Introduction Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 3 / 20

4 Preview of Open-Chain Dynamics Equations of Motion are a set of 2nd-order differential equations: τ = M(θ) θ + h(θ, θ) - θ R n : vector of joint variables; τ R n : vector of joint forces/torques - M(θ) R n n : mass matrix - h(θ, θ) R n : forces that lump together centripetal, Coriolis, gravity, and friction terms that depend on θ and θ Forward dynamics: Determine acceleration θ given the state (θ, θ) and the joint forces/torques: θ = M 1 (θ)(τ h(θ, θ)) Inverse dynamics: Finding torques/forces given state (θ, θ) and desired acceleration θ τ = M(θ) θ + h(θ, θ) Introduction Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 4 / 20

5 Lagrangian vs. Newton-Euler Methods There are typically two ways to derive the equation of motion for an open-chain robot: Lagrangian method and Newton-Euler method Lagrangian Formulation - Energy-based method - Dynamic equations in closed form - Often used for study of dynamic properties and analysis of control methods Newton-Euler Formulation - Balance of forces/torques - Dynamic equations in numeric/recursive form - Often used for numerical solution of forward/inverse dynamics Introduction Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 5 / 20

6 Outline Introduction Euler-Lagrange Equations Lagrangian Formulation of Open-Chain Dynamics Euler-Lagrange Equations Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 6 / 20

7 Generalized Coordinates and Forces Consider k particles. Let f i be the force acting on the ith particle, m i be its mass, p i be its position. Newton s law: f i = m i p i, i = 1,... k Now consider the case in which some particles are rigidly connected, imposing constraints on their positions α j (p 1,..., p k ) = 0, j = 1,..., n c k particles in R 3 under n c constraints 3k n c degree of freedom Dynamics of this constrained k-particle system can be represented by n 3k n c independent variables q i s, called the generalized coordinates { { α j (p 1,..., p k ) = 0 p i = γ i (q 1,..., q n ) j = 1,..., n c i = 1,..., k Euler-Lagrange Equations Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 7 / 20

8 Generalized Coordinates and Forces To describe equation of motion in terms of generalized coordinates, we also need to express external forces applied to the system in terms components along generalized coordinates. These forces are called generalized forces. Generalized force f i and coordinate rate q i are dual to each other in the sense that f T q corresponds to power The equation of motion of the k-particle system can thus be described in terms of 3k n c independent variables instead of the 3k position variables subject to n c constraints. This idea of handling constraints can be extended to interconnected rigid bodies (open chains). Euler-Lagrange Equations Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 8 / 20

9 Euler-Lagrange Equation Now let q R n be the generalized coordinates and f R n be the generalized forces of some constrained dynamical system. Lagrangian function: L(q, q) = K(q, q) P(q) - K(q, q): kinetic energy of system - P(q): potential energy Euler-Lagrange Equations: f = d L dt q L q (1) Euler-Lagrange Equations Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 9 / 20

10 Example: Spherical Pendulum l θ φ mg Figure 4.1: Idealized spherical pendulum. The config tem is described by the angles θ and φ. where L q, L q, and Υ are to be formally regarded as r we often write them as column vectors for notation proof of Theorem 4.1 can be found in most books o chanical systems (e.g., [99]). Lagrange s equations are an elegant formulation a mechanical system. They reduce the number of e describe the motion of the system from n, the numbe system, to m, the number of generalized coordinates. are no constraints, then we can choose q to be the com T = 1 2 mi ṙi 2, and equation (4.5) then reduces to fact, rearranging equation (4.5) as d L dt q = L q + Υ Euler-Lagrange Equations Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 10 / 20

11 Example: Spherical Pendulum (Continued) Euler-Lagrange Equations Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 11 / 20

12 Outline Introduction Euler-Lagrange Equations Lagrangian Formulation of Open-Chain Dynamics Lagrangian Formulation Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 12 / 20

13 Lagrangian Formulation of Open Chains For open chains with n joints, it is convenient and always possible to choose the joint angles θ = (θ 1,..., θ n ) and the joint torques τ = (τ 1,..., τ n ) as the generalized coordinates and generalized forces, respectively. - If joint i is revolute: θ i joint angle and τ i is joint torque - If joint i is prismatic: θ i joint position and τ i is joint force Lagrangian function: L(θ, θ) = K(θ, θ) P(θ, θ) Dynamic Equations: τ i = d L dt θ L i θ i To obtain the Lagrangian dynamics, we need to derive the kinetic and potential energies of the robot in terms of joint angles θ and torques τ. Lagrangian Formulation Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 13 / 20

14 Some Notations For each link i = 1,..., n, Frame {i} is attached to the center of mass of link i. All the following quantities are expressed in frame {i} V i: Twist of link {i} m i: mass; I i: rotational inertia matrix; G i = [ Ii 0 0 m ii ] : Spatial inertia matrix Kinetic energy of link i: K i = 1 2 VT i G iv i J (i) b R 6 i : body Jacobian of link i [ J (i) b = J (i) b,1... J (i) b,i where J (i) b,j = [ Ad e [B i ]θ i e [B j+1 ]θ j+1 ] B j, j < i and J (i) b,i = B i ] Lagrangian Formulation Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 14 / 20

15 Kinetic and Potential Energies of Open Chains J ib = [J (i) b 0] R 6 n Total Kinetic Energy: K(θ, θ) = 1 2 n i=1 VT i G i V i = 1 2 θ T ( n i=1 ( J T ib (θ)g i J ib (θ) )) θ 1 2 θ T M(θ) θ Potential Energy: P(θ) = n i=1 m igh i (θ) - h i(θ): height of CoM of link i Lagrangian Formulation Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 15 / 20

16 Lagrangian Dynamic Equations of Open Chains Lagrangian: L(θ, θ) = K(θ, θ) P(θ) τ i = d dt L θ i L θ i τ i = n M ij (θ) θ j + i=j n n j=1 k=1 Γ ijk (θ) θ j θk + P θ i, i = 1,..., n Γ ijk (θ) is called the Christoffel symbols of the first kind Γ ijk (θ) = 1 ( Mij + M ik M ) jk 2 θ k θ j θ i Lagrangian Formulation Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 16 / 20

17 Lagrangian Dynamic Equations of Open Chains Dynamic equation in vector form: τ = M(θ) θ + C(θ, θ) θ r1c g(θ) C ij(θ, θ) n k=1 Γ θ l2c2 r2c ijk k is called the 0 Coriolis l1s3 0 matrix 0 r2 l1c3 r2 θ3 To compute the manipulator inertia matrix, we first compute the body Jacobians corresponding to each link frame. A detailed, but straightforward, calculation yields J1 = Jsl1(0) b = J3 = Jsl3(0) b = J2 = Jsl2(0) b = 0 r s s c c by: The inertia matrix for the system is given by Γ112 = (Iy2 Iz2 m2r1)c2s2 2 + (Iy3 Iz3)c23s23 M11 M12 M13 m3(l1c2 + r2c23)(l1s2 + r2s23) M(θ) = M21 M22 M23 = J1 T M1J1 + J2 T M2J2 + J3 T M3J3. Γ113 = (Iy3 Iz3)c23s23 m3r2s23(l1c2 + r2c23) M31 M32 M33 The components of M are given by Γ121 = (Iy2 Iz2 m2r 2 1)c2s2 + (Iy3 Iz3)c23s23 m3(l1c2 + r2c23)(l1s2 + r2s23) θ1 L1 θ2 L2 L3 θ4 M11 = Iy2s Iy3s Iz1 + Iz2c Iz3c m2r1c m3(l1c2 + r2c23) 2 M12 = 0 M13 = 0 M21 = 0 Γ131 = (Iy3 Iz3)c23s23 m3r2s23(l1c2 + r2c23) Γ211 = (Iz2 Iy2 + m2r1)c2s2 2 + (Iz3 Iy3)c23s23 + m3(l1c2 + r2c23)(l1s2 + r2s23) Γ223 = l1m3r2s3 L4 M22 = Ix2 + Ix3 + m3l m2r m3r m3l1r2c3 Γ232 = l1m3r2s3 x z S y l1 l0 l2 M23 = Ix3 + m3r2 2 + m3l1r2c3 M31 = 0 M32 = Ix3 + m3r2 2 + m3l1r2c3 M33 = Ix3 + m3r2. 2 Γ233 = l1m3r2s3 Γ311 = (Iz3 Iy3)c23s23 + m3r2s23(l1c2 + r2c23) Γ322 = l1m3r2s3 Note that several of the moments of inertia of the different Finally, links dowe not compute the effect of gravitational forces on the manipulator. These of freedom forces are written as derivative gives appear in this expression. This is because the limited degrees Figure 4.6: SCARA manipulator in its reference configuration. of the manipulator do not allow arbitrary rotations of each joint around each axis. N(θ, The Coriolis and centrifugal forces are computed directly from the θ) = V N(θ, θ) = V 0 θ, here inertia θ = (m2gr1 + m3gl1) cos θ2 m3r2 cos(θ2 + θ3)). matrix via the formula α = Iz1 + r1m1 2 + l1m2 2 + l1m3 2 + l1m4 2 m3gr2 cos(θ2 + θ3)) β = Iz2 + Iz3 + Iz4 + l2m3 2 + l2m4 2 + m2r2 2 Cij(θ, θ) n = Γijk θk = 1 n ( where) V : R n R is the potential energy of the manipulator. For the Mij + Mik three-link Mkj θk. manipulator under consideration here, the potential energy is 2 θk θj θi k=1 k=1 given by γ = l1l2m3 + l1l2m4 + l1m2r2 (4.25) This completes the derivation of the dynamics. V (θ) = m1gh1(θ) + m2gh2(θ) + m3gh3(θ), δ = Lagrangian Iz3 + Iz4. Formulation A very messy calculation Lectureshows 12 (ECE5463 that the nonzero Sp18) values of Γijk are given Wei Zhang(OSU) 17 / 20

18 613, = XO. b216 = * Lagrangian Dynamic Equations of Open Chains Dynamic model of PUMA 560 Arm: metric G2 to These ations; totals 12 = I3 = +I16 * (C223* C * C4 * 5 5) * SC23 * CC4 +I20 * ((I + CC4) * SC23 * SS5 - (1-2 * SS23) * C4 * SC5) +I22 * ((1-2 * ) * C5-2 * SC23 * C4 * S5)) +Ilo * (1-2 * ss23) + (1-2 ss2) ; N * SC XlO- * C * SC ~10-~ * (1-2 * SS23). ate. The g to. Is = I, = I7 = bl13 = 2 {I5 * C2 * C23 + I7 * SC23 - I12 * C2 * 523 +Ils*(2*sc23*c5+(1-2*ss23)*c4*s5) +I16 * C2 * (C23* C5 - S23 * C4 * 5 5) * SC23 * CC4 +I20 * (( 1 + CC4) * 5c23 * (1-2 * 5523) * C4 * 5c5) +I22 (( 1-2 * 5523) * C5-2 * 5c23 * C4 * 55)} +I10 * (1-2 * SS23) ; X 7.44~10~ * C2 * C * SC ~10-~ * C2 * S ~10-~ * (1-2 * SS23). bllr = 2*{-115*sc23*54*s5-116*c2*c23*s4*s5 +I18 * C4 * S5-120 * (SS23* SS5 * SC4 - SC23 * 54 * SC5) -122 * CC23 * S4 * S5 - I21 * SS23 * SC4} ; x -2.50~10-~ * SC23 * S4 * S ~10-~ * C4 * xlO- * C2 * C23 * S4 * S5. ented d mof link rberg center 3 the re the atrix, only e pre- B(q), left in r the t int i. where s of ifugal s for e A7. it to ave a &e; g the mic Part 11. Gravitional Constants 81 = -g*((m~+m4+m5+m~)*az+m2*rz2); gz = g*(m.s*ry3-(m4+m5+m6)*d4-m4*r,4); g3 = g * m2 * ry2 ; 84 = -g*(m4+m5+rn6)*as; g.5 = -g * m.5 * rz6 ; Table AS. Computed Values for the Constants Appearing in the Equations of Forces of Motion. (Inertial constants have units of kilogram meters-squared) 1.43 f f ~10-I f 0.31~10-~ 2.98xlO-' f 0.29~10-~ 2.38~lO-~f 1.20~10-~ -1.42~10-~ f 0.70~10~~ -3.79~10-' f 0.90~10-~ 1.25~10-~ f 0.30~10-~ 6.42xlO-' f 3.00~10-~ 3.00~10-~ f 14.0~10-~ -I.OOX~O-~ f 6.00~10-~ ~.OOXIO-J ~ = 2 * {I20 * (SC5* (CC4* (1- CC23) - CC23) -SC23 * C4 * (1-2 * SS5)) (SS23* S5 - SC23* 6 4 * C5) -116*C2*(S23*S5-C23*C4*C5)+118*S4*C5 +I22 * (CC23* C4 * C5 - SC23 * S5)} N -2.50~10-~* (SS23* S5 - SC23 * C4 * C5) ~ lo- * C2 * (S23* 55 - C23 * C4 * C5) + 8.EOxlO-4 * 54 * C5. b116 = 0. blzs = 2*{-Ib*s23+Ils*C23+116*s23*S4*s5 +I18 * (C23* C4 * S5 + S23 * C5) * C23 * SC4 +I20 * S4 * (C23* C4 * CC5-523 * SC5) +Iz2 * G23 * S4 * S5) ; x 2,67xlO- * S x10- * C = -118 * 2 * S23 * 5 4 * S * 523 * (1- (2* SS4)) +120*S23*(1-2*SS4*CC5)-114*S23; %Ow b125 = I1,*C23*S4+Il8*2*(s23*C4*C5+c23*s5) +120*s4*(C23*(1-2*SS5)-s23*C4*2*SC5); X = -Izs*(S23*C5+C23*C4*S5); xo = blzs * 6156 = b126 * bl45 = 2 * {I15 * S23 * C4 * C * C2 c C4 * C5 +Ils*c23*s4*c5+122*C23*c4*C5}+I17*s23*C4 -I*o * (s23* c4 * (I- 2 * SS5) + 2 * C23 : SC5) ; 6146 = I23 * 5 23 * S4 * 55 ; N 0. b156 = -123 * (6 23 * 5 5 +,523* C4 * c5) ; X 0. b212 = 0. b2ls 0. bzlr = Ilt * S * S23* (1- (2* SS4)) +2*{-Il5*C23*C4*S5+116*S2*C4*S5 +I20 * (523* (CC5* CC4-0.5) + C23 * C4 * SC5) +I22 * S23 * G4 t S5) ; X 1.64X10-s * ~10-~ * C23 * C4 * S ~10- * S2 C4 * S ~10- * 523 * (1- (2* ss4)). bzla =2*{-115*C23*S4*C5+122*S23*S4+C5 +116*S2*S4*C5}-I17*C23*s4 +rzo~(c23~s4~(1-2~ss5)-2~s23~sc4~sc5); X -2.50~10- * C23 * S4 * C ~10-~ * 52 * 54 * c5-6.42x10- * C23 * 54. b424 = 0. (Gravitational constants have units of newton meters) gl = f 00.5 gz = f 0.20 gs = 1.02 f 0.50 g* = 2.49~10-' f 0.25XIO-' gs = -2.82~10-' f 0.56X10-2 b223 =2*{-112*S3+I~*C3+116*(C3*C5-53*C4*S5))j a 2.20~10-~ * S x10- * C Lagrangian Formulation Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 18 / 20

19 More Discussions Lagrangian Formulation Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 19 / 20

20 More Discussions Lagrangian Formulation Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 20 / 20