【MPC】模型预测控制笔记 (1)：无约束MPC

---

前言

本文是 模型预测控制（2022春）lecture 1-1 Unconstrained MPC 的学习笔记，非常感谢诸兵老师的精彩课程。

符号说明

针对离散时不变线性系统：
$$x_{k+1}=A{x}_k+B{u}_k\text{\hspace{1em}(1)}$$
$x_k$为系统状态，$u_k$为系统输入。
MPC需要分析模型预测的状态，将在 $k$ 时刻预测未来第 $i$ 步的状体记为 $x_{(i|k)}$，
对应MPC在 $k$ 时刻优化得到的第 $i$ 步输入为 $u_{(i|k)}$.
最优的状态、输入和代价使用上标 ${}^{\ast }$ 标记。

---

一、无约束MPC求解

1.1 定义代价函数

定义代价函数为：
$$J=\sum _{i=1}^N\left(x_{(i|k)}^{T}Q{x}_{(i|k)}+u_{(i-1|k)}^{T}R{u}_{(i-1|k)}\right)$$
可以写为矩阵形式：
$$J=X^T\mathcal{Q}X+U^T\mathcal{R}U\text{\hspace{1em}(2)}$$
其中 $X=[x_{(1|k)}{x}_{(2|k)}\cdots x_{(N|k)}{]}^T$， $U=[u_{(0|k)}{u}_{(1|k)}\cdots u_{(N-1|k)}{]}^T$，$\mathcal{Q}=\mathrm{diag}(Q,Q,\cdots ,Q)$，$\mathcal{R}=\mathrm{diag}(R,R,\cdots ,R)$.

1.2 状态预测

