T3 时空轨迹优化：把时间变成决策变量¶

前置自测¶

开始前，先回答下面 5 个问题。答不出 2 题以上，建议先回 T1、T2 补一补——T3 是 T1、T2 的直接延续，欠了前面的账，这一章会很吃力。

T1 的 path-speed 解耦把规划拆成"先定路径、再排速度"两步。这个拆分在什么场景下会失真？为什么？（答不出 → 回 T1 §T1.1）
T2 的走廊把"避开障碍"这个非凸约束变成了什么形式的约束？靠的是什么性质？（答不出 → 回 T2 §T2.3）
什么是"同伦类"？为什么连续优化跨不过不同的同伦类、因此需要离散搜索来选？（答不出 → 回 T2 §T2.1）
QP（二次规划）和 NLP（非线性规划）的区别是什么？为什么 QP 能保证全局最优、而一般 NLP 不能？（答不出 → 回 Part 0 优化基础）
什么是 iLQR / DDP？它解决的是哪一类问题，为什么比通用 NLP 求解器在某些场景下更快？（答不出 → 回 Part 0 最优控制基础）

参考答案要点（先自己答，再对照——答得上来说明前置扎实）： 1. 路径与速度**强耦合**时失真（如 cut-in、动态绕障）：先定死的路径可能让速度规划无解或次优——为了不撞必须急刹，但路径已选了贴近障碍的线。解耦把联合问题拆成两个凸子问题求解，代价是丢了二者的相互影响。 2. 变成**线性（凸）约束**：把"轨迹在凸 cube 内"用 Bézier 控制点都在 cube 内**表达；靠的是 Bézier 的**凸包性质（曲线被控制点的凸包包住，管住有限个控制点就管住了曲线上无穷多点）。 3. 同伦类 = 在不碰障碍的前提下能**连续形变互相到达**的轨迹的等价类（如"从障碍左边绕"和"右边绕"是两个不同的类）。连续优化只能在初值所在的类内连续变形、跨不过障碍**到另一类，所以需要离散搜索先选定走哪个类。 4. QP 目标二次、约束线性，可行域是凸的、目标是凸的 → **任何局部最优都是全局最优；一般 NLP 目标/约束非凸，可行域可能有多个"谷"，求解器只能找到初值附近的**局部**最优，不保证全局。 5. iLQR/DDP 解**非线性最优控制**（离散时间、有动力学、最小化轨迹代价）；它利用问题的**时序（链式）结构**做反向动态规划递推，复杂度随步数**线性**增长，而通用 NLP 把所有变量堆成大问题求解（复杂度更高）——所以在"状态-控制序列"这类问题上 iLQR 更快。

本章目标¶

学完本章，你应该能够：

理解 CILQR（Constrained Iterative LQR）如何在 iLQR 框架内用 log-barrier 把约束纳入代价，从而做时空联合优化——以及它"第一次迭代就要可行"这个真实局限。
掌握 TEB（Timed Elastic Band）的数学框架：把轨迹表示成位姿序列 + 段时间序列，所有约束写成 g2o 超图上的加权软约束，并支持多拓扑并行。
理解 OBCA（Optimization-Based Collision Avoidance）的对偶避障技巧：用对偶变量把非凸的"不在障碍内"光滑化成标准 NLP 约束。
掌握 MINCO 的参数映射 \(c=M(q,T)\)：为什么稠密的多项式系数由稀疏的（航路点 \(q\)、段时间 \(T\)）唯一确定，以及这如何把时间分配变成低维优化。
理解 CPC（Complementary Progress Constraints）的互补进度约束：如何让"什么时候经过哪个航路点"本身成为优化变量，从而求出真正时间最优的轨迹。
会**根据场景选择合适的方法**，并理解各自的计算代价与约束处理方式的权衡。

这是一条主线清晰的进阶：T1 给了解耦基线，T2 迈出"搜索定 homotopy + 走廊"的第一步，T3 是数学核心——把时间真正变成连续优化里的决策变量。 CILQR 面向自驾、TEB 面向移动机器人、OBCA 面向泊车、MINCO 面向无人机、CPC 面向无人机竞速，但它们底层的数学工具（iLQR / NLP / QP / L-BFGS）是相通的——这一章要讲透的，正是这套通用的"时空联合优化"思想。

知识导航¶

小节	主题	难度	一句话
§T3.1	CILQR	⭐⭐⭐	iLQR + log-barrier 把约束纳入代价，自驾时空联合
§T3.2	TEB	⭐⭐	位姿+段时间序列，g2o 超图软约束，ROS 标配
§T3.3	OBCA	⭐⭐⭐	对偶变量把非凸避障光滑化，泊车 NMPC
§T3.4	MINCO	⭐⭐⭐	\(c=M(q,T)\) 参数映射降维，无人机时空同步变形
§T3.5	CPC	⭐⭐⭐	互补进度约束让"何时过航点"成为优化变量，时间最优
§T3.6	方法选型	⭐⭐	CILQR/TEB/OBCA/MINCO/CPC 的场景与代价对照

两条阅读线：偏自动驾驶的读者重点看 §T3.1（CILQR）、§T3.3（OBCA）；偏无人机/移动机器人的读者重点看 §T3.2（TEB）、§T3.4（MINCO）、§T3.5（CPC）。但 §T3.1 把"约束怎么进优化"讲得最细，无论哪个方向都建议先读。

前置知识桥接¶

回顾 T1（解耦与其失真）：T1 用 path-speed 解耦把运动规划拆成"先在 SL 图定路径、再在 ST 图排速度"两步顺序求解。它的优雅在于把一个难的联合问题拆成两个凸子问题；它的局限在于——当路径与速度强耦合（如 cut-in、动态绕障）时，先定死的路径会让速度规划陷入次优甚至无解。 T1 末尾的 EM 迭代用"多轮解耦逼近联合"缓解了一部分，但没有真正联合。本章要做的，正是 T1 一直没做的那件事：在一个优化问题里同时求解空间与时间。

回顾 T2（走廊与搜索）：T2 给了两条互补的线——搜索线（时空格、SIPP）在离散时空图上定 homotopy，走廊线（SSC、IRIS/SFC）把非凸避障重构成凸 QP。 T2 的走廊线止步于"走廊内解 QP"。本章正是把这一步推进到"走廊内解 NLP"——处理非线性动力学、更复杂的代价。 T2 的走廊会在本章反复出现：它给 CILQR/MINCO 提供初始化和安全约束。 记住这个分工：T2 的搜索给出粗解和 homotopy，T3 的优化在其基础上求出精轨迹。

前向预告：本章的方法都建立在优化求解器之上（iLQR、g2o、Ipopt/CasADi、L-BFGS）。如果你对这些求解器不熟，把它们暂时当成黑盒即可——本章重点不是求解器内部，而是**怎么把规划问题建模成它们能吃的形式**。具体的求解器原理，Part 0 有详述。本章末尾的选型一节会把"哪个方法配哪个求解器"列清楚。

如果跳过本章会怎样¶

跳过 T3，你会卡在两个具体的地方。 场景一：动态绕障时"路径定死，速度无解"。 你用 T1 的解耦管线给一辆车规划——先定一条贴着前车的变道路径，再排速度。可前车突然减速，你这条已经定死的路径逼着自车要么追尾、要么急刹到乘客不适。问题的根源是路径和速度本该联合决策（该往哪变道，取决于变道后能不能排出可行速度），而解耦把它们拆开了。没有 T3 的时空联合优化，你只能在解耦的框架里反复打补丁，治标不治本。 场景二：无人机轨迹"要么飞不快，要么算不动"。 你想让无人机沿一串航点快速飞过。用简单的多项式（minimum-snap）算出来的轨迹太"软"、发挥不出电机极限，飞不快；想优化得更激进，直接优化几十上百个多项式系数又慢得机载算不动、还要处理一堆段间连续性约束。没有 T3 的 MINCO（参数映射降维）和对时间分配的理解，你会在"飞得快"和"算得动"之间反复碰壁，做不出既快又实时的轨迹。这两个场景的共同点是：它们都需要**把时间当成优化变量、和空间一起联合求解**——这正是 T3 教的。 T1、T2 把舞台搭好了（解耦基线 + 走廊/搜索定 homotopy），但真正"把时间变成决策变量、求出时空联合最优轨迹"这一步，只有 T3 能补上。跳过它，你的规划器就停在"会搜索、会定走廊，但不会在走廊里求出高质量时空轨迹"的半成品状态。

预计阅读时间¶

模式	时长	适合
精读	8-12 小时	第一次系统学时空轨迹优化：逐节读动机→理论→代码，亲手编译运行 5 份 demo，做完每节练习。建议分 5-6 次，每次一个方法。
速读	2-3 小时	有优化基础、想建立全局图景：读章首统一问题形式、每节的"动机"和"理论"小标题、范式小结、§T3.6 选型，跳过代码细节和陷阱。
速查	20-40 分钟	已学过、回来查特定方法：直接定位到对应 §T3.x，看该方法的"理论"核心公式 + "带数字走一遍" + 速记卡；或查章末故障排查、API 速查、选型对照表。

无论哪种模式，§T3.1（CILQR）的"理论"部分都建议读——它把"约束怎么进连续优化"这个全章核心讲得最细，是理解后面四个方法的地基。

本章符号约定¶

符号	含义	首见
\(x_k,\ u_k\)	第 \(k\) 步的状态与控制（iLQR 的离散序列）	§T3.1
\((x, y, \theta, v)\)	车辆状态：位置、朝向、速度	§T3.1
\((a, \delta)\)	车辆控制：加速度、前轮转角	§T3.1
\(J,\ \tilde J\)	原代价 / 加了 barrier 的代价	§T3.1
\(B(\cdot)\)	barrier 函数（把约束转成代价）	§T3.1
\(g_i(\cdot)\)	第 \(i\) 个不等式约束（\(g_i \le 0\) 为可行）	§T3.1
\(\mu\)	barrier 权重（越小越贴近原约束）	§T3.1
\(L\)	车辆轴距（自行车模型）	§T3.1
\(s_i,\ \Delta T_i\)	TEB 的第 \(i\) 个位姿 / 段时间	§T3.2
\(\mathcal{B}\)	TEB 的轨迹（位姿+段时间序列）	§T3.2
\(w_k,\ f_k\)	TEB 第 \(k\) 项约束的权重 / 残差函数	§T3.2

怎么读这章：§T3.1 把"约束怎么进连续优化"讲到底（barrier 法），是全章的方法地基； §T3.2 换一个视角（软约束 + 因子图）； §T3.3–§T3.5 是三种更专门的技巧（对偶、参数映射、互补约束）； §T3.6 收束选型。建议至少先读 §T3.1。

科研发展脉络：时空轨迹优化这十年¶

在钻进具体方法前，先把这条研究线的来龙去脉理一遍——知道每个方法"从哪来、解决了前人什么痛点、又留下什么给后人"，比孤立地记五个名字有用得多。这条线的核心母题始终如一：如何在一个连续优化问题里，同时求解空间轨迹和时间分配，使其动力学可行、无碰撞、且某种代价最优？ 起点：工业级联合优化（2014）。 Ziegler 等人（KIT）为奔驰 "Bertha" 自动驾驶项目做的时空联合 NLP（IV 2014），是首批工业级的尝试——决策变量含位置、朝向、速度、时间，用 Ipopt 求解。它验证了"联合优化可行"，但通用 NLP 求解慢。 iLQR 路线（2017）。 Chen-Zhan-Tomizuka 的 CILQR（ITSC 2017）把约束用 barrier 塞进高效的 iLQR——在保持 iLQR 速度的同时获得约束处理能力，开了"软约束 + iLQR"这一路（§T3.1）。 因子图路线（2012→2017）。 Rösmann 等人（TU Dortmund）的 TEB 从 2012 年的原始版演进到 2017 年的多拓扑版——把段时间显式作变量、用 g2o 稀疏图求解、多拓扑躲局部极小，成了 ROS 标配（§T3.2）。 精确避障（2017→2020）。 Zhang-Liniger-Borrelli 的 OBCA 用强对偶把非凸避障**精确**光滑化（不再软化），H-OBCA 配 Hybrid A* 解决泊车——开了"对偶精确重构"这一路（§T3.3）。 参数化降维（2020→2022）。 浙大 FAST Lab 从 am_traj（RA-L 2020）到 MINCO/GCOPTER（T-RO 2022），用参数映射 \(c=M(q,T)\) 把高维系数压成低维航点+段时间、时空同步变形——让无人机轨迹优化做到百赫兹（§T3.4）。 真正时间最优（2021）。 UZH RPG 的 CPC（Science Robotics 2021）用互补约束让时间分配也成变量，造出第一条真正时间最优的四旋翼轨迹、击败人类飞手（§T3.5）。 前沿（2024→）。 Actor-Critic MPC、MPCC++、Improved CILQR 等把学习和优化融合——用学习补难建模的动力学、用 RL 调优化参数，同时保留优化的可行性保证。看这条线，有两个清晰的趋势：约束处理从"软化"走向"精确重构"（barrier/hinge → 对偶/映射/互补），时间从"隐式/固定"走向"完全作为优化变量"（藏在 \(v\) 里 → 段时间 → 连分配都优化）。这正是本章组织五个方法的两条主线，下一节的"统一问题形式"会把它们框得更清楚。

时空轨迹优化的统一问题形式¶

在分头讲五个方法之前，先给它们一个**共同的数学骨架**——本章所有方法，无论叫 CILQR、TEB 还是 MINCO，都是在解下面这个问题的某个具体版本：

\[ \min_{\text{traj},\ T}\ J(\text{traj}, T) \quad \text{s.t.}\quad \begin{cases} \text{dynamics feasible}\\ \text{collision-free}\\ \text{state/input in bounds} \end{cases} \]

即"在动力学可行、无碰撞、状态与控制都在界内的前提下，对轨迹和时间求某个代价最小"。把它读懂，再看五个方法，就会发现它们只是在三个问题上做了不同的选择。

选择一：用什么表示"轨迹 + 时间"？ 这是最根本的分歧。 CILQR 用**离散的状态-控制序列** \(\{x_k, u_k\}\)，时间隐含在状态 \(v\) 和步长里； TEB 用**位姿 + 段时间序列** \(\{s_i, \Delta T_i\}\)，时间是显式的分段变量； MINCO 用**分段多项式 + 航点 + 段时间** \((q, T)\)，把稠密的多项式系数压缩成稀疏参数； CPC 在轨迹之外再加一组**进度变量** \(\lambda_k\)，把"何时到航点"也变成变量。表示一旦选定，后面的求解器和技巧就大致定了——这就是为什么"用什么表示"是第一位的选择。

选择二：怎么把约束（尤其非凸的避障）塞进优化？ 这是本章反复出现的主题。 CILQR 把约束变成代价里的 log-barrier（软墙）； TEB 把约束变成因子图上的**单边 hinge 惩罚**（软）； OBCA 用**对偶变量**把非凸的"不在障碍内"光滑化成等价的硬约束； MINCO 用**光滑映射**精确消去几何约束、化为无约束优化； CPC 用**互补条件**把离散的"过/不过航点"写成连续可解的约束。五种技巧，从"软化"到"精确重构"，难度递增、保证递强——这正是本章从 §T3.1 到 §T3.5 的递进顺序。

选择三：代价 \(J\) 里放什么？ 平滑性（jerk/snap 的平方积分）、贴近参考、控制省力几乎总在；而是否放**时间项**（\(\sum\Delta T_i\) 或总时长），决定了这个方法是否追求"时间最优"——TEB、MINCO、CPC 都把时间最优作为核心目标，CILQR、OBCA 则更看重跟踪与舒适。

记住这个三选择框架（表示 / 约束处理 / 代价），本章后面每讲一个方法，你都可以问自己：它在这三件事上各选了什么？这样五个看似零散的方法，就被组织成了同一问题的五种解法，而非五个孤立的技巧。下面从 CILQR 开始。

§T3.1 CILQR（Constrained Iterative LQR）⭐⭐⭐¶

动机：iLQR 很快，但它不会处理约束¶

先说 iLQR（iterative LQR）是什么、为什么自驾喜欢它。 iLQR 是求解非线性最优控制问题的一种高效算法：给定一个非线性系统（比如车辆运动学）和一个代价函数，它通过"在当前轨迹附近线性化系统、二次化代价、用 LQR 的 Riccati 反向递推求出控制增量、再前向更新轨迹"反复迭代，逼近最优控制序列。它的好处是**快**——利用了最优控制问题的时序结构（动态规划的反向递推），复杂度随时间步线性增长，而不是像通用 NLP 那样把所有变量堆在一起求解。对自动驾驶这种要求几十赫兹实时重规划的场景，这个速度优势很关键。

但 iLQR 有一个致命短板：它只能处理无约束问题。标准 iLQR 的推导假设代价是光滑的、控制是自由的——它没法直接表达"速度不能超过限速""加速度不能超过舒适上限""不能撞上障碍"这类**不等式约束**。而真实的驾驶规划全是约束：限速、加减速上限、曲率上限（方向盘打不了那么死）、以及最要命的避障。于是问题就成了：怎么让又快又好的 iLQR，学会处理约束？ CILQR 给的答案是——把约束"变成"代价。

如果不这样做会怎样（反面）¶

不解决约束问题，你只有两条退路，都不理想。其一是**用通用 NLP 求解器**（Ipopt、SNOPT 等）直接解带约束的轨迹优化。这能处理约束，但放弃了 iLQR 利用时序结构的速度优势——通用 NLP 把整条轨迹的所有变量当成一个大向量、约束当成一堆方程，求解慢，难以满足自驾的实时预算。其二是**事后裁剪**：用无约束 iLQR 解出一条轨迹，再硬把越界的部分裁回可行区（比如速度超了就削到限速）。这破坏了 iLQR 解的最优性和一致性——裁剪后的轨迹既不是原问题的最优解、又可能在裁剪点产生不连续，下游控制难以跟踪。两条路都说明：约束不能当成事后补丁，必须**在优化过程内部**就被考虑进去。 CILQR 的做法——把约束转成代价里的 barrier 项——正是让 iLQR 在每次迭代里就"感知"到约束的存在。

历史：Tomizuka 组的 ITSC 2017¶

CILQR 由 Jianyu Chen、Wei Zhan、Masayoshi Tomizuka（加州大学伯克利分校）提出，发表在 2017 年 10 月 16–19 日于日本横滨举行的第 20 届 IEEE 智能交通系统会议（ITSC 2017），论文第 1–7 页。他们的出发点正是上面那个矛盾：自动驾驶的轨迹规划面临几个挑战——高动态环境需要空间与时间联合规划、车辆模型非线性且避障约束非凸、以及实时性要求高； iLQR 能很高效地求解非线性系统的预测最优控制问题，但它无法处理约束。他们的解法是：把约束通过加 barrier 函数转移到代价函数里；非线性、非凸的约束会被线性化，barrier 函数再被二次化、加到原代价上；然后用一种基于椭圆的新障碍表示、并把中心线用多项式近似使其连续可微。这套做法把"约束处理"干净地嵌进了 iLQR 的迭代，既保留了 iLQR 的速度、又获得了处理约束的能力——这正是 CILQR 名字里那个 "Constrained" 的含义。

这一节解决什么问题：iLQR 快但不吃约束、通用 NLP 吃约束但慢。 CILQR 用 barrier 把约束变成代价，让 iLQR 在保持速度的同时学会处理约束——这是"约束怎么进连续优化"这个全章核心问题的第一个、也是最具代表性的答案。

理论：log-barrier 把约束变成代价¶

状态、控制与时空联合性。 CILQR 把车辆建模成离散时间系统，状态取 \(x_k = (x, y, \theta, v)\)（位置、朝向、速度），控制取 \(u_k = (a, \delta)\)（加速度、前轮转角）。注意这里**时间是隐式的决策维度**：速度 \(v\) 是状态变量、加速度 \(a\) 是控制变量，二者直接决定了"何时到达轨迹上的哪一点"——所以求解 \(x_k, u_k\) 序列的同时，时间分布也被一起定了。这就是 CILQR "时空联合"的来源：它不像 T1 那样先定路径再排速度，而是在同一个 iLQR 迭代里同时优化位置和速度，空间与时间被一起决定。

核心招式：barrier 函数。 约束的一般形式是一组不等式 \(g_i(x, u) \le 0\)（满足即可行，比如 \(g_v = v - v_{\max} \le 0\) 表示不超速）。 CILQR 不把这些约束硬性地交给求解器，而是为每个约束构造一个 barrier 函数 \(B(g_i)\)，加到原代价 \(J\) 上，得到增广代价：

\[ \tilde J = J + \sum_i \mu\, B\big(g_i(x, u)\big) \]

其中 \(\mu > 0\) 是 barrier 权重。 barrier 函数的设计意图是：当约束**满足**（\(g_i < 0\)，在可行域内部）时，\(B\) 给一个有限的、随着接近边界而增大的"惩罚"，像一堵"软墙"把解往可行域内部推；当约束**逼近或越过边界**时，\(B\) 急剧增大，让优化器强烈避开。一种常见的实现是对数 barrier 加二次惩罚的组合：

\[ B(g) = \begin{cases} -\ln(-g) & g < 0 \quad(\text{feasible})\\[4pt] \tfrac{1}{2}k\,g^2 + (\text{linear, const}) & g \ge 0 \quad(\text{infeasible}) \end{cases} \]

可行侧用对数 barrier（\(-\ln(-g)\) 在 \(g\to 0^-\) 时趋于 \(+\infty\)，形成"墙"），越界侧改用二次惩罚（对数在 \(g\ge 0\) 没有定义，必须换一个有限且光滑衔接的函数）。两段在 \(g=0\) 处做光滑拼接（值、一阶导连续），保证整个 \(B\) 可微——这对 iLQR 至关重要，因为 iLQR 需要对代价求一阶、二阶导。

为什么这招能让 iLQR 处理约束？ 关键在于：加了 barrier 后，约束信息被编码进了代价的**梯度和 Hessian**里。 iLQR 每次迭代要对代价做二次近似（求梯度、Hessian），barrier 项的导数会在接近约束边界时变得很大，于是 iLQR 的反向递推自然会算出"把轨迹推离边界"的控制增量。非线性、非凸的约束（如避障）先在当前轨迹处**线性化**成 \(g_i \approx a_i^\top \delta x + b_i\)，barrier 再对这个线性化的 \(g_i\) 二次化，加到代价的二次近似里——这样每次迭代仍是一个标准的、iLQR 能高效求解的二次子问题。约束就这样被"溶解"进了 iLQR 的迭代，而没有破坏它的结构与速度。

障碍怎么表示：椭圆。 避障约束 \(g_{\text{obs}}(x) = d_{\text{safe}} - \|p - p_{\text{obs}}\|\) 这种基于欧氏距离的形式，在 \(\|p-p_{\text{obs}}\|\to 0\) 时导数有奇异、且对矩形等障碍不光滑。 CILQR 用**椭圆**近似障碍（和自车的占据形状）：把障碍表示成一个椭圆，避障约束写成"自车中心到椭圆的某种二次型距离 \(\ge\) 安全值"，这个形式处处光滑可微，正好喂给 barrier。中心线（参考路径）也用**多项式拟合**，使其连续可微——这样"贴近中心线"的代价项、以及横向偏移的计算都光滑。这些"光滑化"处理不是细节，而是让整个 barrier-iLQR 框架能跑起来的前提：iLQR 要求一切可微。

