CINEMATICA Matrice di rotazione Rappresentazioni dell

CONTROLLO DEI ROBOT
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
CONTROLLO DEI ROBOT
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
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
MATRICE DI ROTAZIONE

R =  x′
y′

x′T x y ′T x z ′T x
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
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Rotazioni elementari
• rotazione di α intorno a z

cos α
Rz (α) =  sin α
0
−sin α
cos α
0

0
0
1
CONTROLLO DEI ROBOT
• rotazione di β intorno a y

cos β
Ry (β) =  0
−sin β
• rotazione di γ intorno a x

1
0
Rx (γ) =  0 cos γ
0 sin γ
Prof. Bruno SICILIANO

0 sin β
1
0 
0 cos β

0
−sin γ 
cos γ
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Rappresentazione di un vettore

p′x
p′ =  p′y 
p′z

px
p =  py 
pz


p =  x′
= Rp′
p′ = RT p

y′

z ′  p′
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Esempio
px = p′x cos α − p′y sin α
py = p′x sin α + p′y cos α
pz = p′z
CONTROLLO DEI ROBOT
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′
CONTROLLO DEI ROBOT
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
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
• Esempio
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
ANGOLI DI EULERO
• matrice di rotazione
⋆ 9 parametri con 6 vincoli
• rappresentazione minima dell’orientamento
⋆ 3 parametri indipendenti
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Angoli ZYZ
R(φ) = Rz (ϕ)Ry′ (ϑ)Rz′′ (ψ)

cϕ cϑ cψ − sϕ sψ −cϕ cϑ sψ − sϕ cψ
=  sϕ cϑ cψ + cϕ sψ −sϕ cϑ sψ + cϕ cψ
−sϑ cψ
sϑ sψ

cϕ sϑ
sϕ sϑ 
cϑ
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Problema inverso
⋆ Assegnata

r11
R =  r21
r31
r12
r22
r32

r13
r23 
r33
i 3 angoli ZYZ sono (ϑ ∈ (0, π))
ϕ = Atan2(r23 , r13 )
q
2 + r2 , r
ϑ = Atan2
r13
23 33
ψ = Atan2(r32 , −r31 )
ovvero (ϑ ∈ (−π, 0))
ϕ = Atan2(−r23 , −r13 )
q
2 + r2 , r
ϑ = Atan2 − r13
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ψ
=  sϕ cϑ sϕ sϑ sψ + cϕ cψ
−sϑ
cϑ sψ

cϕ sϑ cψ + sϕ sψ
sϕ sϑ cψ − cϕ sψ 
cϑ cψ
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Problema inverso
⋆ Assegnata

r11
R =  r21
r31
r12
r22
r32

r13
r23 
r33
i 3 angoli di RPY sono (ϑ ∈ (−π/2, π/2))
ϕ = Atan2(r21 , r11 )
q
2 + r2
ϑ = Atan2 −r31 , r32
33
ψ = Atan2(r32 , r33 )
ovvero (ϑ ∈ (π/2, 3π/2))
ϕ = Atan2(−r21 , −r11 )
q
2 + r2
ϑ = Atan2 −r31 , − r32
33
ψ = Atan2(−r32 , −r33 )
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
ASSE/ANGOLO
R(ϑ, r) = Rz (α)Ry (β)Rz (ϑ)Ry (−β)Rz (−α)
ry
sin α = q
rx2 + ry2
q
sin β = rx2 + ry2
cos α = q
rx
rx2 + ry2
cos β = rz
CONTROLLO DEI ROBOT

Prof. Bruno SICILIANO
rx2 (1 − cϑ ) + cϑ

R(ϑ, r) =  rx ry (1 − cϑ ) + rz sϑ
rx rz (1 − cϑ ) − ry sϑ
rx ry (1 − cϑ ) − rz sϑ
ry2 (1 − cϑ ) + cϑ
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
R =  r21
r31
r12
r22
r32

r13
r23 
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
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
QUATERNIONE UNITARIO
• rappresentazione a 4 parametri Q = {η, ǫ}
ϑ
2
ϑ
ǫ = sin r
2
η = cos
η 2 + ǫ2x + ǫ2y + ǫ2z = 1
⋆ (ϑ, r) e (−ϑ, −r) forniscono lo stesso quaternione

2(η 2 + ǫ2x ) − 1

R(η, ǫ) =  2(ǫx ǫy + ηǫz )
2(ǫx ǫz − ηǫy )
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
R =  r21
r31
r12
r22
r32

r13
r23 
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 }
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
A01


=
R10
0T
• Trasformazione di coordinate
p̃0 = A01 p̃1

o01 

1
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Trasformazione inversa
p̃1 = A10 p̃0 = A01
−1
p̃0
ove
A10


=
R01
0T

−R01 o01 

1
A−1 6= AT
• Successione di trasformazioni
p̃0 = A01 A12 . . . An−1
p̃n
n
CONTROLLO DEI ROBOT
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
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Terna base e terna utensile
• Equazione cinematica diretta
Teb (q)

 nbe (q)
=
0

sbe (q) abe (q) pbe (q) 

0
0
1
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Manipolatore planare a due bracci
Teb (q)

 nbe
=
sbe
abe

pbe 

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 ) . . . An−1
(qn )
n
Teb (q) = T0b Tn0 (q)Ten
CONTROLLO DEI ROBOT
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
CONTROLLO DEI ROBOT
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
CONTROLLO DEI ROBOT
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
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Trasformazione di coordinate
Ai−1
i′
cϑi
s
=  ϑi
0
0
−sϑi
cϑi
0
0

1
′
0
Aii = 
0
0

0
cαi
sαi
0
cϑi
 sϑi
i−1 i′
Ai−1
(q
)
=
A
=
A

′
i
i
i
i
0
0


0 0
0 0

1 di
0 1
0
−sαi
cαi
0
−sϑi cαi
cϑi cαi
sαi
0

ai
0

0
1
sϑi sαi
−cϑi sαi
cαi
0

ai cϑi
ai sϑi 

