EQUAZIONI CINEMATICHE PER MANIPOLATORI qn n i

qi

(n )

i-1

q2
2
1

Il manipolatore si schematizza come un sistema multicorpo seriale a catena cinematica aperta con membri rigidi connessi da accoppiamenti elementari

q1 0 (0)

prismatici rotoidali

Procedura ogni terna (i) solidale al membro i

base (0) organo terminale (n)

ogni giunto i collega i membri i-1 e i la variabile di giunto qi indica la posizione relativa di i rispetto a i-1 prismatico lunghezza dipende dallaccoppiamento elementare rotoidale angolo 1

Equazioni cinematiche per la posizione

(n )

qn n i

qi

i-1

q2
2
1

q1 0 (0)

Le equazioni cinematiche per la posizione si scrivono tenendo conto che la posizione relativa tra due membri i j ij tra i membri i k , note le posizioni tra i j e j k, ik = ijjk tra lorgano terminale n e la base (0)

M 0 n = M 01M12 LM i 1,i M i ,i +1 LM n 2, n 1M n 1, n
definire una procedura conveniente per scrivere i-1,i (qi )
2

Equazioni cinematiche per velocit e accelerazione Le equazioni cinematiche per la velocit e accelerazione si ricavano considerando q2 q3 q1 3 corpi 1, 2 e 3 un sistema di riferimento fisso (0) Nota la cinematica di 1 (0) , 2 1 e 3 2 si scrive
(0 ) (0 ) (0 ) W13 =W12 + W23 (0 ) (0 ) (0 ) (0 ) W = W + W + W (0 ) 03 01 12 23 (0 ) (0 ) W03 =W01 + W13

Velocit

W (0 ) =
0n

(0 ) W i 1,i
i =1

