fangzhen002.m

关于固高科技有限公司一级倒立摆lqr算法的连续系统离散系统matlab源程序及仿真

M=1.096;                                   %小车质量
m=0.109;                                   %摆杆质量
b=0.1;                                     %小车摩擦系数
l=0.25;                                    %摆杆转动轴心到杆质心的长度
I=0.0034;                                  %摆杆惯量
T=0.005;                                   %采样时间
g=9.8;

A=[0               1                           0                       0;
   0  -((I+m*l^2)*b)/(I*(M+m)+M*m*l^2)   m^2*g*l^2/(I*(M+m)+M*m*l^2)   0;
   0               0                           0                     1;
   0   -m*l*b/(I*(M+m)+M*m*l^2)       m*g*l*(M+m)/(I*(M+m)+M*m*l^2)    0]
B=[ 0;   (I+m*l^2)/(I*(M+m)+M*m*l^2);    0;    m*l/(I*(M+m)+M*m*l^2) ]
C=[1 0 0 0;
   0 1 0 0;
   0 0 1 0;
   0 0 0 1 ];
D=[0;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=');
  q33=input('please input q33 \n q33=');
  %disp('your Q matrix is');
  Q=[q11 0 0   0;
      0  0 0   0;
      0  0 q33 0;
      0  0 0   0];
  R=1;
  disp('your feedback matrix ')
  [K,S,e]=dlqr(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 fangzhen001');
fangzhen002;                                      %调用仿真模型