versione - Meccanica Applicata e Robotica Industriale Applied

Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Titolo
Evoluzione di un Sistema Robotico
Comandato Mediante Telemanipolazione e
Retroazionato in Posizione e Forza
Dottorato di Ricerca in Meccanica Applicata
XIX° ciclo a.a. 2003-2004
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
1
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Obiettivi
Obiettivo del progetto è quello di mettere in grado un robot
industriale di funzionare come operatore remoto in un’applicazione
di telemanipolazione con retroazione di forza
Il manipolatore è costituito da un robot avente struttura antropomorfa
dotato, all’estremità del polso sferico, di un utensile in grado di
praticare piccoli fori e di un sensore estensimetrico per la
misurazione della forza istantanea lungo l’asse di foratura
I riferimenti di posizione e orientamento per il manipolatore
vengono generati attraverso un’interfaccia aptica costituita da un
joystick a cinque gradi di libertà la quale ha anche il compito di
riprodurre verso l’operatore la retroazione di forza
Il master aptico ed il manipolatore si trovano fisicamente in luoghi
distinti e remoti e comunicano attraverso la rete internet
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
2
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Sistema di Telecontrollo
Lato Slave
Lato Master
Riferimenti di posizione
e orientamento
SISTEMA DI
CONTROLLO
Postazione Aptica
19/10/2005
Retroazione di forza
Manipolatore
Antropomorfo
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
3
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
GESTIONE DELLA COMUNICAZIONE VIA RETE
Feedback di
forza per il
master aptico
ELABORAZIONE E
TRASMISSIONE
Riferimenti di
posizione e
orientamento
RICEZIONE ED
ELABORAZIONE
Schema a blocchi
del controllore
lato slave
Lo schema viene
eseguito
ciclicamente con
una frequenza di
aggiornamento pari
a 1kHz
Segnale di
forza
GENERAZIONE DEI RIFERIMENTI
DI POSIZIONE PER I GIUNTI
COSTRUZIONE DELLA
MATRICE DI
ROTOTRASLAZIONE
CINEMATICA INVERSA
CONTROLLO DINAMICO
DEGLI ASSI
PIANIFICAZIONE DEL
MOTO DEI GIUNTI
CONTROLLORE PID
E FEED-FORWARD
Segnali di
controllo per gli
assi del robot
Posizione istantanea
degli assi
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
4
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Gestione della comunicazione lato slave
Controllore real-time lato slave
Trasmissione del dato
di forza e richiesta dei
dati di posizione e
orientamento generati
dal master aptico
Ricezione dei dati di
posizione e
orientamento generati
dal master aptico e
inviati dal server di rete
RETE INTERNET
Server di rete
19/10/2005
Master aptico
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
5
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Generazione dei riferimenti di posizione per i giunti
Problematiche legate all’operazione di foratura
Le fasi attraverso le quali l’operatore esegue la foratura possono
essere distinte come di seguito:
1.
2.
3.
4.
5.
movimentazione dell’utensile nei pressi della zona di lavoro
avvicinamento al punto di foratura
contatto dell’utensile con l’oggetto da forare
foratura
allontanamento
Forza sull’utensile costante
durante l’avanzamento
19/10/2005
Picco di forza
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
6
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Introduzione di una funzione ritardante per i
riferimenti di posizione
*
R

R
i 1
Ri*  Ri*1  i
 ti  ti 1     f  0    f   t i  t i 1 
ti  ti 1
ti
( f ) 
 f (t )t
Ri
ti 1
Fmax
Ri*
ti



f (t )t 



ti1
*
*
*
Ri  Ri 1  ( Ri  Ri 1 )  1 

 Fmax ti  ti 1  




ti

f (t ) t
ti  1
Fmax t i  t i1 
t i 1
19/10/2005
t
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
ti
7
Università degli Studi di Padova
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Facoltà di Ingegneria
Convergenza della funzione a regime
ti




f
(
t
)

t



ti 1
1 
  T costante
 Fmax ti  ti 1  




R R
*
i
*
i 1
 ( Ri  R )  T
