ROBOTICA - · PDF fileROBOTICA INDUSTRIALE Prof. Bruno SICILIANO Robotica deﬁnita come...

ROBOTICA INDUSTRIALE Prof. Bruno SICILIANO

ROBOTICA

� Studio di macchine che possano sostituire l’uomo nell’esecuzionedi un compito, sia in termini di attivita fisica che decisionale

� Radici culturali

? mitologia

? automa

? robot (robota= lavoro esecutivo)

? letteratura fantascientifica

� Leggi fondamentali (Asimov)? un robot non puo far del male a un essere umano ne con-

sentire, restando inoperoso, che un essere umano si trovi inpericolo

? un robot deve obbedire agli ordini impartiti da esseri umani,a meno che tali ordini non entrino in conflitto con la primalegge

? un robot deve proteggere la sua esistenza a meno che taleprotezione non vada in conflitto con la prima o la secondalegge


� Robotica definita come scienza che studia laconnessione intel-ligente 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, nu-cleare, militare, ...)

? missioni di servizio (applicazioni domestiche, assistenzamedica, assistenza ai disabili, agricoltura, ...).

? ancora in eta infantile

� Robotica industriale

? progettazione, governo e applicazioni dei robot in ambitoindustriale

? tecnologia matura


� L’automazionee una tecnologia il cui obiettivoe quello di sosti-tuire la macchina all’uomo in un processo di produzione, nonsolo per quanto riguarda l’esecuzione delle operazioni mate-riali, ma anche per cio che concerne l’elaborazione intelligentedelle informazioni sullo stato del processo.

? automazionerigida (produzione in serie di grossi volumi dimanufatti di caratteristiche costanti)

? automazioneprogrammabile(produzione di piccoli e medilotti di manufatti di caratteristiche variabili)

? automazioneflessibile(produzione di lotti variabili di ma-nufatti diversi)

� Robot industriale? macchina con elevate caratteristiche di versatilita e flessibi-

lit a

? un robote una struttura meccanica multifunzionale e ripro-grammabile progettato per spostare materiali, parti, utensilio dispositivi specializzati secondo movimenti variabili pro-grammati per l’esecuzione di una varieta di compiti diversi(Robot Institute of America, 1980)

? componente tipico di sistemi di automazione programma-bile


ROBOT INDUSTRIALE

� Componenti:

? una struttura meccanica omanipolatoreche consiste in uninsieme di corpi rigidi (bracci) interconnessi tra di loro permezzo di articolazioni (giunti); nel manipolatore si indivi-duano unastruttura portante, che ne assicura mobilita, unpolso, che conferisce destrezza, e unorgano terminalecheesegue il compito per cui il robote utilizzato

? attuatori che imprimono il movimento al manipolatore at-traverso l’azionamento dei giunti; si impiegano usualmentemotori elettrici, idraulici e talvolta pneumatici

? sensoriche misurano lo stato del manipolatore (sensori pro-priocettivi) ed eventualmente lo stato dell’ambiente (sensorieterocettivi)

? unaunita di governo(calcolatore) con funzioni di controlloe supervisione dei movimenti del manipolatore


CAPACITA DI IMPIEGO

� trasporto

? palettizzazione (disposizione di oggetti in maniera preordi-nata su un opportuno supporto raccoglitore)

? carico e scarico di magazzini

? carico e scarico di macchine operatrici e macchine utensili

? selezione e smistamento di parti

? confezionamento di merci

� manipolazione (lavorazione/assemblaggio)? saldatura ad arco e a punti

? verniciatura a spruzzo

? fresatura e trapanatura

? incollaggio

? taglio laser e a getto d’acqua

? finitura

? assemblaggio di gruppi meccanici ed elettrici

? montaggio di schede elettroniche

? avvitatura

? cablaggio


� misura

? collaudo dimensionale

? rilevamento di profili

? individuazione di difetti di fabbricazione


STRUTTURA DEI MANIPOLATORI

� struttura meccanica acatena cinematica apertao acatena ci-nematica chiusa

� gradi di mobilita (giuntiprismaticio rotoidali)

� gradi di liberta (descrizione di un compito)

� spazio di lavoro (porzione dell’ambiente circostante a cui puoaccedere l’organo terminale)


Manipolatore cartesiano

� tre giunti prismatici

? ad ogni grado di mobilita corrisponde un grado di liberta

� ottime caratteristiche di rigidezza meccanica

� precisione di posizionamento del polso costante nello spazio dilavoro

� operazioni di trasporto e assemblaggio

� azionamenti elettrici (talvolta pneumatici)


Manipolatore a portale

� manipolazione di oggetti di dimensione e peso rilevanti


Manipolatore cilindrico

� un giunto rotoidale e due prismatici

? ad ogni grado di mobilita corrisponde un grado di liberta (incoordinate cilindriche)

� buone caratteristiche di rigidezza meccanica

� la precisione di posizionamento del polso si riduce al cresceredello sbraccio orizzontale

� operazioni di trasporto di oggetti anche di peso rilevante

� azionamenti idraulici (o elettrici)


Manipolatore sferico

� due giunti rotoidali e uno prismatico

? ad ogni grado di mobilita corrisponde un grado di liberta (incoordinate sferiche)

� discrete caratteristiche di rigidezza meccanica

� la precisione di posizionamento del polso si riduce al cresceredello sbraccio radiale

� operazioni di lavorazione

� azionamenti elettrici


Manipolatore SCARA

� due giunti rotoidali e uno prismatico

� elevata rigidezza a carichi verticali e cedevolezza a carichi oriz-zontali (Selective Compliance Assembly Robot Arm)

� la precisione di posizionamento del polso si riduce al cresceredella distanza del polso stesso dall’asse del primo giunto

� manipolazione di piccoli oggetti



Manipolatore antropomorfo

� tre giunti rotoidali

? spalla e gomito (che connette braccio e avambraccio)

� struttura piu destra

� precisione di posizionamento variabile

� applicazioni molteplici



Polso sferico

� giunti rotoidali

? determinano l’orientamento dell’organo terminale

� caratteristiche di compattezza e destrezza

� disaccoppiamento tra posizione e orientamento

Organo terminale

� specificato in relazione al compito che il robot deve eseguire

? pinza (trasporto)

? utensile o dispositivo specializzato (lavorazione e assem-blaggio)


Alcuni robot industriali

� Robot AdeptOne XL

� Robot Comau SMART S2


� Robot ABB IRB 4400

� Unita lineare Kuka KL 250 con robot KR 15/2


� Robot Robotics Research K-1207i

� Robot Fanuc I-21i


MODELLISTICA E CONTROLLO DIMANIPOLATORI

� Modellistica

? struttura meccanica (cinematica + cinematica differenziale+ statica + dinamica)

? attuatori

? sensori

� Controllo

? pianificazione del moto

? controllo nello spazio libero? controllo nello spazio vincolato

? unita di governo


Cinematica

� relazioni tra posizioni dei giunti e posizione e orientamentodell’organo terminale

Cinematica differenziale

� relazioni tra velocita dei giunti e velocita (lineare e angolare)dell’organo terminale

Statica� relazioni tra forze e coppie applicate ai giunti e forze e momenti

applicati all’organo terminale in situazioni di equilibrio

Dinamica

� equazioni del moto del manipolatore in funzione delle forze emomenti agenti su di esso


Pianificazione di traiettorie

� generazione delle leggi di moto per le variabili di interesse(giunti/organo terminale)

Controllo del moto

� determinazione delle forze/coppie agli attuatori per garantirel’esecuzione delle traiettorie di riferimento

Controllo dell’interazione� gestione del contatto tra organo terminale e ambiente


Attuatori e sensori

� attuazione del moto

� misura di variabili di interesse

Unita di governo

� implementazione delle leggi di controllo

� interfaccia con operatore


CINEMATICA

� relazioni tra posizioni dei giunti e posizione e orientamentodell’organo terminale

Matrice di rotazione

Rappresentazioni dell’orientamento

Trasformazioni omogenee

Cinematica diretta

Spazio dei giunti e spazio operativo

Calibrazione cinematica

Problema cinematico inverso


POSIZIONE E ORIENTAMENTO DI UNCORPO RIGIDO

� Posizione

o0 =

24 o0xo0y

o0z

35� Orientamento

x0 = x

0xx+ x

0yy + x

0zz

y0 = y

0xx+ y

0yy + y

0zz

z0 = z

0xx+ z

0yy + z

0zz


MATRICE DI ROTAZIONE

R =

24x0 y0

z0

35 =

24x0Tx y0T

x z0T

x

x0T

y y0T

y z0T

y

x0T

z y0T

z z0T

z

35

RTR = I

RT = R

�1


Rotazioni elementari

� rotazione di� intorno az

Rz(�) =

24 cos� �sin� 0

sin� cos� 0

0 0 1

35


� rotazione di� intorno ay

Ry(�) =

24 cos� 0 sin�

0 1 0

�sin� 0 cos�

35� rotazione di intorno ax

Rx( ) =

24 1 0 0

0 cos �sin 0 sin cos

35


Rappresentazione di un vettore

p =

24 pxpypz

35 p0 =

24 p0xp0y

p0z

35

p =

24x0 y0

z0

35p0= Rp

0

p0 = R

Tp


� Esempio

px = p0x

cos�� p0y

sin�

py = p0x

sin�+ p0y

cos�

pz = p0z


Rotazione di un vettore

p = Rp0

pTp = p

0TR

TRp

0

� Esempio

px = p0x

cos�� p0y

sin�

py = p0x

sin�+ p0y

cos�

pz = p0z

p = Rz(�)p0


� Matrice di rotazione

? fornisce l’orientamento di una terna di coordinate rispettoad un’altra: i vettori colonna sono i coseni direttori degliassi della terna ruotata rispetto alla terna di partenza

? rappresenta una trasformazione di coordinate che mette inrelazione le coordinate di uno stesso punto in due ternedifferenti (di origine comune)

? e l’operatore che consente di ruotare un vettore in una stessaterna di coordinate


COMPOSIZIONE DI MATRICI DIROTAZIONE

p1 = R

12p

2

p0 = R

01p

1

p0 = R

02p

2

Rj

i= (Ri

j)�1 = (Ri

j)T

� Rotazione interna corrente

R02 = R

01R

12

� Rotazione interna fissa

R02 = R

12R

01


� Esempio


ANGOLI DI EULERO

� matrice di rotazione

? 9 parametri con 6 vincoli

� rappresentazione minima dell’orientamento

? 3 parametri indipendenti


Angoli ZYZ

