目录
- 前言
- 不确定模型
- 稳定性分析
- MATLAB实例1 - 忽略微小得模型参数误差
- MATLAB实例2 - 忽略模型中的非线性项
- 附录1
- 附录2
前言
致谢 【模型预测控制（2022春）lecture 4-2 Robust MPC】
不确定模型
假设系统的真实模型为：
x k + 1 = A x k + B ( u k + δ 1 ( x k , u k ) ) + δ 2 ( x k ) (1) x_{k+1} = Ax_k + B(u_k + \delta_1(x_k, u_k)) + \delta_2(x_k) \tag{1} xk+1=Axk+B(uk+δ1(xk,uk))+δ2(xk)(1)
其中 δ 1 ( x k , u k ) \delta_1(x_k, u_k) δ1(xk,uk) 和 δ 2 ( x k ) \delta_2(x_k) δ2(xk) 为系统的不确定函数.
假设：
(1) δ 1 ( 0 , 0 ) = 0 \delta_1(0, 0) = 0 δ1(0,0)=0， δ 2 ( 0 ) = 0 \delta_2(0) = 0 δ2(0)=0.
(2) ∣ ∣ δ 1 ( x k , u k ) ∣ ∣ ≤ ρ 1 ∣ ∣ x k ∣ ∣ + κ ∣ ∣ u k ∣ ∣ ||\delta_1(x_k, u_k)|| \le \rho_1 ||x_k|| + \kappa ||u_k|| ∣∣δ1(xk,uk)∣∣≤ρ1∣∣xk∣∣+κ∣∣uk∣∣，其中 ρ 1 > 0 \rho_1>0 ρ1>0， 0 ≤ κ < 1 0 \le \kappa <1 0≤κ<1（不确定性的影响应小于控制输入的影响）.
(3) ∣ ∣ δ 2 ( x k ) ∣ ∣ ≤ ρ 2 ∣ ∣ x k ∣ ∣ ||\delta_2(x_k)|| \le \rho_2 ||x_k|| ∣∣δ2(xk)∣∣≤ρ2∣∣xk∣∣，其中 ρ 2 > 0 \rho_2>0 ρ2>0.
可将系统 (1) 记为带干扰项的形式：
x k + 1 = A x k + B u k + D w k (2) x_{k+1} = Ax_k + Bu_k + Dw_k \tag{2} xk+1=Axk+Buk+Dwk(2)
其中 D w = B δ 1 + δ 2 Dw = B\delta_1 + \delta_2 Dw=Bδ1+δ2.
系统 (2) 即可通过 【MPC】模型预测控制笔记 (5)：抗干扰鲁棒MPC 设计鲁棒控制器。
不同的是，此处干扰与系统状态和输入有关，根据假设，系统可以是渐近稳定的。
稳定性分析
设名义MPC的最优代价函数为 J k ∗ J_k^* Jk∗，有 J k ∗ > 0 J_k^*>0 Jk∗>0， J k + 1 ∗ − J k ∗ ≤ − ( z ( 1 ∣ k ) ∗ ) T Q z ( 1 ∣ k ) ∗ + ( u ( 0 ∣ k ) ∗ ) T R u ( 0 ∣ k ) ∗ ) J_{k+1}^* - J_k^* \le - \left( z_{(1|k)}^*)^TQz_{(1|k)}^* + (u_{(0|k)}^*)^TRu_{(0|k)}^* \right) Jk+1∗−Jk∗≤−(z(1∣k)∗)TQz(1∣k)∗+(u(0∣k)∗)TRu(0∣k)∗).
设误差反馈增益为 K K K，满足 ∣ e i g ( A − B K ) < 1 ∣ |\mathrm{eig}(A-BK)<1| ∣eig(A−BK)<1∣，记 A K = A − B K A_K = A-BK AK=A−BK，
有 e k + 1 = A K e k + D w k e_{k+1} = A_Ke_k + Dw_k ek+1=AKek+Dwk，且必然存在 P f b P_{fb} Pfb 为正定对称矩阵，满足 A K T P f b A K − P f b < 0 A_K^T P_{fb} A_K - P_{fb}<0 AKTPfbAK−Pfb<0 .
（可参考【MPC】模型预测控制笔记 (4)：约束输出反馈MPC ）
将 [ z k e k ] T [z_k~~e_k]^T [zk ek]T 定义为增广系统的状态，下面通过李雅普诺夫直接法确定该系统的稳定性.
选取李雅普诺夫函数：
V k = e k T P f b e k + γ J k ∗ V_k = e_k^TP_{fb}e_k + \gamma J_k^* Vk=ekTPfbek+γJk∗
其中 γ > 0 \gamma >0 γ>0，显然 V k > 0 V_k > 0 Vk>0.
有：
V k + 1 − V k = e k + 1 T P f b e k + 1 − e k T P f b e k + γ ( J k + 1 ∗ − J k ∗ ) = e k T ( A K T P f b A K − P f b ) e k + w k T D T P f b D w k + 2 w k T D T P f b A K e k + γ ( J k + 1 ∗ − J k ∗ ) \begin{align*} V_{k+1} - V_{k} &= e_{k+1}^TP_{fb} e_{k+1} - e_k^TP_{fb} e_k + \gamma(J_{k+1}^* - J_{k}^*) \\ &= e_k^T(A_K^TP_{fb}A_K - P_{fb}) e_k + w_k^TD^TP_{fb}Dw_k + 2w_k^TD^TP_{fb}A_Ke_k + \gamma(J_{k+1}^* - J_{k}^*) \end{align*} Vk+1−Vk=ek+1TPfbek+1−ekTPfbek+γ(Jk+1∗−Jk∗)=ekT(AKTPfbAK−Pfb)ek+wkTDTPfbDwk+2wkTDTPfbAKek+γ(Jk+1∗−Jk∗)
上式中， e k T ( A K T P f b A K − P f b ) e k < 0 e_k^T(A_K^TP_{fb}A_K - P_{fb}) e_k <0 ekT(AKTPfbAK−Pfb)ek<0， γ ( J k + 1 ∗ − J k ∗ ) < 0 \gamma(J_{k+1}^* - J_{k}^*)<0 γ(Jk+1∗−Jk∗)<0.
且 D w = B δ 1 + δ 2 Dw = B\delta_1 + \delta_2 Dw=Bδ1+δ2，有：
w k T D T P f b D w k ≤ ρ 3 e k T e k + ρ 4 z k T z k + ρ 5 u k T u k w_k^TD^TP_{fb}Dw_k \le \rho_3e_k^Te_k + \rho_4 z_k^Tz_k + \rho_5 u_k^Tu_k wkTDTPfbDwk≤ρ3ekTek+ρ4zkTzk+ρ5ukTuk
其中 ρ 3 > 0 \rho_3>0 ρ3>0， ρ 4 > 0 \rho_4>0 ρ4>0， ρ 5 > 0 \rho_5>0 ρ5>0.
根据广义柯西不等式（ ( a − b ) T P ( a − b ) = a T P a − 2 a T P b + b T P b ≥ 0 (a - b)^T P (a - b) = a^T P a - 2 a^T P b + b^T P b \ge 0 (a−b)TP(a−b)=aTPa−2aTPb+bTPb≥0 ）：
2 w k T D T P f b A K e k ≤ w k T D T P f b D w k + e k T A K T P f b A K e k 2w_k^TD^TP_{fb}A_Ke_k \le w_k^TD^TP_{fb}Dw_k + e_k^TA_K^TP_{fb}A_Ke_k 2wkTDTPfbAKek≤wkTDTPfbDwk+ekTAKTPfbAKek
当 ρ 3 \rho_3 ρ3、 ρ 4 \rho_4 ρ4、 ρ 5 \rho_5 ρ5 很小时，可以存在有限大的 γ \gamma γ，使 V k + 1 − V k < 0 V_{k+1} - V_{k}<0 Vk+1−Vk<0.
MATLAB实例1 - 忽略微小得模型参数误差
真实系统：
x k + 1 = A x k + B u k \begin{align*} x_{k+1} &= Ax_k + Bu_k \end{align*} xk+1=Axk+Buk
其中， x k ∈ R n x_k \in \mathbb{R}^n xk∈Rn 是系统的全状态， u k ∈ R p u_k \in \mathbb{R}^p uk∈Rp 是系统的输入， A = [ 1.1 2 0 0.95 ] A = \begin{bmatrix} 1.1 & 2 \\ 0 & 0.95 \end{bmatrix} A=[1.1020.95]， B = [ 0 0.079 ] B = \begin{bmatrix} 0 \\ 0.079 \end{bmatrix} B=[00.079]， − 4 ≤ u k ≤ 4 -4 \le u_k \le 4 −4≤uk≤4.
假设估计得到的系统模型不准确，模型为：
z k + 1 = A ˉ z k + B ˉ u k (3) z_{k+1} = \bar{A}z_k + \bar{B}u_k \tag{3} zk+1=Aˉzk+Bˉuk(3)
A ˉ = [ 1 2 0 1 ] \bar{A} = \begin{bmatrix} 1 & 2 \\ 0 & 1 \end{bmatrix} Aˉ=[1021]， B ˉ = [ 0 0.08 ] \bar{B} = \begin{bmatrix} 0 \\ 0.08 \end{bmatrix} Bˉ=[00.08].
定义误差为 e k = x k − z k e_k = x_k - z_k ek=xk−zk，使用 LQR 计算最优反馈增益：
Abar = [1 2;0 1];
Bbar = [0; 0.08];
Q = eye(2);
R = 0.1;K = LQR(Abar, Bbar, Q, R, 500, 1e-6);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
得 K = [ 1.8661 11.8794 ] K = \begin{bmatrix} 1.8661 & 11.8794 \end{bmatrix} K=[1.866111.8794].
通过模型 (3) 设计MPC，未来 N N N 步的状态可表示为：
Z k = G z ( 0 ∣ k ) + H V k Z_k = \mathcal{G}z_{(0|k)} + \mathcal{H}V_k Zk=Gz(0∣k)+HVk
其中，
Z k = [ z ( 1 ∣ k ) z ( 2 ∣ k ) ⋯ z ( N ∣ k ) ] T V k = [ v ( 0 ∣ k ) v ( 1 ∣ k ) ⋯ v ( N − 1 ∣ k ) ] T G = [ A A 2 ⋯ A N ] T H = [ B 0 0 ⋯ 0 A B B 0 ⋯ 0 A 2 B A B B ⋯ 0 ⋮ ⋮ ⋮ ⋱ ⋮ A N − 1 B A 2 B A B ⋯ B ] \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*} ZkVkGH=[z(1∣k) z(2∣k) ⋯ z(N∣k)]T=[v(0∣k) v(1∣k) ⋯ v(N−1∣k)]T=[A A2 ⋯ AN]T= BABA2B⋮AN−1B0BAB⋮A2B00B⋮AB⋯⋯⋯⋱⋯000⋮B
代价函数矩阵形式为：
J k = Z k T Q Z k + V k T R V k J_k = Z_k^T\mathcal{Q}Z_k + V_k^T\mathcal{R}V_k Jk=ZkTQZk+VkTRVk
其中 Q = d i a g ( Q , Q , ⋯ , Q , P ) \mathcal{Q} = \mathrm{diag}(Q,Q, \cdots, Q, P) Q=diag(Q,Q,⋯,Q,P)， R = d i a g ( R , R , ⋯ , R ) \mathcal{R} = \mathrm{diag}(R, R, \cdots, R) R=diag(R,R,⋯,R).
将式 (5) 代入上式，有：
J k = ( G z ( 0 ∣ k ) ) T Q ′ G z ( 0 ∣ k ) + 2 z ( 0 ∣ k ) T G T Q ′ H V k + V k T ( H T Q ′ H + R ) V k 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 Jk=(Gz(0∣k))TQ′Gz(0∣k)+2z(0∣k)TGTQ′HVk+VkT(HTQ′H+R)Vk
取控制时域 N = 10 N = 10 N=10，代价函数权重 Q = d i a g ( 1 , 1 ) Q = \mathrm{diag}(1,1) Q=diag(1,1)， R = 0.1 R = 0.1 R=0.1，取 K = [ 1.8661 11.8794 ] K = \begin{bmatrix} 1.8661 & 11.8794 \end{bmatrix} K=[1.866111.8794] 计算总端代价权重：
Abar = [1 2;0 1];
Bbar = [0; 0.08];
K = [1.8661 11.8794];Q = eye(2);
R = 0.1;
syms P [2 2] % P 为2*2的矩阵
equ = P - (Abar - Bbar*K)' * P * (Abar - Bbar*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)
得 P = [ 3.1830 6.6986 6.6986 29.2465 ] P = \begin{bmatrix} 3.1830 & 6.6986 \\ 6.6986 & 29.2465 \end{bmatrix} P=[3.18306.69866.698629.2465].
考虑控制输入约束为：
V m i n ≤ V k ≤ V m a x V_{min} \le V_k \le V_{max} Vmin≤Vk≤Vmax
其中 V m i n = [ − 3 , − 3 , , ⋯ , − 3 ] V_{min} = [-3,~-3,~,~ \cdots, ~-3] Vmin=[−3, −3, , ⋯, −3]， V m a x = [ 3 , 3 , ⋯ , 3 ] V_{max} = [3,~3,~ \cdots, ~3] Vmax=[3, 3, ⋯, 3].
取终端不变集：
Ω ′ = [ − 0.2 , 0.2 ] × [ − 0.1 , 0.1 ] \Omega^\prime = [-0.2,~0.2] \times [-0.1,~0.1] Ω′=[−0.2, 0.2]×[−0.1, 0.1]
写为不等式形式，为：
A i n V k ≤ b i n A_{in}V_k \le b_{in} AinVk≤bin其中，
A i n = [ 1 0 − 1 0 0 1 0 − 1 ] [ 0 , 0 , ⋯ , I 2 × 2 ] H b i n = [ 0.2 0.2 0.1 0.1 ] − [ 1 0 − 1 0 0 1 0 − 1 ] [ 0 , 0 , ⋯ , I 2 × 2 ] G z ^ ( 0 ∣ k ) \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*} Ainbin= 1−100001−1 [0, 0, ⋯, I2×2]H= 0.20.20.10.1 − 1−100001−1 [0, 0, ⋯, I2×2]Gz^(0∣k).
求解最优控制序列：
V ∗ = a r g min V k J k s . t . V m i n ≤ V k ≤ V m a x A i n V k ≤ b i n \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*} s.t.V∗=argVkminJkVmin≤Vk≤VmaxAinVk≤bin
最终作用于真实系统的控制输入为：
u k = v ( 0 ∣ k ) − K e k u_k = v_{(0|k)} - Ke_k uk=v(0∣k)−Kek
MATLAB代码见 附录1，系统动态效果如下：
MATLAB实例2 - 忽略模型中的非线性项
真实系统：
x k + 1 = A x k + B u k + δ ( x k , u k ) \begin{align*} x_{k+1} &= Ax_k + Bu_k + \delta(x_k, u_k) \end{align*} xk+1=Axk+Buk+δ(xk,uk)
其中， x k ∈ R n x_k \in \mathbb{R}^n xk∈Rn 是系统的全状态， u k ∈ R p u_k \in \mathbb{R}^p uk∈Rp 是系统的输入，
A = [ 1.1 2 0 0.95 ] A = \begin{bmatrix} 1.1 & 2 \\ 0 & 0.95 \end{bmatrix} A=[1.1020.95]， B = [ 0 0.079 ] B = \begin{bmatrix} 0 \\ 0.079 \end{bmatrix} B=[00.079]， δ ( x , u ) = [ 0.4 x 1 2 + 0.1 x 2 3 0.5 x 2 3 + 0.1 u ] \delta(x,u) = \begin{bmatrix} 0.4x_1^2 + 0.1x_2^3 \\ 0.5x_2^3 + 0.1u \end{bmatrix} δ(x,u)=[0.4x12+0.1x230.5x23+0.1u]， − 4 ≤ u k ≤ 4 -4 \le u_k \le 4 −4≤uk≤4.
假设 δ ( x k , u k ) \delta(x_k, u_k) δ(xk,uk) 是被忽略的非线性项，将系统模型简化为：
z k + 1 = A z k + B u k (4) z_{k+1} = Az_k + Bu_k \tag{4} zk+1=Azk+Buk(4)
同样定义误差为 e k = x k − z k e_k = x_k - z_k ek=xk−zk，使用 LQR 计算最优反馈增益得 K = [ 2.4950 12.5106 ] K = \begin{bmatrix} 2.4950 & 12.5106 \end{bmatrix} K=[2.495012.5106].
同上根据模型 (4) 设计MPC.
最终作用于真实系统的控制输入为：
u k = v ( 0 ∣ k ) − K e k u_k = v_{(0|k)} - Ke_k uk=v(0∣k)−Kek
MATLAB代码见 附录2，系统动态效果如下：
附录1
%% 计算G、H、Q、R
N = 10;
[G, H] = getGH(N, Abar, Bbar);
[Qp, Rp] = getQR(N, Q, Psol, R);
%% 约束条件
lb = -3 * ones(N, 1);
ub = 3 * 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];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 = stepHp = 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);zCur = Abar * zCur + Bbar*v; % 更新名义系统xCur = A*xCur + B*u; % x_k+1zLog = [zLog, zCur];xLog = [xLog, xCur];vLog = [vLog, v];uLog = [uLog, u];
endfigure(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', 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')
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')
legend(Location='best', NumColumns=3)
title('u')
grid onfunction [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
附录2
%% 计算总端代价
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)
%% 计算G、H、Q、R
N = 10;
[G, H] = getGH(N, A, B);
[Qp, Rp] = getQR(N, Q, Psol, R);
%% 约束条件
lb = -3 * ones(N, 1);
ub = 3 * 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];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 = stepHp = 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);delta = [0.4*xCur(1)^2 + 0.1*xCur(2)^3; 0.5*xCur(2)^3 + 0.1 * u];zCur = A * zCur + B*v; % 更新名义系统xCur = A*xCur + B*u + delta; % x_k+1zLog = [zLog, zCur];xLog = [xLog, xCur];vLog = [vLog, v];uLog = [uLog, u];
endfigure(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', 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')
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')
legend(Location='best', NumColumns=3)
title('u')
grid on
%% 对比
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 = stepzCur1 = 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+1xLog1 = [xLog1, xCur1];vLog1 = [vLog1, v];
endfigure(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
%%
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
