CINEMATICA DIFFERENZIALE

Transcript

1 CINEMATICA DIFFERENZIALE relazioni tra velocità dei giunti e velocità dell organo terminale Jacobiano geometrico Singolarità cinematiche Analisi della ridondanza Inversione della cinematica differenziale Jacobiano analitico Algoritmi per l inversione cinematica STATICA relazioni tra forze all organo terminale e coppie ai giunti

2 JACOBIANO GEOMETRICO T(q) = R(q) p(q) 0 T 1 Obiettivo ṗ = J P (q) q ω = J O (q) q v = [ ] ṗ ω = J(q) q

3 Derivata di una matrice di rotazione R(t)R T (t) = I Ṙ(t)R T (t) + R(t)ṘT (t) = O Posto S(t) = Ṙ(t)RT (t) S(t) + S T (t) = O Ṙ(t) = S(ω(t))R(t) S = 0 ω z ω y ω z 0 ω x ω y ω x 0

4 Esempio R z (α) = cosα sinα 0 sinα cosα S(t) = = αsinα α cosα 0 cosα sinα 0 α cosα α sinα 0 sinα cosα α 0 α 0 0 = S(ω(t)) 0 0 0

5 p 0 = o R 0 1p 1 ṗ 0 = ȯ R0 1ṗ1 + Ṙ0 1 p1 = ȯ R 0 1ṗ1 + S(ω 0 1)R 0 1p 1 = ȯ R 0 1ṗ1 + ω 0 1 r 0 1

6 Velocità di un braccio Velocità lineare p i = p i 1 + R i 1 r i 1 i 1,i ṗ i = ṗ i 1 + R i 1 ṙ i 1 i 1,i + ω i 1 R i 1 r i 1 i 1,i = ṗ i 1 + v i 1,i + ω i 1 r i 1,i

7 Velocità angolare R i = R i 1 R i 1 i S(ω i )R i = S(ω i 1 )R i + R i 1 S(ω i 1 i 1,i )Ri 1 i = S(ω i 1 )R i + S(R i 1 ω i 1 i 1,i )R i ω i = ω i 1 + R i 1 ω i 1 i 1,i = ω i 1 + ω i 1,i

8 ω i = ω i 1 + ω i 1,i ṗ i = ṗ i 1 + v i 1,i + ω i 1 r i 1,i Giunto prismatico ω i 1,i = 0 v i 1,i = d i z i 1 ω i = ω i 1 ṗ i = ṗ i 1 + d i z i 1 + ω i r i 1,i Giunto rotoidale ω i 1,i = ϑ i z i 1 v i 1,i = ω i 1,i r i 1,i ω i = ω i 1 + ϑ i z i 1 ṗ i = ṗ i 1 + ω i r i 1,i

9 Calcolo dello Jacobiano J = j P1... j Pn j O1 j On Velocità angolare giunto i prismatico q i j Oi = 0 = j Oi = 0 giunto i rotoidale q i j Oi = ϑ i z i 1 = j Oi = z i 1

10 Velocità lineare giunto i prismatico q i j Pi = d i z i 1 = j Pi = z i 1 giunto i rotoidale q i j Pi = ω i 1,i r i 1,n = ϑ i z i 1 (p p i 1 ) j Pi = z i 1 (p p i 1 )

11 Colonna dello Jacobiano geometrico [ jpi j Oi ] [ ] zi 1 0 = [ ] zi 1 (p p i 1 ) z i 1 per un giunto prismatico per un giunto rotoidale z i 1 = R 0 1(q 1 )...R i 2 i 1 (q i 1)z 0 p = A 0 1 (q 1)...A n 1 n (q n ) p 0 p i 1 = A 0 1(q 1 )...A i 2 i 1 (q i 1) p 0

12 Rappresentazione in terna differente [ ] ṗt ω t = = J t = [ ] [ ] R t O ṗ O R t ω [ ] R t O O R t J q [ R t O O R t ] J

13 Manipolatore planare a tre bracci

14 J(q) = [ ] z0 (p p 0 ) z 1 (p p 1 ) z 2 (p p 2 ) z 0 z 1 z 2 p 0 = p 1 = a 1c 1 a 1 s 1 0 p 2 = a 1c 1 + a 2 c 12 a 1 s 1 + a 2 s 12 0 p = a 1c 1 + a 2 c 12 + a 3 c 123 a 1 s 1 + a 2 s 12 + a 3 s z 0 = z 1 = z 2 = 0 0 1

15 J = a 1 s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c J P = [ ] a1 s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c 123

16 Manipolatore antropomorfo

17 J = [ ] z0 (p p 0 ) z 1 (p p 1 ) z 2 (p p 2 ) z 0 z 1 z 2 p 0 = p 1 = 0 0 p 2 = 0 z 0 = p = c 1(a 2 c 2 + a 3 c 23 ) s 1 (a 2 c 2 + a 3 c 23 ) a 2 s 2 + a 3 s 23 z 1 = z 2 = a 2c 1 c 2 a 2 s 1 c 2 a 2 s 2 s 1 c 1 0

18 J = s 1 (a 2 c 2 + a 3 c 23 ) c 1 (a 2 s 2 + a 3 s 23 ) a 3 c 1 s 23 c 1 (a 2 c 2 + a 3 c 23 ) s 1 (a 2 s 2 + a 3 s 23 ) a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23 0 s 1 s 1 0 c 1 c J P = s 1(a 2 c 2 + a 3 c 23 ) c 1 (a 2 s 2 + a 3 s 23 ) a 3 c 1 s 23 c 1 (a 2 c 2 + a 3 c 23 ) s 1 (a 2 s 2 + a 3 s 23 ) a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23

19 Manipolatore di Stanford

20 J = [ z0 (p p 0 ) z 1 (p p 1 ) z 2 z 0 z 1 0 z 3 (p p 3 ) z 4 (p p 4 ) z 5 (p p 5 ) z 3 z 4 z 5 ] p 0 = p 1 = p 3 = p 4 = p 5 = c 1s 2 d 3 s 1 d 2 s 1 s 2 d 3 + c 1 d 2 c 2 d 3 p = c 1s 2 d 3 s 1 d 2 + d 6 (c 1 c 2 c 4 s 5 + c 1 c 5 s 2 s 1 s 4 s 5 ) s 1 s 2 d 3 + c 1 d 2 + d 6 (c 1 s 4 s 5 + c 2 c 4 s 1 s 5 + c 5 s 1 s 2 ) c 2 d 3 + d 6 (c 2 c 5 c 4 s 2 s 5 ) z 0 = z 1 = s 1 c 1 0 z 2 = z 3 = c 1s 2 s 1 s 2 c 2 z 4 = c 1c 2 s 4 s 1 c 4 s 1 c 2 s 4 + c 1 c 4 s 2 s 4 z 5 = c 1c 2 c 4 s 5 s 1 s 4 s 5 + c 1 s 2 c 5 s 1 c 2 c 4 s 5 + c 1 s 4 s 5 + s 1 s 2 c 5 s 2 c 4 s 5 + c 2 c 5

21 SINGOLARITÀ CINEMATICHE v = J(q) q se J diminuisce di rango = singolarità cinematiche (a) perdita di mobilità (b) infinite soluzioni al problema cinematico inverso (c) velocità elevate nello spazio dei giunti (nell intorno di una singolarità) Classificazione Singolarità ai confini dello spazio di lavoro raggiungibile Singolarità all interno dello spazio di lavoro raggiungibile

22 Manipolatore planare a due bracci [ ] a1 s J = 1 a 2 s 12 a 2 s 12 a 1 c 1 + a 2 c 12 a 2 c 12 det(j) = a 1 a 2 s 2 ϑ 2 = 0 ϑ 2 = π [ (a 1 + a 2 )s 1 (a 1 + a 2 )c 1 ] T parallelo a[ a 2 s 1 a 2 c 1 ] T (componenti di velocità dell organo terminale non indipendenti)

