CONTROL CINEMTICA DE VELOCIDAD

ROBOT DE 3GDL
Alvarado oyola jose maximo
Universidad nacional el callao | Robtica | Ing Bustinza

Resumen

Este archivo contiene la simulacin del robot tipo scara de 3gdl, las
simulaciones fueron realizadas en simulink. Se muestra la cinemtica directa y
la cinemtica inversa y el diseo de los controladores para cada uno.

1. Introduccin

El conocer previamente el comportamiento de algn robot manipulador es un requisito en

cualquier aplicacin ya que de esta manera se pueden predecir fallas que causaran la mala
operacin de dichos robots.

2. Objetivos

Realizar la simulacin del comportamiento de 3gdl del robot manipulador tipo scara
aplicando un controlador cinemtica de velocidad.

La siguiente simulacin es para el robot scara de 3 GDL (figura 1). Para realizar esta
simulacin se sigue la misma metodologa que el robot de 2 GDL.

Figura 1: Robot Tipo scara de 3 grados de libertad.

Cinemtica directa:

= 1 (1 ) + 2 (1 + 2 )..(1)
= 1 (1 ) + 2 (1 + 2 )...(2)
= 3 (3)

Cinemtica inversa:

2 (2 )
1 = 1 ( ) 1 ( )...(4)
1 +2 (2 )
12
2 = 1 ( )(5)
2 + 2 1 2 2 2
=
2 1 2

Cinemtica diferencial directa:

= 1 1 sin(1 ) 2 (1 + 2 )sin(1 + 2 ).(6)

= 1 1 cos(1 ) + 2 (1 + 2 )cos(1 + 2 )(7)
= 3 ..(8)

1 sin(1 ) 2 sin(1 + 2 ) 2 sin(1 + 2 ) 0 1

[ ] = [ 1 cos(1 ) + 2 cos(1 + 2 ) 2 cos(1 + 2 ) 0] [2 ]
0 0 1 3

El diseo del controlador es absolutamente el mismo que para el robot de 3GDL. Los
parmetros utilizados para esta simulacin fueron los siguientes:

En los ltimos aos han aparecido numerosos proyectos de investigacin relacionados con
disciplinas de simulacin virtual. Una correcta simulacin dinmica es necesaria como primer
paso para la obtencin de herramientas capaces de ser utilizadas para el anlisis y diseo de
robots. Adems de necesitar una metodologa numricamente correcta, es de gran
importancia el disponer de unas herramientas de visualizacin que permitan comprobar los
resultados obtenidos, de manera que la deteccin de errores sea lo ms intuitiva posible.
Siguiendo esta idea, en esta prctica se va a presentar una serie de ejemplos en los que se
utilizan los modelos dinmicos presentados anteriormente con la finalidad de poder realizar
un correcto control del robot estudiado.
En esta prctica se detalla la simulacin de un robot efectuando una planificacin del extremo
del robot en lnea recta entre dos posiciones del espacio cartesiano. Se va ha realizar el
ejemplo con el robot de 3 gdl,

SINTONIZADO DE LOS MOTORES.

CONTROL DESACOPLADO
SINTONIZADO DEL ROBOT DE TRES GRADOS DE LIBERTAD.
Vamos a suponer que el esquema de control de este robot es desacoplado, por lo que cada
articulacin dispondr de un regulador PID y durante el proceso de sintonizado de una
articulacin concreta, los actuadores del resto de articulaciones permanecern apagados.

MOTOR 1
Para realizar el sintonizado del motor 1 se ha desarrollado en Simulink el modelo que
se encuentra en el fichero sintonizar1d3.mdl. Este fichero contiene el esquema de control en
bucle cerrado de la primera articulacin que permite ajustar su regulador PID para que la
respuesta satisfaga las especificaciones. Si se ejecuta desde el entorno de Matlab
sintonizar1d3 aparecer en pantalla la aplicacin para el sintonizado de la primera
articulacin, que se muestra en la figura
En el bloque correspondiente al motor de la articulacin 1 se deben colocar los datos de
los parmetros del motor que se seleccion en la practica 4 para esa articulacin. Estos
datos se muestran en la tabla

se muestra como vara la posicin de la articulacin 1 ante el escaln introducido. Como se

