Controllo Manipolatori. Massimo Cavallari. Corso di Robotica Prof.ssa Giuseppina Gini 2007/2008

Transcript

1 Controllo Manipolatori Massimo Cavallari Corso di Robotica Prof.ssa Giuseppina Gini 2007/2008 1

2 Scelta della velocità nei punti di via con metodo Euristico: 2) Il sistema con tecniche euristiche calcola in automatico le velocità nei punti di via (magari facendo in modo che le accelerazioni siano continue) [Sono stati specificati solo i punti di passaggio senza specificare le velocità intermedie] 2

3 Connettere due cubiche in un punto di via (imponendo accelerazione continua nel punto di via) Consideriamo 3 punti: ϑ 0 ϑ v ϑ f θ(t) = a 10 + a 11 t+ a 12 t 2 + a 13 t 3 θ(t) = a 20 + a 21 t + a 22 t 2 + a 23 t 3 Traiettoria da θ 0 a θ v Traiettoria da θ v a θ f Con t che va da zero a t f1 per la prima cubica, e da zero a t f2 per la seconda. Deve valere quindi: θ 0 = a 10 θ v = a 10 + a 11 t f1 + a 12 t 2 f1 + a 13 t3 f1 θ v = a 20 θ f = a 20 + a 21 t f2 + a 22 t 2 f2 + a 23 t3 f2 0= a 11 0= a a 22 t f2 +3 a 23 t 2 f2 a 21 = a a 12 t f1 +3 a 13 t 2 f1 2 a 22 =2 a a 13 t f1 Angolo pari a θ 0 per t=0 per la prima cubica Angolo pari a θ v per t=t f1 per la prima cubica Angolo pari a θ v per t=0 per la seconda cubica Angolo pari a θ f per t=t f2 Velocità nulla per t=0 Velocità nulla per t=t f2 Velocità uguale nel punto di via (prima cubica t=t f1 seconda t=0) Accelerazione uguale nel punto di via (prima cubica t=t f1 seconda t=0) 3

4 Esempio) Connessione tra 4 cubiche: Usando polinomi di terzo grado si ha accelerazione discontinua 4

5 5 Se vogliamo imporre condizioni anche per le accelerazioni occorre utilizzare polinomi di grado 5: ) ( t a t a t a t a a t a t = θ Si devono imporre le condizioni al contorno: f f i i f f i i f f i i q t q q t q q t q q t q q t q q t q && && && && & & & & = = = = = = ) ( ) ( ) ( ) ( ) ( ) ( Otteniamo.

6 6 Esempio polinomio di quinto grado: ) ( t a t a t a t a a t a t = θ 2 2 / 0 / 0 / 0 / s q s q s q s q q q f i f i f i = = = = = = && && & &

7 Più 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. 7

8 Più polinomi del 5 ordine. L accelerazione assume un andamento oscillatorio indesiderato (possibili vibrazioni sulla struttura del robot.) 8

9 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. θ goal start - Questo tuttavia implica accelerazione infinita durante la fase iniziale e finale del moto. t 6 DOF (1969) Si utilizzano dei raccordi parabolici durante la fase iniziale finale 9

10 -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). Moto θ(t) 0 Accelerazione elevata t θ(t) 0 Accelerazione moderata t θ Raccordo 2 goal θ (t) θ (t) Velocità 0 t 0 t Tratto lineare θ (t) θ (t) Raccordo 1 Accelerazione 0 0 Start t t t 10

11 L accelerazione usata per consentire il raccodo è tale per cui: 4( ) se && θ f θ0 θ = allora il tratto linerare è nullo 2 t - 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) I punti di via non vengono raggiunti. && θ 4( θ f θ ) t 2 0 start goal - E possibile forzare il passaggio per i punti di via mettendo dei punti di via fittizi. 11

12 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 metodologia - Vogliamo una traiettoria rettilinea nello spazio cartesiano - Possiamo generare delle traiettorie nello spazio dei giunti che interpolino la traiettoria lineare scelta nello spazio cartesiano. 12

