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 %保留圖像的栅格