analisi, estensioni, proprietà, identificazione, usideluca/rob2/05_dinamicalagrangiana_3.pdf ·...
TRANSCRIPT
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
"
#
$ $ $
%
&
' ' '
Robotica 2 A.A. 2008-2009 A. De Luca 3
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
Robotica 2 A.A. 2008-2009 A. De Luca 4
Robot con maglie chiuse
Comau Smart NJ130 MIT Direct Drive Mark II e Mark III
Robotica 2 A.A. 2008-2009 A. De Luca 5
Robot con maglie chiuse
MIT Direct Drive Mark IV(five-bar linkage planare)
UMinnesota Direct Drive Arm(five-bar linkage spaziale)
Robotica 2 A.A. 2008-2009 A. De Luca 6
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
Robotica 2 A.A. 2008-2009 A. De Luca 7
!
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
Robotica 2 A.A. 2008-2009 A. De Luca 8
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
Robotica 2 A.A. 2008-2009 A. De Luca 9
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
Robotica 2 A.A. 2008-2009 A. De Luca 10
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)
Robotica 2 A.A. 2008-2009 A. De Luca 11
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
Robotica 2 A.A. 2008-2009 A. De Luca 12
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)
Robotica 2 A.A. 2008-2009 A. De Luca 13
!
˙ ̇ 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ù…
Robotica 2 A.A. 2008-2009 A. De Luca 14
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
Robotica 2 A.A. 2008-2009 A. De Luca 15
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..)
Robotica 2 A.A. 2008-2009 A. De Luca 16
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
.
. .
Robotica 2 A.A. 2008-2009 A. De Luca 17
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
Robotica 2 A.A. 2008-2009 A. De Luca 18
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)
Robotica 2 A.A. 2008-2009 A. De Luca 19
!
˜ 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
Robotica 2 A.A. 2008-2009 A. De Luca 20
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
Robotica 2 A.A. 2008-2009 A. De Luca 21
… 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)
Robotica 2 A.A. 2008-2009 A. De Luca 22
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)). .
Robotica 2 A.A. 2008-2009 A. De Luca 23
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
#
$ %
&
' (
Robotica 2 A.A. 2008-2009 A. De Luca 24
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à
Robotica 2 A.A. 2008-2009 A. De Luca 25
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!!
Robotica 2 A.A. 2008-2009 A. De Luca 26
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...
Robotica 2 A.A. 2008-2009 A. De Luca 27
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