Spazio dei giunti e spazio operativo

Transcript

1 CINEMATICA relazioni tra posizioni dei giunti e posizione e orientamento dell organo terminale Matrice di rotazione Rappresentazioni dell orientamento Trasformazioni omogenee Cinematica diretta Spazio dei giunti e spazio operativo Calibrazione cinematica Problema cinematico inverso

2 POSA DI UN CORPO RIGIDO Posizione o = o x o y o z Orientamento x = x x x + x y y + x z z y = y xx + y yy + y zz z = z xx + z yy + z zz

3 MATRICE DI ROTAZIONE R = x y z = x T x y T x z T x x T y y T y z T y x T z y T z z T z R T R = I R T = R 1

4 Rotazioni elementari rotazione di α intorno a z R z (α) = cosα sinα 0 sinα cosα

5 rotazione di β intorno a y cosβ 0 sinβ R y (β) = sinβ 0 cosβ rotazione di γ intorno a x R x (γ) = cosγ sinγ 0 sinγ cosγ

6 Rappresentazione di un vettore p = p x p y p z p = p x p y p z p = x y z p = Rp p = R T p

7 Esempio p x = p x cosα p y sinα p y = p x sinα + p y cosα p z = p z

8 Rotazione di un vettore p = Rp p T p = p T R T Rp Esempio p x = p x cosα p y sinα p y = p x sinα + p y cosα p z = p z p = R z (α)p

9 Matrice di rotazione fornisce l orientamento di una terna di coordinate rispetto ad un altra: i vettori colonna sono i coseni direttori degli assi della terna ruotata rispetto alla terna di partenza rappresenta una trasformazione di coordinate che mette in relazione le coordinate di uno stesso punto in due terne differenti (di origine comune) è l operatore che consente di ruotare un vettore in una stessa terna di coordinate

10 COMPOSIZIONE DI MATRICI DI ROTAZIONE p 1 = R 1 2p 2 p 0 = R 0 1p 1 p 0 = R 0 2 p2 R j i = (Ri j ) 1 = (R i j )T Rotazione in terna corrente R 0 2 = R0 1 R1 2 Rotazione in terna fissa R 0 2 = R1 2 R0 1

11 Esempio

12 ANGOLI DI EULERO matrice di rotazione 9 parametri con 6 vincoli rappresentazione minima dell orientamento 3 parametri indipendenti

13 Angoli ZYZ R(φ) = R z (ϕ)r y (ϑ)r z (ψ) = c ϕc ϑ c ψ s ϕ s ψ c ϕ c ϑ s ψ s ϕ c ψ c ϕ s ϑ s ϕ c ϑ c ψ + c ϕ s ψ s ϕ c ϑ s ψ + c ϕ c ψ s ϕ s ϑ s ϑ c ψ s ϑ s ψ c ϑ

14 Problema inverso Assegnata R = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 i 3 angoli ZYZ sono (ϑ (0, π)) ϕ = Atan2(r 23, r 13 ) ( ) ϑ = Atan2 r r2 23, r 33 ψ = Atan2(r 32, r 31 ) ovvero (ϑ ( π, 0)) ϕ = Atan2( r 23, r 13 ) ( ) ϑ = Atan2 r r2 23, r 33 ψ = Atan2( r 32, r 31 )

15 Angoli di RPY R(φ) = R z (ϕ)r y (ϑ)r x (ψ) = c ϕc ϑ c ϕ s ϑ s ψ s ϕ c ψ c ϕ s ϑ c ψ + s ϕ s ψ s ϕ c ϑ s ϕ s ϑ s ψ + c ϕ c ψ s ϕ s ϑ c ψ c ϕ s ψ s ϑ c ϑ s ψ c ϑ c ψ

16 Problema inverso Assegnata R = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 i 3 angoli di RPY sono (ϑ ( π/2, π/2)) ϕ = Atan2(r 21, r 11 ) ( ) ϑ = Atan2 r 31, r r2 33 ψ = Atan2(r 32, r 33 ) ovvero (ϑ (π/2, 3π/2)) ϕ = Atan2( r 21, r 11 ) ( ) ϑ = Atan2 r 31, r r2 33 ψ = Atan2( r 32, r 33 )

17 ASSE/ANGOLO R(ϑ, r) = R z (α)r y (β)r z (ϑ)r y ( β)r z ( α) r y sinα = rx 2 + ry 2 sin β = rx 2 + ry 2 cosα = r x r 2 x + r 2 y cosβ = r z

18 R(ϑ, r) = r 2 x(1 c ϑ ) + c ϑ r x r y (1 c ϑ ) + r z s ϑ r x r y (1 c ϑ ) r z s ϑ r 2 y(1 c ϑ ) + c ϑ r x r z (1 c ϑ ) r y s ϑ r y r z (1 c ϑ ) + r x s ϑ r x r z (1 c ϑ ) + r y s ϑ r y r z (1 c ϑ ) r x s ϑ rz(1 2 c ϑ ) + c ϑ R(ϑ, r) = R( ϑ, r)

19 Problema inverso Assegnata R = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 l angolo e l asse di rotazione sono (sinϑ 0) ( ) ϑ = cos 1 r11 + r 22 + r r = 1 r 32 r 23 r 13 r 31 2 sinϑ r 21 r 12 con r 2 x + r 2 y + r 2 z = 1

20 QUATERNIONE UNITARIO rappresentazione a 4 parametri Q = {η, ǫ} η = cos ϑ 2 ǫ = sin ϑ 2 r η 2 + ǫ 2 x + ǫ2 y + ǫ2 z = 1 (ϑ, r) e ( ϑ, r) forniscono lo stesso quaternione 2(η 2 + ǫ 2 x ) 1 2(ǫ xǫ y ηǫ z ) 2(ǫ x ǫ z + ηǫ y ) R(η, ǫ) = 2(ǫ x ǫ y + ηǫ z ) 2(η 2 + ǫ 2 y) 1 2(ǫ y ǫ z ηǫ x ) 2(ǫ x ǫ z ηǫ y ) 2(ǫ y ǫ z + ηǫ x ) 2(η 2 + ǫ 2 z ) 1

21 Problema inverso Assegnata R = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 il quaternione è (η 0) η = 2 1 r11 + r 22 + r ǫ = 1 sgn(r 32 r 23 ) r 11 r 22 r sgn(r 13 r 31 ) r 22 r 33 r sgn(r 21 r 12 ) r 33 r 11 r quaternione estratto da R 1 = R T Q 1 = {η, ǫ} prodotto tra quaternioni Q 1 Q 2 = {η 1 η 2 ǫ T 1 ǫ 2, η 1 ǫ 2 + η 2 ǫ 1 + ǫ 1 ǫ 2 }

22 TRASFORMAZIONI OMOGENEE Trasformazione di coordinate (traslazione + rotazione) p 0 = o R 0 1p 1 Trasformazione inversa p 1 = R 1 0o R 1 0p 0

23 Rappresentazione omogenea p = p 1 Matrice di trasformazione omogenea A 0 1 = R0 1 o T 1 Trasformazione di coordinate p 0 = A 0 1 p1

24 Trasformazione inversa p 1 = A 1 0 p0 = ( A 0 1) 1 p 0 ove A 1 0 = R1 0 R0o T 1 A 1 A T Successione di trasformazioni p 0 = A 0 1A A n 1 n p n

25 CINEMATICA DIRETTA Manipolatore insieme di bracci connessi tramite giunti Catena cinematica (dalla base all organo terminale) aperta (sequenza unica) chiusa (sequenza forma un anello) Grado di mobilità tipicamente associato a una articolazione = variabile di giunto

26 Terna base e terna utensile Equazione cinematica diretta T b e (q) = nb e(q) s b e(q) a b e(q) p b e(q)

27 Manipolatore planare a due bracci T b e (q) = = nb e s b e a b e p b e s 12 c 12 a 1 c 1 + a 2 c 12 0 c 12 s 12 a 1 s 1 + a 2 s

28 Catena aperta T 0 n (q) = A0 1 (q 1)A 1 2 (q 2)...A n 1 n (q n ) T b e (q) = T b 0T 0 n(q)t n e

