Documente Academic
Documente Profesional
Documente Cultură
CINEMATICA
INVERSA
(Primera parte)
1
Contenido
1. Cinemática
2. Cinemática Inversa
3. Métodos Algebraicos
4. Métodos Geométricos
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
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.
• Dada una matriz homogénea que representa una posición y orientación deseada
R 0
on0
Tn0 (q) n
0 1
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
Se refiere al problema de
calcular el valor de las juntas
usando las herramientas de la
geometría en el espacio.
c2 - a2 -b2
cosθ= =D
2ab θ=atan2(± 1-D ,D)
2
sinθ=± 1-D2
c b
a
FUNCION “atan2”
link i di ai ai
1 1 0 a1 0
2 2 0 a2 0
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
b. Hallando el ángulo 1
θ1 =α-β x
Resumen de formulas
En la practica el valor de
los ángulos también esta
limitado por restricciones
mecánicas.
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
Demo_01.m
Msc. Iván A. Calle Flores
Ejemplo 2. Cinemática inversa de un
manipulador de 2GDL
1 1 d1 0 pi/2
2 2 0 a2 0
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
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
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
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
θ3 =atan2 ± 1-D2 ,D d1 s = z - d1
r = x 2 +y 2
α = atan2(s,r) a2 s
β=atan2(a3s3 , a2 +a3c3 ) θ2
θ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.
link i di ai ai
1 1 d1 0 90
2 2 d2 a2 0
3 3 0 a3 0
θ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
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
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
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
link i di ai ai
1 1 d1 0 0
2 0 d2 0 -90
3 0 d3 0 0
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
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