di
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 , . . . , 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
(qi ) per i = 1, . . . , n
i
9. Calcolare Tn0 (q) = A01 . . . An−1
che fornisce posizione e
n
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′ ) . . . Aj−1
(qj )
j
(qk )
Aik (q ′′ ) = Aii+1′′ (qi+1′′ ) . . . Ak−1
k
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Vincoli
⋆ giunto j + 1 rotoidale
( j
Ri (q ′ ) pij (q ′ ) − pik (q ′′ ) = [ 0
0 djk ]
zji (q ′ ) = zki (q ′′ )
⋆ giunto j + 1 prismatico
 iT ′ x
(q
)
0

j
i
′
i
′′

p
(q
)
−
p
(q
)
=

j
k
 y iT (q ′ )
0
j
zji (q ′ ) = zki (q ′′ )



 iT ′ i ′′
xj (q )xk (q ) = cos ϑjk
⋆ risolti in termini di q . . .
Tn0 (q) = A0i Aij Ajn
T
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
Manipolatore planare a tre bracci
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Braccio
ai
αi
di
ϑi
1
2
3
a1
a2
a3
0
0
0
0
0
0
ϑ1
ϑ2
ϑ3
CONTROLLO DEI ROBOT
Ai−1
i
Prof. Bruno SICILIANO
ci
s
= i
0
0

−si
ci
0
0

0 ai ci
0 ai si 

1
0
0
1
i = 1, 2, 3
T30 = A01 A12 A23
c123
s
=  123
0
0

−s123
c123
0
0

0 a1 c1 + a2 c12 + a3 c123
0 a1 s1 + a2 s12 + a3 s123 

1
0
0
1
CONTROLLO DEI ROBOT
Manipolatore a parallelogramma
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Braccio
ai
αi
di
ϑi
1′
2′
3′
1′′
4
a1′
a2′
a3′
a1′′
a4
0
0
0
0
0
0
0
0
0
0
ϑ1′
ϑ2′
ϑ3′
ϑ1′′
0
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
′
′
A03′ (q ′ ) = A01′ A12′ A23′
 ′ ′ ′
c1 2 3 −s1′ 2′ 3′
c1′ 2′ 3′
s ′ ′ ′
= 123
0
0
0
0
c1′′
 s ′′
A01′′ (q ′′ ) =  1
0
0

1
′
0
A34 = 
0
0


0 a1′ c1′ + a2′ c1′ 2′ + a3′ c1′ 2′ 3′
0 a1′ s1′ + a2′ s1′ 2′ + a3′ s1′ 2′ 3′ 

1
0
0
1
−s1′′
c1′′
0
0
0
1
0
0
0
0
1
0

0 a1′′ c1′′
0 a1′′ s1′′ 

1
0
0
1

a4
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′
′
 −s ′
T40 (q) = A03′ (q)A34 =  1
0
0

s1′
−c1′
0
0

0 a1′′ c1′′ − a4 c1′
0 a1′′ s1′′ − a4 s1′ 

1
0
0
1
CONTROLLO DEI ROBOT
Manipolatore sferico
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Braccio
ai
αi
di
ϑi
1
2
3
0
0
0
−π/2
π/2
0
0
d2
d3
ϑ1
ϑ2
0
CONTROLLO DEI ROBOT
c1
s
A01 =  1
0
0

Prof. Bruno SICILIANO
0 −s1
0
c1
−1
0
0
0

0
0

0
1
1
0
A23 = 
0
0
c2
s
A12 =  2
0
0


0
1
0
0
−s1
c1
0
0
c1 s2
s1 s2
c2
0
0
0
1
0
0 s2
0 −c2
1
0
0
0

0
0 

d3
1
T30 = A01 A12 A23
c1 c2
s c
= 1 2
−s2
0


c1 s2 d3 − s1 d2
s1 s2 d3 + c1 d2 

c2 d3
1

0
0

d2
1
CONTROLLO DEI ROBOT
Manipolatore antropomorfo
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Braccio
ai
αi
di
ϑi
1
2
3
0
a2
a3
π/2
0
0
0
0
0
ϑ1
ϑ2
ϑ3
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
c1
s
A01 =  1
0
0

ci −si
ci
s
= i
0
0
0
0

Ai−1
i
0 s1
0 −c1
1
0
0
0
0
0
1
0

ai ci
ai si 

0
1

0
0

0
1
i = 2, 3
T30 = A01 A12 A23
c1 c23
s c
=  1 23
s23
0

−c1 s23
−s1 s23
c23
0
s1
−c1
0
0

c1 (a2 c2 + a3 c23 )
s1 (a2 c2 + a3 c23 ) 

a2 s2 + a3 s23
1
CONTROLLO DEI ROBOT
Polso sferico
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Braccio
ai
αi
di
ϑi
4
5
6
0
0
0
−π/2
π/2
0
0
0
d6
ϑ4
ϑ5
ϑ6
CONTROLLO DEI ROBOT
c4
s
A34 =  4
0
0

Prof. Bruno SICILIANO
0 −s4
0
c4
−1
0
0
0

0
0

0
1
c6
s
A56 =  6
0
0

c5
s
A45 =  5
0
0

−s6
c6
0
0
0 s5
0 −c5
1
0
0
0

0
0

0
1

0 0
0 0

1 d6
0 1
T63 = A34 A45 A56
c4 c5 c6 − s4 s6
 s c c + c4 s6
= 4 5 6
−s5 c6
0

−c4 c5 s6 − s4 c6
−s4 c5 s6 + c4 c6
s5 s6
0
c4 s5
s4 s5
c5
0

c4 s5 d6
s4 s5 d6 

c5 d6
1
CONTROLLO DEI ROBOT
Manipolatore di Stanford
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
T60
Prof. Bruno SICILIANO
=
T30 T63

 n0
=
0
s0
a0
0
0

p0 

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
2
3
4
5
6
0
a2
0
0
0
0
π/2
0
π/2
−π/2
π/2
0
0
0
0
d4
0
d6
ϑ1
ϑ2
ϑ3
ϑ4
ϑ5
ϑ6
CONTROLLO DEI ROBOT
c3
s
A23 =  3
0
0

0 s3
0 −c3
1
0
0
0
Prof. Bruno SICILIANO

0
0

0
1
c4
s
A34 =  4
0
0

0 −s4
0
c4
−1
0
0
0

0
0

d4
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
2
3
4
5
6
7
0
0
0
0
0
0
0
π/2
π/2
π/2
π/2
π/2
π/2
0
0
0
d3
0
d5
0
d7
ϑ1
ϑ2
ϑ3
ϑ4
ϑ5
ϑ6
ϑ7
CONTROLLO DEI ROBOT
Ai−1
i
Prof. Bruno SICILIANO
ci
 si
=
0
0

0 si
0
0 −ci 0 

i = 1, . . . , 6
1
0
di 
0
0
1


c7 −s7 0 0
 s7 c7 0 0 
6

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
yd5
yd7
zd3
zd5
= s1 s2
= s1 (c2 c3 s4 − s2 c4 ) − c1 s3 s4
= s1 (c2 k1 + s2 k2 ) − c1 k3
= −c2
= 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

a07


(xa c5 + xc s5 )s6 − xb c6
=  (ya c5 + yc s5 )s6 − yb c6 
za s6 − zc c6
xa
xb
xc
ya
yb
yc
za
zb
zc
= (c1 c2 c3 + s1 s3 )c4 + c1 s2 s4
= (c1 c2 c3 + s1 s3 )s4 − c1 s2 c4
= c1 c2 s3 − s1 c3
= (s1 c2 c3 − c1 s3 )c4 + s1 s2 s4
= (s1 c2 c3 − c1 s3 )s4 − s1 s2 c4
= s1 c2 s3 + c1 c3
= (s2 c3 c4 − c2 s4 )c5 + s2 s3 s5
= (s2 c3 s4 + c2 c4 )s5 − s2 s3 c5
= s2 c3 s4 + c2 c4
⋆ se α7 = π/2
c7
 s7
6
A7 = 
0
0

0 s7
0 −c7
0
1
0
0

a7 c7
a7 s7 

0 
1
CONTROLLO DEI ROBOT
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
r
= T30 Tt3 Trt Trh
Trh
0
l
Tlh
= T30 Tt3 Tlt Tlh
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
c23
 −s
Tt3 =  23
0
0

s23
c23
0
0
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
• Esempio
⋆ configurazioni ammissibili
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
⋆ spazio di lavoro
Prof. Bruno SICILIANO
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
• 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)
CONTROLLO DEI ROBOT
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)
∆x =
∂k
∂k
∂k
∂k
∆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 + ∆ζ
. . . finché ∆ζ converge
⋆ stime più 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
• Complessità
⋆ 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 )
s1 =
(a1 + a2 c2 )pW y − a2 s2 pW x
p2W x + p2W y
c1 =
(a1 + a2 c2 )pW x + a2 s2 pW y
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
β = cos
−1


