RECURSIVE FORMULATION FOR MULTIBODY DYNAMICS

Transcription

1 ultbody Dyamcs Lecture Uversty of okyo, Japa Dec. 8, 25 RECURSIVE FORULAION FOR ULIODY DYNAICS Lecturer: Sug-Soo m Professor, Dept. of echatrocs Egeerg, orea Vstg Professor, Ceter for Collaboratve Research Uversty of okyo sookm@cu.ac.kr el:

2 Cotets ematcs of a rgd body Dyamcs of a rgd body ultbody Dyamcs - Cartesa Coordate Formulato - Relatve Jot Coordate Formulato Recursve ematc relatoshp Recursve formulato I for S Recursve formulato II for S Subsystem sythess method 2

3 ULIODY DYNAICS 3

4 Defto of ultbody Systems ad S dyamcs formulatos Defto of S: ultbody systems cosst of collectos of bodes that are costraed to move relatve to oe aother by kematc coectos (Jots betwee bodes. here are two coordate systems maly used for multbody dyamcs formulatos. Oe s Cartesa Coordate formulato ad the other s relatve ot coordate formulato. Cartesa coordate Relatve Jot coordate 4

5 Cartesa coordates formulato I each body, posto ad oretato coordates are assged to descrbe the moto of a multbody system, thus we may choose Cartesa posto ad Euler parameter oretato coordates. I the formulato, we wll use xed ketc-kematc equatos. z r ody z x s y s x ody z y q r,,,, b p 7 r y x he composte vector of Cartesa geeralzed coordates for the multbody system s b 7 b q q, q, q ( + 5

6 Costrat Equatos for ematc Coectos (Jots ematc coecto betwee two bodes must be modeled usg assged Cartesa coordate system Sphercal costrat: S Φ ( P, P r + A s - r - A s Dot costrat: d Φ (a,a a a Φ d (a,a a A A a Dot 2 costrat: d2 Φ (a,d a d 6

7 Costrat Equatos for ematc Coectos (Jots Example: Costrat Equatos for Revolute ots S Φ ( P, P d Φ (h,f d Φ (h,g Pot P the body ad Pot P the body must be cocde durg the moto. Z axs of ot referece frame ad Z axs of ot referece frame must be parallel. 7

8 Costrat Equatos for ematc Coectos (Jots Computg Jacoba matrx for costrat equatos S δφ ( P, P δr δ δ δ As π r + As π δφ d (a,a a A A a δπ a A A a δπ Costrat velocty equatos Φ Φ r +Φ ω +Φ r +Φ ω r π r π Costrat accelerato equatos Φ Φ r +Φ ω +Φ r +Φ ω r π r π (Φ r +Φ r +Φ ω +Φ ω γ r r π π 8

9 Force Elemet betwee wo bodes Example: Sprg Damper ad Actuator Forces f k( l l + cl + F( l, l d r + A s - r - A s l d d 2 l d ( (r + A s - r - A s l d ( (r + - Asω - r Asω l hus, the magtude of the force the sprg-damper-actuator, wth teso take as postve s Vrtual work of ths force s δ A W f δl 9

10 Force Elemet betwee wo bodes he varatoal relatoshp betwee vrtual legth ad varato of Cartesa coordates s d δl ( ( δr δ + - Asω - r Asω l A f δ W d δ δ + ( r - Asω - r Asω l f d f d [ δr, δπ ]{ } + [ δr, δπ ]{- } l s l A d s A d δ Z Q + δ Z Q A A hus, geeralzed force due to sprg-damper-actuator are f d f d A A Q, Q - l s l A d s A d

11 Equatos of moto of costraed spatal systems Revew a sgle rgd body equatos of moto varatoal form where m r F [δ r,δ π ] Jc ω c ω Jω c δz Y - Q ( δr r m F δ Z,,, δ Y Q π ω Jc c ω Jω c For multbody system cosst of + bodes, varatoal form of equatos moto ca be obtaed by addg vrtual work form of each body equatos of moto as ( + ( + + ( δz Y - Q δz Y - Q... δz Y - Q δz where (,, must be cosstet wth costrat,.e., kematcally admssble vrtual dsplacemet vectors, so must be satsfy wth followg equatos δφ Φz δ Z + Φz δ Z Φz δ Z

12 Equatos of moto of costraed spatal systems y the Lagrage ultpler heorem, there are Lagrage multplers, such that δz δz Y - Q Φ λ δz Y - Q Φ λ ( ( Z Z δz Y - Q + Φ λ ( Z For (,, beg arbtrary. hus, by the orthogoalty theorem, S equatos of moto are obtaed as; Y - Q Φ λ + Z Y - Q Φ λ... Y - Q + Φ λ + Z Above equatos of moto have more ukows tha o. of equatos, thus costrat accelerato equatos must be augmeted. Z 2

13 Equatos of moto of costraed spatal systems Y - Q Φ λ + Z Y - Q Φ λ... Y - Q + Φ λ + Z Φ Φ Y + Φ Y Φ Y γ z z z Φ Y + Φ Y Φ Y z z z Z Φ Z Y Q ΦZ λ γ where Y [ Y, Y,..., Y ] Q [ Q, Q,..., Q ] Φ [ Φ, Φ,... Φ ] z z z z... Sce erta matrx s costat due to usg local vector of agular velocty, effectve soluto method for Augmeted atrx could be as follows; λ Φ Φ Φ Q ( ( Z Z ( Z γ Y Q ΦZλ 3

14 RECURSIVE INEAIC RELAIONSHIP 4

15 RECURSIVE INEAICS Oretato relatoshp C A A C A " ( q s the orthogoal trasformato matrx " " " from the x y z frame to the frame. " A x y z s the orthogoal trasformato matrx from the x y z frame to the x y z frame. " " " Posto relatoshp r r + s + d r + A s + A C d " ( q Fg. A par of coected bodes 5

16 RECURSIVE INEAICS Agular velocty relatoshp ω ω + ω ω + H ( A,q q ω s the relatve agular velocty of the body wth respect to the body, ad s also fucto of ot q relatve coordate ad ot relatve velocty. Velocty relatoshp d r r +ω (r - r + d r + rω r + rω + ( + r H q q q q q H s the ut vector of the ot rotatoal axs. d r r + s + d r + ω s + (ACd (q dt d d r + ω s + ω d + q r + ω ( s + d + q q q d r + rω r + + rω rh + q ω ω H q 6

17 RECURSIVE INEAICS Velocty State vector Represetato r + rω r + + rω rh + q ω ω H r + rω ω Y ω Y r + rω the velocty state vectors of bodes ad, respectvely d d + r H q H q I order to have more compact form of the equatos, the composte state vector otato ca be employed Y Y + velocty trasformato matrx from the ot space to the state vector space q Vrtual dsplacemet State vector Represetato δ Z δ Z + δq 7

18 RECURSIVE INEAICS Cartesa Velocty ad Velocty State vector Relatoshp where Y Accelerato state vector represetato Y Y Y + + q q + + D Accelerato state vector ad the Cartesa accelerato vector relatoshp Y I r I Y r ω Y Y + Y Y R δ Z q δ Z Z δπ δ δ r + r δπ δ Z δr δπ 8

19 RECURSIVE INEAICS SUARY A A C A " Posto & Oretato A r " r r A C A + s + A s Velocty Relatoshp Y Y + + d + A C q d ( q Forward kematcs q q Accelerato Relatoshp Y Y Y + + q q + + D q q 9

20 RECURSIVE DYNAICS 2

21 NON CENROIDAL EO FOR A RIGID ODY δ r { mr + m ω ( F d } + δπ (m r+ Jω +ωjω rw m mrw ( r F d [δ r,δ π ] m rw J ω ω Jω rw ω A ωa ρ c A ρca δπ δπ A A(ω +ωω(ω +ωωa J A JA m mrw ( r F d [δ r,δ π ] mrw J ω ωjω where m m ρ, d mωωρ rw c c 2

22 SAE VECOR FOR OF EQ FOR A RIGID ODY m m ρ ( m r F ωωρ c c [δ r,δ π ] m ρ c J ω ωjω ( δz Y - Q δ Z δ Z δr m m ρ r ( F mωωρ δ Z, δ,, π m Y Q ρ c J ω ωjω c c Y Y + Y Y R ( δz Y Q I r I c m I m r c c c c c m r J m r r c c f + m rω Q (Q + R Qc c c c c c c + r f + m r rω ω Jω Proof s gve U. of Iowa Ph.D hess, F.sa, Automated ethods for Hgh Speed Smulato of ultbody Dyamc Systems,

23 RECURSIVE FORULAION FOR OPEN LOOP SYSES + body ope cha system equatos of moto vrtual work form wth state vector otato δz ( Y Q 23

24 RECURSIVE FORULAION I 24

25 Recursve I Formulato (reducto procedure -2-2 δz ( Y Q +δz ( Y Q +δz ( Y Q δ Z δ Z + δq [ + δz ( Y Q +δz ( Y Q ( Y Q ] +δq ( Y Q δz ( Y Q Y Y + q + D +δz [ - (-Y- Q - + (Y + q (Q D ] ( +δq Y + q (Q D - Sce δqs arbtrary, coeffcet of δq must be zero. { Y + q (Q D } Solve for q - - q -( { Y (Q D } - Substtutg expresso q to the remag vrtual work form equatos of moto yelds 25

26 Recursve I Formulato (reducto procedure where 3 - δz ( Y - Q +δz ( Y - Q +δz ( Y - L ( - - L - (Q - D - ( (Q - D L Q + L δz ( Y - L Q L If we apply ths procedure cotually from body - to, the we wll get the base body equatos of moto ad backward recurrece formula as follows; If the base body s ot costraed, the δz s arbtrary We have Y L atrx sze of s always 6x6 26

27 Recursve I Formulato (reducto procedure ackward Recurrece formula for body L Q for body ( for body,,, L - L - D - - ( (L - D fo r b o d y,,, for body,, L Q + L for body,, Jot accelerato for ot - q ( - { Y (L D } - 27

28 Recursve algorthm I Step : Gve ot coordates, compute posto ad oretato alog the forward path from body to body A " r r + s + d r + A s + A C d ( q Step 2: Gve ot velocty, compute velocty state ad Cartesa veloctes of each body alog the forward path from body to body Y Y + q Step 3: Usg Cartesa postos, oretatos ad veloctes of bodes, compute state mass matrces ad state force vectors c m I m r c c c c c m r J m r r c c f + m rω Q (Q + R Qc c c c c c c + r f + m r rω ω Jω Step 4: Compute velocty couplg terms for each ots D q A C A 28

29 Recursve algorthm I Step 5: Compute composte erta ad force alog the backward path from body to body ( for body,,, L - L - D - - ( (L - D fo r b o d y,,, for body,, L Q + L for body,, Ad save followg terms - s -( - s -( ( D - L 2 Step 6: Solve for state accelerato of the base body Y L Step 7: Compute ot acceleratos ad accelerato state of each body alog the forward path from body to body q sy + s2 Y Y + q + D for,2,, - Step 8: Itegrate ot veloctes ad ot acceleratos for ext step Step 9: Go to Step utl the smulato edg tme s reached 29

30 RECURSIVE FORULAION II 3

31 Recursve II Formulato (reducto procedure -2-2 δz (Y Q +δz -(-Y- Q -+δz (Y Q δ Z δ Z + δq [ + ] δz ( Y Q +δz ( Y Q ( Y Q +δq ( Y Q δz ( Y Q Y Y + q + D +δz [ - (-Y- Q - + (Y + q (Q D ] ( +δq Y + q (Q D - Sce δq s arbtrary, coeffcet of δq must be zero. Y + q (Q D - 3

32 Recursve II Formulato (reducto procedure δz ( Y Q +δz [( + Y + q { Q + (Q D }] δz ( Y Q +, L Q + ( L + + D +, L Q δz ( Y Q +δz [ Y + q L ] δ Z δ Z + δq Y Y + q + D 2 2 +δz -2[ ( Y q + q ( Q-2 + ( L -D ] ( +δq Y + q + q ( L D Sce δq s arbtrary, coeffcet of δq must be zero. Y + q + q ( L D

33 Recursve II Formulato (reducto procedure 3 - δz ( Y Q +δz [( + Y + q + q ( Q + ( L D ] L Q + ( L D [ δz ( Y Q +δz Y + q + q L ] ackward recurrece formula +,, 2,,, + L L Q Q + ( L +, 2,,, + D + 33

34 Recursve II Formulato (reducto procedure Y - + q (Q D Y + q + q ( L D Y Y + q + D Y + ( q + D 2 k k k k Y + ( q + D k k k k Y q + q (L D + k k k k k Y + kq k + q + q L D k k k ( If we apply ths procedure cotually from body -2 to, the we wll get a system of equatos of moto ot space. 34

35 Recursve II Formulato (reducto procedure Equato of oto Jot Space. where q P symm q [ q, q,, q, q, Ẏ ] 2 P ( L D ( L D 2 2 ( L 2 2 D ( L D L ackward recurrece formula +,, 2,,, + L Q L Q + ( L D, 2,,,

36 Recursve algorthm II Step : Gve ot coordates, compute posto ad oretato alog the forward path from body to body A " r r + s + d r + A s + A C d ( q Step 2: Gve ot velocty, compute velocty state ad Cartesa veloctes of each body alog the forward path from body to body Y Y + q Step 3: Usg Cartesa postos, oretatos ad veloctes of bodes, compute state mass matrces ad state force vectors c m I m r c c c c c m r J m r r c c f + m rω Q (Q + R Qc c c c c c c + r f + m r rω ω Jω Step 4: Compute velocty couplg terms for each ots D q A C A 36

37 Recursve algorthm II Dk Step 5: Compute composte couplg term alog the forward path k from body to body Step 6: Compute composte erta ad composte force L alog the backward path from body to body +,, 2,,, + L Q L Q + ( L D, 2,,, Step 7: Costruct etres of system matrx ad RHS vector P symm ( L D ( L D P 2 2 ( L 2 2 D ( L D Step 8: Solve for ot acceleratos q P L Step 9: Recoverg accelerato state ad Cartesa accelerato of each body alog the forward path from body to body, f ecessary Step : Itegrate ot veloctes ad ot acceleratos for ext step Step: Go to Step utl the smulato edg tme s reached 37

38 RECURSIVE FORULAION FOR A CLOSED LOOP SYSE 38

39 39 RECURSIVE FORULAION FOR A CLOSED LOOP SYSE RECURSIVE FORULAION FOR A CLOSED LOOP SYSE RECURSIVE FORULAION FOR A CLOSED LOOP SYSE RECURSIVE FORULAION FOR A CLOSED LOOP SYSE (,,, Φ r A r A ( ( ( λ Φ Z Q Y Z λ Φ Z Q Y Z Q Y Z z z δ δ δ δ δ o apply recursve formulato, closed loop system must be ope by cut ot. Cut ot model s represeted as costrat equatos Usg Lagrage multpler theorem, followg equatos are obtaed he recursve formula ca be appled to each brach.

40 RECURSIVE FORULAION FOR A CLOSED LOOP SYSE Covertg Cartesa costrat Jacoba matrces to those Jot space Φ ( r, A, r, A δφ Φ δr + Φ δπ + Φ δr + Φ δπ r π r π Φ δz + Φ δz z z δz δz + δq δz + δq + δq : 2 δz + k δq k δz δz δq + k k k k δφ Φ Τδz z δz Τδz + Φ Τ δz z Φ δz + Φ δz z z 4

41 RECURSIVE FORULAION FOR A CLOSED LOOP SYSE z δφ Φ δz + δq + Φ δz + δq k k z k k k k ( Φ + Φ δz + Φ δq + Φ δq z z z k k z k k k k δq : : [ ] δq Φz Φ z Φz Φ z δq : : δq Φδq q Φ [ Φ Φ Φ Φ ] q z z z z [ Φ Φ ] q l q r δq δq : : δq δq : : δq 4

42 42 RECURSIVE FORULAION FOR A CLOSED LOOP SYSE RECURSIVE FORULAION FOR A CLOSED LOOP SYSE RECURSIVE FORULAION FOR A CLOSED LOOP SYSE RECURSIVE FORULAION FOR A CLOSED LOOP SYSE r l r l r rr r l ll l r l P P P λ q q Y Φ Φ q q ( ( ( λ Φ Z Q Y Z λ Φ Z Q Y Z Q Y Z z z δ δ δ δ δ for rght brach, for rght brach: (,( 2,, for left brach, 2,, + + Cartesa Vrtual work form of EQ for a closed loop system Apply Recursve II formulato to Cartesa Vrtual work form EQ for rght brach (, for left brach:, 2,, for rght brach: (,( 2,, L Q L Q L D

43 43 RECURSIVE FORULAION FOR A CLOSED LOOP SYSE RECURSIVE FORULAION FOR A CLOSED LOOP SYSE RECURSIVE FORULAION FOR A CLOSED LOOP SYSE RECURSIVE FORULAION FOR A CLOSED LOOP SYSE + + ( ( D L D L Q P + + [ ] l 2 2 [ ] 2 2 r ll symm rr symm [ ] z z z l Φ Φ Φ Φ q 2 [ ] 2 z z z r Φ Φ Φ Φ q [ ] l q q q 2 q [ ] r q q q 2 q ( ( ( l D L D L D L P ( ( ( r D L D L D L P r l r l r rr r l ll l r l P P P λ q q Y Φ Φ q q Above EQ has more ukwos tha equatos, eed more equatos

44 RECURSIVE FORULAION FOR A CLOSED LOOP SYSE Augmeted DAE system s obtaed by addg costrat accelerato equatos as; Y yy yq Py Φ q yq qq q Pq Φq λ γ Φ Φ Y + Φ Y + Φ Y z z + Φ where z Φ Y + Φ Y [ ] γ yq l r Costrat accelerato equatos ot space z z z Y yy Φ q [ Φ Φ ] ql qr ll qq P P P q q [ ] l [ ] r q l q r rr Φ ql q l Y Y + ( q + D k k k k Y Y + ( q + D k k k k + Φ q qr r γ Φz D Φz D γ 44