analisi, estensioni, proprietà, identificazione, usideluca/rob2/05_DinamicaLagrangiana_3.pdf · Robotica 2 A.A. 2008-2009 A. De Luca 2 Analisi accoppiamenti inerziali robot cartesiano

Corso di Robotica 2

Prof. Alessandro De Luca

Modello dinamico dei robot:analisi, estensioni, proprietà,

identificazione, usi

Robotica 2 A.A. 2008-2009 A. De Luca

Robotica 2 A.A. 2008-2009 A. De Luca 2

Analisi accoppiamenti inerziali

robot cartesiano

robot cartesiano “diagonale”

robot PR

robot 3R articolato

il modello dinamico risulta lineare se g ≡ 0 B costante ⇒ c ≡ 0

!

B =b

110

0 b22

"

# $

%

& '

!

B(q) =b11 b12(q2)

b21(q2) b22

"

# $

%

& '

!

B =b

11b

12

b21

b22

"

# $

%

& '

d = 0 in PRd2 = 0 in 2R

baricentro braccio 2su asse giunto 2

!

B(q) =

b11 q2,q3( ) 0 0

0 b22(q3) b23(q3)

0 b23(q3) b33

"

#

$ $ $

%

&

' ' '


Analisi termine di gravità bilanciamento statico

distribuzione masse e inerzie (inclusi i motori)

compensazione meccanica sistemi articolati di molle catene cinematiche chiuse

assenza di gravità applicazioni spaziali U costante (moto su piano orizzontale)

!

g(q) " 0


Robot con maglie chiuse

Comau Smart NJ130 MIT Direct Drive Mark II e Mark III


Robot con maglie chiuse

MIT Direct Drive Mark IV(five-bar linkage planare)

UMinnesota Direct Drive Arm(five-bar linkage spaziale)


Robot a parallelogrammacinematica e dinamica (planare)

q1

q2

q2 - π

1

2

34

!

lc1

!

lc2

!

lc3

!

lc4

baricentri:

parallelogramma:

!

l1

= l3

!

l2

= l4

!

lci arbitrarie

!

pc1 =l c1c1

l c1s1

"

# $

%

& '

!

pc2 =l c2c2

l c2s2

"

# $

%

& '

!

pc3 =l 2c2

l 2s2

"

# $

%

& ' +

l c3c1

l c3s1

"

# $

%

& '

!

pc4 =l1c1

l1s1

"

# $

%

& ' (

l c4c2

l c4s2

"

# $

%

& '

POSIZIONI BARICENTRI

5

!

pEE =l1c1

l1s1

"

# $

%

& ' +

l 5 cos(q2 ())

l 5 sin(q2 ())

"

# $

%

& ' =

l1c1

l1s1

"

# $

%

& ' (

l 5 c2

l 5 s2

"

# $

%

& '

!

l5

E-E

x

y

CINEMATICA DIRETTA


!

vc1 ="l c1s1

l c1c1

#

$ %

&

' ( ˙ q 1

!

vc3 ="l c3s1

l c3c1

#

$ %

&

' ( ˙ q 1 +

"l 2s2

l 2s2

#

$ %

&

' ( ˙ q 2

!

vc4 ="l1s1

l1c1

#

$ %

&

' ( ˙ q 1 "

"l c4s2

l c4c2

#

$ %

&

' ( ˙ q 2

Calcolo energia cinetica

VELOCITA’ LINEARI E ANGOLARI

!

vc2 ="l c2s2

l c2c2

#

$ %

&

' ( ˙ q 2

!

"1 ="3 = ˙ q 1

!

"2 ="4 = ˙ q 2

!

T1 =1

2m1l c1

2 ˙ q 12

+1

2I1,zz

˙ q 12

!

T2 =1

2m2l c2

2 ˙ q 22

+1

2I2,zz

˙ q 22

!

T3 =1

2I3,zz

˙ q 12 +

1

2m3 l 2

2 ˙ q 22 + l c3

2 ˙ q 12 + 2l 2l c3c2"1

˙ q 1 ˙ q 2( )

!

T4 =1

2I4,zz

˙ q 22 +

