M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Cinematica differenziale La cinematica...

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion

Cinematica differenziale

La cinematica differenziale caratterizza i legami tra le velocità dei giunti e la corrispondente velocità lineare ed angolare dell’organo terminale

Tali legami sono descritti tramite una matrice denominata Jacobiano Geometrico

Se la postura dell’organo terminale è espressa facendo riferimento ad una rappresentazione in forma minima (angoli di Eulero, Cardano, asse-angolo, quaternioni, …), è possibile calcolare lo Jacobiano direttamente mediante differenziazione rispetto alle variabili di giunto. Quello che si ottiene è lo Jacobiano Analitico (può essere diverso dall’altro)

Lo Jacobiano è utile per:

• l’analisi delle singolarità

• analisi di ridondanza

• inversione cinematica

• determinazione legame tra forze applicate all’organo terminale e le coppie/forze ai giunti

⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢

⎣

⎡

⋅⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡

n

jOnOjO

PnPjP

n

n

q

q

q

JJJ

JJJp

&

&

&

&

...

...

......

......

1

1

1

ω


Cinematica differenziale – Jacobiano Geometrico

Manipolatore ad n gradi di mobilità, l’equazione cinematica diretta:

In cui q = [q1 … qn]T è il vettore delle

variabili di giunto

Legami tra le velocità dei giunti e la corrispondente velocità lineare ed angolare dell’organo terminale:

In forma compatta:

Jacobiano Geometrico funzione delle variabili di giunto q:


Derivata di una matrice di Rotazione

Si tratterà di derivare la matrice di trasformazione per cui è ora necessario ricavare come si deriva una componente fondamentale: la matrice di rotazione

La matrice di rotazione in una traiettoria generica sarà funzione del tempo, per cui:

( ) ( ) ( ) ( )tRtRtRtR TT⋅⋅

⋅−=⋅ ( ) ( ) ( ) ( ) ( ) ( )tRtRtRtRtRtR TT ⋅⋅−=⋅⋅⋅⋅

( ) ( ) ( ) ( )tRtRtRtR T ⋅⋅−=⋅⋅


Se definiamo la SKEW MATRIX:

( ) ( ) 0=+ tStS T ( ) ( )tStS T−=

Ovvero la matrice S(t) è anti-simmetrica (ha gli elementi sulla diagonale nulli e la parte triangolare superiore di segno opposto rispetto alla parte inferiore)

( ) ( ) ( ) ( )tRtRtRtR T ⋅⋅−=⋅⋅

( ) ( ) ( )tRtStR T ⋅−=⋅

Riprendendo il risultato di prima:

( ) ( ) ( )tRtStR ⋅=⋅

Derivata di una matrice di Rotazione


Proprietà della Skew Matrix

Consideriamo un punto p’ solidale ad un corpo sottoposto ad un moto rigido di sola rotazione

La sua posizione nello spazio è pari a p(t) = R(t) p’

Dunque la derivata:

Che può scriversi:

Del resto sappiamo che la rotazione nel tempo di un vettore può essere espressa tramite il vettore velocità angolare (t):



Dunque la Skew Matrix descrive il prodotto vettoriale tra il vettore velocità angolare e la matrice di rotazione:

( ) ( ) ( ) ( ) ( )tRtStRttR ⋅=×=ω&

Tale risultato è di importanza fondamentale in quanto trasforma equazioni in cui è presente la derivata od il prodotto vettoriale in equazioni in cui compare semplicemente il prodotto tra matrici

Tale artificio è in qualche modo analogo all’uso della trasformata di Fourier nel campo dell’analisi segnali in quanto si trasformano relazioni poco trattabili (in quel caso equazioni differenziali nel dominio del tempo) in equazioni algebriche

Poiché la Skew Matrix descrive il prodotto vettoriale tra il vettore velocità angolare e la matrice di rotazione essa sarà pari a:

( ) ( )( )⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−

−==

0

0

0

xy

xz

yz

tStS

ωω

ωω

ωω

ω


Esempio - Skew Matrix

Si consideri la rotazione elementare attorno all’asse z:

Supponendo funzione del tempo e che:

( ) ( )( )⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−

−==

0

0

0

xy

xz

yz

tStS

ωω

ωω

ωω

ω ( )⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

&0

0

t



Cerchiamo adesso una proprietà notevole:

Consideriamo di nuovo un punto p’ solidale ad un corpo sottoposto ad un moto rigido di sola rotazione

