Robotica Industriale

Transcript

ROBOTICA INDUSTRIALE
Prof. Bruno SICILIANO
ROBOTICA
Studio di macchine che possano sostituire l’uomo nell’esecuzione
di un compito, sia in termini di attivit à fisica che decisionale
Radici culturali
?
?
?
?
mitologia
automa
robot (robota = lavoro esecutivo)
letteratura fantascientifica
Leggi fondamentali (Asimov)
?
?
?
un robot non può far del male a un essere umano n é consentire, restando inoperoso, che un essere umano si trovi in
pericolo
un robot deve obbedire agli ordini impartiti da esseri umani,
a meno che tali ordini non entrino in conflitto con la prima
legge
un robot deve proteggere la sua esistenza a meno che tale
protezione non vada in conflitto con la prima o la seconda
legge
Robotica definita come scienza che studia la connessione intelligente tra percezione e azione
?
?
?
sistema meccanico (locomozione + manipolazione)
sistema sensoriale (sensori propriocettivi ed eterocettivi)
sistema di governo
Robotica avanzata
?
?
?
?
spiccate caratteristiche di autonomia
applicazioni in ambiente ostile (spaziale, sottomarino, nucleare, militare, ...)
missioni di servizio (applicazioni domestiche, assistenza
medica, assistenza ai disabili, agricoltura, ...).
ancora in età infantile
Robotica industriale
?
?
progettazione, governo e applicazioni dei robot in ambito
industriale
tecnologia matura
L’automazione è una tecnologia il cui obiettivo è quello di sostituire la macchina all’uomo in un processo di produzione, non
solo per quanto riguarda l’esecuzione delle operazioni materiali, ma anche per ci ò che concerne l’elaborazione intelligente
delle informazioni sullo stato del processo.
?
?
?
automazione rigida (produzione in serie di grossi volumi di
manufatti di caratteristiche costanti)
automazione programmabile (produzione di piccoli e medi
lotti di manufatti di caratteristiche variabili)
automazione flessibile (produzione di lotti variabili di manufatti diversi)
Robot industriale
?
?
?
macchina con elevate caratteristiche di versatilit à e flessibilità
un robot è una struttura meccanica multifunzionale e riprogrammabile progettato per spostare materiali, parti, utensili
o dispositivi specializzati secondo movimenti variabili programmati per l’esecuzione di una variet à di compiti diversi
(Robot Institute of America, 1980)
componente tipico di sistemi di automazione programmabile
ROBOT INDUSTRIALE
Componenti:
?
?
?
?
una struttura meccanica o manipolatore che consiste in un
insieme di corpi rigidi (bracci) interconnessi tra di loro per
mezzo di articolazioni (giunti); nel manipolatore si individuano una struttura portante, che ne assicura mobilit à, un
polso, che conferisce destrezza, e un organo terminale che
esegue il compito per cui il robot è utilizzato
attuatori che imprimono il movimento al manipolatore attraverso l’azionamento dei giunti; si impiegano usualmente
motori elettrici, idraulici e talvolta pneumatici
sensori che misurano lo stato del manipolatore (sensori propriocettivi) ed eventualmente lo stato dell’ambiente (sensori
eterocettivi)
una unità di governo (calcolatore) con funzioni di controllo
e supervisione dei movimenti del manipolatore
CAPACITÀ DI IMPIEGO
trasporto
?
?
?
?
?
palettizzazione (disposizione di oggetti in maniera preordinata su un opportuno supporto raccoglitore)
carico e scarico di magazzini
carico e scarico di macchine operatrici e macchine utensili
selezione e smistamento di parti
confezionamento di merci
manipolazione (lavorazione/assemblaggio)
?
?
?
?
?
?
?
?
?
?
saldatura ad arco e a punti
verniciatura a spruzzo
fresatura e trapanatura
incollaggio
taglio laser e a getto d’acqua
finitura
assemblaggio di gruppi meccanici ed elettrici
montaggio di schede elettroniche
avvitatura
cablaggio
misura
?
?
?
collaudo dimensionale
rilevamento di profili
individuazione di difetti di fabbricazione
STRUTTURA DEI MANIPOLATORI
struttura meccanica a catena cinematica aperta o a catena cinematica chiusa
gradi di mobilità (giunti prismatici o rotoidali)
gradi di libertà (descrizione di un compito)
spazio di lavoro (porzione dell’ambiente circostante a cui pu ò
accedere l’organo terminale)
Manipolatore cartesiano
tre giunti prismatici
?
ad ogni grado di mobilità corrisponde un grado di libert à
ottime caratteristiche di rigidezza meccanica
precisione di posizionamento del polso costante nello spazio di
lavoro
operazioni di trasporto e assemblaggio
azionamenti elettrici (talvolta pneumatici)
Manipolatore a portale
manipolazione di oggetti di dimensione e peso rilevanti
Manipolatore cilindrico
un giunto rotoidale e due prismatici
?
ad ogni grado di mobilità corrisponde un grado di libert à (in
coordinate cilindriche)
buone caratteristiche di rigidezza meccanica
la precisione di posizionamento del polso si riduce al crescere
dello sbraccio orizzontale
operazioni di trasporto di oggetti anche di peso rilevante
azionamenti idraulici (o elettrici)
Manipolatore sferico
due giunti rotoidali e uno prismatico
?
ad ogni grado di mobilità corrisponde un grado di libert à (in
coordinate sferiche)
discrete caratteristiche di rigidezza meccanica
dello sbraccio radiale
operazioni di lavorazione
azionamenti elettrici
Manipolatore SCARA
due giunti rotoidali e uno prismatico
elevata rigidezza a carichi verticali e cedevolezza a carichi orizzontali (Selective Compliance Assembly Robot Arm)
della distanza del polso stesso dall’asse del primo giunto
manipolazione di piccoli oggetti
Manipolatore antropomorfo
tre giunti rotoidali
?
spalla e gomito (che connette braccio e avambraccio)
struttura più destra
precisione di posizionamento variabile
applicazioni molteplici
Polso sferico
giunti rotoidali
?
determinano l’orientamento dell’organo terminale
caratteristiche di compattezza e destrezza
disaccoppiamento tra posizione e orientamento
Organo terminale
specificato in relazione al compito che il robot deve eseguire
?
?
pinza (trasporto)
utensile o dispositivo specializzato (lavorazione e assemblaggio)
Alcuni robot industriali
Robot AdeptOne XL
Robot Comau SMART S2
Robot ABB IRB 4400
Unità lineare Kuka KL 250 con robot KR 15/2
Robot Robotics Research K-1207i
Robot Fanuc I-21i
MODELLISTICA E CONTROLLO DI
MANIPOLATORI
Modellistica
?
?
?
struttura meccanica (cinematica + cinematica differenziale
+ statica + dinamica)
attuatori
sensori
Controllo
?
?
?
?
pianificazione del moto
controllo nello spazio libero
controllo nello spazio vincolato
unità di governo
Cinematica
relazioni tra posizioni dei giunti e posizione e orientamento
dell’organo terminale
Cinematica differenziale
relazioni tra velocità dei giunti e velocità (lineare e angolare)
Statica
relazioni tra forze e coppie applicate ai giunti e forze e momenti
applicati all’organo terminale in situazioni di equilibrio
Dinamica
equazioni del moto del manipolatore in funzione delle forze e
momenti agenti su di esso
Pianificazione di traiettorie
generazione delle leggi di moto per le variabili di interesse
(giunti/organo terminale)
Controllo del moto
determinazione delle forze/coppie agli attuatori per garantire
l’esecuzione delle traiettorie di riferimento
Controllo dell’interazione
gestione del contatto tra organo terminale e ambiente
Attuatori e sensori
attuazione del moto
misura di variabili di interesse
Unità di governo
implementazione delle leggi di controllo
interfaccia con operatore
CINEMATICA
relazioni tra posizioni dei giunti e posizione e orientamento
Matrice di rotazione
Rappresentazioni dell’orientamento
Trasformazioni omogenee
Cinematica diretta
Spazio dei giunti e spazio operativo
Calibrazione cinematica
Problema cinematico inverso
POSIZIONE E ORIENTAMENTO DI UN
CORPO RIGIDO
Posizione
2 o0x 3
o0 = 4 o0y 5
o0z
Orientamento
x0 = x0xx + x0y y + x0z z
y0 = yx0 x + yy0 y + yz0 z
z0 = zx0 x + zy0 y + zz0 z
MATRICE DI ROTAZIONE
2
3 2 x0T x y0T x z0T x 3
R = 4 x0 y0 z0 5 = 4 x0T y y0T y z0T y 5
x0T z y0T z z0T z
RT R = I
RT = R;
1
Rotazioni elementari
rotazione di intorno a z
2 cos ;sin Rz () = 4 sin cos 0
0
3
0
05
1
rotazione di intorno a y
2 cos 0 sin 3
Ry ( ) = 4 0 1 0 5
;sin 0 cos rotazione di intorno a x
21 0
3
0
Rx( ) = 4 0 cos ;sin 5
0 sin cos ROBOTICA INDUSTRIALE
Rappresentazione di un vettore
2 px 3
p = 4 py 5
pz
2 p0x 3
p0 = 4 p0y 5
2
3
p = 4 x0 y0 z0 5 p0
= Rp0
p0 = R T p
p0z
Esempio
px = p0x cos ; p0y sin py = p0x sin + p0y cos pz = p0z
Rotazione di un vettore
p = Rp0
pT p = p0T RT Rp0
Esempio
px = p0x cos ; p0y sin py = p0x sin + p0y cos pz = p0z
p = Rz ()p0
Matrice di rotazione
?
?
?
fornisce l’orientamento di una terna di coordinate rispetto
ad un’altra: i vettori colonna sono i coseni direttori degli
assi della terna ruotata rispetto alla terna di partenza
rappresenta una trasformazione di coordinate che mette in
relazione le coordinate di uno stesso punto in due terne
differenti (di origine comune)
è l’operatore che consente di ruotare un vettore in una stessa
terna di coordinate
COMPOSIZIONE DI MATRICI DI
ROTAZIONE
p =R p
1
1
2
p =R p
p =R p
2
0
0
1
1
0
0
2
2
Rij = (Rji ); = (Rji )T
1
Rotazione in terna corrente
R =R R
0
2
0
1
1
2
Rotazione in terna fissa
R =R R
0
2
1
2
0
1
Esempio
ANGOLI DI EULERO
matrice di rotazione
?
9 parametri con 6 vincoli
rappresentazione minima dell’orientamento
?
3 parametri indipendenti
Angoli ZYZ
R() = Rz (')Ry (#)Rz ( )
2 c c c ; s s ;c c s ; s c c s 3
' #
'
' #
'
' #
= 4 s' c# c + c' s ;s' c# s + c' c s' s# 5
0
;s# c
00
s# s
c#
Problema inverso
?
Assegnata
2r
R = 4r
11
21
r31
3
r
r 5
r12 13
r22 23
r32 r33
i 3 angoli ZYZ sono (# 2 (0; ))
' = Atan2(r23; r13 )
q
2
2
+ r23
; r33
# = Atan2 r13
= Atan2(r32; ;r31 )
ovvero (# 2 (;; 0))
' = Atan2(;r23; ;r13 )
q
2
2
# = Atan2 ; r13
+ r23
; r33
= Atan2(;r32; r31 )
Angoli di RPY
R() = Rz (')Ry (#)Rx( )
2c c c s s ;s c c s c +s s 3
' # ' #
'
' #
'
= 4 s' c# s' s# s + c' c s' s# c ; c' s 5
;s#
c# s
c# c
Problema inverso
?
Assegnata
2r
R = 4r
11
21
r31
3
r
r 5
r12 13
r22 23
r32 r33
i 3 angoli di RPY sono (# 2 (;=2; =2))
' = Atan2(r21 ; r11 )
q
2
2
+ r33
# = Atan2 ;r31 ; r32
= Atan2(r32 ; r33 )
ovvero (# 2 (=2; 3=2))
' = Atan2(;r21; ;r11 )
q
2
2
# = Atan2 ;r31 ; ; r32
+ r33
= Atan2(;r32; ;r33 )
ASSE/ANGOLO
R(#; r) = Rz ()Ry ( )Rz (#)Ry (; )Rz (;)
sin = q ry
rx2 + ry2
q
sin = rx2 + ry2
cos = q rx
rx2 + ry2
cos = rz
2 r (1 ; c ) + c r r (1 ; c ) ; r s
#
#
x y
#
z #
x
6
R(#; r) = 4 rxry (1 ; c# ) + rz s# ry (1 ; c#) + c#
2
2
rx rz (1 ; c# ) ; ry s# ry rz (1 ; c# ) + rx s#
3
rx rz (1 ; c# ) + ry s#
ry rz (1 ; c# ) ; rx s# 5
rz2 (1 ; c# ) + c#
R(#; r) = R(;#; ;r)
Problema inverso
?
Assegnata
2r
R = 4r
11
21
r31
3
r
r 5
r12 13
r22 23
r32 r33
l’angolo e l’asse di rotazione sono (sin # 6= 0)
r + r + r ; 1
# = cos ;
2
2r ;r 3
r = 1 4r ;r 5
1
11
32
22
23
31
2 sin # r13
21 ; r12
con
rx2 + ry2 + rz2 = 1
33
QUATERNIONE UNITARIO
rappresentazione a 4 parametri Q = f;
g
= cos #2
= sin #2 r
2 + 2x + 2y + 2z = 1
? (#; r) e (;#; ;r) forniscono lo stesso quaternione
2 2( + ) ; 1 2(xy ; z ) 2(xz + y ) 3
x
6
R(; ) = 4 2(xy + z ) 2( + y ) ; 1 2(y z ; x ) 75
2
2
2
2(x z ; y )
2
2(y z + x ) 2(2 + 2z ) ; 1
Problema inverso
?
Assegnata
2r
R = 4r
11
21
r31
il quaternione è (
3
r
r 5
r12 13
r22 23
r32 r33
0)
p
= 12 r11 + r22 + r33 + 1
2 sgn (r32 ; r23)pr11 ; r22 ; r33 + 1 3
= 12 64 sgn (r13 ; r31)ppr22 ; r33 ; r11 + 1 75
sgn (r21 ; r12 ) r33 ; r11 ; r22 + 1
quaternione estratto da
R ; = RT
1
Q;1 = f; ;g
prodotto tra quaternioni
Q1 Q2 = f1 2 ; T1 2 ; 1 2 + 2 1 + 1 2 g
TRASFORMAZIONI OMOGENEE
Trasformazione di coordinate (traslazione + rotazione)
p =o +R p
0
0
1
0
1
1
Trasformazione inversa
p = ;R o + R p
1
1
0
0
1
1
0
0
Rappresentazione omogenea
2 3
p~ = 64 p 75
1
Matrice di trasformazione omogenea
2
A = 64 R
0
1
0
1
3
o 75
0
1
0T
1
Trasformazione di coordinate
p~ = A p~
0
0
1
1
Trasformazione inversa
p~ = A p~ = ;A ; p~
1
ove
1
0
0
2
A = 64 R
1
0
1
0
1
0
1
0T
3
;R o 75
1
0
1
A; 6= AT
1
Successione di trasformazioni
p~ = A A : : : Ann; p~n
0
0
1
1
2
0
1
0
1
CINEMATICA DIRETTA
Manipolatore
?
insieme di bracci connessi tramite giunti
Catena cinematica (dalla base all’organo terminale)
?
?
aperta (sequenza unica)
chiusa (sequenza forma un anello)
Grado di mobilità
?
tipicamente associato a una articolazione = variabile di
giunto
Terna base e terna utensile
Equazione cinematica diretta
2
3
b (q) sb (q) ab (q) pb (q) 7
n
6
b
e
e
e 5
Te (q ) = 4 e
0
0
0
1
Manipolatore planare a due bracci
2
b sb
n
6
b
T e (q ) = 4 e e
0 0
20 s
= 64 01 ;c0
12
12
0
0
3
abe pbe 75
0 1
c12 a1 c1 + a2 c12 3
s12 a1 s1 + a2 s12 75
0
0
0
1
Catena aperta
Tn (q) = A (q )A (q ) : : : Ann; (qn)
0
0
1
1
1
2
2
Teb (q) = T b Tn (q)Ten
0
0
1
Convenzione di Denavit-Hartenberg
si sceglie l’asse zi giacente lungo l’asse del giunto i + 1
si individua Oi all’intersezione dell’asse z i con la normale
comune agli assi zi;1 e zi , e con Oi0 si indica l’intersezione
della normale comune con z i;1
si assume l’asse xi diretto lungo la normale comune agli
assi zi;1 e zi con verso positivo dal giunto i al giunto i + 1
si sceglie l’asse yi in modo da completare una terna levogira
Definizione non univoca della terna:
?
?
?
?
?
con riferimento alla terna 0, per la quale la sola direzione
dell’asse z0 risulta specificata: si possono quindi scegliere
arbitrariamente O0 ed x0
con riferimento alla terna n, per la quale il solo asse x n risulta soggetto a vincolo (deve essere normale all’asse z n;1):
infatti non vi è giunto n + 1, per cui non è definito zn e lo
si può scegliere arbitrariamente
quando due assi consecutivi sono paralleli, in quanto la
normale comune tra di essi non è univocamente definita
quando due assi consecutivi si intersecano, in quanto il verso
di xi è arbitrario
quando il giunto i è prismatico, nel qual caso la sola direzione dell’asse zi;1 è determinata
Parametri di Denavit-Hartenberg
ai
di
i
distanza di Oi da Oi0 ;
#i
angolo intorno all’asse z i;1 tra l’asse xi;1 e l’asse xi valutato
positivo in senso antiorario.
coordinata su zi;1 di Oi0 ;
angolo intorno all’asse x i tra l’asse zi;1 e l’asse zi valutato
positivo in senso antiorario;
ai e i sono sempre costanti
se il giunto è rotoidale la variabile è #i
se il giunto è prismatico la variabile è di
Trasformazione di coordinate
2 c# ;s# 0 0 3
Aii; = 64 s0# c0# 01 d0i 75
i
0
1
i
i
i
0
0
0 1
2 1 0 0 ai 3
Aii = 64 00 sc ;cs 00 75
0
i
0
0
i
i
0
i
1
2 c# ;s# c s# s aic# 3
Aii; (qi) = Aii; Aii = 64 s0# c#sc ;cc# s aidsi# 75
i
1
0
1
0
i
0
i
i
i
0
i
i
i
i
i
0
i
i
i
i
1
Procedura operativa
1. Individuare e numerare consecutivamente gli assi dei giunti;
assegnare, rispettivamente, le direzioni agli assi z 0 , : : : , zn;1
2. Fissare la terna base posizionandone l’origine sull’asse z 0 ; gli
assi x0 e y0 sono scelti in maniera tale da ottenere una terna
levogira
Eseguire i passi da 3 a 5 per i = 1; : : : ; n ; 1:
3. Individuare l’origine O i all’intersezione di zi con la normale
comune agli assi zi;1 e zi . Se gli assi zi;1 e zi sono paralleli e
il giunto i è rotoidale, posizionare O i in modo da annullare d i ;
se il giunto i è prismatico, scegliere O i in corrispondenza di
una posizione di riferimento per la corsa del giunto (ad esempio
un fine-corsa)
4. Fissare l’asse xi diretto lungo la normale comune agli assi z i;1
e zi con verso positivo dal giunto i al giunto i + 1
5. Fissare l’asse yi in modo da ottenere una terna levogira
Per completare:
6. Fissare la terna n, allineando z n lungo la direzione di zn;1 se il
giunto n è rotoidale, ovvero scegliendo z n in maniera arbitraria
se il giunto n è prismatico; fissare l’asse x n in accordo al punto 4
7. Costruire per i = 1; : : : ; n la tabella dei parametri a i ; di ; i ; #i
8. Calcolare sulla base dei parametri di cui al punto 7 le matrici
di trasformazione omogenea ii;1 (qi ) per i = 1; : : : ; n
A
9. Calcolare Tn (q ) = A : : : Ann;
0
0
1
1
che fornisce posizione e
orientamento della terna n rispetto alla terna 0
b e T n , calcolare la funzione cinematica diretta
T
e
b
b
n
Te (q) = T Tn Te che fornisce posizione e orientamento della
10. Assegnate
0
0
0
terna utensile rispetto alla terna base
Catena chiusa
Connessione di un singolo braccio con due bracci
giunto virtuale di taglio
Aij (q0) = Aii (qi ) : : : Ajj; (qj )
Aik (q00) = Aii (qi ) : : : Akk; (qk )
+10
+100
+10
+100
1
1
Vincoli
?
giunto j + 1 rotoidale
?
giunto j + 1 prismatico
(
Rij (q0) ;pij (q0) ; pik (q00) = [ 0 0 djk ]T
zji (q0) = zki (q00)
0
8 xiT (q0) ;
>
j
i (q0 ) ; pi (q00 ) =
p
>
k
iT
0
< yj (q ) j
0
i (q0 ) = z i (q00 )
z
>
j
>
: xiT (q0)xi (kq00) = cos #jk
j
k
? risolti in termini di q : : :
Tn (q) = Ai Aij Ajn
0
0
Procedura operativa
1. Selezionare un giunto non attuato della catena chiusa. Ipotizzare
di aprire tale giunto in modo da ottenere una catena aperta con
struttura ad albero
2. Calcolare le trasformazioni omogenee secondo la convenzione
di Denavit-Hartenberg
3. Trovare i vincoli di uguaglianza per le due terne connesse dal
giunto di taglio
4. Risolvere i vincoli in termini di un numero ridotto di variabili
di giunto
5. Esprimere le trasformazioni omogenee in funzione di tali variabili di giunto e calcolare la funzione cinematica diretta per
composizione della varie trasformazioni dalla terna base alla
terna utensile
Manipolatore planare a tre bracci
Braccio
1
2
3
ai
a1
a2
a3
i
0
0
0
di
0
0
0
#i
#1
#2
#3
2 ci ;si 0 aici 3
Aii; = 64 s0i c0i 01 ai0si 75
1
0
0
T =A A A
2 c ;s
= 64 s 0 c 0
0
3
0
1
1
2
123
2
3
123
123
123
0
0
0
1
i = 1; 2; 3
0 a1 c1 + a2 c12 + a3 c123 3
0 a1 s1 + a2 s12 + a3 s123 75
1
0
0
1
Manipolatore a parallelogramma
Braccio
10
20
30
100
4
ai
a1
a2
a3
a1
a4
0
0
0
00
i
0
0
0
0
0
di
0
0
0
0
0
#i
#1
#2
#3
#1
0
0
0
0
00
A (q0) = A A A
2c
;s
c
= 64 s 0
0
0
30
10
20
0
10
20
30
10 20 30
10 20 30
10 20 30
10 20 30
0
0
0 a1 c1 + a2 c1 2 + a3 c1 2 3
0 a1 s1 + a2 s1 2 + a3 s1 2 3
1
0
0
1
2 c ;s
A (q00 ) = 64 s0 c0
0
0
21 0 0
A = 64 00 10 01
00
30
4
0
0
0
0
0
0
0 0
0
0
0
0 0
0
0
0
0 a1 c1
0 a1 s1
1
1
1
0
0
1
a4 3
0 75
0
0 0 0 1
100
0
100
0
100
00
00
00
00
00
3
75
0
3
75
Risoluzione vincoli
?
?
orientamento (OK)
posizione
203
R (q0) ;p (q0) ; p (q00) = 4 0 5
30
0
0
30
0
100
0
+
a1 (c1 + c1 2 3 ) + a1 (c1 2 ; c1 ) = 0
a1 (s1 + s1 2 3 ) + a1 (s1 2 ; s1 ) = 0
#2 = #1 ; #1
#3 = ; #2 = ; #1 + #1
0
0
0
0
0
0
00
0 0
00
0
0
0 0
00
0 0
00
0
00
0
0
0
00
0
Cinematica diretta
2 ;c s 0 a c ; a c 3
T (q) = A (q)A = 64 ;0s ;0c 01 a s 0; a s 75
10
0
4
0
30
30
4
10
0
10
10
0
0
100 100
4 10
100 100
4 10
1
Manipolatore sferico
Braccio
1
2
3
ai
0
0
0
i
;=2
=2
0
di
0
d2
d3
#i
#1
#2
0
2 c 0 ;s 0 3
A = 64 s0 ;01 c0 00 75
1
0
1
1
0
2c 0 s 0 3
A = 64 s0 01 ;0c d0 75
1
2
1
0
0
2
1
2
1
2
2
0 0
0
21 0 0 0 3
A = 64 00 10 01 d0 75
2
3
3
0 0 0 1
T =A A A
2 c c ;s c s c s d ; s d 3
= 64 s;sc c0 scs s s dc d+ c d 75
0
3
0
1
1
2
2
3
1 2
1 2
0
2
1
1
0
1 2
1 2 3
1 2
1 2
1 2 3
1 2
2
0
2 3
1
2
1
Braccio
1
2
3
ai
0
a2
a3
i
=2
0
0
di
0
0
0
#i
#1
#2
#3
2c 0 s 03
A = 64 s0 01 ;0c 00 75
1
0
1
2 ci
Aii; = 64 s0i
1
0
1
0
;si
ci
0
0
T =A A A
2 c c ;c s
= 64 ssc ;sc s
0
3
0
1
1
2
2
3
1 23
1 23
1 23
1 23
23
0
1
23
0
1
0
0
0
1
0
0 1
ai ci 3
aisi 75 i = 2; 3
0
1
s1 c1 (a2 c2 + a3 c23 ) 3
;c1 s1 (a2 c2 + a3 c23 ) 75
0
a2 s2 + a3 s23
0
1
Polso sferico
Braccio
4
5
6
ai
0
0
0
i
;=2
=2
0
di
0
0
d6
#i
#4
#5
#6
2 c 0 ;s 0 3
A = 64 s0 ;01 c0 00 75
4
3
4
2c 0 s 03
A = 64 s0 01 ;0c 00 75
4
4
5
4
0
0
0
5
4
5
1
5
5
0 0
0
1
2 c ;s 0 0 3
A = 64 s0 c0 01 d0 75
6
6
5
6
0
6
6
6
0
0 1
T =A A A
2 c c c ; s s ;c c s ; s c c s c s d 3
= 64 s c ;c s +c c s ;s c ss s+ c c scs scsdd 75
3
6
3
4
4
5
5
6
4 5 6
4 6
4 5 6
4 6
4 5
4 5 6
4 5 6
4 6
4 5 6
4 6
4 5
4 5 6
5
5 6
5 6
0
5 6
0
0
1
Manipolatore di Stanford
2
3
T = T T = 64 n s a p 75
0
6
0
3
3
6
0
0
0
0
0
0
0
1
2c s d ;s d
p = 4s s d +c d
0
1 2 3
1 2
1 2 3
1 2
;
3
+ ;c (c c s + s c ) ; s s s d
+ s (c c s + s c ) + c s s d 5
1
2 4 5
2 5
1 4 5
6
1
2 4 5
2 5
1 4 5
6
c2 d3 + (;s2 c4 s5 + c2 c5 )d6
2 c ;c (c c c ; s s ) ; s s c ; s (s c c + c s ) 3
;
4
n = s c (c c c ; s s ) ; s s c + c (s c c + c s ) 5
;s (c c c ; s s ) ; c s c
2 c ;;c (c c s + s c ) + s s s ; s (;s c s + c c ) 3
s = 4 s ;;c (c c s + s c ) + s s s + c (;s c s + c c ) 5
s (c c s + s c ) + c s s
2 c (c c s + s c ) ; s s s 3
a = 4 s (c c s + s c ) + c s s 5
0
1
2
4 5 6
4 6
2 5 6
1
4 5 6
4 6
1
2
4 5 6
4 6
2 5 6
1
4 5 6
4 6
2
0
4 6
2 5 6
1
2
4 5 6
4 6
2 5 6
1
4 5 6
4 6
1
2
4 5 6
4 6
2 5 6
1
4 5 6
4 6
2
0
4 5 6
4 5 6
1
2 4 5
2 5
1 4 5
1
2 4 5
2 5
1 4 5
;s2 c4 s5 + c2 c5
4 6
2 5 6
Manipolatore antropomorfo con polso sferico
Braccio
1
2
3
4
5
6
ai
0
a2
0
0
0
0
i
=2
0
=2
;=2
=2
0
di
0
0
0
d4
0
d6
#i
#1
#2
#3
#4
#5
#6
2c 0 s 03
A = 64 s0 01 ;0c 00 75
3
2
3
2 c 0 ;s 0 3
A = 64 s0 ;01 c0 d0 75
3
3
4
3
0 0
0
4
3
4
1
4
0
4
0
4
0
1
2 a c c + d c s + d ;c (c c s + s c ) + s s s 3
;
4
p = a s c + d s s + d s (c c s + s c ) ; c s s 5
0
2 1 2
4 1 23
6
1
23 4 5
23 5
1 4 5
2 1 2
4 1 23
6
1
23 4 5
23 5
1 4 5
a2 s2 ; d4 c23 + d6 (s23 c4 s5 ; c23 c5 )
2 c ;c (c c c ; s s ) ; s s c + s (s c c + c s ) 3
n = 4 s ;c (c c c ; s s ) ; s s c ; c (s c c + c s ) 5
s (c c c ; s s ) + c s c
2 c ;;c (c c s + s c ) + s s s + s (;s c s + c c ) 3
s = 4 s ;;c (c c s + s c ) + s s s ; c (;s c s + c c ) 5
;s (c c s + s c ) ; c s s
2 c (c c s + s c ) + s s s 3
a = 4 s (c c s + s c ) ; c s s 5
0
0
1
23
4 5 6
4 6
23 5 6
1
4 5 6
4 6
1
23
4 5 6
4 6
23 5 6
1
4 5 6
4 6
23
4 5 6
23 5 6
1
23
4 5 6
4 6
23 5 6
1
4 5 6
4 6
1
23
4 5 6
4 6
23 5 6
1
4 5 6
4 6
23
0
4 6
4 5 6
1
23 4 5
23 5
1 4 5
1
23 4 5
23 5
1 4 5
s23 c4 s5 ; c23 c5
4 6
23 5 6
SPAZIO DEI GIUNTI E SPAZIO OPERATIVO
Spazio dei giunti
2q 3
q = 4 ... 5
1
qn
? qi = #i (giunto rotoidale)
? qi = di (giunto prismatico)
Spazio operativo
p
x= ? p (posizione)
? (orientamento)
Equazione cinematica diretta
x = k(q)
Esempio
2p 3
2a c +a c +a c 3
x
x = 4 py 5 = k(q) = 4 a s + a s + a s 5
1 1
2 12
3 123
1 1
2 12
3 123
#1 + #2 + #3
Spazio di lavoro
Spazio di lavoro raggiungibile
p = p(q)
?
qim qi qiM i = 1; : : : ; n
elementi di superficie planare, sferica, toroidale e cilindrica
Spazio di lavoro destro
?
orientamenti diversi
Esempio
?
configurazioni ammissibili
?
spazio di lavoro
Accuratezza
?
?
scostamento tra posizione conseguita con la postura assegnata e posizione calcolata tramite la cinematica diretta
valori tipici: (0:2; 1) mm
Ripetibilità
?
?
capacità del manipolatore di tornare in una posizione precedentemente raggiunta
valori tipici: (0:02; 0:2) mm
Ridondanza cinematica
? m < n (intrinseca)
? r < m = n (funzionale)
CALIBRAZIONE CINEMATICA
Valori precisi dei parametri DH per migliorare l’accuratezza di
un manipolatore
Equazione cinematica diretta in funzione di tutti i parametri
x = k(a; ; d; #)
xm
xn
locazione misurata
locazione nominale (parametri fissi + variabili giunto)
x = @@ ak a + @@k + @@ kd d + @@#k #
= (n )
? l misure (lm 4n)
2 x1 3 2 1 3
x = 4 ... 5 = 4 ... 5 = xl
l
Soluzione
= ( T );1 T x
0 = n + : : : finché converge
?
?
stime più accurate dei parametri fissi
correzioni alle misure dei trasduttori
Inizializzazione
postura di riferimento (home)
PROBLEMA CINEMATICO INVERSO
Cinematica diretta
Cinematica inversa
Complessità
? q =) T
? q =) x
? T =) q
? x =) q
?
?
?
?
soluzione analitica (in forma chiusa) ?
soluzioni multiple
infinite soluzioni
non esistono soluzioni ammissibili
Intuizione
?
?
algebrica
geometrica
Tecniche numeriche
Soluzione del manipolatore planare a tre bracci
Soluzione algebrica
= #1 + #2 + #3
pWx = px ; a3 c = a1 c1 + a2 c12
pWy = py ; a3 s = a1 s1 + a2 s12
p2Wx + p2Wy ; a21 ; a22
c2 =
2a1 a2
q 2
s2 = 1 ; c2
#2 = Atan2(s2 ; c2 )
; a2 s2 pWx
s1 = (a1 + a2pc22 )pWy
+ p2
Wx
Wy
Wy
Wy
+ a2 s2 pWy
c1 = (a1 + a2pc22 )pWx
+ p2
#1 = Atan2(s1 ; c1 )
#3 = ; #1 ; #2
Soluzione geometrica
p2Wx + p2Wy ; a21 ; a22
c2 =
.
2a1 a2
#2 = cos ;1 (c2 )
= Atan2(pWy ; pWx )
q
c p2Wx + p2Wy = a1 + a2 c2
0
1
pWx + pWy + a ; a A
;
@
q
= cos
1
2
2
2
1
2a1 p2Wx + p2Wy
#1 = 2
2
Soluzione di manipolatori con polso sferico
pW = p ; d a
6
Soluzione disaccoppiata
?
?
?
?
?
p
calcolare la posizione del polso W (q1 ; q2 ; q3 )
risolvere la cinematica inversa per (q 1 ; q2 ; q3 )
R (q ; q ; q )
calcolare R (# ; # ; # ) = R T R
calcolare
0
3
1
3
6
4
2
5
3
6
0
3
risolvere la cinematica inversa per l’orientamento (# 4; #5 ; #6 )
Soluzione del manipolatore sferico
(A01 );1T30 = A12 A23
2 p c +p s 3 2 d s 3
Wx
Wy
5 = 4 ;d c 5
pW = 4
;pWz
1
1
1
;pWx s1 + pWy c1
3 2
3 2
d2
2
1
;
t
c1 = 1 + t2
s1 = 1 +2tt2
(d2 + pWy )t2 + 2pWx t + d2 ; pWy = 0
q
#1 = 2Atan2 ;pWx pWx + pWy ; d ; d2 + pWy
2
2
2
2
pWx c1 + pWy s1 = d3 s2
;pWz
;d3 c2
#2 = Atan2(pWx c1 + pWy s1 ; pWz )
q
d3 = (pWx c1 + pWy s1 )2 + p2Wz
Soluzione del manipolatore antropomorfo
#1 = Atan2(pWy ; pWx )
#1 = + Atan2(pWy ; pWx )
p2Wx + p2Wy + p2Wz ; a22 ; a23
c3 =
q 2 2a2a3
s3 = 1 ; c3
q
(a2 + a3 c3 )pWz ; a3 s3 p2Wx + p2Wy
s2 =
p2Wx + p2Wy + p2Wz
q2
(a2 + a3 c3 ) pWx + p2Wy + a3 s3 pWz
c2 =
p2Wx + p2Wy + p2Wz
Quattro configurazioni ammissibili
?
soluzione univoca solo se
pWx 6= 0
pWy 6= 0
Soluzione del polso sferico
2 nx sx ax 3
R = 4 ny sy ay 5
3
6
3
3
3
3
3
3
n3z s3z a3z
#4 = Atan2(a3y ; a3x )
q 3 2 3 2 3 #5 = Atan2 (ax ) + (ay ) ; az
#6 = Atan2(s3z ; ;n3z )
#4 = Atan2(;a3y ; ;a3x)
q 3 2 3 2 3
#5 = Atan2 ; (ax ) + (ay ) ; az
#6 = Atan2(;s3z ; n3z )
CINEMATICA DIFFERENZIALE
relazioni tra velocità dei giunti e velocità dell’organo terminale
Jacobiano geometrico
Jacobiano analitico
Singolarità cinematiche
Analisi della ridondanza
Inversione della cinematica differenziale
Algoritmi per l’inversione cinematica
STATICA
relazioni tra forze all’organo terminale e coppie ai giunti
JACOBIANO GEOMETRICO
2
T (q) = 64 R(q)
0T
3
p(q) 75
1
Obiettivo
p_ = JP (q)q_
! = JO (q)q_
p_ v = ! = J (q)q_
Derivata di una matrice di rotazione
R(t)RT (t) = I
R_ (t)RT (t) + R(t)R_ T (t) = O
Posto
S (t) = R_ (t)RT (t)
S (t) + S T (t) = O
R_ (t) = S (!(t))R(t)
2 0 ;! ! 3
z
y
4
S = !z 0 ;!x 5
;!y !x
0
Esempio
2 cos ;sin Rz () = 4 sin cos 0
0
3
0
05
1
2 ;_ sin ;_ cos 0 3 2 cos sin 0 3
S (t) = 4 _ cos ;_ sin 0 5 4 ;sin cos 0 5
0
0 1
2 0 0;_ 0 3 0 0
= 4 _ 0 0 5 = S (!(t))
0
0
0
p =o +R p
0
0
1
0
1
1
p_ = o_ + R p_ + R_ p
= o_ + R p_ + S (! )R p
= o_ + R p_ + ! r
0
0
1
0
1
1
0
1
0
1
1
0
1
0
1
1
0
1
1
0
1
0
1
0
1
0
1
1
Velocità di un braccio
Velocità lineare
pi = pi; + Ri; rii;; ;i
1
1
1
1
p_ i = p_ i; + Ri; r_ ii;; ;i + !i; Ri; rii;; ;i
= p_ i; + vi; ;i + !i; ri; ;i
1
1
1
1
1
1
1
1
1
1
1
1
Velocità angolare
Ri = Ri; Rii;
1
1
S (!i)Ri = S (!i; )Ri + Ri; S (!ii;; ;i)Rii;
1
1
1
1
= S (!i;1 )Ri + S (Ri;1!ii;;11;i )Ri
!i = !i; + Ri; !ii;; ;i
= !i; + !i; ;i
1
1
1
1
1
1
1
!i = !i; + !i; ;i
p_ i = p_ i; + vi; ;i + !i; ri; ;i
1
1
1
1
1
1
Giunto prismatico
!i; ;i = 0
vi; ;i = d_izi;
1
1
1
!i = !i;
p_ i = p_ i; + d_izi; + !i ri; ;i
1
1
1
1
Giunto rotoidale
!i; ;i = #_ izi;
vi; ;i = !i; ;i ri; ;i
1
1
1
1
1
!i = !i; + #_ izi;
p_ i = p_ i; + !i ri; ;i
1
1
1
1
Calcolo dello Jacobiano
2|
3
|
P
Pn
4
5
J=
:::
|O
|On
1
1
Velocità angolare
?
giunto i prismatico
q_i |Oi = 0
?
=)
|Oi = 0
giunto i rotoidale
q_i |Oi = #_ i zi;1
=)
|Oi = zi;
1
Velocità lineare
?
giunto i prismatico
q_i |Pi = d_i zi;1
?
|Pi = zi;
=)
giunto i rotoidale
q_i |Pi = !i;1;i ri;1;n
= #_ i zi;1 (p ; pi;1 )
+
|Pi = zi; (p ; pi; )
1
1
1
Colonna dello Jacobiano geometrico
8 zi; | >
<
Pi = 0
|Oi >
: zi; z(p ; pi; )
i;
1
1
1
1
? zi;1 = R10 (q1 ) : : : Rii;;12(qi;1)z0
? p~ = A01 (q1 ) : : : Ann;1(qn )~p0
? p~ i;1 = A01 (q1 ) : : : Aii;;21 (qi;1)~p0
per un giunto prismatico
per un giunto rotoidale
Rappresentazione in terna differente
p_ t Rt
!t = O
t
R
= O
Rt
Jt = O
Ot p_
R !
Ot J q_
R
Ot J
R
z (p ; p ) z (p ; p ) z (p ; p ) J (q) =
z
z
z
0
0
1
1
0
2
2
1
2
203
2a c 3
2a c +a c 3
p = 405 p = 4a s 5 p = 4a s + a s 5
1 1
0
0
1
1 1
2
0
1 1
2 12
1 1
2 12
2a c +a c +a c 3
p = 4a s +a s +a s 5
0
203
z = z = z = 405
0
1 1
2 12
3 123
1 1
2 12
3 123
1
2
1
0
2 ;a s ; a s ; a s
66 a c + a c + a c
0
J = 666
0
4
0
1 1
1 1
2 12
2 12
3 123
3 123
1
;a s ; a s ; a s
JP =
1 1
2 12
3 123
a1 c1 + a2 c12 + a3 c123
;a2 s12 ; a3 s123 ;a3 s123 3
a2 c12 + a3 c123
0
0
0
1
a3 c123
0
0
0
1
77
77
75
;a2 s12 ; a3 s123 ;a3s123
a2 c12 + a3 c123
a3 c123
z (p ; p ) z (p ; p ) z (p ; p ) J=
z
z
z
0
0
1
1
0
2
1
2
203
2a c c 3
p = p = 405 p = 4a s c 5
0
as
2 c (a c + a c ) 3
p = 4 s (a c + a c ) 5
2 1 2
0
1
2
2 1 2
2 2
1
2 2
3 23
1
2 2
3 23
a2 s2 + a3 s23
203
2s 3
z = 4 0 5 z = z = 4 ;c 5
1
0
1
1
2
2
0
1
2 ;s (a c + a c ) ;c (a s + a s ) ;a c s
66 c (a c + a c ) ;s (a s + a s ) ;a s s
0
a c +a c
ac
J = 666
0
s
s
4
0
;c
;c
1
1
2 2
3 23
2 2
3 23
1
2 2
3 23
3 1 23
1
2 2
3 23
3 1 23
2 2
3 23
1
1
0
3 23
1
1
0
1
3
77
77
75
2 ;s (a c + a c ) ;c (a s + a s ) ;a c s 3
JP = 4 c (a c + a c ) ;s (a s + a s ) ;a s s 5
1
1
2 2
2 2
0
3 23
3 23
1
2 2
3 23
3 1 23
1
2 2
3 23
3 1 23
a2 c2 + a3 c23
a3 c23
Manipolatore di Stanford
z (p ; p ) z (p ; p ) z
J=
z
z
0
z (p ; p ) z (p ; p ) z (p ; p ) z
z
z
0
0
1
1
0
3
2
1
3
4
4
3
5
5
4
5
203
2c s d ; s d 3
p = p = 405 p = p = p = 4s s d + c d 5
0
1
3
0
4
5
1 2 3
1 2
1 2 3
1 2
c2 d3
2 c s d ; s d + d (c c c s + c c s ; s s s ) 3
p = 4 s s d + c d + d (c s s + c c s s + c s s ) 5
1 2 3
1 2
6
1 2 4 5
1 2 3
1 2
6
1 4 5
1 5 2
1 4 5
2 4 1 5
5 1 2
c2 d3 + d6 (c2 c5 ; c4 s2 s5 )
203
2 ;s 3
2c s 3
z = 405 z = 4 c 5 z = z = 4s s 5
1
0
1
1
1
0
1 2
2
3
1 2
c2
2 ;c c s ; s c 3
2c c c s ;s s s +c s c 3
z = 4 ;s c s + c c 5 z = 4 s c c s + c s s + s s c 5
4
1 2 4
1 4
1 2 4
1 4
s2 s4
5
1 2 4 5
1 4 5
1 2 5
1 2 4 5
1 4 5
1 2 5
;s2 c4 s5 + c2 c5
JACOBIANO ANALITICO
p = p(q)
= (q)
p_ = @@pq q_ = JP (q)q_
_ = @@q q_ = J(q)q_
p_ JP (q) x_ = _ = J(q) q_
= JA (q)q_
JA(q) = @ k@(qq)
Velocità di rotazione in terna corrente di angoli di Eulero ZYZ
?
per effetto di '_ :
[ !x !y !z ]T = '_ [ 0 0 1 ]T
?
per effetto di #_ :
[ !x !y !z ]T = #_ [ ;s' c' 0 ]T
?
per effetto di _ :
[ !x !y !z ]T = _ [ c' s# s' s# c# ]T
Composizione di velocità di rotazione elementari
2 0 ;s c s 3
' ' #
! = 4 0 c' s' s# 5 _ = T ()_
1
0
c#
Significato fisico di
!
! = [ =2 0 0 ]T 0 t 1
! = [ 0 =2 0 ]T 1 < t 2
Z
0
2
! = [ 0 =2 0 ]T 0 t 1
! = [ =2 0 0 ]T 1 < t 2
!dt = [ =2 =2 0 ]T
Relazione tra Jacobiano analitico e
I O v = O T () x_ = TA ()x_
J = TA ()JA
?
grandezze di significato fisico
Jacobiano analitico
?
grandezze differenziali di variabili nello spazio operativo
SINGOLARITÀ CINEMATICHE
v = J (q)q_
se
J diminuisce di rango =)
singolarit à cinematiche
(a) perdita di mobilità
(b) infinite soluzioni al problema cinematico inverso
(c) velocità elevate nello spazio dei giunti (nell’intorno di una
singolarità)
Classificazione
?
?
Singolarità ai confini dello spazio di lavoro raggiungibile
Singolarità all’interno dello spazio di lavoro raggiungibile
;a s ; a s
J=
1 1
2 12
a1 c1 + a2 c12
;a2s12
a2 c12
det(J ) = a1 a2 s2
+
#2 = 0
#2 = ? [ ;(a1 + a2 )s1 (a1 + a2 )c1 ]T parallelo a [ ;a2s1 a2 c1 ]T
(componenti di velocità dell’organo terminale non indipendenti)
Disaccoppiamento di singolarità
calcolo delle singolarit à della struttura portante
calcolo delle singolarit à del polso
J
J= J
11
21
J J
12
22
J = z (p ; p ) z (p ; p ) z (p ; p )
J = z z z 12
3
3
22
4
4
3
4
5
5
p = pW =) pW ; pi paralleli a zi, i = 3; 4; 5
J = 0
12
0
0
det(J ) = det(J11 )det(J22 )
det(J11 ) = 0
det(J22 ) = 0
5
Singolarità di polso
z3 parallelo a z5
#5 = 0
?
#5 = rotazioni uguali e opposte di # 4 e #6 non producono alcuna
rotazione dell’organo terminale
Singolarità di struttura portante
det(JP ) = ;a2 a3 s3 (a2 c2 + a3 c23 )
s3 = 0
?
a2 c2 + a3 c23 = 0
Singolarità di gomito
#3 = 0
#3 = ROBOTICA INDUSTRIALE
?
Singolarità di spalla
px = py = 0
ANALISI DELLA RIDONDANZA
Cinematica differenziale
v = J (q)q_
?
?
se %(
J) = r
;
dim R(J ) = r
;
dim N (J ) = n ; r
in generale
;
;
dim R(J ) + dim N (J ) = n
Se N (
J ) 6= ;
ove
?
q_ = q_ + P q_ a
R(P ) N (J )
verifica:
J q_ = J q_ + JP q_ a = J q_ = v
q_ a genera moti interni della struttura
INVERSIONE DELLA CINEMATICA
DIFFERENZIALE
Equazione cinematica non lineare
Equazione cinematica differenziale lineare nelle velocit à
Data
?
v(t) + condizioni iniziali =) (q(t); q_ (t))
se n = r
q_ = J ; (q)v
Zt
q(t) = q_ (& )d& + q(0)
1
0
?
regola di integrazione numerica (Eulero)
q(tk ) = q(tk ) + q_ (tk )t
+1
Manipolatori ridondanti
q
Per una data configurazione , trovare le soluzioni
disfino
v = J q_
e che minimizzino
g(q_ ) = 12 q_ T W q_
?
metodo dei moltiplicatori di Lagrange
g(q_ ; ) = 21 q_ T W q_ + T (v ; J q_ )
@g T
@ q_ = 0
?
@g T
@ = 0
soluzione ottima
q_ = W ; J T (JW ; J T ); v
1
?
se
W =I
1
q_ = J yv
J y = J T (JJ T );
è la pseudo-inversa destra di J
ove
1
1
q_ che sod-
Utilizzo della ridondanza
g0 (q_ ) = 12 (q_ T ; q_ aT )(q_ ; q_ a )
?
come sopra : : :
g0 (q_ ; ) = 21 (q_ T ; q_ aT )(q_ ; q_ a ) + T (v ; J q_ )
?
soluzione ottima
q_ = J yv + (I ; J yJ )q_ a
Caratterizzazione dei moti interni
@w(q) T
q_ a = ka @ q
?
misura di manipolabilit à
q ;
w(q) = det J (q)J T (q)
?
distanza dai fine-corsa dei giunti
n
X
qi ; qi
1
w(q) = ; 2n
i=1 qiM ; qim
?
distanza da un ostacolo
w(q) = min
kp(q) ; ok
p;o
2
Singolarità cinematiche
Le soluzioni precedenti valgono solo se
Se
J è di rango pieno
J non è di rango pieno (singolarità)
? se v 2 R(J ) =) soluzione q_ estraendo tutte le equazioni linearmente indipendenti (traiettoria “fisicamente”
eseguibile)
?
v
J
se 2
= R( ) =)
ria non eseguibile)
il sistema non è risolvibile (traietto-
Inversione nell’intorno di singolarit à
? det(J ) piccolo =) q_ elevate
?
inversa a minimi quadrati smorzata
J ? = J T (JJ T + k I );
2
ove
1
q_ minimizza
g00 (q_ ) = kv ; J q_ k2 + k2 kq_ k2
ALGORITMI PER L’INVERSIONE
CINEMATICA
Inversione cinematica
q(tk ) = q(tk ) + J ; (q(tk ))v(tk )t
1
+1
?
fenomeni di deriva della soluzione
Soluzione algoritmica
?
errore nello spazio operativo
e = xd ; x
e_ = x_ d ; x_
= x_ d ; JA (q)q_
?
trovare
q_ = q_ (e): e ! 0
(Pseudo-)inversa dello Jacobiano
Linearizzazione della dinamica di errore
q_ = JA; (q)(x_ d + Ke)
1
+
e_ + Ke = 0
?
Per un manipolatore ridondante
q_ = JAy (x_ d + Ke) + (I ; JAy JA)q_ a
Trasposta dello Jacobiano
q_ = q_ (e) senza linearizzare la dinamica di errore
Metodo di Lyapunov
V (e) = 12 eT Ke
ove
V (e) > 0 8e 6= 0
V (0) = 0
V_ (e) = eT K x_ d ; eT K x_
= eT K x_ d ; eT KJA (q)q_
?
la scelta
q_ = JAT (q)Ke
comporta che
V_ (e) = eT K x_ d ; eT KJA (q)JAT (q)Ke
?
?
x = 0 =) V_ < 0 con V > 0 (asintotica stabilità)
T ) 6= ; =) V_ = 0 se Ke 2 N (J T )
se N (JA
A
q_ = 0 con e 6= 0 (stallo?)
se _ d
x 6= 0
? e(t) limitato (conviene aumentare la norma di K )
? e(1) ! 0
Se _ d
Esempio
2
0
0
JPT = 4 ;c (a s + a s ) ;s (a s + a s )
1
?
2 2
3 23
;a3 c1 s23
1
2 2
3 23
;a3 s1 s23
J
nullo di PT
y = ; 1
x
tan #1
z = 0
3
0
0 5
a3 c23
Errore di orientamento
Errore di posizione
eP = pd ; p(q)
e_ P = p_ d ; p_
Angoli di Eulero
eO = d ; (q)
e_ O = _ d ; _
p_ + K e q_ = JA; (q) _ dd + KPO ePO
1
?
?
d (t)
richiede comunque il passaggio attraverso R = [ n s a ]
agevole per assegnare l’andamento temporale
Manipolatore con polso sferico
?
?
q =) RW
T Rd =) qO (angoli di Eulero ZYZ)
calcolare RW
calcolare P
Asse/angolo
?
R(#; r) = Rd RT
errore di orientamento
eO = r sin #
= 12 (n nd + s sd + a ad )
e_ O = LT !d ; L!
ove
;
1
L = ; 2 S (nd)S (n) + S (sd)S (s) + S (ad)S (a)
e_ p_ ; J (q)q_ P
e_ = e_ OP = LT !dd ; LJ
O (q)q_
p_ I O = LT d! ; O L J q_
d
_
p
+
K
e
q_ = J ; (q) L; ;LdT !d +P KPO eO 1
1
Quaternione unitario
Q = Qd Q;1
?
errore di orientamento
eO = = (q)d ; d(q) ; S (d)(q)
p_ + K e q_ = J ; (q) !dd + KPO ePO
!d ; ! + KO eO = 0
1
?
propagazione del quaternione
_ = ; 12 T !
_ = 12 (I ; S ()) !
?
studio della stabilità
V = (d ; )2 + (d ; )T (d ; )
V_ = ;eTO KO eO
Confronto tra gli algoritmi per l’inversione
cinematica
x = k(q)
2p 3 2a c +a c +a c 3
x
4 py 5 = 4 a s + a s + a s 5
1 1
2 12
3 123
1 1
2 12
3 123
#1 + #2 + #3
? a1 = a2 = a3 = 0:5 m
2 ;a s ; a s ; a s
JA = 4 a c + a c + a c
1 1
1 1
2 12
2 12
1
3 123
3 123
;a2 s12 ; a3 s123
a2 c12 + a3 c123
1
3
;a s
ac 5
3 123
3 123
1
? qi = [ ;=2 ;=2 ]T rad
+
? pdi = [ 0 0:5 ]T m
?
= 0 rad
traiettoria desiderata
0:25(1 ; cos t) pd(t) = 0:25(2 + sin t)
t
d (t) = sin 24
0t4
0t4
Simulazione in MATLAB con integrazione numerica di Eulero
q(tk ) = q(tk ) + q_ (tk )t
+1
e t = 1 ms
q_ = JA;1 (q)x_
norma errore pos
−3
2
x 10
0
−0.2
[rad]
[m]
1.5
1
0.5
0
0
errore orien
−5
x 10
−0.4
−0.6
−0.8
1
2
3
[s]
4
5
−1
0
1
2
3
[s]
4
5
q_ = JA;1 (q)(x_ d + Ke)
K = diagf500; 500; 100g
pos giunti
vel giunti
5
10
0
3
2
0
1
3
−5
2
−5
0
1
5
[rad/s]
[rad]
1
2
3
4
5
−10
0
1
2
[s]
norma errore pos
−5
x 10
0.8
−1
0.6
−2
0.4
0.2
0
0
4
5
4
5
errore orien
−8
0
[rad]
[m]
1
3
[s]
x 10
−3
−4
1
2
3
[s]
4
5
−5
0
1
2
3
[s]
libero (r = 2, n = 3)
q_ = J y (p_ d + KP eP )
KP = diagf500; 500g
P
norma errore pos
−6
5
x 10
orien
0.5
0
3
[rad]
[m]
4
2
−0.5
1
0
0
1
2
3
4
−1
0
5
1
2
[s]
3
4
5
4
5
[s]
q_ = JPT (q)KP eP
KP = diagf500; 500g
norma errore pos
orien
0.01
0.5
0
0.006
[rad]
[m]
0.008
0.004
−0.5
0.002
0
0
1
2
3
[s]
4
5
−1
0
1
2
3
[s]
q_ = JPy (p_ d + KP eP ) + (I ; JPy JP )q_ a
KP = diagf500; 500g
?
misura di manipolabilità
w(#2 ; #3 ) = 12 (s22 + s23 )
@w(q) T
? q_ a = ka @ q
ka = 50
pos giunti
pos giunti
5
5
1
0
[rad/s]
[rad]
1
3
0
2
2
−5
0
3
1
2
3
4
−5
0
5
1
2
[s]
norma errore pos
−6
5
x 10
3
4
5
3
4
5
[s]
manip
1
0.95
3
[rad]
[m]
4
2
0.9
1
0
0
1
2
3
[s]
4
5
0.85
0
1
2
[s]
?
distanza dai fine-corsa dei giunti
3
X
1
qi ; qi
w (q ) = ; 6
i=1 qiM ; qim
;2 q1 2
;=2 q2 =2
@w(q) T
? q_ a = ka @ q
ka = 250
6
4
4
2
2
[rad]
[rad]
pos giunto 2
6
0
0
−2
−2
−4
−4
−6
−6
1
2
3
2
;3=2 q3 ;=2
pos giunto 1
0
4
5
0
1
2
[s]
3
4
5
4
5
[s]
pos giunto 3
norma errore pos
−4
5
2
x 10
[m]
[rad]
1.5
0
1
0.5
−5
0
1
2
3
[s]
4
5
0
0
1
2
3
[s]
STATICA
Relazione tra forze e momenti (forze) all’organo terminale
e forze e/o coppie (coppie) ai giunti con il manipolatore in
configurazione di equilibrio
?
lavoro elementare compiuto dalle coppie
dW = T dq
?
lavoro elementare compiuto dalle forze
dW = f T dp + T !dt
= f T JP (q)dq + T JO (q)dq
= T J (q)dq
?
spostamenti elementari spostamenti virtuali
W = T q
W = T J (q)q
Principio dei lavori virtuali
?
il manipolatore è in equilibrio statico se e solo se
W = W
+
= J T (q)
8q
Dualità cineto-statica
N (J ) R?(J T )
forze
R(J ) N ?(J T )
2 N (J T ) interamente assorbite dalla struttura
Interpretazione fisica dello schema con la trasposta dello Jacobiano
?
?
= q_
forza elastica Ke che tira l’organo terminale verso la podinamica ideale
stura desiderata nello spazio operativo
?
ha effetto solo se
Ke 2= N (J T )
Trasformazione di velocità e forze
p_ I ;S (r ) p_ ! = O I
!
2
12
1
2
1
r =R r
1
1 12
12
p_ = R p_
! =R !
1
1
1
1
1
1
1
1
p_ = R p_ = R R p_
! =R ! =R R !
2
2
2
2
2
2
2
2
1
1
1
2
1
2
2
2
2
2
p_ R ;R S (r ) p_ ! = O
R
!
v =J v
2
2
2
2
2
1
1
12
2
1
2
2
?
2
1
1
1
1
1
2 1
1 1
in virtù della dualità cineto-statica:
= J T
f f R
O
= S (r )R R 1
1
1
1
1
1
2
1
2
2
1
2
1
12
1
2
1
2
2
2
2
2
Catena chiusa
Struttura ad albero equivalente a catena aperta
Risolvendo i vincoli: u
q qo = qua
q = qu(qa)
q_ o = q_ a2 3
I
= 4 @ qu 5
@ qa
?
per la dualità cineto-statica
a = T o
# qa = #
# qu = #
21 03
= 64 ;01 11 75
10
100
1 ;1
; + a = 11 + 22 ; 33
0
0
0
00
0
0
200
300
0
u = 0
ELLISSOIDI DI MANIPOLABILITÀ
Ellissoide di manipolabilit à in velocità
?
insieme delle velocità ai giunti a norma costante
q_ T q_ = 1
?
manipolatore ridondante
q_ = J y(q)v
+
;
T
T
v J (q)J (q) ;1v = 1
Assi
?
?
u JJ T =) direzioni
p
valori singolari = (JJ T ) =)
autovettori i di
i
i
Volume
?
proporzionale a
q ;
T
w(q) = det J (q)J (q)
dimensioni
Misura di manipolabilità
w = jdet(J )j = a1 a2 js2 j
? max per #2 = =2
? max per a1 = a2 (a parità di estensione a1 + a2 )
Ellissi di manipolabilità in velocità
1
[m]
0.5
0
−0.5
−1
0
0.5
1
[m]
1.5
2
Valori singolari
2.5
max
[m]
2
1.5
1
min
0.5
0
0
0.5
1
[m]
1.5
2
Ellissoide di manipolabilit à in forza
?
insieme delle coppie ai giunti a norma costante
T = 1
+
;
T
T
J (q)J (q) = 1
Dualità cineto-statica
?
una direzione lungo la quale si ha elevata manipolabilit à in
velocità è una direzione lungo la quale si ha scarsa manipolabilità in forza, e viceversa
Ellissi di manipolabilità in velocità
1
[m]
0.5
0
−0.5
−1
0
0.5
1
[m]
1.5
2
Ellissi di manipolabilità in forza
1
[m]
0.5
0
−0.5
−1
0
0.5
1
[m]
1.5
2
Manipolatore trasformatore meccanico di velocit à e forze
dallo spazio dei giunti allo spazio operativo
?
rapporto di trasformazione lungo una direzione per
l’ellissoide in forza
; =
(q) = uT J (q)J T (q)u
1 2
?
rapporto di trasformazione lungo una direzione per
l’ellissoide in velocità
;
; =
;
(q) = uT J (q)J T (q) u
1
?
utilizzazione di gradi di mobilità ridondanti
1 2
Compatibilità della struttura ad eseguire un compito assegnato
lungo una direzione
?
compito di scrittura su superficie orizzontale
vel
piano di scrittura
forza
?
compito di lancio di un peso in direzione orizzontale
forza
vel
direzione di lancio
DINAMICA
equazioni del moto del manipolatore in funzione delle forze e
momenti agenti su di esso
Formulazione di Lagrange
Proprietà notevoli del modello dinamico
Identificazione dei parametri dinamici
Formulazione di Newton-Eulero
Dinamica diretta e dinamica inversa
Modello dinamico nello spazio operativo
Ellissoide di manipolabilità dinamica
FORMULAZIONE DI LAGRANGE
Lagrangiana
(energia cinetica) ; (energia potenziale)
L=T ;U
Equazioni di Lagrange
d @L ; @L = dt @ _ i @i i
?
i = 1; : : : ; n
coordinate generalizzate
2 3
4 ... 5 = q
1
n
Esempio
?
energia cinetica
T = 12 I #_ 2 + 21 Im kr2#_ 2
?
?
energia potenziale
U = mg`(1 ; cos #)
lagrangiana
L = 12 I #_ 2 + 12 Im kr2 #_ 2 ; mg`(1 ; cos #)
?
equazione del moto
(I + Im kr2)# + mg` sin # = = ; F #_
Energia cinetica
Contributo bracci + attuatori
T=
n
X
i=1
(Tì + Tmi )
Braccio i
Z
1
Tì = 2 p_ i T p_ i dV
Vì
Z
1
pì = m` pi dV
Vì
i
?
baricentro
?
velocità lineare della particella elementare
p_ i = p_ ` + !i ri = p_ ` + S (!i)ri
i
i
Traslazionale
Mutuo
1 Z p_ T p_ dV = 1 m p_ T p_
2 Vì ì ì
2 ì ì ì
1Z
!
p_ Tì S (!i)ridV =
2 2
Vì
!
Z
2 12 p_ Tì S (!i ) (pi ; pì )dV = 0
Vì
Rotazionale
1Z
2 Vì
riT S T (!i)S (!i)ridV = 12 !iT
Z
Vì
!
S T (ri)S (ri)dV !i
= 12 !iT Iì !i
?
tensore di inerzia
2 R (r + r )dV ; R r r dV ; R r r dV 3
ix iy
ix iz
iy iz
R
R
I` = 64
(rix + riz )dV R; riy riz dV 75
(rix + riy )dV
2 I ;I ;I 3
` xx
` xy
` xz
6
=4 I` yy ;I` yz 75
2
2
2
i
2
2
i
i
i
i
i
Iì zz
2
Energia cinetica del braccio i
T` = 21 m` p_ T` p_ ` + 12 !iT Ri Iì RiT !i
i
i
i
i
i
= 12 mì q_ T JP(ì )T JP(ì ) q_ + 12 q_ T JO(ì )T Ri Iìi RiT JO(ì ) q_
?
velocità lineare
p_ ` = |P` q_ + : : : + |Pi` q_i = JP` q_
i
( i)
1 1
( i)
zj;
|Pj` = zj; (p` ; pj; )
1
( i)
1
?
1
i
( i)
velocità angolare
!i = |O` q_ + : : : + |Oi` q_i = JO` q_
0
un giunto prismatico
|Oj` = zj; per
( i)
1 1
( i)
1
( i)
( i)
Motore i (elettrico rotante)
?
trasmissione rigida
!m = !i; + kriq_izm
i
1
i
Energia cinetica del rotore i
Tm = 12 mm p_ Tm p_ m + 12 !mT Im !m
i
i
i
i
i
i
T J (mi ) q_
= 12 mmi q_ T JP(mi )T JP(mi ) q_ + 12 q_ T JO(mi )T Rmi Immii Rm
i O
?
velocità lineare
JPm = |Pm : : : |P;im;
(
i)
(
1
|Pjm
(
i
i)
(
zj;
= zj; (pm ; pj; )
1
i
i)
0
1
:::
0
1
1
?
i)
velocità angolare
JOm = |Om : : : |O;im ; |Oim
(
i)
|Ojmi) =
(
(
1
i)
(
|Oj`
kri zm
(
( i)
i
i)
1
(
i)
0
j = 1; : : : ; i ; 1
j=i
:::
0
Energia cinetica totale
Matrice di inerzia
n X
n
X
1
bij (q)q_i q_j = 12 q_ T B (q)q_
T =2
i=1 j =1
B(q) =
n X
i=1
mì JP(ì )T JP(ì ) + JO(ì )T RiIìi RiT JO(ì )
T J (mi )
+mmi JPmi )T JP(mi ) + JO(mi )T Rmi Immii Rm
i O
(
?
?
?
simmetrica
definita positiva
dipendente dalla configurazione (in generale)
Energia potenziale
U=
Braccio i
U` = ;
i
Rotore i
n
X
i=1
Z
Vì
gT pi dV = ;m` gT p`
0
i
0
i
Um = ;mm g0T pm
i
(Uì + Umi )
i
i
Energia potenziale totale
U =;
n
X
i=1
(mì g0T pì + mmi g0T pmi )
Equazioni del moto
L(q; q_ ) = T (q; q_ ) ; U (q)
n ;
n X
n
X
X
1
T
T
=2
bij (q)q_iq_j + mì g0 pì (q)+ mmi g0 pmi (q)
i=1 j =1
i=1
Equazioni di Lagrange
n
n db (q)
X
d @L = d @T = X
ij q_
bij (q)qj +
j
dt @ q_i
dt @ q_i
j =1
j =1 dt
n
n X
n @b (q)
X
X
ij q_ q_
= bij (q)qj +
k j
@q
k
j =1
j =1 k=1
n X
n @b (q)
@T = 1 X
jk q_ q_
@qi 2 j=1 k=1 @qi k j
n
@U = ; X
T @ p`j + mm gT @ pmj
m
g
`
j 0
j 0
@qi
@q
@qi
i
j =1
=;
n X
j =1
= gi (q)
(`j )
(mj )
T
T
m`j g0 |Pi (q) + mmj g0 |Pi (q)
Equazioni del moto
n
X
j =1
bij (q)qj +
ove
n X
n
X
j =1 k=1
hijk (q)q_k q_j + gi (q) = i
ij ; 1 @bjk
hijk = @b
@q 2 @q
k
?
i = 1; : : : ; n
i
termini in accelerazione:
il coefficiente bii rappresenta il momento di inerzia visto
all’asse del giunto i, nella configurazione corrente del manipolatore, quando gli altri giunti sono bloccati
il coefficiente bij tiene conto dell’effetto dell’accelerazione
del giunto j sul giunto i
?
termini quadratici in velocit à:
il termine hijj q_j2 rappresenta l’effetto centrifugo indotto al
giunto i dalla velocità del giunto j ; si noti che h iii = 0,
poiché @bii =@qi = 0
il termine hijk q_j q_k rappresenta l’effetto di Coriolis indotto
al giunto i dalle velocità dei giunti j e k
?
termini dipendenti solo dalla configurazione:
il termine gi rappresenta la coppia generata all’asse del
giunto i nella configurazione corrente del manipolatore per
effetto della gravità
Forze non conservative
?
?
?
?
coppie di attuazione
Fq
coppie di attrito statico ;f s (q ; q_ ) Fs sgn (q_ )
coppie di bilanciamento di forze di contatto ;J T (q )h
coppie di attrito viscoso ; v _
Modello dinamico nello spazio dei giunti
B(q)q + C (q; q_ )q_ + Fv q_ + fs (q_ ) + g(q) = ; J T (q)h
n
X
j =1
cij q_j =
n X
n
X
j =1 k=1
hijk q_k q_j
PROPRIETÀ NOTEVOLI DEL MODELLO
DINAMICO
Anti-simmetria della matrice B_ ; 2C
n
X
j =1
cij q_j =
n X
n
X
Elementi di
?
j =1 k=1 @qk 2
n X
n @b
X
1
ij q_ q_ +
ik
k
j
@qk
2 j=1 k=1 @qj
j =1 k=1
n X
n @b
1X
=2
n X
n @b
X
1
@b
ij
jk
hijk q_k q_j =
;
q_k q_j
j =1 k=1
C
cij =
n
X
k=1
@b
; @qjk q_k q_j
i
cijk q_k
simboli di Christoffel del primo tipo
ij + @bik ; @bjk
cijk = 12 @b
@qk @qj @qi
@qi
Proprietà notevole
N (q; q_ ) = B_ (q) ; 2C (q; q_ ) = ;N T (q; q_ )
Elementi di
C
n
X
1
@bik ; @bjk q_
q
_
+
k
@qk
2k
@qj @qi k
n @b
1X
@b
ik
jk
n @b
1X
ij
cij = 2
k=1
= 21 b_ ij + 2
Elementi di
=1
k=1
@qj ; @qi q_k
N
n @b
X
@b
jk ; ik q_ = ;n
nij = b_ ij ; 2cij =
k
ji
@qi
k=1
@qj
+
wT N (q; q_ )w = 0
?
se
8w
w = q_ :
q_ T N (q; q_ )q_ = 0
8C
Principio di conservazione dell’energia (Hamilton)
1 d ;q_ T B (q)q_ = q_ T ; ; F q_ ; f (q_ ) ; g(q) ; J T (q)h
v
s
2 dt
1 d ;q_ T B (q)q_ = 1 q_ T B_ (q)q_ + q_ T B (q)q
2 dt
2
;
= 12 q_ T B_ (q) ; 2C (q; q_ ) q_
;
T
T
+ q_ ; Fv q_ ; fs (q_ ) ; g(q) ; J (q)h
Linearità nei parametri dinamici
Insieme braccio i e rotore i + 1
Ti = T` + Tm +1
i
i
T` = 21 mì p_ T` p_ ` + 12 !iT I` !i
i
i
i
i
Tm +1 = 12 mm +1 p_ Tm +1 p_ m +1 + 21 !mT +1 Im +1 !m +1
i
?
i
i
i
i
i
con riferimento al baricentro
rotore
pC
dell’insieme braccio–
i
p_ ` = p_ C + !i rC ;`
p_ m +1 = p_ C + !i rC ;m +1
i
i
i
i
i
i
i
rC ;` = p` ; pC
rC ;m +1 = pm +1 ; pC
i
i
i
i
i
i
i
i
i
i
Braccio
T` = 12 mì p_ TC p_ C + p_ TC S (!i)m` rC ;`
i
i
i
i
i
i
i
+ 12 mì !iT S T (rCi ;ì )S (rCi ;ì )!i + 12 !iT Iì !i
= 21 mì p_ TCi p_ Ci + p_ TCi S (!i)mì rCi ;ì + 12 !iT Iì !i
?
per il teorema di Steiner
I` = I` + m` S T (rC ;` )S (rC ;` )
i
i
i
i
i
i
i
Rotore
Tm +1 = 21 mm +1 p_ TC p_ C + p_ TC S (!i)mm +1 rC ;m +1 + 21 !iT Im +1 !i
i
i
i
i
i
i
i
i
i
2
2
T
+ kr;i+1 q_i+1 zmT i+1 Imi+1 !i + 12 kr;i
+1 q_i+1 zmi+1 Imi+1 zmi+1
?
Im +1 = Im +1 + mm +1 S T (rC ;m +1 )S (rC ;m +1 )
i
i
i
i
i
i
i
Energia cinetica
Ti = 12 mi p_ TC p_ C + 12 !iT Ii !i + kr;i+1 q_i+1 zmT +1 Im +1 !i
i
i
2
2
T
+ 12 kr;i
+1 q_i+1 zmi+1 Imi+1 zmi+1
?
i
i
rotore con distribuzione di massa simmetrica intorno al suo
asse di rotazione
2I
0
m xx
m
Im = 4 0 Im xx
3
0
0 5
i
i
i
i
0
0
Imi zz
+
Im +1 zm +1 = Rm +1 Imm +1+1 RmT +1 zm +1 = Im +1 zm +1
i
i
i
i
i
i
i
i
i
Quantità riferite alle terne solidali ai bracci
Ti = 12 mip_ iTC p_ iC + 12 !iiT Iii !ii + kr;i+1 q_i+1 Im +1 zmiT +1 !ii
i
i
i
2
2
+ 21 kr;i
+1 q_i+1 Imi+1
i + p_ iT S (!i )mi ri + 1 !iT I^ i !i
_
p
= 12 mip_ iT
i i
i
i
i;Ci 2 i i i
2
2
+ kr;i+1 q_i+1 Imi+1 zmiTi+1 !ii + 12 kr;i
+1 q_i+1 Imi+1
?
i )S (ri )
Iîi = Iii + miS T (ri;C
i;C
i
i
i
Energia cinetica lineare rispetto a:
?
?
massa mi
momento primo di inerzia
2m ` 3
i Cx
i
4
miri;C = mi `C y 5
i
i
i
?
mi`Ci z
tensore di inerzia
2 I + m (` + ` ) ;I ; m ` `
ixx
i Cy Cz
ixy
i Cx Cy
Iîi = 64
Iiyy + mi(`C z + `C x )
3
;Iixz ; mi `C x `C z
;Iiyz ; mi `C y `C z 75
2
i
2
i
i
2
2 Iîxx ;Iîxy ;Iîxz 3
= 64 Iîyy Iîyz 75
Iîzz
i
2
i
i
i
i
i
2
i
i
Iizz + mi (`2Ci x + `C y )
Energia potenziale
Ui = ;mig0iT piC
i )
= ;g0iT (mipii + mi ri;C
i
i
lineare rispetto a:
?
?
massa mi
momento primo di inerzia
2m ` 3
i Cx
i
miri;C = 4 mi `C y 5
i
i
i
mi`Ci z
Lagrangiana
L=
?
n
X
i=1
(TT i ; UT i )i
vettore (11 1) di parametri dinamici
i = [ mi mi`C x mi`C y mi`C z
i
i
i
Iîxx Iîxy Iîxz Iîyy Iîyz Iîzz Imi ]T
Le operazioni richieste dalle equazioni di Lagrange non alterano la linearità
i =
n
X
j =1
yijT j
ove
T j + @ U j
yij = dtd @@q_Ti j ; @@q
@qi
i
Proprietà notevole
3 2 yT yT : : : yTn 3 2 3
77 = 66 0T yT : : : yTn 77 66 77
5 64 ... ... . . . ... 75 64 ... 75
.
T
n
n
0T 0T : : : ynn
2
66 4 ..
1
11
2
?
12
1
1
22
2
2
in forma compatta:
= Y (q; q_ ; q)
Manipolatore cartesiano a due bracci
Motori
? masse mmi
? momenti di inerzia Imi
? pmi = pi;1 zmi = zi;1
?
Jacobiani
JP`1
(
)
20
= 40
20
JPm1 = 4 0
0
20
JOm1 = 4 0
(
(
)
)
kr1
3
0
05
1 0
3
0
05
0
3
0
05
0
(
JPm2
(
)
JOm2
)
(
20
= 40
1
20
= 40
m +m + k I +m
B = `1 m2 r m1 `2
2
1
3
0
05
0
3
kr2
0 5
0 0
0
m`2 + kr22 Im2
Forze gravitazionali
g1 = (m`1 + mm2 + m`2 )g
)
3
1
05
1 0
Matrice di inerzia
0
JP`2
20
= 40
g2 = 0
Equazioni del moto
(m`1 + mm2 + kr21 Im1 + m`2 )d1 + (m`1 + mm2 + m`2 )g = f1
(m`2 + kr22 Im2 )d2 = f2
Motori
? masse mmi
? momenti di inerzia Imi
? pmi = pi;1 zmi = zi;1
?
JP`1)
(
Jacobiani
2 ;` s 0 3
= 4 ` c 05
JP`2)
1 1
1 1
0
JO`1
(
JPm1)
(
(
0
)
2 ;a s ; ` s
=4 a c +` c
1 1
1 1
20 03
= 40 05
20
= 40
JO`2
(
1 0
3
0
05
0 0
2 0 03
JO(m1) = 4 0 0 5
kr1 0
JPm2)
(
)
2 12
0
2 12
20 03
= 40 05
1 1
2 ;a s 0 3
= 4 a c 05
1 1
1 1
0 0
20 0 3
JO(m2) = 4 0 0 5
1 kr2
3
`c 5
;`2s12
2 12
0
Matrice di inerzia
b (# ) b (# ) B(q) =
11
2
b21 (#2 )
12
b22
2
b11 = I`1 + m`1 `21 + kr21 Im1 + I`2 + m`2 (a21 + `22 + 2a1`2 c2 )
+ Im2 + mm2 a21
b12 = b21 = I`2 + m`2 (`22 + a1 `2 c2 ) + kr2 Im2
b22 = I`2 + m`2 `22 + kr22 Im2
Forze centrifughe e di Coriolis
11
c111 = 21 @b
@q1 = 0
11
c112 = c121 = 21 @b
@q2 = ;m`2 a1 `2 s2 = h
1 @b22 = h
12
;
c122 = @b
@q2 2 @q1
1 @b11 = ;h
21
c211 = @b
;
@q1 2 @q2
22
c212 = c221 = 21 @b
@q1 = 0
22
c222 = 12 @b
@q2 = 0
h#_
_ 1 + #_ 2 ) h
(
#
C (q; q_ ) = ;h#_ 1
0
?
2
anti-simmetria
N (q; q_ ) = B_ (q) ; 2C (q; q_ )
2h#_ h#_ h#_
_
_
h
(
#
+
#
)
2
2
1
2
= _2
;
2
h#2 0
;h#_ 1
0
0
;
2h#_ 1 ; h#_ 2
= 2h#_ + h#_
0
1
2
g1 = (m`1 `1 + mm2 a1 + m`2 a1 )gc1 + m`2 `2 gc12
g2 = m`2 `2 gc12
Equazioni del moto
;I + m ` + k I + I + m (a + ` + 2a ` c )
`1
`1
`2
r m1 `2
+Im2 + mm2 a #
;
+ I + m (` + a ` c ) + k I #
2
1
2
1
2
1
2
2
1 2 2
2
1
1
`2
`2 22
1 2 2
r2 m2 2
; 2m`2 a1 `2 s2 #_ 1 #_ 2 ; m`2 a1 `2 s2 #_ 22
+ (m`1 `1 + mm2 a1 + m`2 a1 )gc1 + m`2 `2 gc12 = 1
;I + m ` + k I # + ;I + m (` + a ` c ) + k I #
`2
`2
`2
`2
r m2
r m2
2
2
2
2
2
+ m`2 a1 `2 s2 #_ 21 + m`2 `2 gc12 = 2
2
2
1 2 2
2
1
Parametrizzazione del modello
= [ ]T
1
2
3
4
5
6
7
8
1 = m1 = m`1 + mm2
2 = m1 `C1 = m`1 (`1 ; a1 )
3 = I^1 = I`1 + m`1 (`1 ; a1 )2 + Im2
4 = Im1
5 = m2 = m`2
6 = m`2 `C2 = m`2 (`2 ; a2 )
7 = I^2 = I`2 + m`2 (`2 ; a2 )2
8 = Im2
y
Y = y1121 yy1222 yy1323 yy1424 yy1525 yy1626 yy1727 yy1828
y11 = a21 #1 + a1 gc1
y12 = 2a1 #1 + gc1
y13 = #1
y14 = kr21 #1
y15 = (a21 + 2a1 a2 c2 + a22 )#1 + (a1 a2 c2 + a22 )#2
; 2a1 a2 s2 #_ 1 #_ 2 ; a1 a2 s2 #_ 22 + a1 gc1 + a2 gc12
y16 = (2a1 c2 + 2a2 )#1 + (a1 c2 + 2a2 )#2 ; 2a1 s2 #_ 1 #_ 2
; a1 s2 #_ 22 + gc12
y17 = #1 + #2
y18 = #1 + kr2 #2
y21 = 0
y22 = 0
y23 = 0
y24 = 0
y25 = (a1 a2 c2 + a22 )#1 + a22 #2 + a1 a2 s2 #_ 21 + a2 gc12
y26 = (a1 c2 + 2a2 )#1 + 2a2 #2 + a1 s2 #_ 21 + gc12
y27 = #1 + #2
y28 = kr2 #1 + kr22 #2 .
Esempio
? a1 = a2 = 1 m `1 = `2 = 0:5 m
m`1 = m`2 = 50 kg I`1 = I`2 = 10 kg m2
? kr1 = kr2 = 100
mm1 = mm2 = 5 kg Im1 = Im2 = 0:01 kg m2
pos giunto 2
4
4
3
3
2
2
[rad]
[rad]
pos giunto 1
1
1
0
0
−1
−1
−2
0
0.1
0.2
0.3
[s]
0.4
−2
0
0.5
0.1
6
6
4
4
2
0
0
0.1
0.2
0.3
[s]
0.4
0.5
0
0.1
20
20
10
10
[rad/s^2]
[rad/s^2]
0.2
0.3
[s]
0.4
0.5
acc giunto 2
30
0
−10
−20
0
−10
−20
0.1
0.2
0.3
[s]
0.4
−30
0
0.5
0.1
coppia giunto 1
6000
6000
4000
4000
2000
2000
0
−2000
−4000
−4000
0.2
0.3
[s]
0.4
0.3
[s]
0.4
0.5
0
−2000
0.1
0.2
coppia giunto 2
[Nm]
[Nm]
0.5
0
acc giunto 1
−6000
0
0.4
2
30
−30
0
0.3
[s]
vel giunto 2
[rad/s]
[rad/s]
vel giunto 1
0.2
0.5
−6000
0
0.1
0.2
0.3
[s]
0.4
0.5
inerz_11
inerz_22
5000
[Nm]
[Nm]
5000
0
−5000
0
0.1
0.2
0.3
[s]
0.4
0.5
0
−5000
0
0.1
0.2
100
0
0
−100
−100
−200
−300
−400
−400
0.1
0.2
0.3
[s]
0.5
0.4
0.5
0.4
0.5
0.4
0.5
−200
−300
−500
0
0.4
inerz_21
100
[Nm]
[Nm]
inerz_12
0.3
[s]
0.4
−500
0
0.5
0.1
0.2
centrif_2 + coriol_12
0.3
[s]
centrif_1
1000
1000
500
[Nm]
[Nm]
12
2
0
−500
0
500
0
0.1
0.2
0.3
[s]
0.4
−500
0
0.5
0.1
0.2
grav_2
800
800
600
600
400
400
[Nm]
[Nm]
grav_1
200
200
0
0
−200
−200
0
0.1
0.2
0.3
[s]
0.3
[s]
0.4
0.5
0
0.1
0.2
0.3
[s]
pos giunto 2
4
3
3
2
2
[rad]
[rad]
pos giunto 1
4
1
1
0
0
−1
−1
−2
0
0.2
0.4
−2
0
0.6
0.2
[s]
4
4
2
2
0
0
−2
−2
−4
−4
0.2
0.4
−6
0
0.6
0.2
[s]
20
20
0
−20
0
−20
0.2
0.4
−40
0
0.6
0.2
0.4
[s]
[s]
coppia giunto 1
coppia giunto 2
0.6
5000
[Nm]
5000
[Nm]
0.6
acc giunto 2
40
[rad/s^2]
[rad/s^2]
acc giunto 1
0
−5000
0
0.4
[s]
40
−40
0
0.6
vel giunto 2
6
[rad/s]
[rad/s]
vel giunto 1
6
−6
0
0.4
[s]
0
−5000
0.2
0.4
[s]
0.6
0
0.2
0.4
[s]
0.6
inerz_22
6000
4000
4000
2000
2000
[Nm]
[Nm]
inerz_11
6000
0
0
−2000
−2000
−4000
−4000
−6000
−6000
0
0.2
0.4
0.6
0
0.2
[s]
1000
1000
500
500
0
0
−500
−500
0.2
0.4
0.6
0
0.2
0.4
[s]
[s]
centrif_1
1000
0.6
1000
12
500
[Nm]
500
[Nm]
0.6
inerz_21
1500
[Nm]
[Nm]
inerz_12
1500
0
0.4
[s]
0
0
2
−500
0
−500
0.2
0.4
0.6
0
0.2
[s]
800
800
600
600
400
400
200
200
0
0
0.2
0.4
[s]
0.6
grav_2
1000
[Nm]
[Nm]
grav_1
1000
0
0.4
[s]
0.6
0
0.2
0.4
[s]
0.6
pos organo terminale
2
[m]
1.5
1
0.5
0
0
0.1
0.2
0.3
[s]
0.4
0.5
vel organo terminale
5
[m/s]
4
3
2
1
0
0
0.1
0.2
0.3
[s]
0.4
0.5
acc organo terminale
40
[m/s^2]
20
0
−20
−40
0
0.1
0.2
0.3
[s]
0.4
0.5
pos giunto 2
4
3
3
2
2
[rad]
[rad]
pos giunto 1
4
1
1
0
0
−1
−1
−2
0
0.1
0.2
0.3
[s]
0.4
−2
0
0.5
0.1
4
4
2
2
0
0
−2
−4
−6
−6
0.1
0.2
0.3
[s]
0.4
−8
0
0.5
0.1
60
60
40
40
20
0
−40
−40
0.3
[s]
0.4
0.5
0
0.1
coppia giunto 1
0.5
0.2
0.3
[s]
0.4
0.5
5000
[Nm]
[Nm]
0.4
coppia giunto 2
5000
0
−5000
0
0.3
[s]
0
−20
0.2
0.2
20
−20
0.1
0.5
acc giunto 2
80
[rad/s^2]
[rad/s^2]
acc giunto 1
80
0
0.4
−2
−4
−8
0
0.3
[s]
vel giunto 2
[rad/s]
[rad/s]
vel giunto 1
0.2
0
−5000
0.1
0.2
0.3
[s]
0.4
0.5
0
0.1
0.2
0.3
[s]
0.4
0.5
inerz_11
4
1
x 10
1
0.5
[Nm]
[Nm]
0.5
0
−0.5
−1
0
inerz_22
4
x 10
0
−0.5
0.1
0.2
0.3
[s]
0.4
−1
0
0.5
0.1
0.2
3000
2000
2000
1000
0
−1000
−1000
0.1
0.2
0.3
[s]
0.4
0.5
0
0.1
0.2
12
[Nm]
[Nm]
0.5
0.4
0.5
0.4
0.5
500
0
−500
0
−500
2
−1000
−1000
0.1
0.2
0.3
[s]
0.4
0.5
0
0.1
0.2
grav_1
1000
800
800
600
600
400
400
200
200
0
0
0.1
0.2
0.3
[s]
0.3
[s]
grav_2
1000
[Nm]
[Nm]
0.4
1000
500
0
0.3
[s]
centrif_1
1000
0
0.5
1000
0
0
0.4
inerz_21
3000
[Nm]
[Nm]
inerz_12
0.3
[s]
0.4
0.5
0
0.1
0.2
0.3
[s]
?
Jacobiani
2 ;` s 0 0 3
2 ;a s ; ` s ;` s
JP`1 =4 ` c 0 0 5 JP`2 = 4 a c + ` c ` c
(
0)
10
10
10 10
0
(
10
0)
10
10 10
0 0
0
2
10 2 0
20 10 20
2;a s ; a s ; ` s ;a s ; ` s
JP`3 =4 a c + a c + ` c
a c +` c
0
0
2 ;` s 3
JP`1 = 4 ` c 5
(
0)
10 10
10 10
20 10 20
20 10 20
30 10 20 30
(
30 10 20 30
20 10 20
20 10 20
20
10 20
2 10 20
30 10 20 30
30 10 20 30
0
3
0
05
0
;`3 s1 2 3
0
0 0
`3 c1 2 3
0
0
0
0 0
0
3
5
100 100
00 )
100 100
0
JO`1
(
0)
20 0 03
20 0 03
20 0 03
= 4 0 0 0 5 JO`2 = 4 0 0 0 5 JO`3 = 4 0 0 0 5
(
0)
1 0 0
(
1 1 0
JO`1
(
00 )
203
= 405.
1
0)
1 1 1
Matrici di inerzia per i manipolatori virtuali
2 b (# ; # ) b (# ; # ) b (# ; # ) 3
B0 (q0) = 4 b (# ; # ) b (# )
b (# ) 5
10 10
20
30
20 10
20
30
0 0
0
0
10 20
b3 1 (#2 ; #3 )
20
30
20 20
30
0 0
0
10 30
20
30
20 30
b3 2 (#3 )
30
b3 3
0
0
b1 1 = I`1 + m`1 `21 + I`2 + m`2 (a21 + `22 + 2a1 `2 c2 ) + I`3
+ m`3 (a21 + a22 + `23 + 2a1 a2 c2 + 2a1 `3 c2 3 + 2a2 `3 c3 )
b1 2 = b2 1 = I`2 + m`2 (`22 + a1 `2 c2 ) + I`3
+ m`3 (a22 + `23 + a1 a2 c2 + a1 `3 c2 3 + 2a2 `3 c3 )
b1 3 = b31 = I`3 + m`3 (`23 + a1 `3 c2 3 + a2 `3 c3 )
b2 2 = I`2 + m`2 `22 + I`3 + m`3 (a22 + `23 + 2a2 `3 c3 )
b2 3 = I`3 + m`3 (`23 + a2 `3 c3 )
b3 3 = I`3 + m`3 `23
0 0
0
0
0
0 0
0
0 0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
X
30
j =1
0
0 0
0
00
0
0
bi j #j
0 0
0
00
.
1 = b1 1 #1
00
0 0
0
00
0
0
0
0
0
0
0
0
00
0
0
0
0
0 0
0
0
0
0
b1 1 = I`1 + m`1 `21
i =
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0 0
0 0
0
0
0 0
0
0 0
0
0
0
0
0
0
Manipolatore a catena chiusa
a = Ba qa
b
ba12
ba22
Ba = baa
11
21
ba11 = I`1 + m`1 `21 + m`2 a21
+ I`3 + m`3 `23 + m`3 a21 ; 2a1 m`3 `3
;
ba12 = ba21 = a1 m`2 `2 + a1 m`3 (a1 ; `3 ) cos (#1 ; #1 )
ba22 = I`1 + m`1 `21 + I`2 + m`2 `22 + m`3 a21 .
0
0
0
0
0
0
0
?
0
0
0
0
0
0
0
0
0
0
00
0
0
0
0
0
0
0
0
00
00
0
matrice costante e diagonale
m`3 `3 = a1
m`2 `2 a1
0
0
0
0
0
00
`
`
ba11 = I`1 + m`1 `21 + m`2 a21 1 + a 2 a3 + I`3
1 1 a a ba22 = I`1 + m`1 `21 + I`2 + m`2 `22 1 + `1 `1
2 3
0
0
0
0
0
0
0
0
0
0
0
00
0
0
0
0
00
0
0
0
0
g1 = (m`1 `1 + m`2 a1 + m`3 a1 )gc1 + (m`2 `2 + m`3 a2 )gc1 2
+ m`3 `3 gc1 2 3
g2 = (m`2 `2 + m`3 a2 )gc1 2 + m`3 `3 gc1 2 3
g3 = m`3 `3 gc1 2 3
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0 0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
g1 = m`1 `1 gc1
00
00
00
00
(m ` + m a ; m ` )gc ga = (m` `1 ` + m``2 ` + m``3 a )gc
1
2
3
0
00
10
0
10
0
30
100
0
20
0
100
10
100
0
0
0
0
IDENTIFICAZIONE DEI PARAMETRI
DINAMICI
Impiego del modello dinamico per simulazione e controllo
Stime approssimate dei parametri dinamici
?
?
semplificazioni di modellazione geometrica
effetti dinamici complessi (attriti, etc.)
Tecniche di identificazione
?
proprietà di linearità
= Y (q; q_ ; q)
Misure
? q e q_ dirette (encoder e tachimetri)
? q indirette (filtro ricostruttore)
? dirette (sensore di coppia) o indirette (misure di forza o
corrente)
N insiemi di misure
2 (t ) 3 2 Y (t ) 3
= 64 ... 75 = 64 ... 75 = Y (tN )
Y (tN )
1
1
Soluzione (a minimi quadrati)
= (Y T Y ); Y T 1
? Y triangolare =) procedimento iterativo
? se %(Y ) < p =) Y ?
Parametri
?
?
?
identificabili
non identificabili
identificabili in combinazione lineare
Carico
?
modifica parametri dell’ultimo braccio
Traiettorie
?
?
di tipo polinomiale (sufficientemente ricche)
attenzione alle dinamiche non modellate
FORMULAZIONE DI NEWTON-EULERO
?
bilancio di forze e momenti agenti sul generico braccio
Equazione di Newton (moto traslazionale del baricentro)
fi ; fi + mig = mipC
+1
0
i
Equazione di Eulero (moto rotazionale)
i + fi ri; ;C ; i ; fi ri;C
d (I ! + k q_ I z )
= dt
i i
r;i i m +1 m +1
1
i
+1
+1
i
+1
+1
i
i
d (I ! ) = R_ Ii RT ! + R Ii R_ T ! + R Ii RT !_
i i i i
i i i i
i i i i
dt i i
= S (!i)Ri Iii RiT !i + Ri Iii RiT S T (!i )!i + Ri Iii RiT !_ i
= Ii !_ i + !i (Ii !i )
d (q_ I z ) = q I z + q_ I ! z
i+1 mi+1 mi+1 i+1 mi+1 i mi+1
dt i+1 mi+1 mi+1
+
Equazione di Eulero
i + fi ri; ;C ; i ; fi ri;C = Ii!_ i + !i (Ii!i)
+ kr;i qi Im +1 zm +1 + kr;i q_i Im +1 !i zm +1
1
+1
+1
i
+1
+1
i
i
+1
i
+1
i
Forza generalizzata al giunto
(
T zi; + kri Imi !_ T zm
f
m
i
i = T
i zi; + kriImi!_ mT zm
1
i
i
1
i
i
i
Accelerazioni di un braccio
!i;
!i = !i; + #_ izi; prismatico
rotoidale
(_
p
i; + d_i zi; + !i ri; ;i
p_ i =
p_ i; + !i ri; ;i
1
1
1
1
1
1
prismatico
1
rotoidale
1
!_ i;
!_ i = !_ + # z + #_ ! z
i;
i i;
i i;
i;
8
>
i; + dizi; + 2d_i!i zi;
p
>
< +!_ i ri; ;i + !i (!i ri; ;i)
pi = >
>
: p+i;!i + !(_!i irir;i;;i;i)
1
1
1
1
1
1
1
1
1
1
1
pC = pi + !_ i ri;C + !i (!i ri;C )
i
i
!_ m = !_ i; + kriqizmi + kriq_i!i; zmi
i
1
prismatico
1
1
i
prismatico
rotoidale
1
rotoidale
Algoritmo ricorsivo
Ricorsione in avanti di velocità e accelerazioni
?
?
! , p ; g , !_
i , p C , !_ m
calcolo di !i , !_ i , p
condizioni iniziali:
0
0
0
i
i
0
Ricorsione all’indietro di forze e momenti
?
?
h = [ fnT
condizioni terminali:
+1
Tn ]T
+1
calcolo di
fi = fi + mipC
+1
i
i = ;fi (ri; ;i + ri;C ) + i + fi ri;C
+ Ii !_ i + !i (Ii !i)
+ kr;i qi Im +1 zm +1 + kr;i q_i Im +1 !i zm +1
1
+1
(
+1
+1
i
i
+1
+1
i
+1
i
i
T zi; + kri Imi !_ T zm + Fvi d_i + Fsi sgn (d_i )
f
m
i
i = T
i zi; + kriImi!_ mT zm + Fvi#_ i + Fsi sgn (#_ i)
i
1
i
i
prismatico
1
i
i
rotoidale
Conviene riferire tutti i vettori alla terna corrente sul braccio i
?
I r
z
i , i;1 sono costanti
le quantità ii , i;C
mi
i
( Ri; T !i;
1
1
prismatico
i
i;1
i
!i = i;1T i;1 _
(4.106)
Ri (!i;1 + #iz0) rotoidale
( Ri;1T !_ i;1
prismatico
i
i;1
i
!_ i = i;1T i;1
(4.107)
i
;
1
_
Ri (!_ i;1 + #iz0 + #i!i;1 z0) rotoidale
8 Ri;1T (pi;1 + diz0) + 2d_i!i Ri;1T z0
>
i
i
i;1
i
>
i ri
i
i
i
<
_
prismatico
+
!
i
i
;
1;i + !i (!i ri;1;i )
i
pi = > i;1T i;1 i i
(4.108)
Ri pi;1 + !_ i ri;1;i
>
: +!i (!i ri )
rotoidale
i
i
i;1;i
i + !i (!i ri )
piCi = pii + !_ ii ri;C
(4.109)
i
i
i;Ci
i
!_ mi;i1 = !_ ii;;11 + kriqizmi;i1 + kriq_i!ii;;11 zmi;i1
(4.110)
+1
fii = Rii+1fii+1
+ mi p iCi
(4.111)
i ) + Ri i+1
ii =; fii (rii;1;i + ri;C
(4.112)
i+1 i+1
i
+1
i + Ii !_ i + !i (I i !i )
+ Rii+1 fii+1
ri;C
i i
i
i i
i
+ kr;i+1qi+1 Imi+1 zmi i+1 + kr;i+1q_i+1 Imi+1 !ii zmi i+1
8 f iT Ri;1T z + k I !_ i;1T zi;1
0
ri mi mi mi
>
i i
>
< +Fvid_i + Fsi sgn (d_i)
prismatico
(4.113)
i = > iT i;1 T
i
;
1 T i;1
kri Imi !_ mi zmi
>
: +i FRi#_ +zF0 +sgn
(#_ i)
rotoidale
vi i
si
Struttura computazionale
Condizioni iniziali
203
p ; g = 4 g 5
0
0
0
0
0
0
0
0
0
Condizioni terminali
f =0
3
3
! = !_ = 0
=0
3
3
Quantità riferite alla terna corrente
2` 3
2a 3
C1
4
r ;C1 = 0 5 r ; = 4 0 5
0
0
2` 3
2a 3
C2
r ;C2 = 4 0 5 r ; = 4 0 5
0
0
203
zm1 = z = zm2 = zm2 = z = 4 0 5
1
1
1
01
2
2
2
12
1
?
1
0
1
2
1
2
2
1
1
matrici di rotazione
2 c ;s
i
i
i
;
Ri = 4 si ci
1
0
0
3
0
0 5 i = 1; 2
1
R =I
2
3
Ricorsione in avanti: braccio 1
203
! = 64 0 75
1
1
#_ 1
203
!_ 11 = 64 0 75
#1
2 ;a1#_ 2 + gs1 3
1
6
p11 = 4 a1#1 + gc1 75
2 ;(`C 0+ a1)#_ 2 + gs1 3
1
1
6
p1C1 = 4 (`C1 + a1)#1 + gc1 75
0
2 0 3
!_ m0 1 = 64 0 75
kr1 #1
Ricorsione in avanti: braccio 2
2 0 3
! = 64 0 75
2
2
#_ 1 + #_ 2
2 0 3
!_ 22 = 64 0 75
#1 + #2
2 a1s2 #1 ; a1 c2#_ 2 ; a2(#_ 1 + #_ 2)2 + gs12 3
1
6
p22 = 4 a1c2#1 + a2(#1 + #2) + a1s2 #_ 21 + gc12 75
0
2 a1s2 #1 ; a1 c2#_ 2 ; (`C + a2)(#_ 1 + #_ 2)2 + gs12 3
1
2
6
p2C2 = 4 a1c2#1 + (`C2 + a2 )(#1 + #2) + a1s2 #_ 21 + gc12 75
0
2 0 3
!_ m1 2 = 64 0 75
#1 + kr2 #2
Ricorsione all’indietro: braccio 2
f = m pC2
2
66
= 666 I zz (# + # ) + m (`C2 + a ) (# + # )
4
+m a (`C2 + a )c #
2
2
2
2
2
2
2
1
2
2
2 1
2
2
2
2
1
1
2
3
77
77
75
+m2 a1 (`C2 + a2 )s2 #_ + m2 (`C2 + a2 )gc12
;
;
2 = I2zz + m2 (`C2 + a2 )2 + a1 (`C2 + a2 )c2 + kr2 Im2 #1
;
+ I2zz + m2 (`C2 + a2 )2 + kr22 Im2 #2
+ m2 a1 (`C2 + a2 )s2 #_ 21 + m2 (`C2 + a2 )gc12
2
1
Ricorsione all’indietro: braccio 1
2 ;m (`C2 + a )s (# + # ) ; m (`C1 + a )#_ ; m a #_
66
;m (`C2 + a )c (#_ + #_ ) + (m + m )gs
f = 666 m (`C1 + a )# + m a # + m (`C2 + a )c (# + # )
4
;m (`C2 + a )s (#_ + #_ ) + (m + m )gc
2
2
2
1
2
2
1
1
1
1
1
1
2
2
2 1
1
2
2
2
1
1
2
2
2
0
2
2
2
2
1
2 1
1
2
1
2
1
2
1
1
1
2
2
1
3
77
77
75
2
66
66 I # + m a # + m (` + a ) # + m a (` + a )c #
zz
C1
C2
6
6
=6
+I zz (# + # ) + m a (`C2 + a )c (# + # )
66
+ # ) + kr Im2 #
+
m
(
`
+
a
)
(
#
C
2
64 +m a (` + a )s #_ ; m a (` + a )s (#_ + #_ )
1
1
1
1
2
2 1
2
1
1
1
2
2
2
1
2 1
2 1
2
2
2 1
2
1
2
2
2
2
2
1
2
1
2
2
2
C2
2
2 1 C2
2 2
1
2
+m1 (`C1 + a1 )gc1 + m2 a1 gc1 + m2 (`C2 + a2 )gc12
2 1
1
1 = I1zz + m1 (`C1 + a1 )2 + kr21 Im1 + I2zz

;
2
2
+m2 a1 + (`C2 + a2 ) + 2a1 (`C2 + a2 )c2 #1

;
2
+ I2zz + m2 (`C2 + a2 ) + a1 (`C2 + a2 )c2 + kr2 Im2 #2
; 2;m2 a1 (`C2 + a2 )s2 #_ 1 #_2 ; m2 a1 (`C2 + a2 )s2 #_ 22
+ m1 (`C1 + a1 ) + m2 a1 gc1 + m2 (`C2 + a2 )gc12
3
77
77
77
77
77
5
Equivalenza col modello di Lagrange
?
parametri di bracci e rotori
m1 = m`1 + mm2
m1 `C1 = m`1 (`1 ; a1 )
I1zz + m1 `2C1 = I^1 = I`1 + m`1 (`1 ; a1 )2 + Im2
m2 = m`2
m2 `C2 = m`2 (`2 ; a2 )
I2zz + m2 `2C2 = I^2 = I`2 + m`2 (`2 ; a2 )2
DINAMICA DIRETTA E DINAMICA INVERSA
Formulazione di Lagrange:
?
?
?
è sistematica e di facile comprensione
fornisce le equazioni del moto in una forma analitica compatta che evidenzia la matrice di inerzia, la matrice a fattore
delle forze centrifughe e di Coriolis e il vettore delle forze
gravitazionali (utile ai fini della sintesi del controllo)
è efficace se si vogliono portare in conto effetti meccanici
più complessi quali ad esempio le deformazioni elastiche
dei bracci
Formulazione di Newton-Eulero:
?
è intrinsecamente un metodo ricorsivo che risulta efficiente
da un punto di vista computazionale
Dinamica diretta
?
?
q
note (t0 ),
per t > t0
q_ (t ), (t) (e h(t)), determinare q(t), q_ (t), q(t)
0
utile in simulazione
Dinamica inversa
?
?
q q_ (t), q(t) (e h(t)), determinare (t)
note (t),
utile per la pianificazione e il controllo
Per un manipolatore ad n giunti, il numero di operazioni per il
calcolo della dinamica è:
? O(n2 ) per la dinamica diretta
? O(n) per la dinamica inversa
Dinamica diretta con Lagrange
q = B; (q)( ; 0 )
1
0(q; q_ ) = C (q; q_ )q_ + Fv q_ + fs (q_ ) + g(q) + J T (q)h
?
?
q(tk ), q_ (tk ), (tk ), si calcola q(tk )
si integra numericamente con passo t: q_ (tk ), q (tk )
note
+1
Dinamica diretta con Newton-Eulero
?
?
+1
qq
0 come per q = 0
colonna bi di B come per g = 0, q_ = 0, qi = 1 e qj = 0
note , _ , si calcola:
per j
6= i
nota , si calcola
0
q e si integra numericamente
MODELLO DINAMICO NELLO SPAZIO
OPERATIVO
Ridondanza
?
coordinate generalizzate (moti interni?)
Singolarità
Modello nello spazio dei giunti
q = ;B; (q)C (q; q_ )q_ ; B; (q)g(q)+ B; (q)J T (q)( ; h)
1
1
1
Cinematica differenziale del secondo ordine
x = JA(q)q + J_ A(q; q_ )q_
= ;JA B ; C q_ ; JA B ; g + J_ A q_ + JA B ; JAT (A ; hA )
1
1
1
Trasformazioni
BA = (JAB; JAT );
CA x_ = BAJAB; C q_ ; BA J_ Aq_
gA = BAJAB; g
1
1
1
1
?
manipolatore non ridondante
BA = JA;T BJA;
CA x_ = JA;T C q_ ; BA J_ Aq_
gA = JA;T g
1
Equazioni del moto
BA (x)x + CA(x; x_ )x_ + gA(x) = A ; hA
Dinamica diretta
?
note
(t) (e h(t)), determinare x (t), x_ (t), x(t)
Soluzione
?
?
dinamica diretta ai giunti
cinematica diretta
Dinamica inversa
?
note
x(t), x_ (t), x (t) (e h(t)), determinare (t)
Soluzione (ridondanza a livello cinematico)
?
?
cinematica inversa
dinamica inversa ai giunti
Soluzione (ridondanza a livello dinamico)
?
?
modello dinamico nello spazio operativo
coppie con gestione di moti interni
Dinamica inversa
A ; hA = BA(x)x + CA (x; x_ )x_ + gA (x)
= BA (x ; J_ A q_ ) + BA JA B ; C q_ + BA JA B ; g
= BA JA q + BA JA B ; C q_ + BA JA B ; g
1
1
?
J
B
JA(q) = B; (q)JAT (q)BA (q)
pseudo-inversa destra di A pesata secondo
1
+
A ; hA = JAT (Bq + C q_ + g)
= JAT ( ; JAT hA )
;
T
T
T
= JA (q)A + I ; JA (q)JA (q) a
1
1
ELLISSOIDE DI MANIPOLABILITÀ
DINAMICA
Sfera nello spazio dei giunti
T = 1
? modello dinamico con q_ = 0, h = 0
B(q)q + g(q) = ? accelerazioni ai giunti (q_ = 0)
v_ = J (q)q
+
= B(q)J y(q)v_ + g(q)
Ellissoide nello spazio operativo
;B(q)J y(q)v_ + g(q)T ;B(q)J y(q)v_ + g(q) = 1
BJ yv_ + g = B(J yv_ + B; g)
= B (J yv_ + B ; g + J yJB ; g ; J yJB ; g)
;
= B J yv_ + J yJB ; g + (I ; J yJ )B ; g
= BJ y(v_ + JB ; g)
1
1
1
1
1
1
1
+
(v_ + JB ;1 g)T J yT B T BJ y(v_ + JB ;1 g) = 1
1
0.5
−1
[m]
JB g
0
−0.5
−0.5
0
0.5
1
[m]
?
manipolatore non ridondante
(v_ + JB ;1 g)T J ;T B T BJ ;1 (v_ + JB ;1 g) = 1
PIANIFICAZIONE DI TRAIETTORIE
generazione degli ingressi di riferimento per il sistema di controllo del moto
Percorso e traiettoria
Traiettorie nello spazio dei giunti
Traiettorie nello spazio operativo
Scalatura dinamica di traiettorie
PERCORSO E TRAIETTORIA
?
generazione di traiettorie a curvatura regolare
Percorso: luogo dei punti dello spazio dei giunti (spazio operativo) che il manipolatore deve descrivere nell’esecuzione del
movimento assegnato (descrizione geometrica)
Traiettoria: percorso su cui sia specificata la legge oraria di
moto (ad es. velocità e/o accelerazioni)
Algoritmo di pianificazione della traiettoria
?
ingressi
definizione del percorso
vincoli al percorso
vincoli dovuti alla dinamica del manipolatore
?
uscita
traiettorie dei giunti (o dell’organo terminale) espresse come
sequenza temporale di valori assunti da posizione, velocit à
e accelerazione
Numero ristretto di parametri
?
percorso
punti estremi
eventuali punti intermedi
primitive geometriche
?
legge di moto
tempo complessivo
velocità e/o accelerazione max
velocità e/o accelerazione in punti assegnati
Traiettorie nello spazio operativo
?
?
?
?
descrizione naturale del compito
vincoli sul percorso
singolarità
ridondanza
Traiettorie nello spazio dei giunti
?
?
inversione cinematica
azione di controllo
TRAIETTORIE NELLO SPAZIO DEI GIUNTI
q
Generazione di una funzione (t) che interpola i valori assegnati per le variabili di giunto nel rispetto dei vincoli imposti.
?
?
?
traiettorie generate poco onerose dal punto di vista computazionale
posizioni e velocità (e accelerazioni) dei giunti funzioni
continue del tempo
effetti indesiderati minimizzati (ad es. traiettorie a curvatura
non regolare)
Moto punto–punto
?
punti estremi e tempo di transizione
Moto su percorso assegnato
?
punti estremi, punti intermedi e tempi di transizione
Moto punto–punto
?
generazione di
tempo tf
q(t) che realizza il moto da q i a qf
Polinomio cubico
q(t) = a3 t3 + a2 t2 + a1 t + a0
q_(t) = 3a3 t2 + 2a2 t + a1
q(t) = 6a3 t + 2a2
?
calcolo dei coefficienti
a0 = qi
a1 = q_i
a3 t3f + a2 t2f + a1 tf + a0 = qf
3a3 t2f + 2a2tf + a1 = q_f
Polinomio quintico (accelerazioni iniziale e finale)
q(t) = a5 t5 + a4 t4 + a3 t3 + a2 t2 + a1 t + a0
in un
Esempio
pos
[rad]
3
2
1
0
0
0.2
0.4
0.6
[s]
0.8
1
0.8
1
0.8
1
vel
5
[rad/s]
4
3
2
1
0
0
0.2
0.4
0.6
[s]
acc
20
[rad/s^2]
10
0
−10
−20
0
0.2
0.4
0.6
[s]
Profilo di velocità trapezoidale
qc
qc tc = qtm ;
;t
m
c
qc = qi + 21 qc t2c
qc t2c ; qc tf tc + qf ; qi = 0
qc assegnata (sgn qc = sgn (qf ; qi ))
s
t2f qc ; 4(qf ; qi )
t
1
f
tc = 2 ; 2
qc
jqc j 4jqft2; qij
f
?
traiettoria
8 q + q t
>
<i c
q(t) = > qi + qc tc (t ; tc =2)
: qf ; qc (tf ; t)
1
2
1
2
2
2
0 t tc
tc < t tf ; tc
tf ; tc < t tf
Esempio
pos
[rad]
3
2
1
0
0
0.2
0.4
0.6
[s]
0.8
1
0.8
1
0.8
1
vel
5
[rad/s]
4
3
2
1
0
0
0.2
0.4
0.6
[s]
acc
20
[rad/s^2]
10
0
−10
−20
0
0.2
0.4
0.6
[s]
q_c assegnata
jqf ; qi j < jq_ j 2jqf ; qi j
tf
c
tf
tc = qi ; qfq_ + q_c tf
c
2
q
_
c
qc = q ; q + q_ t
i f
cf
Moto su percorso assegnato
?
opportunità di specificare punti intermedi (sequenza di
punti)
Assegnati N punti di percorso, trovare una funzione interpolante che passi per tali punti
?
polinomio di grado N
;1
non è possibile assegnare velocità iniziale e finale
al crescere del grado di un polinomio aumenta il suo carattere oscillatorio (traiettorie poco naturali)
l’accuratezza numerica con cui pu ò essere calcolato un polinomio peggiora all’aumentare del suo grado
una volta impostato il sistema di equazioni dei vincoli, risulta oneroso risolverlo
i coefficienti del polinomio dipendono da tutti i punti assegnati =) se si vuole cambiare uno dei punti di percorso, si deve ricalcolare il tutto
Sequenze di polinomi interpolatori di grado pi ù basso (cubici)
uniti tra di loro nei punti assegnati lungo il percorso
?
i valori di q_(t) in corrispondenza dei punti di percorso sono
imposti arbitrariamente
?
i valori di q_(t) in corrispondenza dei punti di percorso sono
assegnati in base ad un opportuno criterio
?
l’accelerazione q(t) deve essere continua in corrispondenza
dei punti di percorso
Sequenze di polinomi di grado minore di tre (passaggio in
prossimità dei punti di percorso imposti)
Sequenza di polinomi con vincoli imposti sulle velocit à nei
punti di percorso
k (tk ) = qk
k (tk+1 ) = qk+1
_ k (tk ) = q_k
_ k (tk+1 ) = q_k+1
?
continuità delle velocità nei punti intermedi
_ k (tk+1 ) = _ k+1 (tk+1 )
Esempio
pos
[rad]
6
4
2
0
0
2
4
6
4
6
4
6
[s]
vel
[rad/s]
5
0
−5
0
2
[s]
acc
[rad/s^2]
20
0
−20
−40
0
2
[s]
Sequenza di polinomi con velocit à calcolate nei punti di percorso
q_1 = 0
0
q_k = 1 (vk + vk+1 )
q_N = 0
2
sgn (vk ) 6= sgn (vk+1 )
sgn (vk ) = sgn (vk+1 )
vk = (qk ; qk;1 )=(tk ; tk;1 )
Esempio
pos
[rad]
6
4
2
0
0
2
4
6
4
6
4
6
[s]
vel
[rad/s]
5
0
−5
0
2
[s]
acc
30
[rad/s^2]
20
10
0
−10
−20
−30
0
2
[s]
Sequenza di polinomi con accelerazione continua nei punti di
percorso (spline)
k;1 (tk ) = qk
k;1 (tk ) = k (tk )
_ k;1 (tk ) = _ k (tk )
k;1 (tk ) = k (tk )
? 4N ; 2 equazioni nei 4(N ; 1) coefficienti incogniti (polinomi di quarto grado per il primo e l’ultimo segmento?)
? 2 punti virtuali (continuità su posizione, velocità e accelerazione) =) N + 1 polinomi cubici
? 4(N ; 2) equazioni per gli N ; 2 punti di percorso interni:
k;1 (tk ) = qk
k;1 (tk ) = k (tk )
_ k;1 (tk ) = _ k (tk )
k;1 (tk ) = k (tk )
? 6 equazioni per i punti iniziale e finale
1 (t1 ) = qi
_ 1 (t1 ) = q_i
1 (t1 ) = qi
N +1 (tN +2 ) = qf
_ N +1 (tN +2 ) = q_f
N +1 (tN +2 ) = qf
? 6 equazioni per i punti virtuali
k;1 (tk ) = k (tk )
_ k;1 (tk ) = _ k (tk )
k;1 (tk ) = k (tk )
?
+
Sistema di 4(N + 1) equazioni per la determinazione dei
4(N + 1) coefficienti degli N + 1 polinomi cubici
?
algoritmo efficiente dal punto di vista computazionale
k (tk )
k (tk+1 )

k (t) = t (tk+1 ; t) + t (t ; tk )
k
k
k (tk )
k (tk+1 )
3
k (t) = 6t (tk+1 ; t) + 6t (t ; tk )3
k
k !
k (tk+1 )
t
(
t
k
k
k
+1 )
(t ; tk )
+
tk ;
6
!
k (tk )
(
t
)
t
k
k
k
+ t ;
(tk+1 ; t)
6
k
? 4 incognite: k (tk ), k (tk+1 ), k (tk ) e k (tk+1 )
?
?
?
?
?
?
N variabili qk per k 6= 2; N + 1 assegnate
vincoli di continuità su q2 e qN +1
vincoli di continuità su qk per k = 3; : : : ; N
q_i e q_f assegnate
vincoli di continuità su qk per k = 2; : : : ; N + 1
qi e qf assegnate
+
_ 1 (t2 ) = _ 2 (t2 )
..
.
_ N (tN +1 ) = _ N +1 (tN +1 )
?
sistema di equazioni lineari
A [ (t )
2a a
66 a a
A = 66 ... ...
40 0
2
2
11
12
21
22
0
: : : N +1 (tN +1 ) ]T = b
:::
:::
..
.
0
0
..
.
: : : aN ;1;N ;1
0 : : : aN;N ;1
3
77
..
77
.
aN ; ;N 5
0
0
1
aNN
Esempio
pos
[rad]
6
4
2
0
0
1
2
3
4
5
3
4
5
3
4
5
[s]
vel
[rad/s]
5
0
−5
0
1
2
[s]
acc
30
20
[rad/s^2]
10
0
−10
−20
−30
0
1
2
[s]
Sequenza di polinomi parabolico-lineari con passaggio in prossimità di punti di via
; qk;1
q_k;1;k = qkt
k;1
; q_k;1;k
qk = q_k;k+1t
0
k
Esempio
pos
[rad]
6
4
2
0
0
1
2
3
4
5
3
4
5
3
4
5
[s]
vel
4
[rad/s]
2
0
−2
−4
−6
0
1
2
[s]
acc
40
[rad/s^2]
20
0
−20
−40
0
1
2
[s]
Applicazione della legge trapezoidale al problema dell’interpolazione
pos
5
[rad]
4
3
2
1
0
0
0.5
1
[s]
1.5
2
1.5
2
1.5
2
vel
4
[rad/s]
3
2
1
0
0
0.5
1
[s]
acc
[rad/s^2]
20
10
0
−10
−20
0
0.5
1
[s]
TRAIETTORIE NELLO SPAZIO OPERATIVO
Sequenze di punti assegnati
?
?
?
inversione cinematica
traiettorie nello spazio dei giunti
microinterpolazione
x(tk )
Punti di percorso
Primitive di percorso
?
?
componenti xi (tk ) raccordate secondo sequenze di polinomi interpolatori
espressione analitica del moto
Primitive di percorso
Rappresentazione parametrica di una curva nello spazio
p = f (s)
t = ddsp
2
d
n = d2p dsp2
2 ds
b=tn
1
Segmento nello spazio
p(s) = pi + kpf ;s pik (pf ; pi)
dp =
1 (p ; p )
ds kpf ; pi k f i
d2 p = 0
ds2
Circonferenza nello spazio
2 cos (s=) 3
p0(s) = 4 sin (s=) 5
0
p(s) = c + Rp0(s)
2 ;sin (s=) 3
dp = R 4 cos (s=) 5
ds
0
2 ;cos (s=)= 3
d p = R 4 ;sin (s=)= 5
ds
2
2
0
Traiettorie di posizione e di orientamento
Posizione
p = f (s)
? s(t) polinomio interpolatore
p_ = s_ ddsp = s_t
? segmento
p(s) = pi + kpf ;s pik (pf ; pi)
p_ = kpf ;s_ pik (pf ; pi) = s_t
p = kpf ;s pik (pf ; pi) = st
?
circonferenza
2 cos (s=) 3
p(s) = c + R 4 sin (s=) 5
0
2 ;s_ sin (s=) 3
p_ = R 4 s_ cos (s=) 5
0
2 ;s_ cos (s=)= ; s sin (s=) 3
p = R 4 ;s_ sin (s=)= + s cos (s=) 5
2
2
0
Orientamento
?
?
n(t), s(t), a(t) ?
interpolazione delle componenti di
(t)
(s) = i + kf ;s ik (f ; i)
interpolazione di
_ = kf ;s_ ik (f ; i)
= kf ;s ik (f ; i)
?
R = RiRfi )
3
r r
r r 5
asse equivalente di rotazione ( f
2r
Rfi = RiT Rf = 4 r
11
12
13
21
22
23
r31 r32 r33
r + r + r ; 1
#f = cos ;
2 r ; r2 3
r = 2 sin1 #f 4 r ; r 5
1
11
22
33
32
23
13
31
r21 ; r12
Ri(t): Ri(0) = I Ri(tf ) = Rfi
R(t) = RiRri (#(t))
#(0) = 0 #(tf ) = #f
SCALATURA DINAMICA DI TRAIETTORIE
?
vincoli dinamici (coppie troppo elevate)
(t) = B(q(t))q(t) + C (q(t); q_ (t))q_ (t) + g(q(t))
= B (q(t))q(t) + ; (q(t))[q_ (t)q_ (t)] + g(q(t))
= s (t) + g(q(t))
C (q; q_ )q_ = ; (q)[q_ q_ ]
[q_ q_ ] = [ q_12 q_1 q_2 : : : q_n;1 q_n q_n2 ]T
?
r(0) = 0 r(tf ) = tf
q(t) = q(r(t))
q_ = r_q0(r)
q = r_2q00 (r) + rq0(r)
scalatura temporale r(t):
+
00
0
0
= r_ B(q(r))q (r) + ; (q(r))[q (r)q (r)] + rB(q(r))q0(r)
+ g(q(r))
= s (t) + g(q(r))
2
s(r) = B(q(r))q00(r) + ; (q(r))[q0(r)q0(r)]
+
s(t) = r_ s(r) + rB(q(r))q0(r)
2
?
scelta semplice r(t) = ct
s(t) = c s(ct)
2
giunto qi in corrispondenza della violazione maggiore
js j = c2
ji ; g(qi )j
CONTROLLO DEL MOTO
Controllo nello spazio dei giunti
Controllo indipendente ai giunti
Compensazione in avanti a coppia precalcolata
Controllo centralizzato
Controllo nello spazio operativo
IL PROBLEMA DEL CONTROLLO
Controllo nello spazio dei giunti
Controllo nello spazio operativo
CONTROLLO NELLO SPAZIO DEI GIUNTI
Modello dinamico
B(q)q + C (q; q_ )q_ + Fv q_ + g(q) = Controllo
?
q(t) = qd(t)
determinare :
trasmissioni
Kr q = qm
?
m = Kr; 1
inerzie medie
B(q) = B + B(q)
r; qm + Fm q_ m + d
m = Kr; BK
1
?
1
attrito viscoso
Fm = Kr; Fv Kr;
1
?
1
disturbo
d = Kr; B(q)Kr; qm + Kr; C (q; q_ )Kr; q_ m + Kr; g(q)
1
1
1
1
1
Manipolatore + attuatori
?
controllo decentralizzato
?
controllo centralizzato
CONTROLLO INDIPENDENTE AI GIUNTI
Manipolatore n sistemi indipendenti (azionamenti di giunto)
?
?
controllo di ogni asse di giunto come un sistema ad un
ingresso ed una uscita
effetti di accoppiamento tra i vari giunti trattati come ingressi di disturbo
Azionamento del giunto i
Fm kRv kt
a
M (s) = s(1 +kmsT )
m
km = k1
Tm = kRakI
v
v t
Controllo in retroazione
Riduzione degli effetti di d su ?
?
elevato valore del guadagno degli amplificatori a monte del
punto di applicazione del disturbo
presenza nel controllore di una azione integrale al fine di
annullare a regime ( costante) l’effetto della componente
gravitazionale sull’uscita
+
Controllore proporzionale-integrale (PI)
C (s) = Kc 1 +ssTc
Struttura generale
Retroazione di posizione
CP (s) = KP 1 +ssTP
CV (s) = 1
CA (s) = 1
kTV = kTA = 0
?
f.d.t. del ramo di azione diretta
P (1 + sTP )
C (s)G(s) = kmsK
2 (1 + sT )
m
?
f.d.t. del ramo in retroazione
H (s) = kTP
?
analisi con il l.d.r.
?
f.d.t. ingresso–uscita a ciclo chiuso
1
(s) =
r (s ) 1 +
kTP
s2 (1 + sTm )
km KP kTP (1 + sTP )
1 (1 + sT )
P
k
TP
W (s) = 2s s2 1 + ! + !2 (1 + s )
n
?
n
f.d.t. disturbo–uscita
sRa
(s) = ; kt KP kTP (1 + sTP )
2
D(s)
s
+ sTm )
1 + k K (1
m P kTP (1 + sTP )
XR = KP kTP
TR = max TP ; !1
n
Retroazione di posizione e di velocit à
CP (s) = KP
CV (s) = KV 1 +ssTV
CA (s) = 1
kTA = 0
?
V)
C (s)G(s) = kmKsP2 (1KV+(1sT+ sT
)
m
?
k
H (s) = kTP 1 + s K TV
P kTP
?
?
scelta dello zero
TV = Tm
?
1
(s) =
kTP
r (s) 1 + skTV +
s2
KP kTP kmKP kTP KV
1
kTP
W (s) =
2
s
2
s
1 + ! + !2
n
n
?
specifiche di progetto
KV kTV = 2k!n
m
2
!
n
KP kTP KV = k
m
?
sRa
(s) = ; kt KP kTP KV (1 + sTm )
2
D(s)
s
sk
TV
1+ K k + k K k K
P TP
XR = KP kTP KV
m P TP V
TR = max Tm ; !1
n
Retroazione di posizione, di velocit à e di accelerazione
CP (s) = KP
G0 (s) =
CV (s) = KV
CA (s) = KA 1 +ssTA
1
0 km T
A
sT
1
+
k
K
k
m
m
A
TA
C
B
T
m
C
B
(1 + km KA kTA ) @1 +
A
(1 + km KA kTA )
?
C (s)G(s) = KP KV KAs2(1 + sTA ) G0 (s)
?
sk
TV
H (s) = kTP 1 + K k
P TP
?
?
scelta dello zero
TA = Tm
km KA kTA TA Tm
kmKA kTA 1
?
1
(s) =
kTP
r (s) 1 + skTV + s2 (1 + km KA kTA )
KP kTP km KP kTP KV KA
?
sRa
(s) = ; kt KP kTP KV KA (1 + sTA )
2
D (s )
s
sk
TA )
TV
1 + K k + k (1K+ kkmKKA kK
P TP
XR = KP kTP KV KA
?
m P TP V A
TR = max TA ; !1
n
specifiche di progetto
2KP kTP = !n
kTV
km KA kTA = km!X2 R ; 1
KP kTP KV KA = XR
n
?
ricostruzione dell’accelerazione
Compensazione in avanti decentralizzata
Elevati valori di velocità e accelerazioni
?
capacità di inseguimento degradate
Impiego di una azione in avanti
Retroazione di posizione
2
s
(1
+
sT
)
m
r0 (s) = kTP + k K (1 + sT
d (s)
m P
P)
Retroazione di posizione e di velocit à
2
sk
s
r0 (s) = kTP + KTV + k K K d (s)
P
m P V
Retroazione di posizione, di velocit à e di accelerazione
2
sk
(1
+
k
K
k
)
s
TV
m
A
TA
0
r (s) = kTP + K + k K K K
d (s)
P
m P V A
COMPENSAZIONE IN AVANTI A COPPIA
PRECALCOLATA
All’uscita del PIDD2
a2 e + a1 e_ + a0 e + a;1
Zt
e(& )d& + Tkm d + k1 _d ; Rk a d
m
m
t
= Tkm + k1 _
m
m
+
a02 e + a01 e_ + a00 e + a0;1
Zt
e(& )d& = Rk a d
t
Ra s
E (s ) =
kt
D(s) a02 s3 + a01 s2 + a00 s + a0;1
?
adozione di guadagni di anello troppo elevati
Coppia precalcolata
?
azione in avanti (inversione di modello)
dd = Kr; B(qd)Kr; qmd+Kr; C (qd; q_ d)Kr; q_ md+Kr; g(qd)
1
?
?
?
1
1
1
1
riduzione degli sforzi di reiezione del disturbo (guadagni
contenuti)
calcolo fuori linea/in linea
compensazione parziale
CONTROLLO CENTRALIZZATO
Manipolatore Sistema multivariabile accoppiato e non lineare
B(q)q + C (q; q_ )q_ + Fv q_ + g(q) = Trasmissioni
Kr q = qm
Sistemi di attuazione
Kr; = Ktia
va = Raia + Kv q_ m
va = Gv vc
1
Manipolatore controllato in tensione
B(q)q + C (q; q_ )q_ + F q_ + g(q) = u
F = Fv + Kr Kt Ra; Kv Kr
u = Kr Kt Ra; Gv vc
1
1
Manipolatore controllato in coppia
?
riduzione della sensibilità alle variazioni parametriche
?
attuatori come generatori controllati di coppia (corrente)
Kt , Kv , Ra
ia = Givc
+
F = Fv
u = Kr Kt Givc = ROBOTICA INDUSTRIALE
Controllo PD con compensazione di gravità
q
Regolazione ad una postura di equilibrio costante d
Metodo diretto di Lyapunov
?
?
qe q_ T ]T
qe = qd ; q
stato [ T
funzione candidata di Lyapunov
V (q_ ; qe) = 21 q_ T B (q)q_ + 12 qeT KP qe > 0
V_ = q_ T B (q)q + 12 q_ T B_ (q)q_ ; q_ T KP qe
8q_ ; qe 6= 0
;
;
= 12 q_ T B_ (q) ; 2C (q; q_ ) q_ ; q_ T F q_ + q_ T u ; g(q) ; KP qe
?
scelta del controllo
u = g(q) + KP qe ; KD q_
+
V_ = ;q_ T (F + KD )q_
V_ = 0
q_ = 0; 8qe
?
dinamica del sistema controllato
B(q)q + C (q; q_ )q_ + F q_ + g(q) = g(q) + KP qe ; KD q_
?
q q 0)
all’equilibrio ( _
KP qe = 0
=)
qe = qd ; q 0
Controllo a dinamica inversa
Modello dinamico
B(q)q + n(q; q_ ) = u
n(q; q_ ) = C (q; q_ )q_ + F q_ + g(q)
Retroazione non lineare dello stato (linearizzazione globale)
? struttura lineare in u
? %(B (q)) = n 8q
u = B(q)y + n(q; q_ )
+
q = y
y stabilizzante
y = ;KP q ; KD q_ + r
Scelta di un controllo
r = qd + KD q_ d + KP qd
+
qe + KD qe_ + KP qe = 0
?
?
cancellazione perfetta
vincoli sull’architettura hardware/software dell’unit à di governo
Controllo robusto
Compensazione imperfetta
u = Bb (q)y + nb (q; q_ )
?
incertezza
Be = Bb ; B
ne = nb ; n
+
b + nb
Bq + n = By
q = y + (B; Bb ; I )y + B; ne = y ; 1
1
= (I ; B; Bb )y ; B; ne
1
Scelta del controllo
y = qd + KD (q_ d ; q_ ) + KP (qd ; q)
+
qe + KD qe_ + KP qe = 1
qe = qd ; y + ?
equazione differenziale del primo ordine
_ = H + D(qd ; y + )
Stima del campo di variabilità dell’incertezza
sup kqdk < QM < 1
t0
kI ; B ;1 (q)Bb (q)k 1
kne k < 1
8qd
8q
8q; q_
y = qd + KD qe_ + KP qe + w
+
f + D( ; w)
_ = H
O
I
f = (H ; DK ) = ;K ;K
H
P
D
Metodo di Lyapunov
V () = T Q > 0
8 6= 0
V_ = _ T Q + T Q_
fT Q + QH
f) + 2T QD( ; w)
= T (H
= ;T P + 2T QD( ; w)
= ;T P + 2zT ( ; w)
Legge di controllo
w = kzk z
>0
+
zT ( ; w) = zT ; kzk zT z
kzkkk ; kzk
= kzk(kk ; )
Scelta di kk
?
8q; q_ ; qd
stima dell’incertezza
;
;
1b
kk kI ; B B k kqdk + kK k kk + kwk + kB ;1 k kne k
QM + kK k kk + + BM 1 ;1 (QM + kK kkk + BM )
+
V_ = ;T P + 2zT ; kzk z < 0
8 6= 0
?
sottospazio di scivolamento (attrattivo)
Eliminazione delle componenti in alta frequenza
8 >
< kzk z
w=>
: z
zk per kz k < per k
Controllo adattativo
Modello dinamico (lineare nei parametri)
B(q)q + C (q; q_ )q_ + F q_ + g(q) = Y (q; q_ ; q) = u
Controllo
u = B(q)qr + C (q; q_ )q_ r + F q_ r + g(q) + KD q_ r = q_ d + qe
qr = qd + qe_
= q_ r ; q_ = qe_ + qe
+
B(q)_ + C (q; q_ ) + F + KD = 0
Metodo di Lyapunov
V (; qe) = 12 T B (q) + 21 qeT M qe > 0
8; eq 6= 0
V_ = T B (q)_ + 21 T B_ (q) + qeT M qe_
= ;T (F + KD ) + qeT M qe_
T
= ;T F ; qe_ KD qe_ ; qeT KD qe
? V_ = 0 solo per qe = qe_ 0 =) [ qeT T ]T = 0
globalmente asintoticamente stabile
Controllo basato su stime dei parametri
u = Bb (q)qr + Cb (q; q_ )q_ r + Fb q_ r + gb + KD = Y (q; q_ ; q_ r ; qr )b + KD +
B(q)_ + C (q; q_ ) + F + KD = ;Be (q)qr ; Ce (q; q_ )q_ r ; Fe q_ r ; ge(q)
= ;Y (q; q_ ; q_ r ; qr )e
Modifica di V
V (; qe; e ) = 12 T B (q)+qeT KD qe+ 21 e T K e > 0
V_ = ;T F ; qe_ T KD qe_ ; qeT KD qe
;
T
T
_
+ e K e ; Y (q; q_ ; q_ r ; qr )
8; qe; e 6=
Legge di adattamento
b_ = K; Y T (q; q_ ; q_ r ; qr )
1
+
V_ = ;T F ; qe_ T KD qe_ ; qeT KD qe
? qe ! 0
? Y (q; q_ ; q_ r ; qr )(b ; ) ! 0
CONTROLLO NELLO SPAZIO OPERATIVO
?
?
?
specifiche di moto
relazioni cinematiche
carico computazionale
Schemi di principio
Controllo con inversa dello Jacobiano
Controllo con trasposta dello Jacobiano
Controllo PD con compensazione di gravità
x
Regolazione ad una posizione di equilibrio costante d
Metodo di Lyapunov
V (q_ ; xe ) = 12 q_ T B (q)q_ + 12 xe T KP xe > 0
8q_ ; xe 6= 0
V_ = q_ T B (q)q + 21 q_ T B_ (q)q_ + xe_ T KP xe
= q_ T B (q)q + 12 q_ T B_ (q)q_ ; q_ T JAT (q)KP xe
= ;q_ T F q_ + q_ T
?
;u ; g(q) ; J T (q)K xe
P
A
scelta del controllo
u = g(q) + JAT (q)KP xe ; JAT KD JA(q)q_
+
V_ = ;q_ T F q_ ; q_ T JAT (q)KD JA (q)q_
V_ = 0
JAT (q)KP xe = 0
Controllo a dinamica inversa
Modello
B(q)q + n(q; q_ ) = u
Controllo linearizzante
u = B(q)y + n(q; q_ )
+
q = y
;
;
_
_

e
e
_
_
= JA (q) xd + KD x + KP x ; JA (q; q)q
1
xe + KD xe_ + KP xe = 0
CONFRONTO TRA GLI SCHEMI DI
CONTROLLO
? a1 = a2 = 1 m `1 = `2 = 0:5 m
m`1 = m`2 = 50 kg I`1 = I`2 = 10 kg m2
? kr1 = kr2 = 100
mm1 = mm2 = 5 kg Im1 = Im2 = 0:01 kg m2
Fm1 = Fm2 = 0:01 N m s/rad Ra1 = Ra2 = 10 ohm
kt1 = kt2 = 2 N m/A kv1 = kv2 = 2 V s/rad
Schemi di controllo
A. Indipendente ai giunti pos + vel
KP = 5 KV = 10
kTP = kTV = 1
B. Indipendente ai giunti pos + vel + acc
KP = 5 KV = 10 KA = 2
kTP = kTV = kTA = 1
C. Come A + compensazione in avanti decentralizzata
D. Come B + compensazione in avanti decentralizzata
E. Coppia precalcolata ai giunti (termini diag. inerzia + gravit à) +
Indipendente ai giunti come A
F. PD + compensazione di gravità +
KP = 3750I
KD = 750I
KD q_ d
G. Dinamica inversa
KP = 25I KD = 5I
b = B , nb = Fv q_ + g)
H. Robusto (B
KP = 25I KD = 5I = 70
I. Come H
= 0:004
= 0:01
J. Adattativo
m2 = 10 kg m2 `C 2 = 11 kg m I^2 = 12:12 kg m2
= 5I KD = 750I K = 0:01I
K. PD cartesiano + compensazione di gravit à +
KP = 16250I
KD = 3250I
L. Dinamica inversa cartesiano
KP = 25I
KD = 5I
KD x_ d
pos giunto 2
3
−0.5
2.5
−1
2
[rad]
[rad]
pos giunto 1
0
−1.5
1.5
−2
1
−2.5
0.5
−3
0
1
2
[s]
3
0
0
4
1
coppie giunti
2
[s]
3
4
3
4
errori pos
1500
0.2
[m]
[Nm]
1000
500
0
−500
0
0.1
0
1
2
[s]
3
4
−0.1
0
1
2
[s]
coppie giunti
1500
1000
1000
[Nm]
[Nm]
coppie giunti
1500
500
0
−500
0
500
0
1
2
[s]
3
−500
0
4
1
norma err pos
2
[s]
3
4
3
4
norma err pos
0.01
0.01
[m]
0.015
[m]
0.015
0.005
0
0
0.005
1
2
[s]
3
4
0
0
1
2
[s]
coppie giunti
norma err pos
1500
0.015
1000
[m]
[Nm]
0.01
500
0.005
0
−500
0
1
2
[s]
3
4
0
0
1
2
[s]
3
4
pos giunto 2
3
−0.5
2.5
−1
2
[rad]
[rad]
pos giunto 1
0
−1.5
1.5
−2
1
−2.5
0.5
−3
0
1
2
[s]
3
0
0
4
1
coppie giunti
3
4
3
4
norma err pos
1500
0.1
0.08
[m]
1000
[Nm]
2
[s]
500
0.06
0.04
0
−500
0
0.02
1
2
[s]
3
4
0
0
1
2
[s]
1500
4
1000
3
500
0
−500
0
norma err pos
−4
[m]
[Nm]
coppie giunti
x 10
2
1
1
2
[s]
3
4
0
0
1
2
[s]
3
4
coppie giunti
1000
1000
[Nm]
1500
500
500
0
0
−500
0
1
4
2
[s]
3
4
−500
0
norma err pos
−4
x 10
4
2
[s]
3
4
3
4
norma err pos
x 10
[m]
3
2
1
0
0
1
−4
3
[m]
[Nm]
coppie giunti
1500
2
1
1
2
[s]
3
4
0
0
1
2
[s]
norma err parametri
[m]
norma err pos
0.01
17
0.008
16
15
0.006
14
0.004
13
0.002
0
0
12
1
2
[s]
3
4
11
0
1
2
[s]
3
4
coppie giunti
norma err pos
1500
0.1
0.08
[m]
[Nm]
1000
500
0.06
0.04
0
−500
0
0.02
1
2
[s]
3
4
0
0
1
2
[s]
3
4
1500
4
1000
3
500
0
−500
0
norma err pos
−4
[m]
[Nm]
coppie giunti
x 10
2
1
1
2
[s]
3
4
0
0
1
2
[s]
3
4
pos giunto 2
3
−0.5
2.5
−1
2
[rad]
[rad]
pos giunto 1
0
−1.5
1.5
−2
1
−2.5
0.5
−3
0
2
4
6
8
0
0
10
2
4
6
[s]
[s]
coppie giunti
errori pos
8
10
8
10
1500
1000
[m]
[Nm]
0.05
500
0
0
−500
0
2
4
6
[s]
8
10
−0.05
0
2
4
6
[s]
coppie giunti
1000
1000
[Nm]
1500
500
500
0
0
−500
0
2
4
6
8
10
−500
0
2
4
[s]
x 10
8
10
8
10
norma err pos
−3
4
x 10
3
[m]
3
2
1
0
0
6
[s]
norma err pos
−3
4
[m]
[Nm]
coppie giunti
1500
2
1
2
4
6
[s]
8
10
0
0
2
4
6
[s]
1500
4
1000
3
500
0
−500
0
norma err pos
−3
[m]
[Nm]
coppie giunti
x 10
2
1
2
4
6
[s]
8
10
0
0
2
4
6
[s]
8
10
pos giunto 2
3
−0.5
2.5
−1
2
[rad]
[rad]
pos giunto 1
0
−1.5
1.5
−2
1
−2.5
0.5
−3
0
2
4
6
8
0
0
10
2
4
[s]
8
10
8
10
norma err pos
0.02
1000
0.015
[m]
[Nm]
coppie giunti
1500
500
0
−500
0
6
[s]
0.01
0.005
2
4
6
[s]
8
10
0
0
2
4
6
[s]
coppie giunti
1500
1
0.8
[m]
1000
[Nm]
norma err pos
−4
x 10
500
0.6
0.4
0
−500
0
0.2
2
4
6
[s]
8
10
0
0
2
4
6
[s]
8
10
coppie giunti
1500
1000
1000
[Nm]
[Nm]
coppie giunti
1500
500
500
0
0
−500
0
2
4
6
8
10
−500
0
2
4
[s]
norma err pos
−4
x 10
0.8
0.8
0.6
0.6
0.4
0.4
0.2
0.2
0
0
2
4
6
[s]
8
10
8
10
norma err pos
−4
1
[m]
[m]
1
6
[s]
8
10
x 10
0
0
2
4
6
[s]
norma err pos
−3
[m]
5
x 10
norma err parametri
17
4
16
3
15
2
14
1
13
0
0
2
4
6
[s]
8
10
12
0
2
4
6
[s]
8
10
norma err pos
0.02
1000
0.015
[m]
[Nm]
coppie giunti
1500
500
0
−500
0
0.01
0.005
2
4
6
[s]
8
10
0
0
2
4
6
[s]
8
10
coppie giunti
1500
1
0.8
[m]
1000
[Nm]
norma err pos
−4
x 10
500
0.6
0.4
0
−500
0
0.2
2
4
6
[s]
8
10
0
0
2
4
6
[s]
8
10
CONTROLLO DELL’INTERAZIONE
Interazione del manipolatore con l’ambiente
Controllo di cedevolezza
Controllo di impedenza
Controllo di forza
Vincoli naturali e vincoli artificiali
Controllo ibrido forza/posizione
INTERAZIONE DEL MANIPOLATORE CON
L’AMBIENTE
Moto vincolato
Controllo posizionale (?)
?
?
errori di posizionamento
incertezza sull’ambiente
Contatto instabile
Misura di forza
Strategie di controllo
?
?
indirette
dirette
CONTROLLO DI CEDEVOLEZZA
Effetti della forza di contatto su schemi di controllo posizionale
(nello spazio operativo)
B(q)q + C (q; q_ )q_ + F q_ + g(q) = u ; J T (q)h
?
all’equilibrio
JAT (q)KP xe = J T (q)h
+
xe = KP; TAT (x)h = KP; hA
1
1
KP;1 cedevolezza del controllo
?
?
Se
lineare nei riguardi di
f
torsionale nei riguardi di
h 2 N (J T ) =) xe = 0 h 6= 0
Modello dell’ambiente
?
elasticamente cedevole
f K O dp h = = Of Km !dt
dp = K !dt
= KTA (x)dx
+
hA = TAT (x)KTA(x)dx
= KA (x)(x ; xe )
KA;1 cedevolezza dell’ambiente
?
solo per le direzioni di moto vincolato
xe = KP; KA (x)(x ; xe)
1
Cedevolezza attiva e cedevolezza passiva
?
all’equilibrio
x1 = ;I + KP; KA (x); (xd + KP; KA (x)xe)
h = ;I + K (x)K ; ; K (x)(x ; x )
1
1
A1
?
?
A
P
1
1
1
A
d
manipolatore cedevole
ambiente cedevole
Dispositivi meccanici di cedevolezza passiva
e
Esempio
KA = Kf = diagfkx; 0g
All’equilibrio
p1 =
?
" kPxxd + kxxe #
kPx + kx
yd
se kPx =kx
se kPx =kx
2 kPx kx
3
f1 = 4 kPx + kx (xd ; xe ) 5
0
1
x1 xd
?
KP = diagfkPx; kPy g
fx1 kx (xd ; xe )
1
x1 xe
fx1 kPx (xd ; xe )
Cedevolezza attiva nello spazio dei giunti
?
all’equilibrio
KP qe = J T (q)h
+
qe = KP; J T (q)h
1
xe = JA(q)KP; JAT (q)hA
1
CONTROLLO DI IMPEDENZA
Controllo a dinamica inversa nello spazio operativo
u = B(q)y + n(q; q_ )
+
q = y ; B; (q)J T (q)h
1
?
scelta del nuovo ingresso di controllo
y = JA; (q)Md; ;Mdx d + KD xe_ + KP xe ; MdJ_ A(q; q_ )q_ 1
1
Mdxe + KD xe_ + KP xe = MdBA; (q)hA
1
BA (q) = JA;T (q)B(q)JA; (q)
1
Impedenza meccanica
?
?
?
massa
Md
smorzamento
rigidezza
KP
KD
Integrazione della misura della forza di contatto
u = B(q)y + n(q; q_ ) + J T (q)h
;
;
1
;
1
_
_
y = J (q)M Mdx d +KD xe +KP xe ;MdJA(q; q_ )q_ ;hA
A
d
+
Mdxe + KD xe_ + KP xe = hA
?
impedenza lineare attiva
Dispositivi meccanici di impedenza passiva
Esempio
Controllo di impedenza con misura di forza
Md = diagfmdx; mdy g
KD = diagfkDx; kDy g
KP = diagfkPx; kPy g
?
dinamica del sistema manipolatore–ambiente lungo le due
direzioni dello spazio operativo (x d costante)
mdxx + kDx x_ + (kPx + kx )x = kx xe + kPx xd
mdy y + kDy y_ + kPy y = kPy yd
Ambiente (xe
Controllo
Manipolatore
= 1)
? più cedevole kx = 103 N/m
? meno cedevole kx = 104 N/m
?
?
?
mdx = mdy = 100
kDx = kDy = 500
kPx = kPy = 2500
? p = [ 1 0 ]T
? pd = [ 1:1 0:1 ]T
pos organo terminale y
forza organo terminale x
300
250
200
[N]
[m]
0.1
0.05
150
100
50
0
0
1
2
[s]
3
0
0
1
2
[s]
3
CONTROLLO DI FORZA
Controllo di cedevolezza o impedenza
retto di forza
=)
controllo indi-
Controllo diretto
?
?
azione di controllo PD sulla forza (misura corrotta da rumore)
chiusura di un anello esterno di regolazione della forza
=) ingresso di riferimento per uno schema di controllo
posizionale a dinamica inversa
Controllo di forza con anello interno di posizione
u = B(q)y + n(q; q_ ) + J T (q)h
;
;
1
;
1
_
y = J (q)M ;KD x_ + KP (xF ; x) ; MdJA(q; q_ )q_
A
d
+
Mdx + KD x_ + KP x = KP xF
xF = CF (hAd ; hA)
?
ambiente
hA = KA (x)(x ; xe)
+
Mdx + KD x_ + KP (I + CF KA )x = KP CF (KA xe + hAd)
Azione PI
Zt
CF = KF + KI () d&
0
?
K K KF , KI =)
scelta di D , P ,
+ banda passante
All’equilibrio (stabile)
hA1 = hAd
KA x1 = KAxe + hAd
margini di stabilità
Controllo di forza con anello interno di velocità
y = JA; (q)Md; ;;KD x_ + KP xF ; MdJ_ A(q; q_ )q_ 1
1
xF = KF (hAd ; hA)
+
Mdx + KD x_ + KP KF KA x = KP KF (KAxe + hAd)
All’equilibrio (stabile)
hA1 = hAd
KA x1 = KAxe + hAd
Controllo parallelo forza/posizione
h 2= R(KA ) =) deriva di x
se hAd 2 R(KA ) =) x (o x_ ) ! 0
Aggiunta di xd
?
?
se Ad
y = JA; (q)Md; ;;KD x_ + KP (xe + xF ) ; MdJ_ A(q; q_ )q_ 1
1
All’equilibrio
x1 = xd + CF ;KA (xe ; x1) + hAd
Esempio
Md, KD , KP ) come sopra
Riferimenti hAd = [ 10 0 ]T pd = [ 1:015 0:1 ]T
Impedenza (
Controllo di forza
CF = diagfcFx; 0g
1. controllo con anello interno di posizione
kFx = 0:00064
kIx = 0:0016
2. controllo con anello interno di velocit à
kFx = 0:0024
3. controllo parallelo
kFx = 0:00064
kIx = 0:0016
pos organo terminale x
1.02
50
40
[N]
[m]
1.015
1.01
30
20
1.005
1
0
10
1
2
0
0
3
1
[s]
3
1.02
50
40
[N]
1.015
[m]
2
[s]
1.01
30
20
1.005
1
0
10
1
2
0
0
3
1
[s]
3
1.02
50
40
[N]
1.015
[m]
2
[s]
1.01
30
20
1.005
1
0
10
1
2
[s]
3
0
0
1
2
[s]
3
VINCOLI NATURALI E VINCOLI
ARTIFICIALI
Riferimenti di forza e posizione scelti in maniera congruente
all’ambiente
?
Analisi cineto-statica dell’interazione
?
?
non è possibile imporre sia una forza che una posizione
lungo la stessa direzione
vincoli naturali imposti dall’ambiente lungo ciascun grado
di libertà del compito (posizione o forza)
vincoli artificiali soggetti al controllo lungo ciascun grado
di libertà del compito (forza o posizione)
Terna di vincolo Oc –xc yc zc
?
descrizione semplificata del compito in termini di vincoli
naturali e vincoli artificiali
Analisi di casi
Scivolamento su superficie piana
Vincoli
Naturali
p_cz
!xc
!yc
fxc
fyc
cz
Vincoli
Artificiali
fzc
cx
cy
p_cx
p_cy
!zc
Inserimento in foro cilindrico
Vincoli
Naturali
p_cx
p_cy
!xc
!yc
fzc
cz
Vincoli
Artificiali
fxc
fyc
cx
cy
p_cz
!zc
Rotazione di una manovella
Vincoli
Naturali
p_cx
p_cz
!xc
!yc
fyc
cz
Vincoli
Artificiali
fxc
fzc
cx
cy
p_cy
!zc
CONTROLLO IBRIDO FORZA/POSIZIONE
Vincoli naturali
vc = vnc
(I ; )hc = hcn
? matrice di selezione
Vincoli artificiali
(I ; )vc = vac
?
?
hc = hca
perfetta pianificazione del compito
in alternativa: controllo parallelo
Esempio
Matrice di selezione
= diagf0; 1g
Terna di vincolo
1=p2 1=p2 Rc = ;1=p2 1=p2
ATTUATORI E SENSORI
Sistema di attuazione dei giunti
Servomotori
Sensori
SISTEMA DI ATTUAZIONE DEI GIUNTI
Organi di trasmissione
Moto dei giunti
?
?
?
denti a sezione larga e alberi tozzi (deformazioni limitate)
convertono il moto di rotazione in moto di traslazione
viti a ricircolazione di sfere precaricate (riduzione attriti,
aumento rigidezza, riduzione gioco)
Cinghie dentate
?
?
variano l’asse di rotazione e/o traslano il punto di applicazione
Coppie vite–madrevite
?
?
elevate coppie
Ruote dentate
?
basse velocità
consentono di allocare il motore lontano dall’asse del giunto
attuato
alte velocità e basse forze (deformabili)
Catene
?
?
consentono di allocare il motore lontano dall’asse del giunto
attuato
basse velocità (vibrazione)
Accoppiamento diretto
?
?
si eliminano elasticità e gioco
algoritmi di controllo pi ù sofisticati
Servomotori
Motori pneumatici
?
energia pneumatica fornita da un compressore, trasformata
in energia meccanica mediante pistoni o turbine ad aria
Motori idraulici
?
energia idraulica immagazzinata in un serbatoio di accumulazione mediante opportune pompe, trasformata in energia
meccanica
Motori elettrici
?
energia elettrica della rete di distribuzione
Motori per robot industriali
?
?
?
?
?
?
bassa inerzia ed elevato valore del rapporto potenza/peso
elevata possibilità di sovraccarico e sviluppo di coppie impulsive
capacità di sviluppare elevate accelerazioni
elevato campo di variazione di velocit à (da 1 a 1000)
elevata precisione di posizionamento (almeno 1/1000 di
giro)
basso fattore di ondulazione della coppia in modo da garantire rotazioni continue anche a bassa velocit à
+
Servomotori
?
?
inseguimento di traiettoria
regolazione di posizione
Servomotori elettrici
?
?
?
servomotori in corrente continua a magneti permanenti
servomotori a commutazione elettronica
motori a passo (micromanipolatori)
Servomotori idraulici
?
?
coppia pistone–cilindro (moti di traslazione
servomotori idraulici con pistoni assiali o radiali
Pro:
?
?
?
?
?
disponibilità diffusa di sorgente di alimentazione
costo contenuto e vasta gamma di prodotti
buon rendimento di conversione di potenza
facile manutenzione
assenza di inquinamento dell’ambiente di lavoro
Contro:
?
?
problemi di surriscaldamento in situazioni statiche a causa
dell’effetto della gravità sul manipolatore (freni di stazionamento)
necessità di particolari protezioni per impiego in ambienti
infiammabili
Pro:
?
?
?
?
non presentano problemi di surriscaldamento in situazioni
statiche
sono autolubrificati e il fluido circolante facilita lo smaltimento del calore
sono intrinsecamente sicuri in ambienti pericolosi
hanno un ottimo rapporto potenza/peso
Contro:
?
?
?
?
?
necessità di una centrale idraulica di alimentazione
costo elevato, ridotta gamma di prodotti, difficolt à di miniaturizzazione
basso rendimento nella conversione di potenza
necessità di manutenzione periodica
inquinamento dell’ambiente di lavoro per perdite di olio
?
?
?
buone caratteristiche dinamiche
ottima flessibilità di controllo
necessità di organi di trasmissione
?
?
caratteristiche dinamiche dipendenti dalla temperatura del
fluido in pressione
coppie elevate a basse velocità
Amplificatori di potenza
Funzione di modulazione del flusso di potenza dall’alimentazione
verso l’attuatore (sotto l’azione del controllo)
?
amplificatori a transistori
convertitori controllati c.c./c.c. (chopper)
convertitori controllati c.c./c.a. (inverter)
?
servovalvole elettroidrauliche
Sorgenti di alimentazione
Azionamenti elettrici
?
trasformatore + ponte raddrizzatore (non controllato)
Azionamenti idraulici
?
pompa azionata da motore primo (asincrono trifase) a velocità costante + serbatoio di accumulazione + filtri + valvole
SERVOMOTORI
Servomotore a corrente continua a magneti permanenti
?
?
?
un circuito statorico che genera il flusso magnetico di eccitazione (induttore): questo generatore è sempre a magneti
permanenti realizzati con materiali ceramici o con terre rare
(campi elevati in spazi ridotti)
un circuito di armatura (indotto) che reca un avvolgimento
costituito dalle spire di un solenoide avvolto su materiale
ferromagnetico che costituisce il rotore
un collettore che consente di collegare, mediante delle spazzole, i circuiti rotorici ad un circuito esterno di alimentazione secondo una logica di commutazione attuata dal
movimento del rotore
Servomotore a commutazione elettronica
?
?
?
un generatore di flusso magnetico di eccitazione (induttore)
che funge da rotore; questo generatore è a magneti permanenti di materiale ceramico o a terre rare
un circuito di armatura (indotto) costituito da un avvolgimento polifase che funge da statore
un circuito di commutazione statico che, a partire dai segnali
forniti da un sensore di posizione posto sull’albero motore,
genera la sequenza di alimentazione delle fasi del circuito
di armatura in dipendenza del moto di rotazione del rotore
Servomotore elettrico con amplificatore
Equilibrio elettrico
Va = (Ra + sLa )Ia + Vg
Vg = kv Equilibrio meccanico
Cm = (sIm + Fm )
+ Cr
Cm = kt Ia
Amplificatore di potenza
Va = Gv
Vc 1 + sTv
Retroazione di corrente
Generatore controllato di velocit à
?
?
?
?
ki = 0
Fm kv kt =Ra
K = Ci (0)Gv
Cr = 0
K
Ra
kv V 0 ; kv kt Cr
=
c
R
I
a
m
1+s k k
1 + s Rka Ikm
v t
?
v t
a regime:
! kK vc0
v
Protezione di corrente
Generatore controllato di coppia
? Kki Ra
? kv =Kki 0
kt
1
= ki FmIm Vc0 ; FmIm Cr
1 + sF
1 + sF
m
?
a regime:
k k
cm kt vc0 ; Kv !
i
m
Coppie pistone–cilindro
?
?
servomotori lineari
servomotori rotanti
Motore idraulico con servovalvola e distributore
SENSORI
Sensori propriocettivi
?
?
?
posizioni dei giunti
velocità dei giunti
coppia ai giunti
Sensori esterocettivi
?
?
?
?
?
?
sensori di forza
sensori tattili
sensori di prossimità
sensori di campo
sistemi di visione
sensori per applicazioni particolari (suono, umidit à, fumo,
pressione, temperatura)
fusione dei dati sensoriali (robot organo intelligente di
connessione tra percezione e azione)
Trasduttori di posizione
Spostamenti lineari (robot di misura)
?
?
?
potenziometri
trasformatori differenziali (LVDT)
inductosyn
Spostamenti angolari
?
?
?
?
potenziometri
encoder (dischi codificatori)
resolver
synchro
Encoder assoluto
#
Codice
#
Codice
0
1
2
3
4
5
6
7
0000
0001
0011
0010
0110
0111
0101
0100
8
9
10
11
12
13
14
15
1100
1101
1111
1110
1010
1011
1001
1000
Encoder incrementali
Ricostruzione della velocità (impulso in corrispondenza di ogni
transizione)
?
?
?
convertitore frequenza/tensione (uscita analogica)
misura della frequenza (digitale)
misura del periodo di campionamento (digitale)
Resolver
Trasduttori di velocità
Dinamo tachimetrica
?
?
?
?
generatore in c.c.
magnete permanente (campo di eccitazione)
tensione di uscita proporzionale alla velocit à angolare
ondulazione residua
Generatore tachimetrico in corrente alternata
?
?
?
?
?
macchina elettrica
rotore a coppa (basso momento di inerzia)
due avvolgimenti statorici in quadratura
tensione sinusoidale proporzionale alla velocit à angolare
tensione residua a rotore fermo
Sensori di forza
Estensimetro (strain gage)
R
Vo =
R
s
R1 + R2 ; R3 + Rs Vi
?
2
impiego di una coppia di estensimetri (riduzione degli effetti
di variazioni di temperatura)
Sensore di coppia all’albero
?
?
?
servomotore come generatore controllato di coppia
misura di coppia con estensimetri montati su un organo
deformabile (albero cavo) interposto tra motore e giunto
(bassa rigidezza torsionale ed elevata rigidezza flessionale)
coppia misurata senza effetti inerziali e di attrito del servomotore
Sensore di forza al polso
? diametri 10 cm
? altezza 5 cm
? (50, 500) N (risoluzione 0.1%)
? (5, 70) Nm (risoluzione 0.05%)
? frequenza in uscita 1 kHz
f c Rc
s
cc =
s c Oc fss
c
c
S (rcs)Rs Rs s
?
sensore di forza a croce di Malta
2 fxs 3 2 0 0 c 0 0 0 c 0
66 fyss 77 66 c 0 0 0 c 0 0 0
66 fzs 77 = 66 0 c 0 c 0 c 0 c
64 xs 75 64 0 0 0 c 0 0 0 c
y
0 c
0 0 0 c
0 0
13
17
21
25
32
34
36
44
sz
52
38
48
56
c61 0 c63 0 c65 0 c67 0
2w
36w
77 666 w
77 66 w
75 66 w
64 w
w
1
2
3
4
5
6
7
w8
3
77
77
77
77
75
Sensori di visione
Telecamera
Elemento fotosensibile (pixel)
?
?
struttura a matrice rettangolare
CCD
carica = integrale dell’illuminazione incidente
?
CMOS
carica = quantità di illuminazione incidente
p~c = Tbcp~
Trasformazione prospettica
c
fp
X = f ; xpc
z
c
fp
Y = f ; ypc
z
?
?
aberrazioni
distorsione geometrica
?
?
campionamento temporale (otturatore)
campionamento spaziale (pixel)
c
fp
x
XI = f ; pxc + X0
z
y fpcy
YI = f ; pc + Y0
z
2 x 3 2 pcx 3
4 yII 5 = 64 ppcyc 75
zI
z
1
2 0 X 0321 0 0 03
x
= 4 0 y Y 0 5 64 00 10 ;10=f 01 75
0 0 1 0
0
0
0 0
0
+
Matrice di calibrazione
= Tbc
?
?
parametri intrinseci ( x ; y ; X0 ; Y0 ; f ) in
T
parametri estrinseci in cb
1
Standard video
?
?
?
CCIR (Europa e Australia): 625 linee, 25 Hz
RS170 (America e Giappone): 525 linee, 30 Hz
segnale video = tensione 1 V picco–picco
Immagine = scansione sequenziale
?
?
linee pari (primo semiquadro)
linee dispari (secondo semiquadro)
Discretizzazione del segnale video
?
frame grabber
Visione a basso livello
?
caratteristiche elementari (centroide, discontinuit à di intensità)
Visione ad alto livello
?
emulazione della conoscenza (riconoscimento di oggetti in
base a modelli CAD)
UNITÀ DI GOVERNO
Architettura funzionale
Ambiente di programmazione
Architettura hardware
ARCHITETTURA FUNZIONALE
Unità di governo
?
?
?
?
abilità di muovere oggetti fisici nell’ambiente di lavoro,
(capacità di manipolazione)
abilità di ottenere informazioni sullo stato del sistema e su
quello dell’ambiente di lavoro (capacit à sensoriali)
abilità di utilizzare informazioni per modificare il comportamento del sistema in maniera preprogrammata (capacit à
di comportamento intelligente)
abilità di immagazzinare, elaborare e rendere disponibili
dati relativi all’attività del sistema (capacità di processamento dati)
Modello di riferimento
Evoluzione verso livelli superiori
?
?
compiti complessi
missioni
Livello del compito
?
?
?
specifica del compito a un livello di astrazione elevato
decomposizione in una serie di azioni (coordinamento
spazio-temporale)
scelta sulla base dei modelli della conoscenza e delle informazioni sulla scena
Livello delle azioni
?
?
comandi simbolici dal livello del compito tradotti in percorsi
di moto (azioni elementari)
scelta sulla base dei modelli del manipolatore e delle informazioni sull’ambiente
Livello delle primitive
?
?
?
?
sequenza di configurazioni dal livello delle azioni
calcolo traiettorie di moto (riferimenti per i servo)
definizione della strategia di controllo
scelta sulla base del modello dinamico e delle informazioni
dei sensori
Livello dei servo
?
?
?
?
direttive dal livello delle primitive
calcolo algoritmi di controllo (segnali di attuazione)
microinterpolazione
aggiornamento sulla base delle misure dei sensori
Architettura funzionale per robot industriali
AMBIENTE DI PROGRAMMAZIONE
Caratteristiche (comuni con quelli dei calcolatori + ...)
?
?
?
?
?
?
?
?
sistema operativo in tempo reale
modellazione del mondo
controllo del moto
lettura dei dati sensoriali
interattività con il sistema fisico
capacità di rivelazione di errori
capacità di ripristino di situazioni operative corrette
struttura specifica dei linguaggi
Requisiti imposti dall’articolazione in moduli
Trattamento dei dati sensoriali
?
?
?
?
servo (condizionamento segnali in tempo reale)
primitive (dati sensoriali in terne di riferimento)
azioni (elaborazione dati ad alto livello)
compito (riconoscimento oggetti nella scena)
Consultazione dei modelli di conoscenza
?
?
?
?
servo (calcolo numerico dei modelli in tempi brevi)
primitive (trasformazioni di coordinate)
azioni (simulazione + rappresentazione CAD)
compito (sistema esperto)
Funzioni decisionali
?
?
?
?
servo (calcolo in tempo reale)
primitive (condizionamento logico)
azioni (sincronizzazione + sistema di interruzioni)
compito (gestione di processi concorrenti + debugger)
Generazioni di ambienti di programmazione
Programmazione a basso livello (Assembly, C)
Programmazione per insegnamento
Programmazione orientata al robot
Programmazione orientata agli oggetti
?
la generazione successiva incorpora le caratteristiche della
generazione precedente
Programmazione per insegnamento
Guida dell’operatore lungo il percorso desiderato attraverso
teach pendant
Registrazione delle posizioni dei giunti
Ripetizione del moto
?
?
?
capacità di programmazione limitata (tecnico di officina)
disponibilità del robot
saldatura a punti, verniciatura, palettizzazione
Adozione di linguaggi
?
?
?
?
?
?
?
acquisizione di posture significative mediante apprendimento
calcolo della locazione dell’organo terminale rispetto ad una
terna di riferimento (cinematica diretta)
assegnazione di primitive di moto con parametri di traiettoria
calcolo dei riferimenti dei servo (cinematica inversa)
condizionamento delle sequenze di apprendimento a semplici sensori (presenza oggetto nella pinza)
correzione delle sequenze di moto a mezzo di semplici editori di testo
semplici operazioni di collegamento tra sottoinsiemi di sequenze elementari
Programmazione orientata al robot
?
integrazione di numerose funzioni di linguaggi di programmazione ad alto livello (BASIC, PASCAL) con quelle specifiche richieste nelle applicazioni robotiche
Alternative
?
?
?
?
?
sviluppo di linguaggi ad hoc per applicazioni robotiche
sviluppo di librerie di programmi per robotica di supporto
a linguaggi standard di programmazione
programmatore esperto
programmazione fuori linea
interazione con altre macchine
Caratteristiche di linguaggio di programmazione strutturata
?
?
?
?
?
?
?
?
?
?
editore di testo
strutture complesse di rappresentazione dei dati
uso estensivo di variabili di stato predefinite
realizzazione di operazioni di algebra matriciale
uso estensivo di rappresentazioni simboliche per le terne di
coordinate
possibilità di specificare il moto coordinato di pi ù terne
relative ad oggetti connessi rigidamente per il tramite di
una sola terna
inclusione di sottoprogrammi con scambio di dati e parametri
uso di funzioni di condizionamento logico e cicli di attesa
mediante semafori
capacità di parallelizzazione del calcolo
funzioni di controllore logico programmabile (PLC)
ARCHITETTURA HARDWARE
Scheda sistema
?
?
?
?
?
?
?
un microprocessore con coprocessore matematico
una memoria EPROM di inizializzazione
una memoria RAM locale
una memoria RAM condivisa con le altre schede attraverso
il bus
un insieme di porte seriali e parallele di interfaccia con il
bus e con il mondo esterno
contatori, registri, temporizzatori
gestore delle interruzioni
Funzioni
?
?
?
?
?
?
?
interfaccia con l’operatore attraverso teach pendant, tastiera,
video, stampante
interfaccia con una memoria esterna (disco rigido) usata per
immagazzinare dati e programmi applicativi
interfaccia con rete locale di connessione (ad esempio,
Ethernet) con stazioni di lavoro e con altre unit à di governo
interfaccia I/O con apparecchiature periferiche presenti
nell’area di lavoro (alimentatori, nastri trasportatori, sensori ON/OFF)
inizializzazione del sistema
interprete del linguaggio di programmazione
arbitro del bus
Processori suppletivi o alternativi (DSP, Transputer)
Funzioni della scheda cinematica
?
?
?
?
calcolo delle primitive di moto
calcolo della cinematica diretta, della cinematica inversa e
dello Jacobiano
verifica di congruenza della traiettoria
gestione della ridondanza cinematica
Funzioni della scheda dinamica
?
calcolo della dinamica inversa
Funzioni della scheda servo
?
?
?
?
?
microinterpolazione dei riferimenti
calcolo dell’algoritmo di controllo
conversione D/A ed interfaccia con amplificatori di potenza
trattamento dati dei trasduttori di posizione e di velocit à
interruzione del moto in situazioni di malfunzionamento
Funzioni della scheda forza
?
?
condizionamento dei segnali forniti dal sensore di forza
rappresentazione delle forze in terne di coordinate assegnate
Funzioni della scheda visione
?
?
?
elaborazione dei segnali forniti dalla telecamera
estrazione di caratteristiche geometriche della scena
localizzazione degli oggetti in terne di coordinate assegnate