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
ω
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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:
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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 ⋅⋅−=⋅⋅
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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):
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Proprietà della Skew Matrix
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
ωω
ωω
ωω
ω
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Proprietà della Skew Matrix
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 ⋅⋅⋅= ωω
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
( ) ( ) RRSRS T ⋅⋅⋅= ωω
In cui, eliminando l’esplicita dipendenza dal tempo:
( ) ( ) TRSRRS ⋅⋅=⋅ ωω
Proprietà della Skew Matrix
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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 ωωω&
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
( ) ( ) ( ) 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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
Calcolo dello Jacobiano
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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:
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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 − − − − − −= + + + × + + ×&
Nel giunto prismatico :
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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 −− +=
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Calcolo dello Jacobiano – Contributo Rotazionale
OjJCalcolo di :
Nel giunto prismatico :
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
( )⎥⎦
⎤⎢⎣
⎡ −×=
−
−−
1
11
j
jnj
j z
ppzJ
Rotoidale
⎥⎦
⎤⎢⎣
⎡= −
01j
j
zJ
Prismatico
Calcolo dello Jacobiano
In definitiva:
1 1j n jz p p− −⎡ ⎤⎣ ⎦
Che dipendono da:
… e quindi dalla postura del robot
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
Calcolo dello Jacobiano
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion