NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING. Dr. Stephen Bruder NMT EE 589 & UNM ME 482/582

Transcription

1 NMT EE 589 & UNM ME 48/58 ROBOT ENGINEERING Dr. Stephen Bruder NMT EE 589 & UNM ME 48/58

2 7. Robot Dynamcs 7.5 The Equatons of Moton Gven that we wsh to fnd the path q(t (n jont space) whch mnmzes the energy of the system mn ( К υ) dq mn ( q, q) dq dqn where the Lagrangan s T ( q, q) q D( q) q υ( q) From Calculus of Varatons we know that the optmal path satsfes (7.5) ( ) d ( ) q dt q (7.6), Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde /

3 7. Robot Dynamcs 7.5 The Equatons of Moton Let's further examne the terms on the lhs of equaton (7.6). Frst expand equaton (7.5) usng D D D n q T q D( q) q [ q qn] (7.7) Dn Dn D nn q n n n D j ( q) qq j j (7.8) Applyng equaton (7.6), n scalar form we get, ( ) d ( ), k,, n qk dt qk (7.9), Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 3 /

4 7. Robot Dynamcs 7.5 The Equatons of Moton For k =,...,n: Startng wth the second term on the lhs of equaton (7.9) ( ) n n υ( q) D kj ( q) q j D ( ) k q q j q (7.) k q n Dk Dk ( ) j D kj q q j (snce D s symmetrc and the last equals ) (7.) d () d n D ( ) j kj q q j dt qk dt (7.) n n d D ( ) ( ) j kj q q j D j kj q q j dt D ( ) D ( ) D ( ) D ( q) n n n kj D ( ) kj q q j q j j q j q ( q, q n n ) ( ) ( ) j j j D q q q q υ n n kj kj kj D ( ) kj q q j q j j q qn q j q q qn (7.3), Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 4 /

5 7. Robot Dynamcs 7.5 The Equatons of Moton Next, the frst term on the lhs of equaton (7.9) suggests that ( ) D ( q) υ( q) q q q n n j qq j j k k k ( q, q n n ) ( j (7.4) ) j ( ) j D q q q q υ Fnally, substtutng these results (equatons (7.3) and (7.4)) back nto equaton (7.9) yelds D ( q) D ( q) υ( q) ( ) n n n kj j D kj q q j q j j q j q qk qk (7.5) Hence, q q q q (7.6) [ Dk( q) Dk ( q) Dkn ( q)] [ Ck( q, q) Ck ( q, q) Ckn( q, q)] Gk ( q) qn qn, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 5 /

6 7. Robot Dynamcs 7.5 The Equatons of Moton Recallng Eqn. 7.5 D ( q) D ( q) υ( q) ( ) n n n kj j D kj q q j q j j q j q qk qk (7.5) we could also wrte the resultng Matrx/Vector eqn as: q qq q q q q q q [ Dk( q) Dk ( q) Dkn ( q)] C( q) Gk ( q) n n n Ths representaton provdes better clarty regardng the nherent structure of the equatons: D(.) s only a fn of the q s and s multpled only by q, C(.) (as shown above) s only a fn of the q s and s multpled by q q j type terms, and The G(.) vector s only a functon of the q s, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 6 /

7 7. Robot Dynamcs 7.5 The Equatons of Moton Collectng rows correspondng to k =,...,n, and rewrtng n matrx form gves: D( q) q C( q, q) q G( q) OR D( q) q C( q) qq j G( q) (7.6) In the general case wheren an external torque (τ) s appled to the system, the rhs of equaton (7.6) becomes non-zero. Specfcally, D( q) q C( q, q) q G( q) (7.7) In our robotcs problems, τ s the vector of jont torques generated by the actuator at each jont. Equaton (7.7) represents a very generalzed form of a dynamc model, whch s vald for any robot wth rgd lnks (and rgd jonts). We could also add other dsspatve terms such as frcton, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 7 /

8 7. Robot Dynamcs 7.5 The Equatons of Moton Summary: The path q(t whch satsfes (7.7) was obtaned by mposng ( ) d ( ) q dt q (7.8) where L К υ, s the dfference between the knetc energy and potental energy., Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 8 /

9 7. Robot Dynamcs 7.6 A Step by Step Procedure to Construct the Dynamc Model Step : Inerta Matrces Frst, compute the nerta matrx of each lnk, preferably n a coordnate system located at the lnk's center of mass (frame { }, =,...,n) of each lnk. For the purpose of ths course, the nerta matrces, =,...,n wll always be gven! Step : Forward Knematcs Construct the D-H table, and thus the forward knematc relatonshps T, T,, n T. Transform these homogeneous matrces to lnk center of mass coordnates: T ( q, q,, q ) T ( q, q,, q ) T,,, n c c R From these we can get Pc, z, and, =,...,n., Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 9 /

10 7. Robot Dynamcs 7.6 A Step by Step Procedure to Construct the Dynamc Model Step 3: Compute Veloctes a) Construct the v c obtaned by drectly dfferentatng Pc. (.e., v c expressed n terms of frame {}), whch can be b) Construct the ω, =,...,n, whch can be obtaned by recallng that T then transform ths vector to frame { } by ( R). Step 4: Knetc & Potental Energy z z z Develop the expressons for the knetc energy К = К, where = ( T T К v v m ) and potental energy υ = υ c c υ ( q) g P m T c n = n, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde /

11 7. Robot Dynamcs 7.6 A Step by Step Procedure to Construct the Dynamc Model Step 5: Euler-Lagrange Eqn Apply the scalar Euler-Lagrange equatons to the Lagrangan L К υ, =,...,n to develop Step 6: Generalzed Form ( ) d ( ) k, k,, n qk dt qk (7.3) Collect the terms and rewrte the expressons generated from Step 5 n the desred matrx form: OR D( q) q C( q, q) q G( q) D( q) q C( q) qq j G( q) (7.3) Ths s a dfferental equaton whch governs the moton of the robot, descrbed n terms of the generalzed coordnates q(t., Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde /

12 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example A Frst Dynamc Model Example: A two lnk RR manpulator. The Denavt-Hartenberg table: D-H params. - a - d d () t () t Handout: Mathematca Fle: RR_bot, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde /

13 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example Step : Inerta Matrces Assume the nerta matrces are computed n the lnk center of mass coordnate frames {c } and {c }, respectvely, as Model the lnks as slender rods c I ; I I I c, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 3 /

14 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example Step : Forward Knematcs From the D-H table, C S C S S C S C T T Hence, C S C S C S T T T, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 4 /

15 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example From the dagram ct ct hence and C S C S C S T T T T c c C S C C S C S S T T T T c c, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 5 /

16 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example Step 3: Compute Veloctes Compute the lnear veloctes for each center of mass (v c, =,), wth respect to frame {}: v v c d dt c C d ( Pc ) S dt (C C ) d d ( Pc ) (S S ) dt dt S C v c v c S S C C, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 6 /

17 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example Compute the angular velocty for each lnk ω (n terms of frame {}), T Then, compute ( R), the angular velocty of each lnk coordnatzed n the same frame n whch s computed (.e., center of mass) Lnk#: z C S T ( R) S C Lnk#: z T ( R) T, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 7 /

18 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example Step 4: Knetc & Potental Energy Notng that gravty acts n the negatve y drecton gves: g [ g ] T where g 9.8 m/s. The potental energy s: υ υ T m g Pc m[ g ] S m gs T mg Pc m g mg C C C [ ] S S (S S ), Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 8 /

19 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example The knetc energy s: ( T T К mv c v c ) S m[ S C ] C [ ] I I ( m I ) К ( m v v ) T T c c (S S ) S 4 m [ (S S ) S (C C ) C ] (C C ) C [ ] I I [ ( ) 4 (( C ) ( C ) )] I m, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde 9 /

20 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example Step 5: Euler-Lagrange Eqn Apply the Euler-Lagrange equatons. К К υ υ m gs 4 m g(s S ) ( m I ) I ( ) 4m ( C ) ( C ) d dt g c m c c m m s m s m c m I I c m I d dt gc m 4m s 4 c m I 4m I, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde /

21 7. Robot Dynamcs 7.7 A Frst Dynamc Model Example Step 6: Generalzed Form Rewrtng n the generalzed matrx form D( q) q C( q, q) q G( q) gves rse to m 8 m ( C ) I I 4 m ( C ) I 8mS 4mS gmc gm (C C ) 4 m ( C ) I 4m I 4mS gm C Optonally, n the alternatve generalzed form D( q) q C( q) qq j G( q) m 8 m ( C ) I I 4 m ( C ) I 4 m ( C ) I 4m I 8mS 4mS gmc gm (C C ) 4mS gmc, Dr. Stephen Bruder ME 48/58: Robotcs Engneerng Tuesday 6th Nov Slde /