线性系统大作业——2.二阶倒立摆建模与控制系统设计（下）
文章目录
- 7.状态观测器设计
-
- 7.1.全阶观测器
- 7.2.最小阶观测器
- 8.基于观测器的状态反馈控制
-
- 8.1.基于全阶观测器的状态反馈控制
- 8.2.基于最小观测器的状态反馈控制
- 9.LQR控制
本文上半部分见：线性系统大作业——2.二阶倒立摆建模与控制系统设计（上）
最后，本文中使用的 MATLAB 代码和 Simulink 仿真模型已经上传到 GitHub 上。
Code: https://github.com/Cc19245/Inverted-pendulum.git
7.状态观测器设计
7.1.全阶观测器
前文已经验证，取系统的输出变量为 x , θ 1 , θ 2 x,\theta_1,\theta_2 x,θ1,θ2，系统是能观的。因此这里可以直接进行观测器的配置。假设所配置的观测器极点是状态反馈控制器的极点的三倍，即为 3 ∗ [ − 2 + j ∗ 2 , − 2 − j ∗ 2 , − 6 , − 7 , − 8 , − 9 ] 3*[-2+j*2,-2-j*2,-6,-7,-8,-9] 3∗[−2+j∗2,−2−j∗2,−6,−7,−8,−9]，编写MATLAB代码进行进行极点配置如下，需要注意的是此时系统属于多输入系统，因此只能使用place命令进行极点配置，而不能使用acker命令进行配置。
A_ = A';
B_ = C';
C_ = B';
J_ = 3*J;
Ke = (place(A_, B_, J_))';
disp('Ke = ');
disp(Ke);
程序输出结果为 K e = [ 132.9964 143.1468 0.4900 − 122.4083 214.2149 − 33.0750 0 − 99.2250 570.5250 27.2900 6.8972 0 − 5.1003 29.7100 0 0 0 45.0000 ] \begin{aligned} K_e = \left[ \begin{array}{ccc} 132.9964 & 143.1468 & 0.4900 \\ -122.4083 & 214.2149 & -33.0750 \\ 0 & -99.2250 & 570.5250 \\ 27.2900 & 6.8972 & 0 \\ -5.1003 & 29.7100 & 0 \\ 0 & 0 & 45.0000 \end{array}\right]\end{aligned} Ke=⎣⎢⎢⎢⎢⎢⎢⎡132.9964−122.4083027.2900−5.10030143.1468214.2149−99.22506.897229.710000.4900−33.0750570.52500045.0000⎦⎥⎥⎥⎥⎥⎥⎤
7.2.最小阶观测器
对于二阶倒立摆系统来说，状态变量 x , θ 1 , θ 2 x,\theta_1,\theta_2 x,θ1,θ2等于输出 y y y，因此可以直接测量，剩余的状态变量这是不可测量的部分。对状态空间模型进行划分，划分后的降阶观测器是6-3=3阶的系统，假设期望配置的极点位置为 3 ∗ [ − 2 + j ∗ 2 , − 2 − j ∗ 2 , − 6 ] 3*[-2+j*2,-2-j*2,-6] 3∗[−2+j∗2,−2−j∗2,−6]，则可编写MATLAB代码如下：
Abb = A(1:3, 1:3);
Aba = A(1:3, 4:end);
Aab = A(4:end, 1:3);
Aaa = A(4:end, 4:end);
J_j = 3*J(1:3); % 选择前三个极点
Kb = K(4:end);
Ke_j = (place(Abb',Aab',J_j))';
disp('Ke_j = '); % 注意这里把状态变量顺序换了，变成[x1;x2;x3;dx1;dx2;dx3]
disp(Ke_j);
程序输出结果为 K e j = [ 6 − 6 0 6 6 0 0 0 18 ] \begin{aligned} K_{ej} = \left[ \begin{array}{ccc} 6 & -6 & 0 \\ 6 & 6 & 0 \\ 0 & 0 & 18 \end{array}\right]\end{aligned} Kej=⎣⎡660−6600018⎦⎤
8.基于观测器的状态反馈控制
8.1.基于全阶观测器的状态反馈控制
搭建具有全阶观测器的状态反馈控制器的二阶倒立摆非线性系统Simulink仿真模型如图所示。
Simulink仿真模型中，全阶状态观测器由于设计变量较多，使用Simulink模块搭建较为复杂，因此编写s-function来实现，代码如下：
% 二阶倒立摆系统的全阶观测器s-function建模
function [sys,x0,str,ts,simStateCompliance] =
order2_all_observer_sfun(t,x,u,flag, x_0, th1_0, th2_0)
switch flag,
case 0,
[sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u, x_0, th1_0, th2_0);
case 1,
sys=mdlDerivatives(t,x,u);
case 2,
sys=mdlUpdate(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
case 9,
sys=mdlTerminate(t,x,u);
otherwise
DAStudio.error('Simulink:blocks:unhandledFlag',
num2str(flag));
end
% 主函数结束
% ---------------------------------------------
function [sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u,x_0, th1_0, th2_0)
% 初始化
sizes = simsizes;% 生成sizes数据结构
sizes.NumContStates = 6;% 连续状态数
sizes.NumDiscStates = 0;% 离散状态数,缺省为 0
sizes.NumOutputs = 6;% 输出量个数,缺省为 0
sizes.NumInputs = 3;% 输入量个数，缺省为 0
sizes.DirFeedthrough = 1;% 是否存在直接馈通。1：存在；0：不存在，缺省为 1
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
x0 = [0; 0; 0; x_0; th1_0; th2_0];% 设置初始状态
str = [];% 保留变量置空
ts = [0 0]; % 连续系统
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes
% ---------------------------------------------
function sys=mdlDerivatives(t, x, u)
assert(all(imag(u)==0), 'u is imaginary or nan');
% 计算导数例程子函数
A =[0, 0, 0, 0, -4.4100, 0.4900;
0, 0, 0, 0, 77.1750, -33.0750;
0, 0, 0, 0, -99.2250, 84.5250;
1, 0, 0, 0, 0, 0;
0, 1, 0, 0, 0, 0;
0, 0, 1, 0, 0, 0];
B = [0.4667; -1.5000; 0.5000; 0; 0; 0];
K = [23.4125, -0.5709, 44.4357, 22.3907, -283.0923, 379.2252];
Ke = [132.9964 143.1468 0.4900;
-122.4083 214.2149 -33.0750;
0 -99.2250 570.5250;
27.2900 6.8972 0;
-5.1003 29.7100 0;
0 0 45.0000];
sys = (A-B*K)*x + Ke*u ; % 注意这里的u就是x(1)的观测误差e(1)
% ---------------------------------------------
function sys=mdlUpdate(t,x,u)
%3. 状态更新例程子函数
sys = [];
%% ---------------------------------------------
function sys=mdlOutputs(t,x,u)
%4. 计算输出例程子函数
sys=x;
% ---------------------------------------------
function sys=mdlGetTimeOfNextVarHit(t,x,u)
% 5. 计算下一个采样时间，仅在系统是变采样时间系统时调用
sampleTime = 1; % Example, set the next hit to be one second later.
sys = t + sampleTime;
% ---------------------------------------------
function sys=mdlTerminate(t,x,u)
% 6. 仿真结束时要调用的例程函数
sys = [];
假设系统初始状态为 [ x 1 , x 2 , x 3 , x 4 , x 5 , x 6 ] = [ x ˙ , θ 1 ˙ , θ 2 ˙ ] = [ 0 , 0 , 0 , 0 , 0 , 5 ∘ ] \left[x_1,x_2,x_3,x_4,x_5,x_6\right]=\left[\dot{x},\dot{\theta_1},\dot{\theta_2}\right]=\left[0,0,0,0,0,5^{\circ}\right] [x1,x2,x3,x4,x5,x6]=[x˙,θ1˙,θ2˙]=[0,0,0,0,0,5∘]，并且状态观测器的初始观测结果为 [ x 1 ~ , x 2 ~ , x 3 ~ , x 4 ~ , x 5 ~ , x 6 ~ ] = [ 0 , 0 , 0 , 0 , 0 , 0 ] \left[\widetilde{x_1},\widetilde{x_2},\widetilde{x_3},\widetilde{x_4},\widetilde{x_5},\widetilde{x_6}\right]=\left[0,0,0,0,0,0\right] [x1 ,x2 ,x3 ,x4 ,x5 ,x6 ]=[0,0,0,0,0,0]，运行Simulink仿真模型，系统的动态响应如图所示，全阶观测器的观测误差如图，可见系统可以很好地稳定。
8.2.基于最小观测器的状态反馈控制
为了检验基于最小阶观测器的状态反馈控制效果，搭建具有最小阶观测器的状态反馈控制器的二阶倒立摆非线性系统Simulink仿真模型如图所示。
Simulink仿真模型中，最小阶状态观测器由于设计变量较多，使用Simulink模块搭建较为复杂，因此编写s-function来实现，代码如下：
% 二阶倒立摆系统的最小阶观测器s-function建模
function [sys,x0,str,ts,simStateCompliance] =
order2_min_observer_sfun(t,x,u,flag, x_0, th1_0, th2_0,
dx_0, dth1_0, dth2_0)
switch flag,
case 0,
[sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u, x_0, th1_0, th2_0,
dx_0, dth1_0, dth2_0);
case 1,
sys=mdlDerivatives(t,x,u);
case 2,
sys=mdlUpdate(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
case 9,
sys=mdlTerminate(t,x,u);
otherwise
DAStudio.error('Simulink:blocks:unhandledFlag',
num2str(flag));
end
% 主函数结束
% ---------------------------------------------
function [sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u,x_0,th1_0,th2_0,dx_0,dth1_0,dth2_0)
% 初始化
sizes = simsizes;% 生成sizes数据结构
sizes.NumContStates = 3;% 连续状态数, 分别是dx, dth1, dth2
sizes.NumDiscStates = 0;% 离散状态数,缺省为 0
sizes.NumOutputs = 6;% 输出量个数,缺省为 0
sizes.NumInputs = 4;% 输入量个数，缺省为 0
sizes.DirFeedthrough = 1;% 是否存在直接馈通。1：存在；0：不存在，缺省为 1
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
x0 = [dx_0; dth1_0; dth2_0] -
[6, -6, 0; 6, 6, 0; 0,0,18]*[x_0; th1_0; th2_0]; % 设置初始状态
str = [];% 保留变量置空
ts = [0 0]; % 连续系统
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes
% ---------------------------------------------
function sys=mdlDerivatives(t, x, u)
% 计算导数例程子函数
A = [0 0 0 0 -4.4100 0.4900;
0 0 0 0 77.1750 -33.0750;
0 0 0 0 -99.2250 84.5250;
1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0];
B = [0.4667; -1.5000; 0.5000; 0; 0; 0];
Ke_j = [6 -6 0;
6 6 0;
0 0 18];
A_hat = A(1:3, 1:3) - Ke_j*A(4:end, 1:3);
B_hat = A_hat*Ke_j + A(1:3, 4:end) - Ke_j*A(4:end, 4:end);
F_hat = B(1:3) - Ke_j*B(4:end);
sys = A_hat*x + B_hat*[u(2);u(3);u(4)] + F_hat*u(1) ; % u1是输入，u2/3/4是可观的变量x,th1,th2
% ---------------------------------------------
function sys=mdlUpdate(t,x,u)
%3. 状态更新例程子函数
sys = [];
% ---------------------------------------------
function sys=mdlOutputs(t,x,u)
%4. 计算输出例程子函数
C_hat = [1 0 0; 0 1 0; 0 0 1; 0 0 0; 0 0 0; 0 0 0];
D_hat = [6 -6 0; 6 6 0; 0 0 18; 1 0 0; 0 1 0; 0 0 1];
sys = C_hat*x + D_hat*[u(2);u(3);u(4)];
% ---------------------------------------------
function sys=mdlGetTimeOfNextVarHit(t,x,u)
% 5. 计算下一个采样时间，仅在系统是变采样时间系统时调用
sampleTime = 1; % Example, set the next hit to be one second later.
sys = t + sampleTime;
% ---------------------------------------------
function sys=mdlTerminate(t,x,u)
% 6. 仿真结束时要调用的例程函数
sys = [];
假设系统初始状态为 [ x 1 , x 2 , x 3 , x 4 , x 5 , x 6 ] = [ x ˙ , θ 1 ˙ , θ 2 ˙ ] = [ 0.05 , 1 0 ∘ / s , 1 0 ∘ / s , 0 , 0 , 5 ∘ ] \left[x_1,x_2,x_3,x_4,x_5,x_6\right]=\left[\dot{x},\dot{\theta_1},\dot{\theta_2}\right]=\left[0.05,10^{\circ}/s, 10^{\circ}/s, 0,0,5^{\circ}\right] [x1,x2,x3,x4,x5,x6]=[x˙,θ1˙,θ2˙]=[0.05,10∘/s,10∘/s,0,0,5∘]，并且状态观测器的初始观测结果为 [ x 1 ~ , x 2 ~ , x 3 ~ , x 4 ~ , x 5 ~ , x 6 ~ ] = [ 0 , 0 , 5 ∘ / s , 0 , 0 , 0 ] \left[\widetilde{x_1},\widetilde{x_2},\widetilde{x_3},\widetilde{x_4},\widetilde{x_5},\widetilde{x_6}\right]=\left[0,0,5^{\circ}/s,0,0,0\right] [x1 ,x2 ,x3 ,x4 ,x5 ,x6 ]=[0,0,5∘/s,0,0,0]，运行Simulink仿真模型，系统的动态响应如图所示，最小阶观测器的观测误差如图，可见系统可以很好地稳定。其中由于状态变量 x , θ 1 , θ 2 x,\theta_1,\theta_2 x,θ1,θ2是系统输出可以直接测量，因此 e 4 , e 5 , e 6 e_4,e_5,e_6 e4,e5,e6的误差一直是0。可见基于最小阶观测器的系统动态响应速度比使用全阶观测器效果更好，这是因为有三个状态变量可以进行准确的观测，因此系统收敛速度更快。
9.LQR控制
选择 Q = I , R = I \boldsymbol Q= \boldsymbol I,\boldsymbol R= \boldsymbol I Q=I,R=I，使用MATLAB求解的代码如下：
Q = eye(6);
R = eye(1);
K_lqr = lqr(A,B,Q,R);
disp('K_lqr = ');
disp(K_lqr);
得到最优状态反馈矩阵为 K l q r = ( 3.2386 , − 10.7073 , 33.2032 , 1.0000 , − 286.7783 , 303.8728 ) K_lqr = (3.2386,-10.7073,33.2032,1.0000,-286.7783,303.8728) Klqr=(3.2386,−10.7073,33.2032,1.0000,−286.7783,303.8728)。
直接使用这个反馈控制矩阵在Simulink中进行二阶倒立摆的非线性模型LQR控制仿真，假设系统初始状态为 [ x 1 , x 2 , x 3 , x 4 , x 5 , x 6 ] = [ x ˙ , θ 1 ˙ , θ 2 ˙ ] = [ 0 , 0 , 0 , 0 , 0 , 5 ∘ ] \left[x_1,x_2,x_3,x_4,x_5,x_6\right]=\left[\dot{x},\dot{\theta_1},\dot{\theta_2}\right]=\left[0,0,0,0,0,5^{\circ}\right] [x1,x2,x3,x4,x5,x6]=[x˙,θ1˙,θ2˙]=[0,0,0,0,0,5∘]，系统的动态响应如图所示，可见系统也可以稳定，不过相对于前面极点配置的控制效果来说系统的动态响应较慢。
