LQG输出调节控制Matlab仿真实例

B站上一位非常优秀的讲控制理论的UP主，他的每一个视频都强烈推荐，尤其是控制理论初学者，可以避免很多弯路。

这两个是同一个视频的油管和B站版本，是很有名的Control Bootcamp系列，面向控制纯初学者。

这是一篇相对比较深入的知乎文章，作者很厉害。

卡尔曼滤波器

同样的，卡尔曼滤波器的教材也很多，同时卡尔曼滤波器也被用在多个领域，在LQG框架中的卡尔曼滤波器，指的是一个最优状态观测器（An optimal observer），这里的观测器就是现代控制理论-状态空间分析中的最常见的观测器，也叫龙伯格观测器（luenberger observer），只不过后者的极点是自由配置的，并且设计后者时并没有考虑噪声的因素，因此不能算是最优。

对于系统

\begin{array}{ll}\dot{x}=A x+B u+G w & \text { (state equation) } \\ y=C x+D u+H w+v & \text { (measurement equation) }\end{array}

其中 w 是过程噪声（process noise）， v 是观测噪声（measurement noise），它们被假设是符合高斯（Gaussian）分布的，这也是LQG中的G的来源。G、H是常系数矩阵，用于描述 x 中的各部分受到的噪声强度不一致的情况。很多教材中， H 被认为是零矩阵，区别不大。

假设

• 系统可观测
• 噪声 w 和 v 符合高斯分布，且协方差矩阵可知：
  E\left(w w^{T}\right)=Q_k, \quad E\left(v v^{T}\right)=R_k, \quad E\left(w v^{T}\right)=N_k

对于给定的观测器动态和代价函数

\begin{equation}\begin{array}{l} \dot{\hat{x}}=A \hat{x}+B u+L(y-C \hat{x}-D u) \end{array}\\ \hat{y}=\hat{x} \end{equation}

\\ J'=\lim _{t \rightarrow \infty} E\left(\{x-\hat{x}\}\{x-\hat{x}\}^{T}\right)

代价函数可以理解为观测误差的累积，之所以是 \heartsuit \heartsuit^T 的形式，是为了不想让正负相反的误差相抵消。

我们旨在找到一个常系数矩阵 L ，使得 J’ 最小。最后的结果是：

L=\left(P C^{T}+\bar{N}\right) \bar{R}^{-1}

其中

\begin{array}{l} \bar{R}=R_k+H N_k+N_k^{T} H^{T}+H Q_k H^{T} \\ \bar{N}=G\left(Q_k H^{T}+N_k\right) \end{array}

同时 P 是以下Riccati方程的解

A^{T} P+P A-(P B+N_k) R_k^{-1}\left(B^{T} P+N_k^{T}\right)+Q_k=0

使用得到的这个 L 的最优观测器，就是LQG框架使用的卡尔曼滤波器。

LQG

控制框架如图，其中的 -K 就是LQ Regulator。

w 是过程噪声（process noise）， v 是观测噪声（measurement noise）

分离性原理

假设系统可控、可观测。

LQR是一种最优的线性状态反馈，Kalman filter是一种最优的状态估计，直接将两者级联，闭环的系统可以写成

