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 