LAB3

1.- PROBLEMA DEL CONTROL DE LEVITACIN

Del programa de levitacin de clase se pide:

a) Calcular el t de control mximo para que se pueda controlar el sistema

b) Agregar el ruido al sensor y analizar sus efectos
c) Analizar el efecto al modificar los pesos en el controlador y el observador

clear all,close all,clc;

Kf = 0.49091e-4;

R = 6;

L = 0.001;

m = 0.050;

g = 9.8182;

vo = 12;

io = 2;

xo = 0.02;

a21 = 2*Kf*io*io/(m*(xo^3));

a23 = -2*Kf*io/(m*(xo^2));

a33 = -R/L;

b3 = 1/L;

% Vector de estado x = [ x-xo xp i-io ]

A = [ 0 1 0

a21 0 a23

0 0 a33 ];

B = [ 0

0
 b3 ];

C = [ 1 0 0 ];

% Ley de Control

q1 = 1e3;

q2 = 1e3;

q3 = 1e3;

Q = diag([ q1 q2 q3 ]);

P = are(A,B*B',Q);

K = B'*P;

k1 = K(1,1);

k2 = K(1,2);

k3 = K(1,3);

% Observador

q1o = 1;

q2o = 1;

q3o = 1;

Qo = diag([ q1o q2o q3o ]);

S = are(A',C'*C,Qo);

LL = S*C';

ti = 0; dt = 10*0.000001; tf = 3; % dt para simulacion

[Ao Bo] = c2d(A-LL*C,B,dt);

[Ao Lo] = c2d(A-LL*C,LL,dt);

% Valores iniciales

i = io;

x = xo + (2)*0.005;

xp = 0;

xh = [ 0; 0; 0 ];
k = 1;

%factor para delta de t

ff=7;

for tt = ti:dt:tf

if mod(k-1,ff)==0

v = vo - k1*xh(1,1) - k2*xh(2,1) - k3*xh(3,1);

%y = x - xo + 0.00*xo*randn(1,1);

%xh = Ao*xh + Bo*(v-vo)+ Lo*y;

end

y = x - xo + 0*0.001*randn(1,1);

xh = Ao*xh + Bo*(v-vo)+ Lo*y;

if(v > 40)

v = 40;

end

pos(k,1) = x;

amp(k,1) = i;

posh(k,1) = xh(1,1)+ xo;

amph(k,1) = xh(3,1) + io;

volt(k,1) = v;

tiempo(k,1) = tt;

ip = -R/L*i + 1/L*v;

x2p = g - Kf/m*i*i/(x*x);
 i = i + ip*dt;

xp = xp + x2p*dt;

x = x + xp*dt;

k = k + 1;

end

figure(1);

plot(tiempo,pos,'-r',tiempo,posh,'--b'); title('Posicion');
legend('Posicin real','Posicin Observada','np=')

figure(2);

plot(tiempo,amp,'-r',tiempo,amph,'--b'); title('Corriente');

figure(3);

plot(tiempo,volt); title('Voltaje');

%figure (4)

%plot(tiempo,xxo); title('Sensor');

2.- CONTROL DE ROBOT DE 2G.D.L. Y LINEALIZACIN

EXTENDIDA
Estudiar el caso cuando el elemento final del robot de 2 grados de libertad recorre
la siguiente trayectoria.

Ayuda:

Tenemos que cada variable de longitud en el tiempo resulta, considerando una

veocidad Vx de entrada:

0.25
t1 =
Vx

x=0.65Vxt

y= 0.1252(x0.525)2

Utilizar

clear; close all; clc;

% Datos para el Robot de 2 eslavones

m1 = 0.15;
L1 = 0.35;

l1 = 0.16;

I1 = 4.1e-3;

m2 = 0.12;

L2 = 0.30;

l2 = 0.12;

I2 = 3.2e-3;

% Sistema Linealizado

M11 = I1 + m1*l1*l1 + m2*L1*L1 + m2*L1*l2;

M12 = m2*L1*l2;

M21 = I2 + m2*l2*l2 + m2*L1*l2;

M22 = I2 + m2*l2*l2;

M = [ M11 M12

M21 M22 ];

S = [ 1 -1

0 1 ];

ceros = zeros(2,2);

iden = eye(2);

A = [ ceros iden

ceros ceros ];

B = [ ceros

inv(M)*S ];

% Calculado el K del controlador

q1=input('Peso q1: ');

q2=input('Peso q2: ');

q3=input('Peso q3: ');

q4=input('Peso q4: ');

Q=diag([q1 q2 q3 q4]);

P=are(A,B*B',Q);

K=B'*P;

%%%%%% Agregando observador:

C=[1 0 0 0;0 1 0 0];

q1o=input('q1 observador: ');

q2o=input('q2 observador: ');

q3o=input('q3 observador: ');

q4o=input('q4 observador: ');

Qo=diag([q1o q2o q3o q4o]);

SS=are(A',C'*C,Qo); % S ya estamos usando por eso SS

L=SS*C';

dt=0.001;

[Ao,Bo]=c2d(A-L*C,B,dt);

[Ao,Lo]=c2d(A-L*C,L,dt);
v=input('Velocidad eje x del robot: ');

%el tiempo t1 en funcion de la velocidad vx

t1=(0.65-0.40)/v;

ti=0;dt=0.001;tf=t1;

tt1=0:dt:t1; tt1=tt1';

% x e y se calculan:

x=-0.25/t1*tt1+0.65;

xr=x;

y=sqrt(0.125^2-(x-0.525).^2);

yr=y;

% los angulos deseados

x2my2=xr.^2+yr.^2;

r1A=acos(xr./sqrt(x2my2));

r1B=acos((x2my2+L1*L1-L2*L2)./(2*L1*sqrt(x2my2)));

r1=atan(yr./xr)+ r1B;

r2=acos((x2my2-(L1*L1+L2*L2))/(2*L1*L2));

r2=-r2;

r1=real(r1);

r2=real(r2);

fi1 = 0; fi2 = 0;
fi1p = 0; fi2p = 0;

ang = [ fi1 fi2 ]';

vel = [ fi1p fi2p ]';

k = 1;

% Estado inicial del observador

xh=[0;0;0;0];

for tt = ti:dt:tf

phi1(k,1)=fi1;

phi2(k,1)=fi2;

t(k,1)=tt;

fi1=ang(1,1);

fi2=ang(2,1);

fi1p=vel(1,1);

fi2p=vel(2,1);

y=[fi1;fi2]+0.01*randn(2,1);

sensor(k,1)=y(1,1);

sensor(k,2)=y(2,1);
 x=[xh(1,1)-r1(k,1);xh(2,1)-r2(k,1); xh(3,1); xh(4,1)];

T=-K*x;

M11a = I1 + m1*l1*l1 + m2*L1*L1 + m2*L1*l2 * cos(fi2);

M12a = m2*L1*l2*cos(fi2);

M21a = I2 + m2*l2*l2 + m2*L1*l2*cos(fi2);

M22a = I2 + m2*l2*l2;

M = [ M11a M12a

M21a M22a ];

C1 = m2*L1*l2*(fi1p+fi2p)^2*sin(fi2);

C2 = -m2*L1*l2*fi1p*fi1p*sin(fi2);

C = [ C1

C2 ];

accel=inv(M)*(C+S*T);

ang=ang+dt*vel;

vel=vel+dt*accel;

xh=Ao*xh+Bo*T+Lo*y;

k=k+1;

end

phi1g=phi1*180/pi;
phi2g=phi2*180/pi;

r1g=r1*180/pi;

r2g=r2*180/pi;

xx=L1*cos(phi1)+L2*cos(phi1+phi2);

yy=L1*sin(phi1)+L2*sin(phi1+phi2);

sensor=sensor*180/pi;

figure(1)

plot(t,phi1g,'-k',t,r1g,'-r',t,sensor(:,1),'-
m');title('Fi1');legend('Observado','Calculado','Medido');

figure(2)

plot(t,phi2g,'-k',t,r2g,'-r',t,sensor(:,2),'-
m');title('Fi2');legend('Observado','Calculado','Medido');

figure(3)

plot(xx,yy,'-r',xr,yr,'-b'); legend('real','ideal');
title('Trayectoria')

Pobar con:

Peso q1: 1e3

Peso q2: 1e3

Peso q3: 2
 Peso q4: 2

q1 observador: 1e2

q2 observador: 1e2

q3 observador: 1e2

q4 observador: 1e2

Velocidad eje x del robot: 0.05

PROBLEMA N3

Utilizando linealizacin extendida, aplicar la ley de control u en el sistema

u=ae x + be x + c
1 2

x 1' =2 x 12 x 2+ u

'
x 2 =x 1 + x 2u

Ayuda:

De la ley de control lineal y no lineal (ecuaciones 1 y 2 respectivamente):

u=uok 1(x 1x 1 o)k 2 ( x2 x2 o) .. (1)

 x1 x2
u=a e + b e + c (2)

Calculando los coeficientes de las ecuaciones (1) y (2):

u
=k 1=a e x o
1

x1

Entonces: a=k 1 ex o 1

. (3)

u
=k 2=b e x o
2

x2

o
Entonces: b=k 2 ex 2

(4)

Entonces reemplazando (3) y (4) en (2):

u=uo=k 1k 2 +c

c=uo+ k 1 +k 2 (5)

Finalmente reemplazando (3), (4) y (5) en (2):

u=uok 1 ex o(e x e x o )k 2 ex o ( e x e x o )
1 1 1 2 2 2
 Nuestro punto de operacin para el anlisis ser

x 1o =0

x 2o =0.5

uo=0.5

y la condicin inicial (2,2)

Utilizar:

clear; clc; close all;

% Nuestro punto de operacin:

x1o = 0; x2o = 0.5; uo = -1/2;

a11 = 4*x1o*x2o;

a12 = 2*x1o*x1o;

a22 = uo;

b2 = x2o;

A = [ a11 a12

1 a22 ];

B = [ 1
 b2 ];

q1 = input('q1: ');

q2 = input('q2: ');

Q = diag([ q1 q2 ]);

R = [ 1 ];

P = are(A,B*inv(R)*B',Q);

K = inv(R)*B'*P;

k1 = K(1,1); k2 = K(1,2);

ti = 0;

tf = 6;

dt = 0.0005;

%Condicin inicial:

x1 = 2;

x2 = 2;

k = 1;

for tt = ti:dt:tf

x11(k,1) = x1;

x22(k,1) = x2;

u=uo-(k1/exp(x1o)).*(exp(x1)-exp(x1o))-(k2/exp(x2o)).*(exp(x2)-
exp(x2o));
 uu(k,1) = u;

t(k,1) = tt;

% Sistema no lineal

x1p = 2*x1*x1*x2 + u;

x2p = x1 + x2*u;

x1 = x1 + dt*x1p;

x2 = x2 + dt*x2p;

k = k + 1;

end

figure(1);

subplot(3,1,1); plot(t,x11(:,1)); title('x1');

subplot(3,1,2); plot(t,x22(:,1)); title('x2');

subplot(3,1,3); plot(t,uu(:,1)); title('u');

Ingresando:

q1: 1e2

q2: 1e4

Obtenemos: