Está en la página 1de 8

Diseo y Simulacin de un sistema de control multivariable por par

computado aplicado a un manipulador robtico de 3DOF mediante


tcnica PID
Hans C. Rojas Caytuiro, Alvaro Aguilar Chavez,
Engineer_hans@hotmail.com, adedtu@hotmail.com

Abstract this thesis is framed in the design of a control


system for computed torque and their development
through technical PID control as part of research for both
academics and professional. Academically serve for
future knowledge researchers, postgraduate students or
students who are interested in this area of robotics, to
serve as a drought in the development of control systems
in the professional field may consider this thesis to
validate the correct the manipulator performs will be
based on the control system proposed here
Palabras Claves Arduino, Control, Robot, 3 DOF,
Simmechanics, Simulink.

I.

INTRODUCCIN

Con los valores obtenido de la tabla I reemplazamos los


valores en una matriz de Denavit-Hartenberg(1)
diferente para cada una de las articulaciones.

(1)
Finalmente para obtener la cinemtica directa
multiplicamos las 4 matrices de Denavit-Hartenberg(2)
y obtenemos una matriz de transformacin final la cual
nos proporcionara la cinemtica directa del robot .
(2)

Hoy en da el avance de la tecnologa se encuentra en


constante crecimiento. El desarrollo tecnolgico es un
aspecto estratgico para todo pas en vas de
crecimiento. La trascendencia del desarrollo cientfico
no se limita a sus consecuencias econmicas, tambin
contribuye a elevar la vida poltica y social, aumenta la
reflexin y conocimiento de la sociedad sobre s misma,
y por tanto la capacidad del pas para dirigir su propio
destino. Asimismo, favorece las posibilidades para que
la poblacin obtenga beneficios colectivos de gran
importancia, entre ellos, mejorar la salud y calidad de
vida.
En base a la experiencia de pases avanzados, el
desarrollo cientfico y tecnolgico influye de manera
significativa en la vida de sus habitantes, reflejndose
en la capacidad para crecer y absorber tecnologas ms
productivas. An ms, la superioridad de una nacin
depende de su capacidad para innovar tecnologa de
manera permanente, de tal forma que, con el avance de
la generacin y aplicacin de los conocimientos se
incrementa la calidad y los procesos de produccin se
vuelven ms complejos y especializados. Ambos
aspectos aumentan la demanda de mano de obra
calificada, misma que exige una remuneracin ms alta
y se abren ms oportunidades para ascender a mejores
puestos de trabajo en todo el aparato productivo. Lo
anterior repercute positivamente en la economa,
puesto que genera ahorro interno.

II.

DESCRIPCCION DEL MANIPULADOR


La estructura a analizar ser un robot esfrico
de tres grados de libertad, el dibujo
esquemtico en tridimensional (3D), se
obtendr mediante el software de diseo
mecnico SOLIDWORKS, debido a sus juntas
rotaciones y prismticas; y a su disposicin de
los ejes coordenados obtendremos un modelos
dinmico
resultante
ms sencillo y
manipulable para efectuar el control y se evite
redundancia en la medida de lo posible.
Esquema del robot se muestra en la siguiente
figura
A. MORFOLOGIA DEL ROBOT
El manipulador tendr
2 grados
rotacionales

B.

PARAMETROS
La utilidad del modelo dinamico resulta
importante cuando se dispone de los
valores numricos de todos los
parmetros del robot; bajo este enfoque en
la siguiente tabla se muestra la
terminologa, significado y valores de un
robot prototipo de 2 gdl.

Significado
Masa del eslabn 1
Longitud del eslabn 1

Notaci
n
m1
I1

Inercia del eslabn 1

Iz1

Inercia del eslabn 1

Iy1

Inercia del eslabn 1


Centro de masa del
eslabn 1
Coeficiente de friccin
viscosa
coeficiente de friccin de
Coulomb
Coeficiente de friccin
esttica
Masa del eslabn 2
Longitud del eslabn 2

Ix1

b1

0.091 m
2.288 Nmseg/rad

fc1

7.17 Nm

fe1
m2
I2

8.8 Nm
30 kg
0.45m
0.084 Nmseg2/rad
0.03 Nmseg2/rad
0.05 Nmseg2/rad

Ic1

Inercia del eslabn 2

Iz2

Inercia del eslabn 2

Iy2

Inercia del eslabn 2


Centro de masa del
eslabn 2
Coeficiente de friccin
viscosa
coeficiente de friccin de
Coulomb
Coeficiente de friccin
esttica

Ix2

III.

Valor
26.9 kg
0.45 m
1.266 Nmseg2/rad
0.089 Nmseg2/rad
0.03 Nmseg2/rad

b2

0.038 m
0.2 Nmseg/rad

fc2

1.9 Nm

Ic2

fe2

2.1 Nm

CINEMATICA DE MANIPULADORES

Los robots clsicos presentan una arquitectura


antropomrfica serial, semejante al brazo humano.
Consisten de una serie de barras rgidas unidas entre
s a travs de articulaciones de un grado de libertad
del tipo rotacional o prismtica. En general cada
articulacin logra su movimiento a travs de un
accionamiento de potencia e incluye otros
dispositivos como reductores de velocidad, frenos y

sensores de posicin o velocidad.


A.

CINEMATICA DIRECTA
La cinemtica directa se plantea en trminos de
encontrar una matriz
de transformacin que relaciona el sistema de
coordenadas ligado al cuerpo en
movimiento respecto a un sistema de coordenadas
que se toma como referencia. Para
lograr esta representacin se usan las matrices de
transformacin homognea 4x4, la
cual incluye las operaciones de traslacin y la
orientacin.
La matriz de transformacin homognea es una
matriz de 4x4 que transforma un
vector expresado en coordenadas homogneas
desde un sistema de coordenadas hasta
otro sistema de coordenadas.
Mediante el mtodo de Denavit Hartenberg y la
matriz de transformacin, obtenemos las
expresiones de la cinemtica directa.
Parmetros de DH (Denavit - Hartenberg).

Para obtener la cinemtica directa del robot


haremos uso de los siguientes programas en MATLAB:
robot_esferico_3dof_final.m
Obteniendo como resultado:

Modificamos el cdigo de directkinematic4 del


laboratorio 2, para nuestro modelo
% DIRECTKINEMATIC3
Direct
Kinematic.
%
A03 = DIRECTKINEMATIC3(Q) devuelve
la matriz de transformacin del
%
primer sistema de coordenadas al
ltimo en funcin del vector Q
%
de variables articulares.
%
%
See also DENAVIT.
function A03 = directkinematic3(q)
%
Parmetros Denavit-Hartenberg del
robot

teta
d
a
alfa

=
=
=
=

[q(1)
[0.45
[0
[pi/2

q(2)
0
0
pi/2

0
];
q(3)];
0
];
0
];

%
Matrices de transformacin
homognea entre sistemas de coordenadas
consecutivos
A01 = denavit(teta(1), d(1), a(1),
alfa(1));
A12 = denavit(teta(2), d(2), a(2),
alfa(2));
A23 = denavit(teta(3), d(3), a(3),
alfa(3));
%
Matriz de transformacin del primer
al ltimo sistema de coordenadas
A03 = A01 * A12 * A23;
Y corremos el programa para las coordenadas
articulares q=[0 0 0] Para obtener la matriz de
trasformacin homognea

II a. Cinemtica del Robot


Mediante el mtodo de Denavit Hartenberg y la
matriz de transformacin, obtenemos las
expresiones de la cinemtica directa.
Parmetros de DH (Denavit - Hartenberg).

Para obtener la cinemtica directa del robot


haremos uso de los siguientes programas en MATLAB:
robot_esferico_3dof_final.m
Obteniendo como resultado:

q=zeros(3,1);
T=directkinematic3(q)
T=
1.0000
0
0
0
B.

0
0
-1.0000 -0.0000
0.0000 -1.0000
0
0

0
0
0.4500
1.0000

II b. Dinmica del Robot


Para obtener la dinmica del robot se aplic el
Mtodo de LaGrange. Para obtener la siguiente
ecuacin:

CINEMATICA INVERSA

La cinemtica inversa consiste en hallar los valores de


las coordenadas articulares del robot [ ]T q q , q ,....,qn
= 1 2 conocida la posicin y orientacin del extremo
del robot.j Las tcnicas que se estudian aqu, se aplican
a un manipulador mecnico de cadena abierta y tratan
la
geometra del movimiento de un robot con respecto a
un sistema de referencia fijo como una funcin del
tiempo sin considerar la dinmica.

