UNIVERSIDAD NACIONAL DE INGENIERIA

FACULTAD DE INGENIERIA MECANICA

PRACTICA N 01

CURSO: Anlisis y Control de Robots (MT517)

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

LPEZ RUMRRILL, Jorge Alejandro 20144052B

SECCION: A"

2017 - II
 PROYECTO DEL CURSO ANALISIS Y CONTROL DE
ROBOTS

1. DISEO MECANICO

Para la realizacin del brazo de 2GDL se ha decidido usar como material de los
links el acrlico de 4cm de espesor. Las razones de la eleccin de este material
son que este es ligero, presenta un costo relativamente bajo y se adapta a
nuestros requerimientos para el diseo.

Las medidas estimadas para el tamao del brazo son de 13cm x 4cm el primer
link y de 8cm x 4cm para el segundo link, con esto podemos hacer los clculos
para ver el torque mnimo 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 clculos a
priori que se realiz, se determin que el torque del motor deba 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 mnimo de 0.6Kg-cm.

As tambin se requiere que los motores tengan encoder de 2 canales para que
sea posible poder controlar la posicin 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 seleccin y compra de los motores.

A continuacin, 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

caractersticas: trabaja a 6v, posee un torque de 6.8 Kg-cm y 78RPM.
Adems, presenta un encoder de efecto Hall, el cual tiene una resolucin de
48 cuentas por revolucin. Tambin se muestra las especificaciones
brindadas por el fabricante.

Figura 1.Motor1 adquirido para el proyecto

Tabla 1. Caractersticas 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

caractersticas: trabaja a 6v, posee un torque de 1.0 Kg-cm y 420 RPM.
Adems, presenta un encoder de efecto Hall, el cual tiene una resolucin de
48 cuentas por revolucin. Tambin se muestra las especificaciones
brindadas por el fabricante.
Figura 3.Motor2 adquirido para el proyecto

Tabla 2. Caractersticas del motor 2

Figura 4. Planos del motor 2 brindado por el fabricante
1.2 DISEO EN SOLIDWORKS

A continuacin, se muestra el diseo del brazo de 2GDL:

1.2.1 MOTOR JUNTA 1

Figura 5. Diseo en Solidworks del motor 1

1.2.2 MOTOR JUNTA 2

Figura 6. Diseo en Solidworks del motor 2

1.2.3 ROBOT PLANAR (VISTAS)

Figura 7. Vista isomtrica del robot planar

Figura 8. Vista frontal del robot planar

 Figura 9. Vista lateral del robot planar

a) LINK 1

Figura 10. Diseo del link 1 en solidworks

b) LINK 2

Figura 11. Diseo del link 2 en solidworks

c) SOPORTE DEL SEGUNDO MOTOR

Figura 12. Diseo del soporte 2 en solidworks

1.3 PLANOS DE DISEO
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 DETERMINACIN DE LOS SISTEMAS COORDENADOS

Figura 18. Sistemas coordenados del robot planar

2.2 DETERMINACIN DE LOS PARMETROS DE DENAVIT HARTENBERG

JUNTA i di ai i

1 1 L2 L1 0

2 2 0 L3 0

2.3 DETERMINACIN DE LAS MATRICES DE TRANSFORMACIN:

1
( ) = , . , . , . ,

C . . .
. . .
1
( ) = [ ]
0
0 0 0 1

Hallando las matrices homogneas:

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 1 2 3 2
2 = =
21 . 3

1 2
2 = tan1

3 . sin 2
1 = tan1 ( ) tan1 ( )
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) CINEMTICA 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 CINEMTICA 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. Simulacin de la cinemtica 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. Simulacin de la cinemtica inversa en Matlab