Corrado Santoro
ARSLAB - Autonomous and Robotic Systems Laboratory Dipartimento di Matematica e Informatica - Universit `a di Catania, Italy
santoro@dmi.unict.it
Programmazione Sistemi Robotici
posizione Motore
Speed pwm Controller target_vel
+ -
Sistema a processore Sistema fisico
Encoder
d/dt Position
Controller +
- target_pos
current_vel current_pos
La tecnica di controllo alternativa al semplice controllo “P” prevede l’uso di unalgoritmo software che genera ilprofilo di velocit `ada seguire per poter raggiungere la posizione target.
I tipici profili di velocit `a dei sistemi meccanici prevedono tre fasi:
1 Una prima fase diaccelerazione, parametro a1=cost
2 Una fase di crociera avelocit `a costante, a2=0, v = Vmax 3 Una fase didecelerazione, con parametro a3=cost
L’obiettivo `e far s`ı che, al termine della fase di decelerazione, la massa da controllare si trovi effettivamente allaposizione target
Il controllore `e totalmente determinato daiparametria1,Vmax ea3, pi `u il tempo di campionamento∆T.
Per determinare l’algoritmo del controllore, useremo le formule del moto uniformemente accelerato che qui ricordiamo:
a = cost v (t) = v (0) + at
x (t) = x (0) + v (0)t + 1
2 at
2Le fasi1e2sono abbastanza semplici: si tratta di incrementare la velocit `a, ad ogni step di simulazione, del valorea1∆T, fino alla saturazioneVmax:
Per implemntare la fase3 `e necessario identificare il punto in cui la fase 3 stessadeve cominiciare
Poich `e possiamo conoscere ladistanza (errore) tra la posizione correntex (t)e la posizione targetd = Xtarget− x(t), possiamo usare questa informazione per calcolarea quale distanza deve iniziare la decelerazione
Determiniamo quindi ladistanza di decelerazione
Operiamo unatraslazione temporale e consideriamo il tempo 0 l’inizio della fase 3
Usando la formula del moto uniformemente accelerato, calcoliamo il tempo necessario per arrivare a velocit `a zero(tempo di
decelerazione) TD:
v (T
D) = v (0) + a
3T
D0 = V
max+ a
3T
DT
D= − V
maxa
3v (T
D) = v (0) + a
3T
D0 = V
max+ a
3T
DT
D= − V
maxa
3Il segno “meno” `e normale perch `e, essendo a3negativo ( `e una decelerazione), accade che TDrisulti alla finepositivo.
Determiniamo a questo punto ladistanza di decelerazione XD =x (TD):
Se la distanza dal target `e inferiore a XDallora siamo nella fase di decelerazione.
x (TD) = x (0) + v (0)TD+1 2a3TD2
x (TD) = 0 + VmaxTD+1 2a3TD2 x (TD) = Vmax(−Vmax
a3
) +1
2a3(−Vmax
a3
)2
x (TD) = −Vmax2
a3
+1 2a3
Vmax2
a23 x (TD) = −Vmax2
a3
+1 2
Vmax2 a3
XD = −Vmax2
2a3
Durante la fase 3, il controllore deve fornire la velocit `a alla quale la massa deve andare
Poich `e conosciamo la distanza traposizione targeteposizione corrente usiamo questa informazione per determinare la velocit `a zero
Dobbiamo quindi trovare una funzione del tipo v (t) = f (e(t)), e(t) = Xtarget− x(t)
A tale scopo usiamo ancora una volta le equazioni della cinematica applicate allasola fase di decelerazione
Consideriamo il tempo 0 l’inizio della fase della decelerazione, quindi v (0) = Vmaxe x (0) = 0, abbiamo:
v (t) = Vmax+a3t x (t) = Vmaxt +1
2a3t2
Poniamo, per brevit `a, x = x (t) e v = v (t) e calcoliamo il tempo dalla prima equazione:
t = v − Vmax
a3
e sostituiamolo nella seconda:
x = V
maxv − V
maxa
3+ 1
2 a
3( v − V
maxa
3)
2x = V
maxv − V
max2a
3+ v
2− 2vV
max+ V
max22a
32a
3x = 2V
maxv − 2V
max2+ v
2− 2V
maxv + V
max22a
3x = −2V
max2+ v
2+ V
max22a
3x = v
2− V
max2v =
q
V
max2+ 2a
3x
x `e la distanza percorsa a partiredall’inizio della fase di decelerazione
Tuttavia noi possediamo ladistanza dal target, quindi ...
v = q
V
max2+ 2a
3x
x `e pari alladistanza di decelerazione XDmeno ladistanza dal target:
x = XD− (target pos − current pos) pertanto ...
v =
q
Vmax2 +2a3(XD− (target pos − current pos))
se a3la si considera in valore assoluto abbiamo:
v =
q
Vmax2 − 2a3(XD− (target pos − current pos))
distanza di 8m, `e
a
1= 0.3m/s
2, V
max= 2m/s, a
3= 0.2m/s
2L’andamento della distanza, con questo tipo di controllo, ed una distanza da percorrere di 8m, `e
a
1= 0.3m/s
2, V
max= 2m/s, a
3= 0.2m/s
2delle costanti di accelerazione e decelerazione
Una volta impostata a1e a3, tali durate sonosempre uguali
Al variare della distanza da percorrere, varier `a pertanto la durata della fase a velocit `a costante
target = 8m target = 5m
La fase a velocit `a costante potrebbe addirittura sparire nel caso la distanza sia particolarmente “corta”
In tal caso il trapezio degenera in untriangolo
target = 4m
discontinuit `a e il comportamento diventa il seguente:
target = 3m
Il problema `e dato dal fatto che siamo gi `a entrati nelladistanza di decelerazione senza aver finito la fase di accelerazione La soluzione consiste nelprolungare la fase di accelerazione fin quando il segmento crescente nonincontra il tratto di decelerazione
target = 3m
La fase 3 deve essere caratterizzata da unainvariabilit `a:la velocit `a da raggiungere deve essere minore della velocit `a corrente, altrimenti non ha senso parlare di decelerazione
Nel caso in figura accade proprio il contrario: quando inizia la fase di decelerazione, la velocit `a calcolata `e maggiore della velocit `a corrente Possiamo pertanto sfruttare questa propriet `a per “far continuare”
l’accelerazione fino all’incontro con il segmento di decelerazione
correttamente:
target = 3m
Immaginiamo un robot “virtuale” che si muove idealmente lungo la traiettoria desiderata
Possiamo derivare unalegge oraria, basata sulle equazioni del moto uniformemente accelerato, che implementa il profilo trapezoidale di velocit `a
Tale legge oraria fornisce laposizione teorica ˜x (t) dove dovrebbe trovarsi, all’istante t il robot
Motore Speed pwm
Controller target_vel+
-
Sistema a processore Sistema fisico
d/dt Position
Controller +
- virtual_pos
current_vel current_pos
posizione Virtual
Robot
Usiamo il “classico” controllo: P sulla posizione e PI sulla velocit `a Un modulo che implementa il moto del virtual robot genera, istante per istante, lavirtual pos
Essa rappresenta dove si trova il robot virtuale e dovedovrebbe trovarsi il robot reale
Il robot reale cerca di raggiungere lavirtual posche tuttaviacambia continuamente
Il moto si conclude al raggiugimento dellaposizione finaledel robot virtuale e (conseguentemente) del robot reale
Motore Speed pwm
Controller target_vel
+ -
Sistema a processore Sistema fisico
d/dt Position
Controller +
- virtual_pos
current_vel current_pos
posizione Virtual
Robot
Il sistema lavora mantenendo costantemente unerrore di posizione non nullo
Concettualmente, il robot virtuale “scappa” e il robot reale “lo insegue”
Il robot reale `e dunque “in ritardo” rispetto al robot virtuale e dunque arriver `a un po’ dopo al target di posizione finale
La costante KP del Position Controller permette regolare tale ritardo
Cinematica “Virtual Robot”
if TargetPos − VirtualRobotPos < DecelDistance then // We are in the deceleration phase tempAccel ← a3;
end else
// We are in the acceleration (or constant-speed) phase tempAccel ← a1;
end
VirtualRobotSpeed ← VirtualRobotSpeed + tempAccel · ∆T ;
if VirtualRobotSpeed > Vmaxthen VirtualRobotSpeed ← Vmax; tempAccel ← 0;
end
if VirtualRobotSpeed < 0 then VirtualRobotSpeed ← 0;
tempAccel ← 0;
end
VirtualRobotPos ← VirtualRobotPos + VirtualRobotSpeed · ∆T +12· tempAccel · ∆T2;
Ritardo Elevato, KP
POS= 0.5
Ritardo Basso, KP
POS= 2
ARSLAB - Autonomous and Robotic Systems Laboratory Dipartimento di Matematica e Informatica - Universit `a di Catania, Italy
santoro@dmi.unict.it
Programmazione Sistemi Robotici
