PIANIFICAZIONE DI TRAIETTORIE Percorso e traiettoria Traiettorie

CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
PIANIFICAZIONE DI TRAIETTORIE
• generazione degli ingressi di riferimento per il sistema di
controllo del moto
Percorso e traiettoria
Traiettorie nello spazio dei giunti
Traiettorie nello spazio operativo
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
PERCORSO E TRAIETTORIA
⋆ generazione di traiettorie a curvatura regolare
• Percorso: luogo dei punti dello spazio dei giunti (spazio
operativo) che il manipolatore deve descrivere nell’esecuzione
del movimento assegnato (descrizione geometrica)
• Traiettoria: percorso su cui sia specificata la legge oraria di
moto (ad es. velocità e/o accelerazioni)
• Algoritmo di pianificazione della traiettoria
⋆ ingressi
definizione del percorso
vincoli al percorso
vincoli dovuti alla dinamica del manipolatore
⋆ uscita
traiettorie dei giunti (o dell’organo terminale) espresse come
sequenza temporale di valori assunti da posizione, velocità
e accelerazione
CONTROLLO DEI ROBOT
• Numero ristretto di parametri
⋆ percorso
punti estremi
eventuali punti intermedi
primitive geometriche
⋆ legge di moto
tempo complessivo
velocità e/o accelerazione max
velocità e/o accelerazione in punti assegnati
• Traiettorie nello spazio operativo
⋆ descrizione naturale del compito
⋆ vincoli sul percorso
⋆ singolarità
⋆ ridondanza
• Traiettorie nello spazio dei giunti
⋆ inversione cinematica
⋆ azione di controllo
Prof. Bruno SICILIANO
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
TRAIETTORIE NELLO SPAZIO DEI GIUNTI
• Generazione di una funzione q(t) che interpola i valori
assegnati per le variabili di giunto nel rispetto dei vincoli
imposti.
⋆ traiettorie generate poco onerose dal punto di vista
computazionale
⋆ posizioni e velocità (e accelerazioni) dei giunti funzioni
continue del tempo
⋆ effetti indesiderati minimizzati (ad es. traiettorie a curvatura
non regolare)
• Moto punto–punto
⋆ punti estremi e tempo di transizione
• Moto attraverso una sequenza di punti
⋆ punti estremi, punti intermedi e tempi di transizione
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Moto punto–punto
⋆ generazione di q(t) che realizza il moto da qi a qf in un
tempo tf
• Polinomio cubico
q(t) = a3 t3 + a2 t2 + a1 t + a0
q̇(t) = 3a3 t2 + 2a2 t + a1
q̈(t) = 6a3 t + 2a2
⋆ calcolo dei coefficienti
a 0 = qi
a1 = q̇i
a3 t3f + a2 t2f + a1 tf + a0 = qf
3a3 t2f + 2a2 tf + a1 = q̇f
• Polinomio quintico (accelerazioni iniziale e finale)
q(t) = a5 t5 + a4 t4 + a3 t3 + a2 t2 + a1 t + a0
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Esempio
pos
[rad]
3
2
1
0
0
0.2
0.4
0.6
[s]
0.8
1
0.8
1
0.8
1
vel
5
[rad/s]
4
3
2
1
0
0
0.2
0.4
0.6
[s]
acc
20
[rad/s^2]
10
0
−10
−20
0
0.2
0.4
0.6
[s]
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Profilo di velocità trapezoidale
q̈c tc =
qm − qc
tm − tc
1
qc = qi + q̈c t2c
2
q̈c t2c − q̈c tf tc + qf − qi = 0
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• q̈c assegnata (sgn q̈c = sgn (qf − qi ))
s
1 t2f q̈c − 4(qf − qi )
tf
−
tc =
2
2
q̈c
|q̈c | ≥
4|qf − qi |
t2f
⋆ traiettoria

1
2

 qi + 2 q̈c t
q(t) = qi + q̈c tc (t − tc /2)


