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 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

21 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

22 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

23 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

24 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)

25 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

26 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

27 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

28 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

29 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

30 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

31 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

32 Manipolatore planare a tre bracci

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

34 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

35 Manipolatore sferico

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

37 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

38 Manipolatore antropomorfo

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

40 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

41 Polso sferico

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

43 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

44 Manipolatore di Stanford

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

46 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

47 Manipolatore antropomorfo con polso sferico

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

49 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

50 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

51 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

52 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

53 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

54 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

55 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)

56 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

57 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

58 Esempio configurazioni ammissibili

59 spazio di lavoro

60 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)

61 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 ϑ ϑ

62 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)

63 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

64 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

65 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

66 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 = α ± β

67 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 )

68 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

69 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

70 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

71 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

72 ϑ 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

73 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)