FONDAMENTI DI ROBOTICA Prof.

Bruno SICILIANO

CINEMATICA

• relazioni tra posizioni dei giunti e posizione e orientamento

dell’organo terminale

Matrice di rotazione

Rappresentazioni dell’orientamento

Trasformazioni omogenee

Cinematica diretta

Spazio dei giunti e spazio operativo

Calibrazione cinematica

Problema cinematico inverso

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

POSA DI UN CORPO RIGIDO

• Posizione

o′x
 

o′ =  o′y 
o′z

• Orientamento

x′ = x′x x + x′y y + x′z z

y ′ = yx′ x + yy′ y + yz′ z
z ′ = zx′ x + zy′ y + zz′ z
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

MATRICE DI ROTAZIONE

x′T x y ′T x z ′T x
   

R =  x′ y′ z ′  =  x′T y y ′T y z ′T y 
x′T z y ′T z z ′T z

RT R = I

RT = R−1
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Rotazioni elementari

• rotazione di α intorno a z

 
cos α −sin α 0
Rz (α) =  sin α cos α 0
0 0 1
FONDAMENTI DI ROBOTICA 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 γ
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Rappresentazione di un vettore

px p′x
   

p =  py  p′ =  p′y 
pz p′z

 

p =  x′ y′ z ′  p′

= Rp′

p′ = RT p
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Esempio

px = p′x cos α − p′y sin α

py = p′x sin α + p′y cos α
pz = p′z
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Rotazione di un vettore

p = Rp′

pT p = p′T RT Rp′

• Esempio

px = p′x cos α − p′y sin α

py = p′x sin α + p′y cos α
pz = p′z

p = Rz (α)p′
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Matrice di rotazione
⋆ fornisce l’orientamento di una terna di coordinate rispetto
ad un’altra: i vettori colonna sono i coseni direttori degli
assi della terna ruotata rispetto alla terna di partenza
⋆ rappresenta una trasformazione di coordinate che mette in
relazione le coordinate di uno stesso punto in due terne
differenti (di origine comune)
⋆ è l’operatore che consente di ruotare un vettore in una stessa
terna di coordinate
FONDAMENTI DI ROBOTICA 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

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Esempio
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

ANGOLI DI EULERO

• matrice di rotazione
⋆ 9 parametri con 6 vincoli

• rappresentazione minima dell’orientamento

⋆ 3 parametri indipendenti
FONDAMENTI DI ROBOTICA 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ϑ
FONDAMENTI DI ROBOTICA 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 )
FONDAMENTI DI ROBOTICA 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ψ
FONDAMENTI DI ROBOTICA 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 )
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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)

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Problema inverso
⋆ Assegnata  
r11 r12 r13
R =  r21 r22 r23 
r31 r32 r33

l’angolo e l’asse 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
FONDAMENTI DI ROBOTICA 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

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Problema inverso
⋆ Assegnata  
r11 r12 r13
R =  r21 r22 r23 
r31 r32 r33

il quaternione è (η ≥ 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 R−1 = RT

Q−1 = {η, −ǫ}

• prodotto tra quaternioni

Q1 ∗ Q2 = {η1 η2 − ǫT1 ǫ2 , η1 ǫ2 + η2 ǫ1 + ǫ1 × ǫ2 }
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

TRASFORMAZIONI OMOGENEE

• Trasformazione di coordinate (traslazione + rotazione)

p0 = o01 + R10 p1

• Trasformazione inversa

p1 = −R01 o01 + R01 p0

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Rappresentazione omogenea

 
p
p̃ =  
1

• Matrice di trasformazione omogenea

 
R10 o01 
A01 =


0T 1

• Trasformazione di coordinate

p̃0 = A01 p̃1

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Trasformazione inversa

−1
p̃1 = A10 p̃0 = A01 p̃0

ove  
R01 −R01 o01 
A10 =


0T 1

A−1 6= AT

• Successione di trasformazioni

p̃0 = A01 A12 . . . An−1

n p̃n
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

CINEMATICA DIRETTA

• Manipolatore
⋆ insieme di bracci connessi tramite giunti

• Catena cinematica (dalla base all’organo terminale)

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

• Grado di mobilità
⋆ tipicamente associato a una articolazione = variabile di
giunto
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Catena aperta

Tn0 (q) = A01 (q1 )A12 (q2 ) . . . An−1

n (qn )

Teb (q) = T0b Tn0 (q)Ten

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Convenzione di Denavit-Hartenberg

• si sceglie l’asse zi giacente lungo l’asse del giunto i + 1

• si individua Oi all’intersezione dell’asse zi con la normale

comune agli assi zi−1 e zi , e con Oi′ si indica l’intersezione
della normale comune con zi−1

• si assume l’asse xi diretto lungo la normale comune agli

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

• si sceglie l’asse yi in modo da completare una terna levogira

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Definizione non univoca della terna:

⋆ con riferimento alla terna 0, per la quale la sola direzione
dell’asse 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
all’asse zn−1 ): infatti non vi è giunto n + 1, per cui non è
definito zn e lo si può scegliere arbitrariamente
⋆ quando due assi consecutivi sono paralleli, in quanto la
normale comune tra di essi non è univocamente definita
⋆ quando due assi consecutivi si intersecano, in quanto il verso
di xi è arbitrario
⋆ quando il giunto i è prismatico, nel qual caso la sola
direzione dell’asse zi−1 è determinata
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Parametri di Denavit-Hartenberg

ai distanza di Oi da Oi′ ;
di coordinata su zi−1 di Oi′ ;
αi angolo intorno all’asse xi tra l’asse zi−1 e l’asse zi valutato
positivo in senso antiorario;
ϑi angolo intorno all’asse zi−1 tra l’asse xi−1 e l’asse xi valutato
positivo in senso antiorario.

• ai e αi sono sempre costanti

• se il giunto è rotoidale la variabile è ϑi

• se il giunto è prismatico la variabile è di

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Trasformazione di coordinate

cϑi −sϑi 0 0
 
s cϑi 0 0
Ai−1
i′ =  ϑi
0 0 1 di

0 0 0 1

1 0 0 ai
 
′ 0 cα i −sαi 0
Aii = 
0 sαi cα i 0

0 0 0 1

cϑi −sϑi cαi sϑi sαi ai cϑi

 
i−1 i′  sϑi cϑi cαi −cϑi sαi ai sϑi 
Ai−1
i (q i ) = A i ′ A i =
0 sαi cα i di
 
0 0 0 1
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Procedura operativa
1. Individuare e numerare consecutivamente gli assi dei giunti;
assegnare, rispettivamente, le direzioni agli assi z0 , . . . , zn−1
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 − 1:
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, ovvero scegliendo zn in maniera arbitraria
se il giunto n è prismatico; fissare l’asse 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 Ai−1
i (qi ) per i = 1, . . . , n
9. Calcolare Tn0 (q) = A01 . . . An−1
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
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Catena chiusa

• Connessione di un singolo braccio con due bracci

• giunto virtuale di taglio

Aij (q ′ ) = Aii+1′ (qi+1′ ) . . . Ajj−1 (qj )

Aik (q ′′ ) = Aii+1′′ (qi+1′′ ) . . . Akk−1 (qk )
FONDAMENTI DI ROBOTICA 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

FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Manipolatore planare a tre bracci

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Braccio ai αi di ϑi
1 a1 0 0 ϑ1
2 a2 0 0 ϑ2
3 a3 0 0 ϑ3
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

ci −si 0 a i ci
 
s ci 0 ai si 
Ai−1
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
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Manipolatore a parallelogramma
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Manipolatore sferico
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Braccio ai αi di ϑi
1 0 −π/2 0 ϑ1
2 0 π/2 d2 ϑ2
3 0 0 d3 0
FONDAMENTI DI ROBOTICA 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 d 3

0 0 0 1
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Manipolatore antropomorfo
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Braccio ai αi di ϑi
1 0 π/2 0 ϑ1
2 a2 0 0 ϑ2
3 a3 0 0 ϑ3
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

c1 0 s1 0
 
s 0 −c1 0
A01 =  1
0 1 0 0

0 0 0 1
ci −si 0 a i ci
 