\left\{\begin{aligned} \left(\begin{array}{c} \dot{x} \\ \dot{\hat{x}} \end{array}\right) &=A_{l q g}\left(\begin{array}{l} x \\ \hat{x} \end{array}\right)+B_{l q g}\left(\begin{array}{c} w(t) \\ v(t) \end{array}\right) \\ y(t) &=C_{l q g}\left(\begin{array}{l} x \\ \hat{x} \end{array}\right)+D_{l q g}\left(\begin{array}{c} w(t) \\ v(t) \end{array}\right) \end{aligned}\right.

其中

A_{lqg}=\left[\begin{array}{cc} A & -BF \\ LC & A-BF-LC \end{array}\right] \quad B_{lqg}=\left[\begin{array}{cc} G & 0 \\ LH & L \end{array} \right] \\ C_{lqg}=\left[\begin{array}{cc} C & -DF \\ 0 & 0 \end{array}\right] \quad D_{lqg}=\left[\begin{array}{cc} H & 0 \\ 0 & I \end{array}\right]

注意到坐标可以这样变换：

\left( \begin{array}{c} {x} \\ e \end{array} \right)=\left( \begin{array}{c} {x} \\ x-\hat{x} \end{array} \right) = \left[\begin{array}{c} I&0 \\ I&-I \end{array}\right]\left( \begin{array}{c} {x} \\ \hat{x} \end{array} \right) \\ \left[\begin{array}{c} I&0 \\ I&-I \end{array}\right]\left( \begin{array}{c} {x} \\ e \end{array} \right)=\left( \begin{array}{c} {x} \\ \hat{x} \end{array} \right)

其中 e=x-\hat{x} 代表观测器的误差。

应用坐标变换，得到整个闭环系统的状态方程

\left( \begin{array}{c} \dot{x} \\ \dot{e} \end{array} \right)= \left[\begin{array}{c} I&0 \\ I&-I \end{array}\right] A_{lqg} \left[\begin{array}{c} I&0 \\ I&-I \end{array}\right]\left( \begin{array}{c} {x} \\ e \end{array} \right)+\left[\begin{array}{c} I&0 \\ I&-I \end{array}\right]B_{lqg}\left[\begin{array}{c} w(t) \\ v(t) \end{array}\right]

即

\left( \begin{array}{c} \dot{x} \\ \dot{e} \end{array} \right)= \left[\begin{array}{cc} A-BF & BF \\ 0 & A-LC \end{array}\right]\left[ \begin{array}{c} {x} \\ {e} \end{array}\right] +\left[\begin{array}{c} I&0 \\ I&-I \end{array}\right]B_{lqg}\left[\begin{array}{c} w(t) \\ v(t) \end{array}\right]

可见，此处的状态矩阵是上三角分块的，并且 A-BF 和 A-LC 都是稳定的，即它们的特征值都严格小于零，根据分块矩阵的特征值得计算规则，这个大的矩阵也是稳定的，这意味着， 我们可以将LQR和Kalman filter级联，得到一个稳定的系统。这被叫做分离性原理。

LQG仿真实例

lqg函数

实现方式很多种，可以直接调用 lqg命令 ，得到一个直接可用的控制器：

但是，lqg的性能参数默认是这种形式的

J=\frac{1}{2} \int_{t_{0}}^{\infty}\left[x(t)^{T} Q x(t)+u(t)^{T} R u(t)\right] d t

可见控制目标是”令系统状态 x 和控制量 u 尽可能地小“，可是有些时候，控制目标是令系统的输出 y=Cx 尽可能地小，这种时候，问题被称作输出调节。lqg命令没办法解决输出调节，需要使用lqr命令，得到输出调节问题的最优线性反馈增益 K ，在此之上，再设计一个卡尔曼滤波器，将两者组合起来，就可以实现LQG的输出调节。

输出调节问题

对于系统

\left\{\begin{array}{l} \dot{x}(t)=A x(t)+B u(t) \\ y(t)=C x(t)+D u(t) \end{array}\right.

输出调节的问题，性能参数长这样

J=\int_{0}^{\infty} y^{T}(t) Q_{y} y(t)+u^{T}(t) R u(t)

将性能参数表达式展开

\begin{align} J&=\int_{0}^{\infty}[C x(t)+D u(t)]^TQ_y[C x(t)+D u(t)]+u^{T}(t) R_{} u(t) \\ &=\int_{0}^{\infty}x^{T}(t){[C^TQ_yC]}x(t)+2x^T(t){[D^TQ_yC]}u(t)+u^T(t){[D^TQ_yD+R]}u(t) \\ &=\int_{0}^{\infty} x^{T}(t){ Q_{x} }x(t)+2 x^{T}(t) {N }u(t)+u^{T}(t){ R_{u} }u(t) \end{align}

这是一个由 x 和 u ，而不是 y 和 u 组成的二次型表达式，这样做的目的，是为了和matlab的lqr函数接口对应

lqr函数

Matlab中 lqr 函数优化的最优性能指标是

J(u)=\int_{0}^{\infty}\left(x^{T} Q x+u^{T} R u+2 x^{T} N u\right) d t

调用格式是

[K,S,e] = lqr(SYS,Q,R,N)

因此，想要解决输出调节问题，只需要先设计输出 y 和控制输入 u 的权重矩阵 Q_y,R ,再根据 Q_x=C^TQ_yC,N=D^TQ_yC,R_u=D^TQ_yD+R ,算出新的权重矩阵，调用函数就可以得到对应的最优线性状态反馈率 K

计算卡尔曼滤波器函数Kalman的调用格式类似，输入对应的噪声协方差矩阵即可。

实例

研究如下的车的悬吊系统

z_0 代表的是路上的颠簸

状态方程

\left(\begin{array}{c} \dot{x}_{1}(t) \\ \dot{x}_{2}(t) \\ \dot{x}_{3}(t) \\ \dot{x}_{4}(t) \end{array}\right)=\left(\begin{array}{cccc} 0 & 1 & 0 & 0 \\ -\frac{\left(k_{1}+k\right)}{M_{1}} & -\frac{f}{M_{1}} & \frac{k}{M_{1}} & \frac{f}{M_{1}} \\ 0 & 0 & 0 & 1 \\ \frac{k}{M_{2}} & \frac{f}{M_{2}} & -\frac{k}{M_{2}} & -\frac{f}{M_{2}} \end{array}\right)\left(\begin{array}{c} x_{1}(t) \\ x_{2}(t) \\ x_{3}(t) \\ x_{4}(t) \end{array}\right)+\underbrace{\left(\begin{array}{c} 0 \\ -\frac{1}{M_{1}} \\ 0 \\ \frac{1}{M_{2}} \end{array}\right)}_{B_{u}} u(t)+\left(\begin{array}{c} 0 \\ \frac{k_{1}}{M_{1}} \\ 0 \\ 0 \end{array}\right) z_{0}(t), \quad(1)

\begin{aligned} &\text { with } x_{1}(t)=z_{1}(t), x_{2}(t)=\frac{d z_{1}(t)}{d t}, x_{3}(t)=z_{2}(t) \text { and } x_{4}=\frac{d z_{2}(t)}{d t} . \text { The values of the parame- }\\ &\text { ters are } M_{1}=30 \mathrm{Kg}, M_{2}=250 \mathrm{Kg}, k=2.10^{4} \mathrm{~N} / \mathrm{m}, k_{1}=2.10^{5} \mathrm{~N} / \mathrm{m} \text { and } f=10^{3} \mathrm{~N} . \mathrm{S} / \mathrm{m} \end{aligned}

定义系统输出为 z_2(t)-z_1(t) ， 这描述了整个车在路不平坦的时候的”颠簸程度“。

因此，对应的 C=[-1\ 0\ 1\ 0], D=0

系统是单输入单输出（SISO）的。控制量是减震器的力，输出则是车身中心到轮胎的距离，但是为了能够自由定义 z_0 ，将系统的 B 矩阵扩展成高维度形式，即系统有两个输入的通道，一个通道对应系统的真实输入，另一个通道对应系统受到的扰动。

代价函数

J=\int_{0}^{\infty} y^{T}(t) Q_{y} y(t)+u^{T}(t) R u(t)

使用的权重系数

clear all 
close all 
% constant
M1 = 30;
M2 = 250;
k = 2*10^4;
k1 = 2*10^5;
f = 10^3;
% system model
A = [0 1 0 0;
    -(k1+k)/M1 -f/M1 k/M1 f/M1;
    0 0 0 1;
    k/M2 f/M2 -k/M2 -f/M2];
Bu = [0; -1/M1 ; 0; 1/M2]; % control
Bz = [0; k1/M1; 0; 0]; % disturbance
B = [Bu, Bz];
C = [-1 0 1 0]; % z2-z1
D = zeros(1,2);
C_full = eye(4);
D_full = zeros(4,2);
sys = ss(A,B,C,D);
% initial states
x0 = [0.0022; -0.1146; 0.0421; -0.0183];
% Properties Check
rank(ctrb(A,Bu)) 
rank(obsv(A,C))
% 根据不同的权重，计算LQR参数 
Rlist= [1, 1e-4, 1e-8,1e-9] ;
for i = 1:1:4
    %LQR参数计算
    Qy = 1;
    R = Rlist(i)
    Q = C.'*Qy*C;
    R = R + 0; % since D = 0
    N = 0; % same reason
    [K,S,e] = lqr(A,Bu,Q,R,N);
    %simulink仿真，得到数据
    sim('LQG.slx')
    subplot(2,1,1)
    plot(t,CL_u,'LineWidth',2,'DisplayName',['R: ',num2str(R)])
    hold on
    subplot(2,1,2)
    plot(t,CL_y,'LineWidth',2,'DisplayName',['R: ',num2str(R)])
    hold on
%显示图例
subplot(2,1,1)