Table of contents Introduction. Controllo dei Robot. Dinamica

Transcript

1 able of cotets Itroductio Cotrollo dei Robot Dipartimeto di Ig. Elettrica e dell Iformazioe (DEI) Politecico di Bari paolo.lio [at] poliba.it Cotrollo dei Robot

2 del maipolatore L = U Lagragiaa del sistema meccaico Eergia cietica totale del sistema U Eergia poteziale totale del sistema Equazioi di Lagrage d dt L λ i L λ i = ζ i i = 1,2,, ζ i è la forza geeralizzata associata alla coordiata geeralizzata λ i

3 del maipolatore d dt L λ i L λ i = ζ i Per u maipolatore a catea aperta la scelta più aturale per le coordiate geeralizzate è data dalle variabili di giuto q = λ 1, λ 2,, λ d dt L q L q = ζ Alle forze geeralizzate darao cotributo le forze o coservative che compioo lavoro su q i, i altre parole le coppie geerate ai giuti dagli attuatori, le coppie d attrito dei giuti, oché le coppie ai giuti idotte da forze esplicate dall orgao termiale sull ambiete i situazioe di cotatto. Nota: Il termie coppia è usato come sioimo della forza geeralizzata al giuto.

4 del maipolatore - Esempio ϑ k r = r = θ m r m θ = ω m ω rapporto di trasmissioe della coppia ciematica Le coppie ai giuti soo forite dai motori tramite opportui orgai di trasmissioe meccaica del moto. I alterativa, si possoo avere giuti azioati co motori calettati direttamete sull asse di rotazioe seza orgai di trasmissioe. Cm Im Fm ϑm F Braccio attuato mediate riduttore meccaico mg l I

5 del maipolatore - Esempio ϑ k r = r = θ m r m θ = ω m ω rapporto di trasmissioe della coppia ciematica Cm Im ϑm F l mg I Fm C m = I m ω m + F m ω m + fr m fr = I ω + Fω + mgl si θ Braccio attuato mediate riduttore meccaico C m = I eq ω m + F eq ω m + mgl si θ m I eq = I m + k r k r I k r 2 F eq = F m + F k r 2

6 del maipolatore - Esempio ϑ = 1 2 I θ I 2 mk r θ 2 U = mgl 1 cos θ Cm Im Fm ϑm F mg l I L = U = 1 2 I θ I 2 mk r θ 2 mgl 1 cos θ Braccio attuato mediate riduttore meccaico d dt L λ i L λ i = ζ i I + I m k r 2 θ + mgl si θ = ζ 2 ζ = τ F θ F m k r θ I + I m k r 2 θ + F + F m k r 2 θ + mgl si θ = τ

7 Determiazioe dell eergia cietica = i=1 eergia cietica del braccio i li + mi eergia cietica del motore che azioa il giuto i vettore velocità lieare li = 1 2 Vli p i p i ρdv desità della particella elemetare di volume dv

8 Determiazioe dell eergia cietica r i = r ix r iy r iz = p i p li particella elemetare p li = 1 m li V li p i ρdv baricetro

9 Determiazioe dell eergia cietica p i = p i 1 + v i 1,i + ω i 1 r i 1,i regola di composizioe delle velocità p i = p li + ω i r i = p li + S ω i r i Sostituedo i li = 1 2 Vli p i p i ρdv traslazioe 1 2 Vli p li pli ρdv = 1 2 m l i p li pli mutuo Vli p li S ωi r i ρdv = p l i S ωi V li p i p li ρdv = 0 rotazioe 1 2 Vli r i S ω i S ω i r i ρdv = 1 2 ω i V li S r i S r i ρdv ω i

10 Determiazioe dell eergia cietica Poiché S r i = 0 r iz r iy r iz 0 r ix r iy r ix Vli r i S ω i S ω i r i ρdv = 1 2 ω i I li ω i Il cotributo rotazioale si può esprimere come: r iy 2 + r iz 2 ρdv r ix r iy ρdv r ix r iz ρdv I li = r ix r iy ρdv r ix 2 + r iz 2 ρdv r iy r iz ρdv = I li xx I li xy I li xz I li xy I li yy I li yx I li xz I li yz I li zz r ix r iz ρdv r iy r iz ρdv r iy 2 + r ix 2 ρdv esore d ierzia relativo al baricetro del braccio i espresso i tera base