13 Algoritmo di Bounded Deviation Path 1. Calcola (cinematica inversa) la configurazione dei giunti di start e 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). 13

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

15 Robot Toolbox Calcola una traiettoria nello spazio di giunto tra due configurazioni assegnate. >> T_1 = jtraj(q0, q1, n) >> T_1 = jtraj(q0, q1, n, qv0, qv1) Il risultato è una traiettoria q nello spazio di giunto tra q0 e q1. Possono essere specificate le velocità iniziali (qv0) e quelle finali (qv1). N indica il numero di configurazioni intermedie fornite. I valori di velocità e accelerazione vengono calcolati utilizzando un polinomio del settimo grado con condizioni al contorno nulle. Disegna il movimento del robot 15

16 Robot Toolbox Robot scara, generazione traiettoria nello spazio dei giunti (pick). qa = [ pi/2 pi/2-1 0] qb = [ pi/2 pi/2-3 0] Tc=0.01; % Tempo di campionamento 10 ms tempo=2; % Tempo totale (s) % Primo tratto della traiettoria t1=[0:tc:tempo]; figure(); plot(myrobot, qriposo ); pause(1) T_1=jtraj(qRiposo,qA, length(t1)); %calcolo traiettoria plot(myrobot, T_1 ); Da qriposo a qa T_2=jtraj(qA,qB, length(t1)); %movimento del robot plot(myrobot, T_2 ); Da qa qb 16

17 Robot Toolbox Grafici dei valori comandati ai giunti da qriposo a qa (prima traiettoria) figure plot(t_1(:,1)) figure plot(t_1(:,2)) figure plot(t_1(:,3)) θ 1 ( t) θ 2 ( t) q ( t 3 ) plot di una determinata colonna T_1(:,3) 17

18 Robot Toolbox Calcola una traiettoria nello spazio operativo tra due configurazioni assegnate. >> TC = ctraj(t0, T1, n) Il risultato è una traiettoria TC nello spazio operativo tra T0 e T1. N indica il numero di punti intermedi. 18

19 Robot Toolbox Definiamo un manipolatore antropomorfo con polso sferico utilizzando il robot toolbox

20 Robot Toolbox % Definizione dei parametri del robot a2=0.8; d4=0.8; d6=0.2; qz=[0, 0, 0, 0, 0, 0]; % definizione dei link di un % manipolatore antropomorfo con polso sferico % e creazione robot L1=link([ pi/ ]); L2=link([ 0 a ]); L3=link([ pi/ ]); L4=link([-pi/2 0 0 d4 0]); L5=link([pi/ ]); L6=link([ d6 0]); RA=robot({L1,L2,L3,L4,L5,L6});

21 % Posizione e orientamento per effettuare la presa pickt=[ -1, 0, 0, 0.7; 0, 1, 0, 0.2; 0, 0,-1, 0; 0, 0, 0, 1]; Robot Toolbox % Sollevamento del pezzo at=[ -1, 0, 0, 0.7; 0, 1, 0, 0.2; 0, 0,-1, 1; 0, 0, 0, 1]; % Avvicinamento alla posizione finale bt=[ 1, 0, 0, 1; 0, -1, 0, 0.2; 0, 0, -1, 0.5; 0, 0, 0, 1]; % Posizione e orientamento finali placet=[ 1, 0, 0, 1; 0, -1, 0, 0.2; 0, 0,-1, 0; Posizione e orientamento per effettuare la presa 0, 0, 0, 1]; z y x

