精读笔记

Problem Setting

这篇论文实际解决的是 embedded autonomous navigation 中一个很具体但长期棘手的问题：非线性 MPC 理论上很适合把动力学、输入约束和避障统一进一个优化问题，但在四旋翼这类嵌入式平台上，单层 MPC 同时要求长预测视野和高控制频率时会直接撞上算力墙。

真正困难点不在于写出一个 nonlinear MPC，也不在于构造局部凸自由空间，而在于三件事必须同时成立：1）规划 horizon 足够长，否则局部避障会卡住或绕不过障碍；2）反馈频率足够高，否则模型误差和扰动下跟踪质量差；3）优化和约束生成必须稳定落在嵌入式实时预算内。以前方法通常只能牺牲其中一个：单层 MPC 牺牲 horizon，分层方法牺牲模型一致性或理论闭环保证，motion primitive / funnel 方法牺牲连续机动性。

因此关键矛盾是“long-term nonconvex-ish planning”和“short-term stabilizing feedback”被硬塞进同一个优化频率后不可扩展。论文的目标是把这个矛盾重新分配到两个时标，同时不让安全性证明断掉。

Motivation

已有路线的问题不是没有 hierarchy，而是 hierarchy 往往没有严肃处理 planner-tracker interface。PMPC 生成一条看似动态可行的轨迹，并不自动意味着 TMPC 在真实状态偏离、终端约束、输入饱和和障碍边界附近仍可跟踪。很多工作靠经验 margin、低阶模型、线性化或 motion primitives 解决实时性，但这样 planner 的能力被限制，且安全性往往只在简化模型或预定义库内成立。

作者的核心观察是：如果能离线刻画 TMPC 的 tracking capability，那么 PMPC 不需要在线考虑 tracker 的全部闭环复杂性，只需在规划时按这个 tracking capability 收紧约束。这样 planner 可以独立低频长 horizon 运行，tracker 高频短 horizon 运行，而二者之间通过一个可证明的误差管/终端集接口连接。

关键缺口就是：同一非线性模型下，如何让 PMPC 的参考轨迹天然满足 TMPC 可跟踪性，并让障碍约束随时间更新时仍保留 shift-based recursive feasibility。

Core Idea

论文最核心的思想是把 HMPC 从“planner 输出轨迹、tracker 尽量跟”改成“planner 只允许输出 tracker 证书可覆盖的轨迹”。这改变了建模方式：PMPC 不再规划到原始可行域边界，而是在由 TMPC 终端集和增量 Lyapunov 证书诱导出的 tightened feasible set 内规划。TMPC 的误差椭球被显式嵌入 PMPC 约束收紧，因此实际闭环轨迹可以偏离参考，但偏离后的整条 tube 仍满足系统约束和障碍约束。

这套结构的 inductive bias 很明确：长期几何/任务决策和短期动态反馈应该在不同时间尺度处理，但二者必须共享同一个动力学模型和同一个安全余量定义。它比 prior 更 scalable 的原因不是优化器更神奇，而是把最昂贵的 constraint generation 和 long-horizon search 降频，同时把高频层变成短 horizon tracking problem。和使用线性 planner 或 motion primitives 的 HMPC 相比，本质区别是 PMPC 仍直接优化非线性模型，而不是把 maneuverability 预先离散化或降阶化。

Method

1）TMPC 终端 ingredients：解决 tracker 是否真的能跟的问题。作者离线求一个增量 Lyapunov 型终端 cost、终端椭球和线性反馈 K，使得在终端集内应用反馈会使 tracking error 下降，并满足原始系统约束。这个机制的核心变化是把 tracker 的能力变成可供 planner 使用的几何 margin，而不是靠在线经验调参。

2）PMPC 约束收紧：解决 planner 轨迹安全但实际闭环不安全的问题。PMPC 在 tightened system constraints 和 tightened obstacle-free regions 内规划。收紧量由 TMPC 的终端椭球尺度 alpha 和常数 c_s、c_o 决定。这样参考轨迹离障碍和系统边界保持足够余量，留给 TMPC 的跟踪误差消耗。

3）参考连续性与低频重规划：解决两层 MPC 时序不一致的问题。PMPC 的初始一段被固定为上一轮最优解的 shifted segment，保证发给 TMPC 的参考在时间上连续且动态可行。这个约束牺牲了一部分 planner 自由度，但换来证明中最关键的候选解构造。

4）稳态终端约束：解决 PMPC 自身递归可行性问题。PMPC 末端要求达到稳态，使下一轮可以 shift 后 append 一个稳态段。这个设计相当保守，但证明非常干净。

5）I-DecompUtil 障碍约束更新：解决在线局部凸自由空间生成的可行性继承问题。障碍约束基于上一轮 plan 的线段生成，并要求更新后一段旧轨迹仍在新 free-space 内。这里是理论与工程结合最紧的地方：没有这种 consistency，PMPC 的递归可行性证明会断。

Key Insight / Why It Works

真正有效的原因不是“用了两个 MPC”，而是两层之间传递的不是裸轨迹，而是带有可跟踪余量的轨迹。PMPC 在收紧集合内规划，相当于提前为 TMPC 的闭环误差留出 tube；TMPC 的终端集和 Lyapunov decrease 让 shift-and-append argument 成立。递归可行性来自两个独立但耦合的 shift 结构：TMPC shift 后 append terminal controller，PMPC shift 后 append steady state。

最核心贡献是 planner-tracker codesign 的闭环化：TMPC 的离线 tracking certificate 被转化为 PMPC 在线约束收紧。这个接口比“planner 生成 smooth trajectory + tracker 跟踪”强很多，因为它直接回答了 planner 轨迹为什么可被 tracker 安全跟踪。