Accelerazione
0) (0 ) (0 ) (0 ) (0 ) H13 = H12 +2W12 W23 + H (23 (0 ) (0 ) (0 ) (0 ) (0 ) H 03 = H 01 +2W01 W13 + H13 W ( 0 ) =W ( 0 ) + W ( 0 ) 12 23 13

(0 ) (0 ) (0 ) (0 ) (0 ) (0 ) (0 ) (0 ) (0 ) H 03 = H 01 +2W01 W12 + W23 + H12 + 2W12 W23 + H 23 0) (0 ) 0) (0 ) (0 ) (0 ) (0 ) (0 ) = H (01 + H12 + H (23 + 2 W01 + W12 W12 + W01 W23

) (

0) (0 ) + 2 H( = H 0n i 1,i i =1

j = 2 k =1

) Wk(0)1,k W (j0 1, j
3

j 1

(n )

qn n i

qi

4
2

i-1

q2
2
1

q1 0 (0)

Le equazioni cinematiche per la velocit e laccelerazione possono assumere una diversa forma usando le le relazioni W (i ) = L(i )q &,
2 2 && + L(i ) q & H (i ) =L(i )q

n (0 ) (0 ) W0 n = Wi 1,i i =1 n ( ) (0 ) q 0 W &i L = n i i 0 1 , i =1

n n j 1 (0 ) (0 ) (0 ) (0 ) H 0n = H i 1,i + 2 Wk 1, k W j 1, j i =1 j = 2 k =1 n n j 1 2 2 ( ) 0 ( ) ( ) (0 ) L(0 ) q 0 0 H & & & &k q &j = + L q L q 2 L + i i n i i i i j j 0 1 , 1 , 1 , k k 1 , i =1 j = 2 k =1

4

Posizionamento delle terne

qi-1
Consideriamo i membri contigui i-1 i

qi i

qi+1
i+1

i-1

La matrice Mi-1,i dipende dalla posizione reciproca delle terne (i-1) (i) e quindi dipende dalla geometria dei membri dal movimento relativo tra i membri i i-1 e quindi dalla coordinata del giunto qi che definisce la posizione relativa dei membri i i-1 Un posizionamento adeguato produce una semplificazione di Mi-1,i Conviene quindi stabilire delle regole di posizionamento delle terne

1o asse (Zi) coincidente con lasse del giunto i+1 tra i membri i i+1 2o asse (Xi o Yi) normale allasse Zi-1 della terna (i-1) 3o asse (Yi o Xi) terna destra 5

CONVENZIONE DI DENAVIT E HARTENBERG

Definisce il posizionamento standard delle terne solidali ai membri di un manipolatore seriale. Si considerano giunti elicoidali. Posizionamento della terna (i ) La terna (i) solidale al membro i si posiziona secondo le seguenti regole asse Zi coincidente lasse del giunto i+1

Joint i

Joint i+1

asse Xi lungo la normale comune agli assi Zi-1 e Zi dal giunto i a i+1 se Zi-1 e Zi sono sghembi Xi lungo la normale comune a Zi-1 e Zi

r r se Zi-1 e Zi si intersecano normale al piano di Zi-1 e Zi ki 1 ki se Zi-1 e Zi sono paralleli se possibile conviene Xi parallelo a Xi -1 se il giunto i prismatico Zi-1 parallelo al membro i e interseca Zi in Oi
origine Oi intersezione tra Zi e la normale comune tra gli assi Zi-1 e Zi asse Yi terna destra
6

Parametri di D&H
Joint i

Il posizionamento delle terne secondo la convenzione esposta definisce dei parametri che costituiscono la descrizione geometrica del robot

Joint i+1

di , offset, distanza dellorigine di (i-1) Xi, valutata lungo Zi-1

i , rotazione, angolo tra Xi-1 Xi, valutata intorno a Zi-1

ai , lunghezza del braccio i, distanza origine di (i) Zi-1, valutata lungo Xi

i , torsione del braccio i , angolo tra Zi-1 Zi, valutata intorno a Xi

I parametri di e i forniscono la descrizione geometrica del giunto i, cio di come i membri i-1 i sono connessi

I parametri ai e i forniscono la descrizione geometrica del braccio i

Giunti elicoidali, prismatici e rotoidali

I parametri di e i fornendo la descrizione del giunto i dipendono dalla posizione del giunto. I parametri ai e i fornendo la descrizione del braccio i sono costanti. Per il giunto elicoidale i

Joint i

la rotazione modifica i la traslazione modifica di La variabile qi di giunto unica e indica lampiezza del movimento relazioni tra la coordinata del giunto qi e i e di
i Joint i+1

i = rqi + 0i d i = pi qi + d 0i

0i valore della rotazione per qi = 0 d0i valore delloffset per qi= 0

stabiliscono la posizione del giunto per qi = 0
8

Joint i

4
2

di distanza O(i-1) Xi, lungo Zi-1 i angolo tra Xi-1 Xi, intorno Zi-1 ai distanza O(i) Zi-1, lungo Xi i angolo tra Zi-1 Zi, intorno Xi

Joint i+1

Per il giunto rotoidale

Per il giunto prismatico

i variabile, di costante di variabile, i costante

giunto elicoidale prismatico rotoidale p pi 1 0 r 1 0 1
9

In generale

i = ri qi + 0i di = pi qi + d 0i

Posizionamento delle terne estreme

Joint i

terna (0) origine coincidente con lintersezione tra lasse Z0 e la normale comune agli assi Z0 e Z1, con questa scelta d1 sempre nullo; X0 e Y0 arbitrari terna (n) origine coincidente con
i Joint i+1

(n-1) e Zn parallelo a Zn-1

rototraslazioni costanti possono essere aggiunte prima della terna (0) e dopo la terna (n) nei casi particolari in cui le regole precedenti lasciano delle indeterminazioni si possono effettuare delle scelte a favore della semplificazione
10

Posizionamento della terna utensile

Ou
Anche se non fa esplicitamente parte della convenzione di Denavit e Hartenberg, si riporta una convenzione esistente per la collocazione della terna utensile (u)

Xu Yu

(u)

Zu

la terna utensile (u) si sceglie in relazione al tipo di organo terminale per un organo di presa tipo pinza

Ou centro pinza Zu direzione di approccio Yu direzione di scivolamento Xu normale a Zu e Yu per formare una terna destra

ci comporta che in genere la terna (n) (u) si introduce unulteriore matrice di posizione Mn,u che per costante
11

Matrice di posizione Mi -1,i

Joint i

di distanza O(i-1) Xi, lungo Zi-1 i angolo tra Xi-1 Xi, intorno Zi-1 ai distanza O(i) Zi-1, lungo Xi i angolo tra Zi-1 Zi, intorno Xi

Joint i+1

esprime la posizione della terna (i) rispetto alla terna (i-1) e quindi del braccio i rispetto il braccio i-1 pu essere pensata come derivante da 4 rototraslazioni successive (i 1) (i 1) (i 1) (i 1) (i ) dalla relazione Q (i 1) = Q (i ) = M i 1,i e dalla regola di composizione di Q(i ) secondo una terna corrente si ha
1 ) M i 1,i = Q (zi 1) (0, di ) Q (zi1 ) ( i ,0 ) Q (xi1 ) (0, ai ) Q (xi ( i ,0 )
12

1 ) M i 1,i = Q (zi 1) (0, di ) Q (zi1 ) ( i ,0 ) Q (xi1 ) (0, ai ) Q (xi ( i ,0 )

8
2

Ponendo

si = sini , ci = cosi e si = sini, ci = cos i si scrive

0 1 0 0 0 0 0 0 1 di 0 1 R 2 i 1 ) ( Q z (i , 0 ) = 0 ci si T2 = 1 0 0 si ci 0 0 0 0 1 0 0 0 0 1 0 0 0 1

1 0 R T 1 1 i 1) Q( ( 0, di ) = 0 1 = z 0 0 1 T3 0 = 1 0 0

Q x

( i 1 ) 0, a = R 3 ( i)
0

0 0 ai 1 0 0 0 1 0 0 0 1

Q x

( i 1) , 0 = R 4 ( i )
0

1 0 T4 0 c i = 1 0 s i 0 0

0 s i c i 0

Eseguendo i prodotti si ha
I T1 R 2 0 M i 1, i = 0 1 1 0 R 2 R 4 R 2 T3 + T1 = 1 0 I 0 T3 R 4 0 1 0 R 2 = 1 0 T1 1 R 4 0 T3 = 1

13

Esplicitando le sottomatrici
R R M i 1, i = 2 4 0 R 2 T3 + T1 1

c i s i 0 s c R = 0 i i 2 0 1 0 0 1 0 R = 0 c s i i 4 0 s i c i

0 T1 = 0 d i ai T3 = 0 0

si ottiene la matrice di posizione per due terne consecutive

M i 1,i

c i s = i 0 0

s i c i c i c i s i 0

s i s i c i s i c i 0

ai c i ai s i di 1

la matrice Mi-1,i dipende da 4 grandezze di , i , ai i, i mentre la posizione relativa di 2 corpi nello spazio dipende da 6 gradi di libert la scelta della terna (i) rispetto alla terna (i-1) vincolata dalla convenzione di D&H la convenzione di D&H descrive geometricamente un robot con una tabella dei valori di di , i , ai , i

14

D & H PER IL MANIPOLATORE DI STANFORD

di distanza O(i-1) Xi, lungo Zi-1 i angolo tra Xi-1 Xi, intorno Zi-1 ai distanza O(i) Zi-1, lungo Xi i angolo tra Zi-1 Zi, intorno Xi
c i s = i 0 0 s i c i c i c i s i 0 s i s i c i s i c i 0 ai c i ai s i di 1

6
5

4
3

M i 1,i

2 1

Manipolatore con 6 gradi di libert e costituito da struttura portante sferica polso sferico

2 coppie rotoidali 1 coppia prismatica 3 coppie rotoidali

15

Posizionamento delle Terne 16

ci si ci si si ai ci s c c c s a s i i i i i i Mi1,i = i 0 si ci di 0 0 0 1
Z0

di , O(i-1) Xi, lungo Zi-1 i , Xi-1 Xi, intorno Zi-1 ai , O(i) Zi-1, lungo Xi i , Zi-1 Zi, intorno Xi

6
5

4
3 2 1

terna 0
X0 O0

Y0

gli altri parametri non sono definiti

Z0 coincidente asse rotoidale tra 0 e 1 X0 arbitrario O0 O1

terna 1
O0 O1 X0 X1

Z0 Z1 Y0 Y1

Z1 coincidente asse rotoidale tra 1 e 2 X1 normale al piano di Z0 e Z1 c1 0 s1 0 s O1 intersezione tra Z0 e Z1 0 0 c 1 M 01 = 1 d1 = 0, 1 = variabile q1 0 1 0 0 a1 = 0 , 1 = -90
0 0 0 1
16

ci si ci si si ai ci s c c c s a s i i i i i i Mi1,i = i 0 si ci di 0 0 0 1

di , O(i-1) Xi, lungo Zi-1 i , Xi-1 Xi, intorno Zi-1 ai , O(i) Zi-1, lungo Xi i , Zi-1 Zi, intorno Xi
5

6 4

Z2 d2

3 O2 2 1

terna 2

O1 X1 Y1

Z1 Y2 X2

Z2 coincidente asse prismatico tra 2 e 3 X2 normale al piano di Z1 e Z2 O2 intersezione tra Z1 e Z2 d2 = d2, 2 = variabile q2 a2 = 0, 2 = 90

M12

c 2 s = 2 0 0

s 2

0 c 2 1 0 0 0

0 0 d2 1
17

4
3

ci si ci si si ai ci s c c c s a s i i i i i i Mi1,i = i 0 si ci di 0 0 0 1

di , O(i-1) Xi, lungo Zi-1 i , Xi-1 Xi, intorno Zi-1 ai , O(i) Zi-1, lungo Xi i , Zi-1 Zi, intorno Xi

6
5

4
3

terna 3
X3 d3

Z3 Y3 O3 Z2 Y2 X2 O2

M 23

1 0 = 0 0

0 1 0 0 0 1 d3 0 0 1 0 0

2 1

Z3 coincidente asse rotoidale tra 3 e 4 X3 parallelo a X2 O3 posizione di riferimento per la traslazione d3 = variabile q3 , 3 = 0 a3 = 0 , 3 = 0

18

ci si ci si si ai ci s c c c s a s i i i i i i Mi1,i = i 0 si ci di 0 0 0 1

di , O(i-1) Xi, lungo Zi-1 i , Xi-1 Xi, intorno Zi-1 ai , O(i) Zi-1, lungo Xi i , Zi-1 Zi, intorno Xi

6
5

4
3 2 1

Z3

terna 4

O3 O4 X3 X4 Y4

Y3 Z4

Z4 coincidente asse rotoidale tra 4 e 5 X4 normale al piano di Z3 e Z4 O4 intersezione tra Z3 e Z4 d4 = 0, 4 = variabile q4 a4 = 0 , 4 = -90

M 34

c 4 s = 4 0 0

0 0 1 0

s 4 c 4 0 0

0 0 0 1
19

ci si ci si si ai ci s c c c s a s i i i i i i Mi1,i = i 0 si ci di 0 0 0 1

di , O(i-1) Xi, lungo Zi-1 i , Xi-1 Xi, intorno Zi-1 ai , O(i) Zi-1, lungo Xi i , Zi-1 Zi, intorno Xi
5

6 4

Z5

3 Y5 Z4 Y4 1 2

terna 5
X4

O4 O5 X5

Z5 coincidente asse rotoidale tra 5 e 6 X5 normale al piano di Z4 e Z5 O5 intersezione tra Z4 e Z5 d5 = 0, 5 = variabile q5 a5 = 0 , 5 = 90

M 45

c 5 s = 5 0 0

0 s 5 0 c 5 1 0 0 0

0 0 0 1
20

M i 1,i

c i s = i 0 0

s i c i c i c i s i 0

s i s i c i s i c i 0

ai c i ai s i di 1

7
6

6
5

4
3

Z5

terna 6

Z6 O5 O6 Y6 Y5

X5

X6

Z6 parallelo a Z5 O6 coincidente con O5 X6 parallelo a X5 d6 = 0 , 6 = variabile q6 a6 = 0 , 6 = 0

M 56

Ponendo si = sin i e ci = cos i

M 06 0 s1 0 c2 c1 s s c 0 0 1 1 2 = 0 1 0 0 0 0 0 1 0 0 q1 = 1 0 1 0 0 c2 0 1 0 d 2 0 0 0 1 0 0 s2 q2 = 2 0 s4 0 c5 0 c4 s c4 0 0 1 0 0 s4 5 0 0 0 0 1 d3 0 1 0 0 1 0 0 0 1 0 q3 = d 3 q4 = 4 0 0

c 6 s = 6 0 0
0 s5

s 6 c 6 0 0

0 0 0 0 1 0 0 1

0 c6 s6 0 0 s 0 0 c 0 c5 0 6 6 0 1 0 1 0 0 0 0 0 1 0 0 1 0 q5 = 5 q6 = 6 21

SPOSTAMENTO INFINITESIMO DELLEFFETTORE Per esprimere il legame tra gli spostamenti infinitesimi dei giunti e dellorgano terminale utile ricordare che la matrice M0k descrive la posizione del membro k rispetto al riferimento (0) M 0k (q1,L qk ) = M 01(q1 ) M12 (q2 ) L M i 2,i 1 (qi 1 ) M i 1,i (qi ) M i ,i +1 (qi +1 ) L M k 1, k (qk ) e quindi M0k dipende dalle variabili ai giunti generica solo se i k ricordando 9 lespressione dP( 0 ) = L( 0 ) P( 0 )dq dello spostamento infinitesimo 9 che M0n contiene i punti notevoli della terna utensile rispetto a (0) 9 che le rototraslazioni infinitesime successive si sommano 9 si pu dire che lo spostamento infinitesimo dellorgano terminale la somma delle rototraslazioni infinitesime dei giunti

q1 qk , cio dipende dalla qi

dM 0 n

n (0 ) = L i 1,i dqi M 0n i =1

22

PROBLEMA CINEMATICO DIRETTO E INVERSO Per mostrare limpostazione e i metodi di soluzione dei vari problemi cinematici opportuno stabilire alcune posizioni

Per semplicit consideriamo il moto del solo organo terminale Adottiamo una rappresentazione in forma minima della posa dellorgano terminale La terna utensile (u) si sceglie in base al tipo di organo terminale Per esempio per un organo di presa tipo pinza si sceglie

Ou centro pinza Zu direzione di approccio Yu direzione di scivolamento

Ou Xu Yu (u) Zu

Xu normale a Zu e Yu per formare una terna destra

In genere la terna (n) (u) si introduce quindi unulteriore matrice di posizione Mn,u costante
23

Rappresentazione nello spazio esterno Le grandezze di interesse nel sistema cartesiano esterno si scrivono

Coordinate del centro pinza Ou = x p

T Angoli di Eulero Z X Z {1 2 3 }

yp

zp

}T
s1s2 c1s2 c2

matrice di rotazione

c1c3 s1c2 s3 RE = s1c3 + c1c2 s3 s2 s3

c1s3 s1c2c3 s1s3 + c1c2c3 s2c3

yp zp

Coordinate esterne S = 1 2 3 Matrice di posizione M0n (S )

xp

(0 ) S,S & =M & Matrice delle velocit W0 0n n

( )

( )

xp R y p E M 0n (S ) = zp 0 0 0 1 1 & S,S M 0n (S )

}T

