UNIVERSIDAD NACIONAL DE TRUJILLO

Facultad de Ingeniería
Escuela Profesional de Ingeniería Mecatrónica

CINEMÁTICA INVERSA DE ROBOTS MANIPULADORES

Informe de Laboratorio
Robótica

AUTORES : Aguilar Valdiviezo, Hans.

Juarez Mercedes, Luis.

DOCENTE : Ing. Josmell Alva Alcántara

CICLO : IX

Trujillo, Perú
19 de Mayo de 2019
 Resumen

Este presente informe consiste en el reforzamiento y aplicación practica de lo realizado en

la teoría sobre cinemática inversa de robots manipuladores para obtener las variables articulares
que representan las trasformaciones necesarias de dichas articulaciones para que el efector final se
localice en la posición deseada. Para lograr esto se hace uso del software MATLAB 2018 para la
implementación según el método geométrico como también el método algebraico en un script que
condense la solución de cada uno de los ejercicios propuestos en el presente laboratorio.

Palabras clave:

Cinemática inversa, robots manipuladores, variables articulares, efector final.

 Abstract

This report consists of the reinforcement and practical application of what has been done
in the theory of inverse kinematics of manipulative robots to obtain the joint variables that
represent the necessary transformations of these articulations so that the final effector is located in
the desired position. To achieve this, MATLAB 2018 software is used for the implementation
according to the geometric method as well as the algebraic method in a script that condenses the
solution of each one of the exercises proposed in this laboratory.

Keywords:

Reverse kinematics, manipulative robots, joint variables, end effector.

 Tabla de contenido

Resumen .......................................................................................................................................... 2

Abstract ........................................................................................................................................... 3

Capítulo1 ......................................................................................................................................... 6

Introducción .................................................................................................................................... 6

Objetivos ..................................................................................................................................... 6

Objetivo General. .................................................................................................................... 6

Objetivos Específicos.............................................................................................................. 6

Materiales .................................................................................................................................... 6

Capítulo 2 ........................................................................................................................................ 7

Marco Teórico ................................................................................................................................. 7

Capítulo 3 ........................................................................................................................................ 8

Metodología .................................................................................................................................... 8

Capítulo 4 ...................................................................................................................................... 32

Conclusiones ................................................................................................................................. 32

Referencias Bibliográficas ............................................................................................................ 33

 Tabla de Figuras

Figura 1. Robot RP. ........................................................................................................................ 8

Figura 2. Espacio de trabajo de robot RP. .................................................................................... 11
Figura 3. Espacio de trabajo con part = 200 en código. ................................................................ 13
Figura 4. Espacio de trabajo con part = 7 en código. .................................................................... 13
Figura 5. Espacio de trabajo con part = 200 en código................................................................. 15
Figura 6. Espacio de trabajo con part = 7 en código. .................................................................... 15
Figura 7. Robot 3R........................................................................................................................ 16
Figura 8. Robot articular 3GDL .................................................................................................... 21
Figura 9. Configuración codo arriba y codo abajo. ...................................................................... 22
Figura 10. Robot 6GDL. ............................................................................................................... 26
Figura 11. Robot 6GDL. ............................................................................................................... 27
 Capítulo1

Introducción

Al momento de definir las variables articulares de un robot manipulador partiendo de la

posición del efector final es necesario realizar un análisis geométrico o algebraico que permitan la
obtención de las rotaciones o traslaciones que se debe hacer el robot para que el efector final se
ubique en la posición deseada.

Objetivos

Objetivo General.

- Estudiar, conocer y determinar la cinemática inversa de Robots Manipuladores.

Objetivos Específicos.

- Realizar un script en Matlab 2018 que condense la solución de los ejercicios

propuestos.
- Resolver los ejercicios con los métodos estudiados: Geométrico y Algebraíco.
- Verificar los resultados.

Materiales

- Computadora personal
- Software MATLAB R2018a
 Capítulo 2

Marco Teórico

EL problema cinemático inverso consiste en hallar aquellos valores que deberían adoptar
las coordenadas articulares q = [q1 q2 … qn]T de un robot para que su extremo se posicione y
oriente según una determinada localización espacial p = [x y z]T, R = [n o a].

A diferencia de la forma como se aborda el problema cinemático directo de manera

independiente de la configuración del robot, el problema cinemático inverso no se puede abordar
sin tomarlo en cuenta, siendo el procedimiento de la obtención de las ecuaciones muy dependiente
de la configuración del robot.

Resolución del problema cinemático inverso por métodos geométricos

Para resolver con este método se hace falta la habilidad en cuanto a la resolución de
problemas geométricos, ya que las rotaciones se expresan en ángulos y las traslaciones en
distancias

Resolución del problema cinemático inverso a partir de la matriz de transformación

homogénea

Este método de resolución consiste en el uso de las matrices de transformación homogénea

hallados con la cinemática directa haciendo uso de Denavit. Dependiendo de nuestra habilidad
operativa y algebraica resolvemos las ecuaciones necesarias entre la matriz halla por Denavit y la
matriz que tenemos de nuestro robot, asi despejando q1, q2,…,q3
 Capítulo 3

Metodología

Con el fin de estudiar y comprender el problema cinemático inverso en un robot para nos
valemos de ejercicios que se presentan a continuación. Para este estudio se hace uso de una
herramienta, un software MATLAB para lograr resolverlos.

