约束下的精准控制：MPC（模型预测控制）基本原理与数学推导及案例分析

模型预测控制（Model Predictive Control, MPC）

模型预测控制（Model predicative control，MPC）是由 Richalet 等人在 1978
年提出的一种基于数值优化的算法。模型预测控制主要包含预测模型、滚动优化和反馈校正这三个要素。模型预测控制的基本实现是根据对象的约束条件和控制目标，使用成本函数来评价系统的行为，当得到优化问题的最优解后，再将这个控制策略的第一个动作应用到对象上，然后重复这个步骤。模型预测控制在对系统进行优化时，能够将处理约束的过程与优化的过程结合在一起，可以减小扰动引起的不确定性。除了能够考虑对象的约束条件和控制目标之外，与障碍物、运动、环境干扰等有关的不确定性因素也都可以在模型预测控制算法的代价函数中形式化。虽然实时性是模型预测控制的一个缺点，但是随着近年来计算机性能的提高、算法结构的改进、计算工具的优化，模型预测控制的实时性得到很大的改善，已被广泛地应用于无人机、无人车及无人船的路径规划中去。

以下从数学建模、预测方程、优化问题、反馈校正等方面进行详细推导。

一、系统建模：离散时间状态空间模型

假设被控对象为线性时不变系统，其离散时间状态空间模型为：
$$\{\begin{array}{l}{\mathbf{x}}_{k+1}=\mathbf{A}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k\\ {\mathbf{y}}_k=\mathbf{C}{\mathbf{x}}_k+\mathbf{D}{\mathbf{u}}_k\end{array}$$

• ${\mathbf{x}}_k\in {\mathbb{R}}^n$：状态向量
• ${\mathbf{u}}_k\in {\mathbb{R}}^m$：控制输入向量
• ${\mathbf{y}}_k\in {\mathbb{R}}^p$：输出向量
• $\mathbf{A}\in {\mathbb{R}}^{n\times n}$：状态转移矩阵
• $\mathbf{B}\in {\mathbb{R}}^{n\times m}$：输入矩阵
• $\mathbf{C}\in {\mathbb{R}}^{p\times n}$：输出矩阵
• $\mathbf{D}\in {\mathbb{R}}^{p\times m}$：直接传递矩阵

二、预测模型：未来状态与输出的预测

在MPC中，控制器需要预测未来 $N_p$ 步的状态和输出（称为预测时域）。假设当前时刻为 $k$，则预测方程如下：

1. 状态预测序列：
   $$\begin{gathered} {\mathbf{x}}_{k+1|k}=\mathbf{A}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k \\ {\mathbf{x}}_{k+2|k}=\mathbf{A}{\mathbf{x}}_{k+1|k}+\mathbf{B}{\mathbf{u}}_{k+1} \\ ⋮ \\ {\mathbf{x}}_{k+N_p|k}={\mathbf{A}}^{N_p}{\mathbf{x}}_k+\sum _{i=0}^{N_p-1}{\mathbf{A}}^i\mathbf{B}{\mathbf{u}}_{k+i} \end{gathered}$$

• ${\mathbf{x}}_{k+j|k}$：基于当前 $k$ 时刻信息对 $k+j$ 时刻的预测状态
• ${\mathbf{u}}_{k+i}$：未来 $N_u$ 步的控制输入（称为控制时域，$N_u\le N_p$）

2. 输出预测序列：
   $${\mathbf{y}}_{k+j|k}=\mathbf{C}{\mathbf{x}}_{k+j|k}+\mathbf{D}{\mathbf{u}}_{k+j}$$

将上述预测方程向量化，得到矩阵形式：
$$\underset{{\mathbf{X}}_k}{\underbrace{\left[\begin{array}{c}{\mathbf{x}}_{k+1|k}\\ {\mathbf{x}}_{k+2|k}\\ ⋮\\ {\mathbf{x}}_{k+N_p|k}\end{array}\right]}}=\underset{\mathbf{\Phi }}{\underbrace{\left[\begin{array}{c}\mathbf{A}\\ {\mathbf{A}}^2\\ ⋮\\ {\mathbf{A}}^{N_p}\end{array}\right]}}{\mathbf{x}}_k+\underset{\mathbf{\Psi }}{\underbrace{\left[\begin{array}{cccc}\mathbf{B}& 0& \cdots & 0\\ \mathbf{AB}& \mathbf{B}& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\mathbf{A}}^{N_p-1}\mathbf{B}& {\mathbf{A}}^{N_p-2}\mathbf{B}& \cdots & \mathbf{B}\end{array}\right]}}\underset{{\mathbf{U}}_k}{\underbrace{\left[\begin{array}{c}{\mathbf{u}}_k\\ {\mathbf{u}}_{k+1}\\ ⋮\\ {\mathbf{u}}_{k+N_u-1}\end{array}\right]}}$$

• $\mathbf{\Phi }\in {\mathbb{R}}^{N_{p}n\times n}$：状态预测基矩阵
• $\mathbf{\Psi }\in {\mathbb{R}}^{N_{p}n\times N_{u}m}$：输入影响矩阵

