Modello dinamico dei robot: approccio Lagrangiano

Transcript

1 Corso di Robotica 2 Modello dinamico dei robot: approccio Lagrangiano Prof. Alessandro De Luca A. De Luca

2 Modello dinamico esprime il legame tra forze generalizzate ut) agenti sul robot movimento del robot configurazioni spaziali qt) assunte nel tempo) u giunti t) u cartesiane t) qt) sistema di equazioni differenziali del 2 ordine "q, q, q ) = u A. De Luca 2

3 dinamica diretta " u 1 ut) = M u N ingressi in t [0,T] soluzione sperimentale Dinamica diretta q0), q 0) stato iniziale in t = 0 " qt) = si applicano coppie con i motori e si misurano le variabili di giunto con gli encoder al passo di campionamento T c ) soluzione per simulazione si usa il modello dinamico e si integrano numericamente le equazioni differenziali con passo di simulazione T s T c ) A. De Luca 3 q 1 M q N "q, q, q ) = u

4 Dinamica inversa dinamica inversa q d t), q d t), q d t) u d t) movimento desiderato per t [0,T] ingressi che lo realizzano per t [0,T] soluzione sperimentale prove ripetute di dinamica diretta con apprendimento iterativo della coppia nominale in base all errore in [0,T] misurato durante l esecuzione della prova precedente di moto soluzione analitica "q, q, q ) = u si usa il modello dinamico e si calcola algebricamente il valore u d t) istante per istante A. De Luca 4

5 Approcci alla modellistica dinamica approccio energetico approccio di Newton-Eulero eq. di Eulero-Lagrange) bilanciamento forze/coppie) equazioni dinamiche in forma simbolica/chiusa equazioni dinamiche in forma numerica/ricorsiva adatte allo studio delle proprietà, all analisi degli schemi di controllo adatte all implementazione dinamica inversa in tempo reale) esistono numerosi formalismi e principi della meccanica in base ai quali derivare il modello dinamico di un robot: principio di d Alembert, di Hamilton, dei lavori virtuali, A. De Luca 5

6 q " IR N Approccio energetico ipotesi: N bracci in moto schematizzati come corpi rigidi + eventuale elasticità concentrata ai giunti) coordinate generalizzate variabili di giunto, ma non solo!) Lagrangiano Lq, q ) = Tq, q ) " Uq) energia cinetica energia potenziale principio di minima azione di Hamilton principio dei lavori virtuali equazioni di Eulero-Lagrange d dt "L "L = u i " q i "q i i = 1,..., N forze generalizzate non conservative che compiono lavoro su q i Robotica 2 esterne o dissipative) A.A A. De Luca 6

7 attrito viscoso θ b l n 1 d l Esempio: Pendolo attuato b m motore trasmissione con riduttore) τ m, θ m " m = n" " m = n" + " m0 " = n" m = 0 q = " T m = 1 2 I 2 m" m T = T m + T l oppure q = " m ) T l = 1 2 I l + md 2 ) " 2 m g F x momento di inerzia del motore momento di inerzia baricentrale del braccio energia cinetica T = 1 2 I l + md 2 + n 2 I m ) " 2 = 1 2 I " 2 A. De Luca 7

8 Esempio: Pendolo attuato cont) τ m, θ m U = U 0 " mgdcos energia potenziale - d cos θ nτ m, θ p x = lsin" F x p x = lcos" " = J x " L = T " U = 1 2 I 2 + mgdcos " U 0 "L " = I d dt "L " = I "L " = mgdsin u = n" m b l nb m m + J T x F x = n" m b l + n 2 b ) m + lcos F x le coppie applicate/dissipate lato motore sono moltiplicate per n, a valle del riduttore coppia equivalente al giunto dovuta alla forza F x applicata nel punto p x somma delle coppie non conservative A. De Luca 8

9 Esempio: Pendolo attuato cont) nτ m, θ τ m, θ m modello dinamico in q = θ I " + mgd sin" = n m b l + n 2 b ) m " + lcos " F x dividendo per n e sostituendo θ = θ m /n I " n 2 m + m n gd sin" m n = m b l n + b 2 m modello dinamico in q = θ m * " m + l ) n cos " m n +F x A. De Luca 9

10 Energia cinetica di un corpo rigido ω r c SR 0 v c SR c corpo B massa m = "x,y,z)dx dy dz = dm B densità di massa baricentro r c = 1 m r dm se tutti i vettori sono riferiti rispetto al baricentro, allora r c = 0 " r dm = 0 energia cinetica T = 1 2 " B v T x,y,z) vx,y,z)dm B " B B relazione fondamentale della cinematica di un corpo rigido v = v c + " r = v c + S")r matrice Robotica 2 anti-simmetrica A.A A. De Luca 10