En este captulo se describir Diseo y Simulacin


de un sistema de control multivariable por par
computado aplicado a un manipulador robtico de
3DOF mediante tcnica PID . Es aqu donde debemos
recordar que nuestra meta final es lograr el control del
movimiento del manipulador robtico, el cual consiste
en determinar los pares aplicados a los servo
actuadores que forman las articulaciones, de tal forma
que las posiciones asociadas a las coordenadas
articulares del robot q(t) sigan con exactitud a la
posicin deseada qd(t) variante en el tiempo.

Los clculos de las matrices H, C y G, fueron


obtenidos de acuerdo a procedimientos enerales
explicados en diversos libros de robtica. A
continuacin se presenta el esquema en simulink
de la ecuacin presentada arriba
A continuacin mostramos el esquema en simulink
para representar al robot.

Un diagrama de nuestro controlador seria:

Este esquema luego es agrupado en un subsistema,


para realizar las simulaciones como se muestra en la
siguiente figura

III. PROGRAMACION MATLAB

III b. Control PID


El controlador PID es utilizado para mejorar la
respuesta dinmica del robot para una referencia de
posicin sobre el efector final, as como tambin para
eliminar el error en estado
estacionario de dicha posicin.
Este control contara con:
Control Proporcional: El controlador proporcional
produce un offset en la repuesta del sistema, esto es,
regula la ganancia en estado estacionario del sistema.

Control Integral: El controlador derivativo agrega un


polo en el origen, para eliminar el error (offset) en
estado estacionario.

Control Derivativo: El controlador derivativo agrega


un cero en el infinito para reducir o eliminar el sobre
impulso (overshoot).

La funcin de transferencia del controlador est


dada por la siguiente expresin:

IV.

PLANIFICACION DE TRAYECTORIAS

h
%
PLANIFICADOR Planificador de
trayectorias con interpolador 4-3-4.
%
%
[T,POS_PLAN, VEL_PLAN,
ACE_PLAN]=PLANIFICADOR(Q1,Q2) calcula
las matrices

%
de posicin, velocidad y
aceleracin de los puntos nudo que
representan
%
la planficiacin de
trayectorias entre los punto Q1 y Q2
cumpliendo con
%
las restricciones de
trayectoria suave y prestaciones de los
actuadores.
%
Utiliza los polinomios 4-3-4 en
los tres segmentos de trayectoria.
%
%
Ver tambin CALCULOCOEF,
SINCRONIZADOR, EVALPOS, EVALVEL,
EVALACEL.
function [t, pos_plan, vel_plan,
ace_plan] = planificador(q1,q2)
%********************************parme
tros de los
accionamientos**************
%------------------------------------------------------------------------% Especificaciones de los tiempos de
arranque y frenado de cada motor.
%------------------------------------------------------------------------tmotor = 0.1*ones(6,2);
%-----------------------------------------------------------------------%Velocidades Maxima de cada motor.
%-----------------------------------------------------------------------velmax =
[1.0472;1.0472;1.0472;1.0472;1.0472;1.0
472];

%-----------------------------------------------------------------------%Inicializacin de la escala de tiempo


y las matrices.
%-----------------------------------------------------------------------t = 0:0.01:(tmaximo+0.15);
% +0.15 se suma con el fin de aumentar
el intervalo de tiempo y muestrear
% todo el intervalo de frenado de la
articulacin, asumiendo las
% aproximaciones realizadas en la
funcin SINCRONIZADOR.
ini=zeros(length(t),1);
pos_plan(:,1)=ini;
vel_plan(:,1)=ini;
ace_plan(:,1)=ini;
%-----------------------------------------------------------------------% Clculo de los coeficientes de los
polinomios y evaluacin de los
% polinomios de interpolacin.
%-----------------------------------------------------------------------for i = 1:6
[caso,A,tt] =
calculocoef(i,velo2,q0,qf,tmotor);
posi=evalpos(t,tt,caso,A);
pos_plan(:,i)=posi';
ve=evalvel(t,tt,caso,A);
vel_plan(:,i)=ve';
ace=evalacel(t,tt,caso,A);
ace_plan(:,i)=ace';
end;
return

