Tesina CdR Falanga-Fontanelli

 UNIVERSITÀ DEGLI STUDI DI NAPOLI FEDERICO II FACOLTÀ DI INGEGNERIA !
!
TESINA DI CONTROLLO DEI ROBOT !!
____________________________________________________________________
Modellistica, pianiAicazione e controllo di un robot SCARA e di un braccio antropomorfo ____________________________________________________________________
!
!
!
!
Candidati Davide FALANGA M58/48 Andrea FONTANELLI M58/64 !
!
!
!
!
Docente Ch.mo Prof. Ing. Bruno SICILIANO Anno Accademico 2013/2014 !
!
INTRODUZIONE Indice !
!
!
1 -­‐ MODELLISTICA 1.1 ANALISI CINEMATICA 1.3 ANALISI DELLA MANIPOLABILITÀ 1.4 RIDONDANZA CINEMATICA 1.5 MODELLO DINAMICO 1.6 PARAMETRIZZAZIONE DEL MODELLO !
2 -­‐ PIANIFICAZIONE 2.1 PIANIFICAZIONE DELLE TRAIETTORIE NELLO SPAZIO OPERATIVO 2.2 INVERSIONE CINEMATICA !
3 -­‐ CONTROLLO 3.1 CONTROLLO ROBUSTO 3.2 CONTROLLO ADATTATIVO 3.3 CONTROLLO DI IMPEDENZA 3.4 CONTROLLO DI FORZA CON ANELLO INTERNO DI POSIZIONE 3.5 CONTROLLO DI FORZA CON ANELLO INTERNO DI VELOCITÀ 3.6 CONTROLLO PARALLELO FORZA/MOTO !
APPENDICE A.1 TABELLE DI D-­‐H E MATRICI DI TRASFORMAZIONE A.2 JACOBIANO -2
! -
A.3 MATRICI DEL MODELLO DINAMICO A.4 REGRESSORE E PARAMETRI DINAMICI A.5 PARAMETRI DEL ROBOT ANTROPOMORFO !
BIBLIOGRAFIA !
!
-3
! -
INTRODUZIONE Con il presente lavoro si intende effettuare un’analisi cinematica e dinamica di
due differenti tipologie di manipolatori: uno SCARA ed un antropomorfo a 5
assi. I due robot saranno quindi confrontati per evidenziare le differenze sia
in termini di comportamento che di tecniche utilizzate tanto per l’analisi dei
manipolatori quanto per la sintesi dei sistemi di controllo.
!
Per non appesantire la lettura dell’elaborato, la maggior parte delle equazioni
e delle matrici utilizzate per la modellistica, il controllo e la simulazione sono
riportate in Appendice. Inoltre, tutte le equazioni cui si farà riferimento
provengono da [1]. Molte delle informazioni legate al robot antropomorfo
(matrici di trasformazione, Jacobiano e modello dinamico) sono riportate in un
M-File allegato al presente documento.
!
Si noti che per il manipolatore SCARA sono stati sviluppati tutti i punti
richiesti dalla traccia dell’elaborato tecnico, mentre per quanto concerne
l’antropomorfo ci si è soffermati soltanto sull’analisi della cinematica, sul
modello dinamico senza il calcolo del regressore e sulla sintesi di un controllo
robusto.
!
Nel Cap. 1 viene presentata l’analisi cinematica e dinamica dei due robot.
!
Nel Cap.2 sono illustrate la traiettoria generata come riferimento nello spazio
operativo e le tecniche adottate per l’inversione della stessa per ottenere dei
riferimenti nello spazio dei giunti.
!
Nel Cap. 3 sono mostrati i risultati ottenuti con diverse tecniche di controllo,
sia del moto che della forza.
!
!
!
!
-4
! -
1 -­‐ MODELLISTICA !
1.1 ANALISI CINEMATICA Per effettuare l’analisi cinematica dei due manipolatori sono state disposte
come di seguito le terne di riferimento secondo la convenzione di Denavit e
Hartenberg:
!
!
!
!
-5
! -
Sulla base dei dati di progetto sono state dunque calcolate le matrici di DH, le
quali sono riportate nell’appendice A.1.
!
Tramite la relazione (2.52) proposta in [1] sono state calcolate le matrici di DH; le matrici di trasformazione tra terna base e end-effector sono state
calcolate utilizzando la relazione (2.61). Anche in questo caso, tali matrici
sono riportate in appendice.
!
Da queste ultime è stato possibile ottenere le funzioni cinematiche dirette. In
particolare, per ottenere l’orientamento ci si è riferiti alla convenzione degli
angoli di Eulero ZYX per lo SCARA utilizzando le relazioni (2.22), mentre per
l’antropomorfo è stat adottata la convenzione dei quaternioni unitari grazie
alle relazioni (2.34) e (2.35).
!
Per l’analisi della cinematica differenziale è stato calcolato lo Jacobiano
geometrico dei due robot, utilizzando la relazione (3.30). Tali matrici sono
riportate in appendice A.2. Per quanto riguarda lo Jacobiano analitico, nel caso
dello SCARA coincide con lo Jacobiano geometrico in quanto tutte le rotazioni
possibili sono intorno ad assi paralleli, mentre per l’antropomorfo è stata
utilizzata la rappresentazione dell’orientamento tramite la convezione dei
quaternioni unitari, per cui non è stato necessario il calcolo dello Jacobiano
analitico.
!
1.3 ANALISI DELLA MANIPOLABILITÀ Per analizzare la manipolabilità delle due strutture sono stati ricavati gli
ellissoidi di manipolabilità in velocità ed in forza adoperando le relazioni
proposte a pag. 153 di [1].
!
Questi ultimi sono riportati nelle figure di seguito per alcuni punti dello
spazio operativo del robot SCARA (p1 = [0.8, 0, 0], p2=[0.1, 0, 0], p3=[-0.8, 0,
0], p4=[-0.1, 0, 0]).
!
-6
! -
!
Ellissoidi di manipolabilità del robot SCARA
Per l’antropomorfo, si riportano invece gli ellissoidi di manipolabilità in 3D,
ottenuti nel primo caso analizzando alcuni punti dello spazio operativo ove il
robot gode di una buona manipolabilità (p1 = [0.5, 0, 0.5], p2=[0.3, 0, 0.1],
p3=[-0.1, 0.1, 0.4], p4=[0.3, 0.2, 0.4], p5=[-0.3, 0.2, 0.5]), nel secondo invece in
prossimità delle configurazioni di singolarità di spalla e di gomito.
!
Ellissoidi di manipolabilità del robot antropomorfo
-7
! -
!
Ellissoidi di manipolabilità del robot antropomorfo in singolarità di spalla e di gomito
Si noti che in prossimità della singolarità di gomito l’ellissoide in velocità
degenera in un piano parallelo all’asse Y, mentre quello in forza degenera in
una retta ortogonale a tale piano: ciò implica l’impossibilità di esercitare
velocità in tale direzione ed elevate forze nelle direzioni parallele all’asse X.
!
Per quanto riguarda la singolarità di spalla, nella direzione ortogonale al piano
formato dai bracci 2 e 3 non è possibile sviluppare velocità in quanto tale
direzione è impedita dalla configurazione. L’ellissoide in forza degenera in
una retta in quanto la forza in tale direzione è compensata dalla struttura: in
via teorica sarebbe dunque possibile esercitare forze infinite.
!
1.4 RIDONDANZA CINEMATICA Per analizzare la ridondanza cinematica sono state rilassate a turno le
componenti X e φ (orientamento) dello spazio operativo per il robot SCARA. In
ciascun caso è stato considerato uno Jacobiano ridotto, rimuovendo le righe
relative alle componenti dello spazio operativo rilassate. L’analisi della
ridondanza è stata effettuata utilizzando come riferimenti nello spazio dei
giunti delle traiettorie ottenute tramite inversione cinematica con gli algoritmi
CLIK descritti nel paragrafo 2.2 tramite pseudo-inversa minima a destra.
-8
! -
!
Rilassando lo spazio operativo i robot sono diventati funzionalmente
ridondanti, per cui è stato possibile migliorarne la destrezza proiettando nel
nullo dello Jacobiano un’opportuna velocità nello spazio dei giunti ottenuta
utilizzando le relazioni (3.55) e (3.56), massimizzando così la misura di
manipolabilità.
!
Nel caso in cui venga rilassata la componente X dello spazio operativo si
evidenziano importanti miglioramenti della manipolabilità del robot. Difatti,
utilizzando come funzione obiettivo la (3.56) per l’esecuzione del task
secondario si ottiene un comportamento tale da far allontanare il robot dai
punti di singolarità.
!
!
!
Traiettorie nello spazio dei giunti quando il vincolo sulla X è rilassato
-9
! -
!
Misura di manipolabilità quando il vincolo sulla X è rilassato
Viceversa, rilassando la componente relativa all’orientamento non vi è alcun
miglioramento nella manipolabilità della struttura. Ciò, come prevedibile, è
conseguenza del totale disaccoppiamento tra posizionamento e orientamento
nello spazio operativo.
!
!
!
!
!
!
!
!
!
!
- 10
!
-
!
Traiettorie nello spazio dei giunti quando il vincolo sull’orientamento è rilassato
!
Misura di manipolabilità quando il vincolo sull’orientamento è rilassato
- 11
!
-
Non si rilevano invece cambiamenti significativi per quanto riguarda l’errore
di inseguimento della traiettoria, in quanto il task secondario non influisce
sull’esecuzione del task primario giacché le velocità q0 sono proiettate nel
nullo dello Jacobiano.
!
1.5 MODELLO DINAMICO Il modello dinamico dei due manipolatori è stato calcolato utilizzando la
formulazione di Lagrange. In particolare, sono state calcolate le matrici
presenti nell’equazione (7.42) grazie alle relazioni (7.32), (7.39) e (7.45). Tali
matrici sono riportate in appendice A.3.
!
La matrice C è stata calcolata adoperando la fattorizzazione dei simboli di
Christoffel del primo tipo. Si noti per lo SCARA la dipendenza dalla
configurazione soltanto di tre termini della matrice di inerzia (ovvero b1,1, b1,2,
b2,1) per cui la maggior parte dei termini della matrice relativa alla forza
centrifuga ed all’effetto di Coriolis sono nulla.
!
Nel caso del manipolatore antropomorfo, come prevedibile, la dipendenza
dalla configurazione delle matrici del modello dinamico è ben più complessa:
mentre nel caso dello SCARA gli assi di rotazione sono tutti paralleli, ciò non
accade con l’antropomorfo e ciò causa un maggiore grado di accoppiamento
tra le dinamiche dei vari link.
!
Sempre con riferimento al robot antropomorfo, per il calcolo del modello
dinamico per le grandezze che caratterizzano la struttura sono stati utilizzati
i valori ottenuti a partire dal relativo modello CAD (momenti di inerzia, masse,
posizione dei baricentri). Le informazioni relative ai motori sono state invece
ricavate dai datasheet degli stessi. Per la stima dell’attrito si è supposto che la
coppia di attrito fosse pari al 10% della coppia erogata, per cui il coefficiente
di attrito è stato calcolato come il 10% del rapporto tra coppia nominale e
velocità nominale.
!
- 12
!
-
I parametri del robot antropomorfo sono riportati in appendice A.5.
!
Modello CAD del robot antropomorfo
!
1.6 PARAMETRIZZAZIONE DEL MODELLO Il modello dinamico del robot SCARA è stato riscritto in funzione di un
apposito vettore di parametri dinamici di dimensioni pari a 14, sfruttando la
proprietà notevole di linearità del modello dinamico di un manipolatore
rispetto a tali parametri (Eq. 7.81).
!
Per ottenere il vettore dei parametri sono state analizzate le quattro equazioni
che esprimono il legame tra la coppia a ciascun giunto, le matrici del modello
dinamico, le variabili di giunto e le loro derivate prima e seconda. È stato così
possibile individuare una serie di termini comuni a molte equazioni, i quali
sono stati utilizzati così come parametri dinamici.
!
In appendice A.4 sono riportati i parametri individuati ed il regressore che
deriva da tale scelta.
- 13
!
-
2 -­‐ PIANIFICAZIONE !
2.1 PIANIFICAZIONE DELLE TRAIETTORIE NELLO SPAZIO OPERATIVO Al fine di simulare il comportamento dei due robot è stata pianificata una
traiettoria nello spazio operativo interna allo spazio di lavoro di ciascun
manipolatore. La traiettoria scelta presenta tre tratti circolari, due
semicircolari e quattro tratti lineari con due punti di via.
!
L’obiettivo finale è stata la simulazione di un’operazione di fresatura del
simbolo “Yin & Yang” [2] su di un piano orizzontale avente quota pari a zero. I
tratti lineari ed i punti di via sono stati utilizzati per realizzare il distacco
dell’utensile dal piano di lavoro per passare tra segmenti di traiettoria non
contigui.
!
Di seguito, la traiettoria nello spazio operativo:
!
!
!
Traiettoria nello spazio operativo: vista 3D
!
- 14
!
-
!
Traiettoria nello spazio operativo: vista dall’alto (piano XY)
2.2 INVERSIONE CINEMATICA La traiettoria nello spazio operativo è stata convertita in una traiettoria nello
spazio dei giunti tramite l’utilizzo di algoritmi di inversione cinematica a ciclo
chiuso.
!
In particolare, sono stati testati gli algoritmi CLIK del primo ordine con inversa
(Fig. 3.11 di [1]) e trasposta (Fig. 3.12) dello Jacobiano, così come quello del
secondo ordine (Fig. 3.14). In particolare, per il robot antropomorfo, dal
momento che lo Jacobiano risulta essere rettangolare alto (6x5) e dunque è
stato necessario adoperare una pseudo-inversa minima a sinistra.
!
- 15
!
-
Di seguito si riportano i risultati in termini di traiettorie e velocità nello spazio
dei giunti e di errore nello spazio operativo per il robot SCARA:
!
!
!
CLIK primo ordine con inversa dello Jacobiano (Kp = [500, 500, 500, 500])
!
!
- 16
!
-
CLIK primo ordine con trasposta dello Jacobiano (Kp = [500, 500, 500, 500])
- 17
!
-
!
- 18
!
-
CLIK secondo ordine con trasposta dello Jacobiano (Kp = [1000, 1000, 1000, 500],
!
Kd =[500, 500, 500, 250])
Per il robot antropomorfo, invece, i risultati ottenuti sono i seguenti:
!
!
- 19
!
-
CLIK primo ordine con inversa dello Jacobiano (Kp = [700 700 700 300 300 300])
CLIK primo ordine con trasposta dello Jacobiano (Kp = [1500 1500 1500 100 100 100])
- 20
!
-
CLIK secondo ordine con trasposta dello Jacobiano (Kp = [700 700 700 200 200 200],
!
Kd = [500 500 500 250 250 250])
- 21
!
-
Analizzando i risultati ottenuti nei tre casi, come prevedibile dalla teoria, è
stato possibile evidenziare una maggiore precisione nell’inseguimento di una
traiettoria variabile nel tempo da parte degli algoritmi basati sull’inversa dello
Jacobiano, a discapito di una maggiore complessità computazionale. La
soluzione basata sulla trasposta, invece, risulta essere più veloce
nell’esecuzione ma adatta esclusivamente alla risoluzione di problemi di
regolazione.
!
Per le ragioni di cui sopra, per l’inizializzazione cinematica dei manipolatori si
è fatto uso di un CLIK del primo ordine con trasposta, mentre per la
generazione delle traiettorie nello spazio dei giunti è stato utilizzato un CLIK
del secondo ordine.
- 22
!
-
3 -­‐ CONTROLLO 3.1 CONTROLLO ROBUSTO La prima tecnica di controllo del moto testata sui due manipolatori è quella
basata sul controllo robusto (Fig. 8.23 di [1]). A tal fine l’azione linearizzante
e disaccoppiante è stata ottenuta sulla base di versioni volutamente
approssimate delle matrici del modello dinamico come previsto
dall’equazione (8.62).
!
Nello specifico, si è scelto di utilizzare una matrice d’inerzia costante e
diagonale, mediando a zero tutti i termini legati alla configurazione in quanto
dipendenti da seni e coseni delle variabili di giunto. La matrice legata a forze
centrifughe e di Coriolis è stata trascurata, mentre sono state lasciate
inalterate le matrici di attrito e gravità.
!
Per il manipolatore SCARA sono stati utilizzati i seguenti guadagni:
Kp = diag([100 100 50 100])
Kd = diag([10 10 5 10])
Q3 = Kd
Q2 = 10Kp
ρ = 10
adottando una soglia ε = 1 per saturare il controllo a versore (Eq. 8.89).
Di seguito, i risultati ottenuti:
Traiettoria desiderata ed effettiva nello spazio operativo
- 23
!
-
Coppie richieste agli attuatori
Errori nello spazio operativo (posizione) e dei giunti (posizione e velocità)
- 24
!
-
!
Si noti l’assenza di chattering nelle azioni di controllo dei vari giunti grazie ad
una scelta sufficientemente ampia della soglia ε. Effettuando diverse
simulazioni per diversi valori di quest’ultima, il fenomeno del chattering si
evidenzia soltanto a partire da valori pari a ε=0.1, come riportato nelle
seguenti figure:
!
Coppie richieste agli attuatori
!
!
!
!
!
!
!
!
!
- 25
!
-
!
!
!
!
!
!
!
!
!
!
!
!
!
!
!
Errori nello spazio operativo (posizione) e dei giunti (posizione e velocità)
- 26
!
-
Per il manipolatore antropomorfo sono stati utilizzati i seguenti guadagni:
Kp = diag([200 200 200 100 10])
Q2 = 10Kp
Kd = diag([20 20 20 10 5])
Q 3 = Kd
ρ = 10
scegliendo ε =1. I risultati ottenuti sono di seguito riportati:
!
Traiettoria desiderata ed effettiva nello spazio operativo
Coppie richieste agli attuatori
!
!
- 27
!
-
!
Errori nello spazio operativo (posizione) e dei giunti (posizione e velocità)
Dal momento che le coppie massime erogabili dagli attuatori sono note grazie
ai datasheet dei motori è stato possibile tarare i guadagni in maniera tale da
contenerle all’interno dell’intervallo ammissibile.
!
Anche per il manipolatore antropomorfo il fenomeno del chattering è visibile a
partire da ε =0.1.
!
!
Coppie richieste agli attuatori
- 28
!
-
Errori nello spazio operativo (posizione) e dei giunti (posizione e velocità)
!
3.2 CONTROLLO ADATTATIVO La seconda tecnica per il controllo del moto adottata è quella basata sul
controllo adattativo (Fig. 8.26 in [1]). Per realizzare lo schema di controllo è
stato dunque utilizzata la proprietà notevole di linearità del modello dinamico
rispetto ai parametri, utilizzando dunque il regressore proposto in appendice
A.4.
!
Il task assegnato al robot prevede l’esecuzione di una traiettoria con una
massa di circa 4 Kg concentrata in punta. L’utilizzo di tale tecnica di controllo
ha dunque permesso la stima del suddetto carico in maniera automatica,
garantendo così un buon inseguimento della traiettoria richiesta.
!
Nello specifico, per la simulazione del robot tramite la dinamica diretta la
massa ml4 (quattordicesimo parametro dinamico, appendice A.4) è stata posta
pari proprio a 4 Kg, così da simulare il comportamento reale del robot con
l’oggetto afferrato tramite l’end-effector. L’algoritmo di controllo è stato
invece sintetizzato considerando nulla tale massa, la quale è stata così
identificata online in maniera soddisfacente:
!
- 29
!
-
Tale parametro è l’unico sul quale è presente incertezza, per cui l’operazione
di identificazione si è concentrata soltanto su di esso. Ciò è stato possibile
utilizzando nella legge di aggiornamento dei parametri (Eq. 8.104) una matrice
dei guadagni Kπ pari alla matrice identità tranne che per l’elemento in
posizione (14,14), il quale è stato invece posto pari a 0.001. La conseguenza è
stata un adattamento dei parametri dinamici del robot legato quasi
esclusivamente alla massa ml4.
Stima del carico in punta
Per quanto concerne gli altri guadagni richiesti per l’azione di controllo, sono
stati scelti:
!
Kd = diag([2000 2000 5000 2000])
λ = diag([5 5 5 5])
!
Di seguito, la traiettoria eseguita nello spazio operativo, l’errore nello spazio
dei giunti e le coppie richieste agli attuatori:
- 30
!
-
!
Traiettoria nello spazio operativo
Coppie richieste
- 31
!
-
Errore nello spazio operativo (posizione) e nello spazio dei giunti (posizione e velocità)
!
3.3 CONTROLLO DI IMPEDENZA La prima tecnica di controllo dell’interazione adottata è quella legata al
controllo di impedenza. In particolare, il robot si comporterà nei confronti
dell’ambiente come un’impedenza meccanica pura (ovvero lineare e
disaccoppiata) avente massa equivalente pari a Md, smorzamento pari a Kd e
rigidezza pari a Kp, come previsto dall’eq. (9.36).
!
Il controllo è stato effettuato tramite una feedback linearization con
compensazione della forza in punta basata sull’eq. (9.30), con azione
stabilizzante basata sull’eq. (9.31). In quest’ultima è stato utilizzato il vettore
delle forze agenti in punta he in luogo del vettore hA in quanto lo Jacobiano
geometrico coincide con quello analitico.
!
- 32
!
-
Nello specifico, i valori scelti per queste tre grandezze sono i seguenti:
!
Md = diag([1 1 1 1])
Kd = diag([70 70 70 70])
Kp =diag([40 40 40 40])
!
Per testare il controllo è stata generata una nuova traiettoria che, partendo da
una quota z=0.5, prevede prima un tratto di discesa verticale lineare, quindi
viene richiesto di compiere una traiettoria circolare nel piano XY. Lungo il
tratto di discesa viene simulata la presenza alla quota z=0.1 di un piano
avente rigidezza pari a 3*10^3 N/m nella sola direzione verticale.
!
I risultati ottenuti sono riportati di seguito:
Traiettoria nello spazio operativo
!
!
!
!
!
!
!
!
- 33
!
-
!
Errore nello spazio operativo
Coppie richieste
!
Si noti come il controllo di impedenza sia un controllo indiretto della forza:
quest’ultima dipende dalla superficie con la quale avviene l’interazione
secondo la relazione (9.38) con la quale per l’appunto è stato modellato
l’ambiente nella simulazione. La forza raggiunge all’impatto un picco massimo
di -17 N per poi raggiungere un valore di -4 N.
- 34
!
-
!
Forza al contatto lungo la direzione verticale
!
3.4 CONTROLLO DI FORZA CON ANELLO INTERNO DI POSIZIONE Nel presente e nei successivi due paragrafi si intende illustrare le metodologie
utilizzate per realizzare un controllo diretto della forza. In particolare, sono
state analizzate tecniche basate sull’utilizzo di un anello interno di posizione,
di un anello interno di velocità ed un controllo parallelo forza/moto.
!
In tutti i casi è stato adoperato anche un anello esterno di forza, così da poter
regolare il valore della stessa sulla base di un riferimento pari a 40 N.
!
Nel caso del controllo con anello interno di posizione (Fig. 9.6), sono stati
utilizzati i seguenti valori:
!
Md = diag([1 1 1 1])
Kd = diag([20 20 100 200])
Kf = diag([0 0 2 0])
Kp =diag([20 20 0.1 20])
Ki = diag([0 0 4 0])
!
!
!
!
- 35
!
-
I risultati ottenuti sono riportati di seguito:
Forza al contatto lungo la direzione verticale
Errore nello spazio operativo lungo l’asse z
!
- 36
!
-
Coppie richieste
!
3.5 CONTROLLO DI FORZA CON ANELLO INTERNO DI VELOCITÀ Per ottenere uno schema di controllo diretto della forza con loop interno di
velocità è sufficiente modificare lo schema in Fig. 9.6 aprendo l’anello di
retroazione della posizione. In tal caso, l’output del controllore di forza
diventa un riferimento di velocità.
!
Per la simulazione sono stati utilizzati i seguenti valori:
!
Md = diag([1 1 1 1])
Kd = diag([20 20 100 200])
Kf = diag([0 0 2 0])
Kp =diag([20 20 0.1 20])
Ki = diag([0 0 4 0])
!
I risultati sono visibili di seguito:
!
- 37
!
-
!
Forza al contatto lungo la direzione verticale
!
Coppie richieste
Come prevedibile, in simulazione il comportamento del manipolatore risulta
essere particolarmente soddisfacente. Tuttavia, un’applicazione di una simile
legge di controllo in un contesto reale porterebbe a risultati lontani da quelli
ottenuti a causa dell’assenza di un’azione integrale nel controllore di forza:
ciò implica l’impossibilità di reiettare disturbi legati a dinamiche non
modellate sulla forza.
!
- 38
!
-
3.6 CONTROLLO PARALLELO FORZA/MOTO La terza ed ultima soluzione adottata per il controllo della forza è quella
basata su di una tecnica che prevede il controllo in parallelo della forza e del
moto. In particolare, lungo le direzioni di moto non vincolato viene controllata
posizione, mentre in quelle vincolate la forza.
!
In questo caso sono state adottate due soluzioni progettuali differenti: in una
il robot presenta una rigidezza costante lungo l’asse Z, nell’altra invece
modifica automaticamente la rigidezza in tale direzione una volta rilevato il
contatto con il piano, così da poter ottenere un comportamento cedevole nei
confronti dell’ambiente.
!
Nel caso di controllo senza modifica del comportamento, sono stati utilizzati i
seguenti valori:
Md = diag([1 1 1 1])
Kp =diag([5000 5000 0.1 5000])
Kd = diag([20 20 50 20])
Kf = diag([0 0 2 0])
Ki = diag([0 0 4 0])
!
Ottenendo i seguenti risultati:
!
Traiettoria nello spazio operativo
!
- 39
!
-
Forza lungo l’asse z
Errore di posizione nello spazio operativo
Coppie richieste
- 40
!
-
!
Nel secondo caso sono stati utilizzati i seguenti valori nel caso in cui non
venga rilevato il contatto:
!
Kd = diag([200 200 200 200])
Kp =diag([50000 10000 5000 10000])
!
Al contatto, il robot modifica tali parametri nei seguenti:
!
Kd = diag([50 50 150 50])
Kp =diag([5000 5000 0.1 5000])
!
I risultati ottenuti sono i seguenti:
!
!
!
!
!
!
!
!
!
Traiettoria nello spazio operativo
- 41
!
-
Forza lungo l’asse z
!
!
Errore di posizione nello spazio operativo
!
- 42
!
-
Coppie richieste
- 43
!
-
APPENDICE !
A.1 TABELLE D-­‐H E MATRICI DI TRASFORMAZIONE • Tabella di D-H del robot SCARA:
Braccio
ai
αi
di
θi
1
0,5
0
1
θ1
2
0,5
0
0
θ2
3
0
0
d3
0
4
0
0
0
θ4
!
• Matrice di trasformazione del robot SCARA:
T0e (q) = [ - s124
c124
0
a2c12 + a1c1
c124
s124
0
a2s12 + a1s1
0
0
-1
d0 + d3
0
0
0
-1
]
!
• Matrice di D-H del robot antropomorfo:
Braccio
ai
αi
di
θi
1
0
π/2
0,229
θ1
2
0,284
0
0
θ2
3
0,265
0
0
θ3
4
0
-π/2
0
θ4
5
0
0
0,12
θ5
!
• Matrice di trasformazione del robot antropomorfo:
La matrice di trasformazione è riportata nel M-file allegato al
documento.
!
!
!
- 44
!
-
A.2 JACOBIANO A.2.1 Robot SCARA
J(q) =
[ - a2s12 - a1s1
- a1s12
0
0
a2c12 + a1c1
a2c12
0
0
0
0
1
0
1
1
0
1]
!
A.2.2 Robot antropomorfo
Lo Jacobiano è riportato nel M-File allegato al documento.
!
!
A.3 MATRICI DEL MODELLO DINAMICO A.3.1 Robot SCARA
• Matrice di inerzia:
b1,1 = Il1 + Il2 + Il3 + Il4 + Im1 Kr12 + Im2 + Im3 + Im4 + a12 ml1 +
+ (a12 +a22)(ml2 + ml3 + ml4) + (l1 - 2a1)ml1l1 + (l2 - 2a2)ml2l2 +
+ 2a1c2[a2(ml2 + ml3 + ml4) - ml2 l2]
!
b2,2 = Il2 + Il3 + Il4 + Im2 Kr22 + Im3 + Im4 + a22(ml2 + ml3 + ml4) +
+ (l2 - 2a2)ml2l2
!
b3,3 = Im3 Kr32 + ml3 + ml4
!
b4,4 = Il4 + Im4 Kr42
!
b1,2 = b2,1 = Il2 + Il3 + Il4 + Im2 Kr2 + Im3 + Im4 +
+ a22(ml2 + ml3 + ml4) + (l2 - 2a2)ml2l2 +
+ a1c2[a2(ml2 + ml3 + ml4) - ml2l2]
!
b1,3 = b2,3 = b3,1 = b3,2 = Im3 Kr3
!
- 45
!
-
b1,4 = b2,4 = b4,1 = b4,2 = Il4 + Im4 Kr4
!
b3,4 = b4,3 = 0
!
•Matrice di forza centrifuga e Coriolis:
c1,1 = [ -a1a2(ml2 + ml3 + ml4) - a1ml2l2 ] s2 dot_q2
c1,2 = [ -a1a2(ml2 + ml3 + ml4) - a1ml2l2 ]s2 (dot_q1 + dot_q2)
c2,1 = [ a1a2(ml2 + ml3 - ml4) - a1ml2l2 ] s2 dot_q1
c1,3 = c1,4 = c2,2 = c2,3 = c2,4 = c3,1 = c3,2 = c3,3 = c3,4 = c4,1 = c4,2 = c4,3 = c4,4 = 0
!
•Matrice degli attriti:
Fv = diag([Fm1 Kr12
Fm2 Kr22
Fm3Kr32
Fm4 Kr42])
!
•Matrice di gravità:
g = [0 0 g(ml3 + ml4) 0]
!
A.3.2 Robot antropomorfo
Siccome il modello dinamico del manipolatore antropomorfo risulta essere
particolarmente complicato, si allega in un M-file l’espressione simbolica delle
quattro matrici.
!
!
A.4 REGRESSORE E PARAMETRI DINAMICI • Parametri dinamici:
!
π1 = Il1 + Im1 Kr12 + (l1 - 2a1)ml1l1
π8 =ml2l2
π2 = Il2 + Il3
π9 = Fm1
π3 = Il4
π10 = Fm2
π4 = Im2
π11 = Fm3
π5 = Im3
π12 = Fm4
π6 = Im4
π13 = ml3
π7 = ml2 + ml3 + ml4
π14 = ml4
- 46
!
-
• Regressore:
y1,1 = dot_dot_qr1
y1,2 = dot_dot_qr1 + dot_dot_qr2
y1,3 = dot_dot_qr1 + dot_dot_qr2 + dot_dot_qr4
y1,4 = dot_dot_qr1 + Kr2 dot_dot_qr2
y1,5 = dot_dot_qr1 + dot_dot_qr2 + Kr3 dot_dot_qr3
y1,6 = dot_dot_qr1 + dot_dot_qr2 + Kr4 dot_dot_qr4
y1,7 = dot_dot_qr1 [(a12+a22) + 2a1c2a2] + dot_dot_qr2 (a22 + a1c2a2) - a1a2 [dot_qr1 s2 dot_q2 + dot_qr2 s2 (dot_q1 + dot_q2)]
y1,8 = dot_dot_qr1 [(L2-2a2) - 2a1c2] + dot_dot_qr2 [(L2-2a2) - a1c2] +
+ a1 [dot_qr1 s2 dot_q2 + dot_qr2 s2 (dot_q1 + dot_q2)]
y1,9 = Kr12 dot_qr1
y1,10 = y1,11 = y1,12 = y1,13 = y1,14 = 0
!
!
!
y2,1 = y2,9 = y2,11 = y2,12 = y2,13 = y2,14 = 0
y2,2 = dot_dot_qr1 + dot_dot_qr2
y2,3 = dot_dot_qr1 + dot_dot_qr2 + dot_dot_qr4
y2,4 = Kr2 dot_dot_qr1 + Kr22 dot_dot_qr2
y2,5 = dot_dot_qr1 + dot_dot_qr2 + Kr3 dot_dot_qr3
y2,6 = dot_dot_qr1 + dot_dot_qr2 + Kr4 dot_dot_qr4
y2,7 = dot_dot_qr1(a22 + a1c2a2) + dot_dot_qr2 a22 + dot_qr1 a1 a2 s2 dot_q1
y2,8 = dot_dot_qr1 [(L2-2a2)-a1c2] + dot_dot_qr2 (L2-2a2) - dot_qr1 a1s2 dot_q1
y2,10 = Kr22 dot_qr2
!
!
!
!
!
!
!
- 47
!
-
y3,1 = y3,2 = y3,3 = y3,4 = y3,6 = y3,7 = y3,8 = y3,9 = y3,10 = y3,12 = 0
y3,5 = Kr3 dot_doq_qr1 + Kr3 dot_dot_qr2 + Kr32 dot_dot_qr3
y3,11 = Kr32 dot_qr3
y3,13 = dot_dot_qr3 + g
y3,14 = dot_dot_qr3 + g
!
y4,1 = y4,2 = y4,4 = y4,5 = y4,7 = y4,8 = y4,9 = y4,10 = y4,11 = y4,13 = y4,14 = 0
y4,3 = dot_dot_qr1 + dot_dot_qr2 + dot_dot_qr4
y4,6 = Kr4 (dot_dot_qr1 + dot_dot_qr2 + Kr4dot_dot_qr4)
y4,12 = Kr42 dot_qr4
!
A.5 PARAMETRI DEL ROBOT ANTROPOMORFO I parametri del robot antropomorfo sono riportati nel M-File allegato al
documento.
!
- 48
!
-
BIBLIOGRAFIA [1] Robotics: Modelling, Planning and Control - B. Siciliano, L. Sciavicco, L.
Villani, G. Oriolo - Springer (2008)
[2] http://it.wikipedia.org/wiki/Yin_e_yang
- 49
!
-