CONTROLLO DEI ROBOT Prof.

Bruno SICILIANO

CINEMATICA

relazioni tra posizioni dei giunti e posizione e orientamento

dellorgano terminale

Matrice di rotazione

Rappresentazioni dellorientamento

Trasformazioni omogenee

Cinematica diretta

Spazio dei giunti e spazio operativo

Calibrazione cinematica

Problema cinematico inverso

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

POSA DI UN CORPO RIGIDO

Posizione

ox

o = oy
oz

Orientamento

x = xx x + xy y + xz z
y = yx x + yy y + yz z
z = zx x + zy y + zz z
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

MATRICE DI ROTAZIONE

xT x y T x z T x

R = x y z = xT y y T y z T y
xT z y T z z T z

RT R = I

RT = R1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Rotazioni elementari

rotazione di intorno a z

cos sin 0
Rz () = sin cos 0
0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

rotazione di intorno a y

cos 0 sin
Ry () = 0 1 0
sin 0 cos

rotazione di intorno a x

1 0 0
Rx () = 0 cos sin
0 sin cos
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Rappresentazione di un vettore

px px

p = py p = py
pz pz

p = x y z p

= Rp

p = RT p
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Esempio

px = px cos py sin
py = px sin + py cos
pz = pz
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Rotazione di un vettore

p = Rp

pT p = pT RT Rp

Esempio

px = px cos py sin
py = px sin + py cos
pz = pz

p = Rz ()p
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Matrice di rotazione
fornisce lorientamento di una terna di coordinate rispetto
ad unaltra: 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)
e loperatore che consente di ruotare un vettore in una stessa
terna di coordinate
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

COMPOSIZIONE DI MATRICI DI
ROTAZIONE

p1 = R21 p2

p0 = R10 p1
p0 = R20 p2

Rij = (Rji )1 = (Rji )T

Rotazione in terna corrente

R20 = R10 R21

Rotazione in terna fissa

R20 = R21 R10

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Esempio
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

ANGOLI DI EULERO

matrice di rotazione
9 parametri con 6 vincoli

rappresentazione minima dellorientamento

3 parametri indipendenti
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Angoli ZYZ

R() = Rz ()Ry ()Rz ()

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
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Problema inverso
Assegnata
r11 r12 r13
R = r21 r22 r23
r31 r32 r33

i 3 angoli ZYZ sono ( (0, ))

= Atan2(r23 , r13 )
q 
= Atan2 2 + r2 , r
r13 23 33

= Atan2(r32 , r31 )

ovvero ( (, 0))

= Atan2(r23 , r13 )
 q 
= Atan2 r13 2 + r2 , r
23 33

= Atan2(r32 , r31 )
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Angoli di RPY

R() = Rz ()Ry ()Rx ()

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
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Problema inverso
Assegnata
r11 r12 r13
R = r21 r22 r23
r31 r32 r33

i 3 angoli di RPY sono ( (/2, /2))

= Atan2(r21 , r11 )
 q 
= Atan2 r31 , r32 2 + r2
33

= Atan2(r32 , r33 )

ovvero ( (/2, 3/2))

= Atan2(r21 , r11 )
 q 
= Atan2 r31 , r32 2 + r2
33

= Atan2(r32 , r33 )
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

ASSE/ANGOLO

R(, r) = Rz ()Ry ()Rz ()Ry ()Rz ()

ry rx
sin = q cos = q
rx2 + ry2 rx2 + ry2
q
sin = rx2 + ry2 cos = rz
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

rx2 (1 c ) + c rx ry (1 c ) rz s
R(, r) = rx ry (1 c ) + rz s ry2 (1 c ) + c

rx rz (1 c ) ry s ry rz (1 c ) + rx s

rx rz (1 c ) + ry s
ry rz (1 c ) rx s
rz2 (1 c ) + c

R(, r) = R(, r)
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Problema inverso
Assegnata
r11 r12 r13
R = r21 r22 r23
r31 r32 r33