%********************************planif
icador
coordinado***********************
%------------------------------------------------------------------------% Inicializacin de los vectores
posicion - Velocidad - aceleracion.
%------------------------------------------------------------------------q = zeros(6,1);
q0 = [q1 q q];
qf = [q2 q q];
%-----------------------------------------------------------------------% Sincronizacin de los motores para
que realizen un movimiento coordinado
%-----------------------------------------------------------------------[velo2,tmaximo]=sincronizador(q0,qf,vel
max);

V.

DINAMICA DEL MANIPULADOR

El algoritmo del modelo Dinmico, del manipulador viene


dado por el siguiente algoritmo, con el siguiente programa:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Programa para Obtener el Modelo
Dinamico de un Maipulador %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all, close all, clc
syms
syms
syms
syms
syms
syms

t1 t2 real;
td1 td2 real;
tdd1 tdd2 real;
l1 l2 real;
m1 m2 real;
g real;

% Escribimos la matris con los


parametros dinamicos del manipulador

dyn = [0 0 0 0 0 2 3 0 0 0 0 0 0 0 0 0
1 0 0 0;
0 3 0 0 0 1 1 0 0 0 0 0 0 0 0 0
1 0 0 0];
q = [t1 t2];
qd = [td1 td2];
qdd = [tdd1 tdd2];
grav = [0 -g 0];
tau = rne(dyn, q, qd, qdd, grav)
simple(tau)
% Evaluar los terminos del par por
separado
M = inertia(dyn,q)
% Matriz de masas M(teta)
% Termino Gravitatorio
G(teta)
V = coriolis(dyn,q,qd)
% Terminos centriguos y de Coriolis V(
Resultado:
tau =
[ 19*tdd1 + tdd2 + cos(t2)*(3*tdd1 +
g*cos(t1)) + 3*cos(t2)*(tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))) +
3*sin(t2)*(sin(t2)*(3*tdd1 + g*cos(t1))
+ cos(t2)*(- 3*td1^2 + g*sin(t1)) (td1 + td2)^2) - sin(t2)*(- 3*td1^2 +
g*sin(t1)) + 6*g*cos(t1), tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))]
Warning: simple will be removed in a
future release. Use simplify instead.
> In sym.simple at 41
In Modelo_Dinamico at 22
simplify:
[ - 3*sin(t2)*td2^2 - 6*td1*sin(t2)*td2
+ 28*tdd1 + tdd2 + g*cos(t1 + t2) +
9*g*cos(t1) + 6*tdd1*cos(t2) +
3*tdd2*cos(t2), 3*sin(t2)*td1^2 + tdd1
+ tdd2 + g*cos(t1 + t2) +
3*tdd1*cos(t2)]
radsimp:

[ - 3*sin(t2)*td2^2 - 6*td1*sin(t2)*td2
+ 28*tdd1 + tdd2 + g*cos(t1 + t2) +
9*g*cos(t1) + 6*tdd1*cos(t2) +
3*tdd2*cos(t2), 3*sin(t2)*td1^2 + tdd1
+ tdd2 + g*cos(t1 + t2) +
3*tdd1*cos(t2)]
combine(sincos):
[ 19*tdd1 + tdd2 + cos(t2)*(3*tdd1 +
g*cos(t1)) + sin(t2)*(3*sin(t2)*(3*tdd1
+ g*cos(t1)) + 3*cos(t2)*(- 3*td1^2 +
g*sin(t1)) - 3*(td1 + td2)^2) sin(t2)*(- 3*td1^2 + g*sin(t1)) +
6*g*cos(t1) + cos(t2)*(3*tdd1 + 3*tdd2
+ 3*cos(t2)*(3*tdd1 + g*cos(t1)) 3*sin(t2)*(- 3*td1^2 + g*sin(t1))),
tdd1 + tdd2 + cos(t2)*(3*tdd1 +
g*cos(t1)) - sin(t2)*(- 3*td1^2 +
g*sin(t1))]
combine(sinhcosh):
[ 19*tdd1 + tdd2 + cos(t2)*(3*tdd1 +
g*cos(t1)) + 3*cos(t2)*(tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))) +
3*sin(t2)*(sin(t2)*(3*tdd1 + g*cos(t1))
+ cos(t2)*(- 3*td1^2 + g*sin(t1)) (td1 + td2)^2) - sin(t2)*(- 3*td1^2 +
g*sin(t1)) + 6*g*cos(t1), tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))]
combine(ln):
[ 19*tdd1 + tdd2 + cos(t2)*(3*tdd1 +
g*cos(t1)) + 3*cos(t2)*(tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))) +
3*sin(t2)*(sin(t2)*(3*tdd1 + g*cos(t1))
+ cos(t2)*(- 3*td1^2 + g*sin(t1)) (td1 + td2)^2) - sin(t2)*(- 3*td1^2 +
g*sin(t1)) + 6*g*cos(t1), tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))]
factor:

