5 Cinematica Inversa
5 Cinematica Inversa
5 Cinematica Inversa
- Tema 5 -
Cinemática Inversa de Robots
Manipuladores
Prof. Oscar E. Ramos, Ph.D.
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
4
Cinemática de Robots Manipuladores
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
Cinemática de Robots Manipuladores
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 (qpara
) algún punto del robot (ejm. efector final)
Encontrar q (q1 , , qn )
6
Cinemática de Robots Manipuladores
Ejemplo: Robot de 2 gdl
ŷ
Variables Variables
y cartesianas articulares
x q
l2 x q 1
q2 y q2
l1 Considerando
solo posición
q1
x x̂
q1 ?
Cinemática
q ? q f 1 ( x) ? Dado x, ¿existe (un
inversa 2 único) q?
7
Cinemática de Robots Manipuladores
Ejemplo: Cinemática Directa
Posición y
orientación
donde:
Manipulador Stanford
8
Cinemática de Robots Manipuladores
Ejemplo: Cinemática Inversa
¡Sistema de ecuaciones
trigonométricas no lineales!
Manipulador Stanford
9
Cinemática Inversa
• 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
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
11
Espacio de Trabajo
13
Espacio de Trabajo
Ejemplo: Robot 2 gdl
y Espacio de
ŷ trabajo
Si l1 > l2
y x
p
y
l2
x
l1 q2
q1
x x̂
14
Espacio de Trabajo
- Para todo p WS2 hay (por lo menos) una solución para cualquier orientación
WS2 WS1
• Relación entre WS1 y WS2: Nota: a WS1 se le denomina reachable y a WS2 dexterous 15
Espacio de Trabajo
• Ejemplo:
Robot IRB 360 de ABB
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
19
Multiplicidad de Soluciones
Ejemplo: Posiciones del Robot PUMA
Multiplicidad de soluciones
PUMA 560 20
Multiplicidad de Soluciones
¿Qué solución escoger?
n articulaciones: q (q1 , , qn )
22
Cinemática Inversa
Complejidad
• Pueden existir:
- Una solución
- Múltiples soluciones
- Infinitas soluciones (cuando hay redundancia)
- Ninguna solución admisible
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
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
25
Soluciones Geométricas
α b c 2 a 2 b 2 2ab cos
a
26
Soluciones Geométricas
• Ejemplo: ŷ
Encontrar la cinemática inversa para la posición del
y
robot RR usando el enfoque geométrico
l2 l 2s 2
l
Solución q2
l1 l 2c 2
Para q2 :
q1
- Usando la ley de cosenos: x x̂
s22 c22 1
Cinemática inversa:
2
s2 1 c 2 q1 atan2( y, x) atan2(l2 s2 , l1 l2c2 )
q2 atan2( s2 , c2 ) q2 atan2( s2 , c2 )
27
Soluciones Algebraicas
- Expandir expresiones del ángulo doble (a veces se pueden simplificar con otros
sumandos)
sin( ) sin cos sin cos
cos( ) cos cos sin sin
a b cos qi e
c d sin q f qi atan2(sin qi , cos qi )
i
28
Soluciones Algebraicas
• Ejemplo 1
Encontrar la cinemática inversa para la posición de un robot RR usando el enfoque
algebraico
Solución ŷ
- Cinemática Directa: y
x l1 c1 l2 c12 y l1 s1 l2 s12
l2
- Para q2 : q2
l1
29
Soluciones Algebraicas
• Ejemplo 1
Encontrar la cinemática inversa para la posición de un robot RR usando el enfoque
algebraico
Solución ŷ
- Para q1 (expandiendo términos de cinemática directa): y
x l1c1 l2 (c1c2 s1s2 )
l2
y l1s1 l2 ( s1c2 c1s2 )
q2
l1
Ecuaciones son lineales en c1, s1: resolver
x c1 (l1 l2c2 ) s1 (l2 s2 ) q1
y s1 (l1 l2 c1 ) c1 (l2 s2 ) x x̂
30
Soluciones Algebraicas
• Ejemplo 2
El efector de un robot de 3 gdl RPR, con respecto a su base está dado por
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
Soluciones Algebraicas
• Ejemplo 2
- Para :
- Para :
- Para :
32
Solución por Transformaciones Inversas
Valores
numéricos
• 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 )
0
T1 (q1 ) 1Tdes n 1Tn (qn ) 1 1T2 (q2 ) n 2Tn 1 (qn 1 )
33
Desacoplo Cinemático
y6
ul a ciones W
as 3 artic ipo {6}
r t
prime cualquier z6
de
d6
Posición y orientación
deseadas para el efector
final {6} con respecto a
la base
px d 6 ax
• Posición de la muñeca: pw p y d 6 a y Valor numérico
pz d 6 az
35
Desacoplo Cinemático
• 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,
q 2, q 3
2. Con los valores de q1, q2, q3 se calcula la rotación 0R3 y la transformación homogénea 0T3
3
T6 (q4 , q5 , q6 ) 0T31Tdes
• 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
q1 0 0 90°
l1
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
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
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
• 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
Soluciones Numéricas
a) Método de Newton
x d f (q k ) J (q k )(q q k )
(q k ) x d (n
1
- Asumiendo J cuadrado e Jinvertible f=(q k ) (q q k )
m):
40
Soluciones Numéricas
a) Método de Newton
• 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
• 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
Soluciones Numéricas
a) Método de Newton
• 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
ŷ
c1 c12
- Cinemática directa: f (q ) s s y
1 12
l2
- Matriz Jacobiana (Jacobiano): q2
l1
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
Soluciones Numéricas
a) Método de Newton
• 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
- Ecuación genérica del método de Newton:
q k 1 q k J 1 (q k )(x d f (q ))
- 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
Soluciones Numéricas
a) Método de Newton
• Ejemplo: robot RR
- Notar que el resultado depende del valor inicial utilizado.
- Resultados:
0.2278 1.3430
q q
1.1157 1.1152
1.4 1.4
1.2 1.2
1 1
0.8 0.8
0.6 0.6
0.4 0.4
0.2 0.2
0 0
0 0.2 0.4 0.6 0.8 1 1.2 0 0.2 0.4 0.6 0.8 1 1.2
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
- 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
2
g (q) xd f (q)
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 ) xd f (qk )
• 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 del gradiente. Asumir que l1 = l2 = 1.
Solución ŷ
c1 c12 y
- Cinemática directa: f (q ) s s
1 12
l2
- Matriz Jacobiana (Jacobiano): q2
l1
f x f x q1
q2 ( s1 s12 ) s12
f (q) q1 x x̂
J (q)
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
Soluciones Numéricas
b) Método del Gradiente
• 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 del gradiente. Asumir que l1 = l2 = 1.
Solución
- 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
Soluciones Numéricas
b) Método del Gradiente
• 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
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
52
Cálculo Numérico del Jacobiano
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 )
x p
x p f (q1 , , qi qi , , qn ) f (q1 , , qn ) Incremento solo para la
- Aproximación de la derivada la posición x con respecto a q
qde
articulación qi
qi i
p qi i
53
Cálculo Numérico del Jacobiano
• 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 ) si n( q1 q2 )
Calcular el Jacobiano numéricamente cuando q1 = 0.5, q2 = 1.0
Solución
- El Jacobiano es: f x f x
q q2
J (q)
1
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
Cálculo Numérico del Jacobiano
• 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 ) si n( q1 q2 )
Calcular el Jacobiano numéricamente cuando q1 = 0.5, q2 = 1.0
Solución
- El Jacobiano es: f x f x
q q2
J (q)
1
f y f y
q1 q2
- Derivadas con respecto a q2 (segunda columna):
f x f (q , q q2 ) f x (q1 , q2 ) cos(q1 ) cos(q1 (q2 q2 )) cos(q1 ) cos( q1 q2 )
x 1 2
q2 q2 q2
• Ejemplo
- Usando Δq1 = Δq2 = 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 y
sen(0.5) sen(0.5 1.0 0.001) sen(0.5) sen(0.5 1.0) 0.0702
q2 0.001
• Ejemplo
Computacionalmente (usando Python):
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])
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
58
Referencias
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
60
Apéndice 1: Método de Newton
- En general: f ( xk ) Aplicar
x0 , x1 , x2 , x3 ,
xk 1 xk iterativamente
f ( xk )
f ( x) f ( x0 ) f '( x0 )( x x0 ) o ‖ x x0 ‖ 2
Términos de orden superior
• 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
63
Apéndice 2: Matlab
b) Método del Gradiente
64
Apéndice 2: Matlab
c) Cálculo numérico del Jacobiano
• Ejemplo
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];
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