puede apreciar en la grfica, con el regulador escogido se satisfacen los requerimientos
impuestos. Figura

MOTOR 2
Para realizar el sintonizado del motor 2 se utiliza el modelo implementado en Simulink
que se encuentra en el fichero sintonizar2d3.mdl.
MOTOR 3
Para realizar el sintonizado del motor 3 se utiliza el modelo implementado en Simulink que se
encuentra en el fichero sintonizar3d3.mdl. Este modelo contiene el esquema de control en
bucle cerrado de la tercera articulacin que permite ajustar su regulador PID para que la
respuesta satisfaga las especificaciones.
SIMULACIN DEL ROBOT DE 3 GDL.
Se ha realizado la simulacin del robot de 3 GDL para que realice una planificacin de
su extremo en lnea recta entre dos puntos del espacio cartesiano. La planificacin
realizada nicamente es en posicin, de manera que el robot parte de la posicin inicial
con velocidad nula. Para realizar la planificacin se ha dividido la trayectoria en un
nmero determinado de intervalos, de forma que en cada iteracin de la simulacin, se
proporciona la posicin que debe alcanzar el extremo del robot.

Los valores de los motores del robot que se utilizan para realizar la simulacin son
aquellos que se emplearon en el proceso de sintonizado mostrado

La simulacin en Matlab:

% DENAVIT Matriz de transformacin homognea.

% DH = DENAVIT(TETA, D, A, ALFA) devuelve la matriz de transformacion
% homognea 4 x 4 a partir de los parametros de Denavit-Hartenberg
% D, ALFA, A y TETA.
%
% See also DIRECTKINEMATIC.

function dh=denavit(teta, d, a, alfa)

dh=[cos(teta) -cos(alfa)*sin(teta) sin(alfa)*sin(teta) a*cos(teta);
 sin(teta) cos(alfa)*cos(teta) -sin(alfa)*cos(teta) a*sin(teta);
0 sin(alfa) cos(alfa) d;
0 0 0 1];
end

% DRAWROBOT3D4 Representacin 3D de un robot.

% DRAWROBOT3D3(Q) realiza una representacin 3D de un robot
% en funcin del vector de variables articulares Q.
%
% See also DENAVIT, DIRECTKINEMATIC4.

function drawrobot3d3(q)

% Parmetros Denavit-Hartenberg del robot

teta = [q(1) 0 0 ];
d = [0.4 q(2) q(3) ];
a = [0 -0.1 0 ];
alfa = [0 -pi/2 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));
A34 = denavit(teta(4), d(4), a(4), alfa(4));

% Matrices de transformacin del primer sistema al correspondiente

A02 = A01 * A12;
A03 = A02 * A23;
A04 = A03 * A34;

% Vector de posicion (x, y, z) de cada sistema de coordenadas

x0 = 0; y0 = 0; z0 = 0;
x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4);
xi = x1; yi = y1; zi = z1 + d(2);
x2 = A02(1,4); y2 = A02(2,4); z2 = A02(3,4);
x3 = A03(1,4); y3 = A03(2,4); z3 = A03(3,4);

% Se dibuja el robot
x = [x0 x1 xi x2 x3 x4];
y = [y0 y1 yi y2 y3 y4];
z = [z0 z1 zi z2 z3 z4];
plot3(x,y,z);
% Se coloca una rejilla a los ejes
grid;
% Se establecen los lmites de los ejes
axis([-1.5 1.5 -1.5 1.5 0 1.5]);

% DIRECTKINEMATIC3 Direct Kinematic.

% A04 = 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 = [q(1) 0 0 ];
d = [0.4 q(2) q(3) ];
a = [0 -0.1 0 ];
alfa = [0 -pi/2 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 ;

% ANIMACION4 Animacin de la trayectoria de un robot de 3 GDL

% ANIMACION(MAT_Q) realiza la animacin de la trayectoria, expresada

% en la matriz MAT_Q, de un brazo robot de 3 GDL. MAT_Q contiene 3 filas
% y una columna para cada disposicin del robot.
%
% Ver tambin DRAWROBOT3D3.

function animacion3(mat_q)

% Parmetros Denavit-Hartenberg del robot. Los parmetros correspondientes

% a variables articulares aparecen con valor 0
teta = [0 0 0 ];
d = [0.4 0 0 ];
a = [0 -0.1 0 ];
alfa = [0 -pi/2 0 ];

% Vector de posicion (x, y, z) del sistema de coordenadas de referencia

x0 = 0; y0 = 0; z0 = 0;

% Se dibuja el sistema de coordenadas de referencia. Se asigna el modo XOR para borrar

% slo el robot dibujado anteriormente. Se utiliza un grosor de lnea de 2 unidades
p = plot3(x0,y0,z0,'EraseMode','xor','LineWidth',2);
% Se asigna una rejilla a los ejes
grid;
% Se establecen los lmites de los ejes
axis([-1.5 1.5 -1.5 1.5 0 1.5]);
% Mantiene el grfico actual
hold on;

% Nmero de columnas de la matriz

n = size(mat_q,2);

% Se dibuja la disposicin del robot correspondiente a cada columna

for i=1:n

% Variables articulares del brazo robot

q1 = mat_q(1,i);
q2 = mat_q(2,i);
q3 = mat_q(3,i);

% Matrices de transformacin homognea entre sistemas de coordenadas consecutivos

A01 = denavit(q1, d(1), a(1), alfa(1));
A12 = denavit(teta(2), q2, a(2), alfa(2));
A23 = denavit(teta(3), q3, a(3), alfa(3));

% Matrices de transformacin del primer sistema al correspondiente

A02 = A01 * A12;
A03 = A02 * A23;

% Vector de posicion (x, y, z) de cada sistema de coordenadas

x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4);
xi = x1; yi = y1; zi = z1 + q2;
x2 = A02(1,4); y2 = A02(2,4); z2 = A02(3,4);
x3 = A03(1,4); y3 = A03(2,4); z3 = A03(3,4);

% Se dibuja el robot
x = [x0 x1 xi x2 x3 x4];
y = [y0 y1 yi y2 y3 y4];
z = [z0 z1 zi z2 z3 z4];
set(p,'XData',x,'YData',y,'ZData',z);
% Se fuerza a MATLAB a actualizar la pantalla
drawnow;
end

% CININV3GDL Cinemtica inversa de un robot de 3GDL.

% Q = CININV3GDL(ENTRADA) devuelve el vector 3x1 de coordenadas
% articulares que contiene la solucin cinemtica inversa.
% ENTRADA es un vector 3x1 que representa la posicin expresada
% en coordenadas cartesianas.
%
% Ver tambin INVERSEKINEMATIC3.

function q = cininv3gdl(entrada)

% Posicin cartesiana
p = entrada;
% Orientacin deseada en el extremo del robot
n = [1 0 0]';
s = [0 0 -1]';
a = [0 1 0]';

% Matriz con la orientacin y posicin deseadas en el extremo del robot

T = [n s a entrada];

% Se calculan las coordenadas articulares

q = inversekinematic3(T);

% DH Matriz de rotacin.
% R = DH(TETA, ALFA) devuelve la matriz de rotacin 3x3 utilizando
% los parmetros de Denavit-Hartenberg TETA y ALFA.
%
% See also DENAVIT.

function R = dh(teta,alfa)

R = [cos(teta) -cos(alfa)*sin(teta) sin(alfa)*sin(teta)

sin(teta) cos(alfa)*cos(teta) -sin(alfa)*cos(teta)
0 sin(alfa) cos(alfa)];

% DINDIR3GDL Dinmica directa de un robot de 3GDL.

% QPP = DINDIR3GDL(ENTRADA) calcula el vector 3x1 de aceleracin de
% cada articulacin utilizando el tercer mtodo de Walker y Orin.
% ENTRADA(1:3) representa el par de entrada a cada articulacin.
% ENTRADA(4:6) es la posicin de cada articulacin. ENTRADA(7:9)
% es la velocidad de cada articulacin.
%
% Ver tambin WALKERORIN4.

function qpp = dindir3gdl(entrada)

tau = entrada(1:3); % Par de entrada a cada articulacin

q = entrada(4:6); % Posicin de cada articulacin
qp = entrada(7:9); % Velocidad de cada articulacin

