D4 B 样条轨迹优化——从 Ewok 到 Fast-Planner 再到 EGO-Planner¶

性质：算法工程教学 | 难度跨度：⭐⭐ ~ ⭐⭐⭐⭐ | 预计精读：15-20 小时

一句话定位：从 2017 年 Ewok 的环形缓冲 ESDF 到 2019 年 Fast-Planner 的 kinodynamic A* + NLopt 架构范式，再到 2021 年 EGO-Planner 以按需 \((p,v)\) 锚点消除 ESDF 依赖——本章完整讲透 B 样条在四旋翼实时轨迹优化中的数学基础、工程实现与演化逻辑：为什么 B 样条的凸包+局部支撑+自动连续性三大性质使它成为在线规划的天然表示、Fast-Planner 如何定义了无人机规划的代码架构范式、以及 EGO-Planner 如何将规划时间从 15 ms 压缩到 1 ms。

前置自测¶

开始前先回答下面 5 个问题。答不出 2 题以上，建议先回前置章节补齐——D4 的每一步优化设计都建立在这些基础之上，欠了账会在碰撞代价的梯度推导处卡住。

分段多项式轨迹的 KKT 拼接约束有多少个？ 对于 \(K\) 段 7 阶多项式（degree-7），衔接点处需要匹配位置、速度、加速度、jerk 共几个等式约束？这些约束的矩阵形式是什么？（答不出 → 回 D3 多项式轨迹生成，\(\S\)D3.3）
微分平坦映射需要平坦输出的几阶导数？ 四旋翼从 \(\sigma(t) = (x,y,z,\psi)^\top\) 恢复全部 12 维状态和 4 维控制输入时，最高需要 \(\sigma\) 的第几阶导数？这决定了轨迹的最低连续性要求。（答不出 → 回 D1 微分平坦，\(\S\)D1.1）
矩阵的条件数 \(\kappa(A) = 10^{14}\) 意味着什么？ 用 64 位浮点数（约 16 位十进制精度）求解 \(Ax=b\) 时，结果最多有几位有效数字？为什么 Mellinger 的 QP 在 20+ 段时会数值崩溃？（答不出 → 回 Part 0 数值线性代数，或 D3 \(\S\)D3.3.8）
L-BFGS 和标准 BFGS 的核心区别是什么？ L-BFGS 为什么只需要 \(O(mn)\) 内存而非 \(O(n^2)\)？Two-loop recursion 的作用是什么？（答不出 → 回 Part 0 优化基础）
凸组合（Convex Combination）的定义是什么？ 给定点集 \(\{P_i\}\) 和权重 \(\{w_i\}\)，满足什么条件时 \(\sum w_i P_i\) 落在 \(\{P_i\}\) 的凸包内？（答不出 → 回 Part 0 凸优化基础）

参考答案要点（先自己答，再对照）：

\(K\) 段 7 阶多项式有 \(8K\) 个系数，\((K-1)\) 个衔接点各需匹配 4 个量（位置、速度、加速度、jerk），共 \(4(K-1)\) 个拼接等式约束。矩阵形式为 \(C \mathbf{c} = \mathbf{d}\)，其中 \(C\) 是 \(4(K-1) \times 8K\) 的稀疏矩阵。当 \(K=10\) 时，KKT 系统为 \(80 \times 80\)——Richter 的端点导数替换就是为了消除这个系统。
四阶导数（snap）。微分平坦映射从位置到推力需要二阶导数（加速度），从推力方向到姿态需要三阶导数（jerk），从角速度到力矩需要四阶导数（snap）。因此轨迹至少需要 \(C^4\) 连续性才能保证所有物理量连续。
\(\kappa = 10^{14}\) 时，64 位浮点的约 16 位精度中被条件数"吃掉" 14 位，结果仅有约 2 位有效数字——几乎不可用。Mellinger 方法使用单项式基 \(\{1, t, t^2, \ldots, t^7\}\)，其 Vandermonde 型映射矩阵的条件数随 \(T^N\) 增长，20 段时 \(\kappa > 10^{14}\)。
标准 BFGS 维护完整的 \(n \times n\) 近似 Hessian 逆矩阵 \(H_k\)，内存 \(O(n^2)\)。L-BFGS 只存储最近 \(m\) 步的差量对 \((s_k, y_k)\)（其中 \(s_k = x_{k+1}-x_k\)，\(y_k = g_{k+1}-g_k\)），内存 \(O(mn)\)。Two-loop recursion 用这 \(m\) 对隐式地近似 \(H_k \cdot g_k\)，无需显式构造 \(H_k\)。
凸组合要求所有权重 \(w_i \geq 0\) 且 \(\sum w_i = 1\)。满足时 \(\sum w_i P_i\) 必然落在 \(\{P_i\}\) 的凸包内。B 样条基函数恰好满足非负性和单位分解，因此曲线点是控制点的凸组合——这是凸包性质的根本来源。

本章目标¶

学完本章后，你应该能够：

**严格论证**为什么均匀 B 样条的五大性质（凸包性、局部支撑、自动连续性、导数=差分、节点插入不变性）使它成为在线无人机轨迹优化的天然数学表示——从多项式轨迹的痛点出发，而非凭直觉
从零推导 Cox-de Boor 递推公式，手工计算三次均匀 B 样条的基矩阵 \(\mathbf{M}_3\)，理解 de Boor 算法的三角形计算结构
完整理解 Fast-Planner 的四包分层架构（plan_env → path_searching → bspline_opt → plan_manage）及其数据流——从深度图输入到控制指令输出
推导并实现 B 样条优化的四项代价函数（\(J_\text{smooth}\)、\(J_\text{collision}\)、\(J_\text{dynamic}\)、\(J_\text{feasibility}\)）及其解析梯度
理解并解释 EGO-Planner 的核心创新——为什么 ESDF 是过度计算、按需 \((p,v)\) 锚点如何替代稠密距离场、以及这一改进如何将规划时间从 ~15 ms 压缩到 ~1 ms
掌握 LBFGS-Lite 的 Lewis-Overton 非光滑线搜索与 Li-Fukushima 谨慎更新机制，理解它为何优于 NLopt 中的 More-Thuente 线搜索
能在 Fast-Planner/EGO-Planner 代码中端到端追踪一次规划调用——从 FSM 状态转移到控制点优化到轨迹发布

本章知识导航¶

本章的知识结构是一棵以"如何用 B 样条实现四旋翼实时轨迹优化"为根的树。树干是三个递进的核心问题，树枝是每个问题的技术细节和工程实现。

为什么多项式轨迹不够用？ (§D4.1 动机与历史)
    │
    ▼
B 样条的数学基础是什么？ (§D4.2 B 样条数学)
    │
    ├─→ 节点向量与 Cox-de Boor (§D4.2.1)
    ├─→ 均匀 B 样条的特殊简化 (§D4.2.2)
    ├─→ 五大核心性质 (§D4.2.3)
    ├─→ de Boor 算法 (§D4.2.4)
    └─→ 导数的矩阵表示 (§D4.2.5)
         │
         ▼
怎样在 B 样条上做轨迹优化？
    │
    ├─→ Fast-Planner 架构 (§D4.3)
    │      ├─→ ESDF 建图 (§D4.3.2)
    │      ├─→ kinodynamic A* (§D4.3.3)
    │      ├─→ 代价函数设计 (§D4.3.4)
    │      └─→ FSM 调度 (§D4.3.5)
    │
    ├─→ 拓扑路径枚举 (§D4.4) ──→ 解决局部极小
    │
    ├─→ EGO-Planner (§D4.5) ──→ 消除 ESDF 依赖
    │      ├─→ (p,v) 锚点机制 (§D4.5.2)
    │      └─→ 迭代更新 (§D4.5.3)
    │
    ├─→ LBFGS-Lite (§D4.6) ──→ 非光滑优化器
    │
    ├─→ Ewok 环形缓冲 (§D4.7) ──→ 嵌入式 ESDF
    │
    ├─→ 代价函数逐项对比 (§D4.8)
    │
    └─→ 数值细节与陷阱 (§D4.9)
              │
              ▼
         B 样条 → MINCO 的历史必然性 (§D4.10)

小节	主题	难度	一句话
§D4.1	动机：为什么需要 B 样条	⭐⭐	多项式的 KKT 拼接与数值病态催生了新表示
§D4.2	B 样条数学基础	⭐⭐	Cox-de Boor 递推、五大性质、de Boor 算法
§D4.3	Fast-Planner 架构	⭐⭐⭐	ESDF + kinodynamic A* + NLopt B 样条优化
§D4.4	拓扑路径枚举	⭐⭐⭐	多同伦类路径避免局部极小
§D4.5	EGO-Planner ESDF-free	⭐⭐⭐	按需 \((p,v)\) 锚点替代稠密 ESDF
§D4.6	LBFGS-Lite 优化器	⭐⭐⭐	Lewis-Overton 线搜索 + 谨慎更新
§D4.7	Ewok 环形缓冲	⭐⭐	环形缓冲 ESDF 的嵌入式实现
§D4.8	代价函数逐项对比	⭐⭐⭐	Fast-Planner vs EGO-Planner 的差异仅在碰撞项
§D4.9	数值细节与调参	⭐⭐⭐	权重、节点距、数值稳定性
§D4.10	B 样条→MINCO 演化	⭐⭐⭐⭐	均匀节点距是根本限制，MINCO 综合两者优势

两条阅读线：

理论线（理解数学）：§D4.1 → §D4.2 → §D4.3.4 → §D4.5.2 → §D4.8 → §D4.10，重点是推导和性质证明
工程线（会用就行）：§D4.1 → §D4.2.2 → §D4.3 → §D4.5 → §D4.6.5 → §D4.9，重点是架构和代码

无论哪条线，§D4.1 和 §D4.2 都是必读——它们是所有后续内容的地基。

前置知识桥接¶

回顾 D3（多项式轨迹生成）：D3 建立了四旋翼轨迹优化的基本框架——给定航路点序列，通过最小化 snap（四阶导数）积分的 QP 生成分段多项式轨迹。Mellinger & Kumar 2011 的原始方法使用受约束 QP，Richter, Bry & Roy 2016 通过端点导数变量替换消除了数值病态并得到闭式解。本章的出发点正是这些方法的**两个根本局限**：(1) KKT 等式约束系统庞大，不适合在线重规划；(2) 每段时间 \(T_i\) 是耦合的非线性参数，时间分配搜索本身是 NP-hard 的。B 样条以其**自动连续性**消除了第一个问题，以**均匀节点距**简化了第二个问题。

回顾 D1（微分平坦）：D1 证明了四旋翼的微分平坦性——给定平坦输出 \(\sigma(t) = (x,y,z,\psi)^\top\) 及其前四阶导数，全部 12 维状态和 4 维控制输入可通过纯代数运算恢复。这意味着**规划问题从 12 维状态空间坍缩为 4 维平坦输出空间的曲线设计**。本章的 B 样条优化正是在这个坍缩后的空间里进行的。微分平坦映射需要到 snap（四阶导数）级别——这决定了 Fast-Planner 选择 \(p=5\)（保证 \(C^4\)）而 EGO-Planner 选择 \(p=3\)（保证 \(C^2\)，牺牲 jerk 连续性换取速度）。

回顾 v8 Ch11（Eigen 深度）：B 样条的基矩阵运算（\(4 \times 4\) 或 \(6 \times 6\)）在 1 ms 规划预算中被调用成百上千次。Fast-Planner/EGO-Planner 用 Eigen 的固定大小矩阵（Matrix4d、Matrix<double, 6, 6>）让这些运算全部栈分配、编译器自动 SIMD 向量化——Matrix4d 比 MatrixXd 快 3-5 倍。如果你在 Ch11 学的"固定 vs 动态大小矩阵"的选型原则，这里会直接用上。

前向预告：本章的 B 样条方法使用均匀节点距 \(\Delta t\)，所有段被迫用同一时间间隔——快段不够快，慢段太精细。下一章（D5）的 MINCO（Minimum Control）将引入**非均匀时间段**，每段时间 \(T_i\) 独立可调，同时保留 B 样条的凸包性质和解析梯度。EGO-Planner-v2（Science Robotics 2022）已将后端从 B 样条更换为 MINCO。现在只需要知道：均匀节点距是 B 样条的一个工程简化，它让问题可以用 L-BFGS 在毫秒级求解，但代价是时间分配的灵活性受限。

如果跳过本章会怎样¶

跳过 D4，你会卡在三个具体的地方。

场景一："轨迹生成太慢，跟不上环境变化"。 你用 D3 的多项式 QP 生成了一条完美的轨迹。但无人机飞到一半时，深度相机发现了一面新的墙。你需要在 20 ms 内重新规划——然而 Mellinger 的 QP 求解加上时间分配搜索需要 50-200 ms。更致命的是，改变航路点数量意味着重新组装 KKT 系统，这在 C++ 中涉及矩阵重分配和复杂的拼接逻辑。没有 D4 的 B 样条表示（自动连续性 + 统一控制点数组），你无法实现毫秒级的增量重规划。

场景二："ESDF 占了 70% 的计算量"。 你实现了 Fast-Planner 的架构，一切工作正常。但 CPU profiler 显示 ESDF 更新占了 ~7 ms——而你的总规划预算只有 15 ms。当你试图在计算能力更弱的嵌入式平台（如 Jetson Nano）上运行时，ESDF 成为不可承受的瓶颈。没有 D4.5 中 EGO-Planner 的 ESDF-free 设计，你不知道碰撞梯度可以完全绕过距离场，仅用占据栅格 + 射线投射就能获得足够好的优化方向。

场景三："优化器在碰撞惩罚处振荡"。 你用 NLopt 的 L-BFGS 优化 B 样条碰撞代价，发现在某些环境下优化器无法收敛——步长忽大忽小，代价值不单调下降。根本原因是碰撞惩罚 \(\max(0, \cdot)^3\) 在安全距离边界处有 \(C^0\) 棱角，违反了标准 Wolfe 线搜索的光滑性假设。没有 D4.6 中 LBFGS-Lite 的 Lewis-Overton 非光滑线搜索知识，你无法诊断和修复这个问题。

预计阅读时间¶

模式	时长	适合
精读	15-20 小时	第一次系统学 B 样条轨迹优化：逐节读动机→推导→代码，理解 Fast-Planner 和 EGO-Planner 的完整架构，做完每节练习。建议分 5-6 次。
速读	4-5 小时	有优化基础、想建立全局图景：读每节的"动机"段、关键公式（框住的）、§D4.8 代价对比表、§D4.10 演化总结。跳过代码细节和数值实验。
速查	30-60 分钟	已学过、回来查特定公式：直接定位到附录 A（B 样条速查表）+ 附录 B（参数速查）+ 对应小节的 boxed 公式。

科研发展脉络¶

在钻进具体方法前，先把这条研究线的来龙去脉理清——知道每个方法"从哪来、解决了前人什么痛点、又留下什么给后人"，比孤立地记名字有用得多。

年份	论文	Venue	核心贡献
2017	Usenko 等 (TUM)	IROS	Ewok：B 样条 + 环形缓冲 ESDF；证明 B 样条适合实时重规划
2019	Zhou, Gao, Wang, Liu, Shen (HKUST)	RA-L / IROS	Fast-Planner：kinodynamic A* 前端 + B 样条后端 + ESDF 梯度优化；定义了规划架构范式
2020	Zhou, Gao, Shen (HKUST)	ICRA	拓扑路径枚举：多同伦类路径避免局部极小；topological PRM
2021	Zhou, Wang, Ye, Xu, Gao (ZJU FAST Lab)	RA-L	EGO-Planner：消除 ESDF，按需 \((p,v)\) 锚点 + 射线投射；规划时间 ~1 ms

架构范式传承：Fast-Planner 定义的 plan_env → path_searching → bspline_opt → plan_manage 四包分层被 EGO-Planner / FUEL / RACER / RAPTOR 等后续工作**完整继承**——这是无人机规划代码的"文件夹圣经"。

实验室脉络：TUM Computer Vision Group（Ewok 先驱）→ HKUST Aerial Robotics Group（Fast-Planner 范式定义）→ ZJU FAST Lab（EGO-Planner 速度极限 + MINCO 统一框架）。

本质洞察：这条演化线有一条清晰的主线——碰撞信息的获取从"全局预计算"走向"按需局部查询"。Ewok 和 Fast-Planner 预计算整个 ESDF（\(256^3\) 体素），但优化只查询 ~15 个控制点的距离；EGO-Planner 认识到这 99.9999% 的冗余计算是不必要的，改为按需射线投射。这不仅是工程优化，更是**信息需求分析**的胜利——先问"优化器真正需要什么信息"，再决定计算什么。

§D4.1 为什么 B 样条取代了多项式——动机与历史 ⭐⭐¶

动机：多项式轨迹的三个工程痛点¶

D3 建立的多项式轨迹生成方法在离线精确规划中表现优秀——Richter 的闭式解在 50+ 段上仍然数值稳定。但当场景从"已知环境的离线规划"切换到"未知环境的在线重规划"时，多项式方法暴露了三个根本性的工程痛点。

痛点一：KKT 等式约束系统庞大，在线重规划困难

多项式轨迹需要在每个衔接点显式添加等式约束来保证连续性。对于 \(K\) 段 7 阶多项式，\(8K\) 个系数被 \(4(K-1)\) 个拼接约束和 \(2 \times 4\) 个端点约束绑定，形成一个规模为 \((8K) \times (8K)\) 的 KKT 线性系统。

这个系统有两个问题。第一，当需要在线增减航路点时（例如发现新障碍物后插入一个中间点），KKT 矩阵的维度发生变化，必须重新组装和分解——这在 C++ 中涉及矩阵内存重分配和拼接逻辑的重构。第二，等式约束的存在使得问题只能用 QP 求解器（如 OSQP、qpOASES），不能用更快的无约束优化器（如 L-BFGS）。

多项式轨迹（10 段 × 8 系数 = 80 个优化变量）：

等式约束数量 = 9 个衔接点 × 4 个连续性条件 + 2 × 4 个端点条件
             = 36 + 8 = 44 个等式约束

→ KKT 系统大小: (80 + 44) × (80 + 44) = 124 × 124
→ 每次航路点增减，矩阵维度都变化
→ 不适合增量更新

痛点二：时间分配是耦合的非线性参数

多项式轨迹中，每段的时间 \(T_i\) 不仅影响本段的系数，还通过拼接约束影响相邻段。改变 \(T_3\) 会改变第 3 段的端点值，进而影响第 2 段和第 4 段的连接条件。这种耦合使得时间分配优化是一个非凸甚至 NP-hard 的组合搜索问题，需要外层搜索（如梯形分配、等时分配）+ 内层 QP 的双层结构。

双层结构在离线规划中可以接受——花几秒钟搜索最优时间分配。但在线重规划要求总时间 < 20 ms，留给时间搜索的预算几乎为零。

痛点三：局部修改代价高

多项式的系数是"全局耦合"的——改变一段的系数会通过拼接约束级联影响相邻段，甚至传播到更远的段。这意味着当只有一小段轨迹与新发现的障碍物冲突时，你不能只修改那一段——必须重新求解整个 QP。

如果不用 B 样条会怎样¶

想象一个具体场景：无人机在室内以 3 m/s 飞行，深度相机每 33 ms 更新一帧。飞行过程中发现了一个 D3 规划时不存在的障碍物。你需要在**下一帧到来前（33 ms）**完成重规划。

用多项式方法：

从当前位置截断旧轨迹 → 新的起始约束
在障碍物附近插入一个中间航路点 → 段数 +1，KKT 矩阵维度变化
重新组装 KKT 矩阵 → ~2 ms（矩阵拼接、内存分配）
时间分配搜索 → ~20-100 ms（外层搜索 5-10 个候选）
对每个候选时间分配求解 QP → 5-10 \(\times\) ~5 ms = ~25-50 ms

总计：~50-150 ms。远超 33 ms 的预算。

用 B 样条方法：

从当前位置找到最近的控制点 → ~0.01 ms
冻结已飞过的控制点，只优化后续的 → 无需重新组装任何矩阵
L-BFGS 无约束优化（连续性自动满足，无等式约束）→ ~1-5 ms

总计：~1-5 ms。轻松满足预算。

本质洞察：多项式轨迹的根本问题不是数学不优美——Richter 的闭式解在数学上非常优雅。问题在于**多项式的全局耦合性质与在线增量更新的需求矛盾**。B 样条的局部支撑性质恰好解决了这个矛盾：改动一个控制点只影响 \(p+1\) 段，其余段完全不变。这不是 B 样条"碰巧好用"，而是**局部支撑**这一数学性质在工程上的精确对接。

B 样条解决多项式痛点的方式¶

下面这张对比表概括了 B 样条如何系统性地解决多项式的三个痛点。每个条目在后续小节中都有完整推导。

性质	多项式（D3）	均匀 B 样条
连续性	需要显式 KKT 拼接约束	自动 \(C^{p-1}\)（\(p=5\) → \(C^4\)）
局部修改	改一个系数影响整段轨迹	改一个控制点只影响 \(p+1\) 段
凸包安全	Bernstein 有，单项式无	天然凸包 → 控制点约束 = 曲线约束
时间分配	必须外层搜索	单一均匀节点距 \(\Delta t\) → 解耦
航路点插值	精确插值	近似（控制点 \(\neq\) 曲线点）
适合场景	离线精确轨迹（已知环境）	在线实时重规划（未知环境）
优化器需求	QP（等式约束多）	L-BFGS（无/少等式约束）
代码复杂度	高（段间拼接逻辑复杂）	低（统一的控制点数组）
数值稳定性	单项式基 → Vandermonde 病态	基函数 \(\in [0,1]\) → 无数量级放大

代价：B 样条不精确插值航路点（控制点不在曲线上），均匀节点距必须保守选择（快段和慢段用同一 \(\Delta t\)）。MINCO（D5）将解决这两个问题。

⚠️ 常见陷阱¶

⚠️ 概念误区：认为 B 样条全面优于多项式 错误做法：所有场景都用 B 样条，包括离线精确轨迹生成 现象/后果：B 样条不精确插值航路点——如果任务要求无人机精确经过某些拍照点，B 样条需要额外的等式约束，失去了 L-BFGS 的速度优势 根本原因：B 样条的优势是在线增量更新，代价是放弃精确插值。当场景不需要在线重规划时（如运动捕捉环境下的离线轨迹），多项式（Richter 闭式解）在精确性和最优性上仍然更好 正确做法：已知环境 + 精确航路点 → 多项式（D3）；未知环境 + 实时重规划 → B 样条（D4）；两者兼需 → MINCO（D5）

⚠️ 思维陷阱：认为"均匀节点距"是 B 样条的固有属性 错误做法：将 B 样条等同于均匀 B 样条 现象/后果：忽视非均匀 B 样条（NURBS 的基础）在 CAD 和几何建模中的广泛应用 根本原因：无人机规划选择均匀 B 样条是**工程简化**，而非数学限制。均匀性使基函数形状平移不变，极大简化了解析梯度推导 正确做法：区分"均匀 B 样条"和"B 样条"——前者是后者在 \(\Delta t = \text{const}\) 条件下的特例

练习¶

（分析题） 对于 20 段 7 阶多项式轨迹，计算 KKT 系统的精确维度（优化变量数 + 等式约束数 + KKT 矩阵大小）。再计算等价的三次 B 样条需要多少个控制点、优化变量维度是多少。两者的比值是多少？
（设计题） 假设你有一个无人机在已知环境中按固定航路点飞行的任务，但偶尔需要闪避突然出现的人类。设计一个混合方案：离线用多项式生成精确轨迹，在线用 B 样条做局部修正。如何在两种表示之间转换？

§D4.2 B 样条数学基础——从零建立直觉 ⭐⭐¶

本节是整个 D4 的数学地基。我们从最基本的分段常值函数出发，逐步构造高阶 B 样条基函数，然后推导出无人机轨迹优化所依赖的五大核心性质。这些推导在后续 Fast-Planner 和 EGO-Planner 的代价函数设计中会被反复引用。

§D4.2.1 节点向量与基函数的 Cox-de Boor 递推 ⭐⭐¶

节点向量（Knot Vector） 是 B 样条的参数域骨架——它决定了曲线的分段结构和连续性等级。

节点向量是一个非递减实数序列：

\[ T = \{t_0, t_1, t_2, \ldots, t_m\} \quad \text{其中 } t_i \leq t_{i+1} \]

节点向量将参数域划分为**节点区间（knot span）**：\([t_i, t_{i+1})\)。节点的重复度（multiplicity）决定了曲线在该处的连续性等级——这是 B 样条理论中最深刻的联系之一：重复度每增加 1，连续性降低一阶。

三种节点向量类型：

类型	定义	例子（\(m=9\), \(p=3\)）	特点
均匀（Uniform）	\(t_{i+1} - t_i = \Delta t = \text{const}\)	\(\{0,1,2,3,4,5,6,7,8,9\}\)	无人机规划首选；简化导数计算
准均匀（Clamped）	首尾节点重复 \(p+1\) 次，内部均匀	\(\{0,0,0,0,1,2,3,4,4,4,4\}\)	CAD 常用；曲线过首尾控制点
非均匀（Non-uniform）	间距任意	\(\{0,0,0,1,2.5,4,7,9,9,9\}\)	NURBS 的基础；灵活但计算复杂

为什么无人机规划选择均匀节点？

均匀节点距 \(\Delta t\) 使得每段曲线的基函数形状完全相同（仅平移），所有性质分析、导数计算、凸包判定都可以用**同一套公式**。这极大简化了实时优化的解析梯度推导——你只需要推导一次基矩阵 \(\mathbf{M}_p\)，所有段共享。代价是：快段和慢段被迫用同一 \(\Delta t\)，即**时间分配不灵活**——这正是 MINCO（D5）要解决的问题。

Cox-de Boor 递推公式

\(p\) 阶（degree \(p\)）B 样条基函数 \(N_{i,p}(t)\) 由以下递推定义。这个递推的直觉是：高阶基函数是低阶基函数的加权混合，权重是关于 \(t\) 的线性函数。每升一阶，支撑区间增宽一个节点区间，函数变得更光滑。

0 阶（分段常值）：

\[ N_{i,0}(t) = \begin{cases} 1, & t_i \leq t < t_{i+1} \\ 0, & \text{otherwise} \end{cases} \]

\(p\) 阶递推：

\[ N_{i,p}(t) = \frac{t - t_i}{t_{i+p} - t_i} N_{i,p-1}(t) + \frac{t_{i+p+1} - t}{t_{i+p+1} - t_{i+1}} N_{i+1,p-1}(t) \]

约定：当分母为零时（即 \(t_{i+p} = t_i\) 或 \(t_{i+p+1} = t_{i+1}\)），对应项为 0（\(0/0 = 0\)）。

读到这里你可能会问："这个递推公式是怎么来的？为什么要这样定义？"答案来自 Schoenberg 1946 的构造性证明：要使 \(p\) 阶分段多项式同时满足非负性、单位分解和最小支撑宽度三个性质，Cox-de Boor 递推是**唯一**的构造方式。换言之，B 样条基函数不是人为设计的，而是这三个需求**逻辑上的必然推论**。

手工计算示例——均匀节点 \(\{0,1,2,3,4,5\}\)，计算 \(N_{1,2}(t)\) 在 \(t=2.5\) 处的值：

Step 1: 识别非零的 0 阶基函数
  N_{1,0}(2.5) = 0  (支撑 [1,2))
  N_{2,0}(2.5) = 1  (支撑 [2,3),  2 ≤ 2.5 < 3 ✓)
  N_{3,0}(2.5) = 0  (支撑 [3,4))

Step 2: 计算 1 阶基函数
  N_{1,1}(2.5) = (2.5-1)/(2-1) · N_{1,0}(2.5) + (3-2.5)/(3-2) · N_{2,0}(2.5)
               = 1.5 · 0 + 0.5 · 1 = 0.5

  N_{2,1}(2.5) = (2.5-2)/(3-2) · N_{2,0}(2.5) + (4-2.5)/(4-3) · N_{3,0}(2.5)
               = 0.5 · 1 + 1.5 · 0 = 0.5

Step 3: 计算 2 阶基函数
  N_{1,2}(2.5) = (2.5-1)/(3-1) · N_{1,1}(2.5) + (4-2.5)/(4-2) · N_{2,1}(2.5)
               = (1.5/2) · 0.5 + (1.5/2) · 0.5
               = 0.375 + 0.375 = 0.75

这个三角形计算结构正是 **de Boor 算法**的核心——我们将在 §D4.2.4 中详细展开。

计算复杂度：de Boor 算法评估一个点需要 \(O(p^2)\) 次乘加运算，与直接求值相比既高效又数值稳定（避免了高次幂的浮点误差累积）。

§D4.2.2 均匀 B 样条的特殊简化 ⭐⭐¶

当节点向量均匀时（\(t_i = i \cdot \Delta t\)），Cox-de Boor 递推可以进一步简化。定义**归一化参数** \(s = (t - t_k) / \Delta t \in [0,1)\)，则基函数的形状只取决于阶数 \(p\)，与具体位置 \(k\) 无关——这是均匀 B 样条的核心优势。

\(p=3\)（三次）均匀 B 样条——EGO-Planner 的选择：

\(s \in [0,1)\) 区间内，四个非零基函数为：

\[ \mathbf{N}_3(s) = \frac{1}{6} \begin{bmatrix} (1-s)^3 \\ 3s^3 - 6s^2 + 4 \\ -3s^3 + 3s^2 + 3s + 1 \\ s^3 \end{bmatrix} \]

写成矩阵形式（用于高效向量化计算）：

\[ \mathbf{N}_3(s) = \frac{1}{6} \underbrace{\begin{bmatrix} -1 & 3 & -3 & 1 \\ 3 & -6 & 3 & 0 \\ -3 & 0 & 3 & 0 \\ 1 & 4 & 1 & 0 \end{bmatrix}}_{\mathbf{M}_3} \begin{bmatrix} s^3 \\ s^2 \\ s \\ 1 \end{bmatrix} \]

其中 \(\mathbf{M}_3\) 是三次均匀 B 样条的**基矩阵**。这个矩阵在 Fast-Planner 和 EGO-Planner 的代码中被直接硬编码。

阶段小结：到这里我们完成了从 Cox-de Boor 递推到基矩阵 \(\mathbf{M}_3\) 的推导。基矩阵把"递推求值"变成了"一次矩阵向量乘法"——后者对 SIMD 极其友好。接下来是 B 样条曲线的定义和求值。

B 样条曲线的求值：给定 \(n+1\) 个控制点 \(\mathbf{P}_0, \mathbf{P}_1, \ldots, \mathbf{P}_n\)，\(p\) 阶 B 样条曲线为：

\[ \mathbf{C}(t) = \sum_{i=0}^{n} N_{i,p}(t) \, \mathbf{P}_i \]

对于三次均匀 B 样条，当 \(t\) 落在第 \(k\) 个节点区间 \([t_k, t_{k+1})\) 时：

\[ \mathbf{C}(t) = \begin{bmatrix} \mathbf{P}_{k-1} & \mathbf{P}_k & \mathbf{P}_{k+1} & \mathbf{P}_{k+2} \end{bmatrix} \cdot \mathbf{M}_3^\top \cdot \begin{bmatrix} s^3 \\ s^2 \\ s \\ 1 \end{bmatrix} \cdot \frac{1}{6} \]

只涉及 **4 个**控制点——这就是**局部支撑**性质在求值中的体现。

\(p=5\)（五次）均匀 B 样条——Fast-Planner 的选择：

五次 B 样条保证 \(C^4\) 连续性（位置、速度、加速度、jerk 均连续），适合优化 snap（四阶导数）。其基矩阵为 \(6 \times 6\)：

\[ \mathbf{M}_5 = \frac{1}{120} \begin{bmatrix} 1 & -5 & 10 & -10 & 5 & -1 \\ 26 & -50 & 20 & 20 & -20 & 5 \\ 66 & 0 & -60 & 0 & 30 & -10 \\ 26 & 50 & 20 & -20 & -20 & 10 \\ 1 & 5 & 10 & 10 & 5 & -5 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \]

为什么 Fast-Planner 选 \(p=5\) 而 EGO-Planner 降到 \(p=3\)？

选择	连续性	每段控制点数	矩阵运算量	动机
\(p=5\)	\(C^4\)（snap 连续）	6	\(6^2 = 36\) 次乘加	微分平坦映射需要到 snap 级别
\(p=3\)	\(C^2\)（加速度连续）	4	\(4^2 = 16\) 次乘加	在线规划中 jerk 连续已足够，计算快 2.25 倍