1

2m4 l1

2 ˙ q 12 + l c4

2 ˙ q 22"2l1l c4c2"1

˙ q 1 ˙ q 2( )

Ti


Matrice di inerzia

!

T = Ti =1

2˙ q TB(q) ˙ q

i=1

4

"

!

B(q) =I1,zz + m1l c1

2 + I3,zz + m3l c3

2 + m4l1

2 symm

m3l 2l c3 "m4l1l c4( )c2"1 I2,zz + m2l c2

2 + I4,zz + m4l c4

2 + m3l 2

2

#

$ % %

&

' ( (

!

m3l

2l

c3= m

4l

1l

c4

B(q) diagonale e costante ⇒ termini centrifughi e di Coriolis ≡ nulli

modello dinamico (a meno di g(q)) meccanicamente DISACCOPPIATO e LINEARE

vantaggio per il progetto del controllore del moto!

(*)condizione strutturalenel progetto meccanico


Energia potenziale e termini di gravitàdalle componenti y dei vettori pci

!

U1 = m1g0l c1s1

!

U2 = m2g0l c2s2

!

U3 = m3g0 l 2s2 + l c3s1( )

!

U4 = m4g0 l1s1 " l c4s2( )

Ui

!

U = Ui

i=1

4

"

!

g(q) ="U

"q

#

$ %

&

' (

T

=g0 m1l c1 + m3l c3 + m4l1( )c1

g0 m2l c2 + m3l 2 )m4l c4( )c2

#

$ %

&

' ( =

g1(q1)

g2(q2)

#

$ %

&

' (

!

b11˙ ̇ q 1 + g1(q1) = u1

b22˙ ̇ q 2 + g2(q2) = u2

nel caso (*)

componenti disaccoppiate

ui coppia (non conservativa)che compie lavoro su qi

ulteriori condizioni strutturali nel progetto meccanico portano a g(q) ≡ 0


Aggiunta di termini dinamici fenomeni dissipativi per attrito ai giunti

viscoso, secco, di primo distacco, di Coulomb … effetti locali ai giunti di difficile modellizzazione in generale, tranne per

inclusione degli attuatori elettrici (visti come corpi rigidi) motore i montato sul braccio i-1 (o precedente), in genere in

asse con il giunto i massa motore (bilanciata sull’asse) inclusa in quella del braccio inerzia (del rotore) da aggiungere nell’energia cinetica trasmissione con riduttori (spesso con rapporto elevato) a volte più motori cooperano per muovere più giunti: matrice di

trasmissione T con accoppiamenti (elementi fuori diagonale)

!

uV,i = "FV,i˙ q i

!

uS,i = "FS,isgn(˙ q i)


Dislocazione dei motori

SR0SR1

SRN - 1

braccio 0(base)

braccio 1

giunto 1

braccio N - 1

braccio 2 braccio Ngiunto 2

SRw(world frame)

giunto N

SRN

motore 1

motore 2

motore N

θm2

.

θm1

.θmN

.

θmi = nri qi

. .

τi = nriτmi


Modello dinamico finale

semplificazione: nella parte rotazionale dell’energia cinetica,si considera solo la velocità di “spinning” del rotore

!

Tmi =1

2Imi

˙ " mi

2=

1

2Iminri

2 ˙ q i2

=1

2Bmi

˙ q i2

!

Tm = Tmii=1

N

" =1

2˙ q T Bm

˙ q

diagonale

con tutti i termini aggiuntivi, il modello diventa

!

B(q) + Bm( )˙ ̇ q + c(q, ˙ q ) + g(q) + FV˙ q + FS sgn(˙ q ) = "

costante non fornisce contributi in c Fv > 0, FS > 0

diagonalicoppie motrici

(a valle deiriduttori)


!

˙ ̇ q 1 c2 2˙ ̇ q 1 + ˙ ̇ q 2( ) " s2˙ q 2

2 + 2˙ q 1˙ q 2( ) ˙ ̇ q 2 c1 c12

0 c2˙ ̇ q 1 + s2

˙ q 12 ˙ ̇ q 2 0 c12

#

$ %

&

' (

a1

a2

a3

a4

a5

#

$

% % % % % %

&

'

( ( ( ( ( (

=u1

u2

#

$ %

&

' (

Parametrizzazione lineare della dinamica

è sempre possibile riscrivere il modello dinamico nella forma

!

B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = Y(q, ˙ q ,˙ ̇ q ) a = u

N × p p × 1

a = vettore dei coefficienti dinamici

ESEMPIO 2R

matrice diregressione

N.B.: con attrito viscoso e secco, quattro coefficienti in più…


Coefficienti dinamici del robot 2R

!

a1

= I1,zz

+ m1d

1

2+ I

2,zz+ m

2d

2

2+ m

2l

1

2

!

a2

= m2l

1d

2

!

a3

= I3,zz

+ m2d

2

2

!

a4 = g0 m1d1 + m2l1( )

!

a5 = g0m2d2

!

a2 = l1m2d2 = l1" a 2 a5 = g0m2d2 = g0

" a 2 # " a 2 = m2d2

N.B. se g0 noto e noto (cinematica!), allora

!

l1

sono sufficienti4 coefficienti!

10 parametri dinamici perogni braccio (corpo rigido):

m [1], rc [3], I [6]

è importante determinare una parametrizzazione minima per l’identificazione sperimentale dei coefficienti dinamici il controllo adattativo del moto in presenza di incertezze parametriche

si combinano per fornire icoefficienti dinamici del robot:tutti e soli quelli rilevanti nelmodello dinamico del robot


Identificazione dinamica

per “usare” il modello, occorre conoscere i valori numericidei coefficienti dinamici del robot

al più, solo i principali parametri dinamici sono forniti dal costruttore

stime approssimate sono deducibili con strumenti CAD i coefficienti di attrito sono (lentamente) variabili nel tempo

lubrificazione dei giunti/trasmissioni

in presenza di un “payload” (carico solidale all’E-E) variano i 10 parametri dell’ultimo braccio questo si riflette in variazioni di tutti (o quasi) i coefficienti dinamici

necessari esperimenti preliminari di identificazione robot in movimento (aspetti “dinamici”, non statici o geometrici!) identificabili solo i coefficienti dinamici (ma non tutti i parametri..)


Esperimenti di identificazione

1. scegliere una traiettoria di moto “eccitante” esplora lo spazio di lavoro e coinvolge tutta la dinamica del robot periodica, con molteplici componenti frequenziali

2. eseguire il moto in modo approssimato con un controllore sfruttando l’informazione disponibile sul modello in genere, u = KP(qd-q)+KD(qd-q) (PD, nessuna informazione)

3. misurare q (encoder) e, se disponibile, q in nc istanti accelerazione (oltre a velocità) ricavabile fuori linea mediante

differenziazione numerica (possibile l’uso di filtri non causali)

4. con tali misure/stime e i comandi applicati, valutare ilregressore Y a sinistra e la parte destra nell’espressione

!

Y(q(tk), ˙ q (tk),˙ ̇ q (tk)) a = u(tk) k = 1,…, nc

.

. .


Identificazione ai minimi quadrati

costruire il sistema lineare di equazioni

traiettorie “eccitanti” e selezione del numero di campioni(nc × N ≫ p) e loro posizione garantiscono rango(Y) = p

risolvere mediante pseudo-inversione della Y

eventualmente si può “pesare” la pseudo-inversa, pertener conto di livelli differenti di rumore sulle misure

!

Y(q(t1), ˙ q (t1),˙ ̇ q (t1))

M

Y(q(tnc), ˙ q (tnc

),˙ ̇ q (tnc))

"

#

$ $ $

%

&

' ' '

a =

u(t1)

u(tnc)

"

#

$ $ $

%

&

' ' '

!

Y a = u

_

_

!

a = Y #u = Y

TY ( )

-1

Y Tu

nc × N


Dinamica inversa

data una traiettoria desiderata di moto qd(t) differenziabile due volte (∃ qd(t)) eventualmente ottenuta da una rd(t) cartesiana, mediante

inversione cinematica (differenziale)

la coppia motrice necessaria ad eseguirla è

..

questo calcolo algebrico (∀t) non è però efficientecon l’approccio Lagrangiano

termini simbolici molto lunghi in tempo reale, meglio il calcolo numerico con Newton-Eulero utile per il controllo (feedforward nominale)

!

"d = B(qd) + Bm( )˙ ̇ q d + c(qd, ˙ q d) + g(qd) + FV˙ q d + FS sgn(˙ q d)


!

˜ x =q

B(q)˙ q

"

# $

%

& '

Equazioni di stato(Dinamica diretta)

!

B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = u

equazioni di stato

N equazionidifferenziali

del 2° ordine

definendo il vettore di variabili di stato

!

x =x1

x2

"

# $

%

& ' =

q

˙ q

"

# $ %

& ' ( IR2N

!

˙ x =˙ x 1˙ x 2

"

# $

%

& ' =

x2

(B(1(x1) c(x1,x2) + g(x1)[ ]

"

# $

%

& ' +

0

B(1(x1)

"

# $

%

& ' u

= f(x) + G(x)u

2N equazionidifferenziali

del 1° ordine

2N×1 2N×N

altra possibile scelta x = ... (esercizio)~.

modello dinamicoLagrangiano

momentogeneralizzato


Simulazione dinamica

u

!

˙ q 1(0)

!

q1(0)

q1

!

˙ ̇ q 1

!

˙ q 1

g(q)

!

˙ q 2 (0)

!

q2 (0)

q2

!

˙ ̇ q 2

!

˙ q 2

!

q, ˙ q

!

q

!

c(q, ˙ q )

B-1(q)

!

"

!

"

!

"

!

"

qui, generico robot a 2 dof

!

q

+

_

_

inizializzazione (coefficienti dinamici e stato iniziale) chiamate a funzioni Matlab per valutazione numerica termini modello scelta metodo di integrazione (e suoi parametri)

comando d’ingresso(ad anelloaperto o

in feedback)

inclusa “inv(B)”

schemaa blocchiSimulink


… una realizzazione scorretta

u1

c1+ g1

!

1

b11

!

"

!

˙ q 1(0)

!

q1(0)

q1

!

˙ ̇ q 1

!

˙ q 1

u2

c2 + g2

!

1

b22

!

˙ q 2 (0)

!

q2 (0)

q2

!

˙ ̇ q 2

!

˙ q 2

b12

b12loop algebrico

NO!viola il

principio di causalità

_

_

_

_

!

"

!

"

!

"

occorre invertire l’intera matrice di inerzia(non solo i suoi termini in diagonale)


Linearizzazione approssimata

!

B(qd)˙ ̇ q d + c(qd, ˙ q d) + g(qd) = ud

si può ricavare un modello dinamico lineare del robot,valido localmente intorno alla condizione operativa utile per l’analisi e il progetto di controllori locali/lineari approssimazione mediante sviluppo di Taylor al primo ordine linearizzazione intorno ad uno stato di equilibrio (constante) o

intorno ad una traiettoria di equilibrio (nominale, tempo variante) si può procedere lavorando sulle equazioni di stato, ma conviene

direttamente usare il modello del secondo ordine il risultato è lo stesso, ma la derivazione è più facile…

!

g(qe) = uestato di equilibrio (q,q) = (qe,0) [ qe=0 ]. ..

traiettoria di equilibrio (q,q) = (qd (t),qd(t)). .


Linearizzazione in un punto

variazioni intorno ad uno stato di equilibrio

tenendo conto della dipendenza quadratica di c dallevelocità (termini ininfluenti intorno a velocità nulla)

nello spazio di stato

!

q = qe + "q ˙ q = ˙ q e + "˙ q = "˙ q ˙ ̇ q = ˙ ̇ q e + "˙ ̇ q = "˙ ̇ q u = ue + "u

!

B qe( )"˙ ̇ q + g(qe) +# g

# qq=qe

"q + o "q , "˙ q ( ) = ue + "u

!

G(qe)

infinitesimi di ordinesuperiore al primo

!

"˙ x =0 I

-B-1 qe( )G(qe) 0

#

$ %

&

' ( "x +

0

B-1 qe( )#

$ %

&

' ( "u = A "x + B "u

!

"x ="q

"˙ q

#

$ %

&

' (


Linearizzazione lungo traiettoria

variazioni intorno ad una traiettoria di equilibrio

sviluppando i vari termini nel modello dinamico

!

q = qd + "q ˙ q = ˙ q d + "˙ q ˙ ̇ q = ˙ ̇ q d + "˙ ̇ q u = ud + "u

!

B qd + "q( )"˙ ̇ q + c(qd + "q, ˙ q d + "˙ q ) + g(qd + "q) = ud + "u

!

B(qd + "q) #B(qd) +$ bi

$ qi=1

N

%q=qd

eiT"q

!

c(qd + "q, ˙ q d + "˙ q ) # c(qd, ˙ q d) +$ c

$ q q=qd˙ q = ˙ q d

"q +$ c

$ ˙ q q=qd˙ q = ˙ q d

"˙ q

!

g(qd + "q) # g(qd) + G(qd)"q

!

C1(qd, ˙ q d)

!

C2(qd, ˙ q d)

i-esima riga dellamatrice Identità


Linearizzazione lungo traiettoria (cont)

semplificando…

con

nello spazio di stato

!

B qd( )"˙ ̇ q + C2(qd, ˙ q d)"˙ q + D(qd, ˙ q d,˙ ̇ q d)"q = "u

!

"˙ x =0 I

-B-1 qd( )D(qd, ˙ q d,˙ ̇ q d) -B-1 qd( )C2(qd, ˙ q d)

#

$ %

&

' ( "x +

0

B-1 qd( )#

$ %

&

' ( "u

= A t( )"x + B t( )"u

!

D(qd, ˙ q d,˙ ̇ q d) = G(qd) + C1(qd, ˙ q d) +" bi

" qi=1

N

#q=qd

˙ ̇ q d eiT

un sistema lineare, ma tempo variante!!


Trasformazione di coordinate

!

B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = B(q)˙ ̇ q + n(q, ˙ q ) = uq

!

q" IRN

!

p" IRN

!

p = f(q)

!

q = f "1(p)

se si vogliono utilizzare nuove coordinate generalizzate p

!

˙ p ="f(q)

"q˙ q = J(q) ˙ q

!

˙ q = J"1(q) ˙ p

!

˙ ̇ p = J(q)˙ ̇ q + ˙ J (q) ˙ q

!

˙ ̇ q = J"1(q) ˙ ̇ p " ˙ J (q)J"1

(q)˙ p [ ]

1

1

!

B(q)J"1(q)˙ ̇ p "B(q)J"1

(q)˙ J (q)J"1(q) ˙ p + n(q, ˙ q ) = JT

(q)up

!

uq = JT(q)up,

!

J"T(q) # premoltiplicando tutta l’equazione...


Trasformazione del modello

!

J"T(q)B(q)J"1

(q)˙ ̇ p + J"T(q) n(q, ˙ q ) "B(q)J"1(q)˙ J (q)J"1

(q)˙ p ( ) = up

!

q"p

!

q, ˙ q "p, ˙ p

!

Bp(p)˙ ̇ p + cp(p, ˙ p ) + gp(p) = up

!

Bp = J"TBJ"1

!

cp = J"T c "BJ"1˙ J J"1˙ p ( ) = J"Tc "Bp

˙ J J"1˙ p

!

gp = J"Tg

!

cp p, ˙ p ( ) = Sp p, ˙ p ( )˙ p

!

˙ B p " 2Sp antisimmetrica

per calcolo e analisiqueste sostituzioni non sono necessarie

simmetrica edefinita positiva

(fuori da singolarità)

dipendenzaquadratica da p

.

se p = posa dell’E-E, questo è il modello dinamico cartesiano del robot

forze generalizzatenon conservativeche compiono lavoro sulle p

Documents

analisi, estensioni, proprietà, identificazione, usideluca/rob2/05_DinamicaLagrangiana_3.pdf · Robotica 2 A.A. 2008-2009 A. De Luca 2 Analisi accoppiamenti inerziali robot cartesiano