性能增益中相当大一部分来自 test-time compute 的重分配和 scaling：PMPC horizon 变长、约束生成降频、TMPC horizon 变短。这不是贬义；论文的价值正是在嵌入式 setting 下把计算预算重新组织得更合理。但如果问算法本身是否带来新的全局规划能力，答案是否定的。它仍是局部 MPC，长 horizon 改善了局部最优问题，但没有变成全局 planner。

I-DecompUtil、compiler optimization、map preprocessing 等属于必要 engineering，而非理论核心。它们对实机成功很重要，但主要解决 runtime bottleneck，不改变 HMPC 的基本思想。

真实实验中 HMPC 对模型失配更稳，原因大概率是短 horizon 高频 TMPC + PMPC margin + 不需要 SMPC slack 的组合效应。严格来说，理论没有覆盖模型失配；因此把这部分说成被证明的 robustness 是不准确的。文中也承认未来需要 robust TMPC。

Relation To Prior Work

这篇属于 planner-tracker codesign / hierarchical MPC / tube-like nonlinear MPC 的交叉谱系。和传统 SMPC 的本质差异是时标分离：SMPC 把规划和跟踪放进同一个优化问题，导致 horizon 与 sampling time 互相制约；HMPC 把长期决策降频、短期反馈高频，从结构上释放计算预算。

和一般 HMPC 相比，真正不同点在于 PMPC 和 TMPC 使用同一非线性模型，并通过 TMPC 终端 ingredients 反向约束 PMPC。很多 prior 为了实时性使用线性 planner、低保真模型或 motion primitive，这些方法的 planner 可行性和 tracker 可跟踪性之间存在模型 gap；本文试图避免这个 gap。

看似新的部分中，shifted candidate、terminal invariant set、constraint tightening、convex corridor 都不是新思想；实质创新在于把这些已有 MPC 工具组合成一个能在嵌入式四旋翼上实时运行、且 PMPC/TMPC 两层证明能闭合的系统。贡献更偏系统级理论-工程整合，而不是单个数学组件的突破。

Dataset / Evaluation

评估覆盖了简单仿真、Gazebo 模型失配和真实四旋翼实验，且代码在 Jetson Xavier NX 上运行，这一点比只在桌面仿真的 MPC 工作更有说服力。核心 claim 是嵌入式实时性、比 SMPC 更长 horizon、更好导航性能和闭环安全结构；实验基本围绕这些 claim 展开。

但任务覆盖仍相对窄：静态、已知/离线生成的 2D grid map，障碍形状规则，主要是四旋翼在室内平面环境中绕障。更复杂 corridor 只在 Gazebo 中展示，不能完全证明在真实 onboard perception + 3D clutter 下仍 scalable。

与 SMPC 的对比能说明长 horizon 的价值，但归因不是完全干净。SMPC 因实时性被迫短 horizon，并且在模型失配下使用 slack；HMPC 有分层、收紧和不同权重设计。论文尽量公平地共享模型和部分 cost，但性能差距仍混合了 horizon scaling、时标分离、constraint softening 策略和 tuning 差异。

理论验证方面，简单仿真 tracking error 为零更多是在同模型闭环下 sanity check；真实实验展示 practical feasibility，但不验证理论假设下的严格安全，因为存在模型失配、测量链路和执行延迟。

Limitation

第一，理论保证依赖无模型失配、静态环境和初始可行性。真实机器人中模型误差不可避免，因此论文最强的 theorem 与最有价值的实验之间存在明确 gap。HMPC 在实验中表现鲁棒，但这不是由当前证明保证的。

第二，PMPC 的递归可行性依赖障碍约束更新一致性和 Assumption 1。这个假设在高障碍密度、高速度、长 PMPC sampling interval 或粗 free-space decomposition 下可能失效。一旦旧轨迹段不再落入新生成凸区域，shift 证明就断。

第三，PMPC 不直接使用真实状态反馈，而是基于自身上一轮最优解演化；真实状态只进入 TMPC。这使规划层更容易保持递归可行，但也把大扰动后的全局重同步问题推给了 tracker。若 tracker 长时间无法贴近 reference，PMPC 仍可能在名义世界中继续规划。文中未充分说明这种失配积累如何处理。

第四，SDP 终端 ingredients 对系统类别有限制。文中明确提到非完整系统可能需要最小速度；半无限 LMI 实际靠 gridding/convexification 近似，严格全域保证取决于网格覆盖。对更高维、更强非线性平台，离线 certificate 可行性和保守性都可能成为瓶颈。

第五，方法仍是 local MPC，不解决全局拓扑选择。长 horizon 可以缓解局部障碍，但在迷宫、多 homotopy、死胡同环境中仍需要 global planner 或更强的 topological reasoning。

Takeaway

1）值得记住的不是“两层 MPC”，而是“把 tracker 的可证明误差管作为 planner 的约束收紧接口”。
这是可迁移的设计模式：任何 planner-tracker 架构都应显式暴露 tracker capability，而不是事后调 margin。
2）嵌入式 MPC 的关键不只是 solver 加速，而是计算职责重排：昂贵的长 horizon 搜索和 constraint generation 降频，高频层只做短 horizon tracking。
这个思想可迁移到 ground vehicles、legged locomotion、manipulation local planning 等场景。

一句话总结

这篇论文是 hierarchical nonlinear MPC 在嵌入式自主导航中的一次扎实系统化推进：它的核心贡献不是新的优化器或避障算法，而是用 TMPC 的离线 tracking certificate 来约束 PMPC，从而把长视野规划和高频安全跟踪可证明地接起来。