1、系统建模
二级倒立摆系统的机械部分主要是由小车、摆杆1、摆杆2、导轨、皮带轮、传送皮带等组成，电气部分由电机、功率放大器、PWM、传感器、驱动电路以及保护电路组成。系统结构如下图所示。
中间的推导过程不再赘述
取 x 1 = x x_1=x x1=x, x 2 = x ˙ x_2=\dot{x} x2=x˙, x 3 = θ 1 x_3=\theta_1 x3=θ1, x 4 = θ ˙ 1 x_4=\dot{\theta}_1 x4=θ˙1, x 5 = θ 2 x_5=\theta_2 x5=θ2, x 6 = θ ˙ 2 x_6=\dot{\theta}_2 x6=θ˙2,系统状态方程为：
即得到形式如下的状态方程：
x ˙ = A X + B U \dot{x}=AX+BU x˙=AX+BU
Y = C X + D U Y=CX+DU Y=CX+DU
2、MATLAB程序
A=[0 1 0 0 0 0;
0 -16.6601 -1.2973 0 0.0857 0;
0 0 0 1 0 0;
0 39.0555 18.0514 0 -7.8603 0;
0 0 0 0 0 1;
0 -68.5120 -14.4458 0 25.9635 0];
B=[0;0.7270;0;-1.7044;0;0.2069];
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]; %这里的ABCD矩阵都可以根据具体的数值进行修改
p=eig(A) %求取A矩阵的特征值
[num,den]=ss2tf(A,B,C,D,1); %将状态空间模型转变成传递函数形式
printsys(num,den) %输出以num为分子多项式，以den为分母多项式的系统传函
Q=[1000 0 0 0 0 0;
0 0 0 0 0 0;
0 0 10 0 0 0;
0 0 0 0 0 0;
0 0 0 0 10 0;
0 0 0 0 0 0]; %根据具体要求进行修改
Tc=ctrb(A,B); %可控性
rank(Tc) %Tc矩阵的秩
To=obsv(A,C); %可观性
rank(To) %To矩阵的秩
R=1;
K=lqr(A,B,Q,R) %求取K矩阵
Ac=[(A-B*K)];
Bc=[B];
Cc=[C];
Dc=[D];
T=0:0.005:20; %时间，第一个和第三个是时间的起始和末尾，中间是采样间隔
U=0.2*ones(size(T)); %系统输入
[Y,X]=lsim(Ac,Bc,Cc,Dc,U,T); %计算系统对输入向量u的响应
plot(T,Y(:,1),':',T,Y(:,2),'-',T,Y(:,3),'*') %绘制系统的响应曲线
legend('cart position ', 'pendulum angle1' ,'angle2') %曲线的参数
grid %保留图像的栅格