第9章：模型预测控制基础

模型预测控制（Model Predictive Control, MPC）已成为现代机器人控制的基石技术。不同于传统的反馈控制器，MPC通过在线求解有限时域优化问题，能够系统性地处理约束、预见未来行为，并实现多目标优化。本章将深入探讨MPC的核心概念，从问题表述到稳定性保证，并通过自动驾驶车辆的案例展示其实际应用。

9.1 MPC问题表述与求解

9.1.1 预测模型与状态预测

MPC的核心是基于系统模型对未来状态进行预测。考虑离散时间系统：

$$\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k)$$ 其中 $\mathbf{x}_k \in \mathbb{R}^n$ 为状态向量，$\mathbf{u}_k \in \mathbb{R}^m$ 为控制输入。

对于预测时域 $N$，我们构建状态预测序列： $$\mathbf{x}_{k+i|k} = f(\mathbf{x}_{k+i-1|k}, \mathbf{u}_{k+i-1|k}), \quad i = 1, \ldots, N$$ 其中 $\mathbf{x}_{k+i|k}$ 表示在时刻 $k$ 对时刻 $k+i$ 状态的预测。

时刻 k:
    |
    v
[x_k] --u_k--> [x_{k+1|k}] --u_{k+1|k}--> ... --u_{k+N-1|k}--> [x_{k+N|k}]
    ^                                                               ^
    |                                                               |
  当前状态                                                      终端状态

模型类型与选择：

在机器人控制中，常见的预测模型包括：

1. 线性时不变（LTI）模型： $$\mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k$$ 适用于工作点附近的小信号控制，计算效率高，但精度受限于线性化误差。

2. 线性时变（LTV）模型： $$\mathbf{x}_{k+1} = \mathbf{A}_k\mathbf{x}_k + \mathbf{B}_k\mathbf{u}_k$$ 通过沿参考轨迹线性化获得，能够处理大范围运动，是轨迹跟踪MPC的标准选择。

3. 非线性模型： 保留系统的完整非线性动态，精度最高但计算代价大。对于机器人系统，通常包含：

• 刚体动力学：$\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}$
• 接触动力学：互补性约束、摩擦锥约束
• 执行器动力学：电机模型、液压系统模型

离散化方法：

连续时间模型 $\dot{\mathbf{x}} = f_c(\mathbf{x}, \mathbf{u})$ 的离散化直接影响预测精度：

1. 欧拉前向法（一阶精度）： $$\mathbf{x}_{k+1} = \mathbf{x}_k + T_s f_c(\mathbf{x}_k, \mathbf{u}_k)$$ 简单但精度低，需要小采样时间。

2. 零阶保持（ZOH）精确离散化： 对线性系统 $\dot{\mathbf{x}} = \mathbf{A}_c\mathbf{x} + \mathbf{B}_c\mathbf{u}$： $$\mathbf{A} = e^{\mathbf{A}_c T_s}, \quad \mathbf{B} = \int_0^{T_s} e^{\mathbf{A}_c \tau} d\tau \cdot \mathbf{B}_c$$ 精确但仅适用于线性系统。

3. Runge-Kutta方法（高阶精度）： RK4提供四阶精度，是非线性MPC的常用选择： $$\mathbf{x}_{k+1} = \mathbf{x}_k + \frac{T_s}{6}(\mathbf{k}_1 + 2\mathbf{k}_2 + 2\mathbf{k}_3 + \mathbf{k}_4)$$ 其中 $\mathbf{k}_i$ 为中间导数评估。

预测误差累积：

预测误差随时域增长而累积，主要来源包括：

• 模型简化误差：忽略的动态、未建模摩擦
• 离散化误差：数值积分精度
• 参数不确定性：质量、惯性、摩擦系数变化
• 外部扰动：未测量的外力、地形变化

误差传播分析： $$\mathbf{e}_{k+i|k} = \prod_{j=0}^{i-1}\left(\mathbf{I} + T_s\frac{\partial f}{\partial \mathbf{x}}\bigg|_{j}\right)\mathbf{e}_{k|k} + \sum_{j=0}^{i-1}\boldsymbol{\delta}_{k+j}$$ 其中 $\mathbf{e}$ 为预测误差，$\boldsymbol{\delta}$ 为单步误差。这解释了为什么MPC需要滚动时域策略来不断纠正累积误差。

9.1.2 代价函数设计

MPC的优化目标通常采用二次型代价函数： $$J = \sum_{i=0}^{N-1} \left[ (\mathbf{x}_{k+i|k} - \mathbf{x}_{\text{ref}})^T \mathbf{Q} (\mathbf{x}_{k+i|k} - \mathbf{x}_{\text{ref}}) + \mathbf{u}_{k+i|k}^T \mathbf{R} \mathbf{u}_{k+i|k} \right] + (\mathbf{x}_{k+N|k} - \mathbf{x}_{\text{ref}})^T \mathbf{P} (\mathbf{x}_{k+N|k} - \mathbf{x}_{\text{ref}})$$ 其中：

• $\mathbf{Q} \succeq 0$ 为状态权重矩阵
• $\mathbf{R} \succ 0$ 为控制权重矩阵
• $\mathbf{P} \succeq 0$ 为终端代价矩阵
• $\mathbf{x}_{\text{ref}}$ 为参考状态

权重矩阵的选择直接影响控制性能：

• 较大的 $\mathbf{Q}$ 使系统快速跟踪参考轨迹
• 较大的 $\mathbf{R}$ 减少控制能量消耗
• 终端代价 $\mathbf{P}$ 对稳定性至关重要

扩展代价函数形式：

实际机器人控制中，常需要更复杂的代价函数：

1. 增量控制代价（减少控制抖动）： $$J_{\Delta u} = \sum_{i=0}^{N-2} (\mathbf{u}_{k+i+1|k} - \mathbf{u}_{k+i|k})^T \mathbf{R}_{\Delta} (\mathbf{u}_{k+i+1|k} - \mathbf{u}_{k+i|k})$$ 这在机械系统中特别重要，可以减少执行器磨损和能量消耗。

2. 输出跟踪代价： 当只关心特定输出 $\mathbf{y} = \mathbf{C}\mathbf{x}$ 时： $$J_y = \sum_{i=0}^{N-1} (\mathbf{y}_{k+i|k} - \mathbf{y}_{\text{ref}})^T \mathbf{Q}_y (\mathbf{y}_{k+i|k} - \mathbf{y}_{\text{ref}})$$ 例如，只跟踪末端执行器位置而不约束关节角度。

3. 经济代价函数： 直接优化经济指标而非跟踪误差： $$J_{\text{econ}} = \sum_{i=0}^{N-1} c(\mathbf{x}_{k+i|k}, \mathbf{u}_{k+i|k})$$ 其中 $c(\cdot)$ 可能表示能量消耗、生产率等。注意经济MPC可能不存在稳态工作点。

4. 鲁棒代价（最坏情况优化）： $$J_{\text{robust}} = \max_{\mathbf{w} \in \mathcal{W}} J(\mathbf{x}, \mathbf{u}, \mathbf{w})$$ 其中 $\mathbf{w}$ 为有界扰动，$\mathcal{W}$ 为不确定性集合。

权重选择策略：

1. 基于物理尺度归一化： $$q_{ii} = \frac{1}{\sigma_i^2}, \quad r_{jj} = \frac{1}{\rho_j^2}$$ 其中 $\sigma_i$ 为状态 $i$ 的可接受偏差，$\rho_j$ 为控制 $j$ 的典型范围。

2. 基于频域分析： 通过期望的闭环带宽 $\omega_b$ 设计： $$\frac{\mathbf{Q}}{\mathbf{R}} \approx \omega_b^2$$

3. LQR反向设计： 从期望的闭环极点出发，通过逆Riccati方程求解权重。

4. 多目标帕累托优化： 系统地探索不同权重组合，生成帕累托前沿：

性能
 ^
 |  o  <- 激进控制
 | o o
 |o o o  <- 帕累托前沿  
 |o o o o
 +--------> 能量消耗
      ^ 保守控制

时变权重策略：

根据任务阶段调整权重可以改善性能： $$\mathbf{Q}_i = \begin{cases} \mathbf{Q}_{\text{approach}} & \text{if } |\mathbf{e}| > \epsilon_{\text{far}} \\ \mathbf{Q}_{\text{track}} & \text{if } \epsilon_{\text{near}} < |\mathbf{e}| \leq \epsilon_{\text{far}} \\ \mathbf{Q}_{\text{precise}} & \text{if } |\mathbf{e}| \leq \epsilon_{\text{near}} \end{cases}$$ 这允许系统在远离目标时快速接近，在接近目标时精确控制。

9.1.3 约束处理

MPC的一大优势是能够显式处理各种约束：

状态约束： $$\mathbf{x}_{\min} \leq \mathbf{x}_{k+i|k} \leq \mathbf{x}_{\max}, \quad i = 1, \ldots, N$$ 输入约束： $$\mathbf{u}_{\min} \leq \mathbf{u}_{k+i|k} \leq \mathbf{u}_{\max}, \quad i = 0, \ldots, N-1$$ 输入变化率约束： $$\Delta\mathbf{u}_{\min} \leq \mathbf{u}_{k+i|k} - \mathbf{u}_{k+i-1|k} \leq \Delta\mathbf{u}_{\max}$$ 混合约束： $$\mathbf{G}\mathbf{x}_{k+i|k} + \mathbf{H}\mathbf{u}_{k+i|k} \leq \mathbf{g}$$ 机器人特定约束：

1. 关节限位与速度限制： $$\mathbf{q}_{\min} \leq \mathbf{q} \leq \mathbf{q}_{\max}, \quad \dot{\mathbf{q}}_{\min} \leq \dot{\mathbf{q}} \leq \dot{\mathbf{q}}_{\max}$$ 保护机械结构，避免碰撞和过载。

2. 扭矩/力矩约束： $$\boldsymbol{\tau}_{\min} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{\max}$$ 反映执行器能力限制。对于冗余机器人，还需考虑零空间投影。

3. 工作空间约束： 末端执行器位置约束： $$\mathbf{p}_{\text{ee}} = \mathbf{f}_{\text{kin}}(\mathbf{q}) \in \mathcal{W}_{\text{safe}}$$ 其中 $\mathcal{W}_{\text{safe}}$ 为安全工作空间。

4. 接触力约束（摩擦锥）： $$\mathbf{f}_{\text{contact}} \in \mathcal{FC}(\mu) = \{\mathbf{f} : f_z \geq 0, \sqrt{f_x^2 + f_y^2} \leq \mu f_z\}$$ 确保接触稳定，防止滑动。

5. ZMP约束（双足机器人）： $$\mathbf{p}_{\text{ZMP}} \in \mathcal{P}_{\text{support}}$$ 零力矩点必须在支撑多边形内以保持平衡。

6. 碰撞避免约束： $$d(\mathbf{p}_i, \mathcal{O}_j) \geq d_{\text{safe}}, \quad \forall i \in \text{links}, j \in \text{obstacles}$$ 其中 $d(\cdot)$ 为距离函数，可用有符号距离场（SDF）高效计算。

约束优先级与软化：

实际系统中，并非所有约束都同等重要：

1. 硬约束（必须满足）： - 关节限位（物理极限） - 碰撞避免（安全） - 执行器饱和（物理能力）

2. 软约束（可以违反但有惩罚）： - 舒适性约束（加速度限制） - 能效约束 - 路径偏差

软约束实现： $$\min J + \sum_{i} \rho_i |\mathbf{s}_i|_p$$ $$\text{s.t.} \quad \mathbf{g}_i(\mathbf{x}, \mathbf{u}) \leq \mathbf{s}_i, \quad \mathbf{s}_i \geq 0$$ 其中 $\mathbf{s}_i$ 为松弛变量，$\rho_i$ 为惩罚权重，$p \in \{1, 2, \infty\}$ 决定惩罚类型。

非凸约束处理：

机器人系统常遇到非凸约束：

1. 障碍物避免（非凸）： 原始形式：$|\mathbf{p} - \mathbf{p}_{\text{obs}}| \geq r_{\text{safe}}$

线性化近似（在当前点 $\mathbf{p}_0$）： $$\mathbf{n}^T(\mathbf{p} - \mathbf{p}_{\text{obs}}) \geq r_{\text{safe}}$$ 其中 $\mathbf{n} = (\mathbf{p}_0 - \mathbf{p}_{\text{obs}})/|\mathbf{p}_0 - \mathbf{p}_{\text{obs}}|$。

2. 混合整数表示： 使用二进制变量 $b_i \in \{0, 1\}$ 选择约束： $$\mathbf{g}_i(\mathbf{x}) \leq M(1 - b_i), \quad \sum_i b_i = 1$$ 其中 $M$ 为大数。这将问题转换为混合整数规划（MIP）。

3. 序列凸优化（SCvx）： 迭代线性化非凸约束，添加信任域： $$|\mathbf{x} - \mathbf{x}^{(k)}| \leq \delta^{(k)}$$ 随迭代收缩信任域半径 $\delta$。

约束激活与去激活：

约束激活模式：
时间 →
约束1: ----[激活]----[非激活]----
约束2: [非激活]----[激活]----[激活]
约束3: ----[非激活]----[激活]----
        ↑            ↑            ↑
      自由运动   避障模式   接触模式

不同模式下的约束集合变化需要平滑过渡，避免控制跳变。

9.1.4 滚动时域原理

MPC采用滚动时域（Receding Horizon）策略：

1. 在每个采样时刻 $k$，测量当前状态 $\mathbf{x}_k$

2. 求解有限时域优化问题： $$\min_{\mathbf{U}_k} J(\mathbf{x}_k, \mathbf{U}_k) \quad \text{s.t.} \quad \text{约束条件}$$ 其中 $\mathbf{U}_k = [\mathbf{u}_{k|k}, \mathbf{u}_{k+1|k}, \ldots, \mathbf{u}_{k+N-1|k}]$

3. 应用优化序列的第一个控制输入：$\mathbf{u}_k = \mathbf{u}_{k|k}^*$

4. 时域向前滚动，重复步骤1-3

时刻 k:   [========预测时域========]
          u_k^*  u_{k+1}  ...  u_{k+N-1}
             ↓
时刻 k+1:      [========预测时域========]
               u_{k+1}^*  u_{k+2}  ...  u_{k+N}

这种策略提供了反馈机制，能够补偿模型误差和外部扰动。

滚动时域的本质优势：

1. 隐式反馈控制： 虽然MPC求解开环优化问题，但通过不断更新初始条件并重新优化，实现了闭环控制： $$\mathbf{u}_k = \kappa_{\text{MPC}}(\mathbf{x}_k; \mathbf{x}_{\text{ref}}, \mathcal{C})$$ 其中 $\kappa_{\text{MPC}}$ 是隐式定义的非线性反馈律。

2. 扰动抑制： 考虑带扰动的系统 $\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) + \mathbf{w}_k$：

• 传统开环：扰动累积导致偏差增大
• MPC滚动时域：每步测量实际状态，自动补偿累积误差

扰动抑制能力取决于重新规划频率与系统动态的时间尺度匹配。

3. 时变系统适应： 对于时变参考轨迹或约束，MPC自然地在每个时刻考虑最新信息： $$J_k = \sum_{i=0}^{N-1} l(\mathbf{x}_{k+i|k}, \mathbf{u}_{k+i|k}, \mathbf{r}_{k+i|k})$$ 其中 $\mathbf{r}_{k+i|k}$ 是在时刻 $k$ 已知的未来参考轨迹。

预测时域长度选择：

预测时域 $N$ 的选择至关重要：

1. 覆盖主要动态： $$N \cdot T_s \geq (2-3) \cdot T_{\text{settling}}$$ 其中 $T_{\text{settling}}$ 是系统调节时间。

2. 考虑约束激活： 时域应足够长以"看到"约束的影响：

• 速度约束：需要看到加速和减速过程
• 障碍物：需要看到绕行路径
• 终端约束：需要足够时间到达目标

3. 计算复杂度权衡： - 线性MPC：$O(N^3)$ 用于QP求解 - 非线性MPC：$O(N \cdot n_{\text{iter}})$ 用于NLP迭代 - 实时约束：$N \cdot T_s$ 计算时间 < $T_s$ 采样时间

4. 变时域策略：

近期密集，远期稀疏：
[Δt][Δt][Δt][2Δt][2Δt][4Δt][4Δt]
└─近期控制─┘└─中期预测─┘└远期趋势┘

这种策略在保持计算效率的同时增加预见性。

控制时域与预测时域：

引入控制时域 $M \leq N$ 可以降低优化变量维度： $$\mathbf{u}_{k+i|k} = \begin{cases} \text{优化变量} & i = 0, \ldots, M-1 \\ \mathbf{u}_{k+M-1|k} & i = M, \ldots, N-1 \quad \text{（保持常值）} \end{cases}$$ 或使用终端控制律： $$\mathbf{u}_{k+i|k} = \mathbf{K}(\mathbf{x}_{k+i|k} - \mathbf{x}_{\text{ref}}), \quad i \geq M$$ 这减少了优化变量从 $m \times N$ 到 $m \times M$，显著降低计算负担。

初始猜测与热启动：

良好的初始猜测加速收敛：

1. 平移策略（最常用）： $$\mathbf{U}_k^{(0)} = [\mathbf{u}_{k+1|k-1}^*, \ldots, \mathbf{u}_{k+N-1|k-1}^*, \mathbf{u}_{k+N-1|k-1}^*]$$ 使用上一时刻的解平移一步。

2. 轨迹库： 根据当前状态从预计算的轨迹库中选择最接近的作为初始猜测。

3. 学习预测： 使用神经网络预测良好初始解： $$\mathbf{U}_k^{(0)} = \mathcal{NN}(\mathbf{x}_k, \mathbf{x}_{\text{ref}})$$

9.2 线性MPC与QP求解器

9.2.1 线性化与二次规划转换

对于线性系统或线性化后的系统： $$\mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k$$ 可以将预测方程写成紧凑形式： $$\mathbf{X} = \mathbf{\Phi}\mathbf{x}_k + \mathbf{\Gamma}\mathbf{U}$$ 其中： $$\mathbf{X} = \begin{bmatrix} \mathbf{x}_{k+1|k} \\ \mathbf{x}_{k+2|k} \\ \vdots \\ \mathbf{x}_{k+N|k} \end{bmatrix}, \quad \mathbf{U} = \begin{bmatrix} \mathbf{u}_{k|k} \\ \mathbf{u}_{k+1|k} \\ \vdots \\ \mathbf{u}_{k+N-1|k} \end{bmatrix}$$

$$\mathbf{\Phi} = \begin{bmatrix} \mathbf{A} \\ \mathbf{A}^2 \\ \vdots \\ \mathbf{A}^N \end{bmatrix}, \quad \mathbf{\Gamma} = \begin{bmatrix} \mathbf{B} & \mathbf{0} & \cdots & \mathbf{0} \\ \mathbf{AB} & \mathbf{B} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{A}^{N-1}\mathbf{B} & \mathbf{A}^{N-2}\mathbf{B} & \cdots & \mathbf{B} \end{bmatrix}$$ 代价函数转换为标准QP形式： $$J = \frac{1}{2}\mathbf{U}^T\mathbf{H}\mathbf{U} + \mathbf{f}^T\mathbf{U} + c$$ 其中：

• $\mathbf{H} = 2(\mathbf{\Gamma}^T\bar{\mathbf{Q}}\mathbf{\Gamma} + \bar{\mathbf{R}})$
• $\mathbf{f} = 2\mathbf{\Gamma}^T\bar{\mathbf{Q}}(\mathbf{\Phi}\mathbf{x}_k - \mathbf{X}_{\text{ref}})$
• $\bar{\mathbf{Q}} = \text{blkdiag}(\mathbf{Q}, \ldots, \mathbf{Q}, \mathbf{P})$
• $\bar{\mathbf{R}} = \text{blkdiag}(\mathbf{R}, \ldots, \mathbf{R})$

稠密vs稀疏形式：

上述为稠密形式，消除了状态变量。稀疏形式保留状态作为优化变量： $$\min_{\mathbf{X}, \mathbf{U}} \sum_{i=0}^{N-1} [\mathbf{x}_{k+i|k}^T\mathbf{Q}\mathbf{x}_{k+i|k} + \mathbf{u}_{k+i|k}^T\mathbf{R}\mathbf{u}_{k+i|k}] + \mathbf{x}_{k+N|k}^T\mathbf{P}\mathbf{x}_{k+N|k}$$ $$\text{s.t.} \quad \mathbf{x}_{k+i+1|k} = \mathbf{A}\mathbf{x}_{k+i|k} + \mathbf{B}\mathbf{u}_{k+i|k}$$ 稀疏形式的优势：

• 保持问题结构，利于专用求解器
• 矩阵带状稀疏，内存效率高
• 易于处理状态约束
• 适合长时域问题

线性时变（LTV）系统处理：

对于沿轨迹线性化的系统： $$\mathbf{x}_{k+1} = \mathbf{A}_k\mathbf{x}_k + \mathbf{B}_k\mathbf{u}_k + \mathbf{c}_k$$ 其中 $\mathbf{c}_k$ 为线性化偏移项。预测矩阵变为： $$\mathbf{\Phi} = \begin{bmatrix} \mathbf{A}_0 \\ \mathbf{A}_1\mathbf{A}_0 \\ \vdots \\ \prod_{i=0}^{N-1}\mathbf{A}_i \end{bmatrix}, \quad \mathbf{\Gamma} = \begin{bmatrix} \mathbf{B}_0 & 0 & \cdots \\ \mathbf{A}_1\mathbf{B}_0 & \mathbf{B}_1 & \cdots \\ \vdots & \vdots & \ddots \end{bmatrix}$$ 需要在每个MPC周期重新计算这些矩阵，计算复杂度 $O(N^2n^2m)$。

约束的矩阵表示：

将所有约束整理为统一形式： $$\mathbf{A}_{\text{ineq}}\mathbf{U} \leq \mathbf{b}_{\text{ineq}}$$ 包含：

1. 输入约束：$\mathbf{u}_{\min} \leq \mathbf{u}_i \leq \mathbf{u}_{\max}$
2. 状态约束（通过预测方程消除）：$\mathbf{x}_{\min} \leq \mathbf{\Gamma}\mathbf{U} + \mathbf{\Phi}\mathbf{x}_0 \leq \mathbf{x}_{\max}$
3. 输入变化率：$\Delta\mathbf{u}_{\min} \leq \mathbf{D}\mathbf{U} \leq \Delta\mathbf{u}_{\max}$

其中差分矩阵： $$\mathbf{D} = \begin{bmatrix} \mathbf{I} & 0 & \cdots & 0 \\ -\mathbf{I} & \mathbf{I} & \cdots & 0 \\ \vdots & \ddots & \ddots & \vdots \\ 0 & \cdots & -\mathbf{I} & \mathbf{I} \end{bmatrix}$$ 数值调理与缩放：

QP问题的数值性质影响求解效率：

1. Hessian矩阵条件数： $$\kappa(\mathbf{H}) = \frac{\lambda_{\max}(\mathbf{H})}{\lambda_{\min}(\mathbf{H})}$$ 目标：$\kappa < 10^6$，否则需要预调理。

2. 变量缩放： $$\tilde{\mathbf{u}} = \mathbf{S}_u\mathbf{u}, \quad \tilde{\mathbf{x}} = \mathbf{S}_x\mathbf{x}$$ 选择 $\mathbf{S}_u, \mathbf{S}_x$ 使变量在 $[-1, 1]$ 范围。

3. 约束归一化： 将约束除以其典型值，使所有约束具有相似的数量级。

4. Cholesky因子化预计算： 对于固定的 $\mathbf{H}$，预计算 $\mathbf{H} = \mathbf{L}\mathbf{L}^T$，加速求解。

9.2.2 主动集法与内点法

主动集法（Active Set Method）：

主动集法通过迭代识别活跃约束来求解QP：

1. 初始化工作集 $\mathcal{W}_0$（活跃约束集合）
2. 求解等式约束子问题
3. 检查KKT条件： - 如果满足，得到最优解 - 否则，更新工作集：添加违反约束或移除非活跃约束
4. 重复步骤2-3

优点：

• 热启动效果好（利用上一时刻的活跃集）
• 对小规模问题效率高

缺点：

• 最坏情况下迭代次数指数增长
• 对大规模问题效率低

内点法（Interior Point Method）：

内点法通过障碍函数将约束优化转换为无约束优化： $$\min_{\mathbf{U}} J(\mathbf{U}) - \mu \sum_i \log(s_i)$$ 其中 $s_i$ 为松弛变量，$\mu$ 为障碍参数。

通过牛顿法求解KKT系统： $$\begin{bmatrix} \mathbf{H} & \mathbf{A}^T \\ \mathbf{A} & -\mathbf{S}\mathbf{Z}^{-1} \end{bmatrix} \begin{bmatrix} \Delta\mathbf{U} \\ \Delta\mathbf{\lambda} \end{bmatrix} = - \begin{bmatrix} \nabla J + \mathbf{A}^T\mathbf{\lambda} \\ \mathbf{A}\mathbf{U} - \mathbf{b} + \mathbf{s} \end{bmatrix}$$ 优点：

• 多项式时间复杂度
• 对大规模问题效率高

缺点：

• 每次迭代计算量大
• 热启动效果不如主动集法

9.2.3 快速QP求解器

OSQP（Operator Splitting Quadratic Program）：

OSQP基于交替方向乘子法（ADMM），特别适合嵌入式系统：

算法流程：

1. 将QP转换为标准形式
2. 应用ADMM迭代： $$\mathbf{x}^{k+1} = (\mathbf{P} + \sigma\mathbf{I})^{-1}(\mathbf{q} + \sigma(\mathbf{z}^k - \mathbf{y}^k))$$ $$\mathbf{z}^{k+1} = \Pi_{\mathcal{C}}(\mathbf{A}\mathbf{x}^{k+1} + \mathbf{y}^k)$$ $$\mathbf{y}^{k+1} = \mathbf{y}^k + \mathbf{A}\mathbf{x}^{k+1} - \mathbf{z}^{k+1}$$ 其中 $\Pi_{\mathcal{C}}$ 为投影算子，$\sigma$ 为惩罚参数。

特点：

• 一阶方法，内存需求低
• 支持热启动和参数更新
• 适合实时控制

qpOASES：

qpOASES采用在线主动集策略，专为MPC设计：

核心特性：

• 利用问题结构的热启动
• Hessian矩阵的高效更新
• 参数化QP的快速重解

热启动策略：
时刻 k:   活跃集 W_k → 最优解 U_k*
            ↓ (传递)
时刻 k+1: 初始活跃集 W_{k+1}^0 ≈ W_k → 快速收敛到 U_{k+1}*

求解器选择指南：

| 特性 | OSQP | qpOASES | 内点法求解器 |

9.3 稳定性与递归可行性

9.3.1 终端约束与终端代价

为保证闭环稳定性，MPC通常采用以下策略之一：

策略1：终端等式约束

强制终端状态到达平衡点： $$\mathbf{x}_{k+N|k} = \mathbf{x}_{\text{eq}}$$ 优点：稳定性证明简单 缺点：可行域可能很小

策略2：终端不等式约束

终端状态必须进入终端集 $\mathcal{X}_f$： $$\mathbf{x}_{k+N|k} \in \mathcal{X}_f$$ 其中 $\mathcal{X}_f$ 为正不变集。

策略3：终端代价函数

选择终端代价 $V_f(\mathbf{x})$ 满足：

1. $V_f(\mathbf{A}\mathbf{x} + \mathbf{B}\kappa(\mathbf{x})) - V_f(\mathbf{x}) \leq -l(\mathbf{x}, \kappa(\mathbf{x}))$
2. $V_f(\mathbf{x}) \geq \alpha|\mathbf{x}|^2$

其中 $\kappa(\mathbf{x})$ 为局部控制律，$l(\cdot)$ 为阶段代价。

常用选择：$V_f(\mathbf{x}) = \mathbf{x}^T\mathbf{P}\mathbf{x}$，其中 $\mathbf{P}$ 满足离散Lyapunov方程： $$(\mathbf{A} + \mathbf{B}\mathbf{K})^T\mathbf{P}(\mathbf{A} + \mathbf{B}\mathbf{K}) - \mathbf{P} = -(\mathbf{Q} + \mathbf{K}^T\mathbf{R}\mathbf{K})$$

9.3.2 Lyapunov稳定性分析

定理（MPC稳定性）：

考虑MPC问题，如果：

1. 优化问题在 $k=0$ 时可行
2. 采用合适的终端约束/代价
3. 系统模型准确

则闭环系统渐近稳定，且最优值函数 $V^*(\mathbf{x})$ 是Lyapunov函数。

证明思路：

设 $\mathbf{U}_k^* = [\mathbf{u}_{k|k}^*, \ldots, \mathbf{u}_{k+N-1|k}^*]$ 为时刻 $k$ 的最优解。

构造候选解： $$\tilde{\mathbf{U}}_{k+1} = [\mathbf{u}_{k+1|k}^*, \ldots, \mathbf{u}_{k+N-1|k}^*, \kappa(\mathbf{x}_{k+N|k}^*)]$$ 其中 $\kappa(\cdot)$ 为终端控制律。

由于终端约束设计，$\tilde{\mathbf{U}}_{k+1}$ 可行，且： $$V^*(\mathbf{x}_{k+1}) \leq J(\mathbf{x}_{k+1}, \tilde{\mathbf{U}}_{k+1}) \leq V^*(\mathbf{x}_k) - l(\mathbf{x}_k, \mathbf{u}_k^*)$$ 因此 $V^*$ 单调递减，系统稳定。

9.3.3 不变集理论

最大输出可许集（Maximal Output Admissible Set）：

定义： $$\mathcal{O}_{\infty} = \{\mathbf{x} \in \mathbb{R}^n : \mathbf{A}^i\mathbf{x} \in \mathcal{X}, \forall i \geq 0\}$$ 其中 $\mathcal{X}$ 为状态约束集。

计算算法：

1. 初始化 $\Omega_0 = \mathcal{X}$
2. 迭代：$\Omega_{k+1} = \mathcal{X} \cap \{\mathbf{x} : \mathbf{A}\mathbf{x} \in \Omega_k\}$
3. 当 $\Omega_{k+1} = \Omega_k$ 时，$\mathcal{O}_{\infty} = \Omega_k$

控制不变集（Control Invariant Set）：

集合 $\mathcal{C}$ 是控制不变的，如果： $$\forall \mathbf{x} \in \mathcal{C}, \exists \mathbf{u} \in \mathcal{U} : f(\mathbf{x}, \mathbf{u}) \in \mathcal{C}$$

不变集示意图：
     ╱────────╲
   ╱            ╲
  │   x(t) →    │  ← 轨迹始终在集合内
  │      ↘      │
  │    x(t+1)   │
   ╲            ╱
     ╲────────╱
      不变集 C

递归可行性保证：

如果：

1. 初始状态 $\mathbf{x}_0 \in \mathcal{F}_N$（N步可行域）
2. 终端集 $\mathcal{X}_f$ 是控制不变的
3. $\mathcal{X}_f \subseteq \mathcal{F}_N$

则MPC问题递归可行，即若时刻 $k$ 可行，则时刻 $k+1$ 必可行。

9.4 案例研究：自动驾驶车辆的MPC实现

9.4.1 车辆动力学模型

考虑自动驾驶车辆的运动学自行车模型： $$\begin{aligned} \dot{x} &= v \cos(\psi + \beta) \\ \dot{y} &= v \sin(\psi + \beta) \\ \dot{\psi} &= \frac{v}{l_r} \sin(\beta) \\ \dot{v} &= a \\ \beta &= \arctan\left(\frac{l_r}{l_f + l_r} \tan(\delta)\right) \end{aligned}$$ 其中：

• $(x, y)$：车辆位置
• $\psi$：航向角
• $v$：速度
• $\delta$：前轮转角
• $a$：加速度
• $\beta$：质心侧偏角
• $l_f, l_r$：前后轴距

车辆模型示意图：
        前轮
         ↑ δ
    ┌────┴────┐
    │    ●    │ ← 质心
    │  ↙ β    │
    └─────────┘
        后轮

离散化模型（采样时间 $T_s = 0.1$秒）： $$\mathbf{x}_{k+1} = \mathbf{x}_k + T_s \cdot f(\mathbf{x}_k, \mathbf{u}_k)$$

9.4.2 MPC控制器设计

状态向量： $$\mathbf{x} = [x, y, \psi, v]^T$$ 控制输入： $$\mathbf{u} = [\delta, a]^T$$ 约束条件：

• 转向角限制：$|\delta| \leq 0.5$ rad（约28.6°）
• 加速度限制：$-3 \leq a \leq 2$ m/s²
• 速度限制：$0 \leq v \leq 30$ m/s
• 转向角速率：$|\dot{\delta}| \leq 0.3$ rad/s
• 加加速度：$|\dot{a}| \leq 1.5$ m/s³

代价函数设计： $$J = \sum_{i=0}^{N-1} \left[ w_e e_{i}^2 + w_{\psi} e_{\psi,i}^2 + w_v (v_i - v_{\text{ref}})^2 + w_{\delta} \delta_i^2 + w_a a_i^2 + w_{\dot{\delta}} \dot{\delta}_i^2 + w_{\dot{a}} \dot{a}_i^2 \right]$$ 其中：

• $e_i$：横向偏差（到参考轨迹的距离）
• $e_{\psi,i}$：航向偏差
• 权重选择：$w_e = 100, w_{\psi} = 50, w_v = 10, w_{\delta} = 1, w_a = 1, w_{\dot{\delta}} = 100, w_{\dot{a}} = 10$

9.4.3 实时实现策略

1. 线性化与QP转换：

在每个时刻，围绕参考轨迹线性化： $$\mathbf{A}_k = \frac{\partial f}{\partial \mathbf{x}}\bigg|_{\mathbf{x}_{\text{ref},k}, \mathbf{u}_{\text{ref},k}}, \quad \mathbf{B}_k = \frac{\partial f}{\partial \mathbf{u}}\bigg|_{\mathbf{x}_{\text{ref},k}, \mathbf{u}_{\text{ref},k}}$$

2. 预测时域选择： - 预测时域：$N = 20$（2秒前瞻） - 控制时域：$M = 10$（1秒控制）

3. 求解器配置（OSQP）：

osqp_settings:

  - rho: 0.1          # ADMM步长
  - eps_abs: 1e-3     # 绝对精度
  - eps_rel: 1e-3     # 相对精度
  - max_iter: 100     # 最大迭代
  - warm_start: true  # 热启动
  - adaptive_rho: true # 自适应步长

9.4.4 性能分析

场景1：高速公路车道保持

条件：

• 初始速度：25 m/s
• 曲率半径：500 m
• 路面干燥

性能指标：

• 横向偏差：最大 0.15 m，RMS 0.08 m
• 计算时间：平均 2.3 ms，最大 4.1 ms
• 控制频率：100 Hz

场景2：紧急避障

条件：

• 初始速度：15 m/s
• 障碍物距离：30 m
• 横向偏移：3.5 m

性能指标：

• 避障成功率：100%
• 最大横向加速度：0.6g
• 计算时间：平均 3.8 ms，最大 6.2 ms

9.4.5 实现要点

1. 坐标系转换：

将全局坐标转换到Frenet坐标系：

全局坐标系 (x, y)
     ↓
Frenet坐标系 (s, d)
s: 沿轨迹的纵向距离
d: 横向偏差

2. 参考轨迹生成：

使用五次多项式拟合： $$d(s) = a_0 + a_1s + a_2s^2 + a_3s^3 + a_4s^4 + a_5s^5$$ 边界条件：

• 初始位置、速度、加速度
• 终端位置、速度、加速度

3. 延迟补偿：

考虑执行延迟 $\tau$（典型值20-50ms）： $$\mathbf{x}_{\text{init}} = \mathbf{x}_{\text{current}} + \tau \cdot f(\mathbf{x}_{\text{current}}, \mathbf{u}_{\text{prev}})$$

4. 故障安全机制：

if solver_time > time_limit:
    use_backup_controller()  # PID备用控制器
elif not is_feasible(solution):
    apply_emergency_brake()   # 紧急制动
else:
    apply_mpc_control()       # 正常MPC控制

9.5 本章小结

本章系统介绍了模型预测控制的基础理论与实践：

核心概念：

• MPC通过求解有限时域优化问题实现最优控制
• 滚动时域策略提供反馈机制，补偿扰动和模型误差
• 显式约束处理是MPC的关键优势

关键公式：

1. MPC优化问题： $$\min_{\mathbf{U}} \sum_{i=0}^{N-1} l(\mathbf{x}_{i|k}, \mathbf{u}_{i|k}) + V_f(\mathbf{x}_{N|k})$$ $$\text{s.t.} \quad \mathbf{x}_{i+1|k} = f(\mathbf{x}_{i|k}, \mathbf{u}_{i|k}), \quad \text{约束条件}$$

2. 线性MPC的QP形式： $$\min_{\mathbf{U}} \frac{1}{2}\mathbf{U}^T\mathbf{H}\mathbf{U} + \mathbf{f}^T\mathbf{U} \quad \text{s.t.} \quad \mathbf{A}_{\text{ineq}}\mathbf{U} \leq \mathbf{b}_{\text{ineq}}$$

3. 稳定性条件： - 终端代价满足Lyapunov递减条件 - 终端集为控制不变集 - 优化问题递归可行

实现要点：

• QP求解器选择：OSQP（嵌入式）、qpOASES（快速MPC）
• 热启动策略显著提升实时性能
• 延迟补偿和故障安全机制确保鲁棒性

9.6 常见陷阱与错误

陷阱1：预测时域选择不当

错误表现：

• 时域过短：无法预见关键约束，导致不可行
• 时域过长：计算负担大，但性能提升有限

解决方案：

• 经验法则：预测时域覆盖系统主要动态（2-3倍时间常数）
• 使用变采样率：近期密集，远期稀疏

陷阱2：权重矩阵调试困难

错误表现：

• 振荡：$\mathbf{Q}$过大，$\mathbf{R}$过小
• 响应慢：$\mathbf{R}$过大，$\mathbf{Q}$过小
• 稳态误差：缺少积分作用

