Informe Final Brazo

“Año de la consolidación del Diálogo y la Reconciliación Nacional”
UNIVERSIDAD NACIONAL DE INGENIERÍA

FACULTAD DE INGENIERÍA MECÁNICA
DINÁMICA DE SISTEMAS MULTICUERPO

INFORME FINAL
IMPLEMENTACIÓN DE UN BRAZO ROBÓTICO
Alumnos:
TOMAIRO LAURA, MEIK KEVIN 20164163D
TINTAYA QUISPE RAMIRO GUSTAVO 20164063J
VITOR CALERO, BRANDON LEE 20154125B
Docente: Calle Flores, Iván Arturo
Sección: B
Fecha de Presentación: 30-NOVIEMBRE-2018
Lima – Perú
2018
DINÁMICA DE SISTEMAS MLTICUERPO – Segunda Práctica Calificada
ÍNDICE
1. Introducción ………………………………………….………………………….3
2. Diseño del manipulador robótico…………………………………………...……4
3. Materiales y costos……………………………………………………………...10
4. Descripción del proceso de fabricación…………………………………………11
5. Cinemática directa……………………………………………………………...13
6. Cinemática inversa……………………………………………………………...18
7. Movimiento permitido …………………………………………………………21
8. Programación………………………………………………………………...…22
8.1. Diseño de interfaz cinemática directa………………………………………24
8.2. Diseño de interfaz cinemática inversa……………………………………...27
9. Desarrollo de actividad…………………………………………………………34
pág. 2
INTRODUCCIÓN
En la actualidad la robótica ha adquirido una gran importancia en diferentes áreas, desde
la industria hasta un uso más personal, existiendo su vez distintas formas de realizar las
acciones deseadas, siendo una de las más comunes la implementación de un brazo
robótico en serie.
En el presente trabajo, se expone el proceso de fabricación de un brazo robótico,
incluyendo tanto el diseño de cada una de las piezas utilizadas, el diseño de los
componentes electrónicos, así como el costo de todo el proyecto, además de una
explicación de cómo fue todo este proceso.
Finalmente, tras haber finalizado el desarrollo del brazo, se espera que en un futuro este
pueda realizar las aplicaciones deseadas, pudiendo ayudar cada vez más en el desarrollo
de nuevas tecnologías.
pág. 3
DISEÑO DEL MANIPULADOR ROBÓTICO

A continuación, se presentan los diseños de las piezas usadas en la fabricación del brazo
robótico:
Diseño de elementos para la garra:
Figura 1. Engranaje 1
Figura 2. Junta 1
pág. 4
Figura 3. Junta 2
Figura 4. Junta 3
Figura 5. Junta 4
pág. 5
Diseño de elementos para el brazo:
Figura 6. Base circular fija
Figura 7. Base total fija
pág. 6
Figura 8. Base circular rotatoria
Figura 9. Unión con la garra
pág. 7
Figura 10. Soporte para base de brazo
Figura 11. Estructura de brazo
Figura 12. Estructura de brazo lado opuesto
pág. 8
Figura 13. Muñeca derecha
Figura 14. Muñeca izquierda
pág. 9
MATERIALES Y COSTOS
A continuación, se presenta una lista de los materiales usados en el diseño del brazo
robótico, junto con sus respectivos costos:
Cantidad Nombre Precio Unitario (S/.) Precio Total (S/.)
2 Rodamientos 625-Z 3.00 6.00

22 min. Corte Láser 0.80 17.60
1 MDF A2 3mm 8.00 8.00
1 Motor Paso a paso 38.00 38.00
Nema 17
2 Servomotor mini 8.50 17.00
SG-90
4 Servomotor SG- 19.00 76.00
5010
1 Driver Pololu 9.00 9.00
A4988
4 Uniones metálicas 2.00 8.00
de Tuberías
4 Ruedas Locas 25 3.00 12.00
mm de altura
2 Protoboards mini 3.00 6.00
18 Pernos 6 mm 0.167 3.00
diámetro
24 Arandelas 6mm 0.0416 1.00
diámetro interior
24 Tuercas de 6 mm de 0.0416 1.00
diametro
6 Barillas roscadas 20 0.167 1.00
cm x 3/8”
3 Barras de silicona 0.50 1.50
1 Arduino UNO 28.00 28.00
Tabla 1. Materiales y costos
Precio total: 233.10 soles
pág. 10
DESCRIPCIÓN DEL PROCESO DE FABRICACIÓN

El presente proyecto fue diseñado casi en su totalidad en la residencia de uno de los
integrantes del grupo, siendo la excepción el corte a láser, el cual fue realizado en las
inmediaciones de la Universidad Nacional de Ingeniería.
A continuación, se detallará el proceso de fabricación e implementación del brazo
robótico:
 Primeramente, fue necesario realizar el diseño del brazo en los softwares
adecuados, para realizar esto, los integrantes del grupo nos reunimos y diseñamos
un nuevo diseño prácticamente desde cero, usándose, sin embargo, ciertos diseños
previos, a fin de poder usarlos como referencia.
En tal ocasión se acordó realizar el brazo robótico de 6 grados de libertad,
incluyendo el de la garra, y se procedió con su diseño.
Este proceso demoró aproximadamente unas 8 horas, tras las cuales el diseño
estaba completamente listo.
 Posteriormente, se debía de materializar el diseño acordado, el cual según lo
acordado, debía de realizarse en láminas de acrílico de 3 mm, y con la ayuda de
un corte a láser, esto se realizó al frente de la Universidad Nacional de Ingeniería,
tomando en total aproximadamente 1 hora, incluyendo el tiempo que demoró
encontrar el material y la tienda adecuados.
 A continuación, se debían de conseguir las piezas adecuadas, para ser usadas
como juntas en la implementación del brazo, lo cuál fue llevado a cabo en el centro
de la ciudad de Lima, en la zona de Las Malvinas, donde además fueron
conseguidas algunas piezas que no fueron previstas, pero que, sin embargo, fueron
de mucha utilidad.
 Tras finalizarse esto, se realizó la implementación del robot en la residencia de
uno de los integrantes del grupo, sin embargo, este procedimiento no fue muy
sencillo, pues había ciertos inconvenientes a la hora de realizar algunas de las
uniones, por lo que se tuvo que improvisar diversas soluciones bastante creativas,
incluyendo entre ellas, el uso de silicona caliente, o el de pequeñas ruedas locas
para ayudar tanto al movimiento como a la estabilidad.
 Sin embargo, tal vez el mayor inconveniente es que fue de mucha dificultad
conseguir las uniones roscadas de tamaño adecuado, pues en algunos casos se
necesitaban diámetros menores a 2 mm, y estos no son muy comerciales, sin
embargo, afortunadamente los pequeños tornillos que llegaban junto con las
ruedas locas pudieron cumplir esta función de manera correcta.
 El proceso de implementación de este brazo se realizó de manera progresiva,
comenzándose primero por la base, la cual fue unida gracias a las juntas metálicas
de tuberías con la base superior fija, colocándose entre ellas el servomotor, el cual,
sin embargo, tuvo que ser fijado con silicona a fin de conseguir una mayor
estabilidad.
 Luego tuvo que ser colocada la base circular móvil, la cuál se unía a la inferior
únicamente con ayuda del pequeño eje del motor paso a paso, por lo que se tuvo
que usar cuatro ruedas locas a fin de conseguir mayor estabilidad y ayuda en el
movimiento, además en la cara superior de la misma tuvieron que ser colocadas
pág. 11
dos pequeños soportes, los cuales fueron fijados en sus respectivas ranuras con la
ayuda de silicona.
 A continuación, tuvieron que ser colocados tanto el brazo, el antebrazo y la
muñeca del robot, siendo el procedimiento usado en los tres bastante similar,
consistiendo en la colocación del servomotor en la ranura correspondiente,
fijándose con ayuda de 4 pernos, además de la unión de los ejes de los motores
con la placa siguiente, lo cuál se realizó con ayuda de una de las pequeñas aletas
que llegan junto al servomotor, además se tuvieron que unir las caras paralelas
con el fin de obtener una mayor estabilidad, esto se realizó con ayuda de los
alambres completamente roscados, y finalmente, a fin de unir las placas en el lado
opuesto al del eje, se decidió usar rodamientos, los cuales se fijaron gracias a la
utilización de pernos, los cuales funcionaron a modo de ejes.
 Finalmente, debía de ser realizada la implementación de la garra. Esto se realizó
con la utilización de los tornillos adecuados, y la colocación de las piezas en el
lugar adecuado.
Con esto concluye el proceso de desarrollo e implementación del brazo robótico, el
cual se espera en un futuro pueda realizar las diversas operaciones para las que fue
diseñado.
pág. 12
CINEMÁTICA DIRECTA
Para llevar a cabo la cinemática directa, es necesario llevar a cabo una serie de pasos los
cuales son detallados a continuación: Z0
1. Asignación de los ejes z
Este paso es de vital importancia, pues nuestros cálculos se guían principalmente
de la posición y la dirección de nuestros ejes z
Figura 15. Asignación de ejes z
2. Asignación de un sistema inercial

Es necesario definir un sistema que nos servirá como referencia para poder
realizar los demás cálculos, en este caso es el que está junto a Z0.
3. Asignación de los ejes x e y

A continuación, vamos a escoger las posiciones de nuestros ejes x e y.
El eje z2 es paralelo a z1, por lo que el eje x2 sigue la línea que une ambos, lo
mismo sucede entre los sistemas 2 y 3.
Los ejes z3 y z4 son perpendiculares, por lo que el eje x4 es perpendicular al plano
formado por ambos.
Finalmente, el eje 5 es paralelo al eje 4, estando este posicionado en el punto final
del brazo.
pág. 13
Figura 16. Ejes del brazo
Figura 17. Representación en Matlab
pág. 14
4. Desarrollo con datos reales
Procedemos ahora a desarrollar la matriz usando los datos reales.
100 mm
Figura 18. Dimensionado de brazo
Tabla 2. Tabla con los resultados reales
pág. 15
5. Reemplazamos los valores en la matriz de transformación
cos⁡(𝜃) −𝑠𝑒𝑛(𝜃) ∗ cos⁡(𝛼) 𝑠𝑒𝑛(𝜃) ∗ 𝑠𝑒𝑛⁡(𝛼) 𝑎 ∗ cos⁡(𝜃)

(𝑠𝑒𝑛(𝜃) cos⁡(𝛼) ∗ 𝑐𝑜𝑠(𝜃) − cos(𝜃) ∗ 𝑠𝑒𝑛(𝛼) 𝑎 ∗ 𝑠𝑒𝑛(𝜃))
0 𝑠𝑒𝑛(𝛼) cos⁡(𝛼) 𝑑
0 0 0 1
cos⁡(θ1 ) 0 𝑠𝑒𝑛(θ1 ) 0
𝐴10 =(𝑠𝑒𝑛(θ1 ) 0 − cos(θ1 ) 0 )
0 1 0 100
0 0 0 1
cos⁡(θ2 ) − 𝑠𝑒𝑛(θ2 ) 0 86 ∗ cos⁡(θ2 )

