(源代码)一类匹配不确定非线性系统的动态逆全程滑模变结构控制

2019-04-13 13:59发布

站内文章 / 模拟电子
16063 0 1766
参考文献：《一类匹配不确定非线性系统的动态逆全程滑模变结构控制》 clear clc H=[5 2 5; 8 1 7; 1 1 2]; invH=inv(H); a11=invH(1,1); a12=invH(1,2); a13=invH(1,3); a21=invH(2,1); a22=invH(2,2); a23=invH(2,3); a31=invH(3,1); a32=invH(3,2); a33=invH(3,3); u=zeros(3,1); x=zeros(6,1); Dx=zeros(6,1); S1=0; S2=0; S3=0; k1=50; k2=50; k3=50; fea1=10; fea2=10; fea3=10; Tf=0.5; t=0; Dt=0.001; n=1; for i=1:14000 p1=x(3)*sin(x(6))*cos(x(5))-x(3)*sin(x(6))*sin(x(5))*sin(x(4))-x(3)*cos(x(6))*cos(x(4))+x(2)*sin(x(6))+x(2)*cos(x(6))*sin(x(5)); p2=-x(1)*sin(x(6))-x(1)*cos(x(6))*sin(x(5))-x(3)*cos(x(6))*cos(x(5))+x(3)*cos(x(6))*sin(x(5))*sin(x(4))-x(3)*sin(x(6))*cos(x(4)); p3=x(2)*cos(x(6))*cos(x(5))-x(2)*cos(x(6))*sin(x(5))*sin(x(4))+x(2)*sin(x(6))*cos(x(4))-x(1)*sin(x(6))*cos(x(5))+x(1)*sin(x(6))*sin(x(5))*sin(x(4))+x(1)*cos(x(6))*cos(x(4)); f1=a11*p1+a12*p2+a13*p3; f2=a21*p1+a22*p2+a23*p3; f3=a31*p1+a32*p2+a33*p3; f4=x(1)+x(2)*sin(x(4))*tan(x(5))+x(3)*cos(x(4))*tan(x(5)); f5=x(2)*cos(x(4))-x(3)*sin(x(4)); f6=x(2)*sin(x(4))*sec(x(5))+x(3)*cos(x(4))*sec(x(5)); f=[f1;f2;f3;f4;f5;f6]; g=[a11 a12 a13;a21 a22 a23; a31 a32 a33; 0 0 0;0 0 0;0 0 0;]; Lg1Lfh1=a11+a21*sin(x(4))*tan(x(5))+a31*cos(x(4))*tan(x(5)); Lg2Lfh1=a12+a22*sin(x(4))*tan(x(5))+a32*cos(x(4))*tan(x(5)); Lg3Lfh1=a13+a23*sin(x(4))*tan(x(5))+a33*cos(x(4))*tan(x(5)); Lg1Lfh2=a21*cos(x(4))-a31*sin(x(4)); Lg2Lfh2=a22*cos(x(4))-a32*sin(x(4)); Lg3Lfh2=a23*cos(x(4))-a33*sin(x(4)); Lg1Lfh3=a21*sin(x(4))*sec(x(5))+a31*cos(x(4))*sec(x(5)); Lg2Lfh3=a22*sin(x(4))*sec(x(5))+a32*cos(x(4))*sec(x(5)); Lg3Lfh3=a23*sin(x(4))*sec(x(5))+a33*cos(x(4))*sec(x(5)); Lf2h1=f1+f2*sin(x(4))*tan(x(5))+f3*cos(x(4))*tan(x(5))+f4*(x(2)*cos(x(4))*tan(x(5))-x(3)*sin(x(4))*tan(x(5)))+f5*(sec(x(5)))^2*(x(2)*sin(x(4))+x(3)*cos(x(4))); Lf2h2=f2*cos(x(4))-f3*sin(x(4))+f4*(-x(2)*sin(x(4))-x(3)*cos(x(4))); Lf2h3=f2*sin(x(4))*sec(x(5))+f3*cos(x(4))*sec(x(5))+f4*(x(2)*cos(x(4))*sec(x(5))-x(3)*sin(x(4))*sec(x(5)))+f5*sec(x(5))*tan(x(5))*(x(2)*sin(x(4))+x(3)*cos(x(4))); A=[Lg1Lfh1 Lg2Lfh1 Lg3Lfh1; Lg1Lfh2 Lg2Lfh2 Lg3Lfh2; Lg1Lfh3 Lg2Lfh3 Lg3Lfh3]; B=[Lf2h1;Lf2h2;Lf2h3]; change=2; d_f=[change*a11*p1+change*a12*p2+change*a13*p3; change*a21*p1+change*a22*p2+change*a23*p3; change*a31*p1+change*a32*p2+change*a33*p3; change*f4;change*f5;change*f6]; d_g=[change*a11 change*a12 change*a13;change*a21 change*a22 change*a23; change*a31 change*a32 change*a33; 0 0 0;0 0 0;0 0 0;]; if t<=5 x6c=sin(0.5*pi*t); dx6c=0.5*pi*cos(0.5*pi*t); ddx6c=-(0.5*pi)^2*sin(0.5*pi*t); elseif t<=10 x6c=1; dx6c=0; ddx6c=0; else x6c=1-0.5*(t-10); dx6c=-0.5; ddx6c=0; end x4c=x6c; dx4c=dx6c; ddx4c=ddx6c; x5c=x6c; dx5c=dx6c; ddx5c=ddx6c; DDX=A*u+B; e1=x(4)-x4c; de1=Dx(4)-dx4c; dde1=DDX(1)-ddx4c; e2=x(5)-x5c; de2=Dx(5)-dx5c; dde2=DDX(2)-ddx5c; e3=x(6)-x6c; de3=Dx(6)-dx6c; dde3=DDX(3)-ddx6c; if i==1 e1_0=e1; e2_0=e2; e3_0=e3; De1_0=de1; De2_0=de2; De3_0=de3; DDe1_0=dde1; DDe2_0=dde2; DDe3_0=dde3; end a00=-10; a10=15; a20=-6; a01=-6; a11=8; a21=-3; a02=-1.5; a12=1.5; a22=-0.5; A1=a00/Tf^3*e1_0+a01/Tf^2*De1_0+a02/Tf*DDe1_0; B1=a10/Tf^4*e1_0+a11/Tf^3*De1_0+a12/Tf^2*DDe1_0; C1=a20/Tf^5*e1_0+a21/Tf^4*De1_0+a22/Tf^3*DDe1_0; A2=a00/Tf^3*e2_0+a01/Tf^2*De2_0+a02/Tf*DDe2_0; B2=a10/Tf^4*e2_0+a11/Tf^3*De2_0+a12/Tf^2*DDe2_0; C2=a20/Tf^5*e2_0+a21/Tf^4*De2_0+a22/Tf^3*DDe2_0; A3=a00/Tf^3*e3_0+a01/Tf^2*De3_0+a02/Tf*DDe3_0; B3=a10/Tf^4*e3_0+a11/Tf^3*De3_0+a12/Tf^2*DDe3_0; C3=a20/Tf^5*e3_0+a21/Tf^4*De3_0+a22/Tf^3*DDe3_0; if tfea1 sat1=sign(S1); else sat1=S1/fea1; end if abs(S2)>fea2 sat2=sign(S2); else sat2=S2/fea2; end if abs(S3)>fea3 sat3=sign(S3); else sat3=S3/fea3; end v=[ddx4c;ddx5c;ddx6c]-[de1-DDP1-DP1;de2-DDP2-DP2;de3-DDP3-DP3]+[-k1*sat1;-k2*sat2;-k3*sat3]; %全程滑模控制 % v=[ddx4c+6*(dx4c-f4)+9*(x4c-x(4));ddx5c+6*(dx5c-f5)+9*(x5c-x(5));ddx6c+6*(dx6c-f6)+9*(x6c-x(6))]; %极点配置 u=-inv(A)*B+inv(A)*v; Dx=f+d_f+g*u+d_g*u; x=x+Dx*Dt; x_store(:,n)=[x;x4c;x5c;x6c]; u_store(:,n)=u; S_store(:,n)=[S1;S2;S3]; n=n+1; t=t+Dt; end figure(1) plot((1:n-1)*Dt,x_store(7,:),(1:300:n-1)*Dt,x_store(4,1:300:end),'rp')%全程滑模控制 %plot((1:350:n-1)*Dt,x_store(4,1:350:end),'g+') xlabel('time/s') ylabel('phi/rad') hold on figure(2) plot((1:n-1)*Dt,u_store(1,:),(1:n-1)*Dt,u_store(2,:),(1:n-1)*Dt,u_store(3,:)) legend('u_1','u_2','u_3') xlabel('time/s') ylabel('u') figure(3) plot((1:n-1)*Dt,S_store(1,:),(1:n-1)*Dt,S_store(2,:),(1:n-1)*Dt,S_store(3,:)) legend('S1','S2','S3') xlabel('time/s') ylabel('S')

热门文章