【MPC】模型预测控制笔记 (4)：约束输出反馈MPC

---

前言

致谢 【模型预测控制（2022春）lecture 3-2 Output feedback MPC】
本文需要是使用先前博客的知识，
控制器求解参考【MPC】模型预测控制笔记 (2)：约束MPC 、
观测器设计参考【MPC】模型预测控制笔记 (3)：无约束输出反馈MPC

以下内容针对系统 (1) 展开：
$$\begin{array}{rl}{x}_{k+1}& =A{x}_k+B{u}_k\\ y_k& =C{x}_k\end{array}\text{\hspace{1em}(1)}$$
其中，$x_k\in {\mathbb{R}}^n$ 是系统的全状态，$y_k\in {\mathbb{R}}^m$ 是可知的系统输出，$u_k\in {\mathbb{R}}^p$ 是系统的输入，$lb\le u_k\le ub$。
系统能控且能观，$m<n$，部分状态不可直接测量，需要增加观测器来观测系统的全状态。

---

一、观测器设计

可设计观测器为：
$${\hat{x}}_{k+1}=A{\hat{x}}_k+B{u}_k+L(y_k-C{\hat{x}}_k)\text{\hspace{1em}(2)}$$
观测误差 ${\tilde{x}}_k=x_k-{\hat{x}}_k$，其状态方程为：
$${\tilde{x}}_{k+1}=(A-LC){\tilde{x}}_k\text{\hspace{1em}(3)}$$
其中满足 $|\mathrm{eig}(A-LC)|<1$.

二、输出反馈MPC设计

由于全状态 $x_k$ 不可直接测量，需使用观测状态 ${\hat{x}}_k$ 完成控制器设计.

2.1 预测模型

在使用式 (2) 进行未来 $N$ 步的预测时，由于未来的 $y_{(i|k)},i=1,2,\cdots ,N$ 不可知，且在观测过程中，$y_k-C{\hat{x}}_k\to 0$.
故使用下式进行预测（${\hat{z}}_0={\hat{x}}_0$）：
$${\hat{z}}_{k+1}=A{\hat{z}}_k+B{u}_k\text{\hspace{1em}(4)}$$
未来 $N$ 步的状态可表示为：
$$Z=\mathcal{G}{z}_{(0|k)}+\mathcal{H}U\text{\hspace{1em}(5)}$$
其中，
$$\begin{array}{rl}Z& =[z_{(1|k)}{z}_{(2|k)}\cdots z_{(N|k)}{]}^T\\ U& =[u_{(0|k)}{u}_{(1|k)}\cdots u_{(N-1|k)}{]}^T\\ \mathcal{G}& ={\left[A{A}^2\cdots A^N\right]}^T\\ \mathcal{H}& =\left[\begin{array}{ccccc}B& 0& 0& \cdots & 0\\ AB& B& 0& \cdots & 0\\ A^{2}B& AB& B& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ A^{N-1}B& A^{2}B& AB& \cdots & B\end{array}\right]\end{array}$$

2.2 代价函数设计

使用 $z_k$ 代替 $x_k$ 来设计代价函数，其矩阵形式为：
$$J_k=Z_k^T\mathcal{Q}{Z}_k+U_k^T\mathcal{R}{U}_k\text{\hspace{1em}(6)}$$
其中 $\mathcal{Q}=\mathrm{diag}(Q,Q,\cdots ,Q,P)$，$\mathcal{R}=\mathrm{diag}(R,R,\cdots ,R)$.
将式 (5) 代入上式，有：
$$J_k=(\mathcal{G}{z}_{(0|k)}{)}^T{\mathcal{Q}}^{\prime }\mathcal{G}{z}_{(0|k)}+2z_{(0|k)}^T{\mathcal{G}}^T{\mathcal{Q}}^{\prime }\mathcal{H}{U}_k+U_k^T({\mathcal{H}}^T{\mathcal{Q}}^{\prime }\mathcal{H}+\mathcal{R})U_k$$

2.3 约束构建

2.3.1 系统约束

输入约束：
$$U_{min}\le U_k\le U_{max}\text{\hspace{1em}(7)}$$
其中 $U_{min}=[lb,lb,,\cdots ,lb]$，$U_{max}=[ub,ub,,\cdots ,ub]$.

2.3.2 终端约束

为保证系统的稳定性，增加终端约束，使优化的终端进去不变集 $\Omega$，且要求该集合内存在最优输入 $u_k=-K{x}_k$ 始终满足输入约束 $\mathcal{U}$：
$${\hat{z}}_{(N|k)}\in \Omega \text{\hspace{1em}(8)}$$