三、优化问题：二次型目标函数与约束

MPC的核心是在每个时刻 $k$ 求解以下优化问题：
$$\underset{{\mathbf{U}}_k}{\min }J=\sum _{j=1}^{N_p}{\left({\mathbf{y}}_{k+j|k}-{\mathbf{r}}_{k+j}\right)}^T\mathbf{Q}\left({\mathbf{y}}_{k+j|k}-{\mathbf{r}}_{k+j}\right)+\sum _{j=0}^{N_u-1}{\mathbf{u}}_{k+j}^T\mathbf{R}{\mathbf{u}}_{k+j}$$
$$\text{s.t.}{\mathbf{u}}_{\text{min}}\le {\mathbf{u}}_{k+j}\le {\mathbf{u}}_{\text{max}},j=0,1,\dots ,N_u-1$$

• ${\mathbf{r}}_{k+j}$：参考轨迹
• $\mathbf{Q}\in {\mathbb{R}}^{p\times p}$：输出误差权重矩阵（半正定）
• $\mathbf{R}\in {\mathbb{R}}^{m\times m}$：输入权重矩阵（正定）
• 约束条件：输入的上下限

将目标函数转换为矩阵形式：
$$J=({\mathbf{Y}}_k-{\mathbf{R}}_k{)}^T{\mathbf{Q}}_f({\mathbf{Y}}_k-{\mathbf{R}}_k)+{\mathbf{U}}_k^T{\mathbf{R}}_f{\mathbf{U}}_k$$

• ${\mathbf{Y}}_k={\mathbf{C}}_f{\mathbf{X}}_k+{\mathbf{D}}_f{\mathbf{U}}_k$
• ${\mathbf{C}}_f=\text{blockdiag}(\mathbf{C},\mathbf{C},\dots ,\mathbf{C})\in {\mathbb{R}}^{N_{p}p\times N_{p}n}$
• ${\mathbf{D}}_f=\text{blockdiag}(\mathbf{D},\mathbf{D},\dots ,\mathbf{D})\in {\mathbb{R}}^{N_{p}p\times N_{u}m}$
• ${\mathbf{Q}}_f=\text{blockdiag}(\mathbf{Q},\mathbf{Q},\dots ,\mathbf{Q})\in {\mathbb{R}}^{N_{p}p\times N_{p}p}$
• ${\mathbf{R}}_f=\text{blockdiag}(\mathbf{R},\mathbf{R},\dots ,\mathbf{R})\in {\mathbb{R}}^{N_{u}m\times N_{u}m}$

四、最优控制序列求解：二次规划（QP）

将目标函数代入预测模型，得到：
$$J={\left({\mathbf{C}}_f\mathbf{\Phi }{\mathbf{x}}_k+{\mathbf{C}}_f\mathbf{\Psi }{\mathbf{U}}_k+{\mathbf{D}}_f{\mathbf{U}}_k-{\mathbf{R}}_k\right)}^T{\mathbf{Q}}_f\left({\mathbf{C}}_f\mathbf{\Phi }{\mathbf{x}}_k+{\mathbf{C}}_f\mathbf{\Psi }{\mathbf{U}}_k+{\mathbf{D}}_f{\mathbf{U}}_k-{\mathbf{R}}_k\right)+{\mathbf{U}}_k^T{\mathbf{R}}_f{\mathbf{U}}_k$$
展开后为关于 ${\mathbf{U}}_k$ 的二次型函数：
$$J={\mathbf{U}}_k^T\mathbf{H}{\mathbf{U}}_k+2{\mathbf{U}}_k^T\mathbf{G}+\text{常数项}$$
其中：
$$\mathbf{H}=({\mathbf{C}}_f\mathbf{\Psi }+{\mathbf{D}}_f{)}^T{\mathbf{Q}}_f({\mathbf{C}}_f\mathbf{\Psi }+{\mathbf{D}}_f)+{\mathbf{R}}_f$$
$$\mathbf{G}=({\mathbf{C}}_f\mathbf{\Phi }{\mathbf{x}}_k-{\mathbf{R}}_k{)}^T{\mathbf{Q}}_f({\mathbf{C}}_f\mathbf{\Psi }+{\mathbf{D}}_f)$$

求解此二次规划问题，得到最优控制序列：
$${\mathbf{U}}_k^{\ast }={\mathbf{H}}^{-1}{\mathbf{G}}^T$$
仅将第一个控制量 ${\mathbf{u}}_k^{\ast }$ 作用于系统，下一时刻重复此过程（滚动优化）。

五、反馈校正：处理模型误差与扰动

实际系统中存在模型误差和外部扰动，因此需要引入反馈校正：
$${\mathbf{x}}_{k+1}=\mathbf{A}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k+{\mathbf{w}}_k$$
$${\mathbf{y}}_k=\mathbf{C}{\mathbf{x}}_k+\mathbf{D}{\mathbf{u}}_k+{\mathbf{v}}_k$$

