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