5 Cinematica Inversa

Fundamentos de Robótica: 2020-1
- Tema 5 -
Cinemática Inversa de Robots
Manipuladores
Prof. Oscar E. Ramos, Ph.D.
- 11 de mayo del 2020 -

Objetivos
• Comprender conceptos básicos de cinemática inversa
• Comprender el concepto de espacio de trabajo
• Obtener la cinemática inversa de robots manipuladores

simples usando métodos analíticos
• Obtener la cinemática inversa de robots manipuladores

complejos mediante métodos numéricos
2
Temas
1. Introducción
2. Espacio de Trabajo (y Existencia de Soluciones)
3. Multiplicidad de Soluciones
4. Soluciones Analíticas
5. Soluciones Numéricas
6. Cálculo Numérico del Jacobiano
3
Cinemática de Robots Manipuladores
{n}
qi+1
Relación entre articulaciones (qi)

y la posición/orientación de algún
punto (ejemplo: sistema {n})
qi
{0}
4
x  f (q)
Cinemática
Espacio directa Espacio
articular Cartesiano
q  (q1 , , qn ) x  ( x, y , z ,  ,  ,  )
- Cinemática directa
Dada una configuración articular (q), halla la posición/orientación (x) de
alguna parte del robot (ejm. efector final)
Dado q  (q1 , , qn )
0
Encontrar Tn (q) o x  f (q ) Por ejemplo: x  ( x, y, z ,  ,  ,  )
5
Espacio Espacio
articular Cartesiano
q  (q1 , , qn ) x  ( x, y , z ,  ,  ,  )
Cinemática
inversa
q  f 1 (x)
- Cinemática inversa
Halla la configuración articular (q) necesaria para alcanzar una posición /
orientación (x) de alguna parte del robot
0
Dado Tn (q) o x  f (q) para algún punto del robot (ejm. efector final)
Encontrar q  (q1 , , qn )
6
Ejemplo: Robot de 2 gdl
• Posición de un robot de 2 grados de libertad
ŷ
 Variables Variables
y cartesianas articulares
 x q 
l2 x  q   1
q2  y  q2 
l1 Considerando
solo posición
q1
x x̂
Cinemática  x  l1 cos( q1 )  l2 cos(q1  q2 )  Dado q, existe

 y    l sin(q )  l sin(q  q )  x  f (q)
directa    1 un único x
1 2 1 2 
Cinemática  q1  ?  Dado x, ¿existe

inversa  q   ?  q  f 1 ( x )  ?
 2   (un único) q?
7
Ejemplo: Cinemática Directa
Efector final con respecto a la base:
Posición y
orientación
donde:
Manipulador Stanford
8
Ejemplo: Cinemática Inversa
Calcular los ángulos para que la posición y orientación del

efector final sean:
Matriz
numérica
Equivalentemente, resolver el sistema:
¡Sistema de ecuaciones
trigonométricas no lineales!
Manipulador Stanford
9
Cinemática Inversa
• Encontrar los ángulos

q  f 1 ( x)
Dada una posición y orientación deseada para el efector final,
encontrar los valores angulares.
• Es un problema de síntesis
• Los datos de entrada (posición y orientación) tienen la forma
 R t x 
T   x   p
0 0 0 1  xr 
Formulación clásica: Formulación generalizada:
Cinemática inversa para el efector final Cinemática inversa para variables de la tarea
• Es un problema no lineal:
- ¿Existe una solución? Respuesta: espacio de
- ¿Existe una única solución o múltiples soluciones? trabajo, redundancia
- ¿Cómo solucionarlo?
10
Temas
1. Introducción
11
Espacio de Trabajo
• Robot ABB IRB 120
[ABB IRB 120 Industrial Robot – Datasheet] 12

Espacio de Trabajo
Ejemplo: Robot 2 gdl
y
ŷ
Si l1 > l2
y   x l1+l2
p 
 y
l2 x
l1 q2
q1
x x̂
Espacio de trabajo
• Espacio de trabajo (l1 > l2) (Workspace: WS)
WS  {p   2 | l1  l2  p  l1  l2 }
con q1 ∈ [0, 2π], q2 ∈ [0, 2π]
Si se limita los ángulos … ¿varía el espacio de trabajo?
13
Espacio de Trabajo
Ejemplo: Robot 2 gdl
y Espacio de
ŷ trabajo
Si l1 > l2
y   x
p 
 y
l2 x
l1 q2
q1
x x̂
• Si q1 ∈ [0, π], q2 ∈ [0, π], ¿cuál es el espacio de trabajo?

- El espacio de trabajo (WS) se reduce al añadir restricciones en los
ángulos
- Expresión analítica del WS es más complicada
14
Espacio de Trabajo
• Espacio de trabajo primario (alcanzable): WS1

Todas las posiciones que se puede alcanzar con al menos una orientación
Cada posición puede ser alcanzada

(no “importa” la orientación)
- Fuera de WS1 no hay solución al problema

- Para todo p ∈ WS1 (con una orientación adecuada), hay por lo menos una solución
• Espacio de trabajo secundario (diestro): WS2

Todas las posiciones que pueden ser alcanzadas con cualquier orientación
El punto se alcanza con todas

las posibles orientaciones
- Para todo p ∈ WS2 hay (por lo menos) una solución para cualquier orientación
• Relación entre WS1 y WS2: WS2 ⊆ WS1
Nota: a WS1 se le denomina reachable y a WS2 dexterous 15

Espacio de Trabajo
• Ejemplo: Robot ABB IRB 120
WS1 (=WS2 para muñeca esférica)
Se utiliza para evaluar al robot para una aplicación específica
[ABB IRB 120 Industrial Robot – Datasheet] 16

Espacio de Trabajo
• Ejemplo: Robot LBR iiwa de KUKA
Side View Top View
[LBR iiwa 7 R800, LBR iiwa 14 R820 Specification Manual] 17

Espacio de Trabajo
• Ejemplo:
Robot IRB 360 de ABB
[IRB 360 Flex Picker Specifications] 18

Temas
1. Introducción
19
Multiplicidad de Soluciones
Ejemplo: Posiciones del Robot PUMA
Las 4 configuraciones llegan al

mismo punto (considerando
solo posición)
Multiplicidad de soluciones
PUMA 560 20
¿Qué solución escoger?
pA Configuración inicial para pA: qA

qA
pB
Configuraciones nuevas para pB: q1, q2
q2
¿Es mejor escoger q1 o q2? q2
q1
Distancia articular entre configuraciones:
d1  q1  q A Menor distancia
es mejor
d2 = q2  q A
En general, si hay N configuraciones posibles para pB escoger:
qb  arg min q  q A para q  {q1 , q 2 , , q N }

q
21
Redundancia
n articulaciones: q  (q1 , , qn )
m variables en el espacio de tarea:

x  ( x1 , x2 , , xm )
Robot redundante con respecto a una tarea si:

n>m
Robot de 4 gdl en
el plano (3 gdl)
• Ejemplo
- Robot de 6 gdl es redundante si solo se
considera la posición (no orientación)
- Robot de 6 gdl no es redundante si se
considera posición y orientación (m=6)
22
Cinemática Inversa
Complejidad
• Las ecuaciones a resolver son no lineales
• Pueden existir:
- Una solución
- Múltiples soluciones
- Infinitas soluciones (cuando hay redundancia)
- Ninguna solución admisible
• ¿Cuándo se garantiza una solución para la posición?

- Cuando la posición (del efector final) pertenece al espacio alcanzable (primario)
• ¿Cuándo se garantiza una solución para la posición y orientación?

- Cuando la posición y orientación (del efector final) pertenecen al espacio de
trabajo diestro (secundario)
23
Cinemática Inversa
Métodos de Solución
• Solución analítica
- Es una solución exacta (descrita mediante ecuaciones)
- Es preferida cuando puede ser hallada
- Métodos
• Enfoque geométrico (ad-hoc)
• Enfoque algebraico (solución de ecuaciones polinomiales)
• Transformaciones inversas (método de Paul)
• Desacoplo cinemático (método de Pieper): robots con 6 gdl
• Solución numérica
- Es una solución iterativa (es un algoritmo)
- Es necesaria cuando el robot es siempre redundante (siempre habrá más
grados de libertad que los necesarios para una tarea)
- Más fácil de obtener (puede ser más lento)
- Métodos usuales: Newton, descenso del gradiente, etc.
24
Temas
1. Introducción
25
Soluciones Geométricas
• Tratan de encontrar los valores articulares q1, q2, …, qn analizando la

geometría (en el plano o espacio) del robot
- No es una solución genérica → depende del robot
• Pasos útiles (depende del caso):

- Proyectar en el plano (y analizar en el plano)
- Realizar “trazos” para encontrar triángulos rectángulos (y obtener tangentes)
- Utilizar la regla del coseno:
c
α b c 2  a 2  b 2  2ab cos 
a
• Aplicable a robots con pocos gdl (3 o menos) … ¿por qué?

• En robots de más gdl puede ser aplicable a los 3 primeros gdl (si los
demás gdl solo se usan para orientación)
26
Soluciones Geométricas
• Ejemplo: ŷ
Encontrar la cinemática inversa para la posición
y
del robot RR usando el enfoque geométrico
l l2
Solución q2
l1
Para q2 :  
q1
- Usando la ley de cosenos: x x̂
l 2  l12  l22  2l1l2 cos(180  q2 )
Para q1 (usando la geometría de la figura)
x 2  y 2  l12  l22  2l1l2 cos(q2 )
q1    
2 2 2 2
x  y  (l  l2 )1
c2    atan2( y, x)
2l1l2
- Usando una identidad trigonométrica:
  atan2(l2 s2 , l1  l2 c2 )
s22  c22  1
Cinemática inversa:
2
s2   1  c 2 q1  atan2( y, x)  atan2(l2 s2 , l1  l2 c2 )
q2  atan2( s2 , c2 ) q2  atan2( s2 , c2 )
27
Soluciones Algebraicas
• Tratan de encontrar los valores articulares q1, q2, …, qn manipulando

algebraicamente las ecuaciones de la cinemática directa
- Aplicable a robots con pocos grados de libertad
• Elementos útiles (depende del caso):

- Elevar al cuadrado para encontrar la identidad trigonométrica (permite
eliminar algún valor articular):
sin 2   cos 2   1
- Expandir expresiones del ángulo doble (a veces se pueden simplificar con otros
sumandos)
sin(   )  sin  cos   sin  cos 
cos(   )  cos  cos   sin  sin 
- Encontrar el seno y coseno de forma matricial y reemplazarlo en atan2
 a b  cos qi   e 
 c d   sin q    f  qi  atan2(sin qi , cos qi )
  i   
28
• Ejemplo 1
Encontrar la cinemática inversa para la posición de un robot RR usando el
enfoque algebraico
Solución ŷ
- Cinemática Directa: y
x  l1 c1  l2 c12 y  l1 s1  l2 s12
l2
- Para q2 :
l1 q2
x 2  l12 c12  l2 2 c12 2  2l1c1l2 c12 q1

De
C.D. x x̂
y 2  l12 s12  l2 2 s12 2  2l1s1l2 s12
x 2  y 2  l12  l2 2  2l1l2 (c1c12  s1s12 )

s22  c22  1
x 2  y 2  l12  l2 2  2l1l2 c2
s2   1  c22
x 2  y 2  (l12  l2 2 )
c2 
2l1l2 q2  atan2( s2 , c2 )
29
• Ejemplo 1
Encontrar la cinemática inversa para la posición de un robot RR usando el
enfoque algebraico
Solución ŷ
- Para q1 (expandiendo términos de cinemática directa): y
x  l1c1  l2 (c1c2  s1s2 )
l2
y  l1s1  l2 ( s1c2  c1s2 )
l1 q2
Ecuaciones son lineales en c1, s1: resolver
x  c1 (l1  l2 c2 )  s1 (l2 s2 ) q1
y  s1 (l1  l2 c1 )  c1 (l2 s2 ) x x̂
En forma matricial: y (l1  l2 c2 )  xl2 s2

s1 
det
l1  l2 c2 l2 s2  c1   x  resolviendo
 ls  x(l  l c )  yl2 s2
 2 2 l1  l2 c2   s1   y  c1  1 2 2
det
q1  atan2( s1 , c1 ) det  l12  l22  2l1l2 c2
30
• Ejemplo 2
El efector de un robot de 3 gdl RPR, con respecto a su base está dado por
donde a3 y d1 son constantes. La configuración deseada del efector final es:
Encontrar la cinemática inversa ( , , ) de este robot como una función de

los elementos de Tdes.
Solución
El procedimiento consiste en obtener relaciones entre ambas matrices, y
aplicar álgebra, para que el valor de cada articulación pueda despejarse.
31
• Ejemplo 2
- Para :
- Para :
- Para :
32
Solución por Transformaciones Inversas
• Realiza multiplicaciones (a la derecha o izquierda) por las inversas de

transformaciones homogéneas
- Busca obtener expresiones “más simples” (menos variables)
• Se desea la transformación homogénea Tdes para el efector final:
Valores
numéricos
• Expresión analítica (posición/orientación) del efector final (cinemática directa)

Tn (q1 , q2 , , qn )  0T1 ( q1 ) 1T2 (q2 ) n 1Tn ( qn ) = Tdes
0
• Idea: (tratar de) encontrar expresiones más simples (términos que

dependan de una sola variable)
0
T1 (q1 ) 1Tdes  1T2 (q2 ) n 1Tn (qn )
T1 (q1 ) 1Tdes n 1Tn (qn ) 1  1T2 ( q2 )  n  2Tn 1 (qn 1 )
0
33
Desacoplo Cinemático
• Condición suficiente (no necesaria) para la existencia de solución

analítica para un robot de 6 grados de libertad:
- El robot tiene 3 articulaciones de revolución consecutivas que se intersecan en
un punto común (muñeca esférica), o
- El robot tiene 3 articulaciones de revolución consecutivas que son paralelas
• Se le conoce como el método de Pieper: se aplica a muñecas esféricas
Bastantes robots industriales

tienen muñecas esféricas
• Idea: separar el problema en 2 subproblemas

- Cinemática Inversa para la posición (de la muñeca [“wrist”])
- Cinemática inversa para la orientación (del efector final con respecto a la muñeca)
34
• Robot con 6 grados de libertad:
y6
W {6}
z6
d6
Posición y orientación
deseadas para el
efector final {6} con
respecto a la base
 px  d 6 ax 
  Valor
• Posición de la muñeca: pw   p y  d 6 a y  numérico
 pz  d 6 az 
3
• Orientación del efector final con respecto a la muñeca: R6
35
• Procedimiento
1. Se obtiene la posición deseada de la muñeca (a partir de Tdes)
 px  d 6 ax 
pw   p y  d 6 a y 
 pz  d 6 az 
y su expresión analítica a partir de 0T3(q1, q2, q3). Relacionando ambos, se

obtiene los ángulos q1, q2, q3
2. Con los valores de q1, q2, q3 se calcula la rotación 0R3 y la transformación

homogénea 0T3
0 3
3. Se obtiene q4, q5, q6 despejando Tdes  T3 T6 (q4 , q5 , q6 ) que lleva a:
3
T6 (q4 , q5 , q6 )  0T31Tdes
Tiene la estructura de
ángulos de Euler ZYZ o ZXZ
• Nota: en los pasos 1 y 3 se requiere análisis algebraico y/o geométrico

36
• Ejercicio
Resolver el problema de cinemática inversa para el siguiente robot de 6 gdl,
si se sabe que sus parámetros de Denavit-Hartenberg son los mostrados en
la tabla
{3} l2
θ d A α
z3
l1 q1 0 0 90°
l3 q2 0 l1 0°
{0} q3 0 0 90°
{6} q4 l2 0 -90°
z6 q5 0 0 90°
l0
q6 l3 0 0°
37
Temas
1. Introducción
38
Soluciones Numéricas
• Se utilizan cuando:
- No existe solución analítica
- Hay infinitas soluciones (ejm. robots redundantes)
- Es “muy complicado” hallarla
• Idea:
x  f (q) x  f (q )  0
Cinemática Encontrar q (por iteración) tal

directa que la diferencia sea cero
• Métodos de solución:
- Método de Newton: encontrar las raíces del problema
- Método de máximo decrecimiento (Gradient descent): minimiza la función
objetivo (idealmente debería ser cero)
- Método CCD (Cyclic Coordinate Descent)
39
a) Método de Newton
• Problema: dado xd (constante), encontrar q tal que

xd: valor deseado para x
x d  f (q )  0 f : función de cinemática directa
• Procedimiento para la solución
Matriz Jacobiana
- Aproximación de primer orden de Taylor para f(q)
f (q k )
f (q )  f (q k )  J (q k )(q  q k ) J (q k )    nm
q
- Reemplazando: n gdl
x d   f (q k )  J (q k )(q  q k )   0 m: tamaño de x
x d  f (q k )  J (q k )(q  q k )
- Asumiendo J cuadrado e invertible (n = m):

J 1 (q k )  x d  f (q k )   (q  q k )
• Solución final (q = qk+1):
q k 1  q k  J 1 (q k )  x d  f (q k ) 
40
• Algoritmo:
- Se comienza con un q0 inicial (suele ser la configuración actual)
- Se actualiza de manera iterativa usando:
q k 1  q k  J 1 (q k )  x d  f (q k )  k = 0,1,2,3,…
- Se detiene cuando:
‖ x d  f (q k ) ‖  o ‖ q k 1  q k ‖   : valor pequeño
error Cartesiano pequeño Incremento articular pequeño
• Comentarios:
- Convergencia si se comienza con q0 (valor inicial) cercano a la solución
- Cuando hay redundancia (m < n):
¡J no es cuadrada (y no existe inversa)! → se usa la pseudo-inversa
- Desventajas:
• Tiempo de cálculo de la inversa (o pseudo-inversa)
• Problemas cerca a las singularidades de J (matriz Jacobiana)
- Ventaja: es rápido (convergencia cuadrática)
41
• Ejemplo: robot RR
Calcular los valores articulares para que el efector final llegue al punto x = 1.2,
y = 1.2 usando el método de Newton. Asumir que l1 = l2 = 1.
Solución
ŷ
c1  c12  
- Cinemática directa: f (q)    y
 s1  s12 
l2
- Matriz Jacobiana (Jacobiano):
l1 q2
 f x f x 
 q2   ( s1  s12 ) q1
f (q)  q1    s12 
J (q)   x
q  f y f y   c  c c12  x̂
   1 12
 q1 q2 
- Posición deseada:
- Inversa del Jacobiano:
1.2 
1  c12 s12  xd   
J 1 (q)   (c  c ) ( s  s )  1.2 
s2  1 12 1 12 
42
y = 1.2 usando el método de Newton. Asumir que l1 = l2 = 1.
Solución
- Ecuación genérica del método de Newton:
q k 1  q k  J 1 (q k )(xd  f (q))
- Reemplazando los valores anteriores:
1  c12 s12   1 c1  c12  

q k 1  qk    (c  c ) ( s  s )   1   s  s  
 s2  1 12 1 12      1 12  
Error actual
- Se aplica iterativamente.
Por ejemplo, tomar q1 = 0.5 y q2 = 0.5 como valores iniciales (nota: no se puede iniciar
con q2 = 0 pues J-1 sería indeterminado)
43
• Ejemplo: robot RR (con Python)

import numpy as np
cos=np.cos; sin=np.sin
xd = np.array([1.2, 1.2]) # Valor deseado en el espacio cartesiano

q = np.array([0.5, 0.5]) # Valor inicial en el espacio articular
epsilon = 1e-3
max_iter = 100 # Maximo numero de iteraciones
# Iteraciones: Método de Newton

for i in range(max_iter):
q1 = q[0]; q2 = q[1];
J = np.array([
[-sin(q1)-sin(q1+q2), -sin(q1+q2)],
[ cos(q1)+cos(q1+q2), cos(q1+q2)]])
f = np.array([cos(q1)+cos(q1+q2), sin(q1)+sin(q1+q2)])
e = xd-f
q = q + np.dot(np.linalg.inv(J), e)
# Condicion de termino
if (np.linalg.norm(e) < epsilon):
break
print(q)
44
- Notar que el resultado depende del valor inicial utilizado.
- Resultados:
0.2278  1.3430 
q  q 
1.1157   1.1152 
1.4
1.2
0.8
0.6
0.4
0.2
0
0 0.2 0.4 0.6 0.8 1 1.2
Configuración obtenida usando Configuración obtenida usando

q=[0.5; 0.5] como valor inicial q = [1; -1] como valor inicial
45
(Método del Gradiente)
• Objetivo:
- Minimizar la función genérica g (q)
min g (q)
q
• Idea:
- Empezar con un valor inicial q0
- “Moverse” en la dirección negativa del gradiente ( g) iterativamente
q k 1  q k  g (q k )    : tamaño de paso
- El tamaño de paso (step size) α > 0 debe garantizar un máximo decrecimiento

de g(q) en cada iteración:
• α muy alto: puede ocasionar divergencia (no se encuentra el mínimo)
• α muy pequeños: genera convergencia muy lenta
Recordar: el gradiente indica la dirección de máxima variación

46
b) Método del Gradiente
• Cinemática directa: x d  f (q) o equivalentemente x d  f (q)  0

error
• Procedimiento:
1
- Definir una función escalar de error: g (q )  ‖ x d  f (q) ‖2 g : n  
2
- Objetivo: minimizar el error: min g (q )
q
- Calcular el gradiente de g(q):
T
1 T  f (q) 
g (q)   x d  f (q)   x d  f (q)  g (q)      x d  f (q) 
2  q 
- Aplicar el método del gradiente J (q)
q k 1  q k  g (q k )
q k 1  q k   J T (q k )  x d  f (q k ) 
• Ventaja: computacionalmente más simple (transpuesta en lugar de inversa)

• Desventaja: puede tener convergencia lenta (se puede usar regla de Armijo)
47
y = 1.2 usando el método del gradiente. Asumir que l1 = l2 = 1.
Solución ŷ
c1  c12  y 
- Cinemática directa: f (q)   
s  s
 1 12 
l2
- Matriz Jacobiana (Jacobiano): l1 q2
 f x f x  q1
 q2   ( s1  s12 )
f (q)  q1    s12  x
J (q)   x̂
q  f y f y   c  c c12 
   1 12
 q1 q2 
- Posición deseada:
- Transpuesta del Jacobiano:
1.2 
xd   
 ( s  s ) c1  c12  1.2 
J T (q)   1 12
  s12 c12 
48
y = 1.2 usando el método del gradiente. Asumir que l1 = l2 = 1.
Solución
- Ecuación genérica del método del gradiente:

q k 1  q k   J T (q k )(x d  f (q))
- Reemplazando los valores anteriores:
 ( s  s ) (c1  c12 )   1 c1  c12  

q k 1  qk    1 12     
  s12 c12 1 s  s
     1 12  
Error actual
- Se aplica iterativamente.
Por ejemplo, tomar q1 = 0.5 y q2 = 0.5 como valores iniciales (en este caso sí se puede
iniciar con q2 = 0)
49
• Ejemplo: robot RR (en Python)

import numpy as np
cos=np.cos; sin=np.sin
xd = np.array([1.2, 1.2]) # Desired value in the Cartesian space

q = np.array([0.5, 0.5]) # Initial value in the joint space
epsilon = 1e-3
max_iter = 1000 # Maximum number of iterations
alpha = 0.5
# Iterations: Gradient descent

for i in range(max_iter):
q1 = q[0]; q2 = q[1];
J = np.array([[-sin(q1)-sin(q1+q2), -sin(q1+q2)],
[ cos(q1)+cos(q1+q2), cos(q1+q2)]])
f = np.array([cos(q1)+cos(q1+q2), sin(q1)+sin(q1+q2)])
e = xd-f
q = q + alpha*np.dot(J.T, e)
# End condition
if (np.linalg.norm(e) < epsilon):
break
print(q)
50
Comparación
• Método de Newton
- Rapidez de convergencia cuadrática (rápido)
- Problemas cerca a las singularidades (las singularidades se pueden evaluar con
los valores singulares de J)
• Método de gradiente
- Rapidez de convergencia lineal (lento)
- No presenta problemas con singularidades
- Se debe escoger el incremento (α): se puede usar métodos adaptativos
como búsqueda lineal (line search)
• Algoritmo eficiente:
- Iniciar con el método del gradiente (seguro pero lento)
- Luego cambiar al método de Newton
51
Temas
1. Introducción
52
Cálculo Numérico del Jacobiano
• Para robots complejos:

- Tedioso calcular manualmente el Jacobiano
• Alternativa:
- Cálculo numérico: diferenciación numérica
• El Jacobiano (considerando solo posición) de un robot de n gdl es:
Cada columna es la
variación con
respecto a una misma
articulación (ángulo) posición
x p  ( x, y , z )
x p  f (q1 , , qn )
- Aproximación de la derivada de la posición xp con respecto a qi

x p x p f (q1 , , qi  qi , , qn )  f (q1 , , qn ) Incremento solo para
  la articulación qi
qi qi qi
53
• Ejemplo
Considerar la siguiente cinemática directa (robot R-R con l1 = l2 = 1):
 f x (q)  cos(q1 )  cos(q1  q2 ) 
f (q)    
 f y (q )   sin( q1 )  s in( q1  q )
2 
Calcular el Jacobiano numéricamente cuando q1 = 0.5, q2 = 1.0

Solución
- El Jacobiano es:  f x f x 
 q q2 
1
J (q)   
 f y f y 
 
 q1 q2 
- Derivadas con respecto a q1 (primera columna):
f x f x (q1  q1 , q2 )  f x (q1 , q2 )   cos(q1  q1 )  cos((q1  q1 )  q2 )   cos(q1 )  cos(q1  q2 ) 

q1 q1 q1
f y

f y (q1  q1 , q2 )  f y (q1 , q2 )

sen(q1  q1 )  sen((q1  q1 )  q2 )   sen(q1 )  sen(q1  q2 ) 
q1 q1 q1
54
• Ejemplo
Considerar la siguiente cinemática directa (robot R-R con l1 = l2 = 1):
 f x (q)  cos(q1 )  cos(q1  q2 ) 
f (q)    
 f y (q )   sin( q1 )  s in( q1  q )
2 
Calcular el Jacobiano numéricamente cuando q1 = 0.5, q2 = 1.0

Solución
- El Jacobiano es:  f x f x 
 q q2 
1
J (q)   
 f y f y 
 
 q1 q2 
- Derivadas con respecto a q2 (segunda columna):
f x f x (q1 , q2  q2 )  f x (q1 , q2 )   cos(q1 )  cos(q1  (q2  q2 ))   cos(q1 )  cos(q1  q2 ) 

q2 q2 q2
f y

f y (q1 , q2  q2 )  f y (q1 , q2 )

sen(q1 )  sen(q1  (q2  q2 ))   sen(q1 )  sen(q1  q2 ) 
q2 q2 q1
55
• Ejemplo
- Usando Δq1 = Δq2 = 0.001:
f x  cos(0.5  0.001)  cos(0.5  0.001  1.0)    cos(0.5)  cos(0.5  1.0) 

  1.4774
q1 0.001
f y

 sen(0.5  0.001)  sen(0.5  0.001  1.0)    sen(0.5)  sen(0.5  1.0)   0.9476
q1 0.001
f x  cos(0.5)  cos(0.5  1.0  0.001)    cos(0.5)  cos(0.5  1.0) 

  0.9975
q2 0.001
f y

 sen(0.5)  sen(0.5  1.0  0.001)    sen(0.5)  sen(0.5  1.0)   0.0702
q2 0.001
Jacobiano usando el enfoque

- Jacobiano usando cálculo numérico: analítico (para comparación)
 1.4774 0.9975  1.4769 0.9975

J (q)    J (q)   
 0.9476 0.0702   0.9483 0.0707 
Valores similares (errores de redondeo)
56
• Ejemplo
Computacionalmente (usando Python):
- Función de cinemática directa
def fkine(q):
x = np.cos(q[0]) + np.cos(q[0]+q[1]);
y = np.sin(q[0]) + np.sin(q[0]+q[1]);
return np.array([x,y])
- Cálculo del Jacobiano
q1 = 0.5; q2 = 1.0
delta = 0.001
JT = 1/delta*np.array([
fkine([q1+delta, q2]) - fkine([q1,q2]),
fkine([q1, q2+delta]) - fkine([q1,q2])])
J = JT.transpose()
print(J)
57
Conclusiones
• La cinemática inversa encuentra configuraciones

articulares dada una posición/orientación deseada
• En cinemática inversa puede existir una solución, varias

soluciones, infinitas soluciones o ninguna solución
(existencia de soluciones definida por el espacio de trabajo)
• Las soluciones analíticas son más complejas, menos

sistemáticas y aplicables a casos simples
• Las soluciones numéricas son más genéricas y son

aplicables en todos los casos
58
Referencias
• B. Siciliano, L. Sciavicco, L. Villani, y G. Oriolo. Robotics: modelling,

planning and control. Springer Science & Business Media, 2010
(Sección 2.2)
• M.W. Spong, S. Hutchinson, y M. Vidyasagar. Robot Modeling and

Control. John Wiley & Sons, 2006 (Secciones 1.4, 3.3)
• K. Lynch and F. Park. Modern Robotics: Mechanics, Planning, and

Control. Cambridge University Press, 2017 (Secciones 6.1-6.2)
59
Apéndice 1: Método de Newton
• Objetivo:
- Encontrar el punto donde la función se hace cero
- Equivalentemente: encontrar la intersección de la curva y=f(x) con eje x,
esto es: f(x) = 0
Idea
Imagen de: https://en.wikipedia.org/wiki/Newton%27s_method
• ¿Cómo? De manera iterativa
60
• En forma escalar (1-D): f ( x)

- Ecuación de la recta tangente en x0
f ( x)  f ( x0 )  f '( x0 )( x  x0 ) f ( x0 )
- Se desea la intersección con cero: f(x) = 0

 f ( x0 )  f '( x0 )( x  x0 )
f ( x0 )
x  x0  x0
f ( x0 ) x
- En general:
f ( xk ) Aplicar
xk 1  xk  iterativamente
x0 , x1 , x2 , x3 ,
f ( xk )
- Alternativamente: expansión de Taylor
f ( x)  f ( x0 )  f '( x0 )( x  x0 )  o ‖ x  x ‖  0
2
Términos de orden superior
Descartando los términos de orden superior se obtiene la ecuación de la recta

61
• Ejemplo:
Encontrar la intersección con cero para f(x) = 3 + (x-2)3 usando el método de
Newton y 10 iteraciones
f ( xk ) 3  ( xk  2)3
xk 1  xk  xk 1  xk 
f ( xk ) 3( xk  2) 2
% Valor inicial (cualquier valor)

2
x0 = 0;
% Iteraciones del método de Newton
for i=1:10 0
x0 = x0 - (3+(x0-2)^3)/(3*(x0-2)^2);
end -2
disp(['Intersección en: ' num2str(x0)])
-4
% Gráfico de la función
x = 0:0.001:1;
-6
y = 3 + (x-2).^3; 0 0.2 0.4 0.6 0.8 1
plot(x,y), hold on, plot(x0,0,'o'), grid on
Usando el programa se encuentra la intersección en: 0.55775

62
Apéndice 2: Matlab
• Ejemplo: robot RR (con matlab)
xd = [1.2; 1.2]; % Valor deseado en el espacio Cartesiano

q = [0.5; 0.5]; % Valor Inicial en el espacio articular
epsilon = 1e-3;
max_iter = 100; % Máximo número de iteraciones
% Iteraciones del método de Newton

for i=1:max_iter
q1=q(1); q2=q(2);
Jinv = 1/sin(q2)*[cos(q1+q2), sin(q1+q2);
-cos(q1)-cos(q1+q2), -sin(q1)-sin(q1+q2)];
f = [cos(q1)+cos(q1+q2); sin(q1)+sin(q1+q2)];
e = xd-f;
q = q + Jinv*e;
% Condición de término
if (norm(e)<epsilon)
break;
end
end
disp(q) % Muestra el valor obtenido
63
Apéndice 2: Matlab
• Ejemplo: robot RR (en Matlab)
xd = [1.2; 1.2]; % Valor deseado en el espacio Cartesiano

q = [0; 0]; % Valor Inicial en el espacio articular
epsilon = 1e-3;
max_iter = 1000; % Máximo número de iteraciones
alpha = 0.5;
% Iteraciones del método del gradiente
for i=1:max_iter
q1=q(1); q2=q(2);
JT = [-sin(q1)-sin(q1+q2), cos(q1)+cos(q1+q2);
-sin(q1+q2), cos(q1+q2)];
f = [cos(q1)+cos(q1+q2); sin(q1)+sin(q1+q2)];
e = xd-f;
q = q + alpha*JT*e;
% Condición de término
if (norm(e)<epsilon)
break;
end
end
disp(q)
64
Apéndice 2: Matlab
c) Cálculo numérico del Jacobiano
• Ejemplo
- Función de cinemática directa
function X = rr_fkine(q)
x = cos(q(1)) + cos(q(1)+q(2));
y = sin(q(1)) + sin(q(1)+q(2));
X=[x;y];
- Cálculo del Jacobiano
q1=0.5; q2=1.0;
J = 1/delta*[ rr_fkine([q1+delta,q2])-rr_fkine([q1,q2]),
rr_fkine([q1,q2+delta])-rr_fkine([q1,q2]) ]
65

5 Cinematica Inversa

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

5 Cinematica Inversa

Cargado por

Copyright:

Formatos disponibles

Fundamentos de Robótica: 2020-1

- 11 de mayo del 2020 -

• Comprender conceptos básicos de cinemática inversa

• Comprender el concepto de espacio de trabajo

• Obtener la cinemática inversa de robots manipuladores

• Obtener la cinemática inversa de robots manipuladores

2. Espacio de Trabajo (y Existencia de Soluciones)

6. Cálculo Numérico del Jacobiano

Relación entre articulaciones (qi)

• Posición de un robot de 2 grados de libertad

Cinemática  x  l1 cos( q1 )  l2 cos(q1  q2 )  Dado q, existe

Cinemática  q1  ?  Dado x, ¿existe

Efector final con respecto a la base:

Calcular los ángulos para que la posición y orientación del

Equivalentemente, resolver el sistema:

• Encontrar los ángulos

2. Espacio de Trabajo (y Existencia de Soluciones)

6. Cálculo Numérico del Jacobiano

• Robot ABB IRB 120

[ABB IRB 120 Industrial Robot – Datasheet] 12

con q1 ∈ [0, 2π], q2 ∈ [0, 2π]

Si se limita los ángulos … ¿varía el espacio de trabajo?

• Si q1 ∈ [0, π], q2 ∈ [0, π], ¿cuál es el espacio de trabajo?

• Espacio de trabajo primario (alcanzable): WS1

Cada posición puede ser alcanzada

- Fuera de WS1 no hay solución al problema

• Espacio de trabajo secundario (diestro): WS2

El punto se alcanza con todas

• Relación entre WS1 y WS2: WS2 ⊆ WS1

Nota: a WS1 se le denomina reachable y a WS2 dexterous 15

• Ejemplo: Robot ABB IRB 120

WS1 (=WS2 para muñeca esférica)

Se utiliza para evaluar al robot para una aplicación específica

[ABB IRB 120 Industrial Robot – Datasheet] 16

• Ejemplo: Robot LBR iiwa de KUKA

Side View Top View

[LBR iiwa 7 R800, LBR iiwa 14 R820 Specification Manual] 17

[IRB 360 Flex Picker Specifications] 18

2. Espacio de Trabajo (y Existencia de Soluciones)

6. Cálculo Numérico del Jacobiano

Las 4 configuraciones llegan al

pA Configuración inicial para pA: qA

Distancia articular entre configuraciones:

En general, si hay N configuraciones posibles para pB escoger:

qb  arg min q  q A para q  {q1 , q 2 , , q N }

m variables en el espacio de tarea:

Robot redundante con respecto a una tarea si:

• Las ecuaciones a resolver son no lineales

• ¿Cuándo se garantiza una solución para la posición?

• ¿Cuándo se garantiza una solución para la posición y orientación?

2. Espacio de Trabajo (y Existencia de Soluciones)

6. Cálculo Numérico del Jacobiano

• Tratan de encontrar los valores articulares q1, q2, …, qn analizando la

• Pasos útiles (depende del caso):

• Aplicable a robots con pocos gdl (3 o menos) … ¿por qué?

• Tratan de encontrar los valores articulares q1, q2, …, qn manipulando

• Elementos útiles (depende del caso):

- Encontrar el seno y coseno de forma matricial y reemplazarlo en atan2

x 2  l12 c12  l2 2 c12 2  2l1c1l2 c12 q1

x 2  y 2  l12  l2 2  2l1l2 (c1c12  s1s12 )

En forma matricial: y (l1  l2 c2 )  xl2 s2

donde a3 y d1 son constantes. La configuración deseada del efector final es:

Encontrar la cinemática inversa ( , , ) de este robot como una función de