29 Convenzione di Denavit-Hartenberg si sceglie l asse z i giacente lungo l asse del giunto i + 1 si individua O i all intersezione dell asse z i con la normale comune agli assi z i 1 e z i, e con O i si indica l intersezione della normale comune con z i 1 si assume l asse x i diretto lungo la normale comune agli assi z i 1 e z i con verso positivo dal giunto i al giunto i + 1 si sceglie l asse y i in modo da completare una terna levogira

30 Definizione non univoca della terna: con riferimento alla terna 0, per la quale la sola direzione dell asse z 0 risulta specificata: si possono quindi scegliere arbitrariamente O 0 ed x 0 con riferimento alla terna n, per la quale il solo asse x n risulta soggetto a vincolo (deve essere normale all asse z n 1 ): infatti non vi è giunto n + 1, per cui non è definito z n e lo si può scegliere arbitrariamente quando due assi consecutivi sono paralleli, in quanto la normale comune tra di essi non è univocamente definita quando due assi consecutivi si intersecano, in quanto il verso di x i è arbitrario quando il giunto i è prismatico, nel qual caso la sola direzione dell asse z i 1 è determinata

31 Parametri di Denavit-Hartenberg a i distanza di O i da O i ; d i coordinata su z i 1 di O i ; α i angolo intorno all asse x i tra l asse z i 1 e l asse z i valutato positivo in senso antiorario; ϑ i angolo intorno all asse z i 1 tra l asse x i 1 e l asse x i valutato positivo in senso antiorario. a i e α i sono sempre costanti se il giunto è rotoidale la variabile è ϑ i se il giunto è prismatico la variabile è d i

32 Trasformazione di coordinate A i 1 i = A i i = c ϑi s ϑi 0 0 s ϑi c ϑi d i a i 0 c αi s αi 0 0 s αi c αi A i 1 i (q i ) = A i 1 i A i i = c ϑi s ϑi c αi s ϑi s αi a i c ϑi s ϑi c ϑi c αi c ϑi s αi a i s ϑi 0 s αi c αi d i

33 Procedura operativa 1. Individuare e numerare consecutivamente gli assi dei giunti; assegnare, rispettivamente, le direzioni agli assi z 0,..., z n 1 2. Fissare la terna base posizionandone l origine sull asse z 0 ; gli assi x 0 e y 0 sono scelti in maniera tale da ottenere una terna levogira Eseguire i passi da 3 a 5 per i = 1,...,n 1: 3. Individuare l origine O i all intersezione di z i con la normale comune agli assi z i 1 e z i. Se gli assi z i 1 e z i sono paralleli e il giunto i è rotoidale, posizionare O i in modo da annullare d i ; se il giunto i è prismatico, scegliere O i in corrispondenza di una posizione di riferimento per la corsa del giunto (ad esempio un fine-corsa) 4. Fissare l asse x i diretto lungo la normale comune agli assi z i 1 e z i con verso positivo dal giunto i al giunto i Fissare l asse y i in modo da ottenere una terna levogira Per completare: 6. Fissare la terna n, allineando z n lungo la direzione di z n 1 se il giunto n è rotoidale, ovvero scegliendo z n in maniera arbitraria se il giuntonèprismatico; fissare l assex n in accordo al punto 4 7. Costruire per i = 1,...,n la tabella dei parametri a i, d i, α i, ϑ i 8. Calcolare sulla base dei parametri di cui al punto 7 le matrici di trasformazione omogenea A i 1 i (q i ) per i = 1,...,n 9. Calcolare Tn(q) 0 = A An 1 n che fornisce posizione e orientamento della terna n rispetto alla terna Assegnate T b 0 e T n e, calcolare la funzione cinematica diretta T b e (q) = T b 0T 0 nt n e che fornisce posizione e orientamento della terna utensile rispetto alla terna base

34 Catena chiusa Connessione di un singolo braccio con due bracci giunto virtuale di taglio A i j(q ) = A i i+1 (q i+1 )...Aj 1j (q j ) A i k(q ) = A i i+1 (q i+1 )...Ak 1 k (q k )

