myfuzzy1.m

用RBF神经网络和模糊控制方法控制二级倒立摆源码

%%%%%%%智能控制作业模糊控制算法----IAIR,王启源%%%%%%%%
%%%%%%%%采用将k求平方和然后利用的方式%%%%%%%%%%
%首先将其中的变量计算出来有利于后续过程，当然这降低了程序的通用性
%首先是系统定义，将系统的状态空间写出来
%定义变量为倾角
%有意函数的形式定义本功能
%初始状态为各角都是0
%1： 将系统的状态表达式求出来（在角都为零的点上将系统线性化）
 ang1=0; 
 ang2=0;
 ang1bar=0;
 ang2bar=0; 
 x=0;
 xbar=0;
M0=1.0;
M1=0.1;
M2=0.1;
F0=10.6;
l1=0.25;
l2=0.25;
F1=0.0015;
F2=0.0015;
J1=0.0044;
J2=0.0044; 
L1=0.5;
g=9.8;
M_ang1_ang2=[M0+M1+M2                   (M2*L1+M1*l1)*cos(ang1)     M2*l2*cos(ang2)
             (M2*L1+M1*l1)*cos(ang1)    J1+M1*l1*l1+M2*L1*L1        M2*l2*L1*cos(ang2-ang1)
             M2*l2*cos(ang2)            M2*l2*L1*cos(ang2-ang1)     J2+M2*l2*l2];
F_ang1_ang2_d1_d2=[F0   -(M2*L1+M1*l1)*ang1bar*sin(ang1)    -M2*l2*ang2bar*sin(ang2)
                   0    F1+F2                               -M2*l2*L1*ang2bar*sin(ang2-ang1)-F2
                   0    M2*l2*L1*ang2bar*sin(ang2-ang1)-F2  F2];
%在倒立摆竖直位置附近 ang1=0  ang2=0处线性化，可得到二级倒立摆的状态方程和输出方程为
%xbar=A*x +bu ，y=c*x 
X=[x;ang1;ang2; xbar; ang1bar ;ang2bar ];
Y=[x; ang1 ;ang2 ]; 
G_u_ang1_ang2=[0
               (M1*l1+M2*L1)*g*sin(ang1)
               M2*l2*g*sin(ang2)];
h0=[1
    0 
    0];
%M_ang1_ang2[xbar;ang1dd;ang2dd]+F_ang1_ang2_d1_d2[xbar;ang1bar;ang2bar]=G_u_ang1_ang2+h0*u;
%从而可得到系统的状态矩阵
G=[ 0      0                0
    0      (M1*l1+M2*L1)*g  0
    0      0                M2*l2*g];
a=[zeros(3,3)               eye(3)
    inv(M_ang1_ang2)*G      -inv(M_ang1_ang2)*F_ang1_ang2_d1_d2];
b=[ 0
    0
    0
   inv(M_ang1_ang2)*h0];
c=eye(6);
d=[0;0;0;0;0;0];
%2： 得到矩阵后分析知系统不稳定  
sys=ss(a,b,c,d);
n=6;
%求可控性矩阵
q=ctrb(sys);
m=rank(q);

if m==n
    %可控求解系统可控规范型
    acl=inv(q)*a*q;
    bcl=inv(q)*b;
    ccl=c*q;
%3： 设计模糊控制器使其稳定
%由线性化模型式可看出二级倒立摆系统蕴含6个状态变量，
%若直接设计模糊控制器则必然遇到规则数目膨胀的困难
%若直接控制将完全丧失模糊控制的优越性
%因此必须想办法减少变量个数
%采用将6个变量综合成两个变量的方法解决
%即将状态信息综合为总偏差e和总偏差变化率ebar
 lqrq=[1 0 0 0 0 0;0 100 0 0 0 0;0 0 1000 0 0 0 ;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;];
lqrr=1;
lqrx=care(a,b,lqrq,lqrr);
%求解最优反馈
k=inv(lqrr)*b'*lqrx;
k2=sqrt(k(1)*k(1)+k(2)*k(2)+k(3)*k(3)+k(4)*k(4)+k(5)*k(5)+k(6)*k(6));
f1_x=[k(1)/k2 k(2)/k2 k(3)/k2    0       0       0 
      0       0       0          k(4)/k2 k(5)/k2 k(6)/k2    ];
e=f1_x*X;
ec=e(2,1);
e=e(1,1);
%4： 用matlab仿真建立仿真图'e=x*0.0109+ang1*(-0.5622)+ang2*(0.8160),ec=xbar*0.0126-ang1bar*0.0093+ang2bar*0.1332'
%仿真结果显示牧户控制器效果良好  x,ang1,ang2,xbar,ang1bar,ang2bar
else 
    disp('System  cannot be fully controlled');
    disp('The rank of System  controllable Matrix is :');
    m         
end