二阶倒立摆LQR仿真

%%parameter1 

clear
close
%参数输入
M0=1.328; %小车、皮带、电机转子、皮带轮归算到小车运动上的等效质量
M1=0.220; %下摆质量
M2=0.187; %上摆质量
J1=0.00496; %下摆转动惯量
J2=0.00482; %上摆转动惯量
L1=0.490; %下摆全长（轴心到轴心）
l1=0.304; %下摆质心与小车——下摆连接轴心距离
l2=0.226; %上摆质心与上摆——下摆连接轴心距离
F01=22.947; %小车与导轨间摩擦力，电机机械摩擦转矩，皮带轮摩擦转矩归算到小车运动上的等效摩擦系数
F1=0.00705; %下摆与小车摩擦力矩的等效摩擦系数
F2=0.00264; %上、下摆间摩擦力矩的等效摩擦系数
R=8.550; %电机等效电阻
Ri=1.252; %功率放大器等效输出电阻
d=0.130; %皮带轮直径
K=8.000; %功率放大器电压增益
Kt=0.946; %转矩系数
g=9.80; %重力加速度

%计算矩阵A,B,C,D
G0=2*Kt*K/(d*(R+Ri)); %放大器输入电压到电机输出之间的的系数
F0=F01+4*(Kt)^2/(d^2*(R+Ri));
M=[M0+M1+M2 M1*l1+M2*L1 M2*l2;M1*l1+M2*L1 M1*l1*l1+M2*L1*L1+J1 M2*L1*l2;M2*l2 M2*L1*l2 M2*l2*l2+J2];
F=[F0 0 0;0 F1+F2 -F2;0 -F2 F2];
N=[0 0 0; 0 -(M1*l1+M2*L1)*g 0; 0 0 -M2*g*l2];

T0=[1 0 0;0 1 0; 0 -1 1];
A21=-T0*inv(M)*N;
A22=-T0*inv(M)*F;
b2=T0*inv(M)*[G0;0;0];
A=[zeros(3,3) eye(3)
A21 A22];
B=[zeros(3,1)
b2];
C=[eye(3) zeros(3,3)];
D=[0 0 0]';

 %%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%

%系统状态方程描述
parameter1 %调用parameter1.m文件

%q和r矩阵的选择
R=[0.1];
N=[0 0 0 0 0 0]';
Q=diag([900,1,1,1,1,1]);

%求解线性二次型最优状态反馈矩阵
[K P E]=lqr(A,B,Q,R,N)
%求解系统闭环状态方程
Ac=A-B*K
%输出系统阶跃仿真
T=0:0.005:10;
U=eye(size(T));
[Y X]=lsim(Ac,B,C,D,U,T);
plot(T,Y(:,1),'g-',T,Y(:,2),'b:',T,Y(:,3),'r-');
grid
legend('小车位移','下摆角位移','上、下摆角位移偏差');