EGO-Planner 降阶的工程判断是：PX4 的位置控制器只需要到加速度级别的指令，\(C^2\) 已经足够；而 \(p=3\) 的梯度计算更快，每段只涉及 4 个控制点，稀疏性更好。

§D4.2.3 B 样条的五大核心性质——无人机规划为什么需要它们 ⭐⭐¶

性质 1：凸包性（Convex Hull Property）

对于 \(p\) 阶 B 样条，任意节点区间 \([t_k, t_{k+1})\) 上的曲线段包含在该段所涉及的 \(p+1\) 个控制点的凸包内。

证明： 1. 非负性：\(N_{i,p}(t) \geq 0\)（由递推定义和归纳法可证：0 阶非负，权重 \(\frac{t-t_i}{t_{i+p}-t_i}\) 和 \(\frac{t_{i+p+1}-t}{t_{i+p+1}-t_{i+1}}\) 在支撑区间内均非负，非负数的非负线性组合仍非负） 2. 单位分解：\(\sum_i N_{i,p}(t) = 1\)（由递推和归纳法：0 阶满足，递推保持） 3. 由 1 和 2，曲线点 \(\mathbf{C}(t) = \sum_i N_{i,p}(t) \mathbf{P}_i\) 是控制点的**凸组合** 4. 凸组合的值必然在这些控制点的凸包内 \(\square\)

为什么对无人机规划至关重要？ 这是 B 样条轨迹优化**可计算性**的根本保证：

如果所有控制点都在障碍物之外（安全距离 > d_safe），
那么整条曲线段也在障碍物之外！

→ 约束控制点 = 约束曲线
→ 优化变量从"无穷多曲线点"减少为"有限个控制点"
→ 碰撞约束从函数空间降到有限维向量空间

与 Bezier 曲线的对比：Bezier 曲线的凸包是**全局**的（所有控制点的凸包），而 B 样条的凸包是**局部**的（只有 \(p+1\) 个控制点的凸包）。局部凸包更紧凑，碰撞检查的虚警率更低。这不是 B 样条"碰巧更好"——而是局部支撑性质的直接数学推论。

性质 2：局部支撑（Local Support）

移动控制点 \(\mathbf{P}_i\) 只影响参数区间 \([t_i, t_{i+p+1})\) 内的曲线；反过来，参数区间 \([t_k, t_{k+1})\) 内的曲线只受 \(\mathbf{P}_{k-p}, \ldots, \mathbf{P}_k\) 共 \(p+1\) 个控制点影响。

计算意义：

维度	含义
Jacobian 稀疏性	\(\partial \mathbf{C}(t) / \partial \mathbf{P}_i\) 是带状矩阵，带宽 \(p+1\)
并行性	不相邻的控制点可以独立优化
增量更新	碰撞检测只需检查受影响的 \(p+1\) 段，而非全部

这就是为什么 EGO-Planner 的碰撞代价可以对**单个控制点**计算梯度，而不需要考虑整条轨迹——局部支撑保证了梯度的稀疏传播。

性质 3：自动连续性（Automatic Continuity）

\(p\) 阶均匀 B 样条在每个内部节点处自动满足 \(C^{p-1}\) 连续性——无需任何额外约束！

阶数 \(p\)	连续性	物理含义
1	\(C^0\)	位置连续（速度可能跳变）——不可用
2	\(C^1\)	速度连续（加速度可能跳变）——几乎不可用
3	\(C^2\)	加速度连续（jerk 可能跳变）——EGO-Planner
5	\(C^4\)	snap 连续——Fast-Planner

与多项式轨迹（D3）的关键对比：

多项式: 10 段 × 8 系数 = 80 个优化变量 + 9 × 4 = 36 个拼接等式约束
         → 需要解 KKT 系统（QP 求解器）

B 样条: 15 个控制点 × 3 维 = 45 个优化变量
         → 连续性自动满足，无等式约束
         → 无约束优化（L-BFGS），或只有不等式约束

本质洞察：自动连续性不是 B 样条的"附加特性"，而是其基函数构造的**逻辑必然**。Cox-de Boor 递推的每一步都是"加权混合"——加权混合天然产生光滑性，因为权重函数本身是 \(p-1\) 次多项式。连续性不是约束带来的，而是基函数的**内在结构**保证的。

性质 4：导数与低阶 B 样条的关系

\(p\) 阶 B 样条曲线的 \(k\) 阶导数仍是 B 样条，阶数为 \(p-k\)，控制点为原控制点的**差分**。

对于均匀节点距 \(\Delta t\)：

\[ \dot{\mathbf{C}}(t) = \frac{1}{\Delta t} \sum_{i=0}^{n-1} N_{i,p-1}(t) \cdot (\mathbf{P}_{i+1} - \mathbf{P}_i) \]

推广到高阶：

物理量	控制点公式	数量
位置	\(\mathbf{P}_i\)	\(n+1\)
速度	\(\mathbf{V}_i = (\mathbf{P}_{i+1} - \mathbf{P}_i)/\Delta t\)	\(n\)
加速度	\(\mathbf{A}_i = (\mathbf{P}_{i+2} - 2\mathbf{P}_{i+1} + \mathbf{P}_i)/\Delta t^2\)	\(n-1\)
Jerk	\(\mathbf{J}_i = (\mathbf{A}_{i+1} - \mathbf{A}_i)/\Delta t\)	\(n-2\)
Snap	\(\mathbf{S}_i = (\mathbf{J}_{i+1} - \mathbf{J}_i)/\Delta t\)	\(n-3\)

关键推论——动力学可行性约束变成控制点的线性不等式：

速度约束 \(|\dot{\mathbf{C}}(t)| \leq v_\max \;\forall t\) 结合凸包性质，退化为有限个线性不等式：

\[ \frac{|\mathbf{P}_{i+1} - \mathbf{P}_i|}{\Delta t} \leq v_\max \quad \forall i \]

同理，加速度约束 \(|\ddot{\mathbf{C}}(t)| \leq a_\max\) 变为：

\[ \frac{|\mathbf{P}_{i+2} - 2\mathbf{P}_{i+1} + \mathbf{P}_i|}{\Delta t^2} \leq a_\max \quad \forall i \]

这就是 Fast-Planner 的 \(J_\text{feasibility}\) 代价项的数学基础！无穷维的连续约束被精确地降维为有限个不等式。

性质 5：节点插入不改变曲线形状

在节点向量中插入一个新节点 \(\bar{t}\)，通过 Boehm 算法计算新的控制点，得到的曲线与原曲线**完全相同**——只是控制多边形更精细了。

Boehm 算法公式：设在 \([t_k, t_{k+1})\) 中插入 \(\bar{t}\)，新控制点 \(\bar{\mathbf{P}}_i\) 为：

\[ \bar{\mathbf{P}}_i = \begin{cases} \mathbf{P}_i & i \leq k - p \\ \alpha_i \mathbf{P}_i + (1 - \alpha_i) \mathbf{P}_{i-1} & k-p+1 \leq i \leq k \\ \mathbf{P}_{i-1} & i \geq k+1 \end{cases} \]

其中 \(\alpha_i = (\bar{t} - t_i) / (t_{i+p} - t_i)\)。

规划中的应用：Fast-Planner 的迭代时间调整（Iterative Time Adjustment）使用节点插入来增加控制点数量而不改变曲线形状——当动力学约束不满足时，增大 \(\Delta t\) 并插入节点保持曲线不变，然后用新的 \(\Delta t\) 重新检查约束。

§D4.2.4 de Boor 算法——高效且数值稳定的求值 ⭐⭐¶

de Boor 算法是 B 样条求值的标准方法，类似于 Bezier 曲线的 de Casteljau 算法。

算法步骤：

输入: 阶数 p, 节点向量 {t_i}, 控制点 {P_i}, 参数值 t

1. 找到 t 所在的节点区间: t ∈ [t_k, t_{k+1})

2. 提取影响该区间的 p+1 个控制点:
   d_i^{[0]} = P_i,  i = k-p, ..., k

3. 迭代 r = 1, 2, ..., p:
   for i = k-p+r to k:
     α_{i,r} = (t - t_i) / (t_{i+p-r+1} - t_i)
     d_i^{[r]} = (1 - α_{i,r}) · d_{i-1}^{[r-1]} + α_{i,r} · d_i^{[r-1]}

4. 结果: C(t) = d_k^{[p]}

三角形表（以 \(p=3\) 为例）：

d_{k-3}^{[0]}
               → d_{k-2}^{[1]}
d_{k-2}^{[0]}                  → d_{k-1}^{[2]}
               → d_{k-1}^{[1]}                  → d_k^{[3]} = C(t)
d_{k-1}^{[0]}                  → d_k^{[2]}
               → d_k^{[1]}
d_k^{[0]}

对均匀节点的简化：当 \(t_i = i \cdot \Delta t\) 时，\(\alpha_{i,r}\) 简化为：

\[ \alpha_{i,r} = \frac{s + (k - i)}{p - r + 1}, \quad s = \frac{t - t_k}{\Delta t} \]

所有 \(\alpha\) 只取决于归一化参数 \(s\) 和索引偏移，可以预计算。

实现（Eigen 风格，教学简化版）：

// 均匀三次 B 样条求值（p=3）
// 输入: ctrl_pts (Eigen::MatrixXd, n×3), t (double), dt (double)
// 输出: 位置 pos (Eigen::Vector3d)
Eigen::Vector3d evaluateUniformBSpline(
    const Eigen::MatrixXd& ctrl_pts,  // n×3 控制点矩阵
    double t,                          // 参数值
    double dt,                         // 节点距 Δt
    int degree = 3)                    // B 样条阶数
{
    int p = degree;
    int k = static_cast<int>(t / dt);    // 节点区间索引
    double s = (t - k * dt) / dt;         // 归一化参数 s ∈ [0,1)

    // 确保 k 在有效范围内
    k = std::max(p, std::min(k, static_cast<int>(ctrl_pts.rows()) - 1));

    // 提取 p+1 个控制点
    Eigen::MatrixXd d(p + 1, 3);
    for (int i = 0; i <= p; ++i) {
        d.row(i) = ctrl_pts.row(k - p + i);
    }

    // de Boor 迭代——三角形计算结构
    for (int r = 1; r <= p; ++r) {
        for (int i = p; i >= r; --i) {
            double alpha = (s + (p - i))
                         / static_cast<double>(p - r + 1);
            d.row(i) = (1.0 - alpha) * d.row(i - 1)
                      + alpha * d.row(i);
        }
    }

    return d.row(p).transpose();
}

或者用矩阵形式（三次，Fast-Planner/EGO-Planner 中更常用）：

Eigen::Vector3d evaluateCubicBSplineMatrix(
    const Eigen::MatrixXd& ctrl_pts,  // n×3 控制点
    int k,       // 节点区间索引
    double s)    // 归一化参数 s ∈ [0,1)
{
    // 基矩阵 M_3 —— 硬编码避免每次重新构造
    Eigen::Matrix4d M;
    M << -1,  3, -3,  1,
          3, -6,  3,  0,
         -3,  0,  3,  0,
          1,  4,  1,  0;
    M /= 6.0;

    // 参数向量 [s^3, s^2, s, 1]
    Eigen::Vector4d S(s*s*s, s*s, s, 1.0);

    // 基函数值 = M^T · S
    Eigen::Vector4d N = M.transpose() * S;

    // 加权求和（只用 4 个控制点——局部支撑）
    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
    for (int i = 0; i < 4; ++i) {
        pos += N(i) * ctrl_pts.row(k - 3 + i).transpose();
    }
    return pos;
}

工程注意事项： - Fast-Planner 使用矩阵形式（预计算 \(\mathbf{M}_p\)），因为 Eigen 的 SIMD 向量化在 \(4 \times 4\) 和 \(6 \times 6\) 矩阵乘法上效率极高 - 避免使用递推形式的 Cox-de Boor，因为它需要递归调用，不利于 CPU 流水线和内联 - Eigen::Matrix<double, 4, 4> 固定大小类型比 Eigen::MatrixXd 动态类型快 3-5 倍（栈分配 vs 堆分配）

§D4.2.5 B 样条导数的矩阵表示 ⭐⭐¶

对于均匀三次 B 样条，位置、速度、加速度**共享同一个基矩阵 \(\mathbf{M}_3\)**——只需改变右侧的参数向量：

\[ \dot{\mathbf{C}}(s) = \frac{1}{\Delta t} \cdot \begin{bmatrix} \mathbf{P}_{k-3} & \mathbf{P}_{k-2} & \mathbf{P}_{k-1} & \mathbf{P}_k \end{bmatrix} \cdot \mathbf{M}_3^\top \cdot \begin{bmatrix} 3s^2 \\ 2s \\ 1 \\ 0 \end{bmatrix} \]

\[ \ddot{\mathbf{C}}(s) = \frac{1}{\Delta t^2} \cdot \begin{bmatrix} \mathbf{P}_{k-3} & \mathbf{P}_{k-2} & \mathbf{P}_{k-1} & \mathbf{P}_k \end{bmatrix} \cdot \mathbf{M}_3^\top \cdot \begin{bmatrix} 6s \\ 2 \\ 0 \\ 0 \end{bmatrix} \]

关键观察：导数只是把参数向量从 \([s^3, s^2, s, 1]^\top\) 换成其导数——基矩阵 \(\mathbf{M}_3\) 不变！这意味着**位置、速度、加速度共享同一个矩阵乘法**，只需改变右侧向量——极大减少了代码重复和计算量。

// 统一的位置/速度/加速度计算（教学简化版）
struct BSplineEval {
    Eigen::Vector3d pos, vel, acc;
};

BSplineEval evaluateAll(const Eigen::MatrixXd& ctrl_pts,
                        int k, double s, double dt)
{
    // 基矩阵 M_3 —— 实际代码中应为类的静态成员
    Eigen::Matrix4d M;
    M << -1,  3, -3,  1,
          3, -6,  3,  0,
         -3,  0,  3,  0,
          1,  4,  1,  0;
    M /= 6.0;

    // 四个控制点拼成 4×3 矩阵
    Eigen::Matrix<double, 4, 3> P;
    for (int i = 0; i < 4; ++i)
        P.row(i) = ctrl_pts.row(k - 3 + i);

    // PM = P^T · M^T，预计算一次，三种导数共享
    Eigen::Matrix<double, 3, 4> PM = (M * P).transpose();

    BSplineEval result;
    result.pos = PM * Eigen::Vector4d(s*s*s, s*s, s, 1.0);
    result.vel = PM * Eigen::Vector4d(3*s*s, 2*s, 1.0, 0.0) / dt;
    result.acc = PM * Eigen::Vector4d(6*s, 2.0, 0.0, 0.0) / (dt * dt);
    return result;
}

⚠️ 常见陷阱¶

⚠️ 编程陷阱：节点区间索引 \(k\) 的边界处理 错误做法：直接用 k = (int)(t / dt) 而不做边界检查 现象/后果：当 \(t\) 恰好等于最后一个有效节点时，\(k\) 越界，访问控制点数组越界导致段错误（segfault）或垃圾值 根本原因：B 样条的有效参数域是 \([t_p, t_{n+1}]\)（对 \(p\) 阶、\(n+1\) 个控制点），不是 \([t_0, t_m]\)。首尾各有 \(p\) 个节点区间没有对应的曲线段 正确做法：k = std::clamp(k, p, n) 并在 s = 1.0 时特殊处理（归入前一段或使用 \(s = 1.0 - \epsilon\)）

⚠️ 概念误区：认为控制点就是曲线上的点 错误做法：把控制点当作航路点，期望曲线精确通过 现象/后果：规划出的轨迹不经过预期的航路点，偏差可达控制点间距的 30% 根本原因：B 样条曲线在控制点的**凸包内**，但不**经过**控制点（除非使用 clamped 节点向量的首尾点）。这与 Lagrange 插值多项式的行为完全不同 正确做法：如果需要精确插值，要么使用 clamped 节点向量，要么添加 \(\mathbf{C}(t_k) = \mathbf{W}_k\) 等式约束（但这会失去无约束优化的优势），要么使用 MINCO（D5）

⚠️ 思维陷阱：凸包性质给出的约束是精确的 错误做法：认为 \(|\mathbf{V}_i| \leq v_\max\) 是速度约束的充要条件 现象/后果：轨迹满足所有控制点级别的约束，但实际速度仍有少量超限 根本原因：凸包约束是**充分条件而非必要条件**——它比严格需要的更保守。实际速度的最大值可能小于控制点级别约束所暗示的上界 正确做法：理解这是保守性的代价。如果保守性不可接受，需要在曲线上密采样检查实际值

练习¶

（手算题） 给定控制点 \(P = \{(0,0), (1,2), (3,3), (4,1), (5,0)\}\)，均匀节点 \(\{0,1,2,3,4,5,6,7\}\)，手工用 de Boor 算法计算 \(C(3.5)\) 的值（三次）。验证结果是否在 \(P_1, P_2, P_3, P_4\) 的凸包内。
（编程题） 从零用 Eigen 实现一个三次均匀 B 样条评估器：给定 10 个控制点，评估 100 个采样点的位置、速度、加速度。用有限差分 \(\dot{C}(t) \approx (C(t+\epsilon) - C(t-\epsilon)) / 2\epsilon\) 验证解析导数的正确性。
（证明题） 证明：对均匀三次 B 样条，基矩阵 \(\mathbf{M}_3\) 的每列之和为 \([0, 0, 0, 6]^\top\)。这个性质的物理含义是什么？（提示：它与单位分解有关）

§D4.3 Fast-Planner 架构——无人机规划的"文件夹圣经" ⭐⭐⭐¶

动机：为什么需要一个完整的规划框架¶

B 样条提供了轨迹的数学表示，但从"有一个表示"到"无人机能飞"还差很远。你需要回答：环境信息从哪来（地图）？初始路径怎么找（搜索）？代价函数怎么设计（优化）？什么时候触发重规划（调度）？

Fast-Planner（Zhou et al., 2019）的历史贡献不仅是某个算法的改进，而是**定义了一套完整的软件架构范式**——后续的 EGO-Planner、FUEL、RACER、RAPTOR 全部沿用了这套四包分层结构。理解 Fast-Planner 的架构，就理解了整个 HKUST/ZJU 无人机规划代码族的骨架。

§D4.3.1 代码目录结构详解 ⭐⭐¶

fast_planner/
├── plan_env/           ← ESDF 地图（flat array 体素网格 + 射线投射更新）
│   ├── sdf_map.cpp     ← 增量 ESDF（双波前 BFS）
│   ├── sdf_map.h       ← SDFMap 类：体素网格 + 距离场 + 梯度场
│   ├── raycast.h       ← Bresenham 3D 射线
│   ├── edt_environment.h ← 环境接口：封装 SDF 查询 + 梯度查询
│   └── obj_predictor.h  ← 动态障碍物预测（线性/多项式外推）
├── path_searching/     ← kinodynamic A*（加速度原语 + PMP 启发式）
│   ├── kinodynamic_astar.cpp  ← ~800 行核心搜索
│   ├── kinodynamic_astar.h    ← PathNode 结构 + 开放/关闭列表
│   └── astar.h                ← 普通 A* 备用
├── bspline_opt/        ← B 样条优化（NLopt 后端 + 解析梯度）
│   ├── bspline_optimizer.cpp  ← 代价函数组装
│   ├── bspline_optimizer.h    ← 优化器类：代价权重 + NLopt 配置
│   └── uniform_bspline.h      ← B 样条表示：控制点 + 节点距 + 求值
├── plan_manage/        ← FSM 调度（状态机）
│   ├── planner_manager.cpp    ← 管线编排：搜索→优化→发布
│   ├── planner_manager.h      ← 持有所有子模块的指针
│   ├── kino_replan_fsm.cpp    ← 有限状态机实现
│   └── kino_replan_fsm.h      ← FSM 状态枚举 + 转移条件
└── traj_utils/         ← 轨迹工具
    ├── planning_visualization.h ← RViz 可视化
    └── polynomial_traj.h      ← 多项式轨迹（对比用）

给 SLAM 工程师的类比（像的部分与不像的部分）： - plan_env/ 类似 ORB-SLAM3 的 Map 模块：维护环境的几何表示。像**的地方：都是后端的"地图数据库"；**不像**的地方：ESDF 是稠密的体素网格，而 ORB-SLAM3 的 Map 是稀疏的特征点+关键帧 - path_searching/ 类似 Tracking 模块：快速找到初始估计。**像：都是"前端"，速度优先于精度；不像：A* 搜索的是路径，Tracking 估计的是位姿 - bspline_opt/ 类似 LocalMapping + LoopClosing：精细化初始估计 - plan_manage/ 类似 System 模块：编排整个管线的生命周期

§D4.3.2 ESDF 建图——plan_env 的核心 ⭐⭐⭐¶

什么是 ESDF（Euclidean Signed Distance Field，欧氏有符号距离场）？

ESDF 是三维体素网格，每个体素存储到最近障碍物表面的**有符号欧氏距离**： - 正值：自由空间，值为到最近障碍表面的距离 - 负值：障碍物内部，值为到最近自由表面的距离（绝对值） - 零：恰好在障碍物表面上

ESDF 的梯度 \(\nabla d(\mathbf{x})\) 指向离 \(\mathbf{x}\) 最近的障碍表面的法方向，模长为 1（除了 Voronoi 脊线上）。这个梯度正是碰撞代价 \(J_\text{collision}\) 的核心驱动力——它告诉优化器"往哪个方向移动控制点可以远离障碍物"。

读到这里你可能会问："为什么不直接用占据栅格（Occupancy Grid）？它只需要一个 bit 表示占据/自由，比 ESDF 省得多。"答案是：占据栅格只能告诉你"碰没碰"，不能告诉你"距碰撞还有多远"和"往哪个方向远离碰撞"。梯度优化需要**连续的标量场和它的梯度**——ESDF 恰好提供这两者。EGO-Planner 后来证明可以绕过 ESDF，但它的替代方案（\((p,v)\) 锚点）本质上是在"按需"局部构造一个**线性近似**的距离场。

增量 ESDF 更新——双波前 BFS：

Fast-Planner 的 ESDF 更新算法受 Voxblox（Oleynikova et al., IROS 2017）和 FIESTA（Han et al., IROS 2019）启发，使用双波前 BFS 实现增量更新：

算法: IncrementalESDFUpdate(changed_voxels)

输入: 变化的占据栅格体素列表
输出: 更新后的 ESDF

1. 初始化两个 BFS 队列:
   raise_queue ← {}   // "抬升"队列：距离需要增大的体素
   lower_queue ← {}   // "降低"队列：距离需要减小的体素

2. 对每个 changed_voxel:
   if 从自由变为占据:
     该体素距离 = 0
     将其加入 lower_queue（邻居可能需要减小距离）
   if 从占据变为自由:
     该体素距离 = +∞
     将其加入 raise_queue（邻居可能需要增大距离）

3. Raise 阶段（BFS 传播）:
   while raise_queue 非空:
     v = raise_queue.pop()
     for each 邻居 n of v:
       if n.closest_obstacle == v.closest_obstacle:
         n.distance = +∞   // n 之前依赖的障碍物已消失
         raise_queue.push(n)
       else:
         lower_queue.push(n)

4. Lower 阶段（BFS 传播）:
   while lower_queue 非空:
     v = lower_queue.pop()
     for each 邻居 n of v:
       d_new = dist(n.position, v.closest_obstacle)
       if d_new < n.distance:
         n.distance = d_new
         n.closest_obstacle = v.closest_obstacle
         lower_queue.push(n)

时间复杂度：\(O(K)\)，其中 \(K\) 是受影响的体素数（而非整个网格 \(O(N^3)\)）。但在无人机快速移动时，\(K\) 可能很大。

ESDF 的计算瓶颈：一次完整的 ESDF 更新约需 7-10 ms（\(256^3\) 网格，Intel i7），占整个规划周期的 ~70%。即使使用增量更新，传播范围大时仍然昂贵。这正是 EGO-Planner 要消除 ESDF 的根本原因。

§D4.3.3 kinodynamic A*——PMP 启发式的路径搜索 ⭐⭐⭐¶

kinodynamic A* 的独特之处：不是在位置空间搜索，而是在**状态空间** \((x, v)\)（位置+速度）中搜索，扩展原语是**加速度指令** \(u\)。

状态转移：双积分器模型 \(\ddot{x} = u\)

\[ \begin{aligned} x_1 &= x_0 + v_0 \tau + \frac{1}{2} u \tau^2 \\ v_1 &= v_0 + u \tau \end{aligned} \]

扩展原语：加速度在每个轴上取 \(\{-a_\max, 0, +a_\max\}\)，形成 \(3^3 = 27\) 个原语（减去全零 = 26）。

PMP 启发式（Pontryagin's Minimum Principle heuristic）——Fast-Planner 的核心理论贡献：

对于双积分器 \(\ddot{x} = u\)，从状态 \((x_0, v_0)\) 到 \((x_f, v_f)\) 的最优控制问题：

\[ \min_u \int_0^T \|u(t)\|^2 \, dt \quad \text{s.t.} \quad \ddot{x} = u \]

由 Pontryagin 最优性原理，最优控制为三次多项式，闭式最优代价为：

\[ J^*(T) = \frac{1}{T^3} \left[ 36 \Delta x^2 - 36 \Delta x (v_0 + v_f) T + 12 (v_0^2 + v_0 v_f + v_f^2) T^2 \right] + T \]

将 \(J^*(T^*)\) 作为 A* 的启发函数 \(h(n)\)——这不是欧氏距离，而是考虑了速度约束的**动力学最优代价**。它是真实代价的紧下界，剪枝效率比欧氏距离高 3-5 倍。

解析单步扩展（Analytic Expansion）：当搜索节点"足够接近"目标时，直接用 PMP 闭式解生成一条从当前节点到目标的最优轨迹，跳过中间搜索。这个技巧使得搜索节点数从 ~5000 降到 ~500。

§D4.3.4 B 样条优化——代价函数设计 ⭐⭐⭐¶

Fast-Planner 的 B 样条优化目标：

\[ \min_{\mathbf{Q}} \quad J = \lambda_s J_\text{smooth} + \lambda_c J_\text{collision} + \lambda_d J_\text{dynamic} + \lambda_f J_\text{feasibility} \]

其中 \(\mathbf{Q} = \{\mathbf{Q}_1, \mathbf{Q}_2, \ldots, \mathbf{Q}_{n-1}\}\) 是**自由控制点**（首尾固定）。

代价项 1：平滑性 \(J_\text{smooth}\)——利用导数=控制点差分的性质：

\[ J_\text{smooth} = \sum_{i=0}^{n-3} \| \mathbf{Q}_{i+3} - 3\mathbf{Q}_{i+2} + 3\mathbf{Q}_{i+1} - \mathbf{Q}_i \|^2 \]

这是**三阶差分的 L2 范数**，等价于 jerk 的积分 \(\propto \int \| \dddot{\mathbf{C}}(t) \|^2 dt\)。

梯度：控制点 \(\mathbf{Q}_j\) 参与的差分项最多 4 个，梯度为：

\[ \frac{\partial J_\text{smooth}}{\partial \mathbf{Q}_j} = \sum_{i \in \mathcal{I}(j)} 2 c_i (\mathbf{Q}_{i+3} - 3\mathbf{Q}_{i+2} + 3\mathbf{Q}_{i+1} - \mathbf{Q}_i) \]

其中 \(c_i \in \{1, -3, 3, -1\}\) 取决于 \(\mathbf{Q}_j\) 在第 \(i\) 个差分中的位置。

// bspline_optimizer.cpp 中的平滑性代价（教学简化版）
void BsplineOptimizer::calcSmoothnessCost(
    const vector<Eigen::Vector3d>& q,   // 控制点
    double& cost,                        // 累积代价
    vector<Eigen::Vector3d>& gradient)   // 累积梯度
{
    cost = 0.0;
    int n = q.size();

    for (int i = 0; i < n - 3; ++i) {
        // 三阶差分 = jerk 的离散近似
        Eigen::Vector3d jerk = q[i+3] - 3*q[i+2] + 3*q[i+1] - q[i];
        cost += jerk.squaredNorm();

        // 梯度传播到 4 个控制点（链式法则）
        gradient[i]   += 2.0 * (-1.0) * jerk;   // ∂/∂Q_i
        gradient[i+1] += 2.0 * ( 3.0) * jerk;   // ∂/∂Q_{i+1}
        gradient[i+2] += 2.0 * (-3.0) * jerk;   // ∂/∂Q_{i+2}
        gradient[i+3] += 2.0 * ( 1.0) * jerk;   // ∂/∂Q_{i+3}
    }
}

代价项 2：碰撞 \(J_\text{collision}\)——利用凸包性质，只需检查控制点处的 ESDF 距离：

\[ J_\text{collision} = \sum_{j=1}^{n-1} c_\text{col}(\mathbf{Q}_j), \quad c_\text{col}(\mathbf{Q}) = \begin{cases} (d_\text{safe} - d(\mathbf{Q}))^3 & \text{if } d(\mathbf{Q}) < d_\text{safe} \\ 0 & \text{otherwise} \end{cases} \]

使用三次惩罚函数而非二次的原因：三次在边界处有 \(C^2\) 连续性（避免梯度突变），且惩罚力度随侵入深度非线性增长。

梯度（使用 ESDF 梯度）：

\[ \frac{\partial J_\text{collision}}{\partial \mathbf{Q}_j} = -3(d_\text{safe} - d(\mathbf{Q}_j))^2 \cdot \nabla d(\mathbf{Q}_j) \]

\(\nabla d(\mathbf{Q}_j)\) 由 ESDF 的三线性插值提供——这就是 ESDF 在优化中的角色：提供解析梯度方向。

代价项 3&4：动力学可行性 \(J_\text{dynamic}\) + \(J_\text{feasibility}\)——利用导数=控制点差分：

\[ J_\text{dynamic} = \sum_j \max\!\left(0, \, \frac{|\Delta^1 \mathbf{Q}_j|}{\Delta t} - v_\max \right)^2 + \sum_j \max\!\left(0, \, \frac{|\Delta^2 \mathbf{Q}_j|}{\Delta t^2} - a_\max \right)^2 \]

这不是硬约束而是**软惩罚**——允许轻微超限，权重 \(\lambda_d\) 控制松紧度。

§D4.3.5 FSM 调度——plan_manage 的状态机 ⭐⭐¶

           ┌──────────┐
           │   INIT   │ ← 系统启动，等待地图和定位
           └────┬─────┘
                │ 地图+定位就绪
           ┌────v─────┐
           │WAIT_TARGET│ ← 等待用户/任务发送目标点
           └────┬─────┘
                │ 收到目标
           ┌────v──────────┐
           │ GEN_NEW_TRAJ  │ ← 全新轨迹生成
           │               │   1. kinodynamic A* 搜索
           │               │   2. B 样条拟合搜索路径
           │               │   3. NLopt 优化
           │               │   4. 时间调整
           └────┬──────────┘
                │ 轨迹生成成功
           ┌────v──────────┐
           │  EXEC_TRAJ    │ ← 执行轨迹（发布到控制器）
           │               │   定时检查：
           │               │   - 是否接近终点？→ WAIT_TARGET
           │               │   - 前方是否有新障碍？→ REPLAN_TRAJ
           └────┬──────────┘
                │ 检测到新障碍 / 偏离轨迹
           ┌────v──────────┐
           │ REPLAN_TRAJ   │ ← 增量重规划（热启动，不需要重新搜索）
           └───────────────┘
                │ 重规划成功 → EXEC_TRAJ

§D4.3.6 完整数据流——从深度图到控制指令 ⭐⭐¶

深度相机 → depth_callback() → SDFMap::inputPointCloud()
         → SDFMap::updateESDF()  [~7ms, 双波前 BFS]
         → FSM::execCallback()   [~50Hz 定时器]
         → PlannerManager::planGlobalTraj()
              ├─▶ KinodynamicAstar::search()  [~3ms]
              ├─▶ UniformBSpline::fitPath()    [~0.5ms]
              ├─▶ BsplineOptimizer::optimize() [~5ms, NLopt]
              └─▶ UniformBSpline::reallocateTime()
         → 发布 /planning/bspline
         → TrajServer 按 Δt 采样 pos/vel/acc
         → PX4/mavros 执行

端到端延迟分析：

阶段	典型耗时	占比
ESDF 更新	~7 ms	47%
kinodynamic A*	~3 ms	20%
B 样条拟合	~0.5 ms	3%
B 样条优化	~5 ms	33%
FSM 调度	~0.1 ms	<1%
总计	~15 ms	100%

关键观察：ESDF 占了近一半的时间——这正是 EGO-Planner 的出发点。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：NLopt 回调函数的静态成员访问 错误做法：在 NLopt 的静态回调函数中直接访问类的非静态成员 现象/后果：编译错误（this 指针不可用），或使用全局变量导致多线程竞争 根本原因：NLopt 的 C 接口要求回调函数是 static 或 C 函数指针。C++ 类的成员函数隐含 this 指针，签名不匹配 正确做法：通过 NLopt 的 void* data 参数传递类实例指针，在静态回调中 reinterpret_cast 回来。这是 C 回调与 C++ OOP 桥接的标准模式

⚠️ 概念误区：认为碰撞代价用二次惩罚比三次更好（更平滑） 错误做法：把 \((d_s - d)^3\) 改为 \((d_s - d)^2\) 现象/后果：控制点在安全边界 \(d = d_s\) 附近的梯度变为零（二次函数在零点的导数为零），优化器无法有效将控制点推出障碍物 根本原因：三次惩罚在 \(d = d_s\) 处的一阶导数为零但二阶导数不为零，提供了 \(C^2\) 的平滑过渡；且当 \(d \ll d_s\) 时，三次增长比二次更陡，提供更强的"推力" 正确做法：保持三次惩罚。如果需要更强的推力，增大 \(\lambda_c\) 而非改变惩罚函数的阶数

练习¶

（架构分析题） 画出 Fast-Planner 的完整 ROS 数据流图：从深度相机 topic 到 mavros 控制指令 topic，标注每一步的 topic 名称和消息类型。哪些步骤是同步的？哪些是异步的？
（代价函数推导题） 对于 8 个自由控制点的三次 B 样条，手工写出 \(J_\text{smooth}\) 的完整展开式（包含多少个三阶差分项？每个控制点参与多少个差分项？），并对 \(\mathbf{Q}_4\) 求解析梯度。

§D4.4 拓扑路径枚举——解决 B 样条优化的局部极小问题 ⭐⭐⭐¶

动机：梯度下降的宿命¶

B 样条优化是**非凸的**——碰撞代价 \(J_\text{collision}\) 引入了非凸性。在复杂环境（U 型、窄缝、柱群）中，不同的初始路径会收敛到不同的局部极小。

       ┌─────────┐
       │ 障碍物   │
       │          │
   S ──┤          ├── G
       │          │
       └─────────┘

   路径 A（上方绕行）：cost = 12.5
   路径 B（下方绕行）：cost = 10.3  ← 全局最优
   → kinodynamic A* 只给出一条路径
   → 如果给出路径 A，优化只能收敛到 A 附近的局部极小

如果不解决局部极小会怎样¶

在开阔场景中，局部极小通常就是全局最优（只有一条合理路径）。但在以下场景中，单路径方法会导致严重问题：

场景	单路径成功率	后果
U 型障碍	~72%	轨迹卡在 U 型内壁，无法"看到"从另一侧绕行的选项
窄缝+柱群	~55%	误选穿缝路径但无法通过，而安全的绕行路径被忽略
对称环境	~80%	两条等代价路径，搜索器随机选择，结果不稳定

同伦类与拓扑等价¶

直觉：两条路径是"拓扑等价"的，当且仅当一条可以通过连续形变（不穿过障碍物）变成另一条。不同的**同伦类**对应**拓扑上本质不同**的路径——从一个类到另一个类必须"翻越"障碍物。

Fast-Planner（ICRA 2020 版本）的拓扑路径枚举分三步：建立 Topological PRM → 用 UVD（Uniform Visibility Deformation）判定同伦类 → 每个类选代表路径并行优化。

效果：成功率从 55%（单路径）提升到 88%（多路径），代价是计算时间增加约 3 倍（~15 ms → ~45 ms）。

⚠️ 常见陷阱¶

⚠️ 思维陷阱：认为拓扑路径枚举总是必要的 错误做法：在所有场景中都启用多路径搜索 现象/后果：开阔场景中计算时间增加 3 倍但路径质量几乎不变 根本原因：在凸形或近凸形自由空间中，只存在一个同伦类，多路径搜索全部收敛到同一解 正确做法：根据环境复杂度动态切换——用障碍物密度或自由空间的拓扑 genus 判断是否需要多路径

练习¶

（分析题） 在什么样的环境拓扑下，多同伦类路径枚举是必须的？在什么环境下，单路径 + 随机重启可以替代？形式化这个条件。

§D4.5 EGO-Planner 的 ESDF-free 创新 ⭐⭐⭐¶

动机：ESDF 是过度计算¶

回顾 Fast-Planner 的碰撞代价：优化器查询 ESDF 来获取距离 \(d(\mathbf{Q}_j)\) 和梯度 \(\nabla d(\mathbf{Q}_j)\)。但仔细看利用率：

ESDF 计算了 256³ ≈ 1670 万个体素的距离值
B 样条优化只查询 ~15 个控制点处的距离值

利用率 = 15 / 16,700,000 ≈ 0.0001%

→ 99.9999% 的 ESDF 计算是浪费的！

EGO-Planner（Zhou, Wang, Ye, Xu, Gao, RA-L 2021）的核心思想：不计算整个 ESDF，而是**按需**为每个碰撞控制点计算一个"伪梯度"——只需要一个障碍物表面点 \(\mathbf{p}\) 和法向量 \(\mathbf{v}\)。

本质洞察：EGO-Planner 的创新不是一个新算法，而是一个**信息需求分析**的胜利。它问了一个看似简单但极具洞察力的问题：优化器真正需要 ESDF 的什么信息？ 答案是：对于每个碰撞控制点，只需要 (1) 一个指向自由空间的方向 \(\mathbf{v}\)，和 (2) 到障碍物表面的距离 \(\mathbf{v} \cdot (\mathbf{Q}_j - \mathbf{p}_j)\)。这两个量不需要全局距离场——一次射线投射就够了。

§D4.5.1 按需 \((p, v)\) 锚点机制 ⭐⭐⭐¶

Step 1：检测碰撞控制点——直接查询占据栅格，不需要 ESDF：

bool isColliding(const Eigen::Vector3d& Q) {
    Eigen::Vector3i idx;
    posToIndex(Q, idx);
    return occupancy_grid_[toAddr(idx)] == OCCUPIED;
}

Step 2：为碰撞控制点生成 \((p, v)\) 锚点

对每个碰撞的控制点 Q_j：
  1. 参考路径 Γ：从前端搜索（A*）得到的无碰撞路径
  2. 找到 Γ 上离 Q_j 最近的点 Q_j^ref
  3. 方向: d_j = Q_j^ref - Q_j（从碰撞点指向安全点）
  4. 沿 d_j 方向从 Q_j 射线投射，找到障碍物表面：
     p_j = 第一个从占据变为自由的体素
  5. 法向量: v_j = d_j / ||d_j||

              ← v_j (法向量)
              │
    Q_j ──────●p_j──────── Q_j^ref
   (碰撞)    (表面点)       (参考路径上)
   [障碍内]   [边界]         [自由空间]

Step 3：定义碰撞代价

\[ J_\text{collision}^{\text{EGO}} = \sum_{j \in \mathcal{C}} \max\!\left(0, \, s_c - \mathbf{v}_j \cdot (\mathbf{Q}_j - \mathbf{p}_j) \right)^3 \]

其中 \(\mathcal{C}\) 是碰撞控制点的索引集，\(s_c\) 是安全余量，\(\mathbf{v}_j \cdot (\mathbf{Q}_j - \mathbf{p}_j)\) 是有符号距离（正=安全侧，负=障碍侧）。

梯度（极其简单！）：

\[ \frac{\partial J_\text{collision}^{\text{EGO}}}{\partial \mathbf{Q}_j} = -3 \max\!\left(0, \, s_c - \mathbf{v}_j \cdot (\mathbf{Q}_j - \mathbf{p}_j) \right)^2 \cdot \mathbf{v}_j \]

对比 Fast-Planner：

	Fast-Planner	EGO-Planner
距离查询	三线性插值 ESDF（8 次内存访问）	点积 \(\mathbf{v} \cdot (\mathbf{Q} - \mathbf{p})\)（3 次乘法 + 2 次加法）
梯度方向	\(\nabla d(\mathbf{Q})\)（三线性插值梯度）	\(\mathbf{v}\)（预计算的常向量）
前置开销	ESDF 建图 ~7ms	射线投射 ~0.1ms
精度	精确距离（体素分辨率级）	近似距离（假设表面局部平坦）

// EGO-Planner 碰撞代价的核心实现（教学简化版）
void BsplineOptimizer::calcDistanceCostRebound(
    const Eigen::Vector3d& q,        // 控制点
    double& cost,
    Eigen::Vector3d& gradient)
{
    // 1. 检查控制点是否碰撞
    Eigen::Vector3i idx;
    grid_map_->posToIndex(q, idx);
    if (!grid_map_->isOccupied(idx)) {
        cost = 0.0;
        gradient.setZero();
        return;
    }

    // 2. 找参考路径上最近点
    Eigen::Vector3d q_ref = getClosestPointOnGuidePath(q);

    // 3. 射线投射找表面点 p 和法向量 v
    Eigen::Vector3d direction = (q_ref - q).normalized();
    Eigen::Vector3d p_surface;
    rayCast(q, direction, p_surface);
    Eigen::Vector3d v_normal = direction;

    // 4. 计算有符号距离
    double signed_dist = v_normal.dot(q - p_surface);
    double diff = safe_clearance_ - signed_dist;

    // 5. 代价和梯度——整个碰撞代价只需一次点积
    if (diff > 0) {
        cost = diff * diff * diff;                     // 三次惩罚
        gradient = -3.0 * diff * diff * v_normal;      // v 是常向量！
    } else {
        cost = 0.0;
        gradient.setZero();
    }
}

§D4.5.2 \((p, v)\) 锚点的迭代更新 ⭐⭐⭐¶

关键问题：优化过程中控制点在移动，但 \((p, v)\) 锚点是固定的——如果控制点移动太远，\((p, v)\) 就不再准确。

解决方案：每隔几次 L-BFGS 迭代，重新计算 \((p, v)\)：

算法: EGO-Planner 优化循环

1. 前端搜索得到参考路径 Γ
2. 初始化 B 样条控制点 Q = fitPath(Γ)
3. for outer_iter = 1 to MAX_OUTER:
     a. 检测碰撞控制点 C
     b. 为每个 Q_j ∈ C 计算 (p_j, v_j)
     c. 用 LBFGS-Lite 优化（inner iterations ≤ 100）:
        min J = λ_s·J_smooth + λ_c·J_collision^EGO + λ_d·J_dynamic
     d. if 无碰撞控制点: break
4. 输出优化轨迹

外层循环通常只需 2-3 次，因为 \((p, v)\) 的近似误差很小。

§D4.5.3 性能对比详解 ⭐⭐¶

指标	Fast-Planner	EGO-Planner	改进
前端搜索	kinodynamic A* ~3ms	A* ~1ms	3x
地图更新	占据栅格+ESDF ~8ms	占据栅格 ~1ms	8x
后端优化	NLopt ~5ms	LBFGS-Lite ~0.5ms	10x
总计	~15ms	~1ms	15x
内存（\(256^3\)）	占据+ESDF ~134MB	占据 ~67MB	2x
成功率（开阔）	98%	97%	持平
成功率（密集）	85%	82%	略低

为什么 EGO-Planner 的成功率略低？

\((p, v)\) 锚点假设障碍物表面是局部平坦的。当障碍物有尖锐凸角时，法向量 \(\mathbf{v}\) 可能指向错误方向。但在实际环境中（室内/森林），障碍物表面通常足够光滑，这个近似可以接受。

⚠️ 常见陷阱¶

⚠️ 概念误区：认为 EGO-Planner 完全不需要距离信息 错误做法：认为 EGO-Planner 只用"碰/没碰"的布尔判断 现象/后果：无法理解代价函数中 \(\mathbf{v}_j \cdot (\mathbf{Q}_j - \mathbf{p}_j)\) 的距离含义 根本原因：EGO-Planner 确实需要距离——但只是**局部的、按需的**近似距离，而非全局的精确距离场。\((p,v)\) 锚点给出的是一个**线性近似的局部距离**：\(d \approx \mathbf{v} \cdot (\mathbf{Q} - \mathbf{p})\) 正确做法：理解 EGO-Planner 是"从 ESDF 的全局计算退化为局部线性近似"，而非"完全不用距离"

⚠️ 编程陷阱：射线投射方向选择 错误做法：从碰撞控制点 \(\mathbf{Q}_j\) 向随机方向射线投射 现象/后果：找到的表面点 \(\mathbf{p}_j\) 可能不在"正确"的方向上，导致优化器将控制点推向更深的障碍物内部 根本原因：射线方向必须指向已知安全的参考路径——这保证了法向量 \(\mathbf{v}_j\) 指向自由空间 正确做法：始终从 \(\mathbf{Q}_j\) 向参考路径上最近点 \(\mathbf{Q}_j^\text{ref}\) 的方向投射

练习¶

（设计题） EGO-Planner 的 \((p,v)\) 锚点假设障碍物表面局部平坦。构造一个反例：尖锐凸角障碍物使得 \((p,v)\) 给出错误的梯度方向。画图说明，并提出一个修复方案。
（分析题） 在什么条件下，EGO-Planner 的 \((p,v)\) 近似距离与精确 ESDF 距离完全一致？（提示：考虑障碍物表面的局部曲率）

§D4.6 LBFGS-Lite——单头文件优化器详解 ⭐⭐⭐¶

动机：为什么替换 NLopt¶

Fast-Planner 使用 NLopt 库的 L-BFGS 实现。NLopt 的问题：

问题	说明
依赖复杂	整个 NLopt 库 ~50000 行 C，但 B 样条优化只需要 L-BFGS
线搜索假设	More-Thuente 假设函数光滑，但碰撞惩罚 \(\max(0,\cdot)^3\) 有 \(C^0\) 棱角
非凸处理	标准 L-BFGS 在非凸问题上不保证收敛

EGO-Planner 开发了 LBFGS-Lite（ZJU-FAST-Lab/LBFGS-Lite），一个 ~800 行的单头文件解决方案，针对 B 样条优化的非光滑特性做了两个关键改进。

§D4.6.1 Lewis-Overton 线搜索 ⭐⭐⭐¶

标准 Wolfe 条件假设 \(f\) 可微。当碰撞惩罚为 \(\max(0, \cdot)^3\) 时，在 \(d = d_\text{safe}\) 处有棱角——不严格满足可微假设。

**Lewis-Overton 线搜索**将 Armijo 条件中的方向导数替换为广义方向导数（Clarke subdifferential），实践中使用稳健的回溯法：

// lbfgs.hpp 中 Lewis-Overton 线搜索的核心逻辑（教学简化版）
int linesearch_lewis_overton(double& step, /* 其他参数 */)
{
    const double c1 = 1e-4;   // Armijo 参数
    const double c2 = 0.9;    // 曲率参数
    double width = 0.5;       // 回溯因子

    while (count < max_linesearch) {
        // 试探新点，计算函数值和梯度
        f = evaluate(x + step * d, g_new);

        // Armijo 条件：步长是否太大？
        if (f > f_init + c1 * step * dg_init) {
            step *= width;    // 缩小步长
            continue;
        }

        // 弱 Wolfe 条件：步长是否太小？
        double dg_new = dot(g_new, d);
        if (dg_new < c2 * dg_init) {
            step *= 2.0;      // 放大步长
            continue;
        }

        return SUCCESS;       // 两个条件都满足
    }
}

为什么不用 More-Thuente/Hager-Zhang？ LBFGS-Lite 的开发者明确说明：插值型线搜索引入大量调参参数并假设函数光滑。Lewis-Overton 没有插值步骤，不假设光滑性，参数更少（只有 \(c_1, c_2, \text{width}\)），在非光滑优化中更鲁棒。

§D4.6.2 Li-Fukushima 谨慎更新 ⭐⭐⭐¶

标准 L-BFGS 要求 \(y_k^\top s_k > 0\)（曲率条件），这在凸优化中自动满足，但在非凸碰撞惩罚下可能违反。

Li-Fukushima 谨慎更新（2001）：只有当曲率条件"足够好"时才更新 \((s_k, y_k)\) 对：

\[ \text{if } y_k^\top s_k \geq \epsilon \|g_k\| \|s_k\|^2 \text{ then update; else skip} \]

效果：避免 Hessian 近似矩阵退化为不定矩阵，保证搜索方向始终是下降方向。

§D4.6.3 LBFGS-Lite 使用示例 ⭐⭐¶

#include "lbfgs.hpp"

// 定义目标函数：value 和 gradient 同时计算
static double bsplineCost(void* instance,
                          const double* x,   // 控制点展平为 1D
                          double* grad,      // 梯度（同维度）
                          const int n)       // 变量个数 = 3×n_ctrl
{
    auto* planner = reinterpret_cast<BsplinePlanner*>(instance);

    // 将 1D 数组重组为控制点
    vector<Eigen::Vector3d> ctrl_pts(n / 3);
    for (int i = 0; i < n / 3; ++i)
        ctrl_pts[i] = Eigen::Map<const Eigen::Vector3d>(x + 3*i);

    // 计算各项代价和梯度
    double cost = 0.0;
    vector<Eigen::Vector3d> gradient(n / 3, Eigen::Vector3d::Zero());
    planner->calcSmoothnessCost(ctrl_pts, cost, gradient);
    planner->calcCollisionCost(ctrl_pts, cost, gradient);
    planner->calcDynamicCost(ctrl_pts, cost, gradient);

    // 展平梯度（Eigen::Map 避免拷贝）
    for (int i = 0; i < n / 3; ++i)
        Eigen::Map<Eigen::Vector3d>(grad + 3*i) = gradient[i];

    return cost;
}

void optimizeBSpline()
{
    int n = 3 * n_ctrl_points;
    double* x = new double[n];

    lbfgs_parameter_t param;
    lbfgs_parameter_init(&param);
    param.mem_size = 16;        // 历史步数（增大提高精度）
    param.max_iterations = 200;
    param.g_epsilon = 1e-5;     // 梯度收敛阈值

    double final_cost;
    int ret = lbfgs(n, x, &final_cost,
                    bsplineCost, nullptr, this, &param);

    delete[] x;
}

嵌入式性能提醒（来自 LBFGS-Lite README）：
cpufreq-set -g performance
在亚毫秒级优化中，如果 CPU 运行在节能模式（ondemand governor），优化耗时可能从 0.5ms 跳到 3ms——因为 CPU 来不及升频。performance governor 锁定最高频率，消除这个不确定性。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：LBFGS-Lite 的 max_step 参数未设置 错误做法：使用默认的 max_step = +∞ 现象/后果：某些迭代中步长过大，控制点从障碍物一侧"飞"到另一侧，导致新的碰撞或优化振荡 根本原因：碰撞惩罚在远离安全边界时为零，梯度可能很小但函数景观非凸——大步长可能跳过好的解 正确做法：设置 param.max_step = 10.0（或与地图大小相关的合理值），限制单步最大位移

练习¶

（实验题） 用 LBFGS-Lite 求解一个 2D 轨迹优化：5 个控制点，代价 = 平滑性 + 与圆形障碍的碰撞惩罚。可视化优化过程（每次迭代画一条曲线）。比较 Lewis-Overton 和标准 Wolfe 线搜索的迭代次数。

§D4.7 Ewok 环形缓冲——嵌入式的极致紧凑 ⭐⭐¶

动机：为什么需要环形缓冲¶

传统 ESDF 地图（如 Voxblox）使用哈希表（voxel hashing）存储体素。Ewok（Usenko et al., IROS 2017）提出用**固定大小的 3D 环形缓冲**替代——内存连续、索引无分支、滑窗式更新零拷贝。

§D4.7.1 环形缓冲的数据结构 ⭐⭐¶

template <int _POW = 6, typename _Datatype = int16_t>
class RingBufferBase {
    static constexpr int _N = (1 << _POW);      // 64（边长）
    static constexpr int _MASK = _N - 1;          // 63 = 0b00111111
    static constexpr int _SIZE = _N * _N * _N;    // 262144 体素

    _Datatype data_[_SIZE];   // 连续内存，栈或静态分配

    // 3D → 1D 索引：位与替代模运算（1 cycle vs ~20 cycles on ARM）
    inline int idx(int x, int y, int z) const {
        return ((x & _MASK) << (2 * _POW))
             | ((y & _MASK) << _POW)
             | (z & _MASK);
    }
};

为什么边长必须是 2 的幂？ 因为 x % N 需要除法指令（~20 cycles on ARM），而 x & (N-1) 只需一条 AND 指令（1 cycle）。当 \(N = 2^k\) 时两者等价，带来 20 倍加速。

环形滑窗更新：数据不移动，只有索引偏移改变。当无人机向右移动时，左边最旧的列被新数据覆盖，offset_.x() += 1。这就是"零拷贝滑窗"。

给 DSP/嵌入式工程师的类比（像的部分与不像的部分）：Ewok 的环形缓冲是 DSP 循环缓冲**在机器人学中的 3D 推广。**像：都是避免数据搬移、用索引偏移替代；不像：DSP 循环缓冲是 1D 的、硬件支持的，Ewok 是 3D 的、纯软件实现的，需要位操作模拟环形寻址。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：环形缓冲的边长不是 2 的幂 错误做法：将 _POW 设为非整数，或直接设 _N = 100 现象/后果：位与索引 x & _MASK 不再等价于模运算 x % _N，数据寻址混乱 根本原因：x & (N-1) ≡ x % N 的前提是 \(N = 2^k\)。非 2 的幂时，_MASK = N-1 的二进制表示不全为 1，位与运算给出错误结果 正确做法：始终用 _POW 模板参数控制边长，_N = 1 << _POW

练习¶

（编程题） 用 C++ 实现一个 1D 环形缓冲（power-of-2 大小），验证位与索引和模运算索引的等价性。测量两种方式在 \(10^7\) 次随机访问下的耗时差异。

§D4.8 代价函数逐项对比——Fast-Planner vs EGO-Planner ⭐⭐⭐¶

§D4.8.1 公式总结¶

Fast-Planner：

\[ J_\text{Fast} = \lambda_s \underbrace{\sum_i \|\Delta^3 \mathbf{Q}_i\|^2}_{J_\text{smooth}} + \lambda_c \underbrace{\sum_j (d_s - d(\mathbf{Q}_j))^3_+}_{J_\text{collision}} + \lambda_d \underbrace{\sum_j \left(\frac{|\Delta^1 \mathbf{Q}_j|}{\Delta t} - v_m\right)^2_+}_{J_\text{vel}} + \lambda_a \underbrace{\sum_j \left(\frac{|\Delta^2 \mathbf{Q}_j|}{\Delta t^2} - a_m\right)^2_+}_{J_\text{acc}} \]

EGO-Planner：

\[ J_\text{EGO} = \lambda_s J_\text{smooth} + \lambda_c \underbrace{\sum_{j \in \mathcal{C}} (s_c - \mathbf{v}_j \cdot (\mathbf{Q}_j - \mathbf{p}_j))^3_+}_{J_\text{collision}^{EGO}} + \lambda_d J_\text{vel} + \lambda_a J_\text{acc} \]

差异只在碰撞项！ 平滑性、速度约束、加速度约束的公式和梯度完全相同。

§D4.8.2 逐项对比表¶

代价项	Fast-Planner	EGO-Planner	相同？
\(J_\text{smooth}\)	\(\sum \\|\Delta^3 Q\\|^2\)	\(\sum \\|\Delta^3 Q\\|^2\)	相同
\(J_\text{collision}\)	\((d_s - d_\text{ESDF}(Q))^3_+\)	\((s_c - \mathbf{v} \cdot (Q - p))^3_+\)	不同
\(\nabla J_\text{collision}\)	\(-3(\cdot)^2 \nabla d_\text{ESDF}\)	\(-3(\cdot)^2 \mathbf{v}\)	不同
\(J_\text{vel}\)	\((\\|\Delta Q\\|/\Delta t - v_m)^2_+\)	相同	相同
\(J_\text{acc}\)	\((\\|\Delta^2 Q\\|/\Delta t^2 - a_m)^2_+\)	相同	相同

在代码中定位差异：

Fast-Planner:  bspline_opt/src/bspline_optimizer.cpp
  → calcCollisionCost()
    → edt_env_->evaluateEDTWithGrad(q, dist, grad_dist);  // ← ESDF 查询

EGO-Planner:   bspline_opt/src/bspline_optimizer.cpp
  → calcDistanceCostRebound()
    → grid_map_->isOccupied(idx);     // ← 只查占据栅格
    → v.dot(q - p)                     // ← 点积替代 ESDF

练习¶

（代码精读题） 在 Fast-Planner 和 EGO-Planner 的 GitHub 仓库中，分别定位碰撞代价的实现函数。用 diff 工具对比两者的 bspline_optimizer.cpp，找到"ESDF 梯度"被"\((p,v)\) 锚点"替换的确切行号。

§D4.9 数值细节与调参指南 ⭐⭐⭐¶

§D4.9.1 权重调参原则¶

典型权重设置（Fast-Planner）:
  λ_smooth     = 10.0     平滑性
  λ_collision  = 1000.0   碰撞（必须远大于平滑，安全优先）
  λ_vel        = 100.0    速度约束
  λ_acc        = 100.0    加速度约束

典型权重设置（EGO-Planner）:
  λ_smooth     = 1.0
  λ_collision  = 1000.0
  λ_vel        = 30.0
  λ_acc        = 30.0

现象	可能原因	调整方向
轨迹穿入障碍物	\(\lambda_c\) 太小	增大 \(\lambda_c\)
轨迹"绷直"不绕行	\(\lambda_s\) 太大	减小 \(\lambda_s\)
轨迹有高频抖动	\(\lambda_s\) 太小	增大 \(\lambda_s\)
电机饱和/振荡	动力学权重太小	增大 \(\lambda_v, \lambda_a\)

§D4.9.2 节点距 \(\Delta t\) 的选择¶

\(\Delta t\)	优点	缺点
过小（0.05s）	凸包紧凑、碰撞检查准确	控制点密集、优化变量多、收敛慢
过大（0.5s）	控制点稀疏、收敛快	凸包松弛、可能漏检碰撞

经验值：室内（<3m/s）→ \(\Delta t \approx 0.1\)s；室外（<5m/s）→ \(\Delta t \approx 0.15\)s；高速（>5m/s）→ \(\Delta t \approx 0.2\)s。

§D4.9.3 数值稳定性注意事项¶

问题 1：三次惩罚的数值溢出——当控制点深入障碍物时，\((d_s - d)^3\) 可能非常大。解决方案：double penalty = std::min(diff*diff*diff, max_penalty);

问题 2：ESDF 梯度在 Voronoi 脊线上不连续——梯度从一侧指向左，从另一侧指向右，导致优化器振荡。Fast-Planner 通过增大三线性插值范围模糊化梯度。EGO-Planner 避免了这个问题（不使用 ESDF）。

问题 3：\(\Delta t\) 与控制点数量的匹配——轨迹总时间 \(T = (n - p) \cdot \Delta t\)。至少需要 \(n \geq 2p + 1\) 个控制点（三次至少 7 个，五次至少 11 个）。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Fast-Planner 和 EGO-Planner 的权重数量级不同 错误做法：把 Fast-Planner 的权重直接复制到 EGO-Planner 中 现象/后果：优化不收敛或轨迹质量极差 根本原因：两者碰撞代价的数量级不同——Fast-Planner 的 \(d_\text{ESDF}\) 量级为 \(O(1)\)（米），EGO-Planner 的 \(\mathbf{v} \cdot (\mathbf{Q} - \mathbf{p})\) 量级也为 \(O(1)\) 但分布不同。更关键的是 EGO-Planner 的碰撞项只对碰撞控制点求和（稀疏），而 Fast-Planner 对所有控制点求和 正确做法：分别针对每个系统调参。先固定碰撞权重 \(\lambda_c = 1000\)，再调其他权重

练习¶

（实验题） 在 Fast-Planner 中，固定其他参数，只改变 \(\lambda_s\)（从 0.1 到 1000），记录轨迹的 jerk 积分、碰撞余量、和优化时间。画出三个指标随 \(\lambda_s\) 变化的曲线，找到最佳平衡点。

§D4.10 从 B 样条到 MINCO——历史必然性 ⭐⭐⭐⭐¶

§D4.10.1 B 样条的两个根本限制¶

限制 1：均匀节点距 → 时间分配不灵活

场景：快速穿过窄缝，前后慢速巡航

理想轨迹：慢(2m/s) → 快(5m/s)穿缝 → 慢(2m/s)
B 样条（Δt = 0.15s）：快速段每 0.15s 移动 0.75m，控制点间距太大
  → 凸包松弛，可能漏检碰撞
  → 必须缩小 Δt 到 0.05s，但慢速段又太精细

→ 均匀 Δt 是"削足适履"

限制 2：不精确插值航路点——B 样条控制点 \(\neq\) 曲线上的点。如需精确经过航路点，须添加等式约束 \(\mathbf{C}(t_k) = \mathbf{W}_k\)，失去无约束优化优势。

§D4.10.2 MINCO 的解决方案（预告）¶

MINCO（Minimum Control, Wang et al., TRO 2022）的核心思想：

非均匀时间段：每段时间 \(T_i\) 独立可调
精确航路点插值：航路点约束被内嵌到表示中
保留 B 样条的优势：凸包性（通过 Bezier 基）+ 解析梯度

多项式（D3）      B 样条（D4）        MINCO（D5）
──────────── → ──────────── → ────────────
精确插值 ✓       近似插值 ✗       精确插值 ✓
非均匀时间 ✓     均匀时间 ✗       非均匀时间 ✓
QP 求解器        L-BFGS            L-BFGS
等式约束多        无等式约束        无等式约束
局部修改 ✗       局部修改 ✓        局部修改 ✓
凸包 △            凸包 ✓            凸包 ✓

本质洞察：这三种轨迹表示的演化不是线性替代（新的全面优于旧的），而是**Pareto 前沿的推进**——每一步都在"精确性-速度-灵活性"的三角形中找到新的非支配点。MINCO 的突破是同时位于三个轴的最优侧。

本章常见误解汇总¶

误解	正确理解
B 样条全面优于多项式	B 样条在在线重规划中更好，但多项式在离线精确轨迹中更好
控制点就是曲线上的点	控制点在曲线的凸包内但不在曲线上
EGO-Planner 完全不需要距离	它需要局部近似距离，只是不需要全局精确距离场
凸包约束是精确的	凸包约束是充分条件（保守的），不是充要条件
均匀节点距是 B 样条的固有属性	它是无人机规划的工程简化，非均匀 B 样条是更一般的理论
L-BFGS 可以处理有约束优化	L-BFGS 是无约束优化器，约束通过惩罚函数转化
ESDF 是碰撞梯度的唯一来源	\((p,v)\) 锚点、有限差分、学习方法都可以提供碰撞梯度

本章小结¶

术语速查表¶

术语	英文	一句话定义
节点向量	Knot Vector	B 样条的参数域划分序列
Cox-de Boor 递推	Cox-de Boor Recursion	从低阶到高阶递推计算 B 样条基函数
凸包性	Convex Hull Property	曲线段在控制点凸包内
局部支撑	Local Support	每个控制点只影响 \(p+1\) 段
ESDF	Euclidean Signed Distance Field	每个体素存储到最近障碍物的有符号距离
kinodynamic A*	Kinodynamic A*	在状态空间（位置+速度）中搜索的 A*
PMP 启发式	Pontryagin Minimum Principle Heuristic	基于最优控制闭式解的 A* 启发函数
\((p,v)\) 锚点	\((p,v)\) Anchor	EGO-Planner 中按需计算的表面点+法向量对
Lewis-Overton 线搜索	Lewis-Overton Line Search	适用于非光滑函数的稳健线搜索
Li-Fukushima 谨慎更新	Li-Fukushima Cautious Update	在非凸优化中跳过不满足曲率条件的 L-BFGS 更新

知识点总表¶

编号	知识点	核心要点	对应节	难度
1	B 样条动机	多项式三个痛点：KKT、时间耦合、全局修改	§D4.1	⭐⭐
2	Cox-de Boor 递推	高阶基函数 = 低阶的加权混合	§D4.2.1	⭐⭐
3	基矩阵 \(\mathbf{M}_3\)	矩阵形式求值，SIMD 友好	§D4.2.2	⭐⭐
4	五大性质	凸包+局部支撑+自动连续+导数差分+节点插入	§D4.2.3	⭐⭐
5	Fast-Planner 架构	plan_env/path_searching/bspline_opt/plan_manage	§D4.3	⭐⭐⭐
6	ESDF 与碰撞梯度	\(\nabla d\) 提供推离障碍物的方向	§D4.3.2	⭐⭐⭐
7	PMP 启发式	双积分器最优代价闭式解 → A* 紧下界	§D4.3.3	⭐⭐⭐
8	四项代价函数	smooth + collision + vel + acc	§D4.3.4	⭐⭐⭐
9	拓扑路径枚举	多同伦类避免局部极小	§D4.4	⭐⭐⭐
10	EGO-Planner \((p,v)\)	按需射线投射替代 ESDF	§D4.5	⭐⭐⭐
11	LBFGS-Lite	Lewis-Overton + Li-Fukushima	§D4.6	⭐⭐⭐
12	环形缓冲	2 的幂边长 + 位与寻址 + 零拷贝滑窗	§D4.7	⭐⭐
13	权重调参	collision >> smooth；分别调参	§D4.9	⭐⭐⭐
14	B 样条→MINCO	均匀节点距是根本限制	§D4.10	⭐⭐⭐⭐

累积项目：B 样条轨迹优化模块¶

在前一章（D3）的多项式轨迹生成器基础上，新增 B 样条轨迹优化模块：

本章新增： 1. 实现三次均匀 B 样条评估器（基矩阵求值 + de Boor 算法） 2. 实现 B 样条优化的四项代价函数（\(J_\text{smooth}\)、\(J_\text{collision}\)、\(J_\text{vel}\)、\(J_\text{acc}\)）及其解析梯度 3. 集成 LBFGS-Lite 作为优化器后端 4. 实现简单的 2D 占据栅格 + 碰撞检测 5. 对比 Fast-Planner 风格（ESDF 梯度）和 EGO-Planner 风格（\((p,v)\) 锚点）的碰撞代价

验证： - 无障碍物时，优化结果与直线一致 - 有圆形障碍物时，轨迹绕行且满足速度/加速度约束 - 优化过程可视化：控制点逐步移动到安全位置

延伸阅读¶

核心论文（按时间排序）：

论文	关键词	难度	说明
Usenko et al., IROS 2017	Ewok, 环形缓冲	⭐⭐	B 样条用于无人机规划的先驱工作
Zhou et al., RA-L 2019	Fast-Planner	⭐⭐⭐	架构范式定义者，必读
Zhou et al., ICRA 2020	拓扑路径枚举	⭐⭐⭐	解决局部极小
Zhou et al., RA-L 2021	EGO-Planner	⭐⭐⭐	ESDF-free 的核心创新
Wang et al., TRO 2022	MINCO	⭐⭐⭐⭐	B 样条的继承者

教材：

书名	章节	说明
de Boor, A Practical Guide to Splines	Ch. 1-9	B 样条理论的经典教材
Piegl & Tiller, The NURBS Book	Ch. 2-4	工业标准参考
Nocedal & Wright, Numerical Optimization	Ch. 7	L-BFGS 的标准参考

本章与后续章节的关系¶

后续章节	关系	本章铺垫的知识点
D5 MINCO	直接继承	B 样条的凸包性质、解析梯度框架、LBFGS-Lite 优化器
D6 感知规划	地图表示	ESDF 建图方法、占据栅格更新
D7 多机协同	单机基础	EGO-Planner 的代价函数框架（扩展机间碰撞项）

故障排查手册¶

故障 1：B 样条优化后轨迹穿入障碍物¶

步骤	操作
症状	可视化显示轨迹穿过障碍物，或无人机与障碍物发生碰撞
可能原因 1	\(\lambda_c\) 太小，碰撞代价不足以将控制点推出
排查	打印优化后的 \(J_\text{collision}\) 值，如果非零说明仍有碰撞
可能原因 2	\(\Delta t\) 太大，凸包松弛导致碰撞检查漏检
排查	在曲线上密采样（如 100 倍于控制点数），逐点检查碰撞
可能原因 3	ESDF 未正确更新（Fast-Planner），障碍物信息过时
排查	可视化 ESDF 切面，确认障碍物处距离值为负
相关章节	§D4.3.4（碰撞代价）、§D4.9.2（\(\Delta t\) 选择）

故障 2：优化器不收敛（振荡或代价不下降）¶

步骤	操作
症状	LBFGS-Lite 返回 `MAX_ITERATIONS_REACHED`，代价值不单调
可能原因 1	线搜索参数不合适（`max_step` 太大或 `c2` 太小）
排查	打印每次迭代的步长和代价值，检查是否有大幅跳变
可能原因 2	碰撞代价在 Voronoi 脊线附近梯度不连续（仅 Fast-Planner）
排查	可视化碰撞控制点的 ESDF 梯度方向，检查是否有突变
可能原因 3	初始路径与最终轨迹的拓扑类不同，优化试图"翻越"障碍
排查	可视化初始 B 样条和最终结果，检查是否有穿越障碍的尝试
相关章节	§D4.6.1（Lewis-Overton 线搜索）、§D4.9.3（数值稳定性）

故障 3：EGO-Planner 在尖锐障碍物处失败¶

步骤	操作
症状	规划成功但轨迹质量差，或控制点被推向错误方向
可能原因	\((p,v)\) 锚点的法向量 \(\mathbf{v}\) 在尖锐凸角处指向错误方向
排查	可视化 \((p,v)\) 锚点的位置和法向量，检查是否与障碍物表面法向一致
解决	增加外层迭代次数 `MAX_OUTER`，让锚点有更多机会更新；或切换到 Fast-Planner 的 ESDF 方案
相关章节	§D4.5.2（锚点迭代更新）

故障 4：规划时间超出预算¶

步骤	操作
症状	单次规划耗时 > 20 ms，无法满足实时要求
可能原因 1	CPU 运行在节能模式（`ondemand` governor）
排查	`cat /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor`，如果不是 `performance` → `cpufreq-set -g performance`
可能原因 2	控制点数量太多（\(n > 50\)）
排查	打印控制点数量，考虑增大 \(\Delta t\) 减少控制点
可能原因 3	ESDF 更新范围太大（仅 Fast-Planner）
排查	限制 ESDF 更新范围到无人机附近的局部区域
相关章节	§D4.6.3（性能提醒）、§D4.9.2（\(\Delta t\) 选择）

故障 5：B 样条求值结果异常（NaN 或极大值）¶

步骤	操作
症状	轨迹采样点出现 NaN 或 \(10^{15}\) 级别的值
可能原因 1	节点区间索引 \(k\) 越界
排查	在求值函数入口添加 `assert(k >= p && k <= n)`
可能原因 2	归一化参数 \(s\) 超出 \([0,1)\)
排查	检查 \(s\) 的计算，确认 `s = (t - k*dt) / dt` 在 \([0,1)\) 内
相关章节	§D4.2.4（de Boor 算法实现）

API 速查表¶

B 样条核心接口¶

函数	签名	说明
求值	`Vector3d evaluate(MatrixXd ctrl_pts, int k, double s)`	给定区间索引和归一化参数，返回位置
导数	`Vector3d derivative(MatrixXd ctrl_pts, int k, double s, int order)`	返回 k 阶导数
拟合	`MatrixXd fitPath(vector<Vector3d> path, double dt)`	最小二乘拟合路径为 B 样条控制点

LBFGS-Lite 核心接口¶

函数	签名	说明
初始化参数	`lbfgs_parameter_init(lbfgs_parameter_t* param)`	设置默认参数
优化	`int lbfgs(int n, double* x, double* fx, proc, progress, instance, param)`	运行 L-BFGS 优化
错误信息	`const char* lbfgs_strerror(int err)`	返回错误码对应的描述

Fast-Planner 核心参数（YAML）¶

参数	默认值	说明
`sdf_map/resolution`	0.1	体素分辨率（m）
`bspline_optimizer/lambda_smooth`	10.0	平滑性权重
`bspline_optimizer/lambda_collision`	1000.0	碰撞权重
`bspline_optimizer/safe_distance`	0.3	安全距离（m）
`kinodynamic_astar/max_vel`	3.0	最大速度（m/s）

研究实践建议¶

入门级（本科高年级 / 硕士一年级）： - 实现三次均匀 B 样条评估器，验证五大性质 - 运行 Fast-Planner 和 EGO-Planner 的 ROS 仿真，理解数据流 - 用 LBFGS-Lite 实现简单的 2D 轨迹优化

进阶级（硕士二年级 / 博士一年级）： - 精读 Fast-Planner 和 EGO-Planner 的完整源码，理解每个代价项的梯度推导 - 实现拓扑路径枚举的 UVD 同伦类判定 - 在真实无人机平台（如 PX4 + 深度相机）上部署 EGO-Planner

研究级（博士）： - 研究 B 样条在动态环境中的局限性（时空轨迹优化） - 探索学习增强的碰撞梯度预测（替代 ESDF 和 \((p,v)\) 锚点） - 研究 MINCO 的理论性质（凸性、对偶性）

附录 A：B 样条速查表¶

─────────────────────── B 样条速查表 ───────────────────────

表示: C(t) = Σ N_{i,p}(t) P_i

Cox-de Boor 递推:
  N_{i,0}(t) = 1 if t_i ≤ t < t_{i+1}, else 0
  N_{i,p}(t) = [(t-t_i)/(t_{i+p}-t_i)] N_{i,p-1}
             + [(t_{i+p+1}-t)/(t_{i+p+1}-t_{i+1})] N_{i+1,p-1}

均匀节点: t_i = i · Δt

p=3 基矩阵:
  M_3 = (1/6) [-1  3 -3  1;  3 -6  3  0;  -3  0  3  0;  1  4  1  0]

求值: C(t) = [P_{k-3} P_{k-2} P_{k-1} P_k] · M_3^T · [s³ s² s 1]^T
  其中 k = floor(t/Δt), s = (t - kΔt)/Δt

导数:
  位置: 控制点 P_i
  速度: V_i = (P_{i+1} - P_i) / Δt
  加速度: A_i = (P_{i+2} - 2P_{i+1} + P_i) / Δt²
  Jerk: J_i = (P_{i+3} - 3P_{i+2} + 3P_{i+1} - P_i) / Δt³

五大性质:
  1. 凸包: C(t) ∈ ConvexHull(P_{k-p},...,P_k) for t ∈ [t_k,t_{k+1})
  2. 局部支撑: 改 P_i 只影响 [t_i, t_{i+p+1})
  3. 连续性: 自动 C^{p-1}
  4. 非负性: N_{i,p}(t) ≥ 0
  5. 单位分解: Σ N_{i,p}(t) = 1
───────────────────────────────────────────────────────────

附录 B：Fast-Planner / EGO-Planner 参数速查¶

─────────── Fast-Planner 关键参数 ───────────

sdf_map:
  resolution: 0.1          # 体素分辨率（m）
  map_size: [50, 50, 5]    # 地图大小（m）

kinodynamic_astar:
  max_vel: 3.0              # 最大速度（m/s）
  max_acc: 3.0              # 最大加速度（m/s²）

bspline_optimizer:
  lambda_smooth: 10.0
  lambda_collision: 1000.0
  lambda_feasibility: 100.0
  safe_distance: 0.3

─────────── EGO-Planner 关键参数 ───────────

grid_map:
  resolution: 0.1
  map_size: [50, 50, 5]

bspline_optimizer:
  lambda_smooth: 1.0
  lambda_collision: 1000.0
  lambda_feasibility: 30.0
  safe_clearance: 0.3
  order: 3                   # B 样条阶数 p=3

fsm:
  replan_thresh: 1.0
  replan_time: 0.1
────────────────────────────────────────────────

附录 C：常见问题 FAQ¶

Q1: B 样条和 Bezier 曲线是什么关系？

Bezier 曲线是 B 样条的特例——当节点向量为 clamped 且只有一段时（即节点为 \(\{0,\ldots,0, 1,\ldots,1\}\)，首尾各重复 \(p+1\) 次）。B 样条可以看作多段 Bezier 曲线在连接点处自动满足 \(C^{p-1}\) 连续性的推广。

Q2: Fast-Planner 用 \(p=5\)，EGO-Planner 用 \(p=3\)，哪个更好？

取决于场景。\(p=5\) 保证 snap 连续，适合需要精确动力学的场景；\(p=3\) 保证加速度连续，计算更快。PX4 的位置控制器只需要到加速度级别的指令，因此 \(p=3\) 在实时性要求高的场景中是更务实的选择。

Q3: EGO-Planner 完全不需要 ESDF 吗？

是的。EGO-Planner 只需要占据栅格（Occupancy Grid），即每个体素是"占据"还是"自由"的布尔判断。碰撞梯度通过按需射线投射获取，不需要预计算距离场。

Q4: LBFGS-Lite 可以处理有约束优化吗？

L-BFGS 是无约束优化器。B 样条优化将所有约束转化为惩罚函数加入代价，变成无约束问题。这引入了权重调参的问题——但在实践中，惩罚函数比约束优化更快且更鲁棒。

Q5: 为什么不直接用梯度下降，而要用 L-BFGS？

梯度下降的收敛速度是线性的（\(O(1/k)\)），L-BFGS 的收敛速度是超线性的。在 45 变量的问题上，梯度下降需要 ~500 次迭代，L-BFGS 只需要 ~30 次——相差一个数量级。

Ewok vs Fast-Planner vs EGO-Planner 总对比¶

特征	Ewok (2017)	Fast-Planner (2019)	EGO-Planner (2021)
前端	无（全局路径拟合）	kinodynamic A*	A*
地图	环形缓冲 ESDF	Voxblox-style ESDF	占据栅格（无 ESDF）
B 样条阶数	\(p=4\)	\(p=5\)	\(p=3\)
优化器	梯度下降	NLopt L-BFGS	LBFGS-Lite
碰撞梯度	ESDF	ESDF	\((p,v)\) 锚点
重规划时间	~3ms	~15ms	~1ms
代码量	~2000 行	~8000 行	~5000 行
历史地位	先驱（证明可行）	范式定义者	速度极限

预计学习时间¶

内容	时间	优先级
§D4.1 动机与历史	1-2 小时	必学
§D4.2 B 样条数学基础	3-4 小时	必学
§D4.3 Fast-Planner 架构	4-5 小时	必学
§D4.4 拓扑路径枚举	1-2 小时	建议
§D4.5 EGO-Planner ESDF-free	3-4 小时	必学
§D4.6 LBFGS-Lite	1-2 小时	必学
§D4.7 Ewok 环形缓冲	1 小时	选学
§D4.8-D4.9 对比与调参	1-2 小时	必学
§D4.10 B 样条→MINCO	1 小时	建议
实战练习（选做 3-4 个）	3-4 小时	建议

版本信息速查¶

组件	版本/年份	来源
Fast-Planner	2019, HKUST	github.com/HKUST-Aerial-Robotics/Fast-Planner
EGO-Planner	2021, ZJU FAST Lab	github.com/ZJU-FAST-Lab/ego-planner
LBFGS-Lite	2021, ZJU FAST Lab	github.com/ZJU-FAST-Lab/LBFGS-Lite
Ewok	2017, TUM	github.com/VladyslavUsenko/ewok
EGO-Planner-v2 (MINCO)	2022, Science Robotics	github.com/ZJU-FAST-Lab/EGO-Planner-v2

附录 D：符号与记号索引¶

前面的"术语速查表"收录的是中英文术语，本附录收录全章出现的**数学符号**。初学者在阅读推导时最容易卡在"这个符号刚才到底指什么"——尤其是 B 样条文献里 \(p\)、\(n\)、\(m\)、\(k\) 这几个字母在不同教材中含义经常互换。下表把本章统一采用的约定固定下来，阅读任意一节遇到陌生符号都可回此速查。

本质洞察：B 样条记号体系的核心矛盾是"节点索引"和"控制点索引"两套下标系统的耦合。节点 \(t_i\) 有 \(m+1\) 个，控制点 \(\mathbf{P}_i\) 有 \(n+1\) 个，二者通过关系式 \(m = n + p + 1\) 绑定。一旦记混这两套索引，整个 de Boor 递推就会全盘错位。所以阅读时务必时刻区分"我现在数的是节点还是控制点"。

D.1 维度与索引符号¶

符号	含义	取值/约束	首次出现
\(p\)	B 样条的阶数（degree）	Fast-Planner \(p=5\)，EGO-Planner \(p=3\)	§D4.2.1
\(n+1\)	控制点个数	控制点索引 \(i \in \{0,\ldots,n\}\)	§D4.2.1
\(m+1\)	节点个数	满足 \(m = n + p + 1\)	§D4.2.1
\(i\)	控制点 / 基函数下标	\(0 \le i \le n\)	§D4.2.1
\(j\)	节点下标	\(0 \le j \le m\)	§D4.2.1
\(k\)	当前参数 \(t\) 所在的节点区间索引	\(k = \lfloor t/\Delta t \rfloor\)，且 \(p \le k \le n\)	§D4.2.4

常见陷阱（记号层面）：很多教材（如 Piegl & Tiller）用 \(p\) 表示阶数、\(n\) 表示"最大控制点下标"，即控制点个数为 \(n+1\)；但另一些资料（如部分图形学课程）用 \(n\) 直接表示控制点**个数**。本章统一采用前者（\(n+1\) 个控制点）。如果你对照外部资料时发现公式 \(m = n + p + 1\) 对不上，多半是两边对 \(n\) 的定义差了 1。

D.2 函数与曲线符号¶

符号	含义	维度	首次出现
\(\mathbf{C}(t)\)	B 样条曲线（位置）	\(\mathbb{R}^3\)（无人机为 3 维）	§D4.2.1
\(N_{i,p}(t)\)	第 \(i\) 个 \(p\) 阶基函数	标量，\(\in [0,1]\)	§D4.2.1
\(\mathbf{P}_i\)	第 \(i\) 个控制点	\(\mathbb{R}^3\)	§D4.2.1
\(t_j\)	第 \(j\) 个节点	标量（时间或参数）	§D4.2.1
\(\mathbf{M}_p\)	\(p\) 阶均匀 B 样条基矩阵	\((p{+}1)\times(p{+}1)\)	§D4.2.2
\(s\)	区间内归一化参数	\(s = (t - k\Delta t)/\Delta t \in [0,1)\)	§D4.2.2
\(\Delta t\)	均匀节点距	标量，正数	§D4.2.2

为什么 \(s\) 必须在 \([0,1)\) 而不是 \([0,1]\)？ 注意右端是开区间。若 \(s=1\)，按 \(k=\lfloor t/\Delta t\rfloor\) 的定义，此时 \(t\) 已经进入下一个区间，应由 \(k{+}1\) 段负责求值。若代码里允许 \(s=1\)，会导致同一个 \(t\) 被两段重复计算，在拼接边界产生 \(C^0\) 级别的可见"接缝"。这正是故障排查手册中"求值结果异常"的隐性诱因之一。

D.3 导数与动力学符号¶

符号	含义	计算式	首次出现
\(\mathbf{V}_i\)	速度控制点	\(\mathbf{V}_i = (\mathbf{P}_{i+1}-\mathbf{P}_i)/\Delta t\)	§D4.2.5
\(\mathbf{A}_i\)	加速度控制点	\(\mathbf{A}_i = (\mathbf{V}_{i+1}-\mathbf{V}_i)/\Delta t\)	§D4.2.5
\(\mathbf{J}_i\)	jerk 控制点	\(\mathbf{J}_i = (\mathbf{A}_{i+1}-\mathbf{A}_i)/\Delta t\)	§D4.2.5
\(v_{\max}, a_{\max}\)	速度/加速度上界	由平台动力学决定	§D4.3.4

桥接（理论→工程）：导数控制点的差分公式是 B 样条"自动满足动力学约束"的工程支点。因为速度曲线本身又是一条 \((p-1)\) 阶 B 样条，其控制点 \(\mathbf{V}_i\) 也满足凸包性——于是只要约束**有限个** \(\mathbf{V}_i\) 落在 \([-v_{\max}, v_{\max}]\) 内，整条速度曲线就被卡在该范围内。这把"对连续曲线的无穷多约束"压缩成"对有限控制点的有限约束"，是 §D4.3.4 可行性代价项能写成有限和的根本原因。

D.4 优化与代价符号¶

符号	含义	备注	首次出现
\(J_\text{smooth}\)	平滑性代价	jerk/snap 平方积分	§D4.3.4
\(J_\text{collision}\)	碰撞代价	ESDF 或 \((p,v)\) 锚点	§D4.3.4
\(J_\text{vel}, J_\text{acc}\)	速度/加速度可行性代价	越界惩罚	§D4.3.4
\(\lambda_s, \lambda_c, \lambda_v, \lambda_a\)	各代价项权重	\(\lambda_c \gg \lambda_s\)	§D4.9.1
\(d(\mathbf{x})\)	点 \(\mathbf{x}\) 到最近障碍的距离	ESDF 查询	§D4.3.2
\(d_\text{safe}\)	安全距离阈值	EGO 默认 0.3 m	§D4.3.4
\((\mathbf{p}, \mathbf{v})\)	锚点（表面点，外法向）	EGO-Planner 专用	§D4.5.1

对比性思维：\(d(\mathbf{x})\) 和 \((\mathbf{p},\mathbf{v})\) 是两种碰撞信息表示的代表。前者是"场"（每个体素一个标量，全局预计算），后者是"锚"（每个越界控制点一对向量，按需局部计算）。记号上的差异直接映射到工程上的差异：场需要 \(O(\text{体素数})\) 的内存和预计算，锚只需要 \(O(\text{越界控制点数})\) 的瞬时计算。这就是 EGO-Planner 把重规划时间从 ~15 ms 压到 ~1 ms 的记号级别的解释。

附录 E：调试实战案例——一次梯度不匹配的排查全过程¶

故障排查手册给的是"症状→原因→步骤"的结构化索引，但真实调试是一个**逐步逼近**的侦探过程。本附录用一个具体案例，完整还原"代价能下降但轨迹明显错误"这类最难缠 bug 的排查思路，目的是把前面零散的调试技巧串成一条可复用的方法论。

E.1 现象：代价在下降，轨迹却越优化越糟¶

假设你自己实现了 §D4.3.4 的四项代价函数，接入 LBFGS-Lite。运行后观察到一个诡异现象：

迭代  总代价      J_smooth   J_collision
 0    1240.5      40.2       1200.3
 5     680.1      35.8        644.3
10     410.7      88.5        322.2     ← J_smooth 反而涨了
15     390.2     145.1        245.1     ← 轨迹肉眼可见地"扭曲"

总代价确实单调下降，优化器没报错，但可视化显示轨迹在障碍物附近剧烈扭曲，\(J_\text{smooth}\) 不降反升。这种"指标在跌、结果在烂"的情况，是初学者最容易误判的——很多人第一反应是"权重没调好"，于是埋头调 \(\lambda\)，结果越调越乱。

思维陷阱：把"代价下降"等同于"实现正确"。代价下降只能说明优化器在沿着**你给它的梯度**下山，但如果梯度算错了，它会非常忠实地走向一个错误的极小点。换句话说，错误的梯度 + 正确的优化器 = 自信地给出错误答案。这是数值优化里最隐蔽的一类 bug——没有任何报错，连代价曲线看起来都很健康。

E.2 第一步：定位是"代价错"还是"梯度错"¶

代价函数实现 bug 分两种：代价值算错**或**梯度算错。二者要分开验证，因为优化器只用梯度，代价值即使写错了也不影响下山方向（只影响线搜索的判据）。

验证梯度的黄金标准是**有限差分检查**（gradient check）——这是任何手写解析梯度的代码都必须先过的一关：

// 有限差分梯度检查：对每个变量扰动 eps，比较数值梯度与解析梯度
void checkGradient(const Eigen::VectorXd& x) {
    const double eps = 1e-6;
    Eigen::VectorXd grad_analytic(x.size());
    double f0 = costFunction(x, grad_analytic);   // 解析梯度

    for (int i = 0; i < x.size(); ++i) {
        Eigen::VectorXd xp = x, xm = x;
        xp[i] += eps;  xm[i] -= eps;
        Eigen::VectorXd dummy(x.size());
        double fp = costFunction(xp, dummy);
        double fm = costFunction(xm, dummy);
        double grad_numeric = (fp - fm) / (2 * eps);   // 中心差分

        double rel_err = std::abs(grad_numeric - grad_analytic[i])
                       / (std::abs(grad_numeric) + 1e-12);
        if (rel_err > 1e-4)                            // 阈值经验值
            printf("变量 %d 梯度不匹配: 解析=%.6e 数值=%.6e 相对误差=%.2e\n",
                   i, grad_analytic[i], grad_numeric, rel_err);
    }
}

为什么用中心差分 \((f_+ - f_-)/2\varepsilon\) 而不是前向差分 \((f_+ - f_0)/\varepsilon\)？ 中心差分的截断误差是 \(O(\varepsilon^2)\)，前向差分是 \(O(\varepsilon)\)。在 \(\varepsilon = 10^{-6}\) 时，中心差分能给出约 8 位有效数字的数值梯度，足以暴露解析梯度里的系数错误；前向差分只有约 6 位，容易被自身误差淹没真实的 bug。代价是多算一次函数值，但调试期间这点开销完全可以接受。

跑完检查，输出指向了具体变量：

变量 12 梯度不匹配: 解析=3.21e+02 数值=1.07e+02 相对误差=2.00e+00
变量 13 梯度不匹配: 解析=2.98e+02 数值=9.93e+01 相对误差=2.00e+00

解析梯度恰好是数值梯度的 3 倍。这个"整数倍"特征是关键线索。

E.3 第二步：从"整数倍误差"反推根因¶

梯度差一个常数倍，几乎总是**链式法则漏项**或**重复累加**。回顾 §D4.3.4：平滑代价 \(J_\text{smooth} = \sum_i \|\mathbf{J}_i\|^2\)，而 jerk 控制点 \(\mathbf{J}_i = (\mathbf{P}_{i+3} - 3\mathbf{P}_{i+2} + 3\mathbf{P}_{i+1} - \mathbf{P}_i)/\Delta t^3\)。

注意一个控制点 \(\mathbf{P}_i\) 会**同时出现在 4 个相邻的 jerk 项**中（\(\mathbf{J}_{i-3}, \mathbf{J}_{i-2}, \mathbf{J}_{i-1}, \mathbf{J}_i\)），系数分别是 \(+1, -3, +3, -1\)。正确的梯度必须把这 4 项的贡献全部累加。差 3 倍的根因，正是实现时只累加了系数绝对值最大的那一项（\(-3\) 那项），漏掉了其余三项里的两项贡献——也就是说 \(\partial J / \partial \mathbf{P}_i\) 少算了局部支撑带来的交叠累加。

本质洞察：B 样条解析梯度的全部复杂度都来自"局部支撑的交叠"。一个控制点影响 \(p+1\) 段曲线，反过来，一段曲线的代价对 \(p+1\) 个控制点都有梯度。手写梯度时最常见的错误就是只考虑"这个控制点自己那一项"，忘了它还出现在邻居的代价项里。这也解释了为什么工程实现普遍采用"遍历每个代价项，把梯度**散射（scatter）**回它涉及的所有控制点"的写法，而不是"遍历每个控制点去收集（gather）"——散射式天然不会漏掉交叠项。

E.4 修复与回归验证¶

把梯度累加改成散射式：

// ✅ 正确：遍历每个 jerk 项，把梯度散射回它涉及的 4 个控制点
for (int i = 0; i + 3 <= n; ++i) {
    Eigen::Vector3d jerk = (P[i+3] - 3*P[i+2] + 3*P[i+1] - P[i]) / dt3;
    Eigen::Vector3d g = 2.0 * jerk / dt3;     // d||J||^2 / dJ * dJ/dP
    grad[i]   += -1.0 * g;                     // 系数 -1
    grad[i+1] +=  3.0 * g;                     // 系数 +3
    grad[i+2] += -3.0 * g;                     // 系数 -3
    grad[i+3] +=  1.0 * g;                     // 系数 +1
}

重新跑 checkGradient，所有变量相对误差降到 \(10^{-7}\) 量级，通过。再跑优化：

迭代  总代价      J_smooth   J_collision
 0    1240.5      40.2       1200.3
10     205.3      18.7        186.6     ← J_smooth 正常下降
20      88.1      12.4         75.7     ← 轨迹平滑绕行

\(J_\text{smooth}\) 恢复单调下降，轨迹平滑。bug 闭环。

E.5 方法论总结¶

把这次排查抽象成一条**可复用的决策链**，遇到任何"优化结果异常"的问题都可照此推进：

阶段	关键动作	判据	对应章节
1. 分类	区分"代价错"还是"梯度错"	优化器只用梯度，先查梯度	§D4.6
2. 验证	中心差分 gradient check	相对误差 > \(10^{-4}\) 即有 bug	本附录 E.2
3. 定位	看误差的结构特征	整数倍 → 漏项/重复累加	本附录 E.3
4. 反推	结合局部支撑交叠分析根因	一点影响 \(p+1\) 段	§D4.2.3
5. 修复	改用散射式累加	不会漏交叠项	本附录 E.4
6. 回归	重跑 gradient check + 优化	误差降到 \(10^{-7}\)，指标单调	本附录 E.4

桥接（调试能力是核心工程素养）：这套"gradient check → 看误差结构 → 反推根因"的流程不仅适用于 B 样条，对后续 D5 MINCO 章节的解析梯度同样有效——MINCO 的梯度推导比 B 样条更复杂（涉及时间分配的链式法则），一旦手写就更容易漏项，gradient check 几乎是唯一能在接入优化器前发现 bug 的手段。建议把 checkGradient 作为累积项目的常驻工具函数保留下来。

附录 F：上手前自检清单¶

在动手实现本章累积项目模块前，用下面的清单自查。如果某一条答不出来或答错，对应小节就是你应该回头精读的地方——这也是 R13 自测检查点的延伸应用：把"学完后的回顾"变成"动手前的过滤网"。

F.1 概念理解自检¶

我能说清为什么 B 样条在**在线重规划**中优于多项式，又为什么在**离线精确轨迹**中可能不如多项式？（§D4.1）
给定阶数 \(p\) 和控制点数 \(n+1\)，我能立刻算出节点数 \(m+1 = n+p+2\)，并说清节点和控制点是两套不同的索引？（附录 D.1）
我能解释"控制点不在曲线上"为什么不是缺陷，反而是凸包约束能用作安全保证的前提？（§D4.2.3）
我能说清 EGO-Planner 的 \((p,v)\) 锚点和 Fast-Planner 的 ESDF 在"碰撞信息表示"上的本质区别，以及它如何带来 10 倍的速度提升？（§D4.5）

F.2 实现就绪自检¶

我的 B 样条求值函数对区间索引 \(k\) 做了 p <= k <= n 的边界断言，对归一化参数 \(s\) 做了 \([0,1)\) 的检查？（§D4.2.4、附录 D.2）
我的解析梯度采用**散射式**累加（遍历代价项散射回控制点），而不是收集式（遍历控制点收集）？（附录 E.3）
我在接入 LBFGS-Lite 之前，已经用中心差分 checkGradient 验证过每一项代价的梯度，相对误差都在 \(10^{-5}\) 以下？（附录 E.2）
我理解 LBFGS-Lite 是无约束优化器，所有速度/加速度/碰撞约束都必须转成惩罚项加进代价，且碰撞权重要远大于平滑权重？（§D4.6、§D4.9.1）

F.3 调试预案自检¶

如果优化后轨迹穿障，我知道先打印 \(J_\text{collision}\) 终值、再在曲线上密采样逐点查碰撞？（故障 1）
如果代价下降但轨迹变糟，我知道这是梯度 bug 的典型信号，应立刻跑 gradient check 而不是盲目调权重？（附录 E.1）
如果规划超时，我知道先确认 CPU governor 是 performance、再查控制点数量和 ESDF 更新范围？（故障 4）

结语：B 样条轨迹优化是无人机自主导航从"实验室能飞"走向"野外敢飞"的关键一跃。它把多项式时代"求解析解、改一点全盘重算"的脆弱范式，换成了"凸包作保、局部支撑、有限约束、惩罚优化"的鲁棒范式——而这套范式的每一个零件，都会在下一章的 MINCO 中被继承、压缩、再升华。把本章的 gradient check 工具和散射式梯度框架带走，你在 D5 会用得上。