Controllo dei Robot A. Rizzo Cinematica differenziale n La cinematica differenziale caratterizza i...

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.


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:


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


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.


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


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


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


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


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


Velocità angolare1

1

iiii RRR

11

11

i

iiiiii RRRRR

11,11

111 )()()(

ii

iiii

iiiiii RSRRRSRS

11,111 )()()(

ii

iiiiiiii RSRRSRS

Derivando:


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


Composizione di velocità

iiii

iiiiiii rvpp

,11

,11,11

Queste relazioni possono essere particolareggiate per giunti prismatici o rotoidali


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


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


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


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


Esempio: Manipolatore a tre bracci

1

11 )()(

i

ii

z

ppzqJ

con i = 1, 2, 3

210

221100)(zzz

ppzppzppzqJ

y3x3


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


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


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


111

000

000

000)(

12331233122123312211

12331233122123312211

cccccc

ssssss

qJ

111

)( 12331233122123312211

12331233122123312211

cccccc

ssssss

qJ

Nel piano x-y :


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

)()(


Legame tra Jacobiano analitico e jacobiano

geometrico

csscs

ssccscsscccs

sccssccssccc

RRRR zyzEUL


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


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


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


Legame tra Jacobiano analitico e jacobiano

geometrico

xTxTo

oIv A

)(

qqJp

v

)(

qqJq

qJ

qJpx A

P

)()(

)(

J = TA()JA


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.


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.


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


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


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 )(


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


Inversione della cinematica differenziale

qqJqqJ

qJpx A

P

)()(

)(

qqJv )(

vqJq )(1

t

dqqtq0

)()0()(

ttqtqtq kkk )()()( 1 ttt kk 1


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).


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.


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


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


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.


qqJp )( qsoluzione che minimizza

pJJJpJq TT 1)(

nqpJq In generale :

nq 0)( nqJJKer Appartiene alDove :

pqJpJJqpJJqJ nn )(

OSSERVAZIONI


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


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 )(


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


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.


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

qq

qq

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


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


(Pseudo-)inversa dello Jacobiano

KexqJq dA )(1

0Kee 0)()( qJJIKexqJq AAdA

Per un manipolatore ridondante:


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


KeqJq TA )(


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

Controllo dei Robot A. Rizzo Cinematica differenziale n La cinematica differenziale caratterizza i...

Documents

Transcript of Controllo dei Robot A. Rizzo Cinematica differenziale n La cinematica differenziale caratterizza i...