langolo e lasse di rotazione sono (sin 6= 0)

 
r11 + r22 + r33 1
= cos 1
2

r r23
1 32
r= r13 r31
2 sin
r21 r12

con
rx2 + ry2 + rz2 = 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

QUATERNIONE UNITARIO

rappresentazione a 4 parametri Q = {, }

= cos
2

= sin r
2

2 + 2x + 2y + 2z = 1

(, r) e (, r) forniscono lo stesso quaternione

2( 2 + 2x ) 1

2(x y z ) 2(x z + y )
R(, ) = 2(x y + z ) 2( 2 + 2y ) 1 2(y z x )

2(x z y ) 2(y z + x ) 2( 2 + 2z ) 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Problema inverso
Assegnata
r11 r12 r13
R = r21 r22 r23
r31 r32 r33

il quaternione e ( 0)

1
= r11 + r22 + r33 + 1
2
sgn (r32 r23 ) r11 r22 r33 + 1
1
= sgn (r13 r31 ) r22 r33 r11 + 1

2
sgn (r21 r12 ) r33 r11 r22 + 1

quaternione estratto da R1 = RT

Q1 = {, }

prodotto tra quaternioni

Q1 Q2 = {1 2 T1 2 , 1 2 + 2 1 + 1 2 }
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

TRASFORMAZIONI OMOGENEE

Trasformazione di coordinate (traslazione + rotazione)

p0 = o01 + R10 p1

Trasformazione inversa

p1 = R01 o01 + R01 p0

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Rappresentazione omogenea

p
p =
1

Matrice di trasformazione omogenea

R10 o01
A01 =


0T 1

Trasformazione di coordinate

p0 = A01 p1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Trasformazione inversa

1
p1 = A10 p0 = A01 p0

ove
R01 R01 o01
A10 =


0T 1

A1 6= AT

Successione di trasformazioni

p0 = A01 A12 . . . An1

n pn
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

CINEMATICA DIRETTA

Manipolatore
insieme di bracci connessi tramite giunti

Catena cinematica (dalla base allorgano terminale)

aperta (sequenza unica)
chiusa (sequenza forma un anello)

Grado di mobilita
tipicamente associato a una articolazione = variabile di
giunto
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Terna base e terna utensile

Equazione cinematica diretta

nbe (q) sbe (q) abe (q) pbe (q)
Teb (q) =
0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Manipolatore planare a due bracci

nbe sbe abe pbe
Teb (q) =
0 0 0 1
0 s12 c12 a1 c1 + a2 c12

0 c12 s12 a1 s1 + a2 s12
=
1 0 0 0

0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Catena aperta

Tn0 (q) = A01 (q1 )A12 (q2 ) . . . An1

n (qn )

Teb (q) = T0b Tn0 (q)Ten

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Convenzione di Denavit-Hartenberg

si sceglie lasse zi giacente lungo lasse del giunto i + 1

si individua Oi allintersezione dellasse zi con la normale

comune agli assi zi1 e zi , e con Oi si indica lintersezione
della normale comune con zi1

si assume lasse xi diretto lungo la normale comune agli

assi zi1 e zi con verso positivo dal giunto i al giunto i + 1

si sceglie lasse yi in modo da completare una terna levogira

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Definizione non univoca della terna:

con riferimento alla terna 0, per la quale la sola direzione
dellasse z0 risulta specificata: si possono quindi scegliere
arbitrariamente O0 ed x0
con riferimento alla terna n, per la quale il solo
asse xn risulta soggetto a vincolo (deve essere normale
allasse zn1 ): infatti non vi e giunto n + 1, per cui non e
definito zn e lo si puo scegliere arbitrariamente
quando due assi consecutivi sono paralleli, in quanto la
normale comune tra di essi non e univocamente definita
quando due assi consecutivi si intersecano, in quanto il verso
di xi e arbitrario
quando il giunto i e prismatico, nel qual caso la sola
direzione dellasse zi1 e determinata
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Parametri di Denavit-Hartenberg

ai distanza di Oi da Oi ;
di coordinata su zi1 di Oi ;
i angolo intorno allasse xi tra lasse zi1 e lasse zi valutato
positivo in senso antiorario;
i angolo intorno allasse zi1 tra lasse xi1 e lasse xi valutato
positivo in senso antiorario.

ai e i sono sempre costanti

se il giunto e rotoidale la variabile e i

se il giunto e prismatico la variabile e di

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Trasformazione di coordinate

ci si 0 0

s ci 0 0
Ai1
i = i
0 0 1 di

0 0 0 1

1 0 0 ai

0 ci si 0
Aii =
0 si ci 0

0 0 0 1

ci si ci si si ai ci

i1 i si ci ci ci si ai si
Ai1
i (q i ) = A i A i =
0 si ci di

0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Procedura operativa
1. Individuare e numerare consecutivamente gli assi dei giunti;
assegnare, rispettivamente, le direzioni agli assi z0 , . . . , zn1
2. Fissare la terna base posizionandone lorigine sullasse 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 1:
3. Individuare lorigine Oi allintersezione di zi con la normale
comune agli assi zi1 e zi . Se gli assi zi1 e zi sono paralleli e
il giunto i e rotoidale, posizionare Oi in modo da annullare di ;
se il giunto i e prismatico, scegliere Oi in corrispondenza di
una posizione di riferimento per la corsa del giunto (ad esempio
un fine-corsa)
4. Fissare lasse xi diretto lungo la normale comune agli assi zi1
e zi con verso positivo dal giunto i al giunto i + 1
5. Fissare lasse yi in modo da ottenere una terna levogira
Per completare:
6. Fissare la terna n, allineando zn lungo la direzione di zn1 se il
giunto n e rotoidale, ovvero scegliendo zn in maniera arbitraria
se il giunto n e prismatico; fissare lasse xn 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 omogenea Ai1
i (qi ) per i = 1, . . . , n
9. Calcolare Tn0 (q) = A01 . . . An1
n che fornisce posizione e
orientamento della terna n rispetto alla terna 0
10. Assegnate T0b e Ten , calcolare la funzione cinematica diretta
Teb (q) = T0b Tn0 Ten che fornisce posizione e orientamento della
terna utensile rispetto alla terna base
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Catena chiusa

Connessione di un singolo braccio con due bracci

giunto virtuale di taglio

Aij (q ) = Aii+1 (qi+1 ) . . . Aj1

j (qj )
Aik (q ) = Aii+1 (qi+1 ) . . . Ak1
k (qk )
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Vincoli

