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 dt L q L q (1) Euler-Lagrange Equations Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 9 / 20

10 Example: Spherical Pendulum l θ φ mg 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 dt L θ i L θ 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 i I ] : Spatial inertia matrix Kinetic energy of link i: K i = 1 2 VT i G i V 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 ]θ B j+1 j, j<iand 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 2 ( Mij θ k + M ik θ j M jk θ 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(θ, θ) θ + g(θ) - C ij (θ, θ) n k=1 Γ ijk θ k is called the Coriolis matrix Γ 112 =(I y2 I z2 m 2r 2 1)c 2s 2 +(I y3 I z3)c 23s 23 m 3(l 1c 2 + r 2c 23)(l 1s 2 + r 2s 23) Γ 113 =(I y3 I z3)c 23s 23 m 3r 2s 23(l 1c 2 + r 2c 23) Γ 121 =(I y2 I z2 m 2r 2 1)c 2s 2 +(I y3 I z3)c 23s 23 θ 3 m 3(l 1c 2 + r 2c 23)(l 1s 2 + r 2s 23) θ 1 θ 2 L 1 L 2 L 3 θ 4 M 11 = I y2 s I y3 s I z1 + I z2 c I z3 c m 2 r 2 1c m 3 (l 1 c 2 + r 2 c 23 ) 2 M 12 =0 M 13 =0 M 21 =0 M 22 = I x2 + I x3 + m 3 l1 2 + m 2 r1 2 + m 3 r2 2 +2m 3 l 1 r 2 c 3 Γ 131 =(I y3 I z3)c 23s 23 m 3r 2s 23(l 1c 2 + r 2c 23) Γ 211 =(I z2 I y2 + m 2r1)c 2 2s 2 +(I z3 I y3)c 23s 23 + m 3(l 1c 2 + r 2c 23)(l 1s 2 + r 2s 23) Γ 223 = l 1m 3r 2s 3 L 4 Γ 232 = l 1m 3r 2s 3 x z S y l 0 l 1 l 2 M 23 = I x3 + m 3 r m 3 l 1 r 2 c 3 M 31 =0 M 32 = I x3 + m 3 r2 2 + m 3 l 1 r 2 c 3 M 33 = I x3 + m 3 r 2 2. Γ 233 = l 1m 3r 2s 3 Γ 311 =(I z3 I y3)c 23s 23 + m 3r 2s 23(l 1c 2 + r 2c 23) Γ 322 = l 1m 3r 2s 3 0 (m 2 gr 1 + m 3 gl 1 )cosθ 2 m 3 r 2 cos(θ 2 + θ 3 )) m 3 gr 2 cos(θ 2 + θ 3 )) Lagrangian Formulation Lecture 12 (ECE5463 Sp18) Wei Zhang(OSU) 17 / 20

18 Lagrangian Dynamic Equations of Open Chains Dynamic model of PUMA 560 Arm: 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