11 Determiazioe dell eergia cietica La posizioe del braccio i dipede dalla cofigurazioe del maipolatore I li fuzioe della cofigurazioe Se la velocità del braccio i viee espressa co riferimeto ad ua tera solidale al braccio i (secodo a covezioe di D H), si ottiee: ω i i = R i ω i matrice di rotazioe dalla tera solidale al braccio i alla tera base I li = R i I i li R i tesore espresso co riferimeto alla tera i (tesore costate) Se la tera solidale al braccio i coicide co la tera cetrale (pricipale) d ierzia, i prodotti d ierzia soo ulli e il tesore d ierzia relativo al baricetro (all origie della tera) è ua matrice diagoale

12 Determiazioe dell eergia cietica li = 1 2 m l i p li pli ω i R i I i li R i ω i i i p li = j pj l i qj = J P l i q ωi = j Oj l i qj = J O l i q l J i P = j l i l p 1 i jpi 0 0 l J i l O j i l O1 i joi 0 0 j pj l i j Oj l i = z j 1 0 z j 1 p li p j 1 z j 1 li = 1 2 m l i q l J i l i P JP q q J O l i Ri I i li R l i J i O q

13 Determiazioe dell eergia cietica Eergia cietica dell attuatore Coppie ai giuti soo forite dai motori tramite orgai di trasmissioe meccaica I alterativa, giuti azioati co motori calettati direttamete sull asse di rotazioe seza orgai di trasmissioe. Il motore del giuto i si ritiee posto sul braccio i 1 (i modo da alleggerire il carico diamico dei primi giuti della catea)

14 Determiazioe dell eergia cietica mi = 1 2 m m i p mi pmi ω i m Imi i ω mi massa del rotore velocità lieare del baricetro del rotore velocità agolare del rotore tesore d ierzia del rotore relativo al baricetro k r = r = θ m r m θ = ω m ω k ri q i = θ mi ω mi = ω i 1 + k ri q i z mi velocità agolare del rotore velocità agolare del braccio i 1 versore dell asse del rotore

15 Determiazioe dell eergia cietica mi = 1 2 m m i p mi pmi ω i m Imi i ω mi m p mi = J i P q m ω mi = J i O q m J i P = j m i m p 1 i jpi m J i m O j i m O1 i joi 0 0 z j 1 j pj m i j Oj m i = j Oj l i z j 1 p mi p j 1 k ri z mi j = 1,2,, i 1 j = i mi = 1 2 m m i q m J i m i P JP q q J O m i m Rmi I i mi R m mi J i O q

16 Determiazioe dell eergia cietica li = 1 2 m l i q l J i l i P JP q q J O l i Ri I i li R l i J i O q mi = 1 2 m m i q m J i m i P JP q q J O m i m Rmi I i mi R m mi J i O q = li + mi = 1 2 i=1 b ij q q i q j = 1 2 q B q q i=1 B q = i=1 l m li J i l i l P JP + i JO Ri I i li R l i J i m O + mmi J i m i m P JP + i m JO Rmi I i mi R m mi J i O

17 Determiazioe dell eergia cietica = 1 2 i=1 b ij q q i q j = q B q q B q = i=1 l m li J i l i l P JP + i JO Ri I i li R l i J i m O + mmi J i m i m P JP + i m JO Rmi I i mi R m mi J i O B q matrice d ierzia, di dimesioe Simmetrica Defiita positiva Dipedete dalla cofigurazioe

18 Determiazioe dell eergia poteziale U = i=1 eergia poteziale del braccio i U li + U mi eergia poteziale del motore che azioa il giuto i U li = V li g 0 p i ρdv = m li g 0 p li vettore accelerazioe gravitazioale riferito alla tera base (e.g. g 0 = g se l asse z è quello verticale) U mi = m mi g 0 p mi U = m li g 0 p li q + m mi g 0 p mi q i=1

19 Equazioi del moto L q, q = q, q U q d dt L q L q = ζ d dt L q i L q i = ζ i B q q + q, q = ζ q, q = B q q 1 2 q q B q q + U q q

20 Equazioi del moto L q, q = q, q U q, q = 1 2 i=1 b ij q q i q j + m li g 0 p li q + m mi g 0 p mi q i=1 L q i = 1 2 bjk q k=1 q i q k q j m lj g p l j q 0 + m q mj g p m j q 0 i q i L q i = 1 2 bjk q k=1 q i q k q j m lj g 0 j pi l j q + m mj g 0 j pi m j q g i q cotributo gravitazioale L q i = 1 2 bjk q k=1 q i q k q j + g i q

21 Equazioi del moto L q, q = q, q U q, q = 1 2 i=1 L = q i d dt L q i b ij q = q j b ij q q j + b ij q q i q j + dbij q dt i=1 q j m li g 0 p li q + m mi g 0 p mi q d dt L q i = b ij q q j + bij q q k k=1 q k q j

22 Equazioi del moto b ij q q j + bij q q k k=1 q k q j 1 2 k=1 bjk q q i q k q j + g i q = ζ i Posto h ijk = b ij 1 b jk q k 2 q i b ij q q j + h ijk q k q j + g i q = ζ i k=1

23 Equazioi del moto Iterpretazioe fisica b ij q q j + k=1 h ijk q k q j + g i q = ζ i ermii di accelerazioe b ii mometo d ierzia visto dall asse del giuto i, ella cofigurazioe correte, quado gli altri giuti soo bloccati b ij tiee coto dell effetto dell accelerazioe del giuti j sul giuto i. ermii quadrati i velocità h ijj q 2 j rappreseta l effetto cetrifugo idotto al giuto i dalla velocità del giuto j h iii = 0 poiché b ii = 0 q i h ijk q k q j rappreseta l effetto di Coriolis idotto al giuto i dalle velocità dei giuti j e k ermii dipedeti solo dalla cofigurazioe g i q rappreseta le coppie geerate all asse del giuto i ella cofigurazioe per effetto della gravità

24 Forze o coservative Forze o coservative che compioo lavoro sui giuti τ F v q + F s sg q J q h Coppie di attuazioe ai giuti Coppie di attrito viscoso Coppie di attrito statico Coppie di attuazioe a bilaciameto di forze di cotatto estere

25 Modello diamico ello spazio dei giuti b ij q q j + k=1 h ijk q k q j + g i q = ζ i B q q + C q, q q + F v q + F s sg q + g q = τ J q h C è ua matrice scelta (o uivoca) i modo tale da soddisfare : c ij q q j = k=1 h ijk q k q j

26 Proprietà otevoli delle equazioi della diamica Possibile scelta per la matrice C c ij q q j = k=1 = 1 2 h ijk q k q j = k=1 bij k=1 q q k q j + 1 k 2 b ij 1 b jk q k 2 q i k=1 q k q j b ik 1 b jk q j 2 q i q k q j c ij = k=1 c ijk q k c ijk = 1 2 b ij q k + b ik q j b jk q i simboli di Christoffel del primo tipo c ijk = c ikj

27 Proprietà otevoli delle equazioi della diamica Atisimmetria della matrice B 2C Posto: N q, q = B q 2C q, q La scelta effettuata geera ua matrice N q, q atisimmetrica I particolare, q N q, q q = 0 per qualuque scelta della matrice C Si può dimostrare che tale relazioe è ua diretta cosegueza del pricipio di coservazioe dell eergia (La derivata totale dell eergia cietica bilacia la poteza geerata da tutte le forze ageti ai giuti del maipolatore)

28 Esempio: Maipolatore plaare a due bracci a i l i lughezza del braccio i distaza del baricetro del braccio i dal giuto i y 0 m li m mi massa del braccio i massa del rotore del motore i l 2 θ 2 I li mometo di ierzia del braccio i relativo al baricetro itoro a z 0 l 1 θ 1 I mi mometo di ierzia del rotore i itoro all asse x 0 Si assume che i due motori siao sugli assi dei giuti, co baricetro i corrispodeza delle origii delle rispettive tere

29 Esempio: Maipolatore plaare a due bracci B q = i=1 l m li J i l i l P JP + i JO Ri I i li R l i J i m O + mmi J i m i m P JP + i m JO Rmi I i mi R m mi J i O p li = J P l i q = jp1 l i jp2 l i jpi l i 0 0 q j Pj l i = zj 1 p li p j 1 ω i = J O l i q = jo1 l i jo2 l i joi l i 0 0 q j Oj l i = zj 1 J P l 1 = j P1 l 1 0 = z0 p l1 p J O l 1 = j O1 l 1 0 = z J P l 2 = j P1 l 2 jp2 l 2 = z0 p l2 p 0 z 1 p l2 p 1 J O l 2 = j O1 l 2 jo2 l 2 = z 0 z 1

30 Esempio: Maipolatore plaare a due bracci p 0 = p 1 = a 1 c 1 a 1 s 1 0 p l1 = l 1 c 1 l 1 s 1 0 p l2 = a 1 c 1 + l 2 c 12 a 1 s 1 + l 2 s 12 0 z 0 = z 1 = P P x, P y, P z Q Q x, Q y, Q z P Q = Q y P z Q z P y Q z P x Q x P z Q x P y Q y P x prodotto vettoriale tra due vettori z 0 p l1 p 0 = l 1 s 1 l 1 c 1 0 z 0 p l2 p 0 = a 1 s 1 l 2 s 12 a 1 c 1 + l 2 c 12 0 z 1 p l2 p 1 = l 2 s 12 l 2 c 12 0

31 Esempio: Maipolatore plaare a due bracci J P l 1 = z 0 p l1 p = l 1 s 1 0 l 1 c 1 0 J O l 1 = z = 1 0 J P l 2 = z 0 p l2 p 0 z 1 p l2 p 1 = a 1 s 1 l 2 s 12 l 2 s 12 a 1 c 1 + l 2 c 12 l 2 c 12 J O l 2 = z 0 z 1 = 1 1

32 Esempio: Maipolatore plaare a due bracci J P m 1 = = J O m 1 = k r1 z m1 0 = k r1 0 J P m 2 = z 0 p m2 p = a 1 s 1 0 a 1 c 1 0 J O m 2 = j O1 l 2 kr2 z m2 = 1 k r2

33 Esempio: Maipolatore plaare a due bracci B q = i=1 l m li J i l i l P JP + i JO Ri I i li R l i J i m O + mmi J i m i m P JP + i m JO Rmi I i mi R m mi J i O J P l 1 JP l 1 = l J P l 2 JP l 2 = a 1s 1 l 2 s 12 a 1 c 1 + l 2 c 12 0 l 2 s 12 l 2 c 12 0 = a l a 1 l 2 c 2 l 2 a 1 c 2 l 2 a 1 c 2 l 2 2 a 1 s 1 l 2 s 12 l 2 s 12 a 1 c 1 + l 2 c 12 l 2 c 12 = J P m 1 JP m 1 = J P m 2 JP m 2 = a 1s 1 a 1 c a 1 s 1 0 a 1 c = a 1 2 s a c 1 0 = a 1

34 Esempio: Maipolatore plaare a due bracci B q = i=1 l m li J i l i l P JP + i JO Ri I i li R l i J i m O + mmi J i m i m P JP + i m JO Rmi I i mi R m mi J i O I li = I li xx I li xy I li xz I li xy I li xx I li yz I li xz I li yz I li zz J O l 1 Il1 J O l 1 = 1 0 I l1 xx I l1 xy I l1 xz I l1 xy I l1 xx I l1 yz I l1 xz I l1 yz I l1 zz 1 0 = I l 1 zz 0 J O l 2 Il2 J O l 2 = 1 1 I l2 xx I l2 xy I l2 xz I l2 xy I l2 xx I l2 yz I l2 xz I l2 yz I l2 zz 1 1 = I l 2 zz I l2 zz I l2 zz I l2 zz

35 Esempio: Maipolatore plaare a due bracci B q = i=1 l m li J i l i l P JP + i JO Ri I i li R l i J i m O + mmi J i m i m P JP + i m JO Rmi I i mi R m mi J i O m I i mi = m I i mi xx m i 0 0 I mi yy R m1 = m I i mi zz R m2 = J O m 1 m Rm1 I 1 m1 R m m1 J 1 O = k r I m1zz k r1 0 = k r1 2 I m1zz 0 J O m 2 m Rm2 I 2 m2 R m m2 J 2 O = k r2 I m2zz = I k m2zz r2i m2zz 1 k k r2 I m2zz k 2 r2 I m2zz r2

36 Esempio: Maipolatore plaare a due bracci B q = b 11 q b 21 q b 12 q b 12 q = b 11 θ 2 b 12 θ 2 b 21 θ 2 b 12 θ 2 b 11 = I l1 + m l1 l k 2 r1 I m1 + I l2 + m l2 a l a 1 l 2 c 2 + I m2 + m m2 a 1 b 12 = b 21 = I l2 + m l2 l a 1 l 2 c 2 + k r2 I m2 b 22 = I l2 + m l2 l k 2 r2 I m2

37 Esempio: Maipolatore plaare a due bracci c ij = k=1 c ijk q k c ijk = 1 2 b ij q k + b ik q j b jk q i c ijk = c ikj b 11 = I l1 + m l1 l k 2 r1 I m1 + I l2 + m l2 a l a 1 l 2 c 2 + I m2 + m m2 a 1 b 12 = b 21 = I l2 + m l2 l a 1 l 2 c 2 + k r2 I m2 b 22 = I l2 + m l2 l k 2 r2 I m2 c 111 = 1 2 b 11 = 0 c q 112 = c 121 = 1 b 11 = m 1 2 q l2 a 1 l 2 s 2 = h 2 c 122 = b 12 1 b 22 = h c q 2 2 q 211 = b 21 1 b 11 = h 1 q 1 2 q 2 c 212 = c 221 = 1 2 b 22 = 0 c q 222 = 1 b 22 = q 2

38 Esempio: Maipolatore plaare a due bracci c ij = k=1 c ijk q k c 111 = 0 c 112 = c 121 = h c 122 = h c 211 = h c 212 = 0 c 222 = 0 C q, q = h θ 2 h θ 1 + h θ 1 0 θ 2

39 Esempio: Maipolatore plaare a due bracci g i q = m lj g 0 J pi l j q + m mj g 0 J pi m j q g 0 = 0 g 0 J P l 1 = l 1 s 1 0 l 1 c 1 0 J P l 2 = a 1 s 1 l 2 s 12 l 2 s 12 a 1 c 1 + l 2 c 12 l 2 c 12 J P m 1 = J P m 2 = a 1 s 1 0 a 1 c 1 0 g 1 = m l1 l 1 + m m2 a 1 + m l2 a 1 gc 1 + m l2 l 2 gc 12 g 2 = m l2 l 2 gc 12

40 Esempio: Maipolatore plaare a due bracci I l1 + m l1 l k 2 r1 I m1 + I l2 + m l2 a l a 1 l 2 c 2 + I m2 + m m2 a 1 θ I l2 + m l2 l a 1 l 2 c 2 + k r2 I m2 θ 2 2m l2 a 1 l 2 s 2 θ 1 θ 2 m l2 a 1 l 2 s 2 θ m l1 l 1 + m m2 a 1 + m l2 a 1 gc 1 + m l2 l 2 gc 12 = τ 1 I l2 + m l2 l a 1 l 2 c 2 + k r2 I m2 θ 1 + I l2 + m l2 l k 2 r2 I m2 θ 2 + +m l2 a 1 l 2 s 2 θ m l2 l 2 gc 12 = τ 2

41 Modello diamico ello spazio operativo Si voglioo descrivere le equazioi del moto direttamete ello spazio operativo, legado le forze geeralizzate ageti sul maipolatore e l isieme miimo di variabili che descrivoo posizioe e orietameto dell orgao termiale ello spazio operativo La caratterizzazioe co la lagragiaa ello spazio operativo o cosete di trattare co maipolatori ridodati, i quato le variabili o costituiscoo u set di coordiate geeralizzate No è ifatti possibile descrivere i questo caso i moti iteri della struttura provocati da u isieme di forze geeralizzate ai giuti il cui effetto sul moto dell orgao termiale sia ullo