% Parmetros de la carga
masaext = 10;
inerciaext = [0.0167 0 0;0 0.0167 0;0 0 0.0167];

% Se convierten los pares de la articulacin 2 y 3 en fuerzas.

n = 0.85; % Eficiencia husillo
p = 0.025; % Paso del husillo (mm)
tau(2:3) = 2*pi*tau(2:3)/p;

% Se calcula la aceleracin utilizando el mtodo de Walker y Orin.

qpp = walkerorin3(q,qp,tau,masaext,inerciaext);

% DININV3GDL Dinmica inversa de un robot de 3GDL.

% PAR = DININV3GDL(ENTRADA) calcula el vector 3x1 de pares/fuerzas de
% entrada a las articulaciones utilizando el mtodo de Newton-Euler.
% ENTRADA(1:3) representa la posicin de cada articulacin.
% ENTRADA(4:6) es la velocidad de cada articulacin. ENTRADA(7:9)
% es la aceleracin de cada articulacin.
%
% See also NEWTONEULER4.

function par = dininv3gld(entrada)

q = entrada(1:3); % Posicin de cada articulacin

qp = entrada(4:6); % Velocidad de cada articulacin
qpp = entrada(7:9); % Aceleracin de cada articulacin

% Parmetros de la carga
masaext = 10;
inerciaext = [0.0167 0 0;0 0.0167 0;0 0 0.0167];

% Se calcula el vector de pares/fuerzas utilizando Newton-Euler

par = newtoneuler3(q,qp,qpp,9.8,masaext,inerciaext);

% DRAWROBOT3D3 Representacin 3D de un robot.

% DRAWROBOT3D3(Q) realiza una representacin 3D de un robot
% en funcin del vector de variables articulares Q.
%
% See also DENAVIT, DIRECTKINEMATIC3.

function drawrobot3d3(q)

% Parmetros Denavit-Hartenberg del robot

teta = [q(1) 0 0 ];
d = [0.4 q(2) q(3) ];
a = [0 -0.1 0 ];
alfa = [0 -pi/2 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));

% Matrices de transformacin del primer sistema al correspondiente

A02 = A01 * A12;
A03 = A02 * A23;

% Vector de posicion (x, y, z) de cada sistema de coordenadas

x0 = 0; y0 = 0; z0 = 0;
x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4);
xi = x1; yi = y1; zi = z1 + d(2);
x2 = A02(1,4); y2 = A02(2,4); z2 = A02(3,4);
x3 = A03(1,4); y3 = A03(2,4); z3 = A03(3,4);

% Se dibuja el robot
x = [x0 x1 xi x2 x3 x4];
y = [y0 y1 yi y2 y3 y4];
z = [z0 z1 zi z2 z3 z4];
plot3(x,y,z);
% Se coloca una rejilla a los ejes
grid;
% Se establecen los lmites de los ejes
axis([-1.5 1.5 -1.5 1.5 0 1.5]);

% F_P Vector de fuerzas articulares.

% Y = F_P(IRI_1, IR0FIA, QPI, BI) calcula el vector 3x1 que representa
% la fuerza de entrada a la articulacin i. IRI_1 es la matriz de rotacin
% del sistema de coordenadas i-simo al (i-1)-simo. IR0FIA es el vector
% 3x1 de fuerza ejercida sobre el elemento i por el elemento i-1. QPI es
% la magnitud de la velocidad de la articulacin i-sima respecto del elemento
% i-1. BI es el coeficiente de amortiguamiento viscoso para la articulacin i.
%
% See also RI0FIA.

function y = f_p(iri_1, ir0fia, qpi, bi);

z = [0; 0; 1];
y = ir0fia'*(iri_1*z) + bi*qpi;

% H3 Matriz de momentos de inercia.

% H = H3(Q, MASAEXT, INERCIAEXT) calcula la matriz de momentos de
% inercia H 3x3 utilizando el tercer mtodo de Walker y Orin. Q es
% el vector 3x1 de variables articulares. MASAEXT es la masa de la
% carga externa. INERCIAEXT es la inercia de la carga externa.
%
% See also WALKERORIN4.

function h = h3(q, masaext, inerciaext)

% Parmetros Denavit-Hartenberg del robot

