UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA

PRACTICA N° 01

CURSO: Análisis y Control de Robots (MT517)

PROFESOR: Calle Flores, Ivan Arturo
ALUMNOS: BERRIOS ROJAS, William 20140004C
CASTRO MARTINES, Alex 20134010E
HURTADO DUAREZ, Johan Antonio 20093501J

LÓPEZ RUMRRILL, Jorge Alejandro 20144052B

SECCION: “A"

2017 - II
 PROYECTO DEL CURSO ANALISIS Y CONTROL DE
ROBOTS

1. DISEÑO MECANICO

Para la realización del brazo de 2GDL se ha decidido usar como material de los
links el acrílico de 4cm de espesor. Las razones de la elección de este material
son que este es ligero, presenta un costo relativamente bajo y se adapta a
nuestros requerimientos para el diseño.

Las medidas estimadas para el tamaño del brazo son de 13cm x 4cm el primer
link y de 8cm x 4cm para el segundo link, con esto podemos hacer los cálculos
para ver el torque mínimo requerido para los motores.

1.1 MOTORES SELECCIONADOS

El motor usado para la primera junta tiene que ser un motor que posea un torque
alto para que pueda soportar la estructura de todo el brazo, mediante cálculos a
priori que se realizó, se determinó que el torque del motor debía ser mayor o
igual a 4Kg-cm.

Para el motor de la junta 2, no se requiere un torque alto, pero si se requerirá

que tenga un torque suficiente para poder mover al link 2, por lo tanto, se
requerirá un torque mínimo de 0.6Kg-cm.

Así también se requiere que los motores tengan encoder de 2 canales para que
sea posible poder controlar la posición del brazo de 2GDL en todo momento. Así
mismo se requerirá que las RPM que entregaran las cajas reductoras de los
motores sean bajas (menores a 200RPM).

En base a estos requerimientos se realizó la selección y compra de los motores.

A continuación, detallaremos las especificaciones y planos de los motores.
1.1.1 MOTOR JUNTA 1

Nombre: 75:1 Metal Gearmotor 25Dx54L mm LP 6V with 48 CPR Encoder

El motor con reductor de velocidad y encoder presenta las siguientes

características: trabaja a 6v, posee un torque de 6.8 Kg-cm y 78RPM.
Además, presenta un encoder de efecto Hall, el cual tiene una resolución de
48 cuentas por revolución. También se muestra las especificaciones
brindadas por el fabricante.

Figura 1.Motor1 adquirido para el proyecto

Tabla 1. Características del motor1

 Figura 2. Planos del motor 1 brindado por el fabricante

1.1.2 MOTOR JUNTA 2

Nombre: 50:1 Micro Metal Gearmotor MP 6V with 48 CPR Encoder

El motor con reductor de velocidad y encoder presenta las siguientes

características: trabaja a 6v, posee un torque de 1.0 Kg-cm y 420 RPM.
Además, presenta un encoder de efecto Hall, el cual tiene una resolución de
48 cuentas por revolución. También se muestra las especificaciones
brindadas por el fabricante.
Figura 3.Motor2 adquirido para el proyecto

Tabla 2. Características del motor 2

Figura 4. Planos del motor 2 brindado por el fabricante
1.2 DISEÑO EN SOLIDWORKS

A continuación, se muestra el diseño del brazo de 2GDL:

1.2.1 MOTOR JUNTA 1

Figura 5. Diseño en Solidworks del motor 1

1.2.2 MOTOR JUNTA 2

Figura 6. Diseño en Solidworks del motor 2

1.2.3 ROBOT PLANAR (VISTAS)

Figura 7. Vista isométrica del robot planar

Figura 8. Vista frontal del robot planar

 Figura 9. Vista lateral del robot planar

a) LINK 1

Figura 10. Diseño del link 1 en solidworks

b) LINK 2

Figura 11. Diseño del link 2 en solidworks

c) SOPORTE DEL SEGUNDO MOTOR

Figura 12. Diseño del soporte 2 en solidworks

1.3 PLANOS DE DISEÑO
a) LINKS 1 Y 2

Figura 13. Planos de los links 1 y 2

b) SOPORTE DEL MOTOR 1

Figura 14. Soporte del motor 1

c) SOPORTE DEL MOTOR2

Figura 15. Planos del soporte del motor 2

d) MOTOR 1

Figura 16. Planos del motor 1

 e) MOTOR 2

Figura 17. Planos del motor 2

2. CINEMATICA DIRECTA
2.1 DETERMINACIÓN DE LOS SISTEMAS COORDENADOS

Figura 18. Sistemas coordenados del robot planar

2.2 DETERMINACIÓN DE LOS PARÁMETROS DE DENAVIT HARTENBERG

JUNTA Өi di ai αi

1 Ө1 L2 L1 0

