doublelqrlqy.m

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------------------doublelqrlqy.m---------------------
%% 最优控制（量纲匹配）:为了使输入与反馈的量纲互相匹配，给输入乘以增益,
%在前面的设计方法中，是把输出信号反馈回来乘以一个系数矩阵,然后与输入量 
%相减得到控制信号 
% 确定开环极点的程序如下
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%系统状态空间方程
A=[0 0 0 1 0 0;
    0 0 0 0 1 0;
    0 0 0 0 0 1;
    0 0 0 0 0 0; 
    0 64.966 -14.865 0 0 0;
    0 -32.483 31.933 0 0 0];
B=[0;0;0;1;5.1124;0.0562];
C=[eye(3) zeros(3)];
D=[0;0;0];
%
Q0=1*diag([1 1 1 0 0 0]);
R0=0.8;
Q1=8*diag([1 1 1]);
R1=0.1;
% 计算LQR控制矩阵
K0 = lqr(A,B,Q0,R0)
% 计算LQY控制矩阵
K1=lqry(A,B,C,D,Q1,R1)
Ac0 = [(A-B*K0)];
Ac1 = [(A-B*K1)];
Bc = [B];
Cc = [C];
Dc = [D];
% 计算LQR增益Nbar0
Cn = [1 0 0 0 0 0];
Nbar0 = rscale(A,B,Cn,0,K0);
Bcn0 = [Nbar0*B];
% 计算LQY增益Nbar1
Nbar1 = rscale(A,B,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),'--')
hold on
[Y2,X] = Lsim(Ac1,Bcn1,Cc,Dc,U,T);
plot(T,Y2(:,1),'-.',T,Y2(:,2),'-',T,Y2(:,3),'--')
hold off
grid
% 计算LQR控制矩阵
K0 = lqr(A,B,Q0,R0)
% 计算LQY控制矩阵
K1=lqry(A,B,C,D,Q1,R1)
Ac0 = [(A-B*K0)];
Ac1 = [(A-B*K1)];
Bc = [B];
Cc = [C];
Dc = [D];
% 计算LQR增益Nbar0
Cn = [1 0 0 0 0 0];
Nbar0 = rscale(A,B,Cn,0,K0);
Bcn0 = [Nbar0*B];
% 计算LQY增益Nbar1
Nbar1 = rscale(A,B,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),'--')
hold on
[Y2,X] = Lsim(Ac1,Bcn1,Cc,Dc,U,T);
plot(T,Y2(:,1),'-.',T,Y2(:,2),'-',T,Y2(:,3),'--')
hold off
grid
legend('LQR-Cart Position','LQR-Pendulum1','LQR-Pendulum2','LQY-Cart Position','LQY-Pendulum1','LQY-Pendulum2')
%=====================end==========================