teta = [q(1) 0 0 ];
d = [0.4 q(2) q(3) ];
a = [0 -0.1 0 ];
alfa = [0 -pi/2 0 ];

% Masa de cada elemento (Kg)

m = [10; 5; 5 ];
% Matrices de Inercias Centroidales. (Kg-m^2.)
J = [ 0.0191 0 0; 0 0.0191 0; 0 0 0.0068;
0.0031 0 0; 0 0.0484 0; 0 0 0.0484;
0.0606 0 0; 0 0.0053 0; 0 0 0.0606;
0.0022 0 0; 0 0.0022 0; 0 0 0.0014];

% La quinta inercia es la de la carga externa.

J(13:15,1:3) = inerciaext;

% Vector Z0.
z0 = [0; 0; 1];

% Condiciones de Carga Externa.

M(5) = masaext;
cj_1j = zeros(3,1);
Ej_1j = J(13:15,1:3);

for j = 4:-1:1
% Constante para sacar la Inercia J.
k = (j-1)*3 + 1;

% Vectores p y s.
p = [a(j); d(j)*sin(alfa(j)); d(j)*cos(alfa(j))];
s = -0.5*p;

% Matrices de transformacion.
aj_1j = dh(teta(j),alfa(j));
ajj_1 = aj_1j';

% Centroide e Inercia de los elementos anteriores.

cjjM1 = cj_1j;
EjjM1 = Ej_1j;

% Masa de todos los elementos anteriores.

M(j) = M(j+1) + m(j);

% Nuevo centroide.
cjj = ((s + p)*m(j) + M(j+1)*(cjjM1 + p))/M(j);
cj_1j = aj_1j*cjj;

% Distancia de traslado de las inercias.

