ROBOTICA Prof. Fabrizio Caccavale
JACOBIANO GEOMETRICO
T (q) =
⎡
⎢
⎣
R(q) p(q)
0T 1
⎤
⎥
⎦
• Obiettivo: scrivere l’equazione della cinematica differenziale diretta
p = JP (q)q
ω = JO(q)q⇒ v = J(q)q
v =
[
pω
]
J =
[
JP
JO
]
J : Jacobiano geometrico
ROBOTICA Prof. Fabrizio Caccavale
Derivata di una matrice di rotazione
• Essendo
R(t)RT (t) = I ⇒ R(t)RT (t) +R(t)RT (t) = O
• Posto: S(t) = R(t)RT (t)
R(t) = S(t)R(t)
con S(t) + ST (t) = O ⇒ S(t) e una matrice antisimmetrica
ROBOTICA Prof. Fabrizio Caccavale
• Interpretazione (p′ costante): p(t) = R(t)p′
p(t) = R(t)p′ ⇒ p(t) = S(ω(t))R(t)p′
essendo (dalla meccanica dei corpi rigidi)
p(t) = ω(t)×R(t)p′
si puo interpretare S(ω) come l’operatore matriciale che descrive il prodotto vettoriale ω×
Pertanto
R(t) = S(ω(t))R(t)
con
S(ω) =
⎡
⎣
0 −ωz ωy
ωz 0 −ωx
−ωy ωx 0
⎤
⎦
• Proprietà: RS(ω)RT = S(Rω)
ROBOTICA Prof. Fabrizio Caccavale
• Esempio: rotazione elementare intorno a z: Rz(α(t)) =
⎡
⎣
cosα(t) −sinα(t) 0sinα(t) cosα(t) 0
0 0 1
⎤
⎦
S(ω(t)) =
⎡
⎣
−α sinα(t) −α(t) cosα(t) 0α cosα(t) −α(t) sinα(t) 0
0 0 0
⎤
⎦
⎡
⎣
cosα(t) sinα(t) 0−sinα(t) cosα(t) 0
0 0 1
⎤
⎦ =
⎡
⎣
0 −α(t) 0α(t) 0 00 0 0
⎤
⎦
Dunque: ω(t) =
⎡
⎣
00α(t)
⎤
⎦ come previsto dalla meccanica
ROBOTICA Prof. Fabrizio Caccavale
• Velocita di una ternap0 = o0
1 +R01p
1 = o01 + r01
p0 = o01 +R0
1p1 + R0
1p1
= o01 +R0
1p1 + S(ω0
1)R01p
1
= o01 +R0
1p1 + ω0
1 × r01
• Se p1 fisso: p0 = o01 + ω0
1 × r01
ROBOTICA Prof. Fabrizio Caccavale
Velocita di un braccio
• Velocita lineare (per le quantita espresse in Σ0 si omette l’apice 0)
pi = pi−1 +Ri−1ri−1i−1,i = pi−1 + ri−1,i
pi = pi−1 +Ri−1ri−1i−1,i + ωi−1 ×Ri−1r
i−1i−1,i
= pi−1 + vi−1,i + ωi−1 × ri−1,i
con vi−1,i = Ri−1ri−1i−1,i (velocita di Oi rispetto a Oi−1 espressa in Σ0)
ROBOTICA Prof. Fabrizio Caccavale
• Velocita angolare
Ri = Ri−1Ri−1i
S(ωi)Ri = S(ωi−1)Ri +Ri−1S(ωi−1i−1,i)R
i−1i
= S(ωi−1)Ri + S(Ri−1ωi−1i−1,i)Ri
ωi = ωi−1 +Ri−1ωi−1i−1,i
= ωi−1 + ωi−1,i
con ωi−1,i = Ri−1ωi−1i−1,i (velocita angolare di Σi rispetto a Σi−1 espressa in Σ0)
ROBOTICA Prof. Fabrizio Caccavale
pi = pi−1 + vi−1,i + ωi−1 × ri−1,i
ωi = ωi−1 + ωi−1,i
• Giunto prismatico:
{
ωi−1,i = 0
vi−1,i = dizi−1
⇒
{
ωi = ωi−1
pi = pi−1 + dizi−1 + ωi × ri−1,i (ωi = ωi−1)
• Giunto rotoidale:
{
ωi−1,i = ϑizi−1
vi−1,i = ωi−1,i × ri−1,i
⇒
{
ωi = ωi−1 + ϑizi−1
pi = pi−1 + ωi × ri−1,i
ROBOTICA Prof. Fabrizio Caccavale
Calcolo dello Jacobiano
• Lo Jacobiano geometrico si puo scrivere come
J = [ ȷ1 . . . ȷn ] =
⎡
⎣
ȷP1 ȷPn
. . .ȷO1 ȷOn
⎤
⎦
dove ȷiqi =
[
ȷPi
ȷOi
]
qi rappresenta il contributo del giunto i-esimo a v =
[
pω
]
ROBOTICA Prof. Fabrizio Caccavale
• Velocita angolare
⋆ giunto i prismatico (ωi,i−1 = 0)
qiȷOi = 0 =⇒ ȷOi = 0
⋆ giunto i rotoidale (ωi,i−1 = ϑizi−1)
qiȷOi = ϑizi−1 =⇒ ȷOi = zi−1
ROBOTICA Prof. Fabrizio Caccavale
• Velocita lineare
⋆ giunto i prismatico (vi−1,i = dizi−1)
qiȷPi = dizi−1 =⇒ ȷPi = zi−1
⋆ giunto i rotoidale (vi−1,n = ωi−1,i × ri−1,n = ϑizi−1 × ri−1,n)
qiȷPi = ωi−1,i × ri−1,n = ϑizi−1 × (p− pi−1) =⇒ ȷPi = zi−1 × (p− pi−1)
ROBOTICA Prof. Fabrizio Caccavale
• Colonna dello Jacobiano geometrico
[
ȷPi
ȷOi
]
=
⎧
⎪
⎪
⎨
⎪
⎪
⎩
[
zi−1
0
]
per un giunto prismatico[
zi−1 × (p− pi−1)zi−1
]
per un giunto rotoidale
⋆ zi−1 = R01(q1) . . .R
i−2i−1(qi−1)z0 : da A0
i−1 (III colonna)
⋆ p = A01(q1) . . .A
n−1n (qn)p0 : da T 0
n (IV colonna)
⋆ pi−1 = A01(q1) . . .A
i−2i−1(qi−1)p0 : da A0
i−1 (IV colonna)
con z0 =
⎡
⎣
001
⎤
⎦ e p0 =
⎡
⎢
⎣
0001
⎤
⎥
⎦
ROBOTICA Prof. Fabrizio Caccavale
• Rappresentazione in terna differente
[
pu
ωu
]
=
[
Ru OO Ru
] [
pω
]
=
[
Ru OO Ru
]
Jq
Ju =
[
Ru OO Ru
]
J
ROBOTICA Prof. Fabrizio Caccavale
Manipolatore planare a tre bracci
J(q) =
[
z0 × (p− p0) z1 × (p− p1) z2 × (p− p2)z0 z1 z2
]
p0 =
⎡
⎣
000
⎤
⎦ p1 =
⎡
⎣
a1c1a1s10
⎤
⎦ p2 =
⎡
⎣
a1c1 + a2c12a1s1 + a2s12
0
⎤
⎦
p =
⎡
⎣
a1c1 + a2c12 + a3c123a1s1 + a2s12 + a3s123
0
⎤
⎦
z0 = z1 = z2 =
⎡
⎣
001
⎤
⎦
ROBOTICA Prof. Fabrizio Caccavale
J =
⎡
⎢
⎢
⎢
⎢
⎢
⎣
−a1s1 − a2s12 − a3s123 −a2s12 − a3s123 −a3s123a1c1 + a2c12 + a3c123 a2c12 + a3c123 a3c123
0 0 00 0 00 0 01 1 1
⎤
⎥
⎥
⎥
⎥
⎥
⎦
JP =
[
−a1s1 − a2s12 − a3s123 −a2s12 − a3s123 −a3s123a1c1 + a2c12 + a3c123 a2c12 + a3c123 a3c123
]
Solo 3 righe non nulle (vz , ωx e ωy nulle)
ROBOTICA Prof. Fabrizio Caccavale
Manipolatore antropomorfo
J =
[
z0 × (p− p0) z1 × (p− p1) z2 × (p− p2)z0 z1 z2
]
p0 = p1 =
⎡
⎣
000
⎤
⎦ p2 =
⎡
⎣
a2c1c2a2s1c2a2s2
⎤
⎦
p =
⎡
⎣
c1(a2c2 + a3c23)s1(a2c2 + a3c23)a2s2 + a3s23
⎤
⎦
z0 =
⎡
⎣
001
⎤
⎦ z1 = z2 =
⎡
⎣
s1−c10
⎤
⎦
ROBOTICA Prof. Fabrizio Caccavale
J =
⎡
⎢
⎢
⎢
⎢
⎢
⎣
−s1(a2c2 + a3c23) −c1(a2s2 + a3s23) −a3c1s23c1(a2c2 + a3c23) −s1(a2s2 + a3s23) −a3s1s23
0 a2c2 + a3c23 a3c230 s1 s10 −c1 −c11 0 0
⎤
⎥
⎥
⎥
⎥
⎥
⎦
Solo 3 righe linearmente indipendenti (solo 3 g.d.m.) e s1ωy = −c1ωx (ω non arbitraria)
JP =
⎡
⎣
−s1(a2c2 + a3c23) −c1(a2s2 + a3s23) −a3c1s23c1(a2c2 + a3c23) −s1(a2s2 + a3s23) −a3s1s23
0 a2c2 + a3c23 a3c23
⎤
⎦
ROBOTICA Prof. Fabrizio Caccavale
JACOBIANO ANALITICOp = p(q)
φ = φ(q)
}
→ x = k(q)
p =∂p
∂qq = JP (q)q
φ =∂φ
∂qq = Jφ(q)q
⎫
⎪
⎪
⎬
⎪
⎪
⎭
→ x =
[
p
φ
]
=∂k(q)
∂qq =
[
JP (q)Jφ(q)
]
q = JA(q)q
• Jacobiano analitico: JA(q) =∂k(q)
∂q
• J = JA poiche ω = φ
ROBOTICA Prof. Fabrizio Caccavale
• Velocita di rotazione in terna corrente di angoli di Eulero ZYZ
⋆ per effetto di ϕ: [ωx ωy ωz ]T = ϕ [ 0 0 1 ]T
⋆ per effetto di ϑ: [ωx ωy ωz ]T = ϑ [−sϕ cϕ 0 ]T
⋆ per effetto di ψ: [ωx ωy ωz ]T = ψ [ cϕsϑ sϕsϑ cϑ ]
T
ROBOTICA Prof. Fabrizio Caccavale
• Composizione di velocita di rotazione elementari
ω =
⎡
⎣
0 −sϕ cϕsϑ0 cϕ sϕsϑ1 0 cϑ
⎤
⎦ φ = T (φ)φ
• Essendo: det(T ) = −sϑ ⇒ T (φ) singolare per ϑ = 0,π ⇒ singolarita di rappresentazione(soluzione degenere per gli angoli)⋆ in tal caso si ha ω2
x + ω2y = ϑ2 ⇒ ωx e ωy non sono indipendenti
• Quindi:⋆ ∀ φ puo essere espressa mediante una ω equivalente⋆ ∃ω che non possono essere espresse mediante una φ equivalente (in singolarita di rappresentazione)
ROBOTICA Prof. Fabrizio Caccavale
• Significato fisico di∫
ω
ω = [π/2 0 0 ]T 0 ≤ t ≤ 1 ω = [ 0 π/2 0 ]T 0 ≤ t ≤ 1ω = [ 0 π/2 0 ]T 1 < t ≤ 2 ω = [π/2 0 0 ]T 1 < t ≤ 2
Due diverse leggi orarie per ω(t) corrispondenti allo stesso integrale∫ 2
0
ω(t)dt = [π/2 π/2 0 ]T
Viceversa,∫ 2
0
φ(t)dt = φ(2)− φ(0)
ROBOTICA Prof. Fabrizio Caccavale
Relazione tra Jacobiano analitico e Jacobiano geometrico
v =
[
I OO T (φ)
]
x = TA(φ)x
J = TA(φ)JA
• Jacobiano geometrico
⋆ grandezze di significato fisico
• Jacobiano analitico
⋆ grandezze differenziali di variabili nello spazio operativo
• Sono equivalenti in particolari casi (ad es., quando la struttura e in grado di imporre rotazioni dell’organoterminale solo intorno ad un asse fisso nello spazio)
⋆ Esempio: per il manipolatore planare a 3 bracci (con φ = ϑ1 + ϑ2 + ϑ3 ed eliminando le righenulle in J ) si ha J ≡ JA
ROBOTICA Prof. Fabrizio Caccavale
SINGOLARITA CINEMATICHE
v = J(q)q
• se J diminuisce di rango =⇒ singolarita cinematiche(a) perdita di mobilita ⇒ non e possibile imporre leggi di moto arbitrarie all’organo terminale(b) si possono avere ∞ soluzioni al problema cinematico inverso(c) nell’intorno di una singolarita si possono generare velocita elevate nello spazio dei giunti (a fronte
di velocita ridotte dell’organo terminale)
• Classificazione
⋆ Singolarita ai confini dello spazio di lavoro raggiungibile: manipolatore tutto steso o ripiegato
⋆ Singolarita all’interno dello spazio di lavoro raggiungibile: allineamento di assi di moto oconfigurazioni particolari dell’organo terminale
ROBOTICA Prof. Fabrizio Caccavale
• Manipolatore planare a due bracci
J =
[
−a1s1 − a2s12 −a2s12a1c1 + a2c12 a2c12
]
⇒ det(J) = a1a2s2
⋆ Due singolarita ai confini dello SdLR: s2 = 0 ⇒ ϑ2 = 0, ϑ2 = π
⋆ [−(a1 + a2)s1 (a1 + a2)c1 ]T parallelo a [−a2s1 a2c1 ]T (componenti di velocita dell’organoterminale non indipendenti)
ROBOTICA Prof. Fabrizio Caccavale
Disaccoppiamento di singolarita
• Manipolatori con struttura portante + polso sferico
⋆ Singolarita della struttura portante
⋆ Singolarita del polso
ROBOTICA Prof. Fabrizio Caccavale
• J ha una struttura a blocchi (3× 3)
J =
[
J11 J12
J21 J22
]
Con (ultimi 3 giunti rotoidali)
J12 =[
z3 × (p− p3) z4 × (p− p4) z5 × (p− p5)]
e J22 =[
z3 z4 z5]
Se p = pW =⇒ pW − pi paralleli a zi (i = 3, 4, 5)
J12 =[
0 0 0]
⇒ J e triangolare a blocchi ⇒ det(J) = det(J11)det(J22)
• Condizioni di singolarita (di struttura portante e di polso)
det(J11) = 0 e/o det(J22) = 0
ROBOTICA Prof. Fabrizio Caccavale
Singolarita di polso
• Essendo J22 = [ z3 z4 z5 ] (con z4 ⊥ z5)
• Due singolarita cinematiche per ϑ5 = 0, ϑ5 = π
⋆ z3 ∥ z5, verificabile ovunque nello SdLR
• Perdita di mobilita della struttura
rotazioni uguali e opposte di ϑ4 e ϑ6 non producono alcuna rotazione dell’organo terminale
la struttura non e in grado di imporre rotazioni intorno a direzioni ⊥ z3, z4
ROBOTICA Prof. Fabrizio Caccavale
Singolarita di struttura portante
• Si noti che, nonostante si abbia JP = J11, si puo comunque utilizzare JP ai fini del calcolo dellesingolarita (ma i valori delle prime tre variabili di giunto andranno valutati rispetto a riferimenti diversi)
• Manipolatore antropomorfo
det(JP ) = −a2a3s3(a2c2 + a3c23) = 0 ⇔ s3 = 0 a2c2 + a3c23 = 0
ROBOTICA Prof. Fabrizio Caccavale
• Singolarita di gomito (s3 = 0)
ϑ3 = 0 ϑ3 = π
⋆ ai confini dello SdLR
ROBOTICA Prof. Fabrizio Caccavale
• Singolarita di spalla (a2c2 + a3c23 = 0)px = py = 0
⋆ ∞ configurazioni singolari caratterizzabili nello spazio cartesiano (asse di rotazione del giunto 1)⋆ ∞ soluzioni al problema cinematico inverso (∀ valore di ϑ1 genera lo stesso p)⋆ il manipolatore non e in grado di generare velocita lineari ⊥ al piano della struttura
ROBOTICA Prof. Fabrizio Caccavale
ANALISI DELLA RIDONDANZA
• Cinematica differenziale: v = J(q)q
⋆ n : numero di gradi di mobilita della struttura (dimensione di q)
⋆ r : numero di gradi di liberta del compito (componenti di v da specificare)
⋆ n− r : gradi di mobilita ridondanti
• Quindi
⋆ v : vettore (r × 1) delle componenti di velocita dell’organo terminale necessarie per specificare ilcompito
⋆ J : matrice (r × n) estratta dalla Jacobiano (righe corrispondenti alle componenti di v)
ROBOTICA Prof. Fabrizio Caccavale
• Cinematica differenziale: trasformazione (configurazione dipendente) fra due sottospazi
(sottospazio) immagine (range) di J : R(J) ⊆ IRr
(sottospazio) nullo di J : N (J) ⊆ IRn
⋆ in generale: dim(
R(J))
+ dim(
N (J))
= n
⋆ se ϱ(J) = r (Jacobiano di rango pieno): dim(
R(J))
= r e dim(
N (J))
= n− r
⇒ R(J) ricopre tutto IRr e N (J) e non vuoto se n > r
⋆ se ϱ(J) < r (singolarita): dim(
R(J))
< r e dim(
N (J))
> n− r
ROBOTICA Prof. Fabrizio Caccavale
• Se N (J) = ∅ (manipolatore ridondante), dato
⋆ un qualunque vettore di velocita ai giunti q∗ (con v∗ = Jq∗)
⋆ un proiettore P in N (J) (una matrice tale che R(P ) ≡ N (J))
il nuovo vettore velocita
q = q∗ + P q0 con q0 arbitrario
e tale che v∗ = Jq
• Infatti
Jq = Jq∗ + JP q0 = Jq∗ = v∗
poiche JP q0 = 0, ∀q0
• Il vettore di velocita q0 genera moti interni della struttura che non alterano v
ROBOTICA Prof. Fabrizio Caccavale
INVERSIONE DELLA CINEMATICA DIFFERENZIALE
• Equazione cinematica non lineare: soluzioni in forma chiusa della cinematica inversa ricavabili soloper strutture “semplici”, non ridondanti e in configurazioni non singolari
• Equazione cinematica differenziale lineare nelle velocita
• Data v(t) + condizioni iniziali =⇒ (q(t), q(t))
⋆ se n = r e J e di rango pienoq = J−1(q)v
q(t) =
∫ t
0
q(ς)dς + q(0)
⋆ regola di integrazione numerica (Eulero)
q(tk+1) = q(tk) + q(tk)∆t
ROBOTICA Prof. Fabrizio Caccavale
Manipolatori ridondanti
• J e (r × n) con r < n ⇒ ∞ soluzioni
⋆ Si pone un problema di ottimo vincolato
• Per una data configurazione q, trovare le soluzioni q che soddisfino la cinematica diretta (vincolo)
v = Jq
e che minimizzino
g(q) =1
2qTWq con W > O e simmetrica
• Metodo dei moltiplicatori di Lagrange (λ ∈ IRr)
g(q,λ) =1
2qTWq + λT (v − Jq)
ROBOTICA Prof. Fabrizio Caccavale
• Condizioni necessarie(
∂g
∂q
)T
= 0 ⇒ Wq − JTλ = 0
(
∂g
∂λ
)T
= 0 ⇒ v − Jq = 0
• Soluzione ottima (J di rango pieno)q = W−1JT (JW−1JT )−1v
che corrisponde ad un minimo poiche∂2g
∂q2= W > O
• Se W = I (min locale di ||q||), la soluzione e
q = J†v
dove J† e la pseudo-inversa destra di J
J† = JT (JJT )−1
ROBOTICA Prof. Fabrizio Caccavale
• Utilizzo della ridondanza: per una data configurazione q, trovare le soluzioni q che soddisfino lacinematica diretta (vincolo)
v = Jq
e che minimizzino
g′(q) =1
2(qT − qT
0 )(q − q0) =1
2||q − q0||
2
(soluzioni “prossime” a q0 e che soddisfano il vincolo cinematico)
• Procedendo sempre con il metodo dei moltiplicatori di Lagrange
g′(q,λ) =1
2(qT − qT
0 )(q − q0) + λT (v − Jq)
si ha la soluzione ottimaq = J†v + (I − J†J)q0
I−J†J e un proiettore in N (J) ⇒ (I−J†J)q0 rappresenta moti interni della struttura (soluzioneomogenea)
ROBOTICA Prof. Fabrizio Caccavale
• Caratterizzazione dei moti interni: funzione obiettivo “secondaria”, w(q), da massimizzare(localmente) compatibilmente con l’obiettivo “primario” (vincolo cinematico)
q0 = k0
(
∂w(q)
∂q
)T
, k0 > 0
⋆ misura di manipolabilitaw(q) =
√
det(
J(q)JT (q))
⋆ distanza dai fine-corsa dei giunti
w(q) = −1
2n
n∑
i=1
(
qi − qiqiM − qim
)2
⋆ distanza da un ostacolo
w(q) = minp,o
∥p(q)− o∥
ROBOTICA Prof. Fabrizio Caccavale
Singolarita cinematiche• Le soluzioni precedenti valgono solo se J e di rango pieno
• Se J non e di rango pieno (singolarita) ⇒ il sistema v = Jq contiene equazioni linearmentedipendenti
⋆ se v ∈ R(J) ⇒ soluzione q estraendo tutte le equazioni linearmente indipendenti (traiettoria“fisicamente” eseguibile)
⋆ se v /∈ R(J) ⇒ il sistema non e risolvibile (traiettoria non eseguibile)
• Inversione nell’intorno di singolarita: det(J) piccolo ⇒ q elevate
⋆ inversa a minimi quadrati smorzata (k > 0: fattore di smorzamento che rende meglio condizionatal’inversione matriciale)
J⋆ = JT (JJT + k2I)−1
⋆ dove q minimizza (vincolo cinematico incorporato in g′′)
g′′(q) = ∥v − Jq∥2 + k2∥q∥2
ALGORITMI PER L’INVERSIONE CINEMATICA
• Cinematica differenziale diretta
v = J(q)q oppure x = JA(q)q
⋆ Traiettoria desiderata (assegnata)
{pd(t),Rd(t)} ,vd(t) =
[
pd(t)ωd(t)
]
oppure xd(t), xd(t)
• Cinematica differenziale inversa
⋆ soluzione per manipolatore non ridondante e non in singolarita
q = J−1(q)vd oppure q = J−1A (q)xd
⋆ soluzione per manipolatore ridondante e non in singolarita
q = J†(q)vd+(
I − J†(q)J(q))
q0 oppure q = J†A(q)xd+
(
I − J†A(q)JA(q)
)
q0
⋆ soluzione con gestione delle singolarita
q = J⋆(q)vd oppure q = J⋆A(q)xd
• Integrazione (q(σ) = una delle soluzioni di cinematica differenziale inversa)
q(t) = q(0) +
∫ t
0
q(σ) dσ
• Integrazione numerica mediante regola di Eulero (q(tk) = una dellesoluzioni di cinematica differenziale inversa)
q(tk+1) = q(tk) + q(tk)∆t
⋆ fenomeni di deriva della soluzione
• Soluzione algoritmica
⋆ errore nello spazio operativo
e = xd − x = xd − k(q)
e = xd − x
= xd − JA(q)q
⋆ trovare q = q(e): e → 0 (fascia di tolleranza)
⋆ errore nello spazio operativo meno facile da esprimere se si usa J
ROBOTICA Prof. Fabrizio Caccavale
(Pseudo-)inversa dello Jacobiano
• Linearizzazione della dinamica di errore
q = J−1A (q)(xd +Ke)
Se K > O ⇒ limt→∞ e(t) = 0
• Per un manipolatore ridondante
q = J†A(xd +Ke) + (I − J
†AJA)q0
ROBOTICA Prof. Fabrizio Caccavale
Trasposta dello Jacobiano
• Si utilizzano solo funzioni di cinematica diretta (k(q) e JA(q))
q = JTA (q)Ke
Se K > O, xd = 0:
JA di rango pieno ⇒ limt→∞ e(t) = 0
JA non di rango pieno ⇒ N (JTA ) = ∅; dunque, se Ke ∈ N (JT
A ) ⇒ q = 0 con e = 0 (stallo)
Se K > O, xd = 0: e(t) limitato (diminuisce all’aumentare della norma di K)
ROBOTICA Prof. Fabrizio Caccavale
• Esempio
JTP =
⎡
⎣
0 0 0−c1(a2s2 + a3s23) −s1(a2s2 + a3s23) 0
−a3c1s23 −a3s1s23 a3c23
⎤
⎦ ⇒ N (JTP ) :
νyνx
= −1
tanϑ1, νz = 0
• Se K = kI3⋆ e ∈ N (JT
P ) (pd lungo la retta ⊥ al piano della struttura passante per W) ⇒ stallo (pd nonfisicamente raggiungibile)
ROBOTICA Prof. Fabrizio Caccavale
Confronto tra gli algoritmi per l’inversione cinematica
• Manipolatore planare a tre bracci
x = k(q)
⎡
⎣
pxpyφ
⎤
⎦ =
⎡
⎣
a1c1 + a2c12 + a3c123a1s1 + a2s12 + a3s123
ϑ1 + ϑ2 + ϑ3
⎤
⎦
⋆ a1 = a2 = a3 = 0.5m
J = JA =
⎡
⎣
−a1s1 − a2s12 − a3s123 −a2s12 − a3s123 −a3s123a1c1 + a2c12 + a3c123 a2c12 + a3c123 a3c123
1 1 1
⎤
⎦
ROBOTICA Prof. Fabrizio Caccavale
• Configurazione iniziale: qi = [π −π/2 −π/2 ]T rad ⇒ pdi = [ 0 0.5 ]T m, φdi = 0 rad
• Traiettoria desiderata:
pd(t) =
[
0.25(1− cosπt)0.25(2 + sinπt)
]
, φd(t) = sinπ
24t, 0 ≤ t ≤ 4
y
x
• Simulazione in MATLAB con integrazione numerica di Eulero (∆t = 1ms)
q(tk+1) = q(tk) + q(tk)∆t
ROBOTICA Prof. Fabrizio Caccavale
• Integrazione a ciclo aperto di q = J−1A (q)x
⋆ deriva numerica
0 1 2 3 4 50
0.5
1
1.5
2x 10−3
[s]
[m]
pos error norm
0 1 2 3 4 5−1
−0.8
−0.6
−0.4
−0.2
0x 10−5
[s]
[rad
]
orien error
ROBOTICA Prof. Fabrizio Caccavale
• Algoritmo di inversione a ciclo chiuso q = J−1A (q)(xd +Ke) K = diag{500, 500, 100}
⋆ deriva numerica assente⋆ errori di inversione piu piccoli
0 1 2 3 4 5−5
0
5
[s]
[rad
]
joint pos
1
2
3
0 1 2 3 4 5−10
−5
0
5
10
[s]
[rad
/s]
joint vel
12
3
0 1 2 3 4 50
0.2
0.4
0.6
0.8
1x 10−5
[s]
[m]
pos error norm
0 1 2 3 4 5−5
−4
−3
−2
−1
0x 10−8
[s]
[rad
]
orien error
ROBOTICA Prof. Fabrizio Caccavale
• Ridondanza funzionale: φ libero (r = 2, n = 3) (senza sfruttamento della ridondanza)
• q = J†P (pd +KPeP ) KP = diag{500, 500}
0 1 2 3 4 50
1
2
3
4
5x 10−6
[s]
[m]
pos error norm
0 1 2 3 4 5−1
−0.5
0
0.5
[s]
[rad
]
orien
• q = JTP (q)KPeP KP = diag{500, 500}
0 1 2 3 4 50
0.002
0.004
0.006
0.008
0.01
[s]
[m]
pos error norm
0 1 2 3 4 5−1
−0.5
0
0.5
[s]
[rad
]
orien
ROBOTICA Prof. Fabrizio Caccavale
• q = J†P (pd +KPeP ) + (I − J
†PJP )q0 , q0 = k0
(
∂w(q)
∂q
)T
, KP = diag{500, 500} , k0 = 50
w(q) = w(ϑ2,ϑ3) =1
2(s22 + s23) (misura di manipolabilita)
0 1 2 3 4 5−5
0
5
[s]
[rad
]
joint pos
1
2
3
0 1 2 3 4 5−5
0
5
[s]
[rad
/s]
joint pos
1
2
3
0 1 2 3 4 50
1
2
3
4
5x 10−6
[s]
[m]
pos error norm
0 1 2 3 4 50.85
0.9
0.95
1
[s]
[rad
]
manip
ROBOTICA Prof. Fabrizio Caccavale
w(q) = −1
6
3∑
i=1
(
qi − qiqiM − qim
)2
(distanza dai fine-corsa dei giunti), k0 = 250
−2π ≤ q1 ≤ 2π, −π/2 ≤ q2 ≤ π/2, −3π/2 ≤ q3 ≤ −π/2
0 1 2 3 4 50
0.5
1
1.5
2x 10−4
[s]
[m]
pos error norm
0 1 2 3 4 5
−6
−4
−2
0
2
4
6
[s]
[rad
]
joint 1 pos
0 1 2 3 4 5
−6
−4
−2
0
2
4
6
[s]
[rad
]
joint 2 pos
0 1 2 3 4 5−5
0
5
[s]
[rad
]
joint 3 pos
ROBOTICA Prof. Fabrizio Caccavale
STATICA• Relazione tra forze e momenti (forze) γ all’organo terminale e forze e/o coppie (coppie) τ ai giunti
con il manipolatore in configurazione di equilibrio
⋆ lavoro elementare compiuto dalle coppie ai giunti (τ ∈ |Rn)
dWτ = τT dq
⋆ lavoro elementare compiuto dalle forze all’organo terminale (γ ∈ |Rr)
dWγ = fT dp+ µTωdt = fTJP (q)dq + µTJO(q)dq = γTJ(q)dq
⋆ vincoli olonomi e indipendenti da t ⇒ spostamenti elementari ≡ spostamenti virtuali
Lavori virtuali associati ai due sistemi di forze
{
δWτ = τT δq
δWγ = γTJ(q)δq
ROBOTICA Prof. Fabrizio Caccavale
• Principio dei lavori virtuali
⋆ il manipolatore e in equilibrio statico se e solo se
δWτ = δWγ ∀δq
⇓
τ = JT (q)γ
⋆ δWγ = 0, ∀δq ∈ N (J) ⇒ τ = 0 all’equilibrio
ROBOTICA Prof. Fabrizio Caccavale
Dualita cineto-staticav = J(q)q τ = JT (q)γ
N (J) ≡ R⊥(JT ) R(J) ≡ N⊥(JT )
• Forze γ ∈ N (JT ) interamente assorbite dalla struttura (reazioni vincolari)
⋆ un manipolatore in configurazione singolare resta nella q corrente ∀γ ∈ N (JT )
ROBOTICA Prof. Fabrizio Caccavale
• Interpretazione fisica dello schema con la trasposta dello Jacobiano
⋆ dinamica ideale (massa nulla e attrito unitario): τ = q ⇒ τ = JTKe
⋆ forza elastica Ke che tira l’organo terminale verso la postura desiderata nello spazio operativo(τ = JTKe)
⋆ ha effetto solo se Ke /∈ N (JT )
ROBOTICA Prof. Fabrizio Caccavale
ELLISSOIDI DI MANIPOLABILITA
• Indici di valutazione delle prestazioni di un manipolatore
⋆ attitudine ad eseguire un dato compito
• Ellissoide di manipolabilita in velocita
⋆ insieme delle velocita ai giunti a norma costante (unitaria)
qT q = 1 Sfera
⋆ manipolatore ridondante in configurazione non singolare: q = J†(q)v (no moti interni)
vT(
J(q)JT(q))−1
v = 1 Ellissoide
ROBOTICA Prof. Fabrizio Caccavale
• Assi dell’ellissoide{
autovettori ui di JJT ⇒ direzionivalori singolari σi =
√
λi(JJT ) ⇒ dimensioni
• Volume dell’ellissoide proporzionale alla misura di manipolabilita
w(q) =√
det(
J(q)JT (q))
⇒
{
w(q) > 0 in configurazione non singolarew(q) = 0 in configurazione singolare
⋆ per manipolatori non ridondanti
w(q) = |det(J)|
ROBOTICA Prof. Fabrizio Caccavale
• Ellissoide di manipolabilita in forza
⋆ insieme delle coppie ai giunti a norma costante
τT τ = 1 Sfera
⇓
γT(
J(q)JT(q))
γ = 1 Ellissoide
• Dualita cineto-statica
⋆ stessi assi, dimensione in proporzione inversa
⋆ una direzione lungo la quale si ha elevata manipolabilita in velocita e una direzione lungo la qualesi ha scarsa manipolabilita in forza, e viceversa
ROBOTICA Prof. Fabrizio Caccavale
Manipolatore planare a due bracci
• Misura di manipolabilita: w(q2) = |det(J)| = a1a2|s2|
⋆ misura normalizzata: w(q2) = s2
⋆ nei casi piu complessi: w(q) =1
cond(J)=σrσ1
⋆ w : maxϑ2per ϑ2 = ±π/2 , maxa1,a2
per a1 = a2 (a parita di estensione a1 + a2)
• Valori singolari al variare della posizione lungo x (a1 = a2 = 1)
0 0.5 1 1.5 20
0.5
1
1.5
2
2.5
min
max
[m]
[m]
ROBOTICA Prof. Fabrizio Caccavale
• Ellissi di manipolabilita in velocita (a1 = a2 = 1)
0 0.5 1 1.5 2
−1
−0.5
0
0.5
1
[m]
[m]
• Ellissi di manipolabilita in forza (a1 = a2 = 1)
0 0.5 1 1.5 2
−1
−0.5
0
0.5
1
[m]
[m]
ROBOTICA Prof. Fabrizio Caccavale
• Manipolatore≡ trasformatoremeccanico di velocita e forze dallo spazio dei giunti allo spazio operativo
⋆ rapporto di trasformazione lungo la direzione u per l’ellissoide in forza
α(q) =
(
uTJ(q)JT (q)u
)−1/2
⋆ rapporto di trasformazione lungo la direzione u per l’ellissoide in velocita
β(q) =
(
uT(
J(q)JT (q))−1
u
)−1/2
⋆ conservazione energia: α ↑ dove β ↓ e viceversa
⋆ utilizzo dei gradi di mobilita ridondanti (ad es., per max α o β)
ROBOTICA Prof. Fabrizio Caccavale
• Compatibilita della struttura ad eseguire un compito assegnato lungo una direzione
⋆ compiti di attuazione (elevato rapporto di trasformazione)
⋆ compiti di controllo (basso rapporto di trasformazione)
• Dualita fra ellissoide in velocita ed ellissoide in forza
⋆ una direzione ottimale per attuare una velocita e anche una direzione ottimale per controllare unaforza
⋆ una direzione ottimale per attuare una forza e anche una direzione ottimale per controllare unavelocita
ROBOTICA Prof. Fabrizio Caccavale
• Esempi:
⋆ scrittura su superficie orizzontale (controllo): controllo fine della velocita orizzontale e della forzaverticale
⋆ lancio di un peso in direzione orizzontale (attuazione): attuazione della velocita orizzontale e dellaforza verticale
force
velocity
writing plane
force
velocity
throwing direction
• Utilizzo della ridondanza per riconfigurare la struttura in maniera ottimale rispetto al compito (α e βfunzioni obiettivo secondarie)
Top Related