R(�) = Rz(')Ry0(#)Rz00( )

=

24 c'c#c � s's �c'c#s � s'c c's#

s'c#c + c's �s'c#s + c'c s's#

�s#c s#s c#

35


� Problema inverso

? Assegnata

R =

24 r11 r12 r13

r21 r22 r23

r31 r32 r33

35

i 3 angoli ZYZ sono (# 2 (0; �))

' = Atan2(r23; r13)

# = Atan2

�qr213 + r223; r33

� = Atan2(r32;�r31)

ovvero (# 2 (��; 0))

' = Atan2(�r23;�r13)

# = Atan2

��q

r213 + r223; r33

� = Atan2(�r32; r31)


Angoli di RPY

R(�) = Rz(')Ry(#)Rx( )

=

24 c'c# c's#s � s'c c's#c + s's

s'c# s's#s + c'c s's#c � c's

�s# c#s c#c

35



? Assegnata

R =

24 r11 r12 r13

r21 r22 r23

r31 r32 r33

35

i 3 angoli di RPY sono (# 2 (��=2; �=2))

' = Atan2(r21; r11)

# = Atan2

��r31;

qr232 + r233

� = Atan2(r32; r33)

ovvero (# 2 (�=2; 3�=2))

' = Atan2(�r21;�r11)

# = Atan2

��r31;�

qr232 + r233

� = Atan2(�r32;�r33)


ASSE/ANGOLO

R(#; r) = Rz(�)Ry(�)Rz(#)Ry(��)Rz(��)

sin� =ryqr2x

+ r2y

cos� =rxqr2x

+ r2y

sin � =q

r2x

+ r2y

cos� = rz


R(#; r) =

264 r2x

(1� c#) + c# rxry(1� c#)� rzs#

rxry(1� c#) + rzs# r2y(1� c#) + c#

rxrz(1� c#)� rys# ryrz(1� c#) + rxs#

rxrz(1� c#) + rys#

ryrz(1� c#)� rxs#

r2z(1� c#) + c#

35

R(#; r) = R(�#;�r)



? Assegnata

R =

24 r11 r12 r13

r21 r22 r23

r31 r32 r33

35

l’angolo e l’asse di rotazione sono (sin# 6= 0)

# = cos�1�

r11 + r22 + r33 � 1

2

�

r =1

2 sin#

24 r32 � r23

r13 � r31

r21 � r12

35con

r2x

+ r2y

+ r2z

= 1


QUATERNIONE UNITARIO

� rappresentazione a 4 parametriQ = f�; �g

� = cos#

2

� = sin#

2r

�2 + �

2x

+ �2y

+ �2z

= 1

? (#; r) e (�#;�r) forniscono lo stesso quaternione

R(�; �) =

264 2(�2 + �2x

)� 1 2(�x�y � ��z) 2(�x�z + ��y)

2(�x�y + ��z) 2(�2 + �2y)� 1 2(�y�z � ��x)

2(�x�z � ��y) 2(�y�z + ��x) 2(�2 + �2z)� 1

375



? Assegnata

R =

24 r11 r12 r13

r21 r22 r23

r31 r32 r33

35

il quaternionee (� � 0)

� =1

2

pr11 + r22 + r33 + 1

� =1

2

264 sgn (r32 � r23)p

r11 � r22 � r33 + 1

sgn (r13 � r31)p

r22 � r33 � r11 + 1

sgn (r21 � r12)p

r33 � r11 � r22 + 1

375

� quaternione estratto daR�1 = RT

Q�1 = f�;��g

� prodotto tra quaternioni

Q1 � Q2 = f�1�2 � �T1 �2; �1�2 + �2�1 + �1 � �2g


TRASFORMAZIONI OMOGENEE

� Trasformazione di coordinate (traslazione+ rotazione)

p0 = o

01 +R

01p

1

� Trasformazione inversa

p1 = �R1

0o01 +R

10p

0


� Rappresentazione omogenea

~p =

264p1

375

� Matrice di trasformazione omogenea

A01 =

264 R01 o

01

0T 1

375

� Trasformazione di coordinate

~p0 = A01~p

1


� Trasformazione inversa

~p1 = A10~p

0 =�A

01

��1~p0

ove

A10 =

264 R10 �R1

0o01

0T 1

375

A�1 6= A

T

� Successione di trasformazioni

~p0 = A01A

12 : : :A

n�1n

~pn


CINEMATICA DIRETTA

� Manipolatore

? insieme dibracciconnessi tramitegiunti

� Catena cinematica (dalla base all’organo terminale)

? aperta (sequenza unica)

? chiusa (sequenza forma un anello)

� Grado di mobilita

? tipicamente associato a una articolazione =variabile digiunto


Terna base e terna utensile

� Equazione cinematica diretta

Tb

e(q) =

264nbe(q) sb

e(q) a

b

e(q) p

b

e(q)

0 0 0 1

375


Manipolatore planare a due bracci

Tb

e(q) =

264nbe sb

ea

b

ep

b

e

0 0 0 1

375

=

2640 s12 c12 a1c1 + a2c12

0 �c12 s12 a1s1 + a2s12

1 0 0 0

0 0 0 1

375


Catena aperta

T0n

(q) = A01(q1)A

12(q2) : : :A

n�1n

(qn)

Tb

e(q) = T

b

0T0n

(q)T ne


Convenzione di Denavit-Hartenberg

� si sceglie l’assezi giacente lungo l’asse del giuntoi+ 1

� si individuaOi all’intersezione dell’assezi con la normalecomune agli assizi�1 e zi, e conO0

i

si indica l’intersezionedella normale comune conzi�1

� si assume l’assexi diretto lungo la normale comune agliassizi�1 ezi con verso positivo dal giuntoi al giuntoi+ 1

� si sceglie l’asseyi in modo da completare una terna levogira


� Definizione non univoca della terna:

? con riferimento alla terna0, per la quale la sola direzionedell’assez0 risulta specificata: si possono quindi sceglierearbitrariamenteO0 edx0

? con riferimento alla ternan, per la quale il solo assexn ri-sulta soggetto a vincolo (deve essere normale all’assezn�1):infatti non vi e giunton + 1, per cui none definitozn e losi puo scegliere arbitrariamente

? quando due assi consecutivi sono paralleli, in quanto lanormale comune tra di essi none univocamente definita

? quando due assi consecutivi si intersecano, in quanto il versodi xi e arbitrario

? quando il giuntoi e prismatico, nel qual caso la sola dire-zione dell’assezi�1 e determinata


Parametri di Denavit-Hartenberg

ai distanza diOi daO0i

;

di coordinata suzi�1 di O0i

;

�i angolo intorno all’assexi tra l’assezi�1 e l’assezi valutatopositivo in senso antiorario;

#i angolo intorno all’assezi�1 tra l’assexi�1 e l’assexi valutatopositivo in senso antiorario.

� ai e�i sono sempre costanti

� se il giuntoe rotoidalela variabilee#i

� se il giuntoeprismaticola variabileedi


� Trasformazione di coordinate

Ai�1i0

=

264c#i �s#i 0 0

s#i c#i 0 0

0 0 1 di

0 0 0 1

375

Ai

0

i=

2641 0 0 ai

0 c�i�s�i

0

0 s�ic�i

0

0 0 0 1

375

Ai�1i

(qi) = Ai�1i0

Ai

0

i=

264c#i �s#ic�i

s#is�iaic#i

s#i c#ic�i�c#is�i

ais#i

0 s�ic�i

di

0 0 0 1

375


Procedura operativa

1. Individuare e numerare consecutivamente gli assi dei giunti;assegnare, rispettivamente, le direzioni agli assiz0, : : : , zn�1

2. Fissare la terna base posizionandone l’origine sull’assez0; gliassix0 e y0 sono scelti in maniera tale da ottenere una ternalevogira

Eseguire i passi da3 a 5 peri = 1; : : : ; n� 1:

3. Individuare l’origineOi all’intersezione dizi con la normalecomune agli assizi�1 ezi. Se gli assizi�1 ezi sono paralleli eil giunto i e rotoidale, posizionareOi in modo da annullaredi;se il giuntoi e prismatico, scegliereOi in corrispondenza diuna posizione di riferimento per la corsa del giunto (ad esempioun fine-corsa)

4. Fissare l’assexi diretto lungo la normale comune agli assizi�1

ezi con verso positivo dal giuntoi al giuntoi+ 1

5. Fissare l’asseyi in modo da ottenere una terna levogira

Per completare:

6. Fissare la ternan, allineandozn lungo la direzione dizn�1 se ilgiunton e rotoidale, ovvero scegliendozn in maniera arbitrariase il giunton e prismatico; fissare l’assexn in accordo al punto4

7. Costruire peri = 1; : : : ; n la tabella dei parametriai; di; �i; #i

8. Calcolare sulla base dei parametri di cui al punto7 le matricidi trasformazione omogeneaAi�1

i(qi) peri = 1; : : : ; n

9. CalcolareT 0n

(q) = A01 : : :A

n�1n

che fornisce posizione eorientamento della ternan rispetto alla terna0

10. AssegnateT b0 e T ne

, calcolare la funzione cinematica diretta

Tb

e(q) = T

b

0T0nT

n

e

che fornisce posizione e orientamento dellaterna utensile rispetto alla terna base


Catena chiusa

� Connessione di un singolo braccio con due bracci

� giunto virtuale di taglio

Ai

j(q0) = A

i

i+10(qi+10) : : :Aj�1j

(qj)

Ai

k(q00) = A

i

i+100(qi+100) : : :Ak�1k

(qk)


� Vincoli

? giuntoj + 1 rotoidale(R

j

i(q0)

�p

i

j(q0)� pi

k(q00)

�= [ 0 0 djk ]

T

zi

j(q0) = z

i

k(q00)

? giuntoj + 1 prismatico8>>><>>>:�

xiT

j(q0)

yiT

j(q0)

� �p

i

j(q0)� pi

k(q00)

�=

�0

0

�z

i

j(q0) = z

i

k(q00)

xiT

j(q0)xi

k(q00) = cos#jk

? risolti in termini diq : : :

T0n

(q) = A0iA

i

jA

j

n


Procedura operativa

1. Selezionare un giunto non attuato della catena chiusa. Ipotizzaredi aprire tale giunto in modo da ottenere una catena aperta construttura ad albero

2. Calcolare le trasformazioni omogenee secondo la convenzionedi Denavit-Hartenberg

3. Trovare i vincoli di uguaglianza per le due terne connesse dalgiunto di taglio

4. Risolvere i vincoli in termini di un numero ridotto di variabilidi giunto

5. Esprimere le trasformazioni omogenee in funzione di tali va-riabili di giunto e calcolare la funzione cinematica diretta percomposizione della varie trasformazioni dalla terna base allaterna utensile


Manipolatore planare a tre bracci


Braccio ai �i di #i

1 a1 0 0 #1

2 a2 0 0 #2

3 a3 0 0 #3


Ai�1i

=

264ci �si 0 aici

si ci 0 aisi

0 0 1 0

0 0 0 1

375 i = 1; 2; 3

T03 = A

01A

12A

23

=

264c123 �s123 0 a1c1 + a2c12 + a3c123

s123 c123 0 a1s1 + a2s12 + a3s123

0 0 1 0

0 0 0 1

375


Manipolatore a parallelogramma



10 a10 0 0 #10

20 a20 0 0 #20

30 a30 0 0 #30

100 a100 0 0 #100

4 a4 0 0 0


A030(q0) = A

010A

10

20A20

30

=

264c102030 �s102030 0 a10c10 + a20c1020 + a30c102030

s102030 c102030 0 a10s10 + a20s1020 + a30s102030

0 0 1 0

0 0 0 1

375

A0100(q00) =

264c100 �s100 0 a100c100

s100 c100 0 a100s100

0 0 1 0

0 0 0 1

375

A30

4 =

2641 0 0 a4

0 1 0 0

0 0 1 0

0 0 0 1

375


� Risoluzione vincoli

? orientamento (OK)

? posizione

R30

0 (q0)

�p

030(q0)� p0100(q00)

�=

24 0

0

0

35+

a10(c10 + c102030) + a100(c1020 � c100) = 0

a10(s10 + s102030) + a100(s1020 � s100) = 0

#20 = #100 � #10

#30 = � � #20 = � � #100 + #10

� Cinematica diretta

T04 (q) = A

030(q)A30

4 =

264�c10 s10 0 a100c100 � a4c10

�s10 �c10 0 a100s100 � a4s10

0 0 1 0

0 0 0 1

375


Manipolatore sferico



1 0 ��=2 0 #1

2 0 �=2 d2 #2

3 0 0 d3 0


A01 =

264c1 0 �s1 0

s1 0 c1 0

0 �1 0 0

0 0 0 1

375 A12 =

264c2 0 s2 0

s2 0 �c2 0

0 1 0 d2

0 0 0 1

375

A23 =

2641 0 0 0

0 1 0 0

0 0 1 d3

0 0 0 1

375

T03 = A

01A

12A

23

=

264c1c2 �s1 c1s2 c1s2d3 � s1d2

s1c2 c1 s1s2 s1s2d3 + c1d2

�s2 0 c2 c2d3

0 0 0 1

375



1 0 �=2 0 #1

2 a2 0 0 #2

3 a3 0 0 #3


A01 =

264c1 0 s1 0

s1 0 �c1 0

0 1 0 0

0 0 0 1

375

Ai�1i

=

264ci �si 0 aici

si ci 0 aisi

0 0 1 0

0 0 0 1

375 i = 2; 3

T03 = A

01A

12A

23

=

264c1c23 �c1s23 s1 c1(a2c2 + a3c23)

s1c23 �s1s23 �c1 s1(a2c2 + a3c23)

s23 c23 0 a2s2 + a3s23

0 0 0 1

375


Polso sferico



4 0 ��=2 0 #4

5 0 �=2 0 #5

6 0 0 d6 #6


A34 =

264c4 0 �s4 0

s4 0 c4 0

0 �1 0 0

0 0 0 1

375 A45 =

264c5 0 s5 0

s5 0 �c5 0

0 1 0 0

0 0 0 1

375

A56 =

264c6 �s6 0 0

s6 c6 0 0

0 0 1 d6

0 0 0 1

375

T36 = A

34A

45A

56

=

264c4c5c6 � s4s6 �c4c5s6 � s4c6 c4s5 c4s5d6

s4c5c6 + c4s6 �s4c5s6 + c4c6 s4s5 s4s5d6

�s5c6 s5s6 c5 c5d6

0 0 0 1

375


Manipolatore di Stanford


T06 = T

03 T

36 =

264n0s

0a

0p

0

0 0 0 1

375


p0 =

24 c1s2d3 � s1d2 +�c1(c2c4s5 + s2c5)� s1s4s5

�d6

s1s2d3 + c1d2 +�s1(c2c4s5 + s2c5) + c1s4s5

�d6

c2d3 + (�s2c4s5 + c2c5)d6

35

n0 =

24 c1�c2(c4c5c6 � s4s6)� s2s5c6

�� s1(s4c5c6 + c4s6)

s1

�c2(c4c5c6 � s4s6)� s2s5c6

�+ c1(s4c5c6 + c4s6)

�s2(c4c5c6 � s4s6)� c2s5c6

35

s0 =

24 c1��c2(c4c5s6 + s4c6) + s2s5s6

�� s1(�s4c5s6 + c4c6)

s1

��c2(c4c5s6 + s4c6) + s2s5s6

�+ c1(�s4c5s6 + c4c6)

s2(c4c5s6 + s4c6) + c2s5s6

35

a0 =

24 c1(c2c4s5 + s2c5)� s1s4s5

s1(c2c4s5 + s2c5) + c1s4s5

�s2c4s5 + c2c5

35


Manipolatore antropomorfo con polso sferico



1 0 �=2 0 #1

2 a2 0 0 #2

3 0 �=2 0 #3

4 0 ��=2 d4 #4

5 0 �=2 0 #5

6 0 0 d6 #6


A23 =

264c3 0 s3 0

s3 0 �c3 0

0 1 0 0

0 0 0 1

375 A34 =

264c4 0 �s4 0

s4 0 c4 0

0 �1 0 d4

0 0 0 1

375

p0 =

24 a2c1c2 + d4c1s23 + d6

�c1(c23c4s5 + s23c5) + s1s4s5

�a2s1c2 + d4s1s23 + d6

�s1(c23c4s5 + s23c5)� c1s4s5

�a2s2 � d4c23 + d6(s23c4s5 � c23c5)

35

n0 =

24 c1�c23(c4c5c6 � s4s6)� s23s5c6

�+ s1(s4c5c6 + c4s6)

s1

�c23(c4c5c6 � s4s6)� s23s5c6

�� c1(s4c5c6 + c4s6)

s23(c4c5c6 � s4s6) + c23s5c6

35

s0 =

24 c1��c23(c4c5s6 + s4c6) + s23s5s6

�+ s1(�s4c5s6 + c4c6)

s1

��c23(c4c5s6 + s4c6) + s23s5s6

�� c1(�s4c5s6 + c4c6)

�s23(c4c5s6 + s4c6)� c23s5s6

35

a0 =

24 c1(c23c4s5 + s23c5) + s1s4s5

s1(c23c4s5 + s23c5)� c1s4s5

s23c4s5 � c23c5

35


SPAZIO DEI GIUNTI E SPAZIO OPERATIVO

� Spazio dei giunti

q =

24 q1...

qn

35? qi = #i (giunto rotoidale)

? qi = di (giunto prismatico)

� Spazio operativo

x =

�p

�

�? p (posizione)

? � (orientamento)

� Equazione cinematica diretta

x = k(q)


� Esempio

x =

24 pxpy�

35 = k(q) =

24 a1c1 + a2c12 + a3c123

a1s1 + a2s12 + a3s123

#1 + #2 + #3

35


Spazio di lavoro

� Spazio di lavororaggiungibile

p = p(q) qim � qi � qiM i = 1; : : : ; n

? elementi di superficie planare, sferica, toroidale e cilindrica

� Spazio di lavorodestro

? orientamenti diversi


� Esempio

? configurazioni ammissibili


? spazio di lavoro


� Accuratezza

? scostamento tra posizione conseguita con la postura asse-gnata e posizione calcolata tramite la cinematica diretta

? valori tipici: (0:2; 1) mm

� Ripetibilita

? capacita del manipolatore di tornare in una posizione pre-cedentemente raggiunta

? valori tipici: (0:02; 0:2) mm

� Ridondanza cinematica

? m < n (intrinseca)? r < m = n (funzionale)


CALIBRAZIONE CINEMATICA

� Valori precisi dei parametri DH per migliorare l’accuratezza diun manipolatore

� Equazione cinematica diretta in funzione di tutti i parametri

x = k(a;�;d;#)

xm locazione misurata

xn locazione nominale (parametri fissi + variabili giunto)

�x =@k

@a�a+

@k

@��+

@k

@d�d+

@k

@#�#

= �(�n)��


? l misure (lm� 4n)

��x =

24�x1...

�xl

35 =

24�1

...

�l

35�� = ��

� Soluzione

�� = (��T ��)�1 ��T��x

�0 = �n +��

: : : finche�� converge? stime piu accurate dei parametri fissi

? correzioni alle misure dei trasduttori

Inizializzazione

� postura di riferimento (home)


PROBLEMA CINEMATICO INVERSO

� Cinematica diretta

? q =) T

? q =) x

� Cinematica inversa

? T =) q

? x =) q

� Complessita

? soluzione analitica (in forma chiusa) ?? soluzioni multiple

? infinite soluzioni

? non esistono soluzioni ammissibili

� Intuizione

? algebrica

? geometrica

� Tecniche numeriche


Soluzione del manipolatore planare a tre bracci

� Soluzione algebrica

� = #1 + #2 + #3

pWx = px � a3c� = a1c1 + a2c12

pWy = py � a3s� = a1s1 + a2s12


c2 =p2Wx

+ p2Wy

� a21 � a22

2a1a2

s2 = �q

1� c22

#2 = Atan2(s2; c2)

s1 =(a1 + a2c2)pWy � a2s2pWx

p2Wx

+ p2Wy

c1 =(a1 + a2c2)pWx + a2s2pWy

p2Wy

+ p2Wy

#1 = Atan2(s1; c1)

#3 = �� #1 � #2


� Soluzione geometrica

c2 =p2Wx

+ p2Wy

� a21 � a22

2a1a2

.

#2 = cos�1(c2)

� = Atan2(pWy; pWx)

c�

qp2Wx

+ p2Wy

= a1 + a2c2

� = cos�1

0@p2Wx+ p2

Wy+ a21 � a22

2a1

qp2Wx

+ p2Wy

1A#1 = ��


Soluzione di manipolatori con polso sferico

pW = p� d6a

� Soluzione disaccoppiata

? calcolare la posizione del polsopW (q1; q2; q3)

? risolvere la cinematica inversa per(q1; q2; q3)

? calcolareR03(q1; q2; q3)

? calcolareR36(#4; #5; #6) = R

03TR

? risolvere la cinematica inversa per l’orientamento(#4; #5; #6)


Soluzione del manipolatore sferico

(A01)

�1T

03 = A

12A

23

p1W

=

24 pWxc1 + pWys1

�pWz

�pWxs1 + pWyc1

35 =

24 d3s2

�d3c2d2

35


c1 =1� t2

1 + t2s1 =

2t

1 + t2

(d2 + pWy)t2 + 2pWxt+ d2 � pWy = 0

#1 = 2Atan2��pWx �

qp2Wx

+ p2Wy

� d22; d2 + pWy

�

pWxc1 + pWys1

�pWz

=d3s2

�d3c2

#2 = Atan2(pWxc1 + pWys1; pWz)

d3 =

q(pWxc1 + pWys1)2 + p2

Wz


Soluzione del manipolatore antropomorfo

#1 = Atan2(pWy; pWx) #1 = � +Atan2(pWy; pWx)

c3 =p2Wx

+ p2Wy

+ p2Wz

� a22 � a23

2a2a3

s3 = �q

1� c23

s2 =(a2 + a3c3)pWz � a3s3

qp2Wx

+ p2Wy

p2Wx

+ p2Wy

+ p2Wz

c2 =(a2 + a3c3)

qp2Wx

+ p2Wy

+ a3s3pWz

p2Wx

+ p2Wy

+ p2Wz


� Quattro configurazioni ammissibili

? soluzione univoca solo se

pWx 6= 0 pWy 6= 0


Soluzione del polso sferico

R36 =

24n3x s3x

a3x

n3y

s3y

a3y

n3z

s3z

a3z

35

#4 = Atan2(a3y; a

3x

)

#5 = Atan2�q

(a3x

)2 + (a3y)2; a3

z

�#6 = Atan2(s3

z;�n3

z)

#4 = Atan2(�a3y;�a3

x)

#5 = Atan2��q

(a3x

)2 + (a3y)2; a3

z

�#6 = Atan2(�s3

z; n

3z)


CINEMATICA DIFFERENZIALE

� relazioni tra velocita dei giunti e velocita dell’organo terminale

Jacobiano geometrico

Jacobiano analitico

Singolarita cinematiche

Analisi della ridondanza

Inversione della cinematica differenziale

Algoritmi per l’inversione cinematica

STATICA

� relazioni tra forze all’organo terminale e coppie ai giunti


JACOBIANO GEOMETRICO

T (q) =

264 R(q) p(q)

0T 1

375

� Obiettivo

_p = JP (q) _q

! = JO(q) _q

v =

�_p

!

�= J(q) _q


Derivata di una matrice di rotazione

R(t)RT (t) = I

_R(t)RT (t) +R(t) _RT (t) = O

� Posto S(t) = _R(t)RT (t)

S(t) + ST (t) = O

_R(t) = S(!(t))R(t)

S =

24 0 �!z !y

!z 0 �!x�!y !x 0

35


� Esempio

Rz(�) =

24 cos� �sin� 0

sin� cos� 0

0 0 1

35

S(t) =

24� _� sin� � _� cos� 0

_� cos� � _� sin� 0

0 0 0

3524 cos� sin� 0

�sin� cos� 0

0 0 1

35=

24 0 � _� 0

_� 0 0

0 0 0

35 = S(!(t))


p0 = o

01 +R

01p

1

_p0 = _o01 +R01 _p

1 + _R01p

1

= _o01 +R01 _p

1 + S(!01)R

01p

1

= _o01 +R01 _p

1 + !01 � r01


Velocita di un braccio

� Velocita lineare

pi = pi�1 +Ri�1ri�1i�1;i

_pi = _pi�1 +Ri�1 _ri�1i�1;i + !i�1 �Ri�1r

i�1i�1;i

= _pi�1 + vi�1;i + !i�1 � ri�1;i


� Velocita angolare

Ri = Ri�1Ri�1i

S(!i)Ri = S(!i�1)Ri +Ri�1S(!i�1i�1;i)R

i�1i

= S(!i�1)Ri + S(Ri�1!i�1i�1;i)Ri

!i = !i�1 +Ri�1!i�1i�1;i

= !i�1 + !i�1;i


!i = !i�1 + !i�1;i

_pi = _pi�1 + vi�1;i + !i�1 � ri�1;i

� Giunto prismatico

!i�1;i = 0

vi�1;i = _dizi�1

!i = !i�1

_pi = _pi�1 + _dizi�1 + !i � ri�1;i

� Giunto rotoidale!i�1;i = _#izi�1

vi�1;i = !i�1;i � ri�1;i

!i = !i�1 + _#izi�1

_pi = _pi�1 + !i � ri�1;i


Calcolo dello Jacobiano

J =

24 |P1 |Pn

: : :

|O1 |On

35� Velocita angolare

? giuntoi prismatico

_qi|Oi = 0 =) |Oi = 0

? giuntoi rotoidale

_qi|Oi = _#izi�1 =) |Oi = zi�1


� Velocita lineare

? giuntoi prismatico

_qi|Pi = _dizi�1 =) |Pi = zi�1

? giuntoi rotoidale_qi|Pi = !i�1;i � ri�1;n

= _#izi�1 � (p� pi�1)

+

|Pi = zi�1 � (p� pi�1)


� Colonna dello Jacobiano geometrico

�|Pi

|Oi

�=

8>><>>:�

zi�1

0

�

per un giuntoprismatico�zi�1 � (p� pi�1)

zi�1

�

per un giuntorotoidale

? zi�1 = R01(q1) : : :R

i�2i�1(qi�1)z0

? ~p = A01(q1) : : :A

n�1n

(qn)~p0

? ~pi�1 = A01(q1) : : :A

i�2i�1(qi�1)~p0


� Rappresentazione in terna differente

�_pt

!t

�=

�R

tO

O Rt

� �_p

!

�=

�R

tO

O Rt

�J _q

Jt =

�R

tO

O Rt

�J


Manipolatore planare a tre bracci


J(q) =

�z0 � (p� p0) z1 � (p� p1) z2 � (p� p2)

z0 z1 z2

�

p0 =

24 0

0

0

35 p1 =

24 a1c1a1s1

0

35 p2 =

24 a1c1 + a2c12

a1s1 + a2s12

0

35

p =

24 a1c1 + a2c12 + a3c123

a1s1 + a2s12 + a3s123

0

35

z0 = z1 = z2 =

24 0

0

1

35


J =

2666664�a1s1 � a2s12 � a3s123 �a2s12 � a3s123 �a3s123a1c1 + a2c12 + a3c123 a2c12 + a3c123 a3c123

0 0 0

0 0 0

0 0 0

1 1 1

3777775

JP =

��a1s1 � a2s12 � a3s123 �a2s12 � a3s123 �a3s123a1c1 + a2c12 + a3c123 a2c12 + a3c123 a3c123

�


J =

�z0 � (p� p0) z1 � (p� p1) z2 � (p� p2)

z0 z1 z2

�

p0 = p1 =

24 0

0

0

35 p2 =

24 a2c1c2a2s1c2

a2s2

35

p =

24 c1(a2c2 + a3c23)

s1(a2c2 + a3c23)

a2s2 + a3s23

35

z0 =

24 0

0

1

35 z1 = z2 =

24 s1

�c10

35


J =

2666664�s1(a2c2 + a3c23) �c1(a2s2 + a3s23) �a3c1s23c1(a2c2 + a3c23) �s1(a2s2 + a3s23) �a3s1s23

0 a2c2 + a3c23 a3c23

0 s1 s1

0 �c1 �c11 0 0

3777775

JP =

24�s1(a2c2 + a3c23) �c1(a2s2 + a3s23) �a3c1s23c1(a2c2 + a3c23) �s1(a2s2 + a3s23) �a3s1s23

0 a2c2 + a3c23 a3c23

35


Manipolatore di Stanford


J =

�z0 � (p� p0) z1 � (p� p1) z2

z0 z1 0

z3 � (p� p3) z4 � (p� p4) z5 � (p� p5)z3 z4 z5

�

p0 = p1 =

24 0

0

0

35 p3 = p4 = p5 =

24 c1s2d3 � s1d2

s1s2d3 + c1d2

c2d3

35

p =

24 c1s2d3 � s1d2 + d6(c1c2c4s5 + c1c5s2 � s1s4s5)

s1s2d3 + c1d2 + d6(c1s4s5 + c2c4s1s5 + c5s1s2)

c2d3 + d6(c2c5 � c4s2s5)

35

z0 =

24 0

0

1

35 z1 =

24�s1c10

35 z2 = z3 =

24 c1s2s1s2

c2

35

z4 =

24�c1c2s4 � s1c4

�s1c2s4 + c1c4

s2s4

35 z5 =

24 c1c2c4s5 � s1s4s5 + c1s2c5

s1c2c4s5 + c1s4s5 + s1s2c5

�s2c4s5 + c2c5

35


JACOBIANO ANALITICO

p = p(q)

� = �(q)

_p =@p

@q_q = JP (q) _q

_� =@�

@q_q = J�(q) _q

_x =

�_p_�

�=

�JP (q)

J�(q)

�_q

= JA(q) _q

JA(q) =@k(q)

@q


� Velocita di rotazione in terna corrente di angoli di Eulero ZYZ

? per effetto di _': [!x !y !z ]T = _' [ 0 0 1 ]

T

? per effetto di _#: [!x !y !z ]T = _# [�s' c' 0 ]

T

? per effetto di _ : [!x !y !z ]T = _ [ c's# s's# c# ]

T


� Composizione di velocita di rotazione elementari

! =

24 0 �s' c's#

0 c' s's#

1 0 c#

35 _� = T (�) _�


� Significato fisico 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

Z 2

0

!dt = [�=2 �=2 0 ]T


Relazione tra Jacobiano analitico eJacobiano geometrico

v =

�I O

O T (�)

�_x = TA(�) _x

J = TA(�)JA

� Jacobiano geometrico

? grandezze di significato fisico

� Jacobiano analitico

? grandezze differenziali di variabili nello spazio operativo


SINGOLARITA CINEMATICHE

v = J(q) _q

� seJ diminuisce di rango =) singolarita cinematiche

(a) perdita di mobilita

(b) infinite soluzioni al problema cinematico inverso

(c) velocita elevate nello spazio dei giunti (nell’intorno di unasingolarita)

� Classificazione

? Singolaritaai confini dello spazio di lavoro raggiungibile

? Singolaritaall’interno dello spazio di lavoro raggiungibile


� Manipolatore planare a due bracci

J =

��a1s1 � a2s12 �a2s12a1c1 + a2c12 a2c12

�det(J) = a1a2s2

+

#2 = 0 #2 = �

? [�(a1 + a2)s1 (a1 + a2)c1 ]T parallelo a[�a2s1 a2c1 ]

T

(componenti di velocita dell’organo terminale non indipen-denti)


Disaccoppiamento di singolarita

� calcolo dellesingolarita della struttura portante

� calcolo dellesingolarita del polso


J =

�J11 J12

J21 J22

�

J12 =�z3 � (p� p3) z4 � (p� p4) z5 � (p� p5)

�J22 =

�z3 z4 z5

�� p = pW =) pW � pi paralleli azi, i = 3; 4; 5

J12 =�

0 0 0�

det(J) = det(J11)det(J22)

det(J11) = 0 det(J22) = 0


Singolarita di polso

� z3 parallelo az5

#5 = 0 #5 = �

? rotazioni uguali e opposte di#4 e#6 non producono alcunarotazione dell’organo terminale


Singolarita di struttura portante

� Manipolatore antropomorfo

det(JP ) = �a2a3s3(a2c2 + a3c23)

s3 = 0 a2c2 + a3c23 = 0

? Singolarita digomito

#3 = 0 #3 = �


? Singolarita dispalla

px = py = 0


ANALISI DELLA RIDONDANZA

� Cinematica differenziale

v = J(q) _q

? se%(J) = r

dim�R(J)

�= r dim

�N (J)

�= n� r

? in generale

dim�R(J)

�+ dim

�N (J)

�= n


� SeN (J) 6= ;_q = _q� +P _qa

ove

R(P ) � N (J)

? verifica:

J _q = J _q� + JP _qa = J _q� = v

� _qa generamoti internidella struttura


INVERSIONE DELLA CINEMATICADIFFERENZIALE

� Equazione cinematica non lineare

� Equazione cinematica differenziale lineare nelle velocita

� Datav(t) + condizioni iniziali =) (q(t); _q(t))

? sen = r

_q = J�1(q)v

q(t) =

Zt

0

_q(&)d& + q(0)

? regola di integrazione numerica (Eulero)

q(tk+1) = q(tk) + _q(tk)�t


Manipolatori ridondanti

� Per una data configurazioneq, trovare le soluzioni_q che sod-disfino

v = J _q

e che minimizzino

g( _q) =1

2_qTW _q

? metodo dei moltiplicatori di Lagrange

g( _q;�) =1

2_qTW _q + �

T (v � J _q)

�@g

@ _q

�T= 0

�@g

@�

�T= 0

? soluzione ottima

_q =W�1

JT (JW�1

JT )�1v

? seW = I

_q = Jyv

ove

Jy = J

T (JJT )�1

e lapseudo-inversa destradi J


� Utilizzo della ridondanza

g0( _q) =

1

2( _qT � _qT

a)( _q � _qa)

? come sopra: : :

g0( _q;�) =

1

2( _qT � _qT

a)( _q � _qa) + �

T (v � J _q)

? soluzione ottima

_q = Jyv + (I � JyJ) _qa


� Caratterizzazione dei moti interni

_qa = ka

�@w(q)

@q

�T

? misura di manipolabilita

w(q) =

qdet

�J(q)JT (q)

�? distanza dai fine-corsa dei giunti

w(q) = � 1

2n

nXi=1

�qi � �qi

qiM � qim

�2

? distanza da un ostacolo

w(q) = minp;o

kp(q)� ok


Singolarita cinematiche

� Le soluzioni precedenti valgono solo seJ e di rango pieno

� SeJ non e di rango pieno (singolarita)

? sev 2 R(J) =) soluzione_q estraendo tutte le equa-zioni linearmente indipendenti (traiettoria “fisicamente”eseguibile)

? sev =2 R(J) =) il sistema none risolvibile (traietto-ria non eseguibile)

� Inversione nell’intorno di singolarita

? det(J) piccolo =) _q elevate

? inversa a minimi quadrati smorzata

J? = J

T (JJT + k2I)�1

ove _q minimizza

g00( _q) = kv � J _qk2 + k

2k _qk2


ALGORITMI PER L’INVERSIONECINEMATICA

� Inversione cinematica

q(tk+1) = q(tk) + J�1(q(tk))v(tk)�t

? fenomeni diderivadella soluzione

� Soluzione algoritmica

? errore nello spazio operativo

e = xd � x

_e = _xd � _x

= _xd � JA(q) _q

? trovare _q = _q(e): e! 0


(Pseudo-)inversa dello Jacobiano

� Linearizzazione della dinamica di errore

_q = J�1A

(q)( _xd +Ke)

+

_e+Ke = 0

? Per unmanipolatore ridondante

_q = JyA

( _xd +Ke) + (I � JyAJA) _qa


Trasposta dello Jacobiano

� _q = _q(e) senza linearizzare la dinamica di errore

� Metodo di Lyapunov

V (e) =1

2e

TKe

ove

V (e) > 0 8e 6= 0 V (0) = 0

_V (e) = eTK _xd � eTK _x

= eTK _xd � eTKJA(q) _q

? la scelta

_q = JT

A(q)Ke

comporta che

_V (e) = eTK _xd � eTKJA(q)J

T

A(q)Ke

? se _xd = 0 =) _V < 0 conV > 0 (asintotica stabilita)

? seN (JTA

) 6= ; =) _V = 0 seKe 2 N (JTA

)

_q = 0 cone 6= 0 (stallo?)


� Se _xd 6= 0

? e(t) limitato (conviene aumentare la norma diK)

? e(1)! 0


� Esempio

JT

P=

24 0 0 0

�c1(a2s2 + a3s23) �s1(a2s2 + a3s23) 0

�a3c1s23 �a3s1s23 a3c23

35

? nullo diJTP

�y

�x= � 1

tan#1�z = 0


Errore di orientamento

� Errore di posizione

eP = pd � p(q)

_eP = _pd � _p

� Angoli di Eulero

eO = �d � �(q)

_eO = _�d � _�

_q = J�1A

(q)

�_pd +KPeP_�d +KOeO

�? agevole per assegnare l’andamento temporale�d(t)

? richiede comunque il passaggio attraversoR = [n s a ]

� Manipolatore con polso sferico

? calcolareqP =) RW

? calcolareRT

WRd =) qO (angoli di Eulero ZYZ)


� Asse/angolo

R(#; r) = RdRT

? errore di orientamento

eO = r sin#

=1

2(n� nd + s� sd + a� ad)

_eO = LT!d �L!

oveL = �1

2

�S(nd)S(n) + S(sd)S(s) + S(ad)S(a)

�

_e =

�_eP_eO

�=

�_pd � JP (q) _qL

T!d �LJO(q) _q

�

=

�_pdL

T!d

��

�I O

O L

�J _q

_q = J�1(q)

�_pd +KPeP

L�1

�L

T!d +KOeO

� �


� Quaternione unitario

�Q = Qd � Q�1

? errore di orientamento

eO = �� = �(q)�d � �d�(q)� S(�d)�(q)

_q = J�1(q)

�_pd +KPeP

!d +KOeO

�!d � ! +KOeO = 0

? propagazione del quaternione

_� = �1

2�T!

_� =1

2(�I � S(�))!

? studio della stabilita

V = (�d � �)2 + (�d � �)T (�d � �)

_V = �eTOKOeO


Confronto tra gli algoritmi per l’inversionecinematica

� Manipolatore planare a tre bracci

x = k(q)24 pxpy�

35 =

24 a1c1 + a2c12 + a3c123

a1s1 + a2s12 + a3s123

#1 + #2 + #3

35

? a1 = a2 = a3 = 0:5mJA =

24�a1s1 � a2s12 � a3s123 �a2s12 � a3s123 �a3s123a1c1 + a2c12 + a3c123 a2c12 + a3c123 a3c123

1 1 1

35


? qi = [� ��=2 ��=2 ]T rad

+

? pdi = [ 0 0:5 ]T m � = 0 rad

? traiettoria desiderata

pd(t) =

�0:25(1� cos�t)

0:25(2 + sin�t)

�0 � t � 4

�d(t) = sin�

24t 0 � t � 4

� Simulazione in MATLAB con integrazione numerica di Eulero

q(tk+1) = q(tk) + _q(tk)�t

e�t = 1ms


� _q = J�1A

(q) _x

0 1 2 3 4 50

0.5

1

1.5

2x 10

−3

[s]

[m]

norma errore pos

0 1 2 3 4 5−1

−0.8

−0.6

−0.4

−0.2

0x 10

−5

[s][r

ad]

errore orien


� _q = J�1A

(q)( _xd +Ke) K = diagf500; 500; 100g

0 1 2 3 4 5−5

0

5

[s]

[rad

]

pos giunti

1

2

3

0 1 2 3 4 5−10

−5

0

5

10

[s][r

ad/s

]

vel giunti

12

3

0 1 2 3 4 50

0.2

0.4

0.6

0.8

1x 10

−5

[s]

[m]

norma errore pos

0 1 2 3 4 5−5

−4

−3

−2

−1

0x 10

−8

[s]

[rad

]

errore orien


� � libero (r = 2, n = 3)

� _q = JyP

( _pd +KPeP ) KP = diagf500; 500g

0 1 2 3 4 50

1

2

3

4

5x 10

−6

[s]

[m]

norma errore pos

0 1 2 3 4 5−1

−0.5

0

0.5

[s]

[rad

]

orien

� _q = JT

P(q)KPeP KP = diagf500; 500g

0 1 2 3 4 50

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]

[rad

]

orien


� _q = JyP

( _pd +KPeP ) + (I � JyPJP ) _qa

KP = diagf500; 500g

? misura di manipolabilita

w(#2; #3) =1

2(s22 + s

23)

? _qa = ka

�@w(q)

@q

�Tka = 50

0 1 2 3 4 5−5

0

5

[s]

[rad

]

pos giunti

1

2

3

0 1 2 3 4 5−5

0

5

[s]

[rad

/s]

pos giunti

1

2

3

0 1 2 3 4 50

1

2

3

4

5x 10

−6

[s]

[m]

norma errore pos

0 1 2 3 4 50.85

0.9

0.95

1

[s]

[rad

]

manip


? distanza dai fine-corsa dei giunti

w(q) = �1

6

3Xi=1

�qi � �qi

qiM � qim

�2

�2� � q1 � 2� ��=2 � q2 � �=2 �3�=2 � q3 � ��=2

? _qa = ka

�@w(q)

@q

�Tka = 250

0 1 2 3 4 50

0.5

1

1.5

2x 10

−4

[s]

[m]

norma errore pos

0 1 2 3 4 5

−6

−4

−2

0

2

4

6

[s]

[rad

]

pos giunto 1

0 1 2 3 4 5

−6

−4

−2

0

2

4

6

[s]

[rad

]

pos giunto 2

0 1 2 3 4 5−5

0

5

[s]

[rad

]

pos giunto 3


STATICA

� Relazione tra forze e momenti (forze) all’organo terminalee forze e/o coppie (coppie) � ai giunti con il manipolatore inconfigurazione di equilibrio

? lavoro elementare compiuto dalle coppie

dW� = �Tdq

? lavoro elementare compiuto dalle forze

dW = fTdp+ �

T!dt

= fTJP (q)dq + �

TJO(q)dq

= TJ(q)dq

? spostamenti elementari � spostamenti virtuali

�W� = �T�q

�W = TJ(q)�q


� Principio dei lavori virtuali

? il manipolatore e in equilibrio staticose e solo se

�W� = �W 8�q

+

� = JT (q)


Dualita cineto-statica

N (J) � R?(JT ) R(J) � N?(JT )

� forze 2 N (JT ) interamente assorbite dalla struttura


� Interpretazione fisica dello schema con la trasposta dello Jaco-biano

? dinamica ideale � = _q

? forza elastica Ke che tira l’organo terminale verso la po-stura desiderata nello spazio operativo

? ha effetto solo se Ke =2 N (JT )


Trasformazione di velocita e forze

�_p2!2

�=

�I �S(r12)O I

� �_p1!1

�

r12 = R1r112

_p1 = R1 _p11 _p2 = R2 _p

22 = R1R

12 _p

22

!1 = R1!11 !2 = R2!

22 = R1R

12!

22


�_p22

!22

�=

�R

21 �R2

1S(r112)

O R21

� �_p11

!11

�v

22 = J

21v

11

? in virtu della dualita cineto-statica:

11 = J

21

T

22�

f11

�11

�=

�R

12 O

S(r112)R12 R

12

� �f

22

�22

�


Catena chiusa

� Struttura ad albero equivalente a catena aperta

qo =

�qa

qu

�

� Risolvendo i vincoli: qu = qu(qa)

_qo = � _qa

� =

24 I

@qu

@qa

35? per la dualita cineto-statica

�a = �T�o

� Manipolatore a parallelogramma

qa =

�#10

#100

�qu =

�#200

#300

�

� =

2641 0

0 1

�1 1

1 �1

375�a =

��10 � �20 + �30

�100 + �20 � �30

��u =

�0

0

�


ELLISSOIDI DI MANIPOLABILITA

� Ellissoide di manipolabilita in velocita

? insieme delle velocita ai giunti a norma costante

_qT _q = 1

? manipolatore ridondante

_q = Jy(q)v

+

vT�J(q)JT(q)

��1v = 1

� Assi

? autovettori ui di JJT =) direzioni

? valori singolari �i =p

�i(JJT ) =) dimensioni

� Volume

? proporzionale a

w(q) =

qdet

�J(q)JT (q)

�



� Misura di manipolabilita

w = jdet(J)j = a1a2js2j

? max per #2 = ��=2

? max per a1 = a2 (a parita di estensione a1 + a2)


� Ellissi di manipolabilita in velocita

0 0.5 1 1.5 2

−1

−0.5

0

0.5

1

[m]

[m]

� Valori singolari

0 0.5 1 1.5 20

0.5

1

1.5

2

2.5

min

max

[m]

[m]


� Ellissoide di manipolabilita in forza

? insieme delle coppie ai giunti a norma costante

�T� = 1

+

T�J(q)JT(q)

� = 1

� Dualita cineto-statica

? una direzione lungo la quale si ha elevata manipolabilit a invelocita e una direzione lungo la quale si ha scarsa manipo-labilita in forza, e viceversa



� Ellissi di manipolabilita in velocita

0 0.5 1 1.5 2

−1

−0.5

0

0.5

1

[m]

[m]

� Ellissi di manipolabilita in forza

0 0.5 1 1.5 2

−1

−0.5

0

0.5

1

[m]

[m]


� Manipolatore � trasformatore meccanicodi velocita e forzedallo spazio dei giunti allo spazio operativo

? rapporto di trasformazione lungo una direzione perl’ellissoide in forza

�(q) =

�u

TJ(q)JT (q)u

��1=2

? rapporto di trasformazione lungo una direzione perl’ellissoide in velocita

�(q) =

�u

T�J(q)JT (q)

��1u

��1=2

? utilizzazione di gradi di mobilita ridondanti


� Compatibilita della struttura ad eseguire un compito assegnatolungo una direzione

? compito di scrittura su superficie orizzontale

forza

vel

piano di scrittura

? compito di lancio di un peso in direzione orizzontale

forza

vel

direzione di lancio


DINAMICA

� equazioni del moto del manipolatore in funzione delle forze emomenti agenti su di esso

Formulazione di Lagrange

Proprieta notevoli del modello dinamico

Identificazione dei parametri dinamici

Formulazione di Newton-Eulero

Dinamica diretta e dinamica inversa

Modello dinamico nello spazio operativo

Ellissoide di manipolabilita dinamica


FORMULAZIONE DI LAGRANGE

� Lagrangiana � (energia cinetica)� (energia potenziale)

L = T � U

� Equazioni di Lagrange

d

dt

@L@ _�i

� @L@�i

= �i i = 1; : : : ; n

? coordinate generalizzate24 �1...

�n

35 = q


� Esempio

? energia cinetica

T =1

2I _#2 +

1

2Imk

2r

_#2

? energia potenziale

U = mg`(1� cos#)

? lagrangiana

L =1

2I _#2 +

1

2Imk

2r

_#2 �mg`(1� cos#)

? equazione del moto

(I + Imk2r)�#+mg` sin# = �

= � � F _#


Energia cinetica

� Contributo bracci + attuatori

T =

nXi=1

(T`i + Tmi)


� Braccio i

Tì =1

2

ZVì

_p�i

T _p�i�dV

? baricentro

pì =1

mì

ZVì

p�i�dV

? velocita lineare della particella elementare

_p�i

= _pì + !i � ri = _pì + S(!i)ri


� Traslazionale

1

2

ZVì

_pTì

_pì�dV =1

2mì

_pTì

_pì

� Mutuo

2

1

2

ZVì

_pTì

S(!i)ri�dV

!=

2

1

2_pTì

S(!i)

ZVì

(p�i� pì)�dV

!= 0


� Rotazionale

1

2

ZVì

rT

iS

T (!i)S(!i)ri�dV =1

2!

T

i

ZVì

ST (ri)S(ri)�dV

!!i

=1

2!

T

iIì!i

? tensore di inerzia

Iì =

264R

(r2iy

+ r2iz

)�dV �R

rixriy�dV �R

rixriz�dV

�R

(r2ix

+ r2iz

)�dV �R

riyriz�dV

� �R

(r2ix

+ r2iy

)�dV

375

=

264 Iìxx �Iìxy �Iìxz� Iìyy �Iìyz� � Iìzz

375


� Energia cinetica del braccio i

Tì =1

2mì

_pTì

_pì +1

2!

T

iRiI

i

ìR

T

i!i

=1

2mì

_qTJ(ì)T

PJ

(ì)

P_q +

1

2_qTJ

(ì)T

ORiI

i

ìR

T

iJ

(ì)

O_q

? velocita lineare

_pì = |(ì)

P1 _q1 + : : :+ |(ì)

Pi_qi = J

(ì)

P_q

|(ì)

Pj=

�zj�1 per un giunto prismatico

zj�1 � (pì � pj�1) per un giunto rotoidale

? velocita angolare

!i = |(ì)

O1 _q1 + : : :+ |(ì)

Oi_qi = J

(ì)

O_q

|(ì)

Oj=

�0 per un giunto prismatico

zj�1 per un giunto rotoidale


� Motore i (elettrico rotante)

? trasmissione rigida

!mi= !i�1 + kri _qizmi


� Energia cinetica del rotore i

Tmi=

1

2mmi

_pTmi

_pmi+

1

2!

T

miImi

!mi

=1

2mmi

_qTJ(mi)T

PJ

(mi)

P_q +

1

2_qTJ

(mi)T

ORmi

Imi

miR

T

miJ

(mi)

O_q

? velocita lineare

J(mi)

P=

�|

(mi)

P1 : : : |(mi)

P;i�1 0 : : : 0�

|(mi)

Pj=

�zj�1 per un giunto prismatico

zj�1 � (pmi� pj�1) per un giunto rotoidale

? velocita angolare

J(mi)

O=

�|

(mi)

O1 : : : |(mi)

O;i�1 |(mi)

Oi0 : : : 0

�

|(mi)

Oj=

(|

(`i)

Ojj = 1; : : : ; i� 1

krizmij = i


� Energia cinetica totale

T =1

2

nXi=1

nXj=1

bij(q) _qi _qj =1

2_qTB(q) _q

� Matrice di inerzia

B(q) =

nXi=1

�mì

J(ì)T

PJ

(ì)

P+ J

(ì)T

ORiI

i

ìR

T

iJ

(ì)

O

+mmiJ

(mi)T

PJ

(mi)

P+ J

(mi)T

ORmi

Imi

miR

T

miJ

(mi)

O

�

? simmetrica

? definita positiva

? dipendente dalla configurazione(in generale)


Energia potenziale

U =

nXi=1

(Uì + Umi)

� Braccio i

Uì = �Z

Vì

gT

0 p�i�dV = �mì

gT

0 pì

� Rotore iUmi

= �mmig

T

0 pmi

� Energia potenziale totale

U = �nXi=1

(mìg

T

0 pì +mmig

T

0 pmi)


Equazioni del moto

L(q; _q) = T (q; _q)� U(q)

=1

2

nXi=1

nXj=1

bij(q) _qi _qj+

nXi=1

�m`i

gT

0 p`i(q)+mmig

T

0 pmi(q)

�

� Equazioni di Lagrange

d

dt

�@L@ _qi

�=

d

dt

�@T@ _qi

�=

nXj=1

bij(q)�qj +

nXj=1

dbij(q)

dt_qj

=

nXj=1

bij(q)�qj +

nXj=1

nXk=1

@bij(q)

@qk_qk _qj

@T@qi

=1

2

nXj=1

nXk=1

@bjk(q)

@qi_qk _qj

@U@qi

= �nXj=1

�m`j

gT

0

@p`j

@qi+mmj

gT

0

@pmj

@qi

�

= �nXj=1

�m`j

gT

0 |(`j)

Pi(q) +mmj

gT

0 |(mj)

Pi(q)

�= gi(q)


� Equazioni del moto

nXj=1

bij(q)�qj+

nXj=1

nXk=1

hijk(q) _qk _qj+gi(q) = �i i = 1; : : : ; n

ove

hijk =@bij

@qk� 1

2

@bjk

@qi

? termini in accelerazione:

il coefficiente bii rappresenta il momento di inerzia vistoall’asse del giunto i, nella configurazione corrente del ma-nipolatore, quando gli altri giunti sono bloccati

il coefficiente bij tiene conto dell’effetto dell’accelerazionedel giunto j sul giunto i

? termini quadratici in velocita:

il termine hijj _q2j rappresenta l’effetto centrifugoindotto algiunto i dalla velocita del giunto j; si noti che hiii = 0,poiche @bii=@qi = 0

il termine hijk _qj _qk rappresenta l’effetto di Coriolis indottoal giunto i dalle velocita dei giunti j e k

? termini dipendenti solo dalla configurazione:

il termine gi rappresenta la coppia generata all’asse delgiunto i nella configurazione corrente del manipolatore pereffetto della gravita


� Forze non conservative

? coppie di attuazione �

? coppie di attrito viscoso �Fv _q

? coppie di attrito statico �fs(q; _q) � Fssgn ( _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 + Fv _q + fs( _q) + g(q) = � � JT (q)h

nXj=1

cij _qj =

nXj=1

nXk=1

hijk _qk _qj


PROPRIETA NOTEVOLI DEL MODELLODINAMICO

Anti-simmetria della matrice _B � 2C

nXj=1

cij _qj =

nXj=1

nXk=1

hijk _qk _qj =

nXj=1

nXk=1

�@bij

@qk� 1

2

@bjk

@qi

�_qk _qj

=1

2

nXj=1

nXk=1

@bij

@qk_qk _qj +

1

2

nXj=1

nXk=1

�@bik

@qj� @bjk

@qi

�_qk _qj

� Elementi di C

cij =

nXk=1

cijk _qk

? simboli di Christoffel del primo tipo

cijk =1

2

�@bij

@qk+

@bik

@qj� @bjk

@qi

�

� Proprieta notevole

N(q; _q) = _B(q)� 2C(q; _q) = �NT (q; _q)


� Elementi di C

cij =1

2

nXk=1

@bij

@qk_qk +

1

2

nXk=1

�@bik

@qj� @bjk

@qi

�_qk

=1

2_bij +

1

2

nXk=1

�@bik

@qj� @bjk

@qi

�_qk

� Elementi di N

nij = _bij � 2cij =

nXk=1

�@bjk

@qi� @bik

@qj

�_qk = �nji

+

wTN(q; _q)w = 0 8w

? se w = _q:

_qTN(q; _q) _q = 0 8C


� Principio di conservazione dell’energia (Hamilton)

1

2

d

dt

�_qTB(q) _q

�= _qT

�� Fv _q � fs( _q)� g(q)� JT (q)h

�

1

2

d

dt

�_qTB(q) _q

�=

1

2_qT _B(q) _q + _qTB(q)�q

=1

2_qT

�_B(q)� 2C(q; _q)

�_q

+ _qT�� Fv _q � fs( _q)� g(q)� JT (q)h

�


Linearita nei parametri dinamici

� Insieme braccio i e rotore i+ 1

Ti = Tì + Tmi+1

Tì =1

2mì _p

T

ì_pì +

1

2!

T

iIì!i

Tmi+1=

1

2mmi+1

_pTmi+1

_pmi+1+

1

2!

T

mi+1Imi+1

!mi+1

? con riferimento al baricentro pCi

dell’ insieme braccio–rotore

_pì = _pCi+ !i � rCi ; i

_pmi+1= _pCi

+ !i � rCi;mi+1

rCi ; i= pì � pCi

rCi;mi+1= pmi+1

� pCi


� Braccio

Tì =1

2mì _p

T

Ci_pCi

+ _pTCi

S(!i)mìrCi ; i

+1

2mì

!T

iS

T (rCi ; i)S(rCi ; i

)!i +1

2!

T

iIì!i

=1

2mì _p

T

Ci_pCi

+ _pTCi

S(!i)mìrCi ; i

+1

2!

T

i�Iì!i

? per il teorema di Steiner

�Iì = Iì +mìS

T (rCi ; i)S(rCi ; i

)

� Rotore

Tmi+1=

1

2mmi+1

_pTCi

_pCi+ _pT

CiS(!i)mmi+1

rCi;mi+1+

1

2!

T

i�Imi+1

!i

+ kr;i+1 _qi+1zT

mi+1Imi+1

!i +1

2k

2r;i+1 _q

2i+1z

T

mi+1Imi+1

zmi+1

? per il teorema di Steiner

�Imi+1= Imi+1

+mmi+1S

T (rCi;mi+1)S(rCi;mi+1

)


� Energia cinetica

Ti =1

2mi _p

T

Ci_pCi

+1

2!

T

i�Ii!i + kr;i+1 _qi+1z

T

mi+1Imi+1

!i

+1

2k

2r;i+1 _q

2i+1z

T

mi+1Imi+1

zmi+1

? rotore con distribuzione di massa simmetrica intorno al suoasse di rotazione

Imi

mi=

24 Imixx0 0

0 Imixx0

0 0 Imizz

35+

Imi+1zmi+1

= Rmi+1I

mi+1

mi+1R

T

mi+1zmi+1

= Imi+1zmi+1


� Quantita riferite alle terne solidali ai bracci

Ti =1

2mi _p

iT

Ci_piCi

+1

2!

iT

i�Iii!

i

i+ kr;i+1 _qi+1Imi+1

ziT

mi+1!

i

i

+1

2k

2r;i+1 _q

2i+1Imi+1

=1

2mi _p

iT

i_pii

+ _piTi

S(!ii)mir

i

i;Ci+

1

2!

iT

iI

i

i!

i

i

+ kr;i+1 _qi+1Imi+1z

iT

mi+1!

i

i+

1

2k

2r;i+1 _q

2i+1Imi+1

? per il teorema di SteinerI

i

i= �Ii

i+miS

T (rii;Ci

)S(rii;Ci

)


� Energia cinetica linearerispetto a:

? massa mi

? momento primo di inerzia

miri

i;Ci=

24mi Cix

mi Ciy

mi Ciz

35? tensore di inerzia

Ii

i=

264�Iixx +mi(`

2Ciy

+ `2Ciz

) ��Iixy �mi Cix Ciy

� �Iiyy +mi(`2Ciz

+ `2Cix

)

� ��Iixz �mi Cix Ciz

��Iiyz �mi Ciy Ciz

�Iizz +mi(`2Cix

+ `2Ciy

)

375

=

264 Iixx �Iixy �Iixz� Iiyy Iiyz

� � Iizz

375


� Energia potenziale

Ui = �migiT

0 pi

Ci

= �giT0 (mipi

i+mir

i

i;Ci)

linearerispetto a:

? massa mi

? momento primo di inerzia

miri

i;Ci=

24mi Cix

mi Ciy

mi Ciz

35


� Lagrangiana

L =

nXi=1

(�TT i � �TUi)�i

? vettore (11� 1) di parametri dinamici

�i = [mi mi Cixmi Ciy

mi Ciz

Iixx Iixy Iixz Iiyy Iiyz Iizz Imi]T

� Le operazioni richieste dalle equazioni di Lagrange non alte-rano la linearita

�i =

nXj=1

yT

ij�j

ove

yij =d

dt

@�T j

@ _qi� @�T j

@qi+

@�Uj

@qi


� Proprieta notevole

2664�1

�2

...

�n

3775 =

26664y

T

11 yT

12 : : : yT

1n

0T yT

22 : : : yT

2n

......

. . ....

0T 0T : : : yT

nn

3777526664

�1

�2

...

�n

37775

? in forma compatta:

� = Y (q; _q; �q)�


Manipolatore cartesiano a due bracci

� Motori

? masse mmi

? momenti di inerzia Imi

? pmi= pi�1 zmi

= zi�1


? Jacobiani

J(`1)

P=

24 0 0

0 0

1 0

35 J(`2)

P=

24 0 1

0 0

1 0

35

J(m1)

P=

24 0 0

0 0

0 0

35 J(m2)

P=

24 0 0

0 0

1 0

35J

(m1)

O=

24 0 0

0 0

kr1 0

35 J(m2)

O=

24 0 kr2

0 0

0 0

35� Matrice di inerzia

B =

�m`1

+mm2+ k2

r1Im1+m`2

0

0 m`2+ k2

r2Im2

�

� Forze gravitazionali

g1 = (m`1+mm2

+m`2)g g2 = 0


(m`1+mm2

+ k2r1Im1

+m`2) �d1 + (m`1

+mm2+m`2

)g = f1

(m`2+ k

2r2Im2

) �d2 = f2



� Motori? masse mmi

? momenti di inerzia Imi

? pmi= pi�1 zmi

= zi�1


? Jacobiani

J(`1)

P=

24�`1s1 0

`1c1 0

0 0

35 J(`2)

P=

24�a1s1 � `2s12 �`2s12a1c1 + `2c12 `2c12

0 0

35

J(`1)

O=

24 0 0

0 0

1 0

35 J(`2)

O=

24 0 0

0 0

1 1

35

J(m1)

P=

24 0 0

0 0

0 0

35 J(m2)

P=

24�a1s1 0

a1c1 0

0 0

35

J(m1)

O=

24 0 0

0 0

kr1 0

35 J(m2)

O=

24 0 0

0 0

1 kr2

35


� Matrice di inerzia

B(q) =

�b11(#2) b12(#2)

b21(#2) b22

�b11 = I`1 +m`1

`21 + k

2r1Im1

+ I`2 +m`2(a21 + `

22 + 2a1`2c2)

+ Im2+mm2

a21

b12 = b21 = I`2 +m`2(`22 + a1`2c2) + kr2Im2

b22 = I`2 +m`2`

22 + k

2r2Im2

� Forze centrifughe e di Coriolisc111 =

1

2

@b11

@q1= 0

c112 = c121 =1

2

@b11

@q2= �m`2

a1`2s2 = h

c122 =@b12

@q2� 1

2

@b22

@q1= h

c211 =@b21

@q1� 1

2

@b11

@q2= �h

c212 = c221 =1

2

@b22

@q1= 0

c222 =1

2

@b22

@q2= 0


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

�


g1 = (m`1`1 +mm2

a1 +m`2a1)gc1 +m`2

`2gc12

g2 = m`2`2gc12



�I`1 +m`1

`21 + k

2r1Im1

+ I`2 +m`2(a21 + `

22 + 2a1`2c2)

+Im2+mm2

a21

��#1

+�I`2 +m`2

(`22 + a1`2c2) + kr2Im2

��#2

� 2m`2a1`2s2

_#1 _#2 �m`2a1`2s2

_#22

+ (m`1`1 +mm2

a1 +m`2a1)gc1 +m`2

`2gc12 = �1�I`2 +m`2

`22 + k

2r2Im2

��#2 +

�I`2 +m`2

(`22 + a1`2c2) + kr2Im2

��#1

+m`2a1`2s2

_#21 +m`2`2gc12 = �2


� Parametrizzazione del modello

� = [�1 �2 �3 �4 �5 �6 �7 �8 ]T

�1 = m1 = m`1+mm2

�2 = m1 C1= m`1

(`1 � a1)

�3 = I1 = I`1 +m`1(`1 � a1)

2 + Im2

�4 = Im1

�5 = m2 = m`2

�6 = m`2 C2= m`2

(`2 � a2)

�7 = I2 = I`2 +m`2(`2 � a2)

2

�8 = Im2


Y =

�y11 y12 y13 y14 y15 y16 y17 y18

y21 y22 y23 y24 y25 y26 y27 y28

�y11 = a

21

�#1 + a1gc1

y12 = 2a1 �#1 + gc1

y13 = �#1

y14 = k2r1

�#1

y15 = (a21 + 2a1a2c2 + a22)

�#1 + (a1a2c2 + a22)

�#2

� 2a1a2s2 _#1 _#2 � a1a2s2_#22 + a1gc1 + a2gc12

y16 = (2a1c2 + 2a2)�#1 + (a1c2 + 2a2)�#2 � 2a1s2 _#1 _#2

� a1s2_#22 + gc12

y17 = �#1 + �#2

y18 = �#1 + kr2�#2

y21 = 0

y22 = 0

y23 = 0

y24 = 0

y25 = (a1a2c2 + a22)

�#1 + a22

�#2 + a1a2s2_#21 + a2gc12

y26 = (a1c2 + 2a2)�#1 + 2a2 �#2 + a1s2_#21 + gc12

y27 = �#1 + �#2

y28 = kr2�#1 + k

2r2

�#2.


Esempio


? a1 = a2 = 1m `1 = `2 = 0:5m

m`1= m`2

= 50 kg I`1 = I`2 = 10 kg�m2

? kr1 = kr2 = 100

mm1= mm2

= 5 kg Im1= Im2

= 0:01 kg �m2


0 0.1 0.2 0.3 0.4 0.5−2

−1

0

1

2

3

4

[s]

[rad

]pos giunto 1

0 0.1 0.2 0.3 0.4 0.5−2

−1

0

1

2

3

4

[s]

[rad

]

pos giunto 2

0 0.1 0.2 0.3 0.4 0.5

0

2

4

6

[s]

[rad

/s]

vel giunto 1

0 0.1 0.2 0.3 0.4 0.5

0

2

4

6

[s]

[rad

/s]

vel giunto 2

0 0.1 0.2 0.3 0.4 0.5−30

−20

−10

0

10

20

30

[s]

[rad

/s^2

]

acc giunto 1

0 0.1 0.2 0.3 0.4 0.5−30

−20

−10

0

10

20

30

[s]

[rad

/s^2

]

acc giunto 2

0 0.1 0.2 0.3 0.4 0.5−6000

−4000

−2000

0

2000

4000

6000

[s]

[Nm

]

coppia giunto 1

0 0.1 0.2 0.3 0.4 0.5−6000

−4000

−2000

0

2000

4000

6000

[s]

[Nm

]

coppia giunto 2


0 0.1 0.2 0.3 0.4 0.5−5000

0

5000

[s]

[Nm

]inerz_11

0 0.1 0.2 0.3 0.4 0.5−5000

0

5000

[s]

[Nm

]

inerz_22

0 0.1 0.2 0.3 0.4 0.5−500

−400

−300

−200

−100

0

100

[s]

[Nm

]

inerz_12

0 0.1 0.2 0.3 0.4 0.5−500

−400

−300

−200

−100

0

100

[s]

[Nm

]

inerz_21

0 0.1 0.2 0.3 0.4 0.5

−500

0

500

1000

[s]

[Nm

]

centrif_2 + coriol_12

12

2

0 0.1 0.2 0.3 0.4 0.5

−500

0

500

1000

[s]

[Nm

]

centrif_1

0 0.1 0.2 0.3 0.4 0.5

−200

0

200

400

600

800

[s]

[Nm

]

grav_1

0 0.1 0.2 0.3 0.4 0.5

−200

0

200

400

600

800

[s]

[Nm

]

grav_2


0 0.2 0.4 0.6−2

−1

0

1

2

3

4

[s]

[rad

]pos giunto 1

0 0.2 0.4 0.6−2

−1

0

1

2

3

4

[s]

[rad

]

pos giunto 2

0 0.2 0.4 0.6−6

−4

−2

0

2

4

6

[s]

[rad

/s]

vel giunto 1

0 0.2 0.4 0.6−6

−4

−2

0

2

4

6

[s]

[rad

/s]

vel giunto 2

0 0.2 0.4 0.6−40

−20

0

20

40

[s]

[rad

/s^2

]

acc giunto 1

0 0.2 0.4 0.6−40

−20

0

20

40

[s]

[rad

/s^2

]

acc giunto 2

0 0.2 0.4 0.6

−5000

0

5000

[s]

[Nm

]

coppia giunto 1

0 0.2 0.4 0.6

−5000

0

5000

[s]

[Nm

]

coppia giunto 2


0 0.2 0.4 0.6

−6000

−4000

−2000

0

2000

4000

6000

[s]

[Nm

]inerz_11

0 0.2 0.4 0.6

−6000

−4000

−2000

0

2000

4000

6000

[s]

[Nm

]

inerz_22

0 0.2 0.4 0.6

−500

0

500

1000

1500

[s]

[Nm

]

inerz_12

0 0.2 0.4 0.6

−500

0

500

1000

1500

[s]

[Nm

]

inerz_21

0 0.2 0.4 0.6

−500

0

500

1000

[s]

[Nm

]


12

2

0 0.2 0.4 0.6

−500

0

500

1000

[s]

[Nm

]

centrif_1

0 0.2 0.4 0.6

0

200

400

600

800

1000

[s]

[Nm

]

grav_1

0 0.2 0.4 0.6

0

200

400

600

800

1000

[s]

[Nm

]

grav_2


0 0.1 0.2 0.3 0.4 0.50

0.5

1

1.5

2

[s]

[m]

pos organo terminale

0 0.1 0.2 0.3 0.4 0.50

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


0 0.1 0.2 0.3 0.4 0.5−2

−1

0

1

2

3

4

[s]

[rad

]pos giunto 1

0 0.1 0.2 0.3 0.4 0.5−2

−1

0

1

2

3

4

[s]

[rad

]

pos giunto 2

0 0.1 0.2 0.3 0.4 0.5−8

−6

−4

−2

0

2

4

[s]

[rad

/s]

vel giunto 1

0 0.1 0.2 0.3 0.4 0.5−8

−6

−4

−2

0

2

4

[s]

[rad

/s]

vel giunto 2

0 0.1 0.2 0.3 0.4 0.5

−40

−20

0

20

40

60

80

[s]

[rad

/s^2

]

acc giunto 1

0 0.1 0.2 0.3 0.4 0.5

−40

−20

0

20

40

60

80

[s]

[rad

/s^2

]

acc giunto 2

0 0.1 0.2 0.3 0.4 0.5

−5000

0

5000

[s]

[Nm

]

coppia giunto 1

0 0.1 0.2 0.3 0.4 0.5

−5000

0

5000

[s]

[Nm

]

coppia giunto 2


0 0.1 0.2 0.3 0.4 0.5−1

−0.5

0

0.5

1x 10

4

[s]

[Nm

]inerz_11

0 0.1 0.2 0.3 0.4 0.5−1

−0.5

0

0.5

1x 10

4

[s]

[Nm

]

inerz_22

0 0.1 0.2 0.3 0.4 0.5

−1000

0

1000

2000

3000

[s]

[Nm

]

inerz_12

0 0.1 0.2 0.3 0.4 0.5

−1000

0

1000

2000

3000

[s]

[Nm

]

inerz_21

0 0.1 0.2 0.3 0.4 0.5

−1000

−500

0

500

1000

[s]

[Nm

]


12

2

0 0.1 0.2 0.3 0.4 0.5

−1000

−500

0

500

1000

[s]

[Nm

]

centrif_1

0 0.1 0.2 0.3 0.4 0.5

0

200

400

600

800

1000

[s]

[Nm

]

grav_1

0 0.1 0.2 0.3 0.4 0.5

0

200

400

600

800

1000

[s]

[Nm

]

grav_2


Manipolatore a parallelogramma


? Jacobiani

J(`10 )

P=

24�`10s10 0 0

`10c10 0 0

0 0 0

35 J(`20)

P=

24�a10s10�`2s1020 �`20s1020 0

a10c10+`20c1020 `2c1020 0

0 0 0

35

J(`30 )

P=

24�a10s10�a20s1020�`30s102030 �a20s1020�`30s102030 �`30s102030

a10c10+a20c1020+`30c102030 a20c1020+`30c102030 `30c102030

0 0 0

35

J(`100)

P=

24�`100s100

`100c100

0

35

J(`10 )

O=

24 0 0 0

0 0 0

1 0 0

35 J(`20 )

O=

24 0 0 0

0 0 0

1 1 0

35 J(`30 )

O=

24 0 0 0

0 0 0

1 1 1

35

J(`100)

O=

24 0

0

1

35 .


� Matrici di inerzia per i manipolatori virtuali

B0(q0) =

24 b1010(#20 ; #30) b1020(#20 ; #30) b1030(#20 ; #30)

b2010(#20 ; #30) b2020(#30) b2030(#30)

b3010(#20 ; #30) b3020(#30) b3030

35b1010 = I`10 +m`10

`210 + I`20 +m`20

(a210 + `220 + 2a10 20c20) + I`30

+m`30(a210 + a

220 + `

230 + 2a10a20c20 + 2a10 30c2030 + 2a20 30c30)

b1020 = b2010 = I`20 +m`20(`220 + a10 20c20) + I`30

+m`30(a220 + `

230 + a10a20c20 + a10 30c2030 + 2a20 30c30)

b1030 = b31 = I`30 +m`30(`230 + a10 30c2030 + a20 30c30)

b2020 = I`20 +m`20`

220 + I`30 +m`30

(a220 + `230 + 2a20 30c30)

b2030 = I`30 +m`30(`230 + a20 30c30)

b3030 = I`30 +m`30`

230

b1010 = I`100 +m`100`

2100 .

�i0 =

30Xj0=10

bi0j0�#j0 �100 = b1010

�#100


� Manipolatore a catena chiusa

�a = Ba�qa

Ba =

�ba11 ba12

ba21 ba22

�ba11 = I`10 +m`10

`210 +m`20

a210

+ I`30 +m`30`

230 +m`30

a210 � 2a10m`30

`30

ba12 = ba21 =�a10m`20

`20 + a100m`30(a10 � `30)

�cos (#100 � #10)

ba22 = I`10 +m`10`

210 + I`20 +m`20

`220 +m`30

a2100 .

? matrice costante e diagonale

m`30�30

m`20`20

=a10

a100

ba11 = I`10 +m`10`

210 +m`20

a210

�1 +

`20�30

a10a100

�+ I`30

ba22 = I`10 +m`10`

210 + I`20 +m`20

`220

�1 +

a10a100

`20�30

�



g10 = (m`10`10 +m`20

a10 +m`30a10)gc10 + (m`20

`20 +m`30a20)gc1020

+m`30`30gc10203

g20 = (m`20`20 +m`30

a20)gc1020 +m`30`30gc10203

g30 = m`30`30gc10203

g100 = m`100`100gc100

ga =

�(m`10

`10 +m`20a10 �m`30

�30)gc10

(m`100`100 +m`20

`20 +m`30a100)gc100

�


IDENTIFICAZIONE DEI PARAMETRIDINAMICI

� Impiego del modello dinamico per simulazione e controllo

� Stime approssimate dei parametri dinamici

? semplificazioni di modellazione geometrica

? effetti dinamici complessi (attriti, etc.)

� Tecniche di identificazione

? proprieta di linearita

� = Y (q; _q; �q)�

� Misure

? q e _q dirette (encoder e tachimetri)

? �q indirette (filtro ricostruttore)

? � dirette (sensore di coppia) o indirette (misure di forza ocorrente)


� N insiemi di misure

�� =

264 � (t1)...

� (tN )

375 =

264 Y (t1)

...

Y (tN )

375� = �Y �

� Soluzione (a minimi quadrati)

� = ( �Y T �Y )�1 �Y T ��

? Y triangolare =) procedimento iterativo

? se %(Y ) < p =) Y?

� Parametri

? identificabili

? non identificabili

? identificabili in combinazione lineare

� Carico

? modifica parametri dell’ultimo braccio

� Traiettorie

? di tipo polinomiale (sufficientemente ricche)

? attenzione alle dinamiche non modellate


FORMULAZIONE DI NEWTON-EULERO

? bilancio di forze e momenti agenti sul generico braccio

� Equazione di Newton(moto traslazionaledel baricentro)

fi � fi+1 +mig0 = mi�pCi

� Equazione di Eulero(moto rotazionale)

�i + fi � ri�1;Ci� �i+1 � fi+1 � ri;Ci

=d

dt(�Ii!i + kr;i+1 _qi+1Imi+1

zmi+1)


d

dt(�Ii!i) = _Ri

�IiiR

T

i!i +Ri

�Iii

_RT

i!i +Ri

�IiiR

T

i_!i

= S(!i)Ri�IiiR

T

i!i +Ri

�IiiR

T

iS

T (!i)!i +Ri�IiiR

T

i_!i

= �Ii _!i + !i � (�Ii!i)

d

dt( _qi+1Imi+1

zmi+1) = �qi+1Imi+1

zmi+1+ _qi+1Imi+1

!i�zmi+1

+

� Equazione di Eulero

�i + fi � ri�1;Ci� �i+1 � fi+1 � ri;Ci

= �Ii _!i + !i � (�Ii!i)

+ kr;i+1�qi+1Imi+1zmi+1

+ kr;i+1 _qi+1Imi+1!i � zmi+1

� Forza generalizzata al giunto

�i =

(f

T

izi�1 + kriImi _!

T

mizmi

per un giunto prismatico

�T

izi�1 + kriImi _!

T

mizmi

per un giunto rotoidale


Accelerazioni di un braccio

!i =

�!i�1 prismatico

!i�1 + _#izi�1 rotoidale

_pi =

(_pi�1 + _dizi�1 + !i � ri�1;i prismatico

_pi�1 + !i � ri�1;i rotoidale

_!i =

�_!i�1 prismatico_!i�1 + �#izi�1 + _#i!i�1 � zi�1 rotoidale

�pi =

8>>><>>>:�pi�1 + �dizi�1 + 2 _di!i � zi�1 prismatico

+ _!i � ri�1;i + !i � (!i � ri�1;i)

�pi�1 + _!i � ri�1;i+!i � (!i � ri�1;i) rotoidale

�pCi= �pi + _!i � ri;Ci

+ !i � (!i � ri;Ci)

_!mi= _!i�1 + kri�qizmi + kri _qi!i�1 � zmi


Algoritmo ricorsivo

� Ricorsione in avantidi velocita e accelerazioni

? condizioni iniziali: !0, �p0 � g0, _!0

? calcolo di !i, _!i, �pi, �pCi

, _!mi

� Ricorsione all’indietro di forze e momenti

? condizioni terminali: h = [fTn+1 �

T

n+1 ]T

? calcolo di

fi = fi+1 +mi�pCi

�i = �fi � (ri�1;i + ri;Ci) + �i+1 + fi+1 � ri;Ci

+ �Ii _!i + !i � (�Ii!i)

+ kr;i+1�qi+1Imi+1zmi+1

+ kr;i+1 _qi+1Imi+1!i � zmi+1

�i =

(f

T

izi�1 + kriImi _!

T

mizmi

+ Fvi_di + Fsi sgn ( _di) prismatico

�T

izi�1 + kriImi _!

T

mizmi

+ Fvi_#i + Fsi sgn ( _#i) rotoidale


� Conviene riferire tutti i vettori alla terna corrente sul braccio i

? le quantita �Iii

, rii;Ci

, zi�1mi

sono costanti

!i

i=

(R

i�1i

T!

i�1i�1 prismatico

Ri�1i

T (!i�1i�1 +

_#iz0) rotoidale(4.106)

_!ii

=

(R

i�1i

T _!i�1i�1 prismatico

Ri�1i

T ( _!i�1i�1 +

�#iz0 + _#i!i�1i�1 � z0) rotoidale

(4.107)

�pii

=

8>>>><>>>>:R

i�1i

T (�pi�1i�1 +

�diz0) + 2 _di!i

i�Ri�1

i

Tz0

+ _!ii� ri

i�1;i + !i

i� (!i

i� ri

i�1;i) prismaticoR

i�1i

T �pi�1i�1 + _!i

i� ri

i�1;i

+!ii� (!i

i� ri

i�1;i) rotoidale

(4.108)

�piCi

= �pii

+ _!ii� ri

i;Ci+ !

i

i� (!i

i� ri

i;Ci) (4.109)

_!i�1mi

= _!i�1i�1 + kri�qiz

i�1mi

+ kri _qi!i�1i�1 � zi�1mi

(4.110)

fi

i=Ri

i+1fi+1i+1 +mi�p

i

Ci

(4.111)

�i

i=�f i

i� (ri

i�1;i+ri

i;Ci) +R

i

i+1�i+1i+1 (4.112)

+Ri

i+1fi+1i+1 � rii;Ci

+ �Iii

_!ii

+ !i

i� (�Ii

i!

i

i)

+ kr;i+1�qi+1Imi+1z

i

mi+1+ kr;i+1 _qi+1Imi+1

!i

i� zi

mi+1

�i =

8>>><>>>:f

i

i

TR

i�1i

Tz0 + kriImi

_!i�1mi

Tz

i�1mi

+Fvi _di + Fsi sgn ( _di) prismatico

�i

i

TR

i�1i

Tz0 + kriImi

_!i�1mi

Tz

i�1mi

+Fvi _#i + Fsi sgn ( _#i) rotoidale

(4.113)


� Struttura computazionale



� Condizioni iniziali

�p00 � g00 =

24 0

g

0

35 !00 = _!0

0 = 0

� Condizioni terminali

f33 = 0 �

33 = 0

� Quantita riferite alla terna correnter

11;C1

=

24 C1

0

0

35 r10;1 =

24 a100

35

r22;C2

=

24 C2

0

0

35 r21;2 =

24 a200

35

z1m1

= z10 = z

1m2

= z2m2

= z21 =

24 0

0

1

35? matrici di rotazione

Ri�1i

=

24 ci �si 0

si ci 0

0 0 1

35 i = 1; 2 R23 = I


� Ricorsione in avanti: braccio 1

!11 =

264 0

0

_#1

375

_!11 =

264 0

0

�#1

375

�p11 =

264�a1_#21 + gs1

a1�#1 + gc1

0

375�p1

C1=

264�( C1+ a1) _#

21 + gs1

( C1+ a1)�#1 + gc1

0

375

_!0m1

=

264 0

0

kr1�#1

375


� Ricorsione in avanti: braccio 2

!22 =

264 0

0

_#1 + _#2

375

_!22 =

264 0

0

�#1 + �#2

375

�p22 =

264 a1s2�#1 � a1c2

_#21 � a2( _#1 + _#2)2 + gs12

a1c2�#1 + a2(�#1 + �#2) + a1s2

_#21 + gc12

0

375

�p2C2

=

264 a1s2�#1 � a1c2

_#21 � ( C2+ a2)( _#1 + _#2)

2 + gs12

a1c2�#1 + ( C2

+ a2)(�#1 + �#2) + a1s2_#21 + gc12

0

375

_!1m2

=

264 0

0

�#1 + kr2�#2

375


� Ricorsione all’ indietro: braccio 2

f22 = m2�p

2C2

�22 =

2666664��

�I2zz(�#1 + �#2) +m2( C2+ a2)

2(�#1 + �#2)

+m2a1( C2+ a2)c2 �#1

+m2a1( C2+ a2)s2 _#

21 +m2( C2

+ a2)gc12

3777775�2 =

��I2zz +m2

�( C2

+ a2)2 + a1( C2

+ a2)c2�

+ kr2Im2

��#1

+�

�I2zz +m2( C2+ a2)

2 + k2r2Im2

��#2

+m2a1( C2+ a2)s2 _#

21 +m2( C2

+ a2)gc12


� Ricorsione all’ indietro: braccio 1

f11 =

2666664�m2( C2

+ a2)s2(�#1 + �#2)�m1( C1+ a1) _#

21 �m2a1

_#21

�m2( C2+ a2)c2( _#1 + _#2)

2 + (m1 +m2)gs1

m1( C1+ a1)�#1 +m2a1

�#1 +m2( C2+ a2)c2(�#1 + �#2)

�m2( C2+ a2)s2( _#1 + _#2)

2 + (m1 +m2)gc1

0

3777775

�11 =

266666666664

��

�I1zz �#1 +m2a21

�#1 +m1( C1+ a1)

2 �#1 +m2a1( C2+ a2)c2 �#1

+�I2zz(�#1 + �#2) +m2a1( C2+ a2)c2(�#1 + �#2)

+m2( C2+ a2)

2(�#1 + �#2) + kr2Im2�#2

+m2a1( C2+ a2)s2 _#

21 �m2a1( C2

+ a2)s2( _#1 + _#2)2

+m1( C1+ a1)gc1 +m2a1gc1 +m2( C2

+ a2)gc12

377777777775�1 =

��I1zz +m1( C1

+ a1)2 + k

2r1Im1

+ �I2zz

+m2

�a

21 + ( C2

+ a2)2 + 2a1( C2

+ a2)c2��

�#1

+�

�I2zz +m2

�( C2

+ a2)2 + a1( C2

+ a2)c2�

+ kr2Im2

��#2

� 2m2a1( C2+ a2)s2 _#1 _#2 �m2a1( C2

+ a2)s2 _#22

+�m1( C1

+ a1) +m2a1

�gc1 +m2( C2

+ a2)gc12


� Equivalenza col modello di Lagrange

? parametri di bracci e rotori

m1 = m`1+mm2

m1 C1= m`1

(`1 � a1)

�I1zz +m1`2C1

= I1 = I`1 +m`1(`1 � a1)

2 + Im2

m2 = m`2

m2 C2= m`2

(`2 � a2)

�I2zz +m2`2C2

= I2 = I`2 +m`2(`2 � a2)

2


DINAMICA DIRETTA E DINAMICA INVERSA

� Formulazione di Lagrange:

? e sistematicae di facile comprensione

? fornisce le equazioni del moto in una forma analitica com-pattache evidenzia la matrice di inerzia, la matrice a fattoredelle forze centrifughe e di Coriolis e il vettore delle forzegravitazionali (utile ai fini della sintesi del controllo)

? e efficace se si vogliono portare in conto effetti meccanicipiu complessi quali ad esempio le deformazioni elastichedei bracci

� Formulazione di Newton-Eulero:

? e intrinsecamente un metodo ricorsivoche risulta efficienteda un punto di vista computazionale


� Dinamica diretta

? note q(t0), _q(t0), � (t) (eh(t)), determinare �q(t), _q(t), q(t)

per t > t0

? utile in simulazione

� Dinamica inversa

? note �q(t), _q(t), q(t) (e h(t)), determinare � (t)

? utile per la pianificazione e il controllo

� Per un manipolatore ad n giunti, il numero di operazioni per ilcalcolo della dinamica e:

? O(n2) per la dinamica diretta? O(n) per la dinamica inversa


� Dinamica diretta con Lagrange

�q = B�1(q)(� � � 0)

�0(q; _q) = C(q; _q) _q + Fv _q + fs( _q) + g(q) + J

T (q)h

? note q(tk), _q(tk), � (tk), si calcola �q(tk)

? si integra numericamente con passo �t: _q(tk+1), q(tk+1)

� Dinamica diretta con Newton-Eulero

? note q, _q, si calcola:

�0 come � per �q = 0

colonna bi diB come � per g0 = 0, _q = 0, �qi = 1 e �qj = 0

per j 6= i

? nota � , si calcola �q e si integra numericamente


MODELLO DINAMICO NELLO SPAZIOOPERATIVO

� Ridondanza

? coordinate generalizzate (moti interni?)

� Singolarita

� Modello nello spazio dei giunti

�q = �B�1(q)C(q; _q) _q�B�1(q)g(q)+B�1(q)JT (q)( �h)

� Cinematica differenziale del secondo ordine

�x = JA(q)�q + _JA(q; _q) _q

= �JAB�1C _q � JAB�1

g + _JA _q + JAB�1

JT

A( A � hA)


� Trasformazioni

BA = (JAB�1

JT

A)�1

CA _x = BAJAB�1

C _q �BA_JA _q

gA = BAJAB�1

g

? manipolatore non ridondante

BA = J�T

ABJ

�1A

CA _x = J�T

AC _q �BA

_JA _q

gA = J�T

Ag

� Equazioni del motoBA(x)�x+CA(x; _x) _x+ gA(x) = A � hA


� Dinamica diretta

? note � (t) (e h(t)), determinare �x(t), _x(t), x(t)

� Soluzione

? dinamica diretta ai giunti

? cinematica diretta


? note x(t), _x(t), �x(t) (e h(t)), determinare � (t)

� Soluzione (ridondanza a livello cinematico)? cinematica inversa

? dinamica inversa ai giunti

� Soluzione (ridondanza a livello dinamico)

? modello dinamico nello spazio operativo

? coppie con gestione di moti interni



A � hA = BA(x)�x+CA(x; _x) _x+ gA(x)

= BA(�x� _JA _q) +BAJAB�1

C _q +BAJAB�1

g

= BAJA�q +BAJAB�1

C _q +BAJAB�1

g

? pseudo-inversa destra di JA pesata secondo B

�JA(q) = B�1(q)JT

A(q)BA(q)

+

A � hA = �JTA

(B�q +C _q + g)

= �JTA

(� � JTA

hA)

� = JT

A(q) A +

�I � JT

A(q)�JT

A(q)

��a


ELLISSOIDE DI MANIPOLABILITADINAMICA

� Sfera nello spazio dei giunti

�T� = 1

? modello dinamico con _q = 0, h = 0

B(q)�q + g(q) = �

? accelerazioni ai giunti ( _q = 0)

_v = J(q)�q

+

� = B(q)Jy(q) _v + g(q)

� Ellissoide nello spazio operativo

�B(q)Jy(q) _v + g(q)

�T �B(q)Jy(q) _v + g(q)

�= 1


BJy _v + g = B(Jy _v +B

�1g)

= B(Jy _v +B�1

g + JyJB

�1g � JyJB�1

g)

= B�J

y _v + JyJB

�1g + (I � JyJ)B�1

g�

= BJy( _v + JB

�1g)

+

( _v + JB�1

g)TJyTBTBJ

y( _v + 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 + JB�1

g)TJ�TBTBJ

�1( _v + JB�1

g) = 1


PIANIFICAZIONE DI TRAIETTORIE

� generazione degli ingressi di riferimento per il sistema di con-trollo del moto

Percorso e traiettoria

Traiettorie nello spazio dei giunti

Traiettorie nello spazio operativo

Scalatura dinamica di traiettorie


PERCORSO E TRAIETTORIA

? generazione di traiettorie a curvatura regolare

� Percorso: luogo dei punti dello spazio dei giunti (spazio ope-rativo) che il manipolatore deve descrivere nell’esecuzione delmovimento assegnato (descrizione geometrica)

� Traiettoria: percorso su cui sia specificata la legge oraria dimoto (ad es. velocita e/o accelerazioni)

� Algoritmo di pianificazione della traiettoria

? ingressi

definizione del percorso

vincoli al percorso

vincoli dovuti alla dinamica del manipolatore

? uscita

traiettorie dei giunti (o dell’organo terminale) espresse comesequenza temporale di valori assunti da posizione, velocit ae accelerazione


� Numero ristretto di parametri

? percorso

punti estremi

eventuali punti intermedi

primitive geometriche

? legge di moto

tempo complessivo

velocita e/o accelerazione max

velocita e/o accelerazione in punti assegnati

� Traiettorie nello spazio operativo? descrizione naturale del compito

? vincoli sul percorso

? singolarita

? ridondanza

� Traiettorie nello spazio dei giunti

? inversione cinematica

? azione di controllo


TRAIETTORIE NELLO SPAZIO DEI GIUNTI

� Generazione di una funzione q(t) che interpola i valori asse-gnati per le variabili di giunto nel rispetto dei vincoli imposti.

? traiettorie generate poco onerose dal punto di vista compu-tazionale

? posizioni e velocita (e accelerazioni) dei giunti funzionicontinue del tempo

? effetti indesiderati minimizzati (ad es. traiettorie a curvaturanon regolare)

� Moto punto–punto

? punti estremi e tempo di transizione

� Moto su percorso assegnato

? punti estremi, punti intermedi e tempi di transizione


Moto punto–punto

? generazione di q(t) che realizza il moto da qi a qf in untempo tf

� Polinomio cubico

q(t) = a3t3 + a2t

2 + a1t+ a0

_q(t) = 3a3t2 + 2a2t+ a1

�q(t) = 6a3t+ 2a2

? calcolo dei coefficienti

a0 = qi

a1 = _qi

a3t3f

+ a2t2f

+ a1tf + a0 = qf

3a3t2f

+ 2a2tf + a1 = _qf

� Polinomio quintico (accelerazioni iniziale e finale)

q(t) = a5t5 + a4t

4 + a3t3 + a2t

2 + a1t+ a0


� Esempio

0 0.2 0.4 0.6 0.8 1

0

1

2

3

pos

[s]

[rad

]

0 0.2 0.4 0.6 0.8 1

0

1

2

3

4

5vel

[s]

[rad

/s]

0 0.2 0.4 0.6 0.8 1

−20

−10

0

10

20

acc

[s]

[rad

/s^2

]


� Profilo di velocita trapezoidale

�qctc =qm � qc

tm � tc

qc = qi +1

2�qct

2c

�qct2c� �qctf tc + qf � qi = 0


� �qc assegnata (sgn �qc = sgn (qf � qi))

tc =tf

2� 1

2

st2f

�qc � 4(qf � qi)

�qc

j�qcj �4jqf � qij

t2f

? traiettoria

q(t) =

8><>:qi +

12

�qct2 0 � t � tc

qi + �qctc(t� tc=2) tc < t � tf � tc

qf � 12

�qc(tf � t)2 tf � tc < t � tf


� Esempio

0 0.2 0.4 0.6 0.8 1

0

1

2

3

pos

[s]

[rad

]

0 0.2 0.4 0.6 0.8 1

0

1

2

3

4

5vel

[s]

[rad

/s]

0 0.2 0.4 0.6 0.8 1

−20

−10

0

10

20

acc

[s]

[rad

/s^2

]


� _qc assegnata

jqf � qijtf

< j _qcj �2jqf � qij

tf

tc =qi � qf + _qctf

_qc

�qc =_q2c

qi � qf + _qctf


Moto su percorso assegnato

? opportunita di specificare punti intermedi (sequenza dipunti)

� Assegnati N punti di percorso, trovare una funzione interpo-lante che passi per tali punti

? polinomio di grado N � 1

non e possibile assegnare velocita iniziale e finale

al crescere del grado di un polinomio aumenta il suo carat-tere oscillatorio (traiettorie poco naturali)

l’accuratezza numerica con cui puo essere calcolato un po-linomio peggiora all’aumentare del suo grado

una volta impostato il sistema di equazioni dei vincoli, ri-sulta oneroso risolverlo

i coefficienti del polinomio dipendono da tutti i punti asse-gnati =) se si vuole cambiare uno dei punti di per-corso, si deve ricalcolare il tutto


� Sequenze di polinomi interpolatoridi grado piu basso (cubici)uniti tra di loro nei punti assegnati lungo il percorso

? i valori di _q(t) in corrispondenza dei punti di percorso sonoimposti arbitrariamente

? i valori di _q(t) in corrispondenza dei punti di percorso sonoassegnati in base ad un opportuno criterio

? l’accelerazione �q(t) deve essere continua in corrispondenzadei punti di percorso

� Sequenze di polinomi di grado minore di tre (passaggio inprossimita dei punti di percorso imposti)


� Sequenza di polinomi con vincoli imposti sulle velocita neipunti di percorso

�k(tk) = qk

�k(tk+1) = qk+1

_�k(tk) = _qk

_�k(tk+1) = _qk+1

? continuita delle velocita nei punti intermedi

_�k(tk+1) = _�k+1(tk+1)


� Esempio

0 2 4 6

0

2

4

6

pos

[s]

[rad

]

0 2 4 6

−5

0

5

vel

[s]

[rad

/s]

0 2 4 6

−40

−20

0

20

acc

[s]

[rad

/s^2

]


� Sequenza di polinomi con velocita calcolate nei punti di per-corso

_q1 = 0

_qk =

�0 sgn (vk) 6= sgn (vk+1)12

(vk + vk+1) sgn (vk) = sgn (vk+1)

_qN = 0

vk = (qk � qk�1)=(tk � tk�1)


� Esempio

0 2 4 6

0

2

4

6

pos

[s]

[rad

]

0 2 4 6

−5

0

5

vel

[s]

[rad

/s]

0 2 4 6

−30

−20

−10

0

10

20

30

acc

[s]

[rad

/s^2

]


� Sequenza di polinomi con accelerazione continua nei punti dipercorso (spline)

�k�1(tk) = qk

�k�1(tk) = �k(tk)

_�k�1(tk) = _�k(tk)

��k�1(tk) = ��k(tk)

? 4N � 2 equazioni nei 4(N � 1) coefficienti incogniti (po-linomi di quarto grado per il primo e l’ultimo segmento?)

? 2 punti virtuali (continuita su posizione, velocita e accele-razione) =) N + 1 polinomi cubici


? 4(N � 2) equazioni per gliN � 2 punti di percorso interni:

�k�1(tk) = qk

�k�1(tk) = �k(tk)

_�k�1(tk) = _�k(tk)

��k�1(tk) = ��k(tk)

? 6 equazioni per i punti iniziale e finale

�1(t1) = qi

_�1(t1) = _qi

��1(t1) = �qi

�N+1(tN+2) = qf

_�N+1(tN+2) = _qf

��N+1(tN+2) = �qf

? 6 equazioni per i punti virtuali

�k�1(tk) = �k(tk)

_�k�1(tk) = _�k(tk)

��k�1(tk) = ��k(tk)

+

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

4(N + 1) coefficienti degli N + 1 polinomi cubici


? algoritmo efficiente dal punto di vista computazionale

��k(t) =��k(tk)

�tk(tk+1 � t) +

��k(tk+1)

�tk(t� tk)

�k(t) =��k(tk)

6�tk(tk+1 � t)3 +

��k(tk+1)

6�tk(t� tk)

3

+

�k(tk+1)

�tk� �tk

��k(tk+1)

6

!(t� tk)

+

�k(tk)

�tk� �tk

��k(tk)

6

!(tk+1 � t)

? 4 incognite: �k(tk), �k(tk+1), ��k(tk) e ��k(tk+1)


? N variabili qk per k 6= 2; N + 1 assegnate

? vincoli di continuita su q2 e qN+1

? vincoli di continuita su qk per k = 3; : : : ; N

? _qi e _qf assegnate

? vincoli di continuita su �qk per k = 2; : : : ; N + 1

? �qi e �qf assegnate

+

_�1(t2) = _�2(t2)

...

_�N (tN+1) = _�N+1(tN+1)

? sistema di equazioni lineari

A [ ��2(t2) : : : ��N+1(tN+1) ]T

= b

A =

266664a11 a12 : : : 0 0

a21 a22 : : : 0 0

......

. . ....

...

0 0 : : : aN�1;N�1 aN�1;N

0 0 : : : aN;N�1 aNN

377775


� Esempio

0 1 2 3 4 5

0

2

4

6

pos

[s]

[rad

]

0 1 2 3 4 5

−5

0

5

vel

[s]

[rad

/s]

0 1 2 3 4 5

−30

−20

−10

0

10

20

30

acc

[s]

[rad

/s^2

]


� Sequenza di polinomi parabolico-lineari con passaggio in pros-simita di punti di via

_qk�1;k =qk � qk�1

�tk�1

�qk =_qk;k+1 � _qk�1;k

�t0k


� Esempio

0 1 2 3 4 5

0

2

4

6

pos

[s]

[rad

]

0 1 2 3 4 5−6

−4

−2

0

2

4

vel

[s]

[rad

/s]

0 1 2 3 4 5

−40

−20

0

20

40acc

[s]

[rad

/s^2

]


� Applicazione della legge trapezoidale al problema dell’ interpolazione

0 0.5 1 1.5 2

0

1

2

3

4

5

pos

[s]

[rad

]

0 0.5 1 1.5 2

0

1

2

3

4

vel

[s]

[rad

/s]

0 0.5 1 1.5 2

−20

−10

0

10

20

acc

[s]

[rad

/s^2

]


TRAIETTORIE NELLO SPAZIO OPERATIVO

� Sequenze di punti assegnati

? inversione cinematica

? traiettorie nello spazio dei giunti

? microinterpolazione

� Punti di percorso x(tk)

? componenti xi(tk) raccordate secondo sequenze di poli-nomi interpolatori

� Primitive di percorso? espressione analitica del moto


Primitive di percorso

� Rappresentazione parametrica di una curva nello spazio

p = f(s)

t =dp

ds

n =1 d2pds2

d2p

ds2

b = t� n


� Segmento nello spazio

p(s) = pi +s

kpf � pik(pf � pi)

dp

ds=

1


d2p

ds2= 0


� Circonferenza nello spazio

p0(s) =

24 � cos (s=�)� sin (s=�)

0

35

p(s) = c+Rp0(s)

dp

ds= R

24�sin (s=�)cos (s=�)

0

35d2p

ds2= R

24�cos (s=�)=��sin (s=�)=�0

35


Traiettorie di posizione e di orientamento

� Posizione p = f(s)

? s(t) polinomio interpolatore

_p = _sdp

ds= _st

? segmento

p(s) = pi +s


_p =_s

kpf � pik(pf � pi) = _st

�p =�s

kpf � pik(pf � pi) = �st

? circonferenza

p(s) = c+R

24 � cos (s=�)� sin (s=�)

0

35

_p = R

24� _s sin (s=�)

_s cos (s=�)

0

35

�p = R

24� _s2cos (s=�)=�� s sin (s=�)

� _s2sin (s=�)=�+ �s cos (s=�)

0

35


� Orientamento

? interpolazione delle componenti di n(t), s(t), a(t) ?

? interpolazione di �(t)

�(s) = �i +s

k�f � �ik(�f � �i)

_� =_s

k�f � �ik(�f � �i)

�� =�s

k�f � �ik(�f � �i)

? asse equivalente di rotazione (Rf = RiRi

f

)

Ri

f= R

T

iRf =

24 r11 r12 r13

r21 r22 r23

r31 r32 r33

35

#f = cos�1�

r11 + r22 + r33 � 1

2

�

r =1

2 sin#f

24 r32 � r23

r13 � r31

r21 � r12

35

Ri(t): R

i(0) = I Ri(tf ) = R

i

f

R(t) = RiRi

r(#(t))

#(0) = 0 #(tf ) = #f


SCALATURA DINAMICA DI TRAIETTORIE

? vincoli dinamici (coppie troppo elevate)

� (t) = B(q(t))�q(t) +C(q(t); _q(t)) _q(t) + g(q(t))

= B(q(t))�q(t) + � (q(t))[ _q(t) _q(t)] + g(q(t))

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

C(q; _q) _q = � (q)[ _q _q]

[ _q _q] = [ _q21 _q1 _q2 : : : _qn�1 _qn _q2n

]T


? scalatura temporale r(t): r(0) = 0 r(tf ) = �tf

q(t) = �q(r(t))

_q = _r�q0(r)

�q = _r2�q00(r) + �r�q0(r)

+

� = _r2�B(�q(r))�q00(r) + � (�q(r))[�q0(r)�q0(r)]

�+ �rB(�q(r))�q0(r)

+ g(�q(r))

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

��s(r) = B(�q(r))�q00(r) + � (�q(r))[�q0(r)�q0(r)]

+

�s(t) = _r2��s(r) + �rB(�q(r))�q0(r)

? scelta semplice r(t) = ct

�s(t) = c2��s(ct)

giunto qi in corrispondenza della violazione maggiore

j�sjj��i � g(qi)j

= c2


CONTROLLO DEL MOTO

Controllo nello spazio dei giunti

Controllo indipendente ai giunti

Compensazione in avanti a coppia precalcolata

Controllo centralizzato

Controllo nello spazio operativo


IL PROBLEMA DEL CONTROLLO

� Controllo nello spazio dei giunti

� Controllo nello spazio operativo


CONTROLLO NELLO SPAZIO DEI GIUNTI

� Modello dinamico

B(q)�q +C(q; _q) _q + Fv _q + g(q) = �

� Controllo � determinare � :

q(t) = qd(t)

? trasmissioni

Krq = qm �m =K�1r

�

? inerzie medie

B(q) = �B +�B(q)

�m =K�1r

�BK�1r

�qm + Fm _qm + d

? attrito viscoso

Fm =K�1r

FvK�1r

? disturbo

d =K�1r

�B(q)K�1r

�qm +K�1r

C(q; _q)K�1r

_qm +K�1r

g(q)


� Manipolatore + attuatori

? controllo decentralizzato

? controllo centralizzato


CONTROLLO INDIPENDENTE AI GIUNTI

� Manipolatore� n sistemi indipendenti (azionamenti di giunto)

? controllo di ogni asse di giunto come un sistema ad uningresso ed una uscita

? effetti di accoppiamento tra i vari giunti trattati come in-gressi di disturbo

� Azionamento del giunto i

Fm � kvkt

Ra

M(s) =km

s(1 + sTm)

km =1

kvTm =

RaI

kvkt


Controllo in retroazione

� Riduzione degli effetti di d su �

? elevato valore del guadagno degli amplificatori a monte delpunto di applicazione del disturbo

? presenza nel controllore di una azione integrale al fine diannullare a regime (� costante) l’effetto della componentegravitazionale sull’uscita

+

� Controllore proporzionale-integrale(PI)

C(s) = Kc

1 + sTc

s

� Struttura generale


� Retroazione di posizione

CP (s) = KP

1 + sTP

sCV (s) = 1 CA(s) = 1

kTV = kTA = 0

? f.d.t. del ramo di azione diretta

C(s)G(s) =kmKP (1 + sTP )

s2(1 + sTm)

? f.d.t. del ramo in retroazione

H(s) = kTP


? analisi con il l.d.r.


? f.d.t. ingresso–uscita a ciclo chiuso

�(s)

�r(s)=

1

kTP

1 +s2(1 + sTm)

kmKPkTP (1 + sTP )

W (s) =

1

kTP(1 + sTP )�

1 +2�s

!n+

s2

!2n

�(1 + s�)

? f.d.t. disturbo–uscita

�(s)

D(s)= �

sRa

ktKPkTP (1 + sTP )

1 +s2(1 + sTm)

kmKPkTP (1 + sTP )

XR = KP kTP TR = max

�TP ;

1

�!n

�


� Retroazione di posizione e di velocita

CP (s) = KP CV (s) = KV

1 + sTV

sCA(s) = 1

kTA = 0


C(s)G(s) =kmKPKV (1 + sTV )

s2(1 + sTm)


H(s) = kTP

�1 + s

kTV

KP kTP

�



? scelta dello zeroTV = Tm



�(s)

�r(s)=

1

kTP

1 +skTV

KPkTP+

s2

kmKP kTPKV

W (s) =

1

kTP

1 +2�s

!n+

s2

!2n

? specifiche di progetto

KV kTV =2�!n

km

KPkTPKV =!2

n

km


�(s)

D(s)= �

sRa

ktKPkTPKV (1 + sTm)

1 +skTV

KPkTP+

s2

kmKP kTPKV

XR = KPkTPKV TR = max

�Tm;

1

�!n

�


� Retroazione di posizione, di velocita e di accelerazione

CP (s) = KP CV (s) = KV CA(s) = KA

1 + sTA

s

G0(s) =

km

(1 + kmKAkTA)

0BB@1 +

sTm

�1 + kmKAkTA

TA

Tm

�(1 + kmKAkTA)

1CCA


C(s)G(s) =KPKVKA(1 + sTA)

s2G

0(s)


H(s) = kTP

�1 +

skTV

KP kTP

�



? scelta dello zero

TA = Tm

kmKAkTATA � Tm kmKAkTA � 1



�(s)

�r(s)=

1

kTP

1 +skTV

KPkTP+

s2(1 + kmKAkTA)

kmKPkTPKVKA


�(s)

D(s)= �

sRa

ktKPkTPKVKA(1 + sTA)

1 +skTV

KP kTP+

s2(1 + kmKAkTA)

kmKP kTPKVKA

XR = KP kTPKVKA TR = max

�TA;

1

�!n

�

? specifiche di progetto

2KP kTP

kTV=

!n

�

kmKAkTA =kmXR

!2n

� 1

KP kTPKVKA = XR


? ricostruzione dell’accelerazione


Compensazione in avanti decentralizzata

� Elevati valori di velocita e accelerazioni

? capacita di inseguimento degradate

� Impiego di una azione in avanti


� Retroazione di posizione

�0r(s) =

�kTP +

s2(1 + sTm)

kmKP (1 + sTP )

��d(s)


� Retroazione di posizione e di velocita

�0r(s) =

�kTP +

skTV

KP

+s2

kmKPKV

��d(s)


� Retroazione di posizione, di velocita e di accelerazione

�0r(s) =

�kTP +

skTV

KP

+(1 + kmKAkTA)s

2

kmKPKVKA

��d(s)


COMPENSAZIONE IN AVANTI A COPPIAPRECALCOLATA

� All’uscita del PIDD2

a2�e+ a1 _e+ a0e+ a�1

Zt

e(&)d& +Tm

km

��d +1

km

_�d �Ra

ktd

=Tm

km

�� +1

km

_�

+

a02�e+ a

01 _e+ a

00e+ a

0�1

Zt

e(&)d& =Ra

ktd

E(s)

D(s)=

Ra

kts

a02s3 + a01s

2 + a00s+ a0�1

? adozione di guadagni di anello troppo elevati


� Coppia precalcolata

? azione in avanti(inversione di modello)

dd =K�1r

�B(qd)K�1r

�qmd+K�1r

C(qd; _qd)K�1r

_qmd+K�1r

g(qd)

? riduzione degli sforzi di reiezione del disturbo (guadagnicontenuti)

? calcolo fuori linea/in linea

? compensazione parziale


CONTROLLO CENTRALIZZATO

� Manipolatore�Sistema multivariabile accoppiato e non lineare

B(q)�q +C(q; _q) _q + Fv _q + g(q) = �

� Trasmissioni

Krq = qm

� Sistemi di attuazione

K�1r

� =Ktia

va = Raia +Kv _qm

va = Gvvc

� Manipolatore controllato in tensione

B(q)�q +C(q; _q) _q + F _q + g(q) = u

F = Fv +KrKtR�1a

KvKr

u =KrKtR�1a

Gvvc


� Manipolatore controllato in coppia

? riduzione della sensibilita alle variazioni parametriche

Kt, Kv , Ra

? attuatori come generatori controllati di coppia (corrente)

ia = Givc

+

F = Fv

u =KrKtGivc = �


Controllo PD con compensazione di gravita

� Regolazione ad una postura di equilibrio costanteqd

� Metodo diretto di Lyapunov

? stato [ eqT _qT ]T eq = qd � q

? funzione candidata di Lyapunov

V ( _q; eq) = 1

2_qTB(q) _q +

1

2eqTKP eq > 0 8 _q; eq 6= 0

_V = _qTB(q)�q +1

2_qT _B(q) _q � _qTKP eq

=1

2_qT

�_B(q)� 2C(q; _q)

�_q � _qTF _q + _qT

�u� g(q)�KPeq�

? scelta del controllo

u = g(q) +KP eq �KD _q

+

_V = � _qT (F +KD) _q

_V = 0 _q = 0;8eq


? dinamica del sistema controllatoB(q)�q +C(q; _q) _q + F _q + g(q) = g(q) +KP eq �KD _q

? all’equilibrio ( _q � �q � 0)

KP eq = 0 =) eq = qd � q � 0


Controllo a dinamica inversa

� Modello dinamico

B(q)�q + n(q; _q) = u

n(q; _q) = C(q; _q) _q + F _q + g(q)

� Retroazione non lineare dello stato (linearizzazione globale)

? struttura lineare in u

? %(B(q)) = n 8q

u = B(q)y + n(q; _q)

+

�q = y


� Scelta di un controllo y stabilizzante

y = �KPq �KD _q + r

r = �qd +KD _qd +KPqd

+

�eq +KD_eq +KP eq = 0

? cancellazione perfetta

? vincoli sull’architettura hardware/software dell’unita di go-verno


Controllo robusto

� Compensazione imperfetta

u = bB(q)y + bn(q; _q)? incertezza

eB = bB �B en = bn� n+

B�q + n = bBy + bn�q = y + (B�1 bB � I)y +B

�1en = y � �

� = (I �B�1 bB)y �B�1en� Scelta del controllo

y = �qd +KD( _qd � _q) +KP (qd � q)

+

�eq +KD_eq +KP eq = �


�eq = �qd � y + �

? equazione differenziale del primo ordine

_� =H� +D(�qd � y + �)

� Stima del campo di variabilita dell’ incertezza

supt�0

k�qdk < QM <1 8�qd

kI �B�1(q) bB(q)k � � � 1 8q

kenk � � <1 8q; _q


� Scelta del controllo

y = �qd +KD_eq +KP eq +w

+

_� = fH� +D(� �w)

fH = (H �DK) =

�O I

�KP �KD

�


V (�) = �TQ� > 0 8� 6= 0

_V = _�TQ� + �TQ _�

= �T (fHT

Q+QfH)� + 2�TQD(� �w)

= ��TP� + 2�TQD(� �w)

= ��TP� + 2zT (� �w)


� Legge di controllo

w =�

kzkz � > 0

+

zT (� �w) = z

T� � �

kzkzTz

� kzkk�k � �kzk

= kzk(k�k � �)

� Scelta di �

� � k�k 8q; _q; �qd

? stima dell’ incertezza

k�k � kI �B�1 bBk�k�qdk+ kKk k�k+ kwk�

+ kB�1k kenk� �QM + �kKk k�k+ ��+BM�

� � 1

1� �(�QM + �kKkk�k+BM�)

+

_V = ��TP� + 2zT�

� � �

kzkz�

< 0 8� 6= 0


? sottospazio di scivolamento(attrattivo)

� Eliminazione delle componenti in alta frequenza

w =

8><>:�

kzkz per kzk � �

�

�z per kzk < �


Controllo adattativo

� Modello dinamico (lineare nei parametri)

B(q)�q +C(q; _q) _q + F _q + g(q) = Y (q; _q; �q)� = u

� Controllo

u = B(q)�qr +C(q; _q) _qr + F _qr + g(q) +KD�

_qr = _qd +�eq�qr = �qd +� _eq� = _qr � _q = _eq +�eq

+

B(q) _� +C(q; _q)� + F� +KD� = 0



V (�; eq) = 1

2�

TB(q)� +

1

2eqTMeq > 0 8�;eq 6= 0

_V = �TB(q) _� +

1

2�

T _B(q)� + eqTM _eq= ��T (F +KD)� + eqTM _eq= ��TF� � _eqTKD

_eq � eqT�KD�eq? _V = 0 solo per eq = _eq � 0 =) [ eqT �

T ]T = 0

globalmente asintoticamente stabile


� Controllo basato su stime dei parametri

u = bB(q)�qr + bC(q; _q) _qr + bF _qr + bg +KD�

= Y (q; _q; _qr; �qr)b� +KD�

+

B(q) _� +C(q; _q)� + F� +KD�

= � eB(q)�qr � eC(q; _q) _qr � eF _qr � eg(q)= �Y (q; _q; _qr; �qr)e�

� Modifica di V

V (�; eq; e�) = 1

2�

TB(q)�+eqT�KDeq+1

2e�TK�e� > 0 8�; eq; e� 6=

_V = ��TF� � _eqTKD_eq � eqT�KD�eq

+ e�T �K�_e� � Y T (q; _q; _qr; �qr)�

�


� Legge di adattamento

_b� =K�1�

YT (q; _q; _qr; �qr)�

+

_V = ��TF� � _eqTKD_eq � eqT�KD�eq

? eq ! 0

? Y (q; _q; _qr; �qr)(b� � �)! 0


CONTROLLO NELLO SPAZIO OPERATIVO

? specifiche di moto

? relazioni cinematiche

? carico computazionale

Schemi di principio

� Controllo con inversadello Jacobiano

� Controllo con traspostadello Jacobiano


Controllo PD con compensazione di gravita

� Regolazione ad una posizione di equilibrio costantexd


V ( _q; ex) = 1

2_qTB(q) _q +

1

2exTKP ex > 0 8 _q; ex 6= 0

_V = _qTB(q)�q +1

2_qT _B(q) _q + _exTKP ex

= _qTB(q)�q +1

2_qT _B(q) _q � _qTJT

A(q)KP ex

= � _qTF _q + _qT�u� g(q)� JT

A(q)KP ex�

? scelta del controllo

u = g(q) + JT

A(q)KP ex� JTAKDJA(q) _q

+

_V = � _qTF _q � _qTJTA

(q)KDJA(q) _q

_V = 0 JT

A(q)KP ex = 0


Controllo a dinamica inversa

� Modello

B(q)�q + n(q; _q) = u

� Controllo linearizzante

u = B(q)y + n(q; _q)

+

�q = y

= J�1A

(q)�

�xd +KD_ex+KP ex� _JA(q; _q) _q

��ex+KD

_ex+KP ex = 0


CONFRONTO TRA GLI SCHEMI DICONTROLLO


? a1 = a2 = 1m `1 = `2 = 0:5m

m`1= m`2

= 50 kg I`1 = I`2 = 10 kg�m2

? kr1 = kr2 = 100

mm1= mm2

= 5 kg Im1= Im2

= 0:01 kg �m2

Fm1 = Fm2 = 0:01N�m�s/rad Ra1 = Ra2 = 10 ohm

kt1 = kt2 = 2N�m/A kv1 = kv2 = 2V�s/rad


� Schemi di controllo

A. Indipendente ai giunti pos + vel

KP = 5 KV = 10 kTP = kTV = 1

B. Indipendente ai giunti pos + vel + acc

KP = 5 KV = 10 KA = 2 kTP = kTV = kTA = 1

C. Come A + compensazione in avanti decentralizzata

D. Come B + compensazione in avanti decentralizzata

E. Coppia precalcolata ai giunti (termini diag. inerzia + gravit a) +Indipendente ai giunti come A


F. PD + compensazione di gravita + KD _qd

KP = 3750I KD = 750I

G. Dinamica inversa

KP = 25I KD = 5I

H. Robusto ( bB = �B, bn = Fv _q + g)

KP = 25I KD = 5I � = 70 � = 0:004

I. Come H � = 0:01

J. Adattativo

�m2 = 10 kg �m2 C2 = 11 kg �m �I2 = 12:12 kg �m2

� = 5I KD = 750I K� = 0:01I

K. PD cartesiano + compensazione di gravita + KD _xd

KP = 16250I KD = 3250I

L. Dinamica inversa cartesiano

KP = 25I KD = 5I


0 1 2 3 4−3

−2.5

−2

−1.5

−1

−0.5

0

[s]

[rad

]

pos giunto 1

0 1 2 3 40

0.5

1

1.5

2

2.5

3

[s]

[rad

]

pos giunto 2

0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 4−0.1

0

0.1

0.2

[s]

[m]

errori pos


0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 40

0.005

0.01

0.015

[s]

[m]

norma err pos

0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 40

0.005

0.01

0.015

[s]

[m]

norma err pos


0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 40

0.005

0.01

0.015

[s]

[m]

norma err pos


0 1 2 3 4−3

−2.5

−2

−1.5

−1

−0.5

0

[s]

[rad

]

pos giunto 1

0 1 2 3 40

0.5

1

1.5

2

2.5

3

[s]

[rad

]

pos giunto 2

0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 40

0.02

0.04

0.06

0.08

0.1

[s]

[m]

norma err pos


0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 40

1

2

3

4x 10

−4

[s]

[m]

norma err pos


0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 40

1

2

3

4x 10

−4

[s]

[m]

norma err pos

0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 40

1

2

3

4x 10

−4

[s]

[m]

norma err pos


0 1 2 3 40

0.002

0.004

0.006

0.008

0.01

[s]

[m]

norma err pos

0 1 2 3 411

12

13

14

15

16

17

[s]

norma err parametri


0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 40

0.02

0.04

0.06

0.08

0.1

[s]

[m]

norma err pos


0 1 2 3 4−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 1 2 3 40

1

2

3

4x 10

−4

[s]

[m]

norma err pos


0 2 4 6 8 10−3

−2.5

−2

−1.5

−1

−0.5

0

[s]

[rad

]

pos giunto 1

0 2 4 6 8 100

0.5

1

1.5

2

2.5

3

[s]

[rad

]

pos giunto 2

0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 10−0.05

0

0.05

[s]

[m]

errori pos


0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 100

1

2

3

4x 10

−3

[s]

[m]

norma err pos

0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 100

1

2

3

4x 10

−3

[s]

[m]

norma err pos


0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 100

1

2

3

4x 10

−3

[s]

[m]

norma err pos


0 2 4 6 8 10−3

−2.5

−2

−1.5

−1

−0.5

0

[s]

[rad

]

pos giunto 1

0 2 4 6 8 100

0.5

1

1.5

2

2.5

3

[s]

[rad

]

pos giunto 2

0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 100

0.005

0.01

0.015

0.02

[s]

[m]

norma err pos


0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1x 10

−4

[s]

[m]

norma err pos


0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1x 10

−4

[s]

[m]

norma err pos

0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1x 10

−4

[s]

[m]

norma err pos


0 2 4 6 8 100

1

2

3

4

5x 10

−3

[s]

[m]

norma err pos

0 2 4 6 8 1012

13

14

15

16

17

[s]

norma err parametri


0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 100

0.005

0.01

0.015

0.02

[s]

[m]

norma err pos


0 2 4 6 8 10−500

0

500

1000

1500

[s]

[Nm

]

coppie giunti

0 2 4 6 8 100

0.2

0.4

0.6

0.8

1x 10

−4

[s]

[m]

norma err pos


CONTROLLO DELL’INTERAZIONE

Interazione del manipolatore con l’ambiente

Controllo di cedevolezza

Controllo di impedenza

Controllo di forza

Vincoli naturali e vincoli artificiali

Controllo ibrido forza/posizione


INTERAZIONE DEL MANIPOLATORE CONL’AMBIENTE

� Moto vincolato

� Controllo posizionale (?)

? errori di posizionamento

? incertezza sull’ambiente

� Contatto instabile

� Misura di forza

� Strategie di controllo

? indirette

? dirette


CONTROLLO DI CEDEVOLEZZA

� Effetti della forza di contatto su schemi di controllo posizionale(nello spazio operativo)

B(q)�q +C(q; _q) _q + F _q + g(q) = u� JT (q)h

? all’equilibrio

JT

A(q)KP ex = J

T (q)h

+

ex =K�1P

TT

A(x)h =K

�1P

hA

� K�1P

� cedevolezzadel controllo

? lineare nei riguardi di f

? torsionale nei riguardi di �

� Se h 2 N (JT ) =) ex = 0 h 6= 0


� Modello dell’ambiente

? elasticamente cedevole

h =

�f

�

�=

�Kf O

O Km

� �dp

!dt

�

=K

�dp

!dt

�

=KTA(x)dx

+

hA = TT

A(x)KTA(x)dx

=KA(x)(x� xe)

� K�1A

� cedevolezzadell’ambiente

? solo per le direzioni di moto vincolato

ex =K�1P

KA(x)(x� xe)


� Cedevolezza attivae cedevolezza passiva

? all’equilibrio

x1 =�I +K

�1P

KA(x)��1

(xd +K�1P

KA(x)xe)

hA1 =�I +KA(x)K

�1P

��1KA(x)(xd � xe)

? manipolatore� cedevole

? ambiente � cedevole

� Dispositivi meccanici di cedevolezza passiva


Esempio

KA =Kf = diagfkx; 0g KP = diagfkPx; kPyg

� All’equilibrio

p1 =

"kPxxd + kxxe

kPx + kxyd

#f1 =

24 kPxkx

kPx + kx(xd � xe)

0

35

? se kPx=kx � 1

x1 � xd fx1 � kx(xd � xe)

? se kPx=kx � 1

x1 � xe fx1 � kPx(xd � xe)


� Cedevolezza attiva nello spazio dei giunti

? all’equilibrio

KPeq = JT (q)h

+

eq =K�1P

JT (q)h

ex = JA(q)K�1P

JT

A(q)hA


CONTROLLO DI IMPEDENZA

� Controllo a dinamica inversa nello spazio operativo

u = B(q)y + n(q; _q)

+

�q = y �B�1(q)JT (q)h

? scelta del nuovo ingresso di controllo

y = J�1A

(q)M�1d

�Md�xd +KD

_ex+KP ex�Md_JA(q; _q) _q

�

Md�ex+KD

_ex+KP ex =MdB�1A

(q)hA

BA(q) = J�T

A(q)B(q)J�1

A(q)

� Impedenza meccanica

? massa Md

? smorzamentoKD

? rigidezzaKP


� Integrazione della misuradella forzadi contatto

u = B(q)y + n(q; _q) + JT (q)h

y = J�1A

(q)M�1d

�Md�xd+KD

_ex+KP ex�Md_JA(q; _q) _q�hA

�+

Md�ex+KD

_ex+KP ex = hA

? impedenza lineareattiva

� Dispositivi meccanici di impedenza passiva


Esempio

� Controllo di impedenza con misura di forzaMd = diagfmdx;mdyg

KD = diagfkDx; kDyg

KP = diagfkPx; kPyg

? dinamica del sistema manipolatore–ambiente lungo le duedirezioni dello spazio operativo (xd costante)

mdx�x+ kDx _x+ (kPx + kx)x = kxxe + kPxxd

mdy�y + kDy _y + kPyy = kPyyd


� Ambiente (xe = 1)

? piu cedevole kx = 103 N/m

? meno cedevole kx = 104 N/m

� Controllo

? mdx = mdy = 100

? kDx = kDy = 500

? kPx = kPy = 2500

� Manipolatore

? p = [ 1 0 ]T

? pd = [ 1:1 0:1 ]T

0 1 2 30

0.05

0.1

[s]

[m]

pos organo terminale y

0 1 2 30

50

100

150

200

250

300

[s]

[N]

forza organo terminale x


CONTROLLO DI FORZA

� Controllo di cedevolezza o impedenza =) controllo indi-retto di forza

� Controllo diretto

? azione di controllo PD sulla forza (misura corrotta da ru-more)

? chiusura di un anello esterno di regolazione della forza

=) ingresso di riferimento per uno schema di controlloposizionale a dinamica inversa


Controllo di forza con anello interno di posizione

u = B(q)y + n(q; _q) + JT (q)h

y = J�1A

(q)M�1d

��KD _x+KP (xF � x)�Md

_JA(q; _q) _q�

+

Md�x+KD _x+KPx =KPxF

� Scelta del controllo

xF = CF (hAd � hA)

? ambiente

hA =KA(x)(x� xe)

+

Md�x+KD _x+KP (I +CFKA)x =KPCF (KAxe + hAd)


� Azione PI

CF =KF +KI

Zt

0

(�) d&

? scelta di KD, KP , KF , KI =) margini di stabilita+ banda passante

� All’equilibrio (stabile)

hA1 = hAd

KAx1 =KAxe + hAd


Controllo di forza con anello interno di velocita

y = J�1A

(q)M�1d

��KD _x+KPxF �Md

_JA(q; _q) _q�

xF =KF (hAd � hA)

+

Md�x+KD _x+KPKFKAx =KPKF (KAxe + hAd)

� All’equilibrio (stabile)

hA1 = hAd

KAx1 =KAxe + hAd


Controllo parallelo forza/posizione

? se hAd =2 R(KA) =) deriva di x

? se hAd 2 R(KA) =) x (o _x) ! 0

� Aggiunta di xd

y = J�1A

(q)M�1d

��KD _x+KP (ex+ xF )�Md

_JA(q; _q) _q�

� All’equilibrio

x1 = xd +CF

�KA(xe � x1) + hAd

�


Esempio

� Impedenza (Md, KD, KP ) come sopra� Riferimenti hAd = [ 10 0 ]T pd = [ 1:015 0:1 ]T

� Controllo di forza

CF = diagfcFx; 0g

1. controllo con anello interno di posizione

kFx = 0:00064 kIx = 0:0016

2. controllo con anello interno di velocita

kFx = 0:0024

3. controllo parallelo

kFx = 0:00064 kIx = 0:0016


0 1 2 31

1.005

1.01

1.015

1.02

[s]

[m]

pos organo terminale x

0 1 2 30

10

20

30

40

50

[s]

[N]


0 1 2 31

1.005

1.01

1.015

1.02

[s]

[m]


0 1 2 30

10

20

30

40

50

[s]

[N]


0 1 2 31

1.005

1.01

1.015

1.02

[s]

[m]


0 1 2 30

10

20

30

40

50

[s]

[N]



VINCOLI NATURALI E VINCOLIARTIFICIALI

� Riferimenti di forza e posizione scelti in maniera congruenteall’ambiente

? non e possibile imporre sia una forza che una posizionelungo la stessa direzione

� Analisi cineto-statica dell’ interazione

? vincoli naturali imposti dall’ambiente lungo ciascun gradodi liberta del compito (posizione o forza)

? vincoli artificiali soggetti al controllo lungo ciascun gradodi liberta del compito (forza o posizione)

� Terna di vincolo Oc–xcyczc

? descrizione semplificata del compito in termini di vincolinaturali e vincoli artificiali

Analisi di casi


� Scivolamento su superficie piana

Vincoli VincoliNaturali Artificiali

_pcz

fcz

!cx

�cx

!cy

�cy

fcx

_pcx

fcy

_pcy

�cz

!cz


� Inserimento in foro cilindrico


_pcx

fcx

_pcy

fcy

!cx

�cx

!cy

�cy

fcz

_pcz

�cz

!cz


� Rotazione di una manovella


_pcx

fcx

_pcz

fcz

!cx

�cx

!cy

�cy

fcy

_pcy

�cz

!cz


CONTROLLO IBRIDO FORZA/POSIZIONE

� Vincoli naturali

�vc = v

c

n(I ��)hc = h

c

n

? � � matrice di selezione

� Vincoli artificiali

(I ��)vc = vc

a�h

c = hc

a

? perfetta pianificazione del compito

? in alternativa: controllo parallelo


Esempio

� Matrice di selezione

� = diagf0; 1g

� Terna di vincolo

Rc =

�1=

p2 1=

p2

�1=p

2 1=p

2

�


ATTUATORI E SENSORI

Sistema di attuazione dei giunti

Servomotori

Sensori


SISTEMA DI ATTUAZIONE DEI GIUNTI


Organi di trasmissione

� Moto dei giunti

? basse velocita

? elevate coppie

� Ruote dentate

? variano l’asse di rotazione e/o traslano il punto di applica-zione

? denti a sezione larga e alberi tozzi (deformazioni limitate)

� Coppie vite–madrevite

? 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 dall’asse del giuntoattuato

? alte velocita e basse forze (deformabili)

� Catene

? consentono di allocare il motore lontano dall’asse del giuntoattuato

? basse velocita (vibrazione)


� Accoppiamento diretto

? si eliminano elasticita e gioco

? algoritmi di controllo piu sofisticati


Servomotori

� Motori pneumatici

? energia pneumatica fornita da un compressore, trasformatain energia meccanica mediante pistoni o turbine ad aria

� Motori idraulici

? energia idraulica immagazzinata in un serbatoio di accumu-lazione mediante opportune pompe, trasformata in energiameccanica

� Motori elettrici

? energia elettrica della rete di distribuzione


� Motori per robot industriali

? bassa inerzia ed elevato valore del rapporto potenza/peso

? elevata possibilita di sovraccarico e sviluppo di coppie im-pulsive

? capacita di sviluppare elevate accelerazioni

? elevato campo di variazione di velocita (da 1 a 1000)

? elevata precisione di posizionamento (almeno 1/1000 digiro)

? basso fattore di ondulazione della coppia in modo da garan-tire rotazioni continue anche a bassa velocita

+

� Servomotori

? inseguimento di traiettoria

? regolazione di posizione

� Servomotori elettrici

? servomotori in corrente continua a magneti permanenti

? servomotori a commutazione elettronica

? motori a passo (micromanipolatori)

� Servomotori idraulici

? coppia pistone–cilindro (moti di traslazione

? servomotori idraulici con pistoni assiali o radiali



Pro:

? disponibilita diffusa di sorgente di alimentazione

? costo contenuto e vasta gamma di prodotti

? buon rendimento di conversione di potenza

? facile manutenzione

? assenza di inquinamento dell’ambiente di lavoro

Contro:

? problemi di surriscaldamento in situazioni statiche a causadell’effetto della gravita sul manipolatore (freni di stazio-namento)

? necessita di particolari protezioni per impiego in ambientiinfiammabili



Pro:

? non presentano problemi di surriscaldamento in situazionistatiche

? sono autolubrificati e il fluido circolante facilita lo smalti-mento del calore

? sono intrinsecamente sicuri in ambienti pericolosi

? hanno un ottimo rapporto potenza/peso

Contro:

? necessita di una centrale idraulica di alimentazione

? costo elevato, ridotta gamma di prodotti, difficolta di minia-turizzazione

? basso rendimento nella conversione di potenza

? necessita di manutenzione periodica

? inquinamento dell’ambiente di lavoro per perdite di olio



? buone caratteristiche dinamiche

? ottima flessibilita di controllo

? necessita di organi di trasmissione


? caratteristiche dinamiche dipendenti dalla temperatura delfluido in pressione

? coppie elevate a basse velocita


Amplificatori di potenza

� Funzione di modulazione del flusso di potenza dall’alimentazioneverso l’attuatore (sotto l’azione del controllo)


? amplificatori a transistori

convertitori controllati c.c./c.c. (chopper)

convertitori controllati c.c./c.a. (inverter)

� Servomotori idraulici? servovalvole elettroidrauliche


Sorgenti di alimentazione

� Azionamenti elettrici

? trasformatore + ponte raddrizzatore (non controllato)

� Azionamenti idraulici

? pompa azionata da motore primo (asincrono trifase) a velo-cita costante + serbatoio di accumulazione + filtri + valvole


SERVOMOTORI

Servomotori elettrici

� Servomotore a corrente continua a magneti permanenti

? un circuito statorico che genera il flusso magnetico di ecci-tazione (induttore): questo generatore e sempre a magnetipermanenti realizzati con materiali ceramici o con terre rare(campi elevati in spazi ridotti)

? un circuito di armatura (indotto) che reca un avvolgimentocostituito dalle spire di un solenoide avvolto su materialeferromagnetico che costituisce il rotore

? un collettore che consente di collegare, mediante delle spaz-zole, i circuiti rotorici ad un circuito esterno di alimen-tazione secondo una logica di commutazione attuata dalmovimento del rotore

� Servomotore a commutazione elettronica

? un generatore di flusso magnetico di eccitazione (induttore)che funge da rotore; questo generatore e a magneti perma-nenti di materiale ceramico o a terre rare

? un circuito di armatura (indotto) costituito da un avvolgi-mento polifase che funge da statore

? un circuito di commutazione statico che, a partire dai segnaliforniti da un sensore di posizione posto sull’albero motore,genera la sequenza di alimentazione delle fasi del circuitodi armatura in dipendenza del moto di rotazione del rotore


� Servomotore elettrico con amplificatore

� Equilibrio elettrico

Va = (Ra + sLa)Ia + Vg

Vg = kv

� Equilibrio meccanico

Cm = (sIm + Fm) + Cr

Cm = ktIa

� Amplificatore di potenza

Va

Vc=

Gv

1 + sTv

� Retroazione di corrente


� Generatore controllato di velocita

? ki = 0

? Fm � kvkt=Ra

? K = Ci(0)Gv

? Cr = 0

=

K

kv

1 + sRaIm

kvkt

V0c

�

Ra

kvkt

1 + sRaIm

kvkt

Cr

? a regime:

! � K

kvv0c


� Protezione di corrente


� Generatore controllato di coppia

? Kki � Ra

? kv=Kki � 0

=

kt

kiFm

1 + sIm

Fm

V0c

�

1

Fm

1 + sIm

Fm

Cr

? a regime:

cm � kt

ki

�v0c� kv

K!

�


Servomotori idraulici

� Coppie pistone–cilindro

? servomotori lineari

? servomotori rotanti

� Motore idraulico con servovalvola e distributore


SENSORI

� Sensori propriocettivi

? posizioni dei giunti

? velocita dei giunti

? coppia ai giunti

� Sensori esterocettivi

? sensori di forza

? sensori tattili

? sensori di prossimita? sensori di campo

? sistemi di visione

? sensori per applicazioni particolari (suono, umidita, fumo,pressione, temperatura)

fusione dei dati sensoriali (robot � organo intelligente diconnessione tra percezione e azione)


Trasduttori di posizione

� Spostamenti lineari (robot di misura)

? potenziometri

? trasformatori differenziali (LVDT)

? inductosyn

� Spostamenti angolari

? potenziometri

? encoder (dischi codificatori)

? resolver? synchro


� Encoder assoluto

# Codice # Codice0 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


� Encoder incrementali

� Ricostruzione della velocita (impulso in corrispondenza di ognitransizione)

? convertitore frequenza/tensione (uscita analogica)

? misura della frequenza (digitale)

? misura del periodo di campionamento (digitale)


� Resolver


Trasduttori di velocita

� Dinamo tachimetrica

? generatore in c.c.

? magnete permanente (campo di eccitazione)

? tensione di uscita proporzionale alla velocita 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 velocita angolare

? tensione residua a rotore fermo


Sensori di forza

� Estensimetro (strain gage)

Vo =

�R2

R1 +R2

� Rs

R3 +Rs

�Vi

? impiego di una coppia di estensimetri (riduzione degli effettidi variazioni di temperatura)

� Sensore di coppia all’albero

? servomotore come generatore controllato di coppia

? misura di coppia con estensimetri montati su un organodeformabile (albero cavo) interposto tra motore e giunto(bassa rigidezza torsionale ed elevata rigidezza flessionale)

? coppia misurata senza effetti inerziali e di attrito del servo-motore


� Sensore di forza al polso

? diametri � 10 cm

? altezza � 5 cm

? (50, 500) N (risoluzione 0.1%)

? (5, 70) N�m (risoluzione 0.05%)

? frequenza in uscita � 1 kHz

�f

c

c

�c

c

�=

�R

c

sO

S(rccs

)Rc

sR

c

s

� �f

s

s

�s

s

�


? sensore di forza a croce di Malta

2666664fsx

fsy

fsz

�sx

�sy

�sz

3777775 =

26666640 0 c13 0 0 0 c17 0

c21 0 0 0 c25 0 0 0

0 c32 0 c34 0 c36 0 c38

0 0 0 c44 0 0 0 c48

0 c52 0 0 0 c56 0 0

c61 0 c63 0 c65 0 c67 0

3777775

26666666664

w1

w2

w3

w4

w5

w6

w7

w8

37777777775


Sensori di visione

� Telecamera

� Elemento fotosensibile (pixel)? struttura a matrice rettangolare

? CCD

carica = integrale dell’ illuminazione incidente

? CMOS

carica = quantita di illuminazione incidente


~pc = Tc

b~p

� Trasformazione prospettica

X =fpc

x

f � pcz

Y =fpc

y

f � pcz

? aberrazioni

? distorsione geometrica


? campionamento temporale (otturatore)

? campionamento spaziale (pixel)

XI =�xfp

c

x

f � pcz

+X0

YI =�yfp

c

y

f � pcz

+ Y0

24xIyIzI

35 =

264pcx

pcy

pcz

1

375

=

24�x 0 X0 0

0 �y Y0 0

0 0 1 0

35264

1 0 0 0

0 1 0 0

0 0 �1=f 1

0 0 0 1

375

+

� Matrice di calibrazione

� = Tc

b

? parametri intrinseci (�x; �y; X0; Y0; f ) in

? parametri estrinseci in T bc


� Standard video

? CCIR (Europa e Australia): 625 linee, 25 Hz

? RS170 (America e Giappone): 525 linee, 30 Hz

? segnale video = tensione 1 V picco–picco

� Immagine = scansione sequenziale

? linee pari (primo semiquadro)

? linee dispari (secondo semiquadro)

� Discretizzazione del segnale video

? frame grabber� Visione a basso livello

? caratteristiche elementari (centroide, discontinuita di inten-sita)

� Visione ad alto livello

? emulazione della conoscenza (riconoscimento di oggetti inbase a modelli CAD)


UNITA DI GOVERNO

Architettura funzionale

Ambiente di programmazione

Architettura hardware


ARCHITETTURA FUNZIONALE

� Unita di governo

? abilita di muovere oggetti fisici nell’ambiente di lavoro,(capacita di manipolazione)

? abilita di ottenere informazioni sullo stato del sistema e suquello dell’ambiente di lavoro (capacita sensoriali)

? abilita di utilizzare informazioni per modificare il compor-tamento del sistema in maniera preprogrammata (capacit adi comportamento intelligente)

? abilita di immagazzinare, elaborare e rendere disponibilidati relativi all’attivita del sistema (capacita di processa-mento dati)


Modello di riferimento

� Evoluzione verso livelli superiori

? compiti complessi

? missioni


� Livello del compito

? specifica del compito a un livello di astrazione elevato

? decomposizione in una serie di azioni (coordinamentospazio-temporale)

? scelta sulla base dei modelli della conoscenza e delle infor-mazioni sulla scena

� Livello delle azioni

? comandi simbolici dal livello del compito tradotti in percorsidi moto (azioni elementari)

? scelta sulla base dei modelli del manipolatore e delle infor-mazioni sull’ambiente

� Livello delle primitive

? sequenza di configurazioni dal livello delle azioni

? calcolo traiettorie di moto (riferimenti per i servo)

? definizione della strategia di controllo

? scelta sulla base del modello dinamico e delle informazionidei sensori

� Livello dei servo

? direttive dal livello delle primitive

? calcolo algoritmi di controllo (segnali di attuazione)

? microinterpolazione

? aggiornamento sulla base delle misure dei sensori


Architettura funzionale per robot industriali


AMBIENTE DI PROGRAMMAZIONE

� Caratteristiche (comuni con quelli dei calcolatori + ...)

? sistema operativo in tempo reale

? modellazione del mondo

? controllo del moto

? lettura dei dati sensoriali

? interattivita con il sistema fisico

? capacita di rivelazione di errori

? capacita di ripristino di situazioni operative corrette

? struttura specifica dei linguaggi


Requisiti imposti dall’articolazione in moduli

� Trattamento dei dati sensoriali

? servo (condizionamento segnali in tempo reale)

? primitive (dati sensoriali in terne di riferimento)

? azioni (elaborazione dati ad alto livello)

? compito (riconoscimento oggetti nella scena)

� Consultazione dei modelli di conoscenza

? servo (calcolo numerico dei modelli in tempi brevi)

? primitive (trasformazioni di coordinate)? azioni (simulazione + rappresentazione CAD)

? compito (sistema esperto)

� Funzioni decisionali

? servo (calcolo in tempo reale)

? primitive (condizionamento logico)

? azioni (sincronizzazione + sistema di interruzioni)

? compito (gestione di processi concorrenti + debugger)


Generazioni di ambienti di programmazione

Programmazione a basso livello (Assembly, C)

� Programmazione per insegnamento

� Programmazione orientata al robot

� Programmazione orientata agli oggetti

? la generazione successiva incorpora le caratteristiche dellagenerazione precedente


Programmazione per insegnamento

� Guida dell’operatore lungo il percorso desiderato attraversoteach pendant

� Registrazione delle posizioni dei giunti

� Ripetizione del moto

? capacita di programmazione limitata (tecnico di officina)

? disponibilita del robot

? saldatura a punti, verniciatura, palettizzazione� Adozione di linguaggi

? acquisizione di posture significative mediante apprendi-mento

? calcolo della locazione dell’organo terminale rispetto ad unaterna di riferimento (cinematica diretta)

? assegnazione di primitive di moto con parametri di traietto-ria

? calcolo dei riferimenti dei servo (cinematica inversa)

? condizionamento delle sequenze di apprendimento a sem-plici sensori (presenza oggetto nella pinza)

? correzione delle sequenze di moto a mezzo di semplici edi-tori di testo

? semplici operazioni di collegamento tra sottoinsiemi di se-quenze elementari


Programmazione orientata al robot

? integrazione di numerose funzioni di linguaggi di program-mazione ad alto livello (BASIC, PASCAL) con quelle spe-cifiche richieste nelle applicazioni robotiche

� Alternative

? sviluppo di linguaggi ad hocper applicazioni robotiche

? sviluppo di librerie di programmiper robotica di supportoa linguaggi standard di programmazione

? programmatore esperto

? programmazione fuori linea

? interazione con altre macchine


� Caratteristiche di linguaggio di programmazione strutturata

? editore di testo

? strutture complesse di rappresentazione dei dati

? uso estensivo di variabili di stato predefinite

? realizzazione di operazioni di algebra matriciale

? uso estensivo di rappresentazioni simboliche per le terne dicoordinate

? possibilita di specificare il moto coordinato di piu ternerelative ad oggetti connessi rigidamente per il tramite diuna sola terna

? inclusione di sottoprogrammi con scambio di dati e para-metri

? uso di funzioni di condizionamento logico e cicli di attesamediante semafori

? capacita di parallelizzazione del calcolo

? funzioni di controllore logico programmabile (PLC)


ARCHITETTURA HARDWARE


� Scheda sistema

? un microprocessore con coprocessore matematico

? una memoria EPROM di inizializzazione

? una memoria RAM locale

? una memoria RAM condivisa con le altre schede attraversoil bus

? un insieme di porte seriali e parallele di interfaccia con ilbus e con il mondo esterno

? contatori, registri, temporizzatori

? gestore delle interruzioni

� Funzioni

? interfaccia con l’operatore attraverso teach pendant, tastiera,video, stampante

? interfaccia con una memoria esterna (disco rigido) usata perimmagazzinare dati e programmi applicativi

? interfaccia con rete locale di connessione (ad esempio,Ethernet) con stazioni di lavoro e con altre unita di governo

? interfaccia I/O con apparecchiature periferiche presentinell’area di lavoro (alimentatori, nastri trasportatori, sen-sori ON/OFF)

? inizializzazione del sistema

? interprete del linguaggio di programmazione

? arbitro del bus

� Processori suppletivi o alternativi (DSP, Transputer)


� Funzioni della scheda cinematica

? calcolo delle primitive di moto

? calcolo della cinematica diretta, della cinematica inversa edello Jacobiano

? verifica di congruenza della traiettoria

? gestione della ridondanza cinematica

� Funzioni della scheda dinamica

? calcolo della dinamica inversa

� Funzioni della scheda servo

? microinterpolazione dei riferimenti

? calcolo dell’algoritmo di controllo

? conversione D/A ed interfaccia con amplificatori di potenza

? trattamento dati dei trasduttori di posizione e di velocita

? 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 - · PDF fileROBOTICA INDUSTRIALE Prof. Bruno SICILIANO Robotica deﬁnita come...

Documents

Transcript of ROBOTICA - · PDF fileROBOTICA INDUSTRIALE Prof. Bruno SICILIANO Robotica deﬁnita come...