2 Ө2 0 L3 0

2.3 DETERMINACIÓN DE LAS MATRICES DE TRANSFORMACIÓN:

𝐴𝑖−1
𝑖 (𝑞𝑖 ) = 𝑅𝑜𝑡𝑧,𝜃𝑖 . 𝑇𝑟𝑎𝑛𝑠𝑧,𝑑𝑖 . 𝑇𝑟𝑎𝑛𝑠𝑥,𝑎𝑖 . 𝑅𝑜𝑡𝑥,𝑎𝑖

C 𝜃𝑖 −𝑆𝜃𝑖 . 𝐶𝛼𝑖 𝑆𝜃𝑖 . 𝑆𝛼𝑖 𝑎𝑖 . 𝐶𝜃𝑖

𝑆𝜃𝑖 𝐶𝜃𝑖 . 𝐶𝛼𝑖 −𝐶𝜃𝑖 . 𝑆𝛼𝑖 𝑎𝑖 . 𝑆𝜃𝑖
𝐴𝑖−1
𝑖 (𝑞𝑖 ) = [ ]
0 𝑆𝛼𝑖 𝐶𝛼𝑖 𝑑𝑖
0 0 0 1

Hallando las matrices homogéneas:

cos 𝜃1 −𝑠𝑖𝑛𝜃1 0 𝐿1 . 𝑐𝑜𝑠𝜃1

𝑠𝑖𝑛𝜃1 𝑐𝑜𝑠𝜃1 0 𝐿1 . 𝑠𝑖𝑛𝜃1
𝑇10 = [ ]
0 0 1 𝐿2
0 0 0 1

cos(𝜃1 + 𝜃2 ) −sin(𝜃1 + 𝜃2 ) 0 𝐿3 . 𝑐𝑜𝑠(𝜃1 + 𝜃2 ) + 𝐿2 . 𝑐𝑜𝑠𝜃1

sin(𝜃1 + 𝜃2 ) cos(𝜃1 + 𝜃2 ) 0 𝐿3 . 𝑠𝑖𝑛(𝜃1 + 𝜃2 ) + 𝐿2 . 𝑠𝑖𝑛𝜃1
𝑇20 = [ ]
0 0 1 𝐿2
0 0 0 1
3. CINEMATICA INVERSA

𝐶𝑎𝑙𝑐𝑢𝑙𝑎𝑛𝑑𝑜 𝜃2 𝑝𝑜𝑟 𝑒𝑙 𝑡𝑒𝑜𝑟𝑒𝑚𝑎 𝑑𝑒 𝑐𝑜𝑠𝑒𝑛𝑜𝑠

𝑥 2 + 𝑦 2 − 𝐿1 2 − 𝐿3 2
𝑐𝑜𝑠𝜃2 = =𝐷
2𝐿1 . 𝐿3

±√1 − 𝐷2
𝜃2 = tan−1
𝐷

𝑦 𝐿3 . sin 𝜃2
𝜃1 = tan−1 ( ) − tan−1 ( )
𝑥 𝐿1 + 𝐿3 . cos 𝜃2

4. CODIGOS EN MATLAB

4.1 CINEMATICA DIRECTA

%CINEMATICA DEL ROBOR ROBOTICA 2

clc;clear all;close all;
%------------------------%
%DEFINICION DE VARIABLES SIMBOLICAS%
%-------------------------%
syms theta1 d1 a1 alpha1
syms theta2 d2 a2 alpha2
syms q1 q2
syms L1 L2 L3
%-----------------------%
%2.DEFINICION DE SISTEMAS COORDENADOS%
%2.1 Sistema Coordenado 1
theta1=q1;
d1=L2;
a1=L1;
alpha1=0;
A1=matriz_homogenea_DH(theta1,d1,a1,alpha1);
%2.2 Sistema coordenado 2
theta2=q2;
d2=0;
alpha2=0;
a2=L3;
A2=matriz_homogenea_DH(theta2,d2,a2,alpha2);
%---------------------%
%CINEMATICA%
%--------------------%
T01=A1;
T02=simplify(A1*A2);
%MATRIZ DE TRANSFORMACION T01
disp('matriz T1')
pretty(T01)
%MATRIZ DE TRANSFORMACION T01
disp('matriz T2')
pretty(T02)

Figura 19. Vista frontal del manipulador

 Figura 20. Vista horizontal del manipulador

4.2 CALCULO DE JACOBIANOS

clc;clear all;close all

%------------------------%
%1.DEFINICION DE VARIABLES SIMBOLICAS%
%-------------------------%
syms theta1 d1 a1 alpha1
syms theta2 d2 a2 alpha2
syms q1 q2
syms L1 L2 L3
syms LCM1x LCM2x LCM1z
%-----------------------%
%2.DEFINICION DE SISTEMAS COORDENADOS%
%2.1 Sistema Coordenado
theta1=q1;
d1=L2;
alpha1=0;
a1=L1;
A1=matriz_homogenea_DH(theta1,d1,a1,alpha1);
T01=A1;
%2.2 Sistema coordenado 2
theta2=q2;
d2=0;
alpha2=0;
a2=L3;
A2=matriz_homogenea_DH(theta2,d2,a2,alpha2);
T02=A1*A2;
%---------------------%
%3. CALCULO DE JACOBIANOS
%--------------------%
%Posicion del centro de masa 1 con respecto al sistema 1
pcm1=[-LCM1x;0;-LCM1z];
%Posicion del centro de masa 1 con respecto al sistema 0
pcm01=T01*[pcm1;1];
%Extraccion de las cordenadas X,Y,Z
pcm01=pcm01(1:3);
%Posicion del centro de masa 2 con respecto al sistema 2
pcm2=[-LCM2x;0;0];
%Posicion del centro de masa 2 con respecto al sistema 0
pcm02=T02*[pcm2;1];
%Extraccion de las cordenadas X,Y,Z
pcm02=pcm02(1:3);
%Calculo de los vectores origenes del sistema y ejes "Z"
o1=T01(1:3,4);
z0=[0;0;1];
z1=T01(1:3,3);
o2=T02(1:3,4);

%JACOBIANO DEL CM1

Jvcm1=[cross(z0,pcm01) zeros(3,1)];
Jwcm1=[z0 zeros(3,1)];
%JACOBIANO DEL CM2
Jvcm2=[cross(z0,pcm02) cross(z1,pcm02-o1)];
Jwcm2=[z0 z1];
%JACOBIANO DEL EFECTOR FINAL
Jvf=[cross(z0,o2) cross(z0,o2-o1)];
Jwf=[z0 z1];

%CENTRO DE MASA DEL PRIMER ESLABON

disp('Jvcm1: ')
pretty(simplify(Jvcm1))
disp('Jwcm1: ')
disp((Jwcm1))

%CENTRO DE MASA DEL SEGUNDO ESLABON

disp('Jvcm2: ')
pretty(simplify(Jvcm2))

disp('Jwcm2: ')
disp(Jwcm2)

%EFECTOR FINAL
disp('Jvf: ')
pretty(simplify(Jvf))

disp('Jwf: ')
disp(Jwf)
4.3 CLASE ROBOT ROBOTICA 2
%----------------------------
% CLASE ROBOT ROBOTICA 2
%---------------------------
classdef robot_ROBO2<handle
%ROBOT PLANAR

properties
%parametros mecanicos
LL1
LL2
LL3
%Parametros DH
d1
a1
alpha1
d2
a2
alpha2
end
 %METODOS DE LA CLASE
methods
%Constructor
function this = robot_ROBO2()
%IniciamosParametros D-H valores fijos
this.d2 = 0;
this.alpha1 = 0;
this.alpha2 = 0;

end
%-------------------------------------------------------------
-
function set_dimensiones(this, L1, L2, L3)
%Metodos para poner dimensiones
this.LL1 = L1;
this.LL2 = L2;
this.LL3 = L3;
% Poner los paremetros DH que faltan
this.d1 = L2 ;
this.a1 = L1 ;
this.a2 = L3 ;
end
%-------------------------------------------------------------

a) CINEMÁTICA INVERSA

function q = cinematica_inversa_ROBO2(this, xc, yc, zc)

%Metodo para la cinematica inversa
L1=this.LL1;
L2=this.LL2;
L3=this.LL3;

num=(xc^2+yc^2-L1^2-L3^2);
den=(2*L1*L3);
D=num/den;
if(D>1 || D<-1)
error('POSICION IMPOSIBLE DE ALCANZAR')
else
%ANGULO "theta2"
q2_star = atan2(+sqrt(1-D^2),D); %dos soluciones codo:
+ arriba,-abajo

%ANGULO "theta1"
num1=L3*sin(q2_star);
den1=L1+L3*cos(q2_star);
q1_star= atan2(yc,xc)- atan2(num1,den1);

%Vector de Angulos
q=[q1_star q2_star];
end
end
%------------------------------------------------------------
function A1=compute_matrix_A1(this,theta1)
% Metodo para hallar A1
A1 = matriz_homogenea_DH(theta1, this.d1, this.a1,this.alpha1);
end

function A2=compute_matrix_A2(this, theta2)

 %metodo para hallar A2
A2 = matriz_homogenea_DH(theta2, this.d2, this.a2, this.alpha2);
end
end
end

4.4 SIMULACION CINEMATICA DIRECTA