giunto j + 1 rotoidale
T
( j

Ri (q ) pij (q ) pik (q ) = [ 0 0 djk ]
zji (q ) = zki (q )

giunto j + 1 prismatico
 iT   
x j (q ) i i

0

y iT (q )
pj (q ) pk (q ) =

j 0

zji (q ) = zki (q )

iT i
xj (q )xk (q ) = cos jk

risolti in termini di q . . .

Tn0 (q) = A0i Aij Ajn

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

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
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Manipolatore planare a tre bracci

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Braccio ai i di i
1 a1 0 0 1
2 a2 0 0 2
3 a3 0 0 3
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

ci si 0 ai ci

s ci 0 ai si
Ai1
i = i i = 1, 2, 3
0 0 1 0

0 0 0 1

T30 = A01 A12 A23

c123 s123 0 a1 c1 + a2 c12 + a3 c123

s c123 0 a1 s1 + a2 s12 + a3 s123
= 123
0 0 1 0

0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Manipolatore a parallelogramma
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Braccio ai i di i
1 a1 0 0 1
2 a2 0 0 2
3 a3 0 0 3
1 a1 0 0 1
4 a4 0 0 0
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

A03 (q ) = A01 A12 A23
c1 2 3 s1 2 3 0 a1 c1 + a2 c1 2 + a3 c1 2 3

s c1 2 3 0 a1 s1 + a2 s1 2 + a3 s1 2 3
= 123
0 0 1 0

0 0 0 1

c1 s1 0 a1 c1

s c1 0 a1 s1
A01 (q ) = 1
0 0 1 0

0 0 0 1
1 0 0 a4

0 1 0 0
A34 =
0 0 1 0

0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Risoluzione vincoli
orientamento (OK)
posizione

0
3 0 0



R0 (q ) p3 (q ) p1 (q ) = 0
0

a1 (c1 + c1 2 3 ) + a1 (c1 2 c1 ) = 0
a1 (s1 + s1 2 3 ) + a1 (s1 2 s1 ) = 0

2 = 1 1
3 = 2 = 1 + 1

Cinematica diretta

c1 s1 0 a1 c1 a4 c1

s c1 0 a1 s1 a4 s1
T40 (q) = A03 (q)A34 = 1
0 0 1 0

0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Manipolatore sferico
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Braccio ai i di i
1 0 /2 0 1
2 0 /2 d2 2
3 0 0 d3 0
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

c1 0 s1 0 c2 0 s2 0

s 0 c1 0 s 0 c2 0
A01 = 1 A12 = 2
0 1 0 0 0 1 0 d2

0 0 0 1 0 0 0 1

1 0 0 0

0 1 0 0
A23 =
0 0 1 d3

0 0 0 1

T30 = A01 A12 A23

c1 c2 s1 c1 s2 c1 s2 d3 s1 d2

s c c1 s1 s2 s1 s2 d3 + c1 d2
= 1 2
s2 0 c2 c2 d3

0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Manipolatore antropomorfo
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Braccio ai i di i
1 0 /2 0 1
2 a2 0 0 2
3 a3 0 0 3
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

c1 0 s1 0

s 0 c1 0
A01 = 1
0 1 0 0

0 0 0 1
ci si 0 ai ci

s ci 0 ai si
Ai1
i = i i = 2, 3
0 0 1 0

0 0 0 1

T30 = A01 A12 A23

c1 c23 c1 s23 s1 c1 (a2 c2 + a3 c23 )

s c s1 s23 c1 s1 (a2 c2 + a3 c23 )
= 1 23
s23 c23 0 a2 s2 + a3 s23

0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Polso sferico
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Braccio ai i di i
4 0 /2 0 4
5 0 /2 0 5
6 0 0 d6 6
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

c4 0 s4 0 c5 0 s5 0

s 0 c4 0 s 0 c5 0
A34 = 4 A45 = 5
0 1 0 0 0 1 0 0

0 0 0 1 0 0 0 1

c6 s6 0 0

s c6 0 0
A56 = 6
0 0 1 d6

0 0 0 1

T63 = A34 A45 A56

c4 c5 c6 s4 s6 c4 c5 s6 s4 c6 c4 s5 c4 s5 d6

s c c + c4 s6 s4 c5 s6 + c4 c6 s4 s5 s4 s5 d6
= 4 5 6
s5 c6 s5 s6 c5 c5 d6

0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Manipolatore di Stanford
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

n0 s0 a0 p0
T60 = T30 T63 =
0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

c1 s2 d3 s1 d2 + c1 (c2 c4 s5 + s2 c5 ) s1 s4 s5
d6
0
p = s1 s2 d3 + c1 d2 + s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5 d6
c2 d3 + (s2 c4 s5 + c2 c5 )d6


c1 c2 (c4 c5 c6 s4 s6 ) s2 s5 c6
s1 (s4 c5 c6 + c4 s6 )
n0 = s1 c2 (c4 c5 c6 s4 s6 ) s2 s5 c6 + c1 (s4 c5 c6 + c4 s6 )
s2 (c4 c5 c6 s4 s6 ) c2 s5 c6


c1 c2 (c4 c5 s6 + s4 c6 ) + s2 s5 s6
s1 (s4 c5 s6 + c4 c6 )
s0 = s1 c2 (c4 c5 s6 + s4 c6 ) + s2 s5 s6 + c1 (s4 c5 s6 + c4 c6 )
s2 (c4 c5 s6 + s4 c6 ) + c2 s5 s6

c1 (c2 c4 s5 + s2 c5 ) s1 s4 s5
a0 = s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5
s2 c4 s5 + c2 c5
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Manipolatore antropomorfo con polso sferico

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Braccio ai i di i
1 0 /2 0 1
2 a2 0 0 2
3 0 /2 0 3
4 0 /2 d4 4
5 0 /2 0 5
6 0 0 d6 6
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

c3 0 s3 0 c4 0 s4 0

s 0 c3 0 s 0 c4 0
A23 = 3 A34 = 4
0 1 0 0 0 1 0 d4

0 0 0 1 0 0 0 1

a2 c1 c2 + d4 c1 s23 + d6 c1 (c23 c4 s5 + s23 c5 ) + s1 s4 s5

p0 = a2 s1 c2 + d4 s1 s23 + d6 s1 (c23 c4 s5 + s23 c5 ) c1 s4 s5
a2 s2 d4 c23 + d6 (s23 c4 s5 c23 c5 )


c1 c23 (c4 c5 c6 s4 s6 ) s23 s5 c6
+ s1 (s4 c5 c6 + c4 s6 )
0
n = s1 c23 (c4 c5 c6 s4 s6 ) s23 s5 c6 c1 (s4 c5 c6 + c4 s6 )
s23 (c4 c5 c6 s4 s6 ) + c23 s5 c6


c1 c23 (c4 c5 s6 + s4 c6 ) + s23 s5 s6
+ s1 (s4 c5 s6 + c4 c6 )
s0 = s1 c23 (c4 c5 s6 + s4 c6 ) + s23 s5 s6 c1 (s4 c5 s6 + c4 c6 )
s23 (c4 c5 s6 + s4 c6 ) c23 s5 s6

c1 (c23 c4 s5 + s23 c5 ) + s1 s4 s5
0
a = s1 (c23 c4 s5 + s23 c5 ) c1 s4 s5
s23 c4 s5 c23 c5
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Manipolatore del DLR

Braccio ai i di i
1 0 /2 0 1
2 0 /2 0 2
3 0 /2 d3 3
4 0 /2 0 4
5 0 /2 d5 5
6 0 /2 0 6
7 0 0 d7 7
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

ci 0 si 0

si 0 ci 0
Ai1
i =
0
i = 1, . . . , 6
1 0 di
0 0 0 1
c7 s7 0 0

6
s7 c7 0 0
A7 =
0

0 1 d7
0 0 0 1

d3 xd3 + d5 xd5 + d7 xd7
p07 = d3 yd3 + d5 yd5 + d7 yd7
d 3 zd3 + d 5 zd5 + d 7 zd7

xd3 = c1 s2
xd5 = c1 (c2 c3 s4 s2 c4 ) + s1 s3 s4
xd7 = c1 (c2 k1 + s2 k2 ) + s1 k3
yd3 = s1 s2
yd5 = s1 (c2 c3 s4 s2 c4 ) c1 s3 s4
yd7 = s1 (c2 k1 + s2 k2 ) c1 k3
zd3 = c2
zd5 = c2 c4 + s2 c3 s4
zd7 = s2 (c3 (c4 c5 s6 s4 c6 ) + s3 s5 s6 ) c2 k2

k1 = c3 (c4 c5 s6 s4 c6 ) + s3 s5 s6
k2 = s4 c5 s6 + c4 c6
k3 = s3 (c4 c5 s6 s4 c6 ) c3 s5 s6
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

((xa c5 + xc s5 )c6 + xb s6 )c7 + (xa s5 xc c5 )s7
n07 = ((ya c5 + yc s5 )c6 + yb s6 )c7 + (ya s5 yc c5 )s7
(za c6 + zc s6 )c7 + zb s7
((xa c5 + xc s5 )c6 + xb s6 )s7 + (xa s5 xc c5 )c7

((ya c5 + yc s5 )c6 + yb s6 )s7 + (ya s5 yc c5 )c7
s07 =
(za c6 + zc s6 )s7 + zb c7

(xa c5 + xc s5 )s6 xb c6
a07 = (ya c5 + yc s5 )s6 yb c6
za s6 zc c6

xa = (c1 c2 c3 + s1 s3 )c4 + c1 s2 s4
xb = (c1 c2 c3 + s1 s3 )s4 c1 s2 c4
xc = c1 c2 s3 s1 c3
ya = (s1 c2 c3 c1 s3 )c4 + s1 s2 s4
yb = (s1 c2 c3 c1 s3 )s4 s1 s2 c4
yc = s1 c2 s3 + c1 c3
za = (s2 c3 c4 c2 s4 )c5 + s2 s3 s5
zb = (s2 c3 s4 + c2 c4 )s5 s2 s3 c5
zc = s2 c3 s4 + c2 c4

se 7 = /2

c7 0 s7 a7 c7

6
s7 0 c7 a7 s7
A7 =
0

0 1 0
0 0 0 1
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Manipolatore umanoide

Braccia costituite da due manipolatori del DLR (7 = /2)

Struttura di collegamento tra lorgano 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

0
Trh = T30 Tt3 Trt Trh
r

0
Tlh = T30 Tt3 Tlt Tlh
l
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

c23 s23 0 0

s c23 0 0
Tt3 = 23
0 0 1 0

0 0 0 1

T30 come nel manipolatore antropomorfo

Trt e Tlt dipendono da
r r
Trh e Tlh come nel manipolatore del DLR
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

SPAZIO DEI GIUNTI E SPAZIO OPERATIVO

Spazio dei giunti

q1

.
q = ..
qn

qi = i (giunto rotoidale)
qi = di (giunto prismatico)

Spazio operativo
 
p
x=

p (posizione)
(orientamento)

Equazione cinematica diretta

x = k(q)
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Esempio

px a1 c1 + a2 c12 + a3 c123
x = py = k(q) = a1 s1 + a2 s12 + a3 s123
1 + 2 + 3
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Spazio di lavoro

Spazio di lavoro raggiungibile

p = p(q) qim qi qiM i = 1, . . . , n

elementi di superficie planare, sferica, toroidale e cilindrica

Spazio di lavoro destro

orientamenti diversi
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Esempio

configurazioni ammissibili
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

spazio di lavoro
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Accuratezza
scostamento tra posizione conseguita con la postura
assegnata e posizione calcolata tramite la cinematica diretta
valori tipici: (0.2, 1) mm

Ripetibilita
capacita 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)
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

