UNVERSITA’ DEGLI STUDI DI CATANIA [Anno]
Corso di Laurea in Ingegneria dell’ Automazione e Controllo dei sistemi complessi
Modellistica e Controllo dei Sistemi Meccanici Anno 2008/09
Prof. Ing. R. Sinatra
Di Mauro Gianluca Patti Giuseppe
Ing. A. Cammarata
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=λ ⋅ (l −
j
j − 1) + ∑ f i i =1
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 f i è 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 Ψ s  Ψ ≥ { }  d  ={0} Ψ 
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  {q} =− {Ψ t }
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 ∂q 1 Ψ q = ∂Ψ m ∂q1
∂Ψ1 ∂q2
∂Ψ m ∂qm
∂Ψ1 ∂t {Ψ t } = ∂Ψ m ∂t
Calcolati questi due termini, si può ricavare :
−1
{q} =− Ψ q {Ψ t }
Per l’analisi alle accelerazioni si considera la seguente formula:
Ψ q {q} = {γ }
Dove:
{γ } =− ( Ψ q {q})q {q} − 2 Ψ qt {q} − [ Ψ tt ]
Ricavando così il vettore delle accelerazioni:
{q} =
Ψ q
−1
{γ }
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: • uˆ = {u1 , u2 , u3 }T 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è: ψ
ψ
= e0 cos = ; e1 u1 sin 2 2
ψ
ψ
e2 u= ; e3 u3 sin = 2 sin 2 2
I quali devono soddisfare la condizione di normalizzazione: e02 + e12 + e22 + e32 = 1
A questo punto la matrice di rotazione per noi indispensabile per eseguire i nostri calcoli assume la forma :
e02 + e12 − 12  |A|=2  e1e2 + e0 e3  e1e3 − e0 e2 
e1e2 −e 0 e3 e02 + e22 − 12 e2 e3 + e0 e1
e1e3 + e0 e2   e2 e3 − e0 e1  e02 + e32 − 12 
OSS: Per
ogni
corpo
si
sono
assegnate
sette
coordinante
generalizzate:
q i = {r x ,r y ,r z ,e0 ,e1 ,e 2 ,e3 }T
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 Pik , Qik ed Rik costituiscono i punti di definizione del giunto. La sua posizione angolare, inoltre,
è definita dalle componenti dei versori { f ik } , {g ik } ed {hik } , ottenuti dalle relazioni
{ f ik } = Rik − Pik , {hik } = Qik − Pik ,
~ {g ik } = [hik ]{ f ik } .
Fig 2
Primo vincolo di ortogonalitĂ 0
Tale condizione, considerando due vettori {���� }0 e ����� � ,solidali rispettivamente ai corpi i e j, viene espressa dalla relazione: 0
0
đ?›šđ?›š đ?‘œđ?‘œ1 ďż˝{đ?‘Žđ?‘Žđ?‘–đ?‘– }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 0
0
đ?›šđ?›š đ?‘œđ?‘œ2 ďż˝{đ?‘Žđ?‘Žđ?‘–đ?‘– }0 , ďż˝đ?‘‘đ?‘‘đ?‘–đ?‘–đ?‘–đ?‘– ďż˝ ďż˝ ≥ {đ?‘Žđ?‘Žđ?‘–đ?‘– đ?‘‡đ?‘‡ }0 ďż˝đ?‘‘đ?‘‘đ?‘–đ?‘–đ?‘–đ?‘– ďż˝ = 0
Vincolo sferico AffinchĂŠ due punti Pi e Pj appartenenti rispettivamente ai corpi i e j siano sovrapposti deve valere che
0
đ?‘—đ?‘—
đ?›šđ?›š đ?‘ đ?‘ ďż˝đ?‘ƒđ?‘ƒđ?‘–đ?‘– , đ?‘ƒđ?‘ƒđ?‘—đ?‘— ďż˝ ≥ ďż˝đ?‘&#x;đ?‘&#x;đ?‘—đ?‘— ďż˝ + [đ??´đ??´]0đ?‘—đ?‘— ďż˝đ?‘ đ?‘ đ?‘—đ?‘— ďż˝ − {đ?‘&#x;đ?‘&#x;đ?‘–đ?‘– }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
Ψ
p1
(
Ψ 01  {hi } , {h=  01 j} Ψ
)
({ f } ,{h })  =  g , h { } { } ( ) i
j
i
j
{0}
Secondo vincolo di parallelismo Il secondo vincolo di parallelismo nasce quando vogliamo che il versore �ℎ�� � debba 0
essere parallelo al generico vettore ������� � . Ciò avviene se viene rispettato il vincolo Ψ
p2
(
Ψ 02  {hi } , {d=  02 ij } Ψ
)
({ f } ,{d })  =  ({g } ,{d }) i
ij
i
ij
{0}
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 {hi } e {h j } siano collineari. La collinearità è garantita dalla condizione che {hi } resti parallelo sia ad {h j } , sia al vettore {d ij } . Dove gli estremi di quest’ultimo sono i punti Pi e Pj . Quanto detto si traduce nelle equazioni:
Ψ
Ψ
p1
p2
(
Ψ 01 {hi } , {h= 01 j} Ψ
({ f } ,{h }) = ({g } ,{h })
{0}
Ψ 02 {hi } , {d= 02 ij } Ψ
({ f } ,{d }) = g d , { } ( { })
{0}
(
)
)
i
j
i
j
i
ij
i
ij
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
Ψ s ( Pi , Pj ) = 0
(
)
Ψ p1 {hi } , {h j } = 0
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. Ψ s ( Pi , Pj ) = 0
(
)
Ψ 01 {hi } , {h j } = 0
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 ( oi , xi , yi , zi ) 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 y1 quello parallelo a X. L’asse z1 è stato preso lungo il link, infine l’asse x1 è 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 z4 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 y2 con Y e z2 con Z .
OSS: 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
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 Ψ o1 ({Y }T ,{ y1 }) {0,1, 0} ⋅ [ A]10 ⋅ {0,1, 0}T }) 0 Ψ p1 ({ X },{ y= = = o1 1 0 T T ({ } ,{ }) {0, 0,1} [ ] {0,1, 0} Ψ Z y ⋅ A ⋅ 1 1 Ψ o1 ({Y }T ,{dij }) }) o1 Ψ ({ X },{d ij= = 0 T Ψ ({Z } ,{dij }) p2
{dij } = {r1 }(0) + [ A]10 {s}(1) = {q1 , q2 , q3 }T + [ A]10 ⋅ {0, 0, −l1 / 2}T Ψ o1 ({Y }T ,{dij })= {0,1, 0} ⋅ ({q1 , q2 , q3 }T + [ A]10 ⋅ {0, 0, −l1 / 2}T ) Ψ o1 ({Z }T ,{dij })= {0, 0,1} ⋅ ({q1 , q2 , q3 }T + [ A]10 ⋅ {0, 0, −l1 / 2}T ) e0 , e1 , e2 , e3 ) A(q4 , q5 , q6 , q7 ) = [ A]0 (= 1
Per il giunto che collega il corpo1 al corpo 4 si ha:
Ψ s ({Pi },{Pj }) ≡ {r4 }(0) + [ A]04 {s4 }(4) − {r1 }(0) + [ A]10 {s1 }(1) = 0 Ψ s ({Pi= },{Pj }) {q22 , q23 , q24 }T + [ A]04 ⋅ {0, 0, −l2 / 2}T − {q1 , q2 , q3 }T − [ A]10 ⋅ {0, 0, −= l1 / 2}T 0 Ψ o1 ({x1 }T ,{ y4 }) {1, 0, 0} ⋅ [ AT ]10 ⋅ [ A]04 ⋅ {0,1, 0} Ψ p1 ({x1 },{x= }) = =0 o1 4 T 0 0 T {1, 0, 0} ⋅ [ A ] ⋅ [ A ] ⋅ {0, 0,1} Ψ ({ x } ,{ z }) 1 4 1 4 = [ A]04 A= (e0 , e1 , e2 , e3 ) A(q25 , q26 , q27 , q28 )
Per i corpi 4-7 si ha:
Ψ s ({Pi },{Pj }) ≡ {r7 }(0) + [ A]07 {s7 }(7) − {r4 }(0) + [ A]04 {s4 }(4) = 0 Ψ s ({P = {q43 , q44 , q45 }T + [ A]07 ⋅ {0, 0, −d }T − {q22 , q23 , q24 }T − [ A]04 ⋅ {0, 0, = l2 / 2}T 0 i },{Pj }) Ψ o1 ({hi }T ,{h j }) = Ψ o1 ({x4 }T ,{ y7 }) = {1, 0, 0} ⋅ [ AT ]04 ⋅ [ A]07 {0,1, 0}T = (e0 , e1 , e2 , e3 ) A(q46 , q47 , q48 , q49 ) [ A]70 A=
gamba 2 Per la coppia telaio-corpo 2:
Ψ o1 ({ X }T ,{ y2 }) {1, 0, 0} ⋅ [ A]02 ⋅ {0,1, 0}T Ψ ({Y },{= y2 }) o1 = = 0 T 0 T Ψ ({Z } ,{ y2 }) {0, 0,1} ⋅ [ A]2 ⋅ {0,1, 0} p1
Ψ o1 ({ X }T ,{dij }) Ψ p 2 ({Y },{d= }) = o1 0 ij T Ψ ({Z } ,{dij }) {dij } = {r2 }(0) + [ A]02 {s}(2) = {q8 , q9 , q10 }T + [ A]02 ⋅ {0, 0, −l1 / 2}T Ψ o1 ({ X }T ,{dij })= {1, 0, 0} ⋅ ({q8 , q9 , q10 }T + [ A]02 ⋅ {0, 0, −l1 / 2}T ) Ψ o1 ({Z }T ,{dij })= {0, 0,1} ⋅ ({q8 , q9 , q10 }T + [ A]02 ⋅ {0, 0, −l1 / 2}T ) [ A]02 A= (e0 , e1 , e2 , e3 ) A(q11 , q12 , q13 , q14 ) =
Per il giunto rotoidale tra il corpo 2 e il corpo 5:
0 Ψ s ({Pi },{Pj }) ≡ {r5 }(0) + [ A]50 {s5 }(5) − {r2 }(0) + [ A]02 {s2 }(2) = l1 / 2}T 0 },{Pj }) {q29 , q30 , q31 }T + [ A]50 ⋅ {0, 0, −l2 / 2}T − {q8 , q9 , q10 }T − [ A]02 ⋅ {0, 0, −= Ψ s ({Pi= Ψ o1 ({x2 }T ,{ y5 }) {1, 0, 0} ⋅ [ AT ]02 ⋅ [ A]50 ⋅ {0,1, 0} Ψ ({x2 },{x= = =0 o1 5 }) T 0 0 T Ψ ({x2 } ,{z5 }) {1, 0, 0} ⋅ [ A ]2 ⋅ [ A]5 ⋅ {0, 0,1} p1
= [ A]50 A= (e0 , e1 , e2 , e3 ) A(q32 , q33 , q34 , q35 )
Per i corpi 5-7:
Ψ s ({Pi },{Pj }) ≡ {r7 }(0) + [ A]70 {s7 }(7) − {r5 }(0) + [ A]50 {s5 }(5) = 0 Ψ s ({= Pi },{Pj }) {q43 , q44 , q45 }T + [ A]07 ⋅ {0, −d , 0}T − {q29 , q30 , q31 }T − [ A]50 ⋅ {0,= 0, l2 / 2}T 0 Ψ o1 ({hi }T ,{h j }) = Ψ o1 ({x5 }T ,{ y7 }) = {1, 0, 0} ⋅ [ AT ]50 ⋅ [ A]07 {0,1, 0}T
gamba 3 Per la coppia telaio-corpo 3:
Ψ o1 ({ X }T ,{ y3 }) {1, 0, 0} ⋅ [ A]30 ⋅ {0,1, 0}T y3 }) o1 0 Ψ p1 ({Z },{= = = T T 0 ({ } ,{ }) {0,1, 0} [ ] {0,1, 0} Y y A Ψ ⋅ ⋅ 3 3 Ψ o1 ({ X }T ,{dij }) Ψ ({Z },{d= = o1 0 ij }) T Ψ ({Y } ,{dij }) p2
{dij } = {r3 }(0) + [ A]30 {s3 }(3) = {q15 , q16 , q17 }T + [ A]30 ⋅ {0, 0, −l1 / 2}T Ψ o1 ({ X }T ,{dij })= {1, 0, 0} ⋅ ({q15 , q16 , q17 }T + [ A]30 ⋅ {0, 0, −l1 / 2}T ) Ψ o1 ({Y }T ,{dij })= {0,1, 0} ⋅ ({q15 , q16 , q17 }T + [ A]30 ⋅ {0, 0, −l1 / 2}T ) [ A]30 A= (e0 , e1 , e2 , e3 ) A(q18 , q19 , q20 , q21 ) =
Si noti l’analisi dei giunti rotoidali.
Tra il corpo 3 e il corpo 6:
0 Ψ s ({Pi },{Pj }) ≡ {r6 }(0) + [ A]06 {s6 }(6) − {r3 }(0) + [ A]30 {s3 }(3) = },{Pj }) {q36 , q37 , q38 }T + [ A]06 ⋅ {0, 0, −l2 / 2}T − {q15 , q16 , q17 }T − [ A]30 ⋅ {0, 0, −= l1 / 2}T 0 Ψ s ({Pi= Ψ o1 ({x3 }T ,{ y6 }) {1, 0, 0} ⋅ [ AT ]30 ⋅ [ A]06 ⋅ {0,1, 0} }) Ψ p1 ({x3 },{x= = =0 o1 6 T 0 0 T A A ⋅ ⋅ ⋅ {1, 0, 0} [ ] [ ] {0, 0,1} ({ } ,{ }) x z Ψ 3 6 3 6 = [ A]50 A= (e0 , e1 , e2 , e3 ) A(q39 , q40 , q41 , q42 )
Per i corpi 6-7 :
Ψ s ({Pi },{Pj }) ≥ {r7 }(0) + [ A]70 {s7 }(7) − {r6 }(0) + [ A]06 {s6 }(6) = 0 Ψ s= ({Pi },{Pj }) {q43 , q44 , q45 }T + [ A]07 â‹… {−d , 0, 0}T − {q36 , q37 , q38 }T − [ A]06 â‹… {0, = 0, l2 / 2}T 0 Ψ o1 ({hi }T ,{h j }) = Ψ o1 ({x6 }T ,{ y7 }) = {1, 0, 0} â‹… [ AT ]06 â‹… [ A]07 {0,1, 0}T
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 3 3 3 eˆ = − xˆ7 − yˆ 7 − zˆ7 3 3 3
Fissando come legge di moto: ÎŚ=
đ?‘Ąđ?‘Ą đ?œ‹đ?œ‹ đ?‘ đ?‘ đ?‘–đ?‘–đ?‘–đ?‘– ďż˝2đ?œ‹đ?œ‹ ďż˝ đ?‘‡đ?‘‡đ?‘“đ?‘“ 6
Definiti questi invarianti naturali si può imporre la seguente legge di moto:  3 Ό q48 = −   â‹… sin   ďŁ2 ďŁ 3 
Ό q46 = cos   ďŁ2
 3 Ό q49 = −   â‹… sin   ďŁ2 ďŁ 3 
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 d  ∂L  dt ďŁŹďŁ âˆ‚q j
 ∂L = Q j (j= 1,‌, n)  − q ∂ j 
(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 L* = L − ΝΨ = T − V − ( Îť1Ψ1 + ... + Îť p Ψ p )
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:
[ M ]{q} +  Ψ q  {Ν} ={Fe } T
dove
[ M ] è la matrice delle masse
(2)
{Fe } è il vettore delle forze generalizzate Ψ q è la matrice Jacobiana trasposta associata al sistema delle equazioni T
{Ψ} ={0} di vincolo dove l’espressione delle forze generalizzate è
δW Fj = = δ qj
nc δ rk Fk ⋅ ∑ δ qj k =1 N
δ W è il lavoro virtuale delle forze esterne e δ q j la variazione virtuale della
j-
esima coordinata q j . Per poter risolvere, a questo punto, il sistema di equazioni composto da da n equazioni differenziali nelle incognite q1 ,..., qn e λ1 ,…, λ p , sia dal sistema costituito da equazioni algebriche di vincolo Ψ1 =0 ,…, Ψ p =0 , per cui il numero di incognite (n+p) è pari a quello complessivo delle equazioni.
[ M ]{q} + Ψ q {λ} ={Fe } T
{Ψ} =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:
{Ψ } ≡ Ψ {q} − {γ } ={0} q
Quest’ultima e la (2) possono risolversi simultaneamente quando si costruisce il sistema
M  Ψq
Ψ Tq   q   Fe     =  , 0  Ν  γ 
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 i mo del nostro robot. L’energia cinetica posseduta dal corpo è fornita dall’espressione:
= Ti
1 mi 2
{ r } { r } T
0
T
0
i
i
+
{ } { J }{ ω }
1 0 ωi 2
T
0
0
i
i
Dove:
{ ω } = 2  0
i
0 i
E  { p i }
Sostituendo diventa:
{ }{ } { }
{ }
{ }
T 1 0 Ti = mi 0 ri ri + 2 p iT  0i E  0 J i  0i E  p i = 2 T T T 1 0 mi 0 ri ri + 2 p i  0i G  i J i  0i G  p i 2
{ }{ } { }
{ }
{ }
In quanto la matrice G per definizione avendo definito E come
Risulta essere
−đ?‘’đ?‘’1 [E]=ďż˝ −đ?‘’đ?‘’2 −đ?‘’đ?‘’3
đ?‘’đ?‘’0 – đ?‘’đ?‘’3 đ?‘’đ?‘’2 đ?‘’đ?‘’3 đ?‘’đ?‘’0 −đ?‘’đ?‘’1 ďż˝ đ?‘’đ?‘’2 đ?‘’đ?‘’1 đ?‘’đ?‘’0
Da cui si ottiene
−đ?‘’đ?‘’1 [G]=ďż˝ −đ?‘’đ?‘’2 −đ?‘’đ?‘’3
đ?‘’đ?‘’0 đ?‘’đ?‘’3 −đ?‘’đ?‘’2 − đ?‘’đ?‘’3 đ?‘’đ?‘’0 đ?‘’đ?‘’1 ďż˝ đ?‘’đ?‘’2 − đ?‘’đ?‘’1 đ?‘’đ?‘’0
  mi 0 0      [03×4 ]   0 mi 0   [ M i ] =   0 0 mi      4  0G T  i J   0G     04×3   i   i   i    
L’energia potenziale posseduta dal corpo si ricava attraverso il vettore delle forze esterne generalizzate agenti sul corpo i mo
{ }   { f }
  0i A i f k  {Qi } = ∑  0 T i k =1  2  G   s     i   ik  n
i
k
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 psiE2 psiE3 psiE4 psiE5 psiE6 psiE7
= = = = = = =
q(4)^2 + q(5)^2 + q(11)^2 + q(12)^2 q(18)^2 + q(19)^2 q(25)^2 + q(26)^2 q(32)^2 + q(33)^2 q(39)^2 + q(40)^2 q(46)^2 + q(47)^2
q(6)^2 + q(7)^2 - 1; + q(13)^2 + q(14)^2 + q(20)^2 + q(21)^2 + q(27)^2 + q(28)^2 + q(34)^2 + q(35)^2 + q(41)^2 + q(42)^2 + q(48)^2 + q(49)^2 -
1; 1; 1; 1; 1; 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;psic1 2;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;q 22;q23;q24;q25;q26;q27;q28;q29;q30;q31;q32;q33;q34;q35;q36;q37;q38;q39;q40;q41;q 42;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 calcolare lo jacobiano
per poter iniziare qeusta analisi dobbiamo prima 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;q 22;q23;q24;q25;q26;q27;q28;q29;q30;q31;q32;q33;q34;q35;q36;q37;q38;q39;q40;q41;q 42;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 lambda10 lambda11 lambda12 lambda13 lambda14 lambda15 lambda16 lambda17 lambda19 lambda20 lambda21 lambda22 lambda23 lambda24 lambda25 lambda26 lambda28 lambda29 lambda30 lambda31 lambda32 lambda33 lambda34 lambda35 lambda37 lambda38 lambda39 lambda40 lambda41 lambda42 lambda43 lambda44 lambda46 lambda47 lambda48 lambda49];
lambda9 lambda18 lambda27 lambda36 lambda45
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; dV2=m2*gr'*[q8; dV3=m3*gr'*[q15; dV4=m4*gr'*[q22; dV5=m5*gr'*[q29; dV6=m6*gr'*[q36;
q2; q9; q16; q23; q30; q37;
%Nella piattaforma le %quindi
q3]; q10]; q17]; q24]; q31]; q38]; distanza tra baricentro e sistema di rif.non è nulla
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)+la mbda(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)+lam bda(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 lambda10 lambda11 lambda12 lambda13 lambda14 lambda15 lambda16 lambda17 lambda19 lambda20 lambda21 lambda22 lambda23 lambda24 lambda25 lambda26 lambda28 lambda29 lambda30 lambda31 lambda32 lambda33 lambda34 lambda35 lambda37 lambda38 lambda39 lambda40 lambda41 lambda42 lambda43 lambda44 lambda46 lambda47 lambda48 lambda49]; lam=[lam(1) lam(2) lam(3) lam(4) lam(5) lam(11) lam(12) lam(13) lam(14) lam(15) lam(21) lam(22) lam(23) lam(24) lam(25) lam(31) lam(32) lam(33) lam(34) lam(35) lam(41) lam(42) lam(43) lam(44) lam(45) eq_din=subs(Din_n,lambda,lam);
lam(6) lam(7) lam(8) lam(9) lam(10) lam(16) lam(17) lam(18) lam(19) lam(20) lam(26) lam(27) lam(28) lam(29) lam(30) lam(36) lam(37) lam(38) lam(39) lam(40) lam(46) lam(47) lam(48) lam(49) ];
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
lambda9 lambda18 lambda27 lambda36 lambda45
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;q 22;q23;q24;q25;q26;q27;q28;q29;q30;q31;q32;q33;q34;q35;q36;q37;q38;q39;q40;q41;q 42;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),U x(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