1
Controllo di ManipolatoriControllo di Manipolatori(Calcolo delle Traiettorie)(Calcolo delle Traiettorie)
Ph.D Ing. Folgheraiter MichelePh.D Ing. Folgheraiter Michele
Corso di Robotica Corso di Robotica Prof.Prof. CagliotiCaglioti
Anno Anno AccAcc. 2005/2006. 2005/2006
DEFINIZIONI
• Cammino geometrico (Path) E una curva definita nello spazio Euclideo. L’end-effector del manipolatore deve seguire tale curva (segmento, arco di circonferenza..). E possibile definire il cammino geometrico con l’ascissa curvilinea.
• Traiettoria (legge oraria) indica una funzione dipendente dal tempo (s=s(t))
2
E’ possibile classificare le traiettorie in:
•Stop to Stop: I giunti si muovono fino ad attivare i sensori di fine corsa (attuazione pneumatica).
•Point to Point: Si fornisce al controllore del robot una sequenza di punti da raggiungere (es Pick-and-Place)
•Traiettoria Controllata: E necessario specificare istante per istante lo stato del robot (posizione, velocità, accelerazione), può essere pre-calcolata o calcolata in real time.
•Traiettoria Continua: Si fa memorizzare al robot la traiettoria punto per punto con un campionamento molto fitto (non c’è bisogno di grosse elaborazioni)
3
14
2
Le traiettorie possono essere espresse nello:
• Spazio Cartesiano: Si stabilisce una traiettoria nello spazio cartesiano, poi punto per punto si passa, con la cinematica inversa, ai valori da mandare ai giunti.
- Molto pesante dal punto di vista computazionale- Problemi di controllo (punti singolari)
• Spazio dei Giunti: Si stabilisce la posizione iniziale e finale del giunto, poi si definisce una legge oraria ?i(t) (nello spazio dei giunti) che passa per questi due punti.
- Permette di risparmiare il calcolo della cinematica inversa per ogni punto.- Le traiettorie nello spazio cartesiano sono imprevedibili
3
Ci sono ostacoli da evitare
Non ci sono ostacoli da evitare
C’è un cammino voluto (rettilineo,
ottimo ecc...)
� Pianificazione Fuori linea
+Inseguimento(Ostacoli fissi)
�Inseguimento
Run Time
Non c’è un cammino voluto (ogni cammino è
sicuro)
� Controllo della posizione
Robotica mobile(Ostacoli Variabili)
Metodo dei Potenziali
�Controllo della
posizione
(Il più semplice)
Traiettoria con Ostacoli, casi possibili:
Se gli ostacoli che vogliamo evitare sono fissi, possiamo usare la rappresentazione del manipolatore nel C-Space.
C- Space: Il C-space è uno spazio n-dimensionale, dove n è il numero dei gradi di libertà della struttura.
-Ogni punto del C-space rappresenta una particolare configurazione del robot-Definiti degli ostacoli, vogliamo che la mano del manipolatore e la sua struttura li eviti.-Nel C-Space ogni ostacolo viene rappresentato come una serie di punti (configurazione dei giunti) non ammissibili
OSS: Non c’è corrispondenza biunivoca tra spazio cartesiano e spazio dei giunti.
4
- La generazione del C-Space è molto onerosa, la complessità è esponenziale col il numero di giunti.
- Per velocizzare i calcoli di collisione, è possibile discretizzare il C-Space
- Se m è il numero di intervalli mn la complessità di calcolo del C-Space (esponenziale)
Strategie di Movimento:
Manipolatore a 2 DOF, vogliamo passare dalla posizione di Start a quella di Goal, esistono infinite traiettorie possibili.
Modalità di movimento1) I giunti possono essere mossi alla stessa velocità2) I giunti vengono mossi uno alla volta3) I giunti devono iniziare e finire assieme il movimento
start
10°
20°
60°
90°goal
θ1 si sposta di 50°
θ2 si sposta di 70°
5
goal
In questo tratto i duegiunti si muovonoassieme (θ1 di 50°e θ2 di 50°)
start
Qui c’è un punto didiscontinuità il giunto 1 èarrivato, da questo punto inpoi si muove solo il giunto 2.
goal
Prima il giunto 1 poi il giunto 2
start
goal
Prima il giunto 2 poi il giunto 1
start
1
2
Calcolo delle traiettorie nello spazio dei giunti
1. Si parte con il definire alcuni punti di via nello spazio cartesiano
2. Attraverso la cinematica inversa si calcolano i valori dei giunti per ogni punto di via.
3. Si considera un giunto alla volta θi
4. Si utilizza un algoritmo che determini delle traiettorie θi(t) interpolatrici di questi punti- Le traiettorie devono essere facilmente computabili- Posizione e velocità devono essere funzioni continue (il robot non può
muoversi a scatti)
6
Traiettorie polinomiali :
Come traiettoria si utilizza una funzione polinomiale di grado n, che ha derivate continue.
Dopo di che si fissano dei vincoli quali per esempio:
- Posizione Iniziale- Posizione Finale- Velocità Iniziale- Velocità Finale- Accelerazione Iniziale- Accelerazione Finale
Con c vincoli serve un polinomio di grado c-1.
nntatataat ++++= ...)( 2
210θ
Polinomio Cubico: solitamente è un buon compromesso in termini qualità della traiettoria e tempo di calcolo per determinarla.Per definire un polinomio cubico servono 4 vincoli:
Es: si consideri il giunto di un robot, supponiamo di muoverlo tra due posizioni :? t1=10°, ? t2=60°, in un tempo pari a 10s, il giunto deve partire fermo e arrivare nella posizione finale fermo. Determinare i parametri della cubica.
33
2210)( tatataat +++=θ
7
1
2
8
Grafici:θ (0) = 10°θ (tf) = 60°.θ(0) = 0 °/s.θ(tf) = 0 °/stf=10s
32
3
2
1
0
1.05.110)(
1.01000
50*2
5.1100
50*3010
ttt
a
a
aa
−+=
−=−
=
==
==
ϑ
)(tϑ )(tϑ&
30
3
20
2
1
00
)(2
)(30
f
f
f
f
ta
ta
aa
ϑϑ
ϑϑ
ϑ
−−=
−=
==
La forma parametrica della soluzione è:
taat
tataat
tatataat
32
2321
33
2210
62)(
32)(
)(
+=
++=
+++=
ϑ
ϑ
θ
&&
&
θ (0) = θ0θ (tf) = θf.θ(0) = 0.θ(tf) = 0
Angolo pari a = θ0 per t=0Angolo pari a = θf per t=tf
Velocità nulla iniziale
Velocità nulla finale
9
ES2: Passaggio per punti di via senza arrestarsi: - abbiamo più punti di via- vogliamo che il manipolatore non si arresti nei punti di via P0 e Pf
θ (0) = θ0θ (tf) = θf. θ (0) = θD0. θ (tf) = θDf
1
10
2
3
11
Forma parametrica della soluzione:
θ (0) = θ0θ (tf) = θf. θ (0) = θD0. θ (tf) = θDf 2
321
10
33
2210
00
32 ffDf
D
ffff
tataa
a
tatataa
a
++=
=
+++=
=
θ
θ
θ
θ
Parametri Cubica:
20
30
3
02
02
01
00
)()(2
2)(3
f
DDf
f
f
f
Df
f
D
f
f
D
tta
ttta
a
a
θθϑϑ
θθθθθθ
++
−−=
−−−
=
==
ESEMPIO: Traiettoria rettilinea nello spazio cartesiano
1. Fissare il numero dei punti di via
2. Per ogni punto applicare la cinematica inversa
3. Per ogni coppia di valori per il singolo giunto calcolare la cubica
4. Supponiamo che le velocità dei giunti nei punti di via siano nulle
x
y
1ϑ
2ϑ
s g
0)(
0)0(
=
=
ftθ
θ&
&
12
Robot Planare
Cinematica Diretta
x
y
1ϑ
2ϑ
l1
l2
12211
12211
slslyclclx
+=+=
( )
)cll,satan2(l-x)atan2(y,
),(2atan 1 , 2
x
)()(
221221
222222
21
22
21
22
2
212211
212211
22
+=
=⇒−=
−−+=
+++=+
θ
θ CSCSll
llyC
slslclclyx
Cinematica Inversa βαβαβαβαβαβα
sensensensensen
⋅⋅=±⋅±⋅=±
mcoscos)cos(coscos)(
y
x
0°
90°
+/-180°
13
322
35241
0053.0079.06.2)(
105.1103.225.0)(
ttt
ttt
+−=
⋅−⋅−= −−
θ
θ
1) N=0 (solo s e g)
s=(0,5), g=(20,5)
La retta che passa per questi punti è y=ax+c
y=5
CinInv(0,5)=(14°,151°)
CinInv(20,5)=(14°,0°)
a0=0.25 b0=2.6
a1=0 b1=0
a2=-2.3*10-4 b2=-0.0791
a3=-1.54*10-5 b3= 0.0053
2
1
2
1
2
1
θ
θ
θ
θ
θθ
&&
&&
&
&
14
2) N=1
3) N=2
Scelta della velocità nei punti di via: ci sono vari metodi:
1) L’utente sceglie le velocità nello spazio cartesiano e utilizzando lo jacobiano inverso vengono calcolate le velocità dei giunti (pesante computazionalmente, al variare di θ va calcolato nuovamente).
Lo jacobiano J è una matrice. Supponiamo di conoscere la cinematica diretta del robot, F.
Xr&
r& 1−= Jϑϑ
r&
r& ⋅= JX
Per variazioni piccolepossiamo approssimare la F con la derivata
⋅
∂∂∂∂∂∂
∂∂∂∂∂∂∂∂∂∂∂∂
=
nnnnn
n
n
n ??
????
?/f?/f?/f
?/f?/f?/f?/f?/f?/f
?X
?X?X 1
1
1
11111
......
..................
...2
2
2222
2
2
J
),...,(
),...,(
)]([)(
1
111
nfX
nfX
tFtX
nn ϑϑ
ϑϑ
ϑ
=
=
=
M
15
=
z
y
x
z
y
x
VVV
X
ωωω
&
Se dividiamo per ∆t, e facciamo tendere l’intervallo di tempo a zero:
Normalmente il vettore è formato da sei componenti, le prime tre sono le velocità lineari dell’EndEffector e le altre tre le sue velocità angolari.
X&
Esempio) Calcolo jacobiano (Robot planare 2DOF)
x
y
1ϑ
2ϑ
(x,y)
l1
l2
16
Jacobiano (Robot planare 2DOF)
++−−−
=
+=+=
12212211
1221221121
12211
12211
),(clclclslslsl
J
slslyclclx
ϑϑ
Oss) L’inversa di una matrice esiste solo se il determinante è diverso da zero.
)()()()( 1221112212212211 clclslclslslJ +⋅−−⋅−−=
Il determinante si annulla per: 180 e 0 21 == ϑϑ