CALIBRAZIONE CINEMATICA

Valori precisi dei parametri DH per migliorare laccuratezza di

un manipolatore

Equazione cinematica diretta in funzione di tutti i parametri

x = k(a, , d, )

xm locazione misurata

xn locazione nominale (parametri fissi + variabili giunto)

k k k k
x = a + + d +
a d

= (n )
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

l misure (lm 4n)

x1 1

. .
x = .. = .. =
xl l

Soluzione

= (T )1 T x

= n +
. . . finche converge

stime piu accurate dei parametri fissi

correzioni alle misure dei trasduttori

Inizializzazione

postura di riferimento (home)

CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

PROBLEMA CINEMATICO INVERSO

Cinematica diretta
q = T
q = x

Cinematica inversa
T = q
x = q

Complessita
soluzione analitica (in forma chiusa) ?
soluzioni multiple
infinite soluzioni
non esistono soluzioni ammissibili

Intuizione
algebrica
geometrica

Tecniche numeriche
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Soluzione del manipolatore planare a tre bracci

Soluzione algebrica

= 1 + 2 + 3

pW x = px a3 c = a1 c1 + a2 c12
pW y = py a3 s = a1 s1 + a2 s12
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

p2W x + p2W y a21 a22

c2 =
2a1 a2
q
s2 = 1 c22