[ 19*tdd1 + tdd2 + cos(t2)*(3*tdd1 +


g*cos(t1)) + 3*cos(t2)*(tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))) +
3*sin(t2)*(sin(t2)*(3*tdd1 + g*cos(t1))
+ cos(t2)*(- 3*td1^2 + g*sin(t1)) (td1 + td2)^2) - sin(t2)*(- 3*td1^2 +
g*sin(t1)) + 6*g*cos(t1), tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))]

[ 19*tdd1 + tdd2 + 9*tdd1*cos(t2)^2 3*td2^2*sin(t2) + 9*tdd1*sin(t2)^2 +


6*g*cos(t1) + 6*tdd1*cos(t2) +
3*tdd2*cos(t2) - 6*td1*td2*sin(t2) +
g*cos(t1)*cos(t2) - g*sin(t1)*sin(t2) +
3*g*cos(t1)*cos(t2)^2 +
3*g*cos(t1)*sin(t2)^2, 3*sin(t2)*td1^2
+ tdd1 + tdd2 + 3*tdd1*cos(t2) +
g*cos(t1)*cos(t2) - g*sin(t1)*sin(t2)]

simplify(Steps = 100):

expand:

[ 19*tdd1 + tdd2 + 9*tdd1*cos(t2)^2 3*td2^2*sin(t2) + 9*tdd1*sin(t2)^2 +


6*g*cos(t1) + 6*tdd1*cos(t2) +
3*tdd2*cos(t2) - 6*td1*td2*sin(t2) +
g*cos(t1)*cos(t2) - g*sin(t1)*sin(t2) +
3*g*cos(t1)*cos(t2)^2 +
3*g*cos(t1)*sin(t2)^2, 3*sin(t2)*td1^2
+ tdd1 + tdd2 + 3*tdd1*cos(t2) +
g*cos(t1)*cos(t2) - g*sin(t1)*sin(t2)]
combine:
[ 19*tdd1 + tdd2 + cos(t2)*(3*tdd1 +
g*cos(t1)) + 3*cos(t2)*(tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))) +
3*sin(t2)*(sin(t2)*(3*tdd1 + g*cos(t1))
+ cos(t2)*(- 3*td1^2 + g*sin(t1)) (td1 + td2)^2) - sin(t2)*(- 3*td1^2 +
g*sin(t1)) + 6*g*cos(t1), tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))]
rewrite(exp):
[ 19*tdd1 + tdd2 + 6*g*(exp(-t1*i)/2 +
exp(t1*i)/2) + (3*tdd1 + g*(exp(t1*i)/2 + exp(t1*i)/2))*(exp(-t2*i)/2 +
exp(t2*i)/2) + 3*(exp(-t2*i)/2 +
exp(t2*i)/2)*(tdd1 + tdd2 + (3*tdd1 +
g*(exp(-t1*i)/2 + exp(t1*i)/2))*(exp(t2*i)/2 + exp(t2*i)/2) - ((exp(t2*i)*i)/2 - (exp(t2*i)*i)/2)*(3*td1^2 + g*(exp(t1*(-i))*(i/2) +
exp(t1*i)*(-i/2)))) + 3*((exp(t2*i)*i)/2 - (exp(t2*i)*i)/2)*((3*tdd1
+ g*(exp(-t1*i)/2 +
exp(t1*i)/2))*((exp(-t2*i)*i)/2 (exp(t2*i)*i)/2) - (td1 + td2)^2 +
(exp(-t2*i)/2 + exp(t2*i)/2)*(- 3*td1^2
+ g*(exp(t1*(-i))*(i/2) + exp(t1*i)*(i/2)))) - ((exp(-t2*i)*i)/2 (exp(t2*i)*i)/2)*(- 3*td1^2 +
g*(exp(t1*(-i))*(i/2) + exp(t1*i)*(i/2))), tdd1 + tdd2 + (3*tdd1 +
g*(exp(-t1*i)/2 + exp(t1*i)/2))*(exp(t2*i)/2 + exp(t2*i)/2) - ((exp(t2*i)*i)/2 - (exp(t2*i)*i)/2)*(3*td1^2 + g*(exp(t1*(-i))*(i/2) +
exp(t1*i)*(-i/2)))]
rewrite(sincos):
[ 19*tdd1 + tdd2 + cos(t2)*(3*tdd1 +
g*cos(t1)) + 3*cos(t2)*(tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))) +
3*sin(t2)*(sin(t2)*(3*tdd1 + g*cos(t1))
+ cos(t2)*(- 3*td1^2 + g*sin(t1)) (td1 + td2)^2) - sin(t2)*(- 3*td1^2 +
g*sin(t1)) + 6*g*cos(t1), tdd1 + tdd2 +

cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))]


rewrite(sinhcosh):
[ 19*tdd1 + tdd2 - sinh(t2*i)*(3*td1^2
+ g*sinh(t1*i)*i)*i +
sinh(t2*i)*(cosh(t2*i)*(3*td1^2 +
g*sinh(t1*i)*i) + (td1 + td2)^2 +
sinh(t2*i)*(3*tdd1 +
g*cosh(t1*i))*i)*3*i +
cosh(t2*i)*(3*tdd1 + g*cosh(t1*i)) +
3*cosh(t2*i)*(tdd1 + tdd2 sinh(t2*i)*(3*td1^2 + g*sinh(t1*i)*i)*i
+ cosh(t2*i)*(3*tdd1 + g*cosh(t1*i))) +
6*g*cosh(t1*i), tdd1 + tdd2 sinh(t2*i)*(3*td1^2 + g*sinh(t1*i)*i)*i
+ cosh(t2*i)*(3*tdd1 + g*cosh(t1*i))]
rewrite(tan):
[ 19*tdd1 + tdd2 +
(2*tan(t2/2)*(3*td1^2 (2*g*tan(t1/2))/(tan(t1/2)^2 +
1)))/(tan(t2/2)^2 + 1) - ((tan(t2/2)^2
- 1)*(3*tdd1 - (g*(tan(t1/2)^2 1))/(tan(t1/2)^2 + 1)))/(tan(t2/2)^2 +
1) - (6*g*(tan(t1/2)^2 1))/(tan(t1/2)^2 + 1) - (3*(tan(t2/2)^2
- 1)*(tdd1 + tdd2 +
(2*tan(t2/2)*(3*td1^2 (2*g*tan(t1/2))/(tan(t1/2)^2 +
1)))/(tan(t2/2)^2 + 1) - ((tan(t2/2)^2
- 1)*(3*tdd1 - (g*(tan(t1/2)^2 1))/(tan(t1/2)^2 + 1)))/(tan(t2/2)^2 +
1)))/(tan(t2/2)^2 + 1) +
(6*tan(t2/2)*((2*tan(t2/2)*(3*tdd1 (g*(tan(t1/2)^2 - 1))/(tan(t1/2)^2 +
1)))/(tan(t2/2)^2 + 1) - (td1 + td2)^2
+ ((tan(t2/2)^2 - 1)*(3*td1^2 (2*g*tan(t1/2))/(tan(t1/2)^2 +
1)))/(tan(t2/2)^2 + 1)))/(tan(t2/2)^2 +
1), tdd1 + tdd2 + (2*tan(t2/2)*(3*td1^2
- (2*g*tan(t1/2))/(tan(t1/2)^2 +
1)))/(tan(t2/2)^2 + 1) - ((tan(t2/2)^2
- 1)*(3*tdd1 - (g*(tan(t1/2)^2 1))/(tan(t1/2)^2 + 1)))/(tan(t2/2)^2 +
1)]
mwcos2sin:
[ 19*tdd1 + tdd2 - (3*tdd1 g*(2*sin(t1/2)^2 - 1))*(2*sin(t2/2)^2 1) - 6*g*(2*sin(t1/2)^2 - 1) sin(t2)*(- 3*td1^2 + g*sin(t1)) 3*(2*sin(t2/2)^2 - 1)*(tdd1 + tdd2 (3*tdd1 - g*(2*sin(t1/2)^2 1))*(2*sin(t2/2)^2 - 1) - sin(t2)*(3*td1^2 + g*sin(t1))) - 3*sin(t2)*((3*td1^2 + g*sin(t1))*(2*sin(t2/2)^2 1) - sin(t2)*(3*tdd1 - g*(2*sin(t1/2)^2
- 1)) + (td1 + td2)^2), tdd1 + tdd2 (3*tdd1 - g*(2*sin(t1/2)^2 -

1))*(2*sin(t2/2)^2 - 1) - sin(t2)*(3*td1^2 + g*sin(t1))]

