Prova Scritta di Robotica II

10 Giugno 2009
Esercizio 1
Ricavare lespressione della matrice dinerzia B(q) per il robot planare PRP mostrato in gura.
Per i = 1, 2, 3, siano: m
i
= massa del braccio i; I
i
= inerzia baricentrale del braccio i rispetto a
un asse normale al piano del moto. Si assumano i baricentri dei bracci disposti come in gura, con
d
c2
= distanza del baricentro del braccio 2 dallasse del giunto 2 e d
c3
= distanza del baricentro
del braccio 3 dallorgano terminale. Si utilizzino le coordinate generalizzate q
1
, q
2
e q
3
indicate.
Esercizio 2
Per un robot con n giunti, il cui modello dinamico `e dato nella forma usuale
B(q) q +c(q, q) +g(q) = ,
sia assegnato un compito x = x
d
(t), con x R
m
e x
d
(t) funzione dierenziabile almeno due volte
rispetto al tempo. Le variabili di compito x sono legate alle variabili di congurazione q R
n
dallespressione cinematica
x = f(q).
Si consideri il caso m < n, ovvero il robot `e ridondante rispetto al compito assegnato.
A) Con il robot in uno stato corrente (q, q) compatibile con il compito, determinare lespressione
della coppia di attuazione che minimizza istantaneamente la funzione obiettivo
H =
1
2

T
B
2
(q)
rispettando il vincolo (in accelerazione) sulla corretta esecuzione della traiettoria del compito.
Si supponga di essere in una congurazione q non singolare per il compito, ossia con
rango J(q) = m, con J(q) =
f(q)
q
.
1
B) Come si semplica lespressione della coppia del punto A) nel caso in cui il robot si muova
in assenza di gravit`a (ad es., su un piano orizzontale) e si trovi in uno stato a velocit`a di
giunto nulla?
C) Come si pu`o modicare lespressione generale della del punto A) nel caso in cui il robot si
trovi allistante t in uno stato di errore rispetto al compito,
e
x
= x
d
x = 0 e/o e
x
= x
d
x = 0,
in modo da ottenere una legge di controllo che riporti tale errore esponenzialmente a zero in
modo disaccoppiato rispetto alle componenti di e
x
?
[150 minuti di tempo; libri aperti]
2
Soluzioni
10 Giugno 2009
Esercizio 1
Occorre ricavare lenergia cinetica T(q, q) del robot. A tale scopo `e utile ricavare le espressioni
delle posizioni e delle velocit`a dei baricentri dei bracci 2 e 3 nel piano (x, y):
p
c2
=
_
d
c2
cos q
2
q
1
+ d
c2
sinq
2
_
v
c2
=
_
d
c2
sinq
2
q
2
q
1
+ d
c2
cos q
2
q
2
_
p
c3
=
_
(q
3
d
c3
) cos q
2
q
1
+ (q
3
d
c3
) sinq
2
_
v
c3
=
_
q
3
cos q
2
(q
3
d
c3
) sinq
2
q
2
q
1
+ q
3
sinq
2
+ (q
3
d
c3
) cos q
2
q
2
_
.
Inoltre il braccio 1 non ruota, mentre la componente della velocit`a angolare dei bracci 2 e 3 normale
al piano di moto `e la stessa, pari a q
2
. Con le usuali notazioni, si ha quindi
T
1
=
1
2
m
1
q
2
1
,
T
2
=
1
2
I
2
q
2
2
+
1
2
m
2
v
c2

2
=
1
2
I
2
q
2
2
+
1
2
m
2
_
q
2
1
+ d
2
c2
q
2
2
+ 2d
c2
cos q
2
q
1
q
2
_
,
T
3
=
1
2
I
3
q
2
2
+
1
2
m
3
v
c3

2
=
1
2
I
3
q
2
2
+
1
2
m
3
_
q
2
1
+ (q
3
d
c3
)
2
q
2
2
+ q
2
3
+ 2(q
3
d
c3
) cos q
2
q
1
q
2
+ 2 sinq
2
q
1
q
3
_
.
La matrice di inerzia B(q), simmetrica e denita positiva, si estrae dalla
T =
3

i=1
T
i
=
1
2
q
T
B(q) q
ed `e pari a
B(q
2
, q
3
) =
_
_
_
m
1
+ m
2
+ m
3
(m
2
d
c2
+ m
3
(q
3
d
c3
)) cos q
2
m
3
sinq
2
(m
2
d
c2
+ m
3
(q
3
d
c3
)) cos q
2
I
2
+ m
2
d
2
c2
+ I
3
+ m
3
(q
3
d
c3
)
2
0
m
3
sinq
2
0 m
3
_
_
_.
Esercizio 2
Posto per compattezza n(q, q) = c(q, q) + g(q) e tralasciando le dipendenze dallo stato corrente
(q, q), si utilizza lespressione del modello dinamico B q+n = per riscrivere la funzione obiettivo
da minimizzare come una forma quadratica (denita positiva) nellaccelerazione
H( q) =
1
2