p2W x + p2W y + a21 − a22


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 l’orientamento (ϑ4, ϑ5 , ϑ6 )
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Soluzione del manipolatore sferico
(A01 )−1 T30 = A12 A23
p1W




pW x c1 + pW y s1
d3 s2
 =  −d3 c2 
=
−pW z
−pW x s1 + pW y c1
d2
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
1 − t2
c1 =
1 + t2
s1 =
2t
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 )
d3 =
q
(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
c2 =
s2 =
Prof. Bruno SICILIANO
q
± p2W x + p2W y (a2 + a3 c3 ) + pW z a3 s3
a22 + a23 + 2a2 a3 c3
q
pW z (a2 + a3 c3 ) ∓ p2W x + p2W y a3 s3
a22 + a23 + 2a2 a3 c3
ϑ2 = Atan2(s2 , c2 )
⇓
⋆ per s+
3 =
p
1 − c23 :
a3 s+
3
q
ϑ2,I = Atan2 (a2 + a3 c3 )pW z −
p2W x + p2W y ,
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
⋆ per
ϑ2,III
ϑ2,IV
s−
3
p
= − 1 − c23 :
q
−
= 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
−
= 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
R63 =  n3y
n3z

s3x
s3y
s3z

a3x
a3y 
a3z
ϑ4 = Atan2(a3y , a3x )
q
3
3
2
3
2
ϑ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 )