Sei sulla pagina 1di 34

2

Cinematica inversa
Posizione e Orientamento
dellEndEffector
Posizione dei Giunti
Obiettivo della cinematica inversa la ricerca delle relazioni per
il calcolo delle variabili di giunto, date la posizione e
l'orientamento dellorgano terminale.
La soluzione del problema cinematico
inverso di notevole importanza per
tradurre la traiettoria di moto dellorgano
terminale nei moti corrispondenti nello
spazio dei giunti.
Traiettoria end-effector
Il problema cinematico inverso
risulta pi complesso
1. in generale bisogna risolvere equazioni non lineari.
2. si possono avere soluzioni multiple o infinite.
3. possono non esistere soluzioni ammissibili.
3
Si vogliono determinare gli angoli di giunto che permettono allend-
effector di raggiungere un determinato punto dello spazio.
Dato P
0
Determinare
1
,
2
e
3
Cinematica Inversa Manipolatore RRR (Antropomorfo)
H
n o a p
n o a p
n o a p
x x x x
y y y y
z z z z
=

(
(
(
(
0 0 0 1
d
1
=0
4
) tan(
1
x
y
p
p
a =
Cinematica Inversa Manipolatore RRR (Antropomorfo)
1
1
23 3 2 2
23 1 3 2 1 2
23 1 3 2 2 1
c
s
P
P
s a s a P
c s a c s a P
c c a c a c P
x
y
z
y
x
=

+ =
+ =
+ =
Non di utilit perch
presenta alcuni problemi pratici..
5
Cinematica Inversa Manipolatore RRR (Antropomorfo)
La risoluzione del problema cinematico inverso necessita di un
frequente utilizzo delle funzioni trigonometriche inverse.
Le funzioni trigonometriche inverse presentano per alcuni
problemi pratici.
1) Seno, coseno e tangente non ammettono funzione inversa
univoca
2) Inoltre vi il problema dellaccuratezza con cui si determinano
gli angoli
Es:
y=atan(x) non definita quando langolo
vale -90 o 90, inoltre per valori prossimi
a questi non garantisce una
sufficiente accuratezza.
6
La funzione atan non univoca
)
cos
sen
atan(

=
1 ) 135 tan(
) 135 cos(
) 135 (
2
2
2
2
2
2
2
2
) 45 cos(
) 45 (
) 45 tan( 1 = =

= = = =
sen sen
) 1 atan( =
45 =
135 =
1

-90
0
90
+/-180
-135
45
x
y
7
Per ovviare a questi problemi si utilizza la funzione trigonometrica inversa
atan2(y,x).
La funzione atan2 calcola langolo utilizzando due argomenti. Il segno di questi
argomenti viene utilizzato per determinare il quadrante del risultato.
Inoltre la funzione atan2 garantisce unaccuratezza uniforme in tutto il campo di
definizione.
-90
0
90
+/-180
x y
- +
x y
+ +
x y
- -
x y
+ -
8
) , ( 2 tan
1 x y
p p A =
a patto che
2
= -
2
e
3
= -
3
) , ( 2 tan
1
'
x y
p p A + =
Anche la seguente una soluzione per
1
Troviamo il primo angolo di giunto utilizzando la funzione atan2
Ma esiste unaltra soluzione
9

2
= -
2

3
= -
3
) , ( 2 tan
1
'
x y
p p A + =
Braccio Destro
Gomito Alto
Braccio Destro
Gomito Basso
Braccio Sinistro
Gomito Alto
Braccio Sinistro
Gomito Basso
Il problema cinematico inverso caratterizzato spesso dalla molteplicit delle
soluzioni.
E possibile infatti portare la terna di estremit nella stessa posizione con un
numero di configurazioni di giunto diverse.
Un manipolatore antropomorfo pu raggiungere lo stesso punto dello spazio
attraverso 4 configurazioni differenti.
10
) , ( 2 tan
3 3 3
c s A =


sen sen
sen sen sen
=
=
m cos cos ) cos(
cos cos ) (
Dobbiamo trovare
2 2 2
3 3 2
2
3
2
2
23 2 23 2 3 2
2
3
2
2
23 3 2 2
2
23
2
3
2
2
2
2 23 3 2 2
2
23
2
3
2
2
2
2
23 3 2 2
2
23
2
3
2
2
2
2
2
2
1
2
1
2
23 3 2 2
2 2 2
23 3 2 2
23 3 2 2 1
23 3 2 2 1
) ( 2
) ( 2
2 2
2 ) ( ) (
) (
) (
z y x
z y x
z
y
x
P P P c a a a a
s s c c a a a a
s a s a s a s a c a c a c a c a
s a s a s a s a s c c a c a P P P
s a s a P
c a c a s P
c a c a c P
+ + = + + =
+ + + =
+ + + + + =
+ + + + + = + +

+ =
+ =
+ =
) ( ) ( )) ( (
) (
) ( ) (
3 3 3 2 2
3 2
2
23 2 23 2 3





Cos Cos Cos
s s c c Cos
s s c c c
= = +
+ =
=
+ =
+ =
) 1 (
2
2
3 3
3 2
2
3
2
2
2 2 2
3
c s
a a
a a P P P
c
z y x
=
+ +
=
11
12
Per calcolare
2
si risolve un sistema algebrico con incognita


sen sen
sen sen sen
=
=
m cos cos ) cos(
cos cos ) (
13


sen sen
sen sen sen
=
=
m cos cos ) cos(
cos cos ) (
) (
) (
) (
) (
)) ( (
3 3 2
3 2 3
2
3 2 3 3 3 2 2
3 2 3 3 2 3 2 2
3 2 3 2 3 2 2
3 2 3 3 2 3 2 2 1
3 2 3 2 3 2 2 1
c a a
s c a P
s
s c a c a a s P
s c a c s a s a P
s c c s a s a P
s s a c c a c a s P
s s c c a c a c P
z
z
z
z
y
x
+

=
+ + =
+ + =

+ + =
+ =
+ =
14


sen sen
sen sen sen
=
=
m cos cos ) cos(
cos cos ) (
|
|

\
|
+
+ + =
+
+ +
|
|

\
|
+
+ + =
|
|

\
|
+
+
|
|

\
|
+

|
|

\
|
+
+ + =
|
|

\
|
+

+ =
+ =
+ =
) ( ) (
) (
) ( ) (
) ( ) (
) (
)) ( (
3 3 2
2
3
2
3 1
3 3 1 2 1 2
3 3 2
3 3 1 3 3 2
3 3 2
2
3
2
3 1
3 3 1 2 1 2
3 3 2
3 3 1
3 3 2
3 3 1
3 3 2
2
3
2
3 1
3 3 1 2 1 2
3 3 2
3 2 3
3 3 1 3 2 3 1 2 2 1
3 2 3 1 3 2 3 1 2 2 1
3 2 3 2 3 2 2 1
c a a
s a c
c a c a c c
c a a
s a c P c a a P
c a a
s a c
c a c a c c
c a a
s a c P
P
c a a
s a c P
c a a
s a c
c a c a c c P
c a a
s c a P
s a c c c a c c a c P
s s a c c c a c c a c P
s s c c a c a c P
z x
z
x
z
x
z
x
x
x
15


sen sen
sen sen sen
=
=
m cos cos ) cos(
cos cos ) (
( )
( )
( )
( )
( )
2
3 3 2
2
3
2
2
3 3 3 3 2
1
2
3 3 2
2
3
2
3
2
3
2
2 1
3 3 1 3 3 2
2
2
3
2
3 3 3 2
2
3
2
3
2
2 1
3 3 1 3 3 2
2
2
3
2
3 3 3 2 3 3 2 1
3 3 1 3 3 2
2
2
3
2
3 3 3 2 3 3 3 3 2 2 1
3 3 1 3 3 2
2
3 3 2
2
3
2
3 1
3 3 2 3 3 2 3 3 1 3 3 2 2 1
3 3 1 3 3 2
2
) (
2 ) (
) (
2
) (
) )( (
) (
) ( ) (
) (
) (
) ( ) ( ) (
) (
c
c a a a a
s a P c a a
c
P
c
c a a s c a a c
s a c P c a a P
c
s a c a a c a a c
s a c P c a a P
c
s a c a a c a a c
s a c P c a a P
c
s a c a a c a c a a a c
s a c P c a a P
c
c a a
s a c
c a a c a a c a c c a a a c
s a c P c a a P
z
x
z x
z x
z x
z x
z x
=
+ +
+ +
=
+ + +
+ +
=
+ + +
+ +
=
+ + +
+ +
=
+ + + +
+ +
=
+
+ + + + +
+ +
Si raccoglie c1
Si raccoglie a2+a3c3
=1
2 2
1
y x
x
P P
c
P
+ =
( )
2 2 2
3 3 2
2
3
2
2
2
z y x
P P P c a a a a + + = + +
16


sen sen
sen sen sen
=
=
m cos cos ) cos(
cos cos ) (
17
Robot SCARA
0
x
0
y
1
x
1
z
0
z
2
x
2
z
3
x
3
z
4
x
4
z
1
a
2
a
d
4
d
3
0
d
1
d
i

4
0

4
0 0 0 3

2
a
2
0 2

1
a
1
0 1

i
a
i

i
i
i angolo fra lasse xi-1 e xi attorno a zi-1
di distanza fra xi-1 e xi misurata lungo la
direzione di zi-1
ai lunghezza del link, distanza fra zi-1 e zi
lungo lasse xi. (lunghezza del link)
i angolo fra gli assi zi-1 e zi intorno a xi.
1
d
3
d
18
Robot SCARA cinematica diretta
d
4
d
3
0
d
1
d
i

4
0

4
0 0 0 3

2
a
2
0 2

1
a
1
0 1

i
a
i

i
i
|
|
|
|
|

\
|
+ +
+
+
=
|
|
|
|
|

\
|

=
|
|
|
|
|

\
|
+
+
+
= =
1 0 0 0
1 0 0
0
0
1 0 0 0
1 0 0
0 0
0 0
1 0 0 0
1 0 0
0
0
4 3 1
12 2 1 1 124 124
12 2 1 1 124 124
0
4 , 0
4
4 4
4 4
3
4 , 3
3 1
12 2 1 1 12 12
12 2 1 1 12 12
2
3 , 2
1
2 , 1
0
1 , 0
0
3 , 0
d d d
s a s a c s
c a c a s c
T
d
c s
s c
A
d d
s a s a c s
c a c a s c
A A A A
19
|
|
|
|
|

\
|
+ +
+
+
=
1 0 0 0
1 0 0
0
0
4 3 1
12 2 1 1 124 124
12 2 1 1 124 124
0
4 , 0
d d d
s a s a c s
c a c a s c
T
Dato P
0
determinare : D
3
,
1
,
2
Non consideriamo lultimo gdl (giunto 4)
D
3
=
H
n o a p
n o a p
n o a p
x x x x
y y y y
z z z z
=

(
(
(
(
0 0 0 1
0
4
= d
1 3
3 1
d P d
d d P
z
z
=
+ =
20
|
|
|
|
|

\
|
+ +
+
+
=
1 0 0 0
1 0 0
0
0
4 3 1
12 2 1 1 124 124
12 2 1 1 124 124
0
4 , 0
d d d
s a s a c s
c a c a s c
T
2=


sen sen
sen sen sen
=
=
m cos cos ) cos(
cos cos ) (
( )
12 1 12 1 2 1
2
2
2
1
2 2
12 2 1 1
2
12
2
2
2
1
2
1 12 2 1 1
2
12
2
2
2
1
2
1
2 2
12 2 1 1
12 2 1 1
2
2 2
s s c c a a a a P P
s a s a s a s a c a c a c a c a P P
s a s a P
c a c a P
y x
y x
y
x
+ + + = +
+ + + + + = +
+ =
+ =
2
c

s s c c Cos + = ) (
) , ( 2 tan 1
2
2
2 2 2
2
2 2
2
2 1
2
2
2
1
2 2
2 2 1
2
2
2
1
2 2
c s a c s
c
a a
a a P P
c a a a a P P
y x
y x
= =
=
+
+ + = +

21
) , ( atan2
1
2
2 2 2
2
2 2
2 1
2
2
2
1
2 2
2
c s
c s
l l
l l P P
c
y x
=
=
+
=

2 1
a a
22
|
|
|
|
|

\
|
+ +
+
+
=
1 0 0 0
1 0 0
0
0
4 3 1
12 2 1 1 124 124
12 2 1 1 124 124
0
4 , 0
d d d
s a s a c s
c a c a s c
T

1
=


sen sen
sen sen sen
=
=
m cos cos ) cos(
cos cos ) (
( )
( )
( )
( ) ( )
( ) ( )
1 2 2
2 2
1 2 2
1 2 2 1
2
2
2
2 1 2 2 2 2
1
1 1
1 2 2
1 2 2
2 2 2 1 2
1 2 2
1 2 2
1
1 2 2 1 2 2 1
1 1 1 2 2 2 1 2
1 1 2 1 2 2 1 2
a c a
P s a
a c a
a c a a s a a c a c a
c P
c a
a c a
c s a P
s a c c a P
a c a
c s a P
s
c s a a c a s P
s a c s a c s a P
c a s s a c c a P
y
x
y
x
y
y
y
x
+

|
|

\
|
+
+ + + +
=
+
|
|

\
|
+

=
+

=
+ + =
+ + =
+ =
23
|
|
|
|
|

\
|
+ +
+
+
=
1 0 0 0
1 0 0
0
0
4 3 1
12 2 1 1 124 124
12 2 1 1 124 124
0
4 , 0
d d d
s a s a c s
c a c a s c
T

1
=


sen sen
sen sen sen
=
=
m cos cos ) cos(
cos cos ) (
( )
( )
( ) ( )
( )
|
|

\
|
+
+ + + +
=
+
+ +
1 2 2
1 2 2 1
2
2
2
2 1 2 2 2 2
1
1 2 2
2 2 1 2 2
a c a
a c a a s a a c a c a
c
a c a
P s a a c a P
y x
( ) ( )
2 2 2
1 2 2 1
2
2
2
1 2 2 1
2
2
2
2 2 2 1
2
2
2
2
1 2 2 1
2
2
2
2 1 2 2 2 2
2
y x
P P a c a a a
a c a a s a c a a c a
a c a a s a a c a c a
+ = + + =
+ + + + =
= + + + +
24
) , ( atan2
) (

) (
1 1 1
2 2
2 2 2 2 1
1
2 2
2 2 2 2 1
1
c s
P P
P s a P c a a
c
P P
P s a P c a a
s
y x
y x
y x
x y
=
+
+ +
=
+
+
=

( )
( ) ( )
( )
1
2 2
2 2 1 2 2
1 2 2
2 2
1
1 2 2
2 2 1 2 2
c
P P
P s a a c a P
a c a
P P
c
a c a
P s a a c a P
y x
y x
y x y x
=
+
+ +
|
|

\
|
+
+
=
+
+ +
25
1

Robot Cilindrico (RTT) Cinematica diretta


d
i
3
2
1

i
a
i

i
i
L
1
d
2
d
3
o
x
o
z
1
x
1
z
2
z
2
x
2
y
1
y
0
y
3
z
3
x
3
y
26
Robot Cilindrico (RTT) Cinematica diretta
|
|
|
|
|

\
|

1 0 0 0
0
1
, 1
i
i
i
i
i i
d c s
s a s c c c s
c a s s c s c
A
i i
i i i i i i
i i i i i i



d
3
d
2
L
1
d
i
0 0 0
3
0 0 90
2

1
0 90
1

i
a
i

i
i
27
Robot Cilindrico (RTT) Cinematica diretta
|
|
|
|
|

\
|

1 0 0 0
0
1
, 1
i
i
i
i
i i
d c s
s a s c c c s
c a s s c s c
A
i i
i i i i i i
i i i i i i



d
3
d
2
L
1
d
i
0 0 0
3
0 0 90
2

1
0 90
1

i
a
i

i
i
28
Robot Cilindrico (RTT) Cinematica diretta
|
|
|
|
|

\
|
+

=
|
|
|
|
|

\
|
|
|
|
|
|

\
|

|
|
|
|
|

\
|

=
1 0 0 0
1 0 0
0
0
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0
0 1 0 0
0 0 0 1
1 0 0 0
0 1 0
0 0
0 0
1 3
1 2 1 1
1 2 1 1
0
3 , 0
3 2 1
1 1
1 1
0
3 , 0
L d
c d c s
s d s c
T
d d L
c s
s c
T
29
Robot Cilindrico (RTT) Cinematica Inversa
|
|
|
|
|

\
|
+

=
1 0 0 0
1 0 0
0
0
1 3
1 2 1 1
1 2 1 1
0
3 , 0
L d
c d c s
s d s c
T
1
1
1
1
1 2
1 2
1 3
1 3
1 2
1 2
2
) , ( 2 tan
c
P
d
P P a
c
s
c d
s d
P
P
P L d
L d P
c d P
s d P
y
y x
y
x
z
z
y
x
=
=

=
=
+ =
=
=

30
Robot Cilindrico (RTT) Cinematica Inversa
1
2
1
1 3
) , ( 2 tan
c
P
d
P P a
P l d
y
y x
z

=
=
=

d
2
1

L
1
d
3
d
2
E possibile risolvere il problema cinematico inverso
utilizzando solo le informazioni riguardo la cinematica diretta
di un robot?
31
Algoritmo numerico per la risoluzione del problema cinematico inverso (Cenni)
il problema che si intende risolvere risulta equivalente alla
ricerca del minimo globale di una funzione multidimensionale.
Lo scopo ideale sarebbe quello di trovare un vettore tale che
Punto da raggiungere
(si conoscono le coordinate Px Py Pz
Ma non si conoscono i valori dei giunti del robot) Simulazione
CinematicaDiretta
Posizione End-Effector
(Calcolata dando ai giunti un valore di partenza)
Distanza tra end-effector
e punto da raggiungere f(x)
f(x)
Variabili di giunto
{ } ) , , ( , ) ... ( tan ) (
1
z y x P q q P za dis q f
n effector end
=
32
Minimi locali e globali
si minimizza la funzione derrore
Algoritmo numerico per la risoluzione del problema cinematico inverso (Cenni)
33
Definizione matematica del gradiente in uno spazio R
3
Il gradiente quindi la grandezza che indica direzione e verso
secondo cui si ha la massima variazione della funzione f(x,y,z)
oltre che lentit di questa variazione.
Il gradiente viene definito come grandezza vettoriale.
Algoritmo numerico per la risoluzione del problema cinematico inverso (Cenni)
34
Per minimizzare la funzione f(x) possiamo
utilizzare la discesa del gradiente.
Lalgoritmo esegue i seguenti passi principali:
1) Calcolo della cinematica diretta
2) Calcolo della distanza tra posizione dellend
effector e posizione da raggiungere
3) Calcolo del gradiente per ogni variabile di
giunto.
Lalgoritmo termina quando lerrore tra la
posizione dellend-effector e il punto P
minore di un determinato valore
Ad ogni iterazione. ogni
giunto viene movimentato
nella direzione che determina
una diminuzione della
distanza tra end-effector e
posizione P da raggiungere.
NB: La discesa del gradiente una tecnica di ottimizzazione di tipo locale che non
garantisce di trovare la soluzione ottima al problema.
Algoritmo numerico per la risoluzione del problema cinematico inverso (Cenni)
35
Algoritmo numerico per la risoluzione del problema cinematico inverso (Cenni)