DE

SC
RIP
CI
N
BR
EV
E

En
este
info
rme
anal
izar
emo
sy
cont
rola

Tabla de contenido
CONTROL PID DE UN MECANISMO ROBTICO DE 4 GRADOS DE LIBERTAD. . .4
INTRODUCCION:...................................................................................................... 4
MODELO:................................................................................................................. 4
CENTROS DE MASA Y MOMENTOS DE INERCIA DE LOS ESLABONES DE LA
ESTRUCTURA ROBTICA.......................................................................................... 5
DINAMICA DEL MECANISMO DE 4GDL.........................................................................7
INICIALIZACIN........................................................................................................ 7
DATOS OPTENIDOS DE LA GEOMTRIA DEL ROBOT................................................7
PARMETROS DEL SISTEMA MULTICUERPO PARA APLICAR EL ALGORITMO DE
DENAVIT-HARTENBERG............................................................................................ 8
MATRIZ ESLABN-ARTICULACIN............................................................................8
CINEMTICA DIRECTA.............................................................................................. 9
TENSORES DE INERCIA RESPECTO A SU CENTRO DE MASA....................................9
TENSORES DE INERCIA RESPECTO DE X0Y0Z0........................................................9
VECTORES DESDE X0Y0Z0 HACIA EL CM DE CADA ELEMENTO.............................10
VELOCIDAD LINEAL Y ANGULAR............................................................................ 10
TENSORES DE INERCIA REFLEJADOS.....................................................................11
ENERGA CINTICA:............................................................................................... 11
ENERGA POTENCIAL:............................................................................................ 11
VECTOR FUERZA O TORQUE..................................................................................11
FUERZA DE FRICCIN ARTICULAR.........................................................................11
VECTOR FUERZA O TORQUE..................................................................................12
EFECTO CENTRFUGO Y DE CORIOLES...................................................................12
EFECTO DE LA GRAVEDAD..................................................................................... 12
CLCULO DEL CONTROLADOR PID........................................................................... 13
REFERENCIAS PARA LA SIMULACIN.....................................................................14
PARMETROS PARA LA SIMULACIN......................................................................14
DIAGRAMA DE BLOQUES EN MATLAB........................................................................15
DIAGRAMA DE BLOQUES DEL MECANISMO ROBTICO:.........................................15
MECANISMO IMPORTADO DE SOLIDWORKS A MATLAB MEDIANTE SIMMECHANIC. 15
INTERFAZ QUE PERMITE MODIFICAR LAS SEALES DE CONTROL A CADA
ARTICULACIN POR SEPARADO............................................................................. 16
RESULTADOS............................................................................................................. 17
POSICIN:.............................................................................................................. 17

VELOCIDAD............................................................................................................ 17
TORQUE:................................................................................................................ 18
ARICULACION 1:................................................................................................. 18
ARICULACION 2:................................................................................................. 19
ARICULACION 3:................................................................................................. 19
ARICULACION 4:................................................................................................. 19
CONCLUSIONES........................................................................................................ 20

CONTROL PID DE UN MECANISMO ROBTICO

DE 4 GRADOS DE LIBERTAD
INTRODUCCION:
En la industria la rapidez y precisin con la que se ejecutan las operaciones manuales
o automticas en la cadena de produccin puede ser determinante en el xito de una
empresa. Por ende como futuros Ingenieros Mecatrnicos estamos en la obligacin de
desarrollar mecanismo automticos que permitan mejorar y optimizar tales procesos de
produccin. En este informe analizaremos y controlaremos mediante la tcnica de
control PID un mecanismo robtico de 4 grados de libertad.

MODELO:

CENTROS DE MASA Y MOMENTOS DE INERCIA DE LOS ESLABONES DE

LA ESTRUCTURA ROBTICA

