threelqrlqy.m

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

%===================================
%parameters
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;             
%state space equation
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)^2];
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]';
sys=ss(A,B,C,D);%
sysc=canon(sys,'modal');%
GG=minreal(sys)   %
Co=ctrb(A,B)
rc=rank(Co)
Ob=obsv(A,C)
or=rank(Ob)
p = eig(A);
% 求向量K
Q =diag([1 1 1 1 1 1 1 1 ]);
R = 0.2; 
K = lqr(A,B,Q,R)
% 计算LQR控制矩阵
Ac = [(A-B*K)];
Bc = [B];
Cc = [C];
Dc = [D];
% 计算增益Nbar
Cn = [1 0 0 0 0 0 0 0];
Nbar = rscale(A,B,Cn,0,K);
Bcn = [Nbar*B];
% 求阶跃响应并显示，小车位置为虚线，摆杆角度为实线
T = 0:0.005:5;
U = 0.2*ones(size(T));
x0=[0;(15/180)*pi;(10/180)*pi;-0.5;0;0;0];
[Y,X] = Lsim(Ac,Bcn,Cc,Dc,U,T,x0);
plot(T,Y(:,1),':',T,Y(:,2),'-',T,Y(:,3),'--',T,Y(:,4),'-.'),figure;
legend('Cart Position','Pendulum Angle1','Pendulum Angle2','Pendulum Angle3')
step(sys,T)
% —————— end ——————
% ———— rscale.m ————
% 求取输入输出匹配系数
function[Nbar] = rscale(A,B,C,D,K) 
s = size(A,1);
Z = [zeros([1,s]) 1];
N = inv([A,B;C,D])*Z';
Nx = N(1:s);
Nu = N(1+s);
Nbar = Nu + K*Nx;
% —————— end ——————