35 Vincoli giunto j + 1 rotoidale { R j i (q ) ( p i j (q ) p i k (q ) ) = [0 z i j (q ) = z i k (q ) 0 d jk ] T giunto j + 1 prismatico [ x it j (q ) yj it(q ) zj i (q ) = zk i(q ) ] (p i j (q ) p i k (q ) ) = x it j (q )x i k (q ) = cosϑ jk risolti in termini di q... [ ] 0 0 T 0 n(q) = A 0 ia i ja j n

36 Procedura operativa 1. Selezionare un giunto non attuato della catena chiusa. Ipotizzare di aprire tale giunto in modo da ottenere una catena aperta con struttura ad albero 2. Calcolare le trasformazioni omogenee secondo la convenzione di Denavit-Hartenberg 3. Trovare i vincoli di uguaglianza per le due terne connesse dal giunto di taglio 4. Risolvere i vincoli in termini di un numero ridotto di variabili di giunto 5. Esprimere le trasformazioni omogenee in funzione di tali variabili di giunto e calcolare la funzione cinematica diretta per composizione della varie trasformazioni dalla terna base alla terna utensile

37 Manipolatore planare a tre bracci

38 Braccio a i α i d i ϑ i 1 a ϑ 1 2 a ϑ 2 3 a ϑ 3

39 A i 1 i = c i s i 0 a i c i s i c i 0 a i s i i = 1, 2, T 0 3 = A 0 1A 1 2A 2 3 = c 123 s a 1 c 1 + a 2 c 12 + a 3 c 123 s 123 c a 1 s 1 + a 2 s 12 + a 3 s

40 Manipolatore a parallelogramma

41 Braccio a i α i d i ϑ i 1 a ϑ 1 2 a ϑ 2 3 a ϑ 3 1 a ϑ 1 4 a

42 A 0 3 (q ) = A 0 1 A1 = 2 A2 3 c s a 1 c 1 + a 2 c a 3 c s c a 1 s 1 + a 2 s a 3 s A 0 1 (q ) = A 3 4 = c 1 s 1 0 a 1 c 1 s 1 c 1 0 a 1 s a

43 Risoluzione vincoli orientamento (OK) posizione R 3 0 (q ) ( p 0 3 (q ) p 0 1 (q ) ) = a 1 (c 1 + c ) + a 1 (c 1 2 c 1 ) = 0 a 1 (s 1 + s ) + a 1 (s 1 2 s 1 ) = 0 ϑ 2 = ϑ 1 ϑ 1 ϑ 3 = π ϑ 2 = π ϑ 1 + ϑ 1 Cinematica diretta T 0 4 (q) = A0 3 (q)a3 4 = c 1 s 1 0 a 1 c 1 a 4 c 1 s 1 c 1 0 a 1 s 1 a 4 s

44 Manipolatore sferico

45 Braccio a i α i d i ϑ i 1 0 π/2 0 ϑ π/2 d 2 ϑ d 3 0

46 A 0 1 = c 1 0 s 1 0 s 1 0 c 1 0 A = A 2 3 = d c 2 0 s 2 0 s 2 0 c d T 0 3 = A 0 1A 1 2A 2 3 = c 1 c 2 s 1 c 1 s 2 c 1 s 2 d 3 s 1 d 2 s 1 c 2 c 1 s 1 s 2 s 1 s 2 d 3 + c 1 d 2 s 2 0 c 2 c 2 d

47 Manipolatore antropomorfo

48 Braccio a i α i d i ϑ i 1 0 π/2 0 ϑ 1 2 a ϑ 2 3 a ϑ 3

49 A i 1 i = A 0 1 = c 1 0 s 1 0 s 1 0 c c i s i 0 a i c i s i c i 0 a i s i i = 2, 3 T 0 3 = A 0 1A 1 2A 2 3 = c 1 c 23 c 1 s 23 s 1 c 1 (a 2 c 2 + a 3 c 23 ) s 1 c 23 s 1 s 23 c 1 s 1 (a 2 c 2 + a 3 c 23 ) s 23 c 23 0 a 2 s 2 + a 3 s

50 Polso sferico

51 Braccio a i α i d i ϑ i 4 0 π/2 0 ϑ π/2 0 ϑ d 6 ϑ 6

52 A 3 4 = c 4 0 s 4 0 s 4 0 c 4 0 A = A 5 6 = c 6 s s 6 c d c 5 0 s 5 0 s 5 0 c T 3 6 = A 3 4A 4 5A 5 6 = c 4 c 5 c 6 s 4 s 6 c 4 c 5 s 6 s 4 c 6 c 4 s 5 c 4 s 5 d 6 s 4 c 5 c 6 + c 4 s 6 s 4 c 5 s 6 + c 4 c 6 s 4 s 5 s 4 s 5 d 6 s 5 c 6 s 5 s 6 c 5 c 5 d

53 Manipolatore di Stanford

54 T 0 6 = T 0 3 T 3 6 = n0 s 0 a 0 p

55 p 0 = n 0 = s 0 = a 0 = c 1 s 2 d 3 s 1 d 2 + ( ) c 1 (c 2 c 4 s 5 + s 2 c 5 ) s 1 s 4 s 5 d6 s 1 s 2 d 3 + c 1 d 2 + ( ) s 1 (c 2 c 4 s 5 + s 2 c 5 ) + c 1 s 4 s 5 d6 c 2 d 3 + ( s 2 c 4 s 5 + c 2 c 5 )d 6 ( ) c 1 ( c2 (c 4 c 5 c 6 s 4 s 6 ) s 2 s 5 c 6 ) s1 (s 4 c 5 c 6 + c 4 s 6 ) s 1 c2 (c 4 c 5 c 6 s 4 s 6 ) s 2 s 5 c 6 + c1 (s 4 c 5 c 6 + c 4 s 6 ) s 2 (c 4 c 5 c 6 s 4 s 6 ) c 2 s 5 c 6 ( ) c 1 ( c2 (c 4 c 5 s 6 + s 4 c 6 ) + s 2 s 5 s 6 ) s1 ( s 4 c 5 s 6 + c 4 c 6 ) s 1 c2 (c 4 c 5 s 6 + s 4 c 6 ) + s 2 s 5 s 6 + c1 ( s 4 c 5 s 6 + c 4 c 6 ) s 2 (c 4 c 5 s 6 + s 4 c 6 ) + c 2 s 5 s 6 c 1(c 2 c 4 s 5 + s 2 c 5 ) s 1 s 4 s 5 s 1 (c 2 c 4 s 5 + s 2 c 5 ) + c 1 s 4 s 5 s 2 c 4 s 5 + c 2 c 5

56 Manipolatore antropomorfo con polso sferico

57 Braccio a i α i d i ϑ i 1 0 π/2 0 ϑ 1 2 a ϑ π/2 0 ϑ π/2 d 4 ϑ π/2 0 ϑ d 6 ϑ 6

58 A 2 3 = c 3 0 s 3 0 s 3 0 c 3 0 A = c 4 0 s 4 0 s 4 0 c d p 0 = n 0 = s 0 = a 0 = ( ) a 2 c 1 c 2 + d 4 c 1 s 23 + d 6 ( c1 (c 23 c 4 s 5 + s 23 c 5 ) + s 1 s 4 s 5 ) a 2 s 1 c 2 + d 4 s 1 s 23 + d 6 s1 (c 23 c 4 s 5 + s 23 c 5 ) c 1 s 4 s 5 a 2 s 2 d 4 c 23 + d 6 (s 23 c 4 s 5 c 23 c 5 ) ( ) c 1 ( c23 (c 4 c 5 c 6 s 4 s 6 ) s 23 s 5 c 6 ) + s1 (s 4 c 5 c 6 + c 4 s 6 ) s 1 c23 (c 4 c 5 c 6 s 4 s 6 ) s 23 s 5 c 6 c1 (s 4 c 5 c 6 + c 4 s 6 ) s 23 (c 4 c 5 c 6 s 4 s 6 ) + c 23 s 5 c 6 ( ) c 1 ( c23 (c 4 c 5 s 6 + s 4 c 6 ) + s 23 s 5 s 6 ) + s1 ( s 4 c 5 s 6 + c 4 c 6 ) s 1 c23 (c 4 c 5 s 6 + s 4 c 6 ) + s 23 s 5 s 6 c1 ( s 4 c 5 s 6 + c 4 c 6 ) s 23 (c 4 c 5 s 6 + s 4 c 6 ) c 23 s 5 s 6 c 1(c 23 c 4 s 5 + s 23 c 5 ) + s 1 s 4 s 5 s 1 (c 23 c 4 s 5 + s 23 c 5 ) c 1 s 4 s 5 s 23 c 4 s 5 c 23 c 5

59 Manipolatore del DLR Braccio a i α i d i ϑ i 1 0 π/2 0 ϑ π/2 0 ϑ π/2 d 3 ϑ π/2 0 ϑ π/2 d 5 ϑ π/2 0 ϑ d 7 ϑ 7

60 A i 1 i = c i 0 s i 0 s i 0 c i d i A 6 7 = c 7 s s 7 c d i = 1,...,6 p 0 7 = x d3 = c 1 s 2 d 3x d3 + d 5 x d5 + d 7 x d7 d 3 y d3 + d 5 y d5 + d 7 y d7 d 3 z d3 + d 5 z d5 + d 7 z d7 x d5 = c 1 (c 2 c 3 s 4 s 2 c 4 ) + s 1 s 3 s 4 x d7 = c 1 (c 2 k 1 + s 2 k 2 ) + s 1 k 3 y d3 = s 1 s 2 y d5 = s 1 (c 2 c 3 s 4 s 2 c 4 ) c 1 s 3 s 4 y d7 = s 1 (c 2 k 1 + s 2 k 2 ) c 1 k 3 z d3 = c 2 z d5 = c 2 c 4 + s 2 c 3 s 4 z d7 = s 2 (c 3 (c 4 c 5 s 6 s 4 c 6 ) + s 3 s 5 s 6 ) c 2 k 2 k 1 = c 3 (c 4 c 5 s 6 s 4 c 6 ) + s 3 s 5 s 6 k 2 = s 4 c 5 s 6 + c 4 c 6 k 3 = s 3 (c 4 c 5 s 6 s 4 c 6 ) c 3 s 5 s 6

61 n 0 7 = s 0 7 = ((x ac 5 + x c s 5 )c 6 + x b s 6 )c 7 + (x a s 5 x c c 5 )s 7 ((y a c 5 + y c s 5 )c 6 + y b s 6 )c 7 + (y a s 5 y c c 5 )s 7 (z a c 6 + z c s 6 )c 7 + z b s 7 ((x a c 5 + x c s 5 )c 6 + x b s 6 )s 7 + (x a s 5 x c c 5 )c 7 ((y a c 5 + y c s 5 )c 6 + y b s 6 )s 7 + (y a s 5 y c c 5 )c 7 (z a c 6 + z c s 6 )s 7 + z b c 7 a 0 7 = (x ac 5 + x c s 5 )s 6 x b c 6 (y a c 5 + y c s 5 )s 6 y b c 6 z a s 6 z c c 6 se α 7 = π/2 x a = (c 1 c 2 c 3 + s 1 s 3 )c 4 + c 1 s 2 s 4 x b = (c 1 c 2 c 3 + s 1 s 3 )s 4 c 1 s 2 c 4 x c = c 1 c 2 s 3 s 1 c 3 y a = (s 1 c 2 c 3 c 1 s 3 )c 4 + s 1 s 2 s 4 y b = (s 1 c 2 c 3 c 1 s 3 )s 4 s 1 s 2 c 4 y c = s 1 c 2 s 3 + c 1 c 3 z a = (s 2 c 3 c 4 c 2 s 4 )c 5 + s 2 s 3 s 5 z b = (s 2 c 3 s 4 + c 2 c 4 )s 5 s 2 s 3 c 5 z c = s 2 c 3 s 4 + c 2 c 4 A 6 7 = c 7 0 s 7 a 7 c 7 s 7 0 c 7 a 7 s

62 Manipolatore umanoide Braccia costituite da due manipolatori del DLR (α 7 = π/2) Struttura di collegamento tra l organo terminale del torso antropomorfo e le terne base dei manipolatori che fungono da braccia meccanismo che permette di mantenere il petto del manipolatore umanoide sempre ortogonale al suolo (ϑ 4 = ϑ 2 ϑ 3 ) Cinematica diretta Trh 0 = T 3 0 T t 3 T r t T rh r Tlh 0 = T3 0 Tt 3 Tl t Tlh l

63 T 0 3 T 3 t = c 23 s s 23 c come nel manipolatore antropomorfo T t r e T t l dipendono da β T r rh e T r lh come nel manipolatore del DLR

64 SPAZIO DEI GIUNTI E SPAZIO OPERATIVO Spazio dei giunti q = q 1.. q n q i = ϑ i (giunto rotoidale) q i = d i (giunto prismatico) Spazio operativo p (posizione) φ (orientamento) x = [ ] p φ Equazione cinematica diretta x = k(q)

65 Esempio x = p x p y φ = 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

66 Spazio di lavoro Spazio di lavoro raggiungibile p = p(q) q im q i q im i = 1,...,n elementi di superficie planare, sferica, toroidale e cilindrica Spazio di lavoro destro orientamenti diversi

67 Esempio configurazioni ammissibili

68 spazio di lavoro

69 Accuratezza scostamento tra posizione conseguita con la postura assegnata e posizione calcolata tramite la cinematica diretta valori tipici: (0.2, 1) mm Ripetibilità capacità del manipolatore di tornare in una posizione precedentemente raggiunta valori tipici: (0.02, 0.2) mm Ridondanza cinematica m < n (intrinseca) r < m = n (funzionale)

70 CALIBRAZIONE CINEMATICA Valori precisi dei parametri DH per migliorare l accuratezza di un manipolatore Equazione cinematica diretta in funzione di tutti i parametri x = k(a, α, d, ϑ) x m locazione misurata x n locazione nominale (parametri fissi + variabili giunto) x = k k a + a α = Φ(ζ n ) ζ k k α + d + d ϑ ϑ

71 l misure (lm 4n) x 1 x =. = x l Φ 1. Φ l ζ = Φ ζ Soluzione ζ = ( Φ T Φ) 1 ΦT x... finché ζ converge ζ = ζ n + ζ stime più accurate dei parametri fissi correzioni alle misure dei trasduttori Inizializzazione postura di riferimento (home)

72 PROBLEMA CINEMATICO INVERSO Cinematica diretta q = T q = x Cinematica inversa T = q x = q Complessità soluzione analitica (in forma chiusa)? soluzioni multiple infinite soluzioni non esistono soluzioni ammissibili Intuizione algebrica geometrica Tecniche numeriche

73 Soluzione del manipolatore planare a tre bracci Soluzione algebrica φ = ϑ 1 + ϑ 2 + ϑ 3 p Wx = p x a 3 c φ = a 1 c 1 + a 2 c 12 p Wy = p y a 3 s φ = a 1 s 1 + a 2 s 12

74 c 2 = p2 Wx + p2 Wy a2 1 a2 2 2a 1 a 2 s 2 = ± 1 c 2 2 ϑ 2 = Atan2(s 2, c 2 ) s 1 = (a 1 + a 2 c 2 )p Wy a 2 s 2 p Wx p 2 Wx + p2 Wy c 1 = (a 1 + a 2 c 2 )p Wx + a 2 s 2 p Wy p 2 Wy + p2 Wy ϑ 1 = Atan2(s 1, c 1 ) ϑ 3 = φ ϑ 1 ϑ 2

75 Soluzione geometrica c 2 = p2 Wx + p2 Wy a2 1 a2 2 2a 1 a 2. ϑ 2 = cos 1 (c 2 ) α = Atan2(p Wy, p Wx ) c β p 2 Wx + p2 Wy = a 1 + a 2 c 2 β = cos 1 p2 Wx + p2 Wy + a2 1 a2 2 2a 1 p 2 Wx + p2 Wy ϑ 1 = α ± β

76 Soluzione di manipolatori con polso sferico p W = p d 6 a Soluzione disaccoppiata calcolare la posizione del polso p W (q 1, q 2, q 3 ) risolvere la cinematica inversa per (q 1, q 2, q 3 ) calcolare R3 0(q 1, q 2, q 3 ) calcolare R6(ϑ 3 4, ϑ 5, ϑ 6 ) = R3 0T R risolvere la cinematica inversa per l orientamento(ϑ 4, ϑ 5, ϑ 6 )

77 Soluzione del manipolatore sferico (A 0 1 ) 1 T 0 3 = A1 2 A2 3 p 1 W = p Wxc 1 + p Wy s 1 p Wz p Wx s 1 + p Wy c 1 = d 3s 2 d 3 c 2 d 2

78 c 1 = 1 t2 1 + t 2 s 1 = 2t 1 + t 2 (d 2 + p Wy )t 2 + 2p Wx t + d 2 p Wy = 0 ( ) ϑ 1 = 2Atan2 p Wx ± p 2Wx + p2wy d22, d 2 + p Wy p Wx c 1 + p Wy s 1 p Wz = d 3s 2 d 3 c 2 ϑ 2 = Atan2(p Wx c 1 + p Wy s 1, p Wz ) d 3 = (p Wx c 1 + p Wy s 1 ) 2 + p 2 Wz

79 Soluzione del manipolatore antropomorfo p Wx = c 1 (a 2 c 2 + a 3 c 23 ) p Wy = s 1 (a 2 c 2 + a 3 c 23 ) p Wz = a 2 s 2 + a 3 s 23 c 3 = p2 Wx + p2 Wy + p2 Wz a2 2 a2 3 2a 2 a 3 s 3 = ± 1 c 2 3 ϑ 3 = Atan2(s 3, c 3 ) ϑ 3,I [ π, π] ϑ 3,II = ϑ 3,I

80 c 2 = s 2 = ± p 2 Wx + p2 Wy (a 2 + a 3 c 3 ) + p Wz a 3 s 3 a a a 2a 3 c 3 p Wz (a 2 + a 3 c 3 ) p 2 Wx + p2 Wy a 3s 3 a a a 2a 3 c 3 ϑ 2 = Atan2(s 2, c 2 ) per s + 3 = 1 c 2 3 : ( ϑ 2,I = Atan2 (a 2 + a 3 c 3 )p Wz a 3 s + 3 p 2 Wx + p2 Wy, ) (a 2 + a 3 c 3 ) p 2 Wx + p2 Wy + a 3s + 3 p Wz ( ϑ 2,II = Atan2 (a 2 + a 3 c 3 )p Wz + a 3 s + 3 p 2 Wx + p2 Wy, ) (a 2 + a 3 c 3 ) p 2 Wx + p2 Wy + a 3s + 3 p Wz per s 3 = 1 c 2 3 : ( ϑ 2,III = Atan2 (a 2 + a 3 c 3 )p Wz a 3 s 3 p 2 Wx + p2 Wy, ) (a 2 + a 3 c 3 ) p 2 Wx + p2 Wy + a 3s 3 p Wz ( ϑ 2,IV = Atan2 (a 2 + a 3 c 3 )p Wz + a 3 s 3 p 2 Wx + p2 Wy, ) (a 2 + a 3 c 3 ) p 2 Wx + p2 Wy + a 3s 3 p Wz

81 ϑ 1,I = Atan2(p Wy, p Wx ) ϑ 1,II = Atan2( p Wy, p Wx ) Quattro configurazioni ammissibili (ϑ 1,I, ϑ 2,I, ϑ 3,I ) (ϑ 1,I, ϑ 2,III, ϑ 3,II ) (ϑ 1,II, ϑ 2,II, ϑ 3,I ) (ϑ 1,II, ϑ 2,IV, ϑ 3,II ) soluzione univoca solo se p Wx 0 p Wy 0

82 Soluzione del polso sferico R 3 6 = n 3 x s 3 x a 3 x n 3 y s 3 y a 3 y n 3 z s 3 z a 3 z ϑ 4 = Atan2(a 3 y, a 3 x) ( ) ϑ 5 = Atan2 (a 3 x) 2 + (a 3 y) 2, a 3 z ϑ 6 = Atan2(s 3 z, n3 z ) ϑ 4 = Atan2( a 3 y, a3 x ( ) ) ϑ 5 = Atan2 (a 3 x )2 + (a 3 y )2, a 3 z ϑ 6 = Atan2( s 3 z, n 3 z)