新建 文本文档.txt

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

%this is an inverted pendulum model
M=1.32;                                   %小车质量
m1=0.04;                                %摆杆1质量
m2=0.132;                                %摆杆2质量
m3=0.208;
l1=0.09;   
l2=0.27;
g=9.8;
k11=0;
k12=1.5*(-2*m1*g-4*g*m2-4*g*m3)/((-4*m1-3*m2-12*m3)*l1)
k13=4.5*m2*g/((-4*m1-3*m2-12*m3)*l1)
k14=0
k15=0
k16=0
k17=1.5*(-2*m1-m2-4*m3)/((-4*m1-3*m2-12*m3)*l1)
k21=0
k22=2*g*(m1+2*(m2+m3))/(4*m2*l2-(16/9)*(m1+3*(m2+m3))*l2)
k23=-(4/3)*g*(m1+3*(m2+m3))/(4*m2*l2-(16/9)*(m1+3*(m2+m3))*l2)
k27=(2*(m1+2*(m2+m3))-(4/3)*(m1+3*(m2+m3)))/(4*m2*l2-(16/9)*(m1+3*(m2+m3))*l2)

A=[ 0      0    0      1       0       0;
    0      0    0      0       1       0;
    0      0    0      0       0       1;
    0      0    0      0       0       0;
    0     k12   k13    0       0       0;
    0     k22   k23    0       0       0]
    
B=[ 0;0;0;1;k17;k27]
C=[1 0 0 0 0 0;
   0 1 0 0 0 0;
   0 0 1 0 0 0];
D=[0;0;0];

%sys1=ss(A,B,C,D);
%po=pole(sys1)    % 开环系统极点
po=eig(A)

Co=ctrb(A,B);
Ob = obsv(A,C);
unctrb=length(A)-rank(Co)                 %可控性
unobv = length(A)-rank(Ob)                %可观性

key=1;
while key~=0
  q11=input('please input q11 \n q11=');
  q22=input('please input q22 \n q22=');
  q33=input('please input q33 \n q33=');
  %disp('your Q matrix is');
  Q=[q11 0     0  0  0   0;
      0  q22   0  0  0   0;
      0  0    q33 0  0   0;
      0  0    0   0  0   0;
      0  0    0   0  0   0;
      0  0    0   0  0   0 ];
  R=1;
  disp('your feedback matrix ')
  [K,S,e]=lqr(A,B,Q,R)
  
  %sys2=ss(A-B*K,B,C,D);
  %pc=pole(sys2)                           %闭环系统极点
  
  %pc=eig(A-B*K)
  key=input('Would you continue? Select 0 to end program,your choice is  '); 
end

%disp('call simulation lqr1');
%lqr1;                                      %调用仿真模型