系统的未来状态与当前状态和输入有关，故可以根据系统模型预测未来的所有状态，并求解最优的系统输入来使代价函数最小化。
根据式 (1) 进行预测，有：
$$\begin{array}{rl}{x}_{(1|k)}& =A{x}_{(0|k)}+B{u}_{(0|k)}\\ x_{(2|k)}& =A{x}_{(1|k)}+B{u}_{(1|k)}\\ & =A(A{x}_{(0|k)}+B{u}_{(0|k)})+B{u}_{(1|k)}\\ & =A^2{x}_{(0|k)}+AB{u}_{(0|k)}+B{u}_{(1|k)}\\ x_{(3|k)}& =A{x}_{(2|k)}+B{u}_{(2|k)}\\ & =A(A^2{x}_{(0|k)}+AB{u}_{(0|k)}+B{u}_{(1|k)})+B{u}_{(2|k)}\\ & =A^3{x}_{(0|k)}+A^{2}B{u}_{(0|k)}+AB{u}_{(1|k)}+B{u}_{(2|k)}\\ & ⋮\\ x_{(N|k)}& =A^N{x}_{(0|k)}+A^{N-1}B{u}_{(0|k)}+A^{N-2}B{u}_{(1|k)}+\cdots +B{u}_{(N-1|k)}\end{array}$$
写为矩阵形式为：
$$X=\mathcal{G}{x}_{(0|k)}+\mathcal{H}U\text{\hspace{1em}(3)}$$
其中，
$$\begin{array}{rl}X& =[x_{(1|k)}{x}_{(2|k)}\cdots x_{(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}$$

1.3 优化求解

将式 (3) 代入式 (2) 得：
$$\begin{array}{rl}J& =(\mathcal{G}{x}_{(0|k)}+\mathcal{H}U{)}^T\mathcal{Q}(\mathcal{G}{x}_{(0|k)}+\mathcal{H}U)+U^T\mathcal{R}U\\ & =(\mathcal{G}{x}_{(0|k)}{)}^T\mathcal{QG}{x}_{(0|k)}+2(\mathcal{H}U{)}^T\mathcal{QG}{x}_{(0|k)}+(\mathcal{H}U{)}^T\mathcal{QH}U+U^T\mathcal{R}U\\ & =(\mathcal{G}{x}_{(0|k)}{)}^T\mathcal{QG}{x}_{(0|k)}+2U^T{\mathcal{H}}^T\mathcal{QG}{x}_{(0|k)}+U^T({\mathcal{H}}^T\mathcal{QH}+\mathcal{R})U\end{array}\text{\hspace{1em}(4)}$$
另导数为 0 求极值，有：
$$\begin{array}{rl}\frac{\partial J}{\partial U}& =2{\mathcal{H}}^T\mathcal{QG}{x}_{(0|k)}+2({\mathcal{H}}^T\mathcal{QH}+\mathcal{R})U=0\\ \Rightarrow U& =-({\mathcal{H}}^T\mathcal{QH}+\mathcal{R}{)}^{-1}{\mathcal{H}}^T\mathcal{QG}{x}_{(0|k)}\end{array}\text{\hspace{1em}(5)}$$
当 ${\partial }^{2}J/{\partial U}^2=2({\mathcal{H}}^T\mathcal{QH}+\mathcal{R})\ge 0$ ，式 (5) 可求得最优控制序列，使代价函数最小化。
式 (6) 可写成 $U=-K{x}_{(0|k)}$ 的形式，与LQR所得到的最优线性反馈控制律 $u=-Kx$ 相似，区别是此处只优化 $N$ 步，LQR是优化无穷步（$N\to \infty$）。
但针对有约束的MPC，式 (4) 的优化求解更复杂，可使用二次规划求解函数进行求解。

二、稳定性分析

最优并不能保证系统可稳定，需要对系统进行稳定性分析。

2.1 离散系统的李雅普诺夫稳定性判据

间接法：自治系统 $x_{k+1}=A{x}_k$ 中，$A$ 的所有特征根的模 $|eig(A)<1|$
直接法：存在李雅普诺夫函数 $V(x_k)>0$，满足 $\Delta V=V(x_{k+1})-V(x_k)<0$

2.2 离散时不变线性系统的稳定性分析

针对系统 (1)，根据式 (5)，在状态 $k$ 可求解出最优控制序列 $U=K{x}_{(0|k)}$，其中 $K=[k_0{k}_1...k_{N-1}]$，即 $u_{(i|k)}=k_i{x}_{(0|k)}$。
在 MPC 的思想中，我们只取控制序列的第一个控制输入应用于系统中。
在定常系统中，$K=-({\mathcal{H}}^T\mathcal{QH}+\mathcal{R}{)}^{-1}{\mathcal{H}}^T\mathcal{QG}$是固定的，故实际控制输入固定为 $u_{(k)}=u_{(0|k)}=k_0{x}_{(0|k)}$.
根据间接法，当满足 $|eig(A-B{k}_0)<1|$ 时，系统稳定。
但当预测空间 $N$ 不同时，求解到的 $k_0$ 也不同，不能保证系统都是稳定的。

2.3 如何保证系统稳定性

2.3.1 优化无限步的预测空间

若系统可控，在 $k$ 时刻可以找到一组可行的最优解 $u_{(i|k)}^{\ast },i=0,1,2,\cdots$.
此时，MPC优化得到的最优代价为：
$$J_k^{\ast }=\sum _{i=1}^N\left((x_{(i|k)}^{\ast }{)}^{T}Q{x}_{(i|k)}^{\ast }+(u_{(i-1|k)}^{\ast }{)}^{T}R{u}_{(i-1|k)}^{\ast }\right)\text{\hspace{1em}(6)}$$
当 $k+1$ 时刻继续按这组控制序列执行，则系统完全按预测的状态进行，有：
$$J_{k+1}=\sum _{i=2}^N\left((x_{(i|k)}^{\ast }{)}^{T}Q{x}_{(i|k)}^{\ast }+(u_{(i-1|k)}^{\ast }{)}^{T}R{u}_{(i-1|k)}^{\ast }\right)$$
注意，因为此时没有重新优化，代价不是状态 $k+1$ 时的最优代价，最优代价 $J_{k+1}^{\ast }\le J_{k+1}$，有：
$$J_{k+1}^{\ast }-J_k^{\ast }\le J_{k+1}-J_k^{\ast }=-\left(x_{(1|k)}^{\ast }{)}^{T}Q{x}_{(1|k)}^{\ast }+(u_{(0|k)}^{\ast }{)}^{T}R{u}_{(0|k)}^{\ast }\right)<0\text{\hspace{1em}(7)}$$

此处认为 $J_{k+1}$ 只有 $N-1$ 项，而 $J_k^{\ast }$ 有 $N$ 项，其中 $N\to \infty$。

故可选代价函数式 (2) 作为李雅普诺夫函数 $V(x_k)$，显然有：$V(x_k)>0$，
同时，根据式 (7) ，有 $V(x_{k+1})-V(x_k)<0$.
故系统渐近稳定。

2.3.2 求 $P$ 以代替无限步的总代价

当优化无限步时，可使得系统渐近稳定，但这无法直接实现。
由于系统渐近稳定，总代价是收敛的，故可计算无限项的总代价来进行等效，有：
$$J(k)=\sum _{i=1}^{\infty }\left((x_{(i|k)}{)}^{T}Q{x}_{(i|k)}+(u_{(i-1|k)}{)}^{T}R{u}_{(i-1|k)}\right)=x_{(0|k)}^{T}P{x}_{(0|k)}-x_{(0|k)}^{T}Q{x}_{(0|k)}\text{\hspace{1em}(8)}$$
当选取一个反馈增益 $K$ 使得系统 $x_{k+1}=(A-BK)x_k$ 渐近稳定时（即满足$|\mathrm{eig}(A-BK)|<1$），$P$ 可通过下式求解：
$$P-(A-BK{)}^{T}P(A-BK)=Q+K^{T}RK\text{\hspace{1em}(9)}$$

证明：
式 (9) 两端同乘 $x_{(k)}$，有：
$$\begin{array}{rl}& x_k^T[P-(A-BK{)}^{T}P(A-BK)]x_k=x_k^T[Q+K^{T}RK]x_k\\ \Rightarrow & x_k^{T}P{x}_k-x_{k+1}^{T}P{x}_{k+1}=x_k^{T}Q{x}_k+u_k^{T}R{u}_k\end{array}$$
根据上式有：
$$\begin{array}{rl}{x}_k^{T}P{x}_k-x_{k+1}^{T}P{x}_{k+1}& =x_k^{T}Q{x}_k+u_k^{T}R{u}_k\\ x_{k+1}^{T}P{x}_{k+1}-x_{k+2}^{T}P{x}_{k+2}& =x_{k+1}^{T}Q{x}_{k+1}+u_{k+1}^{T}R{u}_{k+1}\\ ⋮& \\ x_{k+N}^{T}P{x}_{k+N}-x_{k+N+1}^{T}P{x}_{k+N+1}& =x_{k+N}^{T}Q{x}_{k+N}+u_{k+N}^{T}R{u}_{k+N}\end{array}$$
将以上各式相加得：
$$\begin{array}{r}{x}_k^{T}P{x}_k-x_{k+N+1}^{T}P{x}_{k+N+1}=\sum _{i=k}^{k+N}\left(x_i^{T}Q{x}_i+u_i^{T}R{u}_i\right)\end{array}$$
由于系统渐近稳定，当 $N\to \infty$ 时，$x_{k+N+1}^{T}P{x}_{k+N+1}\to 0$，$u_{k+N}^{T}R{u}_{k+N}\to 0$.
有 ${\sum }_{i=k}^N\left(x_k^{T}Q{x}_k+u_k^{T}R{u}_k\right)={\sum }_{i=k+1}^N\left(x_k^{T}Q{x}_k+u_{k-1}^{T}R{u}_{k-1}\right)+x_k^{T}Q{x}_k$.
得：
$$x_k^{T}P{x}_k-x_k^{T}Q{x}_k=\sum _{i=k+1}^{\infty }\left(x_i^{T}Q{x}_i+u_{i-1}^{T}R{u}_{i-1}\right)$$
即：
$$x_{(0|k)}^{T}P{x}_{(0|k)}-x_{(0|k)}^{T}Q{x}_{(0|k)}=\sum _{i=1}^{\infty }\left((x_{(i|k)}{)}^{T}Q{x}_{(i|k)}+(u_{(i-1|k)}{)}^{T}R{u}_{(i-1|k)}\right)$$

2.3.3 将无限步优化转为有限步优化

$$\begin{array}{rl}J(k)& =\sum _{i=1}^{\infty }\left((x_{(i|k)}{)}^{T}Q{x}_{(i|k)}+(u_{(i-1|k)}{)}^{T}R{u}_{(i-1|k)}\right)\\ & =\sum _{i=1}^N\left((x_{(i|k)}{)}^{T}Q{x}_{(i|k)}+(u_{(i-1|k)}{)}^{T}R{u}_{(i-1|k)}\right)+\sum _{i=N+1}^{\infty }\left((x_{(i|k)}{)}^{T}Q{x}_{(i|k)}+(u_{(i-1|k)}{)}^{T}R{u}_{(i-1|k)}\right)\\ & =\sum _{i=1}^N\left((x_{(i|k)}{)}^{T}Q{x}_{(i|k)}+(u_{(i-1|k)}{)}^{T}R{u}_{(i-1|k)}\right)+x_{(N|k)}^{T}P{x}_{(N|k)}-x_{(N|k)}^{T}Q{x}_{(N|k)}\\ & =\sum _{i=1}^{N-1}\left((x_{(i|k)}{)}^{T}Q{x}_{(i|k)}+(u_{(i-1|k)}{)}^{T}R{u}_{(i-1|k)}\right)+x_{(N|k)}^{T}P{x}_{(N|k)}+u_{(N-1|k)}^{T}R{u}_{(N-1|k)}\end{array}$$
上式可以写成矩阵形式：
$$J=X^T{\mathcal{Q}}^{\prime }X+U^T\mathcal{R}U\text{\hspace{1em}(10)}$$
与式 (2) 唯一不同的是，此处 ${\mathcal{Q}}^{\prime }=\mathrm{diag}(Q,Q,\cdots ,P)$.
由上式进行优化求解即可保证系统是渐近稳定的。

三、求解流程

列如系统：
$$x_{k+1}=A{x}_k+B{u}_k$$
其中，$x\in {\mathbb{R}}^2$，$u\in \mathbb{R}$，$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]$.

3.1 选取预测空间 $N$ 和对角权重矩阵$Q$、$R$

令 $N=4$ ，$Q=I_{2\times 2}$， $R=0.1$。

3.2 选取合适的反馈增益 $K$，注意需满足 $|\mathrm{eig}(A-BK)|<1$

令 $K=[1.45.76]$，使用MATLAB计算特征根：

A = [1.1 2;0 0.95];
B = [0; 0.079];
K = [1.4 5.76];

eigSys = eig(A - B*K);
disp(abs(eigSys))

得$|\mathrm{eig}(A-BK){|}_1=|\mathrm{eig}(A-BK){|}_2=0.8750<1$

3.3 通过方程 (9)求解 P

使用MATLAB求解：

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)

3.4 状态预测

计算式 (3) 中的 $\mathcal{G}$，$\mathcal{H}$：

N = 4;
[G, H] = getGH(N, A, B);
function [G, H] = getGH(N, A, B) % N>1
    tmp = A;
    G = tmp;
    for i=2:N
        tmp = A*tmp;
        G = [G; tmp];
    end
    
    r = size(B, 1);
    c = size(B, 2);
    H = zeros(r * N, c * N);
    
    tmp = B;
    for j = N:-1:1
        H( (j-1)*r+1:j*r, (j-1)*c+1:j*c ) = tmp;
    end
    for i = 2:N
        tmp = A*tmp;
        for j = i:N
            H( (j-1)*r+1:j*r, (j-i)*c+1:(j-i+1)*c ) = tmp;
        end
    end
end

3.5 根据代价函数构建二次规划来求解 $U$

根据式 (4) 有：
$$\begin{array}{rl}J& =(\mathcal{G}{x}_{(0|k)}{)}^T{\mathcal{Q}}^{\prime }\mathcal{G}{x}_{(0|k)}+2U^T{\mathcal{H}}^T{\mathcal{Q}}^{\prime }\mathcal{G}{x}_{(0|k)}+U^T({\mathcal{H}}^T{\mathcal{Q}}^{\prime }\mathcal{H}+\mathcal{R})U\\ & =(\mathcal{G}{x}_{(0|k)}{)}^T{\mathcal{Q}}^{\prime }\mathcal{G}{x}_{(0|k)}+2x_{(0|k)}^T{\mathcal{G}}^T{\mathcal{Q}}^{\prime }\mathcal{H}U+U^T({\mathcal{H}}^T{\mathcal{Q}}^{\prime }\mathcal{H}+\mathcal{R})U\end{array}$$
其中，忽略与输入无关的项（第一项），优化问题可写为：
$$\arg \underset{U}{\min }\frac{1}{2}{U}^{T}HU+f^{T}U\text{\hspace{1em}(11)}$$
其中，$H=2({\mathcal{H}}^T{\mathcal{Q}}^{\prime }\mathcal{H}+\mathcal{R})$，$f^T=2x_{(0|k)}^T{\mathcal{G}}^T{\mathcal{Q}}^{\prime }\mathcal{H}$.
式 (11) 可通过二次规划求解。

x0 = [1;1]; % 设初始状态为[1;1]
[Qp, Rp] = getQR(N, Q, Psol, R);
Hp = 2 * (H' * Qp * H + Rp);
fp = 2 * x0' * G' * Qp * H;
U = quadprog(Hp, fp);
%%
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);
end

3.6 滚动优化

每个周期重复进行优化，只取控制序列 $U$ 中的第一个动作 $u_{(0|k)}$ 作用于系统.
初始状态 $x_0=[11{]}^T$，使用MATLAB演示系统动态：

xCur = [2;2]; % 设初始状态为[1;1]
xLog = xCur;
uLog = [];

options = optimoptions('quadprog','MaxIterations', 100, 'Display','none');

step = 0:50;
for i = step

    Hp = 2 * (H' * Qp * H + Rp);
    fp = 2 * xCur' * G' * Qp * H;
    Hp = 0.5 * (Hp + Hp');
    U = quadprog(Hp, fp, [], [], [], [], [], [], zeros(4,1), options);
    u = U(1);
    xCur = A*xCur + B*u;
    xLog = [xLog, xCur];
    uLog = [uLog, u];
end

figure(1)
subplot(3,1,1)
plot(step, xLog(1,1:end-1))
title('x1')
grid on
subplot(3,1,2)
plot(step, xLog(2,1:end-1))
title('x2')
grid on
subplot(3,1,3)
plot(step, uLog)
title('u')
grid on

得：

可见系统可以在10步的迭代中稳定到 $0$.