2 = Atan2(s2 , c2 )

(a1 + a2 c2 )pW y a2 s2 pW x
s1 =
p2W x + p2W y
(a1 + a2 c2 )pW x + a2 s2 pW y
c1 =
p2W y + p2W y

1 = Atan2(s1 , c1 )

3 = 1 2
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Soluzione geometrica

p2W x + p2W y a21 a22

c2 = .
2a1 a2

2 = cos 1 (c2 )

= Atan2(pW y , pW x )

q
c p2W x + p2W y = a1 + a2 c2

1
p2W x + p2W y + a21 a22
= cos q
2a1 p2W x + p2W y

1 =
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Soluzione di manipolatori con polso sferico

pW = p d6 a

Soluzione disaccoppiata
calcolare la posizione del polso pW (q1 , q2 , q3 )
risolvere la cinematica inversa per (q1 , q2 , q3 )
calcolare R30 (q1 , q2 , q3 )
calcolare R63 (4 , 5 , 6 ) = R30 T R
risolvere la cinematica inversa per lorientamento (4, 5 , 6 )
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Soluzione del manipolatore sferico

(A01 )1 T30 = A12 A23

pW x c1 + pW y s1 d3 s2
p1W = pW z = d3 c2
pW x s1 + pW y c1 d2
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

1 t2 2t
c1 = s1 =
1 + t2 1 + t2

(d2 + pW y )t2 + 2pW x t + d2 pW y = 0

 q 
2 2 2
1 = 2Atan2 pW x pW x + pW y d2 , d2 + pW y

pW x c1 + pW y s1 d3 s2
=
pW z d3 c2

2 = Atan2(pW x c1 + pW y s1 , pW z )

q
d3 = (pW x c1 + pW y s1 )2 + p2W z
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Soluzione del manipolatore antropomorfo

pW x = c1 (a2 c2 + a3 c23 )
pW y = s1 (a2 c2 + a3 c23 )
pW z = a2 s2 + a3 s23

p2W x + p2W y + p2W z a22 a23

c3 =
2a2 a3
q
s3 = 1 c23

3 = Atan2(s3 , c3 )

3,I [, ]
3,II = 3,I
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

q
p2W x + p2W y (a2 + a3 c3 ) + pW z a3 s3
c2 =
a22 + a23 + 2a2 a3 c3
q
pW z (a2 + a3 c3 ) p2W x + p2W y a3 s3
s2 =
a22 + a23 + 2a2 a3 c3
2 = Atan2(s2 , c2 )

p
per s+
3 = 1 c23 :

 q
2,I = Atan2 (a2 + a3 c3 )pW z a3 s+
p2W x + p2W y ,
3
q 
2 2 +
(a2 + a3 c3 ) pW x + pW y + a3 s3 pW z
 q
+
2,II = Atan2 (a2 + a3 c3 )pW z + a3 s3 p2W x + p2W y ,
q 
2 2 +
(a2 + a3 c3 ) pW x + pW y + a3 s3 pW z

p
per s
3 = 1 c23 :

 q

2,III = Atan2 (a2 + a3 c3 )pW z a3 s3 p2W x + p2W y ,
q 
2 2
(a2 + a3 c3 ) pW x + pW y + a3 s3 pW z
 q

2,IV = Atan2 (a2 + a3 c3 )pW z + a3 s3 p2W x + p2W y ,
q 
2 2
(a2 + a3 c3 ) pW x + pW y + a3 s3 pW z
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

1,I = Atan2(pW y , pW x )
1,II = Atan2(pW y , pW x )

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

pW x 6= 0 pW y 6= 0
CONTROLLO DEI ROBOT Prof. Bruno SICILIANO

Soluzione del polso sferico

n3x s3x a3x

R63 = n3y s3y a3y

n3z s3z a3z

4 = Atan2(a3y , a3x )
q 
3 2 3 2 3
5 = Atan2 (ax ) + (ay ) , az
6 = Atan2(s3z , n3z )

4 = Atan2(a3y , a3x )
 q 
3
5 = Atan2 (a3x )2 + (a3y )2 , az
6 = Atan2(s3z , n3z )