MPC for Motion Planning

May 2021

1. 范式边界：轨迹优化 vs MPC

1.1 核心差异与互补关系

维度	轨迹优化 (Trajectory Opt, L5/L6)	模型预测控制 (MPC)
目标	生成平滑、动力学可行、无碰撞的开环参考轨迹 $p(t)$	在扰动/延迟下闭环跟踪参考轨迹/路径，输出控制量 $u_0$
时域	全局/局部一次性求解，时间固定	有限时域 $N$，滚动优化 (Receding Horizon)，仅执行第一步
约束	硬约束 (Corridor/B-Spline凸包) 或软约束 (ESDF惩罚)	硬约束 (执行器极限) + 软约束 (状态偏差) + 鲁棒收紧
计算频率	5~10 Hz (触发式)	20~50 Hz (周期性)
工业定位	前端/中层：提供高质量初值与安全走廊	底层/执行层：抗扰动、处理模型失配、保证递归可行性

关键点：
“轨迹优化负责‘画好线’，MPC负责‘走稳线’”。
纯 MPC 直接优化高维非线性模型+障碍物约束极易超时或发散；纯轨迹优化在动态环境中会因模型失配迅速失效。标准工业架构：全局路径 → 走廊生成 → 轨迹优化(初值) → MPC(闭环跟踪+在线微调)。

2. MPC数学内核：从Condensed到Sparse QP

2.1 标准线性离散模型与代价函数

$$ \begin{aligned} x_{k+1} &= A x_k + B u_k \\ y_k &= C x_k \end{aligned} \quad J = \sum_{k=0}^{N-1} \left( \|y_k - r_k\|_Q^2 + \|u_k\|_R^2 + \|\Delta u_k\|_{R_\Delta}^2 \right) + \|x_N - r_N\|_P^2 $$

2.2 ⚠️ 注意：Condensed Form 的工业局限性

学术推导常使用 Condensed Form (将状态消去，仅优化 $U=[u_0,\dots,u_{N-1}]^T$)：

$$ J(U) = \frac{1}{2} U^T H U + x_0^T F^T U + \text{const} \Rightarrow H = \Gamma^T \bar{Q} \Gamma + \bar{R} $$

工程真相：

$H$ 为稠密矩阵，求逆/分解复杂度 $O(N^3)$，$N>15$ 时实时性崩溃。
工业标准做法：保留状态变量，构建 稀疏非稠密形式 (Non-Condensed / Sparse QP)： $$ \min_{X,U} \frac{1}{2} X^T \bar{Q} X + \frac{1}{2} U^T \bar{R} U \quad \text{s.t.} \quad x_{k+1} = A x_k + B u_k, \quad G X + H U \leq W $$ 此时 $H_{qp}$ 为块三对角矩阵，约束矩阵为稀疏带状。利用结构特性，现代求解器 (OSQP, qpOASES, FORCES) 复杂度降至 $O(N)$ 或 $O(N \log N)$。

✅ 稀疏 QP 结构示意 (C++/Eigen 注释)

// 实际工业实现不直接构建稠密 H，而是按块传递稀疏结构
// Hessian 结构 (以状态+控制混合变量为例):
// [ Q   0   0 | 0   0   0 ]
// [ 0   Q   0 | 0   0   0 ]  <- 块对角 (Block Diagonal)
// [ 0   0   P | 0   0   0 ]
// -----------------------
// [ 0   0   0 | R   0   0 ]
// [ 0   0   0 | 0   R   0 ]
// [ 0   0   0 | 0   0   R ]
// 约束矩阵 A_eq 为下双对角 (Lower Bidiagonal):
// [-I  0  0 | B   0  0] [x1]   [-A x0]
// [ A -I  0 | 0   B  0] [x2] = [    0 ]
// [ 0  A -I | 0   0  B] [x3]   [    0 ]

2.3 MPC 与 LQR 的深刻联系

无约束线性 MPC 的解析解为状态反馈 $u_k^* = -K_k x_k$。
当 $N \to \infty$ 且终端权重 $P$ 满足离散代数 Riccati 方程 (DARE) 时，MPC 等价于 LQR。
工程取舍：LQR 计算 $O(1)$ 但无法处理约束；MPC 在线求解 QP 但天然支持约束与参考轨迹突变。工业常用 LQR 初始化 MPC 终端代价 $P$ 保证递归可行性。

3. 约束处理艺术：Slack、Corridor与鲁棒性

