当前位置：首页 > news >正文

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

news 来源：原创 2025/6/14 14:41:03

前言
- 【基础】二次型是凸函数
- 【基础】常见的凸约束
一、约束MPC的求解
- 1.1 等式约束MPC
- 1.2 不等式约束MPC
二、稳定性分析
- 2.1 终端等式约束
- - 2.1.1 稳定性证明
  - 2.1.2 迭代可行性分析
  - 2.1.3 MATLAB应用实例
  - 2.1.4 总结
- 2.2 终端不等式约束（优化无限时域）
- - 2.2.1 求解步骤与稳定性保证
  - 2.2.2 迭代可行性分析
  - 2.2.3 MATLAB应用实例
  - 2.2.4 总结
附录1
附录2

前言

由上一节可以看到，MPC是通过优化代价函数来求解最优控制序列，这一般是通过凸优化来进行的。
凸优化问题已有高效的算法来进行求解，我们确保构建的优化问题是凸优化即可。
致谢【模型预测控制（2022春）lecture 2-1 Constrained MPC】、【模型预测控制（2022春）lecture 2-2 Constrained MPC】

【基础】二次型是凸函数

凸函数定义：设 $C$ 是非空凸集， $f$ 是定义在 $D$ 上的函数，对任意 $x_1, x_2 \in C$ ， $\lambda \in (0,~1)$ ，均有：
$f(\lambda x_1 + (1-\lambda) x_2) \le \lambda f(x_1)+(1-\lambda)f(x_2) \tag{1}$ 则称 $f$ 为 $C$ 上的凸函数。

证明二次型 $\frac{1}{2}x^THx+f^Tx$ 是凸函数，其中 $\in \mathbb{R}^n$ ：

任取 $x_1, x_2 \in \mathbb{R}^n$ ，
计算式 (1) 左侧：
$\begin{align*} f(\lambda x_1 + (1-\lambda) x_2) &= \frac{1}{2} \left[ \lambda ^2 x_1^THx_1 + (1-\lambda) ^2 x_2^THx_2 + 2\lambda (1-\lambda) x_1^THx_2\right] + f^T \left[ \lambda x_1 + (1-\lambda) x_2\right] \end{align*}$
计算式 (1) 右侧：
$\begin{align*} \lambda f(x_1) + (1-\lambda) f(x_2) &= \frac{1}{2} \lambda x_1^THx_1 + \frac{1}{2} (1-\lambda) x_2^THx_2 + \lambda f^Tx_1 + (1-\lambda) f^Tx_2 \end{align*}$
左侧减右侧得：
$\begin{align*} & \hspace{0.5cm} f(\lambda x_1 + (1-\lambda) x_2) - \left(\lambda f(x_1) + (1-\lambda) f(x_2) \right) \\ &= -\frac{1}{2} \lambda (1 - \lambda )x_1^THx_1 + -\frac{1}{2}\lambda(1-\lambda) x_2^THx_2 + \lambda (1-\lambda) x_1^THx_2 \\ &= \frac{1}{2}\lambda(1-\lambda) \left[ x_1^THx_2 - x_1^THx_1 + x_1^THx_2 - x_2^THx_2 \right] \\ &= \frac{1}{2}\lambda(1-\lambda) \left[ x_1^TH(x_2 - x_1) + (x_1 - x_2)^THx_2 \right] \\ &= \frac{1}{2}\lambda(1-\lambda) \left[ x_1^TH(x_2 - x_1) - x_2^TH(x_2 - x_1) \right] \\ &= -\frac{1}{2}\lambda(1-\lambda) (x_2-x_1)^TH(x_2 - x_1) \le 0 \quad \text{(当 $H$ 半正定)} \end{align*}$
即：
$f(\lambda x_1 + (1-\lambda) x_2) \le \lambda f(x_1) + (1-\lambda) f(x_2)$

【基础】常见的凸约束

（约束条件构成凸集即为凸约束）
凸集定义：如果 $C$ 中任意两点间的线段仍然在 $C$ 中，则该集合为凸集；即对于任意 $x_1, x_2 \in C$ ， $\lambda \in [0,~1]$ ，都有 $\lambda x_1 + (1-\lambda) x_2 \in C$ .
在这里插入图片描述

来源：凸优化(Convex Optimization) (Stephen Boyd, Lieven Vandenberghe, 王书宁译)

常见凸约束：
(1) 等式约束： $A_{eq}x=b_{eq}$
(2) 不等式约束： $\le b$

证明：
（1）任取 $x_1, x_2 \in \{x |A_{eq}x=b_{eq}\}$ ， $\lambda \in [0,~1]$ ，有：
$\begin{align*} &\hspace{0.5cm} A_{eq}\left[\lambda x_1 + (1-\lambda) x_2 \right] \\ &= \lambda A_{eq}x_1 + (1-\lambda) A_{eq}x_2 \\ &=\lambda b_{eq} + (1-\lambda) b_{eq} \\ &= b_{eq} \end{align*}$ 故 $\lambda x_1 + (1-\lambda) x_2 \in \{x |A_{eq}x=b_{eq}\}$ .

（2）任取 $x_1, x_2 \in \{x |Ax \le b\}$ ， $\lambda \in [0,~1]$ ，有：
$\begin{align*} &\hspace{0.5cm} A\left[\lambda x_1 + (1-\lambda) x_2 \right] \\ &\le \lambda Ax_1 + (1-\lambda) Ax_2 \\ &= \lambda b + (1-\lambda) b \\ &= b \end{align*}$ 故 $\lambda x_1 + (1-\lambda) x_2 \in \{x |Ax=b\}$ .

一、约束MPC的求解

约束MPC的求解相似于上一节【MPC】模型预测控制笔记 (1)：无约束MPC，同样是构建二次规划问题来求解最优控制序列 $U^*$ ，即：
$U^*= \mathrm{arg} \min_U \frac{1}{2}U^THU+f^TU$

符号说明：
arg min (argument of the minimum): 使得目标函数取得最小值的自变量.
s. t. (subject to): 满足以下条件

1.1 等式约束MPC

