Recursive Robot Dynamics

Transcription

1 Lecture Recursive Robot Dynamics Prof. S.K. Saha ME Dept., II Delhi

2 What is recursive? #:inertia force #i inertia force i: torque i #i #n: inertia force n: torque n #i: vel., accn. #n #n: vel., accn. : torque #0 # #: vel., accn. : rate, accn + #0: vel., accn. = 0

3 Computational Counts Computational Counts Why recursive? Efficient, i.e., less computations and CPU time Compute jt. torque Compute jt. accn. (Inverse) (Forward) Equal number of -, 2- and 3-DOF joints Proposed Balafoutis Featherstone Angeles 2.5 x , 2- and 3-DOF joints Proposed 0.5 Mohan and Saha 2000 Lilly and Orin Fetherstone otal number of joints otal number of joints Ref. : Shah, S.,V. Saha, S.K., Dutt, J.K, Dynamics of ree-type Robotic Systems, Springer, 203

4 Why recursive? (contd.) Numerically stable Simulation is realistic Recursive Non-recursive (Forward) (Forward) Ref. : Mohan, A., and Saha, S.K., A recursive, numerically stable, and efficient simulation algorithm for serial robots with flexible links, Multibody System Dyn., V. 2, N.,pp. 35.

5 Unrealistic: Constraint Violation θ 3 3 #3 #2 # 2 θ 2 θ #0

6 Review of Dynamic Formulations Euler-Lagrange (EL) Newton-Euler (NE) d L L i dt q i q i m v f i i i I ω ω I ω n i i i i i i M t WM t w i i i i i i Ii O ωi Ο ωi ni Mi, i, i i m W t,w O i Ο Ο v i f i Starting point for the Recursive Dynamics (using the DeNOC matrices)

7 DeNOC matrices Newton-Euler to Euler-Lagrange DeNOC t (ω and v) = matrices Link velocities Rate of Independent coordinate θ. Decoupled form of the velocity transformation matrix, Newton-Euler Equations of motion = Minimal order Equations of motion (Euler-Lagrange) Eliminate constraint forces and moments from the NE equations. Analytical expressions of vector and matrices, Decomposition of inertia Matrix, Recursive algorithms, Dynamics model simplifications, etc.

8 Example: A Moving Mass Force, f f v x f Mass, m f v f : Vertical component Reaction : Horizontal component Motion