%SIMULACION CINEMÁTICA DIRECTA
clc;clear all;close all
%------------------------%
%DEFINICION DE PARAMETROS
%-------------------------%
l1=1;
l2=1;
l3=1;
LCM1x=0.5;
LCM2x=0.5;
LCM1z=0.5;
%DEFINICION DEL SISTEMA INERCIAL
%1.1 MATRIZ HOMOGENEA
T0=eye(4);
%1.2 PLOTEO DEL SISTEMA
plot_frame(T0,'color','k','view','auto')
axis([-2 4 -2 4 0 3]);
hold on
grid on
xlabel('x(m)')
ylabel('y(m)')
%-----------------------%
%2.DEFINICION DE SISTEMAS COORDENADOS%
%2.1 Sistema Coordenado
theta1=pi/6;
d1=l2;
alpha1=0;
a1=l1;
A1=matriz_homogenea_DH(theta1,d1,a1,alpha1);
T01=A1;
h1=plot_frame(T01,'frame','1','color','b');

%2.2 Sistema coordenado 2

theta2=-pi/2;
d2=0;
alpha2=0;
a2=l3;
A2=matriz_homogenea_DH(theta2,d2,a2,alpha2);
T02=A1*A2;
h2=plot_frame(T02,'frame','2','color','g');

disp(' Presione una tecla para esperar la simulacion!!!')

pause
%------------------------
%%LAZO DE SIMULACION
%------------------------
MOVE_JUNTA1 = 1;
MOVE_JUNTA2 = 1;
STEPS = 12;
for i=1:STEPS
if(MOVE_JUNTA1)
theta1 = theta1 + 5*pi/180;
 end
if(MOVE_JUNTA2)
theta2 = theta2 - 2*pi/180;
end
A1 = matriz_homogenea_DH(theta1,d1,a1,alpha1);
A2 = matriz_homogenea_DH(theta2,d2,a2,alpha2);

T01 = A1;
T02 = A1*A2;
plot_frame(h1, T01);
plot_frame(h2, T02);
pause(0.3)
end

Figura 21. Simulación de la cinemática directa en Matlab

4.5 SIMULACION CINEMATICA INVERSA

%SIMULACION DE CINEMATICA INVERSA%
%------------------------%
% 1. Configuracion inicial
% 2. Parametros Mecanicos del Robot
clc; clear all; close all;
L1=1.5;
L2=1.0;
L3=0.7;

xc=1.5;
yc=1.5;
zc=L2;
%1.2. Ploteo del entorno
T0=eye(4);
plot_frame(T0,'color','k','view','auto')
axis([-2 4 -2 4 0 3])
hold on
grid on
xlabel('x(m)')
ylabel('y(m)')
%Ploteo del punto a alcanzar
plot3(xc, yc, zc, 'ro','MarkerFaceColor', 'k', 'MarkerSize', 10)
%Creamos un objeto robot
my_robot = robot_ROBO2();
my_robot.set_dimensiones(L1, L2, L3)

% 3. Cinematica Inversa
%3.1. USAMOS EL METODO "Cinematica inversa"
q = my_robot.cinematica_inversa_ROBO2(xc, yc, zc);
q1d = q(1);
q2d = q(2);
%3.2. SALIDAs
disp('angulos')
disp(q)
disp(q*180/pi)
%---------------------------------------------
%4. Configuracion Incial del Robot
%4.1. Eslabon 1
% -> Hallamos A1
theta1=0;
A1 = my_robot.compute_matrix_A1(theta1);
% -> Ploteamos T01
T01 =A1;
h1 = plot_frame(T01, 'frame', '1', 'color', 'b');
disp(T01)

%4.2. Eslabon 2
theta2 = 0;
A2 = my_robot.compute_matrix_A2(theta2);
T02 = T01*A2;
%4.3 Ploteamso T02
h2 = plot_frame(T02, 'frame', '2', 'color', 'r');
disp(T02)

%---------------------------------------------------------------
% 5. Lazo de simulacion
% 5.1. Mensaje
disp('Robot del tipo ROBO2 de 2 GDL')
disp('Presione una tecla para empezar simulacion')
pause

% 5.2. Definimos los angulos

STEPS = 400;
THETA1 = linspace(theta1, q1d, STEPS);
THETA2 = linspace(theta2, q2d, STEPS);
% 5.3. LAZO PRINCIPAL
for i=1:STEPS
%A. OBTENEMOS LOS ANGULOS
theta1 = THETA1(i);
theta2 = THETA2(i);
%B. Matrices de Transformacion Homoghenea
A1 = my_robot.compute_matrix_A1(theta1);
A2 = my_robot.compute_matrix_A2(theta2);
T01 = A1;
T02 = A1*A2;

%C. Ploteamos los sistemas coordenados

plot_frame(h1, T01);
plot_frame(h2, T02);
 % D. Delta de Tiempo
pause(0.001)
end

Figura 22. Simulación de la cinemática inversa en Matlab