在等式约束下，优化问题可表述为：
$\begin{align*} U^* = &\mathrm{arg} \min_U \frac{1}{2}U^THU+f^TU \\ &\mathrm{s. t.} \quad A_{eq}U = b_{eq} \end{align*}$
可通过拉格朗日乘子法将带约束优化问题转换为无约束优化问题。
引入未知的拉格朗日乘子 $\lambda$ ，构造拉格朗日函数为：
$\mathcal{L} =\frac{1}{2}U^THU+f^TU + \lambda (A_{eq}U - b_{eq})$
通过令一阶导数为 0 求解 $U$ 和 $\lambda$ :
$\left\{ \begin{aligned} \frac{\partial \mathcal{L}}{\partial U} &= HU + f + \lambda A_{eq}^T = 0 \\ \frac{\partial \mathcal{L}}{\partial \lambda} &= A_{eq}U - b_{eq} = 0 \end{aligned} \right.$

1.2 不等式约束MPC

在不等式约束下，优化问题可表述为：
$\begin{align*} U^* = &\mathrm{arg} \min_U \frac{1}{2}U^THU+f^TU \\ &\mathrm{s. t.} \quad AU \le b \end{align*}$
在 active-set 求解方法中，首先忽略约束求解 $U^*_{uc}$ ，若 $U^*_{uc}$ 超出了不等式的约束范围，说明 $U^*$ 落在了不等式约束的某一边界上（即满足 $A^\prime U=b^\prime$ ），此时可根据被激活的约束构建等式约束优化问题来求解。

二、稳定性分析

最优不能保证系统稳定，故需要一些措施来保证系统是渐近稳定的。
以下所有内容均针对线性定常系统：
$x_{k+1} = Ax_{x} + Bu_k$

2.1 终端等式约束

终端等式约束即强制令MPC最后一个状态优化为0，
即在原本的优化问题中增加约束 $x_{(N|k)} = 0$ .

2.1.1 稳定性证明

选取最优的代价函数作为李雅普诺夫函数 $V(x_k)$ ：
$V(x_k) = J^*_k=\sum^N_{i=1} \left(x^T_{(i|k)}Qx_{(i|k)} + u^T_{(i|k)}Ru_{(i-1|k)} \right)$
显然满足 $V(x_k) > 0$ .
设 $J_{k+1}$ 为完全按 $U_k^*$ 序列执行下的代价，且令 $u_{(N-1|k+1)} = 0$ ，
通过终端约束 $x_{(N|k)} = 0$ 可知，在 $U_k^*$ 序列下，有 $x_{(N|k+1)} = Ax_{(N-1|k+1)} + Bu_{(N-1|k+1)} = Ax_{(N|k)} + 0 = 0$ .
故有：
$\begin{align*} J_{k+1}^* - J_{k}^* &\le J_{k+1} - J_{k}^* \\ &= \sum^{N}_{i=1} \left( x^T_{(i|k+1)}Qx_{(i|k+1)} + u^T_{(i-1|k+1)}Ru_{(i-1|k+1)} \right) - \sum^{N}_{i=1} \left( x^T_{(i|k)}Qx_{(i|k)} + u^T_{(i-1|k)}Ru_{(i-1|k)} \right) \\ &= \left[ \sum^{N-1}_{i=1} \left( x^T_{(i+1|k)}Qx_{(i+1|k)} + u^T_{(i|k)}Ru_{(i-1|k)} \right) + 0 + 0 \right] - \sum^{N}_{i=1} \left( x^T_{(i|k)}Qx_{(i|k)} + u^T_{(i-1|k)}Ru_{(i-1|k)} \right) \\ &= - \left( x^T_{(1|k)}Qx_{(1|k)} + u^T_{(0|k)}Ru_{(0|k)} \right) \\ &\le 0 \end{align*}$
其中， $Q$ 、 $R$ 正定。故有 $\Delta V(x) = J_{k+1}^* - J_{k}^* \le 0$ ，当且仅当 $x^T_{(1|k)}=0$ ， $u^T_{(0|k)}=0$ 时，等号成立。
故系统渐近稳定。

2.1.2 迭代可行性分析

终端约束 $x_{(N|k)} = 0$ 其实是难以保证可达到的，
可以证明的是，当在初始状态下可达，则未来迭代中均可达（所谓迭代可行性）。

在无约束系统中，
一阶系统可以在 1 步的跌代中使状态归零，
如系统： $\dot{v} = F/m$ ，无论初值 $v_0$ 为多少，都可施加适当的作用力使速度在一个迭代中归零，即令 $v_0 + (F^*/m) \Delta t = 0$ .
二阶系统可以在 2 步的跌代中使状态归零，
如系统：
$\left\{ \begin{aligned} \dot{s} &= v\\ \dot{v} &= F/m \end{aligned} \right.$
在第一步迭代中， $s_1 = s_0 + v_0 \Delta t$ ，使 $v_1 =s_1/\Delta t$ ，则 $F_0 = m(v_1-v_0)/ \Delta t$ .
在第二步迭代中， $s_2 = s_1 + v_1 \Delta t = 0$ ，使 $v_2 = v_1 + (F_1/m)\Delta t = 0$ 即完成系统状态归零.
但在约束系统中，这是难以保证的。

我们可分析初始状态下终端约束 $x_{(N|k)} = 0$ 是否可达来保证终端约束可达。

迭代可行性：终端约束 $x_{(N|k)} = 0$ 当在初始状态下可达，则未来迭代中均可达

证明：
若在初始状态下有可行解 $U_0 = \left[ u_{(0|0)},~ u_{(1|0)}, ~u_{(2|0)},~\cdots,~ u_{(N-1|0)} \right]$ ，使得 $x_{(N|0)} = 0$ .
则在执行 $u_{(0|0)}$ 后进入下一状态，必然存在可行解 $U_1 = \left[ u_{(1|0)}, ~u_{(2|0)},~\cdots,~ u_{(N-1|0)},~u_{(N-1|1)} \right]$ .
其中 $u_{(N-1|1)} = 0$ .
因 $x_{(N-1|1)} = x_{(N|0)} = 0$ ，仅当 $u_{(N-1|1)} = 0$ 时，有 $x_{(N|1)} = Ax_{(N-1|1)} + Bu_{(N-1|1)} = 0$ .
由此类推，
当在初始状态下有可行解 $U_0 = \left[ u_{(0|0)},~ u_{(1|0)}, ~u_{(2|0)},~\cdots,~ u_{(N-1|0)} \right]$ 时，
系统必然有可行解 $\left[ u_{(0|0)},~ u_{(1|0)}, ~u_{(2|0)},~\cdots,~ u_{(N-1|0)},~0,~ 0,~ \cdots \right]$ 可满足终端约束.

2.1.3 MATLAB应用实例

针对系统：
$x_{k+1} = Ax_k + Bu_k$
其中 $\in \mathbb{R}^2$ ， $\in \mathbb{R}^1$ ， $\begin{bmatrix} 1.1 & 2 \\ 0 & 0.95 \end{bmatrix}$ ， $\begin{bmatrix} 0 \\ 0.079 \end{bmatrix}$ ，且输入需要满足约束 $\le u \le 2$ .

（1） $N$ 步预测空间的状态可写为：
$X=\mathcal{G}x_{(0|k)} + \mathcal{H}U \tag{1}$
其中，
$\begin{align*} 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} &= \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）定义代价函数为：
$J=X^T\mathcal{Q}X +U^T\mathcal{R}U \tag{2}$
（3）将式 (1) 代入式 (2)，并考虑约束条件，增加终端约束，可构建二次规划问题：
$\begin{align*} U_{k}^* = &\mathrm{arg} \min_{U_k} \left[ (\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_k + U_k^T (\mathcal{H}^T \mathcal{Q}^\prime \mathcal{H} + \mathcal{R})U_k \right] \\ &\hspace{3cm} \mathrm{s. t.} \quad \mathbf{-2}_{N \times1} \le U_k \le \mathbf{2}_{N \times1} \\ &\hspace{4cm} x_{(N|k)} = 0 \end{align*}$
其中， $x_{(N|k)} = [0,~0,~\cdots, ~I_{2 \times 2}]X_k = [0,~0,~\cdots, ~I_{2 \times 2}] \left[ \mathcal{G}x_{(0|k)} + \mathcal{H}U_k \right] = 0$ .
可以改写为：
$A_{eq}U_k = b_{eq}$ 其中 $A_{eq} = [0,~0,~\cdots, ~I_{2 \times 2}]\mathcal{H}$ ， $b_{eq} = -[0,~0,~\cdots, ~I_{2 \times 2}]\mathcal{G}x_{(0|k)}$ .
忽略与输入无关的项，优化问题可重新写为：
$\begin{align*} U_{k}^* = &\mathrm{arg} \min_{U_k} \left[ \frac{1}{2}U_k^THU_k + f^TU_k \right] \\ &\mathrm{s. t.} \quad \mathbf{-2}_{N \times1} \le U_k \le \mathbf{2}_{N \times1} \\ &\hspace{1cm} A_{eq}U_k = b_{eq} \end{align*}$ 其中， $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}$ .
以上形式可使用MATLAB的 quadprog 函数求解，MATLAB代码见附录1.
设置控制时域 $N = 20$ ，得到结果如下：
在这里插入图片描述

2.1.4 总结

终端等式约束MPC特点：

+ 简单
+ 可保证系统稳定性，但需确保初始可行
- 需要足够长的控制时域 $N$ 来保证初始可行
- 增加终端等式约束会削弱求解可行性：原来的优化问题是凸优化，加上等式约束后可能使原本的凸约束集变为空集，无法求解

2.2 终端不等式约束（优化无限时域）

与上一节【MPC】模型预测控制笔记 (1)：无约束MPC 一致，优化无限步预测空间时，可通过选取李雅普诺夫直接法证明系统的稳定性。
但找到一个反馈增益 $K$ 使系统 $x_{k+1} = (A-BK)x_{k}$ 渐近稳定时，显然不能在任意状态下，使所有 $u_k = -Kx_k$ 满足约束。

设集合 $\mathcal{U}$ 为满足约束条件的输入集合。
我们可以寻找一个不变集 $\Omega$ : 若 $x_k \in \Omega$ ，则 $x_{k+1} \in \Omega$ .
当找到一个反馈增益 $K$ 可使系统 $x_{k+1} = (A-BK)x_{k}$ 渐近稳定，
且满足 $x_k \in \Omega$ ， $u_k \in -K\Omega \subset \mathcal{U}$ ，
则此时系统未来时刻始终满足约束条件，可使用上一节中的无约束MPC的方法求解。

2.2.1 求解步骤与稳定性保证

（1）增加终端不等式约束 $Ax_{(N,k)} \le b$ ，保证优化的终端状态进入不变集 $x_{(N,0)} \in \Omega$
（2）选择一个反馈增益 $K$ ，使系统 $x_{k+1} = (A-BK)x_{k}$ 渐近稳定，且满足 $u_k \in -K\Omega \subset \mathcal{U}$
（3）通过优化无限时域来保证稳定性（证明参考【MPC】模型预测控制笔记 (1)：无约束MPC）：
终端不等式约束保证了在 $N$ 到 $\infty$ 的时域中，可使 $u_k = -Kx_k$ ，满足 $u_k \in \mathcal{U}$ .

2.2.2 迭代可行性分析

当初始状态可满足终端不等式约束 $Ax_{(N,k)} \le b$ ，未来所有状态都可以满足：

当在初始状态下有可行解 $U_0 = \left[ u_{(0|0)},~ u_{(1|0)}, ~u_{(2|0)},~\cdots,~ u_{(N-1|0)} \right]$ 时，
系统必然有可行解 $\left[ u_{(0|0)},~ u_{(1|0)}, ~u_{(2|0)},~\cdots,~ u_{(N-1|0)},~-Kx_{N+1},~ -Kx_{N+2},~ \cdots \right]$ ，使系统在 $N$ 步状态后始终满足终端不等式约束.

2.2.3 MATLAB应用实例

同样针对系统：
$x_{k+1} = Ax_k + Bu_k$
其中 $\in \mathbb{R}^2$ ， $\in \mathbb{R}^1$ ， $\begin{bmatrix} 1.1 & 2 \\ 0 & 0.95 \end{bmatrix}$ ， $\begin{bmatrix} 0 \\ 0.079 \end{bmatrix}$ ，且输入需要满足约束 $\le u \le 2$ .

（1） $N$ 步预测空间的状态可写为：
$X=\mathcal{G}x_{(0|k)} + \mathcal{H}U$
其中，
$\begin{align*} 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} &= \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）增加终端约束 $x_{(N|k)} \in \Omega$ ，其中，
$x_{(N|k)} = [0,~0,~\cdots, ~I_{2 \times 2}]X_k = [0,~0,~\cdots, ~I_{2 \times 2}] \left[ \mathcal{G}x_{(0|k)} + \mathcal{H}U_k \right]$ ， $\Omega$ 为不变集.
（不变集的选取待补充，目前笔者也不清楚）
可以选取足够小的集合 $\mathcal{X}$ 来替代不变集，但集合范围越大，可行性越强。
此处选取 $\mathcal{X} = [-0.2,~0.2] \times [-0.1,~0.1] ~~(\text{笛卡尔积形式，等价于：}\{(x_1, x_2) \in \mathbb{R}^2 |~ x_1 \in [-0.2,~0.2], x_2 \in [-0.1,~0.1]\})$
约束 $x_{(N|k)} \in \mathcal{X}$ 可以改写为：
$A_{in}U_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} x_{(0|k)} \end{align*}$ .
（3）选取反馈增益 $\quad 5.76]$ ，可验证其满足 $|\mathrm{eig}(A-BK)|<1$ ， $\le -K\mathcal{X} \le 2$
（4）通过 $P-(A-BK)^TP(A-BK) = Q + K^TRK$ 求解 P
（5）定义代价函数为：
$J=X^T\mathcal{Q_p}X +U^T\mathcal{R_p}U$ 其中， $Q_p = \mathrm{diag}(Q, Q, \cdots, P)$ ， $R_p = \mathrm{diag}(R, R, \cdots, R)$ .
（6）构建二次规划问题：
$\begin{align*} U_{k}^* = &\mathrm{arg} \min_{U_k} \left[ \frac{1}{2}U_k^THU_k + f^TU_k \right] \\ &\mathrm{s. t.} \quad \mathbf{-2}_{N \times1} \le U_k \le \mathbf{2}_{N \times1} \\ &\hspace{1cm} A_{in}U_k \le b_{in} \end{align*}$ 其中， $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}$ .
MATLAB代码见附录2.
设置控制时域 $N = 20$ ，得到结果如下：
在这里插入图片描述