9 Newton s 2 nd law: Velocity constraint: Euler-Lagrange: Equation of Motion NOC: f e f c ]x c [i [i] f e mc [ i] [ fi ( f f ) j] [ i] mxi f mx v External force, c j c Mass, m Reaction, f c Note that i [ i] ( f f )[ j] 0 v c

10 Uncoupled NE Equations Newton-Euler (NE) equations for the i th rigid body I ω ω I ω n i i i i i i m v i i f i NE equations in compact form M t WM t w i i i i i i where Ii O ωi ωi Ο ni Mi, i, i, i m t W w O i v i Ο Ο f i

11 Uncoupled NE Equations Separate the bodies n bodies M t WM t w w E C i i i i i i i #i n #n # i 6n NE equations E C Mt WMt w w #0 M diag [ M M M ] 2 W diag [ W W W ] 2 n n

12 Kinematic (Velocity) Constraints O Bij ( j i) r d c ij p i e i e i d i B ij : the 6n 6n twist-propagation matrix p i : the 6n-dimensional joint-rate propagation vector or twist generator

13 DeNOC Matrices NN l N d : the 6n n Decoupled Natural Orthogonal Complement

14 Coupled Equations of Motion Pre-multiplication by N N ( Mt WMt ) N ( w E w C ) Equations in compact form C N w 0 Iθ Cθ τ I : nn Generalized inertia matrix (GIM) n coupled EL equations - no partial differentiation C : nn Matrix of convective inertia (MCI) terms τ : n-dimensional vector of generalized forces due to driving torques/forces, and those resulting from the gravity, environment and dissipation.

15 Generalized Inertia Matrix (GIM) Generalized inertia matrix (GIM) where ~ I N d MN M N MN d l l Each element of the GIM i p M B p ij i i ij j Mass matrix of composite body M M B M B i i i, i i i, i 5

16 Vector of Convective Inertia (VCI) Vector of Convective Inertia where h Cθ N d ~w' ( w ~ ' N Mt' WMt d ) t' N N W) θ and ( l l Each element of h h p w i i i where w w B w, and w w and ' i i i, i i n n w M tw M t i i i i i i

17 Generalized Force (Joint orque) Generalized Force d τ N w E where ~ E w Nl w E Each element is obtained recursively pw E i i i, where w w B w, and w w E E E E E i i i, i i n n

18 ; Example: One-link arm ac 0] 2 2 [ e] [0 0 ] ;[ d] [ as ma [ I] c s 0 Q s c s s c 0 2 ma 2 [ I] [ ] Q I 2 Q s c c where e and I O p M M m e d O I( i ) p Mp e Ie m(e d) (ed) 3 ma 2 Ref: Introduction to Robotics by Saha Moment of inertia about O

19 h p ( MW WM) p e [ I(ee) (eie)] 0 E n N l w [ e ( ed) ] f where [ n] [0 0 ] ;[ f] 2 mgas [ mg 0 0] Equation of motion: 2 ma mgas 3 2

20 Recursive Inverse Dynamics γ M β W M α n n n n n n γ M β W M α B γ n n n n n n n, n n γ M β W M α B γ 2 2 p γ n n n p γ n n n #i n #n p γ #0 # i α p α p α α p α n n n n β p Ω p β p Ω p B β B α 2 2 β p Ω p n n n n n n B β B α n, n n n, n n

21 Lecture 2 Recursive Robot Dynamics Prof. S.K. Saha ME Dept., II Delhi

22 Summary of Previous Lecture Why recursive? Define DeNOC (Decoupled Natural Orthogonal Complement) matrices from velocity constraints Derived NE EL equations (Constrained minimum set) Analytical expressions for the GIM, and other matrices

23 One-link Arm Equation of motion (Dynamic Model) 2 ma mgas 3 2

24 RoboAnalyzer (Free: Spanish (Mexico) Chinese (PRC)

25 Verify the results using MALAB s symbolic tool (MuPAD) I( i) p Mp e Ie m(e d) (ed) ma 3 2 h p ( MW WM) p e [ I(ee) (eie)] 0 E n N l w [ e ( ed) ] f where [ n] [0 0 ] ;[ f] 2 [ mg 0 0] mgas Equation of motion: 2 ma mgas 3 2

26 ; Example: wo-link Manipulator I i i2 ( i2) h ; ; i i h Cθ h τ X 3 M p i22 p 2 M 2 B 22 p 2 :Scalar 2 e e 2 d 2 2 I : 6-dim. vector 2 2 M2 O m 2 O : 6 6 sym. matrix O B22 : 66 identity matrix O Y 3 22 [ e2] 2 [ I2] 2[ e2] 2 2[ d2] 2 [ d2] 2 i m

27 22 [ e2] 2 [ I2] 2[ e2] 2 2[ d2] 2 [ d2] 2 i m [ e2] 2 [0 0 ] ; [ d2] 2 [ a2c2 a2s2 0] 2 2 X 3 Q c2 s2 0 s c Y 3 2 s 2 s 2 c ma 2 [ I2] 2 Q2[ I2] 3 Q2 s 2c 2 c i m a m a m a

28 ; i 2 i2 p 2 M 2 B 2 p ( ) :Scalar B p 2 O : 66 matrix ( 2) r d e e d : 6-dim. vector c s 0 Q s c i2 e2 I2 e m2 d2 d r d2 [ ] [ ] [ ] [ ] ([ ] [ ] ) [ e2] 0 0 ; [ d2] Q[ d2] 2 [ a2c2 a2s2 0] 2 2 [ e] [0 0 ] ; [ d] [ r ] [ ac a2s 0] i2 m2a2 m2a a2c2 m2a2 m2a2 m2a a2c r

29 p MB p :Scalar i O B : 66 identity matrix O X 3 I M O m O ~ ~ M M B 2M 2B : 66 sym. matrix ~ I δ 2 ~ ( c ) 2 2 δ m I I I m ( r d ) δ ~ δ m c 2 2 m~ m m 2 c Y 3 [ e] [ I] [ e ] m [ d] [ d] i ( m a m a ) m a m a a c

30 DH and Inertia parameters h Vector of Convective Inertia p w h p w m2a a2sθ2θ2 ( θ2 θ ) m2a a2sθ2θ Inverse Dynamics Results Link Joint a i (m) b i (m) i (rad) i (rad) r JV [0] 2 r JV [0] Link m i r i,x r i,y r i,z I i,xx I i,xy I i,xz I i,yy I i,yz I i,zz (kg) (m) (kg-m 2 )

31 Joint orques No gravity (horizontal) With gravity (vertical

32 Forward Dynamics & Simulation Equation of motion for one-link arm Forward Dynamics: 2 ma mgas 3 2 Integration (numerical): 2 Simulation 3 ( ) 2 ma 2 mgas y ; y st order form: y y2 y f ( y, t) 3 y2 ( mga sin y 2 ) ma 2 Integrate (say, numerically) to obtain y(t) using, e.g., Runge-Kutta method (ode45 of MALAB)

33 Simulation using RA

34 Recursive Forward Dynamics Equation of the motion Iθ Cθ τ UDU θ, where I UDU and τ Cθ Analytically he joint accelerations are then solved as θ U D U Hence forward dynamics requires three steps Step : Computation of φ Step 2: UDU Decomposition Step 3: Recursive computation of θ 34

35 Observations Derivations appear to be complex for the simpler manipulators It was for demonstration only he computations will be done algorithmically Due to recursive natures, calculations are fast he algorithm should be used for complex robotic systems like 6- or more-dof robots

36 Lecture 3 Recursive Robot Dynamics Prof. S.K. Saha ME Dept., II Delhi

37 Summary of Previous Lecture Use of RoboAnalyzer (RA) for inverse dynamics of one-link arm GIM for 2-link manipulator Inverse dynamics of KUKA robot Forward dynamics and simulation Use of RA for simulation Some observations for using recursive dynamics

38 Stewart Platform Parallel Manipulators

39 Four-bar Mechanism Separate all links Draw Free-body diagrams (FBD) 3 3 #2 a f 32 f 2 4 #3 4 a 3 a 0 Base, #0 a # #3 f 23 #2 f 2 # r C d Equations of motion: 3 per link I = d f d f r f r f x 0y y 0x x 2y y 2x m c f f ; m c f f x 0x 2x y 0y 2y f f 2 x 2 y f 03 Base, #0 f 0 9 equations 3 4 (3 rd law) = 9 unknowns

40 dy dx r y r x I 0' s f 0x mc x f0y mc x d2 y d2x r f 2 y r2 x 2x I2 2 f 2x mc 2 2x f23x mc 2 2y d f 3 y d3x r3 y r 3x 23 y I 3 3 0' s f03x mc 3 3x f03 y mc 3 3y A : 99 matrix x : 9 b : 9 Ghosh and Mallik, heory of Machines and Mechanisms x=a b LUx=b; Ly=b (Forward); Ux=y y (Backward) Disadv.: Need to calculate even the reactions for inv. dyn.

41 hree-link Serial with f 03 as External Join first three links form 3 3 #2 a f 32 f 2 #3 a 3 a # f 03 a 0 Base, #0 #3 f 23 #2 f 2 # r C d Equations of motion f 0 N N ( Mt WMt) N N ( w w ) 33 E C d l d l E C I θ Cθ τ τ Jf03 : 3 eqs. f Base, #0 a a a a 0 J θ

42 Proof of C = J f 03 he DeNOC matrices for 3-link serial manipulator n 23 f 23 t 0' s p 0' s t2 B2 p2 2 t B B 0' s p N :8 8 N :8 3 3 l d C C C w B2( w2 B32w3 ) C B2 B 3 w B32B2B3B2B32 B3 C C C C N l w B32 w2 w2 B32w3 0' s C C w3 w2 C w 3 B ( r2 d3) 0' s n 03 f 03 C n n d f r f w3 f03 f23 C r 3 d 3

43 6 C C C n w w B w n n d f r f f f 32 2 n03 n23 d3 f23 r3 f03 ( r2 d3) ( f03 f23) + f03 f23 23 f w C 2 n n ( r d r ) f d f p2 f03 f C n n p f d f w f03 f r 3 3 f 03 3 d 3 r 2 p 2 C 2 p 2 2

44 Constraint orque C p 0' s w C C Nd w p2 w2 C 0' s p3 w3 p i ei ( i, 2,3) eidi C C p w e ( e d) Scalar f03 f0 = e ( n n p f d f ) ( e d ) ( f f ) 03 0 = e ( ρ f ) ( e ρ ) f n n p f d f 3 a 3 3 r 2 C 2 2 d 2 2 p w e ( ρ f ) ( e ρ ) f C C p w e ( a f ) ( e a ) f C C f 03 p

45 Equations of Motion * : known E Iθ Cθ τ τ τ Jf03 [,0,0] C J e ρ e ρ e ρ a * as a2s2 a3s23 ac a2c2 a3c23 * 2 0 a2s2 a3s23 a2c2 a3c23 f03x * 0 a 3 3s23 a3c23 f03y A:3 3 b:3 x:3 Adv.: Reduced size of 33 (instead of 99) for inverse dynamics

46 4 3 #3 4 Subsystem Recursive Method a 3 3 #2 a 2 a 0 Base, #0 2 Subsystem equations # I I I I I C I I θ C θ τ ( τ ) : eq., 3 (, x, y )? [R: done] a II II II II II C II I θ C θ τ ( τ ) : 2 eqs., 2 ( x, y )? [2R: done] 2 Cut-open in a suitable location Make serial systems Apply serial-chain methods # #2 Base, #0 f 2 () a a0 a2 a3 f 2 (-) #

47 a a a 2 3 J J θ I I II II I as II a s a s a s J ; 2 ac J 22 a3c23 a2c2 a2c 2 I I I II II II en ln d en ln d A N N A N N System + Motion Subsystem II: I θ C θ τ ( τ ) Subsystem I: I II II II II II C II [0,0] II ( J ) C ( ) I I I I I C I I λ II 23 ; θ II Solve II 2 3 ( J ) λ System + Motion Driving torque, Adv.: Subsystem recursion.; Maximum size: 22 (not 99 or 33); Can use existing serial-chain dyn. algo.

48 Joint angles (deg) Driving torque (Nm) 400 Free: Sub-system (link) Mass (Kg) Length (m) I(#) II(#2) II(#3) ime (s) ime (s)

49 Lecture 4 International 203 Recursive Robot Dynamics Int.: 2009 Indian: 203 Prof. S.K. Saha ME Dept., II Delhi

50 Summary of Previous Lecture Dynamics (classical way) of 4-bar mechanism using FBD Cut-open 3+0 constrained dynamics of 4-bar Cut-open 2+ constrained dynamics of 4-bar Inverse dynamics using ReDySim

51 Forward Dynamics of 4-bar Mechanism Constrained equations of motion: I C ( ) I I I I I C I ( J ) I I I I I I O (J ) C II II II II II 0 I -(J ) θ -C θ I II I I II II J -J O λ -J J θ A:55 x:5 b:5 λ I θ C θ τ ( τ ) II II II II II C II [0,0] II ( J ) I I II 0 Velocity constraints: J -J II θ 0 DAE [Diff. Algeb. Eqn.) formulation: System appr.] x=a b + (jt. accn.) jt. vel. & pos. λ

52 System-level Lagrange Multiplier Method Constrained equations of motion: I I I I I I I I O -C ( J ) II II II II II II λ 0 I θ τ -C θ ( J ) I θ:3 :33 J :32 Velocity constraints: I I II I I II II J -J II -J J θ θ 2 J θ jt. vel. & pos. (jt. accn.) C ( ) I I I I I C I ( J ) I θ C θ τ ( τ ) II II II II II C II [0,0] II ( J ) I J θ J Oλ 2 λ JI J (JI ( ) 2) I:22 θ I ( J λ) Adv.: Inversions of smaller (33 and 22) matrices λ λ

53 Five-bar 2-DOF Manipulator Cut here - a a a # a # 2 a 0 2 a 0 Base, #0 Subsystem equations I θ C θ τ ( τ ) I I I I I I I θ C θ τ ( τ ) II II II II II II Base, #0 : 2 eqs., 3 (, x, y )? [2R done] : 2 eqs., ( 2 )? [2R done] Symmetric; Need to combine: 4 eqs., 4 (, 2, x, y )?

54 Subsystem Recursion for 5-bar a Cut 2 here a # a # - a a 0 2 a 0 Base, #0 Subsystem equations I θ C θ τ ( τ ) I I I I I I I θ C θ τ ( τ ) II II II II II II Base, #0 : 3 eqs., 3 (, x, y )? [3R done] : eq., ( 2 )? [R done] No need to combine: Solved

55 Inverse Dynamics (w/o Gravity) Link lengths: m; Mass of links: 6 kg f i 2 t i [ t sin( )] 2

56 Inverse Dynamics (w/gravity)

57 wo-dof Parallel Manipulator P Kinematic constraints Cut here l l2 a0 0 r 2 O 3 r 3 Link b i (m) θ i (m) a i (m) α i (m) l c 2 C 2 #2 C 3 #3 c 43 l /2 2 b 2 [JV] d O O 2 C # F 3 F 2 θ a o #0 O 4 C 4 #4 d 4 x 2 O 5 Link m i r i,x, r i,y r i,z I i,xx, I i,yy (kg) (m) (kg-m 2 ), Sliding velocity (constant): 0. m/sec. wo RP serial manipulators; Combined: 4 eqs., 4 (f, f 2, x, y )?

58

59 hree-dof RRR Parallel Robot Subsystem I: 5 eqs., 5 ( D, 2x, 2y, 7x, 7y )? Subsystem II: eq.. ( D2 )? Subsystem III: eq.. ( D3 )?

60 Inverse Dynamics of 3-DOF RRR Robot

61 Driving orques (w/o gravity)

62 Driving orques (w/gravity)

63 Six-DOF Parallel Robot: Stewart Platform

64 Moving platform f λ Cut joint O 6 O 4 Dimensionless links O 3 # O 4 #2 O 2 O 5 f #5 #4 0 ea 4 #3 N ( w w ) = 0 f + e a f * fi e3 AI A * g λ I I I I 2 4 I Subsystem I (3-DOF) 3 eqs.; 4 ( x, y, x, f )? 2I C c 23 d 2 #2 O 2 # d 3 C 2 r 3 #3 C 3 O 3 d 23 f I O One particular limb O #0

65 FBD of VII Subsystem -f VI λ -f λ -f λ V IV p p5 p p p4 p6 p p3 p p p p2 -f I λ -f II λ Subsystem VII (6-DOF) 6 eqs.; Zero? -f III λ VII c c c I ω+ ω I ω = n VII VII VII m = VII c f c VII λ f I λ VI f c n = pp pp6 f = f f c λ λ I VI VII

66 ; Inverse Dynamics Algorithm ; 6 c n x VII i fi i spp s6p p6 6 c 6 f y s s VII i f VI i J:66 h:6 τ:6 τ J h Ref: Sadana, M., 2009, Dynamic Analysis of 6-DOF Motion Platform, M. ech Project Report, II Delhi

67 rajectory and actuating force Using GUI

68 More Robots using ReDySim Planar biped Waking leg (also used as carpet scrapping mechanism)

69 Legged Robots Spatial biped Spatial quadruped

70 Flexible Rope using ReDySim 0 Base M M i orsion spring Detail of the module M i M s

71 Conclusions Purpose of Recursive Robot Dynamics Efficiency Numerical stability DeNOC matrices for serial-chain NE to EL derivations Constrained equations of motion for serial-chain systems Schemes for Inverse and Forward Dynamics (Simulation) RoboAnalyzer software for robot dynamics Parallel robot application Four-bar mechanism from FBD Cut-open system and subsystem recursion ReDySim software Five-bar, 3-DOF RRR, Stewart platform dynamics Custom-made GUI for Stewart platform Simulation of walking robots and rope using ReDySim

72 Acknowledgements Mr. Majid Koul Mr. Rajivlochana C.G. Dr. Suril V. Shah Mr. Mukesh Sadana Dr. Himanshu Chaudhary Students of II Delhi and other institutes who used the materials mentioned in the lectures and given feedbacks.

73 hank you For comments/questions: