1

Controllo di ManipolatoriControllo di Manipolatori(Calcolo delle Traiettorie 2)(Calcolo delle Traiettorie 2)

Ph.DPh.D Ing.Ing. Folgheraiter MicheleFolgheraiter Michele

Corso di Robotica Corso di Robotica Prof.Prof. GiniGini

Anno Anno AccAcc. 2006/2007. 2006/2007

2) Il sistema con tecniche euristiche calcola in automatico le velocità nei punti di via (magari facendo in modo che le accelerazioni siano continue)

Scelta della velocità nei punti di via con metodo Euristico:

2

Connettere due cubiche in un punto di via

(eguagliando velocità e accelerazione nel punto di via)

Consideriamo 3 punti: fv ϑϑϑ →→0

θ1(t) = a10 + a11 t+ a12 t2 + a13 t3 Traiettoria da θ0 a θvθ2(t) = a20 + a21 t + a22 t2 + a23 t3 Traiettoria da θv a θf

Con t che va da zero a tf1 per la prima cubica, e da zero a tf2 per la seconda. Deve valere quindi:

θ0= a10θv= a10 + a11t f1 + a12t2

f1 + a13t3f1

θv= a20θf= a20 + a21t f2 + a22t2

f2 + a23t3f2

0= a11 0= a21 + 2 a22t f2 +3 a23t2

f2

a21= a11 + 2 a12t f1+3 a13t2f1

2 a22=2 a12+6 a13t f1

Angolo pari a θ0 per t=0 per la prima cubicaAngolo pari a θv per t=tf1 per la prima cubicaAngolo pari a θv per t=0 per la seconda cubicaAngolo pari a θf per t=tf2Velocità nulla per t=0Velocità nulla per t=tf2

Velocità uguale nel punto di via (prima cubica t=tf1 seconda t=0)Accelerazione uguale nel punto di via (prima cubica t=t f1 seconda t=0)

Esempio) Connessione tra 4 cubiche:

3

Se si vogliono imporre anche dei vincoli sulle accelerazioni occorrono polinomi del 5° ordine.

- Si ottengono delle traiettorie più dolci, che sollecitano meno la struttura meccanica del robot.

- Il calcolo della traiettoria risulta essere piùcomplesso.

Utilizzo di funzioni Lineari con raccordi Parabolici

(Spazio dei Giunti)

- Questo metodo è stato sviluppato alla Standford University:

- Utilizza dei segmenti di retta per interpolare i punti.

- Questo tuttavia implica accelerazione infinita durante la fase iniziale e finale del moto.

Si utilizzano de raccordi parabolici durante la fase iniziale finale

6 DOF(1969)

θ

start

goal

t

4

-Si hanno quindi tre tratti: uno lineare e due parabolici

- Si possono avere differenti soluzioni ( a seconda di come scegliamo le accelerazioni iniziali e finali).

θ

Start t

goal

Raccordo 1

Raccordo 2

Tratto lineare

Moto

t

θ(t)

Velocità

t

θ’(t)

Accelerazione

t

θ’’(t)

0

0

0

t

θ(t)

t

θ’(t)

t

θ’’(t)

0

0

0

Accelerazione elevata Accelerazione moderata

5

- Il metodo è buono dal punto di vista computazionale (usa al massimo polinomi di 2° grado)

- La gestione dei punti di via è complessa (i tratti parabolici sono difficili da raccordare)

- E possibile forzare il passaggio per i punti di via mettendo dei punti di via fittizi.

start

goal

I punti di via nonvengono raggiunti.

nullo è linerare trattoil allora )(4

se )(4

20

20

ttff θθ

θθθ

θ−

=−

≥ &&&&

Traiettorie nello Spazio Cartesiano

Problemi:

- Difficoltà nel calcolare la cinematica inversa in Real-time

- Alcuni punti della traiettoria possono trovarsi fuori dallo spazio di lavoro

- Possibile passaggio della traiettoria per punti singolari a velocità infinite.

Possibile soluzione

- Vogliamo una traiettoria rettilinea nello spazio cartesiano

- Possiamo generare delle traiettorie nello spazio dei giunti che interpolino la traiettoria lineare scelta nello spazio cartesiano.

Ma quanti punti di via imponiamo?

6

Algoritmo di Bounded Deviation Path

1. Calcola (cinematica inversa) la configurazione dei giunti nei punti di start e di goal; calcola quindi la traiettoria nello spazio dei giunti fra start e goal;2. Calcola il punto di mezzo nello spazio dei giunti, e applicando la cinematica diretta trova la posizione cartesiana corrispondente;3. Se l’errore nel cammino geometrico è maggiore di quello permesso allora

a. metti un punto di via (knot) a metà del cammino cartesiano fra start e goal;b. dividi il cammino in due parti, da start a knot, da knot a goal;c. richiama ricorsivamente l’algoritmo sulle due metà del cammino;a.altrimenti termina;

4. Ritorna la sequenza dei knot points (punti di vincolo).

Oss:

-Questo metodo, nello spazio cartesiano genera una traiettoria che ondeggia attorno alla traiettoria rettilinea che volevamo approssimare.

7

CinematicaInversa

J-1

controllore

θ forze / momentiX, X

robot(sensori interni)

θ CinematicaDiretta

X

J X

.

.

.

θ

+ attuazioneθ . J = Jacobiano

J–1

= Jacobiano inverso

notare che il controllo èeseguito sulle variabilicartesiane su cui ècalcolato l’errore

errore

Controllore Cinematico Cartesiano

Oss:

Se si movimentano degli oggetti diventa importante anche la dinamica (considerare accelerazioni)

Controllori per Robot Industriali

Il sistema di controllo di un robot è un dispositivo composto in genere da un sistema multi-processore, collegato in rete con altre risorse locali di controllo, monitoraggio e immagazzinamento dati.

Funzioni di base:

• Interazione con l'operatore• Immagazzinamento di dati• Calcolo delle traiettorie• Controllo in tempo reale del moto dei giunti• Monitoraggio sensori• Interazione con altri macchinari• Interazione con altre risorse computazionali

8

Controllo del moto dei giuntiIl controllore ha il compito di mantenere la risposta dinamica (velocità

esecuzione movimento) del manipolatore entro certi limiti, fissati da criteri di prestazione stabiliti a priori.

La soluzione a questo problema è complicata da:

• Forze inerziali

• Accoppiamento tra i vari giunti

• Forza di gravità

• Carico Variabile

Il problema della sintesi del sistema di controllo si risolve:

1) trovando il modello dinamico del manipolatore

2) usando il modello per sintetizzare la legge di controllo in conformità con le specifiche sulle prestazioni.

9

Solitamente nei sistemi industriali, il sistema di controllo tratta ogni giunto del braccio come un semplice servomeccanismoindipendente dagli altri.Questo approccio non è quello ottimale in quanto si trascura il moto e la configurazione istantanea dell’intero braccio.

•Basse velocità nei movimenti

•Riduzione dello smorzamento dei servomeccanismi (oscillazioni)

•Tempi di attuazione più lunghi in presenza di carichi variabili

Controllo dei giunti

Per il controllo ad anello chiuso:

- Ogni giunto è controllato indipendentemente

- Il controllore del giunto è un PID

C a lc o lo d e l letra ie t to r ie

θ

fo rz e /m o m e n tiX , X

ro b o t(se n so r i in te rn i )

.

θ

θ

.

..

. . .

C o n tro l lo red e l g iu n to 1

C o n tro l lo red e l g iu n to 2

C o n tro l lo red e l g iu n to n

θ

θ

θ

.

..

C ’è un con tro llo reper ogn i g iun to levariab ili θ ind ica tesono dei ve tto ri

Cin. inv.

J-1

10

Controllore del Braccio PUMA

Per il braccio della serie PUMA 560, il regolatore consiste in un calcolatore DEL e sei micro-controllori Rockwell ciascuno equipaggiato di encoder, convertitore DAC e amplificatore di corrente.

6 GDL

Attraverso il terminale l’operatore può impartire comandi al braccio attraverso il linguaggio VAL.Una volta che il comando VAL è stato decodificato la CPU interna esegue delle routine memorizzate nella Eprom.

11

Compito delle routine interne è quello di:

• Eseguire le trasformazioni di coordinate, per esempio dato un punto dell’ambiente esterno vengono ricavate le coordinate dei giunti o viceversa

• Pianificazione delle traiettorie interpolate dei giunti (ogni giunto aggiornato ogni 28ms)

• Accertamento che il comando è stato eseguito su ogni singolo giunto.

Ogni servomeccanismo è controllato attraverso un regolatore PID:• I guadagni dei PID sono costanti, questo risulta essere un

problema specialmente in condizioni di carico variabile, infattiil robot Puma a velocità ridotte risulta essere soggetto a forti vibrazioni.

Funzione di trasferimento del singolo GIUNTO

Solitamente nei manipolatori azionati elettricamente, ogni giunto èmosso da un motore a corrente continua a magneti permanenti.

Questo tipo di attuazione offre grossi rapporti momento/potenza e uniformità di funzionamento a basse velocità.

Lo schema elettromeccanico di un singolo giunto può essere rappresentato come figura:

ingranaggi denti Numero:,carico di alberoall' riferito car. inerzia di Momento:

motore alberoall' riferito mot. inerzia di Momento: viscosoattrito Coeff.:,

caricomotore, angolare Posizione:,

motore dal erogata Coppia :tromotricecontroelet Forza:e

armaturad' Grandezze:,,,

b

LM

L

M

LM

LM

aaaa

NNJJ

ff

iRLV

ϑϑ

τ

12

Derivando rispetto il tempo:

La distanza lineare percorsa dagli ingranaggi è la stessa, e i raggi degli ingranaggi sono proporzionali al numero di denti:

Considerando la coppia dissipata sul carico e quella dissipata all’interno del motore possiamo dire che la coppia generata dal motore è:

Dove:

Per il principio di conservazione del lavoro: il lavoro eseguito dal carico riferito all’albero di carico deve essere uguale al lavoro fatto dal carico riferito all’albero motore (*):

13

Sostituendo in

si ottiene:

Quindi

Dove sono momento di inerzia e coefficiente di attrito viscoso effettivi (motore + carico) riferiti all’albero motore.

Sappiamo (elettrotecnica) che la coppia sviluppata dal motore e proporzionale alla corrente di armatura:

La parte elettrica del motore sarà descritta da:

Dove la forza contro-elettromotrice è proporzionale alla velocitàangolare del motore:

Trasformando secondo Laplace e risolvendo rispetto la corrente:

14

Trasformando secondo Laplace anche l’eq. di coppia effettiva:

La coppia generata dal motore (secondo le eq. della parte elettrica) trasformata:

Ma, queste due coppie devono coincidere, quindi:

La costante di tempo della parte elettrica del motore può esseretrascurata rispetto la costante di tempo meccanica, quindi possiamo trascurare l’induttanza La:

dove

15

Utilizzando il rapporto di riduzione dell’ingranaggio possiamo ottenere la funzione di trasferimento del manipolatore a singolo giunto che mette in relazione la tensione applicata al motore con lo spostamento angolare del giunto:

Regolatore di posizione per singolo GIUNTO

Lo scopo del regolatore è fare in modo che lo spostamento del giunto insegua uno spostamento desiderato. La tecnica di regolazione èbasata sull’errore di posizione angolare del giunto.

Si applica al motore una tensione direttamente proporzionale all’errore di posizione.

Dove Kp è il guadagno di reazione all’errore di posizione.

La funzione di trasferimento ad anello aperto del manipolatore a singolo giunto è rappresentata in figura:

)()( effeffm fsJst += ϑτ

Retroazione Intrinseca

16

Trasformando secondo Laplace la legge di controllo:

E sostituendo nella FDT del manipolatore a singolo giunto, si ottiene la FDT ad anello aperto che mette in relazioni lo spostamento con l’errore di posizione del giunto:

Adesso possiamo chiudere l’anello di controllo ottenendo lo schema:

E(s)

17

Come si può notare dalle equazione il sistema ad anello chiuso è del secondo ordine, ed è quindi stabile se tutti i suoi parametri sono positivi.

1. Per ridurre il tempo di risposta, e ridurre l’errore a regime èpossibile aumentare il guadagno proporzionale Kp

2. E possibile introdurre degli smorzamenti artificiali nel sistemaaggiungendo nel controllore anche una derivata dell’errore di posizione (PD).

La velocità desiderata si può ottenere dalle equazioni che descrivono le traiettorie nello spazio dei giunti (es quelle cubiche).

Ora nella FDT del sistema ad anello chiuso è presente anche uno zero. Al variare di questo zero (-Kv/Kp) varia l’overshoot e il tempo di assestamento del sistema.