Date post: | 01-May-2015 |
Category: |
Documents |
Upload: | savina-cirillo |
View: | 221 times |
Download: | 0 times |
Controllo dei Robot A. Rizzo
Cinematica differenziale
La cinematica differenziale caratterizza i legami tra le velocità dei giunti e le corrispondenti velocità lineari e angolare della configurazione del manipolatore.
Tali legami sono espressi da una matrice di trasformazione, dipendente dalla configurazione del manipolatore, denominata Jacobiano geometrico.
Per altra via se la postura dell’organo terminale è espressa facendo riferimento ad una rappresentazione in forma minima dello spazio operativo, è possibile calcolare lo Jacobiano direttamente, mediante operazione di differenziazione della funzione cinematica diretta rispetto alla variabile di giunto; lo Jacobiano che ne viene fuori è denominato Jacobiano analitico ed è in generale diverso da quello geometrico.
Controllo dei Robot A. Rizzo
Jacobiano geometricomanipolatore a n gradi di mobilità
1
)()()( To
qpqRqT
qqJ
qqJp
o
p
)(
)(
velocità lineare velocità angolare dell’organo terminale
Jp rappresenta la matrice (3xn) relativa al contributo delle velocità dei giunti alla velocità lineare dell’organo terminale, mentre Jo è la matrice (3xn) relativa al contributo delle velocità angolare dell’organo terminale
qqJp
v
)(
o
p
J
JJ
equazione cinematica differenziale del manipolatore
Jacobiano geometrico del manipolatore:
Controllo dei Robot A. Rizzo
Derivata di una matrice di rotazione
Si supponga che la matrice di rotazione vari nel tempo, in altre parole R = R(t). Dalla proprietà di ortogonalità di R si ha la relazione R(t)RT(t) = I che, derivata rispetto al tempo, fornisce l’identità
0)()()()( tRtRtRtR TT
)()()( tRtRtS T
Poniamo :
Antisimmetrica : 0)()( tStS T
Moltiplichiamo da destra ambo i membri dell’espressione di S(t) per R(t):
)()()()()( tRtRtRtRtS T )()()( tRtStR
Controllo dei Robot A. Rizzo
Interpretazione fisicaSi consideri un vettore p ed il vettore p(t) = R(t)p (osserviamo che il vettore p non dipende dal tempo).
La derivata temporale di p(t) risulta:
ptRtp )()( ptRtStp )()()(
Dalla meccanica classica risulta che la velocità di un vettore applicato si esprime nella forma:
ptRttp )()()(
dove (t) rappresenta la velocità angolare della terna R(t) all’istante di tempo t rispetto alla terna di riferimento.
Uguagliando le due espressioni si osserva che la matrice S(t) esprime il prodotto vettoriale tra il vettore velocità angolare (t) ed il vettore R(t)p.
Controllo dei Robot A. Rizzo
Velocità angolare
(t) =[x, y, z]T
0)()(
)(0)(
)()(0
)(
00
00
00
0
tt
tt
tt
tS
xy
xz
yz
S(t) = S((t)).
ptRtSptRttp )()()()()(
y
P x’y’
x
p
p solidale a x’,y’x’, y’ ruota con velocità angolare
Controllo dei Robot A. Rizzo
EsempioConsideriamo la matrice di rotazione elementare intorno all’asse z:
100
0cossen
0sencos
)(
zR
Supponiamo che vari nel tempo, calcoliamo la derivata rispetto al tempo di Rz((t)).
000
00
00
000
0
0
000
0
0
)()()(
cs
sc
sc
cs
tRtRtS T
Controllo dei Robot A. Rizzo
quindi il vettore velocità angolare della terna ruotata rispetto alla terna base vale:
0
0
)()()( tRtStR
000
0
0
000
0
0
100
00
00
)()(
sc
cs
cs
sc
tRtS
Controllo dei Robot A. Rizzo
Composizione di velocità10
101
0 pRop 10
110
101
0 pRpRop
101
01
101
01
0 )( pRSpRop
101
01 pRr 0
101
101
01
0 rpRop
se p1 è costante nel tempo (cioè fisso rispetto alla terna 1)
01
01
01
0 rop
Controllo dei Robot A. Rizzo
Velocità di un braccio
1,111
i
iiiii rRpp
1,111
1,111 )(
i
iiiii
iiiii rRSrRpp
iiiiiii rvpp ,11,11
vi-1,i indica la velocità dell’origine della terna i rispetto all’origine della terna i – 1, espressa nella terna base
La terna i è solidale al braccio (conv. DH)
Velocità Traslazionale
Controllo dei Robot A. Rizzo
Velocità angolare1
1
iiii RRR
11
11
i
iiiiii RRRRR
11,11
111 )()()(
ii
iiii
iiiiii RSRRRSRS
11,111 )()()(
ii
iiiiiiii RSRRSRS
Derivando:
Controllo dei Robot A. Rizzo
Velocità angolarePer l’ortogonalità delle matrici di rotazione si può ancora scrivere
11,111 )()()(
ii
iiiiiiii IRSRRSRS
111
1,111 )()()(
i
iiTi
iiiiiiii RRRSRRSRS
iTi
iiiiiiii RRSRRSRS 1
1,111 )()()(
Osserviamo che per le matrici di rotazione vale la relazione, RS()RT = S(R), quindi:
ii
iiiiiii RRSRSRS )()()( 1,111
1,111
i
iiiii R iiii ,11
Controllo dei Robot A. Rizzo
Composizione di velocità
iiii
iiiiiii rvpp
,11
,11,11
Queste relazioni possono essere particolareggiate per giunti prismatici o rotoidali
Controllo dei Robot A. Rizzo
Giunto prismatico
i-1,i = 0 1,1 iiii zdv
1
,1111
ii
iiiiii rzdpp
Giunto rotoidale
11
,11,1,11
iiii
iiiiiiiii
z
rrpp
11
,1,111
iiii
iiiiiii
z
rpp
11
,11
iiii
iiiii
z
rpp
Controllo dei Robot A. Rizzo
Calcolo dello Jacobiano
n
n
oo
ppp
jj
jj
J
JJ
1
1
0
Giunto prismatico
1
1,1
,10
0
ip
o
ii
iiipiii
oiii
zj
j
dq
zdjqv
jq
i
i
i
Giunto rotoidale
)()(
11
1
11,1
1
,1,1,1
1,1
iip
io
ii
iiipiii
iioi
niiipiii
iioiii
ppzj
zj
q
ppzjqv
zjq
rjqv
zjq
i
i
i
i
i
i
Controllo dei Robot A. Rizzo
Calcolo dello jacobiano
rotoidali giunti z
)(z
prismatici giunti 0
1-i
11-i
1
i
i
i
i
pp
z
Jo
JpJ
zi-1 è dato dalla terza colonna della matrice di rotazione e quindi0
1iR
012
11011 )()( zqRqRz i
iii z0 = [0, 0, 1]T
Consente di selezionare la terza colonna
Controllo dei Robot A. Rizzo
p è dato dai primi tre elementi della quarta colonna della matrice di trasformazione 0
nT
01
101
~)()(~ pqAqAp nnn
1
0
0
0
~0p
pi-1 è dato dai primi tre elementi della quarta colonna della matrice di trasformazione0
1nT
012
11011
~)()(~ pqAqAp iiii
Si osserva che lo Jacobiano dipende dalla terna rispetto alla quale viene espressa la velocità dell’organo terminale.
JRo
oRJ
p
Ro
oRpu
uu
u
u
u
u
Controllo dei Robot A. Rizzo
Esempio: Manipolatore a tre bracci
1
11 )()(
i
ii
z
ppzqJ
con i = 1, 2, 3
210
221100)(zzz
ppzppzppzqJ
y3x3
Controllo dei Robot A. Rizzo
1
0
0
0
1000
0100
0
0
~~)(~ 1233123211123123
123312211123123
023
12
010
03
ssscs
cccsc
pAAApqTp
0
123312211
123312211
sss
ccc
p
012
11011
~)()(~ pqAqAp iiii
012
11011 )()( zqRqRz i
iii
Controllo dei Robot A. Rizzo
0
0
0
1
0
0
0
1000
0100
0010
0001
~0
000 pAp
1
0
0
0z
0
1
0
0
0
1000
0100
0
0
~11
111
1
0011
111
111
s
cscs
csc
pAp
1
0
0
1z
0
1
0
0
0
1000
0100
0
0
~2211
22112221
2211
012
102
22
22
ss
ccsscs
ccsc
pAAp
1
0
0
2z
Controllo dei Robot A. Rizzo
prodotto vettoriale tra due vettori P (Px, Py, Pz) e Q (Qx, Qy, Qz)
xyyx
zxxz
yzzy
PQPQ
PQPQ
PQPQ
QP
0
)( 123312211
123312211
00 ccc
sss
ppz
0
)( 1233122
1233122
11 cc
ss
ppz
0
)( 1233
1233
22 c
s
ppz
Controllo dei Robot A. Rizzo
111
000
000
000)(
12331233122123312211
12331233122123312211
cccccc
ssssss
qJ
111
)( 12331233122123312211
12331233122123312211
cccccc
ssssss
qJ
Nel piano x-y :
Controllo dei Robot A. Rizzo
Jacobiano analitico
qqJqq
pp P )(
qqJqq
)(
La derivata rispetto al tempo di non coincide, in generale, con il vettore velocità angolare definito in precedenza
: rappresentazione minima dell’orientamento
qqJqqJ
qJpx A
P
)()(
)(
q
qkqJ A
)()(
Controllo dei Robot A. Rizzo
Legame tra Jacobiano analitico e jacobiano
geometrico
csscs
ssccscsscccs
sccssccssccc
RRRR zyzEUL
Controllo dei Robot A. Rizzo
1
0
0
0
0
z
y
x
0
0
0
c
s
z
y
x
c
ss
sc
z
y
x
0
0
)(
01
0
0
T
c
ssc
scs
Controllo dei Robot A. Rizzo
Singolarità di rappresentazione di .
Il determinante della matrice T è pari a -s, il che significa che la relazione non è invertibile per = 0, .
Ciò significa che, sebbene ogni velocità di rotazione della terna possa essere espressa mediante un opportuno vettore di velocità angolare , esistono velocità angolari che non possono essere espresse mediante , quando l’orientamento della terna utensile impone s = 0.
In questa situazione, infatti, le velocità angolari che possono essere descritte da sono vincolati ad avere componenti nelle direzioni ortogonali all’asse z tra di loro dipendenti .
Gli orientamenti che annullano il determinante della matrice di trasformazione sono dette singolarità di rappresentazione di .
222 yx
Controllo dei Robot A. Rizzo
Significati fisici di e di
Da un punto di vista fisico il significato di è più intuitivo di quello di ’
Tuttavia l’integrale di ’ corrisponde a ed esprime quindi le variazioni impresse sugli angoli di Eulero per passare da un orientamento ad un altro, mentre l’integrale di non ammette alcuna interpretazione fisica
Controllo dei Robot A. Rizzo
Legame tra Jacobiano analitico e jacobiano
geometrico
xTxTo
oIv A
)(
qqJp
v
)(
qqJq
qJ
qJpx A
P
)()(
)(
J = TA()JA
Controllo dei Robot A. Rizzo
Singolarità Cinematiche Tutte quelle configurazioni, per le quali J diminuisce il
suo rango sono chiamate singolarità cinematiche. La caratteristica delle singolarità e di notevole interesse
per i seguenti motivi:a)Le singolarità rappresentano configurazioni in corrispondenza delle
quali si ha una perdita di mobilità della struttura, in altre parole non è possibile imporre all’organo terminale leggi di moto arbitrarie.
b)Quando la struttura è in una configurazione singolare, possono esistere infinite soluzioni al problema cinematico inverso
c)Nell’intorno di una singolarità, velocità ridotte nello spazio operativo possono indurre velocità molto alte nello spazio dei giunti.
Controllo dei Robot A. Rizzo
Singolarità cinematiche
Singolarità ai confini dello spazio di lavoro raggiungibile che si presentano quando il manipolatore è tutto steso o tutto ripiegato su se stesso. Queste singolarità possono essere evitate.
Singolarità all’interno dello spazio di lavoro raggiungibile che sono generalmente causate dall’allineamento di due o più assi di moto, in altre parole dall’assunzione di configurazioni particolari da parte dell’organo terminale. Queste costituiscono un problema serio in quanto, essendo all’interno dello spazio di lavoro, possono essere interessate da traiettorie pianificate nello spazio operativo.
Controllo dei Robot A. Rizzo
Esempio
12212211
12212211
cacaca
sasasaJ
det(J) = a1a2s2
il determinante si annulla per 2 = 0, , mentre 1 è influente ai fini delle determinazioni di posizioni singolari.
Le due configurazioni trovate corrispondono ai casi in cui l’organo terminale del manipolatore è situato al confine esterno (2 = 0, rappresentato in figura) o interno (2 = ).
12121
12121
cacaa
sasaaJ
Controllo dei Robot A. Rizzo
Analisi della ridondanza
qqJv )(
v è da intendersi come il vettore (rx1) delle velocità dell’organo terminale necessarie per specificare il compito;
J come la corrispondente matrice Jacobiana (rxn) estratta dallo Jacobiano geometrico;
il vettore (nx1) della velocità dei giunti.
Se r < n, il manipolatore risulta ridondante da un punto di vista cinematico ed esistono (n-r) gradi di mobilità ridondanti.
q
Controllo dei Robot A. Rizzo
L’immagine di J è il sottospazio R(J) in che individua le velocità dell’organo terminale che possono essere generate dalle velocità di giunto, nella configurazione assegnata al manipolatore;
Il nullo di J è il sottospazio N(J) in cui appartengono le velocità di giunto che non producono alcuna velocità all’organo terminale, nella configurazione assegnata al manipolatore.
qqJv )(
Controllo dei Robot A. Rizzo
Se lo Jacobiano è a rango pieno, si ha:
dim(R(J)) = r dim(N(J)) = n – r
e l’immagine di J ricopre l’intero spazio .
Al contrario, se lo Jacobiano degenera in una singolarità, la dimensione dell’immagine diminuisce e, allo stesso tempo, la dimensione del nullo aumenta, in quanto vale la relazione:
dim(R(J)) + dim(N(J)) = n (n è la dimensione dello spazio dei giunti)
indipendentemente dal rango della matrice J.
*qPer un manipolatore ridondante quando N(J) 0
qqJv )(Data Soluzione della
aqPqq * vqJqJPqJqJ a **
R(P) N(J)aq Vettore arbitrario
Controllo dei Robot A. Rizzo
Inversione della cinematica differenziale
qqJqqJ
qJpx A
P
)()(
)(
qqJv )(
vqJq )(1
t
dqqtq0
)()0()(
ttqtqtq kkk )()()( 1 ttt kk 1
Controllo dei Robot A. Rizzo
Manipolatori ridondantiUn manipolatore viene detto ridondante da un punto di vista cinematico quando possiede un numero di gradi di mobilità maggiore del numero di variabili necessarie alla caratterizzazione di un determinato compito. Detto in termini di spazi sopra introdotti un manipolatore è intrinsecamente ridondante quando la dimensione dello spazio operativo è minore della dimensione dello spazio dei giunti (m < n).
Controllo dei Robot A. Rizzo
Esempio
1000
0100
0
0
)( 1233123211123123
123312211123123
03
ssscs
cccsc
qT
321
123312211
123312211
321
123312211
123312211
0
0
0)(
sss
cccsss
ccc
p
p
p
pqkx z
y
x
Se non si fosse interessati all’orientamento, si ha x = [px, py]T e vi è quindi ridondanza cinematica r < m.
Controllo dei Robot A. Rizzo
Cinematica inversa dei manipolatori ridondanti
Se il manipolatore è ridondante (r < n), lo Jacobiano è una matrice rettangolare bassa e si pone il problema significativo di trovare le soluzioni – ne esisterà più di una – all’equazione .
Un possibile metodo di soluzione è quello di formulare il problema d’ottimo vincolato.
qqJv )(
qWqqg T 2
1)(
dove W è un’opportuna matrice (nxn) di peso, simmetrica e definita positiva.
Minimizzare qqJv )(Con il vincolo
Controllo dei Robot A. Rizzo
Metodo dei moltiplicatori di Lagrange
)(2
1),( qJvqWqqg TT funzionale di costo modificato
dove è un vettore incognito (rx1) di moltiplicatori
Soluzione :
T
T
JWqq
qg 10),(
qJvqg
T
0),(
T
T
JJWvqJv
JWq 11
Controllo dei Robot A. Rizzo
vJJW T 11 TJWq 1Sostituendo in
vJJWJWq TT 111
Caso particolare: la matrice di peso W coincide con la matrice identità I
vJq 1)( TT JJJJ Pseudo inversa destra
La soluzione ottenuta è quella che minimizza localmente la norma della velocità ai giunti.
Controllo dei Robot A. Rizzo
qqJp )( qsoluzione che minimizza
pJJJpJq TT 1)(
nqpJq In generale :
nq 0)( nqJJKer Appartiene alDove :
pqJpJJqpJJqJ nn )(
OSSERVAZIONI
Controllo dei Robot A. Rizzo
dqJJIpJq )(
dq È una qualunque velocità di giunto
JJIJ *
nqqJ *
0* qJJ
Scelta:
dove
l’espressione trovata precedentemente è anche quella che ci assicura il minor discostamento di daq dq
Controllo dei Robot A. Rizzo
pJ velocità di giunto in corrispondenza della quale e solo in corrispondenza della quale si ha una variazione di
.
)( dqJJI velocità di giunto in corrispondenza della quale non può aversi variazione dip
p
dqJJIpJq )(
Controllo dei Robot A. Rizzo
Minimizzazione di obiettivi
dqJJIq
HpJ
q
Hq
q
HH
positivadefinitaqH
Se riusciamo tramite ad imporre:
Potremo avvicinarci ad un minimo assoluto per H
dq
0H
Controllo dei Robot A. Rizzo
0
K
q
HKq
T
d
pJq
H
q
HJJI
q
HKH
T
Il primo addendo è sicuramente negativo, mentre sul segno del secondo non si può dire niente.
Controllo dei Robot A. Rizzo
Esempi di task
0)(min,
qpqHop
p:generico punto del manipolatore0:punto su di un ostacolo Massimizzando H si può riuscire ad aggirare un ostacolo.
2
12
1
n
i imiM
ii
nqH
qqiMim():rappresenta la massima (minima)escursione per la rispettiva variabile giuntoe qi è il valore medio della corsa . Minimizzando H si può riuscire a starelontano dai fine corsa.
)()(det)( qJqJqH TLa H così definita rappresenta una misuradella manipolabilità. Massimizzando H si può riuscire a stare lontani dalle singolarità.
q
HJJIKpJq
Controllo dei Robot A. Rizzo
Inversione cinematica
ttvtqJtqtq kkkk )())(()()( 1
1
Deriva numerica xxe d
qqJxxxe Add )(
E’ necessario, nella definizione di un algoritmo di inversione, far
dipendere da e in modo tale che l’equazione differenziale
precedente produca un e(t) convergente (asintoticamente) a zero
q
Controllo dei Robot A. Rizzo
(Pseudo-)inversa dello Jacobiano
KexqJq dA )(1
0Kee 0)()( qJJIKexqJq AAdA
Per un manipolatore ridondante:
Controllo dei Robot A. Rizzo
Trasposta dello Jacobiano
KeeeV T
2
1)( 0)0(,00)( VeeV
qqKJexKexKexKeV AT
dTT
dT )(
KeqJq TA )(
KeqJqKJexKeV TAA
Td
T )()(
Per un riferimento costante ( ) risulta definita negativa.
Scegliamo
0dx
Controllo dei Robot A. Rizzo
KeqJq TA )(
Controllo dei Robot A. Rizzo
Trasposta dello Jacobiano Richiede solo il computo di funzioni cinematiche
dirette Se il riferimento non è nullo la derivata della funzione
di Lyapunov può essere resa definita negativa tramite un termine dipendente dalla (pseudo)inversa dello jacobiano (perdo i vantaggi)
In ogni caso l’errore di inseguimento si può mantere limitato. Esso sarà tanto più piccolo quanto più grande è la norma della matrice dei guadagni K
Tuttavia l’implementazione a tempo discreto dell’algoritmo impone limiti superiori della norma di K