Date post: | 01-May-2015 |
Category: |
Documents |
Upload: | fioralba-battaglia |
View: | 219 times |
Download: | 1 times |
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 &⋅=
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
)()()()(
)()()( &
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 &⋅=
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)
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
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= ⋅& & &
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)
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
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:
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
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
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 !!!
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ε−= ⋅ +& &
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−= ⋅& & ≠
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
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Serve un richiamo di Simulink?
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−= ⋅ + ⋅& &
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
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)
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= ⋅ ⋅ −&
M. De Cecco - Lucidi del corso di Robotica e Sensor Fusion
Inversione differenziale della Cinematica