La cinematica Inversa - unimi.ithomes.di.unimi.it/.../Slides/L_06_CinematicaInversa.pdfLa cinematica...

$: La cinematica Inversa - unimi.ithomes.di.unimi.it/.../Slides/L_06_CinematicaInversa.pdfLa cinematica Inversa Prof. Alberto Borghese http:\\homes.dsi.unimi.it\∼borghese A.A. 2003-2004$
1

http:\ \homes.dsi.unimi.it\∼borghese1/36A.A. 2003-2004

La cinematica Inversa

Prof. Alberto Borghese


Riassunto

• La cinematica inversa.

• Il Jacobiano.

• Cinematica inversa attraverso il Jacobiano.

• Retargetting.

2


La cinematica inversa

Dalla posizione (e orientamento) di end-point agli angoli.

Problema sotto-determinato (over-constrained). Comportamento stereotipato. Perché?

Soluzione direttaSoluzione linearizzata


Soluzione diretta

Spazio di lavoro: P1 - nessuna soluzione.P2 - due soluzione.P3 - una soluzione.

2122

21 LLYXLL +≤+≤−

P1

P2

P3

Working space

3


Soluzione diretta (calcolo)

E’ un problema di trigonometria!

•Calcolo L = •Teorema di Carnot per calcolare cosθ2 :• Calcolo di cosθT :• Teorema di Carnot per calcolare cos(θ1−θΤ):

