Lecture 23: Newton-Euler Formulation. Vaibhav Srivastava

Transcription

1 Lecture 23: Newton-Euler Formulaton Based on Chapter 7, Spong, Hutchnson, and Vdyasagar Vabhav Srvastava Department of Electrcal & Computer Engneerng Mchgan State Unversty Aprl 10, 2017 ECE 818: Robotcs vabhav/teachng/robotcs.html 1 / 11

2 Newtonan Mechancs Every acton has an equal and opposte reacton If body 1 apples a force f and torque τ to body 2, then body 2 apples a force of f and torque of τ to body 1 The rate of change of the lnear momentum equals the total force appled to the body d(mv) = f dt The rate of change of the angular momentum equals the total torque appled to the body d(i 0 Ω 0 ) = τ 0, dt where I 0 s the moment of nerta of the body about an nertal frame whose orgn s at the center of mass 2 / 11

3 Angular Momentum Equaton n Body Frame Recall ṘR = Ω 0, Ω 0 = RΩ, and Ω = R Ω 0 Angular momentum, expressed n the nertal frame, s h = I 0 Ω 0 = RIR RΩ = RI Ω 3 / 11

4 Angular Momentum Equaton n Body Frame Recall ṘR = Ω 0, Ω 0 = RΩ, and Ω = R Ω 0 Angular momentum, expressed n the nertal frame, s h = I 0 Ω 0 = RIR RΩ = RI Ω ḣ = ṘI Ω + RI Ω Recall Ṙ = Ω 0 R. Thus ḣ = Ω 0 RI Ω + RI Ω 3 / 11

5 Angular Momentum Equaton n Body Frame Recall ṘR = Ω 0, Ω 0 = RΩ, and Ω = R Ω 0 Angular momentum, expressed n the nertal frame, s h = I 0 Ω 0 = RIR RΩ = RI Ω ḣ = ṘI Ω + RI Ω Recall Ṙ = Ω 0 R. Thus ḣ = Ω 0 RI Ω + RI Ω Rate of of change of the angular momentum n body frame s τ = R ḣ = ΩI Ω + I Ω = Ω (I Ω) + I Ω Ω (I Ω) s called the gyroscopc term 3 / 11

6 Newton-Euler Equatons for a sngle lnk Notaton: a c, = the acceleraton of the center of mass of lnk a e, = the acceleraton of the end of lnk (.e., jont + 1) Ω = the angular velocty of frame w.r.t. frame 0 α = the angular acceleraton of frame w.r.t. frame 0 4 / 11

7 More Notaton: Newton-Euler Equatons for a sngle lnk II g = the acceleraton due to gravty (expressed n frame ) f = the force exerted by lnk 1 on lnk τ = the torque exerted by lnk 1 on lnk R +1 = the rotaton matrx from frame + 1 to frame m = the mass of lnk I = the nerta matrx of lnk about a frame parallel to frame whose orgn s at the center of mass of lnk r,c = the vector from jont to the center of mass of lnk r +1,c = the vector from jont + 1 to the center of mass of lnk r,+1 =the vector from jont to jont / 11

8 Newton-Euler Equatons for a sngle lnk III The force balance equaton for lnk The moment equaton s f R +1 f +1 + m g = m a c, τ R +1 τ +1 + f r,c (R +1 f +1 ) r +1,c = I α + Ω (I Ω ) 6 / 11

9 Newton-Euler Equatons for a sngle lnk III The force balance equaton for lnk The moment equaton s f R +1 f +1 + m g = m a c, τ R +1 τ +1 + f r,c (R +1 f +1 ) r +1,c = I α + Ω (I Ω ) If a c,, Ω and α are known, then can solve for f and τ recursvely f = R +1 f +1 m g + m a c, τ = R +1 τ +1 f r,c + (R +1 f +1 ) r +1,c + I α + Ω (I Ω ), wth f n+1 = 0 and τ n+1 = 0 6 / 11

10 Expressng Lnk Veloctes and Acceleraton n Jont Varables Consder revolute jonts and recall Ω 0 = Ω z 1 q Thus, Ω = (R 1 ) Ω 1 + b q, where b = (R 0 ) z 1 (z 1 expressed n Σ ) 7 / 11

11 Expressng Lnk Veloctes and Acceleraton n Jont Varables Consder revolute jonts and recall Ω 0 = Ω z 1 q Thus, Ω = (R 1 ) Ω 1 + b q, where b = (R 0 ) z 1 (z 1 expressed n Σ ) Angular acceleraton: α = (R 0 ) Ω 0 Ω 0 = Ω z 1 q + Ω 0 (z 1 q ) = α = (R 1) α 1 + b q + Ω (b q ) 7 / 11

12 Expressng Lnk Veloctes and Acceleraton n Jont Varables Consder revolute jonts and recall Ω 0 = Ω z 1 q Thus, Ω = (R 1 ) Ω 1 + b q, where b = (R 0 ) z 1 (z 1 expressed n Σ ) Angular acceleraton: α = (R 0 ) Ω 0 Ω 0 = Ω z 1 q + Ω 0 (z 1 q ) = α = (R 1) α 1 + b q + Ω (b q ) Lnear velocty: v 0 c, = v 0 e, 1 + Ω0 r 0,c Dfferentatng a 0 c, = a 0 e, 1 + Ω 0 r 0,c + Ω0 (Ω 0 r 0,c ) Usng the fact that a c, = (R 0 ) a 0 c, a c, = (R 1) a e, 1 + Ω r,c + Ω (Ω r,c ) a e, = (R 1) a e, 1 + Ω r,c + Ω (Ω r,+1 ) 7 / 11

13 Newton-Euler Formulaton Summary 1. Start wth the ntal condtons Ω 0 = 0, α 0 = 0, a c,0 = 0, and a e,0 = 0 2. Recursvely compute Ω, α and a c, usng forward teraton on Ω = (R 1) Ω 1 + b q α = (R 1) α 1 + b q + Ω (b q ) a e, = (R 1) a e, 1 + Ω r,c + Ω (Ω r,+1 ) a c, = (R 1) a e, 1 + Ω r,c + Ω (Ω r,c ) 8 / 11

14 Newton-Euler Formulaton Summary 1. Start wth the ntal condtons Ω 0 = 0, α 0 = 0, a c,0 = 0, and a e,0 = 0 2. Recursvely compute Ω, α and a c, usng forward teraton on Ω = (R 1) Ω 1 + b q α = (R 1) α 1 + b q + Ω (b q ) a e, = (R 1) a e, 1 + Ω r,c + Ω (Ω r,+1 ) a c, = (R 1) a e, 1 + Ω r,c + Ω (Ω r,c ) 3. Start wth the termnal condtons f n+1 = 0 and τ n+1 = 0 and recursvely compute f and τ usng backward teraton on f = R +1 f +1 m g + m a c, τ = R +1 τ +1 f r,c + (R +1 f +1 ) r +1,c + I α + Ω (I Ω ) 8 / 11

15 Planar Elbow Manpulator Revsted Ω 1 = qk, α 1 = q 1 k, Ω 2 = ( q 1 + q 2 )k, and α 2 = ( q 1 + q 2 )k r 1,c1 = l c1, r 2,c1 = (l 1 l c1 ), and r 1,2 = l 1 r 2,c2 = l c2, r 3,c2 = (l 2 l c2 ), and r 2,3 = l 2 Forward Recurson Lnk 1 [ lc1 q a c,1 = q 1 k l c1 + q 1 k ( q 1 k l c1 ) = 1 2 ] l c1 q 1 [ l1 q a e,1 = 1 2 ] l 1 q 1 [ ] g 1 = (R0 1 ) sn q1 gj = g cos q 1 9 / 11

16 Planar Elbow Manpulator Revsted II Forward Recurson Lnk 2 a c,2 = (R 2 1 ) a e,1 + ( q 1 + q 2 )k l c2 + ( q 1 + q 2 )k (( q 1 + q 2 )k l c2 ) 10 / 11