调试技巧：

1. 从对角矩阵开始：$\mathbf{Q} = \text{diag}(q_1, q_2, \ldots)$
2. 归一化权重：$q_i = 1/\sigma_i^2$，其中$\sigma_i$为可接受偏差
3. 使用LQR设计初始权重

陷阱3：数值问题

常见问题：

• Hessian矩阵病态
• 约束矩阵秩亏
• 舍入误差累积

解决方法：

# Hessian正则化
H = H + ε*I  # ε = 1e-6

# 约束预处理
remove_redundant_constraints()
scale_constraints()

# 使用相对误差终止准则
if ||r|| / ||b|| < tol:
    terminate()

陷阱4：不可行性处理

错误表现：

• 硬约束导致无解
• 初始状态在可行域外

鲁棒策略：

1. 软约束： 添加松弛变量 $$\min J + \rho\sum s_i^2 \quad \text{s.t.} \quad \mathbf{g}(\mathbf{x}) \leq \mathbf{s}$$

2. 约束优先级： 关键约束硬化，次要约束软化

3. 可行性恢复： 专门的恢复模式

陷阱5：实时性能问题

性能瓶颈：

• QP构建时间 > 求解时间
• 内存分配开销
• 缓存未命中

优化技巧：

1. 预分配内存：

static double H[N*N];  // 避免动态分配
static double g[N];

2. 利用稀疏性：

H矩阵结构：
[Q+B'PB  -B'PA    0    ]
[-A'PB   Q+A'PA  -A'PA ]
[  0     -A'PA    Q+P  ]

3. 代码生成： 使用ACADO、CasADi等工具生成优化代码

陷阱6：模型失配

表现：

• 跟踪性能退化
• 约束违反
• 振荡或发散

应对策略：

1. 鲁棒MPC： 考虑不确定性集合
2. 自适应MPC： 在线参数估计
3. 偏移补偿： 估计并补偿稳态偏差 $$\mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k + \mathbf{d}_k$$ 其中 $\mathbf{d}_k$ 为估计的扰动

调试清单

调试MPC控制器时的检查项：

• [ ] 开环仿真是否稳定？
• [ ] 约束是否相容（存在可行解）？
• [ ] QP是否正定（$\mathbf{H} \succ 0$）？
• [ ] 热启动是否正确传递？
• [ ] 单位和坐标系是否一致？
• [ ] 延迟补偿是否正确实现？
• [ ] 故障模式是否测试？

9.7 练习题

基础题

题目1：MPC与PID控制对比

考虑一阶系统 $\dot{x} = -x + u$，设计MPC控制器使系统从 $x_0 = 1$ 跟踪到 $x_{\text{ref}} = 0$，约束 $|u| \leq 0.5$。与无约束PID控制器对比，分析两者的控制效果差异。

Hint: 考虑PID可能产生的控制量饱和问题，以及MPC如何预见并避免饱和。

题目2：QP问题构建

给定线性系统 $\mathbf{x}_{k+1} = \begin{bmatrix}1 & 0.1\\0 & 1\end{bmatrix}\mathbf{x}_k + \begin{bmatrix}0\\0.1\end{bmatrix}u_k$，预测时域 $N=3$，写出完整的QP问题矩阵形式。

Hint: 先构建预测矩阵 $\Phi$ 和 $\Gamma$，然后组装Hessian矩阵和梯度向量。

题目3：终端代价计算

对于系统 $x_{k+1} = 0.9x_k + u_k$，局部控制律 $u = -0.4x$，阶段代价 $l(x,u) = x^2 + 0.1u^2$。计算满足稳定性条件的终端代价 $V_f(x) = Px^2$ 中的 $P$ 值。

Hint: 使用离散Lyapunov方程求解。

挑战题

题目4：软约束设计

某机械臂MPC控制器经常因为硬约束导致不可行。设计软约束方案，将关节角速度约束 $|\dot{q}| \leq 1$ rad/s 软化。讨论软约束权重 $\rho$ 对系统性能的影响。

Hint: 引入松弛变量，修改目标函数和约束。

题目5：多速率MPC设计

设计双速率MPC：快速内环（100Hz）处理执行器动态，慢速外环（10Hz）处理轨迹跟踪。推导两个MPC之间的接口设计。

Hint: 考虑时间尺度分离和信息传递。

t=0.0s: 外环计算 → a_ref[0:0.1s]
t=0.0s-0.09s: 内环跟踪 a_ref
t=0.1s: 外环更新 → a_ref[0.1:0.2s]

题目6：MPC参数自适应

设计自适应MPC策略，根据跟踪误差在线调整权重矩阵 $\mathbf{Q}$ 和 $\mathbf{R}$。给出自适应律和稳定性分析。

Hint: 使用性能指标触发权重更新。

if J_perf > J_high:
    Q = min(γ_up * Q, Q_max)     # 增加位置权重
    R = max(R / γ_up, R_min)      # 减少控制权重
elif J_perf < J_low:
    Q = max(Q / γ_down, Q_min)    # 减少位置权重
    R = min(γ_down * R, R_max)    # 增加控制权重

题目7：分布式MPC协调

两个机器人协同搬运，每个机器人运行独立MPC。设计协调机制确保约束一致性和最优性。

Hint: 使用ADMM或协商策略。

题目8：学习增强MPC

设计结合神经网络的MPC，网络预测模型误差 $\Delta f$。讨论如何保证安全性和稳定性。

Hint: 考虑鲁棒性和约束收紧。

if is_safe(u_learned):
    return u_learned
else:
    return project_to_safe_set(u_learned)