23 Disaccoppiamento di singolarità calcolo delle singolarità della struttura portante calcolo delle singolarità del polso

24 J = [ ] J11 J 12 J 21 J 22 J 12 = [ z 3 (p p 3 ) z 4 (p p 4 ) z 5 (p p 5 ) ] J 22 = [ z 3 z 4 z 5 ] p = p W = p W p i paralleli a z i, i = 3, 4, 5 J 12 = [ ] det(j) = det(j 11 )det(j 22 ) det(j 11 ) = 0 det(j 22 ) = 0

25 Singolarità di polso z 3 parallelo a z 5 ϑ 5 = 0 ϑ 5 = π rotazioni uguali e opposte di ϑ 4 e ϑ 6 non producono alcuna rotazione dell organo terminale

26 Singolarità di struttura portante Manipolatore antropomorfo det(j P ) = a 2 a 3 s 3 (a 2 c 2 + a 3 c 23 ) s 3 = 0 a 2 c 2 + a 3 c 23 = 0 Singolarità di gomito ϑ 3 = 0 ϑ 3 = π

27 Singolarità di spalla p x = p y = 0

28 ANALISI DELLA RIDONDANZA Cinematica differenziale v = J(q) q se (J) = r dim ( R(J) ) = r dim ( N(J) ) = n r in generale dim ( R(J) ) + dim ( N(J) ) = n

29 Se N(J) ove q = q + P q a R(P) N(J) verifica: J q = J q + JP q a = J q = v q a genera moti interni della struttura

30 INVERSIONE DELLA CINEMATICA DIFFERENZIALE Equazione cinematica non lineare Equazione cinematica differenziale lineare nelle velocità Data v(t) + condizioni iniziali = (q(t), q(t)) se n = r q = J 1 (q)v q(t) = t 0 q(ς)dς + q(0) regola di integrazione numerica (Eulero) q(t k+1 ) = q(t k ) + q(t k ) t

31 Manipolatori ridondanti Per una data configurazione q, trovare le soluzioni q che soddisfino v = J q e che minimizzino g( q) = 1 2 qt W q metodo dei moltiplicatori di Lagrange g( q, λ) = 1 2 qt W q + λ T (v J q) ( ) T g = 0 q ( ) T g = 0 λ soluzione ottima q = W 1 J T (JW 1 J T ) 1 v se W = I ove q = J v J = J T (JJ T ) 1 è la pseudo-inversa destra di J

32 Utilizzo della ridondanza g ( q) = 1 2 ( qt q T a )( q q a ) come sopra... g ( q, λ) = 1 2 ( qt q T a )( q q a) + λ T (v J q) soluzione ottima q = J v + (I J J) q a

33 Caratterizzazione dei moti interni ( ) T w(q) q a = k a q misura di manipolabilità w(q) = det ( J(q)J T (q) ) distanza dai fine-corsa dei giunti w(q) = 1 2n n ( qi q i i=1 q im q im ) 2 distanza da un ostacolo w(q) = min p,o p(q) o

34 Singolarità cinematiche Le soluzioni precedenti valgono solo se J è di rango pieno Se J non è di rango pieno (singolarità) se v R(J) = soluzione q estraendo tutte le equazioni linearmente indipendenti (traiettoria fisicamente eseguibile) se v / R(J) = il sistema non è risolvibile (traiettoria non eseguibile) Inversione nell intorno di singolarità det(j) piccolo = q elevate inversa a minimi quadrati smorzata J = J T (JJ T + k 2 I) 1 ove q minimizza g ( q) = v J q 2 + k 2 q 2

35 JACOBIANO ANALITICO p = p(q) φ = φ(q) ṗ = p q q = J P(q) q φ = φ q q = J φ(q) q ẋ = [ ] ṗ φ = [ ] JP (q) J φ (q) q = J A (q) q J A (q) = k(q) q

36 Velocità di rotazione in terna corrente di angoli di Eulero ZYZ per effetto di ϕ: [ ω x ω y ω z ] T = ϕ [0 0 1] T per effetto di ϑ: [ ω x ω y ω z ] T = ϑ [ s ϕ c ϕ 0 ] T per effetto di ψ: [ω x ω y ω z ] T = ψ [ c ϕ s ϑ s ϕ s ϑ c ϑ ] T

37 Composizione di velocità di rotazione elementari ω = 0 s ϕ c ϕ s ϑ 0 c ϕ s ϕ s ϑ φ = T(φ) φ 1 0 c ϑ

38 Significato fisico di ω ω = [ π/2 0 0 ] T 0 t 1 ω = [ 0 π/2 0 ] T 0 t 1 ω = [ 0 π/2 0 ] T 1 < t 2 ω = [ π/2 0 0 ] T 1 < t ωdt = [ π/2 π/2 0] T

39 Relazione tra Jacobiano analitico e Jacobiano geometrico v = [ ] I O ẋ = T O T(φ) A (φ)ẋ J = T A (φ)j A Jacobiano geometrico grandezze di significato fisico Jacobiano analitico grandezze differenziali di variabili nello spazio operativo

40 ALGORITMI PER L INVERSIONE CINEMATICA Inversione cinematica q(t k+1 ) = q(t k ) + J 1 (q(t k ))v(t k ) t fenomeni di deriva della soluzione Soluzione algoritmica errore nello spazio operativo e = x d x ė = ẋ d ẋ = ẋ d J A (q) q trovare q = q(e): e 0

41 (Pseudo-)inversa dello Jacobiano Linearizzazione della dinamica di errore q = J 1 A (q)(ẋ d + Ke) ė + Ke = 0 Per un manipolatore ridondante q = J A (ẋ d + Ke) + (I J A J A) q a

42 Trasposta dello Jacobiano q = q(e) senza linearizzare la dinamica di errore Metodo di Lyapunov V (e) = 1 2 et Ke ove V (e) > 0 e 0 V (0) = 0 V (e) = e T Kẋ d e T Kẋ = e T Kẋ d e T KJ A (q) q la scelta q = J T A(q)Ke comporta che V (e) = e T Kẋ d e T KJ A (q)j T A(q)Ke se ẋ d = 0 = V < 0 con V > 0 (asintotica stabilità) se N(J T A ) = V = 0 se Ke N(J T A ) q = 0 con e 0 (stallo?)

43 Se ẋ d 0 e(t) limitato (conviene aumentare la norma di K) e( ) 0

44 Esempio J T P = c 1 (a 2 s 2 + a 3 s 23 ) s 1 (a 2 s 2 + a 3 s 23 ) 0 a 3 c 1 s 23 a 3 s 1 s 23 a 3 c 23 nullo di J T P ν y ν x = 1 tanϑ 1 ν z = 0

45 Errore di orientamento Errore di posizione e P = p d p(q) ė P = ṗ d ṗ Angoli di Eulero e O = φ d φ(q) ė O = φ d φ [ ] q = J 1 A (q) ṗd + K P e P φ d + K O e O agevole per assegnare l andamento temporale φ d (t) richiede comunque il passaggio attraversor = [ n s a ] Manipolatore con polso sferico calcolare q P = R W calcolare R T W R d = q O (angoli di Eulero ZYZ)

46 Asse/angolo R(ϑ, r) = R d R T errore di orientamento e O = r sinϑ = 1 2 (n n d + s s d + a a d ) ė O = L T ω d Lω ove L = 1 ( S(nd )S(n) + S(s d )S(s) + S(a d )S(a) ) 2 ė = [ ėp ė O ] = = [ ṗ d J P (q) q L T ω d LJ O (q) q ] [ ] [ ] ṗ d I O L T J q ω d O L [ ] q = J 1 ṗ (q) d + K P e P L ( ) 1 L T ω d + K O e O

