Modello dinamico dei robot:

Transcript

1 Corso di Robotica 2 Modello dinamico dei robot: analisi, estensioni, proprietà, identificazione, usi Prof. Alessandro De Luca A. De Luca

2 Analisi accoppiamenti inerziali robot cartesiano robot cartesiano diagonale robot PR " B = b 11 0 # 0 b 22 & " B = b 11 b 12 b 21 b # 22 & " b B(q) = 11 b 12 (q 2 ) # b 21 (q 2 ) b 22 & robot 3R articolato il modello dinamico risulta lineare se g 0 B costante c 0 " B(q) = # d = 0 in PR d 2 = 0 in 2R b 11 ( q 2,q 3 ) b 22 (q 3 ) b 23 (q 3 ) 0 b 23 (q 3 ) b 33 & baricentro braccio 2 su asse giunto 2 A. De Luca 2

3 Analisi termine di gravità bilanciamento statico distribuzione masse e inerzie (inclusi i motori) compensazione meccanica sistemi articolati di molle catene cinematiche chiuse assenza di gravità applicazioni spaziali U costante (moto su piano orizzontale) g(q) " 0 A. De Luca 3

4 Robot con maglie chiuse Comau Smart NJ130 MIT Direct Drive Mark II e Mark III A. De Luca 4

5 Robot con maglie chiuse MIT Direct Drive Mark IV (five-bar linkage planare) UMinnesota Direct Drive Arm (five-bar linkage spaziale) A. De Luca 5

6 2 3 l l c3 y c4 l c2 l c1 q 1 Robot a parallelogramma cinematica e dinamica (planare) 4 q 2 1 x 5 l 5 q 2 - π " p EE = l 1c 1 # l 1 s 1 & + " l 5 cos(q 2 ()) # l 5 sin(q 2 ()) & = " l 1c 1 # l 1 s 1 & ( " l 5 c 2 # l 5 s 2 & POSIZIONI BARICENTRI baricentri: l ci arbitrarie parallelogramma: l 1 = l 3 l 2 = l 4 " p c1 = l c1c 1 " # l c1 s p c2 = l c2c 2 " 1 & # l c2 s p c3 = l 2c 2 2 & # l 2 s 2 & + " l c3c 1 " # l c3 s p c4 = l 1c 1 1 & # l 1 s 1 & ( " l c4c 2 # l c4 s 2 & A. De Luca 6 E-E CINEMATICA DIRETTA

7 Calcolo energia cinetica VELOCITA LINEARI E ANGOLARI # v c1 = "l s c1 1 & l c1 c ( # # q 1 v c3 = "l s c3 1 & 1 l c3 c ( # q 1 + "l s 2 2 & v 1 l 2 s ( c2 = "l s c2 2 & l c2 c ( q # v c4 = "l s 1 1 & l 1 c ( # q 1 " "l s c4 2 & 1 l c4 c ( 2 q 2 " 1 = " 3 = q 1 " 2 = " 4 = q 2 q 2 T i T 1 = 1 2 m 2 1l c1 q I 2 1,zz q 1 T 2 = 1 2 m 2 2l c2 q I 2 2,zzq 2 ( q 2 ) T 3 = 1 2 I 3,zz q m l 2 3 2q l 2 q 2 + 2l l c 2"1 q 1 c3 1 2 c3 ( q 2 ) T 4 = 1 2 I 4,zzq m l 2 4 1q l 2 q 2 " 2l l c 2"1 q 1 c4 2 1 c4 A. De Luca 7

8 Matrice di inerzia T = 4 " i=1 T i = 1 2 q T B(q) q B(q) = I + m l # 1,zz 1 c1 + I 3,zz + m 3 l c3 + m 4 l 1 ( 2 m 3 l 2 l c3 "m 4 l 1 l c4 )c 2"1 I 2,zz + m 2 l c2 symm 2 + I 4,zz + m 4 l c4 + m 3 l 2 2 & ( condizione strutturale nel progetto meccanico m 3 l 2 l c3 = m 4 l 1 l c4 (*) B(q) diagonale e costante termini centrifughi e di Coriolis nulli modello dinamico (a meno di g(q)) meccanicamente DISACCOPPIATO e LINEARE vantaggio per il progetto del controllore del moto! A. De Luca 8

9 Energia potenziale e termini di gravità U i dalle componenti y dei vettori p ci U 1 = m 1 g 0 l c1 s 1 U 2 = m 2 g 0 l c2 s 2 U 3 = m 3 g 0 ( l 2 s 2 + l c3 s 1 ) U = m g l 4 4 0( s " l s 1 1 c4 2) # g(q) = "U & ( " q T nel caso (*) U = 4 " i=1 A. De Luca 9 U i # = g 0( m 1 l c1 + m 3 l c3 + m 4 l 1 )c 1 & # ( = g 1(q 1 )& ( g 0 ( m 2 l c2 + m 3 l 2 )m 4 l c4 )c 2 g 2 (q 2 ) b 11 q 1 + g 1 (q 1 ) = u 1 b 22 q 2 + g 2 (q 2 ) = u 2 componenti disaccoppiate u i coppia (non conservativa) che compie lavoro su q i ulteriori condizioni strutturali nel progetto meccanico portano a g(q) 0

10 Aggiunta di termini dinamici fenomeni dissipativi per attrito ai giunti viscoso, secco, di primo distacco, di Coulomb effetti locali ai giunti di difficile modellizzazione in generale, tranne per inclusione degli attuatori elettrici (visti come corpi rigidi) u V,i = "F V,i q i u S,i = "F S,i sgn( q i ) motore i montato sul braccio i-1 (o precedente), in genere in asse con il giunto i massa motore (bilanciata sull asse) inclusa in quella del braccio inerzia (del rotore) da aggiungere nell energia cinetica trasmissione con riduttori (spesso con rapporto elevato) a volte più motori cooperano per muovere più giunti: matrice di trasmissione T con accoppiamenti (elementi fuori diagonale) A. De Luca 10

11 . θ m1 Dislocazione dei motori. θ mn motore 1 motore N SR w (world frame) giunto 2 braccio 1 braccio 2 SR 0 SR 1 SR N - 1 braccio N SR N braccio 0 (base) giunto 1 motore 2 braccio N - 1 giunto N.. θ mi = n ri q i A. De Luca 11. θ m2 τ i = n ri τ mi

12 Modello dinamico finale semplificazione: nella parte rotazionale dell energia cinetica, si considera solo la velocità di spinning del rotore T mi = 1 2 I mi 2 = 1 2 I n 2 mi riq 2 i = 1 2 B mi " mi q i 2 N T m = " T mi = 1 q 2 T B m q con tutti i termini aggiuntivi, il modello diventa i=1 diagonale ( B(q) + B m ) q + c(q, q ) + g(q) + F V q + F S sgn( q ) = " costante non fornisce contributi in c F v > 0, F S > 0 diagonali coppie motrici (a valle dei riduttori) A. De Luca 12

13 Parametrizzazione lineare della dinamica è sempre possibile riscrivere il modello dinamico nella forma matrice di regressione a = vettore dei coefficienti dinamici B(q) q + c(q, q ) + g(q) = Y(q, q, q ) a = u ESEMPIO 2R ( ) " s 2 ( q 2 ) # q 1 c 2 2 q 1 + q 2 q q c 2 q 1 + s 2 q 1 N p p 1 # q 2 c 1 c 12 & q ( 2 0 c 12 A. De Luca 13 a 1 a 2 a 3 a 4 a 5 & ( ( # ( = u 1 & ( u ( 2 ( N.B.: con attrito viscoso e secco, quattro coefficienti in più

14 Coefficienti dinamici del robot 2R a 1 = I 1,zz + m 1 d I 2,zz + m 2 d m 2 l 1 a 2 = m 2 l 1 d 2 2 a 3 = I 3,zz + m 2 d 2 a 4 = g 0 m 1 d 1 + m 2 l 1 a 5 = g 0 m 2 d 2 ( ) 10 parametri dinamici per ogni braccio (corpo rigido): m [1], r c [3], I [6] si combinano per fornire i coefficienti dinamici del robot: tutti e soli quelli rilevanti nel modello dinamico del robot N.B. se g 0 noto e l 1 noto (cinematica!), allora a 2 = l 1 m 2 d 2 = l 1 " a 2 a 5 = g 0 m 2 d 2 = g 0 " a 2 # " a 2 = m 2 d 2 è importante determinare una parametrizzazione minima per sono sufficienti 4 coefficienti! l identificazione sperimentale dei coefficienti dinamici il controllo adattativo del moto in presenza di incertezze parametriche A. De Luca 14

15 Identificazione dinamica per usare il modello, occorre conoscere i valori numerici dei coefficienti dinamici del robot al più, solo i principali parametri dinamici sono forniti dal costruttore stime approssimate sono deducibili con strumenti CAD i coefficienti di attrito sono (lentamente) variabili nel tempo lubrificazione dei giunti/trasmissioni in presenza di un payload (carico solidale all E-E) variano i 10 parametri dell ultimo braccio questo si riflette in variazioni di tutti (o quasi) i coefficienti dinamici necessari esperimenti preliminari di identificazione robot in movimento (aspetti dinamici, non statici o geometrici!) identificabili solo i coefficienti dinamici (ma non tutti i parametri..) A. De Luca 15

16 Esperimenti di identificazione 1. scegliere una traiettoria di moto eccitante esplora lo spazio di lavoro e coinvolge tutta la dinamica del robot periodica, con molteplici componenti frequenziali 2. eseguire il moto in modo approssimato con un controllore sfruttando l informazione disponibile sul modello.. in genere, u = K P (q d -q)+k D (q d -q) (PD, nessuna informazione). 3. misurare q (encoder) e, se disponibile, q in n c istanti accelerazione (oltre a velocità) ricavabile fuori linea mediante differenziazione numerica (possibile l uso di filtri non causali) 4. con tali misure/stime e i comandi applicati, valutare il regressore Y a sinistra e la parte destra nell espressione Y(q(t k ), q (t k ), q (t k )) a = u(t k ) k = 1,, n c A. De Luca 16

17 n c N Identificazione ai minimi quadrati costruire il sistema lineare di equazioni " Y(q(t 1 ), q (t 1 ), q (t 1 )) M Y(q(t nc ), q (t nc ), q # (t nc )) & a = " u(t 1 ) # u(t nc ) & traiettorie eccitanti e selezione del numero di campioni _ (n c N p) e loro posizione garantiscono rango(y) = p risolvere mediante pseudo-inversione della Y a = Y # u = ( Y T Y ) -1 Y T u _ Y a = u eventualmente si può pesare la pseudo-inversa, per tener conto di livelli differenti di rumore sulle misure A. De Luca 17

18 Dinamica inversa data una traiettoria desiderata di moto q d (t).. differenziabile due volte ( q d (t)) eventualmente ottenuta da una r d (t) cartesiana, mediante inversione cinematica (differenziale) la coppia motrice necessaria ad eseguirla è " d = ( B(q d ) + B m ) q d + c(q d, q d ) + g(q d ) + F V q d + F S sgn( q d ) questo calcolo algebrico (t) non è però efficiente con l approccio Lagrangiano termini simbolici molto lunghi in tempo reale, meglio il calcolo numerico con Newton-Eulero utile per il controllo (feedforward nominale) A. De Luca 18

19 modello dinamico Lagrangiano Equazioni di stato (Dinamica diretta) B(q) q + c(q, q ) + g(q) = u " definendo il vettore di variabili di stato x = x 1 x # 2 & = " q q # & (IR2N equazioni di stato N equazioni differenziali del 2 ordine x = " # x 1 x 2 & = " x 2 #(B (1 (x 1 ) c(x 1,x 2 ) + g(x 1 ) [ ] + " 0 & # B (1 (x 1 ) & u 2N equazioni differenziali del 1 ordine = f(x) + G(x)u 2N 1 2N N altra possibile scelta x = " q # B(q) q & momento generalizzato ~. x =... (esercizio) A. De Luca 19

20 Simulazione dinamica schema a blocchi Simulink c(q, q ) q, q qui, generico robot a 2 dof q 1 (0) q 1(0) comando d ingresso (ad anello aperto o in feedback) u + B -1 (q) q 1 q 1 " " q 2 q 2 " " q 1 q 2 g(q) q q q 2 (0) q 2 (0) inclusa inv(b) inizializzazione (coefficienti dinamici e stato iniziale) chiamate a funzioni Matlab per valutazione numerica termini modello scelta metodo di integrazione (e suoi parametri) A. De Luca 20

21 una realizzazione scorretta u 1 u 2 c 1 + g 1 q 1 (0) q 1 (0) _ 1 q 1 q " 1 " _ b 11 b 12 b 12 1 b 22 loop algebrico q 2 (0) q 2 (0) q 2 q 2 " " q 1 q 2 NO! viola il principio di causalità c 2 + g 2 occorre invertire l intera matrice di inerzia (non solo i suoi termini in diagonale) A. De Luca 21

22 Linearizzazione approssimata si può ricavare un modello dinamico lineare del robot, valido localmente intorno alla condizione operativa utile per l analisi e il progetto di controllori locali/lineari approssimazione mediante sviluppo di Taylor al primo ordine linearizzazione intorno ad uno stato di equilibrio (constante) o intorno ad una traiettoria di equilibrio (nominale, tempo variante) si può procedere lavorando sulle equazioni di stato, ma conviene direttamente usare il modello del secondo ordine il risultato è lo stesso, ma la derivazione è più facile... stato di equilibrio (q,q) = (q e,0) [ q e =0 ].. traiettoria di equilibrio (q,q) = (q d (t),q d (t)) g(q e ) = u e B(q d ) q d + c(q d, q d ) + g(q d ) = u d A. De Luca 22

23 Linearizzazione in un punto variazioni intorno ad uno stato di equilibrio q = q e + "q q = q e + " q = " q tenendo conto della dipendenza quadratica di c dalle velocità (termini ininfluenti intorno a velocità nulla) B( q e )" q + g(q e ) + # g "q + o "q, " q # q q=qe nello spazio di stato #"q& "x = " q ( " x = G(q e ) q = q e + " q = " q u = u e + "u ( ) = u e + "u infinitesimi di ordine superiore al primo # 0 I & -B -1 ( q e )G(q e ) 0 ( "x + # 0 B -1 q e ( ) & ("u = A "x + B "u A. De Luca 23

24 Linearizzazione lungo traiettoria variazioni intorno ad una traiettoria di equilibrio q = q d + "q q = q d + " q q = q d + " q u = u d + "u sviluppando i vari termini nel modello dinamico B( q d + "q)" q + c(q d + "q, q d + " q ) + g(q d + "q) = u d + "u B(q d + "q) # B(q d ) + N i=1 b i q e T i "q q=q d C 1 (q d, q d ) c(q d + "q, q d + " q ) # c(q d, q d ) + c "q + q q=q d q = q d g(q d + "q) # g(q d ) + G(q d )"q i-esima riga della matrice Identità A. De Luca 24 c q q=q d q = " q q d C 2 (q d, q d )

25 Linearizzazione lungo traiettoria (cont) semplificando con B( q d )" q + C 2 (q d, q d )" q + D(q d, q d, q d )"q = "u D(q d, q d, q d ) = G(q d ) + C 1 (q d, q d ) + nello spazio di stato N # i=1 " b i " q T q d e i q=q d " x = # 0 I & -B -1 ( q d )D(q d, q d, q d ) -B -1 ( q d )C 2 (q d, q d ) ( "x + # 0 B -1 q d = A t ( ) "x + B ( t) "u un sistema lineare, ma tempo variante!! ( ) & ( "u A. De Luca 25

26 q " IR N Trasformazione di coordinate B(q) q + c(q, q ) + g(q) = B(q) q + n(q, q ) = u q 1 se si vogliono utilizzare nuove coordinate generalizzate p p " IR N p = f(q) q = f "1 (p) p = "f(q) q "q = J(q) q q = J "1 (q) p p = J(q) q + J (q) q, u q = J T (q)u p [ ] q = J "1 (q) p " J (q)j "1 (q) p 1 B(q)J "1 (q) p " B(q)J "1 (q) J (q)j "1 (q) p + n(q, q ) = J T (q)u p J "T (q) # premoltiplicando tutta l equazione... A. De Luca 26

27 Trasformazione del modello ( ) = u p J " T (q)b(q)j "1 (q) p + J " T (q) n(q, q ) "B(q)J "1 (q) J (q)j "1 (q) p q " p queste sostituzioni q, q " p, p B p = J "T BJ "1 B p (p) p + c p (p, p ) + g p (p) = u p ( ) = J "T c " B p c p = J "T c " BJ "1 J J "1 p ( ) = S p p, p c p p, p per calcolo e analisi non sono necessarie simmetrica e definita positiva (fuori da singolarità) ( ) p g p = J "T g J J "1 p B p " 2S p antisimmetrica forze generalizzate non conservative che compiono lavoro sulle p dipendenza quadratica da ṗ se p = posa dell E-E, questo è il modello dinamico cartesiano del robot A. De Luca 27