p1 = (cjjM1 + p - cjj);
d1 = dot(p1,p1)*eye(3) - (p1*p1');
p2 = (s + p - cjj);
d2 = dot(p2,p2)*eye(3) - (p2*p2');

% Nueva Inercia.
Ej_1j = aj_1j*(EjjM1 + M(j+1)*d1 + J(k:k+2,1:3) + m(j)*d2)*ajj_1;

% Fuerza y par de los elementos j hasta N.

if (j == 1) | (j == 4) % Articulaciones rotacionales
Fj_1j = cross(z0,M(j)*cj_1j);
Nj_1j = Ej_1j*z0;
else % Articulaciones prismticas
 Fj_1j = M(j)*z0;
Nj_1j = zeros(3,1);
end

% Elemento j. Componente H(j,j).

fi_1i = Fj_1j;
ni_1i = Nj_1j + cross(cj_1j, Fj_1j);
if (j == 1) | (j == 4) % Articulaciones rotacionales
h(j,j) = ni_1i(3,1);
else % Articulaciones prismticas
h(j,j) = fi_1i(3,1);
end

% Elementos j-1 hasta 1. Componentes H(1:j-1,j)

i = j - 1;
while i >= 1
% Vector p.
p = [a(i); d(i)*sin(alfa(i)); d(i)*cos(alfa(i))];

% Matrices de rotacion.
ai_1i = dh(teta(i),alfa(i));
aii_1 = ai_1i';

% Fuerza y par del anterior elemento.

fiiM1 = fi_1i;
niiM1 = ni_1i;

% Fuerza y par de este elemento.

fi_1i = ai_1i*fiiM1;
ni_1i = ai_1i*(niiM1 + cross(p, fiiM1));

% Componente H(i,j).
if (i == 1) | (i == 4) % Articulaciones rotacionales
h(i,j) = ni_1i(3,1);
else % Articulaciones prismticas
h(i,j) = fi_1i(3,1);
end

% H es simetrica.
h(j,i)= h(i,j);
i = i - 1;
end
end

% INVERSEKINEMATIC3 Inverse Kinematic

% Q = INVERSEKINEMATIC3(T) devuelve el vector de coordenadas
% articulares correspondiente a la solucin cinemtica inversa de
% la mano del manipulador en la posicin y orientacin expresadas
% en la matriz T.
%
% See also DIRECTKINEMATIC3, DENAVIT.

function q = inversekinematic3(T)
p = T(1:3,4); % Posicin de la mano del manipulador

% Inicializacin de las variables articulares a calcular

q1 = 0;
q2 = 0;
q3 = 0;

% Parmetros Denavit-Hartenberg del robot

teta = [q1 0 0 ];
d = [0.4 q2 q3 ];
a = [0 -0.1 0 ];
alfa = [0 -pi/2 0 ];

% Solucin de la primera articulacin: q1

R = sqrt(p(1)^2+p(2)^2);
r = sqrt(R^2-a(2)^2);

sphi = -p(1)/R;
cphi = p(2)/R;
phi = atan2(sphi, cphi);

sbeta = -a(2)/R;
cbeta = r/R;
beta = atan2(sbeta, cbeta);

q1 = phi - beta;

% Solucin de la segunda articulacin: q2

q2 = p(3) - d(1);

% Solucin de la tercera articulacin: q3

q3 = r - d(4);

% Vector de variables articulares

q = [q1 q2 q3 ]';

% NEWTONEULER3 Dinmica inversa utilizando el mtodo de Newton-Euler.

% TAU = NEWTONEULER3(Q, QP, QPP, G, M5, IEXTER) calcula el vector
% 3x1 de pares/fuerzas de entrada a las articulaciones. Q el vector
% 3x1 de coordenadas articulares. QP es el vector 3x1 que representa
% la velocidad de cada articulacin. QPP es el vector 3x1 que indica
% la aceleracin de cada articulacin. G es el valor de la gravedad (m/s^2).
% M5 es la masa de la carga externa(Kg) que transporta el brazo robot.
% IEXTER es la matriz 3x3 de inercia de la carga exterior(Kg-m^2).
%
% See also DH, RI0PI, RI0SI, RI0WI, RI0WIP, RI0VPI_R, RI0AI, RI0FI, RI0NI,
% RI0FIA, RI0NIA, T_R, F_P.

function tau = newtoneuler3(q,qp,qpp,g,m4,Iexter);

% ------------------------------------------------------------
% Parmetros Denavit-Hartenberg del robot
% ------------------------------------------------------------
teta = [q(1) 0 0 ];
d = [0.4 q(2) q(3) ];
a = [0 -0.1 0 ];
alfa = [0 -pi/2 0 ];

% ------------------------------------------------------------
% Factores de posicionamiento de los centros de gravedad
% ------------------------------------------------------------
factor1 = -0.5; factor2 = -0.5; factor3 = -0.5;

% ------------------------------------------------------------
% Masa de cada elemento (Kg)
% ------------------------------------------------------------
m1 = 10; m2 = 5; m3 = 5;

% ------------------------------------------------------------
% Coeficiente de rozamiento viscoso de cada articulacion
% ------------------------------------------------------------
b1 = 0.05; b2 = 0.05; b3 = 0.05;

% ------------------------------------------------------------
% Matrices de Inercia (Kg-m^2)
% ------------------------------------------------------------
r10I_r01 = [0.0191 0 0;0 0.0191 0;0 0 0.0068];
r20I_r02 = [0.0031 0 0;0 0.0484 0;0 0 0.0484];
r30I_r03 = [0.0606 0 0;0 0.0053 0;0 0 0.0606];

% ------------------------------------------------------------
% Vectores ri0pi, ri0si.
% ------------------------------------------------------------
r10p1 = ri0pi(a(1), d(1), alfa(1));
r20p2 = ri0pi(a(2), d(2), alfa(2));
r30p3 = ri0pi(a(3), d(3), alfa(3));
r40p4 = zeros(3,1);

r10s1 = ri0si(a(1), d(1), alfa(1), factor1);

r20s2 = ri0si(a(2), d(2), alfa(2), factor2);
r30s3 = ri0si(a(3), d(3), alfa(3), factor3);
r40s4 = zeros(3,1);

% ------------------------------------------------------------
% Matrices de transformacion
% ------------------------------------------------------------
r01 = dh(teta(1), alfa(1)); r10 = r01';
r12 = dh(teta(2), alfa(2)); r21 = r12';
r23 = dh(teta(3), alfa(3)); r32 = r23';
r43 = r34';
r34 = eye(3);

% ------------------------------------------------------------
% Velocidad angular de las articulaciones
% ------------------------------------------------------------
r00w0 = zeros(3,1);
r10w1 = ri0wi(r10, r00w0, qp(1));
r20w2 = r21*r10w1;
r30w3 = r32*r20w2;
r40w4 = ri0wi(r43, r30w3, 0);

% ------------------------------------------------------------
% Aceleracion angular de las articulaciones
% ------------------------------------------------------------
r00wp0 = zeros(3,1);
r10wp1 = ri0wpi(r10, r00wp0, r00w0, qp(1), qpp(1));
r20wp2 = r21*r10wp1;
r30wp3 = r32*r20wp2;
r40wp4 = ri0wpi(r43, r30wp3, r30w3, 0, 0);

% ------------------------------------------------------------
% Aceleracion lineal articular
% ------------------------------------------------------------
r00vp0 = [0; 0; g];
r10vp1 = ri0vpi_r(r10, r00vp0, r10wp1, r10w1, r10p1);
r20vp2 = ri0vpi_p(r21, r10vp1, r20wp2, r20w2, r20p2, qp(2), qpp(2));
r30vp3 = ri0vpi_p(r32, r20vp2, r30wp3, r30w3, r30p3, qp(3), qpp(3));
r40vp4 = ri0vpi_r(r43, r30vp3, r40wp4, r40w4, r40p4);

% ------------------------------------------------------------
% Aceleracion del centro de masa de cada elemento
% ------------------------------------------------------------
r10a1 = ri0ai(r10vp1, r10wp1, r10w1, r10s1);
r20a2 = ri0ai(r20vp2, r20wp2, r20w2, r20s2);
r30a3 = ri0ai(r30vp3, r30wp3, r30w3, r30s3);
r40a4 = ri0ai(r40vp4, r40wp4, r40w4, r40s4);

% ------------------------------------------------------------
% Fuerza en el centro de masa de cada elemento
% ------------------------------------------------------------

r40f4 = ri0fi(r40a4, m4);

r30f3 = ri0fi(r30a3, m3);
r20f2 = ri0fi(r20a2, m2);
r10f1 = ri0fi(r10a1, m1);

% ------------------------------------------------------------
% Par en el centro de masa de cada elemento
% ------------------------------------------------------------
r40n4 = ri0ni(r40wp4, r40w4, Iexter);

r30n3 = ri0ni(r30wp3, r30w3, r30I_r03);

r20n2 = ri0ni(r20wp2, r20w2, r20I_r02);
r10n1 = ri0ni(r10wp1, r10w1, r10I_r01);

% ------------------------------------------------------------
% Fuerzas articulares
% ------------------------------------------------------------
r40f4a = r40f4;
r30f3a = ri0fia(r34, r40f4a, r30f3);
r20f2a = ri0fia(r23, r30f3a, r20f2);
r10f1a = ri0fia(r12, r20f2a, r10f1);

% ------------------------------------------------------------
% Pares articulares
% ------------------------------------------------------------
r20p1 = r21*(r10p1); r30p2 = r32*(r20p2);
r40p3 = r43*(r30p3);

r40n4a = r40n4;
r30n3a = ri0nia(r34, r40n4a, r40f4a, r30n3, r30f3, r40p3, r30p3, r30s3);
r20n2a = ri0nia(r23, r30n3a, r30f3a, r20n2, r20f2, r30p2, r20p2, r20s2);
r10n1a = ri0nia(r12, r20n2a, r20f2a, r10n1, r10f1, r20p1, r10p1, r10s1);

% ------------------------------------------------------------
% Fuerzas y pares de accionamientos
% ------------------------------------------------------------
t_1 = t_r(r10, r10n1a, qp(1), b1);
t_2 = f_p(r21, r20f2a, qp(2), b2);
t_3 = f_p(r32, r30f3a, qp(3), b3);

tau = [t_1; t_2; t_3];

% PLANIFICA3 Planificacin de trayectorias en lnea recta de un robot de 3GDL

% Q = PLANIFICA3(ENTRADA) devuelve las coordenadas articulares
% correspondientes al instante actual de simulacin en una planificacin
% de trayectoria en lnea recta entre dos puntos cartesianos. ENTRADA(1)
% representa el instante de tiempo actual (seg). ENTRADA(2:4) es la
% posicin cartesiana inicial de la trayectoria. ENTRADA(5:7) es la
% posicin cartesiana final de la trayectoria.
%
% Ver tambin CININV3GDL.

function q = planifica3(entrada)

t = entrada(1); % Instante actual de simuacin (seg)

p1 = entrada(2:4); % Posicin cartesiana inicial
p2 = entrada(5:7); % Posicin cartesiana final

ts = 1; % Tiempo de simulacin (seg)

intervalo = 1e-4; % Intervalo de integracin (seg)

% Nmero de segmentos de la trayectoria

nseg = ts/intervalo;

% Clculo del vector unitario

u = p2-p1;
mu = sqrt(u(1)^2+u(2)^2+u(3)^2);
u = (1/mu)*u;

% Clculo de la distancia entre puntos

d = mu/nseg;
% Nmero de punto actual en la trayectoria (el inicial es 0)
i = t*nseg;

% Clculo de la posicin cartesiana actual de la mano del manipulador

ps = p1+(i*d)*u;

% Clculo de las coordenadas articulares

q = cininv3gdl(ps);

% WALKERORIN3 Tercer mtodo de Walker & Orin.

% QPP = WALKERORIN3(Q, QP, TAU, MASAEXT, INERCIAEXT) calcula la dinmica
% directa del robot de 3GDL devolviendo el vector 3x1 que representa la
% aceleracin de cada articulacin utilizando el tercer mtodo de Walker y Orin.
% Q es el vector 3x1 de variables articulares. QP es el vector 3x1 que
% representa la velocidad de cada articulacin. TAU es el vector 3x1
% que representa el par de entrada a cada articulacin. MASAEXT es
% la masa de la carga externa. INERCIAEXT es la inercia de la carga externa.
%
% See also NEWTONEULER3, H3.

function qpp = walkerorin3(q,qp,tau,masaext,inerciaext)

% Se calcula el vector b.
b = newtoneuler3(q,qp,zeros(3,1),9.8,masaext,inerciaext);

% Se calcula la matriz de momentos de inercia H.

H = h3(q,masaext,inerciaext);

% Se calcula el vector de aceleracin de cada articulacin.

qpp = inv(H)*(tau-b);

% WALKERORIN3 Tercer mtodo de Walker & Orin.

% QPP = WALKERORIN3(Q, QP, TAU, MASAEXT, INERCIAEXT) calcula la dinmica
% directa del robot de 3GDL devolviendo el vector 3x1 que representa la
% aceleracin de cada articulacin utilizando el tercer mtodo de Walker y Orin.
% Q es el vector 3x1 de variables articulares. QP es el vector 3x1 que
% representa la velocidad de cada articulacin. TAU es el vector 3x1
% que representa el par de entrada a cada articulacin. MASAEXT es
% la masa de la carga externa. INERCIAEXT es la inercia de la carga externa.
%
% See also NEWTONEULER3, H3.

function qpp = walkerorin3(q,qp,tau,masaext,inerciaext)

% Se calcula el vector b.
b = newtoneuler3(q,qp,zeros(3,1),9.8,masaext,inerciaext);

% Se calcula la matriz de momentos de inercia H.

H = h3(q,masaext,inerciaext);

% Se calcula el vector de aceleracin de cada articulacin.

qpp = inv(H)*(tau-b);
5. Conclusiones generales

En conclusin podemos decir que el control cinemtico de velocidad es una buena opcin para
controlar manipuladores aunque no la mejor ya que como se pudo ver tiene ciertas fallas por
milmetros, pero en realidad no es algo muy significativo.
Una vez finalizado el diseo y fabricacin del robot manipulador propuesto, y analizados el
proceso y los resultados obtenidos, se llega a las siguientes conclusiones:
Se dise y construy un prototipo de robot manipulador con tres grados de libertad para
fines educativos. El mismo puede ser utilizado para realizar prcticas de programacin de
robots manipuladores, anlisis de mecanismos, uso de sensores y actuadores, control de
procesos.
Tambin puede ser utilizado para la investigacin en el rea de robtica industrial, como
diseo de controladores especficos para tareas especficas, tele-operacin, colaboracin con
otros robots, etc.