二次型最优控制求解二级直线倒立摆稳定性问题--matlab求解

%%二次型最优控制求解二级直线倒立摆稳定性问题

%在这里面不考虑质量块m3的影响
%参数取值情况
% m   1.328kg
% M1  0.22kg
% M2  0.187kg 
% J1  0.0049kg/m^2
% J2  0.0048kg/m^2
% L   0.49m
% L1  0.304m
% L2  0.226m 
% f   22.915N/ms^-1
% g   9.8m/s^2

A=[0 1 0 0 0 0;
0 -16.6601 -1.2973 0 0.0857 0;
0 0 0 1 0 0;
0 39.0555 18.0514 0 -7.8603 0;
0 0 0 0 0 1;
0 -68.512 0 -14.4458  25.9635 0];

B=[0;
0.7270;
0;
-1.7044;
0;
0.2069];

C=[1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0;
0 0 0 1 0 0;
0 0 0 0 1 0;
0 0 0 0 0 1];

D=[0;0;0;0;0;0];

p=eig(A);
[ num, den]=ss2tf(A,B,C,D,1); 
printsys(num, den);

Q=[1000 0 0 0 0 0;
0 0 0 0 0 0;
0 0 10 0 0 0;
0 0 0 0 0 0;
0 0 0 0 10 0;
0 0 0 0 0 0];

Tc=ctrb(A,B);
rank(Tc)    %判断能观性
To=obsv(A,C);
rank(To)    %判断能控性

R=1;
K=lqr(A,B,Q,R); %状态反馈增益矩阵
Ac=(A-B*K);
Bc=B;
Cc=C;
Dc=D;
T=0:0.005:20;
U=0.2*ones(size(T));
[Y,X]=lsim(Ac,Bc,Cc,Dc,U,T);
plot(T,Y(:,1),':',T,Y(:,2),':',T,Y(:,3),':')
legend('car position','anglel','angle2');
grid