22 % Generazione della traiettoria: tempi Tc=0.01; % Tempo di campionamento 10 ms timeat=2; % Tempo per trovarsi in at timebt=4; %Tempo per trovarsi in bt timeplace=6; %Tempo per trovarsi in placet % Primo tratto t1=[0:tc:timeat]; % Calcolo traiettoria nello spazio cartesiano traparziale1=ctraj(pickt,at, length(t1)); % Secondo tratto t2=[timeat+tc:tc:timebt]; % Calcolo traiettoria nello spazio cartesiano traparziale2=ctraj(at,bt, length(t2)); t=[t1 t2 t3]; Ttotale=cat(3,traParziale1,traParziale2,traParziale3); % Traiettoria desiderata nello spazio operativo % Le matrici T e i tempi calcolati precedentemente vengono % concatenati QTraiettoria=ikine(RA,Ttotale); figure pause(2) plot(ra,qtraiettoria); % Calcolo dei valori da comandare ai giunti per ogni % valore campionato sulla traiettoria

23 Robot Toolbox 1 pick 2 at 3 bt 4 place

24

25

26 Calcolo delle traiettorie: l obiettivo può essere caratterizzato da due aspetti fondamentali: 1) Evitare gli ostacoli 2) Avere un cammino definito Analizziamo alcuni casi ottenuti dalla combinazione dei due precedenti: Caso 1: Si vogliono evitare gli ostacoli e c è un cammino voluto. Questo è il caso più complesso, al momento attuale la pianificazione fuori linea è l unica soluzione in quanto affrontare il problema run-time è troppo pesante Caso 2: senza ostacoli ma c è un cammino voluto. Questo caso è più semplice del precedente in quanto viene richiesto solo di seguire un percorso determinato. Il controllore deve raggiungere un insieme di posizioni specificate in sequenza.

27 Caso 3: ci sono solo ostacoli da evitare (qualsiasi traiettoria va bene) Non essendoci vincoli sul percorso gli ostacoli possono essere evitati con metodi reattivi. Caso 2: nessun ostacolo e nessun vincolo sul percorso E il caso più semplice, è sufficiente che la posizione sia raggiunta.

28 Le traiettorie possono essere calcolate Nello spazio dei giunti -Metodo efficiente -Le traiettorie nello spazio cartesiano sono imprevedibili Nello spazio cartesiano -È necessario calcolare la cinematica inversa per poter calcolare i valori da mandare ai giunti -Problemi nel caso in cui si passi anche solo vicino a punti singolari (jacobiano inverso non calcolabile)

29 Calcolo traiettorie nello spazio dei giunti Uso di cubiche -Metodo molto diffuso perché molto efficiente -I punti da raggiungere sono specificati nello spazio cartesiano tramite punti di partenza, di arrivo ed eventualmente punti di via. (la cinematica inversa viene utilizzata per calcolare i valori corrispondenti ai giunti) -Viene evitato il problema di passare per punti singolari -Non è possibile tenere conto delle condizioni del robot: estensione del braccio, peso trasportato.

30 Calcolo traiettorie nello spazio dei giunti Uso di polinomi di grado superiore al terzo -Si possono specificare posizioni, velocità, accelerazioni nei punti iniziali e finali. (polinomio ordine 5) -Permettono di gestire anche la continuità dell accelerazione nei punti di via (polinomi di ordine 7). Svantaggi: -il grado del polinomio dipende da n e per elevati valori la quantità di calcoli da eseguire può essere notevole. -Se si varia anche un solo singolo punto è necessario ricalcolare l intero polinomio. -Si ottengono delle soluzioni che presentano in generale delle oscillazioni indesiderate.

31 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 31

32 Controllo del moto dei giunti Il 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 32 conformità con le specifiche sulle prestazioni.

33 Controllo indipendente ai giunti Solitamente nei sistemi industriali, il sistema di controllo tratta ogni giunto del braccio come un semplice servomeccanismo indipendente dagli altri. Gli effetti di accoppiamento tra i vari giunti dovuti alla configurazione e al moto sono trattati come ingressi di disturbo. 33

34 Controllo indipendente ai giunti 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 34