47 Quaternione unitario errore di orientamento Q = Q d Q 1 e O = ǫ = η(q)ǫ d η d ǫ(q) S(ǫ d )ǫ(q) [ q = J 1 ṗd + K (q) P e P ω d + K O e O ] ω d ω + K O e O = 0 propagazione del quaternione η = 1 2 ǫt ω ǫ = 1 2 (ηi S(ǫ)) ω studio della stabilità V = (η d η) 2 + (ǫ d ǫ) T (ǫ d ǫ) V = e T O K Oe O

48 Algoritmi del secondo ordine derivata della cinematica differenziale ẍ e = J A (q) q + J A (q, q) q soluzione in accelerazione ai giunti q = J 1 A (ẍ (q) e J ) A (q, q) q ë + K D ė + K P e = 0

49

50 Confronto tra gli algoritmi per l inversione cinematica Manipolatore planare a tre bracci p x p y φ x = k(q) = a 1c 1 + a 2 c 12 + a 3 c 123 a 1 s 1 + a 2 s 12 + a 3 s 123 ϑ 1 + ϑ 2 + ϑ 3 a 1 = a 2 = a 3 = 0.5 m J A = a 1s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c

51 q i = [ π π/2 π/2] T rad p di = [ 0 0.5] T m φ = 0 rad traiettoria desiderata p d (t) = [ ] 0.25(1 cosπt) 0.25(2 + sinπt) 0 t 4 φ d (t) = sin π 24 t 0 t 4 Simulazione in MATLAB con integrazione numerica di Eulero q(t k+1 ) = q(t k ) + q(t k ) t e t = 1 ms

52 q = J 1 A (q)ẋ 2 x 10 3 norma errore pos 0 x 10 5 errore orien [m] [s] [rad] [s]

53 q = J 1 A (q)(ẋ d + Ke) K = diag{500, 500, 100} 5 pos giunti 10 vel giunti [rad] [rad/s] [s] [s] 1 x 10 5 norma errore pos 0 x 10 8 errore orien [m] [rad] [s] [s]

54 φ libero (r = 2, n = 3) q = J P (ṗ d + K P e P ) K P = diag{500, 500} 5 x 10 6 norma errore pos 0.5 orien 4 [m] 3 2 [rad] [s] [s] q = J T P (q)k Pe P K P = diag{500, 500} 0.01 norma errore pos 0.5 orien [m] [rad] [s] [s]

55 q = J P (ṗ d + K P e P ) + (I J P J P) q a KP = diag{500, 500} misura di manipolabilità w(ϑ 2, ϑ 3 ) = 1 2 (s2 2 + s 2 3) ( ) T w(q) q a = k a k a = 50 q 5 1 pos giunti 5 1 pos giunti [rad] [rad/s] [s] [s] 5 x 10 6 norma errore pos 1 manip 4 [m] 3 2 [rad] [s] [s]

56 distanza dai fine-corsa dei giunti 3 ( qi q i ) 2 w(q) = 1 6 i=1 q im q im 2π q 1 2π π/2 q 2 π/2 3π/2 q 3 π/2 ( ) T w(q) q a = k a k a = 250 q [rad] pos giunto [s] [rad] pos giunto [s] 5 pos giunto 3 2 x 10 4 norma errore pos 1.5 [rad] 0 [m] [s] [s]

57 STATICA Relazione tra forze e momenti (forze) γ all organo terminale e forze e/o coppie (coppie) τ ai giunti con il manipolatore in configurazione di equilibrio lavoro elementare compiuto dalle coppie dw τ = τ T dq lavoro elementare compiuto dalle forze dw γ = f T dp + µ T ωdt = f T J P (q)dq + µ T J O (q)dq = γ T J(q)dq spostamenti elementari spostamenti virtuali δw τ = τ T δq δw γ = γ T J(q)δq

58 Principio dei lavori virtuali il manipolatore è in equilibrio statico se e solo se δw τ = δw γ δq τ = J T (q)γ

59 Dualità cineto-statica N(J) R (J T ) R(J) N (J T ) forze γ N(J T ) interamente assorbite dalla struttura

60 Interpretazione fisica dello schema con la trasposta dello Jacobiano dinamica ideale τ = q forza elastica Ke che tira l organo terminale verso la postura desiderata nello spazio operativo ha effetto solo se Ke / N(J T )

61 Trasformazione di velocità e forze [ ṗ2 ω 2 ] = [ I S(r12 ) O I ][ ṗ1 ω 1 ] r 12 = R 1 r 1 12 ṗ 1 = R 1 ṗ 1 1 ṗ 2 = R 2 ṗ 2 2 = R 1 R 1 2ṗ2 2 ω 1 = R 1 ω 1 1 ω 2 = R 2 ω 2 2 = R 1 R 1 2ω 2 2

62 [ ṗ2 2 ω 2 2 ] = [ R 2 1 R1 2S(r1 12 ) ] [ ṗ1 1 O R1 2 ω 1 1 ] v 2 2 = J2 1 v1 1 in virtù della dualità cineto-statica: γ1 1 = J1 2T γ2 2 [ f 1 ] [ 1 R 1 ][ 2 O f 2 ] µ 1 = 2 1 S(r12 1 )R1 2 R2 1 µ 2 2

63 Catena chiusa Struttura ad albero equivalente a catena aperta [ ] qa q o = q u Risolvendo i vincoli: q u = q u (q a ) q o = Υ q a Υ = per la dualità cineto-statica I q u q a τ a = Υ T τ o Manipolatore a parallelogramma [ ] ϑ1 q a = q u = ϑ 1 Υ = [ ] τ1 τ τ a = 2 + τ 3 τ 1 + τ 2 τ 3 [ ϑ2 ϑ 3 τ u = ] [ ] 0 0

64 ELLISSOIDI DI MANIPOLABILITÀ Ellissoide di manipolabilità in velocità insieme delle velocità ai giunti a norma costante q T q = 1 manipolatore ridondante q = J (q)v v T( J(q)J T (q) ) 1 v = 1 Assi autovettori u i di JJ T = direzioni valori singolari σ i = λ i (JJ T ) = dimensioni Volume proporzionale a w(q) = det ( J(q)J T (q) )

65 Manipolatore planare a due bracci Misura di manipolabilità w = det(j) = a 1 a 2 s 2 max per ϑ 2 = ±π/2 max per a 1 = a 2 (a parità di estensione a 1 + a 2 )

66 Ellissi di manipolabilità in velocità [m] [m] Valori singolari [m] max min [m]

67 Ellissoide di manipolabilità in forza insieme delle coppie ai giunti a norma costante τ T τ = 1 γ T( J(q)J T (q) ) γ = 1 Dualità cineto-statica una direzione lungo la quale si ha elevata manipolabilità in velocità è una direzione lungo la quale si ha scarsa manipolabilità in forza, e viceversa

68 Manipolatore planare a due bracci Ellissi di manipolabilità in velocità [m] [m] Ellissi di manipolabilità in forza [m] [m]

69 Manipolatore trasformatore meccanico di velocità e forze dallo spazio dei giunti allo spazio operativo rapporto di trasformazione lungo una direzione per l ellissoide in forza α(q) = ( ) 1/2 u T J(q)J T (q)u rapporto di trasformazione lungo una direzione per l ellissoide in velocità β(q) = (u T( J(q)J T (q) ) 1 u ) 1/2 utilizzazione di gradi di mobilità ridondanti

70 Compatibilità della struttura ad eseguire un compito assegnato lungo una direzione compito di scrittura su superficie orizzontale vel piano di scrittura forza compito di lancio di un peso in direzione orizzontale forza vel direzione di lancio