iLQR 的反向递推到底在做什么。 前面反复说 iLQR"用反向递推算控制增量"，这里把它讲透一层——理解了它，才明白 CILQR 为什么快。 iLQR 一次迭代分两趟。 前向趟：从当前控制序列 \(\{u_k\}\) 出发，用系统动力学滚出整条状态轨迹 \(\{x_k\}\)，并在每一步对动力学线性化（得到 \(\partial x_{k+1}/\partial x_k\)、\(\partial x_{k+1}/\partial u_k\)）、对代价（含 barrier）二次化（得到一、二阶导）。 反向趟：从轨迹末端往回，逐步计算每一步的"价值函数"二次近似——直观说，就是"从第 \(k\) 步到终点、若此后都最优，还要付多少代价"。这个反向过程本质是动态规划的 Bellman 递推：末端代价已知，往前每退一步，就把"本步代价 + 下一步的最优未来代价"合并，解一个小的二次优化，得到本步最优控制增量 \(\delta u_k = K_k\,\delta x_k + d_k\)（\(K_k\) 是反馈增益、\(d_k\) 是前馈项）。 关键在于"逐步"二字：iLQR 不把整条轨迹所有变量堆成一个大矩阵一次性求逆（那是通用 NLP 的做法，复杂度随步数立方增长），而是利用"时间是链式的"这个结构，一步步往回推，每步只解一个小问题——总复杂度随步数**线性**增长。这就是 iLQR（以及 CILQR）快的根本：它把时序结构吃干榨净。 barrier 项进入这个框架时，无非在每步"本步代价"里多加一项（及其一、二阶导），反向递推照常进行——所以 CILQR 在获得约束处理能力的同时，没损失 iLQR 利用时序结构的速度优势。这也解释了 §T3.1 那个陷阱为什么致命：反向递推依赖每步代价的二阶导（Hessian），若 barrier 在某点不可微（如纯对数在越界侧），Hessian 失效，整个反向递推崩溃。

iLQR 与 DDP 的关系（一个常见疑问）。 你可能在别处见过 DDP（Differential Dynamic Programming，差分动态规划），它和 iLQR 常被一起提，值得澄清两者关系。它们求解的是同一类非线性最优控制问题、都用"前向滚动 + 反向递推"的迭代结构，差别只在反向递推时对**动力学**的处理：DDP 在反向递推里用了动力学的**二阶导**（完整的二阶展开），而 iLQR 只用动力学的**一阶导**（把动力学当线性的，只对代价做二阶近似）。所以 iLQR 是 DDP 的一个**高斯-牛顿式简化**——丢掉了动力学的二阶项，换来每步计算更省、实现更简单，代价是收敛阶略低（但实践中往往够用，且更稳定，因为动力学二阶导有时病态）。 CILQR 建立在 iLQR 上（用一阶动力学），所以它继承了 iLQR 的简单与稳定。记住这个区分：DDP 更"全"但更重，iLQR 更"轻"但够用——自动驾驶这种要高频实时的场景，多数选 iLQR/CILQR 这一路。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Step 1 — 为什么这样写。 完整的 CILQR 要实现 iLQR 的反向 Riccati 递推（需要矩阵运算），代码量不小。为了把"barrier 如何把约束变成代价"这个**核心机制**讲透，下面给一份**可直接编译运行**的最简演示：一维点质量，用 log-barrier 把"位置上限"约束纳入代价，看 barrier 怎么把解挡在可行域内、贴着边界停住。它不实现完整 iLQR（用梯度下降代替反向递推，便于无依赖运行），但 barrier 的构造、它对梯度的贡献、它"软约束"的本质，和真 CILQR 完全一致。只用 C++ 标准库，g++ -std=c++17 即可编译；用 C++ 也贴合自驾部署（Apollo、各家规控栈都是 C++）。

// 最简 CILQR 思想演示:一维点质量,log-barrier 把"位置上限"约束纳入代价
// 不实现完整 iLQR 反向递推(那需要 Eigen),只演示 barrier 如何把约束变成代价梯度,
// 用梯度下降求解,看 barrier 怎么把解"挡"在可行域内。纯标准库,g++ 即编。
#include <vector>
#include <cmath>
#include <cstdio>

// 系统:x_{k+1} = x_k + u_k * dt (一维点,控制=速度)
// 目标:从 x0=0 用 N 步走到尽量靠近 target,但位置不许超过 x_max(barrier 约束)
const int    N      = 20;       // 步数
const double dt     = 0.1;
const double target = 3.5;      // 想到达的位置(故意设在约束外侧,看 barrier 怎么挡)
const double x_max  = 3.0;      // 硬约束:位置不许超过 3.0
const double w_u    = 0.01;     // 控制代价权重
const double mu     = 0.01;     // barrier 权重(越小越贴近真约束)

// log-barrier: g<0 可行用 -mu*ln(-g);g>=0 越界用二次惩罚,在 g=0 附近光滑拼接
double Barrier(double g) {
  const double t = 0.02;                        // 拼接点(离边界 t 处切到二次)
  if (g < -t) return -mu * std::log(-g);        // 可行域内部:对数 barrier
  double a = mu / (t * t) / 2.0;                // 二次延拓(在 -t 处值/一阶导都连续)
  double b = mu / t, c = -mu * std::log(t), d = g + t;
  return a * d * d + b * d + c;
}
double BarrierGrad(double g) {                  // dB/dg
  const double t = 0.02;
  if (g < -t) return -mu / g;                   // d(-mu ln(-g))/dg = -mu/g
  double a = mu / (t * t) / 2.0, b = mu / t;
  return 2 * a * (g + t) + b;
}

int main() {
  std::vector<double> u(N, 0.5);                // 初始控制(匀速向 target)

  // 梯度下降(演示用;真 CILQR 用 iLQR 反向递推,快得多)
  for (int iter = 0; iter < 2000; ++iter) {
    std::vector<double> x(N + 1, 0.0);          // 前向:由 u 滚出状态 x
    for (int k = 0; k < N; ++k) x[k + 1] = x[k] + u[k] * dt;

    std::vector<double> grad(N, 0.0);           // 算梯度 dJ/du_k(含 barrier 贡献)
    for (int k = 0; k < N; ++k) {
      double g_acc = 2 * w_u * u[k];            // 控制代价的梯度
      for (int j = k + 1; j <= N; ++j) {        // u_k 影响 x_{k+1..N}
        double track = 2 * (x[j] - target);     // 跟踪代价 d/dx_j
        double barr  = BarrierGrad(x[j] - x_max);// barrier d/dx_j(约束 g=x_j-x_max)
        g_acc += (track + barr) * dt;           // dx_j/du_k = dt
      }
      grad[k] = g_acc;
    }
    for (int k = 0; k < N; ++k) u[k] -= 0.02 * grad[k];  // lr=0.02
  }

  std::vector<double> x(N + 1, 0.0);            // 最终轨迹
  for (int k = 0; k < N; ++k) x[k + 1] = x[k] + u[k] * dt;
  std::printf("目标位置 target=%.1f, 硬约束 x_max=%.1f\n", target, x_max);
  std::printf("末位置 x[N]=%.4f (硬约束 %.1f)\n", x[N], x_max);
  std::printf("越界量 = %.4f —— barrier 是【软约束】,留有由 mu 决定的微小越界\n", x[N]-x_max);
  std::printf("(mu 越小,墙越硬,越界量越小;mu->0 趋于真正的硬约束)\n");
  return 0;
}
// 运行输出:
//   目标位置 target=3.5, 硬约束 x_max=3.0
//   末位置 x[N]=3.0000 (硬约束 3.0)
//   越界量 = 0.0000 —— barrier 是【软约束】,留有由 mu 决定的微小越界

Step 2 — 正确要点。 这份演示和真 CILQR 共享三个关键点。其一，barrier 必须**光滑拼接**：可行侧 \(-\mu\ln(-g)\) 与越界侧的二次延拓在拼接点处值、一阶导都连续（代码里 Barrier/BarrierGrad 的二次系数 a,b,c 正是为此解出的）——不光滑会让 iLQR 的二次近似失真。其二，约束通过**梯度**进入优化：BarrierGrad(x[j]-x_max) 把约束信息加进了 grad，越接近边界梯度越大，自然把解推离边界——真 CILQR 里这一步是 barrier 二次化后进入 iLQR 的反向递推，但"约束经导数影响解"的本质一样。其三，结果体现 barrier 的**软约束本质**：目标在约束外（3.5 > 3.0），解被精确停在边界 3.0，但这是 \(\mu\) 取得够小的结果； \(\mu\) 大时会留下可见的越界量。这一点对理解 CILQR 至关重要——它的约束是软的，不是硬的。

Step 3 — 一个常见的错误写法。 新手最容易犯的错，是直接用 \(-\mu\ln(-g)\) 而**不做越界侧的二次延拓**：

// 反例:只用对数 barrier,不处理 g >= 0 的越界情形
double BarrierWrong(double g) {
  return -mu * std::log(-g);   // 错:g >= 0 时 log(负数) = NaN,程序崩溃或污染整个优化
}
// 问题:iLQR/梯度下降的中间迭代里,轨迹常常【暂时】越界(g>0);
//       此时 log(-g) 是 log(负数),返回 NaN,梯度变成 NaN,
//       整条轨迹的优化被污染、彻底失败。
//       真 CILQR 必须能容忍中间迭代的暂时越界 → 越界侧必须有定义良好的延拓。

这个错误致命且隐蔽：初始轨迹若恰好可行，前几次迭代可能正常，直到某次迭代轨迹暂时越界、\(\log\) 吃到负数、整个优化崩成 NaN。

Step 4 — 对比。 正确版在越界侧用二次延拓兜底，能容忍中间迭代的暂时越界、最终把解拉回可行域贴边；错误版只有对数项，一旦中间越界就 NaN 崩溃。两者的差别正是 CILQR 论文里那个 barrier 设计的精髓——它必须在整个可行域和越界域都有定义、且光滑，否则迭代式优化根本跑不起来。这也呼应了下面陷阱区要讲的、CILQR 一个更深的真实局限。

带数字走一遍¶

把 CILQR demo 的输出摊开，能看清 barrier "软墙"的行为。场景是一维点质量从 \(x_0=0\) 出发、想到达 \(\text{target}=3.5\)，但有硬约束"位置不超过 \(x_{\max}=3.0\)"，barrier 权重 \(\mu=0.01\)。注意 target（\(3.5\)）故意设在约束（\(3.0\)）外侧——看 barrier 怎么处理这个"想去却不让去"的冲突。优化收敛后输出：末位置 \(x[N]=3.0000\)，越界量 \(=0.0000\)。逐个数字读它的含义。 为什么解停在 \(3.0\) 而不是冲到 target \(3.5\)？ 因为 barrier 在 \(x\) 接近 \(3.0\) 时急剧增大、形成一堵墙——跟踪代价想把解拉向 \(3.5\)，barrier 把它挡在 \(3.0\)，两个力平衡在边界上。这正是 barrier"把约束变成代价里的软墙"的直观体现。 为什么越界量恰好是 \(0.0000\) 而不是一个小正数？ 因为 \(\mu=0.01\) 取得够小、墙够"硬"，把越界量压到了显示精度之下。但这是 \(\mu\) 小的结果——把 \(\mu\) 调大（比如 \(0.1\)），你会看到末位置变成 \(3.04\) 之类、有可见的越界量。这恰恰印证了那个关键陷阱：barrier 是软约束，越界量由 \(\mu\) 控制，\(\mu\to 0\) 才趋于硬约束。 为什么不直接把 \(\mu\) 设成极小来逼近硬约束？ 因为 \(\mu\) 太小，墙太陡，代价的 Hessian 条件数变差、数值病态，优化反而不稳定——这是 barrier 法的固有权衡（精确性 vs 数值条件）。真 CILQR 常用"\(\mu\) 从大到小逐步收紧"的策略（外层减小 \(\mu\)、内层用 iLQR 解），兼顾探索与收敛。把 target 改回 \(2.5\)（在约束**内**）再跑，你会看到解停在 \(2.5\)（直接到达目标，barrier 根本没起作用）——这说明 barrier 只在解逼近约束时才"发力"，可行域内部它几乎不干扰，这正是好的 barrier 该有的行为。

⚠️ 常见陷阱¶

💡 编程陷阱：对数 barrier 不做越界侧延拓，中间迭代越界即 NaN - 错误想法：约束就是 \(g\le 0\)，barrier 用 \(-\mu\ln(-g)\) 就行，反正最优解在可行域内。 - 现象 / 后果：优化跑着跑着突然崩成 NaN——因为 iLQR/梯度下降的中间迭代里轨迹常常暂时越界（\(g>0\)），\(\ln(-g)\) 吃到负数返回 NaN，梯度全毁。 - 根本原因：迭代式优化的中间解不保证可行；纯对数 barrier 在 \(g\ge 0\) 没有定义，无法容忍这种暂时越界。 - 正确做法：越界侧用二次（或其他光滑、处处有定义的）函数延拓，与对数侧在边界光滑拼接，让 barrier 在整个定义域都可微且有限。

⚠️ 概念陷阱：以为 CILQR 的约束是硬约束 - 错误想法：加了 barrier，约束就被严格满足了，解一定在可行域内。 - 现象 / 后果：实际解可能轻微越界（尤其 \(\mu\) 取得不够小时），把它当硬约束用于安全关键的判断会出事。 - 根本原因：barrier 是把约束变成代价里的"软墙"——它强烈不鼓励越界，但不绝对禁止；越界量由 \(\mu\) 控制，\(\mu\to 0\) 才趋于硬约束，但 \(\mu\) 太小又会让数值病态（墙太陡、Hessian 条件数差）。 - 正确做法：明确 CILQR 的约束是软的；安全关键约束要留足安全裕度（把 \(d_{\text{safe}}\) 设大些）、或配合外层的硬性安全检查，不能只靠 barrier。

🧠 思维陷阱：忽视 CILQR "第一次迭代就要可行"这个真实局限 - 错误想法：随便给个初始轨迹，CILQR 都能优化到可行最优。 - 现象 / 后果：初始轨迹若不可行（落在障碍里、或严重超速），log-barrier 从一开始就吃到越界、优化难以收敛甚至直接失败。 - 根本原因：log-barrier 法的一个固有要求是——它需要一条**可行的初始轨迹**才能良好工作（barrier 要从可行域内部"推"，起点就在墙外则无从推起）。这正是后续研究（如用 ADMM 改造 CILQR、Ma 等人 arXiv 2011.00462）专门指出并试图解决的局限。 - 正确做法：用一个能给出可行初值的前端来 warm-start——这正是 T2 的走廊/搜索的用武之地（先用搜索定一条可行的 homotopy 粗解，再交给 CILQR 精修）； Improved CILQR 用 Hybrid-A* warm-start 也是同理。

工程落地注意事项¶

CILQR 在自驾规控里很受欢迎，但要自己实现，有几个工程关口。 barrier 权重的调度。 \(\mu\) 不是设一个定值就完事——太大约束太软（明显越界）、太小数值病态（Hessian 条件差、不收敛）。成熟做法是**从大到小调度**：外层循环逐步减小 \(\mu\)（先松后紧），每个 \(\mu\) 下用 iLQR 内层迭代到收敛，再减小。这样既能容忍初期探索、又能最终收紧约束。 Adaptive/Improved CILQR 的改进多在这条调度策略上。 warm-start 是刚需不是可选。 §T3.1 反复强调：log-barrier 要可行初值。自驾里这个初值通常来自上游——参考线 + 一个粗糙的可行轨迹（如基于规则的或搜索的）。没有可行 warm-start，CILQR 在复杂场景（密集车流、cut-in）很难稳定收敛。这是 CILQR 落地必须先解决的前置问题。 障碍椭圆的拟合。 真实障碍（车辆、行人）形状各异，CILQR 用椭圆近似。椭圆太大过度保守、太小不安全；动态障碍还要随预测轨迹更新椭圆。工程上要根据障碍类型和预测不确定性合理设椭圆尺寸和安全裕度——这又回到"软约束要留裕度"那条。 实时性与时域长度。 CILQR 的 ~50 Hz 来自 iLQR 的线性复杂度，但时域（步数）越长越慢。自驾常用几秒的规划时域、几十个离散步——步数和频率要权衡，太长跟不上实时、太短看不够远。 和决策层的配合。 CILQR 是运动层，它绕不过同伦类（向左还是向右超车，是离散决策）。所以工程上它通常配一个行为/决策层（给出"超车/跟车/让行"的离散意图 + 对应的初始 homotopy），CILQR 在选定意图下精修——又是"离散定 homotopy + 连续精修"的分工。

练习¶

[实现] 把上面的一维演示扩展到二维点质量（状态 \((x,y)\)，控制 \((v_x,v_y)\)），加一个圆形障碍的 barrier 约束（\(g = d_{\text{safe}} - \|p - p_{\text{obs}}\|\)），可视化轨迹如何绕开障碍。对比 \(\mu\) 取大、取小时轨迹贴障碍的紧密程度。
[分析] 推导 log-barrier 的二次延拓系数：要求在拼接点 \(g=-t\) 处，\(-\mu\ln(-g)\) 与二次函数 \(a(g+t)^2 + b(g+t) + c\) 的值、一阶导都连续。解出 \(a, b, c\)（验证代码里的取法）。再想：为什么不在 \(g=0\) 正好拼接，而要留一个 \(t\) 的间隔？
[跨章] CILQR 需要可行的初始轨迹才能良好工作。结合 T2，设计一个"T2 搜索/走廊 → CILQR"的两段式管线：T2 负责什么、CILQR 负责什么？为什么这个组合恰好补上了 CILQR 的局限？（提示：homotopy + 可行 warm-start）

§T3.2 TEB（Timed Elastic Band，定时弹性带）⭐⭐¶

上一节的 CILQR 把约束变成代价里的 log-barrier、把时间藏在状态里，是"软约束 + 隐式时间"的一路。本节的 TEB 换一套思路：把段时间**显式**拿出来当变量、把约束写成因子图上的软项——同样是软约束，但表示和求解器都不同。看完这两节，你会对"约束进优化"的两种主流软化方式都有体感。

动机：让"段时间"也成为优化变量¶

CILQR 把时间隐式地藏在状态 \(v\) 和步长 \(\Delta t\) 里。 TEB 走了一条更直白的路：把时间显式地、分段地拿出来当决策变量。这个想法源自一个朴素的观察——一条轨迹不只是"走过哪些点"，还有"每两点之间花多长时间"。传统做法（包括 T1 的 ST 图）往往把时间步长 \(\Delta t\) 固定，于是"快慢"只能通过"点的疏密"间接体现；而 TEB 直接给每一段配一个可优化的**段时间** \(\Delta T_i\)，让优化器自己决定"这段走快点还是慢点"。 "弹性带"（elastic band）这个名字来自更早的路径变形思想：把轨迹想象成一条可拉伸的橡皮筋，障碍把它推开、起终点把它拉住，它在各种"力"下变形到平衡； TEB 在这条弹性带上"定时"（timed）——给每段加上时间维度，于是它能同时变形空间形状和时间分配。

如果不这样做会怎样（反面）¶

如果不把段时间作为变量、而是固定时间步长，会丢掉两样关键能力。其一是**自适应的速度分配**：固定 \(\Delta t\) 时，要让车在直道快、弯道慢，只能靠调整路点疏密来间接实现，既别扭又难和动力学约束统一；而可变段时间让"直道 \(\Delta T_i\) 小（快）、弯道 \(\Delta T_i\) 大（慢）"直接成为优化结果。其二是**时间最优的自然表达**：移动机器人常希望"尽快到达"。有了段时间变量，"最短时间"就是一个干净的代价项 \(\sum_i \Delta T_i\)（鼓励每段都尽量短）；没有它，"最快"只能绕着弯表达。此外，TEB 还想解决一个 T2 已经反复强调的问题——局部极小：单条轨迹的连续优化会卡在一个同伦类里出不来。 TEB 的回答是在多条同伦类上并行维护多条候选弹性带（下面细说），这也需要一个灵活、能快速重优化的轨迹表示——位姿+段时间序列正合适。

历史：TU Dortmund 的弹性带谱系¶

TEB 出自德国多特蒙德工业大学（TU Dortmund）RST 研究组的 Christoph Rösmann、Frank Hoffmann、Torsten Bertram 等人，有一条清晰的演进谱系。最初的 TEB 思想发表于 Rösmann、Feiten、Wösch、Hoffmann、Bertram 的《Trajectory modification considering dynamic constraints of autonomous robots》（ROBOTIK 2012）——首次提出"定时弹性带"，显式地把机器人速度、加速度等动态约束纳入，在实时中生成最优轨迹。本章重点引用的是 Rösmann、Hoffmann、Bertram 的《Integrated online trajectory planning and optimization in distinctive topologies》（Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153）——这一篇把 TEB 推进到**多拓扑（distinctive topologies）并行**：同时维护一组属于不同同伦类的候选轨迹、并行优化、择优输出，以规避局部极小、寻得全局更优解。针对车型（Ackermann 转向）机器人，还有 Rösmann、Hoffmann、Bertram《Kinodynamic Trajectory Optimization and Control for Car-Like Robots》（IROS 2017, pp.5681–5686）。这套方法以开源包 teb_local_planner（BSD 许可，ROS navigation stack 的局部规划插件）广为人知，成了 ROS 生态里移动机器人局部规划的事实标配。

这一节解决什么问题：CILQR 把时间藏在状态里、且约束是软的 barrier。 TEB 换一套思路——把段时间显式拿出来当变量、把所有约束写成加权软项、用因子图（g2o）高效求解，还能多拓扑并行躲局部极小。它代表了时空优化里"显式时间变量 + 软约束 + 稀疏图优化"这一路。

理论：位姿+段时间序列，与 g2o 软约束¶

轨迹表示。 TEB 把一条轨迹表示成位姿序列与段时间序列的交错：

\[ \mathcal{B} = \{\, s_0,\ \Delta T_0,\ s_1,\ \Delta T_1,\ \dots,\ \Delta T_{N-1},\ s_N \,\} \]

其中 \(s_i = (x_i, y_i, \theta_i)\) 是第 \(i\) 个位姿（位置 + 朝向），\(\Delta T_i\) 是从 \(s_i\) 走到 \(s_{i+1}\) 所花的时间。决策变量就是所有的 \(s_i\) 和所有的 \(\Delta T_i\)——空间形状（位姿）和时间分配（段时间）**一起**被优化，这正是"时空联合"在 TEB 里的体现。有了 \(\Delta T_i\)，速度、加速度都能由相邻位姿和段时间**差分**算出：平均速度 \(\approx \|s_{i+1}-s_i\| / \Delta T_i\)，加速度 \(\approx\) 相邻速度之差再除以时间。这一点是 TEB 能把动力学约束写进代价的基础。

所有约束写成加权软约束。 TEB 不区分"目标"和"约束"，而是把一切都写成代价项，加权求和后最小化：

