[Anno]

Prof. Ing. R. Sinatra

Ing. A. Cammarata

UNVERSITA’ DEGLI STUDI DI

CATANIA Corso di Laurea in Ingegneria dell’ Automazione e

Controllo dei sistemi complessi

Modellistica e Controllo dei Sistemi Meccanici

Anno 2008/09

Di Mauro Gianluca Patti Giuseppe

1. Introduzione Il robot parallelo da noi analizzato è il 3CRU. Esso è composto da tre catene

cinematiche seriali identiche, le quali collegano tra di loro due piattaforme, una fissa

e una mobile di forma tetraedrica (Fig1). Ciascuna catena è composta da due

membri: il primo è collegato alla base attraverso l’ausilio di un giunto cilindrico (C), il

secondo è collegato per un’estremità al primo membro attraverso un giunto

rotoidale (R), per l’altra collegato alla piattaforma mobile mediante un giunto

universale (U). La nostra analisi comprende sia lo studio della cinematica,

(indispensabile per ottenere le equazioni di vincolo del robot da cui si ricaveranno:

posizioni velocità e accelerazioni), sia quello della dinamica la quale permette,

attraverso i dati acquisiti dalla cinematica, di calcolare le forze agenti sul sistema.

Data la complessità delle equazioni che entrano in gioco, (49 equazioni in 49

incognite), questo studio è stato effettuato con l’ausilio del software Matlab.

Fig.1 3CRU

2. Cenni teorici

2.1 Gradi di libertà

Data la presenza di tre giunti rotoidali che conferiscono un grado di libertà, tre giunti

cilindrici e tre giunti universali con che danno due gradi di libertà, applicando il

criterio di mobilità di Kutzbach-Grübler alla struttura:

L=λ ( ) ∑=

+−−⋅j

fjl1i

i1

dove: λ sono i gradi di libertà (spazio=6 ; piano=3), L è il numero di gradi di libertà di

tutto il sistema, l è il numero dei corpi (o link) compreso il telaio, j è il numero dei

giunti (o coppie cinematiche) ed if è il numero di gradi di libertà associati all’i-esimo

giunto.

Si ottiene:

L=6·(8 – 9 – 1) + 6 + 3 +6 = 3

f(cilindrico) = 6 ; f(rotoidale)= 3 ; f(universale)=6.

Quindi la struttura nel suo complesso è tre volte labile

.

2.2 analisi cinematica

L’analisi cinematica costituisce la premessa a qualsiasi indagine sulla dinamica.

Tra i diversi approcci che possono essere utilizzati, si distingue, per la sua

sistematicità, il metodo delle equazioni di vincolo, tale metodo, si badi bene è

molto flessibile ma data la notevole quantità di equazioni che entrano in

gioco,costituisce un eccellente strumento solo nel momento in cui ci si avvale di

un calcolatore per svolgerne le equazioni.

Analisi delle posizioni

L’analisi delle posizioni si esaurisce nella soluzione del sistema di equazioni

algebriche non lineari

{ } { }0s

d

Ψ Ψ ≡ = Ψ

Dove:

- sΨ vettore dei vincoli strutturali.

- dΨ vettore dei vincoli di moto.

Analisi velocità e accelerazioni

L’analisi di velocità e accelerazione resta completa risolvendo, rispetto alle incognite

�̇�𝑞𝑘𝑘 , il sistema lineare di equazioni

{ } { }q tq Ψ = − Ψ

Dove q Ψ e { }tΨ sono rispettivamente lo Jacobiano delle equazioni ai vincoli uno

rispetto alle “q” è il vettore contenente le derivate parziali delle equazioni ai vincoli rispetto al tempo

1 1

1 2

1

q

m m

m

q q

q q

∂Ψ ∂Ψ ∂ ∂

Ψ = ∂Ψ ∂Ψ ∂ ∂

{ }

1

t

m

t

t

∂Ψ ∂

Ψ = ∂Ψ

∂

Calcolati questi due termini, si può ricavare :

{ } { }1

q tq−

= − Ψ Ψ

Per l’analisi alle accelerazioni si considera la seguente formula:

{ } { }q q γ Ψ =

Dove:

{ } { }( ) { } { } [ ]2q qt ttqq q qγ = − Ψ − Ψ − Ψ

Ricavando così il vettore delle accelerazioni:

{ } { }1

qq γ−

= Ψ

Parametri di Eulero

Per poter determinare l’orientamento del corpo si è scelto di utilizzare i cosiddetti

parametri di Eulero in quanto questo tipo di notazione è più robusta rispetto a

quella degli angoli di Eulero poiché con quest’ultima si possono avere problemi di

riflessioni, ovvero di rotazioni del tipo θ= n± π con n N∈ .

A tale notazione si perviene definendo dapprima:

• Tuuuu },,{ˆ 321= il versore dell’asse u di rotazione;

• Ψ l’angolo di rotazione del corpo attorno ad û;

da ciò l’orientamento di un corpo, ovvero i moti sferici possono essere descritti

utilizzando i parametri di Eulero e cioè:

0 1 1cos ; sin2 2

e e uψ ψ= =

2 2 3 3sin ; sin2 2

e u e uψ ψ= =

I quali devono soddisfare la condizione di normalizzazione:

123

22

21

20 =+++ eeee

A questo punto la matrice di rotazione per noi indispensabile per eseguire i nostri

calcoli assume la forma :

|A|=2

−++−−−+++−−+

212

32010322031

1032212

2203021

20313021212

120

eeeeeeeeeeeeeeeeeeeeeeeeeeeeee

OSS:

Per ogni corpo si sono assegnate sette coordinante generalizzate: T

zyxi eeeerrrq },,,,,,{ 3210=

dove le r rappresentano le distanze dall’origine del sistema di riferimento inerziale

posto sul baricentro del corpo i-esimo.

ANALISI VINCOLI DI BASE

Per poter analizzare i giunti che compongono il 3CRU, cioè quello cilindrico,

rotoidale e universale, bisogna prima trattare i cosiddetti vincoli di base, ossia quelle

condizioni di ortogonalità e parallelismo, tra coppie di vettori, che permettono la

formulazione delle equazioni di vincolo relative sia alla posizione assoluta dei corpi

nello spazio sia a quella relativa tra coppie di corpi.

Deduzione equazione dei vincoli

Ai fini della caratterizzazione dei vincoli è utile introdurre, per la generica coppia

cinematica k, un riferimento di assi cartesiani detto riferimento del giunto (Fig. 2)

avente origine nel punto 𝑃𝑃𝑖𝑖𝑘𝑘 e solidale anch’esso al corpo i e dove i punti ikik QP , ed

ikR costituiscono i punti di definizione del giunto. La sua posizione angolare, inoltre,

è definita dalle componenti dei versori }{ ikf , }{ ikg ed }{ ikh , ottenuti dalle relazioni

ikikik PRf −=}{ ,

ikikik PQh −=}{ ,

}]{~[}{ ikikik fhg = .

Fig 2

Primo vincolo di ortogonalità

Tale condizione, considerando due vettori {𝑎𝑎𝑖𝑖}0 e �𝑎𝑎𝑗𝑗 �0

,solidali rispettivamente ai

corpi i e j, viene espressa dalla relazione:

𝛹𝛹𝑜𝑜1 �{𝑎𝑎𝑖𝑖}0, �𝑎𝑎𝑗𝑗 �0� ≡ {𝑎𝑎𝑖𝑖𝑇𝑇}0�𝑎𝑎𝑗𝑗 �

0 = 0

Secondo vincolo di ortogonalità

Considerando un vettore {𝑎𝑎𝑖𝑖}0 solidale al corpo i e un vettore non nullo

�𝑑𝑑𝑖𝑖𝑗𝑗 �0

congiungente due punto, uno appartenete al corpo i e l’altro al corpo j si ha

che questo vincolo viene espresso da

𝛹𝛹𝑜𝑜2 �{𝑎𝑎𝑖𝑖}0, �𝑑𝑑𝑖𝑖𝑗𝑗 �0� ≡ {𝑎𝑎𝑖𝑖𝑇𝑇}0�𝑑𝑑𝑖𝑖𝑗𝑗 �

0 = 0

Vincolo sferico

Affinché due punti Pi e Pj appartenenti rispettivamente ai corpi i e j siano sovrapposti deve valere che

𝛹𝛹𝑠𝑠�𝑃𝑃𝑖𝑖 ,𝑃𝑃𝑗𝑗 � ≡ �𝑟𝑟𝑗𝑗 �0 + [𝐴𝐴]𝑗𝑗0�𝑠𝑠𝑗𝑗 �

𝑗𝑗 − {𝑟𝑟𝑖𝑖}0 + [𝐴𝐴]𝑖𝑖0{𝑠𝑠𝑖𝑖}𝑖𝑖 = 0

Vincolo di distanza

Considerando una coppia di punti Pi e Pj appartenenti ai corpi i e j, la condizione affinché la distanza tra questi punti sia uguale a C≠ 0 è

𝛹𝛹𝑠𝑠𝑠𝑠�𝑃𝑃𝑖𝑖 ,𝑃𝑃𝑗𝑗 ,𝐶𝐶� ≡ �𝑑𝑑𝑖𝑖𝑗𝑗𝑇𝑇�

0�𝑑𝑑𝑖𝑖𝑗𝑗 �

0 − 𝐶𝐶2 = 0

Primo vincolo di parallelismo

Considerando due punti Pi e Pj appartenenti ai corpi i e j

{ } { }( ){ } { }( ){ } { }( )

{ }01

1

01

,, 0

,

i jpi j

i j

f hh h

g h

Ψ Ψ = = Ψ

Secondo vincolo di parallelismo

Il secondo vincolo di parallelismo nasce quando vogliamo che il versore �ℎ𝑗𝑗 � debba

essere parallelo al generico vettore �𝑑𝑑𝑖𝑖𝑗𝑗 �0

. Ciò avviene se viene rispettato il vincolo

{ } { }( ){ } { }( ){ } { }( )

{ }02

2

02

,, 0

,

i ijpi ij

i ij

f dh d

g d

Ψ Ψ = = Ψ

Analisi dei giunti

Illustrati tutti i tipi di vincoli base si può ora passare alla disamina dei giunti che compongono il nostro robot.

Giunto cilindrico

Il primo giunto che si và ad analizzare è di tipo cilindrico

, questo è caratterizzato

dalla possibilità di poter ruotare e traslare lungo il suo asse.

La definizione in forma analitica dei vincoli imposti da un giunto cilindrico si ottiene

richiedendo che i versori }{ ih e }{ jh siano collineari. La collinearità è garantita dalla

condizione che }{ ih resti parallelo sia ad }{ jh , sia al vettore }{ ijd . Dove gli estremi di

quest’ultimo sono i punti iP e jP .

Quanto detto si traduce nelle equazioni:

{ } { }( ){ } { }( ){ } { }( )

{ }01

1

01

,, 0

,

i jpi j

i j

f hh h

g h

Ψ Ψ = = Ψ

{ } { }( ){ } { }( ){ } { }( )

{ }02

2

02

,, 0

,

i ijpi ij

i ij

f dh d

g d

Ψ Ψ = = Ψ

Nel caso della coppia cilindrica si introducono 4 equazioni di vincolo scalari. Tale

giunto permette 2 gradi di libertà: una rotazione attorno l’asse del cilindro e una

traslazione lungo lo stesso.

Le prime due equazioni scalari si ottengono imponendo il parallelismo tra gli assi di

rotazione, le atre due si ottengono imponendo il parallelismo tra l’asse di rotazione

e il vettore distanza tra le origini dei riferimenti dei giunti.

Giunto rotoidale

Il secondo giunto da analizzare è quello rotoidale; per tale giunto i vincoli

vengono tradotti da una condizione di vincolo sferico ed una di parallelismo,

quindi da

( ){ } { }( )1

, 0

, 0

si j

pi j

P P

h h

Ψ =

Ψ =

In questo caso, si avranno 5 equazioni scalari di vincolo dettate dalla condizione di

coincidenza dei centri della coppia (3 equazioni scalari) e dalla condizione di

parallelismo tra gli assi di rotazione della coppia, la quale si sdoppia in due

condizioni di ortogonalità (2 equazioni scalari).

Giunto universale

L’ultimo giunto da analizzare è quello universale, esso può essere visto come due

giunti rotoidali posti a 90° l’uno dall’altro, ciò comporta che le condizioni di vincolo a

cui sarà soggetto tale giunto sono di vincolo sferico e di ortogonalità tra l’asse del

primo membro e l’asse del secondo membro connessi al giunto.

( ){ } { }( )01

, 0

, 0

si j

i j

P P

h h

Ψ =

Ψ =

Analisi delle posizioni

Per poter effettuare tale analisi si è proceduto fissando inizialmente un sistema di

riferimento inerziale (O,X,Y,Z), successivamente sono stati introdotti, per ogni

membro, dei sistemi di riferimento solidali ( io , ix , iy , iz ) con origine fissata nel

baricentro. La scelta dei suddetti riferimenti, è stata presa in modo da semplificare il

più possibile le equazioni che dovranno essere calcolate. Volendo portare un

esempio per quanto riguarda la prima gamba poiché il giunto cilindrico in questione

può ruotare e traslare lungo l’asse X del sistema di riferimento inerziale, quindi è

stato assegnato come asse 1y quello parallelo a X. L’asse 1z è stato preso lungo il link,

infine l’asse 1x è stato preso in modo da rendere la terna destrogira. Nel baricentro

del link che collega il giunto rotoidale con quello universale è stato assegnato un

sistema di riferimento analogo al link precedente, solo che è roto-traslato, in modo

che la 4z sia parallela al link 4.Tutto ciò si ripete anche per la seconda e la terza

gamba solo che nella seconda poiché il giunto cilindrico trasla e ruota lungo l’asse Y

verranno tenuti paralleli l’asse 2y conY e 2z con Z .

Al fine di semplificare al massimo le equazioni, la base del robot non è più alla

distanza prefissata nel modello reale bensì all’altezza del vertice della piattaforma,

in modo tale da far coincidere mutuamente il sistema di riferimento di base con

l’asse dei vari giunti, fornendo così grandi semplificazioni. Dal punto di vista della

cinematica tale operazione è più che lecita, non si hanno differenze. Per quanto

OSS:

riguarda la dinamica bisogna introdurre un fattore correttivo che tenga conto del

peso della piattaforma.

Equazioni dei vincoli

Fatto questo breve excursus è ora possibile enunciare le equazione dei vincoli, dei vari giunti, che verranno calcolate con matlab

Prima gamba Giunto cilindrico

1 01 1 1

1 1 01 1

12

1

(0) 0 (1) 01 1 1 2 3 1

({ } ,{ }) {0,1,0} [ ] {0,1,0}({ },{ }) 0

({ } ,{ }) {0,0,1} [ ] {0,1,0}

({ } ,{ })({ },{ }) 0

({ } ,{ })

{ } { } [ ] { } { , , } [ ]

o T Tp

o T T

o Tijp

ij o Tij

Tij

Y y AX y

Z y A

Y dX d

Z d

d r A s q q q A

Ψ ⋅ ⋅Ψ = = = Ψ ⋅ ⋅

ΨΨ = = Ψ

= + = + ⋅

( )( )

[ ]

1

1 01 2 3 1 1

1 01 2 3 1 1

10 1 2 3 4 5 6 70

{0,0, / 2}

({ } ,{ }) {0,1,0} { , , } [ ] {0,0, / 2}

({ } ,{ }) {0,0,1} { , , } [ ] {0,0, / 2}

( , , , ) ( , , , )

T

o T T Tij

o T T Tij

l

Y d q q q A l

Z d q q q A l

A e e e e A q q q q

−

Ψ = ⋅ + ⋅ −

Ψ = ⋅ + ⋅ −

= =

Per il giunto che collega il corpo1 al corpo 4 si ha:

(0) 0 (4) (0) 0 (1)4 4 4 1 1 1

0 022 23 24 4 2 1 2 3 1 1

11 1 4

1 4 11 4

({ },{ }) { } [ ] { } { } [ ] { } 0

({ },{ }) { , , } [ ] {0,0, / 2} { , , } [ ] {0,0, / 2} 0

({ } ,{ }) {1,0,0} [({ },{ })

({ } ,{ })

si j

s T T T Ti j

o T Tp

o T

P P r A s r A s

P P q q q A l q q q A l

x y Ax x

x z

Ψ ≡ + − + =

Ψ = + ⋅ − − − ⋅ − =

Ψ ⋅Ψ = = Ψ

0 01 40 01 4

40 0 1 2 3 25 26 27 28

] [ ] {0,1,0}0

{1,0,0} [ ] [ ] {0,0,1}

[ ] ( , , , ) ( , , , )

T

AA A

A A e e e e A q q q q

⋅ ⋅= ⋅ ⋅ ⋅

= =

Per i corpi 4-7 si ha:

(0) 0 (7) (0) 0 (4)7 7 7 4 4 4

0 043 44 45 7 22 23 24 4 2

1 1 0 04 7 4 7

0

({ },{ }) { } [ ] { } { } [ ] { } 0

({ },{ }) { , , } [ ] {0,0, } { , , } [ ] {0,0, / 2} 0

({ } ,{ }) ({ } ,{ }) {1,0,0} [ ] [ ] {0,1,0}

[ ]

si j

s T T T Ti j

o T o T T Ti j

P P r A s r A s

P P q q q A d q q q A l

h h x y A A

A

Ψ ≡ + − + =

Ψ = + ⋅ − − − ⋅ =

Ψ = Ψ = ⋅ ⋅

70 1 2 3 46 47 48 49( , , , ) ( , , , )A e e e e A q q q q= =

gamba 2

Per la coppia telaio-corpo 2:

1 01 2 2

2 1 02 2

12

1

(0) 0 (2) 02 2 8 9 10 2

({ } ,{ }) {1,0,0} [ ] {0,1,0}({ },{ }) 0

({ } ,{ }) {0,0,1} [ ] {0,1,0}

({ } ,{ })({ },{ }) 0

({ } ,{ })

{ } { } [ ] { } { , , } [ ]

o T Tp

o T T

o Tijp

ij o Tij

Tij

X y AY y

Z y A

X dY d

Z d

d r A s q q q A

Ψ ⋅ ⋅Ψ = = = Ψ ⋅ ⋅

ΨΨ = = Ψ

= + = +

( )( )

1

1 08 9 10 2 1

1 08 9 10 2 1

20 0 1 2 3 11 12 13 14

{0,0, / 2}

({ } ,{ }) {1,0,0} { , , } [ ] {0,0, / 2}

({ } ,{ }) {0,0,1} { , , } [ ] {0,0, / 2}

[ ] ( , , , ) ( , , , )

T

o T T Tij

o T T Tij

l

X d q q q A l

Z d q q q A l

A A e e e e A q q q q

⋅ −

Ψ = ⋅ + ⋅ −

Ψ = ⋅ + ⋅ −

= =

Per il giunto rotoidale tra il corpo 2 e il corpo 5:

(0) 0 (5) (0) 0 (2)5 5 5 2 2 2

0 029 30 31 5 2 8 9 10 2 1

11 2 5

2 5 12 5

({ },{ }) { } [ ] { } { } [ ] { } 0

({ },{ }) { , , } [ ] {0,0, / 2} { , , } [ ] {0,0, / 2} 0

({ } ,{ }) {1,0,0} [({ },{ })

({ } ,{ })

si j

s T T T Ti j

o Tp

o T

P P r A s r A s

P P q q q A l q q q A l

x y Ax x

x z

Ψ ≡ + − + =

Ψ = + ⋅ − − − ⋅ − =

Ψ ⋅Ψ = = Ψ

0 02 50 02 5

50 0 1 2 3 32 33 34 35

] [ ] {0,1,0}0

{1,0,0} [ ] [ ] {0,0,1}

[ ] ( , , , ) ( , , , )

T

T

AA A

A A e e e e A q q q q

⋅ ⋅= ⋅ ⋅ ⋅

= =

Per i corpi 5-7:

(0) 0 (7) (0) 0 (5)7 7 7 5 5 5

0 043 44 45 7 29 30 31 5 2

1 1 0 05 7 5 7

({ },{ }) { } [ ] { } { } [ ] { } 0

({ },{ }) { , , } [ ] {0, ,0} { , , } [ ] {0,0, / 2} 0

({ } ,{ }) ({ } ,{ }) {1,0,0} [ ] [ ] {0,1,0}

si j

s T T T Ti j

o T o T T Ti j

P P r A s r A s

P P q q q A d q q q A l

h h x y A A

Ψ ≡ + − + =

Ψ = + ⋅ − − − ⋅ =

Ψ = Ψ = ⋅ ⋅

gamba 3

Per la coppia telaio-corpo 3:

1 01 3 3

3 1 03 3

12

1

(0) 0 (3)3 3 3 15 16 17

({ } ,{ }) {1,0,0} [ ] {0,1,0}({ },{ }) 0

({ } ,{ }) {0,1,0} [ ] {0,1,0}

({ } ,{ })({ },{ }) 0

({ } ,{ })

{ } { } [ ] { } { , , } [

o T Tp

o T T

o Tijp

ij o Tij

Tij

X y AZ y

Y y A

X dZ d

Y d

d r A s q q q A

Ψ ⋅ ⋅Ψ = = = Ψ ⋅ ⋅

ΨΨ = = Ψ

= + = +

( )( )

03 1

1 015 16 17 3 1

1 015 16 17 3 1

30 0 1 2 3 18 19 20 21

] {0,0, / 2}

({ } ,{ }) {1,0,0} { , , } [ ] {0,0, / 2}

({ } ,{ }) {0,1,0} { , , } [ ] {0,0, / 2}

[ ] ( , , , ) ( , , , )

T

o T T Tij

o T T Tij

l

X d q q q A l

Y d q q q A l

A A e e e e A q q q q

⋅ −

Ψ = ⋅ + ⋅ −

Ψ = ⋅ + ⋅ −

= =

Si noti l’analisi dei giunti rotoidali.

Tra il corpo 3 e il corpo 6:

(0) 0 (6) (0) 0 (3)6 6 6 3 3 3

0 036 37 38 6 2 15 16 17 3 1

11 3 6

3 6 13 6

({ },{ }) { } [ ] { } { } [ ] { } 0

({ },{ }) { , , } [ ] {0,0, / 2} { , , } [ ] {0,0, / 2} 0

({ } ,{ }) {1,0,0}({ },{ })

({ } ,{ })

si j

s T T T Ti j

o Tp

o T

P P r A s r A s

P P q q q A l q q q A l

x yx x

x z

Ψ ≡ + − + =

Ψ = + ⋅ − − − ⋅ − =

Ψ ⋅Ψ = = Ψ

0 03 60 03 6

50 0 1 2 3 39 40 41 42

[ ] [ ] {0,1,0}0

{1,0,0} [ ] [ ] {0,0,1}

[ ] ( , , , ) ( , , , )

T

T

A AA A

A A e e e e A q q q q

⋅ ⋅= ⋅ ⋅ ⋅

= =

Per i corpi 6-7 :

(0) 0 (7) (0) 0 (6)7 7 7 6 6 6

0 043 44 45 7 36 37 38 6 2

1 1 0 06 7 6 7

({ },{ }) { } [ ] { } { } [ ] { } 0

({ },{ }) { , , } [ ] { ,0,0} { , , } [ ] {0,0, / 2} 0

({ } ,{ }) ({ } ,{ }) {1,0,0} [ ] [ ] {0,1,0}

si j

s T T T Ti j

o T o T T Ti j

P P r A s r A s

P P q q q A d q q q A l

h h x y A A

Ψ ≡ + − + =

Ψ = + ⋅ − − − ⋅ =

Ψ = Ψ = ⋅ ⋅

Facendo un conteggio si nota che si sono avute 46 equazioni in 49 incognite,

quindi affinché tale sistema possa essere risolvibile è necessario imporre oltre, ai

suddetti vincoli, anche 3 vincoli reonomi.

La nostra scelta è stata quella di definire come versore nello spazio la trisettrice

7 7 73 3 3ˆ ˆ ˆ ˆ

3 3 3e x y z= − − −

Fissando come legge di moto:

Φ =𝜋𝜋6𝑠𝑠𝑖𝑖𝑖𝑖 �2𝜋𝜋

𝑡𝑡𝑇𝑇𝑓𝑓�

Definiti questi invarianti naturali si può imporre la seguente legge di moto:

46 cos2

q Φ =

483 sin

3 2q

Φ = − ⋅

493 sin

3 2q

Φ = − ⋅

Cosi facendo si ha un sistema di 49 equazioni in 49 incognite ed è ora possibile risolvere tale sistema impiegando il software Matlab.

Analisi dinamica

La fase successiva è quella dell’analisi dinamica, tale analisi, che permette di

dedurre le equazioni del moto che descrivono la dinamica del sistema, è ottenuta

considerando l’ equazione di Lagrange

jj j

d L L Qdt q q ∂ ∂

− = ∂ ∂ (j= 1,…, n) (1)

Dove con L T V= − si definisce la funzione lagrangiana

Tali equazioni permettono di ricondurre il problema prima esposto a quello del

calcolo di un minimo di un funzionale. Poiché però il nostro è un sistema vincolato è

necessario procedere considerando il metodo dei moltiplicatori di Lagrange.Tale

metodo che consente di ricondurre un problema di ottimizzazione vincolata ad uno

di ottimizzazione senza vincoli richiede che sia introdotta un funzione lagragiana

estesa definita come segue

( )*1 1 ... p pL L T Vλ λ λ= − Ψ = − − Ψ + + Ψ

Dove i lambda, chiamati moltiplicatori di Lagrange, sono inizialmente indeterminati e le funzioni 𝚿𝚿 rappresentano le relazioni tra le coordinate generalizzate.

L’applicazione delle equazioni di Lagrange alla funzione (1) porta alla definizione

delle seguenti equazioni del moto:

[ ]{ } { } { }T

q eM q Fλ + Ψ = (2)

dove

[ ]M è la matrice delle masse

{ }eF è il vettore delle forze generalizzate

T

q Ψ è la matrice Jacobiana trasposta associata al sistema delle equazioni

{ } { }0Ψ = di vincolo

dove l’espressione delle forze generalizzate è

1

Nnc k

j kkj j

rWF Fq q

δδδ δ=

= = ⋅∑

Wδ è il lavoro virtuale delle forze esterne e jqδ la variazione virtuale della j-

esima coordinata jq .

Per poter risolvere, a questo punto, il sistema di equazioni composto da da n

equazioni differenziali nelle incognite 1q ,..., nq e 1λ ,…, pλ , sia dal sistema costituito da

equazioni algebriche di vincolo 1 0Ψ = ,…, 0pΨ = , per cui il numero di incognite (n+p)

è pari a quello complessivo delle equazioni.

[ ]{ } { } { }T

q eM q Fλ + Ψ =

{ } 0Ψ =

La soluzione di sistemi costituiti sia da equazioni algebriche che differenziali può

essere eseguita avvalendosi di varie metodologie. Nel caso di sistemi meccanici, la

più semplice, è quella che prevede di differenziare due volte rispetto al tempo le

equazioni di vincolo 0Ψ = ottenendo in notazione matriciale:

{ } { } { } { }0q q γ Ψ ≡ Ψ − =

Quest’ultima e la (2) possono risolversi simultaneamente quando si costruisce il

sistema

,0

Teq

q

FM qλ γ

Ψ = Ψ

il quale, essendo note al tempo t le condizioni di posizione e velocità di ciascuna

massa, presenta come incognite lineari il vettore delle accelerazioni { }q e quello dei

moltiplicatori di Lagrange { }λ .

Per il calcolo dell’equazione di Lagrange è necessario ricavare le energie cinetiche e

potenziali associate al corpo moi del nostro robot.

L’energia cinetica posseduta dal corpo è fornita dall’espressione:

{ } { } { } { }{ }0 0 0 0 01 12 2

T T T

i i i i i i iT m r r Jω ω= +

Dove:

{ } { }0 02i i iE pω =

Sostituendo diventa:

{ } { } { } { } { }

{ } { } { } { } { }

0 0 0 0 0

0 0 0 0

1 22

1 22

T Ti i i i i i i i i

T T T ii i i i i i i i

T m r r p E J E p

m r r p G J G p

= + =

+

In quanto la matrice G per definizione avendo definito E come

[E]=� −𝑒𝑒1 𝑒𝑒0 – 𝑒𝑒3 𝑒𝑒2−𝑒𝑒2 𝑒𝑒3 𝑒𝑒0 −𝑒𝑒1

−𝑒𝑒3 𝑒𝑒2 𝑒𝑒1 𝑒𝑒0

�

Risulta essere

[G]=� −𝑒𝑒1 𝑒𝑒0 𝑒𝑒3 −𝑒𝑒2−𝑒𝑒2 − 𝑒𝑒3 𝑒𝑒0 𝑒𝑒1

−𝑒𝑒3 𝑒𝑒2 − 𝑒𝑒1 𝑒𝑒0

�

Da cui si ottiene

[ ][ ]3 4

0 04 3

0 00 0 00 0

0 4

i

i

i i

T ii i i

mm

M m

G J G

×

×

=

L’energia potenziale posseduta dal corpo si ricava attraverso il vettore delle forze

esterne generalizzate agenti sul corpo moi

{ }{ }

{ }

0

01 2

in i k

i T i iki ik k

A fQ

G s f=

=

∑

3 Analisi del codice Di seguito l’analisi delle funzioni implementate in matlab che hanno permesso la nostra analisi

STUDIO DI POSIZIONE

%ANALISI DELLE POSIZIONI %attraverso questo script verranno generate le equazioni di vincolo (psi) function psi_s=posizioni(q,Tf,t) % dati riguardanti la lunghezza e il diametro dei link l1=0.26; l2=0.38; d=0.23; % al fine di definire le psi si devono, come visto nella teoria, creare le matrici % di rotazione necessarie, per uniformare tutte le grandezze rispetto lo stesso sistema di % riferimento e precisamente quello inerziale (0); quindi dal momento in % cui poichè ogni link\giunto avrà il suo sistema di riferimento dovremo avere 7 matrici di % rotazione % %MATRICI DI ROTAZIONE % %per quanto riguarda la generazione di tali matrici, si è scelto di implemetare una funzione "rot" % che ricevendo in ingresso le 4 coordinate generalizzate, calcola di volta in volta le matrici A1=rot(q(4),q(5),q(6),q(7)); A2=rot(q(11),q(12),q(13),q(14)); A3=rot(q(18),q(19),q(20),q(21)); A4=rot(q(25),q(26),q(27),q(28)); A5=rot(q(32),q(33),q(34),q(35)); A6=rot(q(39),q(40),q(41),q(42)); A7=rot(q(46),q(47),q(48),q(49)); % il passo successivo è quello del calcolo delle psi siano esse dovute a % delle coppie cilindriche (psic), rotoidali (psir) o universali (psiu) e % ciò si traduce nell'applicare per ogni gamba i vincoli base già % incontrati in teoria % PRIMA GAMBA % dopo aver stabilito le posizioni dei rispettivi sistemi di orientamento % possiamo deninire i primi due vincoli della coppia cilindrica ossia i due % vincoli di parallelismo

% coppia cilindrica % primo vincolo di parallelismo psic1=[0,1,0]*A1*[0;1;0]; psic2=[0,0,1]*A1*[0;1;0]; % secondo vincolo di parallelismo % per qusto secondo vincolo definiamo prima il vettore dij dij1=[q(1);q(2);q(3)]+A1*[0;0;-l1/2]; psic3=[0,1,0]*dij1; psic4=[0,0,1]*dij1; % COPPPIA ROTOIDALE psir1= [1,0,0]*A1'*A4*[0;1;0]; psir2= [1,0,0]*A1'*A4*[0;0;1]; psir3=[q(22);q(23);q(24)]+A4*[0;0;-l2/2]-[q(1);q(2);q(3)]-A1*[0;0;l1/2]; %coppia uiniversale psiu1=[1, 0, 0]*A4'*A7*[0;1;0]; psiu2=[q(43);q(44);q(45)]+A7*[0;-d;0]-[q(22);q(23);q(24)]-A4*[0;0;l2/2]; %SECONDA GAMBA % coppia cilindrica psic5=[1,0,0]*A2*[0;1;0]; psic6=[0,0,1]*A2*[0;1;0]; dij2=[q(8);q(9);q(10)]+A2*[0;0;-l1/2]; psic7=[1,0,0]*dij2; psic8=[0,0,1]*dij2; %coppia rotoidale psir4= [1,0,0]*A2'*A5*[0;1;0]; psir5= [1,0,0]*A2'*A5*[0;0;1]; psir6=[q(29);q(30);q(31)]+A5*[0;0;-l2/2]-[q(8);q(9);q(10)]-A2*[0;0;l1/2]; %coppia uiniversale psiu3=[1, 0, 0]*A5'*A7*[0;0;1]; psiu4=[q(43);q(44);q(45)]+A7*[0;0;-d]-[q(29);q(30);q(31)]-A5*[0;0;l2/2]; % TERZA GAMBA %coppia cilindrica psic9=[1,0,0]*A3*[0;1;0]; psic10=[0,1,0]*A3*[0;1;0]; dij3=[q(15);q(16);q(17)]+A3*[0;0;-l1/2]; psic11=[1,0,0]*dij3; psic12=[0,1,0]*dij3; %coppia rotoidale psir7= [1,0,0]*A3'*A6*[0;1;0]; psir8= [1,0,0]*A3'*A6*[0;0;1]; psir9=[q(36);q(37);q(38)]+A6*[0;0;-l2/2]-[q(15);q(16);q(17)]-A3*[0;0;l1/2]; % coppia uiniversale

psiu5=[1, 0, 0]*A6'*A7*[1;0;0]; psiu6=[q(43);q(44);q(45)]+A7*[-d;0;0]-[q(36);q(37);q(38)]-A6*[0;0;l2/2]; % dopo aver esaminato tutti i vincoli presenti % mettiamo ora le condizioni di eulero e cioè che r^2+r0^2=1 psiE1 = q(4)^2 + q(5)^2 + q(6)^2 + q(7)^2 - 1; psiE2 = q(11)^2 + q(12)^2 + q(13)^2 + q(14)^2 - 1; psiE3 = q(18)^2 + q(19)^2 + q(20)^2 + q(21)^2 - 1; psiE4 = q(25)^2 + q(26)^2 + q(27)^2 + q(28)^2 - 1; psiE5 = q(32)^2 + q(33)^2 + q(34)^2 + q(35)^2 - 1; psiE6 = q(39)^2 + q(40)^2 + q(41)^2 + q(42)^2 - 1; psiE7 = q(46)^2 + q(47)^2 + q(48)^2 + q(49)^2 - 1; % Vincoli reonomi % per quanto riguarda questi vincoli si è scelto di lavorare inizialmente con gli invarianti naturali assegnando % cosi come versore per la base la trisettrice e come angolo di rotazione % phi; fatto ciò si è poi trasformato il tutto in una notazione che % compatibile con gli angoli di Eulero; trovando così prima e0,e1,e2 e % successivamente i vincoli reonomi phi=(pi/6)*(sin(2*pi*t/Tf)); e_=-(sqrt(3)/3)*[1,1,1]; e0=cos(phi/2); e1=e_(1)*sin(phi/2); e2=e_(1)*sin(phi/2); psiReo1=q(46)-e0; psiReo2=q(48)-e1; psiReo3=q(49)-e2; psi_s=[psic1;psic2;psic3;psic4;psic5;psic6;psic7;psic8;psic9;psic10;psic11;psic12;psir1;psir2;psir3;psir4;psir5;psir6;psir7;psir8;psir9;psiu1;psiu2;psiu3;psiu4;psiu5;psiu6;psiE1;psiE2;psiE3;psiE4;psiE5;psiE6;psiE7;psiReo1;psiReo2;psiReo3];

STUDIO DI VELOCITA’ E ACCELERAZIONE %STUDIO DI VELOCITA' E ACCELERAZIONI function [qvnum,qanum]=Analisi_Delle_Velocita_e_accelerazioni(qpnum,t,psi)

% prima di iniziare questo studio è necessario definire le variabili simboliche delle grandezze interezzate ovvero le posizioni e le velocità syms time syms q1 q2 q3 q4 q5 q6 q7 q8 q9 q10 q11 q12 q13 q14 q15 q16 q17 q18 q19 q20 q21 q22 q23 q24 q25 q26 q27 q28 q29 q30 q31 q32 q33 q34 q35 q36 q37 q38 q39 q40 q41 q42 q43 q44 q45 q46 q47 q48 q49 real syms q1v q2v q3v q4v q5v q6v q7v q8v q9v q10v q11v q12v q13v q14v q15v q16v q17v q18v q19v q20v q21v q22v q23v q24v q25v q26v q27v q28v q29v q30v q31v q32v q33v q34v q35v q36v q37v q38v q39v q40v q41v q42v q43v q44v q45v q46v q47v q48v q49v real % definite le variabili simboliche creiamo le matrici qp=[q1;q2;q3;q4;q5;q6;q7;q8;q9;q10;q11;q12;q13;q14;q15;q16;q17;q18;q19;q20;q21;q22;q23;q24;q25;q26;q27;q28;q29;q30;q31;q32;q33;q34;q35;q36;q37;q38;q39;q40;q41;q42;q43;q44;q45;q46;q47;q48;q49]; qv=[q1v;q2v;q3v;q4v;q5v;q6v;q7v;q8v;q9v;q10v;q11v;q12v;q13v;q14v;q15v;q16v;q17v;q18v;q19v;q20v;q21v;q22v;q23v;q24v;q25v;q26v;q27v;q28v;q29v;q30v;q31v;q32v;q33v;q34v;q35v;q36v;q37v;q38v;q39v;q40v;q41v;q42v;q43v;q44v;q45v;q46v;q47v;q48v;q49v]; % ricordando la teoria per poter iniziare qeusta analisi dobbiamo prima calcolare lo jacobiano psi(q) psiq=jacobian(psi,qp); % differenziamo ora le psi rispetto al tempo psit= diff(psi,time); %poichè non conviene lavorare con le variabili simboliche le convertiamo in %numeriche con il comando subs psiqnum1=subs(psiq,qp,qpnum); psiqnum=subs(psiqnum1,time,t); psitnum1=subs(psit,qp,qpnum); psitnum=subs(psitnum1,time,t); qvnum=-inv(psiqnum)*psitnum; %calcolo accelerazioni X=psiq*qv; psitt=diff(psit,time); Xd=jacobian(X,qp); %convertiamo anche queste variabili psittnum1=subs(psitt,qp,qpnum); psittnum=subs(psittnum1,time,t); Xdnum1=subs(Xd,qp,qpnum); Xdnum2=subs(Xdnum1,qv,qvnum); Xdnum=subs(Xdnum2,time,t); %calcolo di gamma gammanum=-(Xdnum*qvnum)-(psittnum); %accelerazioni qanum=inv(psiqnum)*gammanum;

DINAMICA

%DINAMICA function Din= Dinamica(psi) % lo studio della dinamica ricordando la teoria, viene eseguito attraverso % l'uso della lagrangiana particolare in quanto il nostro è un sistema vincolato quindi % aggiungeremo alla lista delle variabili simboliche anche i lambda % necessari per portare avanti questo studio syms time syms q1 q2 q3 q4 q5 q6 q7 q8 q9 q10 q11 q12 q13 q14 q15 q16 q17 q18 q19 q20 q21 q22 q23 q24 q25 q26 q27 q28 q29 q30 q31 q32 q33 q34 q35 q36 q37 q38 q39 q40 q41 q42 q43 q44 q45 q46 q47 q48 q49 real syms q1v q2v q3v q4v q5v q6v q7v q8v q9v q10v q11v q12v q13v q14v q15v q16v q17v q18v q19v q20v q21v q22v q23v q24v q25v q26v q27v q28v q29v q30v q31v q32v q33v q34v q35v q36v q37v q38v q39v q40v q41v q42v q43v q44v q45v q46v q47v q48v q49v real syms q1a q2a q3a q4a q5a q6a q7a q8a q9a q10a q11a q12a q13a q14a q15a q16a q17a q18a q19a q20a q21a q22a q23a q24a q25a q26a q27a q28a q29a q30a q31a q32a q33a q34a q35a q36a q37a q38a q39a q40a q41a q42a q43a q44a q45a q46a q47a q48a q49a real syms lambda1 lambda2 lambda3 lambda4 lambda5 lambda6 lambda7 lambda8 lambda9 lambda10 lambda11 lambda12 lambda13 lambda14 lambda15 lambda16 lambda17 lambda18 lambda19 lambda20 lambda21 lambda22 lambda23 lambda24 lambda25 lambda26 lambda27 lambda28 lambda29 lambda30 lambda31 lambda32 lambda33 lambda34 lambda35 lambda36 lambda37 lambda38 lambda39 lambda40 lambda41 lambda42 lambda43 lambda44 lambda45 lambda46 lambda47 lambda48 lambda49 real syms dq1 dq2 dq3 dq4 dq5 dq6 dq7 dq8 dq9 dq10 dq11 dq12 dq13 dq14 dq15 dq16 dq17 dq18 dq19 dq20 dq21 dq22 dq23 dq24 dq25 dq26 dq27 dq28 dq29 dq30 dq31 dq32 dq33 dq34 dq35 dq36 dq37 dq38 dq39 dq40 dq41 dq42 dq43 dq44 dq45 dq46 dq47 dq48 dq49 real qp=[q1;q2;q3;q4;q5;q6;q7;q8;q9;q10;q11;q12;q13;q14;q15;q16;q17;q18;q19;q20;q21;q22;q23;q24;q25;q26;q27;q28;q29;q30;q31;q32;q33;q34;q35;q36;q37;q38;q39;q40;q41;q42;q43;q44;q45;q46;q47;q48;q49]; qv=[q1v;q2v;q3v;q4v;q5v;q6v;q7v;q8v;q9v;q10v;q11v;q12v;q13v;q14v;q15v;q16v;q17v;q18v;q19v;q20v;q21v;q22v;q23v;q24v;q25v;q26v;q27v;q28v;q29v;q30v;q31v;q32v;q33v;q34v;q35v;q36v;q37v;q38v;q39v;q40v;q41v;q42v;q43v;q44v;q45v;q46v;q47v;q48v;q49v]; dqp=[dq1;dq2;dq3;dq4;dq5;dq6;dq7;dq8;dq9;dq10;dq11;dq12;dq13;dq14;dq15;dq16;dq17;dq18;dq19;dq20;dq21;dq22;dq23;dq24;dq25;dq26;dq27;dq28;dq29;dq30;dq31;dq32;dq33;dq34;dq35;dq36;dq37;dq38;dq39;dq40;dq41;dq42;dq43;dq44;dq45;dq46;dq47;dq48;dq49]; lambda=[lambda1 lambda2 lambda3 lambda4 lambda5 lambda6 lambda7 lambda8 lambda9 lambda10 lambda11 lambda12 lambda13 lambda14 lambda15 lambda16 lambda17 lambda18 lambda19 lambda20 lambda21 lambda22 lambda23 lambda24 lambda25 lambda26 lambda27 lambda28 lambda29 lambda30 lambda31 lambda32 lambda33 lambda34 lambda35 lambda36 lambda37 lambda38 lambda39 lambda40 lambda41 lambda42 lambda43 lambda44 lambda45 lambda46 lambda47 lambda48 lambda49];

qa=[q1a;q2a;q3a;q4a;q5a;q6a;q7a;q8a;q9a;q10a;q11a; q12a; q13a; q14a; q15a; q16a; q17a; q18a; q19a; q20a; q21a; q22a; q23a; q24a; q25a; q26a; q27a; q28a; q29a; q30a; q31a; q32a; q33a; q34a; q35a; q36a; q37a; q38a; q39a; q40a; q41a; q42a; q43a; q44a; q45a; q46a; q47a; q48a; q49a]; %dati riguardanti le dimensioni dei link l1=0.26; l2=0.38; gr=9.81*[sqrt(3)/3; sqrt(3)/3; sqrt(3)/3]; %caratteristiche inerziali m1=0.2205; m2=0.2205; m3=0.2205;%masse bracci base, M=V*D=pi*r^2*h*d m4=0.3223; m5=0.3223; m6=0.3223;%masse bracci alti, M=V*D=pi*r^2*h*d m7=0.687; %prima di passare al calcolo delle energie cinetiche (T) %calcoliamo il momento d'inerzia J J1=[m1/12*l1^2,0,0;0,m1/12*l1^2,0;0,0,0]; %J link della base J2=[m2/12*l2^2,0,0;0,0,0;0,0,m2/12*l2^2]; %J dei link superiori J3=[1/16*m7*0.346^2,0,0;0,1/6*m7*0.346^2,0;0,0,1/3*m7*0.346^2];%J della base %fatto questo possiamo calcolarci le T che sono la somma di energia %cinetica traslazionale e rotazionale % T link base T1=1/2*m1*[q1v,q2v,q3v]*[q1v;q2v;q3v] +2*[q4v,q5v,q6v,q7v]*G(q4,q5,q6,q7)'* J1 *G(q4,q5,q6,q7)*[q4v;q5v;q6v;q7v]; T2=1/2*m2*[q8v,q9v,q10v]*[q8v;q9v;q10v] +2*[q11v,q12,q13v,q14v]*G(q11,q12,q13,q14)'* J1 *G(q11,q12,q13,q14)*[q11v;q12v;q13v;q14v]; T3=1/2*m3*[q15v,q16v,q17v]*[q15v;q16v;q17v] +2*[q18v,q19v,q20v,q21v]*G(q18,q19,q20,q21)'* J1 *G(q18,q19,q20,q21)*[q18v;q19v;q20v;q21v]; %T link alti T4=1/2*m4*[q22v,q23v,q24v]*[q22v;q23v;q24v] +2*[q25v,q26v,q27v,q28v]*G(q25,q26,q27,q28)'* J2 *G(q25,q26,q27,q28)*[q25v;q26v;q27v;q28v]; T5=1/2*m5*[q29v,q30v,q31v]*[q29v;q30v;q31v] +2*[q32v,q33v,q34v,q35v]*G(q32,q33,q34,q35)'* J2 *G(q32,q33,q34,q35)*[q32v;q33v;q34v;q35v]; T6=1/2*m6*[q36v,q37v,q38v]*[q36v;q37v;q38v] +2*[q39v,q40v,q41v,q42v]*G(q39,q40,q41,q42)'* J2 *G(q39,q40,q41,q42)*[q39v;q40v;q41v;q42v]; %T piattaforma T7=1/2*m7*[q43v,q44v,q45v]*[q43v;q44v;q45v] +2*[q46v,q47v,q48v,q49v]*G(q46,q47,q48,q49)'* J3 *G(q46,q47,q48,q49)*[q46v;q47v;q48v;q49v];

T=T1+T2+T3+T4+T5+T6+T7; %Enegia potenziale dV1=m1*gr'*[q1; q2; q3]; dV2=m2*gr'*[q8; q9; q10]; dV3=m3*gr'*[q15; q16; q17]; dV4=m4*gr'*[q22; q23; q24]; dV5=m5*gr'*[q29; q30; q31]; dV6=m6*gr'*[q36; q37; q38]; %Nella piattaforma le distanza tra baricentro e sistema di rif.non è nulla %quindi dV7=-m7*gr'* 2*[0, 1/sqrt(3)*0.19, -1/sqrt(3)*0.19; -1/sqrt(3)*0.19, 0 ,1/sqrt(3)*0.19; 1/sqrt(3)*0.19, -1/sqrt(3)*0.19, 0]*G(q46,q47,q48,q49)*[q46;q47;q48;q49]; dV=dV1+dV2+dV3+dV4+dV5+dV6+dV7; %si può ora costruire la Lagrangiana particolare (Lst)sottraendo a quella %normale il termine in cui sono presenti i moltiplicatori ovvero Lpsi %Calcolo delle forze generalizzate dW1=[lambda(46);0;0]'*[dq1; dq2; dq3] -[lambda(46);0;0]'* 2*[0,-l1/2,0; l1/2,0,0; 0,0,0]*G(q4,q5,q6,q7)*[dq4;dq5;dq6;dq7]; dW2=[0;lambda(48);0]'*[dq8; dq9; dq10] -[0;lambda(48);0]'* 2*[0,-l1/2,0; l1/2,0,0; 0,0,0]*G(q11,q12,q13,q14)*[dq11;dq12;dq13;dq14]; dW3=[0;0;lambda(49)]'*[dq15; dq16; dq17] -[0;0;lambda(49)]'* 2*[0,-l1/2,0; l1/2,0,0; 0,0,0]*G(q18,q19,q20,q21)*[dq18;dq19;dq20;dq21]; dW=dW1+dW2+dW3; %Calcolo della Lagrangiana L=T-dV; Lpsi=lambda(1)*psi(1)+lambda(2)*psi(2)+lambda(3)*psi(3)+lambda(4)*psi(4)+lambda(5)*psi(5)+lambda(6)*psi(6)+lambda(7)*psi(7)+lambda(8)*psi(8)+lambda(9)*psi(9)+lambda(10)*psi(10)+lambda(11)*psi(11)+lambda(12)*psi(12)+lambda(13)*psi(13)+lambda(14)*psi(14)+lambda(15)*psi(15)+lambda(16)*psi(16)+lambda(17)*psi(17)+lambda(18)*psi(18)+lambda(19)*psi(19)+lambda(20)*psi(20)+lambda(21)*psi(21)+lambda(22)*psi(22)+lambda(23)*psi(23)+lambda(24)*psi(24)+lambda(25)*psi(25)+lambda(26)*psi(26)+lambda(27)*psi(27)+lambda(28)*psi(28)+lambda(29)*psi(29)+lambda(30)*psi(30)+lambda(31)*psi(31)+lambda(32)*psi(32)+lambda(33)*psi(33)+lambda(34)*psi(34)+lambda(35)*psi(35)+lambda(36)*psi(36)+lambda(37)*psi(37)+lambda(38)*psi(38)+lambda(39)*psi(39)+lambda(40)*psi(40)+lambda(41)*psi(41)+lambda(42)*psi(42)+lambda(43)*psi(43)+lambda(44)*psi(44)+lambda(45)*psi(45)+lambda(47)*psi(47); %Lagrangiana particolare Lst=L-Lpsi; for (j=1:49) LEp(j,1)=diff(Lst,qp(j)); % Derivata della Lagrangiana rispetto alle posizioni LEv(j,1)=diff(Lst,qv(j)); % Derivata della Lagrangiana rispetto alle velocità FE(j,1)=diff(dW,dqp(j)); % Derivata delle forze generalizzate rispetto alle posizioni

end LEa=subs(LEv,qv,qa); Din =LEa-LEp-FE;

%questa funzione è stata implemetata per ragioni prettamente pratiche in %quanto come si può notare nel main è utile definire un'altra funzione per %applicare l'fsolve function eq_din=Equ_din(lam,Din_n) syms lambda1 lambda2 lambda3 lambda4 lambda5 lambda6 lambda7 lambda8 lambda9 lambda10 lambda11 lambda12 lambda13 lambda14 real syms lambda15 lambda16 lambda17 lambda18 lambda19 lambda20 lambda21 lambda22 lambda23 lambda24 lambda25 lambda26 lambda27 real syms lambda28 lambda29 lambda30 lambda31 lambda32 lambda33 lambda34 lambda35 lambda36 lambda37 lambda38 lambda39 lambda40 real syms lambda41 lambda42 lambda43 lambda44 lambda45 lambda46 lambda47 lambda48 lambda49 real lambda=[lambda1 lambda2 lambda3 lambda4 lambda5 lambda6 lambda7 lambda8 lambda9 lambda10 lambda11 lambda12 lambda13 lambda14 lambda15 lambda16 lambda17 lambda18 lambda19 lambda20 lambda21 lambda22 lambda23 lambda24 lambda25 lambda26 lambda27 lambda28 lambda29 lambda30 lambda31 lambda32 lambda33 lambda34 lambda35 lambda36 lambda37 lambda38 lambda39 lambda40 lambda41 lambda42 lambda43 lambda44 lambda45 lambda46 lambda47 lambda48 lambda49]; lam=[lam(1) lam(2) lam(3) lam(4) lam(5) lam(6) lam(7) lam(8) lam(9) lam(10) lam(11) lam(12) lam(13) lam(14) lam(15) lam(16) lam(17) lam(18) lam(19) lam(20) lam(21) lam(22) lam(23) lam(24) lam(25) lam(26) lam(27) lam(28) lam(29) lam(30) lam(31) lam(32) lam(33) lam(34) lam(35) lam(36) lam(37) lam(38) lam(39) lam(40) lam(41) lam(42) lam(43) lam(44) lam(45) lam(46) lam(47) lam(48) lam(49) ]; eq_din=subs(Din_n,lambda,lam);

MAIN

%main %dimensioni dei link clc clear all d=0.23; l1=0.26; l2=0.38; %copminciamo con il definire sottoforma di variabili simboliche le 49 %coordinate generalizzate più il tempo syms time

syms q1 q2 q3 q4 q5 q6 q7 q8 q9 q10 q11 q12 q13 q14 q15 q16 q17 q18 q19 q20 q21 q22 q23 q24 q25 q26 q27 q28 q29 q30 q31 q32 q33 q34 q35 q36 q37 q38 q39 q40 q41 q42 q43 q44 q45 q46 q47 q48 q49 real syms q1v q2v q3v q4v q5v q6v q7v q8v q9v q10v q11v q12v q13v q14v q15v q16v q17v q18v q19v q20v q21v q22v q23v q24v q25v q26v q27v q28v q29v q30v q31v q32v q33v q34v q35v q36v q37v q38v q39v q40v q41v q42v q43v q44v q45v q46v q47v q48v q49v real syms q1a q2a q3a q4a q5a q6a q7a q8a q9a q10a q11a q12a q13a q14a q15a q16a q17a q18a q19a q20a q21a q22a q23a q24a q25a q26a q27a q28a q29a q30a q31a q32a q33a q34a q35a q36a q37a q38a q39a q40a q41a q42a q43a q44a q45a q46a q47a q48a q49a real %coordinate lagrangiane qp=[q1;q2;q3;q4;q5;q6;q7;q8;q9;q10;q11;q12;q13;q14;q15;q16;q17;q18;q19;q20;q21;q22;q23;q24;q25;q26;q27;q28;q29;q30;q31;q32;q33;q34;q35;q36;q37;q38;q39;q40;q41;q42;q43;q44;q45;q46;q47;q48;q49]; qv=[q1v;q2v;q3v;q4v;q5v;q6v;q7v;q8v;q9v;q10v;q11v;q12v;q13v;q14v;q15v;q16v;q17v;q18v;q19v;q20v;q21v;q22v;q23v;q24v;q25v;q26v;q27v;q28v;q29v;q30v;q31v;q32v;q33v;q34v;q35v;q36v;q37v;q38v;q39v;q40v;q41v;q42v;q43v;q44v;q45v;q46v;q47v;q48v;q49v]; qa=[q1a;q2a;q3a;q4a;q5a;q6a;q7a;q8a;q9a;q10a;q11a; q12a; q13a; q14a; q15a; q16a; q17a; q18a; q19a; q20a; q21a; q22a; q23a; q24a; q25a; q26a; q27a; q28a; q29a; q30a; q31a; q32a; q33a; q34a; q35a; q36a; q37a; q38a; q39a; q40a; q41a; q42a; q43a; q44a; q45a; q46a; q47a; q48a; q49a]; %dopo aver creato e definito le variabili generalizzate passiamo %all'inizializzazione di quest'ultime %Inizializzazione delle posizioni q0(1)=0.3533; q0(2)=-0.1300; q0(3)=-0.0001; q0(4)=-0.4998; q0(5)=-0.5002; q0(6)=0.5002; q0(7)=-0.4998; q0(8)=-0.0000; q0(9)=0.3534; q0(10)=-0.1300; q0(11)=-0.0000; q0(12)=0.0000; q0(13)=1.0000; q0(14)=-0.0000; q0(15)= -0.1300; q0(16)= -0.0000; q0(17)= 0.3533; q0(18)=-0.5000; q0(19)=-0.5000; q0(20)=0.5000; q0(21)=0.5000; q0(22)=0.1766; q0(23)=-0.1900; q0(24)=-0.0001; q0(25)=-0.6946; q0(26)=0.1324; q0(27)=0.6945; q0(28)=0.1328; q0(29)=-0.0000; q0(30)=0.1767; q0(31)=-0.1900; q0(32)=-0.8272; q0(33)=-0.5620; q0(34)=-0.0000; q0(35)=0.0000; q0(36)=-0.1900; q0(37)=-0.0000; q0(38)=0.1766; q0(39)=-0.1326; q0(40)=-0.6946; q0(41)=-0.6946; q0(42)=-0.1326; q0(43)=-0.0000; q0(44)=-0.0000; q0(45)=-0.0000; q0(46)= 1.0000; q0(47)=0.0003; q0(48)=0; q0(49)=0; lambda0=zeros(49,1); % possiamo ora effettuare l'analisi delle posizioni attraverso la funzione % posizioni usando il comando Tf=4; p=0.05; options=optimset('Display','off'); k=1; psis=posizioni(qp,Tf,time); for t=0.05:p:(Tf/2)-0.05 %Calcolo delle posizioni con fsolve q=fsolve(@posizioni,q0,options,Tf,t);

P(:,k)=q; qp_n=q; %calcolo delle velocità e delle accelerazioni [qv_n,qa_n]=Analisi_Delle_Velocita_e_accelerazioni(qp_n,t,psis); V(:,k)=double(qv_n); Acc(:,k)=double(qa_n); %Calcolo della dinamica Din=Dinamica(psis); Din_n=subs(Din ,[qp;qv;qa],[P(:,k);V(:,k);Acc(:,k)]); lambda_n=fsolve(@Equ_din,lambda0,options,Din_n); L_n(:,k)=double(lambda_n); %plottaggio del 3cru A1=rot(q(4),q(5),q(6),q(7)); A2=rot(q(11),q(12),q(13),q(14)); A3=rot(q(18),q(19),q(20),q(21)); A4=rot(q(25),q(26),q(27),q(28)); A5=rot(q(32),q(33),q(34),q(35)); A6=rot(q(39),q(40),q(41),q(42)); A7=rot(q(46),q(47),q(48),q(49)); Cx=[P(1,k);P(2,k);P(3,k)]+A1*[0;0;-l1/2]; Rx=[P(22,k);P(23,k);P(24,k)]+A4*[0;0;-l2/2]; Ux=[P(43,k);P(44,k);P(45,k)]+A7*[0;-d;0]; Cy=[P(8,k);P(9,k);P(10,k)]+A2*[0;0;-l1/2]; Ry=[P(29,k);P(30,k);P(31,k)]+A5*[0;0;-l2/2]; Uy=[P(43,k);P(44,k);P(45,k)]+A7*[0;0;-d]; Cz=[P(15,k);P(16,k);P(17,k)]+A3*[0;0;-l1/2]; Rz=[P(36,k);P(37,k);P(38,k)]+A6*[0;0;-l2/2]; Uz=[P(43,k);P(44,k);P(45,k)]+A7*[-d;0;0]; plot3([1,0,0],[0,0,0],[0,0,0],'green',[0,0,0],[0,1,0],[0,0,0],'green',[0,0,0],[0,0,0],[0,0,1],'green'); hold on text(1,0,0,'X'); text(0,1,0,'Y'); text(0,0,1,'Z'); gambaX=[Cx(1),Rx(1),Ux(1);Cx(2),Rx(2),Ux(2);Cx(3),Rx(3),Ux(3)]; plot3(gambaX(1,:),gambaX(2,:),gambaX(3,:)); gambaY=[Cy(1),Ry(1),Uy(1);Cy(2),Ry(2),Uy(2);Cy(3),Ry(3),Uy(3)]; plot3(gambaY(1,:),gambaY(2,:),gambaY(3,:)); gambaZ=[Cz(1),Rz(1),Uz(1);Cz(2),Rz(2),Uz(2);Cz(3),Rz(3),Uz(3)]; plot3(gambaZ(1,:),gambaZ(2,:),gambaZ(3,:)); piattaforma=[Ux(1),Uy(1),Uz(1),Ux(1);Ux(2),Uy(2),Uz(2),Ux(2);Ux(3),Uy(3),Uz(3),Ux(3)]; plot3(piattaforma(1,:),piattaforma(2,:),piattaforma(3,:),'red'); q0=q; lambda0=lambda_n; k=k+1; end for k=1:1:49

figure('Name',['Andamento temporale delle q,qp,qpp ' int2str(k)],'NumberTitle','off'); for kk=((k-1)*3+1):1:((k-1)*3+1)+2 subplot(3,1,1); plot(P(k,:)), title(['Posizione: q' int2str(k)]); axis auto; subplot(3,1,2); plot(V(k,:)),title(['Velocità: qp' int2str(k)]); axis auto; subplot(3,1,3); plot(Acc(k,:)), title(['Accelerazione: qpp' int2str(k)]) axis auto; end end for k=1:1:15 figure('Name',['Andamento temporale dei lambda(forze)'],'NumberTitle','off'); for kk=((k-1)*3+1):1:((k-1)*3+1)+2 subplot(3,1,(kk-3*(k-1))); plot(L_n(kk,:)),xlabel('Tempo [s]'),ylabel('lambda'); axis auto; title(['lambda: ' int2str(kk)]); end end figure('Name',['Andamento andamento delle coppie T46,T48 e T49'],'NumberTitle','off'); subplot(3,1,1); plot(L_n(46,:)),title(['coppia: T46']),xlabel('Tempo [s]'),ylabel('Coppia [Nm]'),grid; axis auto; subplot(3,1,2); plot(L_n(48,:)),title(['coppia: T48']),xlabel('Tempo [s]'),ylabel('Coppia[Nm]'),grid; axis auto; subplot(3,1,3); plot(L_n(49,:)),title(['coppia: T49']),xlabel('Tempo [s]'),ylabel('Coppia[Nm]'),grid; axis auto;

Rot function matrice_rot = rot(q1, q2, q3, q4) matrice_rot = 2*[(q1^2)+(q2^2)-(1/2) (q2*q3)-(q1*q4) (q2*q4)+(q1*q3) ; (q2*q3)+(q1*q4) (q1^2)+(q3^2)-(1/2) (q3*q4)-(q1*q2) ; (q2*q4)-(q1*q3) (q3*q4)+(q1*q2) (q1^2)+(q4^2)-(1/2)];

G function out = G(a,b,c,d) out=[-b,a,d,-c; -c,-d,a,b; -d,c,-b,a];

4 Risultati simulazione

POSIZIONI-VELOCITA’-ACCELERAZIONI

LAMBDA

COPPIE