0) & && && & && 1 Matrice delle accelerazioni H( 0n S, S, S = M0n S, S, S M0n (S )
24

Rappresentazione nello spazio dei giunti

Le grandezze di interesse nello spazio dei giunti si scrivono

Coordinate dei giunti Q g = { q1 Matrice di posizione M 0n Q g Matrice della velocit Matrice della accelerazione

q2 L qn

}T

( )
( )

M 0 n Q g = M i 1,i ( qi )

( )

( 0) & =M & & W0n Q g , Q g 0n Q g , Q g

i =1

1 M0 n Qg

( )

(0) & , Q && = M & && && H 0n Q g , Q 0n Q g , Q g , Q g g g

1 M0 n Qg

( )
25

Cinematica diretta

Consideriamo solamente il moto del organo terminale

Consiste nel calcolo del moto dellorgano terminale a partire dalle coordinate dei giunti

M0n ( S ) = M0n Q g ( 0) & = W( 0) Q , Q & velocit W0n S, S g g 0n

posizione accelerazione

( ) ( ) ( ) ( 0) &, S && ) = H( 0) ( Q , Q & ,Q && ) H0n ( S, S g g g 0n

T

Si indichi con

Q g = { q1 q2 L qn } ,

S = {s1 s2 L s6 }

26

5
2

Per la posizione si ha

Incognita

Termini noti

M0n(S)

Qg

Risulta completamente determinato una volta definite le matrici di posizione Mi-1,i(qi) tra i membri contigui i -1 i Si risolve con procedure completamente automatiche
n

M 0n Q g = M 01 ( q1 ) L M i 1,i ( qi ) L M n 1, n ( qn ) = M i 1,i ( qi )
i =1

( )

M 0n ( S ) = M 0n Q g

( )

consiste in 12 equazioni scalari lineari nelle si 6 indipendenti

27

Per la velocit

Incognita

Termini noti

6
3

(0 ) S,S & W0 n

( )

dQg /dt

Risulta completamente determinato una volta definite le matrici dellasse elicoidale Li-1,i tra i membri contigui i -1 i rispetto a una terna locale ricordando
Li 1,i = M 0,i 1 ( q1, q2 ,L , qi 1 ) Li 1,i M i 1,0 ( q1, q2 ,L , qi 1 )

( 0)

( i 1)

si ha

(0) (0) Q , Q (0) Q & )= & &i W0n ( Q g , Q W = L i 1,i ( g g ) i 1,i ( g ) q g

i =1 i =1

(0) & = W ( 0 ) Q, Q & W0 n S, S 0n

)
28

consiste in 12 equazioni scalari lineari nelle dsi /dt 6 indipendenti

Per laccelerazione si ha

Incognita
0) & && H( 0 n S,S, S

Termini noti

d 2Qg /d t 2

(0) (0) + 2 & , Q && ) = H 0n (Q g , Q H i 1,i g g

i =1

j = 2 k =1

j 1

Wk 1, k W j 1, j =

(0)

(0)

= i 1,i Q g i =1 +2
n j = 2 k =1

(0) L

( )
(0)

0) 2 ( &&i + L i 1,i ( Q g ) q &i2 + q

j 1

L k 1, k Q g

( )

L j 1, j Q g

(0)

( )

&k q &j q

H 0n

( 0 ) S, S && ( &, S

(0) & , Q && = H 0n Q g , Q g g

consiste in 12 equazioni scalari lineari nelle d 2si /dt 2 6 indipendenti

29

Cinematica inversa

Consideriamo solamente il moto del organo terminale

&i , q &&i calcolo delle coordinate dei giunti e delle loro derivate qi , q (0 ) S, S & , H (0 ) S, S &,S && noto il moto dellorgano terminale M 0 n (S ), W0 n 0n Per la posizione in genere si risolve con procedure laboriose

( )

Per la soluzione importante la scelta delle coordinate esterne impiegate per descrivere la posa dellorgano terminale
il numero di elementi di

S pari al numero dei GdL della pinza

la posizione definita da quella di un punto di riferimento, per esempio

il centro pinza
lorientamento offre pi possibilit di scelte, ma nessuna soddisfacente

le rappresentazioni minime hanno delle singolarit la matrice di rotazione necessita di vincoli matematici le derivate non corrispondono alle velocit angolari le scelte ottimali sono diverse per la progettazione e per il controllo
30

Incognite gradi di mobilit

Equazioni GdL

9
2

Per la posizione si ha
Incognita

M 0n Q g = M 0n ( S )
Termini noti

( )

Qg

M0n(S)

GdL equazioni non lineari nelle n incognite qi (0) Q , Q ( 0 ) S, S & &) W = W Per la velocit si ha g) 0n ( g 0n (
Incognita Termini noti

dQg /dt

(0 ) S,S & W0 n

( )

GdL equazioni lineari nelle n incognite dqi /dt (0 ) & , Q && ) = H ( 0 ) ( S , S &, S && ) Per laccelerazione si ha H 0n (Q g , Q g g 0n
Incognita Termini noti
0) & && H( 0 n S,S, S

d 2Qg /dt 2

)
31

GDL equazioni lineari nelle n incognite d 2qi /dt 2

ROBOT SCARA CON 3 GRADI DI MOBILIT

Z1 Z0 1 a h 0 Z2 X2 Y1 Z0 X0 Y0 X3 Z3 Y3 Y2 l Z2 2 b 3

Problema diretto

Z1 X1

di , O(i-1) Xi, lungo Zi-1 i , Xi-1 Xi, intorno Zi-1 ai , O(i) Zi-1, lungo Xi i , Zi-1 Zi, intorno Xi d1 = h, 1 = variabile, a1 = a, 1 = 0 d2 = 0, 2 = variabile, a2 = b, 2 = 0 d3 = variabile, 3 = 0, a3 = 0, 3 = 0

32

Z1 X1

Z2 X2 Y1 Z3 Y3 Y2 l

M i 1,i

c i s = i 0 0

s i c i c i c i s i 0

s i s i c i s i c i 0

ai c i ai s i di 1

2
2

Z0 X0 Y0

X3
si = sin i ci = cos i

d1 = h, 1 = variabile, a1 = a, 1 = 0 d2 = 0, 2 = variabile, a2 = b, 2 = 0 d3 = variabile, 3 = 0, a3 = 0, 3 = 0

s 2 c 2 0 0 0 0 1 0 b c 2 1 0 b s 2 M 23 = 0 0 1 0 0 1 0 0 0 0 1 0 0 0 l 1

M 01

c1 s = 1 0 0

s 1 c 1 0 0

0 0 1 0

a c1 c 2 s a s 1 M 12 = 2 0 h 1 0 0 0 1 0

M 03

c1c2 s1 s2 c s + c s = 2 1 1 2 0 0

c2 s1 c1 s2 c1c2 s1 s2 0 0

b (c1c2 s1 s2 ) + a c1 b (c2 s1 + c1 s2 ) + a s1 hl 1

x p = b(c1c2 s1s2 ) + ac1 y p = b(c2 s1 + c1s2 ) + a s1 z = h l p

33

Problema inverso

2 1
O
c1c2 s1s2 c s + c s = 2 1 1 2 0 0

calcolo di 1 , 2 e l nota la matrice M03(Q) la pinza ha 3 GDL quindi le 4 coordinate x p , y p , z p , non sono indipendenti

nella matrice M03 si ha m11= m22 e m21= - m12

0 b (c1c2 s1s2 ) + a c1 A B B A 0 b (c2 s1 + c1s2 ) + a s1 = 0 hl 1 0 0 1 0 0 0 0 1 0 xp yp zp 1

M 03

c2 s1 c1s2 c1c2 s1s2 0 0

Si hanno 5 relazioni significative

9 9

La 5a identifica da sola la coordinata l Dalle prime 2, non indipendenti, e dalla struttura di M03 che definisce una rotazione intorno a Z si ha
A = c1 c 2 s1 s 2 = cos ( 1 + 2 ) B = c 2 s1 + c1 s 2 = sin ( 1 + 2 )

A = c1c2 s1s2 = cos(1 + 2 ) B = c s + c s = sin ( + ) 2 1 1 2 1 2 x p = b(c1c2 s1s2 ) + ac1 y = b(c s + c s ) + as 2 1 1 2 1 p l = h zp z p = h l A = cos = 1 + 2 B = sin 34

4
2

Restano 3 relazioni, per sole 2 coordinate indipendenti

x p = b (c1c2 s1s2 ) + a c1 y p = b (c2 s1 + c1s2 ) + a s1 = + 1 2

Infatti se sono dati xp , yp risulta fissato lorientamento della mano

2 si ottiene quadrando e sommando xp , yp

2 2 2 2 2 x p = b cos (1 + 2 ) + a cos 1 + 2ab cos(1 + 2 )cos1 2 2 2 2 2 ( ) y = b sin + + a sen 1 + 2ab sin (1 + 2 )sin 1 1 2 p

2 2 2 x2 p + y p = b + a + 2ab[cos(1 + 2 )cos1 + sin (1 + 2 )sin 1 ] = 2 2 = b 2 + a 2 + 2ab c1 c2 s1s2c1 + s1 c2 + s2c1s1 = b 2 + a 2 + 2ab cos 2

2 = arccos

2 2 2 + x2 y a b p p

2ab

35

1 si ottiene da semplici considerazioni geometriche

y p x p = tan = tan 1 y p x p b sin 2 1 = tan a + b cos 2
1

5
3

Y yp

a b 2 b sin 2 b cos 2

1
9

xp

Risulta quindi

+ a b 2 = arccos 2ab 1 = atan2 y p ,x p atan2( b sin 2 , a+b cos 2 ) x2 p y2 p

2 2

Le relazioni precedenti insieme allequazione ripetuta a fianco confermano, che una volta stabiliti xp e yp , resta fissato lorientamento della pinza

= 1 + 2
36

ROBOT SFERICO CON 3 GRADI DI MOBILIT

Soluzione del problema cinematico diretto e inverso per la posizione per una struttura portante sferica
3 gradi di mobilit 2 accoppiamenti rotoidali con assi normali tra loro 1 accoppiamento prismatico con asse normale allasse del giunto

rotoidale precedente

37

X1 a

1 Z 0

2 1

b 2

Y2
l

c 3

Per la soluzione del problema cinematico diretto e inverso per la posizione assumiamo posizionamento arbitrario delle terne coordinate dei giunti Q 1 , 2 e l coordinate esterne S M03
X1 Z1 Y1
2

Z2 Y2
l

Z3 Y3 X3

Z0 X0

X2

1 Y0

38

Problema diretto Calcolo della matrice M03 note le coordinate dei giunti 1 , 2 e l
X1 Z1 Y1
2

Z2 Y2
l

Z3 Y3 X3

Z0

X2

Ponendo
c1 s = 1 0 0 c1 s = 1 0 0

s1 c1 0 0 s1c 2 c1c 2 s2 0 0 0 1 0

si = sin i ci = cos i 0 0 a 1 s1 s2 c1 s2 c2 0 1 0 = 0 0 0 c2 s2 0

1 Y0

X0

M 01

M 12

0 s2 c2 0

0 b c2 b s2 1

M 23

1 0 = 0 0

0 1 0 0

0 0 1 0

0 l + c 0 1

M 03

s1c 2 (l + c ) b s1 c 2 c1c 2 (l + c ) + b c1 c 2 s2 (l + c ) + b s2 + a 1

x p = s1c2 (l + c ) b s1c2 y p = c1c2 (l + c ) + b c1c2 z = s (l + c ) + b s + a 2 p 2

39

Problema inverso Calcolo delle coordinate dei giunti

Z1 Y1 X1
2

Z2 Y2
l

1 , 2 e l nota la matrice M03(S)

La pinza ha 3 GDL e quindi la posa si descrive con le 3 coordinate xp , yp , zp

Z3 Y3 X3

Z0 X0

X2

necessario scrivere 3 equazioni

indipendenti
m11 m M 03 (S ) = 21 m31 0 m12 m22 m32 0 m13 m23 m33 0

1 Y0

xp s1s2 s1c2 (l + c ) b s1c2 c1 s1c2 s ( ) + + yp c c c s c c l c b c c 1 2 1 2 1 2 1 2 = M 03 (Q ) = 1 0 zp s2 c2 s2 (l + c ) + b s2 + a 1 0 0 0 1

Consideriamo le coordinate della pinza

xp , yp , zp

2 x2 ( ) [ ] s c l c b = + + x p = s1c2 (l + c + b ) x p = s1c2 (l + c ) b s1c2 1 2 p 2 2 ( ) ( ) y c c l c b c c y c c l c b = + + = + + y p = [c1c2 (l + c + b )] p 12 p 12 1 2 z a 2 = [s (l + c + b )]2 z a = s (l + c + b ) z = s (l + c ) + b s + a 2 2 2 2 p p 40 p

Sommando le precedenti espressioni al quadrato si ha

2 x2 + y p p + zp a

)2 = s12 [c2 (l + c + b)]2 + c12 [c2 (l + c + b)]2 + [s2 (l + c + b)]2 =

