relazione meccanica4
DESCRIPTION
Modellistica e Controllo dei Sistemi Meccanici Anno 2008/09 Corso di Laurea in Ingegneria dell’ Automazione e Controllo dei sistemi complessi Di Mauro Gianluca Patti Giuseppe Ing. A. Cammarata Prof. Ing. R. Sinatra 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 3CRUTRANSCRIPT
[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