22 YX +)LL2/()LLL()cos( 21

222

212 −+=ϑ

22T YX/X)cos( +=ϑ

)2/()()cos( 12

222

1 LLLLLR −+=ϑ

L=


Cerniere 3D

Figura 4.17.

∞1 soluzioniNB: gli umani ne scelgono una sola.

Calcolo la cinematica inversa come sequenza di posizioni.

4


Caratteristiche della cinematica inversa

• Soluzione di equazioni non- lineari.• Workspace (spazio nel quale si può posizionare l’end-effector). • Dexterous workspace. Spazio nel quale si può posizionare l’end-

effector con un qualsiasi orientamento.• C.N. Per potere raggiungere una certa posizione ed orientamento

nello spazio di lavoro, è che il numero di gradi di libertà dei segmenti del braccio robotico sia uguale al numero di gradi di libertà dell’end-point.

• Soluzione geometrica od analitica complessa da determinare.


Riassunto


• Il Jacobiano.


• Retargetting.

5


Soluzione differenziale

Consideriamo la trasformazione joint -> end_point. P(t) = f(α(t), β(t), Τx(t), Τy(t)| l0, l1).

E’ una forma complessa a cui non si possono applicare trasformazioni lineari. Cosa si può fare?

Linearizzare!

y0 = f(x0)

...dxdx

yd21dx

dxdydy 2

xx2

2

xx00

++===

xdxdy

yy0xx

0 ∆=−=

•y

x

P(x0,y0)

Sviluppo di Taylor arrestato al primo ordine = linearizzazione


Linearizzazione della funzione posizione

Consideriamo la trasformazione joint -> end_point. P(t) = f(α(t), β(t), Τx(t), Τy(t)| l0, l1).Px(t) = fx(α(t), β (t), Τx(t), Τy(t)| l0, l1).Py(t) = fy(α(t), β (t), Τx(t), Τy(t)| l0, l1).Pz(t) = fz(α(t), β (t), Τx(t), Τy(t)| l0, l1).

ABS_ABSPe(t) = A(α(t), β(t), Tx(t), Ty(t) | l0, l1)[0 0 0 1]T

A(t) =eABS_ABS

+−+−+++

10

)()(sin))()((sin)()(cos))()((cos

01

01

tTtlttltTtlttl

y

x

ββαββα

6


Il Jacobiano

Consideriamo la trasformazione joint -> end_point. P(t) = f(α(t), β(t), Τx(t), Τy(t)| l0, l1).

Px(t) = fx(α(t), β(t), Τx(t), Τy(t)| l0, l1).Py(t) = fy(α(t), β(t), Τx(t), Τy(t)| l0, l1).Pz(t) = fz(α(t), β(t), Τx(t), Τy(t)| l0, l1).

Consideriamo Zk = [ακ, βκ, Τxk, Τyk] il valore dei parametri liberi all’istante tk.

−

−−−

∂∂

∂∂

∂∂

∂∂

∂

∂

∂

∂

∂

∂

∂

∂

∂∂

∂∂

∂∂

∂∂

=−−−

.........(.)(.)(.)(.)

...(.)(.)(.)(.)

...(.)(.)(.)(.)

yk

xk

k

k

y

z

x

zzz

y

y

x

yyy

y

x

x

xxx

zz

yy

xx

T

T

Tf

Tfff

T

f

T

fff

Tf

Tfff

PPPPPP

kkkk

kkkk

kkkk

k

k

k

δ

γββαα

βα

βα

βα

ZZZZ

ZZZZ

ZZZZ

J


Come vengono trattate le velocità

Elemento chiave è il Jacobiano, J.

Cinematica dell’End-effector

V = J T&

Cinematica dei Joint

7


Esempio di determinazione del Jacobiano

θ

x = r cos(θ)y = r sin(θ)

v⇒ϑ&

V = J T&

V2x1 J2x1 T& 1x1

(x,y).

x

yv

V = ωr Sono due espressioni identiche

(90-θ)

ϑϑϑ

ϑ

ϑ &&&

−=

∂∂∂∂

=

cossin

rr

y

x

yx


Osservazioni sul Jacobiano

∆Pe(t) = J(Z(t), L)∆Z(t)

Consideriamo Z(t)= [A(α(t), β(t), Tx(t), Ty(t) ] il valore dei parametri liberi.

E’ un’equazione (matriciale) lineare ∆Ζ(t) -> ∆Pe(t)

Siamo ancora nel dominio della cinematica diretta.

−

−−−

∂∂

∂∂

∂∂

∂∂

∂

∂

∂

∂

∂

∂

∂

∂

∂∂

∂∂

∂∂

∂∂

=−−−

.........(.)(.)(.)(.)

...(.)(.)(.)(.)

...(.)(.)(.)(.)

yky

xkx

k

k

y

z

x

zzz

y

y

x

yyy

y

x

x

xxx

zz

yy

xx

TT

TT

Tf

Tfff

T

f

T

fff

Tf

Tfff

PPPPPP

kkkk

kkkk

kkkk

k

k

kββαα

βα

βα

βα

ZZZZ

ZZZZ

ZZZZ

8


Jacobiano e velocità

dPe(t) = J(Z(t), L) dZ(t) è dPe(t) / dt = J(Z(t), L) dZ(t) / dt


(t)V),(J)t(V ZP LZe

(t)=


Cinematica diretta

link1

O

link0

+

+

x ey e

ze

P1

X

Y

Z

end effector

link1

link0

+

+

root

Oα

x0

y0

+−+−

+−+

10

cossinsin)cos(

sinsincos)cos(

101

101

y

x

Tlll

Tlll

βαβα

βαβα

x0y0

P0

P0_ABSP = A P0_L0P = eABS_ABS

β

9


Il Jacobiano dell’esempio

P =

+−+−

+−+

10

cossinsin)cos(sinsincos)cos(

101

101

y

x

TlllTlll

βαβαβαβα

eABS_ABS

J(Z,L) =

−+−−+−−−−−

000010cossinsincoscoscoscossinsin01sincossinsincossincoscossin

01111

01111

ββαβαβαβαββαβαβαβα

llllllllll

−+−+−−+−+−

=000010cos)(cos)cos(01sin)sin()sin(

011

011

ββαβαββαβα

llllll


Significato geometrico del Jacobiano

J(Z,L)

y0

end effectorlin

k1

O

link0

+

+

x ey e

ze

P1

α

x0

x0y0root

Pe

Po

X

Y

Z

β

−+−+−−+−+−

=000010cos)(cos)cos(01sin)sin()sin(

011

011

ββαβαββαβα

llllll

α = β = 0

−−−=

0000100100

011 lllJ(Z,L)

(t)V),(J)t(V ZP LZe

(t)=

α = β = 0

−−

−−

−−−=

−−

−

yky

xkx

zz

yy

xx

TTTTlll

PPPP

PP

k

k

k00

000010

0100

100

β

α

10


Riassunto


• Il Jacobiano.


• Retargetting.


Cinematica inversa attraverso il Jacobiano

dPe = J(Z, L) dZ è dPe / dt = J(Z(t), L) dZ / dt


dZ = J-1(Z, L) dPe

E’ un sistema lineare: esiste 1, nessuna o ∞ soluzioni.

Sia n il numero di gradi di libertà dell’end-point (3).Sia m il numero di parametri liberi del braccio (4).

11


I sistemi lineari (m = n)

mnmn33m22m11m

2nn2323222121

1nn1313212111

lxa......xaxaxa

...........................lxa......xaxaxalxa......xaxaxa

=++++

=++++=++++

dPe = J(Z, L) dZ L = AX

Se n = m X = A-1L Esiste 1 soluzione se e solo se: det(A) ≠ 0


I sistemi lineari (m = n)

dPe = J(Z, L) dZ L = AX

Se det(A) = 0 ho infinite soluzioni.

Sono nel caso di sistema “iperstatico” e “ipostatico”, esistono più parametri liberi che movimentano alcuni gradi di libertà, non esisteno parametri liberi in grado di movimentare gli altri gradi di libertà.

12


Sistema lineare m = n

dαdβ

dPexdPey

=

J(Z,L)

−+−+−

−+−+−

ββαβα

ββαβα

cos)cos()cos(

sin)sin()sin(

122

122

lll

lll

link1

O

link0

+

+

x ey e

ze

P1

α

x0

y0

x0y0

P0

β

Come mai ottengo un’unica soluzione tramite Jacobiano?


Risolubilità

Non esiste una soluzione che mi dia uno spostamento in direzione dP

arbitratia.

E’ possibile spostarsi solamente in direzione perpendicolare al braccio.

Verificare con la svd.

end effector

link1

link0

+

+

root

OdP

NB Il valore del Jacobiano è istantaneo.

J(Z,L) per α = 0 – braccio disteso

−−−

−−−

βββ

βββ

coscoscos

sinsinsin

122

122

lll

llldet(J(Z,L) = 0

Esistono dei valori di α e β per cui il sistema non è risolubile per alcuni valori di dP.

13


Sistema lineare: soluzione

J dA = dP J’ J dA = J’ dP

dA = (J’J)-1J’dP

N < M Numero equazioni minore del numero di incognite - det(J’J) = 0 => ∞ soluzioni

N = M Numero equazioni uguale al numero di incognite - det(J’J) = 0 => ∞ soluzionidet(J’J) ? 0 => 1 soluzione

N < M Numero equazioni maggiore del numero incognite - det(J’J) = 0 => ∞ soluzionidet(J’J) ? 0 => 1 soluzione

NB La soluzione minimizza l’errore tra le osservazioni ed il modello:min (JdA – dP)2

(J’ J )-1J’ J dA = (J’ J )-1J’ dP

(J’J)-1J’ – Matrice pseudo-inversa

N gradi di libertàM parametri liberi


Sistema lineare: soluzione robusta

J dA = dP J’ J dA = J’ dP dA = (J’J)-1J’dP

Numero di condizionamento varia circa con (J’ J).

J dA = dP

Soluzione tramite Singular Value Decomposition

U W V dA = dP

DiagonaleOrtonormali

VT W-1 UT U W V dA = VT W-1 UT dP è dA = VT W-1 UT dP

W-1 è diagonale. wii-1 = 1/w iiNumero di condizionamento varia circa con J.

14


Soluzione dei sistemi lineari (N > M)

(JTJ)-1(JTJ)dA = (JTJ)-1JTdP

dA = (JTJ)-1JTdP

J dA = dP

dθ è tale per cui | dPe - J dθ | è minima.

JTJdA = JTdP

Sistema sotto-parametrizzato.

Equazioni normali

dθ = (JTJ)-1JT dPe

dθ = J+ dPe J+ è la pseudo-inversa.

Genero il movimento migliore tra quelli che posso generare.


Soluzione dei sistemi lineari (M > N)

JTJ dA = JTdP (JTJ)-1(JTJ) dA = (JTJ)-1JT dP

[U W V] = svd(J)

J dA = dP

JTJ dA = JTdP

Sistema sovra-parametrizzato.

Equazioni normali

dθ è una combinazione lineare dei parametri liberi a norma unitaria

dθ = Vjmin

det(JTJ) = 0

Prendo come soluzione il vettore di V associato al valore singol are w più piccolo

15


Riassunto

La cinematica inversa consente di determinare la traiettoria dei joint, data la traiettoria desiderata dell’end-point.

La soluzione geometrica o analitica è possibile per scheletri molto semplici.

Per scheletri complessi si utilizza una soluzione linearizzata basato sul Jacobiano. Calcolo della pseudo- inversa.

Possibilità di polarizzare i joint o il peso dei gradi di libertà.

Sperimentare la soluzione analitica per un problema di path-planning con i diversi vincoli possibili.


Riassunto


• Il Jacobiano.


• Retargetting.

16


Pianificazione

• Problema di controllo ottimo.

[ ]{ }∑ =+−

T

i iifftZ

tZkTPTPdMin 0

22

)}({

)(*)()(( λ

Scelta della sequenza di controllo {Z(t)}, tale che:• Minimizza la deviazione tra posizione desiderata e reale

dell’end point:• Minimizza la spesa energetica:

[ ]2)()(( TPTPdff −

{ })(2 tZ i

λ pesa l’importanza dei due obbiettivi.k pesa l’energia associata all’attivazione di ogni grado di libertà.

E’ un problema di ottimizzazione non lineare.


Retargetting

From Motion Capture to Virtual Motion:3D positions → Angles Model fittingMotion correction

17


Motion correction & retargetting

What happens if the arm of the digital character enter inside the shoulder of his girl- friend?

The problem is reframes as an optimal control problem.

Zero error in the final frame.

Minimal deviation of the control actions (the angle sequence).

( )2

)}({

2 )())()((k

d

tk kff taTTb

z

zPP ∑+−

Hard and Soft constraints


Motion retargetting: an example

Data captured have to be adapted to a smaller female.

18


Motion retargetting: an example


Riassunto


• Il Jacobiano.


• Retargetting.

La cinematica Inversa - unimi.ithomes.di.unimi.it/.../Slides/L_06_CinematicaInversa.pdfLa cinematica...

Documents

Transcript of La cinematica Inversa - unimi.ithomes.di.unimi.it/.../Slides/L_06_CinematicaInversa.pdfLa cinematica...