倒立摆的状态观测器设计与仿真

发表于 2023-11-26 分类于大学课程 Waline：本文字数： 987 阅读时长 ≈ 4 分钟

状态估计原理

在倒立摆的状态反馈控制一文中，我们默认系统状态可以直接获得。但实际中有的状态无法直接测量，而且还要考虑噪声的影响，所以经常需要设计状态观测器估计系统状态。

本文采用Luenberger状态观测器，其动态特性由如下方程描述

\[ \dot{\hat{x}}=A\hat{x}+Bu+L(y-\hat{y}) \tag{1} \] \[ \hat{y}=C\hat{x}+Du \tag{2} \]

\((1)\)的最后一项根据实际系统输出\(y\)与估计的系统输出\(\hat{y}\)之间的差值来修正系统状态估计\(\hat{x}\)，状态估计误差\(e=x-\hat{x}\)满足

\[ \dot{e}=\dot{x}-\dot{\hat{x}}=(Ax+Bu)-\left[ A\hat{x}+Bu+L(y-\hat{y}) \right] \tag{3} \]

即

\[ \dot{e}=(A-LC)e \tag{4} \]

如果矩阵\(A-LC\)的特征值都有负的实部，则\(e\)会趋于零，通过配置\(A-LC\)的特征值可以决定\(e\)的收敛速度。在设计控制器和观测器之前，要判断系统的能控性和能观性，在最后的代码中会有体现，这里就不多提了。

基于状态观测器的控制

现在基于状态估计\(\hat{x}\)对倒立摆系统进行反馈控制，反馈控制信号

\[ u=r-K\hat{x} \tag{5} \]

系统状态方程为

\[ \dot{x}=Ax+B(r-K\hat{x})=(A-BK)x+BK(x-\hat{x})+Br \tag{6} \]

即有

\[ \dot{x}=(A-BK)x+BKe+Br \tag{7} \]

将\((4)\)和\((7)\)合并，得到如下矩阵方程

\[ \left[ \begin{array}{c} \dot{x}\\ \dot{e}\\ \end{array} \right] =\left[ \begin{matrix} A-BK& BK\\ 0& A-LC\\ \end{matrix} \right] \left[ \begin{array}{c} x\\ e\\ \end{array} \right] +\left[ \begin{array}{c} B\\ 0\\ \end{array} \right] r \tag{8} \]

系统输出

\[ y=\left[ \begin{matrix} C& 0\\ \end{matrix} \right] \left[ \begin{array}{c} x\\ e\\ \end{array} \right] \tag{9} \]

那么观测器和控制器耦合到一起形成了一个系统，整个系统特征方程变为\(2n\)阶的。系统整体结构图可以在任何一本相关教材上找到，这里就不再贴出。基于倒立摆的状态反馈控制中的代码修改，得到如下MATLAB仿真代码：

% 倒立摆模型
M = 0.5; m = 0.2; b = 0.1; I = 0.006; g = 9.8; l = 0.3;
den = I*(M+m)+M*m*l^2;
A = [0,        1,                 0,        0;
     0, -(I+m*l^2)*b/den, -(m^2*g*l^2)/den, 0;
     0,        0,                 0,        1;
     0,   (m*l*b)/den,     m*g*l*(M+m)/den, 0];
B = [      0;
     (I+m*l^2)/den;
           0;
       -m*l/den];
C = [1, 0, 0, 0;
     0, 0, 1, 0];
D = [0;
     0];
sys_open = ss(A, B, C, D);

% 判断系统的能控性和能观性
co = ctrb(sys_open);
controllability = rank(co);
ob = obsv(sys_open);
observability = rank(ob);

% 控制器和观测器的极点配置
poles = [-5; -10; -5+10j; -5-10j];
K = place(A, B, poles);
poles = [-7; -8; -9; -10];
L = place(A', C', poles)';

% 基于状态观测的反馈控制系统模型
Aco = [A-B*K, B*K;
       zeros(size(A)), A-L*C];
Bco = [B; zeros(size(B))];
Cco = [C, zeros(size(C))]; Dco = 0;
sys_co = ss(Aco, Bco, Cco, Dco);
t = 0:0.01:2;
% 初始状态，状态x和误差e的初始值都要设置
x0 = [-0.2; 0; 0.1; 0]; e0 = -x0;
xe0 = [x0; e0];
% 输入信号设置，使位移x的终值为0.1
rss = 0.1/(-0.0071); r = rss*ones(size(t));
% 同时返回(9)中的输出y和(8)中的状态xe，即[x; e]向量保存到xe中
[y, t, xe] = lsim(sys_co, r, t, xe0);
% 绘图，分别是输出y和状态估计误差e的图像
figure(1); plot(t, y(:, 1:2), 'LineWidth', 1.5); grid on
legend('Cart position (m)', 'Pendulum angle (rad)');
title('Step Response with Observer Feedback Control')
figure(2); plot(t, xe(:, 5:8), 'LineWidth', 1.5); grid on
title('State Estimate Error')

由于观测器估计的系统状态与实际状态有误差，所以系统输出（小车位移和摆杆角度）随时间变化规律与之前的有差异：

状态估计误差随着时间推移衰减为零：

状态估计原理

基于状态观测器的控制

参考资料