滑模控制二级倒立摆控制器S函数代码

function[sys,x0,str,ts]=s_function(t,x,u,flag)switchflag,case0,[sys,x0,str,ts]=mdlInitializeSizes;case1,sys=mdlDerivatives(t,x,u);case3,sys=mdlOutputs(t,x,u);case{2,4,9}sys=[];otherwiseerror(['Unhandledflag=',num2str(flag)]);endfunction[sys,x0,str,ts]=mdlInitializeSizessizes=simsizes;sizes.NumContStates=0;sizes.NumDiscStates=0;sizes.NumOutputs=2;sizes.NumInputs=6;sizes.DirFeedthrough=1;sizes.NumSampleTimes=0;sys=simsizes(sizes);x0=[];str=[];ts=[];functionsys=mdlOutputs(t,x,u)x1=u(1);x2=u(2);x3=u(3);x4=u(4);x5=u(5);x6=u(6);m1=0.04;m2=0.132;m3=0.208;l1=0.09;l2=0.27;g=9.8;k=12;a=0.65;q=522;b=0.2;%m1=0.06;m2=0.14;m3=0.25;l1=0.15;l2=0.27;g=9.8;%k12=3*(-2*g*m1-4*g*m2-4*g*m3)/(2*(-4*m1-3*m2-12*m3)*l1);%k13=9*m2*g/(2*(-4*m1-3*m2-12*m3)*l1);%k17=3*(-2*m1-m2-4*m3)/(2*(-4*m1-3*m2-12*m3)*l1);%k22=2*g*(m1+2*(m2+m3))/(4*m2*l2-16/9*l2*(m1+3*(m2+m3)));%k23=-4*g*(m1+3*(m2+m3))/(3*(4*m2*l2-16/9*l2*(m1+3*(m2+m3))));%k27=-(2*(m1+2*(m2+m3))-4/3*(m1+3*(m2+m3)))/(4*m2*l2-16/9*l2*(m1+3*(m2+m3)));%%A=[000100;000010;000001;000000;0k12k13000;0k22k23000];%B=[0;0;0;1;k17;k27];E=[000001];x=[x1;x2;x3;x4;x5;x6];A=[000100;000010;000001;000000;077.0642-21.1927000;0-38.532137.8186000];B=[0;0;0;1;5.7012;-0.0728];D(:,1)=B;D(:,2)=A*B;D(:,3)=(A^2)*B;D(:,4)=(A^3)*B;D(:,5)=(A^4)*B;D(:,6)=(A^5)*B;%ackermanne=E*D^(-1);%-1,-2,-3,-4,-5p=A^5+15*A^4+85*A^3+225*A^2+274*A+120;%-1,-3,-5,-7,-9%p=A^5+25*A^4+230*A^3+950*A^2+1719*A+1215;c=e*p;%c=[0.343250.1710-48.14150.84093.4424-7.3271];c1=c(1,1);c2=c(1,2);c3=c(1,3);c4=c(1,4);c5=c(1,5);c6=c(1,6);s=c1*x1+c2*x2+c3*x3+c4*x4+c5*x5+c6*x6;n=tan(1);ifq*s1;f=1;elseifq*s-1;f=-1;elsef=atan(n*q*s);end;ifabs(s)a;m=abs(s);elsem=a;end;w=0.5;h=(exp(w*t)-exp(-w*t))/(exp(w*t)+exp(-w*t));%sigmoidº¯Êýi=0.6;sigmoid=(1-exp(-i*s))/(1+exp(-i*s));ut=-(c*B)^(-1)*(c*A*x+k*s+m*f);%ut=-(c*B)^(-1)*(c*A*x+k*s+b*(s^2)*sign(s));%ut=-(c*B)^(-1)*(c*A*x+k*s+m*sigmoid);sys(1)=ut;%sys(2)=c1;%sys(3)=c2;%sys(4)=c3;%sys(5)=c4;%sys(6)=c5;%sys(7)=c6;sys(2)=s;