2 2 [(l + c + b )]2 + s2 [(l + c + b )]2 = [(l + c + b )]2 = c2

Per l esistono 2 soluzioni (l1 e l2)

1,2

2 l = x2 + y p p + zp a

cb

Per ciascun valore di li si trovano 2 valori di 2 e quindi complessivamente

4 valori

zp a zp a = z p a = s2 (l + c + b ) 2 = arcsin arcsin l +c+b x2 + y2 p p + zp a

)2

= 2 2 che fornisce le 4 soluzioni 1l 2 = 2 , 2l 2 = 2 2 + 2

Per 1 si hanno 2 valori

yp xp 1 = atan2 c (l + c + b) , c (l + c + b) 2 2

41

Il problema cinematico inverso per la posizione fornisce 4 soluzioni Assegnati xp , yp e zp 4 insiemi di 1, 2 e l soddisfano la posa, cio esistono 4 posture del robot che soddisfano la posa della pinza Questo risultato pu essere visto geometricamente

6
3

2 1
0

1, + , 1

2 , 2 ,

1 1

2 l = x2 + y p p + zp a

( (

) )

2 2

cb cb

l=

x2 p

y2 p

+ zp a

2 1
0

1, 1 + ,

2 + ,
2 ,

2 2

l = 1l 2 ( c + b ) l = 1l 2 ( c + b )
42

CINEMATICA INVERSA PER LA POSIZIONE: SOLUZIONE ANALITICA La soluzione del problema cinematico inverso per la posizione richiede operazioni non standardizzate e a volte complesse e non sempre pu essere risolto analiticamente. utile quindi fornire ulteriori nozioni. conveniente, se possibile, la soluzione analitica calcolo pi veloce delle qi se si dispone di equazioni determinazione del numero di soluzioni Il metodo di soluzione del problema inverso per la posizione va ricercato caso per caso Per i manipolatori che ammettono una soluzione analitica opportuno stabilire delle procedure il pi possibile automatiche La ricerca della soluzione in forma analitica pu essere facilitata da alcune metodologie
43

Metodo del Grafo di Paul una procedura che aiuta a scrivere le equazioni necessarie alla soluzione Consideriamo un manipolatore con 6 gradi di mobilit e 6 GdL
Rappresentiamo la catena cinematica di un manipolatore con un grafo Tagliamo il grafo per esempio con una linea passante per i membri 2 e 6

M12

M23

3 M 34

M45

M01
0

M06

M56
6

Si pu scrivere percorrendo da 2 6 e da 6 2 seguendo i 2 percorsi possibili

1 1 M 23 M 34 M 45 M 56 = M12 M 01 M 06 1 1 1 1 1 M 45 M 34 M 23 = M 06 M 01 M12 M 56

Per un manipolatore con n gradi di mobilit n (n -1) /2 tagli diversi Si hanno n(n -1) relazioni matriciali equivalenti con forma diversa tra le quali si possono cercare le relazioni scalari pi convenienti
44

Metodo della Scissione della Catena Cinematica

Deriva dal metodo di Paul e ne costituisce unevoluzione

Consiste nel fare tagli convenienti al grafo di Paul Si estrae un membro che abbia una struttura opportuna: ad esempio supponiamo che il membro 4 abbia due accoppiamenti rotoidali con assi paralleli

q4
q3
4

q5 q6
5 6

q4 u4 Z3
4

q5 u5 Z4

q2 q1
0
2
1

1 1 1 1 1 Si ottiene M 34 = M M M M M M 23 12 01 06 56 45 che sinteticamente e

indicando le dipendenze funzionali si scrive

1 (q5 ) M 34 (q4 ) = M (q3 , q2 , q1 , S, q6 )M 45
45

La sottomatrice di rotazione di M34 R34 contiene quattro 0 e un 1, se si adottata per esempio la convenzione di Denavit-Hartenberg per la collocazione delle terne si ha, essendo i = q4 e i = 4 = 0
c i s = i 0 0 s i c i

M i 1,i

c i c i s i 0

s i s i c i s i c i 0

ai c i ai s i di 1

R 34

cq4 = sq4 0

sq4 cq4 0

0 0 1

T 1 La sottomatrice di rotazione di M R 45 45 , essendo i = q5 e i = 5 , ha in generale lespressione

M i 1,i

c i s = i 0 0

s i c i c i c i s i 0

s i s i c i s i c i 0

ai c i ai s i di 1

RT 45

cq5 = sq5c 5 sq5 s 5

sq5 cq5 c 5 cq5 s 5

0 s 5 c 5
46

 Considerando solo le sottomatrici di rotazione della relazione

1 M 34 (q4 ) = M (q1, q2 , q3 , q6 , S )M 45 (q5 )

5
3

si ha
cq4 sq 4 0

T 1 ( ) R 34 (q4 ) = R (q1, q2 , q3 , q6 , S ) R T q R = R essendo 45 5

sq4 cq4 0

0 r11 r 0 = 21 1 r31

r12 r22 r32

r13 cq5 sq c r23 5 5 r33 sq5 s 5

sq5 cq5 c 5 cq5 s 5

0 s 5 c 5

Dalla relazione matriciale precedente si ricavano le relazioni scalari, non tutte indipendenti, in cui non compaiono le variabili di giunto q4 e q5

0 = r12 (q1, q2 , q3 , q6 , S ) s 5 + r13 (q1, q2 , q3 , q6 , S ) c 5 0 = r22 (q1, q2 , q3 , q6 , S ) s 5 + r23 (q1, q2 , q3 , q6 , S ) c 5 1 = r (q , q , q , q , S ) s + r (q , q , q , q , S ) c 32 1 2 3 6 5 33 1 2 3 6 5

Altre relazioni dello stesso tipo si ottengono dalla combinazione di altre relazioni scalari ottenibili dalla relazione matriciale precedente.
47

 In sintesi la relazione matriciale

1 M 34 (q4 ) = M (q1, q2 , q3 , q6 , S )M 45 (q5 )

6
4

in virt di elementi nulli o indipendenti dalle variabili di giunto, consente di ottenere relazioni semplici che non dipendono da q4 e q5 del tipo

1 = f1 [q1, q2 , q3 , q6 , S ] 0 = f 2 [q1, q2 , q3 , q6 , S ]
Ci deriva dal fatto che gli assi dei giunti 4 e 5 restano sempre paralleli. Si pu ricavare la seguente regola: lestrazione del membro i consente di trovare equazioni che non dipendono da qi e qi+1 Questa tecnica pu estendersi nel modo seguente: se due giunti qualsiasi i e j del manipolatore mantengono sempre lo stesso orientamento reciproco, scindere la catena cinematica in corrispondenza di i e j , cio in corrispondenza dei membri i e j-1, permette di trovare equazioni in cui non compaiono le variabili di giunto qi , qi+1 , , qj-1 , qj .

48

CINEMATICA DIFFERENZIALE INVERSA utile richiamare il problema cinematico inverso prima di introdurre il problema cinematico inverso per la velocit e laccelerazione

&i e q &&i per ogni giunto partendo dalla Consiste nel calcolare le qi , q posizione, velocit e accelerazione assolute della pinza espresse nelle
coordinate esterne

&, S && S, S

(0 ) & (0 ) & ,S && in funzione Esprimendo le matrici M 0 n (S ) , W0 n S,S e H 0 n S,S delle matrici di posizione, velocit e accelerazione relative tra i membri &i e q &&i , si ottengono le relazioni funzioni delle qi , q

( )

M 0 n (S ) = M i 1,i (qi ) ,
i =1

(0 ) S, S 0) & = & W0 Wi( 1,i Q, Q = n

i =1

( )

&i ) L(i0)1,i (Q ) q
i =1

n n j 1 (0) 0 ( ) ( 0 ) Q, Q &,S && = & ,Q && + 2 & W ( 0 ) Q, Q & = H 0 n S, S H Q , Q W i 1,i j 1, j k 1, k i =1 j = 2 k =1 n j 1 n (0) 0) 2 ( (0) (0) 2 &&i + Li 1,i ( Q ) q &i + 2 L k 1, k ( Q ) L j 1, j ( Q ) q &k q &j = Li 1,i ( Q ) q i =1 j = 2 k =1

49

 Le matrici che compaiono nelle relazioni precedenti hanno la struttura

xix x iy M i 1,i = x iz 0 0 z 0) ( Wi 1,i = y 0 yix yiy yiz 0 z 0 zix ziy ziz 0 Oy Oz 1
i 1 Ox ( )

0 uz 0) ( Li 1,i = u y 0
0

u z 0 ux 0

uy u x 0 0

t x ( ) ty tz 0 i 1,i
0 0)

x
0

V0 x ( ) x V0 y 0 V0 z 0 0 i 1,i

