x1dot = x2
x2dot = fx +
gx * u
u[1]=x1
u[2]=x2=x1dot
fx = (g*sin(u[1])-m*l*(u[2])^2*cos(u[1])*sin(u[1])
/(mc+m))/(l*(4/3-m*cos(u[1])^2/(mc+m)))
gx = (cos(u[1])/(mc+m))/(l*(4/3-m*cos(u[1])^2/(mc+m)))
x1 the oscillation angle
g = 9,8 ;
mc = 1 kg is
the vehicle mass
m=0.1 is the mass of pendulum bar
l =0.5m is one half of pendulum length
u is the control input
g=9.8;mc=1.0;m=0.1;l=0.5;
%Control law program:
chap5_2ctrl.m
function [sys,x0,str,ts]
= chap5_2ctrl(t,x,u,flag)
switch flag,
case 0,
[sys,x0,str,ts]=mdlInitializeSizes;
case 1,
sys=mdlDerivatives(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);
case {2,4,9}
sys=[];
otherwise
error(['Unhandled
flag = ',num2str(flag)]);
end
function
[sys,x0,str,ts]=mdlInitializeSizes
global xite cij bj h c
sizes = simsizes;
sizes.NumContStates = 10;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 3;
sizes.NumInputs = 3;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 0;
sys = simsizes(sizes);
x0 = 0.1*ones(10,1);
str = [];
ts = [];
cij=[-1 -0.5 0 0.5 1;
-1 -0.5 0 0.5 1];
bj=5;
h=[0,0,0,0,0];
c=5;
xite=0.01;
function
sys=mdlDerivatives(t,x,u)
global xite cij bj h c
thd=u(1);
dthd=0.1*cos(t);
ddthd=-0.1*sin(t);
x1=u(2);
x2=u(3);
e=thd-x1;
de=dthd-x2;
s=c*e+de;
xi=[x1;x2];
for j=1:1:5
h(j)=exp(-norm(xi-cij(:,j))^2/(2*bj^2));
end
for i=1:1:5
wf(i,1)=x(i);
end
for i=1:1:5
wg(i,1)=x(i+5);
end
fxn=wf'*h';
gxn=wg'*h'+0.01;
ut=1/gxn*(-fxn+ddthd+xite*sign(s)+c*de);
gama1=10;gama2=1.0;
S1=-gama1*s*h;
S2=-gama2*s*h*ut;
for i=1:1:5
sys(i)=S1(i);
end
for j=6:1:10
sys(j)=S2(j-5);
end
function
sys=mdlOutputs(t,x,u)
global xite cij bj h c
thd=u(1);
dthd=0.1*cos(t);
ddthd=-0.1*sin(t);
x1=u(2);
x2=u(3);
e=thd-x1;
de=dthd-x2;
s=c*e+de;
for i=1:1:5
wf(i,1)=x(i);
end
for i=1:1:5
wg(i,1)=x(i+5);
end
xi=[x1;x2];
for j=1:1:5
h(j)=exp(-norm(xi-cij(:,j))^2/(2*bj^2));
end
fxn=wf'*h';
gxn=wg'*h'+0.01;
ut=1/gxn*(-fxn+ddthd+xite*sign(s)+c*de);
sys(1)=ut;
sys(2)=fxn;
sys(3)=gxn;
function
[sys,x0,str,ts]=s_function(t,x,u,flag)
switch flag,
case 0,
[sys,x0,str,ts]=mdlInitializeSizes;
case 1,
sys=mdlDerivatives(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);
case {2, 4, 9 }
sys = [];
otherwise
error(['Unhandled
flag = ',num2str(flag)]);
end
function
[sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 2;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 4;
sizes.NumInputs = 1;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 0;
sys=simsizes(sizes);
x0=[pi/60 0];
str=[];
ts=[];
function
sys=mdlDerivatives(t,x,u)
g=9.8;mc=1.0;m=0.1;l=0.5;
S=l*(4/3-m*(cos(x(1)))^2/(mc+m));
fx=g*sin(x(1))-m*l*x(2)^2*cos(x(1))*sin(x(1))/(mc+m);
fx=fx/S;
gx=cos(x(1))/(mc+m);
gx=gx/S;
sys(1)=x(2);
sys(2)=fx+gx*u;
function sys=mdlOutputs(t,x,u)
g=9.8;mc=1.0;m=0.1;l=0.5;
S=l*(4/3-m*(cos(x(1)))^2/(mc+m));
fx=g*sin(x(1))-m*l*x(2)^2*cos(x(1))*sin(x(1))/(mc+m);
fx=fx/S;
gx=cos(x(1))/(mc+m);
gx=gx/S;
sys(1)=x(1);
sys(2)=x(2);
sys(3)=fx;
sys(4)=gx;
fx=g*sin(x(1))-m*l*x(2)^2*cos(x(1))*sin(x(1))/(mc+m);
fx=fx/S;
gx=cos(x(1))/(mc+m);
gx=gx/S;
sys(1)=x(2);
sys(2)=fx+gx*u(1);
function
sys=mdlOutputs(t,x,u)
g=9.8;mc=1.0;m=0.1;l=0.5;
S=l*(4/3-m*(cos(x(1)))^2/(mc+m));
fx=g*sin(x(1))-m*l*x(2)^2*cos(x(1))*sin(x(1))/(mc+m);
fx=fx/S;
gx=cos(x(1))/(mc+m);
gx=gx/S;
sys(1)=x(1);
sys(2)=x(2);
sys(3)=fx;
sys(4)=gx;
Web: http://nguyenvankhoa.com Facebook: http://www.facebook.com/NguyenVanKhoaCom Đăng ký kênh youtube: http://goo.gl/rHDTKK Rất mong được sự ủng hộ của quý vị Xin trân trọng!
Không có nhận xét nào :
Đăng nhận xét