You are on page 1of 452

ROBOTICA A.A. 2013/2014 Prof.

Ciro NATALE
PROGRAMMA DEL CORSO DI ROBOTICA
Introduzione
Cinematica
Cinematica differenziale e statica
Dinamica
Pianicazione di traiettorie
Controllo del moto
Controllo dellinterazione
Unit di governo ed elementi di programmazione di robot
industriali
Cenni su sensori e algoritmi di fusione sensoriale per la robotica
avanzata
Framework di programmazione di robot avanzati (ROS,
OROCOS)
Libro di testo:
B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotica
Modellistica, pianicazione e controllo (3rd Ed.), McGraw-Hill,
2008.
Libro di consultazione:
B. Siciliano, O. Khatib (Eds.) Springer handbook of robotics,
Springer, 2008.
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ROBOTICA
Studio di macchine che possanosostituire luomonellesecuzione
di un compito, sia in termini di attivit` a sica che decisionale
Radici culturali
mitologia
automa
robot (robota = lavoro esecutivo)
letteratura fantascientica
Leggi fondamentali (Asimov)
un robot non pu` o 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 conitto con la prima
legge
un robot deve proteggere la sua esistenza a meno che tale
protezione non vada in conitto con la prima o la seconda
legge
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Robotica denita 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 et` a infantile
Robotica industriale
progettazione, governo e applicazioni dei robot in ambito
industriale
tecnologia matura
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Lautomazione ` e una tecnologia il cui obiettivo ` e quello di
sostituire la macchina alluomo in un processo di produzione,
non solo per quanto riguarda lesecuzione delle operazioni
materiali, ma anche per ci ` o che concerne lelaborazione
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 essibile (produzione di lotti variabili di
manufatti diversi)
Robot industriale
macchina con elevate caratteristiche di versatilit` a e
essibilit` a
un robot ` e una struttura meccanica multifunzionale e
riprogrammabile progettato per spostare materiali, parti,
utensili o dispositivi specializzati secondo movimenti
variabili programmati per lesecuzione di una variet ` a di
compiti diversi (Robot Institute of America, 1980)
componente tipico di sistemi di automazione programmabile
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Installazioni di robot industriali nel mondo
Fattori che hanno favorito la diffusione della robotica
riduzione dei costi di produzione
incremento di produttivit` a
miglioramento degli standard di qualit` a del prodotto
eliminare compiti rischiosi o alienanti per loperatore
Industrie di riferimento per applicazioni di robot industriali
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Robot impiegati in Europa nelle varie operazioni di un processo manifatturiero
Principali campi di applicazione
trasporto: si sfrutta la capacit` a di pick and place (il prodotto
non subisce modiche)
manipolazione: si sfrutta la capacit` a di manipolare oggetti
e/o utensili (trasformazione di prodotto)
misura: si sfrutta la capacit` a di muoversi nello spazio in
posizioni note (collaudo di prodotto)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ROBOTICA AVANZATA
scienza che studia robot con spiccate caratteristiche di
autonomia che operano in ambienti non strutturati o
scarsamente strutturati, le cui caratteristiche geometriche o
siche non siano completamente note a priori.
Robot installati per applicazioni non industriali
Robot per lesplorazione
ambienti a rischio di sopravvivenza
riportare alloperatore informazioni utili tramite opportuni
sensori di bordo
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esplorazione planetaria
Veicoli militari autonomi
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Robot di servizio
ambienti domestici
ambienti medici
ambienti pubblici
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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 lazionamentodei giunti; si impiegano usualmente
motori elettrici, idraulici e talvolta pneumatici
sensori che misurano lo stato del manipolatore (sensori
propriocettivi) ed eventualmente lo stato dellambiente
(sensori eterocettivi)
una unit ` a di governo (calcolatore) con funzioni di controllo
e supervisione dei movimenti del manipolatore
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
STRUTTURA DEI MANIPOLATORI
struttura meccanica a catena cinematica aperta o a catena
cinematica chiusa
gradi di mobilit` a (giunti prismatici o rotoidali)
gradi di libert` a (descrizione di un compito)
spazio di lavoro (porzione dellambiente circostante a cui pu` o
accedere lorgano terminale)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore cartesiano
tre giunti prismatici
ad ogni grado di mobilit` a 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)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore a portale
manipolazione di oggetti di dimensione e peso rilevanti
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore cilindrico
un giunto rotoidale e due prismatici
ad ogni grado di mobilit` a 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)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore sferico
due giunti rotoidali e uno prismatico
ad ogni grado di mobilit` a corrisponde un grado di libert` a (in
coordinate sferiche)
discrete caratteristiche di rigidezza meccanica
la precisione di posizionamento del polso si riduce al crescere
dello sbraccio radiale
operazioni di lavorazione
azionamenti elettrici
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore SCARA
due giunti rotoidali e uno prismatico
elevata rigidezza a carichi verticali e cedevolezza a carichi
orizzontali (Selective Compliance Assembly Robot Arm)
la precisione di posizionamento del polso si riduce al crescere
della distanza del polso stesso dallasse del primo giunto
manipolazione di piccoli oggetti
azionamenti elettrici
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore antropomorfo
tre giunti rotoidali
spalla e gomito (che connette braccio e avambraccio)
struttura pi ` u destra
precisione di posizionamento variabile
applicazioni molteplici
azionamenti elettrici
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Polso sferico
giunti rotoidali
determinano lorientamento dellorgano terminale
caratteristiche di compattezza e destrezza
disaccoppiamento tra posizione e orientamento
Organo terminale
specicato in relazione al compito che il robot deve eseguire
pinza (trasporto)
utensile o dispositivo specializzato(lavorazione e assemblaggio)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Alcuni robot industriali
Robot AdeptOne XL
Robot Comau SMART S2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Robot ABB IRB 4400
Unit` a lineare Kuka KL 250 con robot KR 15/2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Robot Robotics Research K-1207i
Robot Fanuc I-21i
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Kuka lightweight robot
Robot umanoide Justin
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
MODELLISTICA E CONTROLLO DI
MANIPOLATORI
Modellistica
struttura meccanica (cinematica + cinematica differenziale
+ statica + dinamica)
attuatori
sensori
Controllo
pianicazione del moto
controllo nello spazio libero
controllo nello spazio vincolato
unit` a di governo
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Cinematica
relazioni tra posizioni dei giunti e posizione e orientamento
dellorgano terminale
Cinematica differenziale
relazioni tra velocit` a dei giunti e velocit` a (lineare e angolare)
dellorgano terminale
Statica
relazioni tra forze e coppie applicate ai giunti e forze e momenti
applicati allorgano terminale in situazioni di equilibrio
Dinamica
equazioni del moto del manipolatore in funzione delle forze e
momenti agenti su di esso
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Pianicazione 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
lesecuzione delle traiettorie di riferimento
Controllo dellinterazione
gestione del contatto tra organo terminale e ambiente
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Attuatori e sensori
attuazione del moto
misura di variabili di interesse
Unit` a di governo
implementazione delle leggi di controllo
interfaccia con operatore
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CINEMATICA
relazioni tra posizioni dei giunti e posizione e orientamento
dellorgano terminale
Matrice di rotazione
Rappresentazioni dellorientamento
Trasformazioni omogenee
Cinematica diretta
Spazio dei giunti e spazio operativo
Calibrazione cinematica
Problema cinematico inverso
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
POSIZIONE E ORIENTAMENTO DI UN
CORPO RIGIDO
Posizione
o

=
_
_
o

x
o

y
o

z
_
_
Orientamento
x

= x

x
x +x

y
y +x

z
z
y

= y

x
x +y

y
y +y

z
z
z

= z

x
x +z

y
y +z

z
z
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
MATRICE DI ROTAZIONE
R =
_
_
x

_
_
=
_
_
x
T
x y
T
x z
T
x
x
T
y y
T
y z
T
y
x
T
z y
T
z z
T
z
_
_
R
T
R = I
R
T
= R
1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Rotazioni elementari
rotazione di intorno a z
R
z
() =
_
_
cos sin 0
sin cos 0
0 0 1
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
rotazione di intorno a y
R
y
() =
_
_
cos 0 sin
0 1 0
sin 0 cos
_
_
rotazione di intorno a x
R
x
() =
_
_
1 0 0
0 cos sin
0 sin cos
_
_
tutte le rotazioni elementari soddisfano la propriet` a
R
k
() = R
T
k
()
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Rappresentazione di un vettore
p =
_
_
p
x
p
y
p
z
_
_
p

=
_
_
p

x
p

y
p

z
_
_
p =
_
_
x

_
_
p

= Rp

= R
T
p
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
p
x
= p

x
cos p

y
sin
p
y
= p

x
sin +p

y
cos
p
z
= p

z
che in forma compatta si scrive appunto
p =
_
_
cos sin 0
sin cos 0
0 0 1
_
_
_
_
p

x
p

y
p

z
_
_
= R
z
()p

la cui trasformazione inversa ` e


p

= R
T
z
()p = R
z
()p
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Rotazione di un vettore
p = Rp

p
T
p = p
T
R
T
Rp

= p
T
p

Esempio
p
x
= p

x
cos p

y
sin
p
y
= p

x
sin +p

y
cos
p
z
= p

z
p = R
z
()p

ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE


Matrice di rotazione
fornisce lorientamento di una terna di coordinate rispetto
ad unaltra: 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 loperatore che consente di ruotare un vettore in una stessa
terna di coordinate
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
COMPOSIZIONE DI MATRICI DI
ROTAZIONE
p
1
= R
1
2
p
2
p
0
= R
0
1
p
1
p
0
= R
0
2
p
2
se R
j
i
` e la matrice di rotazione le cui colonne sono i versori
della terna i rispetto alla terna j
R
j
i
= (R
i
j
)
1
= (R
i
j
)
T
Rotazione in terna corrente
R
0
2
= R
0
1
R
1
2
Rotazione in terna ssa

R
0
2
=

R
1
2
R
0
1
,= R
0
2
infatti

R
0
2
= R
0
1
R
1
0

R
1
2
R
0
1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ANGOLI DI EULERO
matrice di rotazione
9 parametri con 6 vincoli
rappresentazione minima dellorientamento
3 parametri indipendenti
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Angoli ZYZ
R() = R
z
()R
y
()R
z
()
=
_
_
c

+c

+c

_
_
notazioni trigonometriche compatte
c

= cos
s

= sin
c

= cos ( +)
. . .
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Problema inverso
Assegnata
R =
_
_
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
_
_
i 3 angoli ZYZ sono ( (0, ))
= Atan2(r
23
, r
13
)
= Atan2
_
_
r
2
13
+r
2
23
, r
33
_
= Atan2(r
32
, r
31
)
ovvero ( (, 0))
= Atan2(r
23
, r
13
)
= Atan2
_

_
r
2
13
+r
2
23
, r
33
_
= Atan2(r
32
, r
31
)
dove Atan2(y, x) = arg(x +iy)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Angoli di RPY
R() = R
z
()R
y
()R
x
()
=
_
_
c

+s

+c

_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Problema inverso
Assegnata
R =
_
_
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
_
_
i 3 angoli di RPY sono ( (/2, /2))
= Atan2(r
21
, r
11
)
= Atan2
_
r
31
,
_
r
2
32
+r
2
33
_
= Atan2(r
32
, r
33
)
ovvero ( (/2, 3/2))
= Atan2(r
21
, r
11
)
= Atan2
_
r
31
,
_
r
2
32
+r
2
33
_
= Atan2(r
32
, r
33
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ASSE/ANGOLO
R(, r) = R
z
()R
y
()R
z
()R
y
()R
z
()
se ,= 0
sin =
r
y
_
r
2
x
+r
2
y
cos =
r
x
_
r
2
x
+r
2
y
sin =
_
r
2
x
+r
2
y
cos = r
z
se = 0
R(, r) = R
z
()
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
R(, r) =
_

_
r
2
x
(1 c

) +c

r
x
r
y
(1 c

) r
z
s

r
x
r
y
(1 c

) +r
z
s

r
2
y
(1 c

) +c

r
x
r
z
(1 c

) r
y
s

r
y
r
z
(1 c

) +r
x
s

r
x
r
z
(1 c

) +r
y
s

r
y
r
z
(1 c

) r
x
s

r
2
z
(1 c

) +c

_
_
R(, r) = R(, r)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Problema inverso
Assegnata
R =
_
_
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
_
_
langolo e lasse di rotazione sono (sin ,= 0)
= cos
1
_
r
11
+r
22
+r
33
1
2
_
r =
1
2 sin
_
_
r
32
r
23
r
13
r
31
r
21
r
12
_
_
con
r
2
x
+r
2
y
+r
2
z
= 1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
QUATERNIONE UNITARIO
rappresentazione a 4 parametri Q = ,
= cos

2
= sin

2
r

2
+
2
x
+
2
y
+
2
z
= 1
(, r) e (, r) forniscono lo stesso quaternione
R(, ) =
_

_
2(
2
+
2
x
) 1 2(
x

y

z
) 2(
x

z
+
y
)
2(
x

y
+
z
) 2(
2
+
2
y
) 1 2(
y

z

x
)
2(
x

z

y
) 2(
y

z
+
x
) 2(
2
+
2
z
) 1
_

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Problema inverso
Assegnata
R =
_
_
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
_
_
il quaternione ` e ( 0)
=
1
2

r
11
+r
22
+r
33
+ 1
=
1
2
_

_
sgn(r
32
r
23
)

r
11
r
22
r
33
+ 1
sgn(r
13
r
31
)

r
22
r
33
r
11
+ 1
sgn(r
21
r
12
)

r
33
r
11
r
22
+ 1
_

_
quaternione estratto da R
1
= R
T
Q
1
= ,
prodotto tra quaternioni
Q
1
Q
2
=
1

2

T
1

2
,
1

2
+
2

1
+
1

2

lelemento neutro del prodotto ` e il quaternione 1, 0, infatti


Q Q
1
= 1, 0
la rotazione di un vettore Rp equivale a prendere la parte
vettoriale del quaternione (con Q estratto da R)
Q 0, p Q
1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
TRASFORMAZIONI OMOGENEE
Trasformazione di coordinate (traslazione + rotazione)
p
0
= o
0
1
+R
0
1
p
1
Trasformazione inversa
p
1
= R
1
0
o
0
1
+R
1
0
p
0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Rappresentazione omogenea
p =
_
_
p
1
_
_
Matrice di trasformazione omogenea
A
0
1
=
_
_
R
0
1
o
0
1
0
T
1
_
_
Trasformazione di coordinate
p
0
= A
0
1
p
1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Trasformazione inversa
p
1
= A
1
0
p
0
=
_
A
0
1
_
1
p
0
ove
A
1
0
=
_
_
R
1
0
R
1
0
o
0
1
0
T
1
_
_
A
1
,= A
T
Successione di trasformazioni
p
0
= A
0
1
A
1
2
. . . A
n1
n
p
n
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CINEMATICA DIRETTA
Manipolatore
insieme di bracci connessi tramite giunti
Catena cinematica (dalla base allorgano terminale)
aperta (sequenza unica)
chiusa (sequenza forma un anello)
Grado di mobilit` a
tipicamente associato a una articolazione = variabile di
giunto
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Terna base e terna utensile
Equazione cinematica diretta
T
b
e
(q) =
_
_
n
b
e
(q) s
b
e
(q) a
b
e
(q) p
b
e
(q)
0 0 0 1
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore planare a due bracci
T
b
e
(q) =
_
_
n
b
e
s
b
e
a
b
e
p
b
e
0 0 0 1
_
_
=
_

_
0 s
12
c
12
a
1
c
1
+a
2
c
12
0 c
12
s
12
a
1
s
1
+a
2
s
12
1 0 0 0
0 0 0 1
_

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Catena aperta
dato un manipolatore a n + 1 bracci connessi tramite n giunti
(il braccio 0 ` e convenzionalmente sso a terra)
si ssano n + 1 terne ciascuna solidale ad un braccio
si assume che ogni giunto fornisce un solo grado di mobilit` a
T
0
n
(q) = A
0
1
(q
1
)A
1
2
(q
2
) . . . A
n1
n
(q
n
)
ciascuna matrice di trasformazione dipende da una sola
variabile di giunto!
T
b
e
(q) = T
b
0
T
0
n
(q)T
n
e
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Convenzione di Denavit-Hartenberg
si sceglie lasse z
i
giacente lungo lasse del giunto i + 1
si individua O
i
allintersezione dellasse z
i
con la normale
comune agli assi z
i1
e z
i
, e con O

i
si indica lintersezione
della normale comune con z
i1
si assume lasse x
i
diretto lungo la normale comune agli
assi z
i1
e z
i
con verso positivo dal giunto i al giunto i + 1
si sceglie lasse y
i
in modo da completare una terna levogira
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Denizione non univoca della terna:
con riferimento alla terna 0, per la quale la sola direzione
dellasse z
0
risulta specicata: si possono quindi scegliere
arbitrariamente O
0
ed x
0
con riferimento alla terna n, per la quale il solo
asse x
n
risulta soggetto a vincolo (deve essere normale
allasse z
n1
): infatti non vi ` e giunto n + 1, per cui non ` e
denito z
n
e lo si pu` o scegliere arbitrariamente
quando due assi consecutivi sono paralleli, in quanto la
normale comune tra di essi non ` e univocamente denita
quando due assi consecutivi si intersecano, in quanto il verso
di x
i
` e arbitrario
quando il giunto i ` e prismatico, nel qual caso la sola
direzione dellasse z
i1
` e determinata
le indeterminazioni lasciano la libert` a di effettuare scelte che
semplichino la procedura
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Parametri di Denavit-Hartenberg
a
i
distanza di O
i
da O

i
;
d
i
coordinata su z
i1
di O

i
;

i
angolo intorno allasse x
i
tra lasse z
i1
e lasse z
i
valutato
positivo in senso antiorario;

i
angolo intorno allasse z
i1
tra lasse x
i1
e lasse x
i
valutato
positivo in senso antiorario.
a
i
e
i
sono sempre costanti
se il giunto ` e rotoidale la variabile ` e
i
se il giunto ` e prismatico la variabile ` e d
i
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Trasformazione di coordinate
A
i1
i

=
_

_
c

i
s

i
0 0
s

i
c

i
0 0
0 0 1 d
i
0 0 0 1
_

_
A
i

i
=
_

_
1 0 0 a
i
0 c

i
s

i
0
0 s

i
c

i
0
0 0 0 1
_

_
A
i1
i
(q
i
) = A
i1
i

A
i

i
=
_

_
c

i
s

i
c

i
s

i
s

i
a
i
c

i
s

i
c

i
c

i
c

i
s

i
a
i
s

i
0 s

i
c

i
d
i
0 0 0 1
_

_
se il giunto ` e rotoidale q
i
=
i
se il giunto ` e prismatico q
i
= d
i
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Procedura operativa
1. Individuare e numerare consecutivamente gli assi dei giunti;
assegnare, rispettivamente, le direzioni agli assi z
0
, . . . , z
n1
2. Fissare la terna base posizionandone lorigine sullasse z
0
; gli
assi x
0
e y
0
sono scelti in maniera tale da ottenere una terna
levogira
Eseguire i passi da 3 a 5 per i = 1, . . . , n 1:
3. Individuare lorigine O
i
allintersezione di z
i
con la normale
comune agli assi z
i1
e z
i
. Se gli assi z
i1
e z
i
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 ne-corsa)
4. Fissare lasse x
i
diretto lungo la normale comune agli assi z
i1
e z
i
con verso positivo dal giunto i al giunto i + 1
5. Fissare lasse y
i
in modo da ottenere una terna levogira
Per completare:
6. Fissare la terna n, allineando z
n
lungo la direzione di z
n1
se il
giunto n ` e rotoidale, ovvero scegliendo z
n
in maniera arbitraria
se il giunto n ` e prismatico; ssare lasse x
n
in accordo al punto 4
7. Costruire per i = 1, . . . , n la tabella dei parametri a
i
, d
i
,
i
,
i
8. Calcolare sulla base dei parametri di cui al punto 7 le matrici
di trasformazione omogenea A
i1
i
(q
i
) per i = 1, . . . , n
9. Calcolare T
0
n
(q) = A
0
1
. . . A
n1
n
che fornisce posizione e
orientamento della terna n rispetto alla terna 0
10. Assegnate T
b
0
e T
n
e
, calcolare la funzione cinematica diretta
T
b
e
(q) = T
b
0
T
0
n
T
n
e
che fornisce posizione e orientamento della
terna utensile rispetto alla terna base
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore planare a tre bracci
Braccio a
i

i
d
i

i
1 a
1
0 0
1
2 a
2
0 0
2
3 a
3
0 0
3
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
A
i1
i
=
_

_
c
i
s
i
0 a
i
c
i
s
i
c
i
0 a
i
s
i
0 0 1 0
0 0 0 1
_

_
i = 1, 2, 3
T
0
3
= A
0
1
A
1
2
A
2
3
=
_

_
c
123
s
123
0 a
1
c
1
+a
2
c
12
+a
3
c
123
s
123
c
123
0 a
1
s
1
+a
2
s
12
+a
3
s
123
0 0 1 0
0 0 0 1
_

_
si osservi che
[ x
0
3
y
0
3
z
0
3
] = R
z
(
1
)R
z
(
2
)R
z
(
3
) = R
z
(
1
+
2
+
3
)
la terna 3 non pu` o essere terna utensile
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore sferico
Braccio a
i

i
d
i

i
1 0 /2 0
1
2 0 /2 d
2

2
3 0 0 d
3
0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
A
0
1
=
_

_
c
1
0 s
1
0
s
1
0 c
1
0
0 1 0 0
0 0 0 1
_

_
A
1
2
=
_

_
c
2
0 s
2
0
s
2
0 c
2
0
0 1 0 d
2
0 0 0 1
_

_
A
2
3
=
_

_
1 0 0 0
0 1 0 0
0 0 1 d
3
0 0 0 1
_

_
T
0
3
= A
0
1
A
1
2
A
2
3
=
_

_
c
1
c
2
s
1
c
1
s
2
c
1
s
2
d
3
s
1
d
2
s
1
c
2
c
1
s
1
s
2
s
1
s
2
d
3
+c
1
d
2
s
2
0 c
2
c
2
d
3
0 0 0 1
_

_
naturalmente lorientamento non dipende da d
3
la terna 3 pu` o essere terna utensile
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore antropomorfo
Braccio a
i

i
d
i

i
1 0 /2 0
1
2 a
2
0 0
2
3 a
3
0 0
3
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
A
0
1
=
_

_
c
1
0 s
1
0
s
1
0 c
1
0
0 1 0 0
0 0 0 1
_

_
A
i1
i
=
_

_
c
i
s
i
0 a
i
c
i
s
i
c
i
0 a
i
s
i
0 0 1 0
0 0 0 1
_

_
i = 2, 3
T
0
3
= A
0
1
A
1
2
A
2
3
=
_

_
c
1
c
23
c
1
s
23
s
1
c
1
(a
2
c
2
+a
3
c
23
)
s
1
c
23
s
1
s
23
c
1
s
1
(a
2
c
2
+a
3
c
23
)
s
23
c
23
0 a
2
s
2
+a
3
s
23
0 0 0 1
_

_
la terna 3 non pu` o essere terna utensile
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Polso sferico
Braccio a
i

i
d
i

i
4 0 /2 0
4
5 0 /2 0
5
6 0 0 d
6

