模型预测控制（Model Predictive Control, MPC）已成为现代机器人控制的基石技术。不同于传统的反馈控制器，MPC通过在线求解有限时域优化问题，能够系统性地处理约束、预见未来行为，并实现多目标优化。本章将深入探讨MPC的核心概念，从问题表述到稳定性保证，并通过自动驾驶车辆的案例展示其实际应用。
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}]
^ ^
| |
当前状态 终端状态
模型类型与选择：
在机器人控制中，常见的预测模型包括：
线性时不变（LTI）模型： \(\mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k\) 适用于工作点附近的小信号控制，计算效率高，但精度受限于线性化误差。
线性时变（LTV）模型： \(\mathbf{x}_{k+1} = \mathbf{A}_k\mathbf{x}_k + \mathbf{B}_k\mathbf{u}_k\) 通过沿参考轨迹线性化获得，能够处理大范围运动，是轨迹跟踪MPC的标准选择。
非线性模型： 保留系统的完整非线性动态，精度最高但计算代价大。对于机器人系统，通常包含：
离散化方法：
连续时间模型 $\dot{\mathbf{x}} = f_c(\mathbf{x}, \mathbf{u})$ 的离散化直接影响预测精度：
欧拉前向法（一阶精度）： \(\mathbf{x}_{k+1} = \mathbf{x}_k + T_s f_c(\mathbf{x}_k, \mathbf{u}_k)\) 简单但精度低，需要小采样时间。
零阶保持（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\) 精确但仅适用于线性系统。
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需要滚动时域策略来不断纠正累积误差。
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}})\]其中：
权重矩阵的选择直接影响控制性能：
扩展代价函数形式：
实际机器人控制中，常需要更复杂的代价函数：
增量控制代价（减少控制抖动）： \(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})\) 这在机械系统中特别重要，可以减少执行器磨损和能量消耗。
输出跟踪代价： 当只关心特定输出 $\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}})\) 例如，只跟踪末端执行器位置而不约束关节角度。
经济代价函数： 直接优化经济指标而非跟踪误差： \(J_{\text{econ}} = \sum_{i=0}^{N-1} c(\mathbf{x}_{k+i|k}, \mathbf{u}_{k+i|k})\) 其中 $c(\cdot)$ 可能表示能量消耗、生产率等。注意经济MPC可能不存在稳态工作点。
鲁棒代价（最坏情况优化）： \(J_{\text{robust}} = \max_{\mathbf{w} \in \mathcal{W}} J(\mathbf{x}, \mathbf{u}, \mathbf{w})\) 其中 $\mathbf{w}$ 为有界扰动，$\mathcal{W}$ 为不确定性集合。
权重选择策略：
基于物理尺度归一化： \(q_{ii} = \frac{1}{\sigma_i^2}, \quad r_{jj} = \frac{1}{\rho_j^2}\) 其中 $\sigma_i$ 为状态 $i$ 的可接受偏差，$\rho_j$ 为控制 $j$ 的典型范围。
基于频域分析： 通过期望的闭环带宽 $\omega_b$ 设计： \(\frac{\mathbf{Q}}{\mathbf{R}} \approx \omega_b^2\)
LQR反向设计： 从期望的闭环极点出发，通过逆Riccati方程求解权重。
多目标帕累托优化： 系统地探索不同权重组合，生成帕累托前沿：
性能
^
| 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}\]这允许系统在远离目标时快速接近，在接近目标时精确控制。
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}\)
机器人特定约束：
关节限位与速度限制： \(\mathbf{q}_{\min} \leq \mathbf{q} \leq \mathbf{q}_{\max}, \quad \dot{\mathbf{q}}_{\min} \leq \dot{\mathbf{q}} \leq \dot{\mathbf{q}}_{\max}\) 保护机械结构，避免碰撞和过载。
扭矩/力矩约束： \(\boldsymbol{\tau}_{\min} \leq \boldsymbol{\tau} \leq \boldsymbol{\tau}_{\max}\) 反映执行器能力限制。对于冗余机器人，还需考虑零空间投影。
工作空间约束： 末端执行器位置约束： \(\mathbf{p}_{\text{ee}} = \mathbf{f}_{\text{kin}}(\mathbf{q}) \in \mathcal{W}_{\text{safe}}\) 其中 $\mathcal{W}_{\text{safe}}$ 为安全工作空间。
接触力约束（摩擦锥）： \(\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\}\) 确保接触稳定，防止滑动。
ZMP约束（双足机器人）： \(\mathbf{p}_{\text{ZMP}} \in \mathcal{P}_{\text{support}}\) 零力矩点必须在支撑多边形内以保持平衡。
碰撞避免约束： \(d(\mathbf{p}_i, \mathcal{O}_j) \geq d_{\text{safe}}, \quad \forall i \in \text{links}, j \in \text{obstacles}\) 其中 $d(\cdot)$ 为距离函数，可用有符号距离场（SDF）高效计算。
约束优先级与软化：
实际系统中，并非所有约束都同等重要：
软约束实现： \(\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}$ 决定惩罚类型。
非凸约束处理：
机器人系统常遇到非凸约束：
障碍物避免（非凸）： 原始形式：$|\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}}|$。
混合整数表示： 使用二进制变量 $b_i \in {0, 1}$ 选择约束： \(\mathbf{g}_i(\mathbf{x}) \leq M(1 - b_i), \quad \sum_i b_i = 1\) 其中 $M$ 为大数。这将问题转换为混合整数规划（MIP）。
序列凸优化（SCvx）： 迭代线性化非凸约束，添加信任域： \(\|\mathbf{x} - \mathbf{x}^{(k)}\| \leq \delta^{(k)}\) 随迭代收缩信任域半径 $\delta$。
约束激活与去激活：
约束激活模式：
时间 →
约束1: ----[激活]----[非激活]----
约束2: [非激活]----[激活]----[激活]
约束3: ----[非激活]----[激活]----
↑ ↑ ↑
自由运动 避障模式 接触模式
不同模式下的约束集合变化需要平滑过渡，避免控制跳变。
MPC采用滚动时域（Receding Horizon）策略：
| 应用优化序列的第一个控制输入：$\mathbf{u}k = \mathbf{u}{k | k}^*$ |
时刻 k: [========预测时域========]
u_k^* u_{k+1} ... u_{k+N-1}
↓
时刻 k+1: [========预测时域========]
u_{k+1}^* u_{k+2} ... u_{k+N}
这种策略提供了反馈机制，能够补偿模型误差和外部扰动。
滚动时域的本质优势：
隐式反馈控制： 虽然MPC求解开环优化问题，但通过不断更新初始条件并重新优化，实现了闭环控制： \(\mathbf{u}_k = \kappa_{\text{MPC}}(\mathbf{x}_k; \mathbf{x}_{\text{ref}}, \mathcal{C})\) 其中 $\kappa_{\text{MPC}}$ 是隐式定义的非线性反馈律。
扰动抑制能力取决于重新规划频率与系统动态的时间尺度匹配。
预测时域长度选择：
预测时域 $N$ 的选择至关重要：
覆盖主要动态： \(N \cdot T_s \geq (2-3) \cdot T_{\text{settling}}\) 其中 $T_{\text{settling}}$ 是系统调节时间。
近期密集，远期稀疏：
[Δ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$，显著降低计算负担。
初始猜测与热启动：
良好的初始猜测加速收敛：
平移策略（最常用）： \(\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}^*]\) 使用上一时刻的解平移一步。
轨迹库： 根据当前状态从预计算的轨迹库中选择最接近的作为初始猜测。
学习预测： 使用神经网络预测良好初始解： \(\mathbf{U}_k^{(0)} = \mathcal{NN}(\mathbf{x}_k, \mathbf{x}_{\text{ref}})\)
对于线性系统或线性化后的系统：
\[\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\]其中：
稠密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}}\)
包含：
其中差分矩阵： \(\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问题的数值性质影响求解效率：
Hessian矩阵条件数： \(\kappa(\mathbf{H}) = \frac{\lambda_{\max}(\mathbf{H})}{\lambda_{\min}(\mathbf{H})}\) 目标：$\kappa < 10^6$，否则需要预调理。
变量缩放： \(\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]$ 范围。
约束归一化： 将约束除以其典型值，使所有约束具有相似的数量级。
Cholesky因子化预计算： 对于固定的 $\mathbf{H}$，预计算 $\mathbf{H} = \mathbf{L}\mathbf{L}^T$，加速求解。
主动集法（Active Set Method）：
主动集法通过迭代识别活跃约束来求解QP：
优点：
缺点：
内点法（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}\)
优点：
缺点：
OSQP（Operator Splitting Quadratic Program）：
OSQP基于交替方向乘子法（ADMM），特别适合嵌入式系统：
算法流程：
其中 $\Pi_{\mathcal{C}}$ 为投影算子，$\sigma$ 为惩罚参数。
特点：
qpOASES：
qpOASES采用在线主动集策略，专为MPC设计：
核心特性：
热启动策略：
时刻 k: 活跃集 W_k → 最优解 U_k*
↓ (传递)
时刻 k+1: 初始活跃集 W_{k+1}^0 ≈ W_k → 快速收敛到 U_{k+1}*
求解器选择指南：
| 特性 | OSQP | qpOASES | 内点法求解器 |
|---|---|---|---|
| 问题规模 | 大 | 中小 | 大 |
| 精度要求 | 中 | 高 | 高 |
| 热启动 | 好 | 最好 | 一般 |
| 内存占用 | 低 | 中 | 高 |
| 适用场景 | 嵌入式MPC | 快速MPC | 离线计算 |
为保证闭环稳定性，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})$ 满足：
其中 $\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})\)
定理（MPC稳定性）：
考虑MPC问题，如果：
则闭环系统渐近稳定，且最优值函数 $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^*$ 单调递减，系统稳定。
最大输出可许集（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}$ 为状态约束集。
计算算法：
控制不变集（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
递归可行性保证：
如果：
则MPC问题递归可行，即若时刻 $k$ 可行，则时刻 $k+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}\]其中：
车辆模型示意图：
前轮
↑ δ
┌────┴────┐
│ ● │ ← 质心
│ ↙ β │
└─────────┘
后轮
离散化模型（采样时间 $T_s = 0.1$秒）： \(\mathbf{x}_{k+1} = \mathbf{x}_k + T_s \cdot f(\mathbf{x}_k, \mathbf{u}_k)\)
状态向量： \(\mathbf{x} = [x, y, \psi, v]^T\)
控制输入： \(\mathbf{u} = [\delta, a]^T\)
约束条件：
| 转向角限制：$ | \delta | \leq 0.5$ rad（约28.6°） |
| 转向角速率：$ | \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]\]其中：
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. 预测时域选择：
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 # 自适应步长
场景1：高速公路车道保持
条件：
性能指标：
场景2：紧急避障
条件：
性能指标：
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控制
本章系统介绍了模型预测控制的基础理论与实践：
核心概念：
关键公式：
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{约束条件}\)
线性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}}\)
稳定性条件：
实现要点：
错误表现：
解决方案：
错误表现：
调试技巧：
常见问题：
解决方法：
# Hessian正则化
H = H + ε*I # ε = 1e-6
# 约束预处理
remove_redundant_constraints()
scale_constraints()
# 使用相对误差终止准则
if ||r|| / ||b|| < tol:
terminate()
错误表现：
鲁棒策略：
性能瓶颈：
优化技巧：
static double H[N*N]; // 避免动态分配
static double g[N];
H矩阵结构：
[Q+B'PB -B'PA 0 ]
[-A'PB Q+A'PA -A'PA ]
[ 0 -A'PA Q+P ]
表现：
应对策略：
调试MPC控制器时的检查项：
题目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: 考虑时间尺度分离和信息传递。
题目6：MPC参数自适应
设计自适应MPC策略，根据跟踪误差在线调整权重矩阵 $\mathbf{Q}$ 和 $\mathbf{R}$。给出自适应律和稳定性分析。
Hint: 使用性能指标触发权重更新。
题目7：分布式MPC协调
两个机器人协同搬运，每个机器人运行独立MPC。设计协调机制确保约束一致性和最优性。
Hint: 使用ADMM或协商策略。
题目8：学习增强MPC
设计结合神经网络的MPC，网络预测模型误差 $\Delta f$。讨论如何保证安全性和稳定性。
Hint: 考虑鲁棒性和约束收紧。