qf − 21 q̈c (tf − t)2
0 ≤ t ≤ tc
tc < t ≤ tf − tc
tf − tc < t ≤ tf
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Esempio
pos
[rad]
3
2
1
0
0
0.2
0.4
0.6
[s]
0.8
1
0.8
1
0.8
1
vel
5
[rad/s]
4
3
2
1
0
0
0.2
0.4
0.6
[s]
acc
20
[rad/s^2]
10
0
−10
−20
0
0.2
0.4
0.6
[s]
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• q̇c assegnata
|qf − qi |
2|qf − qi |
< |q̇c | ≤
tf
tf
tc =
qi − qf + q̇c tf
q̇c
q̇c2
q̈c =
qi − qf + q̇c tf
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Moto attraverso una sequenza di punti
⋆ opportunità di specificare punti intermedi (sequenza di
punti)
• Assegnati N punti di percorso, trovare una funzione
interpolante che passi per tali punti
⋆ polinomio di grado N − 1
non è possibile assegnare velocità iniziale e finale
al crescere del grado di un polinomio aumenta il suo
carattere oscillatorio (traiettorie poco naturali)
l’accuratezza numerica con cui può essere calcolato un
polinomio peggiora all’aumentare del suo grado
una volta impostato il sistema di equazioni dei vincoli,
risulta oneroso risolverlo
i coefficienti del polinomio dipendono da tutti i punti
assegnati =⇒ se si vuole cambiare uno dei punti di
percorso, si deve ricalcolare il tutto
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Sequenze di polinomi interpolatori di grado più basso (cubici)
uniti tra di loro nei punti assegnati lungo il percorso
⋆ i valori di q̇(t) in corrispondenza dei punti di percorso sono
imposti arbitrariamente
⋆ i valori di q̇(t) in corrispondenza dei punti di percorso sono
assegnati in base ad un opportuno criterio
⋆ l’accelerazione q̈(t) deve essere continua in corrispondenza
dei punti di percorso
• Sequenze di polinomi di grado minore di tre (passaggio in
prossimità dei punti di percorso imposti)
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Sequenza di polinomi con vincoli imposti sulle velocità nei
punti di percorso
Πk (tk ) = qk
Πk (tk+1 ) = qk+1
Π̇k (tk ) = q̇k
Π̇k (tk+1 ) = q̇k+1
⋆ continuità delle velocità nei punti intermedi
Π̇k (tk+1 ) = Π̇k+1 (tk+1 )
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Esempio
pos
[rad]
6
4
2
0
0
2
4
6
4
6
4
6
[s]
vel
[rad/s]
5
0
−5
0
2
[s]
acc
[rad/s^2]
20
0
−20
−40
0
2
[s]
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Sequenza di polinomi con velocità calcolate nei punti di
percorso
q̇1 = 0
0
q̇k = 1
2 (vk
+ vk+1 )
sgn (vk ) 6= sgn (vk+1 )
sgn (vk ) = sgn (vk+1 )
q̇N = 0
vk = (qk − qk−1 )/(tk − tk−1 )
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Esempio
pos
[rad]
6
4
2
0
0
2
4
6
4
6
4
6
[s]
vel
[rad/s]
5
0
−5
0
2
[s]
acc
30
[rad/s^2]
20
10
0
−10
−20
−30
0
2
[s]
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Sequenza di polinomi con accelerazione continua nei punti di
percorso (spline)
Πk−1 (tk ) = qk
Πk−1 (tk ) = Πk (tk )
Π̇k−1 (tk ) = Π̇k (tk )
Π̈k−1 (tk ) = Π̈k (tk )
⋆ 4N − 2 equazioni nei 4(N − 1) coefficienti incogniti
(polinomi di quarto grado per il primo e l’ultimo segmento?)
⋆ 2 punti virtuali (continuità su posizione, velocità e
accelerazione) =⇒ N + 1 polinomi cubici
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
⋆ 4(N − 2) equazioni per gli N − 2 punti di percorso interni:
Πk−1 (tk ) = qk
Πk−1 (tk ) = Πk (tk )
Π̇k−1 (tk ) = Π̇k (tk )
Π̈k−1 (tk ) = Π̈k (tk )
⋆ 6 equazioni per i punti iniziale e finale
Π1 (t1 ) = qi
Π̇1 (t1 ) = q̇i
Π̈1 (t1 ) = q̈i
ΠN +1 (tN +2 ) = qf
Π̇N +1 (tN +2 ) = q̇f
Π̈N +1 (tN +2 ) = q̈f
⋆ 6 equazioni per i punti virtuali
Πk−1 (tk ) = Πk (tk )
Π̇k−1 (tk ) = Π̇k (tk )
Π̈k−1 (tk ) = Π̈k (tk )
⇓
⋆ Sistema di 4(N + 1) equazioni per la determinazione dei
4(N + 1) coefficienti degli N + 1 polinomi cubici
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
⋆ algoritmo efficiente dal punto di vista computazionale
Π̈k (t) =
Πk (t) =
Π̈k (tk+1 )
Π̈k (tk )
(tk+1 − t) +
(t − tk )
∆tk
∆tk
Π̈k (tk )
Π̈k (tk+1 )
(tk+1 − t)3 +
(t − tk )3
6∆tk
6∆tk
!
Πk (tk+1 ) ∆tk Π̈k (tk+1 )
(t − tk )
−
+
∆tk
6
!
Πk (tk ) ∆tk Π̈k (tk )
+
(tk+1 − t)
−
∆tk
6
⋆ 4 incognite: Πk (tk ), Πk (tk+1 ), Π̈k (tk ) e Π̈k (tk+1 )
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
⋆ N variabili qk per k 6= 2, N + 1 assegnate
⋆ vincoli di continuità su q2 e qN +1
⋆ vincoli di continuità su qk per k = 3, . . . , N
⋆ q̇i e q̇f assegnate
⋆ vincoli di continuità su q̈k per k = 2, . . . , N + 1
⋆ q̈i e q̈f assegnate
⇓
Π̇1 (t2 ) = Π̇2 (t2 )
..
.
Π̇N (tN +1 ) = Π̇N +1 (tN +1 )
⋆ sistema di equazioni lineari
T
A [ Π̈2 (t2 ) . . . Π̈N +1 (tN +1 ) ] = b
a11
 a21
 .
A=
 ..
 0

0
a12
a22
..
.
0
0
...
...
..
.
0
0
..
.
. . . aN −1,N −1
...
aN,N −1
0
0
..
.
aN −1,N
aN N






CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Esempio
pos
[rad]
6
4
2
0
0
1
2
3
4
5
3
4
5
3
4
5
[s]
vel
[rad/s]
5
0
−5
0
1
2
[s]
acc
30
20
[rad/s^2]
10
0
−10
−20
−30
0
1
2
[s]
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Sequenza di polinomi parabolico-lineari con passaggio in
prossimità di punti di via
qk − qk−1
∆tk−1
q̇k,k+1 − q̇k−1,k
q̈k =
∆t′k
q̇k−1,k =
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Esempio
pos
[rad]
6
4
2
0
0
1
2
3
4
5
3
4
5
3
4
5
[s]
vel
4
[rad/s]
2
0
−2
−4
−6
0
1
2
[s]
acc
40
[rad/s^2]
20
0
−20
−40
0
1
2
[s]
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Applicazione della legge trapezoidale al problema dell’interpolazione
pos
5
[rad]
4
3
2
1
0
0
0.5
1
[s]
1.5
2
1.5
2
1.5
2
vel
4
[rad/s]
3
2
1
0
0
0.5
1
[s]
acc
[rad/s^2]
20
10
0
−10
−20
0
0.5
1
[s]
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
TRAIETTORIE NELLO SPAZIO OPERATIVO
• Sequenze di punti assegnati
⋆ inversione cinematica
⋆ traiettorie nello spazio dei giunti
⋆ microinterpolazione
• Punti di percorso x(tk )
⋆ componenti xi (tk ) raccordate secondo sequenze di
polinomi interpolatori
• Primitive di percorso
⋆ espressione analitica del moto
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Primitive di percorso
• Rappresentazione parametrica di una curva nello spazio
p = f (s)
t=
dp
ds
1 d2 p
n= 2 2
d p ds
ds2 b=t×n
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Segmento nello spazio
p(s) = pi +
s
(pf − pi )
kpf − pi k
1
dp
=
(pf − pi )
ds
kpf − pi k
d2 p
=0
ds2
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Circonferenza nello spazio


