T2 时空走廊与时空搜索：从 ST 图迈向 3D 时空规划¶
这一章解决什么问题：T1 用路径-速度解耦把轨迹规划拆成了两段——先在 SL 图上定路径，再在 ST 图上排速度。 可那套范式有个写死的前提：路径和速度弱耦合，且"走哪条路"基本由参考线和静态障碍定下来。 一旦路径与速度强耦合（cut-in、动态绕障、抢行），解耦就开始失真——这正是 T1 结语里反复指向的"该转 T2/T3"。 本章不再把时间藏在 ST 图的速度一维里，而是把它升格为一个**独立的搜索/优化维度**：在 \((x, y, t)\) 或 \((s, l, t)\) 的三维时空里，直接同时决定"走哪、何时到"。 两条主线——时空搜索（格搜索、SIPP）给出离散最优解，时空走廊（SSC、SFC）给出连续凸可行域喂给 QP——构成了从"解耦"迈向"联合"的第一级台阶。
前置自测¶
开始前先花五分钟答这几道题。 答得上来，说明 T1 的地基扎实，可以快进； 答不上来，对应小节要慢读。
- T1 的 ST 图横轴是时间、纵轴是纵向进度 \(s\)，它解决的是"何时到哪"。 那它**没**解决什么？ （提示：ST 图是在哪个前提下画出来的？）
- A* 搜索要求什么样的图才能保证找到最优解、且不必担心重复展开？ 如果给搜索状态加一维"时间 \(t\)"，这个图会有什么特殊的结构？
- 一个动态障碍以 \(2\,\text{m/s}\) 匀速经过某个路口格子。 如果你把时间按 \(0.1\,\text{s}\) 离散，规划 \(10\,\text{s}\)，这个格子在搜索图里会变成多少个"时空节点"？ 这个数字告诉你朴素时间离散的什么毛病？
- 凸多面体可以写成 \(A\mathbf{x} \le \mathbf{b}\)（一组半空间的交）。 如果一条轨迹的所有"控制点"都落在某个凸多面体里，整条轨迹一定也在里面吗？ 这个性质叫什么？
- T1 §T1.3 把路径 QP 的安全约束写成了"每个 \(s_i\) 处 \(l\) 落在左右边界之间"的 box。 如果障碍是动态的、边界随时间变，这套 box 约束还够用吗？
自测答案要点：(1) ST 图没解决"走哪条路"——它是在路径**已经定下**之后画的，路径决策在上游的 SL/DP。 (2) A* 在非负边权下、且配合一致启发可保证最优； 加时间维后图变成**有向无环图（DAG），因为时间只增不减，永远回不到过去。 (3) \(10/0.1 = 100\) 个时空节点——朴素时间离散让单个格子炸成上百个状态，这正是 §T2.2 SIPP 要消灭的浪费。 (4) 一定在里面，这叫**凸包性质（convex hull property），是 §T2.3 Bézier 走廊把安全约束线性化的根基。 (5) 不够——动态边界要么逐时间步重列 box（回到时间离散的爆炸），要么升级成 \((s,l,t)\) 三维走廊（本章主题）。
本章目标¶
学完本章，你应当能够：
- 说清**时空规划与解耦规划的分界**：什么场景下 ST 图的"先定路径、再排速度"会失真，必须把时间提升为独立搜索维度。
- 掌握**时空格搜索（spatiotemporal state lattice）**的框架：在"构型 × 时间"空间里用运动基元建图，理解它为什么天然是 DAG、为什么能用线性时间最短路。
- 透彻理解 **SIPP（Safe Interval Path Planning，安全区间路径规划）**的核心观察——"安全时间步无界，但安全区间有限且很少"——并能手算一个小例子的安全区间、画出 wait-then-move 的转移。
- 理解 **SSC（Spatio-temporal Semantic Corridor，时空语义走廊）**如何把安全区域表示成 \((s, l, t)\) 的 cube 序列，并借 Bézier 的凸包性质把安全约束**线性化**成 QP。
- 理解 **IRIS / SFC（Safe Flight Corridor，安全飞行走廊）**这类凸分解方法如何从占据栅格/点云自动"吹"出无碰撞凸多面体走廊。
- 讲清**走廊与搜索的互补关系**：走廊给连续凸域（喂 QP/NLP），搜索给离散最优解（喂 MAPF 组合优化），以及二者如何组合（搜索给 warm-start、走廊给精修）。
- 把这些零件接回 T1 与后续章节：知道 SIPP 是 T5 多智能体 CBS 的低层默认构件、SSC 是 Apollo/EPSILON 系统里行为层与运动层之间的桥。
本章知识导航¶
本章六节，分两条线再合流：
- 搜索线：§T2.1 时空格搜索（离散运动基元 + DAG 最短路）→ §T2.2 SIPP（把时间轴压成安全区间，搜索空间瘦身）。 这条线产出**离散最优路径**，是多智能体规划（T5）的地基。
- 走廊线：§T2.3 SSC（语义 cube 序列 + Bézier QP）→ §T2.4 IRIS/SFC（从地图自动生成凸走廊）。 这条线产出**连续凸可行域**，喂给 QP/NLP 后端，是自驾/无人机轨迹优化的主流。
- 合流：§T2.5 走廊 vs 搜索的互补与组合 → §T2.6 完整管线实战（把一个动态绕障场景从感知输入走到时空轨迹）。
难度标注：§T2.1 ⭐⭐、§T2.2 ⭐⭐⭐、§T2.3 ⭐⭐⭐、§T2.4 ⭐⭐、§T2.5 ⭐⭐、§T2.6 ⭐⭐。 两个 ⭐⭐⭐（SIPP、SSC）是本章的硬骨头，配了手算算例，建议慢读。
前置知识桥接¶
本章直接踩在 T1 的肩膀上，下面几样东西默认你已经熟了：
- Frenet 坐标 \((s, l)\)（T1 §T1.2）：本章的时空走廊就在 \((s, l, t)\) 里长出来——它是 ST 图的"再加一维"。
- ST 图与 DP-ST 搜索（T1 §T1.4）：SIPP 的安全区间和 ST 图的"阻挡多边形"是同一件事的两种表示，这个对照是 §T2.2 的关键洞察。
- piecewise-jerk QP / OSQP（T1 §T1.3、§T1.5）：走廊线的后端就是在凸走廊里解一个 QP，约束形式与 T1 一脉相承（线性不等式 + 连续性等式）。
- A* / Dijkstra 与图搜索（公共基础）：两条搜索线都是图最短路，A* 的一致启发、非负边权、闭集这些概念要随手能用。
- 凸集与半空间 \(A\mathbf{x}\le\mathbf{b}\)（公共基础 + Eigen）：走廊就是半空间的交，QP 的安全约束就是这些不等式。
如果其中某项发虚，回 T1 对应小节补一下再来，否则本章读起来会处处打滑。
如果跳过本章会怎样¶
跳过 T2 直接奔 T3（时空轨迹优化）或 T5（多机协调），你会缺两块地基：(1) 不懂走廊怎么来的，T3 那些"在走廊里解 NLP"的优化就成了无源之水——你会以为走廊是天上掉下来的凸域，而不知道它是 IRIS/SFC 一个椭球一个椭球吹出来的，也就不理解走廊形状如何影响优化的保守性； (2) 不懂 SIPP，T5 的 CBS/ECBS 低层搜索会变成黑盒——多智能体冲突消解里"给某个 agent 加时空约束再重搜"这步，底层正是 SIPP。 本章是搜索线与走廊线的共同源头，跳过它，后面两个 Part 都会有断层。
预计阅读时间¶
| 阅读方式 | 时间 | 适合谁 |
|---|---|---|
| 精读（含手算 SIPP 算例与代码） | 7–9 小时 | 要能独立实现 SIPP / 走廊 QP 的读者 |
| 速读（跳过 SSC 的 Bézier 推导细节） | 3–4 小时 | 已有规划基础、只需建立时空规划全景的读者 |
| 速查（只看符号表、对比表、API 速查） | 25 分钟 | 回头查 SIPP 转移规则 / 走廊表示时 |
本章符号约定¶
沿用 T1 的记号习惯并补充本章新符号。 横向偏移 \(l\) 仍取左正右负； 时间 \(t\) 在本章是一等公民，不再只藏在速度里。
| 符号 | 含义 | 首见 |
|---|---|---|
| \((x, y, \theta, v)\) | 笛卡尔位姿与速度（车辆构型） | §T2.1 |
| \((s, l)\) | Frenet 纵向弧长 + 横向偏移（沿用 T1） | §T2.1 |
| \(t\) | 时间，本章作为独立搜索 / 优化维度 | §T2.1 |
| \(\Delta t\) | 时间离散步长 / 运动基元时长 | §T2.1 |
| \(\mathcal{P}\) | 运动基元（motion primitive）集合 | §T2.1 |
| \(\text{cfg}\) | 构型（configuration），如 \((x,y,\theta,v)\) 或网格格子 | §T2.2 |
| \(I,\ I_k\) | 安全区间（safe interval），\((\text{cfg}, I)\) 构成 SIPP 状态 | §T2.2 |
| \([t_1, t_2]\) | 某构型被动态障碍占据的**不安全**时间区间 | §T2.2 |
| \(\text{arrival}(n)\) | 到达节点 \(n\) 的最早时间 | §T2.2 |
| \(C_k\) | 时空走廊中的第 \(k\) 个凸多面体 / cube | §T2.3 |
| \(A\mathbf{x}\le\mathbf{b}\) | 凸多面体的半空间表示 | §T2.3 |
| \(\mathbf{c}_i\) | Bézier 曲线的第 \(i\) 个控制点（control point） | §T2.3 |
| \(\mathbf{c}_i^{(1)},\ \mathbf{c}_i^{(2)}\) | Bézier 一阶 / 二阶导函数的控制点（原控制点的线性差分） | §T2.3 |
| \(\mathcal{E}\) | IRIS 迭代中的内切椭球（ellipsoid） | §T2.4 |
| \(C,\ \mathbf{d}\) | 椭球的形状矩阵与中心（\(\mathcal{E}=\{C\mathbf{u}+\mathbf{d}:\lVert\mathbf{u}\rVert\le1\}\)） | §T2.4 |
| \(\mathcal{F}\) | 无碰撞自由空间（free space） | §T2.4 |
| \(k\) | 某构型的安全区间数（≈ 障碍经过次数 +1） | §T2.2 |
| \(\xi\) | 走廊 QP 安全约束的松弛变量（软约束兜底用，\(\xi\ge0\)） | §T2.3 |
怎么读这章：搜索线（§T2.1–§T2.2）和走廊线（§T2.3–§T2.4）可以独立读，但两条线的"为什么需要时间维"是共通的，§T2.1 的动机一节务必先读。 §T2.2（SIPP）和 §T2.3（SSC）各配了手算算例，是检验"是否真懂"的试金石——别跳。 每节末尾的陷阱仍分编程 / 概念 / 思维三类，思维类往往最值钱。
§T2.1 时空格搜索（Spatiotemporal State Lattice）⭐⭐¶
这一节解决什么问题：T1 的 ST 图是"先定路径、再在路径上排速度"。 但当"走哪条路"本身就取决于"动态障碍何时到哪"时，这个先后顺序就拆错了。 时空格搜索给出第一种答案：把时间 \(t\) 直接塞进搜索状态，在"构型 × 时间"的图里一次性搜出"既定路径又定时序"的轨迹。
动机：当"走哪"与"何时"纠缠在一起¶
设想一个最朴素的反例：你要并入一条有车流的主路，前方一辆车正以某速度行驶。 要不要加速抢到它前面、还是减速跟到它后面，这个**横向决策（往哪个 gap 钻）**和**纵向决策（加速还是减速）**是死死绑在一起的——抢前面就得加速、且路径要早早往左切； 跟后面就得减速、路径可以晚点切。 T1 的解耦管线会先在 SL 图上定一条路径（此时还不知道该抢该跟），再在 ST 图上排速度，等于把一个本应联合的决策硬掰成了两步，很容易定出一条"路径假设抢、速度却跟不上"的拧巴解。
时空格搜索的思路直接得多：别分两步，把状态扩展成包含时间的 \((x, y, \theta, v, t)\)，在这个图里搜一条从"此刻此地"到"目标"的最优路径。 搜索过程同时回答了"经过哪些 \((x,y)\)"（路径）和"在什么 \(t\) 经过"（时序），抢与跟自然成了搜索图里两条不同的路径，由代价高低分出胜负。
如果不这样做会怎样（反事实）¶
退一步，假如我们只用**纯空间 lattice**——状态是 \((x, y, \theta)\)，不含时间，会怎样？ 纯空间格搜索在静态地图上工作得很好（它正是 hybrid A* 之类的底子），但它有一个致命盲区：它不知道"何时"经过某处。 于是一个正在移动的障碍，它要么当成"永远占据"（过度保守，明明等一秒就能过，却绕了一大圈甚至判定无解），要么当成"不存在"（直接撞上）。 纯空间格在动态场景里无论怎么调都不对，因为它的状态空间里压根没有表达"时机"的维度。 加上 \(t\) 之后，"等一秒再过"和"现在抢过"成了图里两个不同的状态转移，避动态障碍才第一次变得可表达。
历史：从 Ziegler-Stiller 到 CMU 的 GPU 时空格¶
时空格搜索的奠基之作是 Ziegler 与 Stiller 在 IROS 2009 的《Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios》（pp. 1879–1884，DOI 10.1109/IROS.2009.5354448）。 他们的关键构造是：把构型空间与时间组合成一个流形（manifold），在上面确定性地采样建图。 这张图有个漂亮的性质——因为时间只增不减，图是无环的（acyclic），即有向无环图 DAG，于是可以用线性时间的最短路算法求解，而不必担心一般图里的环和重复展开。 他们生成的轨迹是**五次样条（quintic spline）、二阶连续、满足非完整约束、按最小 jerk 平方优化； 动态障碍的处理方式极简洁——**沿障碍未来路径把对应的时空节点"禁用"掉，于是计算量与场景里障碍多少基本无关。 这套方法跑在 AnnieWAY 实验车上，规划时间稳定在 20 ms 以内。
两年后，CMU 的 McNaughton、Urmson、Dolan、Lee 在 ICRA 2011 的《Motion Planning for Autonomous Driving with a Conformal Spatiotemporal Lattice》把这个思路推到了 GPU。 他们用 \((s, l, v, t)\) 作为状态（注意是贴着参考线的 Frenet-like 坐标，"conformal"即"贴合道路形状"），通过对状态空间的结构化划分避免维度爆炸，并用 GPU 并行评估约 \(10^4\) 条候选轨迹。 这条工作把"时空格能不能实时"从单核的勉强推到了多核的从容，也确立了"贴参考线采样 + 并行评估"这个一直延续到今天的工程范式。
理论：状态、运动基元、DAG 与搜索¶
状态与运动基元。 时空格的状态是离散化的构型加时间。 最常见的两种取法：道路场景用 \((s, l, v, t)\)（贴参考线，维度可控），开阔/越野场景用 \((x, y, \theta, v, t)\)。 状态之间不是随便连边，而是由一组预先设计好的**运动基元（motion primitive）** \(\mathcal{P}\) 连接——每个基元是一段满足车辆运动学（非完整约束、曲率/加速度上限）的、固定时长 \(\Delta t\) 的轨迹片段。 比如"保持车道匀速 \(\Delta t\)"、"向左偏移半个车道并加速"等。 从一个状态出发，应用每个适用的基元，就得到若干后继状态； 基元本身保证了边对应的轨迹是动力学可行的，这是格搜索相对自由图搜索的最大好处——搜出来的路径天生可执行，不像普通 A* 还要事后平滑。
基元库到底怎么设计？ 这是时空格落地最具体的工程问题，值得说几句，否则"运动基元"始终是个抽象名词。 设计基元库的核心矛盾是**覆盖与数量的权衡**：基元太少，可达的运动模式不够、搜不出好路径（比如缺了"急打方向"的基元，遇到突发障碍就避不开）； 基元太多，每个状态的后继爆炸、搜索变慢。 常见的设计套路有三种。 其一是**控制空间采样**：在车辆的控制量（转向角、加速度）上均匀取若干离散值，每个组合积分 \(\Delta t\) 得到一条基元轨迹——简单直接，但生成的末状态不规整，难以落到格点上（接不上下一个基元）。 其二是**状态空间采样（state lattice）：反过来，先在状态空间定好规整的格点（哪些末状态是"合法落点"），再为每条"从当前点到某个格点"的连接**求解一个边值问题（boundary value problem），反推出满足动力学的基元轨迹——这样基元末状态天然落在格点上、能无缝拼接，是 Pivtoraiko 与 Kelly 提出的经典做法，也是多数工业级 lattice planner 的选择。 其三是**学习/优化生成**：用最优控制或学习方法离线生成一组"既覆盖典型机动、又数量精简"的基元集，近年也有工作这么做。 对道路场景还有个省事的简化：贴着参考线、在 Frenet 系里设计基元（如"保持/左移/右移车道 × 减速/匀速/加速"的组合），维度低、语义清晰——这正是 CMU conformal lattice 的思路。 一句话：基元库不是随便列几条，而是要**保证落点规整（能拼接）+ 覆盖必要机动（搜得出解）+ 数量可控（搜得够快）**，这三者的平衡决定了时空格好不好用。
为什么是 DAG。 这是时空格最值得记住的结构性事实。 每条边都让时间前进 \(\Delta t\)（或其整数倍），时间单调递增，因此**永远不可能回到一个时间更早的状态**——图里不存在环。 无环意味着可以做拓扑排序，按时间层逐层推进，最短路用一次扫描（线性时间 \(O(|E|)\)）就能算完； 即便用 A*，也不必担心节点被重复展开后又因更短路径被重新打开的麻烦。 把这个性质和 T1 §T1.4 的 ST 图 DP 对照：那里 DP 之所以能逐时间层递推，本质也是时间单调带来的 DAG 结构——时空格只是把这个结构从一维 \(s\) 扩展到了二维构型。
代价设计。 边代价通常是几段的加权和：平滑度（jerk 平方积分，承接 Ziegler-Stiller 的最小 jerk）、偏离参考线/期望速度的惩罚、以及交通规则代价（压线、超速）。 写成式子，一条边 \(e\)（对应一段运动基元轨迹）的代价是：
其中 \(w_\bullet\) 是各项权重，\(j\) 是 jerk、\(l\) 是横向偏移、\(v_{\text{des}}\) 是期望速度、\(c_{\text{rule}}\) 打包压线/超速等规则惩罚。 搜索最小化的是整条路径上所有边代价之和——这与 T1 §T1.3/§T1.5 的 QP 代价是同一套设计哲学（平滑 + 贴参考 + 守规则），只不过这里离散地摊在每条边上、由搜索而非 QP 来求和最小化。 碰撞不是用代价软惩罚，而是用硬禁用——扫到动态障碍占用的时空节点直接剔除（边代价记 \(+\infty\)）。
和 hybrid A* 是什么关系。 这是初学时空格最容易犯迷糊的地方。 hybrid A*（混合 A*）是自驾泊车/越野里常用的搜索，状态是连续的 \((x, y, \theta)\)、用车辆运动学扩展后继——听起来和时空格的"运动基元扩展"很像，区别到底在哪？ 关键就一条：hybrid A* 的状态里没有时间。 它在 \((x,y,\theta)\) 上搜，解决的是"在静态环境里找一条运动学可行的几何路径"，至于何时到达路径上的某点，它不关心、也无法表达。 时空格则把时间 \(t\)（以及常常还有速度 \(v\)）放进状态，于是它能回答 hybrid A* 回答不了的问题——"何时经过这里"。 可以这样记：hybrid A* 是时空格的"抽掉时间维"的特例； 反过来，时空格是 hybrid A* "长出时间轴"后的推广。 在纯静态泊车场景，二者几乎等价（没有动障，时间维无用）； 一旦有动态障碍，hybrid A* 就回到了本节开头批判的"纯空间格"困境，必须升级成时空格。 这也解释了为什么很多自驾栈里泊车用 hybrid A*、动态道路场景却换时空格或时空走廊——不是两套互不相干的算法，而是同一搜索思想在"要不要管时间"上的两档配置。
动态障碍如何进入。 这是时空格优雅的地方：给定障碍的预测轨迹（一串 \((x, y, t)\) 或 \((s, t)\)），把它在每个时刻占据的格子标记为"禁用"，搜索时这些时空节点不可达。 注意"禁用"是带时间戳的——同一个 \((x,y)\) 格子在 \(t{=}2\) 被禁、在 \(t{=}5\) 又开放，因为那时障碍已经开过去了。 这正是纯空间格做不到的"时机感"。
一个最小时空格算例（把 DAG 走一遍）¶
抽象说一万遍不如把一个最小例子摊开。 我们在 \((s, t)\) 平面上建一个极简时空格（先忽略横向 \(l\)，只看纵向进度——这其实就退化成了一个加了运动基元的 ST 图，便于和 T1 对照）。
设定：\(\Delta t = 1\,\text{s}\)，规划三步到 \(t = 3\)。 每步有三个运动基元——减速（这一秒前进 \(4\,\text{m}\)）、匀速（前进 \(6\,\text{m}\)）、加速（前进 \(8\,\text{m}\)）。 自车从 \(s_0 = 0\) 出发。 一个动态障碍在 \(t = 2\) 时占据 \(s \in [10, 14]\) 这段（比如一辆车切到你前面的瞬间）。
逐层展开这张 DAG：
t=0: s=0 (起点)
│ 减速→4 匀速→6 加速→8
▼
t=1: s∈{4, 6, 8}
│ 每个节点再应用三基元（+4/+6/+8）
▼
t=2: s∈{8,10,12, 10,12,14, 12,14,16} 去重后 {8,10,12,14,16}
│ 障碍禁用 s∈[10,14] → 删掉 t=2 的 {10,12,14}
│ 存活：t=2 时只剩 s∈{8, 16}
▼
t=3: 从 s=8 可达 {12,14,16}；从 s=16 可达 {20,22,24}
看出门道了吗：障碍在 \(t{=}2\) 卡掉了中间一截 \(s\)，于是这张 DAG 在 \(t{=}2\) 被劈成两支——要么**减速到 \(s{=}8\) 以下**（跟在障碍后面），要么**加速冲到 \(s{=}16\) 以上**（抢在障碍前面）。 这两支对应的正是动机里说的"抢"与"跟"两个 homotopy 类，而它们是搜索**自己**从禁用节点中长出来的，不是谁事先指定的。 最后比较两支到 \(t{=}3\) 的累积 jerk 代价（加速冲过去 jerk 大、减速跟着 jerk 小），取小者即最优时空轨迹。
把代价摊开算一遍。 别停在"jerk 大/小"的定性，用数字坐实。 简化起见，把每条边的代价近似为"这一步速度变化量的平方"（速度变化越剧烈、jerk 越大，正比于加速度变化的平方）——减速边（前进 \(4\)，相当于 \(v{=}4\)）、匀速边（\(v{=}6\)）、加速边（\(v{=}8\)），自车初速对应 \(v_0{=}8\)（即起步那刻接近"匀速到加速"之间，取 \(v_0\) 对应进度 \(6\)/秒的惯性）。 - 跟（减速支）：\(s{:}\,0\to 4\to 8\to 12\)。 三步速度序列 \(4, 4, 4\)（一路减速到低速跟车）。 相对初速的变化：第一步 \(|4-6|{=}2\)、第二步 \(|4-4|{=}0\)、第三步 \(0\)。 代价 \(\approx 2^2 + 0 + 0 = 4\)。 轨迹平顺（一次减速后匀速跟住）。 - 抢（加速支）：\(s{:}\,0\to 8\to 16\to 24\)。 三步速度序列 \(8, 8, 8\)（一路加速冲）。 变化：第一步 \(|8-6|{=}2\)、之后 \(0, 0\)。 代价 \(\approx 2^2 = 4\)。 - 两支首步代价相当，但**真实代价还要叠加"是否舒适/是否超速/是否压对向"等规则项**：抢的那支末态 \(s{=}24\)、速度持续高位，若此处有限速或前方视距不足，规则代价 \(c_{\text{rule}}\) 会显著加到加速支上； 跟的那支速度低、更保守，规则代价小。 于是综合代价通常"跟"略优——除非场景明确鼓励抢（如后车紧逼、或前方畅通）。
这个小演算的意义不在精确数字（代价模型是简化的），而在让你看清：搜索是怎么用一个标量代价，把"抢还是跟"这个本来很主观的决策，变成两支路径代价的客观比较。 换个权重（调大 \(w_{\text{rule}}\) 或 \(w_v\)），天平就倾向另一支——这正是 §T2.1 代价设计那条公式在算例里的兑现。
把这个例子和 T1 §T1.4 的 ST-DP 算例并排看：两者都是在时间分层的 DAG 上做最短路，禁用节点都来自障碍的时空占用。 区别只在——ST-DP 假设路径已定（只搜 \(s(t)\)），而完整时空格还会同时搜横向 \(l\)，于是"抢/跟"和"往哪个 gap 切"被一起决定。
插一句：到底什么是"homotopy 类"¶
刚才反复说"抢/跟两个 homotopy 类"，这个词从这里起会贯穿全章（也贯穿 T3、Part-G），值得就地讲透，否则后面会一直似懂非懂。 同伦类（homotopy class）的直觉：两条轨迹属于同一个同伦类，当且仅当其中一条能在不穿过任何障碍的前提下、连续地形变成另一条。 反过来，如果你想把一条轨迹变成另一条，中途**必然要跨过某个障碍**，那它们就属于**不同的同伦类**。 拿绕障碍说最清楚：从障碍**左边**绕过去的所有轨迹，彼此可以连续微调、互相变形（同一类）； 从**右边**绕过去的所有轨迹也是一类； 但"左绕"想变成"右绕"，中途一定要穿过障碍本身——这两类之间隔着障碍，连续形变跨不过去。 回到算例：障碍在 \(t{=}2\) 卡住了中间一截 \(s\)，"跟在后面"（绕过时空障碍的下方）和"抢在前面"（绕过上方）就是两个不同的同伦类，中间那截被禁用的节点正是隔开它们的"墙"。
为什么这个概念对规划这么关键？ 因为它精确地刻画了"连续优化的能与不能"。 连续优化（QP/NLP，走廊线的后端）本质是在做连续微调——它能在**一个同伦类内部**把轨迹优化到最好，却**永远跨不到另一个同伦类**，因为跨越需要穿过障碍，而优化器被约束着不许碰障碍。 这就解释了全章那个反复出现的分工："选哪个同伦类"是离散的、组合的决策，只能靠搜索（或枚举）来定；"在选定的同伦类里求最优"才是连续优化的活。 §T2.5 那句"走廊优化跳不出走廊"、§T2.6 那个"搜索定 homotopy、走廊精修"，本质都是这一条——搜索负责跨同伦类的离散选择，走廊+QP 负责类内的连续精修。 也正因如此，同伦类的数量往往不多（绕一个障碍 2 类、绕两个障碍至多 4 类……），离散搜索只需在这少数几类里挑，不必担心组合爆炸——这是两段式范式高效的根本原因之一。 记住这个词：后面 T3 的拓扑引导轨迹优化、Part-G 的多策略博弈，骨子里都在和"同伦类"打交道。
代码实现（为什么 → 正确 → 错误 → 对比）¶
Step 1 — 为什么这样写。 时空格搜索骨架的核心是三件事：用运动基元生成后继（保证动力学可行）、用带时间戳的占用检查禁用碰撞节点（处理动态障碍）、在 DAG 上跑 A*（利用无环结构）。 下面的骨架把这三件事拆开，便于替换。
// 时空格搜索骨架(教学示意,非完整实现)
// 状态:时空构型 (s, l, v, t);搜索:DAG 上的 A*
struct State { double s, l, v, t; }; // 时空状态
struct Node { // 优先队列节点
double f, g; // f = g + h,g = 已付代价
State state;
std::shared_ptr<Node> parent; // 回溯路径用
};
// 小根堆比较器:f 小者优先
struct NodeCmp {
bool operator()(const std::shared_ptr<Node>& a,
const std::shared_ptr<Node>& b) const { return a->f > b->f; }
};
std::vector<State> SpatiotemporalLatticeSearch(
const State& start, const State& goal,
const std::vector<MotionPrimitive>& primitives,
const ObstacleOccupancy& occ, // occ.IsOccupied(s,l,t):带时间戳!
double dt) {
std::priority_queue<std::shared_ptr<Node>,
std::vector<std::shared_ptr<Node>>, NodeCmp> open;
std::unordered_set<StateKey> visited; // DAG 无环,visited 仅为去重
auto s0 = std::make_shared<Node>();
s0->g = 0.0; s0->f = Heuristic(start, goal); s0->state = start;
open.push(s0);
while (!open.empty()) {
auto cur = open.top(); open.pop();
if (IsGoal(cur->state, goal)) return Reconstruct(cur); // 找到:回溯完整路径
StateKey key = Quantize(cur->state);
if (visited.count(key)) continue;
visited.insert(key);
for (const auto& prim : primitives) {
auto [succ, seg] = ApplyPrimitive(cur->state, prim, dt); // 末状态 + 轨迹片段
// 关键:沿整段基元做带时间戳的碰撞检查,而非只查末点
if (Collides(seg, occ)) continue; // 扫到动障占用 → 禁用这条边
auto nxt = std::make_shared<Node>();
nxt->g = cur->g + EdgeCost(seg); // jerk + 偏离 + 交规
nxt->f = nxt->g + Heuristic(succ, goal);
nxt->state = succ; nxt->parent = cur;
open.push(nxt);
}
}
return {}; // 搜索失败:无可行时空路径
}
Step 2 — 正确要点。 collides 必须沿**整段基元**采样检查、而不是只查末状态——因为一段 \(\Delta t\) 的轨迹中途可能扫过障碍，只查端点会漏碰撞（这是时空格最常见的隐 bug，下文陷阱细说）。
obstacle_occupancy 必须吃 \(t\) 参数：同一空间点在不同时刻占用状态不同。
heuristic 取"忽略动障、按最大速度直奔目标的剩余时间/距离"这类**可采纳（admissible）**下界，保证 A* 最优。
Step 3 — 一个常见的错误写法。 下面这种"先搜空间路径、再套时间"的写法看似省事，实则错得很典型：
// 反例:先用纯空间 A* 搜出路径,再事后给每个点配时间
std::vector<Point2D> path_xy =
AstarSpatial(start, goal, static_obstacles); // 错:搜索时没看动障
Trajectory traj =
AssignTimestamps(path_xy, speed_profile); // 错:时间是事后硬贴的
// 问题:空间路径已经定死,若它恰好穿过某动障的时空占用,
// 事后无论怎么调时间都可能避不开 → 退化成 T1 解耦的拧巴解
它把时空格退化回了"纯空间 + 事后排时间"，等于绕一圈又回到了 T1 解耦的老问题——动机里批判的那种"路径假设抢、速度却跟不上"的解，正是这么来的。
Step 4 — 对比。 正确版在**搜索时**就让时间参与决策，"抢/跟"由代价分胜负； 错误版把时间踢出搜索、事后硬贴，丢掉了时空格存在的全部理由。 判断你的实现对不对，只需问一句：障碍的时间戳有没有进入边的合法性判断？ 进了就对，没进就是假时空格。
一份能跑的完整时空格（无外部依赖）¶
和 SIPP 一样，给一份**可直接编译运行**的最小时空格——把 §T2.1 那个一维 \((s,t)\) 算例（三基元、\(t{=}2\) 禁用 \(s\in[10,14]\)）变成能跑的 C++，看 A* 怎么在 DAG 上自动挑出最优的那一支。
只用标准库，g++ -std=c++17 即可编译。
// 一维时空格搜索的完整可运行最小实现(教学版)
// 状态 (t, s);运动基元=三种"这一秒前进多少米";A* 在时间分层 DAG 上搜最优
#include <vector>
#include <queue>
#include <cstdio>
const std::vector<double> PRIMS = {4.0, 6.0, 8.0}; // 减速/匀速/加速:这一秒前进米数
const double V_REF = 6.0; // 参考速度(算代价)
const int T_MAX = 3; // 规划到 t=3
// 障碍:t=2 时禁用 s∈[10,14](一辆车切到前面那一瞬)
bool Blocked(int t, double s) {
return t == 2 && s >= 10.0 - 1e-9 && s <= 14.0 + 1e-9;
}
struct PQItem {
double f, g; // f = g + h(此处 h=0,退化为 Dijkstra);g = 累计代价
int t; double s; // 时空状态
double last_v; // 上一步速度(算 jerk 代价用)
std::vector<double> hist; // 走过的基元序列(回溯轨迹)
};
struct Cmp { bool operator()(const PQItem& a, const PQItem& b) const { return a.f > b.f; } };
int main() {
std::priority_queue<PQItem, std::vector<PQItem>, Cmp> open;
open.push({0.0, 0.0, 0, 0.0, V_REF, {}});
while (!open.empty()) {
PQItem cur = open.top(); open.pop();
if (cur.t == T_MAX) { // DAG 上时间单调,先弹出的即最优
double s = 0; std::printf("最优时空轨迹 s: 0");
for (double dv : cur.hist) { s += dv; std::printf("->%.0f", s); }
std::printf(" 累计代价=%.1f\n", cur.g);
return 0;
}
for (double dv : PRIMS) {
double ns = cur.s + dv; int nt = cur.t + 1;
if (Blocked(nt, ns)) continue; // 扫到障碍占用 → 禁用这条边
double cost = (dv - cur.last_v) * (dv - cur.last_v); // 速度突变平方 ≈ jerk 代价
auto nh = cur.hist; nh.push_back(dv);
open.push({cur.g + cost, cur.g + cost, nt, ns, dv, nh});
}
}
std::printf("无可行时空路径\n"); // open 耗尽仍没到 T_MAX
return 0;
}
// 运行输出: 最优时空轨迹 s: 0->4->8->12 累计代价=4.0
// ——A* 在"抢"(0->8->16->24)与"跟"(0->4->8->12)两支里挑了代价更小的"跟"
这份代码与正文讲解的三处对应：(a) if (cur.t == T_MAX) 处直接返回——靠的正是 DAG 时间单调性，先弹出到达终层的节点必是最优，不必像一般图那样担心后续出现更短路（§T2.1 "为什么是 DAG"那段的代码兑现）；
(b) if (Blocked(nt, ns)) continue; 就是"带时间戳禁用障碍占用节点"——注意 Blocked 吃 t 参数，同一 \(s\) 在 \(t{\ne}2\) 时并不禁用；
(c) 障碍在 \(t{=}2\) 卡掉 \(s\in[10,14]\)，于是搜索树在那层自然裂成"抢/跟"两支，A* 按累计代价选优——把禁用区间改成 s∈[6,18]（几乎堵死），再跑会发现"抢"那支被全部剪掉、只剩"跟"，正是 §T2.1 算例里讨论的情形。
⚠️ 常见陷阱¶
💡 编程陷阱：碰撞检查只查基元末点，漏掉中途碰撞
- 错误描述：为图省事，collides 只检查运动基元的末状态是否被占用，不检查 \(\Delta t\) 这段中间过程。
- 现象与后果：规划出的轨迹在仿真里"穿模"——明明端点都没碰，车却在两个时间层之间一头扎过障碍。
\(\Delta t\) 越大、车速越快，漏检越严重。
- 根本原因：一段 \(\Delta t\) 的轨迹是连续的，障碍也在连续移动，碰撞可能发生在区间内部而非端点。
只查端点等于假设"区间内无事发生"。
- 正确做法：沿基元轨迹按足够细的步长（或用连续碰撞检测 CCD）采样检查；
\(\Delta t\) 偏大时尤其要加密。
这与 T1 §T1.4 ST 图里"边要扫过阻挡多边形才算碰"是同一回事。
⚠️ 概念陷阱：把时空格当成"维度更高的纯空间格" - 错误描述：以为加一维 \(t\) 只是状态空间变大，算法逻辑照搬纯空间格即可。 - 现象与后果：忘了利用 DAG 结构（仍按一般图防环），或把同一 \((x,y)\) 在不同 \(t\) 的占用混为一谈（用静态占据栅格查动障），导致要么白白多算、要么避障失效。 - 根本原因：时间维不是普通的一维——它**单调**（带来 DAG）且**改变占用语义**（占用是时间的函数）。 这两点都是纯空间格没有的。 - 正确做法：显式利用无环性（按时间层递推或放心用 A* 不怕重开），并坚持用带时间戳的占用查询。
🧠 思维陷阱：以为分辨率越细越好 - 错误描述：默认把 \(\Delta t\)、\(\Delta s\)、基元数量取得越细密，解就越优、越安全。 - 现象与后果：状态数随各维分辨率连乘暴涨，实时性崩溃； 而过粗又会错过窄缝、漏碰撞。 陷入"调分辨率"的泥潭却不知边界在哪。 - 根本原因：格搜索的解质量与计算量被离散分辨率同时绑定，这是离散方法的固有张力——不是参数没调好，而是离散表示本身的代价。 - 正确做法：把分辨率当成"够用即可"的工程取舍（道路场景用 conformal 贴参考线压低维度，CMU 用 GPU 并行扛候选数）； 要真正摆脱分辨率枷锁，得转向连续优化——也就是本章的走廊线（§T2.3 起）。
练习¶
- [实现] 把上面的最小时空格算例（\((s,t)\)、三基元、\(t{=}2\) 禁用 \(s\in[10,14]\)）写成代码，跑 A* 找最优轨迹，并打印"抢"与"跟"两支各自的累积 jerk 代价。 再把障碍占用区间改成 \([6, 18]\)，观察是否还存在"抢"这一支。
- [推导] 证明：若每条边让时间严格前进至少 \(\Delta t > 0\)，则时空格一定无环。 再说明这个无环性如何保证 A* 中一个状态被弹出闭集后不会再被更短路径重新打开。
- [对比] 纯空间 hybrid A*（状态 \((x,y,\theta)\)）和时空格（加 \(v, t\)）在"绕一个静止障碍"时表现几乎一样、在"避一辆横穿的车"时却天差地别。 请各画一张示意图说明差异的来源。
§T2.2 SIPP（Safe Interval Path Planning）⭐⭐⭐¶
这一节解决什么问题：§T2.1 的时空格把时间按 \(\Delta t\) 离散，一个 \(10\,\text{s}\) 的规划就让每个格子炸成上百个时空节点——大量节点其实"长得一样"（都安全、或都被占）。 SIPP 用一个绝妙的观察把这堆冗余压扁：一个格子在时间轴上无非是"安全/不安全"交替的几段，何必逐步存？ 只存那几段**安全区间**就够了。 这一招把动态避障从"时间离散的图爆炸"变回了"几乎和静态一样小的图搜索"，也成了多智能体规划（T5）的标配低层。
动机：时间离散的浪费¶
接着 §T2.1 的痛点想。 一个动态障碍以 \(2\,\text{m/s}\) 经过某格子，你规划 \(10\,\text{s}\)、时间步 \(0.1\,\text{s}\)，这个格子就在搜索图里变成 \(100\) 个时空节点 \((p, t{=}0.0), (p, t{=}0.1), \dots, (p, t{=}9.9)\)。 可这 \(100\) 个节点里，绝大多数是冗余的——障碍只在某个短窗口（比如 \(t\in[3.0, 4.0]\)）占据这里，其余时间这个格子一直安全。 你存了 \(100\) 个状态，真正"不一样"的信息只有"\(3.0\) 之前安全 / \(3.0\)–\(4.0\) 被占 / \(4.0\) 之后又安全"这三段。 时间离散把连续的、分段恒定的占用信息，硬拆成了上百个雷同的离散点——这是纯粹的浪费，且分辨率越细浪费越狠。
如果不这样做会怎样（反事实）¶
不用 SIPP，最直接的替代是 time-expanded graph（时间展开图）：把空间图复制 \(T/\Delta t\) 份，每份对应一个时间层，层间连边表示"花一步时间移动"。 这就是 §T2.1 时空格在纯网格上的样子。 它能用，但状态数 \(= N_{\text{cell}} \times (T/\Delta t)\)（\(N_{\text{cell}}\) 为格子数），时间分辨率一细就爆； 更尴尬的是，要支持"原地等待"（动态避障常常就靠等一下让障碍先过），你得加"等待边"，等待时长又得离散，于是层数进一步膨胀。 朴素时间展开图的全部痛苦，都来自"把本可以用几段区间描述的时间轴，按固定步长切成了海量雷同层"。
历史：Phillips 与 Likhachev 的"安全区间"¶
SIPP 由 Phillips 与 Likhachev 在 ICRA 2011 提出（《SIPP: Safe interval path planning for dynamic environments》，pp. 5628–5635，DOI 10.1109/ICRA.2011.5980306）。 论文的核心观察一句话就能说清，却极其有力：一个构型在时间轴上的"安全时间步"数量可能无界（你能在那等任意久），但"安全时间区间"的数量是有限的、而且通常非常少。 于是不必给每个时间步建一个状态，只需给每个"(构型, 安全区间)"对建一个状态——这样一来，每个构型一般只对应寥寥几个状态，搜索图的规模几乎回到了静态问题的量级。 论文还给出了配套的、时间感知的 A* 变体，并证明在若干假设下（最关键的是**智能体可以原地等待、且能瞬时启停**）该算法既完备又最优（相对给定的空间与时间离散）。 SIPP 后来成了动态环境搜索的事实标准之一，也是多智能体路径规划（MAPF）里 CBS/ECBS/EECBS 的默认低层搜索器。
理论：安全区间、状态、转移与最优性¶
安全区间是什么。 对某个构型（比如网格格子 \(p\)），把所有动态障碍占据它的时间段并起来，得到一组**不安全区间**，比如 \([t_1, t_2]\)； 它在时间轴 \([0, \infty)\) 上的补集，就是一组**安全区间（safe interval）**：
每个安全区间是一段"在此期间待在 \(p\) 都不会被撞"的连续时间。 注意一个格子可能有多个安全区间（障碍来了又走、走了又来）。
到底有多少个安全区间？ 这是 SIPP 能省状态的定量根据，值得算清楚。 某个格子的安全区间，被它的不安全区间分隔——每来一拨障碍占用，就在时间轴上挖掉一段，把原本连续的安全时间劈成更多段。 所以这个格子的**安全区间数量，至多等于"障碍经过该格的次数 + 1"。 而一个格子被障碍经过的次数，由场景里动态障碍的数量和它们的轨迹决定，是个小常数（一条路上同时经过同一格的车能有几辆？） ，与你把时间离散得多细**完全无关。 对比时间展开图：那里每个格子的状态数是 \(T/\Delta t\)（规划时长除以时间步），\(\Delta t\) 取 \(0.1\,\text{s}\)、规划 \(10\,\text{s}\) 就是 \(100\) 个，取 \(0.01\,\text{s}\) 就暴涨到 \(1000\) 个。 SIPP 把这个"随分辨率线性暴涨"的量，换成了"只随障碍经过次数增长"的小常数——这就是论文标题里"safe interval"四个字的全部分量：它让搜索图的规模从被时间分辨率绑架，变回了几乎只取决于场景复杂度。 分辨率再细，SIPP 的状态数不变； 障碍再多，也只是线性增加几个区间。
SIPP 的状态。 不是 \((p, t)\)（那是时间展开图），而是 \((p,\ I)\)——构型加上它的某个安全区间。 直觉上，\((p, I)\) 表示"我在 \(p\)，且处在它的第 \(I\) 段安全窗口里"，至于具体在窗口的哪一刻，搜索时由"最早到达时间"动态记录。 这就是 SIPP 把上百个 \((p, t)\) 压成寥寥几个 \((p, I)\) 的魔法所在。
转移（wait-then-move）。 从状态 \((p, I)\) 到相邻构型 \(p'\) 的某个安全区间 \(I'\)，合法转移要满足：存在一个出发时刻，使得（可能先在 \(p\) 等一会，但不能等过 \(I\) 的右端）移动到 \(p'\) 的到达时刻落在 \(I'\) 内、且移动过程不撞障碍。 换句话说，每条 SIPP 边隐含一个"先等、再走"的复合动作——这正是动态避障的精髓：等障碍先过去，再走。 算法为每个状态维护**到达该状态的最早时间** \(\text{arrival}\)，转移时取"能让到达 \(I'\) 成立的最早出发时刻"，从而保持时间最优。
搜索与最优性。 用 A*（或 Dijkstra）在 \((p, I)\) 图上搜，启发函数用忽略动障的静态距离下界。 论文证明：在智能体可原地等待、且瞬时启停的假设下，SIPP 完备且最优。 这两个假设非常关键，也是后文要反复敲打的局限——真车不能瞬时启停（要减速），真车原地等待也有代价，所以工程上 SIPP 常用作粗搜/低层，再交给连续优化（走廊 QP）去补动力学。
SIPP 的 A* 和普通 A* 有个微妙差别，别踩坑。 普通 A* 里，一个状态被弹出闭集（closed）后就盖棺定论、不再处理。 SIPP 里要小心：同一个 \((p, I)\) 状态，可能以**不同的到达时间**被生成多次——你可能先以 \(t{=}5\) 到达 \((p_2, I_b)\)，后来又找到一条以 \(t{=}4.5\) 到达同一 \((p_2, I_b)\) 的路。 由于 SIPP 的代价（到达时间）越小越好，后来的更早到达是更优的，不能因为 \((p_2, I_b)\) 已在闭集就直接丢弃。 正确做法是：用 \(g\) 值（到达时间）做标准的 A* 松弛——只有当新路径给出的到达时间**严格早于**已记录的最早到达时间时，才更新该状态并重新入队； 否则剪枝。 这与普通 A* 在"发现更短路径时更新"是一个道理，只是这里的"短"指"到得更早"。 漏掉这个更新，SIPP 会得到次优解（明明能更早到，却用了先找到的较晚那条）； 而把"已在闭集就跳过"照搬过来，正是新手最容易犯的错。 记住：SIPP 状态的身份是 \((p, I)\)，但它的代价是连续的到达时间，二者要分开对待。
一个 SIPP 最小算例（手算安全区间与转移）¶
把上面的定义落到一个能手算的小例子上。 考虑一条一维走廊上的格子 \(p_0, p_1, p_2, p_3, p_4\)（想象一条窄道，智能体每步走一格、每步耗时 \(1\,\text{s}\)，也可以选择在原地停 \(1\,\text{s}\)）。 一个动态障碍**占据 \(p_2\) 这一格，时间窗为 \(t\in[2, 4]\)**（它 \(t{=}2\) 走到 \(p_2\)、\(t{=}4\) 离开）。 其余格子全程安全。
第一步：算每个格子的安全区间。 - \(p_0, p_1, p_3, p_4\)：全程安全 → 各只有一个安全区间 \(I = [0, \infty)\)。 - \(p_2\)：不安全区间 \([2, 4]\) → 安全区间两个：\(I_a = [0, 2)\) 和 \(I_b = (4, \infty)\)。
注意 \(p_2\) 这个格子，时间展开图要存它 \(0.1\) 步长下的几十上百个节点，SIPP 只存**两个**状态：\((p_2, I_a)\) 和 \((p_2, I_b)\)。 这就是压缩。
第二步：从 \(p_0\) 出发搜到 \(p_4\)，看 SIPP 怎么"等障碍先过"。
智能体 \(t{=}0\) 在 \((p_0, [0,\infty))\)，每步走一格耗 \(1\,\text{s}\)。 直奔目标的走法是 \(p_0\to p_1\to p_2\to p_3\to p_4\)，到达 \(p_2\) 的时刻是 \(t{=}2\)。 但 \(t{=}2\) 恰好是障碍占住 \(p_2\) 的起点——\((p_2, I_a=[0,2))\) 要求到达时刻 \(< 2\)，而 \(t{=}2\) 不满足（边界冲突）； \((p_2, I_b=(4,\infty))\) 要求到达 \(> 4\)。 于是 SIPP 在转移到 \(p_2\) 时面临选择：
- 走 \((p_1)\to(p_2, I_a)\)：需在 \(t<2\) 到达 \(p_2\)，但最早只能 \(t{=}2\) 到 → 不可行。
- 走 \((p_1)\to(p_2, I_b)\)：需在 \(t>4\) 到达 \(p_2\)。 智能体可以**在 \(p_1\) 等到 \(t{=}4\) 之后再迈进 \(p_2\)**——比如在 \(p_1\) 等到 \(t{=}4\)、\(t{=}5\) 迈入 \(p_2\)（此时 \(p_2\) 已空）。 这条转移合法，到达 \((p_2, I_b)\) 的最早时间是 \(t{=}5\)。
于是最优解是：\(p_0(t{=}0)\to p_1(t{=}1)\to\) 在 \(p_1\) 等到 \(t{=}4\) \(\to p_2(t{=}5)\to p_3(t{=}6)\to p_4(t{=}7)\)。 SIPP 自动算出了"在 \(p_1\) 等三秒让障碍先过"这个 wait-then-move 动作——而它全程只展开了个位数的 \((p, I)\) 状态，没碰任何时间离散的几十层节点。
把搜索状态摊开看一遍。 列出 SIPP 实际生成的 \((p, I)\) 状态及其最早到达时间 \(g\)（这里用 Dijkstra 式按 \(g\) 出队，启发为 \(0\) 便于手算）：
| 出队顺序 | 状态 \((p, I)\) | 最早到达 \(g\) | 由何转移而来 |
|---|---|---|---|
| 1 | \((p_0, [0,\infty))\) | \(0\) | 起点 |
| 2 | \((p_1, [0,\infty))\) | \(1\) | \(p_0\) 走一步 |
| 3 | \((p_2, I_b{=}(4,\infty))\) | \(5\) | \(p_1\) 等到 \(t{=}4\)、\(t{=}5\) 迈入（\(I_a\) 不可达：需 \(t<2\) 到，最早 \(t{=}2\)） |
| 4 | \((p_3, [0,\infty))\) | \(6\) | \(p_2\) 走一步 |
| 5 | \((p_4, [0,\infty))\) | \(7\) | \(p_3\) 走一步，到达目标 |
整个搜索只出队了 \(5\) 个状态——注意 \(p_2\) 那一格，时间展开图（\(0.1\,\text{s}\) 步长、\(10\,\text{s}\)）要存约 \(100\) 个 \((p_2, t)\) 节点，SIPP 这里只生成了**一个有用的** \((p_2, I_b)\)（\((p_2, I_a)\) 因不可达被剪掉）。
关键的一步是出队顺序 3：从 \((p_1, [0,\infty))\) 转移到 \(p_2\) 时，SIPP 发现直接走（\(t{=}2\) 到）落在 \(p_2\) 的不安全区间里，于是按转移规则取"能进入 \(I_b\) 的最早出发"——在 \(p_1\) 等到 \(t{=}4\)、再迈入，到达 \(g{=}5\)。
这个"等"不是预先编排的，而是转移规则 max(earliest_depart + dt_move, s2) 自动算出来的——障碍占用的区间，逼着到达时间往后跳到了下一个安全窗口的开端。
和 T1 ST 图的对照（关键洞察）。 把这个例子翻译到 T1 §T1.4 的 ST 图语言：障碍占据 \(p_2\) 的时间窗 \([2,4]\)，画在 ST 图上就是一块"阻挡多边形"（横轴 \(t\in[2,4]\)、纵轴 \(s\) 在 \(p_2\) 处的那一段）。 ST 图里 DP-ST 搜索那条"绕过阻挡多边形下方"的世界线（先慢后快），和 SIPP 这里"在 \(p_1\) 等到障碍走"的解，是同一个决策的两种表示：ST 图把它画成"世界线绕过阻挡块"，SIPP 把它编码成"选 \(p_2\) 的第二个安全区间 \(I_b\)"。 前者连续、后者离散； 前者天然适合接 QP 精修，后者天然适合多机组合搜索。 记住这个对照，§T2.5 的"走廊 vs 搜索"就水到渠成。
代码实现（为什么 → 正确 → 错误 → 对比）¶
Step 1 — 为什么这样写。 SIPP 的实现有两个独立的部分：(1) 从障碍预测算出每个构型的安全区间（预处理），(2) 在 \((p, I)\) 图上做带"最早到达时间"的 A*。 下面分别给骨架。
// SIPP 安全区间计算 + 转移(教学示意,非完整实现)
using Interval = std::pair<double, double>; // [start, end)
constexpr double INF = std::numeric_limits<double>::infinity();
// 由不安全区间(已按起点排序)算安全区间
std::vector<Interval> SafeIntervals(
const std::vector<Interval>& obstacle_intervals) {
std::vector<Interval> intervals;
double t = 0.0;
for (const auto& [occ_start, occ_end] : obstacle_intervals) {
if (occ_start > t) intervals.emplace_back(t, occ_start); // t 到障碍来临前安全
t = std::max(t, occ_end); // 跳到障碍离开之后
}
intervals.emplace_back(t, INF); // 最后一段直到无穷
return intervals;
}
struct SippState { int cell; Interval interval; double arrival; };
// 对每个邻居,找出所有可达的 (cell', interval'),并算最早到达时间
std::vector<SippState> GetSuccessors(
const SippState& s, const Graph& graph, double dt_move,
const SafeIntervalFn& safe_intervals_of) {
std::vector<SippState> succ;
double earliest_depart = s.arrival;
double latest_depart = s.interval.second; // 不能等过本区间右端
for (int ncell : graph.Neighbors(s.cell)) {
for (const auto& [s2, e2] : safe_intervals_of(ncell)) {
// 移动耗时 dt_move,到达邻居的时刻须落在其安全区间 [s2, e2) 内
double t_arrive = std::max(earliest_depart + dt_move, s2); // 必要时原地等到能进
if (t_arrive < e2 && (t_arrive - dt_move) < latest_depart)
succ.push_back({ncell, {s2, e2}, t_arrive}); // 合法的 wait-then-move
}
}
return succ;
}
Step 2 — 正确要点。 三处易错但必须对的地方：(a) 转移时"先等再走"的等待**不能等过当前安全区间的右端** i_end——等过了你就被障碍撞了；
(b) 到达邻居的时刻取 max(earliest_depart + dt_move, s2)——若直接走到达过早（邻居还没空），就在原地多等到 s2；
(c) 每个 \((p, I)\) 状态记录**最早到达时间**并据此排序展开，才能保证时间最优。
Step 3 — 一个常见的错误写法。 把安全区间退化成"只取第一个"，是新手最常犯的：
// 反例:只用每个格子的第一个安全区间,忽略后续区间
std::vector<SippState> GetSuccessorsWrong(
const SippState& s, const Graph& graph,
const SafeIntervalFn& safe_intervals_of) {
std::vector<SippState> succ;
for (int ncell : graph.Neighbors(s.cell)) {
Interval first = safe_intervals_of(ncell).front(); // 错:只看第一个区间
double t_arrive = s.arrival + 1.0;
if (Within(t_arrive, first))
succ.push_back({ncell, first, t_arrive});
}
// 问题:障碍走后开放的"第二个安全区间"永远进不去
// → 凡是需要"等障碍过去再走"的解全部丢失 → 误判无解
return succ;
}
它丢掉了第二个及以后的安全区间，于是上面那个"等到 \(t{=}5\) 再进 \(p_2\)"的最优解根本搜不到——SIPP 会错误地报告无解。
Step 4 — 对比。 正确版枚举邻居的**所有**安全区间、并允许在原地等待到能进的最早时刻； 错误版只认第一个区间、不会等。 差别集中在一个问题上：你的实现允不允许"等障碍先过去"？ SIPP 的全部价值就在这个"等"字上，丢了它，SIPP 就退化成了一个不会避动障的静态 A*。
一份能跑的完整 SIPP（无外部依赖）¶
上面是分块骨架，便于看清结构。
这里给一份**整合的、可直接编译运行**的最小 SIPP——只用 C++ 标准库（<queue>/<unordered_map>），g++ -std=c++17 即可编译，跑出本节算例那个"在 \(p_1\) 等到 \(t{=}5\)"的解。
学习算法最好的方式是让它真的动起来，而不是停在伪代码；
而且这里用 C++ 也贴合无人机/自驾的实际部署语言（Apollo、EPSILON、libMultiRobotPlanning 都是 C++）。
#include <vector>
#include <queue>
#include <unordered_map>
#include <map>
#include <limits>
#include <algorithm>
#include <cstdio>
using Interval = std::pair<double, double>; // [start, end)
constexpr double INF = std::numeric_limits<double>::infinity();
// 由不安全区间算安全区间(顺带合并重叠)
std::vector<Interval> BuildSafeIntervals(std::vector<Interval> unsafe) {
std::sort(unsafe.begin(), unsafe.end());
std::vector<Interval> iv;
double t = 0.0;
for (auto [a, b] : unsafe) {
if (a > t) iv.emplace_back(t, a); // t 到障碍来临前安全
t = std::max(t, b); // 跳到障碍离开后(合并重叠)
}
iv.emplace_back(t, INF); // 最后一段到无穷
return iv;
}
// 状态键:(cell, 区间起点) 足以唯一标识一个 (p, I)
struct Key { int cell; double iv_start;
bool operator==(const Key& o) const { return cell==o.cell && iv_start==o.iv_start; } };
struct KeyHash { size_t operator()(const Key& k) const {
return std::hash<int>()(k.cell) ^ std::hash<double>()(k.iv_start); } };
// 优先队列节点:按到达时间 g 小根堆
struct PQItem { double g; int cell; Interval iv; Key parent; bool has_parent; };
struct PQCmp { bool operator()(const PQItem& a, const PQItem& b) const { return a.g > b.g; } };
std::vector<std::pair<int,double>> Sipp(
int start, int goal,
const std::map<int, std::vector<int>>& neighbors, // 邻接表
const std::map<int, std::vector<Interval>>& unsafe_map, // 各 cell 的不安全区间
double move_time = 1.0) {
// 安全区间缓存
std::unordered_map<int, std::vector<Interval>> safe_cache;
auto safe_of = [&](int cell) -> const std::vector<Interval>& {
auto it = safe_cache.find(cell);
if (it == safe_cache.end()) {
std::vector<Interval> u;
auto mi = unsafe_map.find(cell);
if (mi != unsafe_map.end()) u = mi->second;
it = safe_cache.emplace(cell, BuildSafeIntervals(u)).first;
}
return it->second;
};
std::priority_queue<PQItem, std::vector<PQItem>, PQCmp> pq;
std::unordered_map<Key, double, KeyHash> best; // (cell,区间) -> 最早到达
std::unordered_map<Key, Key, KeyHash> parent; // 回溯用
std::unordered_map<Key, bool, KeyHash> has_par;
Interval start_iv = safe_of(start).front(); // 起点区间(假设 t=0 安全)
pq.push({0.0, start, start_iv, {}, false});
while (!pq.empty()) {
PQItem cur = pq.top(); pq.pop();
Key key{cur.cell, cur.iv.first};
auto bit = best.find(key);
if (bit != best.end() && bit->second <= cur.g) continue; // 已有更早到达 → 剪枝(SIPP 松弛)
best[key] = cur.g;
parent[key] = cur.parent; has_par[key] = cur.has_parent;
if (cur.cell == goal) { // 到达目标:回溯
std::vector<std::pair<int,double>> path;
Key k = key;
while (true) {
path.push_back({k.cell, best[k]});
if (!has_par[k]) break;
k = parent[k];
}
std::reverse(path.begin(), path.end());
return path;
}
auto nit = neighbors.find(cur.cell);
if (nit == neighbors.end()) continue;
for (int nb : nit->second) {
for (auto [s2, e2] : safe_of(nb)) {
// 先(必要时)在原地等到能进 nb 的最早出发,再走 move_time
double t_arrive = std::max(cur.g + move_time, s2 + move_time);
double depart = t_arrive - move_time;
// 等待不能等过当前区间右端;到达须落在邻居区间内
if (depart < cur.iv.second && s2 <= t_arrive && t_arrive < e2) {
Key nkey{nb, s2};
auto it = best.find(nkey);
if (it == best.end() || t_arrive < it->second)
pq.push({t_arrive, nb, {s2, e2}, key, true});
}
}
}
}
return {}; // 无解
}
// ---- 跑本节算例: p0..p4 一维走廊,障碍占 p2 于 [2,4] ----
int main() {
std::map<int, std::vector<int>> line = { // 邻接(一维链)
{0,{1}}, {1,{0,2}}, {2,{1,3}}, {3,{2,4}}, {4,{3}}};
std::map<int, std::vector<Interval>> unsafe = {
{2, {{2.0, 4.0}}}}; // 仅 p2 在 [2,4] 不安全
auto sol = Sipp(0, 4, line, unsafe, 1.0);
for (auto [cell, t] : sol) std::printf("(p%d, t=%.1f) ", cell, t);
std::printf("\n");
// 期望: (p0,0.0)(p1,1.0)(p2,5.0)(p3,6.0)(p4,7.0) —— 在 p1 等到 t=4、t=5 进 p2
return 0;
}
这份代码值得注意的三处与算法本质对应的地方：(a) if (bit != best.end() && bit->second <= cur.g) continue; 这行就是上文强调的 SIPP 松弛——同一 \((p,I)\) 以更早到达再次出现时不能丢，只在已记录更早时才剪枝；
(b) t_arrive = std::max(cur.g + move_time, s2 + move_time) 把"原地等到邻居安全窗口开端"自动算了出来——这就是"等障碍先过"的来源，没有任何显式的"等待动作"，等待是被区间约束逼出来的；
(c) BuildSafeIntervals 里 t = std::max(t, b) 顺手合并了重叠的不安全区间，避开了陷阱区要讲的那个坑。
把障碍区间改成 {{1.0, 6.0}} 再跑，你会看到到达时间整体推后——窗口越长，等得越久，正是 §T2.6 渐隐练习变体 A 的雏形。
⚠️ 常见陷阱¶
💡 编程陷阱：安全区间合并时把相邻/重叠的不安全区间漏合
- 错误描述：多个动态障碍各自给出不安全区间，直接逐个取补集，没先把重叠或相邻的不安全区间并起来。
- 现象与后果：算出一堆零碎甚至首尾粘连的"安全区间"，其中夹着实际不安全的缝隙；
搜索会让智能体在一个根本不存在的瞬间挤进格子，仿真里表现为偶发碰撞。
- 根本原因：补集运算要求输入是**已合并、不重叠**的不安全区间集合；
否则补出来的"安全"区间会把障碍间的空隙误算进去。
- 正确做法：先对所有不安全区间按起点排序、合并重叠与相邻段，再取补集（上面 safe_intervals 里 t = max(t, occ_end) 就是在做合并）。
💡 编程陷阱：区间边界的开闭与浮点相等判断
- 错误描述：处理"到达时刻恰好等于区间端点"时含糊其辞——用 <= 还是 <、浮点数直接 == 比较，全凭手感。
- 现象与后果：偶发的"恰好擦边"碰撞或误剪。
比如算例里到达 \(p_2\) 恰好 \(t{=}2\)、而不安全区间从 \(t{=}2\) 起：若把进入条件写成 \(t \le 2\) 就放行了一个其实危险的时刻；
浮点下 \(1.9999999\) 与 \(2.0\) 的比较还可能因精度抖动时灵时不灵，bug 难复现。
- 根本原因：安全区间的端点是"安全与不安全的分界"，端点本身算哪边必须明确定义且全程一致；
而浮点运算的舍入误差让"恰好相等"成了不可靠的判断。
- 正确做法：约定统一的开闭惯例（常见做法：不安全区间闭 \([t_1,t_2]\)、安全区间取其补的开/半开形式），进入邻居用严格不等式 + 一个小容差 \(\epsilon\)（如 t_arrive < e2 - eps）兜住浮点误差；
所有区间比较都走同一套带容差的工具函数，不散落手写。
⚠️ 概念陷阱：以为 SIPP 适用于任何动态环境 - 错误描述：把 SIPP 当成万能动态避障器，忽视它的两个前提——智能体可原地等待、且能瞬时启停。 - 现象与后果：用在真车上，规划出"立刻停住等 3 秒再瞬间提速"的解，实车根本执行不了（减速、停车、再加速都要时间和距离），跟踪误差爆表甚至追尾。 - 根本原因：SIPP 的最优性证明建立在"瞬时启停 + 可原地驻留"这两个理想假设上。 质点网格满足，带惯性的车辆/无人机不满足。 - 正确做法：把 SIPP 当**离散粗搜/低层**用，产出"何时该等、走哪条离散路"的决策，再交给连续优化（走廊 QP、§T2.3 起）补上动力学可行的速度剖面； 或改用考虑动力学的 SIPP 变体（如带减速假设的 SIPP-IP）。
🧠 思维陷阱：把"安全区间"看成与 ST 图无关的新发明 - 错误描述：学 SIPP 时孤立地记它的转移规则，没意识到它和刚学过的 T1 ST 图是同一问题的两种语言。 - 现象与后果：知识点散成一地，遇到"该用 SIPP 还是 ST 图"时无从判断，也想不到二者可以互相 warm-start。 - 根本原因：缺少"同一对象、不同表示"的抽象视角——ST 图的阻挡多边形 ⇔ SIPP 的不安全区间，世界线绕行 ⇔ 选后一个安全区间。 - 正确做法：每学一个新表示，主动找它和已知表示的同构。 SIPP（离散、可组合、宜多机）与 ST 图/走廊（连续、可优化、宜单体精修）各擅一端，§T2.5 会把这个判断标准讲透。
多视角对照：同一个"如何在搜索里表达时间"的问题，本章及 T1 给了三种答案，值得并排看清。 时间展开图视角：把时间按 \(\Delta t\) 切成离散层，每层复制一份空间图——最直白，但状态数 \(\propto T/\Delta t\)，分辨率一细就爆，是被 SIPP 取代的朴素基线。 SIPP 视角：只存"分段恒定的安全/不安全"的那几段区间，状态数只随障碍经过次数走——离散、紧凑、可被 CBS 复用，但需"可等待 + 瞬时启停"假设。 ST 图视角（T1）：把动态占用画成连续的"阻挡多边形"，在连续的 \((s,t)\) 平面上绕行——天然接 QP 精修，但只处理纵向、且要路径已定。 三者不是替代关系，而是"离散步 → 离散区间 → 连续平面"的精细化阶梯，各自的甜区不同：多机组合搜索用 SIPP，单体纵向精修用 ST 图，二者都比朴素的时间展开图省。
本质洞察：SIPP 的全部魔力，是把"时间轴上的无穷个时刻"压缩成"少数几段安全区间"——它没有改变问题，只是换了一种**信息无损但表示紧凑**的编码。 这与 T1 ST 图把"动态障碍的时空占用"画成"阻挡多边形"是同一种智慧：找到问题里真正分段恒定的结构（占用要么有要么无），就只存那几段，而不是逐点存。 凡是"状态在某个维度上分段恒定"的问题，都值得想想有没有 SIPP 式的区间压缩。
练习¶
- [手算 + 实现] 把算例改成两个障碍：障碍 A 占 \(p_2\) 于 \([2,4]\)，障碍 B 占 \(p_3\) 于 \([5,6]\)。 手算每个格子的安全区间，再写 SIPP 搜出 \(p_0\to p_4\) 的时间最优解，验证它如何先后避开两个障碍。
- [实现] 在 2D 网格（如 \(20\times 20\)）上实现完整 SIPP：3 个匀速直线运动的动态障碍 → 计算安全区间 → A* 搜索 → 可视化时空路径 \((x, y, t)\)。 对比"有 SIPP"与"把动障当永久静态障碍"两种做法的路径长度和是否找到解。
- [跨章综合题] SIPP 的"安全时间区间"与 Apollo ST 图（T1 §T1.4）的"阻挡多边形"是同一问题的两种表示。 请画一张图把同一个动态障碍同时画成两种形式，标出"SIPP 选后一个安全区间"对应 ST 图里的哪条世界线，并各列两条优劣，说明何时该用哪种。
§T2.3 SSC：时空语义走廊（Spatio-temporal Semantic Corridor）⭐⭐⭐¶
这一节解决什么问题：§T2.2 的 SIPP 给出的是离散格上的路径——一串"何时在哪个格子"。 可真车要的是一条平滑、动力学可行的连续轨迹。 怎么把"安全区域"从离散搜索的语言，翻译成连续优化能直接吃的约束？ SSC 的答案是：把 \((s, l, t)\) 三维自由空间切成一串互相连接的**凸立方体（cube）**，再用 Bézier 曲线的凸包性质，把"避开障碍"这个非凸约束，变成"控制点待在 cube 里"这组线性不等式——于是整个轨迹生成塌缩成一个标准 QP。 这是走廊线的开篇，也是 T3 时空轨迹优化的直接前身。
动机：从离散解到连续优化，凸性是关键¶
设想你已经用某种搜索（SIPP、时空格、或 ST 图 DP）拿到了一条粗解："先在这等等、再往左切、最后加速"。 这条解是离散的、棱角分明的，不能直接给底盘执行。 你想把它精修成平滑轨迹，自然想到上一章学的 QP——可这里有个拦路虎：避障约束是非凸的。" 待在障碍之外"这个集合（自由空间）通常是非凸的，往里塞进 QP，OSQP 这类凸求解器要么不收敛、要么停在一个糟糕的局部解（T1 §T1.3 反复强调过"凸性是 QP 毫秒级求解的前提"）。
SSC 的破局思路：自由空间整体非凸，但可以切成一块块凸的。 把 \((s, l, t)\) 里的无碰撞区域分解成一串凸立方体，轨迹被约束"依次穿过这些 cube"。 每个 cube 内部是凸的，"在 cube 里"是线性约束，于是非凸的避障问题被切成了凸的子问题——这正是 T1 §T1.5 "走廊重获凸性"那句话在三维时空里的兑现。 走廊（corridor）这个名字很形象：它就是自由空间里一条由凸块串成的、保证无碰撞的"管道"，轨迹在管道里随便优化，怎么都撞不到墙。
如果不这样做会怎样（反事实）¶
不切走廊、直接在全 \((s,l,t)\) 空间对每个障碍写"轨迹点离障碍距离 \(\ge\) 安全裕度"的约束，会怎样？ 这类约束形如 \(\|\mathbf{p}(t) - \mathbf{o}(t)\| \ge d\)，是**非凸**的（可行域是障碍外的"甜甜圈"，不是凸集）。 把它塞进优化，你得用非凸求解器（SQP、内点法解 NLP），慢且易陷局部最优； 初值稍差就给出绕错方向甚至穿墙的解。 更糟的是障碍一多，非凸约束叠加，可行域支离破碎，求解器几乎必然卡住。 走廊的全部意义，就是用"预先切好凸块"这步预处理，把后端从痛苦的非凸优化解放成温顺的凸 QP——把难点从在线优化挪到了离线/前端的走廊生成。
历史：Ding 等人的语义时空走廊与 EPSILON¶
SSC 由 Ding、Zhang、Chen、Shen（HKUST 沈劭劼组）在 RA-L 2019 提出（《Safe Trajectory Generation for Complex Urban Environments Using Spatio-temporal Semantic Corridor》，IEEE RA-L 4(3):2997–3004，2019）。 它的贡献不只是"切凸块"——更关键的是"语义（semantic）"二字：cube 的生成不仅看几何碰撞，还吃交通语义（交通灯、限速、让行、车道边界）。 一条红灯前必须停的语义，会反映成 \((s, l, t)\) 里一个限制 \(s\) 上界的 cube； 一个让行义务，会让 cube 在某段时间收窄。 这样，行为决策（该让该抢、能不能闯）就被编码进了走廊形状，运动规划只需在走廊里求最优，无需再操心高层规则。 SSC 后来被集成进 HKUST 的 EPSILON 完整系统，与行为层 EUDM（Efficient Uncertainty-aware Decision Making，一种多策略行为决策）配合——EUDM 在多策略中选一个意图，SSC 据此生成对应的时空走廊，运动层在走廊里解 QP。 这条"行为层定走廊、运动层解 QP"的分工，是工业级自驾栈的一种经典范式。
理论：cube 序列、Bézier 凸包与 QP¶
SSC 把轨迹生成拆成三步：生成 cube 序列 → 用 Bézier 参数化轨迹 → 借凸包性质把安全约束线性化、组装成 QP。
第一步：语义 cube 序列。 在 \((s, l, t)\) 三维网格上，从一条种子轨迹（粗解）出发，对每一段做"膨胀"：在不碰静态/动态障碍、且不违反语义规则的前提下，把一个小盒子尽量往各方向撑大，得到一个无碰撞凸立方体 \(C_k\)。 相邻的 cube 要**有重叠（overlap）**——重叠区是轨迹从一个 cube 过渡到下一个 cube 的"门"，保证走廊连续不断裂。
"语义"二字落到 cube 生成上是什么意思？ 这是 SSC 区别于纯几何走廊（如 SFC）的关键，值得说清。 膨胀一个 cube 时，约束它别越界的不只有几何障碍，还有**语义规则**：红灯要求在停止线前停住，就给 cube 的 \(s\) 上界压一道"不超过停止线"的面； 限速 \(v_{\max}\) 约束了这段时间内 \(s\) 的增长速率，反映成 cube 在 \((s,t)\) 方向的斜率上界； 让行义务让 cube 在某段时间主动收窄，把"该让"编码成"这段时空不可用"。 于是 cube 的每一个面，要么来自"别撞障碍"（几何），要么来自"别违规"（语义）——行为决策就这样被**焊进了走廊形状**，下游 QP 只管在 cube 里求最优，不必再判断红灯该不该停、该不该让，那些都已经体现在 cube 边界上了。 这正是 SSC 能与行为层（EPSILON 的 EUDM）干净配合的原因：行为层的意图 → 一组语义规则 → 一串带语义边界的 cube → 运动层 QP。 overlap 到底要满足什么条件？ 不是"差不多挨着"就行。 两个相邻 cube \(C_k, C_{k+1}\) 的重叠区 \(C_k \cap C_{k+1}\) 必须是一个**非空的、有正体积的凸区域**——因为轨迹要在这个公共区域里完成交接：第 \(k\) 段 Bézier 的末控制点和第 \(k+1\) 段的首控制点（它们因连续性约束相等）这个公共点，必须能同时落在两个 cube 内，也就是必须落在 \(C_k \cap C_{k+1}\) 里。 重叠区若为空，这个公共控制点无处安放，QP 直接无解（这正是 §T2.3 编程陷阱要讲的那个坑）； 重叠区若只是一条线或一个点（零体积），数值上极不稳定。 所以 cube 生成时要显式保证相邻重叠有一个**最小正体积**。
动态障碍怎么进来？ 它在每个时刻占据 \((s,l)\) 的一块，投影到 \((s,l,t)\) 就是一根随 \(t\) 倾斜的"时空柱"，cube 膨胀时绕开这根柱子即可——于是同一个 \((s,l)\) 位置，在障碍经过的时间段被排除、其余时间可用，这和 SIPP 的安全区间是一个道理，只是从一维时间区间升级成了三维 cube。
第二步：Bézier 参数化。 在每个 cube 内，轨迹用一段 Bézier 曲线表示。 一条 \(n\) 阶 Bézier 曲线由 \(n+1\) 个**控制点（control point）** \(\mathbf{c}_0, \dots, \mathbf{c}_n\) 定义：
第三步（核心）：凸包性质把安全约束线性化。 Bézier 曲线有一个金子般的性质——整条曲线一定落在它的控制点的凸包（convex hull）之内。 因为上式里那些 Bernstein 基函数 \(\binom{n}{i}(1-\tau)^{n-i}\tau^i\) 非负、且对 \(i\) 求和恒等于 \(1\)，所以 \(\mathbf{p}(\tau)\) 永远是控制点的一个**凸组合**，凸组合自然落在凸包里。 这意味着：只要所有控制点 \(\mathbf{c}_i\) 都落在凸 cube \(C_k\) 内，整条曲线段就一定在 \(C_k\) 内、绝不出界。 而"控制点在 cube 内"是什么？ cube 是凸多面体 \(C_k = \{\mathbf{x}: A_k\mathbf{x}\le \mathbf{b}_k\}\)，"控制点在内"就是 \(A_k\mathbf{c}_i \le \mathbf{b}_k\)——一组**线性不等式**！ 至此，非凸的"曲线避开障碍"被彻底翻译成了凸的、线性的"控制点满足 \(A_k\mathbf{c}_i\le\mathbf{b}_k\)"。 这是 SSC（以及一切 Bézier/B-spline 走廊方法）的命门所在。
凸包性质不止管位置，还管速度、加速度。 这一点常被忽略，却是工程上把动力学约束也线性化的关键。 一条 \(n\) 阶 Bézier 曲线对参数求导，得到的**导函数仍是一条 Bézier 曲线**——只是降了一阶（\(n-1\) 阶），而且它的控制点是原控制点的**线性差分**：
其中上标 \((1)\) 是一阶导（速度方向）的控制点、\((2)\) 是二阶导（加速度）的控制点，以此类推——每一阶都是上一阶控制点的相邻差分。 既然导函数也是 Bézier、也服从凸包性质，那么"速度落在某速度盒内""加速度落在某加速度盒内"这类动力学约束，就能照搬同一招——只要原控制点的**线性差分**落在对应的盒子里即可。 换句话说，速度上限、加速度上限这些约束，经由"导函数控制点 = 原控制点的线性组合"这一步，全部变成了原控制点的线性不等式。 于是不光"避障"是线性的，连"别开太快、别冲太猛"也是线性的——整个 QP 的约束保持全线性，凸性毫发无损。 这就是为什么走廊 QP 能同时塞进安全和动力学两类约束、还依然是个标准 QP（拓展练习 3 会让你亲手推一遍这些差分关系）。
第四步：组装 QP。 决策变量是所有段所有控制点。 代价：最小化 jerk 或 snap（控制点的高阶差分平方和，承接 T1 piecewise-jerk 的思路）+ 偏离参考线的惩罚。 约束：(a) 安全——每段控制点 \(A_k\mathbf{c}_i\le\mathbf{b}_k\)（线性）； (b) 连续性——相邻段在 cube 重叠区的拼接点位置/速度/加速度相等（线性等式，靠 Bézier 控制点之间的线性关系表达）； (c) 起终点边界。 代价二次、约束全线性 → 标准凸 QP，OSQP 毫秒级求解。 和 T1 §T1.3 的路径 QP 是同一个家族，只是把"box 约束"换成了"cube 半空间约束"、自变量从 \(l(s)\) 换成了 Bézier 控制点。
走廊 QP 也会不可行——软约束兜底。 这里要补一个 T1 §T1.5 在速度 QP 上讲过、走廊 QP 同样躲不开的问题：硬约束有时无解。 cube 序列若因走廊过窄、相邻重叠不足、或动态障碍把某段 cube 挤得太小，安全约束 + 连续性约束可能凑不出可行解，OSQP 直接报 infeasible，规划器哑火。 对策和 T1 一脉相承——把最容易冲突的硬约束**软化**：给安全约束引入松弛变量 \(\xi \ge 0\)，写成 \(A_k\mathbf{c}_i \le \mathbf{b}_k + \xi\)，再在代价里对 \(\xi\) 加一个大权重的惩罚 \(w_\xi\,\xi^2\)。 这样一来，正常情况下 \(\xi\) 被惩罚压到 \(0\)（等价于原硬约束）； 实在无解时 \(\xi\) 取一个尽量小的正值，让 QP 仍能吐出一条"轻微越界但可控"的轨迹，而不是彻底失败——把"硬撞墙"换成"贴墙蹭一下"，工程上远比哑火安全。 注意软化的是安全裕度这类可让步的约束，动力学硬限（如曲率上限）通常不软化，否则解出来根本执行不了。 这个"硬约束软化 + 松弛惩罚"的套路，从 T1 速度 QP 到这里的走廊 QP、再到 T3 的 NLP，是优化式规划兜底鲁棒性的通用手段。
一个最小走廊 QP 算例（控制点在 cube 里）¶
把凸包性质落到一个能想象的二维例子（先看 \((s, l)\) 平面、忽略 \(t\)，便于画图）。 设自由空间被切成三个矩形 cube：\(C_1 = [0,4]\times[-1,1]\)、\(C_2 = [3,7]\times[0,2]\)、\(C_3 = [6,10]\times[-1,1]\)（相邻 cube 在 \(s\) 方向有 \(1\,\text{m}\) 重叠：\(C_1\cap C_2\) 在 \(s\in[3,4]\)，\(C_2\cap C_3\) 在 \(s\in[6,7]\)）。 这串 cube 描述了一条"先直、中间向左凸一下绕过某物、再回正"的走廊。
用三段三阶 Bézier（每段 4 个控制点）表示轨迹，每段约束在对应 cube 内： - 第一段 4 个控制点全部满足 \(0\le s\le 4,\ -1\le l\le 1\)（在 \(C_1\) 内）； - 第二段全部满足 \(3\le s\le 7,\ 0\le l\le 2\)（在 \(C_2\) 内）——注意 \(l\ge 0\) 这条把第二段顶到了上方，正是"向左凸"的来源； - 第三段全部满足 \(6\le s\le 10,\ -1\le l\le 1\)（在 \(C_3\) 内）。
把第二段的约束摊开看。 拿"向左凸"的第二段（4 个控制点 \(\mathbf{c}_0^{(2)}, \dots, \mathbf{c}_3^{(2)}\)，每个是 \((s, l)\) 二维点）举例，"控制点在 \(C_2{=}[3,7]\times[0,2]\) 内"展开成每个控制点 4 条标量不等式、共 \(4\times 4 = 16\) 条。 对每个 \(i\in\{0,1,2,3\}\)：
写成矩阵 \(A_2\,\mathbf{c}_i^{(2)} \le \mathbf{b}_2\) 的形式，每个控制点对应
那条 \(-l_i \le 0\)（即 \(l_i \ge 0\)）就是把整段顶到参考线左侧的硬约束——它来自 \(C_2\) 的下边界 \(l{=}0\)，而 \(C_2\) 之所以下边界抬到 \(0\)，是因为右半空间被障碍占了。 三段合起来，决策变量是 \(3 \times 4 = 12\) 个控制点（\(24\) 个标量），安全约束总共 \(3 \times 16 = 48\) 条线性不等式——全部是控制点的线性函数，凑成 OSQP 吃的稀疏 \(A\) 矩阵。 对比 T1 §T1.3 那个 piecewise-jerk 路径 QP 摊开成 \(9\) 维向量、三对角矩阵的算例：结构如出一辙，只是这里自变量从"每个 \(s_i\) 处的 \(l\)"换成了"Bézier 控制点"，box 约束换成了 cube 半空间约束。
连续性：第一段末控制点 = 第二段首控制点（位置连续），即 \(\mathbf{c}_3^{(1)} = \mathbf{c}_0^{(2)}\)（一条向量等式 = 两条标量等式）； 速度连续要求两段在接缝处的一阶导控制点相等，用前面那个差分关系 \(\mathbf{c}_i^{(1)\prime} = n(\mathbf{c}_{i+1} - \mathbf{c}_i)\) 展开，也是控制点的线性等式； 加速度连续同理。 第二、三段同理。 QP 在这堆线性约束下最小化 jerk，解出来的曲线会**平滑地从 \(C_1\) 钻进 \(C_2\) 的上凸、再落回 \(C_3\)**，全程不出任何 cube——因为凸包性质替我们担保了：控制点在 cube 里，曲线就在 cube 里。 你不需要对曲线上的无穷多个点逐一检查碰撞，只需管住有限个控制点。 这就是把"无穷个安全约束"压缩成"有限个线性约束"的威力。
把维度加回 \(t\)：每个 cube 变成 \((s,l,t)\) 里的立方体，第二段的"\(l\ge 0\)"可能来自"某动态障碍在 \(t\in[2,4]\) 占了右侧"，于是 cube 在那段时间把 \(l\) 下界抬高——走廊形状自动编码了"何时该往左让"。
代码实现（为什么 → 正确 → 错误 → 对比）¶
Step 1 — 为什么这样写。 走廊 QP 的骨架是：拿到 cube 序列 → 为每段每个控制点写 cube 半空间约束 → 写段间连续性等式 → 组装成 OSQP 的 \(P, q, A, l, u\)。 关键是约束**只作用在控制点上**，数量有限。
// 时空走廊 Bézier QP 骨架(教学示意,非完整实现)
// 决策变量:所有段所有控制点;后端用 OSQP(与 T1 一脉相承)
struct Cube { Eigen::MatrixXd A; Eigen::VectorXd b; }; // 半空间 A x <= b
void BuildCorridorQp(const std::vector<Cube>& cubes, int n_order,
Eigen::SparseMatrix<double>* P, // 代价矩阵(输出)
std::vector<LinearConstraint>* constraints) {
const int n_seg = static_cast<int>(cubes.size());
const int n_ctrl = n_order + 1; // 每段控制点数
for (int k = 0; k < n_seg; ++k) {
for (int i = 0; i < n_ctrl; ++i) {
auto c_ki = CtrlPointVar(k, i); // 第 k 段第 i 个控制点(决策变量)
// 安全约束:控制点落在 cube 内 → A_k c_ki <= b_k(线性!凸包性质保证整段在内)
constraints->push_back({cubes[k].A, c_ki, cubes[k].b});
}
}
for (int k = 0; k < n_seg - 1; ++k) {
// 连续性:第 k 段末控制点 == 第 k+1 段首控制点(位置连续)
constraints->push_back(Equal(CtrlPointVar(k, n_ctrl - 1),
CtrlPointVar(k + 1, 0)));
// 速度/加速度连续:用 Bézier 控制点的差分关系写成线性等式
AddDerivativeContinuity(constraints, k, n_order);
}
*P = JerkCostMatrix(n_seg, n_order); // 最小 jerk:控制点高阶差分平方和
}
Step 2 — 正确要点。 安全约束**只写在控制点上**，靠凸包性质免费保证整段曲线安全——这是 SSC 高效的根本，别去对曲线采样点逐个加约束（那会让约束数爆炸且失去线性结构）。 段间连续性必须包含到二阶（位置、速度、加速度），否则轨迹在 cube 接缝处会有加速度跳变、乘客顿挫。 相邻 cube 必须有重叠，拼接点才有合法落点。
Step 3 — 一个常见的错误写法。 对 Bézier 曲线上采样一堆点、逐点加 cube 约束，是误用：
// 反例:对曲线采样,逐个采样点加 cube 约束(而非用控制点)
for (int j = 0; j < 50; ++j) { // 错:在曲线上采 50 个点
double tau = j / 49.0;
Eigen::VectorXd p = BezierEval(ctrl_points, tau); // 错:p 是控制点关于 tau 的非线性函数
constraints->push_back({A_k, p, b_k}); // 约束数 = 50 × 段数,且 p 对 tau 非线性
}
// 问题:(1) 约束数暴涨,QP 变大变慢;
// (2) 采样点之间仍可能出界(只保证采样点在内,不保证整段);
// (3) 丢掉了"管控制点即管全段"的凸包红利
它放着凸包性质这个免费午餐不用，退回到"采样点逐个检查"——既慢、又不能保证段内无穷多点都安全（采样间隙可能出界）。
Step 4 — 对比。 正确版把安全约束加在有限个控制点上、用凸包性质覆盖整段（约束少、严格、线性）； 错误版在曲线上采样加约束（约束多、不严格、可能漏）。 差别就一句话：你信不信凸包性质？ 信了，就只管控制点； 不信去采样，就同时丢了效率和严格性。
一份能跑的凸包性质验证（无外部依赖）¶
走廊 QP 的求解依赖 OSQP，没法"无依赖跑"。
但它赖以成立的**命门——凸包性质——本身可以独立验证**，而且不需要解任何优化。
下面这份代码亲手把它跑出来：把 4 个控制点都放进 §T2.3 算例的 cube \(C_2=[3,7]\times[0,2]\)，然后在曲线上密采样，检查是否真的"控制点在内 ⇒ 整段在内"。
g++ -std=c++17 即可编译。
// 验证 Bézier 凸包性质:控制点都在 cube 内 ⇒ 曲线上每个采样点都在 cube 内
// 这是 §T2.3 走廊 QP 把安全约束线性化的命门,可独立验证(无需解 QP)
#include <vector>
#include <array>
#include <cmath>
#include <cstdio>
using Pt = std::array<double, 2>; // (s, l)
// n 阶 Bézier 在 tau 处求值(Bernstein 基)
Pt BezierEval(const std::vector<Pt>& ctrl, double tau) {
int n = (int)ctrl.size() - 1;
auto binom = [](int n, int i) { // 二项式系数
double r = 1; for (int k = 0; k < i; ++k) r = r * (n - k) / (k + 1); return r; };
Pt p{0.0, 0.0};
for (int i = 0; i <= n; ++i) {
double b = binom(n, i) * std::pow(1 - tau, n - i) * std::pow(tau, i);
p[0] += b * ctrl[i][0]; p[1] += b * ctrl[i][1];
}
return p;
}
int main() {
const double s_lo = 3, s_hi = 7, l_lo = 0, l_hi = 2; // cube C2(算例"向左凸"那段)
auto in_cube = [&](const Pt& p) {
return p[0] >= s_lo-1e-9 && p[0] <= s_hi+1e-9 &&
p[1] >= l_lo-1e-9 && p[1] <= l_hi+1e-9; };
std::vector<Pt> ctrl = {{3.2,0.1},{4.5,1.9},{6.0,1.8},{6.8,0.3}}; // 4 控制点,全在 cube 内
for (auto& c : ctrl)
if (!in_cube(c)) { std::printf("控制点越界,前提不满足\n"); return 1; }
int bad = 0; // 曲线上密采样 1001 点
for (int j = 0; j <= 1000; ++j)
if (!in_cube(BezierEval(ctrl, j / 1000.0))) ++bad;
std::printf("控制点全在 cube 内;曲线 1001 采样点越界数 = %d\n", bad);
std::printf(bad == 0 ? "凸包性质成立:管住控制点 = 管住整条曲线 ✓\n" : "异常越界\n");
ctrl[1][1] = 3.5; // 反证:把一个控制点挪出 cube(l=3.5>2)
int bad2 = 0;
for (int j = 0; j <= 1000; ++j)
if (BezierEval(ctrl, j / 1000.0)[1] > l_hi + 1e-9) ++bad2;
std::printf("挪一个控制点到 l=3.5(越界)后,曲线越界采样点数 = %d\n", bad2);
return 0;
}
// 运行输出:
// 控制点全在 cube 内;曲线 1001 采样点越界数 = 0
// 凸包性质成立:管住控制点 = 管住整条曲线 ✓
// 挪一个控制点到 l=3.5(越界)后,曲线越界采样点数 = 193
跑完你会**亲眼看到**两件事：控制点都在 cube 内时，曲线上密采样的 1001 个点无一越界——这就是 §T2.3 反复强调的"管住有限个控制点 = 管住曲线上无穷多个点"；
而一旦把某个控制点挪出 cube，曲线立刻有近两百个采样点越界。
这正是走廊 QP 只把约束加在控制点上（而非曲线采样点上）却仍能保证整段安全的全部底气。
把这份验证和走廊 QP 骨架对照：骨架里 A_k c_ki <= b_k 那行约束的有效性，靠的就是这里验证的凸包性质。
⚠️ 常见陷阱¶
💡 编程陷阱：相邻 cube 不重叠或重叠为零 - 错误描述：生成 cube 序列时，相邻两个 cube 恰好边对边相接、重叠区为空（或负），却仍要求轨迹连续穿过。 - 现象与后果：连续性约束与安全约束打架——拼接点必须同时落在两个不重叠的 cube 里，可行域为空，QP 报 infeasible，规划器哑火。 - 根本原因：轨迹从一个 cube 进入下一个，必须在两者的**公共区域**完成交接； 没有重叠就没有合法交接点。 - 正确做法：cube 膨胀时强制相邻 cube 有非空重叠（设最小重叠量），或在生成后检查并修补； 这与 SFC（§T2.4）"重叠多面体序列"的要求完全一致。
⚠️ 概念陷阱：以为 cube 越大越好 - 错误描述：默认把每个 cube 撑得越大，可行空间越大、解越优。 - 现象与后果：大 cube 可能跨越了不该跨的语义边界（比如吃进对向车道），或在动态障碍的时空柱旁贴得过近，导致解出"擦着障碍走"的危险轨迹； cube 间重叠过大还会让走廊失去对轨迹的有效引导。 - 根本原因：cube 是"安全 + 语义"的双重容器，盲目放大会突破语义约束、或贴近动态障碍的时变边界。 走廊的价值在于**恰当地约束**，而非最大化自由。 - 正确做法：cube 膨胀要同时尊重几何无碰撞与语义规则（车道、限速、让行），并对动态障碍留足时变裕度； 大小以"够轨迹平滑通过且不越界"为准。
🧠 思维陷阱：把走廊当成"另一种避障约束"，看不到它在重构问题的凸性 - 错误描述：把"切 cube + 控制点约束"仅仅理解为一种具体的避障写法，没抓住它真正做的事——把非凸优化重构成凸优化。 - 现象与后果：遇到新场景（无人机、机械臂）时想不到迁移这个思想，仍硬上非凸 NLP； 也理解不了为什么 T3、无人机 MINCO（Minimum Control，一种高效的轨迹参数化，T3 详讲）等都绕不开走廊。 - 根本原因：缺少"凸性是可以通过空间分解来人为制造的"这一层抽象。 走廊的本质不是"一种约束"，而是"用前端的凸分解，换后端的凸可解性"。 - 正确做法：把走廊记成一个通用范式——遇到非凸可行域，先想能不能切成凸块再优化。 这个思想在 §T2.4（IRIS/SFC 怎么切）、T3（在走廊里解 NLP）、乃至无人机/机械臂规划里反复出现。
多视角对照：走廊里的轨迹该用什么曲线表示？ 三种常见选择各有取舍。 普通多项式视角：直接用 \(a_0 + a_1 t + \dots + a_n t^n\) 参数化——系数无几何意义，"曲线在 cube 内"无法转成系数的简单线性约束，与走廊配合差，基本不用于走廊方法。 Bézier 视角（SSC）：控制点有清晰几何意义、且有凸包性质——"控制点在 cube 内 ⇒ 整段在内"，安全约束直接线性化； 代价是每段独立、段间连续性要显式加约束。 B-spline 视角：控制点同样有凸包性质，且相邻段天然共享控制点、连续性"免费"获得（少写连续性约束）； 代价是凸包比 Bézier 更宽松（更保守）、局部性与约束写法略复杂。 自驾 SSC 多用 Bézier（每段贴一个 cube，直观），无人机的 MINCO 等则常用 B-spline 或其变体（看重连续性自动满足）。 共同点是都靠凸包性质——这才是走廊方法的命门，具体用哪种曲线是工程偏好。
本质洞察：SSC（和一切走廊方法）做的核心动作，是把"避开障碍"这个**非凸**约束，换成"待在凸 cube 里"这个**凸**约束——代价是引入一步"切 cube"的前处理，红利是后端塌缩成毫秒级可解的 QP。 再加上 Bézier 凸包性质这把钥匙，"管住有限个控制点"就等价于"管住曲线上无穷多个点"，安全约束的数量从无穷压到有限。 凸分解（造凸性）+ 凸包性质（压约束），这两招合起来，就是连续时空规划能实时跑起来的底层秘密。
练习¶
- [实现] 取算例里的三个矩形 cube（\(C_1,C_2,C_3\)），用三段三阶 Bézier 在 OSQP 里解最小 jerk 轨迹，约束=控制点在对应 cube 内 + 段间位置/速度/加速度连续。 验证：在曲线上密集采样 200 个点，全部落在并集 \(C_1\cup C_2\cup C_3\) 内。 再把 \(C_2\) 的 \(l\) 下界从 \(0\) 改成 \(0.5\)，观察轨迹"向左凸"的幅度如何变化。
- [推导] 证明 Bézier 凸包性质：利用 Bernstein 基函数非负且对 \(i\) 求和为 \(1\)，说明 \(\mathbf{p}(\tau)\) 是控制点的凸组合，故整条曲线落在控制点凸包内。 再据此论证"控制点全在凸集 \(C\) 内 ⇒ 曲线在 \(C\) 内"。
- [精读] 按
HKUST-Aerial-Robotics/spatiotemporal_semantic_corridor的corridor_generator思路，标注三件事：(1) 一条语义规则（如红灯停）如何转化成对某个 cube 的约束； (2) 动态障碍如何投影到 \((s,l,t)\) 并被 cube 绕开； (3) 相邻 cube 的 overlap 条件在代码里如何保证。
§T2.4 IRIS / SFC：从地图自动生成凸走廊 ⭐⭐¶
这一节解决什么问题：§T2.3 的 SSC 默认你已经有了 cube 序列，但只字没说这些 cube 从哪来。 手画？ 障碍一多就不现实。 这一节补上走廊线最关键的前处理——怎么从占据栅格/点云，自动"吹"出一串无碰撞凸多面体。 IRIS 用"椭球膨胀 + 超平面切割"的交替优化造单个最大凸域，SFC 沿一条骨架路径把它串成走廊。 这是 §T2.3 那串 cube 的来源，也是无人机/自驾连续规划落地的标准前端。
动机：cube 不会自己出现¶
回到 §T2.3 的算例——我们手写了三个矩形 cube。 可真实场景里，障碍是激光雷达扫出来的一片点云、或一张占据栅格，自由空间形状任意、还在动。 你不可能手画 cube，必须有一个算法：输入地图 + 一个种子点（或一条粗路径），输出一个（或一串）尽量大的、保证无碰撞的凸多面体。 这就是 IRIS/SFC 干的活。 它把 §T2.3 "凸分解"那句口号，变成了一个真能跑的几何算法。
如果不这样做会怎样（反事实）¶
不自动生成走廊，凸分解只能靠手工或固定网格切块。 手工不必说，障碍一复杂就崩。 固定网格切块（把空间均匀切成小立方体、剔除含障碍的）则有两个老毛病：(1) 切得粗会漏掉窄缝、或让 cube 含障碍； 切得细则 cube 数量爆炸，下游 QP 段数随之膨胀。 (2) 网格 cube 是轴对齐的死板形状，贴不紧不规则障碍，自由空间利用率低，逼出过度保守的轨迹。 IRIS 这类方法的价值，正是**让凸域自适应障碍形状、尽量大、且数量少**——把"切多少块、每块多大"从拍脑袋变成了优化决定。
历史：IRIS 的椭球膨胀与 SFC 的走廊串接¶
IRIS（Iterative Regional Inflation by Semidefinite programming）由 Deits 与 Tedrake 在 WAFR 2014 提出（《Computing Large Convex Regions of Obstacle-Free Space Through Semidefinite Programming》，Algorithmic Foundations of Robotics XI，STAR vol. 107，2015 出版）。 注意年份——它常被误记成 RSS 2015，实际是 2014 年 8 月在伊斯坦布尔的 WAFR，论文集次年出版。 IRIS 的机制是两个凸优化交替迭代：(1) 给定当前椭球，解一个 QP 找一组**分离超平面**，把椭球和所有障碍分开，这些超平面的交定义出一个无碰撞凸多面体； (2) 在这个多面体里解一个 SDP（半定规划），找体积最大的内切椭球。 然后拿新椭球回到第 (1) 步，反复膨胀，直到椭球体积增长低于阈值。 环境有界时必收敛（椭球长不出边界），但**不保证找到全局最大的凸域**——它给的是一个局部意义上的大凸域。
SFC（Safe Flight Corridor）由 Liu、Watterson、Mohta、Sun、Bhattacharya、Taylor、Kumar 在 RA-L 2017 提出（《Planning Dynamically Feasible Trajectories for Quadrotors Using Safe Flight Corridors in 3-D Complex Environments》，IEEE RA-L 2(3):1688–1695，2017）。
注意这是 UPenn 的 GRASP 实验室（Vijay Kumar 组），不是 HKUST——开源实现 sikang/DecompROS 也出自第一作者 Sikang Liu。
SFC 的思路：先用一次快速图搜索得到一条**分段线性骨架路径**，然后沿这条骨架，对每一段做凸分解（找无碰撞椭球→膨胀成多面体），相邻多面体重叠，串起来就是一条覆盖骨架、保证无碰撞的"飞行走廊"。
再把 \(t\) 作为第四维，走廊就扩展成 4D 时空多面体——这正好接回 §T2.3 SSC 的 \((s,l,t)\) cube。
一句话：IRIS 造一块，SFC 把它们沿路径串成走廊。
理论：IRIS 的两步交替与走廊串接¶
IRIS 的两步交替（造一块）。 给定种子点和障碍集，初始化一个以种子为中心的小椭球 \(\mathcal{E}\)，然后循环：
- 分离超平面（QP/几何）：对每个障碍，找一个把椭球 \(\mathcal{E}\) 与该障碍分开的超平面（取障碍上离椭球最近点处的切超平面）。 所有这些超平面定义半空间，它们的交是一个无碰撞凸多面体 \(\mathcal{P} = \{\mathbf{x}: A\mathbf{x}\le\mathbf{b}\}\)（椭球在其中、障碍在其外）。 "障碍上离椭球最近点"具体怎么算？ 这一步是个小优化：把椭球用仿射变换"拉正"成单位球（变量代换 \(\mathbf{y} = C^{-1}(\mathbf{x}-\mathbf{d})\)，椭球 \(\mathcal{E}\) 变成原点单位球），在这个变换后的空间里，求障碍上离原点最近的点就是一个简单的最近点投影（障碍是凸的话更是标准凸投影）； 找到最近点 \(\mathbf{y}^*\) 后，过它作垂直于 \(\mathbf{y}^*\) 方向的切平面，再把这个平面用逆变换映射回原空间，就得到分离超平面 \(\mathbf{a}^\top\mathbf{x}\le b\)。 直觉上：在"拉正"的空间里椭球是规整的球，"最近点 + 切平面"才有干净的几何意义； 变换回去后，球的切平面对应椭球的切平面，自然把椭球与障碍分开。 对点云障碍，逐点算距离取最小即可； 对凸多面体障碍，是一个小 QP。
- 最大内切椭球（SDP）：在 \(\mathcal{P}\) 内解 SDP，求体积最大的内切椭球，作为新的 \(\mathcal{E}\)。
- 收敛判据：若新椭球体积相比上一轮增长小于阈值，停； 否则回到第 1 步。
为什么"最大内切椭球"是个半定规划（SDP）？ 这步常被一笔带过，但它是 IRIS 名字里"SDP"的由来，值得点透。 把椭球写成一个仿射映射下的单位球：\(\mathcal{E} = \{C\mathbf{u} + \mathbf{d}: \|\mathbf{u}\|\le 1\}\)，其中对称正定矩阵 \(C\) 刻画椭球的形状与朝向、向量 \(\mathbf{d}\) 是中心。 椭球的体积正比于 \(\det C\)，所以"最大体积"就是最大化 \(\det C\)（等价地最大化 \(\log\det C\)，后者是凹函数，更好优化）。 而"椭球被包在多面体 \(\{\mathbf{x}: \mathbf{a}_j^\top\mathbf{x}\le b_j\}\) 里"这个约束，对每个半空间 \(j\)，可以化成一条关于 \((C, \mathbf{d})\) 的约束：
直觉是：椭球沿 \(\mathbf{a}_j\) 方向最远能伸到中心 \(\mathbf{d}\) 再加上 \(\|C\mathbf{a}_j\|\)（椭球在该方向的"半径"），这个最远点不越过半空间边界 \(b_j\)，椭球就被包住了。 于是整个问题成了"在一组上述约束下最大化 \(\log\det C\)、且 \(C\succeq 0\)"——目标是 \(\log\det\)（凹）、约束含半正定锥 \(C\succeq 0\)，这正是**半定规划**的标准形态。 关键收获不在于会手解 SDP（有成熟求解器），而在于看清："找最大内切椭球"这个听起来很几何的问题，其实是一个有现成高效解法的凸优化——这也是 IRIS 能把"造大凸域"做到毫秒级的根本。
两步都凸、都快，迭代单调把椭球（和对应多面体）撑大。 产出是一个无碰撞凸多面体 \(\mathcal{P}\)，可直接当 §T2.3 的一个 cube。 这套"交替两个凸优化"的结构，和 T1 §T1.6 的 EM 块坐标下降异曲同工——都是把一个难问题拆成两个易解的子问题轮流做，也都只保证局部而非全局最优。
把 IRIS 放进"交替优化"大家族看。 到这里，你应该开始嗅到一个反复出现的味道：T1 的 EM（路径子问题 ⇄ 速度子问题）、本节的 IRIS（切超平面 ⇄ 求椭球），骨子里是同一个套路——把一个难的联合问题，拆成若干个各自好解的子块，固定其余、轮流优化一块。 这个套路有个统一的名字：交替优化（alternating optimization）/块坐标下降。 它的家族成员从轻到重排开：最朴素的是 Gauss–Seidel 式坐标下降（EM 就是）； IRIS 是它在"几何造凸域"上的实例（两块分别是超平面和椭球）； 更讲究的是 ADMM（交替方向乘子法），在分块之上引入对偶变量处理块间耦合约束。 它们共享同一组优点（每步只解一个简单子问题，快而稳）和同一个软肋（只保证收敛到块坐标意义下的驻点，不保证全局最优——IRIS 依赖种子、EM 依赖初值，都是这个软肋的体现）。 看清这一点的好处是：以后再遇到"两个变量纠缠、联合优化很难"的问题，你会本能地想——能不能拆成两块轮流做？ 这把锤子，从 T1 一直敲到这里，后面 T3、多机协调里还会反复见到。
SFC 的走廊串接（造一串）。 1. 骨架路径：用快速图搜索（如 JPS（Jump Point Search，跳点搜索，栅格 A* 的加速变体）、A*）在栅格上找一条从起点到目标的分段线性无碰撞路径 \(\langle p_0\to p_1\to\dots\to p_n\rangle\)。 2. 逐段膨胀：对每条线段，找一个包住它的无碰撞椭球，再膨胀成凸多面体（IRIS 风格，或更轻量的方法）。 3. 重叠保证：相邻多面体要有公共区域（骨架路径的连接点天然落在两段交界，提供重叠种子）——这正是 §T2.3 强调过的"cube 必须重叠"在 SFC 里的来源。 4. 时空扩展：把 \(t\) 当第四维，多面体变 4D，动态障碍按其时空占用切掉对应区域。
"空间走廊"和"时空走廊"差在哪?——这是本章标题的点睛。 值得把这个区别讲透，否则容易把两者混为一谈。 最朴素的 SFC（以及 IRIS）造的是**纯空间走廊**：在 \((x,y,z)\) 或 \((s,l)\) 里切凸多面体，它只回答"哪些位置安全"，默认障碍是静态的、不随时间变。 这对静态环境（已知地图里飞一架无人机）够用，但碰到动态障碍就露怯——一个会动的障碍，在纯空间走廊里要么被当成"永远占据"（过度保守），要么干脆没法表达"它现在在这、待会儿就走了"。 时空走廊**则把时间 \(t\) 升成走廊的一个正式维度：cube 从 \((s,l)\) 的二维矩形变成 \((s,l,t)\) 的三维立方体（SSC），或从 \((x,y,z)\) 变成 \((x,y,z,t)\) 的四维多面体（4D SFC）。 多出来的 \(t\) 维让走廊能表达"同一个位置，在 \(t\in[3,5]\) 被占、其余时间可用"——动态障碍的时空柱被精确地切进走廊形状。 一句话对比：**空间走廊是时空走廊在"障碍静止"假设下的截面； 反过来，时空走廊是空间走廊"长出时间轴"后的版本，这恰好呼应了 §T2.1 里时空格相对纯空间格的那个升维。 本章叫"时空走廊"而非"安全走廊"，强调的正是这个时间维——它才是处理动态场景的关键，也是 T2 区别于纯静态走廊方法的根本。
产出是一串重叠的凸多面体——直接喂给 §T2.3 的 Bézier QP。
一个 IRIS 膨胀的最小走查（两轮，带数字）¶
把"椭球膨胀 + 超平面切割"的交替，在一个能手算的二维例子上走两轮，看凸域怎么长大。
设定：二维平面 \((x, y)\)，自由空间里有两个圆形障碍——障碍 A 圆心 \((4, 0)\)、半径 \(1\)，障碍 B 圆心 \((0, 4)\)、半径 \(1\)。 种子点取原点 \((0, 0)\)（无碰撞）。 初始椭球退化成一个以种子为心的小圆，半径 \(0.5\)。
轮 1。 - 切超平面：对障碍 A，找把小圆与 A 分开的超平面——取 A 圆心方向 \((1,0)\) 上、A 表面靠种子一侧的切线，得到半空间 \(x \le 3\)（障碍 A 左缘在 \(x{=}3\)）。 对障碍 B 同理，得 \(y \le 3\)。 两半空间相交：\(\{x\le 3,\ y\le 3\}\)（再加上一个远处的外包边界防止无界，比如 \(x\ge -5, y\ge -5\)）。 这个多面体无碰撞、包住了种子。 - 求最大内切椭球：在 \(\{x\le 3, y\le 3, x\ge -5, y\ge -5\}\) 里找最大内切椭球。 这个矩形区域偏向第三象限，最大内切椭球（这里退化为椭圆）会从原点向 \(x,y\) 负方向和正方向撑开，但被 \(x{=}3, y{=}3\) 两条边卡住右上角——椭圆中心约移到 \((-1, -1)\) 附近、半轴撑到约 \(4\)。 椭球明显比初始小圆大了。
轮 2。 - 切超平面：拿轮 1 撑大的椭球重新对障碍 A、B 切超平面。 因为椭球现在更大、更贴近障碍，切出的超平面比轮 1 更"紧"——可能不再是简单的 \(x\le 3\)，而是带斜率的切线（椭球离 A 最近的点不再正对圆心）。 新的半空间交出一个更贴合障碍形状的多面体。 - 求最大内切椭球：在新多面体里再求最大内切椭球，体积比轮 1 又大了一些。
收敛：继续迭代，每轮椭球体积增长越来越小，直到增量低于阈值（比如轮 3、轮 4 体积几乎不变），停止。 最终输出那个多面体 \(\{A\mathbf{x}\le\mathbf{b}\}\)——它是一个贴着两个障碍、尽量大的无碰撞凸域，可直接当 §T2.3 的一个 cube。
走查的要点：每一轮的两步都让椭球单调变大，而椭球越大、下一轮切的超平面越贴障碍、多面体越大——这个正反馈把凸域从一个小圆"吹"成贴着障碍的大凸块。 但它停在哪、长成什么形状，取决于种子位置（种子在原点，凸域就偏向远离两障碍的第三象限方向），这正是"IRIS 只保证局部、不保证全局最大"的直观来源。
代码实现（为什么 → 正确 → 错误 → 对比）¶
Step 1 — 为什么这样写。 IRIS 骨架就是把"分离超平面"和"最大内切椭球"两步写成循环，直到椭球体积收敛。 下面用伪代码勾勒，省去 SDP/QP 的具体求解细节（实际调 Mosek/SCS 等）。
// IRIS 单凸域生成骨架(教学示意,非完整实现)
struct Ellipsoid { Eigen::MatrixXd C; Eigen::VectorXd d; // {C u + d : ||u|| <= 1}
double Volume() const; };
struct Polytope { Eigen::MatrixXd A; Eigen::VectorXd b; };// A x <= b
Polytope IrisInflate(const Eigen::VectorXd& seed,
const std::vector<Obstacle>& obstacles,
int max_iter = 20, double tol = 1e-3) {
Ellipsoid ellipsoid = InitSmallBall(seed); // 以种子为心的小球
Polytope polytope;
double prev_volume = 0.0;
for (int iter = 0; iter < max_iter; ++iter) {
// 步骤1:为每个障碍找分离超平面,交成无碰撞多面体 A x <= b
std::vector<Eigen::VectorXd> A_rows; std::vector<double> b_vals;
for (const auto& obs : obstacles) {
auto [a_i, b_i] = SeparatingHyperplane(ellipsoid, obs); // 椭球与障碍间的切超平面
A_rows.push_back(a_i); b_vals.push_back(b_i);
}
polytope = AssemblePolytope(A_rows, b_vals);
// 步骤2:在多面体内解 SDP,求最大体积内切椭球(实际调 Mosek/SCS 等)
ellipsoid = MaxInscribedEllipsoidSDP(polytope);
// 步骤3:体积增长低于阈值则收敛
double vol = ellipsoid.Volume();
if (vol - prev_volume < tol) break;
prev_volume = vol;
}
return polytope; // 一个无碰撞凸 cube
}
Step 2 — 正确要点。 分离超平面要取在障碍**离椭球最近点**处，才能让多面体贴得紧、体积大； 种子点必须无碰撞（否则第一步就找不出合法超平面）； 收敛判据用体积增量而非固定迭代数，避免该停不停或该继续却早停。 SFC 串接时，务必让相邻多面体重叠（用骨架连接点当共享种子），否则下游 QP 在接缝处无解（§T2.3 那个 cube 不重叠的陷阱）。
Step 3 — 一个常见的错误写法。 把 IRIS 退化成"只切一刀不迭代"：
// 反例:不迭代,只用初始小球切一次超平面就当最终走廊
Polytope IrisWrong(const Eigen::VectorXd& seed,
const std::vector<Obstacle>& obstacles) {
Ellipsoid ball = InitSmallBall(seed);
std::vector<Eigen::VectorXd> A_rows; std::vector<double> b_vals;
for (const auto& obs : obstacles) {
auto [a_i, b_i] = SeparatingHyperplane(ball, obs); // 只基于初始小球切一次
A_rows.push_back(a_i); b_vals.push_back(b_i);
}
return AssemblePolytope(A_rows, b_vals); // 没膨胀,多面体小得可怜
}
// 问题:初始小球很小 → 切出的多面体也很小 → 走廊过窄
// → 下游 QP 可行空间被严重压缩,轨迹被迫贴着种子点走,失去优化余地
不迭代膨胀，多面体就停在初始小球的尺度，走廊窄得没法用——IRIS 的全部价值在"膨胀"二字，省了它就只剩个空壳。
Step 4 — 对比。 正确版交替膨胀、把凸域撑到贴近障碍的最大； 错误版只切一刀、凸域停在小球尺度。 差别是走廊宽窄，直接决定下游 QP 有多大优化余地。
⚠️ 常见陷阱¶
💡 编程陷阱：种子点落在障碍内或贴边 - 错误描述：从一条粗路径取种子点时，没检查它是否无碰撞，或它恰好贴在障碍表面。 - 现象与后果：分离超平面步找不出能把"含碰撞的椭球"与障碍分开的平面，IRIS 直接失败或产出退化（零体积）多面体。 - 根本原因：IRIS 的前提是种子无碰撞——它是从一个"安全的核"往外膨胀，核本身不安全则整个膨胀无从谈起。 - 正确做法：取种子前做无碰撞检查并留安全裕度； 若种子来自骨架路径，先确保骨架本身无碰撞（SFC 第一步的图搜索就该保证这点）。
⚠️ 概念陷阱：把 IRIS/SFC 的凸域当成全局最大无碰撞凸区 - 错误描述：以为 IRIS 给出的是该位置能取到的最大凸无碰撞区域。 - 现象与后果：对走廊保守性产生错误预期，调试时百思不解"明明还有更大的空间，为什么走廊没覆盖"。 - 根本原因：IRIS 是**局部**优化（交替两步、单调膨胀），结果依赖种子位置和迭代路径，只保证收敛、不保证全局最大——和 EM 一样停在局部最优。 - 正确做法：把 IRIS 的输出理解为"一个够大的局部凸域"，需要更大覆盖时换种子重跑、或沿路径多放几个种子（SFC 的做法），而非指望一次得到全局最优。
🧠 思维陷阱：把"生成走廊"与"在走廊里优化"混为一谈 - 错误描述：把走廊生成（IRIS/SFC，几何前处理）和走廊内轨迹优化（Bézier QP，§T2.3）当成一回事，分不清职责。 - 现象与后果：调参时张冠李戴——轨迹不够平滑去改 IRIS 阈值，走廊太窄却去调 QP 权重，越调越乱。 - 根本原因：没建立"前端造可行域、后端在可行域里求最优"的两段式心智模型。 走廊形状是 IRIS/SFC 的事，轨迹平滑是 QP 的事。 - 正确做法：明确分工——走廊太保守/太窄是生成阶段的问题（种子、膨胀、阈值）； 轨迹抖动/顿挫是优化阶段的问题（jerk 权重、连续性阶数）。 先定位问题属于哪一段，再动对应旋钮。
多视角对照：IRIS 的凸分解，从三个角度看是三件不同的事，合起来才完整。 几何视角：它在"切积木"——把形状任意的自由空间，切成一块块规整的凸多面体，像把不规则房间用若干矩形家具填满。 关注的是覆盖与形状。 优化视角：它在"造凸性"——自由空间非凸、优化难解，凸分解人为制造出凸的子区域，把后端从非凸 NLP 解放成凸 QP。 关注的是可解性。 感知视角：它在"把点云翻译成约束"——激光雷达扫出的一堆点/占据栅格，本身不能直接进优化器； IRIS 把它们转成 \(A\mathbf{x}\le\mathbf{b}\) 这种优化器吃得下的线性约束。 关注的是从感知到规划的接口。 同一个算法，几何看是切积木、优化看是造凸性、感知看是建接口——哪个视角都不全错，但只盯一个就会偏：只看几何会忘了它服务于优化、只看优化会忘了输入是带噪点云。 三视角合起来，你才真正理解 IRIS 在整条感知-规划管线里的位置。
本质洞察：IRIS/SFC 解决的是 §T2.3 刻意跳过的问题——凸域不会从天而降，得有人把它从地图里"长"出来。 而长的方式（椭球膨胀 + 超平面切割的交替）再次印证了一个贯穿规划全书的套路：把一个难的几何问题（找最大无碰撞凸区）拆成两个易解的凸子问题轮流做。 从 T1 的 EM、到这里的 IRIS，"交替优化两个简单子问题"这个锤子，敲开了一个又一个本来难啃的钉子。
练习¶
- [实现] 在 2D 占据栅格上实现简化 IRIS：给一个无碰撞种子点和若干圆形障碍，交替做"切分离超平面→求最大内切椭球（2D 退化为最大内切圆/椭圆）"，可视化每轮膨胀。 对比迭代 1 次 vs 收敛后的凸域面积。
- [推导] 说明为什么 IRIS 的两步都是凸优化：分离超平面步为何可化为凸问题，最大内切椭球为何是 SDP（提示：椭球体积的对数与矩阵行列式、半定约束的关系）。 再论证环境有界时迭代必收敛。
- [精读] 按
sikang/DecompROS的思路，梳理 SFC 从一条骨架路径生成走廊的完整流程，标注：(1) 椭球如何沿线段初始化； (2) 多面体如何膨胀； (3) 相邻多面体的重叠靠什么保证。
§T2.5 走廊 vs 搜索：互补、组合与选型 ⭐⭐¶
这一节解决什么问题：前四节给了两套工具——搜索线（时空格、SIPP）产出离散最优解，走廊线（SSC、SFC）产出连续凸可行域。 它们不是竞争关系，而是各擅一端、还能联手。 这一节把"什么时候用哪个、怎么组合"讲清楚，免得你拿着锤子找钉子。
两条线的本质差异¶
回看全章，两条线的分野其实在一个根本问题上：你要的是"离散决策"还是"连续轨迹"？
- 搜索线（时空格、SIPP）**在离散化的状态空间里找最优路径。 它的强项是**全局性与组合性——在离散空间里能给出全局最优、能干净地表达"抢/跟""走哪个 gap"这类离散选择、能自然扩展到多智能体的组合冲突消解（CBS 给某 agent 加约束再用 SIPP 重搜）。 代价是离散分辨率绑死了解质量与计算量（§T2.1 思维陷阱），且输出是棱角分明的离散路径，需要后续平滑。
- 走廊线（SSC、SFC）**在连续空间里解凸优化。 它的强项是**连续性与可优化性——直接产出平滑、动力学可行的轨迹，毫秒级 QP 可解，天然贴合单体的精细轨迹生成。 代价是只在走廊内局部最优（走廊一旦切定，就出不去那个 homotopy 类了），且对"走哪条路"这类离散决策无能为力——它假设决策已由前端（走廊形状）定好。
一句话对照：搜索擅长"决定走哪"，走廊擅长"把选定的路走平滑"。 这恰好是 T1 §T1.4/§T1.5 那个"DP 定决策、QP 精修"分工的放大版——只不过这里 DP 升级成了时空搜索，QP 升级成了走廊优化。
一张选型对照表¶
| 维度 | 时空走廊（SSC / SFC） | 时空搜索（Lattice / SIPP） |
|---|---|---|
| 输出 | 连续凸可行域 → QP/NLP 后端 | 离散最优路径 |
| 最优性 | 走廊内局部最优 | 离散空间内全局最优 |
| 决策表达 | 由走廊形状隐式编码（前端定） | 搜索显式给出（抢/跟/走哪 gap） |
| 动态障碍 | 编码为 cube/多面体的时变边界 | 编码为安全区间 / 禁用时空节点 |
| 多智能体 | 走廊互斥约束（EGO-Swarm、MADER） | CBS/ECBS/LaCAM 组合搜索 |
| 平滑性 | 直接产出平滑轨迹 | 需后续平滑（除非格本身含动力学基元） |
| 计算代价 | 走廊生成 + QP（毫秒级） | 图搜索（毫秒-秒级，随分辨率） |
| 典型场景 | 自驾/无人机连续轨迹优化 | MAPF/仓储离散路径、多机协调 |
复杂度对照（算法工程视角）¶
选型不只看功能，也看代价。 把本章几个核心算法的复杂度并排，心里才有数：
| 算法 | 时间复杂度 | 空间复杂度 | 复杂度的主导因素 |
|---|---|---|---|
| 时空格搜索 | \(O(\lvert V\rvert \log\lvert V\rvert)\)（A* on DAG，$\lvert V\rvert = $ 状态数） | \(O(\lvert V\rvert)\) | 状态数 \(\propto\) 构型格数 \(\times\) 时间层数 \(\times\) 基元分支，分辨率连乘 |
| 时间展开图 | \(O(N_{\text{cell}} \cdot T/\Delta t \cdot \log(\cdot))\) | \(O(N_{\text{cell}} \cdot T/\Delta t)\) | 时间层数 \(T/\Delta t\)，分辨率一细就暴涨 |
| SIPP | \(O(N_{\text{cell}} \cdot k \cdot \log(\cdot))\)，\(k\) = 平均安全区间数 | \(O(N_{\text{cell}} \cdot k)\) | 安全区间数 \(k\)（≈ 障碍经过次数 +1），与时间分辨率无关 |
| IRIS（单凸域） | 每轮 \(O(\text{SDP})\) + \(O(N_{\text{obs}})\)，迭代数小常数 | \(O(N_{\text{obs}})\) | SDP 规模（椭球矩阵维度）与障碍数 |
| 走廊 Bézier QP | \(O(\text{QP})\)，变量数 = 段数 \(\times\) 控制点数 | 同变量数 | cube 段数 \(\times\) 每段控制点数，通常很小 |
这张表把全章的"为什么"量化了：SIPP 之于时间展开图的优势，就写在那个 \(k\) 取代 \(T/\Delta t\) 上——前者是与场景复杂度挂钩的小常数、后者是被分辨率绑架的大数（§T2.2 反复强调的那点，这里有了复杂度的形式）。 而走廊线（IRIS + QP）的复杂度几乎只取决于障碍数和段数、与"时间/空间分辨率"无关——这正是连续方法相对离散搜索的根本优势：它不靠离散网格，自然摆脱了分辨率对复杂度的连乘式放大（§T2.1 思维陷阱"分辨率越细越好"的反面）。 选型时，离散网格规模大就警惕搜索线的爆炸、障碍极多就留意走廊生成的开销。
落到实测：这些方法到底多快¶
复杂度是渐近量级，工程上还得看真实毫秒数。 把几个有公开数据的系统摆出来，建立"够不够实时"的直觉。 Apollo 的运动规划对实时性的硬要求是：紧急情况下整个系统须在 \(100\,\text{ms}\) 内反应（作为对比，普通人类司机的反应时间约 \(300\,\text{ms}\)），规划轨迹至少覆盖 \(8\,\text{s}\) 或 \(200\,\text{m}\) 的视野——这是给车辆动力学留足余地。 HKUST 的 EPSILON（SSC 所在的完整系统）公布过分层耗时：行为规划层每个周期约 \(5\)–\(20\,\text{ms}\)，基于 SSC 的运动规划层再加约 \(10\)–\(25\,\text{ms}\)，合起来在几十毫秒量级，足以支撑动态驾驶所需的重规划频率。 时空格这边，Ziegler-Stiller 在 2009 年的车载硬件上就把规划压到 \(20\,\text{ms}\) 以内（§T2.1 历史）； SIPP 因状态数小，单次搜索通常也在毫秒级。 把这些数字和复杂度表对照，能读出一个一致的结论：本章这几类方法都设计为毫秒到几十毫秒量级，正好卡在自驾/无人机"几十赫兹重规划"的预算内——这不是偶然，而是它们能进产品栈的前提。 一旦某个环节（比如走廊生成在障碍极多时、或搜索在分辨率过细时）突破这个预算，工程上就必须降分辨率、加并行（CMU 的 GPU 时空格、SFC 各段并行膨胀），或换更轻量的变体（SUPER 的 FIRI 之于 IRIS）。 实时预算是悬在所有规划方法头上的硬约束，复杂度分析的最终落脚点，永远是"它能不能在一个控制周期内算完"。
组合：搜索 warm-start 走廊¶
两条线最常见的联手方式，是**搜索给粗解、走廊给精修**：
- 先用时空搜索（SIPP、时空格、或 ST 图 DP）快速得到一条离散粗解——它定下了关键的离散决策（抢还是跟、走哪个 gap、何时该等）。
- 沿这条粗解切走廊（SFC/IRIS 用粗解当骨架路径，§T2.4 正是这么干的），把粗解所在的那个 homotopy 类包成凸走廊。
- 在走廊里解 QP/NLP，把棱角分明的粗解精修成平滑、动力学可行的轨迹。
这套组合的精妙在于**各取所长、各避其短**：搜索负责"跳出局部、定对决策"（走廊优化做不到的），走廊负责"在选定决策内求最优平滑"（搜索给不出的）。 SUPER（§T2.4 前沿）、MADER、EGO-Swarm 等先进系统，骨子里都是这个"搜索定 homotopy、优化求精修"的两段式。 它也正是 T3 时空轨迹优化的起点——T3 会把这里的"走廊内 QP"换成更强的"走廊内 NLP"，处理非线性动力学。
一个选型小例子¶
落到具体判断。 三个场景，分别该用哪条线？
- 单车在城市道路绕行一辆抛锚车：连续轨迹、单体、要平滑 → 走廊线（SSC，正是它的主场），决策（往左绕）由语义+几何切进 cube 形状。
- 仓储里 50 台 AGV 互不相撞地各自送货：离散网格、多智能体、组合冲突 → 搜索线（SIPP + CBS/LaCAM），连续优化在这种规模和组合性下反而累赘。
- 无人机高速穿越未知森林：连续轨迹、单体、但要先在杂乱障碍中定出"从哪个缝隙穿"→ 组合（搜索定缝隙/homotopy + 走廊 QP 精修），SUPER 就是这条路。
判断的核心永远是那两个问题：要离散决策还是连续轨迹？ 是单体精修还是多体组合？ 答清这两问，选型自明。
一个可操作的选型决策流程¶
把上面的判断固化成一棵决策树，落地时照着走：
问题 1:你要的输出是"离散决策"还是"连续可执行轨迹"?
│
├─ 主要要离散决策(走哪条/谁先谁后/可后续平滑)
│ │
│ └─ 问题 2:单体还是多体组合?
│ ├─ 多体(多 agent 互不碰) → SIPP + CBS/ECBS/LaCAM(搜索线)
│ └─ 单体但需先定 homotopy → 时空格 / SIPP 出粗解
│
└─ 主要要连续平滑轨迹(直接给底盘/飞控执行)
│
└─ 问题 3:决策(走哪个 homotopy)是否已经明确?
├─ 已明确(场景简单,语义已定向) → 直接 SSC/SFC 走廊 + QP(走廊线)
└─ 不明确(杂乱障碍,需先选缝隙) → 组合:搜索定 homotopy → 走廊 QP 精修
这棵树的三个分叉问题，正对应全章的三组工具：多体组合 → 搜索线（SIPP 的主场，喂 CBS）； 单体且决策已明 → 走廊线（SSC 的主场，直接 QP）； 单体但决策未定 → 两段式组合（搜索定 homotopy + 走廊精修，SUPER/MADER 的路子）。 绝大多数真实自驾/无人机场景落在最后一类——这也是为什么"组合"是主流，而纯搜索、纯走廊各自只在两端的极端场景里单独出现。
⚠️ 常见陷阱¶
⚠️ 概念陷阱：以为走廊优化能"跳出"走廊找到更好的解 - 错误描述：期待在走廊里解 QP 能自动发现"其实从另一侧绕更好"。 - 现象与后果：走廊一旦把轨迹框进了"向左绕"的 homotopy 类，QP 无论怎么优化都跳不到"向右绕"——你以为优化器会全局比较，实际它只在走廊内局部最优。 - 根本原因：走廊把可行域限定在一个凸管道内，凸优化只在这个管道里找最优，跨 homotopy 的全局比较是**前端搜索**的职责，不是后端优化的。 - 正确做法：把"走哪个 homotopy"交给搜索（或多走廊并行优化后择优），走廊优化只负责选定 homotopy 内的精修。 指望 QP 做全局决策，是混淆了两条线的分工。
🧠 思维陷阱：把两条线看成"二选一"的对立 - 错误描述：学完两条线后，遇到问题先纠结"该用搜索还是走廊"，把它们当互斥选项。 - 现象与后果：要么硬用搜索去做本该连续优化的精修（解粗糙），要么硬用走廊去做本该离散搜索的决策（陷局部），错过了二者组合的最优解法。 - 根本原因：缺少"前端搜索定决策、后端优化求精修"的流水线视角，把工具看成竞品而非工序。 - 正确做法：默认想"能不能组合"——多数先进系统正是搜索 + 走廊的两段式。 只有在极端场景（纯多机组合、或纯单体平滑）才退化成单用一条线。
本质洞察：走廊与搜索不是竞争，是**分工**——搜索解决"走哪条路"（离散、全局、组合），走廊解决"把这条路走平滑"（连续、局部、可优化）。 它们的组合（搜索定 homotopy + 走廊精修）是当今时空规划的主流骨架，也是 T1 "DP 定决策、QP 精修"那个分工在更高维时空的自然延续。 看清这一点，你就不会再问"用哪个"，而会问"怎么把两个串起来"。
练习¶
- [设计] 为"无人机穿越有 3 个缝隙的墙"设计一个搜索 + 走廊的两段式管线：搜索层选哪个缝隙（homotopy），走廊层在选定缝隙里精修。 画出数据流，标明两层的接口（搜索给走廊什么、走廊回什么）。
- [对比] 同一个"绕抛锚车"场景，分别用纯 SIPP（离散）和纯 SSC（走廊）求解，对比输出形态（离散路径 vs 平滑轨迹）、最优性范围（全局 vs 走廊内）、与执行的距离（要不要平滑）。
- [跨章综合题] T1 §T1.4/§T1.5 是"ST 图 DP 定决策 + speed QP 精修"，本章 §T2.5 是"时空搜索定 homotopy + 走廊 QP 精修"。 请论证这两者是同一范式在不同维度的体现，并指出从 T1 到 T2 这个范式"升维"时，DP 和 QP 各自被替换成了什么、为什么要替换。
§T2.6 完整管线实战：动态绕障从感知到时空轨迹 ⭐⭐¶
这一节解决什么问题：把全章六节的零件串成一条能跑的管线，用一个带数字的动态绕障场景，走完"感知输入 → 搜索定决策 → 切走廊 → QP 精修"的全流程，让你看清每个零件在哪一环、接口是什么。
场景设定（带数字，便于追踪）¶
一条双车道直路，自车沿参考线行驶。 前方 \(30\,\text{m}\) 处有一辆**抛锚车**静止占住右半车道（\(l\in[-2, 0]\)，全程不动）。 同时，一辆**对向慢车**正从前方驶来，在 \(t\in[3, 5]\,\text{s}\) 期间会经过自车左前方的某段（占据 \(l\in[1, 2]\)、\(s\in[40, 50]\)）。 自车初速 \(8\,\text{m/s}\)，要平滑绕过抛锚车、同时不与对向车在那个时间窗冲突。 这正是一个"路径与速度强耦合"的场景——往左绕的时机和速度，受对向车时间窗牵制，T1 解耦会失真，必须上时空联合。
先看 T1 解耦为什么在这里失真¶
在动手跑时空管线前，值得用这个场景的数字把"解耦失真"坐实，否则你不会真心觉得需要升级。 回忆 T1 的解耦顺序：先在 SL 图上定路径，再在 ST 图上排速度。 第一步定路径时，规划器只看静态/准静态障碍——它看到右车道被抛锚车堵死，于是定出一条"向左绕到 \(l\approx 1.5\)、再回正"的路径。 注意此刻它**还不知道对向车的时间窗**（对向车是动态障碍，按解耦约定要等速度规划阶段才处理）。 问题就出在这：这条向左绕的路径，恰好要经过 \(l\in[1,2]\)、\(s\in[40,50]\) 这片区域——而对向车在 \(t\in[3,5]\) 正占着这里。 等到第二步速度规划，ST 图才发现：沿着这条**已经定死**的左绕路径，在 \(s\in[40,50]\) 处撞上了对向车的时间窗。 此时速度规划只有两条糟糕的出路：要么**急加速**抢在 \(t{=}3\) 前冲过 \(s{=}50\)（若初速 \(8\,\text{m/s}\) 不够，需要的加速度可能超过舒适甚至能力上限），要么**急减速**等到 \(t{=}5\) 后再过（但路径已经切到左侧 \(l{=}1.5\)，减速时车还斜在对向车道侧，既危险又别扭）。 根子在于：路径是在"不知道对向车时机"的前提下定死的，速度规划再怎么补救都是戴着镣铐跳舞。 真正的好解，是让"往左绕的横向时机"和"纵向加减速"一起决定——比如"先不急着切左，等对向车过了再切"，或"要切就早切、同时加速抢在前面"。 这种横纵联合的决策，解耦的两步式结构表达不了，必须把 \(l\) 和 \(t\) 放进同一个搜索/优化里——这就是下面时空管线要做的。
数据流与各环职责¶
[感知/预测]
静态障碍:抛锚车 l∈[-2,0](全程)
动态障碍:对向车 (s∈[40,50], l∈[1,2], t∈[3,5])
│
▼
[Step 1 时空搜索定决策](§T2.1 / §T2.2)
在 (s,l,t) 里搜粗解:往左绕(避抛锚车),且避开对向车时间窗
→ 决策:向左绕,且在 t∈[3,5] 前/后通过 s∈[40,50](定 homotopy)
│ 离散粗解(一串 (s,l,t) 路点)
▼
[Step 2 切时空走廊](§T2.4 SFC/IRIS)
以粗解为骨架,沿途膨胀出无碰撞 cube 序列
抛锚车把 cube 的 l 上界压到 >0(逼轨迹左移)
对向车的时空柱(t∈[3,5])把对应 cube 在那段时间收窄
│ 一串重叠的 (s,l,t) 凸 cube
▼
[Step 3 走廊内 QP 精修](§T2.3 SSC)
Bézier 控制点约束在 cube 内(凸包性质保证整段安全)
最小化 jerk + 偏离参考,段间连续到二阶
│ 平滑、动力学可行的时空轨迹 (s(t), l(t))
▼
[输出] 交给跟踪控制器执行
整合伪代码¶
// 动态绕障完整管线(教学示意,把全章零件串起来)
// 返回 std::optional:无解时为 std::nullopt,交上层降级
std::optional<Trajectory> PlanSpatiotemporal(
const ReferenceLine& reference_line,
const std::vector<Obstacle>& static_obs,
const std::vector<Obstacle>& dynamic_obs,
const State& ego_state) {
// Step 1: 时空搜索定决策(定 homotopy:往哪绕、与动障的先后)
// (§T2.1 时空格 或 §T2.2 SIPP,视离散化方式)
auto coarse = SpatiotemporalSearch(reference_line, static_obs, dynamic_obs, ego_state);
if (!coarse) return std::nullopt; // 搜索无解 → 该减速/停车或上报
// Step 2: 沿粗解切时空走廊(§T2.4 SFC/IRIS)
// 粗解当骨架,逐段膨胀出无碰撞 cube,动障按时空占用收窄对应 cube
std::vector<Cube> cubes =
BuildSpatiotemporalCorridor(*coarse, static_obs, dynamic_obs);
// Step 3: 走廊内 Bézier QP 精修(§T2.3 SSC)
// 控制点约束在 cube 内 + 段间连续 + 最小 jerk
Eigen::SparseMatrix<double> P;
std::vector<LinearConstraint> constraints;
BuildCorridorQp(cubes, /*n_order=*/3, &P, &constraints);
Trajectory trajectory = SolveQpOsqp(P, constraints); // 平滑时空轨迹
return trajectory; // (s(t), l(t)):平滑、安全、动力学可行
}
走一遍这个场景¶
跟着数字过一遍。 Step 1 的搜索在 \((s,l,t)\) 里发现：右车道被抛锚车全程堵死，只能往左绕； 而左前方 \(s\in[40,50]\) 在 \(t\in[3,5]\) 被对向车占用，于是搜索给出的粗解是"提前加速、在 \(t{=}3\) 之前就通过 \(s{=}50\)"（抢在对向车时间窗之前），或"减速、等 \(t{=}5\) 之后再过"（让过时间窗）——两个 homotopy，搜索按代价选了更优的一个（假设抢在前面 jerk 更小，选它）。 Step 2 沿这条"提前加速左绕"的粗解切 cube：抛锚车段的 cube 把 \(l\) 上界顶到 \(0\) 以上（强制左移），对向车时间窗对应的 cube 在 \(t\in[3,5]\) 把 \(s\) 上界放宽（因为粗解选择在那之前通过，cube 允许更大的 \(s\)）。 Step 3 在这串 cube 里解 Bézier QP，得到一条平滑左绕、提前加速、在对向车到来前完成超越的连续轨迹——既避了静态抛锚车、又避开了动态对向车的时间窗，且乘客感受到的是一次平顺的变道加速，而非顿挫。
注意全程的分工：决策（抢还是让、往哪绕）在 Step 1 的搜索里定，安全凸域在 Step 2 的走廊里造，平滑轨迹在 Step 3 的 QP 里求。 三环各司其职，正是 §T2.5 那个"搜索定 homotopy、走廊精修"范式的完整落地。
为什么这三步的顺序不能颠倒？ 理解了顺序的必然性，才算真懂这条管线，而不是死记流程。 搜索必须在走廊之前：走廊是沿着某条粗解切出来的（§T2.4 SFC 用粗解当骨架），没有粗解就无从知道该往哪切、切哪个 homotopy——你不可能凭空切一条"向左绕"的走廊，除非搜索先告诉你"该向左绕"。 颠倒过来"先切走廊再搜索"是无源之水。 走廊必须在 QP 之前：QP 需要凸可行域才能毫秒级求解（§T2.3 反复强调凸性是前提），而原始自由空间非凸； 走廊正是把非凸切成凸的那一步。 没有走廊，QP 直接面对非凸避障，要么不收敛要么陷劣解。 颠倒过来"先 QP 再切走廊"则 QP 根本立不起来。 三步是一条信息逐级精化的链：搜索给出粗糙但定对了大方向的离散决策（哪个 homotopy）→ 走廊把这个决策固化成一个凸的安全管道（在哪个范围内安全）→ QP 在管道里求出精确平滑的轨迹（具体怎么走最优）。 每一步都消费上一步的产出、为下一步铺垫，信息从"粗决策"逐级精化到"精轨迹"。 这也是为什么搜索的离散分辨率不拖累最终轨迹（§T2.5 那个 FAQ）——粗的归粗、精的归精，各管一段。
这三步在实时预算里怎么分？ 把 §T2.5 的实测数字落到这条管线上，能建立"时间花在哪"的直觉。 以 Apollo 那个 \(100\,\text{ms}\) 紧急反应、EPSILON 那个几十毫秒重规划周期为参照，一个典型的时空规划周期里：搜索定 homotopy 通常占几毫秒（离散粗搜，状态少）； 走廊生成（沿粗解逐段膨胀）占几到十几毫秒，是三步里相对重的一环，尤其障碍多时； 走廊内 QP 精修占几毫秒（凸 QP，OSQP 毫秒级）。 合起来落在十几到几十毫秒，留出余量给感知、预测、控制等其余模块，整体卡进控制周期。 这个分配也解释了工程上的优化重心：当周期超预算，第一个要开刀的往往是走廊生成（并行各段膨胀、降种子密度、用 FIRI 这类更快的变体），而不是去抠搜索或 QP——因为后两者本就轻。 反过来，若搜索成了瓶颈（离散分辨率过细、或多智能体组合爆炸），那说明问题规模超出了"先搜后优"两段式的舒适区，可能需要换思路（比如直接在连续空间做带拓扑引导的优化，正是 T3 的方向）。 把每一步的耗时摊在预算表上，是把这条管线真正部署到车上的第一课。
渐隐练习（Faded Worked Example）¶
照着上面的完整走查，自己补全下面三个变体的关键环节（提示已给，逐步减少）：
- 变体 A（提示较多）：把对向车时间窗改成 \(t\in[1, 6]\)（窗口变长，几乎堵死"抢在前面"）。 Step 1 的搜索这次会选哪个 homotopy？ （提示：抢在 \(t{=}1\) 前通过 \(s{=}50\) 需要初速远超 \(8\,\text{m/s}\)，不现实 → 搜索只剩"等过去"这一支。） Step 2 的 cube 会如何变化？ （提示：对应时间段的 cube 收窄，逼轨迹……）
- 变体 B（提示中等）：再加一辆后车紧跟自车，限制自车减速幅度。 这对 Step 1 的"等过去"方案有何影响？ 走廊会在哪个维度被进一步约束？
- 变体 C（提示最少）：若 Step 1 搜索返回
std::nullopt（两个 homotopy 都不可行——既抢不过、又因后车不能等），管线应如何降级处理？ 这暴露了解耦/联合规划共同的什么边界？
本章常见误解汇总¶
把全章六节散落的"想当然"集中列出，临考/上手前扫一眼，每条都对应正文里详谈过的陷阱。
| 误解 | 实情 | 出处 |
|---|---|---|
| ST 图已经处理了时间，时空规划是多余的 | ST 图把时间藏在速度一维里、且路径已定；强耦合场景下"走哪"与"何时"纠缠，必须把时间升为独立搜索维 | §T2.1 动机 |
| 时空格只是"维度更高的纯空间格" | 时间维单调（带来 DAG）且改变占用语义（占用是时间的函数），这两点纯空间格都没有 | §T2.1 概念陷阱 |
| 碰撞检查查运动基元末点就够了 | 一段 Δt 的轨迹连续，碰撞可能发生在区间内部；只查端点会"穿模" | §T2.1 编程陷阱 |
| SIPP 适用于任何动态环境 | SIPP 的最优性建立在"可原地等待 + 瞬时启停"假设上，真车不满足，只能当离散粗搜 | §T2.2 概念陷阱 |
| SIPP 取每个格子第一个安全区间即可 | 丢掉后续区间 = 丢掉所有"等障碍过去再走"的解，会误报无解 | §T2.2 编程陷阱 |
| 走廊就是"另一种避障约束" | 走廊真正做的是把非凸可行域重构成凸——用前端凸分解换后端凸可解性 | §T2.3 思维陷阱 |
| Bézier 曲线要逐采样点加 cube 约束 | 凸包性质保证"控制点在 cube 内 ⇒ 整段在内"，只需管有限个控制点 | §T2.3 编程陷阱 |
| cube 越大越好 | cube 是"安全+语义"双重容器，盲目放大会越语义边界、贴近动障时变边界 | §T2.3 概念陷阱 |
| IRIS 给的是全局最大无碰撞凸区 | IRIS 是局部优化（交替两步、单调膨胀），只保证收敛、依赖种子，非全局最优 | §T2.4 概念陷阱 |
| 走廊优化能"跳出"走廊找更好的解 | 走廊把轨迹框进一个 homotopy，凸优化只在管道内局部最优；跨 homotopy 是搜索的职责 | §T2.5 概念陷阱 |
| 走廊和搜索是二选一的竞品 | 二者分工互补（搜索定决策、走廊精修），主流系统是两段式组合 | §T2.5 思维陷阱 |
本章小结¶
本章把"时间"从 T1 ST 图里那条被动的速度维，提升为时空规划里主动的搜索/优化维度，沿两条线展开又合流。 搜索线：时空格搜索（§T2.1）在"构型×时间"的 DAG 上找离散最优轨迹，时间单调带来无环结构、动障靠带时间戳禁用节点； SIPP（§T2.2）用"安全区间"把时间轴的冗余压扁，把动态避障变回近乎静态的图搜索，成为多机 CBS 的低层基石。 走廊线：SSC（§T2.3）用 Bézier 凸包性质把非凸避障线性化成"控制点在 cube 内"的 QP； IRIS/SFC（§T2.4）从地图自动膨胀出无碰撞凸多面体走廊，回答了 cube 的来源。 合流：§T2.5 厘清两条线"搜索定决策、走廊精修"的分工与组合，§T2.6 用动态绕障实战把全部零件串成管线。 贯穿全章的，是 T1 "DP 定决策、QP 精修"那个分工在更高维时空的升级版。
把全章的脉络收成一张图，两条线如何从 T1 长出、又如何合流，一目了然：
T1 ST 图(时间藏在速度一维,路径已定)
│ 强耦合 → 时间升为独立维
▼
┌───────────────┴───────────────┐
搜索线(离散最优) 走廊线(连续凸优化)
│ │
§T2.1 时空格搜索 §T2.3 SSC 语义走廊
(构型×时间 DAG,A*) (cube 序列 + Bézier 凸包 → QP)
│ │
§T2.2 SIPP §T2.4 IRIS / SFC
(安全区间压缩时间轴) (从地图膨胀出凸走廊)
│ │
产出:离散最优路径 产出:连续凸可行域
(喂多机 CBS → T5) (喂 QP/NLP → T3)
└───────────────┬───────────────┘
▼
§T2.5 互补与组合 / §T2.6 完整管线
搜索定 homotopy(离散决策) + 走廊精修(连续轨迹)
│
▼
T3 走廊内 NLP / Part-U 不确定性 / Part-G 博弈
术语速查¶
| 术语 | 英文 | 一句话定义 |
|---|---|---|
| 时空规划 | spatiotemporal planning | 在 (x,y,t) 或 (s,l,t) 中把时间作为独立维同时决定路径与时序 |
| 时空格搜索 | spatiotemporal state lattice | 构型×时间空间里用运动基元建 DAG、做最短路 |
| 运动基元 | motion primitive | 满足车辆运动学的固定时长轨迹片段，连接格状态 |
| 有向无环图 | DAG | 时间单调使时空图无环，可线性时间最短路 |
| 安全区间 | safe interval | 某构型未被动障占据的一段连续时间，SIPP 状态的一半 |
| 安全区间路径规划 | SIPP | 用 (构型,安全区间) 当状态、wait-then-move 转移的动态避障搜索 |
| 时空语义走廊 | SSC | (s,l,t) 里的语义 cube 序列 + Bézier QP 轨迹生成 |
| 凸包性质 | convex hull property | Bézier 曲线落在控制点凸包内 → 控制点在 cube 内则整段在内 |
| 安全飞行走廊 | SFC | 沿骨架路径串起的重叠凸多面体序列 |
| IRIS | Iterative Regional Inflation by SDP | 椭球膨胀+超平面切割交替，生成大无碰撞凸多面体 |
| 分离超平面 | separating hyperplane | 把椭球与障碍分开的超平面，其交定义无碰撞多面体 |
| 同伦类 | homotopy class | "从左绕/右绕""抢/让"等拓扑上不同的解类别 |
| 时空柱 | spatiotemporal column | 动态障碍在 (s,l,t) 里随时间倾斜的占用体 |
知识点总表¶
| 知识点 | 核心结论 | 关联章节 |
|---|---|---|
| 时空格为何是 DAG | 时间单调递增、永不回头 → 无环 → 线性最短路、A* 不怕重开 | §T2.1，T1 §T1.4 |
| 动障如何进入搜索 | 带时间戳禁用其时空占用节点（同点不同时占用不同） | §T2.1 |
| SIPP 核心观察 | 安全时间步无界但安全区间有限且少 → 状态数回到静态量级 | §T2.2 |
| SIPP 的关键假设 | 可原地等待 + 瞬时启停 → 完备最优；真车不满足 → 当粗搜 | §T2.2 |
| Bézier 凸包线性化 | 控制点全在凸 cube 内 ⇒ 整段曲线在内 ⇒ 安全约束=线性不等式 | §T2.3 |
| 走廊的本质 | 用前端凸分解造凸性，换后端 QP 的凸可解性 | §T2.3 |
| IRIS 两步交替 | 切分离超平面（多面体）+ 求最大内切椭球（SDP），单调膨胀 | §T2.4 |
| 走廊 vs 搜索 | 搜索定决策（离散全局组合）、走廊精修（连续局部可优化） | §T2.5 |
| 两段式组合 | 搜索定 homotopy + 走廊 QP 精修 = 主流时空规划骨架 | §T2.5，§T2.6 |
累积项目：时空规划模块¶
本章为贯穿规控线的累积项目（T 线）新增"时空层"。 延续 T1 的解耦管线接口，本章把它升级为可处理强耦合的时空管线。 下面给出本章应交付的模块骨架与验收标准。
本章新增模块¶
// T 线累积项目:时空规划层接口骨架(承接 T1,本章实现)
// 时空规划层:当 T1 解耦管线因强耦合失真时启用
class SpatiotemporalPlanner {
public:
virtual ~SpatiotemporalPlanner() = default;
// Step 1: 时空搜索定决策(§T2.1 时空格 或 §T2.2 SIPP)
// 返回离散粗解(一串 (s,l,t) 路点);无解则 std::nullopt
virtual std::optional<CoarsePath> SearchCoarse(
const ReferenceLine& ref_line, const std::vector<Obstacle>& static_obs,
const std::vector<Obstacle>& dynamic_obs, const State& ego_state) = 0;
// Step 2: 沿粗解切时空走廊(§T2.4 SFC/IRIS)
// 返回重叠的 (s,l,t) 凸 cube 序列
virtual std::vector<Cube> BuildCorridor(
const CoarsePath& coarse, const std::vector<Obstacle>& static_obs,
const std::vector<Obstacle>& dynamic_obs) = 0;
// Step 3: 走廊内 Bézier QP 精修(§T2.3 SSC)
// 返回平滑、动力学可行的时空轨迹 (s(t), l(t))
virtual Trajectory OptimizeInCorridor(
const std::vector<Cube>& cubes, const State& ego_state) = 0;
// 顶层:串起三步;搜索无解时返回 std::nullopt 交上层降级
std::optional<Trajectory> Plan(
const ReferenceLine& ref_line, const std::vector<Obstacle>& static_obs,
const std::vector<Obstacle>& dynamic_obs, const State& ego_state) {
auto coarse = SearchCoarse(ref_line, static_obs, dynamic_obs, ego_state);
if (!coarse) return std::nullopt;
auto cubes = BuildCorridor(*coarse, static_obs, dynamic_obs);
return OptimizeInCorridor(cubes, ego_state);
}
};
// SIPP 低层搜索:可被 T5 多机 CBS 复用
class SippLowLevel {
public:
virtual ~SippLowLevel() = default;
// 每个构型的安全区间列表
virtual SafeIntervalTable ComputeSafeIntervals(
const std::vector<Obstacle>& dynamic_obs) = 0;
// 带可选时空约束的 SIPP 搜索(constraints 供 CBS 注入)
virtual std::optional<CoarsePath> Search(
int start, int goal, const SafeIntervalTable& safe_intervals,
const SpatioTemporalConstraints* constraints = nullptr) = 0;
};
验收标准¶
- 正确性：在 §T2.6 的动态绕障场景下，
plan输出的轨迹在密集采样下全程无碰撞（不撞抛锚车、不在 \(t\in[3,5]\) 与对向车冲突）。 - 搜索-走廊一致性：走廊必须包住搜索粗解所在的 homotopy（验证：粗解路点全部落在生成的 cube 并集内）。
- 连续性：QP 输出轨迹的位置、速度、加速度在 cube 接缝处连续（无跳变）。
- 降级：搜索无解时
Plan返回std::nullopt，上层据此减速/停车，绝不返回带碰撞的轨迹。 - SIPP 可复用：
SippLowLevel::Search接受外部注入的时空约束（为 T5 的 CBS 低层搜索预留接口）。
评价指标¶
时空规划质量从四个维度量化，配一套回归测试支撑"改代码即评分"的闭环。
| 维度 | 指标 | 含义 |
|---|---|---|
| 安全 | 最小碰撞间隙、密采样碰撞率 | 轨迹与所有静/动障的最小时空距离；采样点碰撞比例（应为 0） |
| 舒适 | 最大 jerk、最大向心加速度 | 纵向 jerk 峰值、横向 \(v^2\kappa\) 峰值，越小越平顺 |
| 效率 | 通过时间、平均速度、绕行额外里程 | 完成绕障的耗时、均速、相对直行的多走距离 |
| 一致性 | 帧间轨迹差异、homotopy 翻转次数 | 相邻规划周期轨迹的偏差；决策（抢/让）是否反复横跳 |
回归测试集（每个场景定一组阈值，每次改代码自动跑分）：(1) 直道跟静止抛锚车绕行；
(2) 单动态障碍横穿；
(3) 对向车时间窗（§T2.6 主场景）；
(4) 双动态障碍先后冲突；
(5) 窄缝穿越（考验走廊宽度）；
(6) 搜索无解的降级场景（验证返回 std::nullopt）。
一致性指标尤其重要——时空规划最怕"决策横跳"（这一帧决定抢、下一帧又改让），它往往是搜索离散化抖动或走廊重切引起的，回归测试要专门盯住 homotopy 翻转次数。
FAQ¶
Q：时空规划和 T1 的 ST 图到底差在哪？既然都有时间。 ST 图是在路径**已定**的前提下、只对纵向 \(s(t)\) 排速度，时间是被动的一维。 时空规划把横向 \(l\) 和时间 \(t\) 一起放进搜索/优化，主动决定"走哪条路 + 何时到"。 差别在"路径是不是已经定死"——强耦合场景下路径不能先定，必须时空联合。
Q：SIPP 既然假设瞬时启停、真车用不了，为什么还要学？ 两个理由：(1) 它是多智能体规划（T5）CBS/ECBS 的事实标准低层，不懂 SIPP 就读不懂多机； (2) 它的"安全区间"思想（把分段恒定的时间占用压成区间）是通用的，且工程上常把 SIPP 当离散粗搜，再交走廊 QP 补动力学。 学它是学思想和接口，不是直接拿去开车。
Q：SSC 的 cube 和 SFC 的多面体是一回事吗？ 本质都是"无碰撞凸多面体"，都靠 \(A\mathbf{x}\le\mathbf{b}\) 表示、都用凸包性质约束 Bézier 控制点。 区别在生成方式与侧重：SSC 偏 \((s,l,t)\) 的语义膨胀（吃交通规则），SFC 偏 \((x,y,z,t)\) 的几何膨胀（IRIS 椭球法，无人机居多）。 可以把 SSC 看成"带语义的、自驾版的时空 SFC"。
Q：为什么走廊里非要用 Bézier/B-spline，普通折线不行吗？ 折线不光滑（速度、加速度不连续），不能直接执行。 Bézier/B-spline 既光滑又有凸包性质——后者让"整段曲线在 cube 内"等价于"有限个控制点在 cube 内"，把无穷约束压成有限线性约束。 这个凸包红利是普通参数化给不了的。
Q：IRIS 要解 SDP，会不会太慢，没法实时？ 单次 IRIS 的 SDP 规模不大（变量是椭球的形状矩阵），毫秒级可解； SFC 沿骨架要跑多次 IRIS，但各段独立、可并行。 实际系统（如 SUPER 的 CIRI/FIRI）会用更轻量的变体进一步提速，做到实时。 学习时理解 SDP 的角色即可，工程上有成熟快实现。
Q：搜索和走廊组合时，搜索的离散分辨率会不会拖累最终轨迹质量？ 不会，因为分工恰好规避了这点：搜索只需**定对 homotopy**（往左还是往右、抢还是让），这是个粗粒度的离散决策，对分辨率不敏感； 定下 homotopy 后，连续轨迹的精细程度完全由走廊 QP 决定，与搜索分辨率无关。 这正是两段式的精妙——让离散的归离散、连续的归连续。
Q：动态障碍的预测不准怎么办？走廊是按预测切的。 这是时空规划（乃至所有依赖预测的规划）的共同软肋。 常见对策：(1) 给动态障碍的时空占用加不确定性裕度（cube 切得更保守）； (2) 高频重规划（每周期用最新预测重切走廊重解 QP），用滚动时域吸收预测误差； (3) 把预测不确定性显式建模——这就进入 Part-U（不确定性规划）的范畴了。 本章默认预测给定，不确定性留给后续 Part。
Q：这章和 T3 时空轨迹优化什么关系？ T2 是 T3 的前身。 T2 的走廊线止步于"走廊内解 QP"，T3 会把它推进到"走廊内解 NLP"——处理非线性动力学、更复杂的代价，并引入 MINCO 等更先进的轨迹表示。 可以说 T2 搭好了"搜索定 homotopy + 走廊"的舞台，T3 在走廊里换上更强的优化引擎。
Q：静态障碍和动态障碍，在时空表示里要分开处理吗？ 不必分两套机制——这正是时空表示的优雅之处。 静态障碍可以看成"动态障碍的退化情形"：它在所有时刻占据同一块空间，投影到 \((s,l,t)\) 就是一根**竖直**的时空柱（不随 \(t\) 倾斜）； 动态障碍则是随 \(t\) 倾斜的柱子。 无论搜索（禁用其占据的时空节点 / 切掉对应安全区间）还是走廊（cube 膨胀时绕开柱子），处理逻辑完全一致，只是柱子的形状不同。 所以工程上常把静态、动态障碍统一投影成时空占据，再交给同一套搜索/走廊处理——§T2.6 的管线里抛锚车（静态、竖直柱）和对向车（动态、倾斜柱）就是这么统一进来的。
Q：既然时空规划这么繁琐，为什么不直接用神经网络端到端学一个规划器？ 端到端学习（如近年的 UniAD、VAD 等）确实是活跃方向，但它和本章的优化/搜索式规划各有不可替代的地方，目前更多是互补而非取代。 学习式方法的长处是处理感知到决策的复杂映射、吸收人类驾驶的隐式偏好； 短板是**安全性难以形式化保证**——它给不出"这条轨迹一定不撞、一定满足动力学"的硬证明，而本章的走廊+QP 天然带这种保证（约束写在那里，解必满足）。 所以当前不少系统的做法是：用学习给出意图/粗轨迹，再用走廊+优化做安全兜底和精修——把学习的灵活和优化的可证安全结合起来。 本章的时空规划，正是这套"安全兜底层"的核心； 理解它，才能理解端到端方案里那个"安全护栏"是怎么搭的。
延伸阅读¶
奠基论文（按本章脉络）：Ziegler & Stiller, Spatiotemporal state lattices..., IROS 2009（时空格奠基）； McNaughton et al., Motion Planning... Conformal Spatiotemporal Lattice, ICRA 2011（GPU 时空格）； Phillips & Likhachev, SIPP..., ICRA 2011（安全区间）； Ding, Zhang, Chen, Shen, ...Spatio-temporal Semantic Corridor, RA-L 2019（SSC）； Deits & Tedrake, Computing Large Convex Regions..., WAFR 2014（IRIS）； Liu et al., ...Safe Flight Corridors in 3-D..., RA-L 2017（SFC）。
前沿：Ren et al., Safety-assured high-speed navigation for MAVs（SUPER）, Science Robotics 2025（CIRI 构型走廊 + 双轨迹安全，>20 m/s 林间飞行）。
开源项目：HKUST-Aerial-Robotics/spatiotemporal_semantic_corridor（SSC 参考实现）；
HKUST-Aerial-Robotics/EPSILON（SSC 在完整自驾系统中的集成，配行为层 EUDM）；
sikang/DecompROS（IRIS 风格凸分解的 C++ 实现，被广泛复用）；
whoenig/libMultiRobotPlanning（SIPP 的模板实现 sipp.hpp，含 CBS/ECBS）；
hku-mars/SUPER（CIRI/FIRI 走廊 + 双轨迹规划，ROS1/ROS2）。
与本书其他部分：无人机方向的 MINCO/安全走廊章节对 SFC 有更细致的推导，本章侧重自驾应用，可互为补充。
本章与后续的关系¶
| 后续主题 | 本章提供的基础 | 衔接点 |
|---|---|---|
| T3 时空轨迹优化 | 走廊表示 + 走廊内 QP | 把"走廊内 QP"推进到"走廊内 NLP"（非线性动力学、MINCO） |
| T5 多智能体协调 | SIPP 低层搜索、走廊互斥约束 | CBS/ECBS 用 SIPP 作低层；多机走廊互斥（MADER/EGO-Swarm） |
| Part-U 不确定性规划 | 动障时空占用的确定性表示 | 把占用的预测不确定性显式建模（机会约束、风险敏感） |
| Part-G 博弈规划 | 时空轨迹作为博弈中的策略表示 | 强交互（主动加塞）下，时空轨迹成为博弈均衡的载体 |
| 无人机方向 | SFC 凸分解、Bézier 走廊 | 共享走廊生成与轨迹优化的几何内核 |
🔧 故障排查手册¶
按"现象 → 可能原因 → 排查/修复"组织，覆盖时空规划落地最常见的坑。
| 现象 | 可能原因 | 排查 / 修复 |
|---|---|---|
| 仿真里轨迹"穿模"（端点不碰、中途撞障） | 碰撞检查只查基元/曲线端点，漏中途 | 沿基元/曲线密采样检查或用 CCD；\(\Delta t\) 大时加密（§T2.1 编程陷阱） |
| SIPP 在明明可解的场景报无解 | 只取了第一个安全区间，丢了"等障碍过去"的解 | 枚举每个邻居的全部安全区间，允许原地等到能进的最早时刻（§T2.2 编程陷阱） |
| 走廊 QP 报 infeasible | 相邻 cube 不重叠，拼接点无合法落点 | 检查并强制相邻 cube 非空重叠（§T2.3 编程陷阱） |
| 轨迹贴着障碍/越语义边界走 | cube 撑得过大，越界或贴近动障时变边界 | 收紧 cube 膨胀，尊重语义与动障时变裕度（§T2.3 概念陷阱） |
| IRIS 产出零体积/退化多面体 | 种子点在障碍内或贴边 | 取种子前做无碰撞检查 + 安全裕度（§T2.4 编程陷阱） |
| 走廊窄得轨迹没有优化余地 | IRIS 没迭代膨胀（只切一刀），或种子选得差 | 确认迭代到体积收敛；沿路径多放种子（§T2.4 编程陷阱/概念陷阱） |
| 决策反复横跳（这帧抢、下帧让） | 搜索离散化抖动，或走廊每帧重切引入跳变 | 加 homotopy 滞回/一致性代价；走廊增量更新而非每帧重切；盯回归测试的翻转次数（评价指标） |
| 轨迹在 cube 接缝处顿挫 | 段间连续性只到位置，缺速度/加速度连续 | 连续性约束补到二阶（位置+速度+加速度，§T2.3 正确要点） |
| 期待 QP 自动换边绕行却没换 | 误以为走廊优化能跨 homotopy | 跨 homotopy 是搜索的职责；要么搜索重定、要么多走廊并行择优（§T2.5 概念陷阱） |
API 速查表¶
以教学示意接口为主，实际项目（Apollo / EPSILON / DecompROS）的命名与签名以各自代码为准。
| 功能 | 示意接口 | 说明 |
|---|---|---|
| 时空格搜索 | SpatiotemporalLatticeSearch(start, goal, primitives, occ, dt) |
构型×时间 DAG 上 A*；occ 带时间戳 |
| 应用运动基元 | ApplyPrimitive(state, prim, dt) |
返回末状态 + 中间轨迹片段 |
| 安全区间计算 | SafeIntervals(obstacle_intervals) |
合并不安全区间后取补集 |
| SIPP 后继 | GetSuccessors(state, graph, dt_move, safe_intervals_of) |
wait-then-move 转移 + 最早到达时间 |
| 走廊 QP 组装 | BuildCorridorQp(cubes, n_order, &P, &constraints) |
cube 半空间约束 + 段间连续性 |
| IRIS 单凸域 | IrisInflate(seed, obstacles, max_iter, tol) |
椭球膨胀+超平面切割，返回 \(A\mathbf{x}\le\mathbf{b}\) |
| QP 求解 | SolveQpOsqp(P, constraints) |
OSQP 解凸 QP（注意 1.0 需 P 上三角，见 T1） |
| 顶层时空规划 | PlanSpatiotemporal(ref_line, static_obs, dynamic_obs, ego) |
串起搜索→走廊→QP，返回 std::optional |
研究与实践建议¶
入门（建立全景）：先把 §T2.6 的完整管线跑通（哪怕用最简化实现），建立"搜索定决策 → 切走廊 → QP 精修"的整体直觉，再回头逐节抠细节。 不要一上来就钻 IRIS 的 SDP 推导，容易只见树木。
进阶（动手实现）：按难度递增——先实现 SIPP（概念清晰、无需求解器，最适合练手），再实现走廊 QP（接 OSQP，承接 T1 经验），最后碰 IRIS（要 SDP 求解器，最重）。 每实现一个，配上对应的回归测试场景验证。
深入（读真代码）：精读 libMultiRobotPlanning 的 sipp.hpp 理解 SIPP 的工程实现，精读 EPSILON 的 ssc_planner 理解 SSC 如何与行为层配合，精读 DecompROS 理解凸分解的 C++ 细节。
读代码时带着本章的公式去对照，重点找"代码里有、本章没细讲"的工程裕度（数值缩放、边界处理）。
研究方向（若想往前推）：(1) 动态障碍预测不确定性下的鲁棒走廊（接 Part-U）； (2) 学习式走廊生成（神经网络预测种子/形状，加速凸分解）； (3) 强交互下时空规划与博弈的结合（接 Part-G）； (4) 高维 \((s,l,v,t)\) 走廊的高效生成。 带着 §T2 的开放问题去读最新文献，比泛读更有方向。
版本信息速查¶
| 项目 | 版本 / 状态（截至本章撰写） | 备注 |
|---|---|---|
| OSQP | 1.0.x（2025） | 1.0 C API 与 0.6.x 不兼容（P 须上三角）；多数教程仍用 0.6.x（详见 T1） |
| DecompROS | 长期维护，ROS1 为主 | IRIS 风格凸分解 C++ 实现 |
| EPSILON | 开源，C++/ROS | SSC + 行为层 EUDM 的完整系统 |
| SUPER（CIRI/FIRI） | 2025 发布，支持 ROS1/ROS2 | Science Robotics 2025；硬件亦开源 |
| libMultiRobotPlanning | 稳定，C++ 模板 | 含 SIPP/CBS/ECBS |
版本号会随时间变化，落地前请以各项目官方仓库的最新发布为准。
前沿工作与开放问题¶
构型走廊与形式化安全（CIRI / SUPER）。 HKU MaRS 的 Ren 等人在 Science Robotics 2025 的 SUPER 系统（《Safety-assured high-speed navigation for MAVs》，10(98):eado6187）把走廊推向了新高度：其 CIRI 走廊建在更高效的 FIRI 之上，并采用**双轨迹框架**——同时优化一条"保安全"的轨迹和一条"求速度"的轨迹（思路受 FASTER 启发），在保证安全的前提下逼近速度极限。 实测中 SUPER 在未知林地以超过 \(20\,\text{m/s}\) 飞行、相比基线把失败率降低约 \(35.9\) 倍。 这代表了走廊方法从"经验可行"走向"形式化安全保证"的趋势。
其他活跃方向。 - 动态可达管走廊：用可达性分析（reachability）的管状集合（funnel）作为走廊边界，让走廊自带动力学可达性保证，而非事后检查。 - 学习式走廊生成：用神经网络从地图直接预测最优的走廊种子点和形状，跳过或加速 IRIS 的迭代，把走廊生成压到微秒级。 - 不确定性感知走廊：把动态障碍预测的不确定性显式切进 cube（更保守的时变边界），是 T2 走廊线与 Part-U 不确定性规划的交汇点。
开放问题。 如何在高维 \((s,l,v,t)\) 空间高效生成走廊而不爆炸？ 非凸障碍的凸分解不唯一，如何选出对下游优化最友好的那一种分解？ 走廊的保守性（为安全留的裕度）与轨迹的激进性（为效率求的速度）之间，有没有可证明的最优权衡？ 这些问题，正是把本章内容往博士级研究推进的入口。
拓展练习（综合 / 项目级）¶
- [实现 + 对比] 完整实现 §T2.6 的动态绕障管线（SIPP 粗搜 + 手工/简化走廊 + OSQP 精修），在对向车时间窗场景下跑通，并与"纯 T1 解耦管线"对比：解耦在这个强耦合场景下会给出怎样失真的解？ 把两者的轨迹画在同一张 \((s,t)\) 和 \((s,l)\) 图上。
- [设计 + 测试] 给你的时空规划器接一套"评价指标"（见评价指标一节）自动打分，搭一个含 6 个场景的回归测试集（直道绕抛锚车、单动障横穿、对向车时间窗、双动障先后冲突、窄缝穿越、搜索无解降级），每场景定阈值，跑通"改代码即自动评分"的闭环。 重点让一致性指标（homotopy 翻转次数）真正起作用。
- [推导 + 工程] 把 §T2.3 的 Bézier 凸包性质从"位置在 cube 内"扩展到"速度在某速度盒内、加速度在某加速度盒内"——推导 Bézier 导函数的控制点与原控制点的线性关系，并据此把速度/加速度约束也写成控制点的线性不等式。
- [复现 + 精读] 取
libMultiRobotPlanning的sipp.hpp，逐行对照本章 §T2.2 的安全区间与 wait-then-move 转移，列出"代码里有、本章没细讲"的工程细节（如区间合并的边界处理、等待动作的具体编码），整理成一页对照笔记。 - [开放] 在你的实现里制造一个"决策横跳"：让对向车的预测在两帧间轻微抖动，观察搜索是否在"抢/让"间反复翻转、走廊是否随之跳变。 设计一个缓解方案（homotopy 滞回、一致性代价、或走廊增量更新），并用一致性指标量化改善。
结语：你现在站在哪里¶
本章带你完成了从"解耦"到"联合"的第一次跨越。 T1 把时间藏在 ST 图的速度一维里、且要求路径先定； 本章把时间请出来，立成一个独立的搜索/优化维度，于是"走哪条路"与"何时到哪"第一次能被同时决定——这正是 cut-in、动态绕障、抢行这类强耦合场景所要求的。
你手里现在握着两套互补的工具。 搜索线（时空格、SIPP）在离散的时空图里找全局最优、干净地表达离散决策，是多智能体协调（T5）的地基； 走廊线（SSC、IRIS/SFC）把非凸避障重构成凸 QP，直接产出平滑可执行的轨迹，是自驾/无人机连续规划的主流。 而 §T2.5 揭示的那个分工——搜索定决策、走廊精修——不是两条线的妥协，而是当今先进系统（从 EPSILON 到 SUPER）共同的骨架，更是 T1 "DP 定决策、QP 精修"那个思想在更高维时空的自然生长。
更要紧的是，你也看清了这套时空联合的**位置**：它处理了路径-速度的强耦合，却仍假设动态障碍的预测是给定且可信的、仍假设别的智能体不会因你的动作而改变意图。 预测的不确定性，是 Part-U 要补的； 强交互下的相互博弈，是 Part-G 要啃的； 而把走廊里的 QP 换成更强的 NLP、处理非线性动力学与更先进的轨迹表示，正是下一章 T3 的任务。 时空联合不是终点，而是你从"假设世界静止可知"走向"直面不确定与交互"的第一级台阶。 带着这两套工具和它们的边界，我们进入 T3。