2 & + 0) ( Hi 1,i = 0 0 0

A0 x ( A0 y A0 z 0

i 1,i

Risolto il problema inverso della posizione Q , i problemi inversi della velocit e dellaccelerazione sono rispettivamente lineari && & e Q rispetto a Q

W H

& Q && Q
50

CINEMATICA INVERSA PER LA VELOCIT Consideriamo per le prossime relazioni un sistema di riferimento generico (k) anzich (0)

&i per ogni giunto partendo dalla posizione e Consiste nel calcolare le q velocit della pinza, una volta che le qi sono state gi determinate n & si ottiene & = W (k ) S , S Dalla relazione L(k ) (Q ) q

i =1

i 1,i

0n

( )

u z1 u y1 t x1 0 u u t 0 x1 y1 z1 u y1 u x1 t z1 0 0 0 0 0 0 u +L+ zn u yn 0 u zn 0 u xn 0 u yn u xn 0 0

(k ) u zi u yi t xi 0 u u t 0 xi yi &1 +L + zi &i + q q u yi u xi t zi 0 0 0 0 0 (k ) (k ) z y V0 x t xn 0 0 t yn V 0 z x y q &n = y x 0 t zn V0 z 0 0 0 0 0 0n

(k )

Le matrici che compaiono nellespressione precedente possono essere riorganizzate in un funzione delle V0 x ,V0 y ,V0 z e x , y , z

51

 Dalla riorganizzazione delle matrici precedenti si ha

tx1 t y1 tz1 ux1 u y1 uz1

t x2

txi

t y2 t yi tz 2 K tzi ux 2 uy2 uz 2 uxi u yi uzi

txn t yn K tzn uxn u yn uzn

(k )

(k ) & q1 V0x q V &2 0y V0z M = &i x q y M &n z q

& = V(k ) S, S & J(k ) (Q) Q

( )

se J(k) invertibile si ottiene

& = J (k ) (Q )1 V (k ) S , S & Q

( )

Unopportuna scelta del riferimento (k) pu semplificare i calcoli. ( k ) , V ( k ) e V ( k ) non hanno un significato specifico terna (k) V 0x 0y 0z

terna ausiliaria (a) cio riferimento con orientamento fisso e origine ( k ) ,V ( k ) V ( k ) V nel centro pinza 0x 0y e 0 z rappresentano la velocit del centro pinza, mentre rimane invariato

52

CINEMATICA INVERSA PER LACCELERAZIONE Anche in questo caso consideriamo per le relazioni un sistema di riferimento generico (k) anzich (0)

&&i per ogni giunto partendo dalla posizione, Consiste nel calcolare le q &i sono state velocit e accelerazione della pinza, una volta che le qi e q gi determinate
Dalla relazione

( k )2 ( ( k) k) & & & &&i = H 0 n S , S , S L i 1,i (Q ) q & i2 2 L i 1,i (Q ) q

i =1 i =1

(k ) ( k )2 (k ) (Q ) L(k ) (Q ) q 2 & & & &k q &j ( ) ( ) + L Q q L Q q + 2 L i 1,i i i 0n j 1, j i 1, i k 1, k i =1 j = 2 k =1 &&i 9 si esplicitano le q H (k ) =

j 1

j = 2 k =1

) (Q ) q &k q &j L(kk)1, k (Q ) L(k j 1, j

j 1

~ (k ) (k ) (Q) q & , S, S & ,S && & & L = H Q, Q 9 Il secondo membro noto, si ha i 1,i i
i =1

9 Lespressione del tutto analoga a quella gi ottenuta per la velocit

(k ) (Q ) q (k ) S , S & & L = W i 1,i i 0n

i =1

( )

53

&&i analoga a quella della velocit, si 2 La relazione nei confronti delle q pu quindi riorganizzare lespressione matriciale in modo da utilizzare i ~ (k ) risultati gi ottenuti h14 ~ h 24 ~ h 1 ~ ~ ~ (k ) ~ ~ (k ) 34 && = A (k ) Q && = J (k )(Q ) A (k ) con A (k ) = J (k )(Q )Q = h H ~ ij h 32 ~ n ~ (k ) h ( k) & , S, S & ,S && ) 13 &&i = H (Q , Q L i 1,i (Q ) q ~ h i =1 21

La rielaborazione delle matrici permette di utilizzare la matrice J(k) gi calcolata; se J(k) invertibile il problema risolto
& = V si ha Derivando lespressione della velocit J Q ~ & & & & & 1 ~ & & J 1V A = V & + JQ && = J & +A & =J &Q &Q JQ = V JJ V = A J A=V V ~ & J 1V A =AJ 9 dalla quale

Se scegliamo come riferimento la terna ausiliaria (a) (con origine nel centro pinza e traslante ) i valori che compaiono nellultima colonna della a) H( 0 n sono le accelerazioni del centro pinza

54

CINEMATICA DIFFERENZIALE PER UN ROBOT SCARA

Z1 Z0 X1 l1

X2 l2 Y1 X3

Z2 Z3 Y2
l

Coordinate dei giunti

& && & & && && Q = , Q = Q = l& & l & l

Y3

X0

Y0
s c 0 0 0 l1c c s 0 l1 s M12 = 0 h 1 0 1 0 s c 0 0 0 l2 c 1 0 0 l 2 s M 23 = 0 1 0 0 1 0

c s M 01 = 0 0

0 1 0 0

0 0 0 0 1 l 0 1

c( + ) s ( + ) s( + ) c( + ) M 03 = 0 0 0 0

0 l2 c( + ) + l1c 0 l2 s( + ) + l1s 1 hl 0 1

55

Z1 Z0 X1 l1 Y1 X3
0

Z2 Z3 l2 Y2

utile per gli sviluppi successivi la determinazione delle matrici L degli assi elicoidali
9 2 giunti rotoidali L01 e L12 9 1 giunto prismatico L23

h X0 Y0

Y3

1) L(ik)1,i = M ki L(ii 1,i M ik

0 1 1 0 0) 1) = L(12 = L(01 0 0 0 0

0 0 0 0 0 0 0 0

0 0 2) = L(23 0 0

0 0 0 0 0 0 1 0 0 0 0 0

utile esprimere tutte le matrici L nello stesso sistema di riferimento (0)

1) 0) = M 01 L(12 L(12 M10

l1 s 0 1 0 1 0 0 l c 1 = 0 0 0 0 0 0 0 0

2) 0) = M 02 L(23 L(23 M 20

0 0 = 0 0

0 0

0 0 0 0 0 0 1 0 0 0 56

Velocit: problema diretto

(0 ) in funzione delle coordinate dei giunti & Calcolo della matrice W03 Q

W (0 ) =
03

(0 ) q L i 1,i &i
i =1

Le matrici di velocit relative sono

0 1 1 0 ( ( 0) 0) &= W01 = L 01 0 0 0 0 0 0 (0 ) = L(0 ) l& = W23 23 0 0 0 0

0 0 0 0 & 0 0 0 0

0 1 1 0 (0 ) = L(0 ) & W12 12 = 0 0 0 0

l1s 0 l1c &, 0 0 0 0

0 0 0 0 l& 0 0 1 0 0 0

La matrice che permette di conoscere la distribuzione di velocit dellorgano terminale & 0 & s & l1 0 & & & l c 0 0 + V0 ( ( ( ( 0) 0) 0) 0) 1 W03 = W01 + W12 + W23 = = & 0 0 0 0 0 l 0 0 0 0

57

& (0 ) = W (0 )P (0 ) La velocit del centro pinza si pu ottenere P P 03 P

4
2

& ( 0 ) = W ( 0 ) P( 0 ) = W( 0 ) M P( 3) P P 03 P 03 03 P c ( + ) s ( + ) 0) 3 ) s ( + ) c ( + ) ( ( PP = M 03PP = 0 0 0 0
l2 c ( + ) + l1 c + + l s l s ( ) ( 0) 2 1 PP = h l 1

( 0)

0 l2 c ( + ) + l1 c 0 ( 3) 0 l2 s ( + ) + l1s 0 0 1 hl 0 1 1

& & 0 & &+ 0 0 0 0 ( ) ( ) ( ) & PP = W03 PP = 0 0 0 0

0 0 0 0

0) 0) ( ( & l2 c ( + ) + l1 c l1 s & c + + l s l s l1 ) 1 2 ( hl l& 1 0

58

 Lo stesso risultato si ottiene usando la terna ausiliaria (a) avente origine sempre nel centro pinza, ma traslante
I (a) ( 0) W03 = M a 0 W03 M 0a M 0a = 3 0 1 3 O( 0 ) 0 3 = 0 1 0 0 0 l2 c ( + ) + l1 c 1 0 l2 s ( + ) + l1 s 0 1 hl 0 0 1
3 O( 0 ) 3 =

5
3

3 (0) a ) VO3 I3 O3 V0 I3 ( W03 = = 0 0 0 0 0 0 1 0 & &+ a ) ( W03 = 0 0 & & 0 0 0

( 0) 3O3 + V0 = 0

& & l2 s ( + ) & + 0 l1 s & & + l2 c ( + ) & + 0 l1 c & l 0 0 0

( (

) )

Per lorgano terminale si ha

& Vx = l1sin & l2 sin ( + ) &+ & & + l2 cos( + ) &+ V y = l1cos & Vz = l & &+ z =

( (

) )
59

Velocit: problema inverso Sono assegnate le velocit dellorgano terminale VPx , VPy , VPz e e & si vogliono trovare le Q Equivale a conoscere W (a ) cio la matrice di velocit nel riferimento ausiliario (a)
03

Utilizziamo lespressione

(a ) (Q ) q ( a ) S, S & L = W i 1,i i 03 &

i =1

( )

Occorre calcolare le matrici L rispetto alla terna (a)

0 1 1 0 a 0 ( ) ( ) L01 = Ma0 L01 M0a = 0 0 0 0 0 1 1 0 a) 0) ( ( L12 = Ma0 L12 M0a = 0 0 0 0 0 l2 s ( + ) l1 s 0 l2 c ( + ) + l1 c , 0 0 0 0 0 l2 s ( + ) 0 l2 c ( + ) 0 0 0 0 0 0 a) 0) ( ( , L23 = Ma0 L23 M0a = 0 0 0 0 0 0 0 0 1 0 0 0 0 0
60

 Applicando lespressione
0 1 0 0 1 0 0 0

(a ) (Q ) q ( a ) S, S & L = W i 1,i i 03 &

i =1

( )

si ha il risultato

7
2

0 1 0 l 2 s ( + ) l2 s ( + ) l1 s 0 0 0 0 0 0 0 0 l2 c ( + ) + l1 c l c 1 0 0 + ( ) 2 l& = &+ &+ 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 z y VPx 0 V 0 z x Py = nel caso specifico solo le velocit del V 0 x Pz y centro pinza sono assegnate e comunque 0 0 0 0 solo z sarebbe diverso da zero

& = V (a ) fornisce Lespressione riorganizzata (3 GdL) nella forma J (a )Q

l1sin l2 sin ( + ) l2 sin ( + ) 0 l cos + l cos( + ) ( ) l cos 0 + 1 2 2 0 0 1 & VPx & = VPy l& V Pz
61

& = J (a )1V (a ) La soluzione si ottiene invertendo lo jacobiano Q

Accelerazione: problema diretto

0) && Calcolo della matrice H ( 03 in funzione delle coordinate dei giunti Q
j 1

H (0 ) =
03

(0 ) (0 )2 (Q ) q (0 ) (Q ) L(0 ) (Q ) q 2 & & & &k q &j = ( ) + L Q q L + 2 L i i i 1,i i 1,i j 1, j k 1, k i =1 j = 2 k =1

0 )2 2 0 )2 & 2 0 ) 2 &2 0) 0 ) && 0 )&& 0 ) (0 ) & 0 ) (0 ) & 0 ) (0 ) & & && + L(01 & + L(12 & + L(01 & l + L(12 =L(01 + L(12 + L(23 l + L(23 l + 2 L(01 L12 L 23 L 23 l 1 4 24 3 1 1 2 3 42 4 3
0 0 0

0 )2 (0 )2 (0 )2 0 ) (0 ) 0 ) (0 ) 0 ) (0 ) , L12 , L 23 e i prodotti L(01 Occorre calcolare L(01 L12 , L(01 L 23 , L( 12 L 23

Risultano nulli
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 =0 0 1 0 0 0 1 0 ) ( 0 ) 1 0 ( L01 L 23 = 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 =0 0 1 0 0

0 0 0 ( ) L 23 2 = 0 0

0 1 0) ( 0) 1 0 ( L12 L 23 = 0 0 0 0

0 l1 s 0 0 0 0 0 l1 c 0 0 0 0 0 0 0 0

0 0 0 0 = 0 0 1 0 0

62

 Risultano invece diversi da zero

0 1 1 0 ( 0) L12 2 = 0 0 0 0 0 l1s 0 l1c 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 l1s 1 0 0 1 0 l1c = 0 0 0 0 0 0 0 0 0 1 0 0 1 0 = 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 l1c 0 l1s 0 0 0 0

9
2

0 1 1 0 ( 0) L01 2 = 0 0 0 0

0 1 1 0 0 0 0 0

0 1 0 ) ( 0 ) 1 0 ( L01 L12 = 0 0 0 0

0 0 0 0

0 0 0 0

0 1 1 0 0 0 0 0

0 l1s 1 0 0 1 0 l1c = 0 1 0 0 0 0 0 0

0 l1c 0 l1s 0 1 0 0
63

 Eliminando i termini nulli ed eseguendo i calcoli si ha

0) 0) 0 ) && 0 ) && ( 0 ) 2 2 0) 2 & 2 0 0) & ( ( ( ( ( && + L12 + L 23 l + L01 & + L12 + 2 L(01) L( & H 03 = L01 12

10
3

&& ( 0 ) = H ( 0 ) P( 0 ) Laccelerazione del centro pinza P p 03 p

& & 2 + 0) ( && && + H 03 = 0 0

( ) & )2 & + (
&& && +

0 0

2 && & & & l1 sin 0 l1 cos + l1 sin + 2 0 && l 0 0

&& l sin + & 2 l cos + 2 & & l1 cos 1 1

2 && && 2 cos & & && & + + + + + l sin cos l sin ( ) ( ) 2 1 2 3 A( 0 ) = 3 P 2 sin && & && ( 0 ) = && & && & + + + + + l cos sin l cos ( ) ( ) 2 p p 1 && l

Lo stesso risultato si raggiunge esprimendo la matrice delle accelerazioni a) a rispetto alla terna ausiliaria (a) H( 03 la cui 4 colonna fornisce direttamente laccelerazione del centro pinza

64

Accelerazione: problema inverso Assegnate le accelerazioni dellorgano terminale Ax , Ay , Az , si && vogliono trovare le Q Equivale a conoscere H (a ) cio la matrice di accelerazione nel riferimento ausiliario (a)
03

11

Usiamo lespressione che si ottiene da quella ricavata eliminando i termini nulli

( ( 0 )2 & 2 0 )2 2 ( ( ( ( 0 ) (0 ) & 0) 0) 0 ) && 0 )&& & + L12 + 2L(01 & && + L12 + L 23l + L 01 H 03 = L 01 L12

pre- e post-moltiplicando per Ma0 e M0a e ordinando opportunamente ( ( a )2 2 a )2 & 2 ( ( ( ( a) a ) && a ) && a) & & L 2 L(a ) L(a ) & && + L + L l = H L L
01 12 23 03 01 12 01 12
a) a) a) , L(12 , L(23 L(01 sono state gi calcolate risolvendo il problema inverso

(a ) ( a ) a )2 (a )2 , L12 e il prodotto L 01 L12 della velocit, occorre calcolare L(01

0 1 0 l2 s ( + ) l1s 1 0 0 1 1 0 0 ( ) l c l c + + 2 1 = 0 0 0 0 0 0 0 0 0 0 0 0 0 l2 c( + ) l1c 0 l2 s ( + ) l1 s 0 0 0 0
65

0 1 0 l2 s ( + ) l1s 1 0 0 ( ) l c l c + + 2 1 ( a )2 L 01 = 0 0 0 0 0 0 0 0

12
2

Calcolo delle altre matrici

0 1 1 0 ( a )2 L12 = 0 0 0 0
0 1 1 0 a ) (a ) L(01 L12 = 0 0 0 0

0 l2 s( + ) 0 l2 c( + ) 0 0 0 0

0 1 1 0 0 0 0 0

0 l2 s( + ) 1 0 0 1 0 l2 c( + ) = 0 0 0 0 0 0 0 0

0 l2 c( + ) 0 l2 s ( + ) 0 0 0 0
0 l2 c( + ) 0 l2 s( + ) 0 0 0 0

0 l2 s( + ) l1s 0 1 1 0 0 l2 c( + ) + l1c 0 0 0 0 0 0 0 0

0 l2 s ( + ) 0 1 1 0 0 l2 c( + ) = 0 0 0 0 0 0 0 0

Sostituendo le matrici calcolate in

( ( a )2 2 a )2 & 2 ( ( ( ( a) a ) && a )&& a) a ) (a ) & && + L12 + L 23 l = H 03 L 01 & L12 2L(01 & L 01 L12
si ottiene
66

&& 0 && && && + 0 0 0 0 0

&&l sin ( + ) &&( l2 sin ( + ) l1 sin ) 0 2 &&l cos( + ) &&( l2 cos( + ) + l1 cos ) + 0 2 = & 0 l& 0 0 &2 + & &2 + & 2 + 0 0 & 0 0 0 0

13
3

&2 + & 2 + &2 + & & 0 0

&2 + & l cos( + ) + &2 + & & 2l1 cos Ax + 2 2 2 2 & & & & & Ay + + + l2 sin ( + ) + l1 sin Az 0

( (

) )

~ (a ) ~ (a ) && = A && = J (a )1A che si riorganizza (3 GdL) nella forma J (a )Q Q

2 2 &&l sin ( + ) = A + &2 + & l cos( + ) + &&( l2 sin ( + ) l1 sin ) & & & + l1 cos x 2 2 &&l cos( + ) = A + &2 + & l sin ( + ) + &&( l2 cos( + ) + l1 cos ) + &2 + & & 2l1 sin y 2 2 & l& = Az ~ && Ax l1sin l2 sin ( + ) l2 sin ( + ) 0 ~ l cos + l cos ( + ) l cos ( + ) & & 0 y 2 2 1 = A ~ && 1 0 0 l Az

( (

) )

% ( a ) = A( a ) J & ( a ) J ( a )1V ( a ) In alternativa si calcola A

67