Tareas Corte 3 De TAC

Carlos Agudelo

Nicolás Mejía

- Tarea 1:
-Robot 1DOF:

Constantes mecánicas:

clear all
clc
syms x1 x2 u Y s t z
m=0.3;
g=9.8;
l=0.2;
j=0.5;

Constantes de diseño:

sI=[s 0;0 s];

ts=0.5;
T=ts/10;
zita=.7;
wn=4/(zita*ts)

wn = 11.4286

Jacobiano:

f1=x2;
f2=-m*g*l/j*cos(x1)+1/j*u;
As=jacobian([f1;f2],[x1;x2]);
Bs=jacobian([f1;f2],u);

Puntos de operación dinámicos:

 x1=Y;
x2=0;

Funcion para el torque:

U=m*g*l*cos(Y)

U = 

Modelo continuo:

A=eval(As)

A = 

B=eval(Bs)

B =
0
2

C=[1 0];
Ae=[A,[0;0];-C, 0];
Be=[B;C*B];

Controlador por servosistema con observador:

poldes=conv([1 2*zita*wn wn^2],[1 5*zita*wn]);

phi=Ae^3+poldes(2)*Ae^2+poldes(3)*Ae+poldes(4)*eye(3);
Ke=[0 0 1]*inv([Be Ae*Be Ae^2*Be])*phi;
tso=ts/10;
Wno=4/(zita*tso);
alo=(s^2+2*zita*Wno*s+Wno^2);
pdo=coeffs(alo,s,'ALL')

pdo = 

I=eye(2);
phi=((A^2) + (pdo(2)*(A)) + (pdo(3)*I));
K=Ke(1:2)

K = 
 Ki=Ke(3)

Ki = 

ko=([0 1]*(inv([C;C*A]))*phi)'

ko = 

Modelo discreto:

- Tabla Gain Scheduling para rangos de a 10°:

clear A B
for punto=-45:10:45
punto=punto*(pi/180);
x1=punto;
x2=0;
U=m*g*l*cos(Y);
A=eval(As);
B=eval(Bs);

Discretización para cada punto:

[G,H]=c2d(A,B,T);
Ge=[G,[0;0];C*G, 1];
He=[H;C*H];

Controlador por servosistema con observador:

mag=exp(-T*zita*wn);
fas=T*wn*sqrt(1-zita^2);
 
poldesd=conv([1 -2*mag*cos(fas) m^2],[1 -0.05]);
phi=Ge^3+poldesd(2)*Ge^2+poldesd(3)*Ge+poldesd(4)*eye(3);
Ke=[0 0 1]*inv([He Ge*He Ge^2*He])*phi;
 
K=Ke(1:2);
Ki=Ke(3);
Ko=G^2*inv([C;C*G])*[0;1];
end
SIMULACIÓN:

- Modelo continuo:

- Modelo discreto:
-Levitador Magnético:
Constantes mecánicas:

clear all
clc
syms x1 x2 x3 x4 s u t xd
m=0.2; %% kg
r= 10;
g = 9.8; %% m/s^2
l=0.1;
c=1;
 

Constantes de diseño:

ts2 =0.3;% 4-6

ts21 =ts2/5; % tiempo para el observador
zita2=0.7;
zita21=0.99; % Zita deseado para el observador
wn21=4/(zita21*ts21);
wn2=4/(zita2*ts2);

Jacobiano:

f1 = x2;
f2=g-((c*(x3)^2/(m*x1)));
f3 =(-r/l)*x3+u/l;
 
h = x1;
F = [f1; f2; f3];
x = [x1 x2 x3];
 
As = jacobian(F,x);
Bs = jacobian(F,u);
Cs = jacobian(h,x);

Puntos de operación dinámicos:

x1b = xd; % distancia deseada

x2b = 0; % Velocidad
x3b= sqrt((g*m*xd)/c); % Corriente deseada previamente despejada
Funcion para el torque:

ub = r*x3b

ub = 

Xb = [x1b;x2b;x3b]

Xb = 

Modelo continuo:

A = subs(As,[x1 x2 x3 u],[x1b x2b x3b ub]);

% A= double(A);
B = subs(Bs,[x1 x2 x3 u],[x1b x2b x3b ub]);
B=double(B);
C =[1 0 0];
C=double(C);