𝐴12 =(𝑠𝑒𝑛(θ2 ) cos⁡(θ2 ) 0 86 ∗ 𝑠𝑒𝑛(θ2 ))
0 0 1 0
0 0 0 1
cos⁡(θ3 ) − 𝑠𝑒𝑛(θ3 ) 0 80 ∗ cos⁡(θ3 )

𝐴23 =(𝑠𝑒𝑛(θ3 ) cos⁡(θ3 ) 0 80 ∗ 𝑠𝑒𝑛(θ3 ))
0 0 1 0
0 0 0 1
cos⁡(θ4 ) 0 𝑠𝑒𝑛(θ4 ) 0
𝐴34 =(𝑠𝑒𝑛(θ4 ) 0 − cos(θ4 ) 0)
0 1 0 0
0 0 0 1
cos⁡(θ5 ) − 𝑠𝑒𝑛(θ5 ) 0 0
𝐴45 =(𝑠𝑒𝑛(θ5 ) cos⁡(θ5 ) 0 0 )
0 0 1 206
0 0 0 1
pág. 16
Entonces hallamos una matriz multiplicando a todos los resultados anteriores
𝑎11 𝑎12 𝑎13 𝑎14

𝑎21 𝑎22 𝑎23 𝑎24
𝐴05 =( )
𝑎31 𝑎32 𝑎33 𝑎34
0 0 0 1
Donde:
𝑎11 = 𝑠𝑒𝑛(θ1 ) ∗ 𝑠𝑒𝑛(θ5 ) + cos(θ1 ) ∗ cos(θ5 ) ∗ cos⁡(θ2 + θ3 + θ4 )
𝑎21 = −𝑐𝑜𝑠(θ1 ) ∗ 𝑠𝑒𝑛(θ5 ) + cos(θ5 ) ∗ 𝑠𝑒𝑛(θ1 ) ∗ cos⁡(θ2 + θ3 + θ4 )
𝑎31 = 𝑐𝑜𝑠(θ5 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )
𝑎12 = 𝑐𝑜𝑠(θ5 ) ∗ 𝑠𝑒𝑛(θ1 ) − cos(θ1 ) ∗ 𝑠𝑒𝑛(θ5 ) ∗ cos(θ2 + θ3 + θ4 )
𝑎22 = −𝑐𝑜𝑠(θ1 ) ∗ 𝑐𝑜𝑠(θ5 ) − 𝑠𝑒𝑛(θ1 ) ∗ 𝑠𝑒𝑛(θ5 ) ∗ cos(θ2 + θ3 + θ4 )
𝑎32 = −𝑠𝑒𝑛(θ5 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )
𝑎13 = 𝑐𝑜𝑠(θ1 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )
𝑎23 = 𝑠𝑒𝑛(θ1 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )
𝑎33 = −cos(θ2 + θ3 + θ4 )
𝑎14 = 2 ∗ 𝑐𝑜𝑠(θ1 ) ∗ (43 ∗ 𝑐𝑜𝑠(θ2 ) + 40 ∗ 𝑐𝑜𝑠(θ2 + θ3 ))⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡

+ 206 ∗ 𝑐𝑜𝑠(θ1 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )
𝑎24 = 2 ∗ 𝑠𝑒𝑛(θ1 ) ∗ (43 ∗ 𝑐𝑜𝑠(θ2 ) + 40 ∗ 𝑐𝑜𝑠(θ2 + θ3 ))⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡

+ 206 ∗ 𝑠𝑒𝑛(θ1 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )
𝑎34 = 100 − 206 ∗ 𝑐𝑜𝑠(θ2 + θ3 + θ4 ) + 86⁡∗⁡𝑠𝑒𝑛(θ2 ) + 80 ∗

𝑠𝑒𝑛(θ2 + θ3 )
pág. 17
CINEMÁTICA INVERSA
Para llevar a cabo la cinemática inversa se realizó un esquema del brazo robótico más
sencillo para poder realizar trazos:
Figura 19. Vista general de ángulos
Se analiza la vista superior para hallar 𝜃1
Figura 20. Vista superior
Entonces: 𝜃1 =atan2 (Yc, Xc)
pág. 18
Posteriormente tomamos una vista en la que el eje de las abscisas sea coincidente con la
resultante de Xc e Yc de la primera parte.
Figura 20. Perfil de la pieza
De lo cual:
𝑅𝑐 = √𝑥𝑐 2 + 𝑦𝑐 2
Al ser número de grados de libertad en las rotaciones, mayor de 3, se debe de proceder a

tomar ciertas consideraciones, en este caso se define un ángulo 𝜽𝒇 constante en todo
momento.
Hallamos el punto del origen del sistema 4:
𝑍𝑐2 = 𝑍𝑐 − 𝑙4 ∗ sen⁡(𝜽𝒇 )
𝑅𝑐2 = 𝑅𝑐 − 𝑙4 ∗ cos⁡(𝜽𝒇 )
Además, debido a la utilidad que se le desea dar al brazo, se escoge el siguiente valor
constante valor constante: 𝜽𝒇 =0
pág. 19
Hallamos entonces los valores para el resto de ángulos:

Para 𝜃3 :
(𝑍𝑐2 − 𝐿1)2 + 𝑅𝑐22 − (𝐿22 + 𝐿32 )
𝐷=
2 ∗ 𝐿2 ∗ 𝐿3
𝜃3 = atan2(±√1 − 𝐷2 , 𝐷)
Si bien se pueden usar ambos signos, se prefiere trabajar con el negativo, dado que al usar
este nuestro brazo adquiere más estabilidad, debido al sentido en que se inclina el codo
Para 𝜃2 :
𝜃2 = atan2(Zc2 − L1, Rc2) − atan2(L3 ∗ sen(𝜃3 ), L2 + L3 ∗ cos(𝜃3 ))

Y finalmente, para 𝜃4 :
𝜃4 = 𝜃𝑓 − (𝜃2 + 𝜃3 )
pág. 20
RESTRICCIONES DE MOVIMIENTO
A continuación, se muestran los ángulos que se le permite girar a cada uno de los motores
en el brazo:
𝜃4
𝜃3
𝜃2
Figura 21. Esquema de juntas en el brazo
En la imagen se aprecia los ángulos de rotación de los motores, estos ángulos están
restringidos por la forma de nuestro brazo, definiendo así condiciones mecánicas.
Además se considerará las restricciones del primer motor, el cual no se aprecia en la
gráfica debido al ángulo en el que se enfoca.
Entonces: 𝜃1 ⁡ ∈⁡< −90, 90 >, 𝜃2 ⁡ ∈⁡< 45,135 >, 𝜃3 ⁡ ∈⁡< −90, 90 >, 𝜃4 ⁡ ∈⁡< −90, 90 >
pág. 21
PROGRAMACIÓN
Para poder realizar el programa en Matlab, se tuvo que realizar la siguiente secuencia de
pasos:
Se tuvo que crear funciones que nos entreguen los parámetros tanto de cinemática directa
como de inversa.
Siendo la siguiente, la función de cinemática directa:
function [T01,T02,T03,T04,T05]=cinematica_directa2(q1,q2,q3,q4,q5)
%-------------------------------------------------------%
% 1. LONGITUDES DEL BRAZO
%-------------------------------------------------------%
L1 = 0.5;
L2 = 0.86;
L3 = 0.86;
L4 = 0.85;
%-------------------------------------------------------%
%------------------------------------------------------------%
% 4. CINEMATICA DIRECTA - SISTEMAS COORDENADOS
%------------------------------------------------------------%
% Llenamos con la información de nuestro brazo %INFORMACION FIJA
q4 = pi/2+q4;
theta1 = q1; d1 = L1; a1 = 0; alpha1=pi/2;

theta2 = q2; d2 = 0; a2 = L2; alpha2=0;
theta3 = q3; d3 = 0; a3 = L3; alpha3=0;
theta4 = q4; d4 = 0; a4 = 0; alpha4=pi/2;
theta5 = q5; d5 = L4; a5 = 0; alpha5=0;
%--------------------------------------------------------------------%
% 2. DEFINICION DE SISTEMAS COORDENADOS
%--------------------------------------------------------------------%
% 2.1. SISTEMA COORDENADO 1
A1 = matriz_homogenea_DH(theta1,d1,a1,alpha1);
%--------------------------------------------------------------------%
% 3. CALCULO DE LA MATRIZ DE TRANSFORMACION DEL EFECTOR
%--------------------------------------------------------------------%
T01 = A1;
T02 = T01*A2;
T03 = T02*A3;
T04 = T03*A4;
T05 = T04*A5;
pág. 22
De igual manera, se desarrolló una función para poder trabajar los parámetros de
cinemática inversa:
function [q1,q2,q3,q4,FLAG]=cinematica_inversa(x,y,z,theta_f)
% FLAG=0 ... Todo OK
% FLAG=1 ... Error
FLAG = 0;
%-------------------------------------------------------%
% 1. LONGITUDES DEL BRAZO
%-------------------------------------------------------%
L1 = 1;
L2 = 0.86;
L3 = 0.80;
L4 = 2.06;
%-------------------------------------------------------%
% 3. CINEMATICA INVERSA
%------------------------------------------------------------%
% 3.1. EMPEZAMOS CON "q1=theta1"
q1 = atan2(y,x);
L = sqrt(x^2+y^2);
zp = z-L4*sin(theta_f)-L1;
Lp = L - L4*cos(theta_f);
Dp = sqrt(Lp^2 + zp^2);
%Condición de exitencia
C_num = Dp^2 - (L2^2 + L3^2);
C_den = 2*L2*L3;
C = C_num/C_den;
if(abs(C)>1)
FLAG=1;
q1=-100; q2=-100; q3=-100; q4=-100;
return
end
%Hallando los otros parámetros
q3 = atan2(-sqrt(1-C^2),C);
q2 = atan2(zp,Lp)-atan2(L3*sin(q3),L2+L3*cos(q3));
q4 = theta_f-(q2+q3);
pág. 23
DISEÑO INTERFAZ CINEMÁTICA DIRECTA

En el editor del mismo, colocamos los cuadros necesarios según la cantidad de datos
que queremos ingresar, y los que queremos recibir:
Figura 22. Implementación del interfaz

Seguidamente, realizamos la codificación para definir las funciones que realizará nuestra
interfaz:
 Habilitamos la comunicación con el Arduino, para poder enviar los datos
correspondientes:
s1 = serial('COM6');
fopen(s1);
handles.puerto = s1; % Guardamos el puerto
Programando el botón de cinemática directa
 Realizamos la lectura de los datos de las cajas

q1 = get(handles.q1, 'String');
q1 = str2double(q1); % Esta en sexagesimal
q2 = str2double(q2);
pág. 24
 Verificamos las limitaciones de cada uno de los ángulos, esto también se hace
junto con pruebas físicas, para poder ver hasta donde es capaz de moverse cada
junta:
% 2.1. PARA EL MOTOR 1 - BASE GIRATORIA

if(q1>90 || q1<-90)
fprintf(' q1(sexa):%2.4f\n', q1)
errordlg('Fuera de rango (-90,90)', 'Servo 01')
return
end
% 2.2. PARA EL MOTOR 2 y 3 - BASE
if(q2<45 || q2>135)
errordlg('Fuera de rango (10,150)', 'Servo 02 y 03')
return
end
% 2.2. PARA EL MOTOR 4 - PRIMERA ARTICULACION
if(q3<-90 || q3>90)
errordlg('Fuera de rango (10,150)', 'Servo 04')
return
end
% 2.2. PARA EL MOTOR 5 - SEGUNDA ARTICULACION
if(q4<-90 || q4>90)
return
end
% 2.2. PARA EL MOTOR 6 - ROTACIÓN FINAL
if(q5<-45 || q5>50)
return
end
 Realizamos los cálculos correspondientes a la cinemática directa:
q1 = q1*pi/180;
q2 = q2*pi/180;
q3 = q3*pi/180;
q4 = q4*pi/180;
q5 = q5*pi/180;
% 3.2. HALLAMOS LA CINEMATICA
T05=cinematica_directa(q1,q2,q3,q4,q5);
% 3.3. SACAMOS LA COORDENADA DEL PUNTO FINAL
xx = T05(1,4);
yy = T05(2,4);
zz = T05(3,4);
 Se colocan los resultados en las cajas de texto:

set(handles.x, 'String',round(xx*1000)/1000);
set(handles.y, 'String',round(yy*1000)/1000);
set(handles.z, 'String',round(zz*1000)/1000);
pág. 25
Realizando la configuración para el botón de envío de información al Arduino:
 Realizamos la lectura de los datos enviados por la interfaz:
Los datos entregados en q1, q2, q3, q4 y q5 se desea que se encuentren en

sexagesimales, ya posteriormente se adaptarán para poder realizar los cálculos con
el software.
 Posteriormente se realizó la calibración de los motores, esto se realizó junto con

la manipulación de nuestro modelo en físico, por lo que los datos mostrados se
obtuvieron en base a ensayos de prueba y error.
% 2.1. PARA EL MOTOR 1 (Motor paso a paso)

q1_motor = q1 + 90;
% 2.3. PARA EL MOTOR 2 (Servo base_1)
q2_motor = q2 + 60;
q3_motor = q2 + 100;
% 2.3. PARA EL MOTOR 4 (Servo 1ra articulación)
q4_motor = q3 + 50;
% 2.3. PARA EL MOTOR 5 (Servo 2de articulación)
q5_motor = q4 - 50;
% 2.3. PARA EL MOTOR 6 (Microservo giro)
q6_motor = q5 + 90;
% 2.4. REDONDEAMOS
q1_motor = round(q1_motor);
fprintf('VALORES A ENVIAR A LOS SERVOS\n')
fprintf(' q1_motor:%d\n', q1_motor)
pág. 26
 Tras esto, se envía la información al Arduino.
s1 = handles.puerto;
fwrite(s1,[q1_motor q2_motor q3_motor],'uchar');
 Tras enviar los datos, podemos cerrar el puerto serial:

%s1 = handles.puerto;
%fclose(s1);
disp('...cerrando ventana')
DISEÑO INTERFAZ CINEMÁTICA INVERSA
 Editamos la interfaz para enviar y recibir los datos de la cinemática inversa:
Figura 23. Desarrollo de interfaz para inversa
pág. 27
Entonces editamos el código en Matlab:

 En este caso, a diferencia del anterior, se debe de inicializar primeramente el
manejo de los frames, además de poder inicializar el cuadro del dibujo:
clc
view(3)
grid on
axis([-2 3 -2 3 0 3.5])
xlabel('x(m)')
ylabel('y(m)')
% Inicializamos manejadores de las lineas
h_L1 = line([0 0], [0 0], [0 0], 'LineWidth', 4);
handles.h_L1 = h_L1;
% Inicializamos manejadores de los frames

T00=eye(4);
h0 = plot_frame(T00,'frame','0','color', 'k', 'length',
0.4);
h1 = plot_frame(T00,'frame','1','color', 'r', 'length',
0.4);
0.4);
0.4);
h4 = plot_frame(T00,'frame','4','color', 'm', 'length',
0.4);
0.4);
handles.h1 = h1;
handles.h2 = h2;
handles.h3 = h3;
handles.h4 = h4;
handles.h5 = h5;
 Habilitamos el puerto serial:

% ABRIMOS EL PUERTO SERIAL
fopen(s1);
% GUARDAMOS EL PUERTO
handles.puerto = s1;
pág. 28
Para programar el botón de cinemática inversa

 Leemos los valores de las cajas y los adecuamos para poder trabajarlos
x = get(handles.posicion_x, 'String');
y = get(handles.posicion_y, 'String');
z = get(handles.posicion_z, 'String');
theta_f = get(handles.theta_f, 'String');
x = str2double(x);
y = str2double(y);
z = str2double(z);
theta_f = str2double(theta_f); %Está en sexagesimal
theta_f = theta_f*pi/180; % Convertida a radianes
q5 = str2double(q5); %Esta en sexagesimal
q5 = q5*pi/180; % Convertida a radianes
 Insertamos el código para realizar la cinemática inversa

[q1,q2,q3,q4, FLAG]=cinematica_inversa(x,y,z,theta_f);
if(FLAG==1)
errordlg('Tu brazo es pequeño', 'Fuera rango')
return
end
 Colocamos el resultado en las cajas:

set(handles.angulo_q1, 'String',q1);
set(handles.angulo_q1_sexa, 'String',q1*180/pi);
 Finalmente, dibujamos el resultado

% DIBUJAMOS EL RESULTADO
axes(handles.axes_robot)
[T01,T02,T03,T04,T05]=cinematica_directa2(q1,q2,q3,q4,q5);
% PRIMERA LINEA
x1 = T01(1,4);
y1 = T01(2,4);
z1 = T01(3,4);
%line([0 x1],[0 y1],[0 z1],'LineWidth', 4)
set(handles.h_L1,'xdata',[0 x1],'ydata', [0 y1], 'zdata',
[0 z1]);
% SEGUNDA LINEA
x2 = T02(1,4);
y2 = T02(2,4);
z2 = T02(3,4);
%line([x1 x2], [y1 y2], [z1 z2], 'LineWidth', 4)
set(handles.h_L2,'xdata',[x1 x2],'ydata', [y1 y2], 'zdata',
[z1 z2]);
% TERCERA LINEA
x3 = T03(1,4);
pág. 29
y3 = T03(2,4);
z3 = T03(3,4);
[z2 z3]);
% CUARTA LINEA
x4 = T04(1,4);
y4 = T04(2,4);
z4 = T04(3,4);
[z3 z4]);
% QUINTA LINEA
x5 = T05(1,4);
y5 = T05(2,4);
z5 = T05(3,4);
[z4 z5]);
% PARA PLOTEAR LOS SISTEMAS COORDENADOS
plot_frame(handles.h1, T01);
 Finalmente ejecutamos el código para poder enviar información al Arduino
%--------------------------------------------------%
% 1. LEEMOS LA VARIABLE DEL PUERTO
%--------------------------------------------------%
%--------------------------------------------------%
% 2. LEEMOS EL VALOR DE LOS ANGULOS (caja de texto)
%--------------------------------------------------%
q1 = get(handles.angulo_q1_sexa, 'String');
%--------------------------------------------------%
% 3. APLICAMOS LA CALIBRACION
% (Depende como lo han armado)
%--------------------------------------------------%
% 3.1. PARA EL MOTOR 1
q1_motor = q1 + 90;
q2_motor = q2 + 60;
pág. 30

q3_motor = q2 + 50;
% 3.4. PARA EL MOTOR 4 (Servo 1ra articulación)
q4_motor = q3 + 100;
% 3.5. PARA EL MOTOR 5 (Servo 2de articulación)
q5_motor = q4 - 50;
% 3.6. PARA EL MOTOR 6 (Microservo giro)
q6_motor = q5 + 90;
% 3.7. REDONDEAMOS
fprintf('VALORES A ENVIAR A LOS SERVOS\n')
%--------------------------------------------------%
% 4. ENVIAMOS LOS ANGULOS AL ARDUINO
%--------------------------------------------------%
fwrite(s1,[q1_motor q2_motor q3_motor, q4_motor, q5_motor,
q6_motor],'uchar');
De igual manera se debe de realizar un código de Arduino en su propia IDE para poder
enviar información desde el Matlab:
int dir = 2;
int steps = 3;
int tiempo = 5;
int n = 0;
//--------------------------------------------------------------//
// 2. RUTINA DE CONFIGURACION
//--------------------------------------------------------------//
void setup()
{
//Configuracion del Paso a Paso
pinMode(steps, OUTPUT);
pinMode(dir, OUTPUT);
pág. 31
// CONFIGURACION DE LOS PINES DE LOS SERVOS

my_servo_02.attach(my_servo_02_PIN);
// INICIAMOS COMUNICACION SERIAL
Serial.begin(9600);
}
//--------------------------------------------------------------//
// 3. RUTINA PRINCIPAL
//--------------------------------------------------------------//
void loop()
{
// PROCESAMOS LOS 5 BYTES ENVIADOS DESDE LA PC
if(Serial.available()==5)
{
// A. LEEMOS LOS BYTES ENVIADO
pasos_01 = Serial.read();
angle_02 = Serial.read();
// B. ENVIAMOS EL ANGULO AL SERVO MOTOR
// para el paso a paso
while(pasos_01 > n){
digitalWrite(dir,LOW); // ponemos en alto o 1 logico a pin dir para que gire hacia un sentido
digitalWrite(steps,HIGH); // las intrucciones que se encuntran dentro del lazo
delay(tiempo); // son las que aran que nuestro motor se mueva ,es la parte donde van los pulsos
digitalWrite(steps,LOW); // necesarios para que el motor se mueva
delay(tiempo);
n++; }
pág. 32
while(n > pasos_01){

digitalWrite(dir,HIGH); // ponemos en alto o 1 logico a pin dir para que gire hacia un sentido
digitalWrite(steps,HIGH); // las intrucciones que se encuntran dentro del lazo
delay(tiempo); // son las que aran que nuestro motor se mueva ,es la parte donde van
los pulsos
digitalWrite(steps,LOW); // necesarios para que el motor se mueva
delay(tiempo);
n--;
}
//my_servo_01.write(angle_01);
my_servo_02.write(angle_02);
delay(20);
}
}
pág. 33
DESARROLLO DE ACTIVIDAD
Finalmente se desea que el brazo sea capaz de realizar una actividad previamente
programada, en este caso se escogió que escriba la palabra UNI, realizándose de la
siguiente manera:
 Primeramente, diseñamos la interfaz para poder enviar y recibir datos
provenientes del código
Figura 24. Desarrollo de interfaz para movimiento
 Seguidamente inicializamos los manejadores de las líneas, y habilitamos el puerto