Ejercicios

1. Considerar el robot RP mostrado en la figura 1 en el cual las variables articulares son (𝑞1, 𝑞2), y sólo
es de interés la posición del efector final P= (x, y). La cinemática directa de este robot está dada por.

𝑥 𝑙 cos 𝑞1 − 𝑞2 sin 𝑞1
𝑃 = [𝑦] = [ ]
𝑙 sin 𝑞1 + 𝑞2 cos 𝑞1

Figura 1. Robot RP.

a) Calcular de forma analítica la cinemática inversa de este robot.

Solución:

Primero se halla q2, para esto se eleva al cuadrado ambas expresiones:

 𝑥 2 = (𝑙 cos 𝑞1 − 𝑞2 sin 𝑞1 )2

𝑦 2 = (𝑙 sin 𝑞1 + 𝑞2 cos 𝑞1 )2

Luego se suman ambas expresiones:

𝑥 2 = 𝑙2 cos 2 𝑞1 − 2 sin 𝑞1 cos 𝑞1 + 𝑞22 sin2 𝑞1

𝑦 2 = 𝑙2 sin2 𝑞1 + 2 sin 𝑞1 cos 𝑞1 + 𝑞22 cos2 𝑞1

𝑥 2 + 𝑦 2 = 𝑙2 + 𝑞22

Despejando q2 se obtiene

𝑞2 = ඥ𝑥 2 + 𝑦 2 + 𝑙2

Ahora para despejar q1 tenemos que hallar una expresión a partir de las expresiones de x e
e independientemente para luego unirlas en un tangente.

Para ello primero hacemos un cambio de variable en la ecuación de x.

𝑥 = 𝑙 cos 𝑞1 − 𝑞2 sin 𝑞1
ඥ𝐴2 + 𝐵 2
𝐵
𝑦 = 𝑙 sin 𝑞1 + 𝑞2 cos 𝑞1

Donde 𝐴 = 𝑙 y 𝐵 = 𝑞2 𝐴

Reemplazando y dividiendo entre √𝐴2 + 𝐵2 en cada ecuación tenemos:

𝑥 𝐴 𝐵
= cos 𝑞1 − sin 𝑞1
√𝐴2 + 𝐵2 √𝐴2 + 𝐵2 √𝐴2 + 𝐵2

𝑦 𝐴 𝐵
= sin 𝑞1 + cos 𝑞1
√𝐴2 + 𝐵2 √𝐴2 + 𝐵2 √𝐴2 + 𝐵2

Del triángulo tenemos que

 𝑥
= cos 𝜙 cos 𝑞1 + sin 𝜙 sin 𝑞1
√𝐴2 + 𝐵2
𝑦
= cos 𝜙 sin 𝑞1 + sin 𝜙 cos 𝑞1
√𝐴2 + 𝐵2

Con la propiedad del conseno y seno de la suma de ángulos tenemos

𝑥
= cos(𝑞1 + 𝜙)
√𝐴2 + 𝐵2
𝑦
= sin(𝑞1 + 𝜙)
√𝐴2 + 𝐵2

Dividiendo estas dos ecuaciones tenemos

𝑦
= tan(𝑞1 + 𝜙)
𝑥

Resolviendo

𝑦
𝑞1 = tan−1 − 𝜙
𝑥

Y sabemos que del triángulo tenemos

𝑞2
𝜙 = tan−1
𝑙

Obteniendo y aplicando una propiedad de la tangente inversa

𝑦 𝑞2
𝑞1 = tan−1 − tan−1
𝑥 𝑙
𝑦 𝑞2
−
𝑞1 = tan−1 𝑥 𝑦 𝑙𝑞
1+ 2
𝑥 𝑙

−1
𝑦𝑙 − 𝑥ඥ𝑥 2 + 𝑦 2 + 𝑙2
𝑞1 = tan ( )
𝑥𝑙 + 𝑦ඥ𝑥 2 + 𝑦 2 + 𝑙2

b) Determinar el espacio de trabajo de este robot en el caso en el que los límites articulares son
𝑞1𝜀[−𝜋2.+𝜋2] y 𝑞2𝜀[−𝐿,+𝐿]
Solución:

Para poder determinar el espacio de trabajo del robot RP anterior en los inérvalos
determinamos hacemos uso del software MATLAB, que grafique los puntos a los que
puede acceder el robot y graficarlos, esto no da como resultado un área de trabajo. El código
que se usa es el siguiente

% Valores de ariticulaciones
q1 = -pi/2:pi/500:pi/2;
L = 1;
q2 = -L:pi/500:L;
% Puntos
[Q1,Q2]=meshgrid(q1,q2);
% Ecuaciones
x = L*cos(Q1)-Q2.*sin(Q1);
y = L*sin(Q1)+Q2.*cos(Q1);
% Grafica
plot(x(:),y(:))
daspect([1 1 1])
grid on

Figura 2. Espacio de trabajo de robot RP.

2. Considerar un robot manipulador planar 3R cuyos eslabones miden L1, L2, L3. Asumiendo
que no existen límites articulares (cada articulación tiene rotación ilimitada), determinar el
espacio de trabajo de este robot para los siguientes casos:

En este robot los ejes de las tres articulaciones de revolución son paralelos.

a) L1=1m, L2=0.4m y L3=0.3m.

Solución:

Primero para poder obtener el espacio de trabajo tenemos que hallar el vector posición del
efector final, en este caso usaremos un método geométrico para su cálculo. Se obtiene la
siguiente matriz.

𝐿1 𝑐𝑜𝑠(𝑞1 ) + 𝐿2 𝑐𝑜𝑠(𝑞1 + 𝑞2 ) + 𝐿3 𝑐𝑜𝑠(𝑞1 + 𝑞2 + 𝑞3 )

𝑝=[ ]
𝐿1 𝑠𝑖𝑛(𝑞1 ) + 𝐿2 𝑠𝑖𝑛(𝑞1 + 𝑞2 ) + 𝐿3 𝑠𝑖𝑛(𝑞1 + 𝑞2 + 𝑞3 )

Ahora de manera similar a la parte b del ejercicio anterior realizamos con ayuda de Matlab
el bosquejo del área de trabajo de este robot. Debido a que se considera sus tres
articulaciones paralelas entonces el espacio de trabajo es un área 2D. En las siguientes
imágenes se muestra el área de trabajo completo para un caso ideal en el que la precisión
de los motores es perfecta (intervalo dividido por 200 partes) y una en la que no lo son del
todo (intervalo dividido por 7 partes), para poder visualizar de una manera parcial las
trayectorias posibles.

El código correspondiente a estas graficas es:

% Numero de particiones del rango

part = 7
% Intervalos de las articulaciones
q1 = 0:pi/part:2*pi;
q2 = 0:pi/part:2*pi;
q3 = 0:pi/part:2*pi;
% Valores de las articulaciones
l1 = 1;
l2 = 0.4;
l3 = 0.3;
% Puntos
[Q1,Q2,Q3]=meshgrid(q1,q2,q3);
% Ecuaciones
x = l1*cos(Q1)+l2*cos(Q1+Q2)+l3*cos(Q1+Q2+Q3);
y = l1*sin(Q1)+l2*sin(Q1+Q2)+l3*sin(Q1+Q2+Q3);
% Grafica
plot(x(:),y(:))
daspect([1 1 1])
grid on

Y las gráficas que se obtienen son:

Figura 3. Espacio de trabajo con part = 200 en código.

Figura 4. Espacio de trabajo con part = 7 en código.

b) L1=0.5m, L2=0.7m, and L3=0,5m.

Solución:

De manera análoga al inciso a se reemplaza los valores en la ecuación

𝐿1 𝑐𝑜𝑠(𝑞1 ) + 𝐿2 𝑐𝑜𝑠(𝑞1 + 𝑞2 ) + 𝐿3 𝑐𝑜𝑠(𝑞1 + 𝑞2 + 𝑞3 )

𝑝=[ ]
𝐿1 𝑠𝑖𝑛(𝑞1 ) + 𝐿2 𝑠𝑖𝑛(𝑞1 + 𝑞2 ) + 𝐿3 𝑠𝑖𝑛(𝑞1 + 𝑞2 + 𝑞3 )

Y se plotean las gráficas, una representando el área de trabajo y otra representando algunas
trayectorias.
El código correspondiente a estas graficas es:

% Numero de particiones del rango

part = 7
% Intervalos de las articulaciones
q1 = 0:pi/part:2*pi;
q2 = 0:pi/part:2*pi;
q3 = 0:pi/part:2*pi;
% Valores de las articulaciones
l1 = 0.5;
l2 = 0.7;
l3 = 0.5;
% Puntos
[Q1,Q2,Q3]=meshgrid(q1,q2,q3);
% Ecuaciones
x = l1*cos(Q1)+l2*cos(Q1+Q2)+l3*cos(Q1+Q2+Q3);
y = l1*sin(Q1)+l2*sin(Q1+Q2)+l3*sin(Q1+Q2+Q3);
% Grafica
plot(x(:),y(:))
daspect([1 1 1])
grid on

Y las gráficas que se obtienen son:

Figura 5. Espacio de trabajo con part = 200 en código.

Figura 6. Espacio de trabajo con part = 7 en código.

3. Se tiene un robot 3R, mostrado en la figura 3 (notar que la configuración de este robot es
diferente de la configuración del robot de la pregunta 2).

Figura 7. Robot 3R.

La cinemática directa del efector final de este robot con respecto a la base se describe a través de la
siguiente matriz de transformación homogénea.
cos 𝜃1 cos(𝜃2 + 𝜃3 ) − cos 𝜃1 sin(𝜃2 + 𝜃3 ) sin 𝜃1 (𝐿2 cos 𝜃2 + 𝐿1 ) cos 𝜃1
0 sin 𝜃1 cos(𝜃2 + 𝜃3 ) − sin 𝜃1 sin(𝜃2 + 𝜃3 ) − cos 𝜃1 (𝐿2 cos 𝜃2 + 𝐿1 ) sin 𝜃1
𝑇3 = [ ]
sin(𝜃2 + 𝜃3 ) cos(𝜃2 + 𝜃3 ) 0 sin 𝜃2 𝐿2
0 0 0 1
Suponer que la siguiente matriz de transformación homogénea representa la posición y la orientación
deseadas para el efector final.
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑃𝑥
𝑛 𝑜𝑦 𝑎𝑦 𝑃𝑦
𝑇𝑠𝑒𝑡𝑃𝑜𝑖𝑛𝑡 =[ 𝑦 ]
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑃𝑧
0 0 0 1
Obtener de forma analítica la cinemática inversa de ese robot.

Solución:

El robot 3R mostrado consiste de dos eslabones L1 y L2 y tres articulaciones q1, q2 y q3. Y

de la matriz de transformación homogénea podemos despejar los valores de q1 y q2 a partir de
la posición del efector final.
 𝑥 cos 𝑞1 (𝐿1 + 𝐿2 ∗ cos 𝑞2 )
𝑝 = [𝑦] = [ sin 𝑞1 (𝐿1 + 𝐿2 ∗ cos 𝑞2 ) ]
𝑧 𝐿2 sin 𝑞2
A partir de ello se tiene las siguientes ecuaciones
𝑥 = cos 𝑞1 (𝐿1 + 𝐿2 ∗ cos 𝑞2 )
𝑦 = sin 𝑞1 (𝐿1 + 𝐿2 ∗ cos 𝑞2 )
𝑧 = 𝐿2 sin 𝑞2

Primero hallar q1 dividiendo y por x.

𝑦 sin 𝑞1
= = tan 𝑞1
𝑥 cos 𝑞1
Obteniendo
𝑦
𝑞1 = tan−1 ( )
𝑥
Se eleva x e y al cuadrado y se suman ambas ecuaciones correspondientes
𝑥 2 + 𝑦 2 = (sin 𝑞1 2 + cos 𝑞1 2 )(𝐿1 + 𝐿2 ∗ cos 𝑞2 )2
𝑥 2 + 𝑦 2 = (𝐿1 + 𝐿2 ∗ cos 𝑞2 )2
Despegando q2 se tiene
ඥ𝑥 2 + 𝑦 2 − 𝐿1
cos 𝑞2 = ⋯ (1)
𝐿2
De la relación de la tangente con el coseno se tiene
ඥ1 − cos 𝑞2
tan 𝑞2 =
cos 𝑞2

ඥ1 − cos 𝑞2
𝑞2 = tan−1 ( )
cos 𝑞2

Además, se puede obtener una expresión de q2 a partir de la ecuación en z

𝑧 = 𝐿2 sin 𝑞2
Obteniendo
𝑧
sin 𝑞2 = ⋯ (2)
𝐿2
De las ecuaciones (1) y (2)
 𝑧
tan 𝑞2 =
ඥ𝑥 2 + 𝑦 2 − 𝐿1
Obteniendo
𝑧
𝑞2 = tan−1 ( )
ඥ𝑥 2 + 𝑦 2 − 𝐿1

Ahora para poder obtener el valor de q3 hacemos uso de la matriz de rotación noa de los
elementos (3,1) y (3,2) de las matrices
0
𝑇3 (3,1) = sin(𝑞2 + 𝑞3 ) = 𝑛𝑧
0
𝑇3 (3,2) = cos(𝑞2 + 𝑞3 ) = 𝑜𝑧
De lo cual podemos dividir (3,1) por (3,2)
𝑛𝑧 sin(𝑞2 + 𝑞3 )
= = tan(𝑞2 + 𝑞3 )
𝑜𝑧 cos(𝑞2 + 𝑞3 )
𝑛𝑧
𝑞2 + 𝑞3 = tan−1 ( )
𝑜𝑧
Obteniéndose el valor de q3
𝑛𝑧
𝑞3 = tan−1 ( ) − 𝑞2
𝑜𝑧
Reemplazando q2 tenemos
𝑛𝑧 𝑧
𝑞3 = tan−1 ( ) − tan−1 ( )
𝑜𝑧 ඥ𝑥 + 𝑦2 − 𝐿1
2

𝑛𝑧 𝑧
−
𝑜𝑧 ඥ𝑥2 + 𝑦2 − 𝐿
1
𝑞3 = tan−1 ( 𝑛𝑧 𝑧 )
1+
𝑜𝑧 ඥ𝑥2 + 𝑦2 − 𝐿
1

𝑛𝑧 (ඥ𝑥2 + 𝑦2 − 𝐿1 ) − 𝑧𝑜𝑧
−1
𝑞3 = tan ( )
𝑜𝑧 (ඥ𝑥2 + 𝑦2 − 𝐿1 ) + 𝑧𝑛𝑧

4. Se tiene un manipulador cuya cinemática directa está representada por los siguientes
parámetros DH.

Art. 𝑑𝑖 𝜃𝑖 𝑎𝑖 𝛼𝑖
1 0 𝑞1 d1 -90°
2 𝑞2 -90° 0 -90°
3 𝑞3 90° 0 -90°
 La matriz de transformación homogénea final asociada a estos parámetros es:
sin 𝑞1 − cos 𝑞1 0 𝑞3 cos 𝑞1 − 𝑞2 sin 𝑞1 + 𝑑1 cos 𝑞1
0
𝑇3 = [− cos 𝑞1 − sin 𝑞1 0 𝑞3 sin 𝑞1 + 𝑞2 cos 𝑞1 + 𝑑1 sin 𝑞1 ]
0 0 −1 0
0 0 0 1
a) Calcular la cinemática inversa para este robot de manera analítica. Se puede usar la matriz
dada, que indica posición y orientación deseada.

Solución:

Suponiendo una matriz setpoint de

𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑥
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑦
𝑇𝑠𝑝 = [𝑛 𝑜𝑧 𝑎𝑧 𝑧]
𝑧
0 0 0 1

Primero se halla q1 haciendo uso de la matriz noap de los cuales nos valdremos de los
elementos (1,2) y (2,2)

0
𝑇3 (1,2) = − cos 𝑞1 = 𝑜𝑥
0
𝑇3 (2,2) = − sin 𝑞1 = 𝑜𝑦

Se puede dividir (2,2) por (1,2)

𝑜𝑦 − sin 𝑞1
= = tan(𝑞1 )
𝑜𝑥 − cos 𝑞1

Obteniendo

𝑜𝑦
𝑞1 = tan−1 ( )
𝑜𝑥

De la relación entre matrices de transformación homogénea halladas por Denavit

Hartenberg tenemos

cos 𝑞1 0 − sin 𝑞1 𝑑1 cos 𝑞1

0 sin 𝑞1 0 cos 𝑞1 𝑑1 sin 𝑞1
𝑇1 = [ ]
0 −1 0 0
0 0 0 1
 0 0 1 0 0 0 −1 0
1 −1 0 0 0 2 1 0 0 0
𝑇2 = [
0 −1 0 𝑞2 ] , 𝑇3 = [0 −1 0 𝑞3 ]
0 0 0 1 0 0 0 1

Además, multiplicando 1T2 2T3

0 −1 0 𝑞3
1 2 0 0 1 0
𝑇2 𝑇3 = 1𝑇3 = [ ]
−1 0 0 𝑞2
0 0 0 1

De lo anterior se puede obtener la siguiente relación

( 0𝑇1 )−1 𝑇𝑠𝑝 = 1𝑇2 2𝑇3

−1 0 −1 0 𝑞3
cos 𝑞1 0 − sin 𝑞1 𝑑1 cos 𝑞1 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑥
sin 𝑞1 0 cos 𝑞1 𝑑1 sin 𝑞1 𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑦 0 0 1 0
[ ] [𝑛 𝑜𝑧 𝑎𝑧 𝑧 ] = [−1 0 ]
0 −1 0 0 𝑧 0 𝑞2
0 0 0 1 0 0 0 1 0 0 0 1

cos 𝑞1 sin 𝑞1 0 𝑑1 𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑥 0 −1 0 𝑞3
0 0 −1 0 𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑦 0 0 1 0
[ ][ 𝑧 ] = [−1 ]
− sin 𝑞1 cos 𝑞1 0 0 𝑛𝑧 𝑜𝑧 𝑎𝑧 0 0 𝑞2
0 0 0 1 0 0 0 1 0 0 0 1

Puesto que esta es una igualdad podemos partir de la equivalencia escoger dos elementos de la
matriz resultante, (1,4) y (3,4)

𝑥 cos 𝑞1 + 𝑦 sin 𝑞1 + 𝑑1 = 𝑞3

𝑦 cos 𝑞1 − 𝑥 sin 𝑞1 = 𝑞2

Tomando las ecuaciones anteriores se halla el valor de q2 y q3 reemplazando el valor de q1

𝑜𝑦 𝑜𝑦
𝑞2 = 𝑦 cos (tan−1 ( )) − 𝑥 sin (tan−1 ( ))
𝑜𝑥 𝑜𝑥

𝑜𝑦 𝑜𝑦
𝑞3 = 𝑥 cos (tan−1 ( )) + 𝑦 sin (tan−1 ( )) + 𝑑1
𝑜𝑥 𝑜𝑥

Compactando las expresiones anteriores

𝑜𝑦 𝑥
𝑞2 = ඥ𝑥 2 + 𝑦 2 cos (tan−1 ( ) + tan−1 ( ))
𝑜𝑥 𝑦
 𝑜𝑦 𝑦
𝑞3 = ඥ𝑥 2 + 𝑦 2 cos (tan−1 ( ) − tan−1 ( )) + 𝑑1
𝑜𝑥 𝑥

Por propiedad trigonométrica del arco tangente obtenemos

𝑥𝑜𝑥 + 𝑦𝑜𝑦
𝑞2 = ඥ𝑥 2 + 𝑦 2 cos (tan−1 ( ))
𝑦𝑜𝑥 − 𝑥𝑜𝑦

𝑥𝑜𝑦 − 𝑦𝑜𝑥
𝑞3 = ඥ𝑥 2 + 𝑦 2 cos (tan−1 ( ))
𝑥𝑜𝑥 + 𝑦𝑜𝑦

5. Desarrollar un algoritmo en Matlab, que me permita resolver la cinemática inversa de robot de 3GDL
que se muestra en la figura 3 (desarrollado en clase), tener en cuenta las duplas en la solución. (la
136 FUNDAMENTOS DE ROBÓTICA
solución puede ser por el método geométrico o algebraico).

l3

Z0 l2 pz

Y0
X0 r
px
py

q1

Figura
Figura4.9. Robot
8. Robot articular.
articular 3GDL

Considerar.

Por último,
L1=0.8m,siL2se consideran
=0.4m, L3 =0.2m. robots con capacidad de posicionar y orientar su extremo en
el espacio, esto es, robots con 6 GDL, el método de desacoplamiento cinemático permite,
para determinados tipos de robots, resolver los primeros grados de libertad, dedicados al po-
sicionamiento, de manera independiente a la resolución de los últimos grados de libertad, de-
dicados a la orientación. Cada uno de estos dos problemas más simples podrá ser tratado y re-
suelto por cualquiera de los procedimientos anteriores.
Solución:

Método geométrico

Según se muestra en la figura 8 podemos analizar el plano horizontal de la cual se obtiene directamente

𝑦
𝑞1 = tan−1 ( )
𝑥

Ahora considerando únicamente los eslabones 2 y 3 posicionando el sistema de coordenadas inicial en

la segunda articulación queda analizar la siguiente figura

Figura 9. Configuración codo arriba y codo abajo.

De la figura por inspección se puede determinar las siguientes relaciones

𝑟2 = 𝑥2 + 𝑦2

𝑟 2 + 𝑧 2 = 𝐿22 + 𝐿23 + 2𝐿2 𝐿3 cos 𝑞3

𝑥 2 + 𝑦 2 + 𝑧 2 − 𝐿22 − 𝐿23
cos 𝑞3 =
2𝐿2 𝐿3

Expresando en expresiones de arcotangente tenemos

−1
±ඥ1 − cos2 𝑞3
𝑞3 = tan ( )
cos 𝑞3

También de la figura 9 se sabe que

𝑞2 = 𝛽 − 𝛼
Determinando directamente

𝑧 𝑧
𝛽 = tan−1 = tan−1 ( )
𝑟 ±ඥ𝑥 2 + 𝑦 2

Para hallar 𝛼 aplicamos la ley de senos y cosenos

𝐿3 sin 𝑞3
sin 𝛼 =
±ඥ𝑥 2 + 𝑦 2

𝑥 2 + 𝑦 2 + 𝐿22 − 𝐿23
cos 𝛼 =
2𝐿1 ඥ𝑥 2 + 𝑦 2

𝐿3 sin 𝑞3
tan 𝛼 =
𝐿3 cos 𝑞3 + 𝐿2

𝐿3 sin 𝑞3
𝛼 = tan−1 ( )
𝐿3 cos 𝑞3 + 𝐿2

Para luego finalmente obtener el valor de q2

𝑧 𝐿3 sin 𝑞3
𝑞2 = tan−1 ( ) − tan−1 ( )
±ඥ𝑥 2 + 𝑦 2 𝐿3 cos 𝑞3 + 𝐿2

Sin embargo, los resultados de q1, q2 y q3 se hallaron para un sistema de referencia en la

articulación 2, por esto se traslada hacia la primera articulación todo el sistema siendo esto un
desplazamiento negativo en z de L1 obteniendo las ecuaciones finales como.

𝑦
𝑞1 = tan−1 ( )
𝑥

𝑥 2 + 𝑦 2 + (𝑧 − 𝐿1 )2 − 𝐿22 − 𝐿23
cos 𝑞3 =
2𝐿2 𝐿3

±ඥ1 − cos2 𝑞3
𝑞3 = tan−1 ( )
cos 𝑞3

𝑧 − 𝐿1 𝐿3 sin 𝑞3
𝑞2 = tan−1 ( ) − tan−1 ( )
±ඥ𝑥 2 + 𝑦 2 𝐿3 cos 𝑞3 + 𝐿2
Ahora teniendo los resultados de q1, q2 y q3 y reemplazando los valores de L1=0.8m, L2=0.4m,
L3 =0.2m resulta lo siguiente

𝑦
𝑞1 = tan−1 ( )
𝑥

𝑥 2 + 𝑦 2 + 𝑧 2 − 1.6𝑧 + 0.44
cos 𝑞3 =
2𝐿2 𝐿3

±ඥ1 − cos2 𝑞3
𝑞3 = tan−1 ( )
cos 𝑞3

𝑧 − 0.8 0.2 sin 𝑞3

𝑞2 = tan−1 ( ) − tan−1 ( )
±ඥ𝑥 2 + 𝑦 2 0.2 cos 𝑞3 + 0.4

Notar que estos resultados reflejan ambos resultados de la dupla, es decir dependiendo del signo de la
raíz hace referencia a un sistema de codo arriba o codo abajo. En este caso el signo positivo es codo
arriba mientras que el signo negativo es codo abajo.

Código en Matlab

clc, clear, close all

disp('EJERCICIO 5')
% CINEMATICA INVERSA
disp(' ')
disp('CINEMATICA INVERSA')
disp(' ')
% Datos
x = input('Ingrese posicion en X: ');
y = input('Ingrese posicion en Y: ');
z = input('Ingrese posicion en Z: ');
L1 = 0.8;
L2 = 0.4;
L3 = 0.2;
pi = 3.1415926535897932384626433832795;
codo = input('Posicion de codo [arriba:1/abajo:0]: ');
sign = -(-1)^(codo); % Codo arriba o codo abajo
% Ecuaciones
q1 = atan(y/x);
Cosq3 = (x^2+y^2+(z-L1)^2-L2^2-L3^2)/(2*L2*L3);
Senq3 = sign*sqrt(1-Cosq3^2);
q2 = atan((z-L1)/(sign*sqrt(x^2+y^2)))-atan((L3*Senq3)/(L3*Cosq3+L2));
q3 = atan(sign*Senq3/Cosq3);
% En sexagesimales
q1 = q1*180/pi
q2 = q2*180/pi
q3 = q3*180/pi

%Comprobacion por cinematica directa.

% CINEMATICA DIRECTA
disp(' ')
disp('CINEMATICA DIRECTA')
disp(' ')
q1 = q1*pi/180;
q2 = q2*pi/180;
q3 = q3*pi/180;
disp('Paramámetros Denavit Hartenberg.')
% Parámetros DH
theta = [q1;q2;q3];
d = [L1;0;0];
a = [0;L2;L3];
alpha = [pi/2;0;0];
MDH = [theta d a alpha] % Matriz DH
% Cálculo de la cinematica directa.
T01 = (denavit(theta(1),d(1),a(1),alpha(1)));
T12 = (denavit(theta(2),d(2),a(2),alpha(2)));
T23 = (denavit(theta(3),d(3),a(3),alpha(3)));
disp('Posicion del efector final')
T03 = (T01*T12*T23);
P = ([T03(1,4);T03(2,4);T03(3,4)])
6. Se tiene un robot de 6GD como se muestra en la figura 4, determinar la cinemática inversa por
el método de desacoplo cinemático e implementar en Matlab.

Figura 10. Robot 6GDL.

Tener en cuenta las duplas en la solución de la cinemática inversa.

Tomar las siguientes dimensiones para el robot.

L1= 0.4 m, L2=0.7m, L3=0.5m, L4=0.10m

Solución

Primero tenemos que realizar la cinemática directa del robot de 6GDL. Para ello encontramos
la matriz DH del robot
 X2
Y2
X4
Y4
Z2

Z4

Y1

Z1 X1
Z0

X0 Y0

Figura 11. Robot 6GDL.

Art. 𝜃𝑖 d𝑖 𝑎𝑖 𝛼𝑖
1 q1+90 L1 0 90°
2 q2+90 0 L2 0
3 q3 0 0 90°
4 q4 L3 0 -90°
5 q5 0 0 90°
6 q6 L4 0 0

Primero tenemos que definir la posición en la muñeca como el punto donde se cortan los ejes
𝑧4 , 𝑧4 y 𝑧4 que coincide con el sistema 𝑆3 . Esto permite tratar desde el sistema 𝑆0 hatsa el
sistema 𝑆3 como un robot de 3GLD.

𝑝𝑚𝑥
𝑝𝑚 = [𝑝𝑚𝑦 ]
𝑝𝑚𝑧
Con estos parámetros haciendo uso de las ecuaciones deducidas para un robot de 3GLD
anteriormente se puede definir q1, q2 y q3

𝑝𝑚𝑦
𝑞1 + 90° = tan−1 ( )
𝑝𝑚𝑥

𝑝𝑚𝑦
𝑞1 = tan−1 ( ) − 90°
𝑝𝑚𝑥

𝑝𝑚𝑥 2 + 𝑝𝑚𝑦 2 + (𝑝𝑚𝑧 − 𝐿1 )2 − 𝐿22 − 𝐿23

cos 𝑞3 =
2𝐿2 𝐿3

−1
±ඥ1 − cos2 𝑞3
𝑞3 = tan ( )
cos 𝑞3

𝑝𝑚𝑧 − 𝐿1 𝐿3 sin 𝑞3
𝑞2 + 90° = tan−1 + tan−1 ( )
𝐿3 cos 𝑞3 + 𝐿2
±√𝑝𝑚𝑥 2 + 𝑝𝑚𝑦 2
( )

𝑝𝑚𝑧 − 𝐿1 𝐿3 sin 𝑞3
𝑞2 = tan−1 + tan−1 ( ) − 90°
𝐿3 cos 𝑞3 + 𝐿2
±√𝑝𝑚𝑥 2 + 𝑝𝑚𝑦 2
( )

De la muñeca al efector final se representa por la siguiente ecuación

𝒑𝒎 = 𝒑 − 𝐿4 𝒛𝟒

donde 𝑝 = [𝑥 𝑦 𝑧 ]𝑇

Ahora analizando la rotación entre los sistemas 3 y 6 sabemos que

