Regressore.. - Ingegneria Meccanica, Nucleare e della Produzione

UNIVERSITA’ DI PISADipartimento di Ingegneria Meccanica, Nucleare e della ProduzioneFACOLTA’ DI INGEGNERIACorso di Laurea in Ingegneria dell’AutomazioneMetodo diretto per il calcolo delladinamica nel regressoreRoboticaDocente supervisoreIng. Marco GabicciniStudentiSupervisoreDavide De CarliIng. Andrea BracciMarco FredianelliAnno Accademico 2007/2008

Capitolo 2Formulazione del problemaLa linearità nei parametri dinamici inerziali delle equazioni del moto di un manipolatoread n gradi di libertà garantisce la possibilità di scrivere tali relazioni nellaforma (1.2).Le proprietà inerziali del link i-esimo della catena siano definite dal vettore deiparametri π (i) ∈ R ri×1 . Inoltre siano T (i) (q, ˙q) e U (i) (q) rispettivamente l’energia cineticae potenziale associate al link i-esimo. Il Lagrangiano complessivo del sistemapuò essere definito considerando la somma dei contributi di ciascun link all’energiacinetica e potenziale:L(q, ˙q) = T (q, ˙q) − U(q) =n∑(T (i) (q, ˙q) − U (i) (q)) =i=1n∑L (i) (q, ˙q). (2.1)i=1Allo stesso modo, adoperando le equazioni di Lagrange, le forze di inerzia e conservativeglobali possono essere scritte come[ d ∂Ldt ∂ ˙q − ∂L ] T=∂qn∑[ d ∂L (i)dti=1∂ ˙q] T− ∂L(i)= τ. (2.2)∂qConsiderando i contributi di ciascun link è possibile partizionare il regressore Y e ilvettore dei parametri π ottenendo la relazionen∑[ d ∂L (i)dti=1∂ ˙q] T− ∂L(i)≡∂qn∑Y (i) π (i) (2.3)i=1dove Y (i) ∈ R n×r iè il blocco della matrice regressore relativo al link i-esimo eπ (i) ∈ R r i×1 è la corrispondente porzione del vettore dei parametri dinamici.7

Quindi avendo n corpi rigidi si ottengono n vettori partizione dei parametri e nblocchi regressore in modo che globalmente Y = [Y (1) . . . Y (1) ] ∈ R n×r e π =[π (1) . . . π (n) ] T ∈ R r×1 , con r = ∑ ni=i r i.Esplicitando le energia cinetica e potenziale relative al generico link i si ha[ ddt∂T (i)∂ ˙q−∂T(i)∂q+∂U(i)∂q] T≡ Y (i) π (i) . (2.4)A questo punto, considerando i sistemi di riferimento baricentrico, centrato in G i ,e di Denavit-Hartenberg {i}, centrato in O i , relativi al link i-esimo e rappresentatiin figura 2, si definiscono l’energia cinetica T (i) e potenziale U (i) in modo da potermettere in evidenza i termini inerziali.Figura 2.1: Sistemi di riferimento link i-esimo8

Capitolo 3Calcolo diretto del regressore3.1 Termini derivanti dall’espressione dell’energiacineticaSi considera l’espressione dell’energia cinetica T (i) del link i-esimo ottenuta prendendoil baricentro G i e il tensore d’inerzia baricentrico i I Gi con componenti in terna{S i } centrata in O i . Adoperando il sistema di riferimento fisso {S 0 }, solidale allabase del manipolatore, è possibile applicare il teorema di König:T (i) = 1 2 m i 0 v T G i 0 v Gi + 1 2i ω T ii I Gi i ω i (3.1)Inoltre si ha:i ω i = i R 0 0 ω i = 0 R T i0 ω i . (3.2)Successivamente si esprimono le velocità lineari e angolari del link in funzione dellevelocità ai giunti ˙q tramite l’utilizzo dei Jacobiani relativi ai frames di Denavit-Hartenberg:0 v Gi = 0 v i + 0 ω i × 0 P iGi = J vi ˙q + J ωi ˙q × 0 R i P iGi0 ω i = J ωi ˙q.(3.3)dove P iGi indica il vettore posizione del baricentro G i del link i-esimo definito rispettoal frame {i}, J vi e J ωi rappresentano rispettivamente il Jacobiano di posizione e diorientazione dell’origine del sistema di riferimento i-esimo di Denavit-Hartenberg.9

3.1. Termini derivanti dall’espressione dell’energia cineticaSostituendo le relazioni (3.2) e (3.3) nell’espressione (3.1) dell’energia cinetica, siottiene:T (i) = 1 2 m i( ˙q T J T v i+ (J ωi ˙q × 0 R i P iGi ) T )(J vi ˙q + J ωi ˙q × 0 R i P iGi )+ 1 2 ˙qT (J T ω i 0 R i i I Gi 0 R T i J ωi ) ˙q.(3.4)Considerando l’operatore skew definito come⎛⎞S(f) = S([f 1 f 2 f 3 ] T ) = ⎜⎝0 −f 3 f 2f 3 0 −f 1−f 2 f 1 0⎟⎠(3.5)La matrice skew risultante è antisimmetrica (S(f) = −S(f) T ) e, considerando unamatrice di rotazione R e un vettore a, si ha che S(R a) = R S(a) R T . Utilizzandoqueste ultime relazioni, si ottiene la seguente espressione dell’energia cinetica:T (i) = 1 2 m i ˙q T (J T v iJ vi ) ˙q − 1 2 m i ˙q T { J T v iS( 0 R i P iGi ) J ωi}˙q+ 1 2 m i ˙q { } T Jω T iS( 0 R i P iGi ) J vi ˙q(3.6)+ 1 { [2 ˙qT Jω T 0 iR ii I Gi + m i S T (P iGi ) S(P iGi ) ] } 0 Ri T J ωi ˙qA questo punto si calcolano i termini che compongono le equazioni di Lagrange diogni link (2.4). Successivamente si mettono in evidenza i parametri dinamici (1.3)presenti in ciascun termine.3.1.1 Termini derivanti da d dt() T∂T (i)∂ ˙qDerivando l’espressione dell’energia cinetica (3.6) rispetto a ˙q risulta∂T (i)∂ ˙q= m i ˙q T (J T v iJ vi ) − m i ˙q T { J T v iS( 0 R i P iGi ) J ωi}+ m i ˙q T { J T ω iS( 0 R i P iGi ) J vi}+ ˙q T { J T ω i 0 R i i I i 0 R T i J ωi}(3.7)dove i I i = i I Gi + m i S T (P iGi ) S(P iGi ) è il tensore d’inerzia del link i-esimo riferitoal frame di origine O i .Considerando la proprietà di simmetria della matrice i I i e le caratteristiche delle10

3.1. Termini derivanti dall’espressione dell’energia cineticamatrici antisimmetriche definite tramite l’utilizzo dell’operatore skew, si ottiene( ) ∂T(i) T= (Jv T iJ vi ) ˙q T m i∂ ˙q+ { J T v iS(J ωi ˙q) 0 R i − J T ω iS(J vi ˙q) 0 R i}mi P iGi+ J T ω i 0 R i i I i 0 R T i J ωi ˙q(3.8)A questo punto si hanno i primi due termini lineari rispettivamente in m i e m i P iGie il terzo è funzione di i I i .Al fine di isolare i termini inerziali del secondo ordine contenuti in J T ω i 0 R i i I i 0 R T i J ωisi scrive i I i come prodotto interno tra un tensore E del terzo ordine e il vettore deiparametri π:i I i = E π (i)2 (3.9)˙q,11

3.1. Termini derivanti dall’espressione dell’energia cineticadovei I i =⎡⎢⎣¯J ixx − ¯J ixy − ¯J⎤ixz− ¯J ixy¯Jiyy − ¯J iyz ⎥− ¯J ixz − ¯J⎦iyz¯Jizz⎡ ⎡ ⎤⎤1 0 0⎢ 0 0 0 ⎥⎣ ⎦0 0 0⎡ ⎤0 1 0− ⎢ 1 0 0 ⎥⎣ ⎦0 0 0⎡ ⎤0 0 1−⎢ 0 0 0 ⎥⎣ ⎦1 0 0E =⎡ ⎤0 0 0⎢ 0 1 0 ⎥⎣ ⎦0 0 0⎡ ⎤0 0 0− ⎢ 0 0 1 ⎥⎣ ⎦0 1 0⎡ ⎤0 0 0⎢ ⎢ 0 0 0 ⎥⎥⎣ ⎣ ⎦⎦0 0 1π (i)T2 = [ ¯Jixx ¯Jixy ¯Jixz ¯Jiyy ¯Jiyz ¯Jizz]⎡E 1E 6⎤(3.10)−E 2−E=3∈ R 3×3×6 (3.11)E 4⎢ −E⎣ 5 ⎥⎦In questo modo il terzo termine di (3.8) diventa⎡Jω T 0 iR i i I 0 i Ri T J ωi ˙q = [ Jω T 0 iR i E 0 1 Ri T J ωi ˙q| . . . |Jω T 0 iR i E 0 6 Ri T J ωi ˙q ] ⎢⎣¯J ixx¯J ixy¯J ixz¯J iyy¯J iyz⎤⎥⎦(3.12)¯J izz12

3.1. Termini derivanti dall’espressione dell’energia cineticaRiassumendo si ha( ∂T(i)∂ ˙q) T= X (i)0 π (i)0 + X (i)1 π (i)1 + X (i)2 π (i)2 (3.13)doveX (i)0 = (J T v iJ vi ) ˙q ∈ R n×1 (3.14)X (i)1 = { J T v iS(J ωi ˙q) − J T ω iS(J vi ˙q) } 0 R i ∈ R n×3 (3.15)X (i)2 = J T ω i 0 R i[E1 | − E 2 | . . . | E 6]Jωi ˙q ∈ R n×6 (3.16)π (i)0 = m i (3.17)⎡ ⎤m i P iGixπ (i)1 = ⎢ m⎣ i P iGiy⎥(3.18)⎦⎡m i P iGiz⎤π (i)2 =⎢⎣¯J ixx¯J ixy¯J ixz¯J iyy¯J iyz⎥⎦(3.19)¯J izzQuindi la prima parte di regressore derivante dalle equazioni di Lagrange èddt( ∂T(i)∂ ˙q3.1.2 Termini derivanti da) T= Ẋ(i) 0 π (i)0 + Ẋ(i) 1 π (i)1 + Ẋ(i) 2 π (i)2 (3.20)() T∂T (i)∂qSi considera l’espressione dell’energia cinetica presentata in 3.6 e di seguito riportataT (i) = 1 2 m i ˙q T (J T v iJ vi ) ˙q − 1 2 m i ˙q T { J T v iS( 0 R i P iGi ) J ωi}˙q+ 1 2 m i ˙q { } T Jω T iS( 0 R i P iGi ) J vi ˙q(3.21)+ 1 { [2 ˙qT Jω T 0 iR ii I Gi + m i S T (P iGi ) S(P iGi ) ] } 0 Ri T J ωi ˙q13

3.1. Termini derivanti dall’espressione dell’energia cineticaDerivando rispetto a q si ottiene( ) ∂T(i)= 1 { } ∂∂q 2 ˙qT ∂q (J v T iJ vi )˙q m i+ 1 { ∂ [ ] }2 ˙qT JT∂qviS(J ωi ˙q) 0 R i − Jω T iS(J vi ˙q) 0 R i m i P iGi+ 1 { ∂ [ ] }2 ˙qT JT 0∂qωiR i i I 0 i Ri T J ωi ˙q =(3.22)= W (i)T0 m i + W (i)T1 m i P iGi + 1 { ∂ [ ] }2 ˙qT JT 0∂qωiR i i I 0 i Ri T J ωi ˙qUtilizzando la relazione 3.9 il terzo termine dell’espressione precedente risulta⎡ ⎤{ ∂ [ ] } JT 0∂qωiR i i I 0 i Ri T J ωi = ⎢⎣⎡( )∂∂q JT 0 ωiR i E 0 1 Ri T J ωi∂∂q⎤.⎥( )⎦JT 0 ωiR i E 0 6 Ri T J ωiInfine, eseguendo la trasposizione dei termini risultanti, si ha( ∂T(i)∂qT⎢⎣¯J ixx¯J ixy¯J ixz¯J iyy¯J iyz¯J izz⎥⎦(3.23)) T= W (i)0 π (i)0 + W (i)1 π (i)1 + W (i)2 π (i)2 (3.24)dove⎡0 = 1 2 ˙qT ⎢⎣W (i)W (i)1 = 1 2W (i)⎡⎢⎣⎡2 = 1 2 ˙qT ⎢⎣( )∂∂q 1JTviJ vi.( )∂∂q nJTviJ vi⎤⎥ ˙q (3.25)⎦[∂ 0∂q 1Ri T S T (J ωi ˙q) J vi ˙q − 0 Ri T S T (J vi ˙q) J ωi ˙q ] ⎤.⎥[∂ 0∂q nRi T S T (J ωi ˙q) J vi ˙q − 0 Ri T S T (J vi ˙q) J ωi ˙q ] ⎦( )∂∂q 1JT 0 ωiR i E 0 Ri T J ωi.( )∂∂q nJT 0 ωiR i E 0 Ri T J ωi⎤(3.26)⎥ ˙q (3.27)⎦14

3.2. Termini derivanti dall’espressione dell’energia potenziale3.2 Termini derivanti dall’espressione dell’energiapotenzialeRiferendosi alla relazione (2.4), si calcola l’energia potenziale del link i-esimo definendoi vettori posizione rispetto al frame base {0}:U (i) = −m i g T ( 0 p i + 0 R i P iGi)(3.28)Per ottenere i termini contenuti nelle equazioni di Lagrange è necessario derivaretale energia potenziale rispetto a q:( ∂U(i)∂q) T {= − m i g T ∂ 0 p i+ g T ∂ } 0 TR i∂q ∂q P iG i=[ ] ∂ (g= − Jv T T 0 T(3.29)R i )ig m i −m i P iGi∂qConsiderando il secondo termine dell’espressione precedente, è possibile scrivere:[ ] ∂ (gT 0 TR i m i P iGi )=∂q[ ∂{} ] T(m i P iGi ) T 0 Ri T g∂q⎡= ⎢⎣( )∂( 0 Ri T Tg)⎤∂q 1⎥) T⎦ m i P iGi∂q n.(∂( 0 R T i g)A questo punto si l’espressione della derivata dell’energia potenziale rispetto a q puòessere espressa come somma di due termini lineari rispettivamente in m i e in m i P iGi :dove( ∂U(i)∂q) T= Z (i)0 m i + Z (i)1 m i P iGi (3.30)Z (i)0 = − Jv T ig (3.31)⎡ ( )∂( 0 R T T ⎤i g)∂q 1Z (i)1 = − ⎢ . ⎥(3.32)⎣ ( )∂( 0 Ri T T⎦g)∂q n15

3.3. Costruzione del Regressore3.3 Costruzione del RegressoreLa porzione di equazioni di Lagrange relativa al link i-esimo è definita come segue:⎡ ⎤[ d ∂T (i)−dt ∂ ˙q∂T(i)∂q+∂U(i)∂q] T [≡ Y (i) π (i) =Y (i)0 Y (i)1 Y (i)2]⎢⎣π (i)0π (i)1π (i)2⎥⎦(3.33)dove i blocchi che compongono le colonne della porzione di regressore relativa al linki-esimo Y (i)jriferiscono:sono stati ordinati sulla base dell’ordine dei momenti d’inerzia cui siY (i)0 = vettore colonna che moltiplica il momento d’inerzia di ordine zero m iY (i)1 = porzione che moltiplica i momenti d’inerzia del primo ordine m i P iGi (3.18)Y (i)2 = porzione che moltiplica i momenti d’inerzia del secondo ordine π (i)2 (3.19)Considerando le definizioni delle X (i)je delle Z (i)j(3.14, 3.15, 3.16), delle W (i)j (4.14, 4.15, 3.27)(4.17, 4.18) si può procedere al calcolo diretto del regressore del linki-esimo mediante le formuleY (i)0 = Ẋ(i) 0 − W (i)0 + Z (i)0 (3.34)Y (i)1 = Ẋ(i) 1 − W (i)1 + Z (i)1 (3.35)Y (i)2 = Ẋ(i) 2 − W (i)2 + Z (i)2 (3.36)Le relazioni precedenti possono essere adoperate per calcolare la porzione di regressorerelativa a ciascun link Y (i) = Y (i)0 Y (i)1 Y (i)2 , i = 1, . . . , n; il[]regressorecompleto si ottiene giustapponendo i regressori parziali:Y (q, ˙q, ¨q) =[Y (1) . . . Y (n) ]. (3.37)Analogamente si ottiene il vettore colonna dei parametri dinamici:⎡ ⎤π (1)π = ⎢ . ⎥⎣ ⎦ . (3.38)π (n)16

Capitolo 4Controllo adattativo e calcolodiretto del regressore di Slotine-LiIl controllo adattativo permette di adattare in linea il modello computazionale almodello dinamico del manipolatore nel caso in cui i due modelli corrispondano perquanto riguarda la struttura, ma esiste incertezza nella stima dei parametri che caratterizzanoil manipolatore.Leggi di controllo di tipo adattativo possono essere definite adoperando la proprietàdi linearità nei parametri dinamici del modello. In particolare il controllo adattativodi Slotine-Li si basa su un approccio che considera una legge di controllo a coppiaprecalcolata mediante inversione della dinamica, supponendo che il modello computazionalesia coincidente con il modello dinamico.La legge di controllo considerata èτ = B(q)¨q r + C(q, ˙q) ˙q r + G(q) + K D σ (4.1)con K D matrice definita positiva e˙q r = ˙q d + Λ ˜q ¨q r = ¨q d + Λ ˙˜qσ = ˙q r − ˙q = ˙˜q + Λ ˜q(4.2)˜q = q d − qcon Λ matrice definita positiva.Tale formulazione permette di non utilizzare le misure delle accelerazioni ai giunti17

¨q, spesso affette da notevoli disturbi.La legge di controllo 4.1, nel caso in cui iparametri non siano valutati esattamente, si modifica inτ = ˆB(q)¨q r + Ĉ(q, ˙q) ˙q r + Ĝ(q) + K D σ,= Y r (q, ˙q, ˙q r , ¨q r ) ˆπ + K D σ,(4.3)dove ˆπ rappresenta la stima effettuata dei parametri e, di conseguenza, ˆB, Ĉ, Ĝrappresentano le funzioni stimate dei termini del modello dinamico.La stima del vettore dei parametri è aggiornata nel tempo con la legge di adattamento˙ˆπ = Kπ−1 Yr T (q, ˙q, ˙q r , ¨q r ) σ (4.4)Lo schema a blocchi che rappresenta l’implementazione del controllo adattativo diSlotine-Li è riportato in figura 4.1.Il regressore di Slotine-Li Y r (q, ˙q, ˙q r , ¨q r ) utilizzato sia nella legge di controllo cheFigura 4.1: Schema a blocchi controllo adattativo di Slotine-Linella legge di adattamento non è semplice da calcolare e la maggiore difficoltà risiedenella possibilità di definire la matrice C in modo non univoco.Si considerano delle forme di energia cinetica che, tramite l’utilizzo delle equazionidi Lagrange, riconducano alla[ d ∂T 1dt ∂ ˙q − ∂T ] T2= B(q)¨q r + C(q, ˙q) ˙q r . (4.5)∂q18

Le definizioni di energia cinetica considerate sonoT 1 = ˙q T r B(q) ˙q T 2 = 1 2 ˙qT r B(q) ˙q. (4.6)Si esplicitano i termini derivanti dal primo membro dell’equazione 4.5:[ ] T d ∂T 1= B(q) ¨q r +dt ∂ ˙qḂ ˙q r (4.7)[ ] T ∂T2= 1 ∂B(q) ˙qT ˙q r . (4.8)∂q 2 ∂qIn questo modo si ottiene[ d ∂T 1dt ∂ ˙q − ∂T ] T2= B(q)¨q r +∂q[Ḃ − 1 2]∂B(q) ˙qT∂q˙q r (4.9)dove, per definizione, si ha C(q, ˙q) =Ḃ − 1 2 ˙qT ∂B(q)∂q.Successivamente si adopera la definizione della matrice d’inerzian∑B(q) = B(q) (i)i=1{ }B(q) (i) = m i (Jv T iJ vi ) − m i JTviS( 0 R i P iGi ) J ωi{ }+ m i JTωiS( 0 R i P iGi ) J vi+ { [Jω T 0 iR ii I Gi + m i S T (P iGi ) S(P iGi ) ] }0 Ri T J ωi(4.10)e, utilizzando un procedimento analogo a quello impiegato per il calcolo diretto delregressore nel capitolo 3, si costruiscono i termini che compongono il regressore di19

Slotine-Li.X (i)0 r= (J T v iJ vi ) ˙q r (4.11)X (i)1 r= { J T v iS(J ωi ˙q r ) − J T ω iS(J vi ˙q r ) } 0 R i (4.12)X (i)[ ]2 r= Jω T 0 iR i E1 | − E 2 | . . . | E 6 Jωi ˙q r (4.13)⎡( ) ⎤∂W (i)0 r= 1 ∂q 1JTviJ vi 2 ˙qT r ⎢ . ⎥ ˙q (4.14)⎣( )⎦∂∂q nJTviJ vi⎡ [∂ 0] ⎤W (i)1 r= 1 ∂q 1Ri T S T (J ωi ˙q) J vi ˙q r − 0 Ri T S T (J vi ˙q) J ωi ˙q r ⎢.⎥ (4.15)2 ⎣[∂ 0]⎦∂q nRi T S T (J ωi ˙q) J vi ˙q r − 0 Ri T S T (J vi ˙q) J ωi ˙q r⎡ ( ) ⎤∂W (i)2 r= 1 ∂q 1JT 0 ωiR i E 0 Ri T J ωi 2 ˙qT r ⎢.⎥ ˙q (4.16)⎣( )⎦∂∂q nJT 0 ωiR i E 0 Ri T J ωiInfine i termini derivanti dall’energia potenziale sono analoghi a quelli definiti nelparagrafo 3.2, in quanto il termine G(q) non viene influenzato dall’introduzione dellevelocità di riferimento ˙q r .Z (i)0 = − Jv T ig (4.17)⎡ ( )∂( 0 R T T ⎤i g)∂q 1Z (i)1 = − ⎢ . ⎥(4.18)⎣ ( )∂( 0 Ri T T⎦g)∂q nIl regressore completo di Slotine-Li si ottiene giustapponendo le porzioni di regressorerelative a ciascun link:Y (i)0 r= Ẋ(i) 0 r− W (i)0 r+ Z (i)0 (4.19)Y (i)1 r= Ẋ(i) 1 r− W (i)1 r+ Z (i)1 (4.20)Y (i)2 r= Ẋ(i) 2 r− W (i)2 r+ Z (i)2 (4.21)Y (i)r =Y r (q, ˙q, ˙q r , ¨q r ) =[Y (i)0 rY (i)1 rY (i)2 r][Y r (1) . . . Y r(n), i = 1, . . . , n (4.22)]. (4.23)20

Capitolo 5Calcolo diretto del regressore perun manipolatore di StanfordIl manipolatore è costituito da una catena seriale a sei link di tipo R-R-P-R-R-R(vedi Fig. 5.1). Per la costruzione del regressore di tale robot, è stato utilizzato ilFigura 5.1: Manipolatore di Stanfordsoftware di calcolo simbolico Wolfram Mathematica 6.0, con il quale è stato possibileverificare la correttezza del metodo diretto per il calcolo del regressore attraverso21

5.1. Studio di cinematica diretta, cinematica differenziale e dinamicaun confronto con i termini che si ottengono dallo studio della dinamica in manieraclassica.5.1 Studio di cinematica diretta, cinematica differenzialee dinamicaUtilizzando la convenzione di Denavit-Hartenberg, sono state costruite le terne diriferimento mostrate in Fig. 5.2 e la Tabella (5.1). Dopo la costruzione della tabellaLink a i α i d i θ i1 0 −π/2 0 θ 1 (t)2 0 π/2 d 2 θ 2 (t)3 0 0 d 3 (t) 04 0 −π/2 0 θ 4 (t)5 0 π/2 0 θ 5 (t)6 0 0 d 6 θ 6 (t)Tabella 5.1: Tabella di Denavit-Hartenberg per il manipolatore di Stanfordsono state calcolate le matrici di trasformazione i−1 A i che permettono di ricavare lafunzione cinematica diretta semplicemente per composizione.⎡⎤cos θ i − sin θ i cos α i sin θ i sin α i a i cos θ isin θ i−1 A i =i cos θ i cos α i − cos θ i sin α i a i sin θ i⎢ 0 sin α⎣i cos α i d i ⎥⎦0 0 0 1(5.1)6∏0 A 6 =i−1 A i = 0 A 1 1 A 2 2 A 3 3 A 4 4 A 5 5 A 6i=1Di seguito si riporta il codice Mathematica che implementa tale procedura.Cinematica Diretta22

5.1. Studio di cinematica diretta, cinematica differenziale e dinamicaFigura 5.2: Convenzione di Denavit-Hartenberg per un manipolatore di Stanford23

5.1. Studio di cinematica diretta, cinematica differenziale e dinamicaMatrice di Trasformazione dal sistema di riferimento {i-1} a {i} (di D-H)A[i_] := {{Cos[link[i][[4]]], -Sin[link[i][[4]]]*Cos[link[i][[2]]],Sin[link[i][[4]]]*Sin[link[i][[2]]],link[i][[1]]*Cos[link[i][[4]]]}, {Sin[link[i][[4]]],Cos[link[i][[4]]]*Cos[link[i][[2]]], -Cos[link[i][[4]]]*Sin[link[i][[2]]],link[i][[1]]*Sin[link[i][[4]]]}, {0, Sin[link[i][[2]]],Cos[link[i][[2]]], link[i][[3]]}, {0, 0, 0, 1}};A01 = A[1];A12 = A[2];A23 = A[3];A34 = A[4];A45 = A[5];A56 = A[6];Matrice di trasformazione da terna base a end-effectorA06 = A01.A12.A23.A34.A45.A56;n06 = A06[[1 ;; 3, 1]];s06 = A06[[1 ;; 3, 2]];a06 = A06[[1 ;; 3, 3]];p06 = A06[[1 ;; 3, 4]];Successivamente è stata studiata la cinematica differenziale del manipolatore, checaratterizza i legami tra le velocità dei giunti e le corrispondenti velocità lineare eangolare dell’end effector. Tali legami sono descritti dalla matrice di trasformazionedenominata Jacobiano geometrico.A questo punto si procede affrontando lo studio della dinamica del manipolatoredi Stanford considerato. Tale studio è condotto allo scopo di verificare l’efficienzadel metodo per il calcolo diretto della dinamica nel regressore, tramite un confrontotra le τ ottenute eseguendo il prodotto τ = Y (q, ˙q, ¨q) π e considerando il modellodinamico nello spazio dei giunti τ = B(q) ¨q + C(q, ˙q) ˙q + G(q).Per la definizione di quest’ultimo modello sono stati costruiti i Jacobiani relativi aibaricentri di ciascun link. La matrice B(q) è ottenuta adoperando tali Jacobiani,le caratteristiche inerziali dei bracci e le matrici di rotazione calcolate nell’ambitodella risoluzione della cinematica diretta; la matrice C(q, ˙q) è costruita a partire24

5.1. Studio di cinematica diretta, cinematica differenziale e dinamicadalla matrice B tramite l’utilizzo dei simboli di Christoffel; infine la matrice G(q) èottenuta per derivazione diretta dell’energia potenziale U(q) rispetto al vettore dellevariabili di giunto q.Il codice Mathematica che implementa il calcolo della forma matriciale del modellodinamico è presentato di seguito.DinamicaCalcolo di B (matrice d’inerzia)Formule per il calcolo dei Jacobiani dei vari linkJacobiano di posizionejpl[i_, j_] := z[2] /; j == 3; (* giunto prismatico *)jpl[i_, j_] := S[z[j - 1]].(pg[i] - p[j - 1]);(*giunto rotoidale*)Jacobiano di orientazionejol[i_, j_] := {0, 0, 0} /; j == 3; (* giunto prismatico *)jol[i_, j_] := z[j - 1];(*giunto rotoidale*)Calcolo del Jacobiano del link 1Jp[1] = Transpose[{jpl[1, 1], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0,0, 0}}];Jo[1] = Transpose[{jol[1, 1], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0,0, 0}}];Calcolo del Jacobiano del link 2Jp[2] = Transpose[{jpl[2, 1],jpl[2, 2], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}];Jo[2] = Transpose[{jol[2, 1],jol[2, 2], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}];Calcolo del Jacobiano del link 3Jp[3] = Transpose[{jpl[3, 1], jpl[3, 2],jpl[3, 3], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}];Jo[3] = Transpose[{jol[3, 1], jol[3, 2],jol[3, 3], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}];Calcolo del Jacobiano del link 425

5.1. Studio di cinematica diretta, cinematica differenziale e dinamicaJp[4] = Transpose[{jpl[4, 1], jpl[4, 2], jpl[4, 3],jpl[4, 4], {0, 0, 0}, {0, 0, 0}}];Jo[4] = Transpose[{jol[4, 1], jol[4, 2], jol[4, 3],jol[4, 4], {0, 0, 0}, {0, 0, 0}}];Calcolo del Jacobiano del link 5Jp[5] = Transpose[{jpl[5, 1], jpl[5, 2], jpl[5, 3], jpl[5, 4],jpl[5, 5], {0, 0, 0}}];Jo[5] = Transpose[{jol[5, 1], jol[5, 2], jol[5, 3], jol[5, 4],jol[5, 5], {0, 0, 0}}];Calcolo del Jacobiano del link 6Jp[6] = Transpose[{jpl[6, 1], jpl[6, 2], jpl[6, 3], jpl[6, 4], jpl[6, 5],jpl[6, 6]}];Jo[6] = Transpose[{jol[6, 1], jol[6, 2], jol[6, 3], jol[6, 4], jol[6, 5],jol[6, 6]}];Calcolo dei tensori d’inerzia baricentrici in terna fissaI0[i_] := R0[i].Ig[i].Transpose[R0[i]];Calcolo dei contributi alla matrice d’inerzia BBp[i_] := m[i]*Transpose[Jp[i]].Jp[i];Bo[i_] := Transpose[Jo[i]].I0[i].Jo[i];Costruzione della matrice d’inerzia BB = Sum[Bp[i] + Bo[i], {i, 1, 6}];Calcolo di C (matrice delle forze centrifughe e di Coriolis)calcolo i simboli di Christoffelch[i_, j_, k_] :=1/2 (D[B[[i, j]], q[t][[k]]] + D[B[[i, k]], q[t][[j]]] -D[B[[j, k]], q[t][[i]]])calcolo gli elementi della matrice Cc[i_, j_] := Sum[ch[i, j, k]*qp[t][[k]], {k, 1, 6}]Costruzione della matrice CCoriolis = {{c[1, 1], c[1, 2], c[1, 3], c[1, 4], c[1, 5], c[1, 6]}, {c[2, 1],26

5.2. Calcolo diretto della dinamica nel regressorec[2, 2], c[2, 3], c[2, 4], c[2, 5], c[2, 6]}, {c[3, 1], c[3, 2], c[3, 3],c[3, 4], c[3, 5], c[3, 6]}, {c[4, 1], c[4, 2], c[4, 3], c[4, 4], c[4, 5],c[4, 6]}, {c[5, 1], c[5, 2], c[5, 3], c[5, 4], c[5, 5],c[5, 6]}, {c[6, 1], c[6, 2], c[6, 3], c[6, 4], c[6, 5], c[6, 6]}};Calcolo di G (vettore dei contributi gravitazionali)G = -Sum[m[i] Transpose[Jp[i]].g0, {i, 1, 6}];5.2 Calcolo diretto della dinamica nel regressoreIl regressore e il vettore dei parametri dinamici relativi al manipolatore di Stanfordè ottenuto adoperando le relazioni matematiche analizzate nel Capitolo 3.Il codice che implementa la procedura in Mathematica è presentato in Appendice(7).Infine si verifica la correttezza del metodo considerato confrontando le τ ottenuterispettivamente utilizzando la formulazione classica della dinamica e il calcolo direttodel regressoreτ din = B(q) ¨q + C(q, ˙q) ˙q + G(q)τ regtramite la differenza τ din − τ reg .= Y (q, ˙q, ¨q) π(5.2)27

Capitolo 6Controllo adattativo di Slotine-Liper un manipolatore di StanfordIl calcolo diretto del regressore di Slotine-Li e il relativo controllo adattativo sonostati applicati al manipolatore di Stanford descritto nel capitolo 5. Le simulazionirelative a tale applicazione sono state implementate adoperando il software Mathematica6.0. Sono state scelte delle traiettorie di riferimento nello spazio dei giunticostituite da tratti sinusoidali aventi frequenze diverse alternati a segnali costanti.Inoltre sono stati considerate le seguenti matriciΛ = diag(10, 5, 5, 5, 5, 5)K π = 100 I 60(6.1)K D = 10 I 6dove diag() definisce una matrice diagonale i quali argomenti sono gli elementi delladiagonale e I n è la matrice identità di dimensioni n × n.28

ad0.4Θ10.2Θ1 desiderato20 40 60 80 100 120ts0.20.4Figura 6.1: Inseguimento del riferimento della variabile di giunto q 1rad0.4Θ20.2Θ2 desiderato20 40 60 80 100 120ts0.20.4Figura 6.2: Inseguimento del riferimento della variabile di giunto q 229

m0.4d30.2d3 desiderato20 40 60 80 100 120ts0.20.4Figura 6.3: Inseguimento del riferimento della variabile di giunto q 3radΘ40.5Θ4 desiderato20 40 60 80 100 120ts0.5Figura 6.4: Inseguimento del riferimento della variabile di giunto q 430

adΘ50.40.2Θ5 desiderato20 40 60 80 100 120ts0.20.4Figura 6.5: Inseguimento del riferimento della variabile di giunto q 5radΘ60.5Θ6 desiderato20 40 60 80 100 120ts0.5Figura 6.6: Inseguimento del riferimento della variabile di giunto q 631

mx desiderata0.100.05x20 40 60 80 100 120ts0.05m0.35y desiderata0.30y0.2520 40 60 80 100 120tsm0.7z desiderata0.6z0.50.40.320 40 60 80 100 120tsFigura 6.7: Inseguimento del riferimento dell’end effector (componenti x,y,z)32

Capitolo 7AppendiceUtilitiesOperatore WedgeS[w_] := {{0, -w[[3]], w[[2]]},{w[[3]], 0, -w[[1]]},{-w[[2]], w[[1]], 0}}Matrici di base del tensore d’inerziaE1 = {{1, 0, 0}, {0, 0, 0}, {0, 0, 0}};E2 = {{0, 1, 0}, {1, 0, 0}, {0, 0, 0}};E3 = {{0, 0, 1}, {0, 0, 0}, {1, 0, 0}};E4 = {{0, 0, 0}, {0, 1, 0}, {0, 0, 0}};E5 = {{0, 0, 0}, {0, 0, 1}, {0, 1, 0}};E6 = {{0, 0, 0}, {0, 0, 0}, {0, 0, 1}};EE = {E1, -E2, -E3, E4, -E5, E6};EEE = Transpose[EE, {2, 1, 3}];Costruzione dei Jacobiani dei frames di D-HFormule per il calcolo dei Jacobiani dei frames di D-HJacobiano di posizione33

jpdh[i_, j_] := z[2] /; j == 3; (* giunto prismatico *)jpdh[i_, j_] := S[z[j - 1]].(p[i] - p[j - 1]);(*giunto rotoidale*)Jacobiano di orientamentojodh[i_, j_] := {0, 0, 0} /; j == 3; (* giunto prismatico *)jodh[i_, j_] := z[j - 1];(*giunto rotoidale*)Calcolo del Jacobiano del frame {1}Jpdh[1] =Transpose[{jpdh[1, 1], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0,0}}];Jodh[1] =Transpose[{jodh[1, 1], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0,0}}];Calcolo del Jacobiano del frame {2}Jpdh[2] =Transpose[{jpdh[2, 1],jpdh[2, 2], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}];Jodh[2] =Transpose[{jodh[2, 1],jodh[2, 2], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}];Calcolo del Jacobiano del frame {3}Jpdh[3] =Transpose[{jpdh[3, 1], jpdh[3, 2],jpdh[3, 3], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}];Jodh[3] =Transpose[{jodh[3, 1], jodh[3, 2],jodh[3, 3], {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}];Calcolo del Jacobiano del frame {4}Jpdh[4] =Transpose[{jpdh[4, 1], jpdh[4, 2], jpdh[4, 3],jpdh[4, 4], {0, 0, 0}, {0, 0, 0}}];Jodh[4] =Transpose[{jodh[4, 1], jodh[4, 2], jodh[4, 3],jodh[4, 4], {0, 0, 0}, {0, 0, 0}}];Calcolo del Jacobiano del frame {5}Jpdh[5] =34

Transpose[{jpdh[5, 1], jpdh[5, 2], jpdh[5, 3], jpdh[5, 4],jpdh[5, 5], {0, 0, 0}}];Jodh[5] =Transpose[{jodh[5, 1], jodh[5, 2], jodh[5, 3], jodh[5, 4],jodh[5, 5], {0, 0, 0}}];Calcolo del Jacobiano del frame {6}Jpdh[6] =Transpose[{jpdh[6, 1], jpdh[6, 2], jpdh[6, 3], jpdh[6, 4], jpdh[6, 5],jpdh[6, 6]}];Jodh[6] =Transpose[{jodh[6, 1], jodh[6, 2], jodh[6, 3], jodh[6, 4], jodh[6, 5],jodh[6, 6]}];Quantità che contribuiscono al regressoreTermini da d/dt\[PartialD]T/\[PartialD]\!\(\*OverscriptBox["q", "."]\)Ordine 0X0pL[i_] := D[(Transpose[Jpdh[i]].Jpdh[i]).qp[t], t];Ordine 1X1pL[i_] :=D[(Transpose[Jpdh[i]].S[Jodh[i].qp[t]] -Transpose[Jodh[i]].S[Jpdh[i].qp[t]]).R0[i] , t];Ordine 2X2pL[i_] :=D[Transpose[Jodh[i]].R0[i].EEE.Transpose[R0[i]].Jodh[i].qp[t] , t];Termini da \[PartialD]T/\[PartialD]qOrdine 0W0L[i_] := (1/2) D[qp[t].Transpose[Jpdh[i]].Jpdh[i].qp[t], {q[t]}];Ordine 1W1L[i_] :=Transpose[D[Transpose[R0[i]].Transpose[S[Jodh[i].qp[t]]].Jpdh[i].qp[t], {q[t]}]];35

Ordine 2W2L[i_] := (1/2) Transpose[D[qp[t].Transpose[Jodh[i]].R0[i].EEE.Transpose[R0[i]].Jodh[i].qp[t], {q[t]}]];Termini da \[PartialD]U/\[PartialD]qOrdine 0Z0L[i_] := -Transpose[Jpdh[i]].g0;Ordine 1Z1L[i_] := -Transpose[D[g0.R0[i], {q[t]}]];Blocco di regressore relativo al link i-esimoOrdine 0Y0L[i_] := X0pL[i] - W0L[i] + Z0L[i];Ordine 1Y1L[i_] := X1pL[i] - W1L[i] + Z1L[i];Ordine 2Y2L[i_] := X2pL[i] - W2L[i];Blocco completoY[i_] := StackCols[Y0L[i], Y1L[i], Y2L[i]];Regressore completoY = StackCols[Y[1], Y[2], Y[3], Y[4], Y[5], Y[6]];Costruzione del vettore dei parametri dinamiciCostruzione di \[Pi] del link i-esimo\[Pi]0L[i_] := {m[i]};\[Pi]1L[i_] := m[i]*pgl[i];Idh[i_] := Ig[i] + m[i]*Transpose[S[pgl[i]]].S[pgl[i]];36

\[Pi]2L[i_] := {Idh[i][[1, 1]], Idh[i][[1, 2]], Idh[i][[1, 3]],Idh[i][[2, 2]], Idh[i][[2, 3]], Idh[i][[3, 3]]};Costruzione Vettore Completo dei parametriP[i_] := Join[\[Pi]0L[i], \[Pi]1L[i], \[Pi]2L[i]];P = Join[P[1], P[2], P[3], P[4], P[5], P[6]];Verifica Regressoretaureg = Y.P;taudin = B.qpp[t] + Coriolis.qp[t] + G;prova = (taureg - taudin);prova[[1]] // Simplify0prova[[2]] // Simplify0prova[[3]] // Simplify0prova[[4]] // Simplify0prova[[5]] // Simplify0prova[[6]] // Simplify037

Capitolo 8BibliografiaM. Gabiccini, Dispense del Corso di Robotica I, Facoltà di Ingegneria, Universitàdi Pisa, a.a. 2007/2008L. Sciavicco, B. Siciliano, Robotica Industriale, seconda edizione, McGraw-Hill, Milano,2000M. W. Spong, M. Vidyasagar, Robot Dynamics and Control, John Wiley & Sons,New York, 198938