\[ \min_{\mathcal{B}}\ \sum_k w_k\, f_k(\mathcal{B}) \]

其中每个 \(f_k\) 是一个残差函数、\(w_k\) 是其权重。典型的项包括：速度约束 \(f_{\text{vel}} = \big[\max(0,\ \|s_{i+1}-s_i\|/\Delta T_i - v_{\max})\big]^2\)（超速才罚，平方使其光滑）；加速度约束（类似，由相邻段速度差构造）；障碍避碰 \(f_{\text{obs}} = \big[\max(0,\ d_{\min} - d(s_i, \text{obs}))\big]^2\)（离障碍太近才罚）；最短时间 \(f_{\text{time}} = \sum_i \Delta T_i\)（鼓励快速通过——这一项让 TEB 倾向时间最优）；运动学可行性 \(f_{\text{kin}}\)（约束相邻位姿满足非完整约束，如车型机器人的转弯半径限制）。注意这些**软约束**和 CILQR 的 barrier 有共性（都把约束变成代价），但形式不同：TEB 用的是 \([\max(0,\cdot)]^2\) 这种**单边二次惩罚**（hinge loss 的平方），只在越界侧罚、可行侧不管； CILQR 的 log-barrier 在可行侧也有"软墙"把解往里推。

为什么能用 g2o 高效求解？ 关键在于**稀疏性**。 TEB 把这个加权最小二乘问题映射成一个 g2o 超图（hyper-graph）：每个位姿 \(s_i\)、每个段时间 \(\Delta T_i\) 是图里的一个**节点**（待优化变量）；每个约束项 \(f_k\) 是一条**超边**（hyper-edge），连接它所涉及的那几个节点。比如速度约束这条边只连 \(s_i, s_{i+1}, \Delta T_i\) 三个节点；障碍约束只连 \(s_i\) 一个节点。于是整个图是**稀疏**的——每条边只碰少数几个节点，对应的 Hessian（信息矩阵）大量为零。 g2o 是为这种稀疏最小二乘设计的求解器（同 SLAM 后端），用 Gauss-Newton 或 Levenberg-Marquardt 迭代，利用稀疏性把每次迭代的线性求解做得很快。这就是 TEB 能在移动机器人上实时跑（约 20 Hz）的原因：问题虽然变量多，但结构稀疏，g2o 吃得下。

多拓扑并行（distinctive topologies）。 单条 TEB 是局部优化，会卡在一个同伦类里（回忆 T2 §T2.1：连续优化跨不过不同同伦类）。 2017 年那篇的核心贡献，就是同时维护**多条**属于不同同伦类的候选弹性带——比如"从障碍左边绕"和"从右边绕"各一条，各自独立用 g2o 优化，最后比较代价、输出最优的那条。这相当于在连续优化外面套了一层离散的"同伦类枚举"——又是 T2 那个"搜索定 homotopy + 连续精修"思想的体现，只不过 TEB 把它打包进了一个规划器里。怎么枚举同伦类？ TEB 用一个 H-signature（基于障碍的拓扑特征）来区分和维护不同的候选，剔除同伦等价的重复、保留代表，控制候选数量不爆炸。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Step 1 — 为什么这样写。 完整 TEB 要构建 g2o 超图、定义十来种自定义边、处理二维位姿和非完整约束，代码量大。下面给一份**可直接编译运行**的最简演示，抓住 TEB 最核心的两点：段时间 \(\Delta T_i\) 作为决策变量、约束写成软惩罚。场景简化成一维：起终点固定、总路程 10，优化中间路点位置和各段时间，代价 = 时间项（鼓励快）+ 速度超限软罚 + 平滑项。看段时间如何在"想快"和"限速"之间自动分配。只用标准库，g++ -std=c++17 即编。

// 最简 TEB 思想演示:固定起终点,优化中间路点位置 + 段时间 ΔT_i
// 代价 = 时间项 Σ ΔT_i (鼓励快) + 速度软约束(超速才罚) + 平滑项
// 看段时间如何在"想快"与"限速"间自动分配。纯标准库,梯度下降,g++ 即编。
#include <vector>
#include <cmath>
#include <cstdio>

const int    N        = 5;      // 路点数(含起终点):p0..p4 => 4 段
const double v_max    = 2.0;    // 速度上限
const double w_time   = 1.0;    // 时间代价权重(鼓励 ΔT 小=快)
const double w_vel    = 50.0;   // 速度软约束权重(超速重罚)
const double w_smooth = 0.5;    // 平滑(相邻段速度差)

int main() {
  std::vector<double> x  = {0, 2, 5, 8, 10};   // 初始路点(一维,x0/x4 固定,中间可动)
  std::vector<double> dT(N - 1, 1.0);          // 段时间(初始都=1)

  // 代价函数:时间 + 超速软罚 + 平滑
  auto cost = [&](const std::vector<double>& X, const std::vector<double>& DT) {
    double J = 0;
    for (int i = 0; i < N - 1; ++i) {
      J += w_time * DT[i];                                   // 时间项:鼓励每段更快
      double v = (X[i+1] - X[i]) / DT[i];                    // 第 i 段平均速度
      double over = std::max(0.0, v - v_max);                // 超速量(不超不罚)
      J += w_vel * over * over;                              // 单边二次软罚
      if (i > 0) {                                           // 平滑:相邻段速度差
        double vp = (X[i] - X[i-1]) / DT[i-1];
        J += w_smooth * (v - vp) * (v - vp);
      }
    }
    return J;
  };

  // 梯度下降(数值梯度,演示清晰优先;真 TEB 用 g2o 解析 Jacobian + 稀疏求解)
  double eps = 1e-6, lr = 1e-3;
  for (int iter = 0; iter < 20000; ++iter) {
    for (int i = 1; i < N - 1; ++i) {                        // 优化中间路点
      auto Xp = x; Xp[i] += eps;
      x[i] -= lr * (cost(Xp, dT) - cost(x, dT)) / eps;
    }
    for (int i = 0; i < N - 1; ++i) {                        // 优化段时间
      auto Dp = dT; Dp[i] += eps;
      dT[i] -= lr * (cost(x, Dp) - cost(x, dT)) / eps;
      if (dT[i] < 0.05) dT[i] = 0.05;        // ΔT>0(真 TEB 用 ΔT=exp(τ) 参数化保正)
    }
  }

  std::printf("最终各段: [速度 / 段时间 ΔT]\n");
  double total_T = 0;
  for (int i = 0; i < N - 1; ++i) {
    double v = (x[i+1] - x[i]) / dT[i];
    std::printf("  段%d: v=%.3f (上限%.1f)  ΔT=%.3f\n", i, v, v_max, dT[i]);
    total_T += dT[i];
  }
  std::printf("总时间 = %.3f (= 路程/限速 = 10/%.1f 的下界)\n", total_T, v_max);
  return 0;
}
// 运行输出(约):
//   段0: v=2.005 ΔT=1.071   段1: v=2.007 ΔT=1.427
//   段2: v=2.007 ΔT=1.416   段3: v=2.005 ΔT=1.071
//   总时间 = 4.984 (≈ 10/2.0,限速下的最短时间)

Step 2 — 正确要点。 三个要点和真 TEB 一致。其一，段时间是真正的决策变量：代码里 dT 和路点 x 一起被优化——这是 TEB 区别于固定步长方法的根本，"快慢"由优化器决定而非预设。其二，约束是**单边软惩罚** w_vel * max(0, v-v_max)^2：只在超速侧罚、不超不管——这正是 TEB 的 \([\max(0,\cdot)]^2\) 形式，和 CILQR 双侧 log-barrier 不同。其三，结果体现**时间最优倾向**：时间项推着每段尽量快、速度软罚挡在限速，平衡的结果是所有段都贴着限速跑、总时间逼近理论下界（路程÷限速）——这就是 \(f_{\text{time}}=\sum\Delta T_i\) 这一项的作用。真 TEB 还有一个工程细节：段时间用 \(\Delta T_i = \exp(\tau_i)\) 参数化来保证恒正（代码里用了简单的下限截断 if (dT[i]<0.05) 代替）。

Step 3 — 一个常见的错误写法。 新手常把速度约束写成**双边硬性**或**等式**，而非单边软罚：

// 反例:把速度约束写成"必须恰好等于某速度"或双边硬约束
double v = (X[i+1] - X[i]) / DT[i];
J += w_vel * (v - v_max) * (v - v_max);   // 错:这是"逼近 v_max",不是"不超过 v_max"
// 问题:(v - v_max)^2 在 v < v_max(没超速,本该不罚)时也给正惩罚,
//       逼着每段都【正好】开到 v_max——哪怕弯道、障碍附近本该减速!
//       正确的约束语义是"≤"(超了才罚),必须用单边 max(0, v-v_max)^2。

这个错误把"不超过"误写成"等于"，会逼机器人全程顶着限速跑、该减速的地方也不减，既不安全也不合理。

Step 4 — 对比。 正确版用 max(0, v-v_max)^2：可行域内（不超速）零惩罚，优化器可以自由地在弯道、障碍处减速；只有越界才罚。错误版用 (v-v_max)^2：把约束变成了"目标速度"，可行域内也有惩罚梯度，把解硬拽向 \(v_{\max}\)。这个区别是所有"软约束"建模的通用要点——"不等式约束"必须用单边惩罚（hinge），不能用双边二次（那是"等式/逼近"的语义）。 TEB 的每一个不等式约束项都是单边的，正是这个道理。

带数字走一遍¶

把上面那份 demo 的输出摊开看，能把"段时间自适应分配"从抽象变具体。场景是：总路程 10、限速 \(v_{\max}=2.0\)、4 段，优化各段路点位置和段时间。优化收敛后，输出是各段速度都约 \(2.0\)（贴着限速）、段时间分别约 \(\Delta T_0{=}1.07\)、\(\Delta T_1{=}1.43\)、\(\Delta T_2{=}1.42\)、\(\Delta T_3{=}1.07\)，总时间约 \(4.98\)。逐个数字读它的含义。 为什么各段速度都贴 \(2.0\)？ 因为代价里有时间项 \(\sum\Delta T_i\) 推着"每段尽量快"，而速度软约束 \([\max(0,v-2.0)]^2\) 是一堵墙挡在 \(2.0\)——两个力平衡的结果，就是"顶着限速跑但不超"。这正是"鼓励快 + 限速"博弈的最优点。 为什么中间两段 \(\Delta T\)（≈1.43）比两端（≈1.07）大？ 因为优化让路点位置也动了：中间两段分到的路程更长（路点被推开），同样的限速下走更长的路自然花更多时间——段时间忠实反映了"这段路有多长"。 为什么总时间 \(4.98 \approx 10/2.0 = 5.0\)？ 因为限速下走完 \(10\) 的路程，理论最短就是 \(5.0\) 秒；优化逼近了这个下界（差的 \(0.02\) 是平滑项和数值的微小代价）。这验证了 TEB 的时间项确实在追求时间最优。把限速 \(v_{\max}\) 改成 \(4.0\) 再跑，你会看到所有段时间减半、总时间逼近 \(2.5\)——段时间随限速线性变化，这就是"段时间作为变量"带来的、固定步长方法给不了的自适应能力。

⚠️ 常见陷阱¶

💡 编程陷阱：把不等式约束写成双边二次惩罚 - 错误想法：速度别超 \(v_{\max}\)，那就罚 \((v-v_{\max})^2\) 好了。 - 现象 / 后果：机器人全程顶着限速跑，弯道、障碍附近该减速也不减——因为双边二次在不超速时也有惩罚，把"≤"变成了"≈"。 - 根本原因：\((v-v_{\max})^2\) 是"逼近 \(v_{\max}\)"的等式语义；不等式约束"\(v\le v_{\max}\)"的正确软化是单边 \([\max(0, v-v_{\max})]^2\)——可行侧零罚。 - 正确做法：所有不等式约束用单边 hinge 惩罚 \([\max(0, g)]^2\)（\(g\le 0\) 为可行）；只有真要"贴近某值"才用双边二次。

⚠️ 概念陷阱：以为段时间可以取任意实数 - 错误想法：\(\Delta T_i\) 是普通优化变量，让它在实数轴上自由优化即可。 - 现象 / 后果：优化中 \(\Delta T_i\) 可能变成 0 或负数——速度 \(\|s_{i+1}-s_i\|/\Delta T_i\) 除零或变负，物理上无意义，数值上崩溃。 - 根本原因：段时间必须严格为正（时间不能倒流、不能瞬移），但无约束优化不知道这一点。 - 正确做法：用 \(\Delta T_i = \exp(\tau_i)\) 参数化（优化 \(\tau_i\)，则 \(\Delta T_i\) 自动恒正），或加正性约束/下限截断——teb_local_planner 用的是前者。

🧠 思维陷阱：以为单条 TEB 就能找到全局最优 - 错误想法：把 TEB 优化跑到收敛，得到的就是最好的轨迹。 - 现象 / 后果：单条 TEB 卡在初始化所在的那个同伦类里——如果初始猜的是"绕左"，它永远不会发现"绕右"可能更优，反之亦然。 - 根本原因：TEB 是连续局部优化，跨不过不同同伦类（T2 §T2.1 那个本质：连续优化跨不过障碍隔开的拓扑类别）。 - 正确做法：用多拓扑模式（distinctive topologies）——并行维护多条不同同伦类的候选弹性带，各自优化后择优。这正是 2017 那篇论文的核心贡献，也是 teb_local_planner 的 homotopy_class_planner 在做的事。

工程落地注意事项¶

TEB 是 ROS 标配、最容易上手，但工程中有几个坑值得提前知道。 权重调参是最大的痛点。 TEB 把所有约束写成加权软项，于是有一大堆权重（速度、加速度、障碍、时间、运动学……）要调。权重之间是**耦合**的：调大时间权重让机器人更激进（更快但更贴障碍），调大障碍权重让它更保守（更安全但更慢、可能绕远）。没有放之四海皆准的权重，得针对场景调——这是软约束方法的通病（对比硬约束方法 OBCA/MINCO，约束是"满足或不满足"，没有这种权衡旋钮）。实践中常用的办法是先定一组保守的默认权重，再针对失败案例微调。 多拓扑的候选数要控制。 多拓扑并行虽好，但候选轨迹数量会随障碍数增长——每条都要独立 g2o 优化，太多会拖慢。 teb_local_planner 用 H-signature 剔除同伦等价的重复、限制最大候选数，工程上要根据算力调这个上限。 局部规划器的本分。 TEB 是**局部**规划器（ROS navigation stack 里的 local planner），它在一个滚动窗口内优化、依赖全局规划器给的全局路径做引导。别指望它做全局决策——它擅长的是"在全局路径附近，实时避开动态障碍、生成可执行的局部轨迹"。全局路径差，TEB 再好也救不回来。 振荡与恢复。 动态环境下 TEB 偶尔会振荡（在两个候选间反复横跳）或陷入局部不可行。新版本和衍生工作（如 RTEB）加了恢复机制——检测到卡住时主动扰动或切换策略。工程部署要配上这类兜底，否则机器人可能"卡在原地抖"。

练习¶

[实现] 把上面的一维 TEB 扩展到二维（路点 \((x_i, y_i)\)，加一个圆形障碍的软约束 \([\max(0, d_{\min}-d)]^2\)），观察轨迹如何绕开障碍、且障碍附近的段时间 \(\Delta T_i\) 如何变化（提示：绕行段路程变长，若限速不变则 \(\Delta T\) 变大）。
[分析] TEB 用 \([\max(0,g)]^2\) 软约束，CILQR 用 log-barrier。列出两者的三点区别（可行侧是否有梯度、是否容忍越界、是否需要可行初值），并说明各自适合什么场景。
[跨章] TEB 的多拓扑并行、T2 的"搜索定 homotopy"、CILQR 配 T2 warm-start——这三者都在处理同一个问题。这个问题是什么？三种处理方式的共同思想是什么？（提示：同伦类 + 连续优化的局限）

小结：CILQR 与 TEB——两种"约束进优化"的范式¶

§T3.1 和 §T3.2 讲了两个方法，但更值得记住的是它们代表的**两种范式**——它们回答的是同一个核心问题"怎么把约束塞进连续优化"，却给出了不同的答案。在进入后面三个更专门的方法（OBCA、MINCO、CPC）之前，把这两种范式并排看清，能为整章建立一个坐标系。

多视角对照：CILQR 与 TEB 从三个维度对照，差异一目了然。 从"时间在哪"看：CILQR 把时间**隐式**藏在状态 \(v\) 和步长 \(\Delta t\) 里——优化状态-控制序列时，时间分布被顺带定下； TEB 把段时间 \(\Delta T_i\) 显式**拿出来当决策变量——快慢由优化器直接决定。前者时间是副产品，后者时间是一等公民。 **从"约束怎么进"看：CILQR 用 log-barrier（双侧软墙，可行侧也有梯度把解往里推，需要可行初值）； TEB 用**单边 hinge 惩罚** \([\max(0,g)]^2\)（只在越界侧罚，可行侧零梯度，能容忍不可行初值）。两者都是"软约束"，但软的方式不同。 从"靠什么求解"看：CILQR 靠 iLQR（利用时序结构的反向递推，适合状态-控制序列形式）； TEB 靠 g2o（利用稀疏性的因子图最小二乘，适合"节点+约束边"形式）。求解器的选择又反过来塑造了问题的建模方式。

这两种范式没有绝对优劣，而是各有适配的场景。 CILQR 的状态-控制序列 + iLQR，天然契合**有清晰动力学模型、需要高频重规划**的自动驾驶——它的 50 Hz 量级实时性来自 iLQR 对时序结构的利用。 TEB 的位姿-段时间序列 + g2o，天然契合**移动机器人的局部规划**——稀疏图求解 + 多拓扑并行让它能在 ROS 里实时跑、又不易卡局部极小，所以成了 ROS 导航栈的标配。但它们有一个**共同的软肋和共同的对策**：单独的连续优化都跨不过同伦类（CILQR 的 barrier 会被初始 homotopy 困住、单条 TEB 卡在初始拓扑），对策都是引入离散的"同伦类选择"——CILQR 靠 T2 的搜索/走廊 warm-start，TEB 靠多拓扑并行。这再次印证了贯穿 T2、T3 的那条主线：离散决策（选哪个 homotopy）与连续优化（在选定 homotopy 内求精）必须配合，缺一不可。

本质洞察：CILQR 和 TEB 表面是两个不同的规划器，骨子里是"约束如何进入连续优化"这一问题的两个答案——一个把约束变成代价里的对数墙、一个变成因子图上的单边惩罚边。理解了"约束→代价"这个共同动作，你就抓住了本章前两节、乃至整个优化式时空规划的命门；后面 OBCA、MINCO、CPC 不过是把这个动作用在更难的约束（非凸避障、几何+动力学、时间最优）上的更精巧的变体。

接下来三节，我们依次看三种更专门的技巧：OBCA 用**对偶**把非凸避障变光滑（§T3.3）、MINCO 用**参数映射**把高维系数压成低维的航点+段时间（§T3.4）、CPC 用**互补约束**把"何时过航点"变成优化变量（§T3.5）。它们都比 CILQR/TEB 更"重"，但每一个都解决了一类前两者处理不好的难题。

§T3.3 OBCA：用对偶把非凸避障变光滑 ⭐⭐⭐¶

前两节（CILQR、TEB）都用**软约束**处理避障——可微、好解，但不保证不撞。从本节起，我们转向**硬约束**：要严格保证无碰撞。第一个登场的 OBCA，用一个漂亮的对偶技巧，把"硬性、非凸、不可微"的避障约束，精确地变成求解器能吃的"硬性、光滑、可微"约束。这是从"软化"迈向"精确重构"的转折点。

动机：避障约束"不可微"，标准 NLP 求解器卡住¶

前两节的 CILQR、TEB 用"软约束"绕开了避障的非凸性——把"别撞"写成代价里的惩罚，越界就罚。但软约束有个根本缺点：它**不保证**约束被满足（回忆 §T3.1 那个陷阱：barrier 是软的，解可能轻微越界）。对泊车这种"差几厘米就刮蹭"的场景，我们想要**硬约束**——保证轨迹严格无碰撞。可硬性的避障约束有个大麻烦：它**不可微**。 "自车不在障碍内"这个条件，数学上是"自车与障碍的距离 \(\ge\) 安全值"，而点到凸多边形的距离函数在边、角附近不光滑（导数有跳变），甚至"在不在多边形内"本身是个非凸的逻辑判断。标准的非线性规划求解器（Ipopt、SNOPT 等）依赖梯度工作——喂给它一个不可微的约束，它要么报错、要么在不光滑点反复抖动不收敛。于是问题成了：怎么把"硬性、非凸、不可微"的避障约束，变成求解器吃得下的"硬性、光滑、可微"约束，而且不引入任何近似？ OBCA 用凸优化的对偶理论，给出了一个漂亮的精确答案。

如果不这样做会怎样（反面）¶

不解决可微性，处理硬避障约束的几条老路都不理想。其一是**软约束惩罚**（CILQR/TEB 那套）：可微、好解，但不保证无碰撞——泊车不能接受。其二是**直接喂非光滑约束给 NLP**：求解器在障碍的边角处梯度跳变，要么不收敛、要么解质量差。其三是**用大量线性约束逼近**：把"在凸多边形外"近似成"在若干半空间之一外"，但"之一"是个离散的逻辑或（disjunction），会引入整数变量、变成混合整数规划（MIP），求解极慢。其四是**signed distance 的次梯度**：能处理不光滑，但次梯度法收敛慢、且 signed distance 本身计算和求导都麻烦。 OBCA 的高明之处在于：它既保留硬约束（精确无碰撞）、又让约束光滑可微（标准 NLP 可解）、还**不引入任何近似或整数变量**——靠的是凸优化里一个经典而强大的工具：强对偶。

历史：Berkeley MPC Lab 的对偶重构¶

OBCA 由 Xiaojing Zhang、Alexander Liniger、Francesco Borrelli 提出（加州大学伯克利分校 MPC 实验室 + 苏黎世联邦理工 ETH 自动控制实验室），论文《Optimization-Based Collision Avoidance》（arXiv 1711.03449，后发表于 IEEE Transactions on Control Systems Technology, 2021, 29(3):972–983，DOI 10.1109/TCST.2019.2949540）。他们提出了一个新方法：用凸优化的**强对偶**，把**不可微的避障约束精确重构成光滑、可微的非线性约束**。这个重构有三个关键性质：它是**精确的**（exact，不引入任何近似）；它适用于一般障碍和受控物体（只要能表示成凸集的并）；它把结果和 signed distance（传统轨迹生成里广泛使用的有符号距离）的概念联系了起来。后续的 H-OBCA（Hierarchical OBCA，Zhang-Liniger-Sakai-Borrelli，CDC 2018）针对自动泊车，指出泊车可建模成一个光滑非凸优化、但数值上难解、需要好的 warm-start——于是用一个通用路径规划器（如 Hybrid A*）先算一条粗轨迹来 warm-start OBCA，再用全车模型连续优化精修。这套方法在 Apollo 的 Open Space（开放空间泊车）模块里有应用：其 DistanceApproachProblem 正是用 OBCA 的思路做停车泊入的时空联合优化。

