View
3
Download
0
Category
Preview:
Citation preview
1
http:\ \homes.dsi.unimi.it\∼borghese1/36A.A. 2003-2004
La cinematica Inversa
Prof. Alberto Borghese
http:\ \homes.dsi.unimi.it\∼borghese2/36A.A. 2003-2004
Riassunto
• La cinematica inversa.
• Il Jacobiano.
• Cinematica inversa attraverso il Jacobiano.
• Retargetting.
2
http:\ \homes.dsi.unimi.it\∼borghese3/36A.A. 2003-2004
La cinematica inversa
Dalla posizione (e orientamento) di end-point agli angoli.
Problema sotto-determinato (over-constrained). Comportamento stereotipato. Perché?
Soluzione direttaSoluzione linearizzata
http:\ \homes.dsi.unimi.it\∼borghese4/36A.A. 2003-2004
Soluzione diretta
Spazio di lavoro: P1 - nessuna soluzione.P2 - due soluzione.P3 - una soluzione.
2122
21 LLYXLL +≤+≤−
P1
P2
P3
Working space
3
http:\ \homes.dsi.unimi.it\∼borghese5/36A.A. 2003-2004
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=
http:\ \homes.dsi.unimi.it\∼borghese6/36A.A. 2003-2004
Cerniere 3D
Figura 4.17.
∞1 soluzioniNB: gli umani ne scelgono una sola.
Calcolo la cinematica inversa come sequenza di posizioni.
4
http:\ \homes.dsi.unimi.it\∼borghese7/36A.A. 2003-2004
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.
http:\ \homes.dsi.unimi.it\∼borghese8/36A.A. 2003-2004
Riassunto
• La cinematica inversa.
• Il Jacobiano.
• Cinematica inversa attraverso il Jacobiano.
• Retargetting.
5
http:\ \homes.dsi.unimi.it\∼borghese9/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese10/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese11/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese12/36A.A. 2003-2004
Come vengono trattate le velocità
Elemento chiave è il Jacobiano, J.
Cinematica dell’End-effector
V = J T&
Cinematica dei Joint
7
http:\ \homes.dsi.unimi.it\∼borghese13/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese14/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese15/36A.A. 2003-2004
Jacobiano e velocità
dPe(t) = J(Z(t), L) dZ(t) è dPe(t) / dt = J(Z(t), L) dZ(t) / dt
Consideriamo Z(t)= [A(α(t), β(t), Tx(t), Ty(t) ] il valore dei parametri liberi.
(t)V),(J)t(V ZP LZe
(t)=
http:\ \homes.dsi.unimi.it\∼borghese16/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese17/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese18/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese19/36A.A. 2003-2004
Riassunto
• La cinematica inversa.
• Il Jacobiano.
• Cinematica inversa attraverso il Jacobiano.
• Retargetting.
http:\ \homes.dsi.unimi.it\∼borghese20/36A.A. 2003-2004
Cinematica inversa attraverso il Jacobiano
dPe = J(Z, L) dZ è dPe / dt = J(Z(t), L) dZ / dt
Consideriamo Z(t)= [A(α(t), β(t), Tx(t), Ty(t) ] il valore dei parametri liberi.
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
http:\ \homes.dsi.unimi.it\∼borghese21/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese22/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese23/36A.A. 2003-2004
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?
http:\ \homes.dsi.unimi.it\∼borghese24/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese25/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese26/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese27/36A.A. 2003-2004
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.
http:\ \homes.dsi.unimi.it\∼borghese28/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese29/36A.A. 2003-2004
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.
http:\ \homes.dsi.unimi.it\∼borghese30/36A.A. 2003-2004
Riassunto
• La cinematica inversa.
• Il Jacobiano.
• Cinematica inversa attraverso il Jacobiano.
• Retargetting.
16
http:\ \homes.dsi.unimi.it\∼borghese31/36A.A. 2003-2004
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.
http:\ \homes.dsi.unimi.it\∼borghese32/36A.A. 2003-2004
Retargetting
From Motion Capture to Virtual Motion:3D positions → Angles Model fittingMotion correction
17
http:\ \homes.dsi.unimi.it\∼borghese33/36A.A. 2003-2004
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
http:\ \homes.dsi.unimi.it\∼borghese34/36A.A. 2003-2004
Motion retargetting: an example
Data captured have to be adapted to a smaller female.
18
http:\ \homes.dsi.unimi.it\∼borghese35/36A.A. 2003-2004
Motion retargetting: an example
http:\ \homes.dsi.unimi.it\∼borghese36/36A.A. 2003-2004
Riassunto
• La cinematica inversa.
• Il Jacobiano.
• Cinematica inversa attraverso il Jacobiano.
• Retargetting.
Recommended