serial, para poder realizar el dibujo:
clc
view(3)
grid on
axis([-1 0.8 -1 0.3 0 2.2])
xlabel('x(m)')
ylabel('y(m)')
% Inicializamos manejadores de las lineas
h_L1 = line([0 0], [0 0], [0 0],'color','k', 'LineWidth',
4);
pág. 34
% Inicializamos manejadores de los frames

T00=eye(4);
h0 = plot_frame(T00,'frame','0','color', 'k', 'length',
0.2);
h1 = plot_frame(T00,'frame','1','color', 'b', 'length',
0.2);
0.2);
0.2);
0.2);
0.2);
handles.h1 = h1;
handles.h2 = h2;
handles.h3 = h3;
handles.h4 = h4;
handles.h5 = h5;
% ABRIMOS EL PUERTO SERIAL
fopen(s1);
% GUARDAMOS EL PUERTO
handles.puerto = s1;
 Realizamos entonces un código para poder realizar el envío de datos a para

realizar la simulación:
%Los puntos a dibujar serán:
theta_f=0;
q5=0;
X=-1.0;
Zmax=2.0;
Zmin=1.4;
A = [X, 0.0, Zmax];
B = [X, 0.0, Zmin];
C = [X, -0.26, Zmin];
D = [X, -0.26, Zmax];
E = [X, -0.38, Zmax];
F = [X, -0.38, Zmin];
G = [X, -0.38, Zmax];
H = [X, -0.6, Zmin];
I = [X, -0.6, Zmax];
J = [X, -0.8, Zmax];
K = [X, -0.8, Zmin];
PUNTOS = [A;B;C;D;E;F;G;H;I;J;K];
STEPS = 20;
for p = 1:size(PUNTOS,1)-1
% Punto inicial "x0"
xo = PUNTOS(p,1);
yo = PUNTOS(p,2);
zo = PUNTOS(p,3);
% Punto final "xf"
xf = PUNTOS(p+1,1);
yf = PUNTOS(p+1,2);
pág. 35
zf = PUNTOS(p+1,3);
% Ploteamos
plot3(xo,yo,zo,'ro','MarkerFaceColor','k','MarkerSize',4)
plot3(xf,yf,zf,'ro','MarkerFaceColor','r','MarkerSize',4)
if (p~=4 && p~=9)
line([xo xf], [yo yf],[zo zf],'Color','k','LineWidth', 3)
fprintf('LINEA %d\n', p)
end
fprintf('LINEA %d\n', p)
for i=1:STEPS
% A. CINEMATICA INVERSA
x = xo + (xf-xo)*i/STEPS;
y = yo + (yf-yo)*i/STEPS;
z = zo + (zf-zo)*i/STEPS;
[q1,q2,q3,q4,
FLAG]=cinematica_inversa(x,y,z,theta_f);
if(FLAG==1)
% Error
% - Apagar hardware, etc. Desconectar arduino, etc
error('FINALIZANDO POSICION IMPOSIBLE DE ALCANZAR')
end
% PONEMOS EL RESULTADO
set(handles.posicion_x, 'String',round(x*1000)/1000);
set(handles.posicion_y, 'String',round(y*1000)/1000);
set(handles.posicion_z, 'String',round(z*1000)/1000);
% DIBUJAMOS EL RESULTADO
axes(handles.axes_robot)
[T01,T02,T03,T04,T05]=cinematica_directa2(q1,q2,q3,q4,q5);
% PRIMERA LINEA
x1 = T01(1,4);
y1 = T01(2,4);
z1 = T01(3,4);
%line([0 x1],[0 y1],[0 z1],'LineWidth', 4)
set(handles.h_L1,'xdata',[0 x1],'ydata', [0 y1],
'zdata', [0 z1]);
% SEGUNDA LINEA
x2 = T02(1,4);
y2 = T02(2,4);
z2 = T02(3,4);
set(handles.h_L2,'xdata',[x1 x2],'ydata', [y1 y2],
'zdata', [z1 z2]);
% TERCERA LINEA
x3 = T03(1,4);
y3 = T03(2,4);
z3 = T03(3,4);
pág. 36

'zdata', [z2 z3]);
% CUARTA LINEA
x4 = T04(1,4);
y4 = T04(2,4);
z4 = T04(3,4);
'zdata', [z3 z4]);
% QUINTA LINEA
x5 = T05(1,4);
y5 = T05(2,4);
z5 = T05(3,4);
'zdata', [z4 z5]);
%Mandamos los datos al brazo
q5 = 0;
%PARA EL MOTOR 1
q1_motor = q1 + 90;
%PARA EL MOTOR 2 (Servo base_1)
q2_motor = q2 + 60;
%PARA EL MOTOR 3 (Servo base_2)
q3_motor = q2 + 50;
%PARA EL MOTOR 4 (Servo 1ra articulación)
q4_motor = q3 + 100;
%PARA EL MOTOR 5 (Servo 2de articulación)
q5_motor = q4 - 50;
%PARA EL MOTOR 6 (Microservo giro)
q6_motor = q5 + 90;
%REDONDEAMOS
%--------------------------------------------------%
% 4. ENVIAMOS LOS ANGULOS AL ARDUINO
%--------------------------------------------------%
%s1 = handles.puerto;
%fwrite(s1,[q1_motor q2_motor q3_motor],'uchar');
pause(0.1)
end
end
pág. 37
 Finalmente, obtenemos el siguiente resultado de la interfaz
Figura 24. Interfaz durante simulación
pág. 38

Informe Final Brazo