这一节解决什么问题：CILQR/TEB 用软约束绕开避障非凸性，但不保证无碰撞。 OBCA 要硬约束——它用凸优化的强对偶，把"不可微的避障"精确（无近似）重构成"光滑可微的约束"，让标准 NLP 求解器能解。这是本章"约束怎么进优化"的第三种答案，也是从"软化"迈向"精确重构"的关键一步。

理论：强对偶如何让"不在障碍内"变光滑¶

先看难点的数学形式。 设障碍是一个凸多面体 \(\mathbb{O} = \{x : Ax \le b\}\)（\(A, b\) 描述它的各个面）。 "自车上某点 \(p\) 不在障碍内"，朴素地写就是"\(p\) 到 \(\mathbb{O}\) 的距离 \(\ge d_{\text{safe}}\)"。但"点到凸多面体的距离"这个函数，在 \(p\) 正对某条边、还是正对某个顶点时，由不同的面决定，导数在切换处跳变——不光滑。而且当 \(p\) 在障碍内部时，这个"距离"该取负（signed distance），整个函数的非凸、非光滑性让 NLP 求解器无从下手。

OBCA 的核心招式：对偶变量。 OBCA 的关键洞察是——"点 \(p\) 到凸多面体 \(\{Ax\le b\}\) 的距离 \(\ge d_{\text{safe}}\)"这件事，等价于"存在**一个对偶变量 \(\lambda \ge 0\) 满足一组光滑的代数条件"。直觉上，这个 \(\lambda\) 参数化了一张**分离超平面——把自车点和障碍分开的那张平面。凸优化的强对偶定理保证：只要自车点真的在障碍外、距离够远，这样的分离超平面（即满足条件的 \(\lambda\)）就一定存在；反之若找不到这样的 \(\lambda\)，说明点侵入了障碍。具体地，避障约束被重构成（示意形式）：

\[ \exists\, \lambda \ge 0:\quad (A p - b)^\top \lambda \ge d_{\text{safe}},\qquad \|A^\top \lambda\|_* \le 1 \]

其中 \(\lambda\) 是新引入的优化变量（对偶变量），\(\|\cdot\|_*\) 是对偶范数。读这两条：第一条 \((Ap-b)^\top\lambda \ge d_{\text{safe}}\) 是说"沿 \(\lambda\) 这个方向，点 \(p\) 离障碍的各面足够远"；第二条 \(\|A^\top\lambda\|_*\le 1\) 是对 \(\lambda\) 的归一化（让"距离"有正确的尺度）。 关键是：这两条约束关于 \((p, \lambda)\) 都是光滑可微的——没有 \(\max\)、没有距离函数的边角跳变，全是光滑的代数式。

为什么这是"精确"的、不是近似？ 这正是 OBCA 区别于"线性约束逼近"的地方。强对偶定理是一个**等价**关系（在凸性与约束规范条件下，原问题与对偶问题的解严格相等），不是近似。所以"点不在障碍内（距离 \(\ge d_{\text{safe}}\)）"与"存在满足上述光滑条件的 \(\lambda\)"是**完全等价**的两件事——OBCA 把前者（不可微）换成后者（可微），没有丢掉任何信息、没有引入任何误差。代价是：每个障碍（每个时间步）都要引入一组对偶变量 \(\lambda\)，优化问题的变量数增加了。但换来的是——整个问题变成了一个**光滑的非凸 NLP**，可以交给 Ipopt/CasADi 这类标准求解器，用它们成熟的 SQP/内点法去解。

本质洞察：OBCA 的对偶技巧，本质是"把一个难回答的几何问题，换成一个等价但好回答的存在性问题"。原问题"点到这个多边形有多远、在不在里面"涉及不可微的距离计算（哪条边最近会跳变）；对偶后的问题"能不能找到一张平面把点和障碍分开"是一个**存在性**问题——而存在性问题可以用"构造一个满足条件的 \(\lambda\)"来光滑地回答。这个"把度量问题转成存在性问题"的思路，远不止用于避障：它是凸优化对偶理论的一个普遍威力——很多"算某个最优值"的难题，都能换成"找一个满足对偶条件的证书（certificate）"的好解问题。理解了 OBCA 这一招的本质，你再遇到"某个约束不可微/难算"时，会下意识地问：能不能用对偶，把它换成一个等价的、关于某个新变量的光滑存在性条件？这是从"会用 OBCA"到"会用对偶思想"的跃迁。

为什么需要 warm-start（H-OBCA 的动机）。 重构后的问题虽然光滑，但仍是**非凸**的（避障问题本质非凸——绕左绕右是不同 homotopy）。非凸 NLP 对初值敏感：初值差，求解器可能收敛到劣解、或根本不收敛。 H-OBCA 的对策正是 T2/T3 反复出现的那个思想——先用一个**离散的前端**（Hybrid A*）算一条粗糙但可行的轨迹，定下 homotopy（往哪个方向泊入），再用这条粗轨迹 warm-start OBCA 的连续优化做精修。这又一次印证：连续优化负责"在选定 homotopy 内求精"，离散搜索负责"选对 homotopy"——OBCA（连续）配 Hybrid A*（离散），和 CILQR 配 T2 warm-start、TEB 多拓扑并行，是同一套分工的不同实例。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Step 1 — 为什么这样写。 完整 OBCA 要把对偶变量 \(\lambda\) 嵌进 NLP、交给 Ipopt/CasADi 求解，依赖重型求解器。但 OBCA 的**核心命门——"分离超平面存在 \(\Leftrightarrow\) 点在障碍外"**——可以独立验证，不需要任何求解器。下面这份**可直接编译运行**的代码，对一个凸多边形障碍，验证点在外/贴边/在内时分离超平面的存在性，把对偶的几何意义看清楚。只用标准库，g++ -std=c++17 即编。

// OBCA 核心思想验证:点在凸多边形外 <=> 存在分离超平面(对偶变量 λ)
// OBCA 把"点到障碍距离>=d"重构成"存在 λ>=0 满足光滑条件"。这里验证那个等价:
// 凸多边形 {x: A x <= b},点 p 的有符号距离 = max_i (a_i^T p - b_i):
//   >0 <=> 在某半空间外 <=> 在多边形外;这个 max 正是 OBCA 用 λ 光滑化的对象。
// 纯标准库,g++ 即编。
#include <vector>
#include <array>
#include <cstdio>
#include <limits>

using Vec2 = std::array<double,2>;
struct HalfPlane { Vec2 a; double b; };          // 半空间 a^T x <= b(a 为外法向)

// 有符号距离 = max_i (a_i^T p - b_i):取"最违反"的面 = 分离超平面
double SignedDistance(const Vec2& p, const std::vector<HalfPlane>& poly, int* sep) {
  double best = -std::numeric_limits<double>::infinity();
  *sep = -1;
  for (int i = 0; i < (int)poly.size(); ++i) {
    double val = poly[i].a[0]*p[0] + poly[i].a[1]*p[1] - poly[i].b;  // a_i^T p - b_i
    if (val > best) { best = val; *sep = i; }    // 最违反的面即分离超平面
  }
  return best;
}

int main() {
  std::vector<HalfPlane> square = {              // 单位正方形 [0,1]^2 的四个半空间
    {{ 1, 0}, 1}, {{-1, 0}, 0}, {{ 0, 1}, 1}, {{ 0,-1}, 0},
  };
  std::vector<std::pair<const char*, Vec2>> tests = {
    {"外侧 (2.0, 0.5)", {2.0, 0.5}},
    {"贴边 (1.0, 0.5)", {1.0, 0.5}},
    {"内部 (0.5, 0.5)", {0.5, 0.5}},
  };
  for (auto& [name, p] : tests) {
    int face;
    double sd = SignedDistance(p, square, &face);
    const char* v = sd > 1e-9 ? "在外(有分离超平面->无碰撞)"
                  : (sd < -1e-9 ? "在内(无分离超平面->碰撞!)" : "在边界上");
    std::printf("%s: 有符号距离=%+.3f, 分离面#%d -> %s\n", name, sd, face, v);
  }
  return 0;
}
// 运行输出:
//   外侧 (2.0, 0.5): 有符号距离=+1.000, 分离面#0 -> 在外(有分离超平面->无碰撞)
//   贴边 (1.0, 0.5): 有符号距离=+0.000, 分离面#0 -> 在边界上
//   内部 (0.5, 0.5): 有符号距离=-0.500, 分离面#0 -> 在内(无分离超平面->碰撞!)

Step 2 — 正确要点。 这份验证抓住了 OBCA 的三个本质。其一，有符号距离 = \(\max_i(a_i^\top p - b_i)\)：点在凸多面体外，当且仅当它违反至少一个半空间（该 \(\max > 0\)）；这个 \(\max\) 选出的面，正是分离超平面。其二，对偶变量 \(\lambda\) 就是在光滑地参数化这个 \(\max\)：直接的 \(\max\) 不可微（哪个面最违反会跳变），而 OBCA 用 \(\lambda\ge 0\) 把"选最违反的面"变成"存在满足光滑条件的 \(\lambda\)"——\(\max\) 被对偶变量"吸收"成了光滑约束。其三，等价性是精确的：分离超平面存在 \(\Leftrightarrow\) 点在障碍外，这个等价（强对偶）没有近似，所以 OBCA 重构后的硬约束与原避障约束严格等价。真 OBCA 把这套用到整条轨迹的每个时间步、每个障碍上，但核心就是这里验证的对偶等价。

Step 3 — 一个常见的错误写法。 新手常把避障约束写成"点到障碍中心的距离 \(\ge\) 半径"，用中心距离近似多边形：

// 反例:用"到障碍中心的距离 >= 安全半径"近似凸多边形避障
double dx = p[0] - cx, dy = p[1] - cy;           // cx,cy 为障碍中心
double dist = std::sqrt(dx*dx + dy*dy);
bool safe = dist >= radius;                       // 错:把多边形当成了圆!
// 问题:(1) 对长条形、L 形障碍,外接圆比障碍大得多,
//           安全区被严重高估 —— 明明能过的缝隙被判成不可过(过度保守);
//       (2) 内切圆又会漏掉角落 —— 明明会撞的角被判成安全(危险);
//       (3) 无论外接还是内切,都不是【精确】的,这正是 OBCA 要避免的近似。

用圆近似多边形，要么过度保守（外接圆）、要么危险漏判（内切圆），且永远不精确。

Step 4 — 对比。 正确版（OBCA 思路）用半空间表示障碍、有符号距离判内外，对**任意凸多边形**都精确；错误版用圆近似，对非圆障碍必然失真。这个对比正是 OBCA 的价值所在——它处理**一般凸障碍**（甚至凸集的并）而不做形状近似，靠的就是"半空间表示 + 对偶变量光滑化"，既精确又可微。这是软约束（CILQR/TEB）和圆近似都给不了的。

带数字走一遍¶