0 𝑜 𝑎] = 0𝑅3 3𝑅6
𝑅6 = [𝑛

3 −1 0
𝑅6 = 0𝑅3 𝑅6

3 𝑇
𝑅6 = 0𝑅3 [𝑛 𝑜 𝑎]

Haciendo uso de Matlab podemos determinar la matriz 3𝑅6

3
𝑅6
cos 𝑞4 cos 𝑞5 cos 𝑞6 − sin 𝑞4 sin 𝑞6 − cos 𝑞6 sin 𝑞4 − cos 𝑞4 cos 𝑞5 sin 𝑞6 cos 𝑞4 sin 𝑞5
= [cos 𝑞4 sin 𝑞6 + cos 𝑞5 cos 𝑞6 sin 𝑞4 cos 𝑞4 cos 𝑞6 − cos 𝑞5 sin 𝑞4 sin 𝑞6 sin 𝑞4 sin 𝑞5 ]
− cos 𝑞6 sin 𝑞5 sin 𝑞5 sin 𝑞6 cos 𝑞5

Ademas tenemos que

𝑟11 𝑟12 𝑟13

3 𝑟
𝑅6 = [𝑟𝑖𝑗 ] = [ 21 𝑟22 𝑟23 ]
𝑟31 𝑟32 𝑟33

𝑟11 𝑟12 𝑟13 𝑛𝑥 𝑜𝑥 𝑎𝑥

𝑇
[𝑟21 𝑟22 𝑟23 ] = 𝑅3 [𝑛𝑦
0 𝑜𝑦 𝑎𝑦 ]
𝑟31 𝑟32 𝑟33 𝑛𝑧 𝑜𝑧 𝑎𝑧

Tomando el elemento (3,3) obtenemos q5, los elementos (1,3) y (2,3) y dividiéndolos
obtenemos q4 y por último q6 obtenemos de (3,2) y (3,1) dividiendolos

𝑟23
𝑞4 = tan−1
𝑟13

𝑞5 = cos −1 𝑟33

𝑟32
𝑞6 = tan−1
𝑟31

Codigo en Matlab:

clc, clear, close all

disp('EJERCICIO 5')

% CINEMATICA DIRECTA
disp(' ')
disp('CINEMATICA DIRECTA')
disp(' ')
% Datos de entrada
q1 = input('Ingrese valor de q1: ');
q2 = input('Ingrese valor de q2: ');
q3 = input('Ingrese valor de q3: ');
q4 = input('Ingrese valor de q4: ');
q5 = input('Ingrese valor de q5: ');
q6 = input('Ingrese valor de q6: ');
l1 = 0.4;
l2 = 0.7;
l3 = 0.5;
l4 = 0.1;
q1 = pi/180*(q1);
q2 = pi/180*(q2);
q3 = pi/180*(q3);
q4 = pi/180*(q4);
q5 = pi/180*(q5);
q6 = pi/180*(q6);
disp('Paramámetros Denavit Hartenberg.')
% Parámetros DH
theta=[q1;q2;q3+pi/2;q4;q5;q6];
d=[l1;0;0;l3;0;l4];
a=[0;l2;0;0;0;0];
alpha=[pi/2;0;pi/2;-pi/2;pi/2;0];
DH = [theta d a alpha]
% Cálculo de la cinematica directa.
T01 = denavit(theta(1),d(1),a(1),alpha(1));
T12 = denavit(theta(2),d(2),a(2),alpha(2));
T23 = denavit(theta(3),d(3),a(3),alpha(3));
T34 = denavit(theta(4),d(4),a(4),alpha(4));
T45 = denavit(theta(5),d(5),a(5),alpha(5));
T56 = denavit(theta(6),d(6),a(6),alpha(6));

T03 = T01*T12*T23*T34*T45*T56

R = [T03(1,1) T03(1,2) T03(1,3);

T03(2,1) T03(2,2) T03(2,3);
T03(3,1) T03(3,2) T03(3,3)]

P = [T03(1,4);T03(2,4);T03(3,4)]

% CINEMATICA INVERSA
disp(' ')
disp('CINEMATICA INVERSA')
disp(' ')
% Datos de entrada
Rot = input('Ingrese matriz de rotación: ');
Pos = input('Ingrese vector de posición: ');
% Elementos noa
nx = Rot(1,1);
ny = Rot(2,1);
nz = Rot(3,1);
ox = Rot(1,2);
oy = Rot(2,2);
oz = Rot(3,2);
ax = Rot(1,3);
ay = Rot(2,3);
az = Rot(3,3);
% posicion del efector final
px = Pos(1);
py = Pos(2);
pz = Pos(3);
% relacion de muñeca a efector final
pmx = px-l4*ax;
pmy = py-l4*ay;
pmz = pz-l4*az;
% Ecuaciones
q1 = atan(pmy/pmx);
c3 = (pmx^2+pmy^2+(pmz-l1)^2-l2^2-l3^2)/(2*l2*l3);
q3 = atan(sqrt(1-c3^2)/c3);
q2 = atan((pmz-l1)/sqrt(pmx^2+pmy^2))-atan((l3*sin(q3))/(l2+l3*c3));
c5 = cos(q1)*cos(q2+q3)*ax+sin(q1)*cos(q2+q3)*ay+sin(q2+q3)*az;
q5 = atan(sqrt(1-c5^2)/c5);
q4 = atan(sin(q1)*ax-cos(q1)*ay)/(-cos(q1)*sin(q2+q3)*ax-
sin(q1)*sin(q2+q3)*ay+cos(q2+q3)*az);
q6 = atan(-
(cos(q1)*cos(q2+q3)*ox+sin(q1)*cos(q2+q3)*oy+sin(q2+q3)*oz)/(cos(q1)*cos(q
2+q3)*nx+sin(q1)*cos(q2+q3)*ny+sin(q2+q3)*nz));
% Salidas
q1 = 180/pi*(q1)
q2 = 180/pi*(q2)
q3 = 180/pi*(q3)
q4 = 180/pi*(q4)
q5 = 180/pi*(q5)
q6 = 180/pi*(q6)
 Capítulo 4

Conclusiones

- Se estudiaron y aplicaron la cinemática inversa adecuadamente.

- Se logró realizar correctamente un script en Matlab 2018 que condensa la solución de los
ejercicios propuestos.
 Referencias Bibliográficas

[1] Barrientos, A. (2da Edición). (2007). Fundamentos de Robótica. Madrid, España: McGraw-
Hill/Interamericana.

[2] Reyes, F. (1era Edición). (2011). Robótica, control de robots manipuladores. C. V., México:
Alfaomega Grupo Editor.

[3] Corke, P. (2da Edición). (2017). Robotics, Vision and Control. Berlín, Alemania: Springer