3.1 约束分类与工程哲学

约束类型	数学形式	适用场景	工程铁律
Hard Constraints	$G z \leq w$	执行器物理极限、安全硬边界	输入约束必用 Hard。违反必炸机/撞车。
Soft Constraints	代价惩罚 $\lambda \\|v\\|^2$	传感器噪声影响的状态边界	状态约束优先 Soft。测量噪声易导致 QP 不可行。
Slack Variables	$G z \leq w + \epsilon, \epsilon \geq 0$	将 Soft 转化为显式优化变量	工业标配。配合大权重 $\rho$ 实现“准 Hard”，保证 QP 永远有解。

3.2 安全走廊 (Flight Corridor) 与凸约束转化

纯软约束在狭窄空间易“贴边”。工业级做法：前端生成凸多面体走廊 $\mathcal{C}_k = \{ p \mid A_k p \leq b_k \}$，后端强制轨迹段完全落入走廊。

关键技巧：利用 B-Spline 凸包性质降维 B-Spline 曲线 $P(t)$ 必位于控制点 $\{Q_i\}$ 的凸包内。因此：

$$ Q_i \in \mathcal{C}_k, \quad \forall i \in \text{segment } k \implies P(t) \in \mathcal{C}_k, \quad \forall t $$

优势：将无限维连续约束 $A p(t) \leq b, \forall t \in [0,1]$ 转化为有限维线性约束 $A Q_i \leq b$，QP 求解时间从 100ms 降至 <3ms。

3.3 鲁棒约束收紧 (Constraint Tightening)

模型失配与扰动会导致实际轨迹越界。Tube MPC 思想：将约束向安全区域收缩 $\delta_k$：

$$ G x_k \leq w - \delta_k, \quad \delta_k = \sum_{i=0}^{k-1} \| (A+BK)^i \| \cdot w_{dist} $$

工程简化：固定经验值收紧（如位置收紧 0.2m，速度收紧 0.5m/s），配合高增益底层 PID 实现近似鲁棒性。

4. MPC变体与对比 (LTV/NMPC/Tube/MPCC)

4.1 变体对比矩阵

变体	核心思想	计算负载	适用场景	工业现状
Linear MPC	固定 $A,B$，标准 QP	极低 (1~5ms)	巡航、AGV 直线跟踪	成熟，广泛部署
LTV MPC	沿参考轨迹线性化：$x_{k+1}=A_k x_k+B_k u_k+g_k$	中 (5~15ms)	自动驾驶车道保持、泊车	主流，依赖 Warm-Start
NMPC	直接优化非线性模型，SQP/IPM	高 (20~100ms)	足式机器人、无人机特技	仅限高端算力平台
Tube MPC	标称QP优化 + 辅助反馈控制器 $u=u^+K(x-x^)$	中 (标称QP固定)	强扰动/模型不确定系统	理论强，工程调参难
MPCC/CMPCC	引入路径进度变量 $\theta$，平衡跟踪与前行	中 (5~10ms)	赛车、无人机高速穿越	前沿落地，抗风扰极佳

4.2 关键变体详解

LTV MPC 的“鸡生蛋”问题：线性化点依赖未来轨迹。工业解法：Warm-Start 线性化。用上一帧最优解 $(x^*, u^*)$ 作为当前工作点，首次迭代用参考轨迹近似。
MPCC (Model Predictive Contouring Control)：传统 MPC 跟踪时间参数化轨迹，易因扰动导致“时间不同步”。MPCC 引入进度变量 $\theta \in [0,1]$： $$ J = \sum_{k=0}^{N-1} \left( e_{cont,k}^T Q_c e_{cont,k} + e_{lag,k}^T Q_l e_{lag,k} + \|u_k\|_R^2 - \lambda \dot{\theta}_k \right) $$
- $e_{cont}$：轮廓误差（垂直路径方向）
- $e_{lag}$：滞后误差（沿路径方向）
- $\dot{\theta} \geq 0$ 保证单向行进，$\lambda$ 权衡“贴线”与“快速通过”。
- CMPCC：结合前端凸走廊，将安全约束线性化， onboard 求解 <5ms，天然抗碰撞/风扰。

5. 参数化空间与实时性优化

5.1 控制参数化策略降维

直接优化 $u_k$ 维度高且不平滑。工业常用参数化：

参数空间	数学表达	优点	缺点
Zero-Order Hold	$u(t) = u_k$	实现简单	控制阶梯跳变，需高频执行
B-Spline / Bezier	$u(t) = \sum c_i B_{i,p}(t)$	天然平滑，凸包性质保安全	约束映射需基变换
JLT (Jerk Limited Trajectory)	加加速度受限的分段常值	解析求解 TPBVP，极快	仅适用于积分链模型

5.2 延迟补偿与状态预测

传感器/通信延迟 $\tau$ 是 MPC 稳定性的隐形杀手。 解决方案：

$$ x_{k+\tau} = A^\tau x_k + \sum_{j=0}^{\tau-1} A^j B u_{k-1-j} $$

将预测状态 $x_{k+\tau}$ 作为 MPC 初始条件，而非当前测量值 $x_k$。确保 $N \geq \tau$，否则优化视界覆盖不到未来。

5.3 数值稳定性

空间缩放：将坐标映射至 $[-10, 10]$ 沙盒求解，反变换回物理单位。
时间缩放：相对时间 $s = (t - T_j)/\Delta T_j \in [0,1]$，避免 $\Delta T \to 0$ 导致矩阵病态。
权重归一化：$Q, R, P$ 量级应匹配状态/控制物理范围。常用启发式：$Q_{ii} \approx 1/\text{max\_state}_i^2, R_{jj} \approx 1/\text{max\_input}_j^2$。
条件数监控：求解前检查 $H_{qp}$ 条件数 $\kappa(H) < 10^6$，超限则触发 Fallback。

6. Pipeline架构与实战避坑

6.1 流程图

graph LR
    A[感知/全局地图] --> B(前端: A*/Lattice/Corridor Gen)
    B --> C[安全走廊 & 参考路径]
    C --> D{MPC 求解器}
    D -->|LTV/MPCC| E[最优控制序列 u0]
    E --> F[底层执行器/伺服]
    F -->|状态反馈 x_meas| D
    G[监控与降级] -->|重规划触发| B
    G -->|QP失败/超时| H[保守 PID / DWA / 悬停]
    
    style B fill:#e3f2fd
    style D fill:#fff3e0
    style E fill:#e8f5e9
    style G fill:#f3e5f5
    style H fill:#ffcdd2

6.2 常见问题

陷阱现象	根本原因	工业级解决方案
QP 频繁不可行	约束过紧、扰动超界、Horizon 过短	引入 Slack 变量、固定经验值约束收紧、增加终端集/终端代价
求解超时 (>控制周期)	矩阵稠密、未热启动、维度过高	使用稀疏求解器、强制 Warm-Start、降阶参数化 (B-Spline/JLT)
轨迹振荡/控制抖振	$R$ 权重过小、离散化频率不匹配	增大控制惩罚 $R$、添加 $\Delta u$ 平滑项、确保 $f_{mpc} \geq 2 f_{ctrl}$
长时域 Myopic (短视)	Horizon $N$ 不足，未见前方障碍	添加终端势场/终端约束、动态调整 $N$ 随车速/环境复杂度变化
模型失配累积漂移	线性化点偏离、参数未辨识	在线递推最小二乘 (RLS) 辨识、切换至 Tube MPC 或 LTV + 实时线性化

6.3 部署细节

热启动 (Warm-Start)：$x_0 \leftarrow x_1^*, u_0 \leftarrow u_1^*, \dots, u_{N-2} \leftarrow u_{N-1}^*, u_{N-1} \leftarrow u_{N-1}^*$。首次启动用参考轨迹填充。
延迟补偿：$N \geq \lceil \tau_{total} / \Delta t \rceil$，初始状态用前向积分预测。
数值缩放：物理量 $\to$ 归一化沙盒 $\to$ QP 求解 $\to$ 反变换 $\to$ 下发。
Fallback 机制：QP 返回非最优/超时 > 50ms 时，立即切换至保守 PID 跟踪上一帧或 DWA 局部避障。严禁输出零向量或残差。
监控埋点：记录 $J_{cost}, \|u\|_\infty, \|x-r\|_2, \text{solver\_time}$，接入 Prometheus/Grafana。

附录：核心代码模板与调参清单

A.1 稀疏 QP 热启动与 Warm-Start 逻辑 (C++ 伪代码)

// 实际工程中不使用 dense Eigen::MatrixXd，而是直接传递稀疏块结构给求解器
// 此处展示 Warm-Start 数据流
void warmStartMPC(const std::vector<VectorXd>& prev_u, 
                  const std::vector<VectorXd>& prev_x,
                  std::vector<VectorXd>& curr_u_init, int N) {
    // 滚动移位 (Receding Horizon Shift)
    for (int k = 0; k < N - 1; ++k) {
        curr_u_init[k] = prev_u[k + 1];
    }
    curr_u_init[N - 1] = prev_u[N - 1]; // 末端保持
    
    // 若使用非稠密形式，同时提供状态初值
    // curr_x_init[k] = A*prev_x[k] + B*prev_u[k] (预测校正)
}

// 求解器调用示例 (OSQP 风格)
OSQPSettings settings;
osqp_set_default_settings(&settings);
settings.warm_start = 1;
settings.verbose = 0;
osqp_setup(&work, P, q, A, l, u, n, m, &settings); // P, A 为稀疏矩阵
osqp_warm_start_x(work, x_init.data());
osqp_warm_start_y(work, y_init.data());
int exitflag = osqp_solve(work);
if (exitflag != 1) trigger_fallback(); // 非最优解或不可行

A.2 MPCC 代价函数与进度变量约束

// MPCC 核心代价 (离散化形式)
// theta[k] 为进度变量, p_ref(theta) 为参考路径参数化
double cost = 0.0;
for (int k = 0; k < N; ++k) {
    Vector3d p_ref = ref_path.eval(theta[k]);
    Vector3d e_cont = state[k].pos - p_ref;
    Vector3d e_lag = e_cont; // 简化示意，实际需投影到Frenet系
    
    // 轮廓误差惩罚 + 滞后误差惩罚 + 控制惩罚 - 进度奖励
    cost += e_cont.transpose() * Qc * e_cont + 
            e_lag.transpose() * Ql * e_lag + 
            u[k].transpose() * R * u[k] - 
            lambda * (theta[k+1] - theta[k]);
}
// 约束: 0 <= theta[0] <= theta[1] <= ... <= 1, dot_theta_max >= (theta[k+1]-theta[k])/dt >= 0

A.3 工业求解器选型

求解器	类型	语言/依赖	特点	推荐场景
OSQP	凸 QP (ADMM)	C/C++	开源、支持热启动、嵌入式友好、中等规模极快	无人机/小车在线 MPC
qpOASES	凸 QP (Active Set)	C++	高度优化、支持 warm-start、工业验证久	自动驾驶 LTV MPC
ACADO / CasADi	NMPC/SQP 工具链	C++/Python	符号微分、自动生成 C 代码、适合非线性	足式/机械臂 NMPC
FORCES Pro	商业嵌入式求解器	MATLAB → C	代码生成、极低延迟、微秒级、结构 exploited	车规级控制器
Gurobi / MOSEK	通用凸优化	Python/C++	极致稳定、支持 MIP/QCQP、商业授权	离线验证/高层调度

A.4 调参细节

预测时域 $N$：覆盖系统主要动态响应时间（通常 $1.5 \sim 3$ 秒）。高速场景 $N$ 需增大，但计算预算需同步提升。
权重矩阵 $Q, R$：$Q/R \approx 10 \sim 100$ 为常见起点。跟踪紧则调大 $Q$，防抖则调大 $R$。使用对角矩阵，避免交叉耦合病态。
离散时间 $\Delta t$：必须与底层控制周期整数倍匹配。通常 $\Delta t \in [0.05, 0.2]s$。
Slack 权重 $\rho$：状态约束 $\rho_{state} \approx 10^4 \sim 10^6$，动力学约束 $\rho_{dyn} \approx 10^3$。过大退化为 Hard，过小失去约束力。
终端代价 $P$：用无约束 LQR 的 DARE 解初始化，或设为 $P = \alpha Q$ ($\alpha \in [5, 20]$) 保证递归可行性。

总结：
🔹 MPC是“带约束的前瞻性闭环控制器”。模型精度、求解速度、递归可行性三者需动态平衡。
🔹 工业落地首选 LTV MPC + Slack 约束 + B-Spline/Corridor 安全映射 + OSQP/qpOASES + Warm-Start 组合。
🔹 遇到高速/强扰动场景，果断切换 CMPCC 或 Tube MPC；遇到微控制器，评估 Explicit MPC 或 JLT 参数化。
🔹 数值缩放、延迟补偿、Fallback 机制 决定算法能否走出实验室。不要为追求理论最优牺牲鲁棒性。