把 OBCA demo 的输出摊开，能看清"分离超平面存在 \(\Leftrightarrow\) 点在障碍外"这个对偶等价。障碍是单位正方形 \([0,1]^2\)（四个半空间），测三个点。外侧点 \((2.0, 0.5)\)：有符号距离 \(=\max_i(a_i^\top p - b_i)=+1.000\)（由 \(x\le 1\) 这条边给出，\(2.0-1=1\)），分离面 \(\#0\)，判定"在外（有分离超平面 → 无碰撞）"。贴边点 \((1.0, 0.5)\)：有符号距离 \(=+0.000\)（恰在 \(x=1\) 边界上），判定"在边界上"。内部点 \((0.5, 0.5)\)：有符号距离 \(=-0.500\)（所有边都满足、取最大的那个仍为负），判定"在内（无分离超平面 → 碰撞）"。逐个数字读它的含义。 为什么外侧点的有符号距离是正的、且 = 到最近边的距离？ 因为 \(\max_i(a_i^\top p - b_i)\) 取了"最违反"的那个半空间——外侧点违反了 \(x\le 1\)（违反量 \(=1\)），这个违反量正是它到该边的距离，也正是分离超平面能"插"进去的间隙。 为什么内部点是负的？ 因为内部点不违反任何半空间（所有 \(a_i^\top p - b_i\) 都 \(\le 0\)），\(\max\) 取最大的那个负值（\(-0.5\)，离最近的边还有 \(0.5\)）——没有任何边能分离它和障碍，对应"无分离超平面 = 碰撞"。 这个 \(\max\) 和对偶变量 \(\lambda\) 什么关系？ \(\max\) 本身不可微（哪个边最违反会跳变），而 OBCA 的 \(\lambda\) 正是把"选最违反的边"这个离散操作，光滑地参数化成"存在 \(\lambda\ge 0\) 满足 \((Ap-b)^\top\lambda\ge d_{\text{safe}}\)"——\(\lambda\) 像给各条边分配的权重，最优时把权重压在最违反的边上，等价于那个 \(\max\)，但全程可微。把测试点换成长条形障碍 \([0,4]\times[0,1]\) 的外侧点，会看到有符号距离仍精确反映到最近边的距离——这就是 OBCA 处理任意凸多边形（而非圆近似）的精确性。

⚠️ 常见陷阱¶

💡 编程陷阱：用障碍外接圆/内切圆近似多边形避障 - 错误想法：避障就是"离障碍够远"，用"到障碍中心距离 \(\ge\) 半径"判断最简单。 - 现象 / 后果：外接圆把安全区高估（窄缝明明能过却被禁，过度保守）；内切圆把角落漏判（会撞的角被当安全，危险）。 - 根本原因：把多边形当成圆，丢掉了障碍的真实形状；圆近似无论内切外接都不精确。 - 正确做法：用半空间表示 \(\{Ax\le b\}\) 描述凸多边形障碍，用有符号距离（OBCA 的对偶重构）做精确、可微的避障约束。

⚠️ 概念陷阱：以为"点到凸多边形的距离"是光滑函数 - 错误想法：距离就是个连续函数，直接当约束喂给 NLP 求解器就行。 - 现象 / 后果：求解器在障碍的边、角附近反复抖动、不收敛——因为距离函数在"由哪条边/哪个顶点决定最近点"切换处导数跳变。 - 根本原因：点到凸多面体的距离是**分片光滑**的（不同区域由不同面决定），在切换边界不可微； signed distance 在障碍内外还变号。 - 正确做法：用 OBCA 的对偶重构把这个不可微的距离约束，等价换成关于 \((p,\lambda)\) 的光滑约束——这正是 OBCA 存在的理由。

🧠 思维陷阱：以为对偶重构消除了非凸性 - 错误想法：OBCA 把避障约束变光滑了，问题就成了好解的凸问题。 - 现象 / 后果：直接随便给初值跑 OBCA，收敛到劣解或不收敛，却以为是 bug。 - 根本原因：OBCA 消除的是**不可微性**，不是**非凸性**——避障问题本质非凸（绕左/绕右是不同 homotopy），重构后仍是非凸 NLP，对初值敏感。 - 正确做法：用离散前端（如 Hybrid A*）warm-start——这正是 H-OBCA 的做法：先定 homotopy、再连续精修。光滑 \(\ne\) 凸，warm-start 不能省。

工程落地注意事项¶

OBCA 主要用在泊车/开放空间这类需要硬避障的场景，落地有几个特点。 对偶变量让问题变大。 OBCA 为每个障碍、每个时间步引入一组对偶变量 \(\lambda\)——障碍多、时域长时，变量数显著增加，求解变慢（~5 Hz 量级，比 CILQR/MINCO 慢）。所以 OBCA 适合泊车这类**对实时性要求不那么极致、但对精确避障要求高**的场景，不适合高速行车的高频重规划。 warm-start 用 Hybrid A*。 这是 H-OBCA 的标准搭配，也是 OBCA 落地的关键：泊车空间窄、机动复杂（要倒库、揉方向），非凸优化极易卡劣解或不收敛。 Hybrid A* 先用简化模型搜一条可行的粗轨迹（定下"怎么倒、往哪个方向揉"的 homotopy），再 warm-start OBCA 用全车模型精修。少了这一步，OBCA 在泊车里基本跑不稳。 障碍的凸分解。 OBCA 处理凸障碍（或凸集的并）。真实环境的障碍（不规则停车位边界、其他车辆）要先做**凸分解**——拆成若干凸多边形，每个用一组半空间表示。分解的质量影响约束数量和求解效率。 车辆形状也要建模。 OBCA 的精确性体现在它同时考虑障碍形状**和自车形状**（自车也表示成凸多边形或凸集的并），算的是两个凸体之间的距离——这比"把车当质点 + 安全半径"精确得多，正是泊车刮蹭判断需要的。代价是建模和求解都更复杂。 Apollo 的实现可参考。 工业落地可参考 Apollo Open Space 的 DistanceApproachProblem（OBCA 思路 + IPOPT/OSQP）——它展示了 OBCA 在产品级泊车里怎么和上游的 Hybrid A* 搜索、下游的轨迹跟踪衔接。

练习¶

[实现] 把上面的有符号距离判据扩展成"点到凸多边形的最近距离"（不只是判内外，还算出具体距离值）：当点在外时，最近距离 = 到最近边/顶点的欧氏距离。观察这个距离在点绕多边形一周时，在边-角切换处的不光滑（导数跳变）——这正是 OBCA 要光滑化的对象。
[分析] 强对偶定理保证"点在凸障碍外 \(\Leftrightarrow\) 存在分离超平面"。这个等价用了凸性的哪个性质？如果障碍是**非凸**的（比如 L 形），这个等价还成立吗？ OBCA 怎么处理非凸障碍？（提示：凸集的并）
[跨章] OBCA 配 Hybrid A* warm-start、CILQR 配 T2 走廊 warm-start、TEB 多拓扑并行——三者都在解决"非凸优化对初值敏感、会卡 homotopy"的问题。请把这三种"离散定 homotopy + 连续精修"的组合列成一张对照表（前端是什么、后端是什么、各自保证什么）。

§T3.4 MINCO：参数映射把高维系数压成低维 ⭐⭐⭐¶

OBCA 用对偶**精确消去**了不可微的避障约束。本节的 MINCO 把"精确消去"用到另一个地方——它用参数映射**精确消去**了多项式系数和段间连续性约束，把约束优化变成无约束优化。两者都是"精确重构"思路（不像 CILQR/TEB 那样软化），但 OBCA 消的是避障约束、MINCO 消的是系数与连续性约束。 MINCO 还顺带解决了"时间分配"，是无人机轨迹的主流方案。

动机：多项式轨迹的系数太多，直接优化太贵¶

无人机轨迹常用**分段多项式**表示——每段是一个高阶多项式（比如 5 阶、7 阶），整条轨迹由若干段拼成。为什么用高阶多项式？因为无人机是**微分平坦**系统：位置的高阶导数（速度、加速度、jerk、snap）直接对应推力、力矩等控制量，而高阶多项式能保证这些导数光滑连续——最小化 snap（4 阶导）的平方积分能得到平滑省力的轨迹。

微分平坦到底是什么、为什么对无人机这么关键。 这个概念值得单独讲透，因为它是无人机轨迹规划（minimum-snap、MINCO 都建立在它之上）的理论基石。一个系统"微分平坦"，是指存在一组**平坦输出**（flat output），使得系统的所有状态和控制输入，都能写成这组平坦输出及其有限阶导数的代数函数——换句话说，只要定下平坦输出的轨迹，整个系统的状态和控制就被**唯一、显式地**确定了，不用再解微分方程。对四旋翼，平坦输出恰好是**位置 \((x,y,z)\) 加偏航角 \(\psi\)（共 4 个，对应 4 个旋翼的自由度）。这意味着：你只要规划好"无人机的位置怎么随时间变"这条轨迹，那么任意时刻需要的姿态、角速度、四个旋翼的推力，全都能从这条位置轨迹的各阶导数**算出来——不用考虑复杂的旋转动力学。这就是为什么无人机轨迹规划能"只规划位置曲线"：姿态和推力是位置轨迹的"副产品"。而为什么要高阶多项式、为什么最小化 snap？因为推力和力矩对应到位置轨迹的**高阶导数**（粗略说，加速度关联推力大小、jerk 关联姿态变化率、snap 关联角加速度/力矩）——要让控制量光滑、不剧烈跳变，就得让位置轨迹的高阶导数光滑，于是用足够高阶的多项式（snap 是 4 阶导，多项式至少 5 阶以上才有意义）、并最小化 snap 的平方积分（让力矩变化最小、最省控制）。 MINCO 的 "Minimum Control"（最小控制量）正是这个思路的一般化：它最小化的"控制量"就是这类高阶导数的能量。理解了微分平坦，你就明白 MINCO 的 \(c=M(q,T)\) 为什么成立——"最小控制量"是个关于高阶导数的变分问题，其最优性条件给出系数的线性方程，而平坦性保证了这套位置轨迹的优化能完全对应到物理可行的飞行。

但这带来一个计算难题：多项式系数太多。一段 \(p\) 阶多项式有 \(p+1\) 个系数，每段每个维度都要这么多； \(N\) 段三维轨迹，系数总数是 \(3 \times N \times (p+1)\)——动辄几十上百个。如果直接把这些系数全当优化变量，问题维度高、还要加一堆约束（段间连续性：相邻段的位置、速度、加速度……要接得上），求解又慢又难。更要命的是**时间分配**：每段花多长时间（段时间 \(T_i\)）也该优化（直道快、弯道慢），但段时间和多项式系数**强耦合**——改一个段时间，所有系数都得重算。于是问题成了：能不能不优化那一大堆多项式系数，只优化少数几个有意义的量（航点位置、段时间），而系数自动跟着确定？ MINCO 给出的答案是一个漂亮的参数映射。

如果不这样做会怎样（反面）¶

不做降维、直接优化全部多项式系数，会同时撞上几堵墙。其一是**维度高**：几十上百个系数变量，非线性优化的规模大、迭代慢，难以满足无人机机载实时（百赫兹级）的要求。其二是**连续性约束多且繁**：段间要保证位置、速度、加速度乃至更高阶导连续，每个连接点每个维度每一阶都是一条等式约束——约束数量爆炸，求解器负担重。其三是**时空耦合难处理**：段时间和系数纠缠，想"同时优化空间形状和时间分配"时，梯度关系复杂，要么固定时间只优化空间（丢了时间最优）、要么交替优化（慢、易震荡）。传统做法（如 minimum-snap 的 Mellinger-Kumar 2011）能解，但要么把时间分配单独拎出来用启发式或外层搜索定、要么承受上述的高维和约束负担。 MINCO 的突破在于：用一个**参数映射**把"系数"这个高维、带约束的表示，换成"航点 + 段时间"这个低维、稀疏的表示——系数由后者**唯一确定**，连续性**自动满足**，时空可**同步优化**。

历史：ZJU FAST Lab 的 GCOPTER¶

MINCO（Minimum Control，最小控制量）由王哲培、周鑫、许超、高飞（浙江大学 FAST Lab）提出，核心论文是《Geometrically Constrained Trajectory Optimization for Multicopters》（IEEE Transactions on Robotics, T-RO, 2022, 38(5):3259-3278，DOI 10.1109/TRO.2022.3160022；arXiv 2103.00190）。论文提出一个面向多旋翼的优化框架，处理几何空间约束和用户定义的动力学约束。它的基础是一种**新的轨迹表示**——建立在作者提出的"无约束控制量最小化"的新最优性条件之上。在这个表示上，他们设计了**线性复杂度**的操作来做"空间-时间变形"（spatial-temporal deformation）以适应各种规划需求；用**光滑映射**（smooth map）以轻量的方式**精确消除**几何约束；通过"稠密约束评估与稀疏参数化解耦 + 平坦性映射的反向微分"支持各种状态-输入约束。最终，这个框架把一个一般约束的多旋翼规划问题，转化成一个无约束优化，从而能可靠高效地求解。 MINCO 的前身是 am_traj（《Alternating Minimization Based Trajectory Generation...》，RA-L 2020）——用**交替最小化**（固定时间 \(T\) 优化航点 \(q\) → 固定 \(q\) 优化 \(T\)）迭代，各子问题有闭式解； GCOPTER 统一了交替和联合两种模式。代码以 ZJU-FAST-Lab/GCOPTER（MIT 许可，C++ header-only）开源，核心是 minco.hpp（约 500 行），实现了参数映射 \(M(q,T)\) 的解析构建和梯度回传。

MINCO 站在 minimum-snap 的肩膀上。 理解 MINCO 的位置，要把它和经典的 minimum-snap（Mellinger-Kumar, ICRA 2011）对照——后者是无人机多项式轨迹的开山之作、MINCO 的直接前驱。 minimum-snap 也用分段多项式、也最小化 snap、也靠微分平坦只规划位置，但它有两个 MINCO 改进的痛点：其一，它把多项式系数当优化变量、连续性写成约束（正是 §T3.4 那个"反例"），维度高；其二，它的**时间分配**往往是单独拎出来、用启发式或外层搜索定的，没和空间形状一起优化。 MINCO 的两大贡献正是针对这两点：用 \(c=M(q,T)\) 把系数和连续性消成"航点+段时间"的低维无约束问题（解决痛点一），用解析梯度让空间（\(q\)）和时间（\(T\)）同步**优化（解决痛点二）。所以可以说：**minimum-snap 确立了"微分平坦 + 多项式 + 最小化高阶导"的范式，MINCO 则把这个范式的求解效率和时空联合性推到了新高度。知道这条传承，你读无人机轨迹规划的文献会更有脉络——从 minimum-snap（2011）到 am_traj（2020）到 MINCO/GCOPTER（2022），是同一条线越走越精的过程。

这一节解决什么问题：多项式轨迹系数多、连续性约束繁、时空耦合难。 MINCO 用参数映射 \(c=M(q,T)\) 把"高维系数 + 一堆连续性约束"换成"低维的航点 + 段时间、且系数唯一确定、连续性自动满足"——把约束优化变成无约束优化，时空同步变形。这是本章"约束怎么进优化"的第四种答案，关键词是"精确消除几何约束"。

理论：\(c = M(q, T)\) 为什么成立、为什么强大¶

核心断言。 MINCO 的核心结论是：阶次为 \(2s-1\) 的分段多项式轨迹，如果满足"每段内控制量（\(s\) 阶导）能量最小"这个最优性条件，那么它的全部多项式系数 \(c\)，由**中间航点 \(q\)（各段连接处的位置）和段时间 \(T\)（各段时长）唯一确定**：

\[ c = M(q, T) \]

换句话说：一旦你定下"轨迹经过哪些航点、每段花多长时间"，那条"在这些航点间最省控制量"的多项式轨迹就**唯一确定**了，系数不用你操心、自动算出。

为什么这能成立？——直觉。 考虑相邻两段之间的连接点：要保证轨迹光滑（位置、速度、加速度……到 \(2s-2\) 阶都连续），同时这条轨迹要"最省 \(s\) 阶导能量"。 "最省能量"是一个变分问题，它的最优性条件（欧拉-拉格朗日方程）给出了系数必须满足的一组**线性方程**；加上"经过指定航点"和"段间连续"的条件，这些线性方程的数量恰好等于系数的数量——于是系数被唯一解出。关键是：这组方程关于 \((q, T)\) 是**结构化的、稀疏的**（每段只和相邻段耦合），所以 \(M(q,T)\) 可以用线性复杂度（\(O(N)\)）算出，而不是解一个稠密大矩阵。这就是论文说的"建立在新最优性条件之上的轨迹表示"——把"最省控制量"这个条件用上，系数就从"自由变量"变成了"\((q,T)\) 的函数"。

为什么这强大？——降维 + 自动连续 + 同步变形。 这个映射带来三个直接红利。其一，降维：不再优化 \(3N(p+1)\) 个系数，只优化 \(q\)（中间航点，\(3(N-1)\) 个）和 \(T\)（段时间，\(N\) 个）——共约 \(4N-3\) 个稀疏参数，维度大幅下降。其二，连续性自动满足：段间的位置、速度、加速度连续性，已经被编码进 \(M(q,T)\) 的构造里（最优性条件就包含连续性），不用再写成显式约束——约束优化变无约束优化。其三，时空同步变形：因为系数是 \((q,T)\) 的解析函数，代价 \(J\) 对航点的梯度 \(\partial J/\partial q\) 和对段时间的梯度 \(\partial J/\partial T\) 都能通过 \(M(q,T)\) 的解析微分**回传**——于是空间形状（\(q\)）和时间分配（\(T\)）可以在同一个梯度步里**一起**更新，这就是"空间-时间同步变形"。

多视角对照：\(c=M(q,T)\) 这个映射，从三个角度看是三件事，合起来才看全它的精妙。 从变分最优看：它是"最小控制量"这个变分问题的最优性条件的解——给定边界（航点）和时长，那条"最省高阶导能量"的曲线由欧拉-拉格朗日方程唯一确定。这个视角告诉你它**为什么唯一**（最优性条件 + 边界条件凑齐了方程）。 从降维看：它是一个"坐标变换"——把高维的系数空间，换成低维的 \((q,T)\) 空间，两者一一对应。这个视角告诉你它**为什么高效**（优化在低维空间进行，维度从"阶数×段数"量级降到"段数"量级）。 从自动微分看：它是一个可微的计算图节点——前向算系数、反向传梯度，像神经网络的一层。这个视角告诉你它**为什么能时空同步**（梯度能同时流回 \(q\) 和 \(T\)）。三个视角不矛盾：变分视角给唯一性、降维视角给效率、微分视角给同步性。只盯着公式 \(c=M(q,T)\) 会觉得抽象，换这三个视角看，它的"唯一+高效+同步"三个好处就各有了来处。

时间正性与求解。 段时间必须为正（\(T_i > 0\)）。 MINCO 用一个变量代换消掉这个约束：令 \(T_i = e^{\tau_i}\)（或类似的光滑正映射），优化无约束的 \(\tau_i\)，则 \(T_i\) 自动恒正——和 TEB 用 \(\exp\) 参数化段时间是同一招。整个问题于是变成一个**无约束优化**（航点 \(q\) + 变换后的时间 \(\tau\)），用 L-BFGS（一种高效的拟牛顿无约束优化器）求解，配合解析梯度，能做到很高的求解频率（百赫兹级）。对几何约束（如走廊：轨迹要待在 T2 那种凸多面体里），MINCO 用**光滑映射**把"在凸区域内"这个约束也精确消去（把约束坐标映射到无约束空间）——这就是论文说的"smooth map 精确消除几何约束"。所以 MINCO 的整条链路是：T2 给走廊（几何约束）→ 光滑映射消去几何约束 + \(M(q,T)\) 消去系数和连续性约束 → 无约束优化（L-BFGS）→ 时空同步变形出最优轨迹。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Step 1 — 为什么这样写。 完整 MINCO 用 minco.hpp 实现 \(M(q,T)\) 的解析构建和梯度回传，还要接 L-BFGS、光滑映射，代码精巧但重。下面给一份**可直接编译运行**的最简演示，把 MINCO 最核心的断言验证出来：只给航点 \(q\) 和段时间 \(T\)，多项式系数被"经过航点 + 段间连续"唯一确定（\(c=M(q,T)\)）。场景简化成一维、\(N\) 段三次多项式，直接解那个确定系数的线性系统，验证轨迹精确过航点、段间连续。只用标准库（含一个自写的小高斯消元），g++ -std=c++17 即编。

// MINCO 核心思想验证:给定航点 q + 段时间 T,系数由"经过航点+段间连续"唯一确定 c=M(q,T)
// 简化:一维,N 段三次多项式。约束(共 4N 条,正好定 4N 系数):
//   每段端点过航点(2N) + 内部连接点速度/加速度连续(2(N-1)) + 起终点速度=0(2)。
// 演示"定下(q,T),系数被唯一解出,无需把系数当优化变量"。纯标准库,g++ 即编。
#include <vector>
#include <cmath>
#include <cstdio>

// 极简高斯消元解 Ax=b(n 小,演示用)
std::vector<double> Solve(std::vector<std::vector<double>> A, std::vector<double> b) {
  int n = b.size();
  for (int i = 0; i < n; ++i) {
    int piv = i;
    for (int r = i+1; r < n; ++r) if (std::fabs(A[r][i]) > std::fabs(A[piv][i])) piv = r;
    std::swap(A[i], A[piv]); std::swap(b[i], b[piv]);
    for (int r = 0; r < n; ++r) if (r != i) {
      double f = A[r][i] / A[i][i];
      for (int c = i; c < n; ++c) A[r][c] -= f * A[i][c];
      b[r] -= f * b[i];
    }
  }
  std::vector<double> x(n);
  for (int i = 0; i < n; ++i) x[i] = b[i] / A[i][i];
  return x;
}

int main() {
  std::vector<double> q = {0.0, 2.0, 1.0, 3.0};   // 4 航点 => 3 段
  std::vector<double> T = {1.0, 1.5, 1.0};        // 3 段时间
  int N = T.size(), dim = 4 * N;
  std::vector<std::vector<double>> A(dim, std::vector<double>(dim, 0.0));
  std::vector<double> b(dim, 0.0);
  int row = 0;
  auto idx = [&](int seg, int k){ return 4*seg + k; };  // 段 seg 第 k 个系数

  for (int i = 0; i < N; ++i) {                   // 每段端点过航点
    double Ti = T[i];
    A[row][idx(i,0)] = 1; b[row] = q[i]; ++row;                          // 起点=q[i]
    A[row][idx(i,0)]=1; A[row][idx(i,1)]=Ti; A[row][idx(i,2)]=Ti*Ti;
    A[row][idx(i,3)]=Ti*Ti*Ti; b[row] = q[i+1]; ++row;                   // 终点=q[i+1]
  }
  for (int i = 0; i < N-1; ++i) {                 // 连接点速度、加速度连续
    double Ti = T[i];
    A[row][idx(i,1)]=1; A[row][idx(i,2)]=2*Ti; A[row][idx(i,3)]=3*Ti*Ti;
    A[row][idx(i+1,1)]=-1; ++row;                                        // 速度连续
    A[row][idx(i,2)]=2; A[row][idx(i,3)]=6*Ti; A[row][idx(i+1,2)]=-2; ++row; // 加速度连续
  }
  A[row][idx(0,1)] = 1; ++row;                    // 起点速度=0
  double Tl = T[N-1];
  A[row][idx(N-1,1)]=1; A[row][idx(N-1,2)]=2*Tl; A[row][idx(N-1,3)]=3*Tl*Tl; ++row; // 终点速度=0

  std::vector<double> c = Solve(A, b);            // c = M(q,T):系数被唯一解出

  auto eval = [&](int seg, double t){
    double r=0,tp=1; for(int k=0;k<4;++k){r+=c[idx(seg,k)]*tp; tp*=t;} return r; };
  std::printf("航点 q={0,2,1,3}, 段时间 T={1,1.5,1};只给 (q,T),系数自动解出:\n");
  for (int i = 0; i < N; ++i)
    std::printf("  段%d: 起点=%.4f(应%.1f) 终点=%.4f(应%.1f)\n",
                i, eval(i,0), q[i], eval(i,T[i]), q[i+1]);
  double maxerr = 0;
  for (int i = 0; i < N-1; ++i)
    maxerr = std::max(maxerr, std::fabs(eval(i,T[i]) - eval(i+1,0)));
  std::printf("连接点最大不连续 = %.2e (≈0 => 段间连续 OK)\n", maxerr);
  return 0;
}
// 运行输出:
//   段0: 起点=0.0000(应0.0) 终点=2.0000(应2.0)  ...各段精确过航点
//   连接点最大不连续 = 0.00e+00 (≈0 => 段间连续 OK)

Step 2 — 正确要点。 这份演示抓住了 MINCO 的本质。其一，系数是因变量、不是优化变量：代码里我们只给了 \(q\) 和 \(T\)，系数 \(c\) 由线性系统 Solve(A,b) 解出——这正是 \(c=M(q,T)\)。真 MINCO 优化时只动 \(q\) 和 \(T\)，系数永远跟着自动算，维度因此大降。其二，连续性被"焊进"了系统：连接点的速度、加速度连续是线性系统的一部分约束，所以解出的系数**自动满足**连续性（验证里不连续误差为 0）——无需在外层优化里再写连续性约束，约束优化变无约束优化。其三，约束数 = 系数数：\(N\) 段三次多项式有 \(4N\) 个系数，而"过航点 + 连续 + 边界"恰好给出 \(4N\) 条线性方程——系数被唯一确定。真 MINCO 用更高阶多项式 + "最小控制量"最优性条件给出这组方程，但"\((q,T)\) 唯一定系数"的机制一样。真 MINCO 还会对段时间用 \(T_i=e^{\tau_i}\) 保正、用解析微分回传 \(\partial J/\partial q\) 和 \(\partial J/\partial T\) 做时空同步变形——这份 demo 只验证了映射本身，没做优化。

Step 3 — 一个常见的错误写法。 新手最容易犯的错，是**把多项式系数也当成优化变量**，再外加连续性约束：

// 反例:把所有系数当优化变量,连续性写成额外的硬约束塞给求解器
// 决策变量 = 全部 4N 个系数 c[]
// 约束:段间位置/速度/加速度连续(一堆等式) + 过航点(一堆等式)
optimize(c, subject_to = {continuity_constraints, waypoint_constraints, ...});
// 问题:(1) 维度高 —— 4N 个系数 vs MINCO 的 ~4N-3 个 (q,T),且系数无直观意义难初始化;
//       (2) 连续性约束多 —— 每个连接点每阶导都是一条约束,求解器负担重;
//       (3) 时间分配难嵌入 —— 段时间一变,所有系数约束都要重写,时空耦合处理复杂。
//       MINCO 把"连续性"和"系数"都通过 c=M(q,T) 消掉了,根本不让它们进优化。

这个错误不是"跑不出来"，而是"又慢又笨"——它优化了本可以被 \(M(q,T)\) 自动确定的量，白白扛下高维和大量约束。

Step 4 — 对比。 正确版（MINCO 思路）只优化 \((q,T)\)、系数由映射自动确定、连续性自动满足——低维、无连续性约束、时空可同步变形。错误版优化全部系数 + 显式连续性约束——高维、约束多、时空耦合难处理。两者解出的最优轨迹可以一样，但 MINCO 的参数化让优化问题小一个量级、还天然支持时空同步——这就是它能在无人机上做到百赫兹级求解的根本。这也是"选对表示"（统一问题形式的"选择一"）威力的最佳例证。

带数字走一遍¶

把 MINCO demo 的运行摊开，能看清 \(c=M(q,T)\) "系数被唯一确定"到底是怎么回事。输入只有两样：4 个航点 \(q=\{0, 2, 1, 3\}\) 和 3 个段时间 \(T=\{1, 1.5, 1\}\)——没有给任何多项式系数。 3 段三次多项式共 \(4\times 3 = 12\) 个系数；约束有：每段两端过航点（\(2\times 3 = 6\) 条）、两个内部连接点的速度和加速度连续（\(2\times 2 = 4\) 条）、起终点速度为零（\(2\) 条）——恰好 \(12\) 条，与系数数相等。解这个 \(12\times 12\) 的线性系统，\(12\) 个系数被**唯一**解出。验证输出：段 0 起点 \(=0.0000\)（应 \(=q_0=0\)）、终点 \(=2.0000\)（应 \(=q_1=2\)）；段 1、段 2 同样精确过各自航点；连接点位置最大不连续 \(=0.00\text{e}{+}00\)——段间严丝合缝。这组数字坐实了三件事。 系数是因变量：我们一个系数都没给、没优化，它们由 \((q,T)\) 通过线性系统自动算出——这就是 \(c=M(q,T)\)，真 MINCO 优化时只动 \(q\) 和 \(T\)。 连续性自动满足：连接点不连续误差为 \(0\)，因为连续性是线性系统的约束之一、被"焊"进了解——无需在外层再写连续性约束。 约束数 = 系数数：\(12 = 12\) 不是巧合，正是它让系数"唯一确定"——少一条则不唯一、多一条则无解。把段时间改成 \(T=\{1, 3, 1\}\)（中间段拉长）再解，会看到中间段的系数变化、轨迹在该段变得更"舒缓"——这就是"时间分配"如何通过 \(M(q,T)\) 影响轨迹形状，也是 MINCO "时空同步变形"的微观体现。

⚠️ 常见陷阱¶

💡 编程陷阱：把多项式系数当优化变量 + 显式连续性约束 - 错误想法：轨迹就是多项式，那就优化它的系数，段间连续性加几条等式约束。 - 现象 / 后果：优化问题维度高（\(4N\) 个无直观意义的系数）、约束多（每连接点每阶导一条）、求解慢、初始化难。 - 根本原因：没用上"系数可由 \((q,T)\) 唯一确定"这一结构——把本该是因变量的系数当成了自由变量。 - 正确做法：用 \(c=M(q,T)\)，只优化稀疏的航点 \(q\) 和段时间 \(T\)，系数和连续性都由映射自动处理（无约束优化）。

⚠️ 概念陷阱：以为 \(M(q,T)\) 是某种近似或拟合 - 错误想法：\(c=M(q,T)\) 大概是用 \((q,T)\) 去"拟合"一条多项式吧，应该有误差。 - 现象 / 后果：怀疑 MINCO 轨迹不精确、不敢用于高精度场景。 - 根本原因：\(M(q,T)\) 是"最小控制量 + 连续性 + 过航点"这组**最优性条件的精确解**，是解析的、唯一的、无近似的——它不是拟合，是"在给定 \((q,T)\) 下那条最优轨迹的闭式表达"。 - 正确做法：理解 \(M(q,T)\) 是精确映射：给定 \((q,T)\)，系数被一组线性方程唯一确定，轨迹精确过航点、精确连续。

🧠 思维陷阱：以为 MINCO 只是"换个参数化"、没有本质区别 - 错误想法：优化系数还是优化 \((q,T)\)，不就是变量换一下，最优解一样有啥区别。 - 现象 / 后果：低估 MINCO，在该用它的高频无人机场景里还用全系数优化，跑不到实时。 - 根本原因：参数化的选择决定了**问题的维度、约束数量、和能否时空同步**——这些直接决定求解速度，不是"换汤不换药"。 - 正确做法：把"选对表示"当成一等大事（统一问题形式的第一选择）：MINCO 的降维 + 自动连续 + 同步变形，让同一个最优解的求解快一个量级，这正是表示的威力。

工程落地注意事项¶

MINCO/GCOPTER 是无人机轨迹的事实标准之一，落地时有几点要留意。 走廊质量决定一切。 MINCO 的几何约束（轨迹待在凸多面体内）来自前端给的走廊（如 T2 的 SFC）。走廊切得好（够宽、重叠充分、贴合可飞空间），MINCO 优化出的轨迹就既安全又激进；走廊切得差（过窄、有缝隙），再强的优化也飞不出好轨迹——"垃圾进，垃圾出"。所以 MINCO 落地的一大半功夫在前端的走廊生成上。 段数与航点的选择。 段数（航点数）是个权衡：段多则轨迹表达能力强、但优化变量多、慢；段少则快、但可能不够灵活绕过复杂障碍。工程上常按走廊的 cube 数来定段（每个 cube 一段或几段），让段的划分和几何走廊对齐。 L-BFGS 的初值与数值。 MINCO 用 L-BFGS 解无约束问题，虽快但仍需合理初值（航点的初始猜测）——通常用前端粗解或走廊中心连线初始化。段时间的 \(\exp\) 参数化要注意数值范围，\(\tau\) 过大过小都会让 \(T=e^\tau\) 数值病态。 header-only 的便利与约束。 GCOPTER 的 minco.hpp 是 header-only、依赖 Eigen，集成方便（拷进项目即可）；但它面向多旋翼设计，用到车辆等其他动力学时要改写平坦性映射部分，不是开箱即用。 和控制器的衔接。 MINCO 输出的是参考轨迹，真飞还要一个轨迹跟踪控制器（如几何控制器、或 MPC）去跟。规划频率（百赫兹）和控制频率要协调，规划出的轨迹要在控制器能跟踪的动力学范围内——这就是为什么 MINCO 的动力学约束要设得和真机一致。

练习¶

[实现] 把上面的一维 \(c=M(q,T)\) 扩展：(a) 改段时间 \(T\)（比如让中间段更长），重新解系数，观察轨迹形状变化——这就是"时间分配"对轨迹的影响； (b) 把三次多项式升到五次（minimum-jerk → minimum-snap），相应增加边界/连续性约束到 \(6N\) 条，验证仍唯一确定。
[分析] MINCO 说系数由 \((q,T)\) 唯一确定，前提是"满足最小控制量的最优性条件"。如果不要求"最小控制量"、只要求"过航点 + 连续"，系数还唯一吗？（提示：数一数自由度和约束数）这说明"最优性条件"在 \(M(q,T)\) 里扮演什么角色？
[跨章] MINCO 的几何约束（轨迹待在走廊内）用"光滑映射"精确消去。回忆 T2 的走廊（SSC/SFC 给的凸多面体）。请说明：T2 的走廊如何成为 MINCO 的输入？" 光滑映射消去几何约束"和 T2 §T2.3 那个"控制点在 cube 内 = 线性约束"是什么关系？（提示：都是把"在凸区域内"变成优化能处理的形式，但方式不同）

§T3.5 CPC：让"何时过航点"成为优化变量 ⭐⭐⭐¶

MINCO 已经能优化段时间了，但它（和前面所有方法）默认"哪个航点在第几段通过"是预先固定的。本节的 CPC 走到这条路的尽头——连"何时通过哪个航点"都交给优化。它用一个最巧也最难解的工具（互补约束）做到这点，代价是只能离线，但换来了真正的时间最优——快到击败人类飞手。这是本章"约束进优化"五种答案里最后、也最精妙的一种。

动机：时间最优的最后一块拼图——时间分配本身¶

前面几个方法（尤其 MINCO）已经能优化段时间了，但它们有一个**没说破的前提**：航点和时间步的对应关系是**预先固定**的——"第 \(i\) 个航点在第几段（或大致第几个时间步）通过"是事先指定的，优化只在这个分配下进行。对追求极致的**时间最优**（如无人机竞速），这个前提是致命的。为什么？因为真正的时间最优轨迹，"何时通过哪个航点"本身就该是**自由的**——也许最优策略是"早早冲过前两个门、在第三个门附近多花点时间调整姿态"，这种灵活的时间分配，预先固定的分配表达不出来。更深一层：早期的多项式轨迹法（如 minimum-snap）因为多项式**固有的平滑性**，发挥不出四旋翼执行器的全部潜力（极限机动需要 bang-bang 式的推力切换，多项式太"软"）；而数值优化法虽然灵活，却要求把航点作为代价或约束**分配到特定的离散时刻**——可这个时间分配事先并不知道，于是这些方法都产不出真正时间最优的轨迹。问题就成了：怎么让"什么时候通过哪个航点"也成为优化变量，而不是预先固定？ CPC 用一个巧妙的互补约束，给出了答案，并因此造出了第一条真正时间最优的四旋翼轨迹——快到击败了人类职业飞手。

如果不这样做会怎样（反面）¶

不把时间分配交给优化、而是预先固定航点-时刻对应，会丢掉时间最优的关键自由度。其一，次优的时间分配：你猜"每个航点该在第几段通过"，猜得不准，最终轨迹就不是最优——而最优分配往往反直觉（不是均匀的、不是等距的）。其二，多项式平滑性的束缚：若用多项式轨迹回避时间分配问题，多项式的光滑性会"磨平"该有的急加急减，发挥不出推力的极限——时间最优轨迹在执行器饱和时常常是 bang-bang 式的（推力在最大/最小间切换），多项式表达不了。其三，人为离散化的误差：把航点硬绑到某个离散时间步，离散网格的粗细直接限制了时间分配的精度。 CPC 的突破在于：它既不固定时间分配（让优化自己定何时过航点）、又用直接的状态-输入轨迹优化（不受多项式平滑性束缚，能产出 bang-bang）、还把"通过航点"用连续的互补约束表达（避免硬性离散化）——三管齐下，得到真正的时间最优。

历史：UZH RPG 击败人类飞手¶

CPC（Complementary Progress Constraints，互补进度约束）由 Philipp Foehn、Angel Romero、Davide Scaramuzza（苏黎世大学 UZH 机器人与感知组 RPG）提出，论文《Time-optimal planning for quadrotor waypoint flight》（Science Robotics, 2021, 6(56):eabh1221，DOI 10.1126/scirobotics.abh1221；arXiv 2108.04537）。论文开门见山地指出前人的困境：四旋翼是最敏捷的飞行机器人之一，但"在执行器极限上、穿过多个航点的时间最优轨迹规划"仍是开放问题——早期多项式法因固有平滑性发挥不出执行器全部潜力，近期数值优化法又要求把航点作为代价或约束分配到特定离散时刻，而这个时间分配事先未知，使前人方法都无法产出真正时间最优的轨迹。他们的解法是**互补进度约束**：引入一个沿轨迹的"进度"度量，用互补条件把它和"航点接近度"耦合起来——直观说，一个航点只有当四旋翼进入它一定容差范围内时，才能被标记为"已完成"，这样就允许**同时优化状态、输入轨迹、以及航点的时间分配**。他们在世界最大的动作捕捉场地之一做了真机飞行验证，在 3D 竞速赛道上，圈速和一致性都**超过了两名职业人类无人机飞手**（Michael Isler 与 Timothy Trowbridge）。这是无人机自主飞行第一次在竞速中击败顶尖人类飞手，代表了时间最优轨迹规划的里程碑。代码以 uzh-rpg/rpg_time_optimal（CasADi/Python）开源。

这一节解决什么问题：前面方法（含 MINCO）默认航点-时刻对应是固定的，产不出真正时间最优。 CPC 用互补进度约束让"何时过航点"也成优化变量——进度变量 + "靠近才算通过"的互补条件，同时优化轨迹和时间分配。这是本章"约束怎么进优化"的第五种、也是最巧的答案，关键词是"用互补约束表达离散的'过/不过'"。

理论：进度变量与互补约束¶

核心想法：把"通过航点"变成连续可优化的条件。 传统做法是硬性规定"在时刻 \(t_k\) 必须位于航点 \(w_i\)"——这是个把航点钉死在时刻的等式约束，时间分配被写死。 CPC 反其道而行：它不规定"何时"过航点，只规定"必须按顺序通过所有航点"，让优化器**自己决定**每个航点在轨迹的哪个位置被通过。为此，它给每个航点 \(i\)、每个离散时间步 \(k\) 引入一个**进度变量** \(\lambda_{i,k}\)，表示"截至第 \(k\) 步，航点 \(i\) 的完成进度"。再用**互补约束**把进度和"接近航点"绑定。

互补约束的形式。 CPC 的互补约束（示意形式）大致是：

\[ \mu_{i,k} \cdot \big(\|p_k - w_i\|^2 - \epsilon^2\big) = 0,\qquad \mu_{i,k}\ge 0 \]

这里 \(\mu_{i,k}\) 是与进度关联的乘子、\(p_k\) 是第 \(k\) 步的位置、\(w_i\) 是第 \(i\) 个航点、\(\epsilon\) 是容差半径。读这个互补条件：两个因子的乘积为零，意味着至少一个为零。要么 \(\mu_{i,k}=0\)（这一步不"消费"航点 \(i\) 的进度），要么 \(\|p_k-w_i\|^2 - \epsilon^2 = 0\) 即 \(\|p_k-w_i\|=\epsilon\)（位置到达航点 \(i\) 容差圈的边界）。（这是高度简化的示意——CPC 论文的真实形式还包含沿轨迹的进度变量及其单调推进约束，并用容差不等式 \(\|p_k-w_i\|\le\epsilon\) 配合互补条件；这里抓住"进度与接近度互补"这一核心机制即可。）换句话说：只有当四旋翼真的飞到航点 \(i\) 的容差范围内时，航点 \(i\) 的进度才被允许推进——这正是论文说的"一个航点只有当四旋翼在其一定容差内才能被标记为完成"。把这些互补约束 + "进度必须从 0 推进到 1（所有航点按序完成）" + 四旋翼动力学 + 执行器极限，全部放进一个优化问题，最小化总时间——优化器就会**自动决定**每个航点在轨迹的哪一点被通过，时间分配成了优化的产物而非输入。

为什么这能产出真正时间最优（bang-bang）？ 因为 CPC 直接优化状态和输入轨迹（不用多项式参数化），输入（单旋翼推力）只受执行器上下限约束。当优化目标是"最小化总时间"时，最优解会让执行器**尽可能多地工作在极限**——推力在最大/最小之间切换（bang-bang 控制），这是时间最优控制的经典特征（庞特里亚金极小值原理）。多项式法因为平滑性产不出这种切换，而 CPC 的直接轨迹优化能——这就是它"充分利用执行器潜力"、比前人更快的根本。代价是：互补约束让问题变得很难解（互补条件是一类特殊的非凸约束，可行域有组合结构），求解慢、对初值敏感，所以 CPC 主要用于**离线**生成竞速轨迹，而非在线实时规划。

多视角对照：把本章五个方法对"时间"的处理排成一条光谱，CPC 站在最右端，对照着看最清楚它的位置。 左端——时间隐式：CILQR 把时间藏在状态 \(v\) 和步长里，时间不是显式变量、是状态演化的副产品。最省事，但"快慢"只能间接体现。 中段——段时间显式：TEB、MINCO 把段时间 \(\Delta T_i\) 拿出来当变量，能直接优化"每段多快"，但"哪个航点在第几段"仍是预先固定的。这是大多数实用方法的位置——时间可优化，但分配有预设。 右端——连分配都优化：CPC 把"何时通过哪个航点"也变成优化变量（靠互补约束），时间分配完全自由。这是光谱的尽头——最接近"真正时间最优"，代价是最难解、只能离线。这条光谱不是"越右越好"，而是"越右越自由、也越难"：左端实时好用，右端极致但离线。它和"约束怎么进优化"那条主线（软化→精确重构）正交——一个方法在两条轴上各有位置，合起来才定义了它。 CPC 在"时间自由度"轴上最右、在"约束精确度"轴上也最硬（互补），所以它是五个方法里最"极致"、也最"专用"的一个。

互补约束的难点。 互补条件 \(\mu \cdot g = 0\) 这种"乘积为零"的约束，数学上属于 MPCC（Mathematical Program with Complementarity Constraints，带互补约束的数学规划）——它违反了标准 NLP 求解器依赖的约束规范条件（constraint qualification），在可行点处梯度信息退化。实践中要用专门的技巧（松弛、罚函数、或分支处理）来求解，且求解时间长。这也是 CPC 和 §T3.1–§T3.4 的本质区别：前面几个方法都力求**实时**（在线重规划），而 CPC 为了**极致时间最优**牺牲了实时性，专注离线算出那条"理论最快"的轨迹。理解这个取舍很重要——CPC 不是用来在线开车/飞行的，它是用来回答"这条赛道理论上最快能多快飞"的。

为什么互补约束这么难解——一个几何直觉。 值得再深一层看 MPCC 难在哪，因为它解释了 CPC 为什么只能离线。普通的不等式约束 \(g\le 0\)，可行域是一片"实心"的区域，光滑、有内部，求解器可以在内部自由移动、沿梯度下降。而互补约束 \(\mu\cdot g=0\)（\(\mu\ge 0, g\le 0\)）的可行域**不是实心的**——"乘积为零"要求 \(\mu\) 和 \(g\) 至少一个为零，于是可行域是两条边界的并：要么 \(\mu=0\)（落在 \(\mu\) 轴上），要么 \(g=0\)（落在 \(g\) 边界上），形如一个字母 "L"（两条线段拼成的拐角），只在拐角 \((\mu=0, g=0)\) 处相交。这个"L 形"可行域没有内部、在拐角处有尖点——标准 NLP 求解器赖以工作的**约束规范条件**（要求可行域在解附近是光滑流形、梯度线性无关）在这里被破坏：拐角处梯度信息退化，求解器不知道该往 \(\mu\) 轴走还是往 \(g\) 边界走。这就是 MPCC 难解的几何根源——可行域的这种"非光滑、组合"结构，让通用求解器要么不收敛、要么收敛极慢。所以求解 CPC 要用专门技巧：松弛（把 \(\mu\cdot g=0\) 放成 \(\mu\cdot g\le\delta\)，把尖锐的 L 形"撑开"成有内部的区域，再让 \(\delta\) 退火趋零）、或罚函数、或显式分支处理每个互补对的两种情况。这些技巧都要反复求解、慢——这正是 CPC 离线的根本原因。理解了这个几何，你也就明白前沿那个开放问题"互补约束能否加速到在线"为什么难：它要在保持时间最优（需要互补约束）的同时，绕过这个 L 形可行域的求解困难——这是个尚未完全解决的硬骨头。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Step 1 — 为什么这样写。 完整 CPC 要把互补约束嵌进 CasADi/Ipopt 的 MPCC 求解，处理互补约束的数值难点，是离线重型计算。但 CPC 的**核心机制——互补约束让"进度只在靠近航点时推进"**——可以独立演示。下面这份**可直接编译运行**的代码，模拟一条按序飞过 3 个航点的轨迹，演示进度何时被互补条件"允许"推进，把 CPC "不固定时间分配、让优化自己定何时过航点"的思想看清。只用标准库，g++ -std=c++17 即编。

// CPC 核心思想验证:互补约束 μ·g=0 让"通过航点"只在靠近时才计数
// 互补条件 μ_i·(||p-w_i||^2 - ε^2)=0 => 要么 μ=0(不推进进度),要么 ||p-w_i||=ε(进容差圈)
// 模拟一条飞过 3 航点的轨迹,演示进度何时被"允许"推进。纯标准库,g++ 即编。
#include <vector>
#include <array>
#include <cmath>
#include <cstdio>

using Vec2 = std::array<double,2>;
double dist(const Vec2& a, const Vec2& b){ return std::hypot(a[0]-b[0], a[1]-b[1]); }

int main() {
  std::vector<Vec2> wp = {{2,0},{4,2},{6,0}};      // 3 个航点(竞速门),按序穿过
  double eps = 0.5;                                // 容差半径:进此圈才算"到达"

  // 模拟轨迹:从起点依次飞向并穿过三个航点(分段线性插值)
  std::vector<Vec2> anchors = {{0,0},{2,0},{4,2},{6,0}};
  std::vector<Vec2> traj;
  for (int seg = 0; seg + 1 < (int)anchors.size(); ++seg)
    for (int j = 0; j < 7; ++j) {
      double a = j / 7.0;
      traj.push_back({anchors[seg][0]*(1-a)+anchors[seg+1][0]*a,
                      anchors[seg][1]*(1-a)+anchors[seg+1][1]*a});
    }
  traj.push_back(anchors.back());

  // CPC 核心:进度只在 ||p-w_i|| <= eps(进容差圈,g<=0)时才被允许推进
  std::printf("3 航点竞速,容差 eps=%.1f。演示进度何时被允许推进:\n", eps);
  std::vector<bool> done(wp.size(), false);
  int next_wp = 0;                                 // 下一个待通过航点(按序)
  for (int k = 0; k < (int)traj.size() && next_wp < (int)wp.size(); ++k) {
    double g = dist(traj[k], wp[next_wp]) - eps;   // g = ||p-w|| - eps
    if (g <= 1e-9) {                               // 互补:g<=0 时进度才可推进
      std::printf("  步%2d (%.2f,%.2f): 入航点%d容差圈(g=%.3f) -> 航点%d完成\n",
                  k, traj[k][0], traj[k][1], next_wp, g, next_wp);
      done[next_wp] = true; ++next_wp;
    }
  }
  int n = 0; for (bool d : done) n += d;
  std::printf("通过航点数: %d / %zu\n", n, wp.size());
  return 0;
}
// 运行输出:
//   步 6 (1.71,0.00): 入航点0容差圈(g=-0.214) -> 航点0完成
//   步13 (3.71,1.71): 入航点1容差圈(g=-0.096) -> 航点1完成
//   步20 (5.71,0.29): 入航点2容差圈(g=-0.096) -> 航点2完成
//   通过航点数: 3 / 3

Step 2 — 正确要点。 这份演示抓住了 CPC 的本质。其一，进度只在靠近航点时推进：代码里只有 g = ||p-w||-eps <= 0（进入容差圈）时才标记航点完成——这正是互补条件 \(\mu\cdot g=0\) 的离散体现（\(g>0\) 时进度乘子 \(\mu\) 必须为 0，进度不能推进）。其二，何时过航点不是预先指定的：代码没有规定"第几步必须到第几个航点"，而是让轨迹自然飞过、在进圈那一刻才计数——真 CPC 里这一步由优化器决定（它会调整轨迹和时间分配，让总时间最短）。其三，按序约束：next_wp 保证航点按顺序完成——竞速要按门的顺序过。真 CPC 把这套互补约束 + 动力学 + 执行器极限放进 MPCC，最小化总时间，同时优化轨迹、输入、时间分配——这份 demo 只演示了互补约束"卡住进度"的核心逻辑。

Step 3 — 一个常见的错误写法。 新手处理"按序过航点"时，最容易犯的错是**把航点硬绑到固定时间步**：

// 反例:预先规定"第 i 个航点必须在第 k_i 步通过"(固定时间分配)
for (int i = 0; i < n_wp; ++i) {
  int k_i = (i + 1) * total_steps / (n_wp + 1);  // 错:人为均匀分配航点到时间步
  add_constraint(traj[k_i] == wp[i]);             // 硬性:第 k_i 步必须在航点 i
}
// 问题:(1) 时间分配被写死且通常【非最优】——最优分配往往不均匀
//           (比如该在前段快冲、后段慢调,均匀分配做不到);
//       (2) 离散网格粗 => 分配精度差;
//       (3) 这正是 CPC 论文批判的"把航点作为约束分配到特定离散时刻"——
//           使前人方法无法产出真正时间最优的轨迹。

这个错误把"时间分配"这个本该优化的量预先猜死，猜得再准也只是某个固定分配下的最优，不是全局时间最优。

Step 4 — 对比。 正确版（CPC 思路）用互补约束，进度"靠近才推进"，何时过航点由优化决定——时间分配是产物。错误版预先把航点钉到固定时间步——时间分配是输入、且通常非最优。两者的差别正是 CPC 击败人类飞手的关键：把时间分配从"人为预设"解放成"优化变量"，配合直接轨迹优化（产 bang-bang），才逼出了真正的时间最优。代价是互补约束难解、只能离线——这是为极致性能付的账。

带数字走一遍¶

把 CPC demo 的输出摊开，能看清互补约束"卡住进度"的逻辑。场景是 3 个航点（竞速门）\(\{(2,0),(4,2),(6,0)\}\)、容差 \(\epsilon=0.5\)，一条从起点依次飞向三门的轨迹。输出显示：在第 6 步、位置 \((1.71,0.00)\) 时进入航点 0 的容差圈（\(g=\|p-w_0\|-\epsilon=-0.214\le 0\)），航点 0 被标记完成；第 13 步、位置 \((3.71,1.71)\) 进入航点 1 容差圈（\(g=-0.096\)），航点 1 完成；第 20 步同理完成航点 2；最终 \(3/3\) 通过。逐个数字读它的含义。 为什么进度只在 \(g\le 0\) 时推进？ 因为互补条件 \(\mu\cdot g=0\) 要求"乘积为零"：当 \(g>0\)（在容差圈外）时，要让乘积为零就必须 \(\mu=0\)（进度乘子为零，进度不能推进）；只有 \(g\le 0\)（进圈了）时 \(\mu\) 才被允许为正、进度才能推进。代码里那行 if (g <= 1e-9) 正是这个条件的离散体现。 为什么三个 \(g\) 值都是负的小数（-0.214、-0.096）？ 因为轨迹是从门附近"擦过"的——进了容差圈（\(g<0\)）但没正中靶心（\(g\) 没到 \(-\epsilon\) 那么负），这正是"进圈即算通过"的容差语义：不要求精确命中航点，只要进入 \(\epsilon\) 半径内。 为什么是按 6、13、20 这个步序、而非均匀分布？ 因为门的位置和轨迹形状决定了"何时进圈"——这一步在真 CPC 里不是预设的，而是优化器调整轨迹和时间分配的结果。这正是 CPC 的核心：何时过哪个门，是优化的产物，不是输入。把容差 \(\epsilon\) 改小到 \(0.1\) 再跑，你会看到某些门可能"通不过"（轨迹擦得不够近、\(g\) 始终 \(>0\)）——这说明容差 \(\epsilon\) 控制着"多接近才算通过"，是 CPC 里一个需要权衡的参数（太大则过门不精确，太小则难满足）。

⚠️ 常见陷阱¶

💡 编程陷阱：把航点硬绑到固定离散时间步 - 错误想法：要按序过 \(N\) 个航点，那就把它们均匀分配到时间步上，每个航点一条"第 \(k_i\) 步必须在此"的约束。 - 现象 / 后果：轨迹被迫按这个人为分配走，得到的"最优"只是该固定分配下的最优，不是真正时间最优——而最优分配往往是不均匀的（前段冲刺、后段调整）。 - 根本原因：时间分配本该是优化变量，预先钉死就丢了这个自由度；离散网格还限制了精度。 - 正确做法：用 CPC 的互补进度约束，让"何时过哪个航点"成为优化的产物——进度只在靠近航点时推进，分配由优化器自己定。

⚠️ 概念陷阱：以为 CPC 能像 MINCO 一样实时在线跑 - 错误想法：CPC 这么强，那拿来在线实时规划无人机飞行吧。 - 现象 / 后果：求解慢得离谱（秒级甚至更久），根本满足不了在线重规划的频率。 - 根本原因：互补约束属于 MPCC，违反标准 NLP 的约束规范条件、可行域有组合结构，求解既慢又对初值敏感——CPC 是**离线**生成时间最优轨迹的工具，不是在线规划器。 - 正确做法：明确 CPC 的定位——离线算出"这条赛道理论最快"的参考轨迹；在线跟踪/重规划要用别的（如 MPCC++、acados NMPC 这类实时方法）。

🧠 思维陷阱：以为时间最优轨迹是平滑的 - 错误想法：最优轨迹应该是光滑优雅的曲线，像 minimum-snap 那样。 - 现象 / 后果：用多项式（平滑）方法去逼近时间最优，结果总是慢于 CPC，却不明白为什么。 - 根本原因：时间最优控制在执行器饱和时是 bang-bang 的（推力在极限间切换，庞特里亚金极小值原理），本质上**不平滑**；多项式的固有平滑性磨掉了这种切换，发挥不出执行器全部潜力。 - 正确做法：理解"时间最优 ≠ 平滑"——要榨干性能就得允许 bang-bang，这要求直接优化状态-输入轨迹（CPC 的做法），而非用平滑的多项式参数化。

工程落地注意事项¶

CPC 的定位很特殊——它是离线的极致性能工具，落地方式和前几个在线方法很不一样。 它是离线"算标准答案"的。 CPC 不在飞机上跑，而是在地面计算机上、针对已知赛道**事先**算出时间最优参考轨迹（可能算几分钟到几十分钟）。这条轨迹是"这条赛道理论能飞多快"的标准答案、是研究和比赛的基准。日常飞行不用它。 互补约束的数值技巧。 直接把 \(\mu\cdot g=0\) 喂给 Ipopt 往往不收敛（违反约束规范）。实践中要用**松弛**（把 \(\mu\cdot g=0\) 放松成 \(\mu\cdot g\le \delta\)，\(\delta\) 从大到小退火）或**罚函数**等技巧，配合好的初值，才能求解。 rpg_time_optimal 的 CasADi 实现包含了这些处理，是学习互补约束求解的好材料。 初值与赛道几何。 CPC 对初值敏感（非凸 + 互补约束）。通常用一条朴素的轨迹（如航点间直线、或一条 minimum-snap 轨迹）做初值。赛道航点的顺序、间距、容差 \(\epsilon\) 的设置都影响收敛——这些是 CPC 调试的主要旋钮。 怎么用到真飞行。 把 CPC 离线算出的时间最优轨迹当**参考**，在线用一个能实时跟踪的控制器（如 MPCC、或 §T3.6 提到的 acados NMPC）去跟、并实时微调应对扰动和建模误差。这就是"离线 + 在线"的组合：CPC 给极致的参考，在线方法保实时跟踪。 Actor-Critic MPC、MPCC++ 等前沿工作正是沿这个方向，把"接近 CPC 的性能"搬到在线。 模型保真度决定上限。 CPC 的"时间最优"是相对它用的模型而言的——模型越准（含气动阻力等），算出的轨迹越接近真机能飞的极限。论文里用了带线性阻力的四旋翼模型；要榨干更多性能，就要更精确的（甚至学到的）动力学模型——这正是 MPCC++ 嵌入"学到的气动残差"的动机。

练习¶

[实现] 把上面的 CPC demo 改进：(a) 加一个"按序"的强约束检查（航点必须 0→1→2 顺序完成，不能跳过）； (b) 故意构造一条"漏过某个航点"的轨迹，验证它通不过该航点（进度卡住）。体会互补约束如何强制"必须真的飞到才算数"。
[分析] 互补约束 \(\mu \cdot g = 0\) 为什么难解？（提示：在 \(\mu=0, g=0\) 这个点附近，可行域是两条线的并——像字母 "L"，不是光滑流形）。这种"乘积为零"的约束属于 MPCC，标准 NLP 求解器的哪条假设被它破坏了？
[跨章] 本章五个方法对"时间"的处理逐步深入：CILQR（时间隐含在 \(v\)）→ TEB/MINCO（段时间 \(\Delta T_i\) 作变量）→ CPC（连航点-时刻对应都作变量）。请画一条"时间作为决策变量"的深入程度光谱，标出五个方法的位置，并说明为什么越往后越接近"真正时间最优"、代价是什么。

§T3.6 方法选型指南 ⭐⭐¶

五个方法讲完，你已经看清它们各自的招式和适用场景。但工程中真正的问题不是"理解某个方法"，而是"面对手头这个任务，到底该选哪个、怎么组合"。这一节把选型这件事讲透——不是让你背对照表，而是给你一套能自己推导出答案的思考框架。

讲完五个方法，最实际的问题是：面对一个具体任务，该选哪个？ 这一节给一张对照表和一套决策逻辑，并回到本章开头那个"三选择框架"收束全章。

五大方法对照表¶

方法	适用场景	时间怎么处理	约束怎么处理	求解器	实时性	代码
CILQR	自驾城市交互	隐含在状态 \(v\)、步长	软（log-barrier）	iLQR 内部	~50 Hz	需自写
TEB	移动机器人局部规划	段时间 \(\Delta T_i\) 作变量	软（加权 hinge）	g2o（GN/LM）	~20 Hz	ROS 标配
OBCA	自驾泊车	状态序列含时间	硬（对偶重构）	CasADi/Ipopt	~5 Hz	Apollo 内含
MINCO	无人机轨迹	段时间 \(T\) + 同步变形	硬（光滑映射消去）	L-BFGS	~100 Hz	ZJU 开源
CPC	无人机竞速	连航点-时刻都作变量	硬（互补约束）	Ipopt/CasADi	离线	UZH 开源
acados NMPC	通用时空 NMPC	状态序列含时间	硬（SQP-RTI）	HPIPM	~500 Hz	BSD-2 工业级

（末行 acados 不是本章主讲方法，但它是工业界做通用时空联合 NMPC 的常用底座，列在这里供选型参考。）

决策逻辑：三个问题定方向¶

选型不必死记表格，问自己三个问题就能定方向。

问题 1:你的载体和场景是什么?
  ├─ 自动驾驶(车) ──┬─ 城市道路动态交互 → CILQR(软约束,~50Hz,iLQR 快)
  │                 └─ 泊车/开放空间    → OBCA(硬避障,精确,Hybrid A* warm-start)
  ├─ 移动机器人(地面) → TEB(ROS 标配,多拓扑,g2o,直接能用)
  └─ 无人机(空) ─────┬─ 一般飞行/在线   → MINCO(降维,~100Hz,时空同步)
                     └─ 竞速/极致时间最优 → CPC(离线,互补约束,bang-bang)

问题 2:要实时在线,还是离线算最优?
  ├─ 在线(几十~几百 Hz) → CILQR / TEB / MINCO / acados
  └─ 离线(求极致时间最优) → CPC

问题 3:避障要硬保证,还是软的够用?
  ├─ 软约束够用(允许微小越界 + 安全裕度) → CILQR / TEB
  └─ 必须硬保证(泊车、窄缝、竞速) → OBCA / MINCO / CPC

这三个问题对应的，正是"统一问题形式"那三个选择的实践版：问题 1 关乎**表示**（状态序列 vs 段时间 vs 互补变量），问题 3 关乎**约束处理**（软 vs 硬），而问题 2 关乎**代价**里时间项的极致程度。把这三问答清，方法自然浮现。

用统一框架回看五个方法¶

本章开头给了一个"三选择框架"（表示 / 约束处理 / 代价），承诺每讲一个方法都回来填。现在五个方法讲完，把这张表填齐，五个看似零散的方法就被组织成了同一问题的五种解法。

方法	表示（轨迹+时间）	约束处理（怎么进优化）	代价（时间项的角色）
CILQR	离散状态-控制序列 \(\{x_k,u_k\}\)，时间隐含	log-barrier 软化（可行侧也推）	跟踪+舒适为主，时间隐含
TEB	位姿+段时间序列 \(\{s_i,\Delta T_i\}\)	单边 hinge 软化（越界才罚）	\(\sum\Delta T_i\) 显式时间项
OBCA	状态序列（含时间）	对偶变量精确重构（硬、光滑）	跟踪+平滑为主
MINCO	多项式+航点+段时间 \((q,T)\)	光滑映射精确消去（硬、无约束化）	控制能量+段时间
CPC	状态-输入序列+进度变量 \(\lambda\)	互补约束（硬、MPCC）	总时间为唯一/主目标

读这张表能看出三条规律。 表示越往下越"高级"：从直接的状态序列（CILQR/OBCA），到引入段时间（TEB），到压缩参数化（MINCO），到额外的进度变量（CPC）——表示的精巧程度，决定了能优化的自由度和求解效率。 约束处理从软到硬、从近似到精确：CILQR/TEB 软化（不保证满足）、OBCA/MINCO 精确重构（保证且光滑）、CPC 互补（保证但最难解）——这正是"约束怎么进优化"那条主线的递进。 时间项的地位决定"时间最优"的程度：CILQR/OBCA 把时间当副产品、重跟踪舒适，TEB/MINCO 把段时间作显式代价、追求快，CPC 把总时间当唯一主目标、连分配都优化——这条线对应"时间作为决策变量"的深入程度。框架填齐后，你面对一个新方法（比如本章没讲的 acados NMPC、或某篇新论文），也可以用同样的三栏去拆解它——这比记住每个方法的名字有用得多。

一个常被忽略的事实：它们可以组合¶

这些方法不是互斥的单选项，工程上常组合用。最常见的组合是**前端定 homotopy + 后端连续精修**——这是贯穿 T2、T3 的主线：Hybrid A*（离散）→ OBCA（连续），搜索/走廊（T2）→ CILQR/MINCO（T3）。前端给可行初值、定对绕行方向，后端在其上求精——既补了连续优化"对初值敏感、卡 homotopy"的短板，又发挥了它"轨迹光滑、动力学精确"的长处。另一种组合是**离线 + 在线**：用 CPC 离线算出赛道的时间最优参考轨迹，再用 MPCC++/acados 在线跟踪并实时微调——把 CPC 的极致最优和在线方法的实时性结合。

实时性从哪来：一个共同的工程秘密¶

§T3.6 的对照表里，CILQR ~50 Hz、MINCO ~100 Hz、acados 甚至到几百 Hz——这些"实时"不是凭空来的，背后有一个共同的工程秘密值得点破：充分利用最优控制问题的时序结构 + 不追求每步解到收敛。前者前面讲过：iLQR/MINCO/acados 都不把整条轨迹堆成一个大矩阵硬解，而是利用"时间是链式的"做结构化求解（iLQR 的反向递推、MINCO 的线性复杂度 \(M(q,T)\)、acados 的 Riccati 型 QP 求解），复杂度随步数线性而非立方增长。后者是一个更"激进"的工程权衡——以 acados 的**实时迭代（RTI, Real-Time Iteration）**为代表：标准 SQP 会在每个控制周期把非线性问题迭代到收敛，而 RTI **每个周期只做一次线性化、解一个 QP 子问题**就输出，不等收敛。这听起来"不够优"，但巧妙之处在于：控制是高频滚动的，这一周期没解到的，下一周期接着解——相当于把"迭代到收敛"摊到了连续多个控制周期上，每个周期的计算量固定且小，于是能跑出固定的高频率。 RTI 还能把计算拆成"准备阶段"（在拿到当前状态前就能算的部分）和"反馈阶段"（拿到状态后必须算的部分），把控制延迟压到最低。 acados 用 HPIPM（基于 BLASFEO 线性代数库、针对 CPU 架构调优的内点法 QP 求解器）做底层求解，把单次 QP 压到微秒级——这是它能上几百赫兹的根本。理解这个秘密，你就明白为什么"实时"和"最优"常常要折中：在线方法（CILQR/MINCO/acados）用"结构化 + 不解到收敛"换实时，离线方法（CPC）用"放弃实时"换极致最优——没有免费的午餐，选型时这个折中是绕不开的核心考量。

选型时容易踩的几个误区¶

最后提醒几个工程选型里反复出现的误判，避开它们比记住对照表更重要。 误区一：盲目追求"最强"的方法。 看到 CPC 击败人类飞手，就想"那都用 CPC"——忘了它是离线的，根本上不了在线。方法没有绝对强弱，只有"适不适合你的实时性、约束硬度、载体"。日常飞行用 MINCO 远比"硬上 CPC"现实。 误区二：忽视前端的重要性。 把全部精力放在调后端优化器上，却给一个糟糕的初值/走廊。本章反复强调：这些非凸优化都对初值敏感，前端（搜索/走廊/Hybrid A*）的质量常常比后端调参更决定成败。" 垃圾进垃圾出"在这里尤其成立。 误区三：软硬约束选反。 在泊车这种刮蹭场景用软约束（CILQR），留的裕度不够就刮了；或在高速行车的高频重规划里硬上 OBCA（~5 Hz），跟不上实时。软硬约束的选择要匹配"后果严重性"和"实时性要求"——见 FAQ 那条。 误区四：在错误的抽象层解决问题。 想用一个时空优化器解决本该由决策层解决的问题（比如指望 CILQR 自己想清楚"该超车还是跟车"）。这些方法是**运动层**，绕不过同伦类——离散决策（超车/让行、绕左/绕右）该由上游的行为/决策层给，优化器只在选定决策下精修。层次搞混，怎么调都不对。 误区五：脱离动力学谈轨迹。 优化出一条数学上漂亮、但真车/真机跟不了的轨迹（动力学约束设得和真实执行器不符）。规划的动力学约束必须和真实平台一致、且和下游跟踪控制器的能力匹配——否则规划再优，执行也走样。这五个误区背后是同一个道理：时空优化器只是整个自主系统的一环，它的效果取决于前端给的初值、上游给的决策、下游控制器的跟踪、以及和真实动力学的吻合。把它孤立地调到最优，不等于系统最优。

本章常见误解汇总¶

把全章散落的"想当然"集中列出，临考/上手前扫一眼。

误解	实情	出处
iLQR 能直接处理约束	iLQR 本身只能解无约束问题；CILQR 靠 barrier 把约束变成代价才让它"会"处理约束	§T3.1
CILQR 的约束是硬约束	barrier 是软的，解可能轻微越界，越界量由 \(\mu\) 控制；安全关键要留裕度	§T3.1
给 CILQR 任意初值都能优化好	log-barrier 要求第一次迭代就可行，需要可行 warm-start（T2/Hybrid A*）	§T3.1
速度约束写成 \((v-v_{\max})^2\) 即可	那是"逼近 \(v_{\max}\)"的等式语义；不等式约束要用单边 \([\max(0,\cdot)]^2\)	§T3.2
段时间可以是任意实数	必须恒正，用 \(\Delta T_i=e^{\tau_i}\) 参数化或正性约束	§T3.2
单条 TEB 能找全局最优	单条卡在初始 homotopy；要多拓扑并行才躲局部极小	§T3.2
用障碍外接圆/内切圆近似避障	外接圆过度保守、内切圆危险漏判，都不精确；OBCA 用半空间精确处理	§T3.3
OBCA 的对偶重构消除了非凸性	它消除的是不可微性，不是非凸性；重构后仍是非凸 NLP，需 warm-start	§T3.3
MINCO 的 \(M(q,T)\) 是某种拟合/近似	是"最小控制量+连续+过航点"最优性条件的精确解，无近似	§T3.4
优化系数和优化 \((q,T)\) 没本质区别	参数化决定维度、约束数、能否时空同步——直接决定求解速度	§T3.4
CPC 能在线实时跑	互补约束属 MPCC，求解慢，CPC 是离线生成时间最优轨迹的工具	§T3.5
时间最优轨迹是平滑的	执行器饱和时是 bang-bang（不平滑）；多项式平滑性发挥不出极限	§T3.5

本章小结¶

本章完成了 T 线的数学核心——把时间真正变成连续优化里的决策变量。沿着"统一问题形式"的三选择框架（表示 / 约束处理 / 代价），五个方法各有侧重：CILQR（§T3.1）用 log-barrier 把约束变成代价、在 iLQR 框架内做自驾时空联合； TEB（§T3.2）把段时间显式作变量、用 g2o 超图软约束、多拓扑并行，成为 ROS 标配； OBCA（§T3.3）用强对偶把非凸避障精确光滑化、做泊车硬约束 NMPC； MINCO（§T3.4）用参数映射 \(c=M(q,T)\) 把高维系数压成低维航点+段时间、时空同步变形、无人机百赫兹求解； CPC（§T3.5）用互补进度约束让"何时过航点"也成变量、离线求真正时间最优、击败人类飞手。 §T3.6 给出选型。贯穿全章的两条主线：一是"约束怎么进连续优化"（从软化的 barrier/hinge，到精确重构的对偶/光滑映射/互补约束，难度与保证递增）；二是 T2、T3 共同的"离散定 homotopy + 连续精修"分工（CILQR/OBCA 配 warm-start、TEB 多拓扑、MINCO 配 T2 走廊）。

术语速查¶

术语	英文	一句话定义
时空轨迹优化	spatiotemporal trajectory optimization	在一个连续优化里同时求空间轨迹与时间分配
约束 iLQR	CILQR	iLQR + log-barrier，把约束纳入代价的时空联合优化
定时弹性带	TEB	位姿+段时间序列、g2o 软约束、多拓扑并行
段时间	segment time \(\Delta T_i\)	相邻路点间的时长，作为决策变量
优化式避障	OBCA	用强对偶把非凸避障精确重构成光滑约束
对偶变量	dual variable \(\lambda\)	参数化分离超平面，让避障约束可微
参数映射	\(c=M(q,T)\)	MINCO 中系数由航点+段时间唯一确定
互补进度约束	CPC	用互补条件让"何时过航点"成为优化变量
bang-bang 控制	bang-bang control	执行器在极限间切换，时间最优的特征
互补约束规划	MPCC	含"乘积为零"约束的数学规划，难解
微分平坦	differential flatness	状态/控制可由平坦输出及其导数代数确定（无人机=位置+偏航）
实时迭代	RTI	每控制周期只做一次线性化+一次 QP，不解到收敛，换固定高频
热启动	warm-start	用前端粗解当初值，破非凸优化对初值敏感、卡 homotopy 的局限

知识点总表¶

知识点	核心结论	关联
约束进优化的两类	软化（barrier/hinge，可微但不保证满足）vs 精确重构（对偶/映射/互补，保证但更难）	§T3.1–§T3.5
CILQR 的局限	barrier 软 + 要可行初值 → 配 warm-start	§T3.1
TEB 的核心	段时间作变量 + g2o 稀疏 + 多拓扑躲局部极小	§T3.2
OBCA 的本质	强对偶把不可微避障精确变光滑（非消除非凸）	§T3.3
MINCO 的威力	\(c=M(q,T)\) 降维+自动连续+时空同步	§T3.4
CPC 的突破	时间分配成为变量 → 真正时间最优（bang-bang，离线）	§T3.5
贯穿主线	离散定 homotopy + 连续精修	§T3.1,§T3.3,§T3.6
微分平坦	无人机只规划位置，姿态/推力是位置轨迹的副产品	§T3.4
实时 vs 最优	在线方法靠"结构化+不解到收敛"换实时，离线方法弃实时换极致最优	§T3.6

累积项目：把规划器升级到"时空联合优化"¶

延续 T1、T2 的累积项目（T 线规划器），本章把它的"运动层"从 T1 的解耦 QP、T2 的走廊 QP，升级到**时空联合的连续优化**。

本章新增模块¶

// T 线累积项目:时空轨迹优化层接口(承接 T2 走廊,本章实现)
// 把 T2 走廊内的 QP 升级为 NLP:处理非线性动力学、时空联合
class SpatiotemporalOptimizer {
 public:
  virtual ~SpatiotemporalOptimizer() = default;

  // 输入:T2 给的走廊(凸 cube 序列,提供安全约束与 homotopy)+ 可行初值(warm-start)
  // 输出:平滑、动力学可行、时空联合最优的轨迹;失败返回 nullopt
  virtual std::optional<Trajectory> Optimize(
      const std::vector<Cube>& corridor,        // T2 走廊:几何约束 + homotopy
      const Trajectory& warm_start,             // 可行初值(T2 搜索粗解):破 CILQR/OBCA 的局限
      const VehicleModel& model,                // 非线性动力学(自行车/四旋翼)
      const Limits& limits) = 0;                // 速度/加速度/曲率等界
};

// 具体实现可以是 CILQR(自驾)、MINCO(无人机)等;接口统一
// 关键:warm_start 来自 T2,补上连续优化"对初值敏感、卡 homotopy"的短板

验收标准¶

正确性：在动态绕障场景下，输出轨迹密采样无碰撞、满足动力学（速度/加速度/曲率在界内）。
承接 T2：走廊和 warm-start 来自 T2 的搜索/走廊；优化结果落在 warm-start 所在的 homotopy 内（验证：不无故跨到另一侧）。
时空联合：与 T1 解耦管线在强耦合场景对比，时空联合的轨迹更优（更平滑、通过时间更短、无"路径定死导致速度无解"）。
降级：优化失败（不收敛/不可行）时返回 nullopt，上层降级减速，不输出劣质轨迹。
软硬约束意识：若用软约束（CILQR），安全关键约束留足裕度；若用硬约束（OBCA/MINCO），验证严格满足。

渐进式实现路线¶

从零搭一个时空优化层，别想一步到位。下面给一条由易到难、每步可验证的路线——每一步都能独立跑通、看到结果，再进入下一步，避免"写完一大坨全是 bug 无从查起"。 第 1 步：先把 demo 跑通。 拿本章对应方法的最简 demo（CILQR barrier / MINCO 的 \(M(q,T)\)）当起点，确认它在你的环境编译运行、输出符合预期。这一步是为了建立信心和基线。 第 2 步：升到二维 + 真动力学。 把一维 demo 扩到二维（位置 \((x,y)\) 甚至加朝向），把简单积分器换成真实车辆/无人机模型（自行车模型 / 四旋翼）。先不加避障，只验证"能从起点优化到终点、轨迹光滑、满足动力学界"。 第 3 步：加避障约束。 加入一个静态障碍——软约束方法用 barrier/hinge，硬约束方法用 OBCA 的有符号距离。验证轨迹绕开障碍。这一步最容易出 bug（barrier 的越界延拓、距离的可微性），用本章陷阱清单对照排查。 第 4 步：接 T2 的 warm-start。 把 T2 的搜索/走廊接进来——用 T2 粗解做初值、用 T2 走廊做几何约束。验证"难初始化的场景（障碍多、绕行复杂）现在能稳定收敛"——这一步直接体现 warm-start 补 CILQR/OBCA 局限的价值。 第 5 步：加时间维与降级。 把段时间纳入优化（若方法支持）、加入动态障碍的时空约束；并实现降级逻辑（优化失败返回 nullopt、上层减速）。这一步让它从"静态轨迹优化"变成真正的"时空规划层"。 第 6 步：性能与回归测试。 测求解时间是否在控制周期内、跑评价指标那套回归测试集（静态/动态/窄缝/时间最优/不可行/warm-start 敏感性）。这一步决定它能不能上车。每步都留一个可视化（轨迹、速度曲线、约束违反量），出问题时一眼能定位是哪一环。这条路线对应本章的知识顺序：第 1-3 步用 §T3.1–§T3.5 的方法，第 4 步用 T2，第 5-6 步用本章的降级和评价指标讨论。

评价指标¶

时空轨迹优化的质量从四个维度量化。

维度	指标	含义
最优性	代价值、通过时间	优化目标的最终值、完成任务的总时间（时间最优方法尤其看后者）
可行性	约束违反量、密采样碰撞率	动力学/避障约束的最大违反（软约束方法重点查）、轨迹采样碰撞比例
平滑性	最大 jerk/snap、控制突变	高阶导峰值、控制量的不连续程度（影响执行与舒适）
计算	求解时间、迭代次数、收敛率	单次优化耗时（实时性）、迭代到收敛的次数、收敛成功的比例

回归测试集：(1) 静态绕障（基础）； (2) 动态障碍横穿（时空耦合）； (3) 窄缝穿越（硬约束/走廊宽度）； (4) 时间最优赛道（多航点，考验时间分配）； (5) 不可行场景（验证降级）； (6) warm-start 敏感性（同场景换初值，看是否卡不同 homotopy）。其中"求解时间"和"收敛率"对选型尤其关键——CILQR/MINCO 必须在控制周期内算完，CPC 则不限时但要收敛到全局更优。

这些指标之间会打架，要看场景定主次。 四个维度不是独立的，常常此消彼长，理解它们的权衡比单看某个数字重要。 最优性 vs 计算：追求更优（更短时间、更低代价）往往要更多迭代、更长求解时间——CPC 把最优性推到极致，代价是离线（计算时间不限）；在线方法为了实时，主动牺牲一点最优性（如 RTI 不解到收敛）。选型时先问"实时性是硬约束吗"，是则最优性让步。 可行性 vs 最优性：留更大的安全裕度（更可行、更安全）通常意味着更保守的轨迹（更慢、绕更远，最优性下降）。软约束方法靠调权重在这两者间滑动，硬约束方法则把可行性设成不可让步、在其下求最优。安全攸关场景，可行性优先。 平滑性 vs 最优性：时间最优轨迹是 bang-bang（不平滑），而平滑轨迹（如 minimum-snap）牺牲了一点时间最优换来执行友好。要舒适/易跟踪就重平滑性，要榨干性能就允许不平滑——这正是 §T3.5 那个"时间最优≠平滑"的指标体现。 怎么读这套指标：先按场景定一个"主指标"（自驾城市道路可能是平滑性+可行性，竞速是通过时间，泊车是可行性+精确性），再把其余指标当"约束"（必须达标但不追求极致）。脱离场景比较"哪个方法指标更好"没有意义——一个为竞速优化到极致时间最优的方法，拿到要求舒适平稳的载人场景反而是缺点。这也是为什么本章反复说"没有最好的方法，只有最适合场景的方法"。

FAQ¶

Q：CILQR、TEB、OBCA、MINCO、CPC 这么多，到底有没有一个"最好"的？ 没有。它们是为不同场景设计的：自驾交互（CILQR）、移动机器人（TEB）、泊车（OBCA）、无人机（MINCO）、竞速（CPC）。选型看载体、实时性要求、避障是否要硬保证（见 §T3.6 三问）。底层数学（iLQR/NLP/QP/L-BFGS）相通，但工程取舍不同。

Q：软约束（CILQR/TEB）和硬约束（OBCA/MINCO/CPC）到底怎么选？ 看后果。软约束可微、好解、能容忍不可行初值，但不保证约束满足——适合"轻微越界可接受 + 留安全裕度"的场景（城市道路保持车道）。硬约束保证满足但更难解、对初值更敏感——适合"差一点就出事"的场景（泊车刮蹭、窄缝、竞速撞门）。不确定时，软约束 + 足够安全裕度 + 外层硬性安全检查，是稳妥的折中。

Q：为什么这些方法都需要 warm-start / 前端？ 因为避障问题本质非凸（绕左绕右是不同 homotopy），连续优化只能在初值所在的 homotopy 内找局部最优、跨不过去。 warm-start（来自 T2 搜索/走廊、或 Hybrid A*）负责"定对 homotopy + 给可行初值"，连续优化负责"在其上求精"。这是 T2、T3 贯穿的分工，不是某个方法的特例。

Q：MINCO 和 CILQR 都做时空联合，区别在哪？ 表示不同（统一问题形式的"选择一"）。 CILQR 用离散状态-控制序列 + iLQR，通用、适合有清晰动力学模型的自驾； MINCO 用多项式 + 参数映射 \(c=M(q,T)\)，降维、适合无人机这种要高频求解、且轨迹可用多项式良好表示的场景。 CILQR 更通用，MINCO 更专精高效。

Q：CPC 既然能击败人类飞手，为什么不普及到所有无人机？ 因为它是**离线**的——互补约束求解慢（秒级以上），只能事先算好赛道的时间最优参考轨迹，没法在线实时重规划。日常飞行（要避未知障碍、要实时反应）用 MINCO/acados 这类实时方法； CPC 用于"已知赛道、离线榨干性能"的特定场景。

Q：这章和 T2 到底什么关系？ T2 是 T3 的前端和初始化来源。 T2 的走廊（SSC/SFC）给 T3 的优化提供**几何约束**（轨迹待在凸 cube 内）和 homotopy； T2 的搜索（时空格/SIPP）给 T3 提供**可行的 warm-start 粗解**。可以说 T2 搭好舞台、定好方向，T3 在舞台上用更强的优化引擎（NLP）求出精轨迹。 T2 的"走廊内 QP"被 T3 推进成了"走廊内 NLP"。

Q：学完 T3，时空规划就完整了吗？ 方法论核心完整了，但还有两个重要假设没破：本章默认动态障碍的**预测是给定且可信**的（不确定性留给 Part-U）、默认其他智能体**不会因你的动作改变意图**（强交互的博弈留给 Part-G）。 T1-T3 建立了"假设世界可预测"下的时空规划基础；直面不确定与交互，是后续 Part 的任务。

Q：既然这些优化方法这么复杂，为什么不直接用强化学习端到端学一个规划器？ RL/端到端是活跃方向，但和本章的优化方法目前是**互补**而非取代。优化方法的核心优势是**可解释 + 可证保证**：约束写在那里，解必满足（硬约束）或近似满足（软约束），出了问题能定位到是哪个约束、哪个权重。 RL 学到的策略是黑盒，难以形式化保证"一定不撞、一定满足动力学"，在安全攸关的规控里这是硬伤。前沿做法（Actor-Critic MPC、本章末尾提到的）正是**结合**两者——用 RL 调优化器的参数、或用学习补难建模的动力学，而把可行性保证留给优化。理解本章的优化方法，恰恰是理解"为什么端到端方案里还要留一个优化的安全护栏"的前提。

Q：TEB 用 g2o、SLAM 也用因子图（如 GTSAM），它们能互换吗？轨迹优化和 SLAM 后端是一回事？ 本质上都是**稀疏非线性最小二乘**——把问题建模成"节点（变量）+ 因子/边（约束）"，利用稀疏性高效求解。所以 g2o 和 GTSAM 在数学内核上相通，理论上可互换后端（确有用 GTSAM 做轨迹优化的工作，如 GPMP2）。但侧重不同：SLAM 后端的因子来自传感器观测（里程计、回环），轨迹优化的因子来自规划约束（速度、加速度、避障、时间）；变量也不同（SLAM 是位姿/路标，TEB 是位姿+段时间）。" 结构一样、内容不同"——理解了这点，你会发现 SLAM 和轨迹优化在工具层面是近亲，这也是为什么学过因子图 SLAM 的人上手 TEB 会很快。

Q：本章反复说"用 T2 的搜索/走廊做 warm-start"，具体是怎么"喂"给优化器的？ 分两部分喂。一是**初值**：T2 搜索给出的粗解（一串离散的 \((s,l,t)\) 路点），插值/采样成优化器需要的初始轨迹——对 CILQR 是初始状态-控制序列，对 MINCO 是初始航点 \(q\)，对 OBCA 是 Hybrid A* 的粗轨迹。优化器从这个"已经大致对"的初值出发，而非从零乱猜，于是更易收敛、且收敛到正确的 homotopy。二是**约束**：T2 走廊给出的凸 cube 序列，直接作为优化的几何约束（轨迹/控制点要待在 cube 内）——MINCO 用光滑映射消化它、走廊 QP 直接用它做线性约束。所以"warm-start"在工程上是两件事：给个好初值（定 homotopy + 加速收敛）、给组安全约束（圈定可行空间）。这也是为什么 §T3 的累积项目接口里，Optimize 同时接收 corridor 和 warm_start 两个来自 T2 的输入。

延伸阅读¶

奠基与代表论文（按本章顺序）：Chen, Zhan, Tomizuka, Constrained iterative LQR for on-road autonomous driving motion planning, ITSC 2017（CILQR）； Rösmann, Hoffmann, Bertram, Integrated online trajectory planning and optimization in distinctive topologies, RAS 2017（TEB 多拓扑）； Zhang, Liniger, Borrelli, Optimization-Based Collision Avoidance, arXiv 1711.03449 / T-CST 2021 29(3):972–983（OBCA）； Zhang et al., Autonomous Parking Using Optimization-Based Collision Avoidance, CDC 2018（H-OBCA）； Wang, Zhou, Xu, Gao, Geometrically Constrained Trajectory Optimization for Multicopters, T-RO 2022（MINCO/GCOPTER）； Foehn, Romero, Scaramuzza, Time-optimal planning for quadrotor waypoint flight, Science Robotics 2021（CPC）。

前沿（2024–2026）：Romero et al., Actor-Critic Model Predictive Control, T-RO 2025（可微 MPC 作 actor、RL 调参，真机竞速）； Krinner et al., MPCC++, 2024（Frenet-Serret 隧道约束 + 学到的气动残差，100 Hz）； Improved CILQR, MDPI 2025（分段 barrier 截断 + Hybrid-A* warm-start）。

开源项目：rst-tu-dortmund/teb_local_planner（TEB，ROS）； ZJU-FAST-Lab/GCOPTER（MINCO，C++ header-only，minco.hpp 是参数映射核心）； XiaojingGeorgeZhang/OBCA 与 H-OBCA（OBCA，Julia）； uzh-rpg/rpg_time_optimal（CPC，CasADi/Python）； acados/acados（通用 NMPC，BSD-2）；以及自驾里 Apollo 的 Open Space（OBCA 思路）。

与本书其他部分：无人机方向对 MINCO/GCOPTER 有更细的推导，本章侧重时空联合的通用原理，可互为补充； Part 0 有 iLQR/NLP/QP 求解器的底层细节。

本章与后续的关系¶

后续主题	本章提供的基础	衔接点
Part-U 不确定性规划	时空联合优化的确定性框架	把动态障碍预测的不确定性显式建模（机会约束、风险敏感、鲁棒 MPC）
Part-G 博弈规划	时空轨迹作为博弈中的策略	强交互下，自车轨迹与他车意图相互影响——博弈均衡
工业框架（Apollo/Autoware）	CILQR/OBCA 的工程实现	本章方法在工业栈里的落地与工程化
多智能体协调	单体时空优化	多机各自优化 + 相互避让/协调约束
学习驱动前沿	可微优化作为可学习模块	Actor-Critic MPC、学到的动力学残差嵌入时空优化

🔧 故障排查手册¶

现象	可能原因	排查 / 修复
CILQR/barrier 优化中途崩成 NaN	对数 barrier 没做越界侧延拓，中间迭代越界吃到 \(\log\) 负数	越界侧加二次延拓、与对数侧光滑拼接（§T3.1 编程陷阱）
CILQR 不收敛、轨迹乱跳	初始轨迹不可行（log-barrier 要可行初值）	用 T2 搜索/走廊或 Hybrid A* 给可行 warm-start（§T3.1 思维陷阱）
机器人全程顶着限速、该减速不减	速度约束写成双边二次 \((v-v_{\max})^2\)（变成"逼近"）	改单边 hinge \([\max(0, v-v_{\max})]^2\)（§T3.2 编程陷阱）
优化中段时间变 0 或负、除零崩溃	段时间当普通变量、没保正	用 \(\Delta T_i=e^{\tau_i}\) 参数化或正性约束（§T3.2 概念陷阱）
TEB 总绕一边、发现不了更优的另一边	单条 TEB 卡在初始 homotopy	开多拓扑模式（distinctive topologies）并行（§T3.2 思维陷阱）
避障"明明能过的缝却过不去"或"该撞的角没躲"	用外接圆/内切圆近似多边形障碍	用半空间 + 有符号距离（OBCA 思路）精确处理（§T3.3 编程陷阱）
OBCA 求解器在障碍边角抖动不收敛	直接喂了不可微的距离约束	用对偶重构把约束变光滑（§T3.3 概念陷阱）
OBCA 收敛到劣解	以为光滑就是凸、没给好初值	光滑≠凸，用 Hybrid A* warm-start 定 homotopy（§T3.3 思维陷阱）
MINCO 优化又慢又难初始化	把多项式系数当成了优化变量	用 \(c=M(q,T)\)，只优化 \((q,T)\)，系数自动确定（§T3.4 编程陷阱）
CPC 求解极慢、上不了实时	误把 CPC（离线、MPCC）当在线规划器	CPC 离线生成参考轨迹，在线跟踪用 MPCC++/acados（§T3.5 概念陷阱）
时间最优轨迹做不快、慢于 CPC	用平滑多项式逼近、磨掉了 bang-bang	直接优化状态-输入轨迹，允许执行器饱和切换（§T3.5 思维陷阱）

API 速查表¶

以教学示意接口为主，实际项目（teb_local_planner / GCOPTER / OBCA / rpg_time_optimal）的命名与签名以各自代码为准。

功能	示意接口	说明
barrier 函数	`Barrier(g)` / `BarrierGrad(g)`	log-barrier + 越界二次延拓，光滑拼接
TEB 段时间参数化	`dT = exp(tau)`	保证段时间恒正
TEB 速度软约束	`w_vel * pow(max(0, v-v_max), 2)`	单边 hinge 惩罚
OBCA 有符号距离	`SignedDistance(p, polygon)`	\(\max_i(a_i^\top p - b_i)\)，对偶光滑化的对象
MINCO 参数映射	`c = M(q, T)`	系数由航点+段时间唯一确定（解线性系统）
CPC 互补检查	`mu * (norm(p-w)^2 - eps^2) == 0`	进度只在靠近航点时推进
时空优化顶层	`Optimize(corridor, warm_start, model, limits)`	承接 T2 走廊 + warm-start

版本信息速查¶

项目	版本 / 状态（截至本章撰写）	备注
teb_local_planner	长期维护，ROS1/ROS2	BSD，移动机器人局部规划标配
GCOPTER（MINCO）	开源，C++ header-only	MIT，`minco.hpp` 参数映射核心
OBCA / H-OBCA	开源，Julia	OBCA 通用 + H-OBCA 泊车
rpg_time_optimal（CPC）	开源，CasADi/Python	UZH RPG，离线时间最优
acados	~稳定，C/Python	BSD-2，工业级实时 NMPC
Ipopt / CasADi	稳定	OBCA/CPC 的常用 NLP 求解底座

版本会随时间变化，落地前请以各项目官方仓库为准。

研究实践建议¶

按你的目标和阶段，给三层建议。

入门（想搞懂、能复现）。 先把本章 5 份 demo 全部编译跑通、改参数观察行为（这是最快建立直觉的方式）。然后选一个和你方向最近的方法深入：自驾选 CILQR/OBCA，无人机选 MINCO/CPC，移动机器人选 TEB。深入的标志是"能从零实现一个二维版本"——比如用 Eigen 写一个带 barrier 的二维 CILQR、或实现 \(c=M(q,T)\) 的完整映射。配合精读对应论文（本章延伸阅读列了），把代码和论文对照着读。这一阶段别贪多，吃透一个方法的"表示+约束处理+求解"，胜过泛泛看五个。

进阶（想做对比、能调研）。 跑通开源实现并做横向对比：在同一组场景下比较两三个方法（如 CILQR vs MINCO），用本章"评价指标"那套维度量化（最优性/可行性/平滑性/计算），写对比报告、落到选型结论。这一阶段重点训练两个能力：一是**用统一框架拆解任意方法**（表示/约束/代价三栏），读新论文时能快速定位它的创新点；二是**理解工程权衡**（软硬约束、实时与最优、前端与后端的分工），这是从"会跑代码"到"会做系统决策"的关键。建议精读 T2、T3 的衔接（warm-start 怎么接），因为真实系统里前后端的配合往往比单个算法更决定成败。

研究（想做创新、能推进前沿）。 盯住本章末尾那几个开放问题：软约束方法的非凸约束正式处理、学到的动力学残差安全嵌入、MINCO 与 CILQR 框架的统一、互补约束的在线加速。前沿方向（Actor-Critic MPC、MPCC++）的共同思路是"学习与优化融合"——学习补难建模的部分、优化保可行性，这是当前最活跃的赛道，值得深入。做研究要同时具备三样：扎实的优化基础（Part 0 + 本章）、对真实系统瓶颈的理解（什么是实时的真正约束、什么是安全的硬底线）、以及动手复现 SOTA 的能力。建议从复现一篇近两年的顶会/顶刊工作起步，在复现中找到它的局限，那往往就是你的研究起点。

前沿工作与开放问题¶

可微优化 + 学习（Actor-Critic MPC）。 Romero 等人的 Actor-Critic Model Predictive Control（T-RO 2025）把可微 MPC 当作 RL 的 actor、用强化学习优化 MPC 的参数，在真机无人机竞速中达到 21 m/s 量级——代表了"用学习调优化、用优化保证可行"的融合趋势。

学到的动力学残差（MPCC++）。 Krinner 等人的 MPCC++（2024）在 Frenet-Serret 隧道约束的轨迹优化里，嵌入**学到的气动残差**（神经网络建模高速飞行的复杂气动效应），并用贝叶斯优化调超参，做到 100 Hz——把"难以解析建模的动力学"用学习补上，再喂进优化。

改进的 CILQR。 Improved CILQR（MDPI 2025）用分段 barrier 截断 + Hybrid-A* warm-start，把 CILQR 的收敛速度提升数倍——正面回应了 §T3.1 那个"barrier 要可行初值"的局限。

开放问题。 如何在 CILQR/TEB 这类软约束方法里**正式地**处理非凸约束（而不只是惩罚）？如何把学到的动力学残差**安全地**嵌入时空联合优化（保证学习误差不破坏可行性）？能否统一 MINCO（无人机、多项式参数化）和 CILQR（自驾、状态-控制序列）的数学框架，得到一个通用的时空优化器？互补约束（CPC）的求解能否加速到在线可用？这些问题，正是把本章内容往博士级研究推进的入口。

把这些前沿串起来看，有三个清晰的趋势。 趋势一：学习与优化的深度融合。 从"纯优化"（CILQR/MINCO/CPC）到"学习调优化"（Actor-Critic MPC 用 RL 调 MPC 参数）到"学习补优化"（MPCC++ 把学到的气动残差喂进优化）——学习负责难以解析建模的部分（复杂气动、人类偏好、参数调优），优化负责可行性与安全保证。这个分工正在成为规控的主流范式，因为它既要了学习的灵活、又保住了优化的可证性。 趋势二：把离线的极致最优搬到在线。 CPC 证明了"真正时间最优能有多快"，但它离线。后续大量工作（Actor-Critic MPC 的 21 m/s 真机、MPCC++ 的 100 Hz）都在回答同一个问题：怎么在保住实时的前提下，逼近 CPC 那样的极致性能？这条线推动着在线方法不断变快变强。 趋势三：求解器与硬件协同。 acados/HPIPM/BLASFEO 这套针对 CPU 架构调优的求解栈、以及 GPU 并行的 iLQR，都在把"同样的优化问题解得更快"——求解器层面的进步，直接拓宽了"什么能实时"的边界。算法和求解器是一对，光有好算法、求解器跟不上也落不了地。理解了这三个趋势，你就能判断一篇新论文"新在哪、属于哪条线"——这比追逐每个具体方法更重要。而所有这些前沿，都没有脱离本章的两条主线：约束如何进优化、时间如何作为决策变量。新方法只是在这两个问题上，用了更聪明（常常是学习辅助）的招式。

拓展练习（综合 / 项目级）¶

[实现 + 对比] 用 Eigen 实现一个完整的二维 CILQR（自行车模型 + 静态障碍椭圆 barrier），可视化 iLQR 迭代过程——观察轨迹如何从初始化逐步绕开障碍。再对比有无 barrier、\(\mu\) 大小不同时的轨迹差异，体会软约束的特性。
[运行 + 精读] 在 Gazebo + TurtleBot3 上跑 teb_local_planner，观察段时间 \(\Delta T_i\) 如何随障碍变化；开多拓扑模式，看"绕左/绕右"两条候选的代价对比。再精读其 g2o_types/ 下的自定义边（速度、加速度、障碍、时间间隔），标注每种边连接哪些节点、误差函数和 Jacobian 怎么写。
[实现 + 实验] 用 GCOPTER 在 2D 多航点场景做轨迹优化，改变时间惩罚权重——观察段时间 \(T_i\) 如何从均匀分布变成"直道快、弯道慢"的最优分配。这就是"时空同步变形"的直观体现。
[精读 + 推导] 精读 Zhang-Liniger-Borrelli 的 OBCA 论文，推导对偶避障的完整数学形式：标注分离超平面定理如何保证"凸障碍外 ⇔ 存在分离超平面"、对偶变量 \(\lambda\) 的物理含义、H-OBCA 的 warm-start 机制。
[思考 + 设计] 本章五个方法对"时间作为决策变量"的深入程度不同（CILQR 隐含 → TEB/MINCO 段时间 → CPC 连分配都优化）。设计一个"理想的统一时空优化器"：它该如何选择表示、处理约束、安排时间项，才能兼顾通用性、实时性、和时间最优？指出现有方法离这个理想还差什么。
[跨章综合·T1→T2→T3] 选一个具体场景——"自车在城市道路上，前方有一辆车突然 cut-in（切入）"，用 T1、T2、T3 学的东西，从头到尾设计一套完整的规划方案：(a) T1 的解耦管线在这个场景会怎么失真（为什么 cut-in 是强耦合）？ (b) T2 怎么用搜索定 homotopy（让/抢）+ 走廊把可行空间框出来？ (c) T3 用哪个方法在走廊里精修、warm-start 从哪来、软硬约束怎么选？ (d) 整条管线在哪一步可能失败、怎么降级？这道题要你把三章串成一个能落地的系统，而非三个孤立知识点。
[研究性·复现与对比] 选两个方法（如 CILQR vs MINCO，或 TEB vs OBCA），在同一个 2D 场景（起点、终点、几个障碍）下分别实现或用开源实现跑，按"评价指标"那套维度（最优性/可行性/平滑性/计算）量化对比。重点观察：同一场景下它们的轨迹差异、求解时间差异、对初值的敏感性差异。写一份对比报告，结论要落到"什么场景该选哪个"。这道题模拟真实的方法选型调研过程。

速记卡：一句话记住每个方法¶

复习时，下面这几张"卡片"帮你快速唤起每个方法的灵魂——每张抓一个最关键的记忆锚点，串起它的表示、招式、和适用场景。

CILQR——"把约束变成 iLQR 代价里的软墙"。 记忆锚点：barrier。 iLQR 本来不吃约束，CILQR 用 log-barrier 在可行域边界筑一堵软墙，把约束溶进代价、保住 iLQR 的速度。软约束、要可行初值、自驾城市道路、~50 Hz。一句话：让快但挑食的 iLQR，学会用"软墙"避开约束。

TEB——"把轨迹画成带时间戳的橡皮筋"。 记忆锚点：段时间 + 因子图。位姿加段时间串成弹性带，约束写成 g2o 超图上的单边软项，多拓扑并行躲局部极小。软约束、ROS 标配、移动机器人、~20 Hz。一句话：一条能拉伸变形、每段还标着时长的橡皮筋，在 g2o 上被各种"力"拉到平衡。

OBCA——"用分离超平面把'别撞'变光滑"。 记忆锚点：对偶变量 λ。强对偶保证"在障碍外 ⟺ 存在分离超平面"，对偶变量 λ 把不可微的避障精确变光滑，硬约束、泊车、配 Hybrid A* warm-start、~5 Hz。一句话：与其算到障碍多近，不如问"能不能插一张平面把我和它分开"——这个问题是光滑的。

MINCO——"只报航点和段时间，系数自动到位"。 记忆锚点：\(c=M(q,T)\)。多项式系数由航点+段时间唯一确定，连续性自动满足，时空同步变形，硬约束、无人机、~100 Hz。一句话：别去优化那一堆系数——告诉我经过哪些点、每段多久，最优的那条曲线自己就定了。

CPC——"连'何时过门'都让优化自己决定"。 记忆锚点：互补约束。进度只在靠近航点时推进，时间分配成为优化变量，bang-bang，硬约束、无人机竞速、离线、击败人类飞手。一句话：我不告诉你第几秒该到哪个门，只要求按序穿过所有门、总时间最短——剩下你自己算。

把五张卡片连起来读，就是本章的两条主线：招式从"软墙"到"对偶/映射/互补"（约束越来越精确地进优化）、时间从"藏在状态里"到"连分配都优化"（时间越来越成为一等公民）。记住这五个锚点（barrier / 段时间+因子图 / 对偶 λ / \(c=M(q,T)\) / 互补），就记住了这一章。

结语：你现在站在哪里¶

本章带你走完了从"解耦"到"联合"的数学核心。 T1 给了解耦基线，T2 迈出"搜索定 homotopy + 走廊"的第一步，而本章——把时间真正变成连续优化里的决策变量——补上了时空联合的最后一块、也是最硬核的一块拼图。

你手里现在握着五种工具，它们看似各自为政（自驾的 CILQR、机器人的 TEB、泊车的 OBCA、无人机的 MINCO、竞速的 CPC），但你已经看清：它们其实是同一个问题"在动力学可行、无碰撞、状态在界内的前提下，对轨迹与时间求最优"的五种解法，只在三件事上做了不同选择——用什么表示、怎么处理约束、代价里放什么。更要紧的是那条贯穿始终的主线：约束如何进入连续优化（从软化的 barrier/hinge，到精确的对偶/映射/互补，难度与保证一路递增），以及 T2、T3 共有的"离散定 homotopy + 连续精修"分工——它解释了为什么这些方法都需要一个前端来 warm-start。

但你也该看清这套时空联合优化的**边界**：它处理了路径-速度的强耦合、处理了非线性动力学与硬约束，却仍站在两个假设之上——动态障碍的预测是给定且可信的、其他智能体不会因你的动作而改变意图。预测的不确定性，是 Part-U 要补的；强交互下的相互博弈，是 Part-G 要啃的。 T1-T3 为你建立了"假设世界可预测"下的时空规划完整方法论；带着这套方法和它的边界，你已经准备好，去直面真实世界的不确定与交互了。