Cargado por

Información del documento

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Informe Final Brazo

Cargado por

Copyright:

Formatos disponibles

“Año de la consolidación del Diálogo y la Reconciliación Nacional”

UNIVERSIDAD NACIONAL DE INGENIERÍA

DINÁMICA DE SISTEMAS MULTICUERPO

TINTAYA QUISPE RAMIRO GUSTAVO 20164063J

VITOR CALERO, BRANDON LEE 20154125B

Docente: Calle Flores, Iván Arturo

Fecha de Presentación: 30-NOVIEMBRE-2018

DISEÑO DEL MANIPULADOR ROBÓTICO

Diseño de elementos para el brazo:

Figura 6. Base circular fija

Figura 7. Base total fija

Figura 8. Base circular rotatoria

Figura 9. Unión con la garra

Figura 10. Soporte para base de brazo

Figura 11. Estructura de brazo

Figura 12. Estructura de brazo lado opuesto

Figura 13. Muñeca derecha

Figura 14. Muñeca izquierda

Cantidad Nombre Precio Unitario (S/.) Precio Total (S/.)

2 Rodamientos 625-Z 3.00 6.00

Tabla 1. Materiales y costos

Precio total: 233.10 soles

DESCRIPCIÓN DEL PROCESO DE FABRICACIÓN

Figura 15. Asignación de ejes z

2. Asignación de un sistema inercial

3. Asignación de los ejes x e y

Figura 16. Ejes del brazo

Figura 17. Representación en Matlab

4. Desarrollo con datos reales

Procedemos ahora a desarrollar la matriz usando los datos reales.

Figura 18. Dimensionado de brazo

Tabla 2. Tabla con los resultados reales

5. Reemplazamos los valores en la matriz de transformación

cos⁡(𝜃) −𝑠𝑒𝑛(𝜃) ∗ cos⁡(𝛼) 𝑠𝑒𝑛(𝜃) ∗ 𝑠𝑒𝑛⁡(𝛼) 𝑎 ∗ cos⁡(𝜃)

cos⁡(θ2 ) − 𝑠𝑒𝑛(θ2 ) 0 86 ∗ cos⁡(θ2 )

cos⁡(θ3 ) − 𝑠𝑒𝑛(θ3 ) 0 80 ∗ cos⁡(θ3 )

Entonces hallamos una matriz multiplicando a todos los resultados anteriores

𝑎11 𝑎12 𝑎13 𝑎14

𝑎11 = 𝑠𝑒𝑛(θ1 ) ∗ 𝑠𝑒𝑛(θ5 ) + cos(θ1 ) ∗ cos(θ5 ) ∗ cos⁡(θ2 + θ3 + θ4 )

𝑎21 = −𝑐𝑜𝑠(θ1 ) ∗ 𝑠𝑒𝑛(θ5 ) + cos(θ5 ) ∗ 𝑠𝑒𝑛(θ1 ) ∗ cos⁡(θ2 + θ3 + θ4 )

𝑎31 = 𝑐𝑜𝑠(θ5 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )

𝑎12 = 𝑐𝑜𝑠(θ5 ) ∗ 𝑠𝑒𝑛(θ1 ) − cos(θ1 ) ∗ 𝑠𝑒𝑛(θ5 ) ∗ cos(θ2 + θ3 + θ4 )

𝑎22 = −𝑐𝑜𝑠(θ1 ) ∗ 𝑐𝑜𝑠(θ5 ) − 𝑠𝑒𝑛(θ1 ) ∗ 𝑠𝑒𝑛(θ5 ) ∗ cos(θ2 + θ3 + θ4 )

𝑎32 = −𝑠𝑒𝑛(θ5 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )

𝑎13 = 𝑐𝑜𝑠(θ1 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )

𝑎23 = 𝑠𝑒𝑛(θ1 ) ∗ 𝑠𝑒𝑛(θ2 + θ3 + θ4 )

𝑎14 = 2 ∗ 𝑐𝑜𝑠(θ1 ) ∗ (43 ∗ 𝑐𝑜𝑠(θ2 ) + 40 ∗ 𝑐𝑜𝑠(θ2 + θ3 ))⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡

𝑎24 = 2 ∗ 𝑠𝑒𝑛(θ1 ) ∗ (43 ∗ 𝑐𝑜𝑠(θ2 ) + 40 ∗ 𝑐𝑜𝑠(θ2 + θ3 ))⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡⁡

𝑎34 = 100 − 206 ∗ 𝑐𝑜𝑠(θ2 + θ3 + θ4 ) + 86⁡∗⁡𝑠𝑒𝑛(θ2 ) + 80 ∗

Figura 19. Vista general de ángulos

Se analiza la vista superior para hallar 𝜃1

Figura 20. Vista superior

Entonces: 𝜃1 =atan2 (Yc, Xc)

Figura 20. Perfil de la pieza

Al ser número de grados de libertad en las rotaciones, mayor de 3, se debe de proceder a

Hallamos entonces los valores para el resto de ángulos:

𝜃2 = atan2(Zc2 − L1, Rc2) − atan2(L3 ∗ sen(𝜃3 ), L2 + L3 ∗ cos(𝜃3 ))

Figura 21. Esquema de juntas en el brazo

theta1 = q1; d1 = L1; a1 = 0; alpha1=pi/2;

DISEÑO INTERFAZ CINEMÁTICA DIRECTA

Figura 22. Implementación del interfaz

Programando el botón de cinemática directa

 Realizamos la lectura de los datos de las cajas

% 2.1. PARA EL MOTOR 1 - BASE GIRATORIA