Spazio dei giunti e spazio operativo

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

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

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

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

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

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

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

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

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

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

Esempio

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

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 ϑ

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 r13 2 + r2 23, r 33 ψ = Atan2(r 32, r 31 ) ovvero (ϑ ( π, 0)) ϕ = Atan2( r 23, r 13 ) ( ) ϑ = Atan2 r13 2 + r2 23, r 33 ψ = Atan2( r 32, r 31 )

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 ψ

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, r32 2 + r2 33 ψ = Atan2(r 32, r 33 ) ovvero (ϑ (π/2, 3π/2)) ϕ = Atan2( r 21, r 11 ) ( ) ϑ = Atan2 r 31, r32 2 + r2 33 ψ = Atan2( r 32, r 33 )

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

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)

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 33 1 2 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

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

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 33 + 1 ǫ = 1 sgn(r 32 r 23 ) r 11 r 22 r 33 + 1 sgn(r 13 r 31 ) r 22 r 33 r 11 + 1 2 sgn(r 21 r 12 ) r 33 r 11 r 22 + 1 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 }

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

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

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

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

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) 0 0 0 1

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

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

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

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

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

Trasformazione di coordinate A i 1 i = A i i = c ϑi s ϑi 0 0 s ϑi c ϑi 0 0 0 0 1 d i 0 0 0 1 1 0 0 a i 0 c αi s αi 0 0 s αi c αi 0 0 0 0 1 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 0 0 0 1

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 + 1 5. 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 0 1...An 1 n che fornisce posizione e orientamento della terna n rispetto alla terna 0 10. 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

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 )

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

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

Manipolatore planare a tre bracci

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

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, 3 0 0 1 0 0 0 0 1 T 0 3 = A 0 1A 1 2A 2 3 = c 123 s 123 0 a 1 c 1 + a 2 c 12 + a 3 c 123 s 123 c 123 0 a 1 s 1 + a 2 s 12 + a 3 s 123 0 0 1 0 0 0 0 1

Manipolatore a parallelogramma

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

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

Risoluzione vincoli orientamento (OK) posizione R 3 0 (q ) ( p 0 3 (q ) p 0 1 (q ) ) = 0 0 0 a 1 (c 1 + c 1 2 3 ) + a 1 (c 1 2 c 1 ) = 0 a 1 (s 1 + s 1 2 3 ) + 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 1 0 0 1 0 0 0 0 1

Manipolatore sferico

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

A 0 1 = c 1 0 s 1 0 s 1 0 c 1 0 A 1 2 0 1 0 0 = 0 0 0 1 A 2 3 = 1 0 0 0 0 1 0 0 0 0 1 d 3 0 0 0 1 c 2 0 s 2 0 s 2 0 c 2 0 0 1 0 d 2 0 0 0 1 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 3 0 0 0 1

Manipolatore antropomorfo

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

A i 1 i = A 0 1 = c 1 0 s 1 0 s 1 0 c 1 0 0 1 0 0 0 0 0 1 c i s i 0 a i c i s i c i 0 a i s i 0 0 1 0 0 0 0 1 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 23 0 0 0 1

Polso sferico

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

A 3 4 = c 4 0 s 4 0 s 4 0 c 4 0 A 4 5 0 1 0 0 = 0 0 0 1 A 5 6 = c 6 s 6 0 0 s 6 c 6 0 0 0 0 1 d 6 0 0 0 1 c 5 0 s 5 0 s 5 0 c 5 0 0 1 0 0 0 0 0 1 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 6 0 0 0 1

Manipolatore di Stanford

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

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

Manipolatore antropomorfo con polso sferico

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

A 2 3 = c 3 0 s 3 0 s 3 0 c 3 0 A 3 4 0 1 0 0 = 0 0 0 1 c 4 0 s 4 0 s 4 0 c 4 0 0 1 0 d 4 0 0 0 1 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

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

A i 1 i = c i 0 s i 0 s i 0 c i 0 0 1 0 d i 0 0 0 1 A 6 7 = c 7 s 7 0 0 s 7 c 7 0 0 0 0 1 d 7 0 0 0 1 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

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

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

T 0 3 T 3 t = c 23 s 23 0 0 s 23 c 23 0 0 0 0 1 0 0 0 0 1 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

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)

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

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

Esempio configurazioni ammissibili

spazio di lavoro

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)

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

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)

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

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

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

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

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 )

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

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

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

c 2 = s 2 = ± p 2 Wx + p2 Wy (a 2 + a 3 c 3 ) + p Wz a 3 s 3 a 2 2 + a2 3 + 2a 2a 3 c 3 p Wz (a 2 + a 3 c 3 ) p 2 Wx + p2 Wy a 3s 3 a 2 2 + a2 3 + 2a 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

ϑ 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

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)