• ${\mathbf{w}}_k$：过程噪声
• ${\mathbf{v}}_k$：测量噪声

通过实时测量的 ${\mathbf{y}}_k$ 计算预测误差：
$${\mathbf{e}}_k={\mathbf{y}}_k-{\mathbf{y}}_{k|k-1}$$
将误差反馈到预测模型中，更新未来预测：
$${\mathbf{x}}_{k+1|k}=\mathbf{A}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k+\mathbf{L}{\mathbf{e}}_k$$

• $\mathbf{L}$：反馈增益矩阵（通常通过卡尔曼滤波或LQR设计）

六、约束处理与可行性分析

MPC的优势在于能够显式处理输入、状态和输出约束。通过将约束转化为线性不等式：
$$\begin{gathered} {\mathbf{u}}_{\text{min}}\le {\mathbf{U}}_k\le {\mathbf{u}}_{\text{max}} \\ {\mathbf{x}}_{\text{min}}\le {\mathbf{X}}_k\le {\mathbf{x}}_{\text{max}} \\ {\mathbf{y}}_{\text{min}}\le {\mathbf{Y}}_k\le {\mathbf{y}}_{\text{max}} \end{gathered}$$
这些约束可直接嵌入二次规划问题中求解。若优化问题不可行，需调整预测时域或引入松弛变量。

好的，我将用一个自动泊车的简单例子解释模型预测控制（MPC）的核心原理。

案例场景

假设你要让一辆智能汽车从当前位置（A点）水平泊入目标车位（B点），车身姿态需与车位对齐。使用MPC实现这一过程：

1. 建立系统模型

• 模型简化：假设汽车为单车模型，控制输入为方向盘转角（$\delta$）和加速度（$a$）。
• 状态变量：位置$(x,y)$、航向角$\theta$、速度$v$。
• 运动学方程（离散化后）：
  $$\{\begin{array}{l}{x}_{k+1}=x_k+v_k\cos {\theta }_k\cdot \Delta t\\ y_{k+1}=y_k+v_k\sin {\theta }_k\cdot \Delta t\\ {\theta }_{k+1}={\theta }_k+\frac{v_k}{L}\tan {\delta }_k\cdot \Delta t\\ v_{k+1}=v_k+a_k\cdot \Delta t\end{array}$$
  （$L$为轴距，$\Delta t$为采样时间）

2. 预测未来行为

• 预测时域：假设未来5秒的轨迹（$N=5$步）。
• 初始状态：当前位置$(x_0,y_0)$、航向角${\theta }_0$、速度$v_0=0$。
• 预测过程：
  1. 假设控制输入序列 { δ 0 , a 0 , δ 1 , a 1 , . . . , δ 4 , a 4 } \{\delta_0, a_0, \delta_1, a_1, ..., \delta_4, a_4\} {δ0​,a0​,δ1​,a1​,...,δ4​,a4​}。
  2. 用模型递推计算未来5步的状态 { x 1 , y 1 , . . . , x 5 , y 5 } \{x_1, y_1, ..., x_5, y_5\} {x1​,y1​,...,x5​,y5​}。

3. 优化控制输入

• 目标函数：最小化与目标位置的偏差，同时限制控制量变化：
  $$J=\sum _{k=1}^N\left[(x_k-x_B{)}^2+(y_k-y_B{)}^2\right]+\lambda \sum _{k=0}^{N-1}\left({\delta }_k^2+a_k^2\right)$$
  （$\lambda$为权重系数，平衡轨迹精度与控制平滑性）
• 约束条件：
  • 方向盘转角范围：${\delta }_{\text{min}}\le {\delta }_k\le {\delta }_{\text{max}}$
  • 加速度范围：$a_{\text{min}}\le a_k\le a_{\text{max}}$
• 求解优化：使用二次规划（QP）或其他方法，得到最优控制序列 { δ 0 ∗ , a 0 ∗ , δ 1 ∗ , . . . } \{\delta_0^*, a_0^*, \delta_1^*, ...\} {δ0∗​,a0∗​,δ1∗​,...}。

4. 滚动执行与更新

• 执行第一步：仅实施第一个控制量$({\delta }_0^{\ast },a_0^{\ast })$，驱动汽车移动。
• 重新测量：获取新的位置$(x_1^{\prime },y_1^{\prime })$、航向角${\theta }_1^{\prime }$、速度$v_1^{\prime }$。
• 更新预测：以新状态为起点，重复步骤2-4，重新优化未来5秒的轨迹。

关键优势

1. 提前规划：考虑未来多步的影响，避免局部最优（如PID可能震荡）。
2. 处理约束：直接限制方向盘转角和加速度，符合物理可行性。
3. 动态调整：实时更新状态，适应突发障碍或模型误差。

与传统控制对比

方法	泊车路径规划方式	应对突发情况能力
MPC	每一步重新计算全局最优路径	强
PID	根据当前误差调整方向和速度	弱（可能撞线）

通过这个例子，可以直观理解MPC的核心逻辑：预测-优化-执行-更新，类似于人类驾驶员不断预判下一步动作并动态调整策略。