[
[
[
[

][
][
][
][

I xx 1 I xy 1 I xz 1
2.19
1
0.25
I 1 = I yx 1 I yy 1 I yz 1 = 1
3.77 0.13
0.25
0.13 3.96
I zx 1 I zy 1 I zz1

I xx 2 I xy 2 I xz 2
5.2 0.001 0.0026
I 2 = I yx 2 I yy 2 I yz 2 = 0.001 5.336 0.01
0.002 0.01 0.500
I zx 2 I zy 2 I zz2
I xx3 I xy3 I xz 3
1
0.27 0.4
I 3 = I yx 3 I yy 3 I yz 3 = 0.27 5.56 0.04
0.4 0.04 5.55
I zx 3 I zy 3 I zz 3
I xx 1 I xy 1 I xz1
0.4 0.01 0
I 4= I yx 1 I yy 1 I yz1 = 0.01 0.02 0
0
0
0.4
I zx 1 I zy 1 I zz 1
M 1=78.3 M 2 =54.1 M 3=62.1 M 4=6.80

DINAMICA DEL MECANISMO DE 4GDL

Las operaciones matemticas para determinar la dinmica de mecanismo, estn codificadas en
el archivo Dinamica.m, el cual al ejecutarlo, automticamente inicia la simulacin.
En todos los clculos se considera los motores DC de control por armadura, adems se
considera sus masas y sus momentos de inercia.
Al iniciar la ejecucin la simulacin se abrir una interface en la que se puede seleccionar entre
dos tipos de seales de entrada, as como sus respectivas frecuencias.

A continuacin presentamos algunos clculos importantes del proceso:

%-----------------------------------------------------------------% %%%%%%%%%% TERCERA PRCTICA DE CONTROL DE ROBOTOS %%%%%%%%%%%%%%%

%
% CONTROL PID DE UN UN MECANISMO ROBTICO DE 4 GRADOS DE LIBERTAD.
%
% ALUMNO: FERNNDEZ VILLANUEVA DAVID
% COD:

20081028B

% SECCIN: A
%
%-------------------------------------------------------------------

INICIALIZACIN
clc; clear all; close all; format short;

DATOS OPTENIDOS DE LA GEOMTRIA DEL ROBOT

g=[0;0;-9.81];disp(g);
L1=0.3125; L2=0.25; L3=0.3; L4=0.3;
M1=1.165736; M2=0.34181593; M3=0.20006593; M4=0.25149512;
M=[M1;M2;M3;M4]; disp(M);
% Variables y sus derivadas

syms q1 q2 q3 q4;
syms dq1 dq2 dq3 dq4 ddq1 ddq2 ddq3 ddq4 ;

PARMETROS DEL SISTEMA MULTICUERPO PARA APLICAR EL

ALGORITMO DE DENAVIT-HARTENBERG
%1
a1=0;alf1=pi/2;d1=L1;the1=q1;
%2
a2=L2;alf2=0;d2=0;the2=q2;
%3
a3=L3;alf3=0;d3=0;the3=q3;
%4
a4=L4;alf4=0;d4=0;the4=q4;
% Ei
E1=1; E2=1; E3=1; E4=1;
q=[q1;q2;q3;q4]; disp(q);
dq=[dq1;dq2;dq3;dq4]; disp(dq);
ddq=[ddq1;ddq2;ddq3;ddq4]; disp(ddq);

MATRIZ ESLABN-ARTICULACIN.
Matrices correspondientes a cada par articulacin-eslabn teniendo en cuenta la
tabla anterior de parmetros.
T01=DH(a1,alf1,d1,the1);disp(T01)
T12=DH(a2,alf2,d2,the2); disp(T12);
T23=DH(a3,alf3,d3,the3); disp(23);
T34=DH(a4,alf4,d4,the4); disp(34);
T02=T01*T12;disp(T02);
T03=T01*T12*T23;disp(T03);
% Matriz total de transformaicin homogenea que relaciona el referencial
% X4Y4Z4 y el referencial X0Y0Z0
T04=T01*T12*T23*T34;disp(T04);
R01=T01(1:3,1:3);disp(R01);
R02=T02(1:3,1:3);disp(R02);

R03=T03(1:3,1:3);disp(R03);
R04=T04(1:3,1:3);disp(R04);
P1=T01(1:3,4);disp(P1);
P2=T02(1:3,4);disp(P2);
P3=T03(1:3,4);disp(P3);

CINEMTICA DIRECTA
P4=T04(1:3,4);disp(P4);

TENSORES DE INERCIA RESPECTO A SU CENTRO DE MASA

Tensores de inercia de los elementos que constituyen al sistema respecto de sus
respectivos referenciales trasladados a sus centros de masa son respectivamente:

I1=[2.1900

1.0000

0.2500;

1.0000

3.7700

0.1300;

0.2500

0.1300

3.9600]

disp(I1);
I2=[5.2000

0.0010

0.0020;

0.0010

5.3360

0.0100;

0.0020

0.0100

0.5000];

disp(I2);

I3=[1.0000

0.2700

0.4000;

0.2700

5.5600

0.0400;

0.4000

0.0400

5.5500];

disp(I3);

I4=[0.4000
0.0100
0
disp(I4);

0.0100
0.0200
0

0;
0;

0.4000];

TENSORES DE INERCIA RESPECTO DE X0Y0Z0

Los tensores de inercia de los elementos que constituyen al sistema respecto de la
referencial base X0Y0Z0 trasladados a su centro de masa son respectivamente:
Dr1=simplify(R01*I1*inv(R01));disp(Dr1);
Dr2=simplify(R02*I2*inv(R02));disp(Dr2);
Dr3=simplify(R03*I3*inv(R03));disp(Dr3);
Dr4=simplify(R04*I4*inv(R04));disp(Dr4);

VECTORES DESDE X0Y0Z0 HACIA EL CM DE CADA ELEMENTO

Los vectores desde el origen de la referencial base hasta el centro de masa de cada
elemento son respectivamente:
DC1=[0.00000506;-0.17076778;0.00073506]; disp(DC1);
DC2=[-0.12747755;0;0.00250687]; disp(DC2);
DC3=[-0.07714947;-0.00000162;0.00428303]; disp(DC3);
DC4=[-0.09701615;0;0]; disp(DC4);
% Los vectores desde el origen del referencial base hasta el centro de masa
% de cada elemento son descritos como:
Cr1=P1+R01*DC1;disp(Cr1);
Cr2=P2+R02*DC2;disp(Cr2);
Cr3=P3+R03*DC3;disp(Cr3);
Cr4=P4+R04*DC4;disp(Cr4);
Cr

VELOCIDAD LINEAL Y ANGULAR

Derivando los vectores de los respectivos centros de masa se obtiene las matrices
que relacionan la velocidad lineal cartesiana del centro de masa con el vector
velocidad articular:
Jv1=simplify(jacobian(Cr1,q));disp(Jv1);
Jv2=simplify(jacobian(Cr2,q));disp(Jv2);
Jv3=simplify(jacobian(Cr3,q));disp(Jv3);
Jv4=simplify(jacobian(Cr4,q));disp(Jv4);

% vectores Zi
Z0=[0;0;1]; disp(Z0);
Z1=T01(1:3,3); disp(Z1);
Z2=T02(1:3,3); disp(Z2);
Z3=T03(1:3,3); disp(Z3);
Z4=T04(1:3,3); disp(Z4);
% Las matrices que relacionan las velocidades angulares relativas al centro
% de masa de cada elemento respecto del referencial base con el vector
% velocidad articular respectivamente son:
Jw4=[E1*Z0 E2*Z1 E3*Z2 E4*Z3];disp(Jw4);
Jw1=[Jw4(:,1),zeros(3,3)]; disp(Jw1);
Jw2=[Jw4(:,1:2),zeros(3,2)]; disp(Jw2);
Jw3=[Jw4(:,1:3),zeros(3,1)]; disp(Jw3);

TENSORES DE INERCIA REFLEJADOS

los tensores de inercia reflejados a los ejes de giro de cada articulacin expresados
respecto del referencial base se expresan respectivamente como:
D1=simplify(TRANS(Jv1)*M1*Jv1+TRANS(Jw1)*Dr1*Jw1); disp(D1);
D2=simplify(TRANS(Jv2)*M2*Jv2+TRANS(Jw2)*Dr2*Jw2); disp(D2);
D3=simplify(TRANS(Jv3)*M3*Jv3+TRANS(Jw3)*Dr3*Jw3); disp(D3);
D4=simplify(TRANS(Jv4)*M4*Jv4+TRANS(Jw4)*Dr4*Jw4); disp(D4);
% Tensor de Inercia Total neto del sistema respecto de los ejes de rotacion
% o traslacion relativos al referencial base de obtiene sumand los tensores
% de cada elemento como:
D=simplify(D1+D2+D3+D4); disp(D);

ENERGA CINTICA:
Ec=simplify(1/2*TRANS(dq)*D*dq);disp(Ec); disp(Ec);

ENERGA POTENCIAL:
Ep=simplify(-g'*(M1*Cr1+M2*Cr2+M3*Cr3+M4*Cr4));disp(Ep);

% LA Exprecin del lagrangeano queda como:

L=Ec-Ep;disp(L); disp(L);

VECTOR FUERZA O TORQUE

[V1,C1,h1] = VFT(D,Cr,M,1,4,g,[q1;q2;q3;q4],[dq1;dq2;dq3;dq4],[ddq1;ddq2;ddq3;ddq4]);% 4:n; x={1, 2, 3,4 }
[V2,C2,h2] = VFT(D,Cr,M,2,4,g,[q1;q2;q3;q4],[dq1;dq2;dq3;dq4],[ddq1;ddq2;ddq3;ddq4]);
[V3,C3,h3] = VFT(D,Cr,M,3,4,g,[q1;q2;q3;q4],[dq1;dq2;dq3;dq4],[ddq1;ddq2;ddq3;ddq4]);
[V4,C4,h4] = VFT(D,Cr,M,4,4,g,[q1;q2;q3;q4],[dq1;dq2;dq3;dq4],[ddq1;ddq2;ddq3;ddq4]);

FUERZA DE FRICCIN ARTICULAR

Bc1=0.005;Bd1=0.02;
if(dq1==0)
sdq1=0;
else
sdq1=dq1/abs(dq1);
end
b1=sdq1*Bc1+Bd1*dq1
Bc2=0.02;Bd2=0.01;
if(dq2==0)
sdq2=0;
else
sdq2=dq2/abs(dq2);
end
b2=sdq2*Bc2+Bd2*dq2;
Bc3=0.05;Bd3=0.02;
if(dq3==0)
sdq3=0;
else
sdq3=dq3/abs(dq3);
end
b3=sdq3*Bc3+Bd3*dq3;
Bc4=0.02;Bd4=0.01;
if(dq4==0)
sdq4=0;
else
sdq4=dq4/abs(dq4);
end
b4=sdq4*Bc4+Bd4*dq4;

% Matriz fuerza de friccin:

B=[b1;b2;b3;b4]; disp(B);

VECTOR FUERZA O TORQUE

vft=[simplify(V1) + b1;simplify(V2) + b2;simplify(V3) + b3;simplify(V4) + b4];
disp(vft)

EFECTO CENTRFUGO Y DE CORIOLES

C=[C1;C2;C3;C4];disp(C)

EFECTO DE LA GRAVEDAD
h=[h1;h2;h3;h4];disp(h)

CLCULO DEL CONTROLADOR PID

Para
Dmax=subs(D,{q1,q2,q3,q4},{0 0 0 0}) % substituye
v=5;% voltaje de alimentacion del motor
dqmax=120*(3.1416/30)*v
Bmax=subs(B,{dq1,dq2,dq3,dq4},{dqmax,dqmax,dqmax,dqmax})
%parmetros del motor:
Jm=0.012; %inercia de la carga
Bm=0.01; %%n-m/(rad/s)
n=1;% realacion de engranaje eje motor y carga
Jtotal=Dmax+Jm; % inecercia de la carga ms inercia del motor: n^2*Dmax+Jm; (n^2*Jl+Jm)
Btotal=Bmax+Bm; % fuerzas de friccion de la carga mas fuerzas de frccion del motor: n^2*Bmax+Bm;
(((n^2)*Fl+Fm))
%datos del motor: //%valores tomados del lab5
kimotor=0.036; % constante de par del motor
kb=0.010; % constante de fuerza contra electromotriz
Ra=8; % resistencia armadura

La=0; % inducatancia armadura

%datos del Amplificador
A=1;
Cc=6*kb;%conversion de angulo a voltaje
%raices para la ecuacion caracteristica
r1=[-8+3i -8-3i -80];%[-3+1i -3-1i -30];
r2=[-8+3i -8-3i -80];
r3=[-8+3i -8-3i -80];
r4=[-8+3i -8-3i -80];
R=[r1;r2;r3;r4];
for j=1:length(D)
d=Jtotal(j,j); % matriz Jtotal=[ , , , ; , , , ; , , , ; , , , ]
b=Btotal(j); % matriz Btotal=[ , , , ]
Kd(j)=(-(R(j,1)+R(j,2)+R(j,3))*Ra*d-Ra*b-kimotor*kb)/(A*kimotor*Cc);
Kp(j)=(R(j,1)*R(j,2)+R(j,1)*R(j,3)+R(j,2)*R(j,3))*Ra*d/(A*kimotor*Cc);
Ki(j)=-R(j,1)*R(j,2)*R(j,3)*Ra*d/(A*kimotor*Cc);
end
Kd=double(Kd); disp(Kd);
Kp=double(Kp); disp(Kp);
Ki=double(Ki); disp(Ki);
Cc=double(Cc); disp(Cc);
disp('Matriz Kd; Kp; Ki')
K=[Kd; Kp; Ki]; disp(K)

Matriz Kd; Kp; Ki

1.0e+06 *
0.0691

0.0712

0.0258

0.0056

1.0398

1.0377

0.4326

0.1136

4.4882

4.4791

1.8670

0.4902

REFERENCIAS PARA LA SIMULACIN

dq1_ref=0;
dq2_ref=0;
dq3_ref=0;
dq4_ref=0;

PARMETROS PARA LA SIMULACIN

G11=1;G12=0;A1=2;F1=0.5;
G21=1;G22=0;A2=2;F2=0.5;
G31=1;G32=0;A3=2;F3=0.5;
G41=1;G42=0;A4=2;F4=0.5;
open_system('EnsambleFinal')
set_param(gcs,'SimulationCommand','start');
run('InterfazFinal')

DIAGRAMA DE BLOQUES EN MATLAB

DIAGRAMA DE BLOQUES DEL MECANISMO ROBTICO:

MECANISMO IMPORTADO DE SOLIDWORKS A MATLAB MEDIANTE

SIMMECHANIC

INTERFAZ QUE PERMITE MODIFICAR LAS SEALES DE CONTROL A

CADA ARTICULACIN POR SEPARADO

Published with MATLAB R2015a

RESULTADOS
Luego de ejecutarse la semulacin (10 segundos), los resultados lo optenemos
ejecutando el archivo Reporte.m.

POSICIN:
Resultados de la posicin para cada articulacin. En el eje Y se encuentran los valores
de la posicin angular en radianes, mientras que en el eje X se encuentra el tiempo en
segundos.

VELOCIDAD
Resultados de la velocidad para cada articulacin. En el eje Y se encuentra los valores
de la velocidad angular en radianes por segundo, mientras que en el eje X se
encuentra el tiempo en segundos.

TORQUE:
ARICULACION 1:

ARICULACION 2:

ARICULACION 3:

ARICULACION 4:

CONCLUSIONES

Despus de varias pruebas, observamos que para la seal de entra senoidal

responde de manera adecuada, no obstante para la seal de entrada cuadrada
presenta un sobre impulso de aproximadamente 20%.

Se observa que los torque que se deben aplicar en cada articulacin es

considerablemente grande, pues para la primera articulacin se debe aplicar
aproximadamente 400 KN, lo que implica que el modelo no lo soportara. Para
las dems articulaciones ocurre algo similar.

Para su representacin de la dinmica en MatLab utilicemos el software

SIMECHANICS el cual nos permiti evitar la enorme tarea de representar la
dinmica en Simulink.

En la simulacin solo hemos considerado 4 segundos, debido la lentitud del

proceso de simulacin.