在 【模型预测控制（2022春）lecture 3-2 Output feedback MPC】 中，考虑的是真实状态 $x_k$ 的终端约束，即：
$$x_{(N|k)}\in \Omega \text{\hspace{1em}(A.1)}$$
假设初始的误差 ${\tilde{x}}_0\in {\mathcal{X}}_0$，保守地通过以下形式来满足式 (A.1)：
$${\hat{z}}_{(N|k)}\in \Omega \setminus (A-LC{)}^N{\mathcal{X}}_0$$
其中 $\setminus$ 为集合减，即认为 $x_{(N|k)}\in {\hat{z}}_{(N|k)}+(A-LC{)}^N{\mathcal{X}}_0$.
但目前本人暂无法给出证明，因为实际观测是由式 (2) 进行的，但预测时只能按式 (4) 进行，其中仍然需要分析。
本文认为式 (8) 的终端约束即可保证系统的稳定性。

2.4 构建二次规划求解

综上，最优控制序列可通过二次规划问题求解：
$$\begin{array}{rl}& U^{\ast }=\arg \underset{U_k}{\min }{J}_k\\ s.t.& U_{min}\le U_k\le U_{max}\\ & {\hat{z}}_{(N|k)}\in \Omega \end{array}$$

三、系统稳定性分析

3.1 构造李雅普诺夫函数

选取李雅普诺夫函数 $V(x_k)$：
$$\begin{array}{rl}V(x_k)& =J_k^{\ast }+E_{err}+c_o{\tilde{x}}_k^T{P}_o{\tilde{x}}_k\end{array}$$
其中，$J_k^{\ast }={\min }_{U_k}{J}_k$，$x_k^{T}Q{x}_k=z_k^{T}Q{z}_k+E_{err}$ （ $V(x_k)$ 包含 $x_k$），常数 $c_o>0$，
$P_o$ 为正定对称矩阵，满足 $(A-LC{)}^T{P}_o(A-LC)-P_o<0$.

证明：针对系统 (3)，必然存在正定对称矩阵 $P_o$，满足 $(A-LC{)}^T{P}_o(A-LC)-P_o<0$
设存在对称矩阵 $Q_o>0$，构造 $P_o$，使得 $(A-LC{)}^T{P}_o(A-LC)-P_o=-Q_o$，有：
$$\begin{array}{rl}(A-LC{)}^T{P}_o(A-LC)-P_o& =-Q_o\\ {(A-LC{)}^2}^T{P}_o(A-LC{)}^2-(A-LC{)}^T{P}_o(A-LC)& =-(A-LC{)}^T{Q}_o(A-LC)\\ {(A-LC{)}^3}^T{P}_o(A-LC{)}^3-{(A-LC{)}^2}^T{P}_o(A-LC{)}^2& =-{(A-LC{)}^2}^T{Q}_o(A-LC{)}^2\\ ⋮& \\ {(A-LC{)}^{n+1}}^T{P}_o(A-LC{)}^{n+1}-{(A-LC{)}^n}^T{P}_o(A-LC{)}^n& =-{(A-LC{)}^n}^T{Q}_o(A-LC{)}^n\end{array}$$
以上各式相加可得：
$${(A-LC{)}^{n+1}}^T{P}_o(A-LC{)}^{n+1}-P_o=-\sum _{k=1}^n\left({(A-LC{)}^k}^T{Q}_o(A-LC{)}^k\right)$$
因为 $|\mathrm{eig}(A-LC)|<1$，有
$${\underset{n\to +\infty }{\lim }(A-LC{)}^{n+1}}^T{P}_o(A-LC{)}^{n+1}=0$$
所以有：
$$P_o=\sum _{k=1}^{\infty }{(A-LC{)}^k}^T{Q}_o(A-LC{)}^k>0$$
故 $P_o$ 对称且正定。

有
$$\begin{array}{rl}{c}_o{\tilde{x}}_k^T{P}_o{\tilde{x}}_k+E_{err}& =c_o\left(x_k^T{P}_o{x}_k+z_k^T{P}_o{z}_k-2z_k^T{P}_o{x}_k\right)+\left(x_k^{T}Q{x}_k-z_k^{T}Q{z}_k\right)\\ & =x_k^T(c_o{P}_o+Q)x_k+z_k^T(c_o{P}_o-Q)z_k-2z_k^T{P}_o{x}_k\\ & \ge x_k^T(c_o{P}_o+Q)x_k+z_k^T(c_o{P}_o-Q)z_k-\max (x_k^T(2P_o)x_k,z_k^T(2P_o)z_k)\end{array}$$
显然存在有限常数 $c_o$，使得 $c_o{P}_o+Q-2P_0$ 或 $c_o{P}_o-Q-2P_0$ 正定，$c_o{\tilde{x}}_k^T{P}_o{\tilde{x}}_k+E_{err}>0$.
又因 $J_k^{\ast }>0$，故 $V(x_k)>0$.