2.2.4 总结

终端不等式约束相比终端等式约束：

+ 可行性更强，但仍需确保初始可行
- 更复杂

附录1

%%
A = [1.1 2;0 0.95];
B = [0; 0.079];
Q = eye(2);
R = 0.1;% 预测空间
N = 20; % 当控制时域过小时（如 N=10），将无法满足终端约束条件，求解会导致错误[G, H] = getGH(N, A, B);
[Qp, Rp] = getQR(N, Q, Q, R);xCur = [1;1]; % 设初始状态为[1;1]
%% 约束条件
lb = -2 * ones(N, 1);
ub = 2 * ones(N, 1);n = size(A, 2);
tmp = kron(ones(1, N-1), zeros(n));
tmp = [tmp, eye(n)];
% Aeq = tmp * H;
% beq = -tmp * G * xCur;
%% 效果演示
xCur = [1; 1]; % 设初始状态为[1;1]
xLog = xCur;
uLog = [];options = optimoptions('quadprog', 'MaxIterations', 200, 'Display','none');step = 0:50;
for i = stepHp = 2 * (H' * Qp * H + Rp);fp = 2 * xCur' * G' * Qp * H;Hp = 0.5 * (Hp + Hp');Aeq = tmp * H;beq = -tmp * G * xCur;U = quadprog(Hp, fp, [], [], Aeq, beq, lb, ub, zeros(N,1), options);u = U(1);xCur = A*xCur + B*u;xLog = [xLog, xCur];uLog = [uLog, u];
endfigure(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
%%
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

附录2

%%
A = [1.1 2;0 0.95];
B = [0; 0.079];
Q = eye(2);
R = 0.1;% 预测空间
N = 20;% 求解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] = getGH(N, A, B);
[Qp, Rp] = getQR(N, Q, Psol, R);xCur = [1;1]; % 设初始状态为[1;1]
%% 约束条件
lb = -2 * ones(N, 1);
ub = 2 * 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;
%% 效果演示
xCur = [1; 1]; % 设初始状态为[1;1]
xLog = xCur;
uLog = [];options = optimoptions('quadprog', 'MaxIterations', 200, 'Display','none');step = 0:50;
u = zeros(N, 1);
for i = stepHp = 2 * (H' * Qp * H + Rp);fp = 2 * xCur' * G' * Qp * H;Hp = 0.5 * (Hp + Hp');Ain = tmpAin * tmpReshape * H;bin = tmpbin - tmpAin * tmpReshape * G * xCur;U = quadprog(Hp, fp, Ain, bin, [], [], lb, ub, u, options);u = U(1);xCur = A*xCur + B*u;xLog = [xLog, xCur];uLog = [uLog, u];
endfigure(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
%%
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