35 Controllo dei giunti Per il controllo ad anello chiuso: - Ogni giunto è controllato indipendentemente - Il controllore del giunto è un PID. X, X C a lc o lo d elle traie tto rie Cin. inv. J -1 θ. θ.. θ C o n tro llo re d e l g iu n to 1 C o n tro llo re d e l g iu n to 2... fo rze/m o m e n ti ro b o t (sensori in te rn i) C è un controllore per ogni giunto le variabili θ indicate sono dei vettori C o n tro llo re d e l g iu n to n θ. θ.. θ 35

36 Controllore Cinematico Cartesiano. X, X errore Cinematica Inversa J -1 X. X θ forze / momenti controllore. (guadagno) J = Jacobiano θ J 1 = Jacobiano inverso robot (sensori interni) Cinematica Diretta J θ. θ notare che il controllo è eseguito sulle variabili cartesiane su cui è calcolato l errore Oss: Se si movimentano degli oggetti diventa importante anche dinamica (considerare accelerazioni) la 36

37 Controllore nello spazio dei giunti Per risolvere il problema del controllo nello spazio dei giunti è necessario risolvere due problemi: 1) Inversione cinematica per convertire le specifiche di moto espresse nello spazio operativo in specifiche di moto nello spazio dei giunti 2) Sistema di controllo nello spazio dei giunti in grado di garantire l inseguimento dei riferimenti ottenuti tramite l inversione cinematica. 37

38 Controllo nello spazio dei giunti Inversione q d Controllore Attuatori Trasmissioni Manipolatore x x d Cinematica Trasduttori q 38

39 Controllo di un manipolatore nello spazio dei giunti Sistema di controllo PID (regolatore ad azione Proporzionale- Integrativa-Derivativa) In generale assume la seguente formulazione matematica: Y = K e + K e& + K p q d q i e q ( t) dt e p = q d q e& p = de dt p q q d = θ = θ d segnale di comando segnale in uscita 39

40 PID anti wind-up Il fenomeno di caricamento e scaricamento, legato al fatto di avere processi reali che saturano l uscita, viene chiamato wind-up. Il fenomeno si presenta quando l integratore si carica continuamente a causa di un segnale di errore elevato una volta che il processo è in saturazione. 40

41 PID anti wind-up Quando il controllore satura il termine integrale viene ricalcolato, diminuendo la sua azione di una quantità proporzionale alla profondità di saturazione. 41

42 PID anti wind-up Risposta al gradino (con anti wind-up) 42

43 Algoritmo controllore Implementazione software della funzione PID Disponendo di un sistema a microprocessore, capace di elaborare solo grandezze discrete, è necessario discretizzare la funzione PID. La corrispondente funzione analogica viene aggiornata e simulata tramite software. Derivando rispetto al tempo ambo i membri della precedente equazione, si ottiene la seguente forma discreta utilizzata per il calcolo della funzione PID: y = f ( x) = K1X + K 2 Xdt + K 3 dx dt dy dt = K 1 dx dt + K 2 X + K 3 2 d X 2 dt u( t) = θ ( t) d y( t) = θ ( t) segnale di comando segnalein uscita X(t) = e(t) = u(t) y(t) Errore 43

44 Algoritmo del controllo ( ) ( ) ( ) T K K T K K K K X X X K X K X X K Y Y T X X X X K X K T X X K T Y Y d i p n n n d n i n n p n n n n n n n n n n n ) ( ) ( ) ( = = = = + + = T: tempo di aggiornamento (intervallo tra due successivi aggiornamenti dell uscita Y). Xn : ultimo valore campionato. L accuratezza della funzione è tanto più elevata quanto più frequenti sono i campionamenti. La discretizzazione si traduce in un approssimazione a gradini dell andamento della variabile continua nel sistema reale. 44

45 Risposta discretizzata

46 Algoritmo del controllo La scelta della frequenza di campionamento deve nascere da un buon compromesso tra esigenze contrapposte. Il costo del sistema impone di scegliere un valore basso affinchè la potenza di calcolo necessaria per eseguire gli algoritmi di controllo sia minima e avere convertitori A/D D/A economici. Per avere buone prestazioni del sistema di controllo è necessario non scendere oltre un determinato limite inferiore per la frequenza di campionamento. Capacità di compensare i disturbi che agiscono sul processo Capacità di filtrare i rumori associati ai segnali provenienti dai sensori Capacità di inseguire il riferimento 46

47 Algoritmo del controllo Codice eseguito ad ogni periodo di campionamento. 1.Acquisizione ingressi e conversione A/D 2.Calcolo variabile di controllo u(t) 3.Aggiornamento dell uscita y(t) 4.Aggiornamento dello stato Oss: è necessario che il tempo computazionale della routine di controllo sia inferiore al tempo di campionamento scelto durante la sintesi del regolatore PID. Il mancato rispetto di questo vincolo porterebbe ad instabilità. 47

48 Hardware per sistemi di controllo MPC555 Features 40MHz Core with Floating Point Unit 26 Kbytes of Static RAM 448 Kbytes Flash EEPROM Memory with 5-V programming (CMF) Flexible Memory Protection Unit General-Purpose I/O Support Two Time Processor Units (TPU3) 18-Channel Modular I/O System (MIOS1) Two Queued Analog-to-Digital Converter Modules (QADC) Two CAN 2.0B Controller Modules (TouCANs) Queued Serial Multi-Channel Module (QSMCM) U-Bus System Interface Unit (USIU) Microcontrollore Motorola MPC555 48

49 Hardware per sistemi di controllo Controllore multiassi Galil (azienda USA leader mondiale nel settore) modello DMC-2143, dotato di interfaccia Ethernet che ne consente il comando e la supervisione da PC. Il controllore si fa carico della generazione dei comandi di movimento da inviare agli stadi di potenza dei motori, della lettura degli encoder che restituiscono la posizione degli assi chiudendo il loop di controllo, della gestione dei sensori di finecorsa e di home di ciascun asse. Modello DMC-2143 Galil 49

50 Controllore del Braccio PUMA 6 GDL 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. 50

51 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. 51

52 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, infatti il robot Puma a velocità ridotte risulta essere soggetto a forti vibrazioni. 52

53 Robot Industriali (Breve Panoramica) 53

54 George Devol e Engelberger fondano nel 1956 la prima azienda di Robotica UNIMATION (USA) Aziende produttrici nel Mondo ABB (Nata in Europa dalla fusione della svedese ASEA e della svizzera BBC, leader nella Robotica) FANUC (USA-Giapponese, 1986) STÄUBLI (Europea, 1989, compera la Unimation) ADEPT (1983, USA) MOTOMAN (USA, 1989) KUKA (Tedesca) KAWASAKI (1969, Giapponese) 54

55 ABB Robots Installazioni 55

56 Esempio Manipolatore ABB: Applicazioni Saldatura ad arco Assemblaggio Pulizia Taglio ed eliminazione sbavature Incollaggio e sigillatura - E il robot più utilizzato al mondo (14000 installazioni) 56

57 Saldatura ad arco Eliminazione sbavature Taglio 57

58 58

59 Axis 3 Axis 2 Accurato Il migliore nella propria classe per quanto riguarda l accuratezza del percorso e la ripetibilità della posizione (RP = 0.06 mm). Axis 1 59

60 Sicurezza nei Robot I "sistemi di sicurezza" principali presenti in una cella robotica si suddividono in: barriere fisiche: reti, barre luci lampeggianti sistemi elettronici: rilevano la presenza di di un intruso nello spazio di lavoro del robot segnali di pericolo sistemi intelligenti: sono in grado di arrestare il robot o di alterare automaticamente la traiettoria 60

61 Filmato Il controllo dei robot viene effettuato utilizzando Matlab e Simulink Robots ABB, task coordinato - Controllo di forza sull End-Effector - Fase prototipale Nella prima parte della dimostrazione i robot cooperano per assemblare un dado e un perno. Nella seconda parte un robot prende una sbarra e la passa al secondo robot che provvede a metterla nella 61 posizione iniziale.