Controlador por servosistema con observador:

PDC=(s^2+s*zita2*wn2+(wn2)^2)*(s+5*zita2*wn2);
PDC=coeffs(PDC,s)

PDC = 

KC=[0,0,1]*inv([B A*B (A^2)*B])*(A^3+A^2*PDC(3)+A*PDC(2)+PDC(1)*eye(3))

KC = 

 
PDOC=(s^2+s*zita21*wn21+(wn21)^2)*(s+5*zita21*wn21);
PDOC=coeffs(PDOC,s)

PDOC = 
 Kec=(A^3+A^2*PDOC(3)+A*PDOC(2)+PDOC(1)*eye(3))*inv([C; C*A;C*(A^2)])*[0;0;1]

Kec = 

wd=wn2*sqrt(1-zita2^2);
wd1=wn21*sqrt(1-zita21^2);

Modelo discreto:

- Tabla Gain Scheduling para rangos de a 0.02m:

clear all
syms X x1 x2 x3 m c R L g u z s t T z
 
g=9.81;c=1;m=0.1;L=0.01;R=1;i=0;
ts=1;t=ts/10;zita=0.7;T=ts/10;
wn=4/(zita*ts);
 
for punto=0.02:0.02:0.08
i=i+1;
x1p=x2;
x2p=g-(c/m)*(x3^2/x1);
x3p=-(R/L)*x3+(1/L)*u;
y=x1;
F=[x1p x2p x3p];
As=jacobian(F,[x1,x2,x3]);
Bs=jacobian(F,[u]);
x1x=punto;
x2x=0;
x3x=sqrt(m*g*punto/c);
ux=R*sqrt(m*g*punto/c);
Ax=simplify(subs(As,[x1,x2,x3,u],[x1x,x2x,x3x,ux]));
Bx=simplify(subs(Bs,[x1,x2,x3,u],[x1x,x2x,x3x,ux]));
A=double(Ax);
B=double(Bx);
C=[1 0 0];
 
Discretización para cada punto:

[G,H]=c2d(A,B,t)
Gm(:,:,i)=G;
Hm(:,:,i)=H;
Ge=[G,[0;0;0] ;C*G, 1];
He=[H;C*H];

Controlador por servosistema con observador:

mag=exp(-T*zita*wn);
fas=T*wn*sqrt(1-zita^2);
poldes2=conv([1 -2*mag*cos(fas) m^2],[1 -0.05]);
poldes=conv(poldes2,[1 -0.05]);
phi=Ge^4+Ge^3*poldes(2)+Ge^2*poldes(3)+Ge*poldes(4)+poldes(5)*eye(4);
Ke=[0 0 0 1]*inv([He Ge*He Ge^2*He Ge^3*He])*phi;
K=Ke(1:3);
Ki=Ke(4);
tsOb=1/10;
tOb=tsOb/10;
zitaOb=0.7;
ttOb=tOb;
wnOb=4/(tOb*zitaOb);
zangsOb=ttOb*wnOb*sqrt(1-zitaOb^2);
zmagsOb=exp(-ttOb*zitaOb*wnOb);
[realOb imagOb]=pol2cart(zangsOb,zmagsOb);
ang_zOb=ttOb*wnOb*sqrt(1-zitaOb^2);
mag_zOb=exp(-ttOb*zitaOb*wnOb);
pdzOb=double(coeffs(((z^2)-(2*mag_zOb*(cos(ang_zOb))*z)+(mag_zOb^2))*((z-0.05)^1),z,'ALL'));
LM=place(G,H,roots(pdzOb)).';
end

G =
4.6340 0.2043 -0.2327
100.2128 4.6340 -5.3496
0 0 0.0000
H =
-0.8051
-23.2681
1.0000
G =
2.4983 0.1462 -0.1231
35.8531 2.4983 -2.1726
0 0 0.0000
H =
-0.4820
-12.3069
1.0000
G =
1.9351 0.1296 -0.0906
21.1841 1.9351 -1.4168
0 0 0.0000
H =
-0.3719
-9.0613
1.0000
 G =
1.6784 0.1217 -0.0744
14.9269 1.6784 -1.0842
0 0 0.0000
●
H =
-0.3130
-7.4411
1.0000

SIMULACIÓN:

- Modelo continuo:
- Modelo discreto:

- EXAMEN FINAL:
- Constantes del modelo:

clear all
clc
syms l1 l2 lc1 lc2 t q1(t) q2(t) q1a q2a q1ap q2ap q1app q2app m1 m2 m3 m4 t1 t2 q1t q2t z v
g=9.8;
l1=0.2;
l2=0.2;
lc1=0.1;
lc2=0.1;
m1=0.1;
m2=0.1;
m3=0;
m4=0.3;
 v1=0;
v2=0;

- Modelo matemático brazo 2DOF:

x1=lc1*sin(q1);
y1=-lc1*cos(q1);
x1p=diff(x1,t);
y1p=diff(y1,t);
 
x2=l1*sin(q1)+lc2*sin(q1+q2);
y2=-l1*cos(q1)-lc2*cos(q1+q2);
x2p=diff(x2,t);
y2p=diff(y2,t);
 
x3=l1*sin(q1)+l2*sin(q1+q2);
y3=-l1*cos(q1)-lc2*cos(q1+q2);
x3p=diff(x3,t);
y3p=diff(y3,t);
 
x4=l1*sin(q1);
y4=-l1*cos(q1);
x4p=diff(x4,t);
y4p=diff(y4,t);
 
q1p=diff(q1,t);
q2p=diff(q2,t);
q1pp=diff(q1p,t);
q2pp=diff(q2p,t);
 
y1a=lc1*cos(q1t);
y2a=l1*cos(q1t)-lc2*cos(q1t+q2t);
y3a=l1*cos(q1t)-lc2*cos(q1t+q2t);
y4a=l1*cos(q1t);

- Lagrangiano:

LL=l1+l2;
x1p^2 + y1p^2;
k=simplify((1/2*m1*(x1p^2 + y1p^2))+(1/2*m2*(x2p^2 + y2p^2))+(1/2*m3*(x3p^2 + y3p^2))+(1/2*m4*
u=simplify((m1*g*(LL-lc1*cos(q1)))+(m4*g*(LL-l1*cos(q1)))+(m2*g*(LL*cos(q1)- lc2*cos(q1+q2)))+
L=k-u;
 
a1=subs(L,q1p,q1ap);
a1=diff(a1,q1ap);
b1=subs(L,q1,q1a);
b1=diff(b1,q1a);
a11=diff(a1,t);
 
T1=a11-b1-t1;
T1=subs(T1,q1,q1a);
T1=subs(T1,q2pp,q2app);
T1=subs(T1,q2,q2a);
T1=subs(T1,q1pp,q1app);
v11=v1*q1ap;
 T1=T1+v11;
 
a2=subs(L,q2p,q2ap);
a2=diff(a2,q2ap);
b2=subs(L,q2,q2a);
b2=diff(b2,q2a);
a22=diff(a2,t);
 
T2=a22-b2-t2;
T2=subs(T2,q2,q2a);
T2=subs(T2,q1pp,q1app);
T2=subs(T2,q1,q1a);
T2=subs(T2,q2pp,q2app);
v22=v1*q2ap;
T2=T2+v22;

- Despeje de las aceleraciones de cada juntura:

q2ppecu=simplify(solve(T2,q1app));
q1ppecu=simplify(solve(T1,q2app));
[num1,den1]=numden(q1ppecu);
[num2,den2]=numden(q2ppecu);
 
x2a=q1ap;
x4a=q2ap;
 
f1=x2a;
f2=q1ppecu;
f3=x4a;
f4=q2ppecu;

- Jacobiano:

As=jacobian([f1;f2;f3;f4],[q1a,q1ap,q2a,q2ap]);
Bs=jacobian([f1;f2;f3;f4],[t1,t2]);
 
q1t=ang1;
q2t=ang2;
 
t1=m1*y1a+m2*y2a+m3*y3a+m3*y4a;
t2=m2*y2a+m3*y3a;
t1=eval(t1);
t2=eval(t2);
 

- Tabla Gain Scheduling para rangos de a 5°:

for punto=-45:5:45
 
punto=punto*(pi/180);

q1a=punto;
q1ap=0;
 q2a=punto;
q2ap=0;
 
q1t=q1a;
q2t=q2a;
 
t1=m1*y1a+m2*y2a+m3*y3a+m3*y4a;
t2=m2*y2a+m3*y3a;
t1=eval(t1);
t2=eval(t2);
A=eval(As);
B=eval(Bs);
C=[1 0 0 0;
0 0 1 0];

- Cálculo de servosistema con Observador:

ts=0.5;
t=ts/11;
zita=1.2;
tt=t;
wn=4/(ts*(zita-(sqrt((zita^2)-1))));
[G,H]=c2d(A,B,t);
 
Ge=[G,[0 0;0 0;0 0;0 0];C*G eye(2)];
He=[H;C*H];
 
zangs=tt*wn*sqrt(1-zita^2);
zmags=exp(-tt*zita*wn);
[real imag]=pol2cart(zangs,zmags);
ang_z=tt*wn*sqrt(1-zita^2);
mag_z=exp(-tt*zita*wn);
pdz=double(coeffs(((z^2)-(2*mag_z*(cos(ang_z))*z)+(mag_z^2))*((z-0.05)^4),z,'ALL'));
kplace=place(Ge,He,roots(pdz)');
 
kR=[kplace(1,1:4);kplace(2,1:4)];
ki=[kplace(1,5:6);kplace(2,5:6)];

Ob=[C ; C*A ; C*A^2];

detOb= rank(Ob);
tsOb=0.5/10;
tOb=tsOb/11;
zitaOb=1.2;
ttOb=tOb;
wnOb=4/(tsOb*(zitaOb-(sqrt((zitaOb^2)-1))));
zangsOb=ttOb*wnOb*sqrt(1-zitaOb^2);
zmagsOb=exp(-ttOb*zitaOb*wnOb);
[realOb imagOb]=pol2cart(zangsOb,zmagsOb);
ang_zOb=ttOb*wnOb*sqrt(1-zitaOb^2);
mag_zOb=exp(-ttOb*zitaOb*wnOb);
pdzOb=double(coeffs(((z^2)-(2*mag_zOb*(cos(ang_zOb))*z)+(mag_zOb^2))*((z-0.05)^2),z,'ALL'));
LM=place(G,H,roots(pdzOb)).';
end
- Montaje:

- Respuesta para viscosidad de 0.1N*s/m y masa de 50g, con puntos de operación entre
-45 y 45:
- Respuesta para viscosidad de 0.2N*s/m y masa de 50g, con puntos de operación entre
-45 y 45:

- Respuesta para viscosidad de 0.2N*s/m y masa de 100g, con puntos de operación

entre -45 y 45:

- Respuesta para viscosidad de 0.5N*s/m y masa de 50g, con puntos de operación entre
-45 y 45:
- Respuesta para viscosidad de 0.5N*s/m y masa de 100g, con puntos de operación
entre -45 y 45:

- CONCLUSIONES:
Se puede observar que el control dinámico para un sistema MIMO cumple con los requisitos deseados,
pues cada una de las articulaciones sigue la señal sinusoidal de referencia para la posición deseada
entre 45° y -45°, por lo que se confirma que la linealización mediante la tabla de Gain Scheduling
se efectuó correctamente; además podemos ver como la dinámica del robot va variando según la
viscosidad y la masa de carga aplicada.

Pudimos ver que con la viscosidad en 0 el sistema se vuelve inestable y pierde su capacidad de
controlar, además vimos que con viscosidades mayores a 0.2 N*s/m el sistema empieza a adoptar un
comportamiento forzoso y deja de seguir por completo a la referencia, por lo que decidimos que el valor
de viscosidad para nuestro diseño debe variar de 0.1 N*s/m a 0.2 N*s/m para que el robot tenga un
comportamiento óptimo.

Además se puede detallar como a mayor viscosidad en el sistema, el manipulador puede aumentar su
capacidad de carga, pues al aplicar una carga de 100g con la viscosidad mínima de diseño (0.1 N*s/
m) vemos que el sistema tiene oscilaciones violentas en los primeros 10 segundos, lo cual puede llegar
a ser peligroso al momento de usar este; mientras que haciendo la misma prueba con la viscosidad
máxim de diseño (0.2 N*s/m) vemos que estas oscilaciones se anulan casi completamente, teniendo
el sistema estable a los 2 segundos. Por lo que se comprobó que mayor carga el sistema oscila más y
vimos que al superar la carga de 100g el sistema se vuelve inestable, esto puede darse por la robustez
y calidad del controlador diseñado.