一级倒立摆MATLAB源代码3

%倒立摆程序
%M为小车质量 m为摆杆质量 b为小车摩擦系数 L为摆杆转动轴心到摆杆质心的长度 I为摆杆转动惯量
%function dlb
clear; %清参数
clc; %清屏
M=1.096;
m=0.109;
b=0.1;
l=0.25;
I=0.00223; %转动惯量I=mL'^2/12 <本题应是以摆杆终点为旋转中心>；摆杆长度L'=2L
g=9.8;
a=-[(I+m*l*l)*b]/[I*(M+m)+M*m*l*l]; %受力分析后得出的
b=(m*m*g*l*l)/[I*(M+m)+M*m*l*l];
c=-(m*l*b)/[I*(M+m)+M*m*l*l];
d=[m*g*l*(M+m)]/[I*(M+m)+M*m*l*l];
e=(I+m*l*l)/[I*(M+m)+M*m*l*l];
f=m*l/[I*(M+m)+M*m*l*l];
A=[0 1 0 0;0 a b 0;0 0 0 1;0 c d 0]; %构造状态空间表达式
B=[0;e;0;f];
C=[1 0 0 0;0 0 1 0];
D=[0;0];
%a=[m*l*(M+m)*g]/[I*(M+m)+M*m*l*l]; %31.6925
%b=-(m*m*l*l*g)/[I*(M+m)+M*m*l*l]; %-0.7167
%c=(m*l)/[I*(M+m)+M*m*l*l]; %2.6838
%d=(I+m*l*l)/[I*(M+m)+M*m*l*l]; %0.8906
%A=[0 1 0 0;a 0 0 0;0 0 0 1;b 0 0 0];
%B=[0;c;0;d];
%C=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
%D=[0;0;0;0];
% C=[1 0 0 0;0 1 0 0];
% D=[0;0];
[num,den]=ss2tf(A,B,C,D) %状态空间表达式——>传递函数为-2.6838/(s^2-31.6926)
[z,p,k]=tf2zp(num,den) %传递函数——>零极点型
uc=ctrb(A,B) %判断能控性
r=rank(uc) %求秩
ac=obsv(A,C) %判断能观性
q=rank(ac) %求秩
p=[-10 -10 -2+3.3j -2-3.3j]; %期望极点配置
K=acker(A,B,p) %求状态增益K
Ac=A-B*K;
Bc=B;
Cc=C;
Dc=D;
[num1,den1]=ss2tf(Ac,Bc,Cc,Dc)
[z1,p1,k1]=tf2zp(num1,den1);
uc1=ctrb(Ac,Bc);
r1=rank(uc);
ac1=obsv(Ac,Cc);
q1=rank(ac)
T=0:0.005:5; %T=(0,O.OO5,0;010,;;;;;;;5) 1行*1001列
U=0.2*ones(size(T)); %输入U=0.2*T规模的单位阵 size规模 ones单位阵；即U=（0.2,0.2,0.2,............0.2） 1行*1001列
[Y,X]=lsim(Ac,Bc,Cc,Dc,U,T); %系统在U输入下的时域响应（Y是输出，X是状态；Y1列，Y2列对应分别两个输入车位移和摆的摆角；X对应四个状态X,X',ф，ф'）
figure %开始作图
plot(T,X(:,1),'-b',T,Y(:,2),'-r') %将车位移X（Y）的第一列(X)，<蓝>；画摆角Y（X）的第二列(ф），<红>，画在一幅图中
legend('Cart Position','Pendulum Angle')%分别标成'Cart Position'<车位置>,'Pendulum Angle'<摆角>
grid %清除网格线

%根轨迹法
%传递函数为：(m*l)/[(I+m*l*l)*s*s-m*g*l] 3/(s*s-29.5)