doublependul_inv1.m

关于固高科技有限公司的二级倒立摆lqr算法仿真及matlab源程序

m0=1.600;m1=0.185;m2=0.2;%m0是小车及驱动系统的等效质量,m1是下摆质量
j1=0.00547;j2=0.00549;   %m2是上摆质量,j1是下摆圆心对转轴处转动惯量
d1=0.283;d2=0.245;d3=0.483;%j2是上摆圆心对转轴处转动惯量 
f0=11.8519;f1=0.006415;f2=0.006717;%d1下摆圆心之转轴间的距离,d2上摆圆心之转轴间的距离
G0=6.6889;g=10; %d3是上下摆转轴间的距离,f0小车系统的摩擦系数,f1下摆摩擦阻力系数

G=[G0, 0, 0].';  %f2上摆摩擦阻力系数,G0力与控制电压之比
M=[m0+m1+m2, m1*d1+m2*d3, m2*d2; m1*d1+m2*d3, j1+m1*d1^2+m2*d3^2, m2*d2*d3; m2*d2, m2*d2*d3, j2+m2*d2^2];
F=[-f0, 0, 0; 0, -(f1+f2), f2; 0, f2, -f2];
N=[0, 0, 0; 0, (m1*d1+m2*d3)*g, 0; 0, 0, m2*d2*g];
T0=[1, 0, 0;0, 1, 0; 0, -1, 1];
A21=-T0*inv(M)*N*inv(T0);
A22=inv(T0)*inv(M)*F*T0;
B2=T0*inv(M)*G;
H1=zeros(3);
H2=eye(3);
H3=zeros(3,1);
A=[H1,H2; A21, A22];
B=[H3; B2];
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]
Q=[1 0 0 0 0 0;0 50 0 0 0 0;0 0 300 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0];
R=1
[K,S,E]=lqr(A,B,Q,R)