【MPC】模型预测控制笔记 (5)：抗干扰鲁棒MPC-EW帮帮网

前言

致谢【模型预测控制（2022春）lecture 4-1 Robust MPC】

针对存在外界干扰的约束系统：
$x_{k+1} = Ax_k + Bu_k + Dw_k \tag{1}$
其中， $x_k \in \mathbb{R}^n$ 是系统状态， $u_k \in \mathbb{R}^p$ 是系统的输入， $u_k \in \mathcal{U}$ ， $w_k$ 是有界的干扰.
针对以上系统，由于干扰是无法预计的，故只能通过名义系统来设计名义MPC，
并在此基础上增加误差反馈控制，使名义系统状态与真实状态的误差有界。

一、误差反馈控制

1.1 名义系统与误差反馈控制

令 $z_0 = x_0$ ，记系统 (1) 的名义系统为：
$z_{k+1} = Az_{k} + Bv_k \tag{2}$
针对该系统，可设计MPC得到控制输入 $v_k$ ，但名义系统与真实系统存在差异。
故定义误差为 $e_k = x_k - z_k$ ，在 $v_k$ 基础上增加误差反馈控制来保证误差有界，有：
$u_k = v_k - Ke_k \tag{3}$
由式 (1) 减式(2) 得误差状态方程：
$e_{k+1} = (A - BK)e_k + Dw_k \tag{4}$
由于干扰项的存在，系统误差不能稳定到 0，但只要干扰强度在可控范围内，
令 $|\mathrm{eig}(A-BK)<1|$ ，可保证误差有界（即系统稳定但不是渐近稳定）.

1.2 误差分析

由式 (4)，记 $A_K = A-BK$ ，有：
$\begin{align*} e_0 &= x_0 - z_0 = 0 \\ e_1 &= A_Ke_0 + Dw_0 = Dw_0 \\ e_2 &= A_Ke_1 + Dw_1 = A_KDw_0 + Dw_1 \\ e_3 &= A_Ke_2 + Dw_3 = A_K^2Dw_0 + ADw_1 + Dw_2 \\ &\vdots \\ e_k &= \sum_{i=1}^{k} A_K^{i-1} Dw_{k-i} \end{align*}$
因为 $|\mathrm{eig}(A_K)<1|$ ，误差是有限的，记 $w_k \in \mathcal{W}$ ，有：
$e_k = \sum_{i=1}^{k} A_K^{i-1} Dw_{k-i} \in \sum_{i=1}^{k} A_K^{i-1} D \mathcal{W} \sub \sum_{i=1}^{\infty} A_K^{i-1} D \mathcal{W} \triangleq \Gamma$
即 $e_k \in \Gamma$ ，且 $\Gamma$ 是有界的。

$\Gamma$ 计算：
由于 $|\mathrm{eig}(A_K)<1|$ ，存在 $N_c$ 使：
$A_K^{N_c}D \mathcal{W} \sub \alpha D \mathcal{W} \tag{5}$
其中 $A_K^{N_c}D \mathcal{W}$ 和 $\mathcal{W}$ 是维数相同的列向量，常数 $\alpha \in [0,~1)$ .
有：
$\begin{align*} \Gamma &= \sum_{i = 1}^\infty A_K^{i-1} D \mathcal{W} \\ &= \Gamma_{N_c} + \sum_{i = N_c + 1}^\infty A_K^{i-1} D \mathcal{W} \\ &= \Gamma_{N_c} + \sum_{i = 1}^\infty A_K^{i-1} A_K^{N_c} D \mathcal{W} \\ &\sub \Gamma_{N_c} + \sum_{i = 1}^\infty A_K^{i-1} \alpha D \mathcal{W} \\ &= \Gamma_{N_c} + \alpha \Gamma \end{align*}$
其中 $\Gamma_{N_c} = \sum_{i = 1}^{N_c} A_K^{i-1} D \mathcal{W}$ ，有：
$\Gamma \sub (1 - \alpha)^{-1}\Gamma_{N_c} \tag{6}$
可通过上式计算保守的 $\Gamma$ 范围。

二、名义MPC设计 (nomianal MPC)

2.1 预测模型

根据名义系统，未来 $N$ 步的状态可表示为：
$Z_k = \mathcal{G}z_{(0|k)} + \mathcal{H}V_k \tag{7}$
其中，
$\begin{align*} Z_k &= [z_{(1|k)} ~ z_{(2|k)} ~ \cdots~z_{(N|k)}]^T \\ V_k &= [v_{(0|k)} ~ v_{(1|k)} ~ \cdots~v_{(N-1|k)}]^T \\ \mathcal{G} &= \left[ A ~ A^2 ~\cdots ~ A^N \right]^T \\ \mathcal{H} &= \begin{bmatrix} B & 0 & 0 & \cdots & 0\\ AB & B & 0 & \cdots & 0\\ A^2B & AB & B & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ A^{N-1}B & A^2B & AB & \cdots & B \end{bmatrix} \end{align*}$

2.2 代价函数设计

代价函数矩阵形式为：
$J_k = Z_k^T\mathcal{Q}Z_k + V_k^T\mathcal{R}V_k \tag{8}$
其中 $\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} V_k + V_k^T (\mathcal{H}^T \mathcal{Q}^\prime \mathcal{H} + \mathcal{R})V_k$

2.3 约束构建

2.3.1 系统约束

为了满足：
$u_k \in \mathcal{U}$
其中 $u_k = v_k - Ke_k$ ， $Ke_k \in K\Gamma$ ，需满足：
$v_k \in \mathcal{U} \setminus -K\Gamma$

若存在状态约束，要求 $\in \mathcal{X}$ ，可使 $\in \mathcal{Z} = \mathcal{X} - \Gamma$ 来满足要求。

2.3.2 终端约束

在【MPC】模型预测控制笔记 (4)：约束输出反馈MPC 的终端约束中，
令每一周期 $\hat{z}_{(0|k)} = \hat{x}_{(0|k)}$ ，需要对真实系统的 $x_{(N|k)}$ 进行约束，以保证下一周期的迭代可行性。
但在本文中，仅在 $k = 0$ 时刻，有 $z_0 = x_0$ ，后续 $z_k$ 状态是根据名义系统 (2) 进行更新的，
故直接约束 $z_{(N|k)}$ 即可.

使终端 $z_{(N|k)}$ 进去不变集 $\Omega_z$ ，且要求该集合内存在最优输入 $v_k=-Kz_k$ ， $u_k = v_k - Ke_k$ 始终满足输入约束 $\mathcal{U}$ ：
$z_{(N|k)} \in \Omega_z \tag{9}$

讨论：如果每一周期令 $z_{(0|k)} = x_{(0|k)}$ ，系统会怎样呢？
此时， $e_k$ 将始终为 0，控制约束变为 $v_k \in \mathcal{U}$ ，但需针对 $x_{(N|k)}$ 进行终端约束。
使终端 $x_{(N|k)}$ 进去不变集 $\Omega_x$ ，且要求该集合内存在最优输入 $v_k=-Kz_k$ 始终满足输入约束 $\mathcal{U}$ ：
$x_{(N|k)} = x_{(N|k)} + e_N \in \Omega_x$
有：
$z_{(N|k)} \in \Omega_x - \Gamma$
$x_{k}$ 是在干扰下更新的，不变集 $\Omega_x$ 可能会更难确定？

2.4 构建二次规划求解

最优控制序列可根据实际系统，构建二次规划问题求解：
$\begin{align*} & \hspace{-0.2cm }V^* = \mathrm{arg} \min_{V_k} J_k \\ \mathrm{s. t.}& \quad v_k \in \mathcal{U} \setminus -K\Gamma \\ & \hspace{-0.4cm} v_{k+i} \in \mathcal{U} , \quad i = 1,2,\cdots\\ & \hspace{0.3cm} z_{(N|k)} \in \Omega^\prime \end{align*}$

三、稳定性分析

由 1.2 知 $e_k \in \Gamma$ 是有界的，而名义MPC系统中， $z_k$ 是渐近稳定的（可参考【MPC】模型预测控制笔记 (2)：约束MPC），
故 $x_k = e_k + z_k$ 也是最终有界的：
$\begin{align*} \lim_{k \to \infty} z_k \to 0 \Rightarrow \lim_{k \to \infty} x_k = \lim_{k \to \infty} e_k \in \Gamma \end{align*}$

四、MATLAB实例

针对系统 (1)，设系统中 $\begin{bmatrix} 1.1 & 2 \\ 0 & 0.95 \end{bmatrix}$ ， $\begin{bmatrix} 0 \\ 0.079 \end{bmatrix}$ ， $\begin{bmatrix} 0.1 \\ 0.5 \end{bmatrix}$ ， $\le u_k \le 4$ ， $\le w_k \le 0.1$ .

