EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators 1. Introducton Iteratve General Dynamc Model for Seral-Lnk Manpulators In ths set of notes, we are gong to develop a method for computng a general dynamc model for seral-lnk manpulators. Ths dynamc model wll relate the set of torques or forces τ requred at the jonts of the manpulator (torques for revolute jonts, forces for prsmatc jonts) to acheve a partcular set of jont postons, veloctes and acceleratons ( ΘΘ,, Θ ): τ Note that n equaton (1), τ, Θ, Θ and Θ are all n 1 vectors, where n s the number of jonts n the manpulator, and h s some nonlnear functon. In order to derve such a dynamc model, we wll do the followng: 1. Relate lnear and angular acceleratons of coordnate frames wth respect to one another. 2. Generalze the basc laws of motons to three dmensons and apply those laws of moton to the seral confguraton of manpulators. 3. Extend the concept of moments of nerta to three dmensons (nerta tensor). [Ths part of the dscusson wll not be part of ths set of notes, but wll be handled elsewhere. 2. cceleratons between coordnate frames. asc defntons The lnear acceleraton Q of a pont Q wth respect to some coordnate frame { } s defned as the tme dervatve of the lnear velocty V Q : Note that ths defntons s smlar to that of lnear velocty tself (from Chapter 5): Smlarly, the angular acceleraton Ω of coordnate frame { } wth respect to coordnate frame { } s defned as the tme dervatve of the angular acceleraton Ω :. Notaton h( ΘΘ,, Θ ) d V Q Q ( V Q ) lm ( t+ t) V Q () t t 0 -------------------------------------------------- t d V Q ( Q) lm Q( t+ t) Q() t t 0 --------------------------------------------- t d Ω ( Ω ) lm ( t+ t) Ω () t t 0 --------------------------------------------------- t We defne the followng short-hand notaton for the lnear and angular acceleratons of some coordnate frame { } wth respect to a fxed unversal reference frame { } : (1) (2) (3) (4) v Ȧ ω ORG (smlar to v VORG from Chapter 5) (5) Ω (smlar to ω Ω from Chapter 5) (6) C. Lnear acceleratons between coordnate frames In Chapter 5, we derved the followng mportant equaton for two coordnate frames { } and { } wth concdent orgns (.e. no translaton between { } and { } ): VQ R( VQ) + [ R( Q) ({ } and { } orgns concdent). (7) - 1 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators Let us rewrte equaton (7) to establsh an mportant formula for the tme dervatve of a rotaton matrx tmes a vector: VQ R ( VQ) + [ R( Q) d [ R ( Q) R( Q ) + [ R( Q) (8) (9) d [ R ( Q) R( Q ) + [ R( Q) Note that equaton (10) gves us a general formula for the tme dervatve of RQ where R s some rotaton matrx and Q s some vector. Now, let us dfferentate equaton (8) wth respect to tme: (10) VQ R ( VQ) + [ R( Q) d Q R [ ( VQ) Ω [ R( Q) d + + Ω [ R ( Q) In equaton (12) we made use of the followng dentty for any 3-space vectors Q and P : (11) (12) d dq dp ( Q P) P + Q Q P + Q P. (13) Let us now substtute equaton (10) nto equaton (12): d Q R [ ( VQ) Ω [ R( Q) d + + Ω [ R ( Q) Q R( Q ) R + [ ( VQ) + Ω [ R( Q) + Ω R( Q ) R + [ ( Q) In equaton (15), let Q VQ so that: Q R( Q ) R + [ ( VQ) + Ω [ R( Q) + Ω R( VQ) R( + [ Q) (14) (15) (16) Note that we can combne two cross-product terms n equaton (16), Q R( Q ) R + [ ( VQ) + Ω [ R( Q) + Ω R( VQ) R( + [ Q) Q R( Q ) 2 R + [ ( VQ) + Ω [ R( Q) + [ Ω R( Q) (17) (18) - 2 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators Q R( Q ) 2 R + [ ( VQ) + Ω [ R( Q) + [ R( Q) (19) Equaton (19) gves the angular acceleraton Q for a vector Q defned wth respect to coordnate frame { }, when the orgns of coordnate frames { } and { } are concdent wth one another. If the orgn of coordnate frame { } s acceleratng wth respect to { } equaton (19) s easly modfed to nclude that adonal lnear acceleraton: Q ORG R( Q ) 2 R + + [ ( VQ) + Ω [ R( Q) + [ Ω R( Q) (20) Q ORG R( Q ) 2 R + + [ ( VQ) + Ω [ R( Q) + [ R( Q) (21) Equaton (21) allows for the possblty that vector Q s movng wth respect to coordnate frame { }. Let us now consder a more restrctve case namely, that vector Q s fxed wth respect to coordnate frame { }. In other words, we wll assume that there s movement only between coordnate frames { } and { } (and not wthn coordnate frame { } ), so that: VQ 0 Q 0 (22) (23) Ths assumpton smplfes equaton (21) substantally: Q ORG R( Q ) 2 R + + [ ( VQ) + Ω [ R( Q) + [ Ω R( Q) (24) Q ORG + Ω [ R( Q) + [ R( Q), VQ Q 0 (25) D. ngular acceleratons between coordnate frames Now, let us consder angular acceleratons between dfferent coordnate frames. Let us begn wth the relatonshp of angular veloctes between three coordnate frames { }, { } and { C} : ΩC + R( ΩC) Dfferentatng (26) and agan substtutng equaton (10), we get: Ω Ω d C + [ R ( ΩC) Ω Ω C + R( Ω C ) + [ R( ΩC) Ω Ω C + R( Ω C ) + [ R( ΩC) (26) (27) (28) (29) E. Summary of results elow, we summarze our results on lnear and angular acceleratons: Q ORG R( Q ) 2 R + + [ ( VQ) + Ω [ R( Q) + [ Ω R( Q) (30) Q ORG + Ω [ R( Q) + [ Ω R( Q), VQ Q 0 (31) Ω Ω C + R( Ω C ) + [ R( ΩC) (32) - 3 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators 3. asc equatons of moton. Newton s law Consder Fgure 1 below, whch depcts a rgd body, whose center of mass s acceleratng wth acceleraton v Ċ under a net force F actng on the body. (Crag, Fg. 6.3) Fgure 1: Rgd body, whose center of mass s acceleratng under the acton of a net force F. Newton s second law of moton relates F and v Ċ : F mv Ċ (33) where, F m v Ċ net force actng on the body, (34) mass of the body, and, (35) acceleraton of the center of mass of the body. (36). Euler s law Consder Fgure 2 below, whch depcts a rgd body, whch s rotatng wth angular velocty ω and angular acceleraton ω under a net moment N actng on the body. (Crag, Fg. 6.4) Fgure 2: Rgd body, whch s rotatng under the acton of a net moment N. - 4 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators Euler s law of rotatonal moton for rgd bodes relates N and ( ωω, ): N where, C I ω ω C + I ω (37) N net moment actng on the body, (38) C I ω ω 3 3 nerta tensor, wrtten wth respect to coordnate frame { C} (at center of mass), (39) angular velocty of the body, and, (40) angular acceleraton of the body. (41) C. Dynamc modelng Gven the results of Secton 2 on acceleratons, and the basc laws of moton n the prevous two sub-sectons, we wll now proceed as follows n dervng the dynamc model for a seral-lnk manpulator: τ h( ΘΘ,, Θ ) We wll assume that the jont postons Θ, jont veloctes Θ and jont acceleratons Θ are known, so that we can compute the correspondng jont torques/forces τ requred to acheve the known jont trajectory. We then break down the development of the dynamc model n equaton (42) nto three man tasks: 1. Outward propagaton of veloctes and acceleraton from the base coordnate frame { 0} to the end-effector coordnate frame { N}. 2. Newton s and Euler s equatons of moton from the base coordnate frame { 0} to the end-effector coordnate frame { N}. 3. Inward propagaton of force balance and moment balance equatons from coordnate frame { N} to coordnate frame { 1}. 4. Propagaton of veloctes and acceleratons. ngular veloctes and acceleratons In Chapter 5, we developed the followng relatonshp for angular veloctes between consecutve lnks: + 1 + 1 ω + 1 R + 1 ω + θ + 1 Ẑ + 1 (42) (velocty propagaton) (43) Now, we want to develop an analogous relatonshp for angular acceleratons: + 1 ω + 1 g ( ω, ω ) where g () represents some functonal mappng. To do ths, we wll apply the general relatonshp that we developed n Secton 2 for the propagaton of angular acceleratons: (44) ΩC + R( ΩC) (45) Ω Ω C + R( Ω C ) + [ R( ΩC). (46) + 1 Frst, we wll get equaton (43) nto the same form as equaton (45). Recall that ω and + 1 ω are shorthand notaton, and can be wrtten less compactly as, ω lso note that, R + 1 + 1 ( Ω) and ω + 1 ( Ω + 1). (47) - 5 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators + 1 θ + 1 Z +ˆ 1 + 1 R( Ω + 1). (48) Equaton (48) requres some explanaton. The rght-hand sde of (48) denotes the angular velocty of coordnate frame { + 1} wth respect to {} expressed n the { + 1} coordnate frame. Now, thnk about the left-hand sde of (48). The angular velocty between frames {} and { + 1} s gven exactly by the jont rate θ + 1 orented along the Ẑ + 1 axs, and that the left-hand sde of (48) s expressed n terms of the { + 1} coordnate frame. Substtutng (47) and (48) nto equaton (43), + 1 + 1 ω + 1 R + 1 ω + θ + 1 Ẑ + 1 + 1 + 1 ( Ω + 1) R + 1 ( Ω) + R( Ω + 1) + 1 + 1 ( Ω + 1) R + 1 ( Ω) + R( Ω + 1) Now, let and + 1 C so that equaton (51) becomes: C C ( ΩC) C R ( Ω) + C R( ΩC) ( ΩC) C ( Ω) + C R( ΩC) Multplyng equaton (53) by CR and lettng, CR C ( ΩC) CR C ( Ω) + CR C R( ΩC) (49) (50) (51) (52) (53) (54) ΩC Ω + R( ΩC) (55) ΩC Note that we have now transformed equaton (43) nto (45), whch means that equaton (46) can be used to propagate angular acceleratons from one lnk to the next for seral-lnk manpulators. In summary, + R( ΩC) (56) + 1 + 1 ω + 1 R + 1 ω + θ + 1 Ẑ + 1 C Ω + R( ΩC) wth substtutons: + 1 + 1 ω + 1 ( Ω + 1) ω ( Ω) (57) (58) (59) + 1 θ + 1 Z +ˆ 1 + 1 R( Ω + 1), (60), + 1 C and. (61) From equaton (46), angular acceleratons are propagated by, Ω Ω C + R( Ω C ) + [ R( ΩC) Let us now transform ths relatonshp nto lnk notaton by reverse substtuton. Frst let, C + 1 and, so that: Ω + 1 Ω + R( Ω + 1) + Ω [ R( Ω + 1). (63) (62) - 6 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators + 1 Multply equaton (63) by : + 1 + 1 + 1 Ω + 1 Ω + 1 R( Ω + 1) Ω R + + [ ( Ω + 1) (64) In order to further develop equaton (64), we wll need the followng dentty: for any 3-space vectors Q, P and rotaton matrx R, RQ ( P) ( RQ) ( RP). (65) Thus, we can modfy the last term of (64) usng (65): + 1 + 1 + 1 Ω + 1 Ω R + 1 R( Ω + 1) Ω R + + [ ( Ω + 1) + 1 + 1 + 1 Ω + 1 Ω R + 1 R( Ω + 1) R + 1 + + [ Ω [ R( Ω + 1) sng, (66) (67) + 1 R + 1 + 1 + 1 R and R, (68) equaton (67) further smplfes to: + 1 + 1 + 1 + 1 Ω + 1 Ω R Ω + 1 R + 1 + + [ Ω ( R Ω + 1) + 1 + 1 Ω + 1 R + 1 + 1 ( Ω ) R Ω + 1 R R + 1 + + [ ( Ω) ( R Ω + 1) Fnally, we make the followng substtutons nto equaton (70): (69) (70) Ω ω (by defnton) (71) + 1 + 1 Ω ω and ( Ω + 1 ) ω + 1 (by defnton) (72) + 1 R + 1 Ω + 1 θ + 1 Ẑ + 1 + 1 + 1 R Ω + 1 θ + 1 Ẑ + 1 These substtutons result n: [prevously explaned n dscusson followng (48) (73) [analogous to (73) above (74) + 1 + 1 Ω + 1 R + 1 + 1 ( Ω ) R Ω + 1 R + 1 + + [ ( Ω) ( R Ω + 1) + 1 + 1 + 1 + 1 ω + 1 R ω θ + 1 Ẑ + 1 R + 1 + + ( ω) ( θ + 1 Ẑ + 1) + 1 + 1 + 1 ω + 1 R ω R + 1 + 1 + ( ω) θ + 1 Ẑ + 1 + θ + 1 Ẑ + 1 (75) (76) (77). ngular veloctes and acceleratons: summary Summarzng the results developed n the prevous secton, angular veloctes and acceleratons are propagated from lnk to lnk + 1 usng the followng two equatons (for revolute jonts): + 1 + 1 ω + 1 R + 1 ω + θ + 1 Ẑ + 1 [from (43) (78) - 7 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators + 1 + 1 + 1 ω + 1 R ω R + 1 + 1 + ( ω) θ + 1 Ẑ + 1 + θ + 1 Ẑ + 1 [from (77) (79) + 1 Note that equatons (78) and (79) allow us to compute the angular velocty ω + 1 of lnk frame { + 1} + 1 n terms of the angular velocty ω of lnk frame {} and θ + 1 ; and the angular acceleraton ω + 1 of lnk frame { + 1} n terms of the angular acceleraton ω and angular velocty ω of lnk frame {} and θ 1. lso, note that for a prsmatc jont, + θ 1 + θ + 1 0 so that (78) and (79) smplfy (for prsmatc jonts) to: + 1 + 1 ω + 1 R ω + 1 + 1 ω + 1 R ω (80) (81) (82) C. Lnear acceleratons of lnk frame orgns In Secton 2, we derved the followng relatonshp for the propagaton of lnear acceleratons: Q ORG R( Q ) 2 R + + [ ( VQ) + Ω [ R( Q) + [ Ω R( Q) We wll now convert equaton (83) nto lnk-specfc form as we dd above for angular acceleratons. Frst, let us make the followng substtutons: (83) (84) (85) Q + 1, ORG (for subscrpts) (86) Gven these substtutons, note that Q now denotes the orgn of lnk frame { + 1} n terms of lnk frame {} ; n short, Q P + 1, ORG Thus, equaton (83) becomes, (87) + 1, ORG, ORG + R ( + 1, ORG ) + 2 Ω [ R ( V + 1, ORG ) + Ω [ R ( P + 1, ORG ) + Ω [ Ω R ( P + 1, ORG ) + 1 Let us now pre-multply equaton (88) by : 1 + 1 + + 1, ORG, ORG + 1 + R ( + 1, ORG ) + + 1 2 + 1 Ω [ R ( V + 1, ORG ) + + 1 + 1 Ω [ R ( P + 1, ORG ) + + 1 + 1 Ω + 1 [ Ω R ( P + 1, ORG ) + 1 Note that we used vector dentty (65) n dstrbutng over the cross product of vectors n (89). Let us now consder each of the terms n equaton (89) one by one. The left-hand sde of (89) can be wrtten n short-hand notaton as: (88) (89) 1 + 1 + + 1, ORG + 1 (by defnton) (90) - 8 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators Smlarly for the frst term on the rght-hand sde of equaton (89): 1 + 1 + ORG, R ( ORG, ) + 1 R Equatons (90) and (91) smplfy equaton (89) to: (by defnton) (91) + 1 + 1 + 1 R + 1 + R ( + 1, ORG ) + + 1 2 + 1 Ω [ R ( V + 1, ORG ) + + 1 + 1 Ω [ R ( P + 1, ORG ) + + 1 + 1 Ω + 1 [ Ω R ( P + 1, ORG ) Let us now consder the second term on the rght-hand sde of equaton (92). The notaton + 1, ORG ndcates the lnear acceleraton of the orgn of lnk frame { + 1} wth respect to (and n terms of) lnk frame {}. Thus, for a seral-lnk manpulator, we can wrte: (92) + 1 R R ( + 1, ORG ) + 1 R( + 1, ORG ) ḋ + 1 + 1 Ẑ + 1 Smlarly, (93) + 1 R ( V + 1, ORG ) + 1 R( V + 1, ORG ) d + 1 + 1 Ẑ + 1 Note that lnear relatonshps n (93) and (94) are analogous to the manpulator-specfc angular relatonshps n (73) and (74). Equaton (92) now smplfes to: (94) + 1 + 1 + 1 + 1 R + ḋ + 1 Ẑ + 1 + + 1 2 + 1 Ω d + 1 Ẑ + 1 + + 1 + 1 Ω [ R ( P + 1, ORG ) + + 1 + 1 Ω + 1 [ Ω R ( P + 1, ORG ) Let us now consder the angular velocty and acceleraton terms n (95). From pror dscusson, + 1 + 1 Ω R + 1 ( Ω) R ω + 1 Ω + 1 R + 1 ( Ω ) R ω so that equaton (95) further reduces to: + 1 + 1 + 1 + 1 R + ḋ + 1 Ẑ + 1 + + 1 2 R + 1 ( ω) d + 1 Ẑ + 1 + + 1 + 1 R ω [ R R ( P + 1, ORG ) + + 1 R + 1 ω R + 1 [ ω R ( P + 1, ORG ) Next, let us make the followng substtuton n (98): + 1 R ( P + 1, ORG ) + 1 R( P + 1, ORG ) + 1 R ( P + 1) (95) (96) (97) (98) (99) - 9 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators where P s smply short-hand notaton for + 1 P + 1, ORG. Thus, equaton (98) reduces to: + 1 + 1 + 1 + 1 R + 1 ḋ + 1 Ẑ + 1 2 R + 1 + + ( ω) d + 1 Ẑ + 1 + Fnally, note that the relatonshp n (65) can be rewrtten as, so that we can rearrange and group terms n equaton (100): Thus (for a prsmatc jonts): + 1 + 1 R ω R + 1 [ ( P + 1) R + 1 ω R + 1 + [ ω R( P + 1) ( RQ) ( RP) RQ ( P) + 1 + 1 + 1 + 1 R + 1 ḋ + 1 Ẑ + 1 2 R + 1 + + ( ω) d + 1 Ẑ + 1 + + 1 + 1 R ω R + 1 [ ( P + 1) R + 1 ω R + 1 + [ ω R( P + 1) + 1 + 1 + 1 + 1 R + 1 ḋ + 1 Ẑ + 1 2 R + 1 + + ( ω) d + 1 Ẑ + 1 + + 1 + 1 R( ω P + 1) + R[ ω ( ω P + 1) (100) (101) (102) (103) + 1 + 1 + 1 R ω P + 1 ω ω P [ + ( + 1) + + + 1 2 R + 1 + 1 ( ω) d + 1 Ẑ + 1 + ḋ + 1 Ẑ + 1 + 1 Note that equaton (104) allow us to compute the lnear acceleraton + 1 of lnk frame { + 1} n terms of the angular acceleraton ω, lnear acceleraton and angular velocty ω of lnk frame {}, and d + 1 and ḋ + 1. lso, note that for a revolute jont, (104) ḋ + 1 d + 1 0 so that (104) smplfes (for revolute jonts) to: (105) + 1 + 1 + 1 R ω P. (106) + 1 ω ω P [ + ( + 1) + D. Lnear acceleraton of a lnk s center of mass In order to apply Newton s second law of moton n equaton (33), we need to know not just the lnear acceleraton of lnk frame {} but of the center of mass C of lnk as well. Once agan, we wll begn wth the relatonshp for the propagaton of lnear acceleratons derved n Secton 2: Q ORG R( Q ) 2 R + + [ ( VQ) + Ω [ R( Q) + [ Ω R( Q) (107) Let us make the followng substtutons: (108) (109) Q C (for subscrpts) (110) Gven these substtutons, note that Q now denotes the orgn of the center of mass of lnk frame { + 1} n terms of lnk frame {} ; n short, Q PC (111) - 10 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators Thus, equaton (107) becomes, C, ORG + R( ) + 2 Ω [ R( V ) + C Ω [ R( P ) + Ω [ Ω R( P ) C C C (112) In equaton (112), note that for rgd lnks, C VC 0 (113) so that (112) reduces from, C, ORG + R( ) + 2 Ω [ R( V ) + C Ω [ R( P ) + Ω [ Ω R( P ) C C C (114) to the smplfed form, C, ORG + Ω [ R( P ) + Ω [ Ω R( P ) C C (115) Let us now pre-multply equaton (115) by : C, ORG + Ω [ R( P ) + Ω [ Ω R( P ) C C (116) Note that we used vector dentty (65) n dstrbutng over the cross product of vectors n (116). Smlar to earler dervaton, we now make the followng substtutons: C C (by defnton) (117) ORG, (by defnton) (118) R Ω ω and R Ω ω (by defnton) (119) Equaton (116) consequently reduces to: C + ω [ R R( P ) + ω [ ω R( P ) C C (120) Notng that, R( P ) P C C (121) equaton (120) now reduces to: C C + ω P + ω [ ω P C C + ω P + ω [ ω P C C (122) (123) - 11 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators 5. Lnk-specfc equatons of moton. Manpulator-specfc equatons of motons We can rewrte the basc equatons of moton n (33) and (37) n more lnk specfc notaton. Specfcally, F m C (124) N C I ω + ω C I ω (125) where, F m C N net force actng on lnk, (126) mass of lnk, (127) acceleraton of center of mass of lnk, (128) net moment actng on lnk, (129) C I nerta tensor, wrtten n frame { C } located at the center of mass of lnk, (130) ω ω angular velocty of lnk, and, (131) angular acceleraton of lnk. (132) We can now completely and succnctly wrte the outward propagaton of veloctes and net forces/moments. In the followng subsectons, we do so for revolute and prsmatc jonts, respectvely.. Summary of outward teraton 1. Revolute jonts: + 1 + 1 ω + 1 R + 1 ω + θ + 1 Ẑ + 1 + 1 + 1 + 1 ω + 1 R ω R + 1 + 1 + ( ω) θ + 1 Ẑ + 1 + θ + 1 Ẑ + 1 + 1 + 1 + 1 R ω P + 1 ω ω P [ + ( + 1) + 2. Prsmatc jonts: + 1 + 1 ω + 1 R ω + 1 + 1 ω + 1 R ω (133) (134) (135) (136) (137) + 1 + 1 + 1 R ω P + 1 ω ω P [ + ( + 1) + + + 1 2 R + 1 + 1 ( ω) d + 1 Ẑ + 1 + ḋ + 1 Ẑ + 1 (138) 3. oth jont types: + 1 C + 1 + 1 + 1 + 1 ω + 1 + 1 + 1 PC ω + 1 + 1 + + + 1 + 1 [ ω + 1 PC + 1 (139) - 12 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators + 1 + 1 F + 1 m + 1 v C + 1 C 1 + 1 N + + 1 + 1 + 1 I + 1 ω + 1 ω + + 1 + + 1 I + 1 ω + 1 6. Force/moment balance equatons C 1 (140) (141). Introducton Equatons (133) through (141) gve us the net forces/moments at a gven lnk requred to cause the desred/ known jont moton ( ΘΘ,, Θ ). We now must fgure out what part of those net forces/moments must be suppled by the jont actuators. To do ths, we wll wrte force/moment balance equatons about the center of mass of each lnk. Consder Fgure 3 below, whch llustrates the forces and moments actng on lnk. In Fgure 3 we use the followng notaton: P + 1 V 2 V 1 Fgure 3: Forces and moments acton on lnk. (Crag, Fg. 6.5) f force exerted on lnk by lnk 1, (142) f + 1 force exerted on lnk + 1 by lnk, (143) n moment exerted on lnk by lnk 1, (144) n + 1 moment exerted on lnk + 1 by lnk, (145) and, as before, F N net force actng on lnk, and, (146) net moment actng on lnk. (147) lso, P + 1 vector from the orgn of frame {} to the orgn of frame { + 1}, (148) V 1 vector from the center of mass of lnk to the orgn of coordnate frame {}, and, (149) - 13 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators V 2 vector from the center of mass of lnk to the orgn of coordnate frame { + 1}. (150). Force balance equaton Gven the above notaton, we can wrte the force balance equaton for lnk : F f f + 1 (151) We can, of course, express equaton (151) wth respect to any coordnate frame. Let us rewrte (151) n terms of coordnate frame {} : F F F f f + 1 f ( + 1R ) + 1 f + 1 f ( + 1R ) + 1 f + 1 (152) (153) (154) C. Moment balance equaton Now, let us wrte the moment balance equaton about the center of mass of lnk : N n n + 1 + V 1 f V 2 f + 1 (155) Note that V 1 f and V 2 f gve the moments nduced by forces f + 1 and f, respectvely, about the + 1 center of mass of lnk {}. Let us now wrte expressons for V 1 and V 2 n lnk-specfc notaton: V 1 ( P ) C (156) V 2 ( P + 1 P ). (157) C Substtutng (156) and (157) nto (155) and expressng wth respect to frame {} : N n n + 1 + ( P ) f ( P + 1 P ) f + 1 C C (158) Rearrangng terms and keepng equaton (152) n mnd, N N N N n n + 1 PC ( f f + 1) P + 1 f + 1 n n + 1 PC F P + 1 f + 1 n n + 1 PC F P + 1 + 1 [ ( + 1R) f + 1 n n + 1 PC F P + 1 ( + 1R ) + 1 [ f + 1 (159) (160) (161) (162) D. Inward teraton of lnk forces and moments Thus the force and moment balance equatons at lnk are gven by, F f ( + 1R ) + 1 f + 1 and (163) N n n + 1 PC F. (164) P + 1 ( + 1R ) + 1 [ f + 1-14 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators We can rewrte equatons (163) and (164) as teratons that propagate f and n from the end-effector to the lnk frame { 1} : f n ( + 1R ) + 1 f + 1 + F N ( + 1R ) + 1 n + 1 PC F P + 1 ( + 1R ) + 1 + + + [ f + 1 (165) (166) Note that equatons (165) and (166) allow us to recursvely compute the forces and moments that each lnk exerts on ts neghborng lnks by nwardly propagatng from coordnate frame { N} to frame { 1}. The last remanng queston s, once equatons (165) and (166) are computed, what should be the torques/forces for the actuators to acheve the desred jont moton? ll components of the force and moment vectors f and n are ressted by the structure of the mechansm tself, except for the torque/force about/along the jont axs. Therefore the requred torque for a revolute jont s gven by, τ n Ẑ (167) whle the requred force for a prsmatc jont s gven by, τ f Ẑ. (168) 7. Complete formulaton of the teratve Newton-Euler dynamcs Ths secton summarzes the complete formulaton of the teratve Newton-Euler dynamcs model. It conssts of (1) the outward propagaton of angular veloctes and lnear and angular acceleratons, (2) the outward propagaton of net moments and forces actng on the lnks, and (3) the nward propagaton of forces and torques between lnks. Collectvely, equatons (170) through (182) mplctly defne the relatonshp we were lookng for at the begnnng of ths dscusson namely, τ h( ΘΘ,, Θ ). Outward teraton 1. Revolute jonts: (169) + 1 + 1 ω + 1 R + 1 ω + θ + 1 Ẑ + 1 + 1 + 1 + 1 ω + 1 R ω R + 1 + 1 + ( ω) θ + 1 Ẑ + 1 + θ + 1 Ẑ + 1 + 1 + 1 + 1 R ω P + 1 ω ω P [ + ( + 1) + 2. Prsmatc jonts: + 1 + 1 ω + 1 R ω + 1 + 1 ω + 1 R ω + 1 + 1 3. oth jont types: + 1 C + 1 + 1 R ω P + 1 ω ω P [ + ( + 1) + + + 1 2 R + 1 + 1 ( ω) d + 1 Ẑ + 1 + ḋ + 1 Ẑ + 1 + 1 + 1 + 1 ω + 1 + 1 + 1 PC ω + 1 + 1 + + + 1 + 1 [ ω + 1 PC + 1 (170) (171) (172) (173) (174) (175) (176) - 15 -
EEL6667: Knematcs, Dynamcs and Control of Robot Manpulators + 1 + 1 F + 1 m + 1 v C + 1 C 1 C 1 + 1 N + + 1 + 1 + 1 I + 1 ω + 1 ω + + 1 + + 1 I + 1 ω + 1 (177) (178). Inward teraton 1. oth jont types: f n ( + 1R ) + 1 f + 1 + F N ( + 1R ) + 1 n + 1 PC F P + 1 ( + 1R ) + 1 + + + [ f + 1 (179) (180) 2. Revolute jonts: τ n Ẑ (181) 3. Prsmatc jonts: τ f Ẑ. (182) C. Intalzaton of propagatons In order to compute equatons (170) through (175), we need to know 0ω, 0 0 0 ω 0 and 0 that s, the angular velocty, and lnear and angular acceleraton of the base coordnate frame { 0}. For a fxed-base manpulator, 0ω 0 000 T, (183) 0ω 0 000 T, and, (184) 0 0 0 G, (185) 0 where G denotes the gravty vector. Note that (185) s equvalent to sayng that the base of the robot s acceleratng upward wth acceleraton g, and therefore easly ncorporates the effects of gravty loadng on the lnks wthout any adonal effort. N + 1 N + 1 In order to compute equatons (179) and (180), we need to know fn + 1 and nn + 1 that s, the forces and moments from the envronment actng on the end-effector of the manpulator. When the manpulator end-effector s not n contact wth any object or obstacle, these are smply gven by, N + 1 fn + 1 000 T, and, (186) N + 1 nn + 1 000 T. (187) - 16 -