M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Simulazione - Inversione Cinematica Si...

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

Simulazione - Inversione Cinematica

Si vuole confrontare tramite simulazione i diversi algoritmi di inversione differenziale della cinematica

Si supponga di avere un manipolatore planare a tre bracci di lunghezza 0.5 m

La postura iniziale sia q0= []T corrispondente nello spazio operativo a [0

0.5]T

Si vuole che l’organo terminale esegua un percorso circolare di raggio 0.25 m con centro in (0.25, 0.5) con la seguente traiettoria:


Simulazione - Inversione Cinematica

La postura iniziale sia q0= []T corrispondente nello spazio operativo a [0

0.5]T

Si vuole che l’organo terminale esegua un percorso circolare di raggio 0.25 m con centro in (0.25, 0.5) con la seguente traiettoria:

0.5 m


Simulazione – Calcolo Jacobiano


PROVARE A GENERARE TRAIETTORIE:- VICINE AL LIMITE DEL CAMPO OPERATIVO

- VICINE ALLE SINGOLARITA'

NOTA: calcola il setpoint dei giunti (loro derivata rispetto al tempo) mediante inversione dello Jacobiano

INVERSIONE DELLA CINEMATICA MEDIANTE INVERSIONE DELLO JACOBIANO

FAR VEDERE COSA SUCCEDE CON LA DINAMICA DEI MOTORI !!!!!!!!!!!

XY Graph

Xd

To Workspace2

t

To Workspace1

X

To Workspace

Matrix

Multiply

Product

Tempo Posa

PianificazioneTraiettoria

MATLABFunction

Jacobiano Inverso

1s

Integrator1

du/dt Derivative

Demux

Clock

q X

Cinematica Diretta

dq/dt

Simulazione – Catena Aperta

( )1dq J q x−= ⋅& &








XY Graph

To Workspace2

To Workspace1

To Workspace

Product

Tempo Posa


Jacobiano InversoIntegrator1

Derivative

Clock

Cinematica Diretta

dq/dt

Blocco di pianificazione traiettoria:








XY Graph

To Workspace2

To Workspace1

To Workspace

Product



Derivative

Clock

q X

Cinematica Diretta

dq/dt

Blocco di Cinematica diretta:


INVERSIONE DELLA CINEMATICA MEDIANTE RETROAZIONE CON JACOBIANO INVERSO

NOTA: occorre definire la condizione iniziale [pi –pi/2 –pi/2] dell’integratore come un vettore altrimenti le compatibilità dimensionali non vengono verificate




Blocco di Inversione Jacobiano:






XY Graph

To Workspace2

To Workspace1

To Workspace

Product


MATLABFunction


Derivative

Clock

Cinematica Diretta

dq/dt

function Jinv = JacobianoINV(q)a1 = 0.5;a2 = 0.5;a3 = 0.5;J = [ (-a1*sin(q(1))-a2*sin(q(1)+q(2))-a3*sin(q(1)+q(2)+q(3))) (-a2*sin(q(1)+q(2))-a3*sin(q(1)+q(2)+q(3))) (-a3*sin(q(1)+q(2)+q(3))); ... (a1*cos(q(1)) + a2*cos(q(1)+q(2)) + a3*cos(q(1)+q(2)+q(3))) (a2*cos(q(1)+q(2)) + a3*cos(q(1)+q(2)+q(3))) (a3*cos(q(1)+q(2)+q(3))); ... 1 1 1];Jinv = J^-1;


Risultati della Simulazione – Catena Aperta

Si nota uno scostamento circa progressivo tra la traiettoria desiderata e quella eseguita in simulazione

0 0.1 0.2 0.3 0.4 0.5

0.25

0.3

0.35

0.4

0.45

0.5

0.55

0.6

0.65

0.7

0.75 traiettoria eseguitatraiettoria desiderata

0 2 4 6 8 10 12-0.02

-0.01

0

0.01

0.02

0.03Errore in X

[m]

0 2 4 6 8 10 12-0.04

-0.02

0

0.02

0.04Errore in Y

[m]



Si nota un SALTO!!!

… dovuto allo Jacobiano che si è avvicinato troppo allo zero!

-0.5 0 0.5 1 1.5-1.2

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8traiettoria eseguitatraiettoria desiderata

0 5 10 15 20 25 300

0.05

0.1

0.15

0.2

0.25Valore assoluto del determinante dello Jacobiano

tempo [s]



Primo: -250°

Secondo: 0°

Terzo: 219°

-1 -0.5 0 0.5 1-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

Cosa succede in corrispondenza del salto?


Simulazione – Catena Chiusa

Si simuli adesso il metodo di integrazione basato sulla retroazione dall’errore nello spazio operativo e Jacobiano trasposto

500 0 0

0 500 0

0 0 100

K

⎡ ⎤⎢ ⎥=⎢ ⎥⎢ ⎥⎣ ⎦

Si consideri come prima ipotesi:





NOTA: calcola il setpoint dei giunti mediante l'errore di POSA moltiplicandolo per lo Jacobiano trasposto

INVERSIONE DELLA CINEMATICA MEDIANTE RETROAZIONE CON TRASPOSTA DELLO JACOBIANO

XY Graph

DD

To Workspace3

Xd

To Workspace2

t

To Workspace1

X

To Workspace

MatrixMultiply

Product

Tempo Posa


K*u

K

MATLABFunction

Jacobiano Trasposto1s

Integrator1

Demux

30

Clock

q X

Cinematica Diretta

MATLABFunction

ABS(DET(J))

3

3

3

3

3

3

33

3dq/dt

3

33

[3x3]

[3x3]3 3

( )Tq J q K e= ⋅ ⋅&Si noti che non ci sono più le operazioni di:

- inversione dello Jacobiano

- derivazione della traiettoria


0 5 10 15 20 25 30-0.15

-0.1

-0.05

0

0.05Errore in X

[m]

0 5 10 15 20 25 30-0.15

-0.1

-0.05

0

0.05Errore in Y

[m]

0 0.1 0.2 0.3 0.4 0.50.25

0.3

0.35

0.4

0.45

0.5

0.55

0.6

0.65

0.7


Risultati della Simulazione – Catena Chiusa



Si simuli adesso il metodo di integrazione basato sulla retroazione dall’errore nello spazio operativo e Jacobiano inverso

INVERSIONE DELLA CINEMATICA MEDIANTE RETROAZIONE CON JACOBIANO INVERSO

XY Graph

qq

To Workspace4

DD

To Workspace3

Xd

To Workspace2

t

To Workspace1

X

To WorkspaceMatrix

Multiply

ProductTempo Posa


K*u

K

MATLABFunction

Jacobiano Inverso

1s

Integrator1

du/dt

Derivative Demux

30

Clock

q X

Cinematica Diretta

MATLABFunction

ABS(DET(J))

3

3

3

3

3

3

3

33dq/dt

3

3

3

33

[3x3]

[3x3]3 3

3

3 3

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


Risultati della Simulazione – Catena Chiusa

0 0.1 0.2 0.3 0.4 0.50.25

0.3

0.35

0.4

0.45

0.5

0.55

0.6

0.65

0.7


0 5 10 15 20 25

-2

0

2x 10

-4 Errore in X

[m]

tempo [s]

0 5 10 15 20 25-2

0

2

4

6

8x 10

-4 Errore in Y

[m]

M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Simulazione - Inversione Cinematica Si...

Documents

Transcript of M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion Simulazione - Inversione Cinematica Si...