6
il verso di x
4
e x
5
` e indeterminato
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
A
3
4
=
_

_
c
4
0 s
4
0
s
4
0 c
4
0
0 1 0 0
0 0 0 1
_

_
A
4
5
=
_

_
c
5
0 s
5
0
s
5
0 c
5
0
0 1 0 0
0 0 0 1
_

_
A
5
6
=
_

_
c
6
s
6
0 0
s
6
c
6
0 0
0 0 1 d
6
0 0 0 1
_

_
T
3
6
= A
3
4
A
4
5
A
5
6
=
_

_
c
4
c
5
c
6
s
4
s
6
c
4
c
5
s
6
s
4
c
6
c
4
s
5
c
4
s
5
d
6
s
4
c
5
c
6
+c
4
s
6
s
4
c
5
s
6
+c
4
c
6
s
4
s
5
s
4
s
5
d
6
s
5
c
6
s
5
s
6
c
5
c
5
d
6
0 0 0 1
_

_
la matrice di rotazione R
3
6
coincide con la matrice di
rotazione corrispondente agli angoli di Eulero ZYZ della
terna 6 rispetto alla terna 3 per cui questi coincidono con gli
angoli di giunto
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore di Stanford
non occorre ricalcolare la tabella in quanto la terna 3 del
polso sferico e quella della struttura portante (manipolatore
sferico) coincidono
T
0
6
= T
0
3
T
3
6
=
_
_
n
0
s
0
a
0
p
0
0 0 0 1
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
p
0
=
_
_
c
1
s
2
d
3
s
1
d
2
+
_
c
1
(c
2
c
4
s
5
+s
2
c
5
) s
1
s
4
s
5
_
d
6
s
1
s
2
d
3
+c
1
d
2
+
_
s
1
(c
2
c
4
s
5
+s
2
c
5
) +c
1
s
4
s
5
_
d
6
c
2
d
3
+ (s
2
c
4
s
5
+c
2
c
5
)d
6
_
_
n
0
=
_
_
c
1
_
c
2
(c
4
c
5
c
6
s
4
s
6
) s
2
s
5
c
6
_
s
1
(s
4
c
5
c
6
+c
4
s
6
)
s
1
_
c
2
(c
4
c
5
c
6
s
4
s
6
) s
2
s
5
c
6
_
+c
1
(s
4
c
5
c
6
+c
4
s
6
)
s
2
(c
4
c
5
c
6
s
4
s
6
) c
2
s
5
c
6
_
_
s
0
=
_
_
c
1
_
c
2
(c
4
c
5
s
6
+s
4
c
6
) +s
2
s
5
s
6
_
s
1
(s
4
c
5
s
6
+c
4
c
6
)
s
1
_
c
2
(c
4
c
5
s
6
+s
4
c
6
) +s
2
s
5
s
6
_
+c
1
(s
4
c
5
s
6
+c
4
c
6
)
s
2
(c
4
c
5
s
6
+s
4
c
6
) +c
2
s
5
s
6
_
_
a
0
=
_
_
c
1
(c
2
c
4
s
5
+s
2
c
5
) s
1
s
4
s
5
s
1
(c
2
c
4
s
5
+s
2
c
5
) +c
1
s
4
s
5
s
2
c
4
s
5
+c
2
c
5
_
_
se fosse d
6
= 0 la posizione della terna utensile
coinciderebbe con la posizione dellorgano terminale della
sola struttura portante
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore antropomorfo con polso sferico
occorre ricalcolare la tabella in quanto la terna 3 del polso
sferico e quella della struttura portante non coincidono
Braccio a
i

i
d
i

i
1 0 /2 0
1
2 a
2
0 0
2
3 0 /2 0
3
4 0 /2 d
4

4
5 0 /2 0
5
6 0 0 d
6

6
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
A
2
3
=
_

_
c
3
0 s
3
0
s
3
0 c
3
0
0 1 0 0
0 0 0 1
_

_
A
3
4
=
_

_
c
4
0 s
4
0
s
4
0 c
4
0
0 1 0 d
4
0 0 0 1
_

_
p
0
=
_
_
a
2
c
1
c
2
+d
4
c
1
s
23
+d
6
_
c
1
(c
23
c
4
s
5
+s
23
c
5
) +s
1
s
4
s
5
_
a
2
s
1
c
2
+d
4
s
1
s
23
+d
6
_
s
1
(c
23
c
4
s
5
+s
23
c
5
) c
1
s
4
s
5
_
a
2
s
2
d
4
c
23
+d
6
(s
23
c
4
s
5
c
23
c
5
)
_
_
n
0
=
_
_
c
1
_
c
23
(c
4
c
5
c
6
s
4
s
6
) s
23
s
5
c
6
_
+s
1
(s
4
c
5
c
6
+c
4
s
6
)
s
1
_
c
23
(c
4
c
5
c
6
s
4
s
6
) s
23
s
5
c
6
_
c
1
(s
4
c
5
c
6
+c
4
s
6
)
s
23
(c
4
c
5
c
6
s
4
s
6
) +c
23
s
5
c
6
_
_
s
0
=
_
_
c
1
_
c
23
(c
4
c
5
s
6
+s
4
c
6
) +s
23
s
5
s
6
_
+s
1
(s
4
c
5
s
6
+c
4
c
6
)
s
1
_
c
23
(c
4
c
5
s
6
+s
4
c
6
) +s
23
s
5
s
6
_
c
1
(s
4
c
5
s
6
+c
4
c
6
)
s
23
(c
4
c
5
s
6
+s
4
c
6
) c
23
s
5
s
6
_
_
a
0
=
_
_
c
1
(c
23
c
4
s
5
+s
23
c
5
) +s
1
s
4
s
5
s
1
(c
23
c
4
s
5
+s
23
c
5
) c
1
s
4
s
5
s
23
c
4
s
5
c
23
c
5
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
SPAZIO DEI GIUNTI E SPAZIO OPERATIVO
Spazio dei giunti
q =
_
_
q
1
.
.
.
q
n
_
_
q
i
=
i
(giunto rotoidale)
q
i
= d
i
(giunto prismatico)
Spazio operativo
x =
_
p

_
p (posizione)
(orientamento)
Equazione cinematica diretta
x = k(q)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
x =
_
_
p
x
p
y

_
_
= k(q) =
_
_
a
1
c
1
+a
2
c
12
+a
3
c
123
a
1
s
1
+a
2
s
12
+a
3
s
123

1
+
2
+
3
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Spazio di lavoro
Spazio di lavoro raggiungibile
p = p(q) q
im
q
i
q
iM
i = 1, . . . , n
elementi di supercie planare, sferica, toroidale e cilindrica
Spazio di lavoro destro
orientamenti diversi
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
congurazioni ammissibili
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
spazio di lavoro
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Accuratezza
scostamento tra posizione conseguita con la postura
assegnata e posizione calcolata tramite la cinematica diretta
valori tipici: (0.2, 1) mm
Ripetibilit` a
capacit` a del manipolatore di tornare in una posizione
precedentemente raggiunta
valori tipici: (0.02, 0.2) mm
parametro importante nella tecnica di programmazione per
insegnamento
Ridondanza cinematica
m < n (intrinseca)
r < m = n (funzionale)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CALIBRAZIONE CINEMATICA
Valori precisi dei parametri DH per migliorare laccuratezza di
un manipolatore
Equazione cinematica diretta in funzione di tutti i parametri
x = k(a, , d, )
x
m
locazione misurata
x
n
locazione nominale (parametri ssi + variabili giunto)
x =
k
a
a +
k

+
k
d
d +
k

= (
n
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
l misure (lm 4n)
x =
_
_
x
1
.
.
.
x
l
_
_
=
_
_

1
.
.
.

l
_
_
=

Soluzione
= (

T

)
1

T
x

=
n
+
. . . nch e converge
stime pi ` u accurate dei parametri ssi
correzioni alle misure dei trasduttori
Inizializzazione
postura di riferimento (home)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
PROBLEMA CINEMATICO INVERSO
Cinematica diretta
q = T
q = x
Cinematica inversa
T = q
x = q
Complessit` a
soluzione analitica (in forma chiusa) ?
soluzioni multiple
innite soluzioni
non esistono soluzioni ammissibili
Intuizione
algebrica
geometrica
Tecniche numeriche
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Soluzione del manipolatore planare a tre bracci
Cinematica diretta nello spazio operativo
=
1
+
2
+
3
p
x
= a
1
c
1
+a
2
c
12
+a
3
c
123
p
y
= a
1
s
1
+a
2
s
12
+a
3
s
123
Soluzione algebrica
p
Wx
= p
x
a
3
c

= a
1
c
1
+a
2
c
12
p
Wy
= p
y
a
3
s

= a
1
s
1
+a
2
s
12
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
quadrando e sommando
c
2
=
p
2
Wx
+p
2
Wy
a
2
1
a
2
2
2a
1
a
2
s
2
=
_
1 c
2
2
due soluzioni!

2
= Atan2(s
2
, c
2
)
noto
2
il problema ` e lineare in s
1
e c
1
s
1
=
(a
1
+a
2
c
2
)p
Wy
a
2
s
2
p
Wx
p
2
Wx
+p
2
Wy
c
1
=
(a
1
+a
2
c
2
)p
Wx
+a
2
s
2
p
Wy
p
2
Wy
+p
2
Wy

1
= Atan2(s
1
, c
1
)

3
=
1

2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Soluzione geometrica
B
applicando il teorema dei coseni al triangolo OBW
c
2
=
p
2
Wx
+p
2
Wy
a
2
1
a
2
2
2a
1
a
2
.

2
= cos
1
(c
2
) due soluzioni: gomito alto e basso
calcolando langolo ausiliare
= Atan2(p
Wy
, p
Wx
)
e ancora per il teorema dei coseni
c

_
p
2
Wx
+p
2
Wy
= a
1
+a
2
c
2
= cos
1
_
_
p
2
Wx
+p
2
Wy
+a
2
1
a
2
2
2a
1
_
p
2
Wx
+p
2
Wy
_
_

1
=
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Soluzione di manipolatori con polso sferico
p
W
= p d
6
a
Soluzione disaccoppiata
calcolare la posizione del polso p
W
(q
1
, q
2
, q
3
)
risolvere la cinematica inversa per (q
1
, q
2
, q
3
)
calcolare R
0
3
(q
1
, q
2
, q
3
)
calcolare R
3
6
(
4
,
5
,
6
) = R
0
3
T
R
risolvere la cinematica inversa per lorientamento(
4
,
5
,
6
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Soluzione del manipolatore sferico
per separare le variabili da cui dipende p
W
esprimiamo le
sue componenti in terna 1
(A
0
1
)
1
T
0
3
= A
1
2
A
2
3
p
1
W
=
_
_
p
Wx
c
1
+p
Wy
s
1
p
Wz
p
Wx
s
1
+p
Wy
c
1
_
_
=
_
_
d
3
s
2
d
3
c
2
d
2
_
_
si osserva che nella terza equazione compare solo
lincognita
1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
c
1
=
1 t
2
1 +t
2
s
1
=
2t
1 +t
2
(d
2
+p
Wy
)t
2
+ 2p
Wx
t +d
2
p
Wy
= 0
esistono dunque due soluzioni

1
= 2Atan2
_
p
Wx

_
p
2
Wx
+p
2
Wy
d
2
2
, d
2
+p
Wy
_
quadrando e sommando le altre due equazioni si ricava d
3
d
3
=
_
(p
Wx
c
1
+p
Wy
s
1
)
2
+p
2
Wz
dalle stesse equazioni si evince che se d
3
= 0,
2
` e qualsiasi,
mentre se d
3
,= 0 si ha

2
= Atan2(p
Wx
c
1
+p
Wy
s
1
, p
Wz
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Soluzione del manipolatore antropomorfo
vedi pg. 71 con d
6
= 0, d
4
= a
3
,
3

3
+/2

1
= Atan2(p
Wy
, p
Wx
)
1
= +Atan2(p
Wy
, p
Wx
) (
2

2
)
c
3
=
p
2
Wx
+p
2
Wy
+p
2
Wz
a
2
2
a
2
3
2a
2
a
3
s
3
=
_
1 c
2
3

3
= Atan2(s
3
, c
3
)
s
2
=
(a
2
+a
3
c
3
)p
Wz
a
3
s
3
_
p
2
Wx
+p
2
Wy
p
2
Wx
+p
2
Wy
+p
2
Wz
c
2
=
(a
2
+a
3
c
3
)
_
p
2
Wx
+p
2
Wy
+a
3
s
3
p
Wz
p
2
Wx
+p
2
Wy
+p
2
Wz

2
= Atan2(s
2
, c
2
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Quattro congurazioni ammissibili
nel caso p
Wx
= 0, p
Wy
= 0 la variabile di giunto
1
pu` o
assumere qualsiasi valore
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Soluzione del polso sferico
R
3
6
=
_
_
n
3
x
s
3
x
a
3
x
n
3
y
s
3
y
a
3
y
n
3
z
s
3
z
a
3
z
_
_

4
= Atan2(a
3
y
, a
3
x
)

5
= Atan2
__
(a
3
x
)
2
+ (a
3
y
)
2
, a
3
z
_

6
= Atan2(s
3
z
, n
3
z
)

4
= Atan2(a
3
y
, a
3
x
)

5
= Atan2
_

_
(a
3
x
)
2
+ (a
3
y
)
2
, a
3
z
_

6
= Atan2(s
3
z
, n
3
z
)
naturalmente se conosco gli angoli di Eulero (spazio
operativo) la soluzione ` e immediata!
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CINEMATICA DIFFERENZIALE
relazioni tra velocit` a dei giunti e velocit` a dellorgano terminale
Jacobiano geometrico
Jacobiano analitico
Singolarit` a cinematiche
Analisi della ridondanza
Inversione della cinematica differenziale
Algoritmi per linversione cinematica
STATICA
relazioni tra forze allorgano terminale e coppie ai giunti
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
JACOBIANO GEOMETRICO
T(q) =
_
_
R(q) p(q)
0
T
1
_
_
Obiettivo
p = J
P
(q) q
= J
O
(q) q
v =
_
p

_
=
_
J
P
(q)
J
O
(q)
_
q = J(q) q
La linearit` a rispetto a q della relazione ` e ovvia per la velocit` a
lineare
p(q(t)) =
p(q)
q
q(t)
Verr` a invece dimostrata nel seguito per la velocit` a angolare
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Derivata di una matrice di rotazione
R(t)R
T
(t) = I

R(t)R
T
(t) +R(t)

R
T
(t) = O
Posto S(t) =

R(t)R
T
(t) (

R(t) = S(t)R(t))
S(t) +S
T
(t) = O S(t) ` e antisimmetrica
S(t) ` e completamente specicata da un vettore di soli 3 numeri
reali, sia esso (t) = (
x
(t)
y
(t)
z
(t) )
T
, detto velocit ` a
angolare
scegliendo (t) lungo lunico autovettore reale di S(t)
(nella direzione del nullo di S(t))
S(t) =
_
_
0
z

y

z
0
x

y

x
0
_
_
= S((t))
Interpretazione
corpo rigido in pura rotazione (orig. rifer. nel punto sso)
p(t) = R(t)p

p(t) =

R(t)p

= S((t))R(t)p

= (t) p(t)
propriet` a: ` e la stessa per tutti i punti del corpo rigido
propriet` a: S(R) = RS()R
T
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
R
z
() =
_
_
cos sin 0
sin cos 0
0 0 1
_
_
S(t) =
_
_
sin cos 0
cos sin 0
0 0 0
_
_
_
_
cos sin 0
sin cos 0
0 0 1
_
_
=
_
_
0 0
0 0
0 0 0
_
_
= S(), =
_
_
0
0

_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Regola di composizione delle velocit` a:
p
0
= o
0
1
+R
0
1
p
1
p
0
= o
0
1
+R
0
1
p
1
+

R
0
1
p
1
= o
0
1
+R
0
1
p
1
+S(
0
1
) R
0
1
p
1
. .
rr
0
1
=pp
0
oo
0
1
= o
0
1
+S(
0
1
)r
0
1
+R
0
1
p
1
= o
0
1
+
0
1
r
0
1
. .
contributo dovuto alle velocita

della terna mobile


+ R
0
1
p
1
. .
velocita

relativa
se p
1
` e costante (ad es. punto di un corpo rigido) allora
p
0
= o
0
1
+S(
0
1
)r
0
1
se anche o
0
1
` e costante (corpo rigido in pura rotazione)
p
0
= S(
0
1
)r
0
1
=
0
1
R
0
1
p
1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Velocit` a di un braccio
Velocit` a lineare
p
i
= p
i1
+R
i1
r
i1
i1,i
p
i
= p
i1
+
i1
R
i1
r
i1
i1,i
+R
i1
r
i1
i1,i
. .
vv
i1,i
= p
i1
+
i1
r
i1,i
+v
i1,i
la velocit` a v
i1,i
` e quella dellorigine della terna i rispetto
alla terna i 1, espressa in terna base (velocit` a relativa)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Velocit` a angolare
R
i
= R
i1
R
i1
i
derivando rispetto al tempo
S(
i
)R
i
= S(
i1
)R
i1
R
i1
i
+R
i1
S(
i1
i1,i
)R
i1
i
= S(
i1
)R
i
+R
i1
S(
i1
i1,i
)R
T
i1
R
i1
R
i1
i
= S(
i1
)R
i
+S(R
i1

i1
i1,i
)R
i
=
_
S(
i1
) +S(R
i1

i1
i1,i
)
_
R
i
= S(
i1
+R
i1

i1
i1,i
)R
i

i
=
i1
+R
i1

i1
i1,i
=
i1
+
i1,i
dunque a fronte di rotazioni successive, mentre le matrici di
rotazione si moltiplicano, le velocit` a angolari si sommano!
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE

i
=
i1
+
i1,i
p
i
= p
i1
+v
i1,i
+
i1
r
i1,i
Giunto i prismatico (r
i1
i1,i
= [ a
i
c

i
a
i
s

i
d
i
]
T
, con
i
costante)

i1,i
= 0
v
i1,i
= R
i1
r
i1
i1,i
= R
i1

d
i
z
0
=

d
i
z
i1

i
=
i1
p
i
= p
i1
+

d
i
z
i1
+
i
r
i1,i
Giunto i rotoidale (r
i1,i
ha modulo costante)

i1,i
=

i
z
i1
v
i1,i
= R
i1
r
i1
i1,i
=
i1,i
r
i1,i

i
=
i1
+

i
z
i1
p
i
= p
i1
+
i1,i
r
i1,i
+
i1
r
i1,i
= p
i1
+
i
r
i1,i
Le relazioni delle velocit` a angolari sono evidentemente lineari
nelle q
i
basta dimostrarlo per induzione grazie alla ricorsivit` a
ci ` o giustica la relazione lineare in q: = J
O
(q) q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Calcolo dello Jacobiano
J =
_
_

P1

Pn
. . .

O1

On
_
_
q
i

Pi
` e il contributo del giunto i alla velocit` a lineare dello.t.
q
i

Oi
` e il contributo del giunto i alla velocit` a angolare dello.t.
Velocit` a angolare
applicando ricorsivamente dal giunto n (o.t.) al giunto 1
(di base) la regola di composizione delle velocit` a angolari
risulta
=
n
=
n

i=1

i1,i
=
n

i=1
q
i

Oi
e tenendo conto che q
i
compare solo nellimo termine
della sommatoria
giunto i prismatico
q
i

Oi
= 0 =
Oi
= 0
giunto i rotoidale
q
i

Oi
=

i
z
i1
=
Oi
= z
i1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Velocit` a lineare (approccio geometrico)
giunto i prismatico
q
i

Pi
=

d
i
z
i1
=
Pi
= z
i1
giunto i rotoidale
q
i

Pi
=
i1,i
r
i1,n
=

i
z
i1
(p p
i1
)

Pi
= z
i1
(p p
i1
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Velocit` a lineare (approccio analitico)
applicando ricorsivamente dal giunto n (o.t.) al giunto 1 (di
base) la regola di composizione delle velocit` a lineari risulta
p = p
n
=
n

i=1
v
i1,i
+
i1
r
i1,i
=
n

i=1

Pi
q
i
per determinare
Pi
occorre isolare i coefcienti di q
i
osservando che q
i
compare in pi ` u termini della sommatoria!
giunto i prismatico
osserviamo che q
i
compare in un solo termine della
sommatoria e cio` e v
i1,i
v
i1,i
= q
i
z
i1

Pi
= z
i1
giunto i rotoidale
osserviamo che q
i
compare in tutti i termini della
sommatoria a partire dal imo, vediamo quali sono
i : v
i1,i
=
i1,i
r
i1,i
= q
i
z
i1
r
i1,i
i + 1 : () +
i
r
i,i+1
= () + (
i1
+ q
i
z
i1
) r
i,i+1
=
= () + q
i
z
i1
r
i,i+1
i + 2 : () +
i+1
r
i+1,i+2
= () + (
i
+
i,i+1
) r
i+1,i+2
=
= () + q
i
z
i1
r
i+1,i+2
.
.
.
raccogliendo a fattor comune q
i
si ottiene inne
q
i
z
i1

k=i
r
k1,k
= q
i
z
i1
(pp
i1
)
Pi
= z
i1
(pp
i1
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Colonna dello Jacobiano geometrico
_

Pi

Oi
_
=
_

_
_
z
i1
0
_
per un giunto prismatico
_
z
i1
(p p
i1
)
z
i1
_
per un giunto rotoidale
z
i1
= R
0
1
(q
1
) . . . R
i2
i1
(q
i1
)z
0
p = A
0
1
(q
1
) . . . A
n1
n
(q
n
) p
0
p
i1
= A
0
1
(q
1
) . . . A
i2
i1
(q
i1
) p
0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Rappresentazione in terna differente
_
p
t

t
_
=
_
R
t
O
O R
t
_ _
p

_
=
_
R
t
O
O R
t
_
J q
J
t
=
_
R
t
O
O R
t
_
J
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore planare a tre bracci
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
J(q) =
_
z
0
(p p
0
) z
1
(p p
1
) z
2
(p p
2
)
z
0
z
1
z
2
_
p
0
=
_
_
0
0
0
_
_
p
1
=
_
_
a
1
c
1
a
1
s
1
0
_
_
p
2
=
_
_
a
1
c
1
+a
2
c
12
a
1
s
1
+a
2
s
12
0
_
_
p =
_
_
a
1
c
1
+a
2
c
12
+a
3
c
123
a
1
s
1
+a
2
s
12
+a
3
s
123
0
_
_
z
0
= z
1
= z
2
=
_
_
0
0
1
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
J =
_

_
a
1
s
1
a
2
s
12
a
3
s
123
a
2
s
12
a
3
s
123
a
3
s
123
a
1
c
1
+a
2
c
12
+a
3
c
123
a
2
c
12
+a
3
c
123
a
3
c
123
0 0 0
0 0 0
0 0 0
1 1 1
_

_
J
P
=
_
a
1
s
1
a
2
s
12
a
3
s
123
a
2
s
12
a
3
s
123
a
3
s
123
a
1
c
1
+a
2
c
12
+a
3
c
123
a
2
c
12
+a
3
c
123
a
3
c
123
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore antropomorfo
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
J =
_
z
0
(p p
0
) z
1
(p p
1
) z
2
(p p
2
)
z
0
z
1
z
2
_
p
0
= p
1
=
_
_
0
0
0
_
_
p
2
=
_
_
a
2
c
1
c
2
a
2
s
1
c
2
a
2
s
2
_
_
p =
_
_
c
1
(a
2
c
2
+a
3
c
23
)
s
1
(a
2
c
2
+a
3
c
23
)
a
2
s
2
+a
3
s
23
_
_
z
0
=
_
_
0
0
1
_
_
z
1
= z
2
=
_
_
s
1
c
1
0
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
J =
_

_
s
1
(a
2
c
2
+a
3
c
23
) c
1
(a
2
s
2
+a
3
s
23
) a
3
c
1
s
23
c
1
(a
2
c
2
+a
3
c
23
) s
1
(a
2
s
2
+a
3
s
23
) a
3
s
1
s
23
0 a
2
c
2
+a
3
c
23
a
3
c
23
0 s
1
s
1
0 c
1
c
1
1 0 0
_

_
J
P
=
_
_
s
1
(a
2
c
2
+a
3
c
23
) c
1
(a
2
s
2
+a
3
s
23
) a
3
c
1
s
23
c
1
(a
2
c
2
+a
3
c
23
) s
1
(a
2
s
2
+a
3
s
23
) a
3
s
1
s
23
0 a
2
c
2
+a
3
c
23
a
3
c
23
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore di Stanford
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
J =
_
z
0
(p p
0
) z
1
(p p
1
) z
2
z
0
z
1
0
z
3
(p p
3
) z
4
(p p
4
) z
5
(p p
5
)
z
3
z
4
z
5
_
p
0
= p
1
=
_
_
0
0
0
_
_
p
3
= p
4
= p
5
=
_
_
c
1
s
2
d
3
s
1
d
2
s
1
s
2
d
3
+c
1
d
2
c
2
d
3
_
_
p =
_
_
c
1
s
2
d
3
s
1
d
2
+d
6
(c
1
c
2
c
4
s
5
+c
1
c
5
s
2
s
1
s
4
s
5
)
s
1
s
2
d
3
+c
1
d
2
+d
6
(c
1
s
4
s
5
+c
2
c
4
s
1
s
5
+c
5
s
1
s
2
)
c
2
d
3
+d
6
(c
2
c
5
c
4
s
2
s
5
)
_
_
z
0
=
_
_
0
0
1
_
_
z
1
=
_
_
s
1
c
1
0
_
_
z
2
= z
3
=
_
_
c
1
s
2
s
1
s
2
c
2
_
_
z
4
=
_
_
c
1
c
2
s
4
s
1
c
4
s
1
c
2
s
4
+c
1
c
4
s
2
s
4
_
_
z
5
=
_
_
c
1
c
2
c
4
s
5
s
1
s
4
s
5
+c
1
s
2
c
5
s
1
c
2
c
4
s
5
+c
1
s
4
s
5
+s
1
s
2
c
5
s
2
c
4
s
5
+c
2
c
5
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
SINGOLARIT
`
A CINEMATICHE
v = J(q) q
se J diminuisce di rango = singolarit ` a cinematiche
(a) perdita di mobilit` a
(b) possibili innite soluzioni al problema cinematico inverso
(c) velocit` a elevate nello spazio dei giunti (nellintorno di una
singolarit` a)
Classicazione
Singolarit` a ai conni dello spazio di lavoro raggiungibile
Singolarit` a allinterno dello spazio di lavoro raggiungibile
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore planare a due bracci
J =
_
a
1
s
1
a
2
s
12
a
2
s
12
a
1
c
1
+a
2
c
12
a
2
c
12
_
det(J) = a
1
a
2
s
2

2
= 0
2
=
[ (a
1
+a
2
)s
1
(a
1
+a
2
)c
1
]
T
parallelo a [ a
2
s
1
a
2
c
1
]
T
(componenti di velocit` a dellorgano terminale non
indipendenti perdita di mobilit` a)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Disaccoppiamento di singolarit` a
Per manipolatori con polso sferico
calcolo delle singolarit ` a della struttura portante
calcolo delle singolarit ` a del polso
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
J =
_
J
11
J
12
J
21
J
22
_
J
12
=
_
z
3
(p p
3
) z
4
(p p
4
) z
5
(p p
5
)

J
22
=
_
z
3
z
4
z
5

scegliendo d
6
= 0 p = p
W
p
W
p
i
paralleli a
z
i
, i = 3, 4, 5 (in realt` a p
4
= p
5
= p
W
)
J
12
=
_
0 0 0

det(J) = det(J
11
)det(J
22
)
det(J
11
) = 0 det(J
22
) = 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Singolarit` a di polso
J
22
= [ z
3
z
4
z
5
] perde rango se almeno due colonne sono
parallele
lunica possibilit` a ` e: z
3
parallelo a z
5

5
= 0
5
=
rotazioni uguali e opposte di
4
e
6
non producono alcuna
rotazione dellorgano terminale
non sono possibili rotazioni attorno allasse ortogonale al
piano formato da z
3
e z
4
si pu` o incontrare dovunque nello spazio di lavoro
raggiungibile
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Singolarit` a di struttura portante
Esempio: manipolatore antropomorfo
det(J
P
) = a
2
a
3
s
3
(a
2
c
2
+a
3
c
23
)
s
3
= 0 a
2
c
2
+a
3
c
23
= 0
Singolarit` a di gomito
s
3
= 0
3
= 0
3
=
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Singolarit` a di spalla
a
2
c
2
+a
3
c
23
= 0 p
x
= p
y
= 0
Le congurazioni delle singolarit` a di struttura portante sono
individuate da speciche zone dello spazio Cartesiano e quindi
sono evitabili in fase di pianicazione del percorso utensile
Le congurazioni delle singolarit` a di polso invece sono molto
pi ` u difcili da evitare in fase di pianicazione in quanto ` e
difcile prevedere in fase di pianicazione lallineamento di z
3
e z
5
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ANALISI DELLA RIDONDANZA
Cinematica differenziale
con J(q) indichiamo le sole righe dello jacobiano
geometrico necessarie a specicare il compito
v = J(q) q
se (J) = r (rango pieno)
dim
_
1(J)
_
= r dim
_
A(J)
_
= n r
in generale
dim
_
1(J)
_
+ dim
_
A(J)
_
= n
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Se il manipolatore ` e non ridondante n = r A(J) =
Se il manipolatore ` e ridondante A(J) ,=
un qualunque velocit` a di giunto q

e la velocit` a di giunto q
q = q

+P q
a
con q
a
arbitraria e dove
1(P) A(J)
producono la stessa velocit` a dellorgano terminale
verica:
J q = J q

+JP q
a
= J q

= v
q
a
genera moti interni della struttura nel senso che se si sceglie
q

A(J) lorgano terminale ` e fermo qualsiasi sia la velocit` a


di giunto q
a
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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
1
(q)v
q(t) =
_
t
0
q()d +q(0) =
_
t
0
J
1
(q())v()d +q(0)
regola di integrazione numerica (Eulero)
q(t
k+1
) = q(t
k
)+t q(t
k
) = q(t
k
)+tJ
1
(q(t
k
))v(t
k
)
Tutto ci ` o se J(q(t)) ` e quadrato (manipolatore non ridondante)
e di rango pieno t (assenza di singolarit ` a cinematiche lungo
la traiettoria)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatori ridondanti
Per una data congurazione q, trovare le soluzioni q che
soddisno
v = J q
e che minimizzino (W ` e simmetrica e d.p.)
g( q) =
1
2
q
T
W q
metodo dei moltiplicatori di Lagrange
g( q, ) =
1
2
q
T
W q +
T
(v J q)
_
g
q
_
T
= 0
_
g

_
T
= 0
soluzione ottima
q = W
1
J
T
(JW
1
J
T
)
1
v
se W = I
q = J

v
ove
J

= J
T
(JJ
T
)
1
` e la pseudo-inversa destra di J
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Utilizzo della ridondanza
v = J(q) q

= J(q) q, q = q

+P q
a
g

( q) =
1
2
( q
T
q
T
a
)( q q
a
)
come sopra . . .
g

( q, ) =
1
2
( q
T
q
T
a
)( q q
a
) +
T
(v J q)
soluzione ottima
q = J

v + (I J

J) q
a
il secondo termine ` e detto soluzione omogenea ma ` e chiaro
che non sar` a mai uguale a q a causa del primo termine che
consente di soddisfare sempre il vincolo primario v = J q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Caratterizzazione dei moti interni
q
a
= k
a
_
w(q)
q
_
T
q
a
si muove nella direzione positiva (k
a
> 0) del
gradiente per cui tende a massimizzare w(q) (detto obiettivo
secondario)
misura di manipolabilit ` a
w(q) =
_
det
_
J(q)J
T
(q)
_
distanza dai ne-corsa dei giunti
w(q) =
1
2n
n

i=1
_
q
i
q
i
q
iM
q
im
_
2
distanza da un ostacolo
w(q) = min
p,o
|p(q) o|
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Singolarit` a cinematiche
Le soluzioni precedenti valgono solo se J ` e di rango pieno
Se J non ` e di rango pieno (singolarit` a)
se v 1(J) = soluzione q estraendo tutte le equa-
zioni linearmente indipendenti (traiettoria sicamente
eseguibile)
se v / 1(J) = il sistema non ` e risolvibile
(traiettoria non eseguibile)
Inversione nellintorno di singolarit` a
det(J) piccolo = q elevate
inversa a minimi quadrati smorzata
J

= J
T
(JJ
T
+k
2
I)
1
ove q minimizza
g

( q) =
1
2
|v J q|
2
+
1
2
k
2
| q|
2
il fattore di smorzamento" pu` o essere anche scelto
dipendente dalla congurazione k(q) in modo da renderlo
nullo al di fuori dellintorno della congurazione singolare
(se per` o ` e nota)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
JACOBIANO ANALITICO
p = p(q)
= (q)
x = k(q)
p =
p
q
q = J
P
(q) q

=

q
q = J

(q) q
x =
_
p

_
=
_
J
P
(q)
J

(q)
_
q
= J
A
(q) q
J
A
(q) =
k(q)
q
` e chiaro che J
P
` e lo stesso dello jacobiano geometrico
invece J

non pu` o essere uguale a J


O
in quanto la velocit` a
angolare non ` e affatto la derivata del vettore degli angoli
di Eulero
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Velocit` a di rotazione in terna corrente di angoli di Eulero ZYZ
la matrice di rotazione complessiva ` e
R() = R
z
()R
y
()R
z
()
per la legge di composizione delle velocit` a angolari
= z +

R
z
()y

+

R
z
()R
y
()z

per effetto di : [
x

y

z
]
T
= [ 0 0 1 ]
T
per effetto di

: [
x

y

z
]
T
=

[ s

0 ]
T
per effetto di

: [
x

y

z
]
T
=

[ c

]
T
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Composizione di velocit` a di rotazione elementari
=
_
_
0 s

0 c

1 0 c

_
_
= T()

Singolarit` a di rappresentazione
si osservi che det(T()) = s

la matrice di trasformazione T ` e singolare per = 0,


in corrispondenza di certi orientamenti dellorgano
terminale, data una qualsiasi velocit` a angolare non ` e
possibile denire la derivata degli angoli di Eulero
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Signicato sico di
= [ /2 0 0 ]
T
0 t 1 = [ 0 /2 0 ]
T
0 t 1
= [ 0 /2 0 ]
T
1 < t 2 = [ /2 0 0 ]
T
1 < t 2
mentre lintegrale di entrambe le velocit` a angolari ` e lo stesso
_
2
0
dt = [ /2 /2 0 ]
T
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Relazione tra Jacobiano analitico e
Jacobiano geometrico
v =
_
I O
O T()
_
x = T
A
() x
J = T
A
()J
A
In assenza di singolarit` a di rappresentazione, la relazione
J
A
= T
1
A
()J =
_
I O
O T
1
()
_
J
pu` o essere usata per determinare lo jacobiano analitico spesso
non calcolabile in forma chiusa (cos` come la cinematica diretta
k(q)!)
Jacobiano geometrico
grandezze di signicato sico
Jacobiano analitico
grandezze differenziali di variabili nello spazio operativo
Per particolari strutture di manipolazione sono sostanzialmente
equivalenti
rotazioni dellorgano terminale attorno ad assi ssi
(manipolatore planare a tre bracci)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ALGORITMI PER LINVERSIONE
CINEMATICA
Inversione cinematica
q(t
k+1
) = q(t
k
) + J
1
(q(t
k
))v(t
k
)t
fenomeni di deriva della soluzione
Soluzione algoritmica
errore nello spazio operativo
e = x
d
x
e = x
d
x
= x
d
J
A
(q) q
trovare q = q(e): e 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
(Pseudo-)inversa dello Jacobiano
Linearizzazione della dinamica di errore
q = J
1
A
(q)( x
d
+Ke)

e +Ke = 0
Per un manipolatore ridondante
q = J

A
( x
d
+Ke) + (I J

A
J
A
) q
a
questo nellipotesi che il manipolatore non sia in singolarit` a
cinematica
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Trasposta dello Jacobiano
q = q(e) senza linearizzare la dinamica di errore
Metodo di Lyapunov
V (e) =
1
2
e
T
Ke
ove
V (e) > 0 e ,= 0 V (0) = 0

V (e) = e
T
K x
d
e
T
K x
= e
T
K x
d
e
T
KJ
A
(q) q
la scelta
q = J
T
A
(q)Ke
comporta che

V (e) = e
T
K x
d
e
T
KJ
A
(q)J
T
A
(q)Ke
se x
d
= 0 =

V < 0 con V > 0 (asintotica stabilit ` a)
se A(J
T
A
) ,= =

V = 0 se Ke A(J
T
A
)
q = 0 con e ,= 0 (stallo?)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Se x
d
,= 0
e(t) limitato (conviene aumentare la norma di K)
e() 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
J
T
P
=
_
_
0 0 0
c
1
(a
2
s
2
+a
3
s
23
) s
1
(a
2
s
2
+a
3
s
23
) 0
a
3
c
1
s
23
a
3
s
1
s
23
a
3
c
23
_
_
nullo di J
T
P

x
=
1
tan
1

z
= 0
se il percorso ha componenti solo nel nullo di J
T
P
non ` e
sicamente ammissibile e lalgoritmo va in stallo
se il percorso ha componenti anche fuori del nullo di J
T
P
` e sicamente ammissibile e lalgoritmo converge in quanto
Ke , A(J
T
P
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Errore di orientamento
Errore di posizione
e
P
= p
d
p(q)
e
P
= p
d
p
Angoli di Eulero
e
O
=
d
(q)
e
O
=

q = J
1
A
(q)
_
p
d
+K
P
e
P

d
+K
O
e
O
_
agevole per assegnare landamento temporale
d
(t)
richiede comunque il passaggio attraverso R= [ n s a]
J
A
` e ben denito solo non in singolarit` a di rappresentazione
Manipolatore con polso sferico
calcolare q
P
= R
W
calcolare R
T
W
R
d
= q
O
(angoli di Eulero ZYZ)
Manipolatori senza polso sferico
pu` o essere pi ` u conveniente luso delloJacobiano geometrico
occorre riformulare lerrore di orientamento
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Asse/angolo
R(, r) = R
d
R
T
errore di orientamento
e
O
= r sin
=
1
2
(n n
d
+s s
d
+a a
d
)
e
O
=
T

ove
=
1
2
_
S(n
d
)S(n) +S(s
d
)S(s) +S(a
d
)S(a)
_
e =
_
e
P
e
O
_
=
_
p
d
J
P
(q) q

d
J
O
(q) q
_
=
_
p
d

d
_

_
I O
O
_
J q
q = J
1
(q)
_
p
d
+K
P
e
P

1
_

d
+K
O
e
O
_
_
e +Ke = 0
luso dello jacobiano geometrico non soffre delle singolarit` a
di rappresentazione
algoritmo computazionalmente oneroso
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Quaternione unitario
Q = Q
d
Q
1
errore di orientamento
e
O
= = (q)
d

d
(q) S(
d
)(q)
q = J
1
(q)
_
p
d
+K
P
e
P

d
+K
O
e
O
_

d
+K
O
e
O
= 0
propagazione del quaternione
=
1
2

=
1
2
(I S())
studio della stabilit` a
V = (
d
)
2
+ (
d
)
T
(
d
)

V = e
T
O
K
O
e
O
Vantaggi
uso dello jacobiano geometrico
computazionalmente efciente
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Confronto tra gli algoritmi per linversione
cinematica
prima derivare lalgoritmo del II ordine...
Manipolatore planare a tre bracci
x = k(q)
_
_
p
x
p
y

_
_
=
_
_
a
1
c
1
+a
2
c
12
+a
3
c
123
a
1
s
1
+a
2
s
12
+a
3
s
123

1
+
2
+
3
_
_
a
1
= a
2
= a
3
= 0.5 m
J
A
=
_
_
a
1
s
1
a
2
s
12
a
3
s
123
a
2
s
12
a
3
s
123
a
3
s
123
a
1
c
1
+a
2
c
12
+a
3
c
123
a
2
c
12
+a
3
c
123
a
3
c
123
1 1 1
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
q
i
= [ /2 /2 ]
T
rad

p
di
= [ 0 0.5 ]
T
m = 0 rad
traiettoria desiderata
p
d
(t) =
_
0.25(1 cos t)
0.25(2 + sint)
_
0 t 4

d
(t) = sin

24
t 0 t 4
Simulazione in MATLAB con integrazione numerica di Eulero
q(t
k+1
) = q(t
k
) + q(t
k
)t
e t = 1 ms
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
q = J
1
A
(q) x
0 1 2 3 4 5
0
0.5
1
1.5
2
x 10
3
[s]
[
m
]
norma errore pos
0 1 2 3 4 5
1
0.8
0.6
0.4
0.2
0
x 10
5
[s]
[
r
a
d
]
errore orien
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
q = J
1
A
(q)( x
d
+Ke) K = diag500, 500, 100
0 1 2 3 4 5
5
0
5
[s]
[
r
a
d
]
pos giunti
1
2
3
0 1 2 3 4 5
10
5
0
5
10
[s]
[
r
a
d
/
s
]
vel giunti
1
2
3
0 1 2 3 4 5
0
0.2
0.4
0.6
0.8
1
x 10
5
[s]
[
m
]
norma errore pos
0 1 2 3 4 5
5
4
3
2
1
0
x 10
8
[s]
[
r
a
d
]
errore orien
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
libero (r = 2, n = 3)
q = J

P
( p
d
+K
P
e
P
) K
P
= diag500, 500
0 1 2 3 4 5
0
1
2
3
4
5
x 10
6
[s]
[
m
]
norma errore pos
0 1 2 3 4 5
1
0.5
0
0.5
[s]
[
r
a
d
]
orien
q = J
T
P
(q)K
P
e
P
K
P
= diag500, 500
0 1 2 3 4 5
0
0.002
0.004
0.006
0.008
0.01
[s]
[
m
]
norma errore pos
0 1 2 3 4 5
1
0.5
0
0.5
[s]
[
r
a
d
]
orien
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
q = J

P
( p
d
+K
P
e
P
) + (I J

P
J
P
) q
a
K
P
= diag500, 500
misura di manipolabilit` a
w(
2
,
3
) =
1
2
(s
2
2
+s
2
3
)
q
a
= k
a
_
w(q)
q
_
T
k
a
= 50
0 1 2 3 4 5
5
0
5
[s]
[
r
a
d
]
pos giunti
1
2
3
0 1 2 3 4 5
5
0
5
[s]
[
r
a
d
/
s
]
pos giunti
1
2
3
0 1 2 3 4 5
0
1
2
3
4
5
x 10
6
[s]
[
m
]
norma errore pos
0 1 2 3 4 5
0.85
0.9
0.95
1
[s]
[
r
a
d
]
manip
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
distanza dai ne-corsa dei giunti
w(q) =
1
6
3

i=1
_
q
i
q
i
q
iM
q
im
_
2
2 q
1
2 /2 q
2
/2 3/2 q
3
/2
q
a
= k
a
_
w(q)
q
_
T
k
a
= 250
0 1 2 3 4 5
0
0.5
1
1.5
2
x 10
4
[s]
[
m
]
norma errore pos
0 1 2 3 4 5
6
4
2
0
2
4
6
[s]
[
r
a
d
]
pos giunto 1
0 1 2 3 4 5
6
4
2
0
2
4
6
[s]
[
r
a
d
]
pos giunto 2
0 1 2 3 4 5
5
0
5
[s]
[
r
a
d
]
pos giunto 3
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
STATICA
Relazione tra forze e momenti (forze) allorgano terminale
e forze e/o coppie (coppie) ai giunti con il manipolatore in
congurazione di equilibrio
lavoro elementare compiuto dalle coppie
dW

=
T
dq
lavoro elementare compiuto dalle forze
dW

= f
T
dp +
T
dt
= f
T
J
P
(q)dq +
T
J
O
(q)dq
=
T
J(q)dq
spostamenti elementari spostamenti virtuali (vincoli
olonomi e stazionari)
W

=
T
q
W

=
T
J(q)q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Principio dei lavori virtuali
il manipolatore ` e in equilibrio statico se e solo se
W

= W

= J
T
(q)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Dualit` a cineto-statica
A(J) 1

(J
T
) 1(J) A

(J
T
)
forze A(J
T
) interamente assorbite dalla struttura
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Interpretazione sica dello schema con la trasposta dello
Jacobiano
dinamica ideale = q
forza elastica Ke che tira lorgano terminale verso la
postura desiderata nello spazio operativo
ha effetto solo se Ke / A(J
T
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Trasformazione di velocit` a e forze
p
2
= p
1
+r
12
p
2
= p
1
+ r
12
= p
1
+
1
r
12
_
p
2

2
_
=
_
I S(r
12
)
O I
_ _
p
1

1
_
r
12
= R
1
r
1
12
p
1
= R
1
p
1
1
p
2
= R
2
p
2
2
= R
1
R
1
2
p
2
2

1
= R
1

1
1

2
= R
2

2
2
= R
1
R
1
2

2
2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
dunque la relazione che lega la velocit` a del corpo rigido
rispetto alla terna base ma espressa nella terna solidale 1
alla stessa velocit` a ma espressa nella terna solidale 2 ` e:
_
p
2
2

2
2
_
=
_
R
2
1
R
2
1
S(r
1
12
)
O R
2
1
_ _
p
1
1

1
1
_
v
2
2
= J
2
1
v
1
1
in virt ` u della dualit` a cineto-statica, si pu` o ricavare la
trasformazione di forze da una terna ad unaltra:

1
1
= J
2
1
T

2
2
_
f
1
1

1
1
_
=
_
R
1
2
O
S(r
1
12
)R
1
2
R
1
2
_ _
f
2
2

2
2
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ELLISSOIDI DI MANIPOLABILIT
`
A
Denizione di indici che caratterizzino le capacit` a del
manipolatore
Ellissoide di manipolabilit ` a in velocit ` a
insieme delle velocit` a ai giunti a norma costante
q
T
q = 1
manipolatore ridondante
q = J

(q)v

v
T
_
J(q)J
T
(q)
_
1
v = 1
Assi
autovettori u
i
di JJ
T
= direzioni
valori singolari
i
=
_

i
(JJ
T
) = dimensioni
Volume
proporzionale a
w(q) =
_
det
_
J(q)J
T
(q)
_
se J ` e quadrato
w(q) = [det
_
J(q)
_
[
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore planare a due bracci
Misura di manipolabilit` a
w = [det(J)[ = a
1
a
2
[s
2
[
max per
2
= /2
max per a
1
= a
2
(a parit` a di estensione a
1
+a
2
)
nulla in singolarit` a (
2
= 0, )
Si noti che il valore assoluto di w dipende anche dai parametri
cinematici, quindi non rappresenta in assoluto un buon indice
di distanza dalla singolarit` a
con a
i
= 0.01 m e a
i
= 1 m otterrei valori molto diversi di
w
si pu` o scegliere come indice il rapporto tra min e max valore
singolare
difcile da calcolare in forma simbolica come funzione di
q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Ellissi di manipolabilit` a in velocit` a
0 0.5 1 1.5 2
1
0.5
0
0.5
1
[m]
[
m
]

Valori singolari al variare della posizione dellorgano terminale
lungo x
0 0.5 1 1.5 2
0
0.5
1
1.5
2
2.5
min
max
[m]
[
m
]

ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Ellissoide di manipolabilit ` a in forza
insieme delle coppie ai giunti a norma costante

T
= 1

T
_
J(q)J
T
(q)
_
= 1
Dualit` a cineto-statica
una direzione lungo la quale si ha elevata manipolabilit` a
in velocit` a ` e una direzione lungo la quale si ha scarsa
manipolabilit` a in forza, e viceversa
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore planare a due bracci
Ellissi di manipolabilit` a in velocit` a
0 0.5 1 1.5 2
1
0.5
0
0.5
1
[m]
[
m
]

Ellissi di manipolabilit` a in forza
0 0.5 1 1.5 2
1
0.5
0
0.5
1
[m]
[
m
]

ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore trasformatore meccanico di velocit` a e forze
dallo spazio dei giunti allo spazio operativo
rapporto di trasformazione lungo una direzione per
lellissoide in forza
(q) =
_
u
T
J(q)J
T
(q)u
_
1/2
rapporto di trasformazione lungo una direzione per
lellissoide in velocit` a
(q) =
_
u
T
_
J(q)J
T
(q)
_
1
u
_
1/2
utilizzazione di gradi di mobilit` a ridondanti
per compiti di controllo ` e opportuno avere un rapporto di
trasformazione basso per essere pi sensibili allerrore
per compiti di attuazione ` e opportuno avere un rapporto di
trasformazione alto per generare uscite elevate con ingressi
bassi
per dualit` a, una direzione ottimale per il controllo di una
velocit` a ` e una direzione ottimale per lattuazione di una
forza
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Compatibilit` a della struttura ad eseguire un compito assegnato
lungo una direzione
compito di scrittura su supercie orizzontale
forza
vel
piano di scrittura
compito di lancio di un peso in direzione orizzontale
forza
vel
direzione di lancio
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
DINAMICA
equazioni del moto del manipolatore in funzione delle forze e
momenti agenti su di esso
Formulazione di Lagrange
Propriet` a notevoli del modello dinamico
Identicazione dei parametri dinamici
Dinamica diretta e dinamica inversa
Modello dinamico nello spazio operativo
Ellissoide di manipolabilit` a dinamica
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
FORMULAZIONE DI LAGRANGE
Lagrangiana (energia cinetica) (energia potenziale)
L = T |
Equazioni di Lagrange
d
dt
L

i
=
i
i = 1, . . . , n
coordinate generalizzate
_
_

1
.
.
.

n
_
_
= q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
energia cinetica
T =
1
2
I

2
+
1
2
I
m
k
2
r

2
energia potenziale
| = mg(1 cos )
lagrangiana
L =
1
2
I

2
+
1
2
I
m
k
2
r

2
mg(1 cos )
equazione del moto
(I +I
m
k
2
r
)

+mg sin =
= F

ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE


Energia cinetica
Contributo bracci + attuatori
T =
n

i=1
(T

i
+T
m
i
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Braccio i
T

i
=
1
2
_
V

i
p

i
T
p

i
dV
massa
m

i
=
_
V

i
dV
baricentro
p

i
=
1
m

i
_
V

i
p

i
dV
velocit` a lineare della particella elementare
p

i
= p

i
+
i
r
i
= p

i
+S(
i
)r
i
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
sostituendo si ha
T

i
=
1
2
_
V

i
( p
T

i
+r
T
i
S
T
(
i
))( p

i
+S(
i
)r
i
)dV
Traslazionale
1
2
_
V

i
p
T

i
p

i
dV =
1
2
m

i
p
T

i
p

i
Mutuo
2
_
1
2
_
V

i
p
T

i
S(
i
)r
i
dV
_
=
2
_
1
2
p
T

i
S(
i
)
_
V

i
(p

i
p

i
)dV
_
= 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Rotazionale
1
2
_
V

i
r
T
i
S
T
(
i
)S(
i
)r
i
dV =
1
2

T
i
_
_
V

i
S
T
(r
i
)S(r
i
)dV
_

i
=
1
2

T
i
I

i
tensore di inerzia
I

i
=
_

_
_
(r
2
iy
+r
2
iz
)dV
_
r
ix
r
iy
dV
_
r
ix
r
iz
dV

_
(r
2
ix
+r
2
iz
)dV
_
r
iy
r
iz
dV

_
(r
2
ix
+r
2
iy
)dV
_

_
=
_

_
I

i
xx
I

i
xy
I

i
xz
I

i
yy
I

i
yz
I

i
zz
_

_
si noti come il tensore di inerzia in terna base dipenda dalla
congurazione del robot, mentre espresso in terna solidale
` e costante
I

i
= R
i
I
i

i
R
T
i
I
i

i
=
_
V

i
S
T
(r
i
i
)S(r
i
i
)dV
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Energia cinetica del braccio i
T

i
=
1
2
m

i
p
T

i
p

i
+
1
2

T
i
R
i
I
i

i
R
T
i

i
=
1
2
m

i
q
T
J
(
i
)T
P
J
(
i
)
P
q +
1
2
q
T
J
(
i
)T
O
R
i
I
i

i
R
T
i
J
(
i
)
O
q
velocit` a lineare
p

i
=
(
i
)
P1
q
1
+. . . +
(
i
)
Pi
q
i
= J
(
i
)
P
q
J
(
i
)
P
=
_

(
i
)
P1
. . .
(
i
)
Pi
0 . . . 0

(
i
)
Pj
=
_
z
j1
per un giunto prismatico
z
j1
(p

i
p
j1
) per un giunto rotoidale
velocit` a angolare (` e la stessa in tutti i punti del braccio!)

i
=
(
i
)
O1
q
1
+. . . +
(
i
)
Oi
q
i
= J
(
i
)
O
q
J
(
i
)
O
=
_

(
i
)
O1
. . .
(
i
)
Oi
0 . . . 0

(
i
)
Oj
=
_
0 per un giunto prismatico
z
j1
per un giunto rotoidale
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Motore i (elettrico rotante) ` e assunto solidale al braccio i 1
trasmissione rigida

m
i
=
i1
+k
ri
q
i
z
m
i
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Energia cinetica del rotore i
T
m
i
=
1
2
m
m
i
p
T
m
i
p
m
i
+
1
2

T
m
i
I
m
i

m
i
=
1
2
m
m
i
q
T
J
(m
i
)T
P
J
(m
i
)
P
q +
1
2
q
T
J
(m
i
)T
O
R
m
i
I
m
i
m
i
R
T
m
i
J
(m
i
)
O
q
velocit` a lineare
J
(m
i
)
P
=
_

(m
i
)
P1
. . .
(m
i
)
P,i1
0 . . . 0

(m
i
)
Pj
=
_
z
j1
per un giunto prismatico
z
j1
(p
m
i
p
j1
) per un giunto rotoidale
velocit` a angolare
J
(m
i
)
O
=
_

(m
i
)
O1
. . .
(m
i
)
O,i1

(m
i
)
Oi
0 . . . 0

(m
i
)
Oj
=
_

(
i
)
Oj
j = 1, . . . , i 1
k
ri
z
m
i
j = i
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Energia cinetica totale
T =
1
2
n

i=1
n

j=1
b
ij
(q) q
i
q
j
=
1
2
q
T
B(q) q
Matrice di inerzia
B(q) =
n

i=1
_
m

i
J
(
i
)T
P
J
(
i
)
P
+J
(
i
)T
O
R
i
I
i

i
R
T
i
J
(
i
)
O
+m
m
i
J
(m
i
)T
P
J
(m
i
)
P
+J
(m
i
)T
O
R
m
i
I
m
i
m
i
R
T
m
i
J
(m
i
)
O
_
simmetrica
denita positiva
dipendente dalla congurazione (in generale)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Energia potenziale
| =
n

i=1
(|

i
+|
m
i
)
Nellipotesi di bracci e giunti (trasmissioni) rigidi lunica
energia potenziale ` e quella gravitazionale
Braccio i
|

i
=
_
V

i
g
T
0
p

i
dV = m

i
g
T
0
p

i
g
0
` e il vettore accelerazione di gravit` a espresso in terna base
Rotore i
|
m
i
= m
m
i
g
T
0
p
m
i
Energia potenziale totale
| =
n

i=1
(m

i
g
T
0
p

i
+m
m
i
g
T
0
p
m
i
)
non dipende dalle q ma solo dalle q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Equazioni del moto
L(q, q) = T (q, q) |(q)
=
1
2
n

i=1
n

j=1
b
ij
(q) q
i
q
j
+
n

i=1
_
m

i
g
T
0
p

i
(q)+m
m
i
g
T
0
p
m
i
(q)
_
Equazioni di Lagrange
d
dt
_
L
q
i
_
=
d
dt
_
T
q
i
_
=
n

j=1
b
ij
(q) q
j
+
n

j=1
db
ij
(q)
dt
q
j
=
n

j=1
b
ij
(q) q
j
+
n

j=1
n

k=1
b
ij
(q)
q
k
q
k
q
j
T
q
i
=
1
2
n

j=1
n

k=1
b
jk
(q)
q
i
q
k
q
j
(visto che b
jk
= b
kj
)
|
q
i
=
n

j=1
_
m

j
g
T
0
p

j
q
i
+m
m
j
g
T
0
p
m
j
q
i
_
=
n

j=1
_
m

j
g
T
0

(
j
)
Pi
(q) +m
m
j
g
T
0

(m
j
)
Pi
(q)
_
= g
i
(q)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Equazioni del moto
n

j=1
b
ij
(q) q
j
+
n

j=1
n

k=1
h
ijk
(q) q
k
q
j
+g
i
(q) =
i
i = 1, . . . , n
ove
h
ijk
=
b
ij
q
k

1
2
b
jk
q
i
termini in accelerazione:
il coefciente b
ii
rappresenta il momento di inerzia visto
allasse del giunto i, nella congurazione corrente del
manipolatore, quando gli altri giunti sono bloccati
il coefciente b
ij
tiene conto delleffetto dellaccelerazione
del giunto j sul giunto i
termini quadratici in velocit ` a:
il termine h
ijj
q
2
j
rappresenta leffetto centrifugo indotto al
giunto i dalla velocit` a del giunto j; si noti che h
iii
= 0,
poich e b
ii
/q
i
= 0
il termine h
ijk
q
j
q
k
rappresenta leffetto di Coriolis indotto
al giunto i dalle velocit` a dei giunti j e k
termini dipendenti solo dalla congurazione:
il termine g
i
rappresenta la coppia generata allasse del
giunto i nella congurazione corrente del manipolatore per
effetto della gravit` a
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Forze non conservative
coppie di attuazione
coppie di attrito viscoso F
v
q
coppie di attrito statico f
s
(q, q) F
s
sgn ( q)
coppie di bilanciamento di forze di contatto J
T
(q)h
Modello dinamico nello spazio dei giunti
B(q) q +C(q, q) q +F
v
q +f
s
( q) +g(q) = J
T
(q)h
n

j=1
c
ij
q
j
=
n

j=1
n

k=1
h
ijk
q
k
q
j
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
PROPRIET
`
A NOTEVOLI DEL MODELLO
DINAMICO
Anti-simmetria della matrice

B 2C
n

j=1
c
ij
q
j
=
n

j=1
n

k=1
h
ijk
q
k
q
j
=
n

j=1
n

k=1
_
b
ij
q
k

1
2
b
jk
q
i
_
q
k
q
j
=
1
2
n

j=1
n

k=1
b
ij
q
k
q
k
q
j
+
1
2
n

j=1
n

k=1
_
b
ik
q
j

b
jk
q
i
_
q
k
q
j
si noti lo scambio degli indici j e k di sommatoria sfruttando
il fatto che b
kj
= b
jk
Elementi di C
c
ij
=
n

k=1
c
ijk
q
k
simboli di Christoffel del primo tipo
c
ijk
=
1
2
_
b
ij
q
k
+
b
ik
q
j

b
jk
q
i
_
c
ijk
= c
ikj
perch e b
ij
= b
ji
, i ,= j
Propriet` a notevole
N(q, q) =

B(q) 2C(q, q) = N
T
(q, q)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Elementi di C
c
ij
=
1
2
n

k=1
b
ij
q
k
q
k
+
1
2
n

k=1
_
b
ik
q
j

b
jk
q
i
_
q
k
=
1
2

b
ij
+
1
2
n

k=1
_
b
ik
q
j

b
jk
q
i
_
q
k
Elementi di N
n
ij
=

b
ij
2c
ij
=
n

k=1
_
b
jk
q
i

b
ik
q
j
_
q
k
= n
ji

w
T
N(q, q)w = 0 w
se w = q:
q
T
N(q, q) q = 0 C
(no boldface!)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Principio di conservazione dellenergia (Hamilton)
Derivata totale energia cinetica = potenza forza esterne
1
2
d
dt
_
q
T
B(q) q
_
= q
T
_
F
v
q f
s
( q) g(q) J
T
(q)h
_
1
2
d
dt
_
q
T
B(q) q
_
=
1
2
q
T

B(q) q + q
T
B(q) q
=
1
2
q
T
_

B(q) 2C(q, q)
_
q
+ q
T
_
F
v
q f
s
( q) g(q) J
T
(q)h
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Linearit` a nei parametri dinamici
Insieme braccio i e rotore i + 1
T
i
= T

i
+T
m
i+1
T

i
=
1
2
m
i
p
T

i
p

i
+
1
2

T
i
I

i
T
m
i+1
=
1
2
m
m
i+1
p
T
m
i+1
p
m
i+1
+
1
2

T
m
i+1
I
m
i+1

m
i+1
con riferimento al baricentro p
C
i
dellinsieme braccio
rotore
p

i
= p
C
i
+
i
r
C
i
,
i
p
m
i+1
= p
C
i
+
i
r
C
i
,m
i+1
r
C
i
,
i
= p

i
p
C
i
r
C
i
,m
i+1
= p
m
i+1
p
C
i
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Braccio
T

i
=
1
2
m
i
p
T
C
i
p
C
i
+ p
T
C
i
S(
i
)m

i
r
C
i
,
i
+
1
2
m

T
i
S
T
(r
C
i
,
i
)S(r
C
i
,
i
)
i
+
1
2

T
i
I

i
=
1
2
m
i
p
T
C
i
p
C
i
+ p
T
C
i
S(
i
)m

i
r
C
i
,
i
+
1
2

T
i

I

i
per il teorema di Steiner

i
= I

i
+m

i
S
T
(r
C
i
,
i
)S(r
C
i
,
i
)
Rotore
T
m
i+1
=
1
2
m
m
i+1
p
T
C
i
p
C
i
+ p
T
C
i
S(
i
)m
m
i+1
r
C
i
,m
i+1
+
1
2

T
i

I
m
i+1

i
+k
r,i+1
q
i+1
z
T
m
i+1
I
m
i+1

i
+
1
2
k
2
r,i+1
q
2
i+1
z
T
m
i+1
I
m
i+1
z
m
i+1
per il teorema di Steiner

I
m
i+1
= I
m
i+1
+m
m
i+1
S
T
(r
C
i
,m
i+1
)S(r
C
i
,m
i+1
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Energia cinetica
T
i
=
1
2
m
i
p
T
C
i
p
C
i
+
1
2

T
i

I
i

i
+k
r,i+1
q
i+1
z
T
m
i+1
I
m
i+1

i
+
1
2
k
2
r,i+1
q
2
i+1
z
T
m
i+1
I
m
i+1
z
m
i+1
rotore con distribuzione di massa simmetrica intorno al suo
asse di rotazione
I
m
i
m
i
=
_
_
I
m
i
xx
0 0
0 I
m
i
xx
0
0 0 I
m
i
zz
_
_

I
m
i+1
z
m
i+1
= R
m
i+1
I
m
i+1
m
i+1
R
T
m
i+1
z
m
i+1
= I
m
i+1
z
m
i+1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Quantit` a riferite alle terne solidali ai bracci
T
i
=
1
2
m
i
p
iT
C
i
p
i
C
i
+
1
2

iT
i

I
i
i

i
i
+k
r,i+1
q
i+1
I
m
i+1
z
iT
m
i+1

i
i
+
1
2
k
2
r,i+1
q
2
i+1
I
m
i+1
=
1
2
m
i
p
iT
i
p
i
i
+ p
iT
i
S(
i
i
)m
i
r
i
i,C
i
+
1
2

iT
i

I
i
i

i
i
+k
r,i+1
q
i+1
I
m
i+1
z
iT
m
i+1

i
i
+
1
2
k
2
r,i+1
q
2
i+1
I
m
i+1
per il teorema di Steiner

I
i
i
=

I
i
i
+m
i
S
T
(r
i
i,C
i
)S(r
i
i,C
i
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Energia cinetica lineare rispetto a:
massa m
i
momento primo di inerzia
m
i
r
i
i,C
i
=
_
_
m
i

C
i
x
m
i

C
i
y
m
i

C
i
z
_
_
tensore di inerzia

I
i
i
=
_

I
ixx
+m
i
(
2
C
i
y
+
2
C
i
z
)

I
ixy
m
i

C
i
x

C
i
y


I
iyy
+m
i
(
2
C
i
z
+
2
C
i
x
)

I
ixz
m
i

C
i
x

C
i
z

I
iyz
m
i

C
i
y

C
i
z

I
izz
+m
i
(
2
C
i
x
+
2
C
i
y
)
_

_
=
_

I
ixx

I
ixy

I
ixz


I
iyy

I
iyz


I
izz
_

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Energia potenziale
|
i
= m
i
g
iT
0
p
i
C
i
= g
iT
0
(m
i
p
i
i
+m
i
r
i
i,C
i
)
lineare rispetto a:
massa m
i
momento primo di inerzia
m
i
r
i
i,C
i
=
_
_
m
i

C
i
x
m
i

C
i
y
m
i

C
i
z
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Lagrangiana
L =
n

i=1
(
T
T i

T
Ui
)
i
vettore (11 1) di parametri dinamici

i
= [ m
i
m
i

C
i
x
m
i

C
i
y
m
i

C
i
z

I
ixx

I
ixy

I
ixz

I
iyy

I
iyz

I
izz
I
m
i
]
T
Le operazioni richieste dalle equazioni di Lagrange non
alterano la linearit` a

i
=
n

j=1
y
T
ij

j
ove
y
ij
=
d
dt

T j
q
i


T j
q
i
+

Uj
q
i
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Propriet` a notevole
_

2
.
.
.

n
_

_
=
_

_
y
T
11
y
T
12
. . . y
T
1n
0
T
y
T
22
. . . y
T
2n
.
.
.
.
.
.
.
.
.
.
.
.
0
T
0
T
. . . y
T
nn
_

_
_

2
.
.
.

n
_

_
in forma compatta:
= Y (q, q, q)
dunque, in assenza di forze allorgano terminale
Y (q, q, q) = B(q) q +C(q, q) q +F
v
q +f
s
( q) +g(q)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore cartesiano a due bracci
Motori posizionati con gli assi lungo gli assi di giunto
masse m
m
i
momenti di inerzia I
m
i
p
m
i
= p
i1
z
m
i
= z
i1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Jacobiani
J
(
1
)
P
=
_
_
0 0
0 0
1 0
_
_
J
(
2
)
P
=
_
_
0 1
0 0
1 0
_
_
J
(m
1
)
P
=
_
_
0 0
0 0
0 0
_
_
J
(m
2
)
P
=
_
_
0 0
0 0
1 0
_
_
J
(m
1
)
O
=
_
_
0 0
0 0
k
r1
0
_
_
J
(m
2
)
O
=
_
_
0 k
r2
0 0
0 0
_
_
Matrice di inerzia
B =
_
m

1
+m
m
2
+k
2
r1
I
m
1
+m

2
0
0 m

2
+k
2
r2
I
m
2
_
non solo B ` e indipendente da q ma ` e anche diagonale
con k
ri
elevati le inerzie dei rotori non sono trascurabili
Forze gravitazionali
g
1
= (m

1
+m
m
2
+m

2
)g g
2
= 0
Equazioni del moto
(m

1
+m
m
2
+k
2
r1
I
m
1
+m

2
)

d
1
+ (m

1
+m
m
2
+m

2
)g = f
1
(m

2
+k
2
r2
I
m
2
)

d
2
= f
2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore planare a due bracci
Motori con gli assi lungo gli assi di giunto
masse m
m
i
momenti di inerzia I
m
i
p
m
i
= p
i1
z
m
i
= z
i1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Jacobiani
J
(
1
)
P
=
_
_

1
s
1
0

1
c
1
0
0 0
_
_
J
(
2
)
P
=
_
_
a
1
s
1

2
s
12

2
s
12
a
1
c
1
+
2
c
12

2
c
12
0 0
_
_
J
(
1
)
O
=
_
_
0 0
0 0
1 0
_
_
J
(
2
)
O
=
_
_
0 0
0 0
1 1
_
_
J
(m
1
)
P
=
_
_
0 0
0 0
0 0
_
_
J
(m
2
)
P
=
_
_
a
1
s
1
0
a
1
c
1
0
0 0
_
_
J
(m
1
)
O
=
_
_
0 0
0 0
k
r1
0
_
_
J
(m
2
)
O
=
_
_
0 0
0 0
1 k
r2
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Matrice di inerzia
B(q) =
_
b
11
(
2
) b
12
(
2
)
b
21
(
2
) b
22
_
b
11
= I

1
+m

2
1
+k
2
r1
I
m
1
+I

2
+m

2
(a
2
1
+
2
2
+ 2a
1

2
c
2
)
+I
m
2
+m
m
2
a
2
1
b
12
= b
21
= I

2
+m

2
(
2
2
+a
1

2
c
2
) +k
r2
I
m
2
b
22
= I

2
+m

2
2
+k
2
r2
I
m2
Forze centrifughe e di Coriolis
c
111
=
1
2
b
11
q
1
= 0
c
112
= c
121
=
1
2
b
11
q
2
= m

2
a
1

2
s
2
= h
c
122
=
b
12
q
2

1
2
b
22
q
1
= h
c
211
=
b
21
q
1

1
2
b
11
q
2
= h
c
212
= c
221
=
1
2
b
22
q
1
= 0
c
222
=
1
2
b
22
q
2
= 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
C(q, q) =
_
h

2
h(

1
+

2
)
h

1
0
_
anti-simmetria
N(q, q) =

B(q) 2C(q, q)
=
_
2h

2
h

2
h

2
0
_
2
_
h

2
h(

1
+

2
)
h

1
0
_
=
_
0 2h

1
h

2
2h

1
+h

2
0
_
Forze gravitazionali
g
1
= (m

1
+m
m
2
a
1
+m

2
a
1
)gc
1
+m

2
gc
12
g
2
= m

2
gc
12
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Equazioni del moto
_
I

1
+m

2
1
+k
2
r1
I
m
1
+I

2
+m

2
(a
2
1
+
2
2
+ 2a
1

2
c
2
)
+I
m
2
+m
m
2
a
2
1
_

1
+
_
I

2
+m

2
(
2
2
+a
1

2
c
2
) +k
r2
I
m
2
_

2
2m

2
a
1

2
s
2

2
m

2
a
1

2
s
2

2
2
+ (m

1
+m
m
2
a
1
+m

2
a
1
)gc
1
+m

2
gc
12
=
1
_
I

2
+m

2
2
+k
2
r2
I
m
2
_

2
+
_
I

2
+m

2
(
2
2
+a
1

2
c
2
) +k
r2
I
m
2
_

1
+m

2
a
1

2
s
2

2
1
+m

2
gc
12
=
2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Parametrizzazione del modello
= [
1

2

3

4

5

6

7

8
]
T

1
= m
1
= m

1
+m
m
2

2
= m
1

C
1
= m

1
(
1
a
1
)

3
=

I
1
= I

1
+m

1
(
1
a
1
)
2
+I
m
2

4
= I
m
1

5
= m
2
= m

6
= m

C
2
= m

2
(
2
a
2
)

7
=

I
2
= I

2
+m

2
(
2
a
2
)
2

8
= I
m
2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Y =
_
y
11
y
12
y
13
y
14
y
15
y
16
y
17
y
18
y
21
y
22
y
23
y
24
y
25
y
26
y
27
y
28
_
y
11
= a
2
1

1
+a
1
gc
1
y
12
= 2a
1

1
+gc
1
y
13
=

1
y
14
= k
2
r1

1
y
15
= (a
2
1
+ 2a
1
a
2
c
2
+a
2
2
)

1
+ (a
1
a
2
c
2
+a
2
2
)

2
2a
1
a
2
s
2

2
a
1
a
2
s
2

2
2
+a
1
gc
1
+a
2
gc
12
y
16
= (2a
1
c
2
+ 2a
2
)

1
+ (a
1
c
2
+ 2a
2
)

2
2a
1
s
2

2
a
1
s
2

2
2
+gc
12
y
17
=

1
+

2
y
18
=

1
+k
r2

2
y
21
= 0
y
22
= 0
y
23
= 0
y
24
= 0
y
25
= (a
1
a
2
c
2
+a
2
2
)

1
+a
2
2

2
+a
1
a
2
s
2

2
1
+a
2
gc
12
y
26
= (a
1
c
2
+ 2a
2
)

1
+ 2a
2

2
+a
1
s
2

2
1
+gc
12
y
27
=

1
+

2
y
28
= k
r2

1
+k
2
r2

2
.
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
Manipolatore planare a due bracci
a
1
= a
2
= 1 m
1
=
2
= 0.5 m
m

1
= m

2
= 50 kg I

1
= I

2
= 10 kgm
2
k
r1
= k
r2
= 100
m
m
1
= m
m
2
= 5 kg I
m
1
= I
m
2
= 0.01 kg m
2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 0.1 0.2 0.3 0.4 0.5
2
1
0
1
2
3
4
[s]
[
r
a
d
]
pos giunto 1
0 0.1 0.2 0.3 0.4 0.5
2
1
0
1
2
3
4
[s]
[
r
a
d
]
pos giunto 2
0 0.1 0.2 0.3 0.4 0.5
0
2
4
6
[s]
[
r
a
d
/
s
]
vel giunto 1
0 0.1 0.2 0.3 0.4 0.5
0
2
4
6
[s]
[
r
a
d
/
s
]
vel giunto 2
0 0.1 0.2 0.3 0.4 0.5
30
20
10
0
10
20
30
[s]
[
r
a
d
/
s
^
2
]
acc giunto 1
0 0.1 0.2 0.3 0.4 0.5
30
20
10
0
10
20
30
[s]
[
r
a
d
/
s
^
2
]
acc giunto 2
0 0.1 0.2 0.3 0.4 0.5
6000
4000
2000
0
2000
4000
6000
[s]
[
N
m
]
coppia giunto 1
0 0.1 0.2 0.3 0.4 0.5
6000
4000
2000
0
2000
4000
6000
[s]
[
N
m
]
coppia giunto 2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 0.1 0.2 0.3 0.4 0.5
5000
0
5000
[s]
[
N
m
]
inerz_11
0 0.1 0.2 0.3 0.4 0.5
5000
0
5000
[s]
[
N
m
]
inerz_22
0 0.1 0.2 0.3 0.4 0.5
500
400
300
200
100
0
100
[s]
[
N
m
]
inerz_12
0 0.1 0.2 0.3 0.4 0.5
500
400
300
200
100
0
100
[s]
[
N
m
]
inerz_21
0 0.1 0.2 0.3 0.4 0.5
500
0
500
1000
[s]
[
N
m
]
centrif_2 + coriol_12
12
2
0 0.1 0.2 0.3 0.4 0.5
500
0
500
1000
[s]
[
N
m
]
centrif_1
0 0.1 0.2 0.3 0.4 0.5
200
0
200
400
600
800
[s]
[
N
m
]
grav_1
0 0.1 0.2 0.3 0.4 0.5
200
0
200
400
600
800
[s]
[
N
m
]
grav_2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 0.2 0.4 0.6
2
1
0
1
2
3
4
[s]
[
r
a
d
]
pos giunto 1
0 0.2 0.4 0.6
2
1
0
1
2
3
4
[s]
[
r
a
d
]
pos giunto 2
0 0.2 0.4 0.6
6
4
2
0
2
4
6
[s]
[
r
a
d
/
s
]
vel giunto 1
0 0.2 0.4 0.6
6
4
2
0
2
4
6
[s]
[
r
a
d
/
s
]
vel giunto 2
0 0.2 0.4 0.6
40
20
0
20
40
[s]
[
r
a
d
/
s
^
2
]
acc giunto 1
0 0.2 0.4 0.6
40
20
0
20
40
[s]
[
r
a
d
/
s
^
2
]
acc giunto 2
0 0.2 0.4 0.6
5000
0
5000
[s]
[
N
m
]
coppia giunto 1
0 0.2 0.4 0.6
5000
0
5000
[s]
[
N
m
]
coppia giunto 2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 0.2 0.4 0.6
6000
4000
2000
0
2000
4000
6000
[s]
[
N
m
]
inerz_11
0 0.2 0.4 0.6
6000
4000
2000
0
2000
4000
6000
[s]
[
N
m
]
inerz_22
0 0.2 0.4 0.6
500
0
500
1000
1500
[s]
[
N
m
]
inerz_12
0 0.2 0.4 0.6
500
0
500
1000
1500
[s]
[
N
m
]
inerz_21
0 0.2 0.4 0.6
500
0
500
1000
[s]
[
N
m
]
centrif_2 + coriol_12
12
2
0 0.2 0.4 0.6
500
0
500
1000
[s]
[
N
m
]
centrif_1
0 0.2 0.4 0.6
0
200
400
600
800
1000
[s]
[
N
m
]
grav_1
0 0.2 0.4 0.6
0
200
400
600
800
1000
[s]
[
N
m
]
grav_2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 0.1 0.2 0.3 0.4 0.5
0
0.5
1
1.5
2
[s]
[
m
]
pos organo terminale
0 0.1 0.2 0.3 0.4 0.5
0
1
2
3
4
5
[s]
[
m
/
s
]
vel organo terminale
0 0.1 0.2 0.3 0.4 0.5
40
20
0
20
40
[s]
[
m
/
s
^
2
]
acc organo terminale
e calcolando le traiettorie di giunto tramite inversione
cinematica ...
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 0.1 0.2 0.3 0.4 0.5
2
1
0
1
2
3
4
[s]
[
r
a
d
]
pos giunto 1
0 0.1 0.2 0.3 0.4 0.5
2
1
0
1
2
3
4
[s]
[
r
a
d
]
pos giunto 2
0 0.1 0.2 0.3 0.4 0.5
8
6
4
2
0
2
4
[s]
[
r
a
d
/
s
]
vel giunto 1
0 0.1 0.2 0.3 0.4 0.5
8
6
4
2
0
2
4
[s]
[
r
a
d
/
s
]
vel giunto 2
0 0.1 0.2 0.3 0.4 0.5
40
20
0
20
40
60
80
[s]
[
r
a
d
/
s
^
2
]
acc giunto 1
0 0.1 0.2 0.3 0.4 0.5
40
20
0
20
40
60
80
[s]
[
r
a
d
/
s
^
2
]
acc giunto 2
0 0.1 0.2 0.3 0.4 0.5
5000
0
5000
[s]
[
N
m
]
coppia giunto 1
0 0.1 0.2 0.3 0.4 0.5
5000
0
5000
[s]
[
N
m
]
coppia giunto 2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 0.1 0.2 0.3 0.4 0.5
1
0.5
0
0.5
1
x 10
4
[s]
[
N
m
]
inerz_11
0 0.1 0.2 0.3 0.4 0.5
1
0.5
0
0.5
1
x 10
4
[s]
[
N
m
]
inerz_22
0 0.1 0.2 0.3 0.4 0.5
1000
0
1000
2000
3000
[s]
[
N
m
]
inerz_12
0 0.1 0.2 0.3 0.4 0.5
1000
0
1000
2000
3000
[s]
[
N
m
]
inerz_21
0 0.1 0.2 0.3 0.4 0.5
1000
500
0
500
1000
[s]
[
N
m
]
centrif_2 + coriol_12
12
2
0 0.1 0.2 0.3 0.4 0.5
1000
500
0
500
1000
[s]
[
N
m
]
centrif_1
0 0.1 0.2 0.3 0.4 0.5
0
200
400
600
800
1000
[s]
[
N
m
]
grav_1
0 0.1 0.2 0.3 0.4 0.5
0
200
400
600
800
1000
[s]
[
N
m
]
grav_2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
IDENTIFICAZIONE DEI PARAMETRI
DINAMICI
Impiego del modello dinamico per simulazione e controllo
Stime approssimate dei parametri dinamici
semplicazioni di modellazione geometrica
effetti dinamici complessi (attriti, etc.)
Tecniche di identicazione
propriet` a di linearit` a
= Y (q, q, q)
Misure
q e q dirette (encoder e tachimetri)
q indirette (ltro ricostruttore)
dirette (sensore di coppia) o indirette (misure di forza o
corrente)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
N insiemi di misure
=
_

_
(t
1
)
.
.
.
(t
N
)
_

_
=
_

_
Y (t
1
)
.
.
.
Y (t
N
)
_

_
=

Y
Soluzione (a minimi quadrati)
= (

Y
T

Y )
1

Y
T

Y triangolare = procedimento iterativo
tuttavia gli errori di stima possono cumularsi
se (Y ) < p = Y

Parametri
identicabili
non identicabili
identicabili in combinazione lineare
Carico
modica parametri dellultimo braccio
Traiettorie
di tipo polinomiale (sufcientemente ricche)
attenzione alle dinamiche non modellate
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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 ni della sintesi del controllo)
` e efcace se si vogliono portare in conto effetti meccanici
pi ` u complessi quali ad esempio le deformazioni elastiche
dei bracci
Formulazione di Newton-Eulero:
` e intrinsecamente un metodo ricorsivo che risulta efciente
da un punto di vista computazionale
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Dinamica diretta
note q(t
0
), q(t
0
), (t) (e h(t)), determinare q(t), q(t), q(t)
per t > t
0
utile in simulazione
Dinamica inversa
note q(t), q(t), q(t) (e h(t)), determinare (t)
utile per la pianicazione e il controllo
Per un manipolatore ad n giunti, il numero di operazioni per il
calcolo della dinamica ` e:
O(n
2
) per la dinamica diretta
O(n) per la dinamica inversa
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Dinamica diretta con Lagrange
q = B
1
(q)(

(q, q) = C(q, q) q +F
v
q +f
s
( q) +g(q) +J
T
(q)h
note q(t
k
), q(t
k
), (t
k
), si calcola q(t
k
)
si integra numericamente con passo t: q(t
k+1
), q(t
k+1
)
Dinamica diretta con Newton-Eulero
note q, q, si calcola:

come per q = 0
colonna b
i
di Bcome per g
0
= 0, q = 0, q
i
= 1 e q
j
= 0
per j ,= i
nota , si calcola q e si integra numericamente
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
MODELLO DINAMICO NELLO SPAZIO
OPERATIVO
Ridondanza
coordinate generalizzate (moti interni?)
Singolarit` a
Modello nello spazio dei giunti ( = J
T
(q)
e
)
q = B
1
(q)C(q, q) qB
1
(q)g(q)+B
1
(q)J
T
(q)(
e
h
e
)
Cinematica differenziale del secondo ordine
x
e
= J
A
(q) q +

J
A
(q, q) q
= J
A
B
1
C q J
A
B
1
g +

J
A
q +J
A
B
1
J
T
A
(
A
h
A
)
dove

A
= T
T
A
(x)
e
J = T
A
J
A
h
A
= T
T
A
(x)h
e
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Trasformazioni
B
A
= (J
A
B
1
J
T
A
)
1
C
A
x = B
A
J
A
B
1
C q B
A

J
A
q
g
A
= B
A
J
A
B
1
g
manipolatore non ridondante
B
A
= J
T
A
BJ
1
A
C
A
x = J
T
A
C q B
A

J
A
q
g
A
= J
T
A
g
Equazioni del moto
B
A
(x
e
) x
e
+C
A
(x
e
, x
e
) x
e
+g
A
(x
e
) =
A
h
A
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Dinamica diretta
note (t) (e h
e
(t)), determinare x
e
(t), x
e
(t), x
e
(t)
Soluzione
dinamica diretta ai giunti
cinematica diretta
Dinamica inversa
note x
e
(t), x
e
(t), x
e
(t) (e h
e
(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
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Dinamica inversa

A
h
A
= B
A
(x
e
) x
e
+C
A
(x
e
, x
e
) x
e
+g
A
(x
e
)
= B
A
( x
e


J
A
q) +B
A
J
A
B
1
C q +B
A
J
A
B
1
g
= B
A
J
A
q +B
A
J
A
B
1
C q +B
A
J
A
B
1
g
pseudo-inversa destra di J
A
pesata secondo B

J
A
(q) = B
1
(q)J
T
A
(q)B
A
(q)

A
h
A
=

J
T
A
(B q +C q +g)
=

J
T
A
( J
T
A
h
A
) =

J
T
A
h
A
= J
T
A
(q)
A
+
_
I J
T
A
(q)

J
T
A
(q)
_

a
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ELLISSOIDE DI MANIPOLABILIT
`
A
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
e
= J(q) q

= B(q)J

(q) v
e
+g(q)
Ellissoide nello spazio operativo
_
B(q)J

(q) v
e
+g(q)
_
T
_
B(q)J

(q) v
e
+g(q)
_
= 1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
BJ

v
e
+g = B(J

v
e
+B
1
g)
= B(J

v
e
+B
1
g +J

JB
1
g J

JB
1
g)
= B
_
J

v
e
+J

JB
1
g + (I J

J)B
1
g
_
= BJ

( v
e
+JB
1
g)

( v
e
+JB
1
g)
T
J
T
B
T
BJ

( v
e
+JB
1
g) = 1
0.5 0 0.5 1
0.5
0
0.5
1
JB g
1
[m]
[
m
]

manipolatore non ridondante
( v
e
+JB
1
g)
T
J
T
B
T
BJ
1
( v
e
+JB
1
g) = 1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
PERCORSO E TRAIETTORIA
generazione di traiettorie a curvatura regolare
Percorso: luogo dei punti dello spazio dei giunti (spazio
operativo) che il manipolatore deve descrivere nellesecuzione
del movimento assegnato (descrizione geometrica)
Traiettoria: percorso su cui sia specicata la legge oraria di
moto (ad es. velocit` a e/o accelerazioni)
Algoritmo di pianicazione della traiettoria
ingressi
denizione del percorso
vincoli al percorso
vincoli dovuti alla dinamica del manipolatore
uscita
traiettorie dei giunti (o dellorgano terminale) espresse come
sequenza temporale di valori assunti da posizione, velocit` a
e accelerazione
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Numero ristretto di parametri
percorso
punti estremi
eventuali punti intermedi
primitive geometriche
legge di moto
tempo complessivo
velocit` a e/o accelerazione max
velocit` a e/o accelerazione in punti assegnati
Traiettorie nello spazio operativo
descrizione naturale del compito
vincoli sul percorso
singolarit` a
ridondanza
azione di controllo nello spazio dei giunti

necessit` a dellinversione cinematica


Traiettorie nello spazio dei giunti
programmazione fuori linea inversione cinematica
programmazione per insegnamento acquisizione diretta
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
TRAIETTORIE NELLO SPAZIO DEI GIUNTI
Generazione di una funzione q(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 velocit` a (e accelerazioni) dei giunti funzioni
continue del tempo
effetti indesiderati minimizzati (ad es. traiettorie a curvatura
non regolare)
Moto puntopunto
punti estremi e tempo di transizione
Moto su percorso assegnato
punti estremi, punti intermedi e tempi di transizione
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Moto puntopunto
generazione di q(t) che realizza il moto da q
i
a q
f
nel tempo t
f
Volendo minimizzare lenergia dissipata dal motore si pu` o
scegliere la legge di moto che minimizzi lindice
_
t
f
0

2
(t) dt
col vincolo che
I = ,
_
t
f
0
(t) dt = q
f
q
i
Polinomio cubico
q(t) = a
3
t
3
+a
2
t
2
+a
1
t +a
0
q(t) = 3a
3
t
2
+ 2a
2
t +a
1
q(t) = 6a
3
t + 2a
2
calcolo dei coefcienti
a
0
= q
i
a
1
= q
i
a
3
t
3
f
+a
2
t
2
f
+a
1
t
f
+a
0
= q
f
3a
3
t
2
f
+ 2a
2
t
f
+a
1
= q
f
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
0 0.2 0.4 0.6 0.8 1
0
1
2
3
pos
[s]
[
r
a
d
]
0 0.2 0.4 0.6 0.8 1
0
1
2
3
4
5
vel
[s]
[
r
a
d
/
s
]
0 0.2 0.4 0.6 0.8 1
20
10
0
10
20
acc
[s]
[
r
a
d
/
s
^
2
]
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Polinomio quintico (accelerazioni iniziale e nale)
q(t) = a
5
t
5
+a
4
t
4
+a
3
t
3
+a
2
t
2
+a
1
t +a
0
, t [0, t
f
]
Versione parametrizzata
q(t) = q
i
+ (q
f
q
i
) q()[
=t/t
f
, [0, 1]
q(0) = 0, q(1) = 1
Esempio
velocit` a iniziale e nale nulle
accelerazione iniziale e nale nulle
calcolo dei coefcienti
q(0) = 0 a
0
= 0

q(0) = 0 a
1
= 0

q(0) = 0 a
2
= 0
q(1) = 1 a
5
+a
4
+a
3
= 1

q(1) = 0 5a
5
+ 4a
4
+ 3a
3
= 0

q(1) = 0 20a
5
+ 12a
4
+ 6a
3
= 0

q() = 6
5
15
4
+ 10
3
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Prolo di velocit` a trapezoidale
q
c
= q
c
t
c
=
q
m
q
c
t
m
t
c
, q
m
= (q
f
+q
i
)/2, t
m
= t
f
/2
q
c
= q
i
+
1
2
q
c
t
2
c
q
c
t
2
c
q
c
t
f
t
c
+q
f
q
i
= 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
q
c
assegnata (sgn q
c
= sgn(q
f
q
i
))
t
c
=
t
f
2

1
2

t
2
f
q
c
4(q
f
q
i
)
q
c
[ q
c
[
4[q
f
q
i
[
t
2
f
traiettoria
q(t) =
_

_
q
i
+
1
2
q
c
t
2
0 t t
c
q
i
+ q
c
t
c
(t t
c
/2) t
c
< t t
f
t
c
q
f

1
2
q
c
(t
f
t)
2
t
f
t
c
< t t
f
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
0 0.2 0.4 0.6 0.8 1
0
1
2
3
pos
[s]
[
r
a
d
]
0 0.2 0.4 0.6 0.8 1
0
1
2
3
4
5
vel
[s]
[
r
a
d
/
s
]
0 0.2 0.4 0.6 0.8 1
20
10
0
10
20
acc
[s]
[
r
a
d
/
s
^
2
]
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
q
c
assegnata
[q
f
q
i
[
t
f
< [ q
c
[
2[q
f
q
i
[
t
f
la prima disuguaglianza corrisponde al caso di prolo di
velocit` a rettangolare (t
c
= 0)
(senza uguaglianza perch e si avrebbe q
c
= )
la seconda disuguaglianza corrisponde al caso di prolo di
velocit` a triangolare (t
c
= t
f
/2)
ricordando che q
c
= q
c
t
c
e lequazione
q
c
t
2
c
q
c
t
f
t
c
+q
f
q
i
= 0
si ricavano
t
c
=
q
i
q
f
+ q
c
t
f
q
c
e
q
c
=
q
2
c
q
i
q
f
+ q
c
t
f
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Moto su percorso assegnato
opportunit` a di specicare 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 velocit` a iniziale e nale
al crescere del grado di un polinomio aumenta il suo
carattere oscillatorio (traiettorie poco naturali)
laccuratezza numerica con cui pu` o essere calcolato un
polinomio peggiora allaumentare del suo grado
una volta impostato il sistema di equazioni dei vincoli,
risulta oneroso risolverlo
i coefcienti del polinomio dipendono da tutti i punti
assegnati = se si vuole cambiare uno dei punti di
percorso, si deve ricalcolare il tutto
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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
laccelerazione q(t) deve essere continua in corrispondenza
dei punti di percorso
Sequenze di polinomi di grado minore di tre (passaggio in
prossimit` a dei punti di percorso imposti)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Sequenza di polinomi con vincoli imposti sulle velocit` a nei
punti di percorso
N punti ed N 1 polinomi 4N 4 incognite
k = 1, . . . , N 1

k
(t
k
) = q
k

k
(t
k+1
) = q
k+1

k
(t
k
) = q
k

k
(t
k+1
) = q
k+1
occorre per` o garantire la continuit` a delle velocit` a nei punti
intermedi imponendo

k
(t
k+1
) =

k+1
(t
k+1
)
con k = 1, . . . , N 2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
0 2 4 6
0
2
4
6
pos
[s]
[
r
a
d
]
0 2 4 6
5
0
5
vel
[s]
[
r
a
d
/
s
]
0 2 4 6
40
20
0
20
acc
[s]
[
r
a
d
/
s
^
2
]
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Sequenza di polinomi con velocit` a calcolate nei punti di
percorso
assumendo di interpolare la sequenza di punti con segmenti
rettilinei (nel tempo)
q
1
= 0
q
k
=
_
0 sgn(v
k
) ,= sgn(v
k+1
)
1
2
(v
k
+v
k+1
) sgn(v
k
) = sgn(v
k+1
)
q
N
= 0
v
k
= (q
k
q
k1
)/(t
k
t
k1
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
0 2 4 6
0
2
4
6
pos
[s]
[
r
a
d
]
0 2 4 6
5
0
5
vel
[s]
[
r
a
d
/
s
]
0 2 4 6
30
20
10
0
10
20
30
acc
[s]
[
r
a
d
/
s
^
2
]
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Sequenza di polinomi con accelerazione continua nei punti di
percorso (spline)
con k = 1, . . . , N

k1
(t
k
) = q
k

k1
(t
k
) =
k
(t
k
)

k1
(t
k
) =

k
(t
k
)

k1
(t
k
) =

k
(t
k
)

0
e
N
non esistono
4(N2) +6 = 4N2 equazioni nei 4(N1) coefcienti
incogniti (polinomi di quarto grado per il primo e lultimo
segmento?)
2 punti virtuali (continuit` a su posizione, velocit` a e
accelerazione) = N + 1 polinomi cubici
convenzionalmente i 2 punti virtuali sono negli istanti t
2
e
t
N+1
(secondo e penultimo)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
4(N 2) equazioni per gli N 2 punti di percorso interni
(k = 3, . . . , N 2)

k1
(t
k
) = q
k

k1
(t
k
) =
k
(t
k
)

k1
(t
k
) =

k
(t
k
)

k1
(t
k
) =

k
(t
k
)
6 equazioni per i punti iniziale e nale

1
(t
1
) = q
i

1
(t
1
) = q
i

1
(t
1
) = q
i

N+1
(t
N+2
) = q
f

N+1
(t
N+2
) = q
f

N+1
(t
N+2
) = q
f
6 equazioni per i punti virtuali (k = 2, N + 1)

k1
(t
k
) =
k
(t
k
)

k1
(t
k
) =

k
(t
k
)

k1
(t
k
) =

k
(t
k
)

Sistema di 4(N + 1) equazioni per la determinazione dei


4(N + 1) coefcienti degli N + 1 polinomi cubici
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Algoritmo efciente dal punto di vista computazionale
la derivata seconda di
k
(t) ` e una funzione lineare

k
(t) =

k
(t
k
)
t
k
(t
k+1
t) +

k
(t
k+1
)
t
k
(t t
k
)
t
k
= t
k+1
t
k
integrando due volte

k
(t) =

k
(t
k
)
6t
k
(t
k+1
t)
3
+

k
(t
k+1
)
6t
k
(t t
k
)
3
+
_

k
(t
k+1
)
t
k

t
k

k
(t
k+1
)
6
_
(t t
k
)
+
_

k
(t
k
)
t
k

t
k

k
(t
k
)
6
_
(t
k+1
t)
4 incognite:
k
(t
k
),
k
(t
k+1
),

k
(t
k
) e

k
(t
k+1
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
N variabili q
k
per k ,= 2, N + 1 assegnate
vincoli di continuit` a su q
2
e q
N+1
vincoli di continuit` a su q
k
per k = 3, . . . , N
q
i
e q
f
assegnate
vincoli di continuit` a su q
k
per k = 2, . . . , N + 1
q
i
e q
f
assegnate

1
(t
2
) =

2
(t
2
)
.
.
.

N
(t
N+1
) =

N+1
(t
N+1
)
sistema di equazioni lineari
A[

2
(t
2
) . . .

N+1
(t
N+1
) ]
T
= b
A =
_

_
a
11
a
12
. . . 0 0
a
21
a
22
. . . 0 0
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
0 0 . . . a
N1,N1
a
N1,N
0 0 . . . a
N,N1
a
NN
_

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
0 1 2 3 4 5
0
2
4
6
pos
[s]
[
r
a
d
]
0 1 2 3 4 5
5
0
5
vel
[s]
[
r
a
d
/
s
]
0 1 2 3 4 5
30
20
10
0
10
20
30
acc
[s]
[
r
a
d
/
s
^
2
]
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Sequenza di polinomi parabolico-lineari con passaggio in
prossimit` a di punti di via
q
k1,k
=
q
k
q
k1
t
k1
q
k
=
q
k,k+1
q
k1,k
t

k
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
0 1 2 3 4 5
0
2
4
6
pos
[s]
[
r
a
d
]
0 1 2 3 4 5
6
4
2
0
2
4
vel
[s]
[
r
a
d
/
s
]
0 1 2 3 4 5
40
20
0
20
40
acc
[s]
[
r
a
d
/
s
^
2
]
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Applicazione della legge trapezoidale al problema dellinterpolazione
0 0.5 1 1.5 2
0
1
2
3
4
5
pos
[s]
[
r
a
d
]
0 0.5 1 1.5 2
0
1
2
3
4
vel
[s]
[
r
a
d
/
s
]
0 0.5 1 1.5 2
20
10
0
10
20
acc
[s]
[
r
a
d
/
s
^
2
]
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
TRAIETTORIE NELLO SPAZIO OPERATIVO
Sequenze di punti assegnati
inversione cinematica
traiettorie nello spazio dei giunti
microinterpolazione
Punti di percorso x(t
k
)
componenti x
i
(t
k
) raccordate secondo sequenze di
polinomi interpolatori
Primitive di percorso
espressione analitica del moto
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Primitive di percorso
Rappresentazione parametrica di una curva nello spazio
p = f(s)
t =
dp
ds
n =
1
_
_
_
_
d
2
p
ds
2
_
_
_
_
d
2
p
ds
2
b = t n
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Segmento nello spazio
p(s) = p
i
+
s
|p
f
p
i
|
(p
f
p
i
)
dp
ds
=
1
|p
f
p
i
|
(p
f
p
i
)
d
2
p
ds
2
= 0
s [0, |p
f
p
i
|]
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Circonferenza nello spazio
p

(s) =
_
_
cos (s/)
sin(s/)
0
_
_
, s [0, 2]
= p
i
d, c = d + (
T
r)r, || > [
T
r[
p(s) = c +Rp

(s)
dp
ds
= R
_
_
sin(s/)
cos (s/)
0
_
_
d
2
p
ds
2
= R
_
_
cos (s/)/
sin(s/)/
0
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Traiettorie di posizione e di orientamento
Posizione p
e
= f(s)
s(t) polinomio interpolatore t.c. s(0) = 0, s(t
f
) = l()
p
e
= s
dp
e
ds
= st
segmento
p
e
(s) = p
i
+
s
|p
f
p
i
|
(p
f
p
i
)
p
e
=
s
|p
f
p
i
|
(p
f
p
i
) = st
p
e
=
s
|p
f
p
i
|
(p
f
p
i
) = st
circonferenza
p
e
(s) = c +R
_
_
cos (s/)
sin(s/)
0
_
_
p
e
= R
_
_
s sin(s/)
s cos (s/)
0
_
_
p
e
= R
_
_
s
2
cos (s/)/ s sin(s/)
s
2
sin(s/)/ + s cos (s/)
0
_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Orientamento
interpolazione delle componenti di n
e
(t), s
e
(t), a
e
(t) ?
interpolazione di
e
(t)

e
(s) =
i
+
s
|
f

i
|
(
f

i
)

e
=
s
|
f

i
|
(
f

i
)

e
=
s
|
f

i
|
(
f

i
)
asse equivalente di rotazione (R
f
= R
i
R
i
f
)
R
i
f
= R
T
i
R
f
=
_
_
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
_
_

f
= cos
1
_
r
11
+r
22
+r
33
1
2
_
r
i
=
1
2 sin
f
_
_
r
32
r
23
r
13
r
31
r
21
r
12
_
_
R
i
r
(t): R
i
r
(0) = I R
i
r
(t
f
) = R
i
f
R
e
(t) = R
i
R
i
r
((t)),
e
(t) = R
i
r
i

(t),
e
(t) = R
i
r
i

(t)
(0) = 0 (t
f
) =
f
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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
2
1
q
1
q
2
. . . q
n1
q
n
q
2
n
]
T
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
scalatura temporale r(t): r(0) = 0 r(t
f
) =

t
f
q(t) = q(r(t))
q = r q

(r)
q = r
2
q

(r) + r q

(r)

= r
2
_
B( q(r)) q

(r) +( q(r))[ q

(r) q

(r)]
_
+ rB( q(r)) q

(r)
+g( q(r))
=
s
(t) +g( q(r))

s
(r) = B( q(r)) q

(r) +( q(r))[ q

(r) q

(r)]

s
(t) = r
2

s
(r) + rB( q(r)) q

(r)
scelta semplice r(t) = ct

s
(t) = c
2

s
(ct)
giunto q
i
in corrispondenza della violazione maggiore
[
s
[
[
i
g(q
i
)[
= c
2
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CONTROLLO DEL MOTO
Controllo nello spazio dei giunti
Controllo indipendente ai giunti
Compensazione in avanti a coppia precalcolata
Controllo centralizzato
Controllo nello spazio operativo
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
IL PROBLEMA DEL CONTROLLO
Controllo nello spazio dei giunti
Controllo nello spazio operativo
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CONTROLLO NELLO SPAZIO DEI GIUNTI
Modello dinamico
B(q) q +C(q, q) q +F
v
q +g(q) =
Controllo determinare :
q(t) = q
d
(t)
trasmissioni
K
r
q = q
m

m
= K
1
r

inerzie medie
B(q) =

B +B(q)

m
= K
1
r

BK
1
r
q
m
+F
m
q
m
+d
attrito viscoso
F
m
= K
1
r
F
v
K
1
r
disturbo
d = K
1
r
B(q)K
1
r
q
m
+K
1
r
C(q, q)K
1
r
q
m
+K
1
r
g(q)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore + attuatori
controllo decentralizzato
controllo centralizzato
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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
F
m

k
v
k
t
R
a
M(s) =
k
m
s(1 +sT
m
)
k
m
=
1
k
v
T
m
=
R
a
I
k
v
k
t
I =

b
ii
k
2
ri
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo in retroazione
Riduzione degli effetti di d su
elevato valore del guadagno degli amplicatori a monte del
punto di applicazione del disturbo
presenza nel controllore di una azione integrale al ne di
annullare a regime ( costante) leffetto della componente
gravitazionale sulluscita

Controllore proporzionale-integrale (PI)


C(s) = K
c
1 +sT
c
s
Struttura generale
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Retroazione di posizione
C
P
(s) = K
P
1 +sT
P
s
C
V
(s) = 1 C
A
(s) = 1
k
TV
= k
TA
= 0
f.d.t. del ramo di azione diretta
C(s)G(s) =
k
m
K
P
(1 +sT
P
)
s
2
(1 +sT
m
)
f.d.t. del ramo in retroazione
H(s) = k
TP
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
analisi con il l.d.r.
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
f.d.t. ingressouscita a ciclo chiuso
(s)

r
(s)
=
1
k
TP
1 +
s
2
(1 +sT
m
)
k
m
K
P
k
TP
(1 +sT
P
)
W(s) =
1
k
TP
(1 +sT
P
)
_
1 +
2s

n
+
s
2

2
n
_
(1 +s)
f.d.t. disturbouscita
(s)
D(s)
=
sR
a
k
t
K
P
k
TP
(1 +sT
P
)
1 +
s
2
(1 +sT
m
)
k
m
K
P
k
TP
(1 +sT
P
)
X
R
= K
P
k
TP
T
R
= max
_
T
P
,
1

n
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Retroazione di posizione e di velocit` a
C
P
(s) = K
P
C
V
(s) = K
V
1 +sT
V
s
C
A
(s) = 1
k
TA
= 0
f.d.t. del ramo di azione diretta
C(s)G(s) =
k
m
K
P
K
V
(1 +sT
V
)
s
2
(1 +sT
m
)
f.d.t. del ramo in retroazione
H(s) = k
TP
_
1 +s
k
TV
K
P
k
TP
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
analisi con il l.d.r.
scelta dello zero
T
V
= T
m
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
f.d.t. ingressouscita a ciclo chiuso
(s)

r
(s)
=
1
k
TP
1 +
sk
TV
K
P
k
TP
+
s
2
k
m
K
P
k
TP
K
V
W(s) =
1
k
TP
1 +
2s

n
+
s
2

2
n
speciche di progetto
K
V
k
TV
=
2
n
k
m
K
P
k
TP
K
V
=

2
n
k
m
f.d.t. disturbouscita
(s)
D(s)
=
sR
a
k
t
K
P
k
TP
K
V
(1 +sT
m
)
1 +
sk
TV
K
P
k
TP
+
s
2
k
m
K
P
k
TP
K
V
X
R
= K
P
k
TP
K
V
T
R
= max
_
T
m
,
1

n
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Retroazione di posizione, di velocit` a e di accelerazione
C
P
(s) = K
P
C
V
(s) = K
V
C
A
(s) = K
A
1 +sT
A
s
G

(s) =
k
m
(1 +k
m
K
A
k
TA
)
_
_
_
_
1 +
sT
m
_
1 +k
m
K
A
k
TA
T
A
T
m
_
(1 +k
m
K
A
k
TA
)
_
_
_
_
f.d.t. del ramo di azione diretta
C(s)G(s) =
K
P
K
V
K
A
(1 +sT
A
)
s
2
G

(s)
f.d.t. del ramo in retroazione
H(s) = k
TP
_
1 +
sk
TV
K
P
k
TP
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
analisi con il l.d.r.
scelta dello zero
T
A
= T
m
k
m
K
A
k
TA
T
A
T
m
k
m
K
A
k
TA
1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
f.d.t. ingressouscita a ciclo chiuso
(s)

r
(s)
=
1
k
TP
1 +
sk
TV
K
P
k
TP
+
s
2
(1 +k
m
K
A
k
TA
)
k
m
K
P
k
TP
K
V
K
A
f.d.t. disturbouscita
(s)
D(s)
=
sR
a
k
t
K
P
k
TP
K
V
K
A
(1 +sT
A
)
1 +
sk
TV
K
P
k
TP
+
s
2
(1 +k
m
K
A
k
TA
)
k
m
K
P
k
TP
K
V
K
A
X
R
= K
P
k
TP
K
V
K
A
T
R
= max
_
T
A
,
1

n
_
speciche di progetto
2K
P
k
TP
k
TV
=

n

k
m
K
A
k
TA
=
k
m
X
R

2
n
1
K
P
k
TP
K
V
K
A
= X
R
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ricostruzione dellaccelerazione
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Compensazione in avanti decentralizzata
Elevati valori di velocit` a e accelerazioni
capacit` a di inseguimento degradate
Impiego di una azione in avanti
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Retroazione di posizione

r
(s) =
_
k
TP
+
s
2
(1 +sT
m
)
k
m
K
P
(1 +sT
P
)
_

d
(s)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Retroazione di posizione e di velocit` a

r
(s) =
_
k
TP
+
sk
TV
K
P
+
s
2
k
m
K
P
K
V
_

d
(s)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Retroazione di posizione, di velocit` a e di accelerazione

r
(s) =
_
k
TP
+
sk
TV
K
P
+
(1 +k
m
K
A
k
TA
)s
2
k
m
K
P
K
V
K
A
_

d
(s)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
COMPENSAZIONE IN AVANTI A COPPIA
PRECALCOLATA
Alluscita del PIDD
2
a
2
e +a
1
e +a
0
e +a
1
_
t
e()d +
T
m
k
m

d
+
1
k
m

R
a
k
t
d
=
T
m
k
m

+
1
k
m

2
e +a

1
e +a

0
e +a

1
_
t
e()d =
R
a
k
t
d
E(s)
D(s)
=
R
a
k
t
s
a

2
s
3
+a

1
s
2
+a

0
s +a

1
adozione di guadagni di anello troppo elevati
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Coppia precalcolata
azione in avanti (inversione di modello)
d
d
= K
1
r
B(q
d
)K
1
r
q
md
+K
1
r
C(q
d
, q
d
)K
1
r
q
md
+K
1
r
g(q
d
)
riduzione degli sforzi di reiezione del disturbo (guadagni
contenuti)
calcolo fuori linea/in linea
compensazione parziale
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CONTROLLO CENTRALIZZATO
Manipolatore Sistema multivariabile accoppiato e nonlineare
B(q) q +C(q, q) q +F
v
q +g(q) =
Trasmissioni
K
r
q = q
m
Sistemi di attuazione
K
1
r
= K
t
i
a
v
a
= R
a
i
a
+K
v
q
m
v
a
= G
v
v
c
Manipolatore controllato in tensione
B(q) q +C(q, q) q +F q +g(q) = u
F = F
v
+K
r
K
t
R
1
a
K
v
K
r
u = K
r
K
t
R
1
a
G
v
v
c
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Manipolatore controllato in coppia
riduzione della sensibilit` a alle variazioni parametriche
K
t
, K
v
, R
a
attuatori come generatori controllati di coppia (corrente)
i
a
= G
i
v
c

F = F
v
u = K
r
K
t
G
i
v
c
=
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo PD con compensazione di gravit` a
Regolazione ad una postura di equilibrio costante q
d
Metodo diretto di Lyapunov
stato [ q
T
q
T
]
T
q = q
d
q
funzione candidata di Lyapunov
V ( q, q) =
1
2
q
T
B(q) q +
1
2
q
T
K
P
q > 0 q, q ,= 0

V = q
T
B(q) q +
1
2
q
T

B(q) q q
T
K
P
q
=
1
2
q
T
_

B(q) 2C(q, q)
_
q q
T
F q + q
T
_
u g(q) K
P
q
_
scelta del controllo
u = g(q) +K
P
q K
D
q

V = q
T
(F +K
D
) q

V = 0 q = 0, q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
dinamica del sistema controllato
B(q) q +C(q, q) q +F q +g(q) = g(q) +K
P
q K
D
q
allequilibrio ( q q 0)
K
P
q = 0 = q = q
d
q 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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 q
u = B(q)y +n(q, q)

q = y
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Scelta di un controllo y stabilizzante
y = K
P
q K
D
q +r
r = q
d
+K
D
q
d
+K
P
q
d

q +K
D

q +K
P
q = 0
cancellazione perfetta
vincoli sullarchitettura hardware/software dellunit` a di
governo
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo robusto
Compensazione imperfetta
u =

B(q)y + n(q, q)
incertezza

B =

B B n = n n

B q +n =

By + n
q = y + (B
1

B I)y +B
1
n = y
= (I B
1

B)y B
1
n
in assenza di incertezza = 0
Scelta del controllo
y = q
d
+K
D
( q
d
q) +K
P
(q
d
q)

q +K
D

q +K
P
q =
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE

q = q
d
q = q
d
y +
posto
=
_
q

q
_
, H =
_
O I
O O
_
, D =
_
O
I
_
equazione differenziale (non lineare) del primo ordine

= H +D( q
d
y +)
Obiettivo: trovare y tale che lorigine del sistema dinamico di
errore sia asintoticamente stabile nonostante lincertezza
Stima del campo di variabilit` a dellincertezza
sup
t0
| q
d
| < Q
M
< q
d
|I B
1
(q)

B(q)| 1 q
| n| < q, q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Scelta del controllo
y = q
d
+K
D

q +K
P
q +w

=

H +D( w)

H = (H DK) =
_
O I
K
P
K
D
_
Metodo di Lyapunov
V () =
T
Q > 0 ,= 0

V =

T
Q +
T
Q

=
T
(

H
T
Q+Q

H) + 2
T
QD( w)
=
T
P + 2
T
QD( w)
=
T
P + 2z
T
( w), (z = D
T
Q)
K
P
, K
D
> 0

H Hurwitz, per cui Q > 0 P > 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Legge di controllo (a struttura variabile)
w =

|z|
z > 0

z
T
( w) = z
T


|z|
z
T
z (div. per |z| si ha dip. lineare da z)
|z||| |z|
= |z|(|| )
Scelta di
|| q, q, q
d
stima dellincertezza
|| |I B
1

B|
_
| q
d
| +|K| || +|w|
_
+|B
1
| | n|
Q
M
+|K| || + +B
M


1
1
(Q
M
+|K||| +B
M
)

V =
T
P + 2z
T
_


|z|
z
_
< 0 ,= 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
sottospazio di scivolamento (attrattivo)
Eliminazione delle componenti in alta frequenza causate dal
tempo di risposta nito degli attuatori (chattering)
w =
_

|z|
z per |z|

z per |z| <


ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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) q
r
+C(q, q) q
r
+F q
r
+g(q) +K
D

q
r
= q
d
+ q
q
r
= q
d
+

q
= q
r
q =

q + q

B(q) +C(q, q) +F +K
D
= 0
a ciclo chiuso il sistema ` e ancora non lineare ma con la
stessa dinamica del ciclo aperto (approccio passivo)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Metodo di Lyapunov
V (, q) =
1
2

T
B(q) +
1
2
q
T
M q > 0 , q ,= 0

V =
T
B(q) +
1
2

T

B(q) + q
T
M

q
=
T
(F +K
D
) + q
T
M

q
=
T
F

q
T
K
D

q q
T
K
D
q


V = 0 solo per q =

q 0 = [ q
T

T
]
T
= 0
globalmente asintoticamente stabile
n qui abbiamo ipotizzato sempre di conoscere esattamente
il modello
se invece ipotizziamo di conoscere solo una stima dei
parametri dinamici...
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo basato su stime dei parametri
u =

B(q) q
r
+

C(q, q) q
r
+

F q
r
+ g +K
D

= Y (q, q, q
r
, q
r
) +K
D

B(q) +C(q, q) +F +K
D

B(q) q
r


C(q, q) q
r


F q
r
g(q)
= Y (q, q, q
r
, q
r
)
Modica di V
V (, q, ) =
1
2

T
B(q)+ q
T
K
D
q+
1
2

T
K

> 0 , q, ,= 0

V =
T
F

q
T
K
D

q q
T
K
D
q
+
T
_
K


Y
T
(q, q, q
r
, q
r
)
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Legge di adattamento

= K
1

Y
T
(q, q, q
r
, q
r
)

V =
T
F

q
T
K
D

q q
T
K
D
q
q 0
Y (q, q, q
r
, q
r
)( ) 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CONTROLLO NELLO SPAZIO OPERATIVO
speciche di moto
relazioni cinematiche
carico computazionale
Schemi di principio
Controllo con inversa dello Jacobiano
Controllo con trasposta dello Jacobiano
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo PD con compensazione di gravit` a
Regolazione ad una posizione di equilibrio costante x
d
Metodo di Lyapunov
V ( q, x) =
1
2
q
T
B(q) q +
1
2
x
T
K
P
x > 0 q, x ,= 0

V = q
T
B(q) q +
1
2
q
T

B(q) q +

x
T
K
P
x
= q
T
B(q) q +
1
2
q
T

B(q) q q
T
J
T
A
(q)K
P
x
= q
T
F q + q
T
_
u g(q) J
T
A
(q)K
P
x
_
scelta del controllo
u = g(q) +J
T
A
(q)K
P
x J
T
A
K
D
J
A
(q) q

V = q
T
F q q
T
J
T
A
(q)K
D
J
A
(q) q

V = 0 J
T
A
(q)K
P
x = 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo a dinamica inversa
Modello
B(q) q +n(q, q) = u
Controllo linearizzante
u = B(q)y +n(q, q)

q = y
= J
1
A
(q)
_
x
d
+K
D

x +K
P
x

J
A
(q, q) q
_

x +K
D

x +K
P
x = 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CONFRONTO TRA GLI SCHEMI DI
CONTROLLO
Manipolatore planare a due bracci
a
1
= a
2
= 1 m
1
=
2
= 0.5 m
m

1
= m

2
= 50 kg I

1
= I

2
= 10 kgm
2
k
r1
= k
r2
= 100
m
m
1
= m
m
2
= 5 kg I
m
1
= I
m
2
= 0.01 kg m
2
F
m1
= F
m2
= 0.01 Nms/rad R
a1
= R
a2
= 10 ohm
k
t1
= k
t2
= 2 Nm/A k
v1
= k
v2
= 2 Vs/rad
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Schemi di controllo
A. Indipendente ai giunti pos + vel
K
P
= 5 K
V
= 10 k
TP
= k
TV
= 1
B. Indipendente ai giunti pos + vel + acc
K
P
= 5 K
V
= 10 K
A
= 2 k
TP
= k
TV
= k
TA
= 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
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
F. PD + compensazione di gravit` a + K
D
q
d
K
P
= 3750I K
D
= 750I
G. Dinamica inversa
K
P
= 25I K
D
= 5I
H. Robusto (

B =

B, n = F
v
q +g)
K
P
= 25I K
D
= 5I = 70 = 0.004
I. Come H = 0.01
J. Adattativo
m
2
= 10 kg m
2

C2
= 11 kg m

I
2
= 12.12 kg m
2
= 5I K
D
= 750I K

= 0.01I
K. PD cartesiano + compensazione di gravit` a + K
D
x
d
K
P
= 16250I K
D
= 3250I
L. Dinamica inversa cartesiano
K
P
= 25I K
D
= 5I
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3 4
3
2.5
2
1.5
1
0.5
0
[s]
[
r
a
d
]
pos giunto 1
0 1 2 3 4
0
0.5
1
1.5
2
2.5
3
[s]
[
r
a
d
]
pos giunto 2
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0.1
0
0.1
0.2
[s]
[
m
]
errori pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0
0.005
0.01
0.015
[s]
[
m
]
norma err pos
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0
0.005
0.01
0.015
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0
0.005
0.01
0.015
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3 4
3
2.5
2
1.5
1
0.5
0
[s]
[
r
a
d
]
pos giunto 1
0 1 2 3 4
0
0.5
1
1.5
2
2.5
3
[s]
[
r
a
d
]
pos giunto 2
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0
0.02
0.04
0.06
0.08
0.1
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0
1
2
3
4
x 10
4
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0
1
2
3
4
x 10
4
[s]
[
m
]
norma err pos
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0
1
2
3
4
x 10
4
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3 4
0
0.002
0.004
0.006
0.008
0.01
[s]
[
m
]
norma err pos
0 1 2 3 4
11
12
13
14
15
16
17
[s]
norma err parametri
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0
0.02
0.04
0.06
0.08
0.1
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3 4
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 1 2 3 4
0
1
2
3
4
x 10
4
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 2 4 6 8 10
3
2.5
2
1.5
1
0.5
0
[s]
[
r
a
d
]
pos giunto 1
0 2 4 6 8 10
0
0.5
1
1.5
2
2.5
3
[s]
[
r
a
d
]
pos giunto 2
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0.05
0
0.05
[s]
[
m
]
errori pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0
1
2
3
4
x 10
3
[s]
[
m
]
norma err pos
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0
1
2
3
4
x 10
3
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0
1
2
3
4
x 10
3
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 2 4 6 8 10
3
2.5
2
1.5
1
0.5
0
[s]
[
r
a
d
]
pos giunto 1
0 2 4 6 8 10
0
0.5
1
1.5
2
2.5
3
[s]
[
r
a
d
]
pos giunto 2
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0
0.005
0.01
0.015
0.02
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0
0.2
0.4
0.6
0.8
1
x 10
4
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0
0.2
0.4
0.6
0.8
1
x 10
4
[s]
[
m
]
norma err pos
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0
0.2
0.4
0.6
0.8
1
x 10
4
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 2 4 6 8 10
0
1
2
3
4
5
x 10
3
[s]
[
m
]
norma err pos
0 2 4 6 8 10
12
13
14
15
16
17
[s]
norma err parametri
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0
0.005
0.01
0.015
0.02
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 2 4 6 8 10
500
0
500
1000
1500
[s]
[
N
m
]
coppie giunti
0 2 4 6 8 10
0
0.2
0.4
0.6
0.8
1
x 10
4
[s]
[
m
]
norma err pos
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CONTROLLO DI FORZA
Interazione del manipolatore con lambiente
Controllo di cedevolezza
Controllo di impedenza
Controllo di forza
Moto vincolato
Vincoli naturali e vincoli articiali
Controllo ibrido forza/moto
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
INTERAZIONE DEL MANIPOLATORE CON
LAMBIENTE
Moto vincolato
Controllo posizionale (?)
errori di posizionamento
incertezza sullambiente
Contatto instabile
Misura di forza
Strategie di controllo
indirette
dirette
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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
e
allequilibrio
J
T
A
(q)K
P
x = J
T
(q)h
e

x = K
1
P
T
T
A
(x)h
e
= K
1
P
h
A
K
1
P
cedevolezza attiva del controllo
lineare nei riguardi di f
torsionale nei riguardi di
Se h
e
A(J
T
) = x = 0 h
e
,= 0
problemi di accoppiamento e singolarit di rappresentazione
causati dalla rappresentazione dellorientamento
T
T
A
(x)K
P
x = h
e
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Cedevolezza passiva
spostamento elementare tra due corpi rigidi Re S accoppiati
elasticamente (terna di riferimento indifferente)
dx
r,s
=
_
do
r,s

r,s
dt
_
= v
r,s
dt

h
s
=
_
f
s

s
_
=
_
K
f
K
c
K
T
c
K

_ _
do
r,s

r,s
dt
_
= Kdx
r,s
h
r
= h
s
Matrice di rigidezza K (simmetrica e semi-denita positiva)
K
f
rigidezza traslazionale
K

rigidezza rotazionale
K
c
rigidezza di accoppiamento
Matrice di cedevolezza C
dx
r,s
= Ch
s
Remote Center of Compliance (RCC)
K
c
= O (Video: /AA06-07/2-Compensator-demo.wmv)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Cedevolezza attiva
diversa denizione di spostamento: posizione e orientamento
terna utensile rispetto a terna desiderata
T
d
e
= (T
d
)
1
T
e
=
_
R
d
e
o
d
d,e
0
T
1
_
R
d
e
= R
T
d
R
e
o
d
d,e
= R
T
d
(o
e
o
d
)

x =
_
o
d
d,e

d,e
_
siccome R
d
e o
d
sono costanti
o
d
d,e
= R
T
d
o
e
,

d,e
= T
1
(
d,e
)
d
d,e
= T
1
(
d,e
)R
T
d

x = J
A
d
(q, x) q
J
A
d
(q, x) = T
1
A
(
d,e
)
_
R
T
d
O
O R
T
d
_
J(q)
Controllo PD con compensazione di gravit` a
u = g(q) +J
T
A
d
(q, x)K
P
x J
T
A
d
K
D
J
A
d
(q, x) q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Analisi di stabilit` a
V =
1
2
q
T
B(q) q +
1
2
x
T
K
P
x > 0 q, x ,= 0,
allequilibrio
J
T
A
d
(q)K
P
x = J
T
(q)h
e
h
d
e
= T
T
A
(
d,e
)K
P
x
in termini di spostamenti elementari
d x =

x

x=0
dt = T
1
A
(0)(v
d
d
v
d
e
)dt = T
1
A
(0)dx
e,d
scegliendo gli angoli di Eulero XYZ (T
A
(0) = I)

h
e
= K
P
dx
e,d
dx
e,d
= K
1
P
h
e
Modello dellambiente elasticamente cedevole
h
e
= Kdx
r,e
K rigidezza dellambiente
semi-denita positiva (solo per le direzioni di moto
vincolato)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Cedevolezza attiva vs. cedevolezza passiva . . . allequilibrio
h
e
=
_
I
6
+KK
1
P
_
1
Kdx
r,d
dx
e,d
= K
1
P
_
I
6
+KK
1
P
_
1
Kdx
r,d
manipolatore cedevole
ambiente cedevole
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
K = K
f
= diagk
x
, 0 K
P
= diagk
Px
, k
Py

Allequilibrio
f
e
=
_
_
k
Px
k
x
k
Px
+k
x
(x
d
x
r
)
0
_
_
o
e
=
_
k
Px
x
d
+k
x
x
r
k
Px
+k
x
y
d
_
se k
Px
/k
x
1
x
e
x
d
f
x
k
x
(x
d
x
r
)
se k
Px
/k
x
1
x
e
x
r
f
x
k
Px
(x
d
x
r
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Cedevolezza attiva nello spazio dei giunti
allequilibrio
K
P
q = J
T
(q)h
e

q = K
1
P
J
T
(q)h
e
d x = J(q)K
1
P
J
T
(q)h
e
Video: AA10-11/Materiale Fabrizio/Force/Force/PRISMAMov
- Compliance Control.WMV
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CONTROLLO DI IMPEDENZA
Controllo a dinamica inversa nello spazio operativo
u = B(q)y +n(q, q)

q = y B
1
(q)J
T
(q)h
e
scelta del nuovo ingresso di controllo
y = J
1
A
(q)M
1
d
_
M
d
x
d
+K
D

x +K
P
x M
d

J
A
(q, q) q
_
M
d

x +K
D

x +K
P
x = M
d
B
1
A
(q)h
A
B
A
(q) = J
T
A
(q)B(q)J
1
A
(q)
Impedenza meccanica
massa M
d
smorzamento K
D
rigidezza K
P
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Integrazione della misura della forza di contatto
u = B(q)y +n(q, q) +J
T
(q)h
e
y = J
1
A
(q)M
1
d
_
M
d
x
d
+K
D

x+K
P
xM
d

J
A
(q, q) qh
A
_

M
d

x +K
D

x +K
P
x = h
A
impedenza lineare attiva
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Per evitare problemi di singolarit` a di rappresentazione e
accoppiamenti dipendenti dallorientamento . . .
terna desiderata tempovariante

x = J
A
d
(q, x) q +b( x, R
d
, o
d
,
d
)
b( x, R
d
, o
d
,
d
) =
_
R
T
d
o
d
+S(
d
d
)o
d
d,e
T
1
(
d,e
)
d
d
_

x = J
A
d
q

J
A
d
q +

b
nuovo ingresso di controllo
y = J
1
A
d
M
1
d
_
K
D

x +K
P
x M
d

J
A
d
q +M
d

b h
d
e
_

M
d

x +K
D

x +K
P
x = h
d
e
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Dispositivi meccanici di impedenza passiva
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
Controllo di impedenza con misura di forza
M
d
= diagm
dx
, m
dy

K
D
= diagk
Dx
, k
Dy

K
P
= diagk
Px
, k
Py

dinamica del sistema manipolatoreambiente lungo le due


direzioni dello spazio operativo (x
d
costante)
m
dx
x
e
+k
Dx
x
e
+ (k
Px
+k
x
)x
e
= k
x
x
r
+k
Px
x
d
m
dy
y
e
+k
Dy
y
e
+k
Py
y
e
= k
Py
y
d
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Ambiente (x
r
= 1)
pi ` u cedevole k
x
= 10
3
N/m
meno cedevole k
x
= 10
4
N/m
Controllo
m
dx
= m
dy
= 100
k
Dx
= k
Dy
= 500
k
Px
= k
Py
= 2500
Manipolatore
x
e
= [ 1 0 ]
T
x
d
= [ 1.1 0.1 ]
T
0 1 2 3
0
0.05
0.1
[s]
[
m
]
pos organo terminale y
0 1 2 3
0
50
100
150
200
250
300
[s]
[
N
]
forza organo terminale x
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo di ammettenza
Impedenza meccanica tra terna desiderata e terna cedevole t
M
t

z +K
Dt

z +K
Pt
z = h
d
e
comportamento cedevole durante linterazione conlambiente
buona robustezza dellanello interno di controllo del moto
(maggiore banda passante)
Video: AA10-11/Materiale Fabrizio/Force/Force/PRISMAMov
- Impedance Control.WMV
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CONTROLLO DI FORZA
Controllo di cedevolezza o impedenza = controllo
indiretto di forza
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
Ipotesi: sole variabili di posizione
modello di ambiente elastico
f
e
= K(x
e
x
r
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo di forza con anello interno di posizione
u = B(q)y +n(q, q) +J
T
(q)h
e
y = J
1
(q)M
1
d
_
K
D
x
e
+K
P
(x
F
x
e
) M
d

J(q, q) q
_

M
d
x
e
+K
D
x
e
+K
P
x
e
= K
P
x
F
Scelta del controllo
x
F
= C
F
(f
d
f
e
)

M
d
x
e
+K
D
x
e
+K
P
(I
3
+C
F
K)x
e
= K
P
C
F
(Kx
r
+f
d
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Azione PI
C
F
= K
F
+K
I
_
t
() d
scelta di K
D
, K
P
, K
F
, K
I
= margini di stabilit` a
+ banda passante
Allequilibrio (stabile)
f
e
= f
d
Kx
e
= Kx
r
+f
d
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo di forza con anello interno di velocit` a
y = J
1
(q)M
1
d
_
K
D
x
e
+K
P
x
F
M
d

J(q, q) q
_
x
F
= K
F
(f
d
f
e
)

M
d
x
e
+K
D
x
e
+K
P
K
F
Kx
e
= K
P
K
F
(Kx
r
+f
d
)
Allequilibrio (stabile)
f
e
= f
d
Kx
e
= Kx
r
+f
d
attenzione dinamiche non modellate (assenza azione
integrale)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo parallelo forza/posizione
se f
d
/ 1(K) = deriva di x
e
se f
d
1(K) = x
e
(o x
e
) 0
Aggiunta di x
d
y = J
1
(q)M
1
d
_
K
D
x
e
+K
P
( x +x
F
) M
d

J
A
(q, q) q
_
Allequilibrio
f
e
= f
d
x
e
= x
d
+C
F
_
K(x
r
x
e
) +f
d
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
Impedenza (M
d
, K
D
, K
P
) come sopra
Riferimenti f
d
= [ 10 0 ]
T
x
d
= [ 1.015 0.1 ]
T
Controllo di forza
C
F
= diagc
Fx
, 0
1. controllo con anello interno di posizione
k
Fx
= 0.00064 k
Ix
= 0.0016
2. controllo con anello interno di velocit` a
k
Fx
= 0.0024
3. controllo parallelo
k
Fx
= 0.00064 k
Ix
= 0.0016
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
0 1 2 3
1
1.005
1.01
1.015
1.02
[s]
[
m
]
pos organo terminale x
0 1 2 3
0
10
20
30
40
50
[s]
[
N
]
forza organo terminale x
0 1 2 3
1
1.005
1.01
1.015
1.02
[s]
[
m
]
pos organo terminale x
0 1 2 3
0
10
20
30
40
50
[s]
[
N
]
forza organo terminale x
0 1 2 3
1
1.005
1.01
1.015
1.02
[s]
[
m
]
pos organo terminale x
0 1 2 3
0
10
20
30
40
50
[s]
[
N
]
forza organo terminale x
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
MOTO VINCOLATO
Situazione di contatto reale
vincoli cinematici imposti dallambiente sul moto
dellorgano terminale
durante il moto vincolato, lorgano terminale pu` o esercitare
forze e momenti di tipo dinamico
la forza e il momento di contatto possono dipendere
dalla cedevolezza strutturale del manipolatore, a quella del
sensore di forza o di dispositivi
deformazioni locali delle parti a contatto (aree di contatto
distribuito), attrito statico e dinamico
Ipotesi semplicative per il controllo
manipolatore + ambiente perfettamente rigidi, vincoli
puramente cinematici
manipolatore perfettamente rigido +cedevolezza localizzata
nellambiente
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Ambiente rigido
Equazioni di vincolo (vincoli olonomi)
(q) = 0

(q) q = 0
J

(q) = /q: Jacobiano di vincolo (m6)


In assenza di attrito
J

(q)q = 0
principio dei lavori virtuali
= J
T

(q)
(ipotesi di contatto mantenuto)
h
e
= J
T
(q) = S
f
(q)
= S

f
(q)h
e
S

f
= (S
T
f
WS
f
)
1
S
T
f
W
Problema invarianza della soluzione
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
J

(q) q = 0
S
f
= J
T
(q)J
T

(q)

(q)J
1
(q)J(q) q = S
T
f
v
e
= 0
Reciprocit` a
h
T
e
v
e
= 0
1(S
f
) sottospazio di controllo di forza
1(S
v
) sottospazio di controllo di velocit ` a
v
e
= S
v
(q)
S
T
f
(q)S
v
(q) = O
= S

v
(q)v
e
Matrici di selezione: S
f
, S
v
, S

f
, S

v
proiettore in 1(S
f
): P
f
= S
f
S

f
proiettore in 1(S
v
): P
v
= S
v
S

v
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Ambiente cedevole
Moto parzialmente vincolato
ambiente modellato come coppia di corpi rigidi S e R
h
e
= S
f

dx
r,s
= Ch
e
spostamento elementare rispetto alla posa di equilibrio
dx
r,e
= dx
v
+dx
f
dx
v
= P
v
dx
r,e
dx
f
= (I
6
P
v
)dx
r,e
= (I
6
P
v
)dx
r,s
S
T
f
dx
r,e
= S
T
f
dx
r,s
= S
T
f
CS
f

h
e
= K

dx
r,e
K

= S
f
(S
T
f
CS
f
)
1
S
T
f
dx
f
= C

h
e
C

= (I
6
P
v
)C
possibilit` a di contatto rigido in alcune direzioni
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
VINCOLI NATURALI E VINCOLI
ARTIFICIALI
Riferimenti di forza e velocit` a scelti in maniera congruente
allambiente
non ` e possibile imporre sia una forza che una velocit` a lungo
la stessa direzione
Analisi cineto-statica dellinterazione
vincoli naturali imposti dallambiente lungo ciascun grado
di libert` a del compito (velocit` a o forza)
vincoli articiali soggetti al controllo lungo ciascun grado
di libert` a del compito (forza o velocit` a)
Terna di vincolo O
c
x
c
y
c
z
c
descrizione semplicata del compito in termini di vincoli
naturali e vincoli articiali
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Analisi di casi
Scivolamento su supercie piana
Vincoli Vincoli
Naturali Articiali
o
c
z
f
c
z

c
x

c
x

c
y

c
y
f
c
x
o
c
x
f
c
y
o
c
y

c
z

c
z
S
f
=
_

_
0 0 0
0 0 0
1 0 0
0 1 0
0 0 1
0 0 0
_

_
S
v
=
_

_
1 0 0
0 1 0
0 0 0
0 0 0
0 0 0
0 0 1
_

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
in caso di piano elasticamente cedevole (lungo z
c
, intorno a
x
c
, y
c
)
K

m
=
_
_
c
3,3
c
3,4
c
3,5
c
4,3
c
4,4
c
4,5
c
5,3
c
5,4
c
5,5
_
_
1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Inserimento in foro cilindrico
Vincoli Vincoli
Naturali Articiali
o
c
x
f
c
x
o
c
y
f
c
y

c
x

c
x

c
y

c
y
f
c
z
o
c
z

c
z

c
z
S
f
=
_

_
1 0 0 0
0 1 0 0
0 0 0 0
0 0 1 0
0 0 0 1
0 0 0 0
_

_
S
v
=
_

_
0 0
0 0
1 0
0 0
0 0
0 1
_

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Rotazione di una manovella
Vincoli Vincoli
Naturali Articiali
o
c
x
f
c
x
o
c
z
f
c
z

c
x

c
x

c
y

c
y
f
c
y
o
c
y

c
z

c
z
S
f
=
_

_
1 0 0 0
0 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
0 0 0 0
_

_
S
v
=
_

_
0 0
1 0
0 0
0 0
0 0
0 1
_

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
CONTROLLO IBRIDO FORZA/MOTO
Modello dinamico nello spazio operativo
v
e
= J(q) q +

J(q) q
B
e
(q) v
e
+n
e
(q, q) =
e
h
e
B
e
= J
T
BJ
1
n
e
= J
T
(C q +g) B
e

J q
Ambiente cedevole
dx
r,e
= P
v
dx
r,e
+C

S
f
.
(terna r ssa)
v
e
= S
v
+C

S
f

= S

v
(q)v
e
C

= (I
6
P
v
)C
terna comune terna base,

S
v
= O,

S
f
= O,

C

= O
v
e
= S
v
+C

S
f

controllo a dinamica inversa

e
= B
e
(q)+n
e
(q, q) +h
e
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE

v
e
=
Controllo ibrido forza/velocit ` a
= S
v

+C

S
f
f

= f

controllo di velocit` a

=
d
+K
P
(
d
) +K
I
_
t
0
(
d
() ())d
controllo di forza
f

d
+K
D
(


) +K
P
(
d
)

= S

h
e
. . .

= S

f
K

J(q) q
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
funziona anche in caso di stima disponibile

C

ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE


Esempio
S
c
f
=
_
0
1
_
S
c
v
=
_
1
0
_
P
c
f
=
_
0 0
0 1
_
P
c
v
=
_
1 0
0 0
_
K
c
=
_
0 0
0 c
1
2,2
_
C
c
=
_
0 0
0 c
2,2
_
R
c
=
_
1/

2 1/

2
1/

2 1/

2
_
v
c
e
= S
c
v
+C
c
S
c
f

ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE


se f
c
e
= [ f
c
x
f
c
y
]
T
, v
c
e
= [ o
c
x
o
c
y
]
T
= o
c
x
, = f
c
y
in terna base
S
f
= R
c
S
c
f
=
_
1/

2
1/

2
_
S
v
= R
c
S
c
v
=
_
1/

2
1/

2
_
C

= R
c
C
c
R
T
c
= c
2,2
_
1/2 1/2
1/2 1/2
_
controllo ibrido forza/velocit` a
= o
c
x
=


=

f
c
y
= f

ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE


Ambiente rigido
B
e
(q) v
e
+n
e
(q, q) =
e
h
e
h
e
= S
f

= B
f
(q)
_
S
T
f
B
1
e
(q)(
e
n
e
(q, q)) +

S
T
f
v
e
_
B
f
= (S
T
f
B
1
e
S
f
)
1
B
e
(q) v
e
+S
f
B
f
(q)

S
T
f
v
e
= P(q)(
e
n
e
(q, q))
P = I
6
S
f
B
f
S
T
f
B
1
e
modello dinamico di ordine ridotto
v
e
= S
v
+

S
v

6 m equazioni indipendenti
B
v
(q) = S
T
v
_

e
n
e
(q, q) B
e
(q)

S
v

_
= B
f
(q)S
T
f
B
1
e
(q)
_

e
n
e
(q, q) B
e
(q)

S
v

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo ibrido forza/velocit ` a

e
= B
e
(q)S
v

v
+S
f
f

+n
e
(q, q) +B
e
(q)

S
v

= f

controllo di forza
f

=
d
+K
P
(
d
)
f

=
d
+K
I
_
t
0
(
d
() ())d
controllo di posizione

= r
d
+K
Dr
( r
d
) +K
Pr
(r
d
r)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ATTUATORI E SENSORI
Sistema di attuazione dei giunti
Servomotori
Sensori
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
SISTEMA DI ATTUAZIONE DEI GIUNTI
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Organi di trasmissione
Moto dei giunti
basse velocit` a
elevate coppie
Ruote dentate
variano lasse di rotazione e/o traslano il punto di
applicazione
denti a sezione larga e alberi tozzi (deformazioni limitate)
Coppie vitemadrevite
convertono il moto di rotazione in moto di traslazione
viti a ricircolazione di sfere precaricate (riduzione attriti,
aumento rigidezza, riduzione gioco)
Cinghie dentate
consentono di allocare il motore lontano dallasse del giunto
attuato
alte velocit` a e basse forze (deformabili)
Catene
consentono di allocare il motore lontano dallasse del giunto
attuato
basse velocit` a (vibrazione)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Accoppiamento diretto
si eliminano elasticit` a e gioco
algoritmi di controllo pi ` u sosticati
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Motori per robot industriali
bassa inerzia ed elevato valore del rapporto potenza/peso
elevata possibilit` a di sovraccarico e sviluppo di coppie
impulsive
capacit` a 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 pistonecilindro (moti di traslazione)
servomotori idraulici con pistoni assiali o radiali
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Servomotori elettrici
Pro:
disponibilit` a diffusa di sorgente di alimentazione
costo contenuto e vasta gamma di prodotti
buon rendimento di conversione di potenza
facile manutenzione
assenza di inquinamento dellambiente di lavoro
Contro:
problemi di surriscaldamento in situazioni statiche a
causa delleffetto della gravit` a sul manipolatore (freni di
stazionamento)
necessit` a di particolari protezioni per impiego in ambienti
inammabili
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Servomotori idraulici
Pro:
non presentano problemi di surriscaldamento in situazioni
statiche
sono autolubricati e il uido circolante facilita lo
smaltimento del calore
sono intrinsecamente sicuri in ambienti pericolosi
hanno un ottimo rapporto potenza/peso
Contro:
necessit` a di una centrale idraulica di alimentazione
costo elevato, ridotta gamma di prodotti, difcolt` a di
miniaturizzazione
basso rendimento nella conversione di potenza
necessit` a di manutenzione periodica
inquinamento dellambiente di lavoro per perdite di olio
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Servomotori elettrici
buone caratteristiche dinamiche
ottima essibilit` a di controllo
necessit` a di organi di trasmissione
Servomotori idraulici
caratteristiche dinamiche dipendenti dalla temperatura del
uido in pressione
coppie elevate a basse velocit` a
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Amplicatori di potenza
Funzione di modulazione del usso di potenza dallalimentazione
verso lattuatore (sotto lazione del controllo)
Servomotori elettrici
amplicatori a transistori
convertitori controllati c.c./c.c. (chopper)
convertitori controllati c.c./c.a. (inverter)
Servomotori idraulici
servovalvole elettroidrauliche
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Sorgenti di alimentazione
Azionamenti elettrici
trasformatore + ponte raddrizzatore (non controllato)
Azionamenti idraulici
pompa azionata da motore primo (asincrono trifase) a
velocit` a costante + serbatoio di accumulazione + ltri +
valvole
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
SERVOMOTORI
Servomotori elettrici
Servomotore a corrente continua a magneti permanenti
un circuito statorico che genera il usso 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 usso 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 sullalbero motore,
genera la sequenza di alimentazione delle fasi del circuito
di armatura in dipendenza del moto di rotazione del rotore
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Servomotore elettrico con amplicatore
Equilibrio elettrico
V
a
= (R
a
+sL
a
)I
a
+V
g
V
g
= k
v

Equilibrio meccanico
C
m
= (sI
m
+F
m
) +C
r
C
m
= k
t
I
a
Amplicatore di potenza
V
a
V
c
=
G
v
1 +sT
v
Retroazione di corrente
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Generatore controllato di velocit` a
k
i
= 0
F
m
k
v
k
t
/R
a
K = C
i
(0)G
v
C
r
= 0
=
K
k
v
1 +s
R
a
I
m
k
v
k
t
V

c

R
a
k
v
k
t
1 +s
R
a
I
m
k
v
k
t
C
r
a regime:

K
k
v
v

c
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Protezione di corrente
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Generatore controllato di coppia
Kk
i
R
a
k
v
/Kk
i
0
=
k
t
k
i
F
m
1 +s
I
m
F
m
V

c

1
F
m
1 +s
I
m
F
m
C
r
a regime:
c
m

k
t
k
i
_
v

k
v
K

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Servomotori idraulici
Coppie pistonecilindro
servomotori lineari
servomotori rotanti
Motore idraulico con servovalvola e distributore
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
SENSORI
Sensori propriocettivi
posizioni dei giunti
velocit` a dei giunti
coppia ai giunti
Sensori esterocettivi
sensori di forza
sensori tattili
sensori di prossimit` a
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)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Trasduttori di posizione
Spostamenti lineari (robot di misura)
potenziometri
trasformatori differenziali (LVDT)
inductosyn
Spostamenti angolari
potenziometri
encoder (dischi codicatori)
resolver
synchro
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Encoder assoluto
# Codice # Codice
0 0000 8 1100
1 0001 9 1101
2 0011 10 1111
3 0010 11 1110
4 0110 12 1010
5 0111 13 1011
6 0101 14 1001
7 0100 15 1000
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Encoder incrementali
Ricostruzione della velocit` a (impulso in corrispondenza di ogni
transizione)
convertitore frequenza/tensione (uscita analogica)
misura della frequenza (digitale)
misura del periodo di campionamento (digitale)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Trasduttori di velocit` a
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
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Sensori di forza
Estensimetro (strain gage)
V
o
=
_
R
2
R
1
+R
2

R
s
R
3
+R
s
_
V
i
impiego di una coppia di estensimetri (riduzione degli effetti
di variazioni di temperatura)
Sensore di coppia allalbero
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 essionale)
coppia misurata senza effetti inerziali e di attrito del
servomotore
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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
c

c
c
_
=
_
R
c
s
O
S(r
c
cs
)R
c
s
R
c
s
_ _
f
s
s

s
s
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
sensore di forza a croce di Malta
_

_
f
s
x
f
s
y
f
s
z

s
x

s
y

s
z
_

_
=
_

_
0 0 c
13
0 0 0 c
17
0
c
21
0 0 0 c
25
0 0 0
0 c
32
0 c
34
0 c
36
0 c
38
0 0 0 c
44
0 0 0 c
48
0 c
52
0 0 0 c
56
0 0
c
61
0 c
63
0 c
65
0 c
67
0
_

_
_

_
w
1
w
2
w
3
w
4
w
5
w
6
w
7
w
8
_

_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Sensori di visione
Telecamera
Elemento fotosensibile (pixel)
struttura a matrice rettangolare
CCD
carica = integrale dellilluminazione incidente
CMOS
carica = quantit` a di illuminazione incidente
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
p
c
= T
c
b
p
Trasformazione prospettica
X
f
=
fp
c
x
p
c
z
Y
f
=
fp
c
y
p
c
z
aberrazioni
distorsione geometrica
il segno meno pu essere eliminato denendo un
piano immagine ttizio davanti alla lente (trasformazione
prospettica frontale)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
campionamento temporale (otturatore)
campionamento spaziale (pixel)
X
I
=

x
fp
c
x
p
c
z
+X
0
Y
I
=

y
fp
c
y
p
c
z
+Y
0
tale relazione non lineare diventa lineare nelle coordinate
omogenee (x
I
, y
I
, )
x
I
= X
I
, y
I
= Y
I
_
_
x
I
y
I

_
_
=
_
_
X
I
Y
I
1
_
_
=
_

_
p
c
x
p
c
y
p
c
z
1
_

_
= T
c
b
p
=
_
_
f
x
0 X
0
0 f
y
Y
0
0 0 1
_
_
=
_
_
1 0 0 0
0 1 0 0
0 0 1 0
_
_

Matrice di calibrazione
= T
c
b
parametri intrinseci (
x
,
y
, X
0
, Y
0
, f) in
parametri estrinseci in T
b
c
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Ai ni computazionali ` e conveniente usare le coordinate
normalizzate (X, Y )

_
_
X
Y
1
_
_
=
_

_
p
c
x
p
c
y
p
c
z
1
_

_
coincidono con le coordinate metriche (X
f
, Y
f
) per f = 1
legame (invertibile) con le coordinate in pixel
_
_
X
I
Y
I
1
_
_
=
_
_
X
Y
1
_
_
noti i parametri intrinseci () le coordinate normalizzate si
ottengono direttamente dalle coordinate in pixel
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Standard video
CCIR (Europa e Australia): 625 linee, 25 Hz
RS170 (America e Giappone): 525 linee, 30 Hz
segnale video = tensione 1 V piccopicco
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
intensit` a)
Visione ad alto livello
emulazione della conoscenza (riconoscimento di oggetti in
base a modelli CAD)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
UNIT
`
A DI GOVERNO
Architettura funzionale
Ambiente di programmazione
Architettura hardware
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ARCHITETTURA FUNZIONALE
Unit` a di governo tale da conferire al sistema robotico
abilit` a di muovere oggetti sici nellambiente di lavoro,
(capacit` a di manipolazione)
abilit` a di ottenere informazioni sullo stato del sistema e su
quello dellambiente di lavoro (capacit` a sensoriali)
abilit` a di utilizzare informazioni per modicare il
comportamento del sistema in maniera preprogrammata
(capacit` a di comportamento intelligente)
abilit` a di immagazzinare, elaborare e rendere disponibili
dati relativi allattivit` a del sistema (capacit` a di processamento
dati)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Modello di riferimento
Livelli di attivit organizzati gerarchicamente
livelli inferiori dedicati alla realizzazione del movimento
livelli superiori dedicati alla pianicazione delle azioni
Moduli funzionali
Evoluzione verso livelli superiori
compiti complessi
missioni
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Livello del compito
specica 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 sullambiente
Livello delle primitive
sequenza di congurazioni dal livello delle azioni
calcolo traiettorie di moto (riferimenti per i servo)
denizione 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
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Architettura funzionale per robot industriali
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
AMBIENTE DI PROGRAMMAZIONE
Caratteristiche (comuni con quelli dei calcolatori + ...)
sistema operativo in tempo reale
modellazione del mondo
controllo del moto
lettura dei dati sensoriali
interattivit` a con il sistema sico
capacit` a di rivelazione di errori
capacit` a di ripristino di situazioni operative corrette
struttura specica dei linguaggi
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Requisiti imposti dallarticolazione 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)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Programmazione per insegnamento
Guida delloperatore lungo il percorso desiderato attraverso
teach pendant
Registrazione delle posizioni dei giunti
Ripetizione del moto
capacit` a di programmazione limitata (tecnico di ofcina)
disponibilit` a del robot
saldatura a punti, verniciatura, palettizzazione
Adozione di linguaggi per superarne le limitazioni principali
acquisizione di posture signicative mediante apprendimento
calcolo della locazione dellorgano 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
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Programmazione orientata al robot
integrazione di numerose funzioni di linguaggi di
programmazione ad alto livello (BASIC, PASCAL) con
quelle speciche 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
(necessit di un ambiente di simulazione della cella)
interazione con altre macchine
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Caratteristiche di linguaggio di programmazione strutturata
editore di testo
strutture complesse di rappresentazione dei dati
uso estensivo di variabili di stato predenite
realizzazione di operazioni di algebra matriciale
uso estensivo di rappresentazioni simboliche per le terne di
coordinate
possibilit` a di specicare 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
capacit` a di parallelizzazione del calcolo
funzioni di controllore logico programmabile (PLC)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ARCHITETTURA HARDWARE
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
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 loperatore 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
nellarea 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)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Funzioni della scheda cinematica
calcolo delle primitive di moto
calcolo della cinematica diretta, della cinematica inversa e
dello Jacobiano
verica di congruenza della traiettoria
gestione della ridondanza cinematica
Funzioni della scheda dinamica
calcolo della dinamica inversa
Funzioni della scheda servo
microinterpolazione dei riferimenti
calcolo dellalgoritmo di controllo
conversione D/A ed interfaccia con amplicatori 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
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ROBOT MOBILI
Vincoli anolonomi
Modello cinematico
Pianicazione
Controllo del moto
Localizzazione odometrica
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Robot mobili
I problemi fondamentali della robotica mobile
localizzazione (dove sono?)
pianicazione del percorso e della traiettoria (come
raggiungo lobiettivo?)
controllo del moto (come faccio a muovermi?)
Saper risolvere simultaneamente i tre problemi di sopra in
ambienti
incerti
non strutturati
dinamici

Autonomia
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Architettura deliberativa
Percezione
propriocettiva: posizione, orientamento, velocit` a del robot
stesso
esterocettiva: posizione relativa di ostacoli, altri robot,
persone
ampia gamma di sensori
propriocettivi: encoders, inertial navigation systems (INS),
global positioning systems (GPS)
esterocettivi: rangenders, videocamere, sensori tattili,
sensori di prossimit` a
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Altre architetture
architettura reattiva
non pensare, (re-)agisci
architettura ibrida
pensa ed agisci simultaneamente
architettura comportamentale
pensa alla modalit ` a di azione
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Vincoli anolonomi
Vincoli cinematici che limitano la mobilit` a locale
non impediscono di raggiungere qualsiasi punto dello spazio
delle congurazioni (in assenza di ostacoli)
q c
localmente q IR
n
(lo spazio delle congurazioni ` e una
variet` a topologica (manifold))
Vincoli olonomi o (integrabili)
h
i
(q) = 0 i = 1, . . . , k < n
la dimensione dello spazio delle congurazioni si riduce
Vincoli cinematici
a
i
(q, q) = 0 i = 1, . . . , k < n
limitano i moti istantanei ammissibili
forma Pfafana (lineare nelle velocit` a)
a
T
i
(q) q = 0 i = 1, . . . , k < n
a
i
: c IR
n
di classe C

o in forma matriciale
A
T
(q) q = 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Se un sistema meccanico ` e soggetto a vincoli olonomi allora ` e
soggetto anche a vincoli cinematici (in forma Pfafana)
h
i
(q) = 0
dh
i
(q)
dt
=
h
i
(q)
q
q = 0 i = 1, . . . , k
Il viceversa ` e vero solo se il vincolo cinematico ` e integrabile,
altrimenti esso di dice anolonomo
condizione di integrabilit` a per un singolo vincolo Pfafano
h(q) :
h
q
= (q)a
T
(q)
con fattore di integrazione (q) ,= 0, e quindi
h(q) = c supercie di livello di dimensione n 1
perdita di accessibilit` a dello spazio delle congurazioni

vincolo olonomo
I vincoli anolonomi non riducono la dimensione dello spazio
delle congurazioni per` o limitano la mobilit` a del sistema
sono ammissibili solo le velocit` a nel A(a
T
(q))
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio

Disco che rotola senza strisciare


coordinate generalizzate
q = [ x y ]
T
vincolo cinematico (puro rotolamento)
xsin ycos = [ sin cos 0 ] q = 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE

non c` e perdit` a di accessibilit` a


il disco pu` o essere condotto da una qualunque congurazione
q
i
= [ x
i
y
i

i
]
T
ad una qualunque congurazione q
f
=
[ x
f
y
f

f
]
T
senza violare il vincolo con la sequenza di tre
manovre
ruoto il disco intorno allasse verticale in modo che lasse
sagittale passi per il punto (x
f
, y
f
)
rotolo il disco no a raggiungere (x
f
, y
f
)
ruoto attorno allasse verticale no a che =
f
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Condizioni di integrabilit` a
Per stabilire se il vincolo ` e olonomo o anolonomo, occorre
stabilire se esso ` e integrabile o no
Condizione necessaria e sufciente afnch e un singolo vincolo
Pfafano
a
T
(q) q =
n

j=1
a
j
(q) q
j
= 0
sia integrabile, ` e lesistenza di una funzione scalare h(q) e un
fattore di integrazione (q) ,= 0 tali che
(q)a
j
(q) =
h(q)
q
j
j = 1, . . . , n
Una condizione equivalente che non contiene la funzione
incognita h(q) si ottiene applicando il teorema di Schwarz
sulle derivate seconde
(a
k
)
q
j
=
(a
j
)
q
k
j, k = 1, . . . , n, j ,= k
basta osservare che, per il suddetto teorema
(a
j
)
q
k
=

2
h
q
k
q
j
=

2
h
q
j
q
k
=
(a
k
)
q
j
Nel caso di vincoli Pfafani multipli, useremo un diverso
approccio
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Esempio
Si consideri il vincolo cinematico di puro rotolamento del disco
su un piano orizzontale
xsin ycos = [ sin cos 0 ][ x y

]
T
= 0
applicando la condizione di integrabilit` a
sin

y
= cos

x
cos

= sin
sin

= cos
quadrando e sommando le ultime due risulta

= , per
cui
cos = sin
sin = cos
che ha come unica soluzione = 0 e quindi il vincolo non
` e integrabile per cui ` e anolonomo
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Modello cinematico
Consideriamo un sistema di k vincoli Pfafani
A
T
(q) q = 0
le velocit` a ammissibili sono quelle che appartengono al
A(A
T
(q)) che ha dimensione n k, una cui base sia
g
1
(q), . . . , g
nk
(q)
dunque le velocit` a ammissibili sono tali che
q =
m

j=1
g
j
(q)u
j
= G(q)u m = n k
Tale equazione pu` o essere vista come quella di un sistema
dinamico non lineare del primo ordine con stato q IR
n
e
ingresso di controllo u IR
m
le soluzioni del sistema sono le traiettorie q(t) eseguite dal
sistema meccanico vincolato
esistono innite scelte possibili per G(q) e pu` o accadere
che u non abbia il signicato degli ingressi di controllo
q = G(q)u
modello cinematico
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Si pu` o dimostrare che lanolonomia dei vincoli cinematici ` e
equivalente alla controllabilit ` a del modello cinematico (sistema
non lineare)
q = G(q)u
se il sistema ` e controllabile vuol dire che comunque date
due congurazioni q
i
e q
f
in c, esiste sempre un ingresso
u(t) che porta da q
i
a q
f
, cio` e esiste una traiettoria q(t)
che unisce q
i
e q
f
rispettando i vincoli
se il sistema non ` e controllabile vuol dire che i vincoli
cinematici riducono laccessibilit ` a di c a seconda della
dimensione del sottospazio di accessibilit` a
m < < n vincoli parzialmente integrabili

sistema meccanico anolonomo


m = vincoli completamente integrabili

sistema meccanico olonomo


Lo studio della controllabilit` a dei sistemi non lineari in generale
esula dai limiti di questo corso
nel caso di un singolo vincolo cinematico la condizione di
integrabilit` a coincide con quella di involutivit` a della
= spang
1
, . . . , g
n1

ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE


Uniciclo

`
E un veicolo con una sola ruota che rotola senza strisciare
coordinate generalizzate q = [ x y ]
T
vincolo cinematico di puro rotolamento
xsin ycos = [ sin cos 0 ] q = 0
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Modello cinematico
lo spazio nullo della matrice (vettore riga) A
T
(q) ha per
base g
1
(q) e g
2
(q) colonne della matrice
G(q) = [ g
1
(q) g
2
(q) ] =
_
_
cos 0
sin 0
0 1
_
_
le velocit` a si possono quindi scrivere come combinazione
lineare di tali colonne
_
_
x
y

_
_
=
_
_
cos
sin
0
_
_
v +
_
_
0
0
1
_
_

v ` e la velocit` a lineare del punto di contatto (R

)
` e la velocit` a angolare del robot (rotazione intorno allasse
verticale)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
I robot reali non hanno mai una sola ruota per problemi di
stabilit` a meccanica
veicolo a trazione differenziale
v =
r(
R
+
L
)
2
=
r(
R

L
)
d
r ` e il raggio delle ruote
d ` e la distanza tra i centri delle ruote
il punto medio dellasse delle ruote ` e il punto di coordinate
(x, y)
` e lorientamento delle ruote e quindi del veicolo
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Pianicazione
Separazione cammino-legge oraria
q(t), t [t
i
, t
f
]
q(t
i
) = q
i
, q(t
f
) = q
f
cammino
q(s), s [0, L] : q(0) = q
i
, q(L) = q
f
dq(s)
ds
,= 0 s
legge oraria
s(t), s(t
i
) = 0, s(t
f
) = L
s(t) 0, t [t
i
, t
f
]
la velocit nello spazio delle congurazioni ` e allora parallela
alla tangente al cammino
q =
dq
dt
=
dq
ds
s = q

s
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
I vincoli anolonomi cui ` e soggetto il robot mobile sono allora
A
T
(q) q = A
T
(q)q

s = 0
se s(t) > 0
ammissibilit geometrica
A
T
(q)q

= 0 s
il cammino q(s) ` e la soluzione del sistema dinamico non lineare
q

= G(q) u
u sono gli ingressi geometrici
u(t) = u s(t)
assegnati gli ingressi geometrici il cammino univocamente
determinato
come fare a determinare gli ingressi geometrici tali che il
corrispondente cammino soddisfa il vincolo di anolonomia?
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Uscite piatte
I modelli cinematici di molti robot mobili (tra cui uniciclo e
biciclo) godono della propriet` a di piattezza differenziale
x = f(x) +G(x)u
esiste un insieme di uscite y tali che lo stato e gli ingressi
sono esprimibili algebricamente in funzione di tale uscita e
un certo numero di sue derivate
x = x(y, y, y, . . . , y
(r)
)
u = u(y, y, y, . . . , y
(r)
)
Per il biciclo e luniciclo le uscite piatte sono proprio la
posizione (x, y) nel piano cartesiano
per luniciclo i cammini geometricamente ammissibili sono
le soluzioni del sistema
x

= cos v
y

= sin u

=
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Dato un cammino cartesiano (x(s), y(s)) lo stato ad esso
associato ` e q(s) = [ x(s) y(s) (s) ]
T
, dove
(s) = Atan2(y

(s), x

(s)) +k k = 0, 1
k = 0 corrisponde alla marcia in avanti
k = 1 corrisponde alla marcia indietro
Gli ingressi geometrici sono facilmente ottenibili algebricamente
v(s) =
_
((x

(s))
2
+ (y

(s))
2
(s) =
y

(s)x

(s) x

(s)y

(s)
(x

(s))
2
+ (y

(s))
2
Casi particolari
la scelta del segno di v(s) ` e legata al senso di marcia
se per qualche s risulta x

( s) = y

( s) = 0 lorientamento
e la velocit` a angolare vanno calcolati per continuit` a (come
limite per s s

)
se il cammino degenera in un punto e non sono
ricostrouibili
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Pianicazione di cammini mediante polinomi
cartesiani
Se il modello cinematico di un robot mobile ammette uscite
piatte
si pianica il cammino per tali uscite
dal modello cinematico si ricavano (algebricamente) il
cammino per lo stato e i corrispondenti ingressi geometrici
questa procedura garantisce che il cammino nello spazio
delle congurazioni q(s) soddis i vincoli anolonomi
Pianicazione per un uniciclo
congurazione iniziale q
i
= q(0) = [ x
i
y
i

i
]
T
congurazione nale q
f
= q(1) = [ x
f
y
f

f
]
T
(x, y) sono uscite piatte ` e sufciente interpolare con
due polinomi (cubici) x(s) e y(s) i valori iniziali e nali
x(s) = s
3
x
f
(s 1)
3
x
i
+
x
s
2
(s 1) +
x
s(s 1)
2
y(s) = s
3
y
f
(s 1)
3
y
i
+
y
s
2
(s 1) +
y
s(s 1)
2
per soddisfare le condizioni al contorno sullorientamento
x

(0) = k
i
cos
i
x

(1) = k
i
cos
f
y

(0) = k
i
sin
i
y

(1) = k
i
sin
f
con k
i
e k
f
parametri liberi
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ponendo ad esempio k
i
= k
f
= k > 0 si ottiene
_

y
_
=
_
kcos
f
3x
f
ksin
f
3y
f
_ _

y
_
=
_
kcos
i
+ 3x
i
ksin
i
+ 3y
i
_
il cammino dellorientamento e gli ingressi geometrici
vengono determinati dal modello cinematico come visto
il valore di k inuenza la forma del cammino pianicato
manovra di parcheggio parallelo
-3 -2 -1 0 1 2 3
0
1
2
3
4
|m|
|
m
|
-3 -2 -1 0 1 2 3
0
1
2
3
4
|m|
|
m
|

k = 5 k = 20
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Pianicazione di traiettorie
Le tecniche viste nora potrebbero essere applicate anche
considerando la variabile s come la variabile temporale s = t
la durata della traiettoria ` e stabilita a priori t
f
t
i
= s
f
s
i
se le velocit` a calcolate eccedono i limiti ammissibili per il
robot
[v(t)[ < v
max
[(t)[ <
max
t
allora occorrerebbe aumentare la durata e quindi ri-
pianicare il cammino
adottando invece la separazione tra cammino e legge oraria, ` e
sufciente scalare uniformemente la traiettoria
v(t) = v(s) s(t) = v(s)
ds
d
d
dt
= v(s)
ds
d
1
T
(t) = (s) s(t) = (s)
ds
d
d
dt
= (s)
ds
d
1
T
con = t/T e T = t
f
t
i
sufcientemente alto in modo
da rispettare i limiti di velocit` a
la legge oraria s(t) si pu` o ricavare con i metodi gi` a visti per la
pianicazione di traiettorie per i robot manipolatori
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo del moto

I problemi di controllo del moto sono i due classici


inseguimento di traiettoria (sinistra)
regolazione di congurazione (destra)
Il primo ` e quello pi ` u rilevante in pratica
permette di gestire la presenza di ostacoli
Gli ingressi di controllo sono quasi sempre le velocit` a e non le
forze generalizzate
disponibilit` a di anelli di controllo di basso livello gi` a
integrati nellarchitettura hw/sw del robot
uso del modello cinematico per risolvere il problema del
controllo del moto
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Inseguimento di traiettorie
Traiettoria desiderata ammissibile
x
d
= v
d
cos
d
y
d
= v
d
sin
d

d
=
d
uso degli schemi di pianicazione studiati, e.g. uscite piatte
x
d
(t), y
d
(t)

d
= Atan2( y
d
(t), x
d
(t)) +k k = 0, 1
v
d
(t) =
_
x
2
d
(t) + y
2
d
(t)

d
(t) =
y
d
(t) x
d
(t) x
d
(t) y
d
(t)
x
2
d
(t) + y
2
d
(t)
Denizione dellerrore di inseguimento
e =
_
_
e
1
e
2
e
3
_
_
=
_
_
cos sin 0
sin cos 0
0 0 1
_
_
_
_
x
d
x
y
d
y

_
_
= R
z
()
_
_
x
d
x
y
d
y

_
_
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Considerando il modello cinematico delluniciclo, si ottiene la
dinamica dellerrore
e
1
= v
d
cos e
3
v +e
2

e
2
= v
d
sine
3
e
1

e
3
=
d

trasformazione (invertibile) degli ingressi


v = v
d
cos e
3
u
1
=
d
u
2
dinamica dellerrore
e =
_
_
0
d
0

d
0 0
0 0 0
_
_
e+
_
_
0
sine
3
0
_
_
v
d
+
_
_
1 e
2
0 e
1
0 1
_
_
_
u
1
u
2
_
primo termine lineare tempo-variante
secondo e terzo termine non lineari
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo via linearizzazione
linearizzando attorno allorigine e = 0
e =
_
_
0
d
0

d
0 v
d
0 0 0
_
_
e +
_
_
1 0
0 0
0 1
_
_
_
u
1
u
2
_
legge di controllo (in retroazione) lineare nello stato
u
1
= k
1
e
1
u
2
= k
2
e
2
k
3
e
3
sistema a ciclo chiuso
e = A(t)e =
_
_
k
1

d
0

d
0 v
d
0 k
2
k
3
_
_
e +
_
_
1 0
0 0
0 1
_
_
e
il polinomio caratteristico ` e
p() = ( +k
1
)( +k
3
) +
2
d
( +k
3
) +v
d
k
2
( +k
1
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
se
d
e v
d
sono costanti il sistema a ciclo chiuso ` e stabile
scegliendo
k
1
= k
3
= 2a k
2
=
a
2

2
d
v
d
infatti
p() = ( + 2a)(
2
+ 2a +a
2
)
e quindi lorigine e = 0 ` e localmente asintoticamente
stabile
si noti come k
2
quando v
d
0

traiettorie cartesiane persistenti


gli ingressi di controllo in velocit` a vanno cos` ricostruiti
v = v
d
cos e
3
u
1
=
d
u
2
che tendono ai riferimenti quando lerrore tende a zero
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Controllo non lineare
dinamica dellerrore di inseguimento
e
1
= e
2
+u
1
e
2
= v
d
sine
3
e
1

e
3
= u
2
legge di controllo non lineare
u
1
= k
1
(v
d
,
d
)e
1
u
2
= k
2
v
d
sin e
3
e
3
e
2
k
3
(v
d
,
d
)e
3
nellipotesi che
k
1
(, ) > 0, k
3
(, ) > 0 limitate e con derivate limitate
k
2
> 0 costante
v
d
,
d
limitate e con derivate limitate e non entrabi nulle
lerrore di inseguimento converge globalmente a zero
Dimostrazione basata sul lemma di Barbalat...
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Linearizzazione ingresso-uscita
nuove uscite: posizione di un punto B lungo lasse sagittale
a distanza [b[ ,= 0 dal punto di contatto della ruota
y
1
= x +bcos
y
2
= y +bsin
dinamica delle uscite
_
y
1
y
2
_
=
_
cos bsin
sin bcos
_ _
v

_
= T()
_
v

_
con T() invertibile (det(T()) = b ,= 0)
controllo linearizzante
_
v

_
= T
1
()
_
u
1
u
2
_
sistema a ciclo chiuso
y
1
= u
1
y
2
= u
2

=
u
2
cos u
1
sin
b
rimane non controllata
legge di controllo lineare per portare lerrore a zero
u
1
= y
1d
+k
1
(y
1d
y
1
)
u
2
= y
2d
+k
2
(y
2d
y
2
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Simulazione: traiettoria circolare e controllo via linearizzazione
x
d
(t) = x
c
+Rcos (
d
t)
y
d
(t) = y
c
+Rsin(
d
t)
0 5 10 15
0
1
2
3
4
|s|
|
m
|
errore cartesiano
-6 -4 -2 0
0
1
2
3
4
5
6
|m|
|
m
|
Simulazione: traiettoria a otto e controllo non lineare
x
d
(t) = x
c
+R
1
sin(2
d
t)
y
d
(t) = y
c
+R
2
sin(
d
t)
-6 -4 -2 0
0
1
2
3
4
5
6
|m|
|
m
|
0 5 10 15 20 25 30
0
0.5
1
1.5
2
2.5
3
3.5
|s|
|
m
|
errore cartesiano
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Simulazione: traiettoria quadrata e controlloconlinearizzazione
ingresso-uscita (b = 0.2)
0 2 4 6 8 10 12 14 16
0.5
1
1.5
|s|
|
m
/
s
|
velocita lineare
0 2 4 6 8 10 12 14 16
0
10
20
30
|s|
|
r
a
d
/
s
|
velocita angolare
0 1 2 3 4 5 6
0
1
2
3
4
5
|m|
|
m
|
Simulazione: traiettoria quadrata e controlloconlinearizzazione
ingresso-uscita (b = 0.75)
0 2 4 6 8 10 12 14 16
-0.5
0
0.5
1
|
m
/
s
|
velocita lineare
0 2 4 6 8 10 12 14 16
0
0.5
1
1.5
|s|
|
r
a
d
/
s
|
velocita angolare
0 1 2 3 4 5 6
0
1
2
3
4
5
|m|
|
m
|
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Localizzazione odometrica
Gli algoritmi di controllo del moto richiedono la misura della
congurazione del robot mobile (posizione e orientamento in
un sistema di riferimento sso)
gli encoder montati sulle ruote forniscono solo la rotazione
di queste ultime
occorre un sistema di stima della posizione assoluta del
robot mobile (localizzazione)
Localizzazione passiva (dead reckoning)
si integra numericamente il modello cinematico assumendo
che le velocit` a sianocostanti nellintervallodi campionamento
metodo di Eulero in avanti
x
k+1
= x
k
+v
k
T
s
cos
k
y
k+1
= y
k
+v
k
T
s
sin
k

k+1
=
k
+
k
T
s
metodo di Runge-Kutta
x
k+1
= x
k
+v
k
T
s
cos
_

k
+

k
T
s
2
_
y
k+1
= y
k
+v
k
T
s
sin
_

k
+

k
T
s
2
_

k+1
=
k
+
k
T
s
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE

`
E bene non usare gli ingressi di riferimento ma usare misure da
sensori propriocettivi
v
k
T
s
= s
k
T
s
=
per un veicolo a trazione differenziale
s =
r
2
(
R
+
L
) =
r
d
(
R

L
)
` e necessario conoscere la congurazione iniziale
fenomeno della deriva per errori numerici di integrazione
slittamento delle ruote, raggi delle ruote non noti
perfettamente

Localizzazione attiva
uso di sensori esterocettivi per la correzione della stima
ottenuta tramite la localizzazione odometrica
tecniche di fusione sensoriale (stima Bayesiana)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
FUSIONE SENSORIALE
Scopi della Sensor Fusion
Applicazioni della SF in robotica mobile e articolata
Tecniche di SF basate sulla teoria del ltraggio Bayesiano
Metodi a griglia
Filtri Kalman-like
Metodi Numerici
Metodi Monte-Carlo (Particle Filters)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
SENSOR FUSION
Utilizzare dati provenienti da pi sorgenti per migliorare la
"qualit" dellinformazione ottenuta
Ma pi precisamente, cosa si intende per qualit?
Robustezza
Copertura spaziale
Copertura temporale
Afdabilit
Se la SF viene usata in maniera indiscriminata porta a danni
economici e/o di performance
In linea di principio occorre sempre considerare che:
combinando dati provenienti unicamente da sensori di
cattiva qualit i miglioramenti sono scarsi
combinando dati provenienti da sensori di ottima qualit, i
miglioramenti sono scarsi
combinando sensori di ottima qualit con sensori di scarsa
qualit i risultati sono scarsi
grandi miglioramenti si hanno combinando un numero
moderato di sensori di diversa natura e di ragionevole qualit
` e cruciale una attenta scelta sia dellhardware che degli
algortimi di fusione
quanticare il miglioramento apportato dalla SF ` e
unattivit non banale
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
APPLICAZIONI
Applicazioni militari
sorveglianza degli oceani: rilevamento, identicazione e
tracciamento di bersagli ostili
sorvegliaza aerea
sorveglianza di interi campi di battaglia
Applicazioni civili
sorveglianza in ambito civile
diagnosi mediche
robotica
Sensori pi utilizzati
elettrici
acustici (sonar, sismometri)
elettromagnetici ( radar)
ottici (laser, sensori elettro-ottici)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
APPLICAZIONI IN ROBOTICA
Robotica mobile
mapping: il robot mobile costruisce una mappa dellambiente
ignoto in cui si muove
localizzazione: il robot localizza la sua posizione allinterno
di una mappa che ha in memoria
SLAM (simultaneous localization and mapping)
Robotica articolata
misura robusta dei valori assunti dalle variabili di giunto
riconoscimento di oggetti
riconoscimento di compiti di manipolazione
la SF assume unimportanza cruciale quando si passa da
un ambiente perfettamente noto (robotica industriale) ad un
ambiente parzialmente o totalmente ignoto (robotica avanzata)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
APPROCCIO PROBABILISTICO: IL
FILTRAGGIO BAYESIANO
Supponiamo di voler applicare la SF a un sistema stocastico
tempo-discreto S e di avere una rapresentazione i-s-u di tale
sistema:
x
k
= f
k1
(x
k1
, v
k1
)
z
k
= h
k
(x
k
, w
k
)
x
k
` e lo stato del sistema allistante k
z
k
` e il vettore delle misure di tutti i sensori allistante k
Z
k
= z
0
, z
1
, ......, z
k
` e la storia delle osservazioni
il Filtraggio Bayesiano consiste nel calcolare istante per istante
la pdf:
p(x
k
[Z
k
)
e scegliere il valore di x
k
per cui tale pdf ` e massima.
Dunque si indicher col simbolo x
k
lo stato stimato
allistante k
si pu dimostrare che il ltro cos denito ` e uno stimatore
che minimizza lerrore quadratico medio di stima.
La Sensor Fusion sta nello scegliere linsieme Z
k
come
insieme di osservazioni provenienti da pi di un sensore;
altrimenti ottengo un algoritmo di ltraggio semplice.
Come ricaviamo la precedente espressione avendo a
disposizione la i-s-u del sistema?
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Si utilizza il teorema di Bayes:
p(x
k
[Z
k
) =
p(z
k
[x
k
)p(x
k
[Z
k1
)
p(z
k
[Z
k1
)
il secondo termine al numeratore pu essere scritto come:
p(x
k
[Z
k1
) =
_
p(x
k
, x
k1
[Z
k1
)dx
k1
=
=
_
p(x
k
[x
k1
, Z
k1
)p(x
k1
[Z
k1
)dx
k1
=
=
_
p(x
k
[x
k1
)p(x
k1
[Z
k1
)dx
k1
Si ` e assunto che lo stato futuro dipenda solo dallingresso
corrente e dallo stato corrente (processo di Markov)
Il termine al denominatore ` e pari a:
p(z
k
[Z
k1
) =
_
p(z
k
[x
k
)p(x
k
[Z
k1
)dx
k
Si noti che:
p(z
k
[x
k
) ` e nota dalla relazione stato-uscita
p(x
k
[x
k1
) ` e nota dalla relazione di aggiornamento dello
stato
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
PREDIZIONE E AGGIORNAMENTO
Gli algoritmi di sensor fusion basati sullapproccio bayesiano
si dividono in due passi: predizione e aggiornamento
il calcolo della pdf p(x
k
[Z
k1
) ` e detto passo di predizione
il calcolo della pdf p(x
k
[Z
k
) a partire dalla conoscenza
della pdf di predizione ` e detto passo di aggiornamento
il calcolo dei precedenti integrali in generale ` e impossibile in
forma chiusa
sono necessarie ipotesi semplicative oppure approssimazioni
numeriche e/o analitiche
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
FILTRI OTTIMI
FILTRO DI KALMAN
quando il sistema ` e lineare ed il rumore ` e AWGN otteniamo il
ltro di Kalman.
sotto le precedenti ipotesi la i-s-u diventa:
x
k
= F
k1
x
k1
+v
k1
z
k
= H
k
x
k
+w
k
v
k1
e w
k
sono gaussiane a media nulla con matrici di
covarianza Q
k1
e R
k
si ha quindi:
p(x
k1
[Z
k1
) = N(x
k1
; x
k1|k1
, P
k1|k1
)
Passo di predizione:
p(x
k
[Z
k1
) = N(x
k
; x
k|k1
, P
k|k1
)
Passo di aggiornamento:
p(x
k
[Z
k
) = N(x
k
; x
k|k
, P
k|k
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
dove:
x
k|k1
= F
k1
x
k1|k1
P
k|k1
= Q
k1
+F
k1
P
k1
F
T
k1
x
k
= x
k|k1
+K
k
(z
k
H
k
x
k|k1
)
P
k
= P
k|k1
K
k
S
k
K
T
k
con
S
k
= H
k
P
k|k1
H
T
k
+R
k
K
k
= P
k|k1
H
T
k
S
1
k
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
METODI A GRIGLIA
Quando il sistema ` e a stati niti otteniamo i metodi a griglia
supponiamo che lo spazio di stato allistante k 1 consista
negli stati x
i
k1
, i = 1, ....., N
per ogni stato x
i
k1
, sia w
i
k1
:= Px
i
k1
[Z
k1

la pdf p(x
k1
[Z
k1
) pu scriversi come:
p(x
k1
[Z
k1
) =
N

i=1
w
i
k1
(x
k1
x
i
k1
)
sostituendo la precedente espressione nelle relazioni del
ltraggio bayesiano ottengo:
p(x
k
[Z
k1
) =
N

i=1
w
i
k|k1
(x
k1
x
i
k1
)
p(x
k
[Z
k
) =
N

i=1
w
i
k
(x
k1
x
i
k1
)
dove:
w
i
k|k1
=
N

j=1
w
j
k1
p(x
i
k
[x
j
k1
)
w
k
i
=
w
i
k|k1
p(z
k
[x
i
k
)

N
j=1
w
j
k|k1
p(z
k
[x
j
k
)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
FILTRI SUBOTTIMI
Approssimazioni analitiche: EKF (Extendend Kalman Filter)
Per ogni k, approssimo le funzioni f
k1
e h
k
con funzioni
pi semplici che consentono di ottenere soluzioni in forma
chiusa agli integrali di predizione e aggiornamento
Approssimazioni numeriche
Approssimazione mediante metodi Monte-Carlo: PF (Particle
Filters)
Non ` e necessaria lipotesi di gaussianit delle pdf del
rumore.
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
EXTENDED KALMAN FILTER
Si suppone che la i-s-u del sistema abbia la seguente forma:
x
k
= f
k1
(x
k1
) +v
k1
z
k
= h
k
(x
k
) +w
k
I processi aleatori v
k1
e w
k
sono mutuamente indipendenti
e hanno pdf gaussiane a media nulla e matrici di covarianza
rispettivamente Q
k1
e R
k
Lidea che sta alla base dellEKF ` e approssimare le funzioni
f
k1
e h
k
con il loro sviluppo in serie di Taylor troncato al
primo ordine.
x
k|k1
= f
k1
( x
k1
)
P
k|k1
= Q
k1
+

F
k1
P
k1

F
T
k1
x
k
= x
k|k1
+K
k
(z
k
H
k
x
k|k1
)
P
k
= P
k|k1
K
k
S
k
K
T
k
con
S
k
=

H
k
P
k|k1

H
T
k
+R
k
K
k
= P
k|k1

H
T
k
S
1
k

F
k1
= [
x
k1
f
T
k1
(x
k1
)]
T
[
x
k1
= x
k1

H
k
= [
x
k1
f
T
k1
(x
k1
)]
T
[
x
k1
pu essere applicato solo a funzioni differenziabili
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
UNSCENTED KALMAN FILTER
Fa le stesse ipotesi semplicative dellEKF, sembra essere pi
efciente sia dal punto di vista computazionale sia dal punto di
vista prestazionale
Non richiede il calcolo degli Jacobiani
Sfrutta la Unscented Transform
La UT permette di calcolare media e covarianza dellimmagine
di una variabile aleatoria mediante una applicazione non lineare
deterministica
Sia a una variabile aleatoria di media a e matrice di covarianza
P
a
, g una traformazione non lineare tale che g : R
n
a
R
n
b
e b limmagine di a mediante g: b = g(a)
A
0
= a W
0
=
k
n
a
+k
i = 0
A
i
= a+
_
_
(n
a
+k)P
a
_
i
W
i
=
1
2(n
a
+k)
i = 1, ..., n
a
A
i
= a
_
_
(n
a
+k)P
a
_
i
W
i
=
1
2(n
a
+k)
i = n
a
+1, ..., 2n
a
k ` e una fattore di scala tale che k +n
a
,= 0

_
_
(n
a
+k)P
a
_
i
` e la i-esima riga della radice quadrata
della matrice (n
a
+k)P
a
, cio` e la matrice L tale che (n
a
+
k)P
a
= L
T
L
I pesi W
i
godono della seguente propriet:

2n
a
i=0
W
i
= 1
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Ogni campione A
i
` e propagato attraverso la funzione non
lineare g, si ottiene cos:
B
i
= g(A
i
) i = 0, 1, ..., 2n
a
Calcoliamo media e covarianza della variabile aleatoria b
come segue:

b =
2n
a

i=0
W
i
B
i
P
b
=
n
a

i=0
W
i
(B
i

b)(B
i

b)
T
Si dimostra che le precedenti stime sono accurate al terzo ordine
dello sviluppo in serie della funzione g se a ha pdf gaussiana
Come uso la Unscented Transform per stimare lo stato di un
sistema dinamico, dato il modello e le osservazioni provenienti
da pi sensori?
Le ipotesi sono le stesse utilizzate per lEKF, la differenza ` e
che non si approssimano le funzioni non lineari f
k1
e h
k
,
ma si approssimano le pdf gaussiane attraverso un numero di
campioni scelti con la tecnica descritta precedentemente
Equazioni dell Unscented Kalman Filter
Come nel caso dellEKF, si suppone che p(x
k1
[Z
k1
) =
N(x
k
; x
k1
, P
k1
)
Il primo passo consiste nel rappresentare tale pdf con un
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
insieme di N campioni
N
k1
e di pesi W
i
k1
con i =
0, ..., N 1
x
k|k1
=
N1

i=0
W
i
k1
f
k1
(
i
k1
)
P
k|k1
= Q
k1
+
N1

i=0
W
i
k1
[f
k1
(
i
k1
) x
k|k1
][f
k1
(
i
k1
) x
k|k1
]
T
La pdf di predizione p(x
k
[Z
k1
) N(x
k
; x
k|k1
, P
k|k1
)
` e a sua volta rappresentata da una insieme di N campioni:

i
k|k1
= f
k1
(
i
k1
)
La predzione sul vettore delle misure si calcola in questo
modo:
z
k|k1
=
N1

i=0
W
i
k1
h
k
(
i
k|k1
)
Il passo di update, ossia il calcolo della p(x
k
[Z
k
), consiste
nelle relazioni:
x
k
= x
k|k1
+K
k
(z
k
z
k|k1
)
P
k
= P
k|k1
K
k
S
k
K
T
k
dove
K
k
= P
x
zS

k
1
S
k
= R
k
+P
z
z
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
P
xz
=
N1

i=0
W
i
k1
h
k
(
i
k|k1
x
k|k1
)(h
k
(
i
k|k1
z
k|k1
)
T
P
zz
=
N1

i=0
W
i
k1
(h
k
(
i
k|k1
) z
k|k1
)(h
k
(
i
k|k1
) z
k|k1
)
T
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
METODI NUMERICI
Si scopone lo spazio di stato in tante celle discrete
Si utilizzano i metodi a griglia visti nelle slide precedenti
E interessante notare che in questo caso lapprossimazione
viene introdotta nella rappresentazione dello stato, mentre
lalgoritmo di ltraggio ` e ottimo
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
PARTICLE FILTER
Mentre i ltri Kalman-like rappresentano le pdf di predizione
e aggiornamento in forma paramentrica (denendo media e
matrice di covarianza), i Particle Filter rappresentano le pdf
p(x
k
[Z
k1
) e p(x
k
[Z
k
) mediante una serie di "campioni", detti
particle, delle pdf stesse
Al contrario dei ltri alla Kalman, i Particle Filter non
presuppongono lipotesi di Gaussianit delle pdf
Poich e sono onerosi computazionalmente, i PF vanno usati solo
in caso di fallimento dei metodi descritti in precedenza
I campioni della pdf sono denotati come:

t
:= x
[1]
t
, x
[2]
t
, ...., x
[M]
t
M denota il numero di campioni presenti nel particolare
insieme
t
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Algoritmo di ltraggio:
AlgorithmParticleFilter(
t
, z
t
)
1.
t
=
t
=
2. for(m = 0; m < M; m+ +)
3. x
[m]
t
p(x
t
[x
[m]
t1
)
4. w
[m]
t
= p(z
t
[x
[m]
t
)
5.
t
=
t
+ (x
[m]
t
, w
[m]
t
)
6. endfor
7. for(m = 0; m < M; m+ +)
8. genera i con probabilit w
[i]
t
9. aggiungi x
[i]
t
a
i
10. endfor
11. return(
i
)
La linea 3 genera uno "stato ipotetico" x
[m]
t
per listante t a
partire dal campione x
[m]
t1
La pdf p(x
t
[x
t1
) ` e nota dal modello del sistema dinamico.
Lapice mindica che x
m
t
` e generato a partire dalla m-esimo
campione dellinsieme
t1
Dopo M iterazioni ottengo unapprossimazione della pdf di
predizione p(x
t
[Z
t1
)
La linea 4 calcola per ogni campione x
[m]
t
, il cosiddetto
"importance factor", denotato con w
[m]
t
.
Gli importance factors sono calcolati per includere negli
insiemi di campioni
t
informazioni sulle misure allistante
t: w
[m]
t
= p(z
t
[x
[m]
t
).
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
la pdf p(z
t
[x
[m]
t
) ` e nota dal modello di misura del sistema
dinamico.
Nelle linee 7-10 viene eseguito il cosiddetto passo di
"importance sampling", detto anche di "resampling".
In questa fase linsieme provvisorio
t
di cardinalit M
viene sostituito con linsieme , ancora di cardinalit M.
In ogni passo del ciclo, la probabilit di includere in
t
un
campione x
[m]
t
appartenente a
t
` e pari a w
[m]
i
Prima dell importace sampling, i campioni erano distribuiti
secondo la pdf di predizione p(x
t
[Z
t1
), dopo limportance
sampling i campioni sono distribuiti come:
p(z
t
[x
[m]
t
)p(x
t
[Z
t1
)
quindi, approssimativamente, secondo la pdf di aggiornamento
p(x
t
[Z
t
)
La bont dellapprossimazione dipende dalla quantit di
campioni (particles)
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
ESEMPI APPLICATIVI
Problema di Robotica Articolata: Stima degli angoli ai giunti
di una mano umana o robotica
Sencori a disposizione:
Sensori angolari optoelettronici di basso costo applicati ai
giunti
Sistema di motion capture ottico che misura la posizione
nello spazio di sferette riettenti detti marker
Alcune domande che bisogna porsi sono:
1. "Perch e utilizzare la fusione sensoriale?"
2. "Che risultati otterrei utilizzando solo il "migliore" dei
sensori che ho a disposizione?"
3. "La sensor fusion fornirebbe unbuon rapportoprestazioni/costi?"
Riettiamo sui sensori che abbiamo a disposizione...
Sensori angolari:
sono di basso costo
possono fornire misure a una frequenza di campionamento
elevata (es. 1024Hz)
sono rumorosi
non possono misurare tutti i gradi di libert (es.
ladduzione/abduzione)
Sistema di Motion Capture
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
misurano molto precisamente le posizioni dei marker,
lerrore e inferiore a mezzo millimetro
forniscono le misure al pi con una frequenza di 60Hz
le misure dei marker sono soggette al fenomeno
dellocclusione
durante i movimenti delle dita, si verica il fenomeno dello
scivolamento della pelle rispetto alle ossa
Conclusioni:
Utilizzando solo le misure di posizione dei marker non
riesco a gestire il problema delle occlusioni
Inoltre campiono ad una frequenza relativamente bassa
(60Hz)
Aggiungendo i sensori angolari riesco a gestire le occlusioni
e aggiungo informazioni, anche se meno precise di quelle
dei marker, ad una cadenza pi alta (1024 Hz)
Anche in assenza di occlusioni, aggiungendo i sensori
angolari riesco ad ottenere uninformazione pi afdabile,
a fronte di un basso aumento dei costi (meno di 1 euro per
ogni sensore angolare)
Modellazione del sistema (dito indice):
Lo stato del sistema pu essere espresso come:
x = (

ll hh)
T
dove:
= (
1

2

3

4
)
T
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE

= (

4
)
T
ll = ( ll
1
ll
2
)
T
hh = ( hh
1
hh
2
)
T
Mappa di evoluzione dello stato (tempo-discreto) del
sistema:

k+1
=
k
+

k
t

k+1
=

K
+w

k
ll
k+1
= ll
k
+w
ll
k
hh
k+1
= hh
k
+w
hh
k
Relazione stato-uscita:
y
k
=
_
M
k
(x)
V
k
(x)
_
+v
k
M
k
(x) ` e la posizione dei marker in funzione delle variabili
di stato
V
k
(x) ` e la luscita in tensione dei sensori angolari
A partire dal modello del sistema si progetta un ltro di
Kalman Esteso, che acquisisce le misure dei sensori angolari
a 1024 Hz e le misure dal sistema di motion capture a 60Hz
Il ltro deve essere dotato anche dellintelligenza per la
gestione delle occlusioni dei marker
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Problema di Robotica Mobile: Stima della posizione di un
robot mobile avendo a disposizione un INS (Inertial Navigation
System) e misure di posizione via radio
LINS fornisce laccelerazione, da cui si pu ricavare la
posizione per doppia integrazione
La misura di posizione dellINS pu essere espressa come:
p
INS
= p +p
p ` e la posizione esatta del robot nello spazio
p ` e lerrore nella misura di posizione dellINS
Utilizzando un approccio leggermente diverso, possiamo
considerare come sistema da osservare mediante la misura
via radio lerrore di posizione dellINS
In forma matriciale abbiamo:
_
_
p
v
a
_
_
=
_
_
0 1 0
0 0 1
0 0 0
_
_
_
_
p
v
a
_
_
Discretizzando il sistema con passo di campionamento T,
la matrice dinamica A diventa:
= e
AT
=
_
_
1 T
T
2
2
0 1 T
0 0 1
_
_
A causa della doppia integrazione, utilizzando solo lINS,
lerrore di stima della posizione cresce come un polinomio
del secondo ordine
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Utilizziamo le misura di posizione via radio, che ` e
disponibile ogni 30s, possiamo migliorare di gran lunga
le prestazioni:
p
radio
= p e
p
e
p
` e lerrore di misura del sistema radio
Utilizzato le misurazioni radio per osservare il sistema
dinamico (lerrore di misura dellINS)
La relazione stato-uscita del sistema sar:
z = p
radio
p
INP
= p +e
p
In forma matriciale ottengo il sistema:
x(k + 1) =
_
_
1 T
T
2
2
0 1 T
0 0 1
_
_
x(k)
z(k) = ( 1 0 0 ) x(k) +e
p
(k)
Si progetta un ltro di Kalman con T = 1 e si eseguono
ogni 30s le misure via radio.
I parametri di simulazione sono:
R = 10
1
Q = 10
3
I
3
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Errore di posizione utilizzando unicamente lINS
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Errore di posizione usando lINS e le misure via radio
ROBOTICA A.A. 2013/2014 Prof. Ciro NATALE
Errore di velocit utilizzando lINS e le misure radio