[3]
[4]

collect(t1):

[5]

[ 19*tdd1 + tdd2 + cos(t2)*(3*tdd1 +


g*cos(t1)) + 3*cos(t2)*(tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))) +
3*sin(t2)*(sin(t2)*(3*tdd1 + g*cos(t1))
+ cos(t2)*(- 3*td1^2 + g*sin(t1)) (td1 + td2)^2) - sin(t2)*(- 3*td1^2 +
g*sin(t1)) + 6*g*cos(t1), tdd1 + tdd2 +
cos(t2)*(3*tdd1 + g*cos(t1)) sin(t2)*(- 3*td1^2 + g*sin(t1))]

[6]
[7]
[8]
[9]
[10]

[11]
[12]
[13]

ans =

[14]

[ - 3*sin(t2)*td2^2 - 6*td1*sin(t2)*td2
+ 28*tdd1 + tdd2 + g*cos(t1 + t2) +
9*g*cos(t1) + 6*tdd1*cos(t2) +
3*tdd2*cos(t2), 3*sin(t2)*td1^2 + tdd1
+ tdd2 + g*cos(t1 + t2) +
3*tdd1*cos(t2)]

[15]
[16]
[17]

.
M =
[ 3*cos(t2) + 9*sin(t2)^2 +
3*cos(t2)*(3*cos(t2) + 1) + 19,
3*cos(t2) + 1]
[
3*cos(t2) + 1,
1]
V =
[ 3*td1^2*sin(t2) 3*sin(t2)*(3*td1^2*cos(t2) + (td1 +
td2)^2) + 9*td1^2*cos(t2)*sin(t2),
3*td1^2*sin(t2)]

VI.

Mediante el modelamiento cinemtico y dinmico del


manipulador, es posible crear un controlador
multivariable, debido a todas las ecuaciones
dinmicas generados por los algoritmos mostrados

VII.
[1]

[2]

CONCLUSIONES

REFERENCIAS

Servicio de Informacin y Noticias Cientficas. (2012). Las


sustancias qumicas peligrosas causan ms muertes que los
accidentes de trabajo. Espaa.
Ing. Andrs Gonzales, PhD. Cristhian Duran: El TMR-1. Un robot
mvil teleoperado Revista Colombiana de tecnologas avanzadas
2009.

Wabesen, Bluetooth HC-06 Datasheet, http://www.mcuturkey.com/wp-content/uploads/2013/01/HC-Serial-BluetoothProducts-201104.pdf


Microchip Data Sheet PIC16F87XA 28/40/44-Pin Enhanced Flash
Microcontrollers.
http://vegard.hammerseth.com/2009/03/school-project-mechanicalhand/395/
http://robots-rgentina.com.ar/Actuadores_manos.html
http://inmoov.blogspot.com/
Mikel Izquierdo, Biomecnica y bases neuromusculares de la
actividad fsica y el deporte, Mdica Panamericana, 2008.
Comite Espaol de Automatica, "Libro blanco de la robotica",
CEA-GTRob, 2008.
SpectraSimbol,
Flex
sensor
Datasheet,
https://www.sparkfun.com/datasheets/Sensors/Flex/FLEXSENSOR
(REVA1).pdf
Stephen L. Herman., Electronics for Electricians, Cengage
Learning, 2012.
Ramn Palls Areny: Sensores y Acondicionadores de Seal,
Alfaomega, 2007.
Robin
Abbott,
PIC
C
Compiler
Developments,
http://www.fored.co.uk/html/Manual.pdf. Junio 2010.
Rafael
Enriquez
H.,
Arduino
User
Manual,
http://creativecommons.org/licenses/by-nc-a/3.0/.CreativeCommon,
2009
Michael Margolis, Arduino Cookbook. OReilly Media, 2011.
Reza N. Jazar, Theory of Applied Robotics, Springer, 2010
Katsuhiko Ogata, Ingeniera de Control Moderna, Pearson
Education, 2010.