42 Modello diamico ello spazio operativo B q q + C q, q q + F v q + F s sg q + g q = τ J q h rascurado le forze di attrito ai giuti: q = B 1 q C q, q q B 1 q g q + B 1 q τ J q h τ = J q h q = B 1 q C q, q q B 1 q g q + B 1 q J q γ h x = J A q q x = J A q q + J A q, q q x = J A q B 1 q C q, q q J A q B 1 q g q + +J A q B 1 q J q γ h + J A q, q q

43 Modello diamico ello spazio operativo x = J A q B 1 q C q, q q J A q B 1 q g q + J A q B 1 q J q γ h + J A q, q q Legame tra Jacobiao aalitico e geometrico J = A φ J A q J = J A q A φ x = J A q B 1 q C q, q q J A q B 1 q g q + +J A q B 1 q J A q A φ γ h + J A q, q q Poedo: A φ γ = γ A A φ h = h A x = J A q B 1 q C q, q + J A q, q q J A q B 1 q g q + q + J A q B 1 q J A q γ A h A

44 Modello diamico ello spazio operativo B A = J A B 1 J A 1 Posto: C A x = B A J A B 1 C q B A J A q g A = B A J A B 1 g B A x = B A J A B 1 C q B A J A B 1 g + B A J A q + B A J A B 1 J A γ A h A B A x = C A x g A + γ A h A B A x x + C A x, x x + g A x = γ A h A

45 Modello diamico ello spazio operativo Il modello è formalmete aalogo a quello ello spazio dei giuti Come ello studio della ciematica differeziale, el caso di sigolarità o è possibile effettuare l iversa dello jacobiao e quidi la trattazioe ecessita di particolari accorgimeti Il modello è valido ache per maipolatori ridodati, beché le variabili x o costituiscao u isieme di coordiate geeralizzate I questo caso la matrice B A caratterizza ua pseudo-eergia cietica

46 diretta e iversa ello spazio operativo diretta: determiare le accelerazioi all orgao termiale assegado le coppie ai giuti e le forze/coppie applicate all orgao termiale. Per u maipolatore ridodate il modello diamico ello s.o. o è direttamete utilizzabile i quato τ = J q γ ha soluzioi i γ solo se τ Im J I modelli di simulazioe, si lavora ello spazio dei giuti per poi otteere le variabili dello s.o. tramite la ciematica diretta

47 diretta e iversa ello spazio operativo Iversa: determiare le coppie ai giuti ecessarie alla geerazioe di u moto specifico assegato (i termii di posizioe, velocità, accelerazioe dell orgao termiale) Si può ivertire la ciematica e lavorare successivamete ello spazio dei giuti (calcolo delle coppie mediate modello diamico ello spazio dei giuti) I alterativa si può usare il modello ello s.o. per calcolare le γ A e poi calcolare le τ tramite trasposta dello Jacobiao. Co tali teciche la ridodaza o viee sfruttata, i quato le coppie calcolate o geerao moti iteri per la struttura

48 diretta e iversa ello spazio operativo E possibile risolvere la ridodaza a livello diamico Ricordado che: B A = J A B 1 J A 1 C A x = B A J A B 1 C q B A J A q g A = B A J A B 1 g x = J A q q + x J A q, q J A q, q q q = J A q q Il modello ello spazio operativo B A x = B A J A B 1 C q B A J A B 1 g + B A J A q + B A J A B 1 J A γ A h A può essere scritto come B A x J A q + B A J A B 1 C q + B A J A B 1 g = γ A h A B A J A q q + B A J A B 1 C q B A J A B 1 g = γ A h A

49 diretta e iversa ello spazio operativo B A J A q q + B A J A B 1 C q B A J A B 1 g = γ A h A Posto J A = B 1 J A B A J A = B A J A B 1 = B A J A B 1 J A B q + C q + g = γ A h A modello diamico ello spazio dei giuti J A τ J A h A = γ A h A J A τ = γ A La soluzioe i τ di questa equazioe è τ = J A q γ A + I J A q J A τ a

50 diretta e iversa ello spazio operativo τ = J A q γ A + I J A q J A τ a La soluzioe si ottiee teedo coto del fatto che J A è ua pseudo-iversa destra di J A pesata secodo la matrice B 1 Il vettore τ a o dà cotributo di forza all orgao termiale, ma geera moti iteri della struttura da impiegare per la gestioe della ridodaza a livello diamico