triplelqrlqy.m

此文件是本人的毕业设计所做

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%===============triplelqrlqy.m=============================
%系统参数设置、状态空间方程建立、LQR/LQY控制器设计及各自的
%阶跃响应曲线
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%程序如下
%参数设置
M1=1.320;           %小车质量/kg                
M2=0.208;          %1、2摆杆间的质量快/kg 
M3=0.208;          %2、3摆杆间的质量快/kg       
L1=0.090;          %1摆质心到转轴长度/m         
L2=0.110;          %2摆质心到转轴长度/m         
L3=0.270;          %3摆质心到转轴长度/m         
c0=0;              %小车与导轨间的摩擦系数      
c1=0;              %1摆杆与小车间的摩擦系数     
c2=0;              %1、2摆杆间摩擦系数          
c3=0;              %2、3摆杆间摩擦系数          
m1=0.040;          %第一摆杆质量/kg             
m2=0.050;          %第二摆杆质量/kg             
m3=0.132;          %第三摆杆质量/kg             
x=0;               %小车位置/m 
g=9.81;             
%状态空间方程
k1=m1+m2+m3+M1+M2;
k2=m1+2*m2+2*m3+2*M1+2*M2;
k3=m1/3+m2+m3+M1+M2;
k4=m1+2*m3+2*M2;
k5=m2/3+m3+M2;
G=[k1 k2*L1 k4*L2 k3*L3;
    k2*L1 4*k3*(L1)^2 2*k4*L1*L2 2*k3*L1*L3;
    k4*L2 2*k4*L1*L2 4*k5*(L2)^2 2*k3*L2*L3;
    k3*L3 2*k3*L1*L3 2*k3*L2*L3 4/3*k3*(L3)^3];
H0=[1 0 0 0]';
H1=[-c0 0 0 0;
    0 -c1 0 0;
    0 c2 -c2 0;
    0 0  c3 -c3];
H2=[0 0 0 0;
    0 -k2*g*L1 0 0;
    0 0 -k4*g*L2 0;
    0 0 0 -k3*g*L2];
A=[zeros(4) eye(4);
    inv(G)*H2 inv(G)*H1];
B=[zeros(4,1);
    inv(G)*H0];
C=[eye(4) zeros(4)];
D=[0 0 0 0]';
op=eig(A)
Q0=1*diag([1 1 1 1 0 0 0 0]);%状态变量的敏感程度
R0=1;%输入量的敏感度
K0=lqr(A,B,Q0,R0)  % 计算LQR控制矩阵
Ac0=A-B*K0;
Q1 = 8*eye(4);
R1 =0.1; 
K1=lqry(A,B,C,D,Q1,R1)  % 计算LQY控制矩阵
Ac1=A-B*K1;
Bc=[B];
Cc = [C];
Dc = [D];
% 计算LQR增益Nbar0
Cn = [1 0 0 0 0 0 0 0];
Nbar0 = rscale(A,Bc,Cn,0,K0);
Bcn0 = [Nbar0*B];
% 计算LQY增益Nbar1
Nbar1 = rscale(A,Bc,Cn,0,K1);
Bcn1 = [Nbar1*B];
% 求阶跃响应并显示
T = 0:0.005:5;
U = 0.2*ones(size(T));
[Y1,X] = Lsim(Ac0,Bcn0,Cc,Dc,U,T);
plot(T,Y1(:,1),'-.',T,Y1(:,2),'-',T,Y1(:,3),'--',T,Y1(:,4),':')
legend('LQR-Cart Position','LQR-Pendulum1','LQR-Pendulum2','LQR-Pendulum3')
grid
figure
[Y2,X] = Lsim(Ac1,Bcn1,Cc,Dc,U,T);
plot(T,Y2(:,1),'-.',T,Y2(:,2),'-',T,Y2(:,3),'--',T,Y2(:,4),':')
legend('LQY-Cart Position','LQY-Pendulum1','LQY-Pendulum2','LQY-Pendulum3')
grid
%==============end=======================