Você está na página 1de 6

Facultad de Ingeniera de la Universidad

Nacional Autnoma De Mxico.





Laboratorio de control de robots
industriales.

Reporteprctica6
ControlPIDdelRobotA465

Elaborado por:
Armando Delgado del Ro

Grupo de teora: 1
Grupo de laboratorio: 1

Profesora:
M.I. Alejandra Icxiuh De La Guerra Carrasco

Lunes, 12 de mayo de 2014


Desarrollo.
En esta prctica se dise un control PID para el robot A465 de CRS Robotics, controlando
nicamente la posicin del mismo, es decir solo se controlaron las 3 primeras articulaciones.
En la siguiente se muestra un esquema del robot.



Acti vidad 1 Diseo de la trayectoria a seguir.
Como primer paso se dise una trayectoria sin colisiones ni singularidades, que
sigue los puntos de un polinomio y cumple con las restricciones que se ocuparon en
la prctica pasada (rapidez y aceleracin al comienzo y final de la trayectoria es
cero), cuyos puntos inicial y final son:

=
0.538
0
0.634
;

=
0
0.5
0.4


A continuacin se muestra el cdigo Tray.m
%Tr ayect or i a a segui r
f unct i on [ P] =Tr ay( t f )

gl obal b3x b3y b3z b4x b4y b4z b5x b5y b5z pf pi t f

%Ci cl o f or par a el cl cul o de l os punt os en coor denadas car t esi anas.
f or t =1: t f
px=pi ( 1, 1) +b3x*t ^3+b4x*t ^4+b5x*t ^5;
py=pi ( 2, 1) +b3y*t ^3+b4y*t ^4+b5y*t ^5;
pz=pi ( 3, 1) +b3z*t ^3+b4z*t ^4+b5z*t ^5;
end
P=[ px py pz] ;
end

nota: todas las constantes se declararon en el archivo de datos con el fin de agilizar los
clculos en tiempo real que realiza la computadora.

Acti vidad 2. Verificacin de la trayectoria.
Ejecutamos el programa de la simulacin en bloque VR Sink, el cual consta del
movimiento del robot hacia una referencia como no hay lazo de control en la simulacin. El
brazo cae como si fuera un pndulo. La siguiente imagen es una captura de la simulacin
en el bloque VR Sink.

Acti vidad 3. Control PID.
En el archivo sistema.mdl se program un controlador PID, tomando en cuenta que la
referencia es la cinemtica inversa de la trayectoria programada anteriormente, diagrama
de bloques se muestra a continuacin:



El bloque bloques PID se sintonizaron a prueba y error considerando que se debe
obtener un error mnimo y un sobrepaso mximo del 5% de la magnitud de la referencia.
Eso se logra sabiendo que la ganancia proporcional se encarga casi de todo el control,
mientras que las otras dos constantes son el ajuste fino. Dado que es una simulacin es
posible darle cualquier ganancia a estas constantes sin miedo a daar nada.

A continuacin se muestran las ganancias utilizadas:


En la siguiente imagen se muestra el error obtenido en cada una de las
articulaciones:
Archivo datos.m:
cl ear al l ;
cl c;
%Par met r os del r obot A465
gl obal d1 a2 a3 d13 d14
d1 = 0. 33;
a2 = 0. 305;
d13 = 0. 33;
d14 = 0. 208;
a3 = d13 + d14;



gl obal p1 p2 p3 p4 p5 p6 p7 p8 p9 p10 p11 p12 p13 p14 p15 p16 p17 p18
p1 = 0. 0055;
p2 = 0. 008;
p3 = 0. 0024;
p4 = 0. 0118;
p5 = 0. 0041;
p6 = 0. 0009;
p7 = 0. 0007;
p8 = 2. 0007;
p9 = 11. 8;
p10 = 2. 8;
p11 = 25;
p12 = 35;
p13 = 36;
p14 = 0. 2;
p15 = 2. 5;
p16 = 2. 5;
p17 = 22;
p18 = 11;


%Ti empo de muest r eo
gl obal T
T = 0. 005;

gl obal q0 qp0
q0 = [ 0 pi / 2 - pi / 2] ' ;
qp0 = [ 0 0 0] ' ;

%Const ant es par a l a t r ayect or i a
gl obal b3x b3y b3z b4x b4y b4z b5x b5y b5z pf pi t f
t f =5;
pi =[ 0. 538 0. 000 0. 634] ' ;
pf =[ 0. 000 0. 500 0. 400] ' ;

b3x=10*( pf ( 1, 1) - pi ( 1, 1) ) / ( t f ^3) ;
b3y=10*( pf ( 2, 1) - pi ( 2, 1) ) / ( t f ^3) ;
b3z=10*( pf ( 3, 1) - pi ( 3, 1) ) / ( t f ^3) ;
b4x=- 15*( pf ( 1, 1) - pi ( 1, 1) ) / ( t f ^4) ;
b4y=- 15*( pf ( 2, 1) - pi ( 2, 1) ) / ( t f ^4) ;
b4z=- 15*( pf ( 3, 1) - pi ( 3, 1) ) / ( t f ^4) ;
b5x=6*( pf ( 1, 1) - pi ( 1, 1) ) / ( t f ^5) ;
b5y=6*( pf ( 2, 1) - pi ( 2, 1) ) / ( t f ^5) ;
b5z=6*( pf ( 3, 1) - pi ( 3, 1) ) / ( t f ^5) ;

Archivo inversa.m
%Ci nemt i ca i nver sa a465

f unct i on [ q] = i nver sa( u)

gl obal d1 a2 a3

x = u( 1) ;
y = u( 2) ;
z = u( 3) ;

D = ( ( z- d1) *( z- d1) + x*x + y*y - a2*a2 - a3*a3) / ( 2*a2*a3) ;

q1 = at an2( y, x) ;
q3 = at an2( - sqr t ( 1 - D*D) , D) ;
q2 = at an2( z- d1, sqr t ( x*x + y*y) ) - at an2( a3*si n( q3) , a2+a3*cos( q3) ) ;
q = [ q1; q2; q3] ;

Archivo directa.m
%Ci nemt i ca di r ect a a465

f unct i on [ x] = di r ect a( q)

gl obal d1 a2 a3

q1 = q( 1) ;
q2 = q( 2) ;
q3 = q( 3) ;
c1 = cos( q1) ;
s1 = si n( q1) ;
c2 = cos( q2) ;
s2 = si n( q2) ;
s23 = si n( q2+q3) ;
c23 = cos( q2+q3) ;
x( 1) = a2*c1*c2 + a3*c1*c23;
x( 2) = a2*s1*c2 + a3*s1*c23;
x( 3) = d1 + a2*s2 + a3*s23;

Conclusiones:
En esta prctica pudimos simular el control PID de un robot antropomrfico. La
importancia de la simulacin radica en la sintonizacin puede resultar compleja y
adems peligrosa, dado que los movimientos del robot pueden resultar inestables.
Mediante la simulacin se disminuyen los riesgos tomando en cuenta que solo es
una simulacin y se deben tomar las precauciones necesarias.