( ) ( ) 'ptRtp ⋅= ( ) ( ) ( ) 'ptRttp ⋅×=ω&

Il risultato del prodotto vettoriale tra due vettori p1 e p2 ruotato di R è pari al prodotto

vettoriale dei due vettori ruotati di R: (scaturisce da semplici considerazioni geometriche di tipo vettoriale)

1 2Vp p p= × ( ) ( )1 2VR p R p R p⋅ = ⋅ × ⋅

Si noti come tale proprietà denoti che il prodotto vettoriale non segue affatto le regole di composizione algebrica

( ) ( ) ( ) ( )( ) ( ) ( )( ) ( ) ( )( ) ( ) ( ) '' ptRtRttRSptRtRttRtptR ⋅⋅⋅⋅=⋅⋅×⋅=⋅ ωω&

( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )' ' 'Tp t R t S R t t R t R t p R t p S R t pω ω= ⋅ ⋅ ⋅ ⋅ ⋅ = ⋅ = ⋅ ⋅&&

( )( ) ( ) ( ) ( )( ) ( )tRttRStRtS T ⋅⋅⋅= ωω


( ) ( ) RRSRS T ⋅⋅⋅= ωω

In cui, eliminando l’esplicita dipendenza dal tempo:

( ) ( ) TRSRRS ⋅⋅=⋅ ωω



Velocità di un braccio - Lineare

Nota: si omette l’indicazione esplicita dell’apice “0” delle grandezze riferite alla terna di base

i

i-1

0 terna di base

1,111

1,111

−−−−

−−−− ⋅×+⋅+= i

iiiii

iiiii rRrRpp &&&

iiiiiii rvpp ,11,11 −−−− ×++= &&

Espressione della velocità lineare del braccio i in funzione della velocità lineare ed angolare del braccio i-1

= 0 giunto rotativo

0 giunto prismatico

(quando in apice non appare simbolo vuol dire che il vettore è espresso rispetto alla terna di base)

derivando


Velocità di un braccio - Angolare

Partendo dalla composizione delle rotazioni e derivando:

11

−− ⋅= i

iii RRR d/dt

( ) ( )1 1 11 1 1 1,

i i ii i i i i i i iR S R R R S Rω ω− − −

− − − −= ⋅ ⋅ + ⋅ ⋅&

( ) 11

11

−−

−− ⋅+⋅=⋅= i

iiiiiiii RRRRRSR &&& ω

Dove indica la velocità angolare della terna i rispetto alla i-1 espressa nella terna i-111,ii i −−

( ) ( ) ( ) iiiii

iii

Ti

iiii

ii

iiii RRSRRRSRRSR ⋅⋅=⋅⋅⋅⋅=⋅⋅ −

−−−

−−−−−

−−−−

1,11

111

1,11

11,11 ωωω

( ) ( ) ( ) iiiiiiiiii RRSRSRSR ⋅⋅+⋅=⋅= −

−−−1,111 ωωω&


( ) ( ) ( ) iiiiiiiiii RRSRSRSR ⋅⋅+⋅=⋅= −

−−−1,111 ωωω&

iiiiiiiii RRRR ×⋅+×=× −

−−−1,111

( ) 01,111 =×⋅−− −

−−− iiiiiii RR ωωω

Velocità di un braccio - Angolare

iiii ,11 −− +=

Espressione della velocità angolare del braccio i in funzione della velocità angolare del braccio i-1 ed angolare relativa tra braccio i ed i-1


iiii

iiiiiii rvpp

,11

,11,11

−−

−−−−

+=

×++=

&&

In definitiva:Da cui si nota come l’effetto di un giunto j si ripercuota solamente sui giunti successivi (come del resto doveva essere trattandosi di catene cinematiche aperte)

Vogliamo adesso calcolare lo Jacobiano definito come:

(generalizzando al caso rotativo e prismatico)

⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢

⎣

⎡

⋅⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡

n

jOnOjO

PnPjP

n

n

q

q

q

JJJ

JJJp

&

&

&

&

...

...

......

......

1

1

1

ω

Calcolo dello Jacobiano


Mediante moltiplicazione matriciale:

⎥⎦

⎤⎢⎣

⎡⋅++⋅++⋅⋅++⋅++⋅

=⎥⎦

⎤⎢⎣

⎡

nOnjOjO

nPnjPjP

n

n

qJqJqJ

qJqJqJp

&&&

&&&&

......

......

11

11

ω

In cui, evidentemente:

rappresenta il contributo della derivata della variabile di giunto j alla velocità lineare dell’end-effector (con tutti gli altri giunti fermi)

rappresenta il contributo della derivata della variabile di giunto j alla velocità angolare dell’end-effector (con tutti gli altri giunti fermi)

Oj

Pj

J

J

Il ché vuol dire che per determinare gli elementi dello Jacobiano si può applicare il principio di sovrapposizione degli effetti (del resto l’operazione di derivazione porta ad una approssimazione lineare)

Nota:

dqj/dt è uno scalare

JPj e JOj sono vettori 3x1



iiiiiii rvpp ,11,11 −−−− ×++= &&

Calcolo dello Jacobiano – Contributo Lineare

1 1, 1, 1 1, 1 1,... ...n j j j n n j j j n n np p v v r r − − − − − − −= + + + + × + + ×& &

PjJCalcolo di :

1, 1, 1 1, 1 1,...n j j n n j j j n n np v v r r − − − − − −= + + + × + ×&

Solo il braccio j si muove, quindi alcuni termini si annullano

Nel giunto prismatico :

1,1 −− ⋅= jjjj zdv &

jj dq &&=

Nel giunto rotoidale :

1,1 −− ⋅= jjjj zϑ &

jjq ϑ&&=

Poiché l’effetto di un giunto j si ripercuote solamente sui giunti successivi:


Giunto Prismatico – Contributo Lineare

PjJCalcolo di :

1, 1, 1 1, 1 1,... ...n j j n n j j j n n np v v r r − − − − − −= + + + × + + ×&


1,1 −− ⋅= jjjj zdv &

jj dq &&=

= 0 poiché gli altri giunti prismatici sono fermi

= 0 poiché non ci sono giunti rotoidali in azione

1−⋅= jjn zdp &&

1−= jPj zJ

Prismatico


Giunto Rotoidale – Contributo Lineare

PjJCalcolo di :

nnnjjjnnjjn rrvvp ,11,11,1,1 ...... −−−−−− ×++×++= &

= 0 poiché non ci sono giunti prismatici in azione

Tutte le velocità angolari sono uguali in quanto si ha l’effetto del giunto rotoidale anche sugli altri giunti

( )11 −− −×= jnjPj ppzJNel giunto rotoidale :

1,1 −− ⋅= jjjj zϑ &

jjq ϑ&&=

( ) njjnnjjjnnjjjjn rrrrrp ,11,1,11,11,11 ...... −−−−−−−−− ×=++×=×++×= ωωωω&

( ) ( )1111 −−−− −×⋅=−×=

jnjjjnjn ppzppp ϑω &&

Rotoidale

iiii ,11 −− +=


Calcolo dello Jacobiano – Contributo Rotazionale

OjJCalcolo di :


jj dq &&=

Nel giunto rotoidale :

1,1 −− ⋅= jjjj zϑ &

jjq ϑ&&=

iiii ,11 −− +=

0,1 =− jj

0=OjJPrismatico

1−= jOj zJRotoidale

1,n j j −=

= 0 poiché solo il giunto i_esimo è in azione


( )⎥⎦

⎤⎢⎣

⎡ −×=

−

−−

1

11

j

jnj

j z

ppzJ

Rotoidale

⎥⎦

⎤⎢⎣

⎡= −

01j

j

zJ

Prismatico


In definitiva:

1 1j n jz p p− −⎡ ⎤⎣ ⎦

Che dipendono da:

… e quindi dalla postura del robot


Tali relazioni consentono il calcolo dello Jacobiano in maniera semplice e sistematica sulla base delle relazioni cinematiche dirette, in particolare:

( ) ( ) ( )⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡⋅⋅⋅⋅=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡⋅= −

−−−

−−−−

1

0

0

...

1

0

0

1212

321

01

011 j

jjj

jjjj qRqRqRRz

( ) ( ) ( ) ( ) ( )3...1

1

0

0

0

...3...1

1

0

0

0

11

211

01

0

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⋅⋅⋅⋅=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⋅= −−

−− n

nnn

nnnn qAqAqAAp

( ) ( ) ( ) ( ) ( )3...1

1

0

0

0

...3...1

1

0

0

0

1212

321

01

011

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⋅⋅⋅⋅=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⋅= −−−−

−−−− j

jjj

jjjj qAqAqAAp


M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Cinematica differenziale La cinematica...

Documents

Transcript of M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Cinematica differenziale La cinematica...