q - Polimi

Controllo del moto e
robotica industriale
Dinamica del manipolatore
Prof. Paolo Rocco ([email protected])
Energia cinetica
Consideriamo un punto materiale di massa m, la
cui posizione sia descritta dal vettore p rispetto ad
una terna xyz.
Definiamo energia cinetica del punto la quantità:
1
T = mp T p
2
Analogamente, per un sistema di punti materiali:
1
T=
2
z
P
x
n
∑
mi piT pi
z
i =1
Consideriamo ora un corpo rigido, di massa m,
volume V e densità ρ. L’energia cinetica è
definita come:
T=
1
2
y
O
∫
pT pρdV
P
O
dV
y
x
V
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [2]
Energia potenziale
Un sistema di forze posizionali (cioè dipendenti solo dalle posizioni dei
punti d’applicazione) si dice conservativo se il lavoro compiuto da
ciascuna forza non dipende dalla traiettoria seguita dal punto di
applicazione ma solo dalla sua posizione iniziale e finale.
In questo caso il lavoro elementare coincide con il differenziale, cambiato
di segno, di una funzione che prende il nome di energia potenziale:
dW = − dU
Un esempio di sistema di forze conservativo è la forza gravitazionale.
Per un punto materiale avremo l’energia potenziale:
U = − mg 0T p
dove g0 è il vettore accelerazione di gravità.
Per un corpo rigido:
U =−
∫
V
g 0T pρdV = − mg 0T pl
con pl posizione del baricentro.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [3]
Sistemi di corpi
Si consideri ora un sistema di r corpi rigidi (quali, per esempio, i bracci di un
robot). Se tutti i corpi sono liberi di muoversi nello spazio, il moto del sistema è
ad ogni istante descrivibile per mezzo di 6r coordinate x.
Supponiamo ora che esistano delle limitazioni al moto dei corpi che
costituiscono il sistema (quali quelle indotte dalla connessione di un braccio
con il successivo tramite un giunto, che elimina cinque su sei gradi di mobilità
relativi tra i due bracci).
Diciamo che sul moto dei corpi esiste un vincolo, che esprimeremo con la
relazione:
h( x ) = 0
Un vincolo rappresentato da un’equazione di questo tipo si dice olonomo
(perché dipende solo dalle coordinate di posizione e non dalle velocità) e
stazionario (perché non dipende dal tempo).
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [4]
Coordinate libere
h( x ) = 0
Se il vincolo h è costituito da s componenti scalari e tutte sono continue e
differenziabili con continuità, è possibile, per mezzo del vincolo, eliminare s
coordinate dalle equazioni del sistema.
Le rimanenti n=6r – s coordinate prendono il nome di coordinate libere, o
lagrangiane, o generalizzate, e n è il numero di gradi di libertà del sistema
meccanico.
Per esempio, in un robot a 6 giunti, delle 36 coordinate originarie, se ne
eliminano 30 in virtù dei vincoli imposti dai 6 giunti e ne rimangono 6 che
costituiscono le coordinate lagrangiane: tipicamente si scelgono le coordinate di
giunto (l’angolo ϑ o la distanza d, rispettivamente per giunto rotazionale o
prismatico).
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [5]
Equazioni di Lagrange
Sia dato quindi un sistema di corpi rigidi, le posizioni ed orientamenti dei
quali si possano esprimere per mezzo di n coordinate generalizzate qi.
Definiamo lagrangiana del sistema meccanico la quantità:
L = T −U
essendo T e U rispettivamente l’energia cinetica e l’energia potenziale del
sistema. Siano poi ξi le forze generalizzate associate alle coordinate
generalizzate qi. Il lavoro elementare compiuto dalle forze agenti sul sistema si
può esprimere come:
n
dW =
∑ ξ dq
i
i
i =1
Si può dimostrare che la dinamica del sistema di corpi è retta dalle seguenti
equazioni di Lagrange:
d ∂L ∂L
−
= ξ i , i = 1, …, n
dt ∂qi ∂qi
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [6]
Esempio
Consideriamo un sistema costituito da un
motore rigidamente connesso ad un carico
soggetto a forza gravitazionale.
Siano Im e I i momenti di inerzia di motore
e carico rispetto all’asse del motore, m la
massa del carico, l la distanza del
baricentro del carico dall’asse del motore.
Energia cinetica del motore
y
C
l
ϑ
x
τ
mg
1
Tm = I m ϑ 2
2
Energia cinetica del carico
Tc =
1 2
Iϑ
2
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [7]
Esempio
ϑ
l
τ
mg
Lagrangiana:
Energia potenziale gravitazionale:
⎡l cos ϑ⎤
U = −mg 0T pl = −m[0 − g ]⎢
= mgl sin ϑ
⎥
⎣ l sin ϑ ⎦
L = Tm + Tc − U =
1
(I m + I )ϑ2 − mgl sin ϑ
2
Equazione di Lagrange:
d ∂L ∂L
d
(I + I m )ϑ + mgl cos ϑ = τ
−
=τ ⇒
dt ∂ϑ ∂ϑ
dt
(
)
Pertanto:
(I + I m )ϑ + mgl cos ϑ = τ
L’equazione si lascia facilmente interpretare come l’equilibrio dei momenti
rispetto all’asse di rotazione.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [8]
Energia cinetica di un braccio
Il contributo di energia cinetica di
un singolo braccio di un
manipolatore si può calcolare per
mezzo dell’integrale:
1
*T *
Ti =
pi pi ρdV
V
2 i
pi* punto generico sul link
∫
Posizione del baricentro:
pli =
1
mi
∫
Vi
pi*ρdV
*
Velocità del generico punto: pi = pli + ω i × ri = pli + S (ω i )ri
dove:
⎡ 0
S (ω i ) = ⎢ ωiz
⎢
⎢⎣− ωiy
− ωiz
0
ωix
ωiy ⎤
− ωix ⎥
⎥
0 ⎥⎦
(matrice antisimmetrica).
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [9]
Energia cinetica di un braccio
Contributo traslazionale:
1
2
∫
Vi
pli T pli ρdV =
1
mi pli T pli
2
Contributo mutuo:
∫
Vi
∫ (p
pli T S (ω i )ri ρdV = pli T S (ω i )
Vi
*
i
)
− pli ρdV = 0
Contributo rotazionale:
1
2
∫
Vi
riT S T
1 T⎛
(ωi )S (ωi )ri ρdV = ωi ⎜ S T (ri )S (ri )ρdV ⎞⎟ωi =
2
⎠
⎝ Vi
1
= ωTi I i ω i
2
∫
N.B.
S (ω i )ri = − S (ri )ω i
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [10]
Tensore di inerzia
Definiamo il tensore di inerzia:
∫(
)
⎡ r 2 + r 2 ρdV
⎢ iy iz
*
Ii = ⎢
⎢
⎢
*
⎢⎣
∫
− rix riy ρdV
∫ (r
2
ix
)
+ riz2 ρdV
*
− rix riz ρdV ⎤
⎥ ⎡ I ixx
− riy riz ρdV ⎥ = ⎢⎢ *
⎥
⎢
rix2 + riy2 ρdV ⎥ ⎣ *
⎥⎦
∫(
∫
∫
)
− I ixy
I iyy
*
− I ixz ⎤
− I iyz ⎥⎥
I izz ⎥⎦
(matrice simmetrica)
Il tensore di inerzia, espresso in terna base, dipende dalla configurazione del
manipolatore. Se la velocità angolare è espressa rispetto ad una terna
solidale al braccio i (per esempio la terna DH), ovvero:
ω ii = RiT ω i
e il tensore di inerzia rispetto a tale terna è costante. Risulta inoltre:
I i = Ri I ii RiT
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [11]
Somma dei contributi
Sommiamo il contributo traslazionale con quello rotazionale:
1
1 T
T
Ti = mi pli pli + ω i Ri I ii RiT ω i
2
2
Velocità lineare:
(li )
pli = j P(l1i )q1 + … + j Pi
qi = J P(li )q
Velocità angolare:
(li )
ω i = jO(l1i )q1 + … + jOi
qi = J O(li )q
Colonne dello Jacobiano:
[
(li )
j Pi
0
0
]
[
(li )
jOi
0
0
]
J P(li ) = j P(l1i )
J O(li ) = jO(l1i )
⎧⎡ z j −1 ⎤
⎪⎢
⎥
⎪
(
)
l
⎢⎣ 0 ⎥⎦
⎡ j Pji ⎤
⎪
⎢
⎥=⎨
(
)
l
⎢j i ⎥ ⎪
⎡ z j −1 × pli − p j −1
⎣ Oj ⎦
⎪⎢
⎪⎢
z j −1
⎩⎣
(
giunto prismatico
)⎤
⎥
⎥⎦
giunto rotoidale
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [12]
Matrice di inerzia
Sviluppando le velocità lineari ed angolari:
1
1 T (li )T
T (li )T (li )
Ti = mi q J P J P q + q J O Ri I ii RiT J O(li )q
2
2
Sommando i contributi di tutti i bracci si ottiene l’espressione dell’energia
cinetica dell’intero manipolatore:
1
T=
2
n
n
∑∑
bij (q )qi q j =
i =1 j =1
1 T
q B (q )q
2
dove:
B (q ) =
( ) ( )
( )
(
m
J
J
+
J
RI R
∑
n
i
li T
P
li
P
li T
O
i
i i
T (li )
i JO
i =1
è la matrice di inerzia del manipolatore.
)
⎧ simmetrica
⎪⎪
⎨ definita positiva
⎪
⎪⎩ dipendente da q
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [13]
Energia potenziale
L’energia potenziale di un braccio rigido è dovuta solo alla forza peso:
Ui = −
∫
V
g 0T pi*ρdV = − mi g 0T pli
dove g0 è il vettore dell’accelerazione di gravità espresso in terna base.
L’energia potenziale dell’intero manipolatore è semplicemente la somma
dei contributi relativi ai singoli link:
n
n
U=
∑
i =1
Ui = −
∑
i =1
mi g 0T pli
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [14]
Equazioni del moto
Lagrangiana del manipolatore:
n n
1
L(q, q ) = T (q, q ) − U (q ) =
bij (q )qi q j +
2 i =1 j =1
∑∑
n
∑
mi g 0T pli (q )
i =1
Deriviamo la lagrangiana:
d ⎛ ∂L
⎜
dt ⎜⎝ ∂qi
⎞ n
bij (q )q j ⎟ =
bij (q )q j +
⎟
j =1
⎠ j =1
n ⎡ n
n
∂bij (q ) ⎤
bij (q )q j +
qk ⎥ q j
=
⎢
⎥⎦
j =1 ⎢
j =1
⎣ k =1 ∂q k
⎞ d ⎛ ∂T
⎟= ⎜
⎟ dt ⎜ ∂q
⎠
⎝ i
∑
⎞ d ⎛⎜
⎟=
⎟ dt ⎜
⎠
⎝
n
∑
∑
n
∑
dbij (q )
j =1
dt
qj =
∑∑
Inoltre:
∂T 1 n n ∂b jk (q )
= ∑∑
qk q j
∂qi 2 j =1k =1 ∂qi
n
n
∂plj
∂U
(l )
T
= − m j g0
= − m j g 0T j P j (q ) = g i (q )
i
∂qi
∂qi
j =1
j =1
∑
∑
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [15]
Equazioni del moto
Dalle equazioni di Lagrange otteniamo
n
n
n
∑ b (q )q + ∑∑ h
ij
j =1
dove:
ijk
j
(q )qk q j + g i (q ) = ξ i
i = 1,… , n
j =1 k =1
hijk
∂bij
1 ∂b jk
=
−
∂q k 2 ∂qi
termine gravitazionale dipendente
solo dalla configurazione
Termini di accelerazione:
bii : momento di inerzia visto all’asse del giunto i
bij : effetto dell’accelerazione del giunto j sul giunto i
Termini quadratici in velocità:
.
hijjqj2 : effetto centrifugo indotto al giunto i dalla velocità del giunto j
..
hijkqjqk : effetto di Coriolis indotto al giunto i dalle velocità dei giunti j e k
hiii = 0
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [16]
Forze non conservative
Oltre alle forze gravitazionali conservative, sul manipolatore agiscono:
τ
• coppie di attuazione
• coppie di attrito viscoso
− Fv q
• coppie di attrito statico
− f s (q, q )
T
• coppie che equilibrano le forze di contatto − J (q )h
Fv
matrice diagonale di coefficienti di attrito viscoso
f s (q, q )
funzione che modella l’attrito statico al giunto
h
forze e momenti al contatto
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [17]
Modello dinamico completo
In termini vettoriali il modello matematico del manipolatore si esprime:
B (q )q + C (q, q )q + Fv q + f s (q, q ) + g (q ) = τ − J T (q )h
dove C è un’opportuna matrice n×n i cui elementi soddisfano la relazione:
n
n
n
∑ c q = ∑∑ h
ij
j =1
ijk q k q j
j
j =1 k =1
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [18]
Calcolo degli elementi di C
La scelta della matrice C non è univoca. Una possibilità è la seguente:
n
n
n
n
∑ c q = ∑∑ h
ij
ijk q k q j
j
j =1
1
=
2
=
j =1 k =1
n
n
∑∑
j =1
1
qk q j +
2
∂q k
k =1
∑∑
j =1
∂bij
n
⎛ ∂bij 1 ∂b jk ⎞
⎜
⎟
⎜ ∂q − 2 ∂q ⎟q k q j =
k
i ⎠
k =1 ⎝
⎛ ∂bik ∂b jk
⎜
−
⎜ ∂q j
∂qi
k =1 ⎝
n
∑∑
j =1
n
⎞
⎟q k q j
⎟
⎠
Il generico elemento di C risulta:
n
cij =
∑c
ijk q k
k =1
dove:
cijk
1 ⎛⎜ ∂bij ∂bik ∂b jk
=
+
−
⎜
2 ⎝ ∂q k ∂q j
∂qi
⎞
⎟
⎟
⎠
Simboli di Christoffel del
primo tipo
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [19]
.
Anti-simmetria di B−2C
La precedente scelta della matrice C consente di dimostrare un’importante
proprietà del modello dinamico del manipolatore. Definita la matrice:
N (q, q ) = B (q ) − 2C (q, q )
essa risulta anti-simmetrica:
w T N (q, q )w = 0, ∀w
Infatti:
⎛ ∂bik ∂b jk
⎜
−
⎜ ∂q j
∂qi
k =1 ⎝
n ⎛
∂bik ∂b jk ⎞⎟
1
1
⎜
qk
= bij +
−
⎜
⎟
2
2 k =1 ⎝ ∂q j
∂qi ⎠
1
cij =
2
n
∂bij
1
qk +
2
∂q k
k =1
∑
n
∑
⎞
⎟qk =
⎟
⎠
∑
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [20]
.
Anti-simmetria di B−2C
Ne consegue:
⎛ ∂b jk ∂bik
⎜
nij = bij − 2cij =
−
⎜ ∂qi
∂q j
k =1 ⎝
n
∑
⎞
⎟q k
⎟
⎠
e quindi:
nij = − n ji
ovvero la matrice è anti-simmetrica.
.
Ponendo w = q si ottiene, in particolare:
q T N (q, q )q = 0
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [21]
Principio di conservazione dell’energia
L’equazione:
q T N (q, q )q = 0
(caso particolare della precedente) è tuttavia valida qualunque sia la scelta
della matrice C. Infatti, dal principio di conservazione dell’energia, la derivata
dell’energia cinetica bilancia la potenza generata da tutte le forze agenti ai
giunti del manipolatore:
(
)
(
)
(
)
1 d T
q B (q )q = q T τ − Fv q − f s (q, q ) − g (q ) − J T (q )h
2 dt
eseguendo la derivata a primo membro, ed utilizzando l’equazione del modello:
(
)
1 d T
1
1
q B (q )q = q T B (q )q + q T B (q )q = q T B (q ) − 2C (q, q ) q
2 dt
2
2
+ q T τ − Fv q − f s (q, q ) − g (q ) − J T (q )h
(
)
da cui l’equazione cercata.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [22]
Linearità nei parametri dinamici
Se si assume per la funzione che definisce l’attrito statico un’espressione
semplificata:
f s (q, q ) = Fs sgn (q )
è possibile dimostrare che il modello dinamico del manipolatore risulta lineare
rispetto ad un insieme opportuno di parametri dinamici (masse, momenti primi
di inerzia, momenti di inerzia).
Si può in altre parole scrivere:
τ = Y (q, q, q )π
π: vettore di p parametri costanti
Y: matrice n×p funzione di posizioni, velocità ed accelerazioni dei giunti
(regressore)
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [23]
Manipolatore cartesiano a due bracci
d2
m2
z1
m1
⎡d ⎤
q = ⎢ 1⎥
⎣d 2 ⎦
z0
x0
d1
Consideriamo un manipolatore cartesiano a due bracci,
caratterizzati dalle masse m1 e m2.
Il vettore delle coordinate generalizzate risulta:
Gli Jacobiani necessari per il calcolo della matrice di inerzia sono i seguenti:
(l1 )
JP
⎡0 0 ⎤
= ⎢0 0 ⎥
⎢⎣1 0⎥⎦
(l2 )
JP
⎡0 1 ⎤
= ⎢0 0 ⎥
⎢⎣1 0⎥⎦
mentre non ci sono contributi dovuti a velocità angolare.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [24]
Manipolatore cartesiano a due bracci
Calcolando la matrice di inerzia con la formula generale, si ottiene:
⎡ m + m2
B = m1J P(l1 )T J P(l1 ) + m2 J P(l2 )T J P(l2 ) = ⎢ 1
⎣ 0
0⎤
m2 ⎥⎦
Si osservi che B è costante. Ne consegue che C=0, ovvero non ci sono contributi
centrifughi e di Coriolis.
Inoltre, poiché:
⎡ 0 ⎤
g0 = ⎢ 0 ⎥
⎢⎣− g ⎥⎦
il vettore dei termini gravitazionali risulta:
⎡(m + m2 )g ⎤
g=⎢ 1
⎥⎦
0
⎣
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [25]
Manipolatore cartesiano a due bracci
In assenza di attriti e forze all’end effector, le equazioni del moto sono le
seguenti:
(m1 + m2 )d1 + (m1 + m2 )g =
f1
m2 d 2 = f 2
f1 e f2: forze agenti lungo le coordinate generalizzate
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [26]
Manipolatore planare a due bracci
m2, I2
y0
l2
m1, I1
l1
Consideriamo un manipolatore planare a due bracci,
caratterizzati da:
• masse: m1 e m2
• lunghezze: a1 e a2
• distanze dei baricentri dagli assi dei giunti: l1 e l2
• momenti di inerzia baricentrali intorno a z0: I1 e I2
a2
ϑ2
a1
ϑ1
x0
⎡ ϑ1 ⎤
q
=
Coordinate generalizzate:
⎢⎣ϑ 2 ⎥⎦
Gli Jacobiani necessari per il calcolo della matrice di inerzia sono i seguenti:
⎡− l1s1 0⎤
J P(l1 ) = ⎢ l1c1 0⎥
⎢ 0
0⎥⎦
⎣
(l1 )
JO
⎡0 0 ⎤
= ⎢0 0 ⎥
⎢⎣1 0⎥⎦
⎡− a1s1 − l 2 s12
J P(l2 ) = ⎢ a1c1 + l 2 c12
⎢
0
⎣
(l2 )
JO
− l 2 s12 ⎤
l 2 c12 ⎥
0 ⎥⎦
⎡0 0 ⎤
= ⎢0 0 ⎥
⎢⎣1 1⎥⎦
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [27]
Manipolatore planare a due bracci
Tenendo conto che i vettori velocità angolare ω1 e ω2 sono allineati con z0 le
matrici di rotazione Ri sono irrilevanti, per cui il calcolo della matrice di inerzia
conduce a:
(l1 )T (l1 )
B (q ) = m1J P
(l2 )T (l2 )
J P + m2 J P
(
(l1 )T (l1 )
J P + I1 J O
(l2 )T (l2 )
JO + I2JO
JO
⎡ b11 (ϑ) b12 (ϑ)⎤
=⎢
⎥
(
)
b
b
ϑ
22 ⎦
⎣ 21
)
b11 = m1l12 + I1 + m2 a12 + l 22 + 2a1l 2 c2 + I 2
(
)
b12 = b21 = m2 l 22 + a1l 2 c2 + I 2
b22 = m2 l 22 + I 2
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [28]
Manipolatore planare a due bracci
Dalla matrice di inerzia ricaviamo i simboli di Christoffel:
c111 =
1 ∂b11
=0
2 ∂q1
c112 = c121 =
1 ∂b11
= − m2 a1l 2 s 2 = h
2 ∂q 2
c122 =
∂b12 1 ∂b22
−
= − m2 a1l 2 s 2 = h
∂q 2 2 ∂q1
c211 =
∂b21 1 ∂b11
= m2 a1l 2 s 2 = −h
−
∂q1 2 ∂q 2
c212 = c221
c222 =
h = − m2 a1l 2 s 2
1 ∂b22
=
=0
2 ∂q1
1 ∂b22
=0
2 ∂q 2
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [29]
Manipolatore planare a due bracci
L’espressione della matrice C è quindi:
⎡ hϑ 2
C (q, q ) = ⎢
⎣− hϑ1
(
)
h ϑ1 + ϑ 2 ⎤ ⎡− m2 a1l 2 s 2 ϑ 2
⎥=⎢
0
⎦ ⎣ m2 a1l 2 s 2 ϑ1
(
)
− m2 a1l 2 s 2 ϑ1 + ϑ 2 ⎤
⎥
0
⎦
Verifichiamo l’anti-simmetria della matrice N:
⎡ 2 hϑ 2
N (q, q ) = B (q ) − 2C (q, q ) = ⎢
⎣ hϑ 2
⎡
0
=⎢
⎣2hϑ1 + hϑ 2
hϑ 2 ⎤ ⎡ hϑ 2
⎥ − 2⎢
0 ⎦ ⎣− hϑ1
(
)
h ϑ1 + ϑ 2 ⎤
⎥=
0
⎦
− 2hϑ1 − hϑ 2 ⎤
T
(q, q )
N
=
−
⎥
0
⎦
Inoltre, poiché g0 = [0 −g 0]T, il vettore dei termini gravitazionali risulta:
⎡(m l + m2 a1 )gc1 + m2 gl 2 c12 ⎤
g =⎢ 11
⎥⎦
m2 gl 2 c12
⎣
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [30]
Manipolatore planare a due bracci
In assenza di attriti e forze all’end effector, le equazioni del moto sono le
seguenti:
(m l
2
11
(
) ) ( (
) )
+ I1 + m2 a12 + l 22 + 2a1l 2 c2 + I 2 ϑ1 + m2 l 22 + a1l 2 c2 + I 2 ϑ 2 +
− 2m2 a1l 2 s 2 ϑ1ϑ 2 − m2 a1l 2 s 2 ϑ 22 +
+ (m1l1 + m2 a1 )gc1 + m2 gl 2 c12 = τ1
(m (l
2
2 2
) ) (
)
+ a1l 2 c2 + I 2 ϑ1 + m2 l 22 + I 2 ϑ 2 +
+ m2 a1l 2 s 2 ϑ12 + m2 l 2 gc12 = τ 2
τi : coppie applicate ai due giunti
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [31]
Manipolatore planare a due bracci
Per ispezione delle equazioni, possiamo ricavare i parametri dinamici rispetto a
cui il modello è lineare, ovvero rispetto ai quali si può scrivere:
τ = Y (q, q, q )π
Risulta:
π = [π1
π2
π3
π4
π 5 ]T
π1 = m1l1
π 2 = I1 + m1l12
π 3 = m2
π 4 = m2 l 2
π 5 = I 2 + m2 l 22
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [32]
Manipolatore planare a due bracci
⎡y
Y (q, q, q ) = ⎢ 11
⎣ 0
y12
0
y13
0
y14
y 24
y15 ⎤
y 25 ⎥⎦
y11 = gc1
y12 = ϑ1
y13 = a12 ϑ1 + a1 gc1
y14 = 2a1c2 ϑ1 + a1c2 ϑ 2 − 2a1s 2 ϑ1ϑ 2 − a1s 2 ϑ 22 + gc12
y15 = ϑ1 + ϑ 2
y 24 = a1c2 ϑ1 + a1s 2 ϑ12 + gc12
y 25 = ϑ1 + ϑ 2
I coefficienti di Y dipendono da ϑ1, ϑ2, dalle loro derivate prime e seconde, da g e
da a1.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [33]
Identificazione dei parametri dinamici
La proprietà di linearità del modello dinamico rispetto ai parametri dinamici:
τ = Y (q, q, q )π
consente di impiegare una procedura per l’identificazione sperimentale dei
parametri stessi, di norma sconosciuti o incerti.
Si eseguono opportune traiettorie di moto lungo le quali si misurano le posizioni
dei giunti q, si misurano o si ricavano per derivazione le velocità q. e si ricavano
..
per derivazione filtrata le accelerazioni q.
Si misurano anche le coppie τ, direttamente (con opportuni sensori) o
indirettamente dalle misure di corrente nei motori.
Si supponga di avere le misure (dirette o indirette) di tutte le variabili negli istanti
di tempo t1, …, tN.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [34]
Identificazione dei parametri dinamici
Con N insiemi di misure:
⎡ τ (t1 ) ⎤ ⎡ Y (t1 ) ⎤
⎥=⎢
⎥π = Y π
τ = ⎢⎢
⎥ ⎢
⎥
⎢⎣ τ (t N )⎥⎦ ⎢⎣Y (t N )⎥⎦
Risolvendo con una tecnica ai minimi quadrati:
(
π= Y Y
T
)
−1
Y τ
T
−
Matrice pseudo-inversa sinistra di Y
Sono identificabili solo gli elementi di π la cui corrispondente colonna di Y non è
nulla. Inoltre alcuni parametri potrebbero essere identificabili solo in
combinazione lineare con altri.
Le traiettorie da utilizzare devono essere sufficientemente ricche (buon
− −
condizionamento di YTY) ma non tali da eccitare dinamiche non modellate.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [35]
Formulazione di Newton-Eulero
Una strada alternativa per la formulazione del modello dinamico del
manipolatore è quella che va sotto il nome di metodo di Newton-Eulero.
Si tratta di scrivere i bilanci di forze e momenti agenti sul singolo braccio,
mettendo in evidenza le interazioni con i bracci contigui nella catena cinematica.
f
mo orze
me
nti
acc veloc
ele ità
raz
io n
i
Si ottiene un sistema di equazioni che possono essere risolte in modo ricorsivo,
propagando le velocità ed accelerazioni dalla base verso l’organo terminale, e le
forze ed i momenti in senso opposto:
La ricorsività rende l’algoritmo di
Newton-Eulero computazionalmente
efficiente.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [36]
Definizione dei parametri
Consideriamo il generico braccio i della catena cinematica:
Il disegno è tratto dal testo:
L.Sciavicco, B.Siciliano
Robotica industriale – Modellistica e controllo di robot
manipolatori (2a ed.)
Mc Graw-Hill, 2000
Definiamo i parametri:
mi e Ii: massa e tensore di inerzia del braccio
ri−1,Ci vettore dall’origine della terna (i−1) al baricentro Ci
ri,Ci vettore dall’origine della terna i al baricentro Ci
ri−1, i vettore dall’origine della terna (i−1) all’origine della terna i
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [37]
Definizione delle variabili
.
pCi velocità lineare del baricentro Ci
.
pi velocità lineare dell’origine della terna i
ω
.. i velocità angolare del braccio
pCi accelerazione lineare del baricentro Ci
..
pi accelerazione lineare dell’origine della terna i
.
ωi accelerazione angolare del braccio
g0 accelerazione di gravità
fi forza esercitata dal braccio i−1 sul braccio i
−fi+1 forza esercitata dal braccio i+1 sul braccio i
µi momento esercitato dal braccio i−1 sul braccio i rispetto all’origine
della terna i−1
−µi+1 momento esercitato dal braccio i+1 sul braccio i rispetto all’origine
della terna i
Tutti i vettori si intendono espressi in terna base.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [38]
Equazioni di Newton e Eulero
Equazione di Newton (moto traslazionale baricentro)
f i − f i +1 + mi g 0 = mi pCi
Equazione di Eulero (moto rotazionale)
µ i + f i × ri −1,Ci − µ i +1 − f i +1 × ri ,Ci
si dimostra
effetto giroscopico
d
= (I i ω i ) = I i ω i + ω i × (I i ω i )
dt
Forza generalizzata al giunto i:
⎧⎪ f iT z i −1 giunto prismatico
τi = ⎨ T
⎪⎩µ i z i −1 giunto rotoidale
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [39]
Accelerazioni di un braccio
Propagazione delle velocità:
giunto prismatico
⎧ω
ω i = ⎨ i −1
⎩ω i −1 + ϑi z i −1 giunto rotoidale
⎧⎪ pi −1 + d i z i −1 + ω i × ri −1,i
pi = ⎨
⎪⎩ pi −1 + ω i × ri −1,i
giunto prismatico
giunto rotoidale
Propagazione delle accelerazioni:
giunto prismatico
⎧ω i −1
ωi = ⎨
⎩ω i −1 + ϑi z i −1 + ϑi ω i −1 × z i −1 giunto rotoidale
⎧⎪ pi −1 + d i z i −1 + 2d i ω i × z i −1 + ω i × ri −1,i + ω i × (ω i × ri −1,i ) giunto prismatico
pi = ⎨
giunto rotoidale
⎪⎩ pi −1 + ω i × ri −1,i + ω i × (ω i × ri −1,i )
(
pCi = pi + ω i × ri ,Ci + ω i × ω i × ri ,Ci
)
baricentro
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [40]
Algoritmo ricorsivo
. .. ..
• calcolo di ωi, ωi, pi, pCi
Si effettua una ricorsione all’indietro di forze e momenti:
• condizioni terminali su fn+1 e µn+1
• calcolo di:
f i = f i +1 + mi pCi
(
f
mo orze
me
nti
acc vel oc
ele ità
raz
ion
i
Si effettua una ricorsione in avanti di velocità ed accelerazione:
..
.
• condizioni iniziali su ω0, p0−g0, ω0
)
µ i = − f i × ri −1,i + ri ,Ci + µ i +1 + f i +1 × ri ,Ci + I i ω i + ω i × (I i ω i )
Si calcola la forza generalizzata al giunto i:
⎧⎪ f iT z i −1 + Fvi d i + f si
τi = ⎨ T
⎪⎩µ i z i −1 + Fvi ϑi + f si
contributi di attrito
giunto prismatico
giunto rotoidale
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [41]
Sistemi di riferimento locali
Finora si è supposto che tutti i vettori fossero riferiti alla terna base.
In realtà risulta più conveniente riferire i vettori alla terna di coordinate corrente
sul braccio i.
In questo modo infatti i vettori ri−1,i e ri,Ci ed il tensore di inerzia Ii sono costanti,
il che rende l’algoritmo computazionalmente più efficiente.
Le equazioni risultano modificate in alcuni termini (occorre moltiplicare i vettori
per opportune matrici di rotazione) ma non cambia la natura del metodo.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [42]
Manipolatore planare a due bracci
m2, I2
y0
l2
m1, I1
l1
a2
ϑ2
a1
ϑ1
x0
Consideriamo nuovamente un manipolatore planare
a due bracci con giunti rotoidali, il cui modello è
già stato ricavato con il metodo di Lagrange:
• masse: m1 e m2
• lunghezze: a1 e a2
• distanze dei baricentri dagli assi dei giunti: l1 e l2
• momenti di inerzia baricentrali intorno a z0: I1 e I2
Condizioni iniziali per la ricorsione di velocità ed accelerazioni:
p00 − g 00 = [0 g 0]T , ω 00 = ω 00 = 0
Condizioni iniziali per la ricorsione di forze e momenti:
f 33 = 0, µ 33 = 0
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [43]
Definizione vettori e matrici
Riferiamo tutte le quantità alla terna corrente sul braccio. Si hanno i seguenti
vettori costanti:
r11,C1
⎡l1 − a1 ⎤
⎡a1 ⎤
⎡l 2 − a2 ⎤
⎡a 2 ⎤
= ⎢⎢ 0 ⎥⎥, r01,1 = ⎢⎢ 0 ⎥⎥, r22,C2 = ⎢⎢ 0 ⎥⎥, r12, 2 = ⎢⎢ 0 ⎥⎥
⎢⎣ 0 ⎥⎦
⎢⎣ 0 ⎥⎦
⎢⎣ 0 ⎥⎦
⎢⎣ 0 ⎥⎦
Le matrici di rotazione sono:
⎡c1
R10 = ⎢⎢ s1
⎢⎣ 0
− s1 0⎤
⎡c 2
c1 0⎥⎥, R12 = ⎢⎢ s 2
⎢⎣ 0
0 1⎥⎦
− s2
c2
0
0⎤
⎡1 0 0⎤
0⎥⎥, R32 = ⎢⎢0 1 0⎥⎥
⎢⎣0 0 1⎥⎦
1⎥⎦
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [44]
Ricorsione in avanti: braccio 1
⎡0⎤
ω 0 + ϑ1 z 0 = ⎢⎢ 0 ⎥⎥
⎢⎣ϑ1 ⎥⎦
ω11 = R10T
(
)
ω11 = R10T
⎡0⎤
ω 0 + ϑ1 z 0 + ϑ1ω 0 × z 0 = ⎢⎢ 0 ⎥⎥
⎢⎣ϑ1 ⎥⎦
(
)
(
p11 = R10T p0 + ω11 × r01,1 + ω11 × ω11 × r01,1
(
p1C1 = p11 + ω11 × r11,C1 + ω11 × ω11 × r11,C1
)
)
⎡− a1ϑ12 + gs1 ⎤
⎢
⎥
= ⎢ a1ϑ1 + gc1 ⎥
⎢
⎥
0
⎣
⎦
⎡− l1ϑ12 + gs1 ⎤
⎢
⎥
= ⎢ l1ϑ1 + gc1 ⎥
⎢
⎥
0
⎣
⎦
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [45]
Ricorsione in avanti: braccio 2
⎡ 0 ⎤
ω11 + ϑ 2 z 0 = ⎢⎢ 0 ⎥⎥
⎢⎣ϑ1 + ϑ 2 ⎥⎦
ω 22 = R12T
(
)
ω 22 = R12T
⎡ 0 ⎤
ω11 + ϑ 2 z 0 + ϑ 2 ω11 × z 0 = ⎢⎢ 0 ⎥⎥
⎢⎣ϑ1 + ϑ 2 ⎥⎦
(
)
(
p22 = R12T p11 + ω 22 × r12, 2 + ω 22 × ω 22 × r12, 2
(
pC2 2 = p22 + ω 22 × r22,C2 + ω 22 × ω 22 × r22,C2
)
(
)
)
(
)
)
⎡a s ϑ − a c ϑ 2 − a ϑ + ϑ 2 + gs ⎤
2 1
2
12 ⎥
⎢ 1 2 1 1 2 1
= ⎢ a1c2 ϑ1 + a 2 ϑ1 + ϑ 2 + a1s 2 ϑ12 + gc12 ⎥
⎢
⎥
0
⎢⎣
⎥⎦
)
(
⎡a s ϑ − a c ϑ 2 − l ϑ + ϑ 2 + gs ⎤
2
12 ⎥
⎢ 1 2 1 1 2 1 2 1
= ⎢ a1c2 ϑ1 + l 2 ϑ1 + ϑ 2 + a1s 2 ϑ12 + gc12 ⎥
⎢
⎥
0
⎢⎣
⎥⎦
(
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [46]
Ricorsione all’indietro: braccio 2
f 22 = R32 f 33 + m2 pC2 2 = m2 pC2 2
(
(
)
)
⎡m ⎛ a s ϑ − a c ϑ 2 − l ϑ + ϑ 2 + gs ⎞⎤
2
12 ⎟ ⎥
⎢ 2 ⎜⎝ 1 2 1 1 2 1 2 1
⎠
⎢
⎥
2
= ⎢ m2 a1c2 ϑ1 + l 2 ϑ1 + ϑ 2 + a1s 2 ϑ1 + gc12 ⎥
⎢
⎥
0
⎢⎣
⎥⎦
(
(
)
(
)
)
µ 22 = − f 22 × r12, 2 + r22,C2 + R32 µ 33 + R32 f 33 × r22,C2 + I 22 ω 22 + ω 22 × I 22 ω 22 =
∗
⎡
⎤
⎥
= ⎢⎢
∗
⎥
2
2
⎢⎣ I 2 + m2 l 2 ϑ1 + ϑ 2 + m2 a1l 2 c2 ϑ1 + m2 a1l 2 s 2 ϑ1 + m2 l 2 gc12 ⎥⎦
(
)(
τ 2 = µ 22T R12T z 0 =
(
(
)
)) (
)
= I 2 + m2 l 22 + a1l 2 c2 ϑ1 + I 2 + m2 l 22 ϑ 2 + m2 a1l 2 s 2 ϑ12 + m2 l 2 gc12
(coincidente con quanto trovato con il metodo di Lagrange).
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [47]
Ricorsione all’indietro: braccio 1
(
f11 = R12 f 22 + m2 p1C1
(
)
(
)
⎡− m l s ϑ + ϑ − m l ϑ 2 − m a ϑ 2 − m l c ϑ + ϑ 2 + (m + m )gs ⎤
2
11 1
2 1 1
2 2 2 1
2
1
2
1⎥
⎢ 22 2 1
2
= ⎢ (m1l1 + m2 a1 )ϑ1 + m2 l 2 c2 ϑ1 + ϑ 2 − m2 l 2 s 2 ϑ1 + ϑ 2 + (m1 + m2 )gc1 ⎥
⎥
⎢
0
⎥⎦
⎢⎣
(
)
)
(
(
)
)
µ11 = − f11 × r01,1 + r11,C1 + R12 µ 22 + R12 f 22 × r11,C1 + I11ω11 + ω11 × I11ω11 =
⎡
⎢
∗
⎢
=⎢
∗
2
2
⎢ I + m a + m l + m a l c ϑ + I + m a l c + m l2 ϑ + ϑ
2 1
11
2 1 2 2 1
2
2 1 2 2
2 2
1
2
⎢ 1
⎢+ m a l s ϑ 2 − m a l s ϑ + ϑ 2 + (m l + m a )gc + m l gc
2 1 2 2 1
2 1 2 2 1
2
11
2 1
1
2 2 12
⎣
(
τ1 = µ11T R10T z 0 =
(
(
(
) (
)(
)
)) (
(
)
⎤
⎥
⎥
⎥
+⎥
⎥
⎥
⎦
))
= I1 + m1l12 + I 2 + m2 a12 + l 22 + 2a1l 2 c2 ϑ1 + I 2 + m2 l 22 + a1l 2 c2 ϑ 2
− 2m2 a1l 2 s 2 ϑ1ϑ 2 − m2 a1l 2 s 2 ϑ 22 + (m1l1 + m2 a1 )gc1 + m2 l 2 gc12
(coincidente con quanto trovato con il metodo di Lagrange).
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [48]
Lagrange vs. Newton-Eulero
Formulazione di Lagrange
• è sistematica e più facilmente comprensibile
• fornisce le equazioni del moto in forma analitica compatta, mettendo in
evidenza la matrice di inerzia, i termini centrifughi e di Coriolis, ed i termini
gravitazionali. Questi elementi sono utili ai fini del progetto di un controllore
basato sul modello.
• si presta all’introduzione nel modello di effetti dinamici più complessi
(deformabilità ai giunti o nei link)
Formulazione di Newton-Eulero
• è un metodo ricorsivo, efficiente dal punto di vista computazionale
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [49]
Dinamica diretta ed inversa
B (q )q + C (q, q )q + g (q ) + Fv q + f s (q, q ) = τ
Dinamica diretta
..
Assegnate le coppie ai giunti τ(t), determinare le accelerazioni ai giunti q(t) e,
.
note le posizioni iniziali q(t0) e le velocità iniziali q(t0), le posizioni q(t) e le
.
velocità q(t).
• Problema la cui soluzione è utile per la simulazione numerica della dinamica
• È risolubile sia con l’approccio di Lagrange sia con l’approccio di NewtonEulero
Dinamica inversa
..
.
Assegnate le accelerazioni q(t), le velocità q(t) e le posizioni q(t) determinare le
coppie ai giunti τ(t) necessarie alla generazione del movimento.
• Problema la cui soluzione è utile per la pianificazione della traiettoria e per il
controllo basato sul modello.
• Si può risolvere efficientemente con l’approccio di Newton-Eulero
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [50]
Calcolo della dinamica diretta ed inversa
Il calcolo della dinamica inversa si conduce facilmente sia con il metodo di
Lagrange sia con quello di Newton-Eulero.
Per quanto riguarda il calcolo della dinamica diretta, riscriviamo il modello
dinamico del manipolatore in questi termini:
B (q )q + n(q, q ) = τ
dove:
n(q, q ) = C (q, q )q + g (q ) + Fv q + f s (q, q )
Si tratta pertanto di integrare numericamente il sistema di equazioni differenziali
esplicito:
q = B (q )−1 (τ − n(q, q ))
in cui tutti gli elementi necessari per costruire il sistema sono direttamente
calcolati dal metodo di Lagrange.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [51]
Calcolo della dinamica diretta ed inversa
Come calcolare la dinamica diretta con il metodo di Newton-Eulero?
.
Con i valori correnti di q e q, si esegue una prima iterazione del metodo ponendo
..
q = 0. In questo modo le coppie τ calcolate dal metodo forniscono direttamente il
vettore n.
.
Successivamente si pone g0 = 0 (per eliminare gli effetti gravitazionali) e q = 0
(per eliminare gli effetti di Coriolis, centrifughi e di attrito). Si eseguono quindi n
..
..
iterazioni del metodo con qi = 1 e qj = 0, j≠i. In questo modo si costruisce, per
colonne, la matrice B e si hanno tutti gli elementi per costruire il sistema di
equazioni.
In alternativa si può utilizzare un solutore numerico in grado di integrare sistemi
di equazioni differenziali impliciti o, più in generale, sistemi di equazioni DAE
(equazioni algebrico-differenziali).
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [52]
Modello dinamico nello spazio operativo
Può essere di interesse formulare il modello dinamico del manipolatore nello
spazio operativo, anziché nello spazio dei giunti.
Faremo riferimento ad un manipolatore a sei gradi di libertà, in configurazione
non singolare.
Trascurando gli attriti, dal modello dinamico nello spazio dei giunti otteniamo:
[
q = B (q )−1 − C (q, q )q − g (q ) + τ − J T (q )h
]
Derivando la relazione cinematica differenziale:
x = J A (q )q
si ottiene:
x = J A (q )q + J A (q, q )q
Poniamo inoltre:
τ = J TA (q )γ A , J T (q )h = J TA (q )hA
γA: forze e momenti equivalenti all’organo terminale
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [53]
Modello dinamico nello spazio operativo
..
..
Sostituendo in x l’espressione di q:
x = − J A B −1Cq − J A B −1 g + J A q + J A B −1J TA (γ A − hA )
Poniamo ora:
(
B A = J A B −1J TA
)
−1
= J A−T BJ A−1
C A x = B A J A B −1Cq − B A J A q = J A−T Cq − B A J A q
g A = J A−T g
Il modello dinamico nello spazio operativo è il seguente:
B A ( x )x + C A ( x , x )x + g A ( x ) = γ A − h A
Il modello risulta formalmente valido anche per un manipolatore ridondante: in
questo caso però le x non costituiscono un insieme di coordinate generalizzate.
Controllo del moto e robotica industriale - Dinamica del manipolatore- P. Rocco [54]