17 Planar Elbow Manpulator Revsted II Forward Recurson Lnk 2 a c,2 = (R1 2 ) a e,1 + ( q 1 + q 2 )k l c2 + ( q 1 + q 2 )k (( q 1 + q 2 )k l c2 ) [ l1 q = 1 2 cos q 2 + l 1 q 1 sn q 2 l c2 ( q 1 + q 2 ) 2 ] l 1 q 1 2 sn q 2 + l 1 q 1 cos q 2 l c2 ( q 1 + q 2 ) 10 / 11

18 Planar Elbow Manpulator Revsted II Forward Recurson Lnk 2 a c,2 = (R1 2 ) a e,1 + ( q 1 + q 2 )k l c2 + ( q 1 + q 2 )k (( q 1 + q 2 )k l c2 ) [ l1 q = 1 2 cos q 2 + l 1 q 1 sn q 2 l c2 ( q 1 + q 2 ) 2 ] l 1 q 1 2 sn q 2 + l 1 q 1 cos q 2 l c2 ( q 1 + q 2 ) g 2 = g [ ] sn(q1 + q 2 ) cos(q 1 + q 2 ) 10 / 11

19 Planar Elbow Manpulator Revsted II Backward Recurson Lnk 2 f 2 = m 2 a c,2 m 2 g 2 τ 2 = I 2 α 2 + Ω 2 (I 2 Ω 2 ) f 2 l c2 11 / 11

20 Planar Elbow Manpulator Revsted II Backward Recurson Lnk 2 f 2 = m 2 a c,2 m 2 g 2 τ 2 = I 2 α 2 + Ω 2 (I 2 Ω 2 ) f 2 l c2 τ 2 = I 2 ( q 1 + q 2 )k + (m 2 l 1 l c2 sn q 2 q 2 1+ m 2 l 1 l c2 cos q 2 q 1 + m 2 l 2 c2( q 1 + q 2 ) + m 2 l c2 g cos(q 1 + q 2 ))k 11 / 11

21 Planar Elbow Manpulator Revsted II Backward Recurson Lnk 2 f 2 = m 2 a c,2 m 2 g 2 τ 2 = I 2 α 2 + Ω 2 (I 2 Ω 2 ) f 2 l c2 τ 2 = I 2 ( q 1 + q 2 )k + (m 2 l 1 l c2 sn q 2 q 2 1+ m 2 l 1 l c2 cos q 2 q 1 + m 2 l 2 c2( q 1 + q 2 ) + m 2 l c2 g cos(q 1 + q 2 ))k Backward Recurson Lnk 1 f 1 = m 1 a c,1 + R 2 1 f 2 m 1 g 1 τ 1 = R 2 1 τ 2 f 1 l c,1 (R 2 1 f 2 ) (l 1 l c1 ) + I 1 α 1 + Ω 1 (I 1 Ω 1 ) 11 / 11

22 Planar Elbow Manpulator Revsted II Backward Recurson Lnk 2 f 2 = m 2 a c,2 m 2 g 2 τ 2 = I 2 α 2 + Ω 2 (I 2 Ω 2 ) f 2 l c2 τ 2 = I 2 ( q 1 + q 2 )k + (m 2 l 1 l c2 sn q 2 q 2 1+ m 2 l 1 l c2 cos q 2 q 1 + m 2 l 2 c2( q 1 + q 2 ) + m 2 l c2 g cos(q 1 + q 2 ))k Backward Recurson Lnk 1 f 1 = m 1 a c,1 + R1 2 f 2 m 1 g 1 τ 1 = R1 2 τ 2 f 1 l c,1 (R1 2 f 2 ) (l 1 l c1 ) + I 1 α 1 + Ω 1 (I 1 Ω 1 ) τ 1 = τ 2 + (m 1 l 2 c1 + m 1 l c1 g cos q 1 + m 2 l 1 cos q 1 + I 1 q 1 + m 2 l 2 1 q 1 m 1 l 1 l c2 ( q 1 + q 2 ) 2 sn q 2 + m 2 l 1 l c2 ( q 1 + q 2 ) cos q 2 )k 11 / 11