Controllo dei robot

Controllo dei robot
Dinamica
Prof. Paolo Rocco ([email protected])
Centro di massa
z
Consideriamo un sistema di punti materiali,
ciascuno dei quali abbia massa mi e la cui
posizione sia descritta dal vettore pi rispetto ad
una terna xyz.
Definiamo centro di massa, o baricentro, il
punto individuato dal vettore:
n
∑pm
i
pc =
i =1
m
P1
Pi
x
i
n
, m=
pc
pρdV
V
Pn
y
O
∑m
z
i
i =1
P
Consideriamo ora un corpo rigido, di massa m,
volume V e densità ρ. Il centro di massa è
individuato dal vettore:
∫
=
P2
O
dV
y
x
m
Controllo dei robot - Dinamica - P. Rocco [2]
Momento d’inerzia
Consideriamo un punto materiale di massa m ed
un asse r. Sia d la distanza del punto dall’asse.
Definiamo momento d’inerzia del punto rispetto
all’asse la quantità:
z
d
I r = md 2
Analogamente, per un sistema di punti materiali:
n
Ir =
∑
r
x
mi d i2
z
Consideriamo ora un corpo rigido, di massa m,
volume V e densità ρ. Il momento d’inerzia
rispetto all’asse è definito come:
∫
V
y
O
i =1
Ir =
P
d(P)
dV
r
d 2 ( p )ρdV
O
P
y
x
Controllo dei robot - Dinamica - P. Rocco [3]
Tensore d’inerzia
Il momento d’inerzia di un corpo rigido rispetto ad un asse passante per O
e avente versore r si può esprimere come:
Ir = rT Ior
dove la matrice IO, di dimensioni 3×3 e simmetrica, prende il nome di tensore
d’inerzia del corpo relativo al polo O. Questa matrice assume l’espressione:
⎡ I Oxx
⎢
I O = ⎢ I Oxy
⎢ I Oxz
⎣
I Oxy
I Oyy
I Oyz
I Oxz ⎤
⎥
I Oyz ⎥
I Ozz ⎥⎦
Momenti d’inerzia
∫ (p
= ∫ (p
= ∫ (p
I Oxx =
I Oyy
I Ozz
V
V
V
)
)ρdV
)ρdV
Dipende dal polo O e dalla terna di
riferimento.
Prodotti d’inerzia
∫ −p
=∫ −p
=∫ −p
2
y
+ p z2 ρdV
I Oxy =
2
z
+ p x2
I Oxz
2
x
+ p 2y
I Oyz
V
V
V
x p y ρdV
x p z ρdV
y
p z ρdV
Controllo dei robot - Dinamica - P. Rocco [4]
Tensore d’inerzia
Il tensore d’inerzia dipende sia dal polo che dall’orientamento della terna
di riferimento.
Se si considera una nuova terna, ruotata secondo la matrice R rispetto alla
terna originaria, il tensore d’inerzia si trasforma secondo la seguente legge:
I ′ = RIR T
Poiché il tensore d’inerzia è una matrice definita positiva, esiste una terna
per cui esso assume forma diagonale. Gli assi di tale terna prendono il
nome di assi principali d’inerzia.
Se la terna ha origine nel baricentro, gli assi prendono anche il nome di
assi centrali d’inerzia.
Controllo dei robot - Dinamica - P. Rocco [5]
Momento rispetto ad assi paralleli
Sia IC il momento d’inerzia di un corpo di massa m rispetto ad un asse
passante per il baricentro.
Consideriamo un asse parallelo al precedente, a distanza ∆ da esso. Il
momento d’inerzia IA rispetto a questo nuovo asse risulta:
I A = I C + m∆2
Sia ora IC il tensore d’inerzia del corpo di massa m rispetto ad una terna
avente come origine il baricentro.
Il tensore d’inerzia, rispetto ad una terna parallela a quella baricentrale e
avente origine in un diverso punto O, è dato da:
I O = I C + mS T ( pC )S ( pC )
dove pC è la posizione del baricentro nella terna avente come origine O e S
è l’operatore anti-simmetrico.
Controllo dei robot - Dinamica - P. Rocco [6]
Alcuni esempi
Cilindro circolare omogeneo
dr
Calcoliamo il momento d’inerzia rispetto all’asse:
r
4
2
R
R
=m
I = ρr 2 dV = ρr 2 2πrhdr = 2πρh
V
4
2
∫
h
∫
0
essendo m=ρπR2h la massa del cilindro.
R
Asta omogenea
Calcoliamo il momento d’inerzia rispetto ad un asse
perpendicolare passante per il baricentro:
l
r
A
R
dr
C
IC = 2
∫
l 2
0
l 2
3⎤
⎡
r
1
r 2 ρdr = 2ρ ⎢ ⎥ = ml 2
12
⎢⎣ 3 ⎥⎦ 0
2
Rispetto ad un asse passante per A:
1
⎛l⎞
I A = I c + m⎜ ⎟ = ml 2
3
⎝2⎠
Controllo dei robot - Dinamica - P. Rocco [7]
Energia cinetica
Consideriamo un punto materiale di massa m e 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
P
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
O
dV
y
x
V
Controllo dei robot - Dinamica - P. Rocco [8]
Energia cinetica di un corpo rigido
Per il calcolo dell’energia cinetica di un corpo rigido esiste il seguente
risultato notevole (teorema di König):
1
1
mpC T pC + ωT I C ω
2
2
dove pC indica la posizione del baricentro, ω la velocità angolare e IC è il
tensore di inerzia rispetto ad una terna con origine il baricentro e parallela alla
terna di riferimento.
T=
Nel caso particolare di rotazione rispetto ad asse fisso, assunto questo
asse coincidente con l’asse z, il risultato si semplifica nel seguente:
1
1
mpC T pC + I cz ω2z
2
2
dove Icz è il momento d’inerzia del corpo rispetto ad un asse parallelo
all’asse z, passante per il baricentro e ωz è la proiezione di ω lungo l’asse.
T=
Controllo dei robot - Dinamica - P. Rocco [9]
Energia cinetica di un sistema di corpi
Consideriamo ora un sistema di corpi rigidi soggetto a vincoli di mobilità
(come un manipolatore robotico), per il quale si possa esprimere il moto
in termini di un vettore q di coordinate generalizzate.
L’energia cinetica del sistema si può esprimere per mezzo della seguente
forma quadratica:
T=
1 T
q B (q )q
2
La matrice B(q) prende il nome di matrice di inerzia del sistema di corpi
rigidi e generalmente dipende dal vettore di coordinate q.
Poiché l’energia cinetica è una quantità positiva per velocità diverse da
zero, la matrice di inerzia, simmetrica, è sempre definita positiva.
Controllo dei robot - Dinamica - P. Rocco [10]
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 T p
dove g è il vettore accelerazione di gravità.
Per un corpo rigido:
U = − g T pρdV = − mg T pC
∫
V
Controllo dei robot - Dinamica - P. Rocco [11]
Equazioni di Lagrange
Consideriamo 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 è retta dalle seguenti equazioni
di Lagrange:
d ∂L ∂L
−
= ξ i , i = 1, …, n
dt ∂qi ∂qi
Controllo dei robot - Dinamica - P. Rocco [12]
Esempio
y
Consideriamo un sistema costituito da un
motore rigidamente connesso ad un carico
C
ϑ
soggetto a forza gravitazionale.
l
x
Siano Im e I i momenti di inerzia di motore
τ
e carico rispetto all’asse del motore, m la
mg
massa del carico, l la distanza del
baricentro del carico dall’asse del motore.
Energia cinetica del motore
Poiché il baricentro è fisso e l’asse di rotazione è baricentrale, si ha:
1
Tm = I m ϑ 2
2
Energia cinetica del carico
Dal teorema di König:
1
1
1
1
1
1
Tc = mpC T pC + I cz ϑ 2 = ml 2 ϑ 2 + I cz ϑ 2 = ml 2 + I cz ϑ 2 = Iϑ 2
2
2
2
2
2
2
(
)
Controllo dei robot - Dinamica - P. Rocco [13]
Esempio
ϑ
l
τ
mg
Lagrangiana:
Energia potenziale gravitazionale:
⎡l cos ϑ⎤
U = −mg T pC = − 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 dei robot - Dinamica - P. Rocco [14]
Manipolatore robotico: lagrangiana
Per un manipolatore robotico, il cui moto è caratterizzato dalle n
coordinate libere di giunto qi, l’energia cinetica è data dalla forma
quadratica:
1
T (q, q ) = q T B (q )q
2
mentre l’energia potenziale è la somma delle energie potenziali dei singoli
bracci (calcolate per mezzo delle posizioni dei baricentri pCi) :
U (q ) =
n
∑
− mi g T pCi
i =1
Ne consegue l’espressione della Lagrangiana:
n n
1
L(q, q ) = T (q, q ) − U (q ) =
bij (q )qi q j +
2 i =1 j =1
∑∑
n
∑
mi g T pCi
i =1
dove bij sono gli elementi della matrice di inerzia B.
Controllo dei robot - Dinamica - P. Rocco [15]
Manipolatore robotico: derivazioni
Deriviamo la lagrangiana:
d ⎛ ∂L
⎜
dt ⎜⎝ ∂qi
⎞ d ⎛ ∂T
⎟= ⎜
⎟ dt ⎜ ∂q
⎠
⎝ i
⎞ d ⎛⎜
⎟=
⎟ dt ⎜
⎠
⎝
⎞
bij (q )q j ⎟ =
⎟
j =1
⎠
n
∑
n
n
∑ b (q )q + ∑
ij
j =1
j
j =1
dbij (q )
dt
qj =
⎡ n ∂bij (q ) ⎤
=
bij (q )q j +
qk ⎥ q j
⎢
⎥⎦
j =1
j =1 ⎢
⎣ k =1 ∂q k
n
∑
n
∑∑
Inoltre:
∂T 1 n n ∂b jk (q )
qk q j
= ∑∑
∂qi 2 j =1k =1 ∂qi
n
∂pCj
∂U
T
= − ∑mjg
= g i (q )
∂qi
∂qi
j =1
Controllo dei robot - Dinamica - P. Rocco [16]
Manipolatore robotico: modello
Dalle equazioni di Lagrange otteniamo
n
n
n
∑ b (q )q + ∑∑ h
ij
j
ijk
j =1
(q )qk q j + g i (q ) = ξ i
i = 1,… , n
j =1 k =1
dove:
hijk
1 ∂b jk
−
=
∂q k 2 ∂qi
∂bij
In termini matriciali:
B (q )q + C (q, q )q + g (q ) = τ
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 dei robot - Dinamica - P. Rocco [17]
Significato dei termini
B (q )q + C (q, q )q + g (q ) = τ
Coppie ai giunti
Termini gravitazionali
Termini centrifughi e di Coriolis
Termini inerziali
Esistono metodi sistematici per ricavare i coefficienti della matrice d’inerzia e
quindi, per derivazione, quelli centrifughi e di Coriolis.
Tuttavia la scrittura del modello dinamico basata sulle equazioni di Lagrange,
pur dando luogo ad un modello in forma chiusa, facilmente interpretabile ed
utilizzabile nella sintesi del controllore, costituisce un procedimento
inefficiente dal punto di vista computazionale.
Controllo dei robot - Dinamica - P. Rocco [18]
Metodo 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.
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.
Il disegno è tratto dal testo:
L.Sciavicco, B.Siciliano
Robotica industriale – Modellistica e controllo di robot
manipolatori (2a ed.)
Mc Graw-Hill, 2000
Controllo dei robot - Dinamica - P. Rocco [19]
Dinamica diretta ed inversa
B (q )q + C (q, q )q + g (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 che 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 dei robot - Dinamica - P. Rocco [20]
Sistema dinamico
B (q )q + C (q, q )q + g (q ) = τ
Dal punto di vista della teoria dei sistemi, il manipolatore costituisce un sistema
dinamico di ordine 2n. Definite infatti le variabili di stato:
z1 = q ∈ ℜ n , z 2 = q ∈ ℜ n
e le variabili di ingresso:
u = τ ∈ ℜn
il sistema è composto dalle seguenti equazioni di stato:
⎧ z1 = z 2
⎨
−1
−1
(
)
(
(
)
(
)
)
(
)
=
−
+
+
z
B
z
C
z
,
z
z
g
z
B
z
u
2
1
1
2
2
1
1
⎩
Si osservi che in corrispondenza di un ingresso costante, lo stato di equilibrio si
caratterizza come segue:
⎧ g (z1 ) = u
⎨
⎩ z2 = 0
Controllo dei robot - Dinamica - P. Rocco [21]
Manipolatore cartesiano a due bracci
d2
m2
z1
m1
z0
d1
x0
Consideriamo un manipolatore cartesiano a due bracci,
caratterizzati dalle masse m1 e m2.
Il vettore delle coordinate generalizzate risulta:
⎡ d1 ⎤
q=⎢ ⎥
⎢⎣d 2 ⎥⎦
I baricentri dei due bracci hanno posizioni (a meno di costanti) e velocità date in
terna base da:
⎡d 2 ⎤
⎡0⎤
⎡d 2 ⎤
⎡0⎤
⎢ ⎥
⎢
⎥
⎢
⎥
⎢
⎥
pc1 = ⎢ 0 ⎥, pc 2 = ⎢ 0 ⎥, pc1 = ⎢ 0 ⎥, pc 2 = ⎢ 0 ⎥
⎢d ⎥
⎢⎣d1 ⎥⎦
⎢⎣ d1 ⎥⎦
⎢⎣d1 ⎥⎦
⎣ 1⎦
Il vettore dell’accelerazione di gravità è:
⎡ 0 ⎤
g = ⎢⎢ 0 ⎥⎥
⎢⎣− g ⎥⎦
Controllo dei robot - Dinamica - P. Rocco [22]
Manipolatore cartesiano a due bracci
d2
m2
z1
m1
z0
d1
x0
Calcoliamo l’energia cinetica, tenendo conto che la velocità
angolare di entrambi i bracci è nulla:
1
1
T
T = m1 pc1 pc1 + m2 pc 2T pc 2 =
2
2
(
1
1
2
= m1d1 + m2 d12 + d 2 2
2
2
)
Per quanto riguarda invece l’energia potenziale gravitazionale, definita a meno di
una costante, si ha:
U = −m1 g T pc1 − m2 g T pc 2 = m1 gd1 + m2 gd1
La lagrangiana è quindi:
L = T −U =
1
(m1 + m2 )d12 + 1 m2 d 2 2 − (m1 + m2 )gd1
2
2
Controllo dei robot - Dinamica - P. Rocco [23]
Manipolatore cartesiano a due bracci
d2
m2
z1
m1
z0
d1
x0
Risulta quindi:
Le equazioni di Lagrange sono le seguenti:
d ∂L ∂L
−
= f1
f1 e f2: forze agenti lungo le
dt ∂d1 ∂d1
coordinate generalizzate
d ∂L
∂L
−
= f2
dt ∂d 2 ∂d 2
(m1 + m2 )d1 + (m1 + m2 )g =
f1
m2 d 2 = f 2
In termini vettoriali:
⎡m1 + m2 0 ⎤ ⎡ d1 ⎤ ⎡(m1 + m2 )g ⎤ ⎡ f1 ⎤
=⎢ ⎥
⎢ ⎥+⎢
⎥
⎢ 0
⎥
m2 ⎦ ⎣⎢d 2 ⎦⎥ ⎣
0
⎦ ⎣ f2 ⎦
⎣
B
g
C =0
Controllo dei robot - Dinamica - P. Rocco [24]
Manipolatore planare a due bracci
a2
m2, I2
y0
l2
m1, I1
l1
ϑ2
a1
ϑ1
x0
Consideriamo un manipolatore planare a due
bracci, caratterizzati dalle masse m1 e m2 e dalle
lunghezze a1 e a2.
Il vettore delle coordinate generalizzate risulta:
⎡ϑ ⎤
q = ⎢ 1⎥
⎣ϑ 2 ⎦
I baricentri dei due bracci hanno posizioni e velocità date in terna base da:
⎡l1c1 ⎤
pc1 = ⎢⎢l1s1 ⎥⎥,
⎢⎣ 0 ⎥⎦
pc 2
⎡a1c1 + l 2 c12 ⎤
= ⎢⎢ a1s1 + l 2 s12 ⎥⎥,
⎢⎣
⎥⎦
0
dove:
a
a
l1 = 1 , l 2 = 2
2
2
⎡− ϑ1l1s1 ⎤
⎢
⎥
pc1 = ⎢ ϑ1l1c1 ⎥,
⎢ 0 ⎥
⎣
⎦
pc 2
(
)
⎡− ϑ1a1s1 − l 2 ϑ1 + ϑ 2 s12 ⎤
⎢
⎥
= ⎢ ϑ1a1c1 + l 2 ϑ1 + ϑ 2 c12 ⎥
⎢
⎥
0
⎣
⎦
Il vettore dell’accelerazione di gravità è:
(
)
⎡ 0 ⎤
g = ⎢⎢− g ⎥⎥
⎢⎣ 0 ⎥⎦
Controllo dei robot - Dinamica - P. Rocco [25]
Manipolatore planare a due bracci
a2
m2, I2
y0
l2
m1, I1
l1
ϑ2
I1 =
a1
ϑ1
Per calcolare l’energia cinetica, introduciamo i
momenti di inerzia delle due aste rispetto ad assi
passanti per i rispettivi baricentri e paralleli a z0:
1
1
m1a12 , I 2 = m2 a 22
12
12
x0
Utilizzando per entrambe le aste il teorema di König:
T=
=
1
1
1
1
m1 pc1T pc1 + I1ω2z1 + m2 pc 2T pc 2 + I 2 ω2z 2 =
2
2
2
2
(
)
(
)
(
2
1
1
1
1
m1l12 ϑ12 + I1ϑ12 + m2 ⎡ϑ12 a12 + ϑ1 + ϑ 2 l 22 + 2ϑ1 ϑ1 + ϑ 2 a1l 2 c2 ⎤ + I 2 ϑ1 + ϑ 2
⎥⎦ 2
2
2
2 ⎢⎣
Per quanto riguarda invece l’energia potenziale gravitazionale, definita a meno di
una costante, si ha:
U = −m1 g T pc1 − m2 g T pc 2 = m1 gl1s1 + m2 g (a1s1 + l 2 s12 )
Controllo dei robot - Dinamica - P. Rocco [26]
)2
Manipolatore planare a due bracci
Le equazioni di Lagrange sono le seguenti:
d ∂L
∂L
−
= τ i , i = 1,2 τi : coppie agenti lungo le coordinate generalizzate
dt ∂ϑi ∂ϑi
Prima equazione:
(m l
2
11
) (
)
+ I1 + m2 a12 + m2 l 22 + 2m2 a1l 2 c2 + I 2 ϑ1 + m2 l 22 + m2 a1l 2 c2 + I 2 ϑ 2 +
− 2m2 a1l 2 s 2 ϑ1ϑ 2 − m2 a1l 2 s 2 ϑ 22 +
+ m1 gl1c1 + m2 ga1c1 + m2 gl 2 c12 = τ1
Seconda equazione:
(m l
2
2 2
) (
)
+ m2 a1l 2 c2 + I 2 ϑ1 + m2l 22 + I 2 ϑ 2 +
+ m2 a1l 2 s 2 ϑ12 +
+ m2 gl 2 c12 = τ 2
Controllo dei robot - Dinamica - P. Rocco [27]
Manipolatore planare a due bracci
Le equazioni in forma vettoriale si scrivono:
⎡m1l12 + I1 + m2 a12 + m2 l 22 + 2m2 a1l 2 c2 + I 2
⎢
m2 l 22 + m2 a1l 2 c2 + I 2
⎣⎢
B (q )
⎡− 2m2 a1l 2 s 2 ϑ 2
⎢
⎣ m2 a1l 2 s 2 ϑ1
m2 l 22 + m2 a1l 2 c2 + I 2 ⎤ ⎡ ϑ1 ⎤
⎥⎢ ⎥ +
2
m2 l 2 + I 2
⎦⎥ ⎣ϑ 2 ⎦
− m2 a1l 2 s 2 ϑ 2 ⎤ ⎡ ϑ1 ⎤ ⎡(m1l1 + m2 a1 )gc1 + m2 gl 2 c12 ⎤ ⎡ τ1 ⎤
=⎢ ⎥
⎥⎢ ⎥ + ⎢
⎥
m2 gl 2 c12
0
⎦ ⎣τ 2 ⎦
⎦ ⎣ϑ 2 ⎦ ⎣
g (q )
C (q, q )
Si osservi che l’espressione della matrice C non è univoca. Si potrebbe anche
prendere, ad esempio:
⎡− m a l s ϑ − m2 a1l 2 s 2 ϑ1 + ϑ 2 ⎤
C (q, q ) = ⎢ 2 1 2 2 2
⎥
0
⎦
⎣ m2 a1l 2 s 2 ϑ1
(
)
Controllo dei robot - Dinamica - P. Rocco [28]