Matriz Jacobiana

Planeamiento de Trayectoria

Francisco Javier Triveño Dr.

Escuela de post-graduación
Universidad Mayor de San Simón
trivenoj@yahoo.com.br

1
 Contenido

• Matriz Jacobiana
– Singularidad
• Planeamiento de trayectoria

2
 Cinemática Inversa
• Matriz de transformación θ1 
θ 
 nx sx ax px   2
n sy ay p y  θ 3 
T06 =  y = T01T12T23T34T45T56  
 nz

sz az pz 
 θ 4 
0 0 0 1 θ 5 
 
θ 6 
Solución no única depende del tipo de robot
La solución sistemática cerrada en general no está disponible
Los casos especiales hacen posible la solución (cerrada):
1. Tres ejes de unión adyacentes que se cruzan (PUMA, Stanford)
2. Tres ejes de unión adyacentes paralelos entre sí (MINIMOVER)

3
 Ejemplo
• Manipulador Stanford
 nx sx ax px 
n sy ay p y 
T06 =  y = T01T12T23T34T45T56
 nz sz az pz 
 
0 0 0 1

(T01 ) −1T06 = T12T23T34T45T56 = T16

X X X Cθ1 p x + Sθ1 p y   X X X Sθ 2 ⋅ d 3 
X X X − pz  X X X − Cθ 2 ⋅ d 3 
T16 =   =
X X X − Sθ1 p x + Cθ1 p y   X X X 0.1 
   
0 0 0 1  0 0 0 1 
4
 Ejemplo
• Resolviendo la cinemática inversa
− Sinθ1 ⋅ p x + cos θ1 ⋅ p y = 0.1 Eq (1)

− cos θ1 ⋅ p x + sin θ1 ⋅ p y = sin θ 2 ⋅ d 3 Eq (2)

− p z = − cos θ 2 ⋅ d 3 Eq (3)
En Eq. (1), es
py
p x = r ⋅ cos α , p y = r ⋅ sin α , r= p +p ,
2
x
2
y α = a tan 2( )
px
sin(α − θ1 ) = 0.1 
sin α ⋅ cos θ1 − sin θ1 ⋅ cos α = 0.1 ⇒ r 
r cos(α − θ1 ) = ± 1 − (0.1 / r ) 
2

py 0.1
θ1 = a tan 2( ) − a tan 2( )
px ± r − 0.1
2 2

cos θ1 p x + sin θ1 p y pz
θ 2 = a tan 2( ) d =
cos θ 2
3
pz
5
 Ejemplo
• Resolviendo la cinemática inversa
X X Sθ 5 0
X X − Cθ 5 0
(T34 ) −1 (T23 ) −1 (T12 ) −1 (T01 ) −1T06 = T45T56 = 
X X 0 0
 
From term (3,3) 0 0 0 1

− Sθ 4 [Cθ 2 (Cθ1a x + Sθ1a y ) − Sθ 2 a z ] + Cθ 4 (− Sθ1a x + Cθ1a y ) = 0

− Sθ1a x + Cθ1a y Sθ 5
θ 4 = a tan 2( ) θ = a tan 2 ( )
Cθ 5
5
Cθ 2 (Cθ1a x + Sθ1a y ) − Sθ 2 a z
From term (1,3), (2,3)
Sθ 5 = Cθ 4 (Cθ 2 (Cθ1a x + Sθ1a y ) − Sθ 2 a z ] + Sθ 4 ( − Sθ1a x + Cθ1a y )

 Cθ 5 = Sθ 2 (Cθ1a x + Sθ1a y ) + Cθ 2 a z )

6
 Ejemplo
• Resolviendo la cinemática inversa
Cθ 6 − Sθ 6 0 0
 Sθ Cθ 6 0 0
(T45 ) −1 (T34 ) −1 (T23 ) −1 (T12 ) −1 (T01 ) −1T06 = T56 =  6
 0 0 1 0
 
 0 0 0 1

Sθ 6 = −Cθ 5{Cθ 4 [Cθ 2 (Cθ1s x + Sθ1s y ) − Sθ 2 s z ] + Sθ 4 (− Sθ1s x + Cθ1s y )} + Sθ 5 [ Sθ 2 (Cθ1s x + Sθ1s y ) + Cθ 2 s z ]

 Cθ 6 = − Sθ 4 [Cθ 2 (Cθ1s x + Sθ1s y ) − Sθ 2 s z ] + Cθ 4 (− Sθ1s x + Cθ1s y )

Sθ 6
θ 6 = a tan 2( )
Cθ 6
7
 Matriz Jacobiana

θ1  Directa x 
θ  y Matriz
 2  
θ 3  z  Jacobiana
  Cinemática  
θ 4  φ 
θ 5  θ 
   
θ 6  Inversa ϕ 

Espacio Espacio
Articulaciones Tarea

8
 Matriz Jacobiana
Cinemática Directa
x   q1   h1 (q1 , q2 ,Λ , q6 ) 
y q  h (q , q , Λ , q )
    2  2 1 2 6 
z 
  = h (
 q3 
  6×1 
) =
 h3 ( q1 , q2 ,Λ , q6 ) 

Y6×1 = h(qn×1 )
φ
  q
 4 h (
 4 1 2q , q , Λ , q )
6 
θ   q5   h5 (q1 , q2 ,Λ , q6 ) 
       h1 (q ) 
ϕ   6 
q  h (
 6 1 2q , q , Λ , q 6 
)  6×1 h (q)
Y6×1 = h(q ) =  2 
 Μ
 
h
 6 ( q )

dh(q)
J=
dq
9
Matriz Jacobiana

Es función de q, no es
constante!

 ∂h1 ∂h1 ∂h1 

 ∂q Λ
∂q2 ∂qn 
 1 
 dh(q )   ∂h2 ∂h2
Λ
∂h2 
J =   =  ∂q1 ∂q2 ∂qn 
 dq  6×n  Μ Μ Μ Μ
 ∂h ∂h6 ∂h6 
 6 Λ 
 ∂q1 ∂q2 ∂qn  6×n
10
 Matriz Jacobiana
Cinemática directa
 x   h1 (q) 
p =  y  = h2 (q )
n s a p  z   h3 (q ) 
T =
6

1  4×4
0
0 0 0 φ (q )  h4 (q )
 h1 (q )  {n, s, a} → θ (q )  =  h5 (q ) 
h ( q ) ψ (q ) h6 (q ) 
Y6×1 = h(q ) =  2 
 Μ
 
h6 (q)  Velocidad Lineal

Velocidad Angular

11
 Ejemplo
• Robot 2-DOF (x , y)
– Dado l1, l2 , encontrar el jacobiano
θ2
 x  l1 cos θ1 + l2 cos(θ1 + θ 2 )  h1 (θ1 ,θ 2 )  l2
 y  =  l sin θ + l sin(θ + θ )  = h (θ ,θ )
  1 1 2 1 2   2 1 2 

θ1 l1

 ∂h1 ∂h1 
 ∂θ ∂θ 2  − l1 sin θ1 − l2 sin(θ1 + θ 2 ) − l2 sin(θ1 + θ 2 )
J = 1 =
 ∂h2 ∂h2   l1 cosθ1 + l2 cos(θ1 + θ 2 ) l2 cos(θ1 + θ 2 ) 
 ∂θ1 ∂θ 2 
12
 Jacobian Matrix
• Jacobiano Inverso
q5

q1

• Singularidad
– rank(J)<min{6,n}, Matriz jacobiana no es de rank completo
– No es posible invertir la matriz

13
 Pregunta
• Encontrar la singularidad de un manipulador de
dos grados de libertad

− l1 sin θ1 − l2 sin(θ1 + θ 2 ) − l2 sin(θ1 + θ 2 )

J = 
V (x , y)
l
 1 cos θ 1 + l 2 cos(θ1 + θ 2 ) l 2 cos(θ1 + θ )
2 

l2
Determinante (J)=0 rank
Y
incompleto
θ2 =0
θ2 = 0
Det(J)=0 θ1 l1
x
14
 Matriz Jacobiana
• Pseudoinversa
– Sea A una matriz mxn, and A+ la pseudo inversa de A.
+
Si A If A es de rank completo, entonces A puede ser
calculada como: T −1
 A [ AA ]
T
m≤n
+  −1
A = A m=n
[ AT A]−1 AT m≥n
– Ejemplo: 

1 0 2 3 x = A+ b = 1 / 9[−5,13,16]T
1 − 1 0 x = − 2
   
1 1  −1 1 4 
+ T −1   5 1  1 
A = A [ AA ] = 0 − 1 
T
 =  1 − 5 
1 2
2 0    9
4 − 2
15
Planeamiento de trayectoria
Tasks
Task Plan

Action Plan

Path Plan

Trajectory
Plan

Controller

Robot

Sensor

16
 Planeamiento de trayectoria

Restricciones

Planeador
Especificaciones De
Trayectorias

17
 Planeamiento de trayectoria
Joint i
Final
q(tf)
q(t2)
Set down
• Perfil de posición

• Perfil de velocidad q(t1)

Lift-off
Initial
q(t0)

• Perfil de aceleración
t0 t1 t2 tf Time
Speed

t0 t1 t2 tf Time
Acceleration

t0 t1 t2 tf Time

18
 Condiciones de contorno
1) Posición inicial
2) Velocidad inicial
3) Aceleración inicial
4) Posición de salida
5) Continuidad en posición en t1
6) Continuidad en velocidad en t1
7) Continuidad en aceleración en t1
8) Posición valor constante
9) Continuidad en posición en t2
10) Continuidad en velocidad en t2
11) Continuidad en aceleración en t2
12) Posición final
13) Velocidad final
14) Aceleración final
19
 Requisitos
• Posición inicial
– Posición (dada)
– Velocidad dada (igual a cero)
– Aceleración dada (igual a cero)
• Posición final
– Posición (dada)
– Velocidad dada (igual a cero)
– Aceleración dada (igual a cero)

20
 Requisitos
• Posiciones intermedias
– Posición constante(dato)
– Posición constante para el segmento
siguiente
– Velocidad continua con respecto al tramo
anterior
– Aceleración continua con respecto al tramo
anterior

21
 Requisitos
• Posiciones intermedias
– Posición inicial (dato)
– Posición inicial para el segmento siguiente
– Velocidad continua con respecto al tramo
anterior
– Aceleración continua con respecto al tramo
anterior

22
 Planeamiento de trayectoria
• Polinomio de enésimo orden debe satisfacer 14
condiciones
• Polinomio de 13-th orden
a 13 t 13 + Λ + a 2 t 2 + a 1 t + a 0 = 0
• 4-3-4 trayectoria
h1 (t ) = a14t 4 + a13t 3 + a12t 2 + a12t + a10 t0 t1, 5 incógnitas

h2 (t ) = a23t 3 + a22t 2 + a21t + a20 t1 t2, 4 incógnitas

hn (t ) = an 4t 4 + an 3t 3 + an 2t 2 + an 2t + an 0 t2 tf, 5 incógnitas

23
Preguntas

24
 Gracias
z
y
z
y x
z z
x y
y x

25