T
B
2
=
1
2
(B q +n)
T
B
2
(B q +n) =
1
2
q
T
q +n
T
B
1
q +
1
2
n
T
B
2
n.
Si noti che tale funzione obiettivo `e una quadratica completa (ossia contenente anche un termine
lineare in q e uno costante).
3
Dierenziando due volte la relazione cinematica del compito si ha
x = f(q) x = J(q) q, x = J(q) q +

J(q) q.
Posto r = x
d

J(q) q, il vincolo di esecuzione del compito in un generico stato (q, q) compatibile
con esso risulta lineare nellaccelerazione incognita
J q r = 0,
dove si sono di nuovo tralasciate le dipendenze dallo stato corrente (q, q).
Il problema posto si formula quindi come problema di ottimizzazione lineare-quadratica e si
risolve con il metodo dei moltiplicatori di Lagrange. Introdotta la funzione
L = H +
T
(J q r),
le condizioni necessarie e sucienti di minimo vincolato per H( q) sono

q
L =
_
L
q
_
T
= q +B
1
n +J
T
= 0, (1)

L =
_
L

_
T
= J q r = 0. (2)
Ricavando la q dalla (1),
q = B
1
n J
T
, (3)
e sostituendo nella (2), si ottengono i moltiplicatori R
m
:
=
_
JJ
T
_
1 _
r +JB
1
n
_
. (4)
Inserendo la (4) nella (3), si ha inne
q = J
T
_
JJ
T
_
1 _
r +JB
1
n
_
B
1
n = J
#
r
_
I J
#
J
_
B
1
n, (5)
dove compare lespressione della pseudoinversa dello Jacobiano (nellipotesi fatta di rango pieno).
Nellultima uguaglianza si `e messa in evidenza lespressione con il proiettore nel nucleo di J.
Sostituendo laccelerazione (5) nel modello dinamico e riespandendo i termini, si ottiene inne
la soluzione cercata
= B(q)J
#
(q)
_
x
d


J(q) q +J(q)B
1
(q) (c(q, q) +g(q))
_
. (6)
In modo alternativo, si sarebbe potuto arrivare alla soluzione generale (6) risolvendo un proble-
ma di ottimizzazione (minimizzazione) non vincolata, come mostrato qui di seguito. Una possibile
espressione di tutte le accelerazioni che soddisfano la relazione J q = r (il vincolo del compito) `e
data dalla
q = J
#
r +
_
I J
#
J
_
q
0
, (7)
in cui q
0
R
n
`e unarbitraria accelerazione di giunto (proiettata poi nello spazio nullo dello
Jacobiano). Scegliendo la struttura (7) per q, il vincolo risulta sempre soddisfatto. Sostituendo
questa espressione nel modello dinamico,
= BJ
#
r +B
_
I J
#
J
_
q
0
+n, (8)
4
e quindi tale coppia nella funzione obiettivo H si ottiene
H( q
0
) =
1
2
q
T
0
_
I J
#
J
_
q
0
+
_
J
#
r +B
1
n
_
T
_
I J
#
J
_
q
0
+
1
2
_
J
#
r +B
1
n
_
T
_
J
#
r +B
1
n
_
,
dove si `e utilizzata la simmetria e lidempotenza del proiettore
_
I J
#
J
_
. La condizione neces-
saria di minimo non vincolato per tale funzione quadratica completa `e

q
0
H =
_
H
q
0
_
T
=
_
I J
#
J
__
q
0
+J
#
r +B
1
n
_
= 0.
Sfruttando la propriet`a
_
I J
#
J
_
J
#
= O della pseudoinversa, tale condizione equivale alla
_
I J
#
J
_
q
0
=
_
I J
#
J
_
B
1
n. (9)
Sostituendo la (9) nella (8) si ottiene di nuovo lespressione (6) trovata in precedenza per .
Per il punto B), nel caso in cui il robot si muova in assenza di gravit`a (g(q) 0) e si trovi in
uno stato a velocit`a di giunto nulla ( q = 0), la soluzione (6) si semplica in
= B(q)J
#
(q) x
d
. (10)
Per il punto C), basta modicare lespressione (6) includendo unazione proporzionale-derivativa
rispetto allerrore di compito e
x
= x
d
x (feedback), accanto al termine di accelerazione desiderata
di compito x
d
(feedforward):
= = B(q)J
#
(q)
_
x
d
+K
D
( x
d
J(q) q) +K
P
(x
d
f(q))

J(q) q
+J(q)B
1
(q) (c(q, q) +g(q))
_
,
(11)
dove le matrici K
P
e K
D
sono diagonali e denite positive. Tale risultato `e dovuto infatti alla
linearizzazione esatta e al disaccoppiamento ottenuti con la (6) nello spazio del compito fuori da
singolarit`a (catene di doppi integratori per ogni componente di x). Lespressione (11) realizza
quindi la stabilizzazione asintotica dellerrore di compito imponendo il comportamento
e
x
+K
D
e
x
+K
P
e
x
= 0, (12)
per cui lerrore e
x
(e ogni sua derivata) converge nel tempo in modo esponenziale a zero in quanto
la dinamica (12) `e lineare.

5