dlb_lqr.m

倒立摆-采用LQR（线性二次型最优控制）方法实现倒立摆控制

% 倒立摆-采用LQR（线性二次型最优控制）方法实现倒立摆控制 
% 水平悬臂/摆杆一阶倒立摆
m1=128.57/1000;L1=0.6;
m2=190/1000;L2=0.115;
C=0.00157;km=0.02;
g=9.8;
p=(3*m1*m1+4*m1*m2)*L1*L2;
A=[0 1 0 0;
   0 0 -9*m1*m1*g*L1*L2/(p*L1) 18*m1*L2*C/(p*L1);
   0 0 0 1;
   0 0 (18*m1*m1+6*m1*m2)*g*L1*L2/(p*L2) -(36*m1+12*m2)*L2*C/(p*L2)];
B=[0;12*m1*L1*km/(p*L1);0;-18*m1*L1*km/(p*L2)];
C=[1 0 0 0;0 0 1 0];
D=[0;0];
sys=ss(A,B,C,D);
T=0:0.01:10;
y=step(sys,T) %离散化处理
figure(1);
plot(T,y)
legend('水平杆偏角','垂直杆偏角');

co=CTRB(A,B); ob=obsv(A,C);

controllabilit=rank(co);  %求秩
observability=rank(ob);
%x=1;y=1;
x=5000;y=100;
Q=[x 0 0 0;0 0 0 0;0 0 y 0;0 0 0 0];
R=1;
K=lqr(A,B,Q,R)
Ac=[(A-B*K)];Bc=[B];
Cc=[C];Dc=[D];
%t=0.01:10;
T=0:0.001:1.5;
U=0.2*ones(size(T));
Cn=[1 0 0 0];
figure(2);
[Y,X]=lsim(Ac,Bc,Cc,Dc,U,T);
plot(T,Y)
legend('水平杆偏角','垂直杆偏角');
Nbar=-55.55;%rscale(A,B,Cn,0,K);
Bc=[Nbar*B];
figure(3);
[Y,X]=lsim(Ac,Bc,Cc,Dc,U,T);
plot(T,Y)
legend('水平杆偏角','垂直杆偏角');