m. de cecco - lucidi del corso di robotica e sensor fusion simulazione - inversione cinematica si...
TRANSCRIPT
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:
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Simulazione – Calcolo Jacobiano
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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−= ⋅& &
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Simulazione – Catena Aperta
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
To Workspace2
To Workspace1
To Workspace
Product
Tempo Posa
PianificazioneTraiettoria
Jacobiano InversoIntegrator1
Derivative
Clock
Cinematica Diretta
dq/dt
Blocco di pianificazione traiettoria:
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Simulazione – Catena Aperta
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
To Workspace2
To Workspace1
To Workspace
Product
PianificazioneTraiettoria
Jacobiano InversoIntegrator1
Derivative
Clock
q X
Cinematica Diretta
dq/dt
Blocco di Cinematica diretta:
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
Simulazione – Catena Aperta
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Simulazione – Catena Aperta
Blocco di Inversione 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
To Workspace2
To Workspace1
To Workspace
Product
PianificazioneTraiettoria
MATLABFunction
Jacobiano InversoIntegrator1
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;
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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]
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Risultati della Simulazione – Catena Aperta
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]
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Risultati della Simulazione – Catena Aperta
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?
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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:
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Simulazione – Catena Chiusa
PROVARE A GENERARE TRAIETTORIE:- VICINE AL LIMITE DEL CAMPO OPERATIVO
- VICINE ALLE SINGOLARITA'
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
PianificazioneTraiettoria
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
0.75traiettoria eseguitatraiettoria desiderata
Risultati della Simulazione – Catena Chiusa
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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
To Workspace4
DD
To Workspace3
Xd
To Workspace2
t
To Workspace1
X
To WorkspaceMatrix
Multiply
ProductTempo Posa
PianificazioneTraiettoria
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−= ⋅ + ⋅& &
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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.75traiettoria eseguitatraiettoria desiderata
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 – Catena Chiusa