11 Energia cinetica di un corpo rigido cont) T = 1 2 B [ v c + S")r] T [ v c + S")r]dm = 1 2 v T c v c dm + v T c S")r dm r T S T ")S")r dm B B B a T b=trace{ab T } = 1 = v T c S") r dm = 0 2 m v T c v c = 1 2 trace{s")r r T S T ")}dm energia cinetica traslazionale tutta la massa concentrata nel baricentro) + Teorema di König B energia cinetica rotazionale di tutto il corpo) matrice di inerzia A. De Luca 11 B = 1 2 trace{s") r r T dm* ST ")} B ) = 1 2 trace{s") J ST ")} = 1 2 " T I " somma elementi sulla diagonale matrice di Eulero

12 b c Esempi di matrici d inerzia corpi omogenei di massa m con assi di simmetria z y a x " I = parallelepipedo rettangolo di lati a altezza), b, c base) I xx I yy I zz ) 1 " m 12 b2 + c 2 = 1 m 12 a2 + c 2 ) cilindro cavo di altezza h, raggio esterno a, raggio interno b ) 1 12 m a2 + b 2 y h b 1 x " m 2 a2 + b 2 ) 1 I = m 3 12 a2 + b 2 ) + h a ) 2 z z I zz I zz = I yy I zz = Izz + mh 2) 2 teorema della traslazione dell asse generalizzazione: Teorema di Steiner I = I c + m r T r "E 33 rr T ) effetto sulla matrice di inerzia di una pura traslazione r della terna di calcolo inerzia matrice Robotica 2 baricentrale Identità A.A A. De Luca 12

13 Energia cinetica del robot T = N " i=1 T i T i = T i q j, q j,j " i) N corpi rigidi + base ferma) in una catena cinematica aperta ω i v ci Teorema di König T i = 1 2 m iv T ci v ci " T i I i " i SR ci i-esimo braccio corpo) del robot velocità lineare ASSOLUTA del baricentro velocità angolare ASSOLUTA dell intero corpo A. De Luca 13

14 Energia cinetica di un braccio T i = 1 2 m i v T civ ci " T i I i " i ω i, I i vanno espresse nello stesso sistema di riferimento, ma il prodotto ω it I i ω i è invariante rispetto al sistema di riferimento scelto nel riferimento SR ci baricentrale) solidale al braccio i costante! " y 2 + z 2 )dm " x y dm " x zdm i I i = " x 2 + z 2 )dm " y zdm symm x 2 + y 2 " )dm ) ) ) A. De Luca 14

15 . Dipendenza formale di T da q e q w i v ci braccio 1 braccio i-1 q N q 1 q 2 q i-1 q i q i+1 braccio i braccio N Jacobiani parziali) espressi di solito in SR 0 v ci = J Li q) q = " i = J Ai q) q = " 1 1 L 0 L 0 L 0 L 0 i L 0 L 0 L 0 L 0 L 0 L 0 i L 0 L 0 A. De Luca 15 q q 3 righe 3 righe

16 Espressione finale di T T = 1 2 = 1 2 N i=1 q T m i v T ci v ci + " T i I i " ) i N " i=1 m i J T Li q)j Li q) + J T Ai q)i i J Ai q) q Tq, q ) = 1 2 q T Bq) q costante se ω i espressa in SR ci altrimenti 0 I i q)= 0 R i q) i I i 0 R i T q) matrice d inerzia generalizzata del robot simmetrica definita positiva, q sempre invertibile A. De Luca 16

17 Energia potenziale del robot ipotesi: solo contributi GRAVITAZIONALI U = N " i=1 U i ) U i = U i q j,j " i N corpi rigidi + base a quota costante) in una catena cinematica aperta U i = " m i g T r 0,ci dipendenza da q vettore di accelerazione di gravità " r 0,ci 1 = 0 A 1 q 1 ) 1 A 2 q 2 posizione dell i-esimo baricentro )L i1 A i q i " ) r i,ci 1 tipicamente in SR 0 costante in SR i A. De Luca N.B. si lavora con le coordinate omogenee 17

18 energia cinetica energia potenziale Lagrangiano Riassumendo... T = 1 2 q T Bq) q = 1 2 " b ij q) q i 0 i,j U = Uq) q j Lq, q ) = Tq, q ) " Uq) forma quadratica definita positiva T = 0 q = 0 equazioni di Eulero-Lagrange d dt "L "L = u k k = 1,...,N " q k "q k forze generalizate forze o coppie) non conservative dissipative/attive) che compiono lavoro sulla coordinata q k A. De Luca 18

19 "L = " q k j b kj q j Elaborando... Lq, q ) = 1 " b ij q) q i q j Uq) 2 i,j d dt "L = " q k + j b kj q j i,j "b kj "q i q i q j dipendenze da q tralasciate) "L "q k = 1 2 "b ij q i q j "U i,j "q k "q k termini LINEARI nelle ACCELERAZIONI q.. termini QUADRATICI nelle VELOCITA q. termini NONLINEARI nelle CONFIGURAZIONI q A. De Luca 19

20 d dt "L "L = u " q k "q k k La k-esima equazione dinamica " j b kj b kj q) q j + 1 b ij " q i 2 q * q i q j + U = u i,j k ) q k k scambiando gli indici muti i,j L + 1 "b kj + "b ki "b * ij ) q 2 " q i " q j " q i q j +L i,j k c kij = c kji simboli di Christoffel del primo tipo A. De Luca 20

21 ed il significato dei vari termini " b kj q) q j + " c kij q) q i q j + U = u q k k j i,j k = 1,,N termini INERZIALI termini CENTRIFUGHI i=j) e di CORIOLIS i j) termini g k q) GRAVITAZIONALI b kk q) = inerzia al giunto k quando accelera il giunto k > 0) b kj q) = inerzia vista al giunto k quando accelera il giunto j c kii q) = coefficiente della forza centrifuga al giunto k quando si muove il giunto i c iii = 0, i) c kij q) = coefficiente della forza di Coriolis vista al giunto k quando si muovono sia il giunto i che il giunto j A. De Luca 21

22 Due espressioni vettoriali della dinamica 1. B q) q + c q, q ) + g q) = u k-esima colonna di Bq) c k q, q ) = q T C k q) q C k q) = 1 2 "b k " q + "b k " q T ) " B " q k k-esima componente di c simmetrica 2. B q) q + S q, q ) q + g q ) = u N.B.: sono nella forma "q, q, q ) = u cercata NON simmetrica ) = " c kij q s kj q, q ) A. De Luca 22 i q i fattorizzazione di c con S non unica!

23 PROVA b kj = la matrice Proprietà strutturale B " 2S è antisimmetrica se si utilizzano i simboli di Christoffel per definire S) "b kj q i, 2s "q kj = 2" c kji q i = 2 i i i " i 1 2 b kj q i + b ki q j b ij q k * ) q i b kj " 2s kj = n jk = b jk " 2s jk = * i * i b ij " b ki q k q j b ik q j " b ji q k ) q i = n kj ) q i = "n kj per la simmetria di B )x = 0, x x T B " 2S A. De Luca 23

24 Conservazione dell energia totale energia totale E = T + U = 1 2 q T Bq) q + Uq) evoluzione nel tempo usando il modello dinamico) E = q T Bq) q + 1 q 2 T B q) q + "U q " q ) = q T u Sq, q ) q gq) = q T u+ 1 2 q T B q) 2Sq, q )) q q T B q) q + q T gq) qui, qualunque fattorizzazione di c con S se u0, l energia totale si conserva non si dissipa né si accresce) E = 0 q T B " 2S) q = 0, q, q più debole della antisimmetria, perchè la velocità esterna è la stessa che E = q T u in generale, la variazione di energia totale è pari al lavoro delle forze non conservative appare nelle matrici interne A. De Luca 24

25 Modello dinamico robot PR y d p c2 q 2 x T = T 1 + T 2 T 1 = 1 2 m 2 1 q 1 U = costante piano orizzontale) T 2 = 1 2 m 2 v T c2v c " T 2I 2 " 2 q 1 p c2 = " q 1 + d cos q 2 d sinq 2 0 v c2 = q 1 " d sin q 2 d cos q 2 q 2 0 q 2 " 2 = 0 0 q 2 T 2 = 1 2 m 2 q d 2 q 2 2 " 2d sinq 2 q 1 q ) I 2,zz q 2 2 A. De Luca 25

26 Bq) = dove C 1 q) = 1 2 C 2 q) = 1 2 Modello dinamico robot PR cont) m 1 + m 2 "m 2 d sinq 2 "m 2 d sinq 2 I 2,zz + m 2 d 2 C k q) = 1 2 c q, q b 1 b c 2 k q, q "b k " q + "b T k " q ) " B " q k ) = c q, 1 ) ) = "m 2 d cosq "m 2 d cosq " c 1 q, q " q c 2 q, q ) q T C k q) q ) = "m 2 d cos q 2 0 "m 2 d cos q "m 2 d cos q 2 0 " 0 "m 2 d cos q 2 "m 2 d cos q 2 0 = 0 c 2 q, q ) = 0 A. De Luca 26 q 2 2

27 Modello dinamico robot PR cont) Bq) q + cq, q ) = u m 1 + m 2 "m 2 d sinq 2 "m 2 d sinq 2 I 2,zz + m 2 d 2 q 1 q 2 + "m2d cos q 2 0 q 2 2 = u 1 u 2 N.B. l elemento b NN qui N=2) è sempre costante! Q1: perchè non ci sono termini di Coriolis? Q2: se fornisco una forza u 1, il secondo giunto accelera? sempre? Q3: qual è un espressione della matrice S? è unica? Q4: qual è la configurazione a massima inerzia generalizzata)? A. De Luca 27