Inversione differenziale della Cinematica

22
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Inversione differenziale della Cinematica Si era visto a proposito dell’inversione della cinematica tramite cinematica diretta, che il problema ammette soluzioni in forma chiusa solo per strutture di manipolazione semplici Sorgono immediatamente problemi quando: • il manipolatore ha una struttura complessa (non facilmente risolvibile o soluzioni non in forma esplicita) • è ridondante (infinite soluzioni) • in corrispondenza di posture singolari … ma adesso abbiamo un nuovo strumento : la Cinematica Differenziale che ci fornisce la relazione tra la velocità ai giunti e le velocità nello spazio operativo (in funzione della postura) Dunque perché non invertire la relazione cinematica differenziale ed integrare? q J v & =

description

Si era visto a proposito dell’inversione della cinematica tramite cinematica diretta, che il problema ammette soluzioni in forma chiusa solo per strutture di manipolazione semplici Sorgono immediatamente problemi quando: - PowerPoint PPT Presentation

Transcript of Inversione differenziale della Cinematica

Page 1: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Si era visto a proposito dell’inversione della cinematica tramite cinematica diretta, che il problema ammette soluzioni in forma chiusa solo per strutture di manipolazione semplici

Sorgono immediatamente problemi quando:

• il manipolatore ha una struttura complessa (non facilmente risolvibile o soluzioni non in forma esplicita)

• è ridondante (infinite soluzioni)

• in corrispondenza di posture singolari

… ma adesso abbiamo un nuovo strumento: la Cinematica Differenziale che ci fornisce la relazione tra la velocità ai giunti e le velocità nello spazio operativo (in funzione della postura)

Dunque perché non invertire la relazione cinematica differenziale ed integrare?

qJv &⋅=

Page 2: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Basterà assegnare la traiettoria specificando v(t) e le sue condizioni iniziali di posizione ed assetto nello spazio operativo e di postura nello spazio dei giunti e determinare q(t) tramite:

( ) )0()(0

1

qdqtq

vJqt

+⋅=

⋅=

ξξ&

&

L’integrazione potrà essere fatta nel tempo discreto (tramite ad esempio la formula di Eulero) in forma ricorsiva ed implementata quindi in tempo reale:

( )Ckkkk

Ckkkk

Ckkk

TvJqq

TtvtqJtqtq

Ttqtqtq

⋅⋅+=

⋅⋅+=

⋅+=

−−−−

−−−

−−

1111

111

1

11

)()()()(

)()()( &

Page 3: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

… ma lo Jacobiano deve essere quadrato e di rango pieno

Se il manipolatore è ridondante (r < n) tale condizione non è verificata (J è rettangolare bassa) ed in generale esistono più soluzioni

Per risolvere il problema della determinazione della soluzione tra tante, occorre specificare criteri di scelta

E quindi trasformare il problema in un problema di ottimizzazione vincolata (in cui ci sarà un funzionale da minimizzare che determinerà la scelta):

ad esempio si cerca quella soluzione che minimizza il funzionale: (ovvero le velocità ai giunti)

Con il vincolo:

il funzionale è quadratico con matrice W simmetrica e definita positiva (ovvero il funzionale è sempre > 0 per ogni valore di dq/dt ≠ 0)

qJv &⋅=

Page 4: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Il problema di minimo vincolato può essere risolto con i moltiplicatori di Lagrange mediante il funzionale modificato

è il vettore 1 x r dei moltiplicatori che permette di incorporare il vincolo nel funzionale (non modificandolo in quanto il vincolo è una equazione pari a zero)

il funzionale (funzione di dq/dt e ) viene derivato per determinarne il minimo (il funzionale è una forma quadratica associata ad una matrice definita positiva, nel caso scalare è come se fosse a·x2 il cui minimo si determina mediante semplice derivazione)

Page 5: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

( ) ( ) ( ) ( )

( ) ( ) ( )

( )

( )

1 1,

2 2

1 1

2 2

1 1

2 2

1 1

2 2

1 1

2 2

T T T

T T T

T T T

T T T

T T T T T

g q q Wq q W q J qq q q q

q Wm m W q J I m qq q

q Wm m W I Jq

m Wq m W Jq

m W m W J m W J

λ λ

λ

λ

λ

λ λ

∂ ∂ ∂ ∂= ⋅ + − =

∂ ∂ ∂ ∂

∂ ∂⋅ + − ⋅ = =

∂ ∂

∂⋅ + ⋅ − =

∂+ − =

+ − = −

& & & & & && & & &

& & & & & && &

& & &&

& & &&

& & &

TW W=Matrice simmetrica

1 Tq W J −=&

È uno scalare quindi uguale al suo trasposto

(con il suggerimento di Bertolazzi)

˙ m TW − λT J = 0

W ˙ m − JT λ = 0

W ˙ q − JT λ = 0

Page 6: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Page 7: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Nel caso in cui W = I la soluzione impiega la pseudo-inversa J†, e la soluzione trovata è quindi quella che minimizza la norma di tutte le velocità ai giunti senza distinzione

Se invece:

⎥⎦

⎤⎢⎣

⎡=

10

09W

In questo caso la prima componente delle velocità dei giunti viene minimizzata in maniera più marcata rispetto alla seconda (1/3)

( ) 1

2Tg q q q= ⋅& & &

Page 8: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Come visto in precedenza, evitare le configurazioni di singolarità è un requisito fondamentale nella pianificazione della traiettoria

Dunque perché non specificare un funzionale in cui la soluzione si avvicini il più possibile ad una soluzione in grado di evitare le singolarità?

Trovata la traiettoria delle variabili di giunto in grado di evitare le singolarità

Si stabilisce il seguente funzionale da minimizzare:

0q&

0* qPqq &&& ⋅+=… che risulta essere proprio della forma:

Con il vincolo:

(in maniera analoga a quanto visto prima)

Page 9: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Se infatti:

Ovvero P è proprio il proiettore sul nullo di J

P = I − J¬ ⋅J

Si ha che:

∀q0 : J ⋅P ⋅q0 = J ⋅ I − J¬ ⋅J( ) ⋅q0 =

J ⋅ I − JT ⋅ J ⋅JT( )

−1⋅J( ) ⋅q0 = J − J ⋅JT ⋅ J ⋅JT

( )−1⋅J( ) ⋅q0 =

J − J( ) ⋅q0 = 0

Page 10: Inversione differenziale della Cinematica

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

Come evitare le SINGOLARITA’

Rimane solo da determinare in modo tale che le singolarità vengano evitate.

Definita la funzione:

0q&

Con k0 > 0

Spiegazione: la funzione w(q) è pari a zero in corrispondenza delle singolarità, cresce man mano che ci si allontana dalle configurazioni singolari

Dunque scegliere lungo il gradiente vuol dire allontanarsi dalle configurazioni non desiderate!

0q&

Basta scegliere:

Page 11: Inversione differenziale della Cinematica

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

Come evitare situazioni di FINE CORSA

… se si vuole evitare le situazioni di fine-corsa (anche esse generalmente singolari) che creano situazioni di limitazione alla destrezza del manipolatore

La w(q) la si definisce come segue:

dove:

qiM e qim rappresentano la massima e minima escursione alla rispettiva variabile di

giunto qi

il valore medio della corsaiq

iq iq

( )w q

iMqimq

Page 12: Inversione differenziale della Cinematica

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

Come evitare OSTACOLI

… se si vuole evitare ostacoli

La w(q) la si definisce come segue:

dove:

o è il vettore posizione di un opportuno punto dell’ostacolo (ad esempio il centro)

p è un opportuno punto (od una collezione di punti, oppure …) della struttura come ad esempio l’organo finale in operazioni di inserimento

Page 13: Inversione differenziale della Cinematica

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

( )11 1 1 1( ) ( ) ( ) ( ) ( )k k k k C kq t q t J q t v t T tε−

− − − −= + ⋅ ⋅ +

… siamo sicuri che determinare lo stato delle variabili di giunto mediante una operazione di integrazione non crei dei problemi?

Inversione differenziale della Cinematica

NO, LA DERIVA !!!

Page 14: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Per ovviare all’inconveniente della deriva che porta a seguire traiettorie che si discostano in modo pressoché proporzionale con il tempo dalla traiettoria desiderata, occorre considerare l’errore nello spazio operativo:

( ) ( ) ( )de t x t x t= −

Dove xd(t) è la traiettoria desiderata ed x(t) è la traiettoria calcolata mediante

inversione differenziale della cinematica

Derivando rispetto al tempo:

de x x= −& & & ( )de x J q q= − ⋅& & &

Che, mediante la scelta , conduce a problemi di deriva

in quanto il termine di incertezza è sempre presente (ad esempio errori numerici o di modellazione)

( ) ( )1dq J q x tε−= ⋅ +& &

Page 15: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

( )de x J q q= − ⋅& & &

Dunque bisogna scegliere l’andamento di dq/dt che tenga conto dell’errore nello spazio operativo e possibilmente che presenti una dinamica stabile dell’errore in modo che converga a zero:

Con K definita positiva ed autovalori tutti > 0

Ricordando:

e K e=− ⋅&

( )dK e x J q q− ⋅ = − ⋅& &

( ) ( )1dq J q x K e−= ⋅ + ⋅& &( )1

dq J q x−= ⋅& & ≠

Page 16: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Quale è lo schema a blocchi dinamico che implementa tale metodo di integrazione basato sulla retroazione dall’errore nello spazio operativo?

Impiegare i blocchi sottostanti:

(Ingressi) (uscita)Cinematica diretta

Iacobiano inverso

Integratore

Costante di retroazione

Page 17: Inversione differenziale della Cinematica

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

Serve un richiamo di Simulink?

Page 18: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Quale è lo schema a blocchi dinamico che implementa il metodo di integrazione basato sulla retroazione dall’errore nello spazio operativo, di seguito riportato?

( ) ( )1dq J q x K e−= ⋅ + ⋅& &

Page 19: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Se il manipolatore è ridondante la J non è quadrata e non è possibile invertirla, per cui si ha la generalizzazione:

Stessa formula con il solo impiego della pseudo-inversa

Parte dedicata al controllo dei moti interni indirizzati ad evitare le singolarità, gli ostacoli, i finecorsa, a minimizzare l’energia, etc

P: proiettore nel nullo

Page 20: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

A volte invertire lo Jacobiano può risultare non ottimale dal punto di vista computazionale (nel caso di microprocessori a scarsa velocità/memoria/potenza di calcolo, caso sempre meno frequente ma ancora possibile in talune applicazioni industriali)

Dunque si vuole evitare l’inversione

… e quindi perché non retroazionare semplicemente (con una semplice operazione di trasposizione) rispetto all’errore?

( ) ( ) ( )( ) ( ) ( )

T

Td

J q q J q J q K e

x J q J q K x x

⋅ = ⋅ ⋅ ⋅

= ⋅ ⋅ ⋅ −

&

&

In questo caso si ha una variazione dello stato nello spazio operativo solo se l’errore è diverso da zero. Equivale ad una azione proporzionale.

Si dimostra che il metodo è asintoticamente stabile (mediante il criterio di Liapunov)

Page 21: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica

Quale è lo schema a blocchi dinamico che implementa tale metodo di integrazione basato sulla retroazione dall’errore nello spazio operativo?

( ) ( )Tdq J q K x x= ⋅ ⋅ −&

Page 22: Inversione differenziale della Cinematica

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

Inversione differenziale della Cinematica