ROBOTICA
CINEMATICA
INVERSA
(Primera parte)
Ing. Iván A. Calle Flores
1
Contenido
1. Cinemática
2. Cinemática Inversa
3. Métodos Algebraicos
4. Métodos Geométricos
Msc. Iván A. Calle Flores
1. CINEMATICA DIRECTA
Se refiere al calculo de la
configuración del efector final
en función de las variables de
las juntas.
Orientación Posición
nn0 (q) sn0 (q) an0 (q) on0 (q)
T (q)
n
0
0 0 0 1
q R nx1 : Vector de variables de las juntas
Msc. Iván A. Calle Flores
Cinemática directa
• El procedimiento consiste en asignar sistemas coordenados siguiendo el
criterio de Denavit-Hartenberg para luego hallar las matrices de cinemática.
z
y
z - Posición
y x - Orientación
z 2
z x y
3 Rn0 on0
T (q)
n
0
y x 0 1
1
x
n 1
T (q) A (q1 ) A (q2 )... A (qn )
n
0 0
1
1
2 n
Msc. Iván A. Calle Flores
Cinemática directa
• En el calculo de la cinemática inversa se trabaja con los sistemas coordenados
asignados en la cinemática inversa.
Msc. Iván A. Calle Flores
2. CINEMATICA INVERSA
Se refiere al problema de
calcular el valor de las juntas
para que el efector (sistema
coordenado final) tenga una
posición y orientación deseada.
Msc. Iván A. Calle Flores
Problema general de la cinemática inversa
De manera general el problema de la cinemática inversa se puede establecer
de la siguiente manera:
• Dada una matriz homogénea que representa una posición y orientación deseada
• Hallar los valores de “qi” tal que se cumpla:
• De donde se tiene un sistema de 12 ecuaciones no lineales en “n” variables
Msc. Iván A. Calle Flores
Desacoplamiento cinemático
En el caso mas genérico un robot
tiene 6DOF y usa una muñeca
esférica:
Msc. Iván A. Calle Flores
Desacoplamiento cinemático
En esta primera parte nos
enfocaremos en analizar
solamente el problema de
posicionamiento.
• Nótese que algunos robots
solo pueden establecer su
posición de manera arbitraria.
Msc. Iván A. Calle Flores
3. METODOS ALGEBRAICOS
Se refiere al problema de
calcular el valor de las juntas
usando la matriz T de la
cinemática directa.
R 0
on0
Tn0 (q) n
0 1
Msc. Iván A. Calle Flores
Método Algebraico
Ejemplo. Halle la cinemática inversa
del manipulador cilíndrico mostrado.
c1 0 s1 s1d 3
s 0 c1 c1d 3
T30 A1 A2 A3 1
0 1 0 d1 d 2
0 0 0 1
Msc. Iván A. Calle Flores
Método Algebraico
Ejemplo. Halle la cinemática inversa
del manipulador cilíndrico mostrado.
c12 s12 0 a1c1 a2c12
s c12 0 a1s1 a2 s12
T20 A1 A2 12
0 0 1 0
0 0 0 1
Msc. Iván A. Calle Flores
Método Algebraico
Ejemplo. Halle la cinemática inversa
del manipulador cilíndrico mostrado.
c1c23 - c1s23 s1 c1 (a2c2 a3c23 )
s c - s s - c s1 (a2c2 a3c23 )
T30 (q) A10 A21 A32 1 23 1 23 1
s23 c23 0 d1 + a2 s2 a3 s23
0 0 0 1
• Por lo general es difícil resolver el sistema de ecuaciones en el caso de
manipuladores de mas de 2GDL.
• Es preferible usar métodos geométricos, los que además nos permiten visualizar
el caso de múltiples soluciones
Msc. Iván A. Calle Flores
4. METODOS GEOMETRICOS
Se refiere al problema de
calcular el valor de las juntas
usando las herramientas de la
geometría en el espacio.
Msc. Iván A. Calle Flores
LEY DE COSENOS
c2 - a2 -b2
cosθ= =D
2ab θ=atan2(± 1-D ,D)
2
sinθ=± 1-D2
c b
a
FUNCION “atan2”
La función arco tangente con dos argumentos “atan2” permite conocer el
cuadrante de un ángulo computado.
Nótese que Matlab y otros lenguajes usan el formato: atan2(y,x)
El formato usado en el libro de texto de Spong es: atan2(x, y)
Ejemplo 1. Cinemática inversa de un
manipulador planar
Halle la cinemática inversa
del manipulador de dos
grados de libertad (GDL)
mostrado en la figura
Msc. Iván A. Calle Flores
Ejemplo 1. Cinemática inversa de un
manipulador planar
Paso 0: Cinemática directa
link i di ai ai
1 1 0 a1 0
2 2 0 a2 0
c12 -s12 0 a1c1 +a2c12
s c12 0 a1s1 +a2s12
T2 =
0 12
0 0 1 0
0 0 0 1
Ejemplo 1. Cinemática inversa de un
manipulador planar atan2(y,x)
a. Hallando el ángulo 2
x2 +y 2 - a12 - a22
cosθ2 = =D
2a1a2
y
senθ2 =± 1-D2
Nótese los valores validos de D
Especificando la solución usando
“atan2” tenemos:
θ2 = atan2(± 1-D2 ,D) x
Msc. Iván A. Calle Flores
Ejemplo 1. Cinemática inversa de un
manipulador planar
Dependiendo del signo elegido tenemos dos posibles soluciones
θ2 = atan2(± 1-D2 ,D)
Msc. Iván A. Calle Flores
Ejemplo 1. Cinemática inversa de un
manipulador planar
b. Hallando el ángulo 1
Hallamos ángulos auxiliares
y
α =atan2(y, x)
β=atan2(a2s2 , a1 +a2c2 )
De donde tenemos la solución: β α
θ1 =α-β x
Msc. Iván A. Calle Flores
Ejemplo 1. Cinemática inversa de un
manipulador planar
Resumen de formulas
θ1 =atan2(y, x) atan2(a2s2 , a1 +a2c2 )
y
θ2 = atan2(± 1-D2 ,D)
En la practica el valor de
los ángulos también esta
limitado por restricciones
mecánicas.
Además se debe notar que x
existen puntos que no tienen
solución (el robot no puede
llegar a dichos puntos).
Msc. Iván A. Calle Flores
Ejemplo 1. Cinemática inversa de un
manipulador planar
2.5
Implemente la solución
en MATLAB 2 X
2
1.5
Y2 Z{2}2
y(m)
1 Y Y1
X1
0.5 Z1
{1}
0 {}Z X
-0.5
-0.5 0 0.5 1 1.5 2 2.5
x(m)
Demo_01.m
Msc. Iván A. Calle Flores
Ejemplo 2. Cinemática inversa de un
manipulador de 2GDL
Paso 0: Cinemática directa
link i di ai ai
1 1 d1 0 pi/2
2 2 0 a2 0
c1c2 - c1s2 s1 a2c1c2
s c -s1s2 - c1 a2s1c2
T20 =
1 2
s2 c2 0 d1 +a2s2
0 0 0 1
Msc. Iván A. Calle Flores
Ejemplo 2. Cinemática inversa de un
manipulador de 2GDL
PRIMERA SOLUCION
y
s
r r
x z=d1 +s
r = x2 +y 2
θ1 =atan2 y, x θ2 =atan2 s,r
Msc. Iván A. Calle Flores
Ejemplo 2. Cinemática inversa de un
manipulador de 2GDL
Resumen de formulas
PRIMERA SOLUCION
θ1 =atan2 y, x
θ2 =atan2 s,r
r = x2 +y 2
s z-d1
En esta solución se asume
que el eje x1 esta de frente
al punto deseado
θ2 ∈ -π, π
Ejemplo 2. Cinemática inversa de un
manipulador de 2GDL
SEGUNDA SOLUCION
y s
r
r
x
θ1 = π+atan2 y, x θ2 = π- atan2 s,r
Msc. Iván A. Calle Flores
Ejemplo 2. Cinemática inversa de un
manipulador de 2GDL
Implemente la primera
solución en MATLAB3
Y2
2.5
Y1 X2
Z2 {2}
2
Z1 {1} X1
1.5
Z
1
0.5
{} -1
Y
X 0
0 1
Demo_02.m -1 -0.5 0 0.5 1 1.5 2 2
x(m)
Msc. Iván A. Calle Flores
y(m)
Ejemplo 2. Cinemática inversa de un
manipulador de 2GDL
Implemente la segunda
solución en MATLAB 3
2.5
Z2 X2
2 Y1 {2}
Z1 Y2
1.5
X1 {1}
1
Z
0.5
Y
0 {}
-1
2
0 X
1
1 0
Demo_02b.m 2 -1
y(m)
x(m)
Msc. Iván A. Calle Flores
Ejemplo 3. Cinemática inversa de un
manipulador de 2GDL con hombro
link i di a ai
1 1 d1 0 pi/2
2 2 d2 a2 0
Ejemplo 3. Cinemática inversa de un
manipulador de 2GDL con hombro
Primera solución hombro
derecho mirando hacia adelante
y
d2 a2
s
β
α r
r
x z
d1
r 2 = x2 +y 2 -d22
α =atan2(y, x)
β=atan2(d2 ,r)
θ1 =α+β θ2 =atan2 s,r
Ejemplo 3. Cinemática inversa de un
manipulador de 2GDL con hombro
Segunda solución hombro
derecho mirando hacia atrás
α =atan2(y, x)
β=atan2(d2 ,r)
d2 y s
r
r
α β z
d1
x
r 2 = x2 +y 2 -d22
θ1 =α-β+π θ2 = π- atan2 s,r
Msc. Iván A. Calle Flores
Ejemplo 4. Cinemática inversa del
manipulador RRR
Msc. Iván A. Calle Flores
Ejemplo 4. Cinemática inversa del
manipulador RRR
Paso 0: Cinemática directa
Msc. Iván A. Calle Flores
Ejemplo 4. Cinemática inversa del
manipulador RRR
Nótese que para hallar 1 hay dos soluciones
y
La otra solución se da
cuando el eje x1 esta
mirando para atrás
θ1 = π+atan2 y, x
x
θ1 =atan2 y, x
Msc. Iván A. Calle Flores
Ejemplo 4. Cinemática inversa del
manipulador RRR
Para hallar 3 usamos la
vista del plano “r”
a3
r +s - a - a
2 2 2 2 θ3
cosθ3 = =D
2 3
s
2a2a3 a2
θ2
De donde tenemos las dos
soluciones del ángulo 3: r
θ3 =atan2 ± 1-D2 ,D d1 s = z - d1
r = x 2 +y 2
Msc. Iván A. Calle Flores
Ejemplo 4. Cinemática inversa del
manipulador RRR
Para hallar 2 usamos la
vista del plano “r”
Hallamos ángulos auxiliares
a3
θ3
α = atan2(s,r) a2 s
β=atan2(a3s3 , a2 +a3c3 ) θ2
De donde tenemos la solución:
r
θ2 =α-β d1 s = z - d1
r = x 2 +y 2
Msc. Iván A. Calle Flores
Ejemplo 4. Cinemática inversa del
manipulador RRR
Tenemos las dos
primeras soluciones
θ1 =atan2 y, x
θ2 =α-β
θ3 =atan2 ± 1-D2 ,D
Estas soluciones aplican
cuando el robot mira
hacia adelante “x1”
Ejemplo 4. Cinemática inversa del
manipulador RRR
Implemente la primera y
segunda solución en 3.5
MATLAB 3
Y3 X3
2.5
Z{3}
3
2
Y2
X2
Y1 {2}
Z2
1.5 X1
{1}
Z1
1
0.5
Z
0 Y
{}
-1
-0.5
X 2
1.5
0 1
Demo_04.m 0.5
1 0
0.5
1.5 -0.5
Msc. Iván A. Calle Flores 2 -1
y(m)
Ejemplo 4. Cinemática inversa del
manipulador RRR
La tercera y cuarta solución se da
cuando el eje x1 mira hacia atrás θ3
Hallamos 3
r 2 +s2 - a22 - a32
cosθ3 = =D
2a2a3 β s
θ2
θ3 = atan2(± 1-D2 ,D) α
Hallamos 2 r z
α = atan2(s,r)
β=atan2(a3s3 , a2 +a3c3 )
θ2 = π-(α+β)
Demo_04b.m
Msc. Iván A. Calle Flores
Ejemplo 4. Cinemática inversa del
manipulador RRR
Configuraciones singulares
Si xc=yc=0, 1 no esta
definido.
Msc. Iván A. Calle Flores
Ejemplo 5. Cinemática inversa del
manipulador PUMA
Msc. Iván A. Calle Flores
Ejemplo 5. Cinemática inversa del
manipulador PUMA
Soluciones con el Soluciones con el
hombro derecho hombro izquierdo
Msc. Iván A. Calle Flores
Ejemplo 5. Cinemática inversa del
manipulador PUMA
Paso 0: Cinemática directa
link i di ai ai
1 1 d1 0 90
2 2 d2 a2 0
3 3 0 a3 0
c1c23 - c1s23 s1 c1 (a2c2 +a3c23 )+d2s1
s1c23 -s1s23 - c1 s1 (a2c2 +a3c23 )- d2c1
T3 =
0
s23 c23 0 d1 +a2s2 +a3s23
0 0 0 1
Ejemplo 5. Cinemática inversa del
manipulador PUMA
Primera solución hombro La otra solución se da
derecho mirando hacia adelante cuando el eje x1 esta
mirando para atrás
y
d2
α =atan2(y, x)
β=atan2(d2 ,r)
θ1 =α-β+π
θ1 =α+β
Msc. Iván A. Calle Flores
Ejemplo 5. Cinemática inversa del
manipulador PUMA
Hallamos 3
r 2 +s2 - a22 - a32
cosθ3 = =D a3
2a2a3 θ3
s
a2
θ3 =atan2 ± 1-D ,D 2
θ2
r
Hallamos 2
α = atan2(s,r) d1 s = z - d1
β=atan2(a3s3 , a2 +a3c3 ) r = x 2 +y 2
θ2 =α-β
Msc. Iván A. Calle Flores
Ejemplo 5. Cinemática inversa del
manipulador PUMA
La tercera y cuarta solución se da
cuando el eje x1 mira hacia atrás θ3
Hallamos 3
r 2 +s2 - a22 - a32
cosθ3 = =D
2a2a3 β s
θ2
θ3 = atan2(± 1-D2 ,D) α
Hallamos 2 r z
α = atan2(s,r)
β=atan2(a3s3 , a2 +a3c3 )
θ2 = π-(α+β)
Msc. Iván A. Calle Flores
Ejemplo 5. Cinemática inversa del
manipulador PUMA
Este robot no tiene
configuraciones singulares
Pero no puede llegar a la
posición xc=yc=0
Msc. Iván A. Calle Flores
Ejemplo 6. Cinemática inversa del
manipulador planar de 4 GDL
Y 4 X4
3 {4}
Z4
Y3
Y1 X3
2.5 Y2 Z {3}
3
Z1 Z
{1}
2 X1 Z {2} X2
2
1.5 -1
{} -0.5
1
Y
0
X 0.5
0.5
0
1.5
-1 -0.5 0 0.5 2 x(m)
1 1.5
Msc. Iván A. Calle Flores 2
Ejemplo 6. Cinemática inversa del
manipulador RRRR de 4 GDL
Nótese que para hallar 1 hay dos soluciones
y
La otra solución se da
cuando el eje x1 esta
mirando para atrás
θ1 = π+atan2 y, x
x
θ1 =atan2 y, x
Ejemplo 6. Cinemática inversa del
manipulador RRR de 4 GDL
A diferencia del
manipulador tenemos la
libertad de especificar el
ángulo final deseado
θd
Para hallar la solución el
usuario especifica las
coordenadas del punto
final, y además la
orientación final
Ejemplo 6. Cinemática inversa del
manipulador RRR de 4 GDL
Hallamos 3
rr 2 +ss2 - a22 - a32
cosθ3 = =D
2a2a3 a4
θ4
θ3 =atan2 ± 1-D2 ,D
a3 s
Hallamos 2 θ3
a2 ss
α = atan2(ss,rr)
β=atan2(a3s3 , a2 +a3c3 ) θ2
r
θ2 =α-β rr
Hallamos 4 d1 rr =r-a4cos(θd )
θ4 =θd - θ2 +θ3 ss=s-a4sin(θd )
Ejemplo 7. Cinemática inversa del
manipulador cilíndrico
Paso 0: Cinemática directa
link i di ai ai
1 1 d1 0 0
2 0 d2 0 -90
3 0 d3 0 0
c1 0 -s1 -s1d3
s 0 c1 c1d3
T3 =
0 1
0 -1 0 d1 +d2
0 0 0 1
Ejemplo 7. Cinemática inversa del
manipulador cilíndrico
θ1 =atan2 -x, y
y
d3
-x z
θ1
d2 = z-d1
d3 = x +y
2 2
Ejemplo 7. Cinemática inversa del
manipulador Stanford
Paso 0: Cinemática directa
link i di ai ai
1 1 d1 0 -90
2 2 d2 0 90
3 0 d3 0 0
Ejemplo 7. Cinemática inversa del
manipulador Stanford
d2
y
r d3
β s
θ1
x α
θ2 r
α =atan2(y, x) d1
β=atan2(d2 ,r)
θ1 =α-β θ2 = atan2 r,s