4.1 设计误差反馈增益

使用LQR（可参考离散LQR原理）设计最优误差反馈增益：

A = [1.1 2;0 0.95];
B = [0; 0.079];
Q = eye(2);
R = 0.1;

K = LQR(A, B, Q, R, 500, 1e-6);
disp(abs(eig(A-B*K)))
%%
function K = LQR(A, B, Q, R, maxIter, eps)
% A、B分别为系统矩阵和输入矩阵，Q和R分别为状态误差和输入的对角权重矩阵
% maxIter为最大迭代步数N，eps为迭代精度C
	i = 1; P = Q; delta = 1e9;
	while i < maxIter && delta > eps
	    Pn = Q + A' * (P - P*B* inv(R+B'*P*B) *B'*P) * A;
	    delta = max(abs(Pn-P), [], "all");
	    P = Pn;
	    i = i+1;
	end
	K = inv(R + B' * P * B) * B' * P * A;
end

得 $\begin{bmatrix} 2.4950 & 12.5106 \end{bmatrix}$ ， $|\mathrm{eig}(A-BK)|_1 = |\mathrm{eig}(A-BK)|_2 = 0.5933 <1$ .

4.2 计算误差范围

$\mathcal{W} = \{w ~|~ -0.1 \le w_k \le 0.1\} \Rightarrow D\mathcal{W} = \{[d_1~d_2]^T ~|~ -0.01 \le d_1 \le 0.01,~ -0.05 \le d_2 \le 0.05\}$ .

寻找满足 $A_K^{N_c}D \mathcal{W} \sub \alpha D \mathcal{W}$ 的 $N_c$ ，并计算 $\alpha$ 、 $\Gamma$ ：

A = [1.1 2;0 0.95];
B = [0; 0.079];
AK = A - B*K;
lbDW = [-0.01; -0.05];
ubDW = [0.01; 0.05];
[Nc, alpha, lbGamma, ubGamma] = findNc(AK, lbDW, ubDW, 10, 500);
%%
function [Nc, alpha, lbGamma, ubGamma] = findNc(AK, lbDW, ubDW, NcMin, iterMax)
    n = length(lbDW);
    lGammaNc = lbDW;
    uGammaNc = ubDW;
    vertex = [lbDW, ubDW]; % 将上下界构成n*2的矩阵，对每个子状态x_i选择上界或下界，穷举所有组合以确定新边界    
    tmp = eye(n);
    for i = 1:iterMax
        tmp = AK * tmp;
        lbADW = tmp * lbDW;
        ubADW = tmp * ubDW;
        for j = 0:n^2-1 % 使用二进制来表示每个组合
            num = j;
            X = zeros(n, 1);
            for k = 1:n % vertex(k, 0+1)和vertex(k, 1+1)分别表示子状态x_k选择下界和上界
                X(k) = vertex(k, mod(num, 2)+1);
                num = bitshift(num, -1);
            end
            lbADW = min([lbADW, tmp*X], [], 2);
            ubADW = max([ubADW, tmp*X], [], 2);
        end
        if(sum(lbADW > lbDW) == n && sum(ubADW < ubDW) == n && i >= NcMin)
            break;
        else
            lGammaNc = lGammaNc + lbADW;
            uGammaNc = uGammaNc + ubADW;
        end
    end
    if i==iterMax
        disp("warning")
    end
    Nc = i;
    alpha = max([lbADW./lbDW; ubADW./ubDW]);
    lbGamma = lGammaNc/(1 - alpha);
    ubGamma = uGammaNc/(1 - alpha);
end

当 $N_c = 6$ 时，满足条件，但 $N_c = 50$ 时，可计算得到更小的范围，且 $N_c = 50$ 继续增大时范围不再缩小。
得 $\Gamma = \{([e_1 ~~ e_2]^T ~|~ -0.4039 \le e_1 \le 0.4039,~ -0.1286 \le e_2 \le 0.1286 \}$

4.3 名义MPC设计

取控制时域 $N = 10$ ，代价函数权重 $\mathrm{diag}(1,1)$ ， $R = 0.1$ .
选取 $K_P = \begin{bmatrix} 2.4950 & 12.5106 \end{bmatrix}$ ，计算终端代价：

A = [1.1 2;0 0.95];
B = [0; 0.079];
K = [2.4950 12.5106];

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)

得 $\begin{bmatrix} 4.0373 & 8.5226 \\ 8.5226 & 31.5400 \end{bmatrix}$ .

根据 $u_k = v_k - Ke_k$ ， $\le u_k \le 4$ ， $K\Gamma = \{v_{e} ~|~ -2.6164\le e_1 \le 2.6164 \}$ ，得：
$\le v_k \le 1.3836$
因为在控制中只取 $v_{(0|k)}$ 作用于系统，为了提高名义MPC的可行性，只对 $v_{(0|k)}$ 做以上约束，其余 $\le v_k \le 4$ .
即：
$V_{min} \le V_k \le V_{max}$
其中 $V_{min} = [-1.3836,~-4,~, -4,~ \cdots, ~-4]$ ， $V_{max} = [1.3836,~4,~, 4,~ \cdots, ~4]$ .

直接选择足够小的范围作为终端不变集：
$\Omega^\prime = [-0.2,~0.2] \times [-0.1,~0.1]$
终端约束写为不等式形式，为：
$A_{in}V_k \le b_{in}$ 其中，
$\begin{align*} A_{in} &= \begin{bmatrix} 1 & 0 \\ -1 & 0 \\ 0 & 1 \\ 0 &-1 \end{bmatrix} [0,~0,~\cdots, ~I_{2 \times 2}] \mathcal{H} \\ b_{in} &= \begin{bmatrix} 0.2 \\ 0.2 \\ 0.1 \\ 0.1 \end{bmatrix}-\begin{bmatrix} 1 & 0 \\ -1 & 0 \\ 0 & 1 \\ 0 &-1 \end{bmatrix}[0,~0,~\cdots, ~I_{2 \times 2}] \mathcal{G} \hat{z}_{(0|k)} \end{align*}$ .
最终即可求解名义系统的最优控制序列：
$\begin{align*} & \hspace{0.4cm }V^* = \mathrm{arg} \min_{V_k} J_k \\ \mathrm{s. t.}& \quad V_{min} \le V_k \le V_{max} \\ & \hspace{0.8cm} A_{in}V_k \le b_{in} \end{align*}$

4.4 结果演示

将 $u_k = v_k - Ke_k$ 作用于真实系统， $v_k$ 作用于名义系统，MATLAB代码见附录1，系统动态如下：
在这里插入图片描述
存在问题：输入还远不到约束值，可能是 $\Gamma$ 的计算中过于保守，
且与【模型预测控制（2022春）lecture 4-2 Robust MPC】给出的结果不一致，若本文计算存在错误，欢迎指出。

4.5 对比与讨论

讨论1：如果MPC中每一优化周期令 $z_{(0|k)} = x_{(0|k)}$ （记为MPC1），系统会怎样呢？
此时 $e_k = 0$ 始终成立，误差反馈项将无效，MPC的控制约束可扩展为：
$V_{min} = [-4,~-4,~, -4,~ \cdots, ~-4]$ ， $V_{max} = [4,~4,~, 4,~ \cdots, ~4]$ .
效果对比如下：
在这里插入图片描述
MPC1效果更好，这是因为鲁棒MPC中约束范围的保守定义，使其性能显著下降.
将MPC1的控制约束同样设为 $V_{min} = [-1.3836,~-4,~, -4,~ \cdots, ~-4]$ ， $V_{max} = [1.3836,~4,~, 4,~ \cdots, ~4]$ ：

对比可发现，在同样的条件下，增加鲁棒控制项可以提升MPC性能。
对比部分MATLAB代码见附录2.

讨论2：是否可在适当时刻令名义系统中 $z_{(0|k)} = x_{(0|k)}$ ？
【模型预测控制（2022春）lecture 4-2 Robust MPC】给出了答案：
可对比在由名义系统状态更新的和 $z_{(0|k)} = x_{(0|k)}$ 更新的两种MPC给出的控制序列下，
得到的最优代价函数（分别记为 $J_k^*(z_k)$ 和 $J_k^*(x_k)$ ）是否满足：
$J_k^*(x_k) \le J_k^*(z_k)$
满足时可令 $z_{(0|k)} = x_{(0|k)}$ ，因为分析稳定性的李雅普诺夫函数是通过代价函数定义的，
此时李雅普诺夫函数衰减得更快，使系统的稳定性分析仍然成立。
但如 2.3.2 中讨论的，终端约束的迭代可行性需要重新确定。

附录1

%% 计算G、H、Q、R
N = 10;
[G, H] = getGH(N, A, B);
[Qp, Rp] = getQR(N, Q, Psol, R);
%% 约束条件
lb = -4 * ones(N, 1);
ub = 4 * ones(N, 1);
lb(1) = -1.3836;
ub(1) = 1.3836;

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];
D = [0.1; 0.5];