ρ cos (s/ρ)
p (s) =  ρ sin (s/ρ) 
0
′
p(s) = c + Rp′ (s)


−sin (s/ρ)
dp
= R  cos (s/ρ) 
ds
0


−cos
(s/ρ)/ρ
d2 p
= R  −sin (s/ρ)/ρ 
2
ds
0
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
Traiettorie di posizione e di orientamento
• Posizione p = f (s)
⋆ s(t) polinomio interpolatore
ṗ = ṡ
dp
= ṡt
ds
⋆ segmento
p(s) = pi +
s
(pf − pi )
kpf − pi k
ṗ =
ṡ
(pf − pi ) = ṡt
kpf − pi k
p̈ =
s̈
(pf − pi ) = s̈t
kpf − pi k
⋆ circonferenza


ρ cos (s/ρ)
p(s) = c + R  ρ sin (s/ρ) 
0


−ṡ sin (s/ρ)
ṗ = R  ṡ cos (s/ρ) 
0
 2

−ṡ cos (s/ρ)/ρ − s̈ sin (s/ρ)
p̈ = R  −ṡ2 sin (s/ρ)/ρ + s̈ cos (s/ρ) 
0
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Sequenza di punti
pe = p0 +
N
X
j=1
sj
(pj − pj−1 )
kpj − pj−1 k
0

sj (t) = s′j (t)

kpj − pj−1 k
j = 1, . . . , N
0 ≤ t ≤ tj−1
tj−1 < t < tj
tj ≤ t ≤ tf
⇓
ṗe =
N
X
j=1
p̈e =
N
X
j=1
N
X
ṡj
ṡj tj
(pj − pj−1 ) =
kpj − pj−1 k
j=1
N
X
s̈j
s̈j tj
(pj − pj−1 ) =
kpj − pj−1 k
j=1
• Punti di via

0
sj (t) = s′j (t + ∆tj )

kpj − pj−1 k
∆tj = ∆tj−1 + δtj
0 ≤ t ≤ tj−1 − ∆tj
tj−1 − ∆tj < t < tj − ∆tj
tj − ∆tj ≤ t ≤ tf − ∆tN
j = 1, . . . , N
∆t0 = 0
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
• Orientamento
⋆ interpolazione delle componenti di n(t), s(t), a(t) ?
⋆ interpolazione di φ(t)
s
(φf − φi )
kφf − φi k
ṡ
(φf − φi )
φ̇ =
kφf − φi k
s̈
φ̈ =
(φf − φi )
kφf − φi k
φ(s) = φi +
⋆ asse equivalente di rotazione (Rf = Ri Rfi )
Rfi
=
RiT Rf

r11
=  r21
r31
r12
r22
r32

r13
r23 
r33
r11 + r22 + r33 − 1
2


r32 − r23
1
i
 r13 − r31 
r =
2 sin ϑf
r21 − r12
ϑf = cos −1
Ri (t):
Ri (0) = I
Ri (tf ) = Rfi
R(t) = Ri Rri (ϑ(t))
ϑ(0) = 0
ϑ(tf ) = ϑf
CONTROLLO DEI ROBOT
Prof. Bruno SICILIANO
ω i = ϑ̇ r i
ω̇ i = ϑ̈ r i
⇓
Re (t) = Ri Ri (t)
ωe (t) = Ri ω i (t)
ω̇e (t) = Ri ω̇ i (t)