3.2 证明李雅普诺夫函数递减

$$\begin{array}{rl}V(x_{k+1})-V(x_k)& =J_{k+1}^{\ast }-J_k^{\ast }+c_o\left({\tilde{x}}_{k+1}^T{P}_o{\tilde{x}}_{k+1}-{\tilde{x}}_k^T{P}_o{\tilde{x}}_k\right)+E_{err}^{\prime }\\ & =J_{k+1}^{\ast }-J_k^{\ast }+c_o\left({\tilde{x}}_k^T(A-LC{)}^T{P}_o(A-LC){\tilde{x}}_k-{\tilde{x}}_k^T{P}_o{\tilde{x}}_k\right)+E_{err}^{\prime }\\ & =J_{k+1}^{\ast }-J_k^{\ast }+c_o{\tilde{x}}_k^T\left((A-LC{)}^T{P}_o(A-LC)-P_o\right){\tilde{x}}_k+E_{err}^{\prime }\end{array}$$
其中 $(A-LC{)}^T{P}_o(A-LC)-P_o<0$，$c_o>0$，故 $c_o{\tilde{x}}_k^T\left((A-LC{)}^T{P}_o(A-LC)-P_o\right){\tilde{x}}_k<0$，
当 ${\tilde{x}}_k\to 0$，$E_{err}^{\prime }\to 0$，否则存在有限常数 $c_o>0$，使得 $c_o{\tilde{x}}_k^T\left((A-LC{)}^T{P}_o(A-LC)-P_o\right){\tilde{x}}_k+E_{err}^{\prime }<0$.

故只需保证 $J_{k+1}^{\ast }-J_k^{\ast }<0$ 即可证明 $V(x_k)$ 递减。
参考【MPC】模型预测控制笔记 (1)：无约束MPC、【MPC】模型预测控制笔记 (2)：约束MPC，
$J_{k+1}^{\ast }-J_k^{\ast }<0$ 成立，故
$$V(x_{k+1})-V(x_k)=J_{k+1}^{\ast }-J_k^{\ast }+c_o{\tilde{x}}_k^T\left((A-LC{)}^T{P}_o(A-LC)-P_o\right){\tilde{x}}_k+E_{err}^{\prime }<0$$

综上，在观测器和输出反馈MPC联合作用下，系统 (1) 渐近稳定。

四、MATLAB实例

针对系统 (1)，设系统中 $A=\left[\begin{array}{cc}1.1& 2\\ 0& 0.95\end{array}\right]$，$B=\left[\begin{array}{c}0\\ 0.079\end{array}\right]$，$C=\left[\begin{array}{cc}1& 0\end{array}\right]$，$-4\le u_k\le 4$.

设计式 (2) 形式的观测器，其中最优增益 $L=\left[\begin{array}{c}1.8342\\ 0.3571\end{array}\right]$.
终端约束中取不变集 $\Omega =[-0.2,0.2]\times [-0.1,0.1]$，写为关于 $U_k$ 的不等式约束：
$$A_{in}{U}_k\le b_{in}$$其中，
$$\begin{array}{rl}{A}_{in}& =\left[\begin{array}{cc}1& 0\\ -1& 0\\ 0& 1\\ 0& -1\end{array}\right][0,0,\cdots ,I_{2\times 2}]\mathcal{H}\\ b_{in}& =\left[\begin{array}{c}0.2\\ 0.2\\ 0.1\\ 0.1\end{array}\right]-\left[\begin{array}{cc}1& 0\\ -1& 0\\ 0& 1\\ 0& -1\end{array}\right][0,0,\cdots ,I_{2\times 2}]\mathcal{G}{\hat{z}}_{(0|k)}\end{array}$$.
取 $Q=\mathrm{diag}(1,1)$，$R=0.1$，$N=20$.
计算得 $P=\mathrm{diag}(1,1)$：
实现代码如下：