*
i 1
Riferimenti
Differenziale di posizione
1
1-T
2
2-3T+T2
3
3-6T+4T2-T3
4
4-10T+10T2 -5T3+T4
5
5-15T+20T2 -15T3+6T4-T5
19/10/2005
Riferimenti
Riferimenti ritardati
1
T
2
3T-T2
3
6T-4T2+T3
4
10T-10T2 +5T3-T4
5
15T-20T2 +15T3-6T4+T5

n


1

T

n 1
S
1T
1T 1

 1
1  (1  T )
T
T
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
8
Università degli Studi di Padova
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Facoltà di Ingegneria
Indice di ritardo
Andam ento del ritardo nel tem po
con forza costante
1
S  m 1
T
2,5
2
m 1
ritardo
1,5
Ritardo a regime al variare di T e per diversi valori di "m"
1
100
0,5
90
1
2
3
4
5
6
7
8
9
10
11
12
13
tem po
andamento del ritardo con T=0,3
14
15
16
ritardo a regime
80
0
17
70
18
60
50
40
30
20
10
0
0
0,5
1
1,5
T
m=1
19/10/2005
m=2
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
m=4
9
Università degli Studi di Padova
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Facoltà di Ingegneria
Matrice di rototraslazione e cinematica inversa
R CARDANO
c c c s s  s c c s c  s s   cos( xx) cos( xy ) cos( xz ) 


 R z ( )R y ( )R x ( )   s c s s s  c c s s c  c s   cos( yx) cos( yy ) cos( yz ) 


  s
  cos( zx) cos( zy ) cos( zz ) 
c s
c c
: angolo di imbardata
q: angolo di beccheggio
: angolo di rollio
 0
  asin (cos( yz ))
  a tan 2cos( xz), cos( zz ) cos( zz )  0
z
X
u
Y
u
Z
u
y
x
x
19/10/2005
cos( zz )  0
 0
* *
* R
6, 0

* *

0 0
x
* y 
* z

0 1
*
= [T3,O ]  [T6,3 )
q1
q2
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
q3
q4
q5
q6
10
Università degli Studi di Padova
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Facoltà di Ingegneria
Pianificazione del moto
•
•
•
Pianificazione nello spazio dei giunti (cinematica inversa prima della pianificazione del moto)
I riferimenti di posizione da raggiungere non sono noti a priori
Gli intervalli temporali tra i riferimenti sono variabili in maniera casuale e piuttosto ampia a
causa del transito dei pacchetti di dati attraverso la rete internet
PIANIFICAZIONE
DEL MOTO
Intervalli di
pianificazione fissi
pari a 20ms
Caratteristiche dinamiche del sistema di
generazione dei riferimenti (interfaccia aptica)
•
Caratteristiche dinamiche del manipolatore
•
Caratteristiche del mezzo di trasmissione
dei dati dal master aptico al manipolatore
1 20  Ri  Ri 1 

vm 

20 i 1  ti 
14
13
12
14
11
13
10 12
9
11
8
10
7
9
6
8
5
7
4
6
3
5
12 2 4 3
1
3
12 2
Posizione [x10-3 rad]
14
13
12
11
10
9
8
7
6
5
4
3
2
1
Posizione [x10-3 rad]
Posizione [x10-3 rad]
•
•
• Velocità finale = vm
4
5
6
7
8
3
4
5
6
7
9
10 11 12 13 14 15 16 17 18 19 20
Tempo [ms]
8 9 10 11 12 13 14 15 16 17 18 19 20
Tempo [ms]
1
1
2
3
4
5
6
7
19/10/2005
8
9
10 11 12 13 14 15 16 17 18 19 20
Tempo [ms]
• Posizione finale = ultimo
riferimento presente
all’interno della finestra
temporale
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
•
spline cubiche
interpolanti i riferimenti
di posizione e
opportunamente
raccordate tra un
intervallo e il
successivo
11
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Caratteristiche del pianificatore
•
Tempo minimo di azionamento costante e pari a 20 millisecondi.
Il giunto più “lento” deve raggiungere la posizione finale pianificata entro tale intervallo di
tempo (velocità ed accelerazioni degli assi sempre al di sotto di quelle massime raggiungibili)
•
Introduzione di un ritardo che varia da un minimo di 20 ad un massimo di 40 millisecondi a
seconda dell’istante in cui arriva l’ultimo pacchetto all’interno della generica finestra temporale
•
Riducendo l’intervallo di pianificazione si riduce l’entità del ritardo introdotto ma aumenta la
probabilità di avere vuoti informativi (mancanza del riferimento di posizione all’interno della
generica finestra temporale)
POSIZIONE
VELOCITA’
•
Arresti e ripartenze ad
intervalli di tempo molto
brevi con conseguente
produzione di vibrazioni e
notevole degrado del
movimento complessivo
•
•
•
19/10/2005
100 campioni al
secondo
Intervallo di
pianificazione: 20ms
100 campioni al
secondo
Intervallo di
pianificazione: 5ms
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
12
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Controllo di posizione
• Controllore decentralizzato: ogni asse viene controllato come un sistema ad un ingresso ed
una uscita
• Ciascun giunto è controllato in velocità ossia la velocità di rotazione dei motori risulta
proporzionale alla tensione di comando presente in ingresso all’azionamento
Anello di velocità
Retroazione di posizione
PID
+
-
rif di
posiz.
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
13
Università degli Studi di Padova
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Facoltà di Ingegneria
Regolatore PID e compensazioni feed-forward
2
double (6)
Enc
6
6
double (6)
U U(E)
18
double (6)
6
In1uscita_pid
double (6)
Selector2
PID
6
1
double (18)
qd
pos,vel,acc
U U(E)
18
double (6)
U U(E)
double (6)
double (6)
6
double (6)
-KFFW acc
Selector
double
-K-
guadagno
proporzionale
1
double
double
I
double
i1
double
double
double
s
In_1
Integrale
guadagno
integrale
saturazione
Sum
desaturazione
double
double
-K-
D.s
double
d1
double
1/Ns+1
derivata filtrata
19/10/2005
1
rif_vel
FFW vel
Selector1
18
double (6)
-K-
guadagno
derivativo
1
Out_1
• Il ramo integrale presenta una
catena di anti wind-up che ha la
funzione di limitare il contributo
del guadagno durante le fasi di
transitorio
• Il calcolo della derivata
dell’errore viene preceduto da
un filtro passa-basso a 10 Hz
che ha la funzione di ridurre gli
effetti dei disturbi elettrici
presenti sul canale encoder
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
14
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Test sperimentali
Set-up utilizzato
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
15
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Test sperimentali
Inseguimento di un riferimento di posizione sinusoidale
Riferimento di posizione: sinusoide lungo un asse cartesiano (asse Z)
Ampiezza: 10mm
Frequenza: 0.5 rad/s
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
16
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Test sperimentali
Inseguimento di una rampa di posizione
Inseguimento di un riferimento di posizione a rampa con introduzione di ritardo
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
17
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Test sperimentali
foratura senza l’impiego dell’algoritmo di ritardo (m=0)
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
18
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Test sperimentali
foratura con l’impiego dell’algoritmo di ritardo (m=8)
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
19
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Test sperimentali
foratura con l’impiego dell’algoritmo di ritardo (m=16)
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
20
Università degli Studi di Padova
Facoltà di Ingegneria
Dottorato di ricerca in Meccanica Applicata
XIX ciclo a.a. 2003-04
Fasi da sviluppare nel corso dell’ultimo anno
1. Completamento dei test sperimentali in remoto
con e senza l’impiego della funzione ritardante
al fine di testare l’affidabilità complessiva del
sistema di telemanipolazione
2. Progettazione e realizzazione di un prototipo di
ambiente reale non strutturato
3. Esecuzione di intervento chirurgico su fantoccio
19/10/2005
Presentazione dell’attività svolta durante il II° anno
Dottorando: Nicola De Rossi
21