%%二次型最优控制求解二级直线倒立摆稳定性问题
%在这里面不考虑质量块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
二次型最优控制求解二级直线倒立摆稳定性问题--matlab求解
最新推荐文章于 2021-03-20 06:49:20 发布