s ci 0 ai si 
Ai−1
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
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Polso sferico
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Braccio ai αi di ϑi
4 0 −π/2 0 ϑ4
5 0 π/2 0 ϑ5
6 0 0 d6 ϑ6
FONDAMENTI DI ROBOTICA 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 d 6

0 0 0 1
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Manipolatore di Stanford
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

 
 n0 s0 a0 p0 
T60 = T30 T63 = 
0 0 0 1
FONDAMENTI DI ROBOTICA 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
0
a =  s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5 
−s2 c4 s5 + c2 c5
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Manipolatore antropomorfo con polso sferico

FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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
a0 =  s1 (c23 c4 s5 + s23 c5 ) − c1 s4 s5 
s23 c4 s5 − c23 c5
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

ci 0 si 0
 
 si 0 −ci 0 
Ai−1
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

 
d 3 xd 3 + d 5 xd 5 + d 7 xd 7
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
FONDAMENTI DI ROBOTICA 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 a 7 c7
 

6
 s7 0 −c7 a7 s7 
A7 = 
0

0 1 0 
0 0 0 1
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Manipolatore umanoide

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

• Struttura di collegamento tra l’organo terminale del torso

antropomorfo e le terne base dei manipolatori che fungono
da braccia

⋆ meccanismo che permette di mantenere il ‘petto’ del

manipolatore umanoide sempre ortogonale al suolo (ϑ4 =
−ϑ2 − ϑ3 )

• Cinematica diretta

0
Trh r
= T30 Tt3 Trt Trh
0
Tlh = T30 Tt3 Tlt Tlh
l
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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)
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Esempio

   
px a1 c1 + a2 c12 + a3 c123
x =  py  = k(q) =  a1 s1 + a2 s12 + a3 s123 
φ ϑ1 + ϑ2 + ϑ3
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

• Esempio

⋆ configurazioni ammissibili
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

⋆ spazio di lavoro
FONDAMENTI DI ROBOTICA 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

• Ripetibilità
⋆ capacità del manipolatore di tornare in una posizione
precedentemente raggiunta
⋆ valori tipici: (0.02, 0.2) mm

• Ridondanza cinematica
⋆ m < n (intrinseca)
⋆ r < m = n (funzionale)
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

CALIBRAZIONE CINEMATICA

• Valori precisi dei parametri DH per migliorare l’accuratezza di

un manipolatore

• Equazione cinematica diretta in funzione di tutti i parametri

x = k(a, α, d, ϑ)

xm locazione misurata

xn locazione nominale (parametri fissi + variabili giunto)

∂k ∂k ∂k ∂k
∆x = ∆a + ∆α + ∆d + ∆ϑ
∂a ∂α ∂d ∂ϑ

= Φ(ζn )∆ζ
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

⋆ l misure (lm ≫ 4n)

∆x1 Φ1
  
. .
∆x̄ =  ..  =  ..  ∆ζ = Φ̄∆ζ
∆xl Φl

• Soluzione

∆ζ = (Φ̄T Φ̄)−1 Φ̄T ∆x̄

ζ ′ = ζn + ∆ζ
. . . finché ∆ζ converge

⋆ stime più accurate dei parametri fissi

⋆ correzioni alle misure dei trasduttori

Inizializzazione

• postura di riferimento (home)

FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

PROBLEMA CINEMATICO INVERSO

• Cinematica diretta
⋆ q =⇒ T
⋆ q =⇒ x

• Cinematica inversa
⋆ T =⇒ q
⋆ x =⇒ q

• Complessità
⋆ soluzione analitica (in forma chiusa) ?
⋆ soluzioni multiple
⋆ infinite soluzioni
⋆ non esistono soluzioni ammissibili

• Intuizione
⋆ algebrica
⋆ geometrica

• Tecniche numeriche
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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 = α ± β
FONDAMENTI DI ROBOTICA 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 l’orientamento (ϑ4, ϑ5 , ϑ6 )
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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 )
⇓
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

ϑ3,I ∈ [−π, π]
ϑ3,II = −ϑ3,I
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA 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
FONDAMENTI DI ROBOTICA Prof. Bruno SICILIANO

Soluzione del polso sferico

n3x s3x a3x

 

R63 =  n3y s3y a3y 

n3z s3z a3z

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

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