K = [2.4950, 12.5106];
options = optimoptions('quadprog', 'MaxIterations', 200, 'Display','none');

xCur = [1.2;-0.7]; % 设初始状态为[1;1]
xLog = xCur;
zCur = xCur;
zLog = zCur;
vLog = [];
uLog = [];

step = 0:50;
v = 0;
for i = step

    Hp = 2 * (H' * Qp * H + Rp);
    fp = 2 * zCur' * G' * Qp * H;
    Hp = 0.5 * (Hp + Hp');
    Ain = tmpAin * tmpReshape * H;
    bin = tmpbin - tmpAin * tmpReshape * G * zCur;

    V = quadprog(Hp, fp, Ain, bin, [], [], lb, ub, v, options);
    v = V(1);
    u = v - K*(xCur - zCur);
    
    w = (rand - 0.5)/0.5 * 0.1;
    zCur = A * zCur + B*v; % 更新名义系统
    xCur = A*xCur + B*u + D*w; % x_k+1

    zLog = [zLog, zCur];
    xLog = [xLog, xCur];
    vLog = [vLog, v];
    uLog = [uLog, u];
end

figure(1)
subplot(3,1,1)
hold on
plot(step, xLog(1,1:end-1), DisplayName='x')
plot(step, zLog(1,1:end-1), DisplayName='z')
legend(Location='best')
title('x1')
grid on
subplot(3,1,2)
hold on
plot(step, xLog(2,1:end-1), DisplayName='x')
plot(step, zLog(2,1:end-1), DisplayName='z')
legend(Location='best')
title('x2')
grid on
subplot(3,1,3)
hold on
plot(step, uLog, DisplayName='u')
plot(step, vLog, DisplayName='v')
legend(Location='best')
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);
end

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

附录2

%% 对比
A = [1.1 2;0 0.95];
B = [0; 0.079];
D = [0.1; 0.5];

lb = -4 * ones(N, 1);
ub = 4 * ones(N, 1);
lb(1) = -1.3836;
ub(1) = 1.3836;

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

xCur1 = [1.2;-0.7]; % 设初始状态为[1;1]
xLog1 = xCur1;
zCur1 = xCur1;
vLog1 = [];

step = 0:50;
v = 0;
for i = step

    zCur1 = xCur1;
    Hp = 2 * (H' * Qp * H + Rp);
    fp = 2 * zCur1' * G' * Qp * H;
    Hp = 0.5 * (Hp + Hp');
    Ain = tmpAin * tmpReshape * H;
    bin = tmpbin - tmpAin * tmpReshape * G * zCur1;

    V = quadprog(Hp, fp, Ain, bin, [], [], lb, ub, v, options);
    v = V(1);
    u = v;
    
    w = (rand - 0.5)/0.5 * 0.1;
    xCur1 = A*xCur1 + B*u + D*w; % x_k+1

    xLog1 = [xLog1, xCur1];
    vLog1 = [vLog1, v];
end

figure(1)
subplot(3,1,1)
hold on
plot(step, xLog(1,1:end-1), DisplayName='x')
plot(step, zLog(1,1:end-1), DisplayName='z')
plot(step, xLog1(1,1:end-1), DisplayName='x1')
legend(Location='best', NumColumns=3)
title('x1')
grid on
subplot(3,1,2)
hold on
plot(step, xLog(2,1:end-1), DisplayName='x')
plot(step, zLog(2,1:end-1), DisplayName='z')
plot(step, xLog1(2,1:end-1), DisplayName='x1')
legend(Location='best', NumColumns=3)
title('x2')
grid on
subplot(3,1,3)
hold on
plot(step, uLog, DisplayName='u')
plot(step, vLog, DisplayName='v')
plot(step, vLog1, DisplayName='v1')
legend(Location='best', NumColumns=3)
title('u')
grid on

【MPC】模型预测控制笔记 (5)：抗干扰鲁棒MPC

目录

前言