Cinematica diretta Un manipolatore è costituito da un insieme di corpi rigidi (bracci) connessi in cascata tramite coppie cinematiche (giunti). Si assume che ad ogni giunto corrisponde un grado di mobilità della struttura. Ad ogni giunto inoltre viene associata una variabile detta variabile di giunto. I giunti possono essere fondamentalmente di due tipi: giunti di rotazione o rotoidali e giunti di traslazione o prismatici. Cinematica diretta Si consideri un manipolatore costituito da n +1 bracci connessi in cascata (catena aperta) tramite n giunti (il braccio 0 è la base). Obiettivo della cinematica diretta è la determinazione della posizione e orientamento dell’organo terminale del manipolatore, noti i valori che assumono le variabili di giunto per ottenere detta posizione ed orientamento. posizione e orientamento dell’ organo terminale sono descritte, rispetto alla Terna utensile terna base, dalla matrice di trasformazione omogenea ⎡n 0 (q) s 0 (q) a 0 (q) T (q) = ⎢ 0 0 ⎣ 0 0 p 0 (q)⎤ ⎥ 1 ⎦ q è il vettore nx1 delle variabili di giunto n, s, a versori della terna utensile p posizione della terna utensile Terna base La terna O0 – x0y0z0 è detta terna base. La terna solidale all’organo terminale, detta terna utensile, si sceglie in maniera conveniente di caso in caso. Con riferimento ad un organo pinza, l’origine della terna utensile si pone al centro della pinza, il versore a (approccio) si sceglie nella direzione d’avvicinamento ad un oggetto, il versore s (scivolamento) si sceglie normale ad a nel piano di scorrimento degli elementi prensili, il versore n si sceglie in modo da completare la terna e renderla levogira. Convenzione di Denavit – Hartenberg Metodo sistematico per l’individuazione delle terne e la scrittura della cinematica diretta Su ogni link viene posizionata una terna posta sul giunto di connessione con il link successivo assunto come asse i l’asse del giunto che connette il braccio i-1 al braccio i , per definire la terna i, solidale al braccio i opereremo secondo i passi: Si sceglie l’asse zi giacente lungo l’asse del giunto i+1; Si individua Oi nel punto d’intersezione tra l’asse zi e la normale comune agli assi zi-1 e zi. Indichiamo, poi, con Oi′ l’intersezione della normale comune, ricavata prima, con l’asse zi-1; (La normale comune tra due rette sghembe è la retta a cui appartiene il segmento di minima distanza tra le rette) Si assume l’asse xi diretto lungo la detta normale diretto da zi a zi+1; Si sceglie l’asse yi in modo da completare la terna levogira. In generale la scelta della terna, secondo questa convenzione, è univoca tranne che per i casi: Con riferimento alla terna base, l’origine O0 e la direzione di x0 non sono univocamente determinate, essendo mancante il giunto O-1, quindi non si può determinare la normale comune. Pertanto solo la direzione dell’asse z0 è determinata. In questo caso O0 e x0 si scelgono arbitrari. Con riferimento all’ultima terna, poiché non esiste il giunto n+1, l’asse zn non può essere determinato, mentre xn deve essere normale all’asse zn-1. Poiché generalmente il giunto n è rotoidale, l’asse zn si sceglie parallelo all’asse zn-1. Quando due assi consecutivi sono paralleli, la terna non è univocamente determinata, perché è impossibile stabilire la normale comune, in tal caso si pone xi in modo che passi per l’origine della terna i-1. Quando due assi consecutivi si intersecano è impossibile stabilire il verso di xi, essendo la normale comune un punto, in tal caso si pone coincidente al prodotto vettoriale zi-1Xzi. Quando il giunto i è prismatico solo la direzione dell’asse zi-1 è determinata. In tutti questi casi l’indeterminazione può essere sfruttata per semplificare la procedura ricercando, ad esempio, condizioni d’allineamento tra assi delle terne consecutive. Una volta definite le terne solidali ai bracci, la posizione e l’orientamento della terna i rispetto alla terna i-1 risultano completamente definite dai seguenti parametri: 1. 2. 3. 4. ai distanza tra le origini Oi ed Oi′ (lunghezza del link); di coordinata di Oi′ sull’asse zi-1 (offset) αi angolo intorno all’asse xi tra l’asse zi-1 e l’asse zi, positivo antiorario (torsione del link) ϑi angolo intorno all’asse zi-1 tra l’asse xi-1 e l’asse xi, positivo antiorario (rotazione del link) Dei quattro parametri due (ai, αi) sono costanti, una volta individuata la terna. Degli altri due solo uno è variabile in dipendenza del tipo di giunto, e precisamente • se il giunto è rotoidale varia ϑi • se il giunto è prismatico varia di. A questo punto si è in grado di definire la trasformazione di coordinate che lega la terna i alla terna i-1. 1. Si parte dalla terna coincidente con la terna i-1; 2. Si trasla la terna scelta di di lungo l’asse zi-1, in modo da fare coincidere le due origini Oi-1 ed Oi′, successivamente si ruota la terna traslata di un angolo pari a ϑi in torno all’asse zi-1, in modo da fare sovrapporre le due terne. Queste operazioni sono descritte dalla matrice: ⎡cϑi ⎢s Aii′−1 = ⎢ ϑi ⎢0 ⎢ ⎣0 − sϑi cϑi 0 0 0 0⎤ 0 0 ⎥⎥ 1 di ⎥ ⎥ 0 1⎦ 3. Si trasla la terna ora sovrapposta lungo, l’asse xi′ della quantità ai in modo da fare coincidere le due origini Oi-1 ed Oi. Quindi si ruota la terna trasposta di un angolo pari ad αi intorno all’asse xi′, in modo da fare coincidere le terne zi e zi-1. Le operazioni appena eseguite sono rappresentate dalla matrice di rotazione: ⎡1 0 ⎢0 c αi Aii′ = ⎢ ⎢0 sα i ⎢ ⎣0 0 0 − sα i cα i 0 ai ⎤ 0 ⎥⎥ 0⎥ ⎥ 1⎦ 4. Avendo operato due trasformazioni di coordinate definite rispetto alla terna corrente, la trasformazione complessiva si ottiene dalla matrice: ⎡cϑi ⎢s Aii −1 (qi ) = Aii′−1 Aii′ = ⎢ ϑi ⎢0 ⎢ ⎣0 − sϑi cα i cϑi cα i sϑi sα i − cϑi sα i sα i 0 cα i 0 ai cϑi ⎤ ai sϑi ⎥⎥ di ⎥ ⎥ 1 ⎦ Cinematica diretta Definita la singola matrice di trasformazione, possiamo realizzare la procedura di costruzione della funzione cinematica diretta per composizione delle singole trasformazioni di coordinate, in un’unica matrice . Dal momento che su ogni braccio è stata definita una terna ad esso solidale , la trasformazione di coordinate complessiva che esprime posizione ed orientamento della terna n rispetto alla terna base c’è data da: Tn0 (q ) = A10 (q1 )A21 (q2 )... Ann −1 (qn ) Procedura operativa: 1. Individuare e numerare consecutivamente gli assi dei giunti; assegnare, rispettivamente, le direzioni agli assi z0, … , zn 2. Fissare la terna base posizionandone l’origine sull’asse z0; gli assi x0 e y0 sono scelti in maniera tale da ottenere una terna levogira Eseguire i passi da 3 a 5 per i = 1 ,…, n 3. Individuare l’origine Oi all’intersezione di zi con la normale comune agli assi zi-1 e zi Se gli assi zi-1 e zi sono paralleli e il giunto i è rotoidale, posizionare Oi in modo da annullare di; se il giunto i è prismatico, scegliere Oi in corrispondenza di una posizione di riferimento per la corsa del giunto (ad esempio un fine-corsa) 4. Fissare l’asse xi diretto lungo la normale comune agli assi zi-1 e zi con verso positivo dal giunto i al giunto i + 1 5. Fissare l’asse yi in modo da ottenere una terna levogira Per completare: 6. Fissare la terna n, allineando zn lungo la direzione di zn-1 se il giunto n è rotoidale, o scegliendo zn in maniera arbitraria se il giunto è prismatico; fissare l’asse in accordo al punto 4 7. Costruire per i = 1 ,…, n la tabella dei parametri ai, di, αi, θi 8. Calcolare sulla base dei parametri di cui al punto 7 le matrici di trasformazione omogenee Aii −1 ( qi ) 0 0 n −1 T ( q ) = A L A 1 n 9. Calcolare n 10. Calcolare la funzione cinematica diretta che fornisce posizione e orientamento della terna utensile rispetto alla terna base Teb (q ) = T0bTn0Ten (eventuali rototraslazioni arbitrarie costanti possono essere aggiunte prima della terna 0 e dopo la terna n per rappresentare il movimento dell’utensile in coordinate più naturali) manipolatore planare a tre bracci Gli assi di rotazione sono paralleli, gli assi xi sono scelti lungo i rispettivi bracci e giacenti nel piano x0y0 Si sceglie l’asse zi giacente lungo l’asse del giunto i+1 (in questo caso z1 z2 e z3 sono paralleli) 3 2 1 2 1 distanza tra o e o’ 3 Si individua Oi nel punto d’intersezione tra l’asse zi e la normale comune agli assi zi-1 e zi Si assume l’asse xi diretto lungo la normale diretto da zi a zi+1; se due assi consecutivi sono paralleli si pone xi in modo che passi per l’origine della terna i-1 Si sceglie l’asse yi in modo da completare la terna levogira. link Lunghezza ai torsione αi Offset di Rotazione ϑi 1 a1 0 0 ϑ1 2 a2 0 0 ϑ2 3 a3 0 0 ϑ3 angolo tra gli assi z coord. di o’ su zi-1 angolo tra gli assi x I giunti sono tutti rotoidali quindi: ⎡cϑi ⎢s A10 (ϑ1 ) = A21 (ϑ2 ) = A32 (ϑ3 ) = ⎢ ϑi ⎢0 ⎢ ⎣0 − sϑi cϑi 0 0 ⎡c123 ⎢s 123 0 0 1 2 ⎢ ( ) ( ) ( ) ( ) Tn q = A1 q1 A2 q2 A3 q3 = ⎢ 0 ⎢ ⎣ 0 0 ai cϑi ⎤ 0 ai sϑi ⎥⎥ 1 0 ⎥ ⎥ 0 1 ⎦ − s123 c123 0 0 q = [ϑ1 ϑ2 ϑ3 ]T 0 a1c1 + a2 c12 + a3c123 ⎤ 0 a1s1 + a2 s12 + a3 s123 ⎥⎥ ⎥ 1 0 ⎥ 0 1 ⎦ Manipolatore sferico La terna 0 è stata scelta in modo da avere d1=0 (origine all’intersezione di z0 e z1) Si sceglie l’asse zi giacente lungo l’asse del giunto i+1 Si individua Oi nel punto d’intersezione tra l’asse zi e la normale comune agli assi zi-1 e zi Si assume l’asse xi diretto lungo la normale comune a zi e zi+1; se due assi consecutivi sono paralleli e il giunto i è prismatico, scegliere Oi in corrispondenza di una posizione di riferimento per la corsa del giunto Si sceglie l’asse yi in modo da completare la terna levogira. Quando due assi consecutivi si intersecano è il verso di xi, si pone coincidente al prodotto vettoriale zi-1Xzi. Quando il giunto i è prismatico solo la direzione dell’asse zi-1 è determinata. Braccio ai distanza tra o e o’ αi di ϑi 1 0 -π/2 0 ϑ1 2 0 π/2 d2 ϑ2 3 0 0 d3 0 angolo tra gli assi z coord. di o’ su zi-1 angolo tra gli assi x ⎡c1 ⎢s A10 (ϑ1 ) = ⎢ 1 ⎢0 ⎢ ⎣0 − s1 0⎤ c1 0⎥⎥ 0 − 1 0 0⎥ ⎥ 0 0 1⎦ ⎡c 2 ⎢s A21 (ϑ2 ) = ⎢ 2 ⎢0 ⎢ ⎣0 0 s2 0 − c2 ⎡1 ⎢0 A32 (d 3 ) = ⎢ ⎢0 ⎢ ⎣0 0 1 0 0 1 0 0 0 0 0⎤ 0 ⎥⎥ d2 ⎥ ⎥ 1⎦ 0 0⎤ 0 0 ⎥⎥ 1 d3 ⎥ ⎥ 0 1⎦ Tn0 (q ) = A10 (ϑ1 )A21 (ϑ2 )A32 (d 3 ) = ⎡c1c2 ⎢s c ⎢ 1 2 ⎢ − s2 ⎢ ⎣ 0 − s1 c1s2 c1 0 0 s1s2 c2 0 c1s2 d 3 − s1d 2 ⎤ s1s2 d 3 − c1d 2 ⎥⎥ ⎥ c2 d 3 ⎥ 1 ⎦ Manipolatore antropomorfo ⎡c1c23 ⎢s c T03 (q ) = ⎢ 1 23 ⎢ s23 ⎢ ⎣ 0 Braccio ai αi di ϑi 1 0 -π/2 0 ϑ1 2 a2 0 0 ϑ2 3 a3 0 0 ϑ3 − c1s23 s1 − s1s23 c23 − c1 0 0 0 c1 (a2 c2 + a3c23 )⎤ s1 (a2 c2 + a3c23 )⎥⎥ a2 s2 + a3 s23 ⎥ ⎥ 1 ⎦ Polso sferico La numerazione parte da 4 perché il polso Braccio e’ connesso ad una struttura 4 Gli assi di rotazione si intersecano nello stesso punto 5 Il verso di x4 e x5 è indeterminato 6 ⎡c4 c5c6 − s4 s6 ⎢s c c + c s T63 (q ) = ⎢ 4 5 6 4 6 ⎢ − s5c6 ⎢ 0 ⎣ ai αi di ϑi 0 -π/2 0 ϑ4 0 π/2 0 ϑ5 0 0 d6 ϑ6 − c4 c5 s6 − s4 c6 − s4 c5 s6 + c4 c6 c4 s5 s4 s5 s5 s6 0 c5 0 c4 s5 d 6 ⎤ s4 s5 d 6 ⎥⎥ c5 d 6 ⎥ ⎥ 1 ⎦ Spazio dei giunti e spazio operativo Se si fa riferimento ad una rappresentazione minima dell’orientamento è possibile descrivere il posizionamento del manipolatore mediante un vettore, mx1, con m ≤ 6. ⎡ p⎤ x=⎢ ⎥ ⎣φ ⎦ dove p caratterizza la posizione dell’organo terminale (tre componenti) e φ il suo orientamento (altre tre componenti). Lo spazio in cui è definito il vettore x è quello rispetto al quale viene, tipicamente, specificata l’operazione richiesta al manipolatore. Pertanto esso è denominato spazio operativo. Il vettore x, come sappiamo, è funzione delle variabili di giunto. Lo spazio in cui è definito il vettore nx1 delle variabili di giunto è definito spazio dei giunti o delle configurazioni. qi =ϑi per giunti rotoidali e qi = di per giunti prismatici. ⎡ q1 ⎤ q = ⎢⎢ M ⎥⎥ ⎢⎣qn ⎥⎦ x = k(q) Equazione della CINEMATICA DIRETTA Spazio di lavoro Con riferimento allo spazio operativo, si definisce spazio di lavoro di un manipolatore la regione descritta, dall’origine della terna utensile, quando ai giunti del manipolatore si fanno eseguire tutti i moti possibili. spazio di lavoro raggiungibile è lo spazio di lavoro, che la terna utensile può descrivere con almeno un orientamento, spazio di lavoro destro (o di destrezza) è la regione che l’origine della terna utensile può descrivere assumendo tutti gli orientamenti possibili. p = p(q) qim ≤ qi ≤ qiM i = 1, …, n Ovviamente lo spazio di lavoro destro è un sottoinsieme dello spazio di lavoro raggiungibile. Problema cinematico inverso Tn0 (q ) = A10 (q1 ) A21 (q2 )... Ann −1 (qn ) • Le equazioni da risolvere sono, in genere, non lineari delle quali non sempre è possibile trovare una soluzione analitica (in forma chiusa); • Si possono avere soluzioni multiple • Si possono avere infinite soluzioni, come nel caso di un manipolatore ridondante; • Possono non esistere soluzioni ammissibili, data la struttura cinematica del manipolatore. Per quanto riguarda l’esistenza di soluzioni, questa è garantita se posizione ed orientamento assegnati appartengono allo spazio di lavoro destro del manipolatore. intuizione algebrica - intuizione geometrica tecniche numeriche di soluzione Soluzione del manipolatore planare a 3 bracci vogliamo calcolare le variabili di giunto specifichiamo posizione ed orientamento in termini di un numero minimo di parametri scegliamo due coordinate della posizione Wx e Wy e l’angolo φ con l’asse x0 noto l’orientamento una delle equazioni da risolvere è: ϕ = ϑ1 + ϑ2 + ϑ3 ⎡c123 ⎢s T30 (q ) = ⎢ 123 ⎢ 0 ⎢ ⎣ 0 − s123 c123 0 ϑ0 ϑ2 1 0 a1c1 + a2 c12 + a3c123 ⎤ ⎡cφ 0 a1s1 + a2 s12 + a3 s123 ⎥⎥ ⎢⎢ sφ = ⎥ ⎢0 1 0 ⎥ ⎢ 1 ϑ0 ⎦ ⎣0 3 ⎡cφ ⎢s =⎢ φ ⎢0 ⎢ ⎣0 − sφ cφ 0 a1c1 + a2 c12 + a3cφ ⎤ 0 a1s1 + a2 s12 + a3 sφ ⎥⎥ = ⎥ 1 0 ⎥ 0 1 ⎦ 0 0 − sφ cφ 0 0 0 Wx + a3cφ ⎤ 0 W y + a3 sφ ⎥⎥ ⎥ 1 0 ⎥ 0 1 ⎦ Soluzione del manipolatore planare a 3 bracci il sistema da risolvere è: ⎧Wx = a1c1 + a2 c12 ⎪ ⎨W y = a1s1 + a2 s12 ⎪ ⎩φ = ϑ1 + ϑ2 + ϑ3 da cui: 2 2 2 2 ⎧ W + W − a + a x y 1 2 ⎧⎪Wx2 + W y2 = a12 + a22 + 2a1a2 c2 ⎪c2 = 2a1a2 ⇒⎨ ⇒ ϑ2 = A tan 2(s2 , c2 ) ⎨ 2 2 ⎪⎩s2 + c2 = 1 ⎪ 2 s = ± 1 − c 2 ⎩ 2 Osserviamo che per esistere la soluzione deve essere –1 ≤ c2 ≤ 1, altrimenti il punto assegnato sta al di fuori dello spazio di lavoro. Inoltre il segno positivo indica la configurazione a gomito alto mentre il segno negativo quella a gomito basso. Ricavato ϑ2 si sostituisce nel sistema trovato precedentemente, il quale diventa un sistema in tre incognite. Da esso si ricavano s1 e c1 e quindi ϑ1 = atan2(s1, c1). Infine si ricava ϑ3 = φ - ϑ1 - ϑ2. Soluzione di manipolatori con polso sferico Una struttura cinematica a 6 gradi di mobilità ha soluzione analitica alla cinematica inversa se: • Tre assi di giunti adiacenti s’incontrano in un punto, come avviene appunto nel caso di polso sferico; • Tre assi di giunti rotoidali adiacenti sono paralleli. il primo passo è quello di individuare un punto della struttura in grado di disaccoppiare posizione e orientamento, per un manipolatore con polso sferico la scelta naturale del punto W è in coincidenza con il punto d’intersezione dei tre assi di giunto. una volta assegnati posizione ed orientamento della terna utensile in termini di p e di R = [n, s, a], la posizione del centro del polso è individuata dalla relazione W = p – d6a. Nel caso di strutture portanti a tre gradi di mobilità (non ridondante), la cinematica inversa può essere risolta secondo i seguenti passi: Calcolare la posizione del polso W(q1, q2, q3) secondo la relazione W = p – d6a. Risolvere la cinematica inversa per (q1, q2, q3). Calcolare R30 (q1 , q2 , q3 ) Calcolare R63 (ϑ1 , ϑ2 , ϑ3 ) = R30 R , essendo il polso sferico Risolvere la cinematica inversa per l’orientamento (ϑ1, ϑ2, ϑ3).