%% 观测器 
%%
A = [1.1 2;0 0.95];
B = [0; 0.079];
C = [1, 0];Ap = A';
Bp = C';
%% LQR 最优增益
K = LQR(Ap, Bp, eye(2), 0.1, 500, 1e-6);
L = K';
disp(eig(A - L*C))
disp(abs(eig(A - L*C)))%% 控制器
%% 选取K并检验系统是否稳定 
A = [1.1 2;0 0.95];
B = [0; 0.079];
K = [1.4 5.76];eigSys = eig(A - B*K);
disp(abs(eigSys))
%% 求解P
K = [1.4 5.76];
Q = eye(2);
R = 0.1;
syms P [2 2] % P 为2*2的矩阵
equ = P - (A - B*K)' * P * (A - B*K) == Q + K'*R*K;
Psol = solve(equ, P);
Psol = [Psol.P1_1, Psol.P2_1; Psol.P2_1, Psol.P2_2];
Psol = double(Psol);
disp(Psol)
%% 计算G、H、Q、R
N = 20;
[G, H] = getGH(N, A, B);
[Qp, Rp] = getQR(N, Q, Psol, R);
%% 约束条件
lb = -4 * ones(N, 1);
ub = 4 * ones(N, 1);n = size(A, 2);
tmpReshape = kron(ones(1, N-1), zeros(n));
tmpReshape = [tmpReshape, eye(n)];
tmpAin = [1  0;-1  0;0  1;0 -1];
tmpbin = [0.2; 0.2; 0.1; 0.1];
% Ain = tmpAin * tmpReshape * H;
% bin = tmpbin - tmpAin * tmpReshape * G * xCur;
%% 效果演示
A = [1.1 2;0 0.95];
B = [0; 0.079];
C = [1, 0];L = [1.8342; 0.3571];
options = optimoptions('quadprog', 'MaxIterations', 200, 'Display','none');xCur = [1.2;-0.7]; % 设初始状态为[1;1]
xLog = xCur;
xHatCur = [0;0]; % 设初始状态为[0;0]
xHatLog = xHatCur;
uLog = [];step = 0:50;
u = 0;
for i = stepHp = 2 * (H' * Qp * H + Rp);fp = 2 * xHatCur' * G' * Qp * H;Hp = 0.5 * (Hp + Hp');Ain = tmpAin * tmpReshape * H;bin = tmpbin - tmpAin * tmpReshape * G * xHatCur;U = quadprog(Hp, fp, Ain, bin, [], [], lb, ub, u, options);u = U(1);yCur = C * xCur; % y_kxHatCur = A * xHatCur + B*u + L * (yCur - C * xHatCur); % xHat_k+1xCur = A*xCur + B*u; % x_k+1xHatLog = [xHatLog, xHatCur];xLog = [xLog, xCur];uLog = [uLog, u];
endfigure(1)
subplot(3,1,1)
hold on
plot(step, xLog(1,1:end-1))
plot(step, xHatLog(1,1:end-1))
title('x1')
grid on
subplot(3,1,2)
hold on
plot(step, xLog(2,1:end-1))
plot(step, xHatLog(2,1:end-1))
title('x2')
grid on
subplot(3,1,3)
plot(step, uLog)
title('u')
grid on
%%
function [Qp, Rp] = getQR(N, Q, P, R)Qp = eye(N);Qp(end) = 0;Qp = kron(Qp, Q) + kron(eye(N)-Qp, P);Rp = eye(N);Rp = kron(Rp, R);
endfunction [G, H] = getGH(N, A, B) % N>1tmp = A;G = tmp;for i=2:Ntmp = A*tmp;G = [G; tmp];endr = size(B, 1);c = size(B, 2);H = zeros(r * N, c * N);tmp = B;for j = N:-1:1H( (j-1)*r+1:j*r, (j-1)*c+1:j*c ) = tmp;endfor i = 2:Ntmp = A*tmp;for j = i:NH( (j-1)*r+1:j*r, (j-i)*c+1:(j-i+1)*c ) = tmp;endend
end
function K = LQR(A, B, Q, R, maxIter, eps)
% A、B分别为系统矩阵和输入矩阵，Q和R分别为状态误差和输入的对角权重矩阵
% maxIter为最大迭代步数N，eps为迭代精度Ci = 1; P = Q; delta = 1e9;while i < maxIter && delta > epsPn = Q + A' * (P - P*B* inv(R+B'*P*B) *B'*P) * A;delta = max(abs(Pn-P), [], "all");P = Pn;i = i+1;endK = inv(R + B' * P * B) * B' * P * A;
end

最终效果：