D5 MINCO 轨迹表示与安全走廊：从多项式系数到路点-时间的范式跃迁

本章定位：本章是无人机轨迹规划专题中**算法密度最高、工程复用价值最大**的一章。它对应"多项式轨迹 → B 样条 → MINCO"这条演化主线的终点——MINCO（Minimum Control，最小控制量）轨迹表示，以及与之配套的安全飞行走廊（Safe Flight Corridor, SFC）生成方法。MINCO 不是又一种多项式参数化的变体，而是从决策变量、求解方法到约束处理的**全栈革新**——它把四旋翼轨迹优化从"有约束 QP/SQP"推进到"全无约束 L-BFGS"，把计算复杂度从 O(M^3) 压到 O(M)，把单段规划时间压到 1 微秒。ZJU FAST Lab 围绕 MINCO 建立的开源生态（GCOPTER、EGO-Planner-v2、FIRI、DDR-opt 等）已成为四旋翼规划的事实标准。

性质：本章内容**专属四旋翼**——MINCO 的数学基础是微分平坦，而微分平坦将全状态/输入映射到位置及其导数的性质是四旋翼特有的（机械臂和足式机器人不具备）。安全走廊也是无人机在三维自由空间中飞行的特有几何约束表达。

读者画像：已完成 D3（多项式轨迹基础）和 D4（B 样条轨迹），掌握 Eigen 块操作（v8 Ch11）、凸优化基础（QP/SOCP）、L-BFGS 基本原理的算法工程师。

在累积项目中的位置：本章给无人机规划测试台加装"MINCO 后端 + SFC 走廊生成"模块——替换 D3 的 Mellinger QP 后端，获得 10-100 倍的速度提升和原生时间优化能力；替换手工走廊为 FIRI 自动生成，获得种子包含保证和 100 倍走廊生成加速。

---

前置自测

进入本章前，确认以下 5 题。答不出 2 题以上，建议先补对应前置。

1. 写出四旋翼微分平坦的定义：平坦输出是什么？从平坦输出恢复全状态需要几阶导数？（不会 → 回 D3 微分平坦基础）
2. 给定 M 段 7 次多项式轨迹，Mellinger 2011 的 minimum snap QP 有多少个决策变量？为什么条件数随段数增长？（不会 → 回 D3 §3.2）
3. 什么是带状矩阵（banded matrix）？带宽为 w 的 n 阶带状矩阵做 LU 分解的复杂度是多少？（不会 → 回线性代数 / Eigen 稀疏矩阵基础）
4. L-BFGS 和标准 BFGS 的区别是什么？L-BFGS 的"L"代表什么？它的每次迭代复杂度是多少？（不会 → 回优化算法基础）
5. 凸多面体（convex polytope）的半空间表示 \(\{x : Ax \le b\}\) 是什么意思？两个凸多面体的交集还是凸集吗？（不会 → 回凸优化基础）

这 5 题对应本章的支柱：微分平坦（MINCO 的物理基础）、多项式 QP 的局限（MINCO 要解决的问题）、带状矩阵（MINCO 的核心数据结构）、L-BFGS（MINCO 的优化引擎）、凸多面体（安全走廊的数学表示）。

---

本章目标

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

1. 推导 MINCO 轨迹类的数学定义——给定中间路点 \(q\) 和段时间 \(T\)，MINCO 如何通过带状线性方程 \(M(T)c = b(q)\) 隐式确定多项式系数 \(c\)，以及为什么这是**精确的最小表示**
2. 说清 MINCO 相对 Mellinger/Richter 的三重跃迁：决策变量从 \(O(sM)\) 降到 \(O(M)\)、求解从 QP 变为无约束 L-BFGS、梯度从自动微分变为 O(M) 隐式微分
3. 独立精读 GCOPTER 的 header-only C++ 实现——minco.hpp（轨迹类）、firi.hpp（走廊生成）、gcopter.hpp（外层优化）、flatness.hpp（微分平坦映射），理解每个头文件的核心数据流
4. 理解 安全飞行走廊（SFC）的三代演进：DecompUtil（椭球膨胀，2017）→ IRIS（SDP 最大椭球，2014）→ FIRI（SOCP + 种子包含保证，2025）→ CIRI（构型空间膨胀，2025），并能据此选型
5. 理解 SUPER 的双轨迹安全范式——primary 轨迹可伸入未知空间争取速度，backup 轨迹严格在已知自由空间内且末端悬停，递归可行性保证"任何时刻都有安全后备"
6. 实现一个 2D 简化版 MINCO（minimum jerk, s=2），验证带状矩阵求解和隐式微分的正确性
7. 对比 **MINCO 与 B 样条**在局部控制性、时间优化、最优性、计算复杂度上的权衡，并能根据应用场景选择

---

本章知识导航 ⭐

本章沿一条主线展开：四旋翼轨迹参数化的终极形态——从"多项式系数"到"路点+时间"的范式跃迁，以及与之配套的"安全走廊"几何约束表达。

             D3 多项式轨迹基础 (Mellinger 2011 / Richter 2016)
                          │ "决策变量太多、QP 条件数爆炸"
                          ▼
    §D5.1 MINCO 数学本质：路点+时间 → 带状 M(T)c=b(q) → O(M) 求解
                          │
    §D5.2 GCOPTER 源码精读：minco.hpp / gcopter.hpp / flatness.hpp
                          │
                          ├─────── 轨迹需要在安全区域内 ───────┐
                          │                                   │
    §D5.3 安全走廊三代演进                                     │
          DecompUtil → IRIS → FIRI → CIRI                    │
                          │                                   │
                          ▼                                   ▼
    §D5.4 SUPER：双轨迹安全范式 (primary + backup)
                          │
    §D5.5 EGO-Planner-v2：MINCO 群飞应用
                          │
    §D5.6 MINCO 生态与前沿：DDR-opt / MIGHTY / 学习型方法
                          │
    §D5.7 MINCO vs B 样条：系统对比与选型

本章方法谱系：四旋翼轨迹优化从 2011 年 Mellinger-Kumar 的 minimum snap QP 起步（24M 个决策变量的约束 QP），2016 年由 Richter-Bry-Roy 改进为闭式端点导数参数化（降到 12(M+1) 变量），2021 年由 Wang Zhepei 的 large_scale_traj_optimizer 发现带状矩阵结构（降到 4M-3 变量 + O(M) 求解），2022 年由 GCOPTER/MINCO 论文完成完整理论框架（一般 s 阶 + diffeomorphic 全无约束化 + L-BFGS）。安全走廊从 2014 年 IRIS 的 SDP 方法起步，经 2017 年 DecompUtil 的椭球膨胀工程化，到 2025 年 FIRI 用 SOCP 替代 SDP 实现 100 倍加速并保证种子包含，再到 SUPER 的 CIRI 将走廊推广到构型空间。

小节	主题	难度	预计阅读
§D5.1	MINCO 数学本质：从历史根源到完整推导	⭐⭐⭐	3-4 小时
§D5.2	GCOPTER header-only C++ 精读	⭐⭐	2-3 小时
§D5.3	安全飞行走廊三代演进	⭐⭐⭐	2-3 小时
§D5.4	SUPER 双轨迹安全范式	⭐⭐⭐	2 小时
§D5.5	EGO-Planner-v2 群飞应用	⭐⭐	1-2 小时
§D5.6	MINCO 生态与前沿	⭐⭐	1 小时
§D5.7	MINCO vs B 样条系统对比	⭐⭐	1 小时

推荐阅读路径：§D5.1 是全章地基（MINCO 的数学本质），务必读透——理解了"为什么决策变量可以只有路点和时间""带状矩阵从何而来""隐式微分如何给出 O(M) 梯度"这三个问题，后续内容水到渠成。§D5.2 紧随其后做源码精读。若你的目标是"尽快在工程中用上 MINCO"，读完 §D5.1-D5.2 后可直接跳到 §D5.3（走廊生成），把 §D5.4-D5.7 留作进阶。若追求理论完整，按顺序读。

---

前置知识桥接

回顾——Mellinger 2011 的 minimum snap QP 做了什么：你在 D3 中学过，四旋翼的微分平坦性质将全状态/输入映射到平坦输出 \(\sigma = [x, y, z, \psi]^T\) 及其导数。minimum snap 将轨迹表示为分段 7 次多项式，最小化四阶导数（snap）的积分 \(\int_0^T \|\sigma^{(4)}(t)\|^2 dt\)，通过一个**约束 QP** 求解。这个 QP 的决策变量是 \(24M\) 个多项式系数（M 段 \(\times\) 8 系数/段 \(\times\) 3 维），约束是端点导数连续性。QP 的 Hessian 矩阵 \(H\) 的条件数随段数 \(M\) 急剧增长——这是 MINCO 要解决的第一个问题。

回顾——Richter 2016 的改进：Richter 发现 Mellinger 的 QP 有闭式解——通过将决策变量从多项式系数换成端点导数，约束矩阵变为可逆方阵，QP 退化为无约束最小二乘。但决策变量维度仍为 \(O(sM)\)，且映射矩阵 \(A(T)^{-1}\) 不是带状的，无法做 \(O(M)\) 回代。

本章在前置之上加什么：MINCO 把决策变量从端点导数进一步精简为**仅有中间路点和段时间**——这是**精确的最小表示**，不是近似。多项式系数通过一个**带状线性方程 \(M(T)c = b(q)\)** 隐式确定，求解和梯度计算都是 \(O(M)\)。加上 diffeomorphic 参数化消除所有约束后，整个问题变为无约束优化，可用 L-BFGS 以每次迭代 \(O(M)\) 的代价求解。

本质洞察：MINCO 的核心思想是"让最优性条件自动确定多项式系数"。在 Mellinger 的方法中，多项式系数是自由的决策变量，需要约束来保证连续性；在 MINCO 中，连续性和最优性是同一件事——给定路点和时间后，满足连续性约束的最优系数由带状方程唯一确定。决策变量的减少不是因为扔掉了信息，而是因为"最优性条件替你做了选择"。

---

如果跳过本章会怎样

场景一：用 Mellinger QP 做 50 段轨迹，条件数爆炸求解失败。 你要在密林中规划一条穿越 50 棵树的轨迹，用 Mellinger 的方法需要求解一个 \(1200 \times 1200\) 的 QP（\(24 \times 50\) 变量）。QP 的 Hessian 条件数超过 \(10^{15}\)，求解器报 numerical issues，输出的轨迹在中间段出现不物理的振荡。MINCO 的决策变量只有 \(4 \times 50 - 3 = 197\) 个，带状方程条件数稳定在 \(O(1)\)，1 毫秒内求解完成。

场景二：走廊生成的种子点被"切"到走廊外面，优化找不到可行解。 你用 DecompUtil 生成安全走廊，但生成的凸多面体不包含原始种子点——MINCO 的路点被约束在走廊交集内，而交集为空（因为种子不在多面体中），优化器报 infeasible。FIRI 的 Restrictive Inflation 保证种子点始终在走廊内，根本杜绝这个问题。

这两个场景会在累积项目的 D5 模块里被复现：先看 Mellinger baseline 在多段场景下的数值崩溃，再看 MINCO + FIRI 的稳定求解。

---

预计阅读时间

阅读方式	时间	适合谁
精读（含推导 + 源码精读 + 实现 2D MINCO）	14-18 小时	第一次系统学 MINCO 的读者
速读（抓 §D5.1 本质 + §D5.2 源码 + §D5.3 走廊选型）	5-6 小时	有多项式轨迹背景、要快速用上 GCOPTER 的工程师
速查（只看各节对比表 + API 速查 + 故障排查）	40 分钟	已在实现、回来查某个 API 或选型问题

---

科研发展脉络 ⭐

在深入技术细节之前，先建立历史全景——理解每个方法"解决了什么问题、留下了什么问题"，才能理解 MINCO 为什么是现在这个样子。

年份	论文	Venue	引用量	核心贡献	留下的问题
2011	Mellinger, Kumar	ICRA	~2500	奠基：微分平坦 + minimum snap QP	24M 变量、条件数爆炸
2014	Deits, Tedrake (IRIS)	WAFR	~800	SDP 最大体积内切椭球 → 凸走廊	SDP 慢、不保证种子包含
2016	Richter, Bry, Roy	ISRR	~1200	闭式 QP：端点导数参数化	仍 O(sM) 变量、\(A^{-1}\) 非带状
2017	Liu 等 (DecompUtil)	RA-L	~500	SFC 工程化：椭球膨胀 → 凸多面体	不保证种子包含、体积非最大
2021	Wang 等 (large_scale)	ICRA	~200	MINCO 前身：发现带状结构、O(M) 梯度	仅 s=4 特例、无完整理论
2022	Wang, Zhou, Xu, Gao (GCOPTER)	TRO	~400	MINCO 完整理论：一般 s 阶 + L-BFGS 全无约束化	header-only 代码
2022	Zhou 等 (EGO-v2)	Sci. Rob.	~500	MINCO 用于 10 机林地群飞	去中心化碰撞为软约束
2025	Wang Q. 等 (FIRI)	TRO	~50	SOCP 替代 SDP、种子包含保证、100 倍加速	—
2025	Ren 等 (SUPER)	Sci. Rob.	~100	双轨迹范式 + CIRI 构型空间走廊、20 m/s 林地夜飞	—

演化主线：

Mellinger 2011 ──→ Richter 2016 ──→ large_scale 2021 ──→ GCOPTER/MINCO 2022
  (min-snap QP)    (闭式端点 QP)    (O(M) 解析梯度)      (隐式系数 + L-BFGS)
     24M 变量       12(M+1) 变量     4M-3 变量             全无约束化
                                                                │
SFC: IRIS 2014 ──→ DecompUtil 2017 ──→ FIRI 2025 ──→ CIRI 2025
     (SDP, 慢)    (椭球膨胀, 工程化)  (SOCP, 100x)   (构型空间)
                                                                │
                                                    MINCO + SFC ──→ EGO-v2 群飞
                                                                │
                                                    MINCO + CIRI + 双轨迹 ──→ SUPER

多视角理解：这条演化线的两种读法

• 数学视角：每一步都在**减少决策变量**并**提升矩阵结构的可利用性**——从稠密 Hessian 到带状矩阵到无约束。这是数值线性代数驱动的优化范式演进。
• 工程视角：每一步都在**压缩计算时间**——从 10ms 到 1ms 到 0.1ms。这不是渐进改善，而是量级跃迁，使得从离线规划到在线实时规划到群飞规划成为可能。两种视角指向同一个结论：MINCO 的数学优雅和工程高效是同一枚硬币的两面。

---

§D5.1 MINCO 的数学本质——从多项式系数到路点+时间 ⭐⭐⭐

D5.1.1 动机：为什么需要 MINCO

三个历史阶段的痛点累积

在深入 MINCO 的数学之前，必须理解它解决了什么问题。让我们逐步追溯多项式轨迹规划的三个历史阶段，看清每一步的改进和遗留问题。

第一阶段：Mellinger-Kumar 2011——直接系数参数化

Mellinger 和 Kumar 在 2011 年 ICRA 上发表的 "Minimum Snap Trajectory Generation and Control for Quadrotors" 是四旋翼轨迹优化的奠基之作。他们利用四旋翼的**微分平坦**性质（下文 §D5.1.2 详细推导），将轨迹规划问题转化为位置轨迹的优化问题。

核心方法是将每段轨迹表示为 7 阶多项式（\(2 \times 4 - 1 = 7\)），最小化 snap（四阶导数）的积分。M 段三维轨迹的决策变量是 \(8M \times 3 = 24M\) 个多项式系数，通过连续性约束和边界条件构成一个约束 QP：

\[\min_c \ c^T H c \quad \text{s.t.} \quad Ac = b\]

其中 \(H\) 是 snap 的 Hessian 矩阵，\(A\) 编码连续性和边界条件。

这个方法有三个根本问题：

问题	具体表现	根本原因
决策变量维度高	\(24M\)（M=50 时为 1200）	每个系数都是自由变量
数值不稳定	\(H\) 条件数随 \(M\) 指数增长	单项式基的高次幂在端点处数值差异巨大
时间优化困难	\(T\) 和 \(c\) 耦合在 \(H\) 和 \(A\) 中	改变 \(T\) 需要重新组装整个 QP

读到这里你可能会问："为什么不直接用正交基（如 Legendre 多项式）替换单项式基来改善条件数？"确实有人这样做过（如 Bernstein 基的 Btraj 2018），但这只解决了数值稳定性的表面问题，没有解决**决策变量维度过高**和**时间优化困难**的根本问题。MINCO 的突破在于换了一种完全不同的思路。

第二阶段：Richter-Bry-Roy 2016——端点导数参数化

Richter 等人在 2016 年发现，Mellinger 的 QP 实际上有**闭式解**。他们的关键观察是：如果把决策变量从多项式系数换成**端点导数**（每个节点处的位置、速度、加速度、jerk 值），约束矩阵 \(A\) 变为可逆方阵。

设映射矩阵为 \(A(T)\)，将端点导数 \(d\) 映射为多项式系数：\(c = A(T)^{-1} \cdot d\)。代入 snap 代价后：

\[\min_d \ d^T \underbrace{[A^{-T} H A^{-1}]}_{Q(T)} d\]

这是一个无约束二次型，闭式解为 \(d^* = Q^{-1}(T) \cdot (\text{固定导数贡献})\)。改进是显著的——闭式求解，无需 QP solver。但遗留问题仍然严重：

改进	仍有的问题
闭式解，不需要 QP solver	决策变量维度仍为 \(O(sM)\)，s=4 时为 \(4(M+1) \times 3\)
自由/固定导数分离	\(A(T)^{-1}\) 不是带状的 → 无法 O(M) 回代
段时间可梯度下降	\(\partial J / \partial T\) 需要对 \(A^{-1}\) 求导 → 复杂且不稳定

对比性思维："不是减少变量，而是选对了变量"。Richter 的改进是换了**表示基**（系数 → 端点导数），但变量数量级没变。MINCO 的突破不是在已有变量中做更好的参数化，而是发现了一个**全新的最小变量集**——中间路点和段时间。这个变量集之所以"够用"，是因为最优性条件自动确定了所有其他信息。

第三阶段：large_scale_traj_optimizer 2021——MINCO 前身

Wang Zhepei 在 2021 年 ICRA 论文 "Generating Minimum-Snap Quadrotor Trajectories Really Fast" 中发现了关键的数学结构：

对于 minimum snap 问题（\(s=4\)），如果中间点只约束**位置**（不约束高阶导数），那么最优解的多项式系数 \(c\) 可以通过一个**带状线性方程组** \(M(T) \cdot c = b(q)\) 直接确定——带宽仅为 \(2s = 8\)。

这个发现带来了三个革命性改进：

1. 决策变量维度大幅下降：从 \(4(M+1) \times 3\) 降到 \((M-1) \times 3 + M = 4M - 3\)（约为 Richter 的 1/4）
2. O(M) 带状求解：带状 PLU 分解只需 \(O(M \cdot s^2) = O(M)\)（\(s\) 是固定常数）
3. O(M) 解析梯度：通过隐式微分获得 \(\partial J / \partial q\) 和 \(\partial J / \partial T\)，无需显式构造 Jacobian

这就是 large_scale_traj_optimizer 仓库中的核心算法，它能以 **1 微秒/段**的速度生成轨迹。

第四阶段：GCOPTER/MINCO 2022——完整理论框架

GCOPTER（Wang, Zhou, Xu, Gao, IEEE T-RO, 38(5):3259-3278, 2022）将第三阶段的 minimum snap 特例推广为一般理论：

• 不局限于 \(s=4\)（snap），支持任意阶 \(s\)（\(s=2\) 即 minimum jerk，\(s=3\) 即 minimum crack 等）
• 中间点可以约束任意阶导数（不仅限于位置）
• 提出了 **diffeomorphic 参数化**将所有约束消除为无约束问题
• 完整的 **L-BFGS 无约束优化**框架

这就是 MINCO 的全貌。

阶段小结：到这里我们理解了 MINCO 的历史定位——它不是一个增量改进，而是四旋翼轨迹参数化的**范式跃迁**。接下来我们先推导微分平坦（MINCO 的物理基础），然后进入 MINCO 的严格数学定义。

D5.1.2 四旋翼微分平坦的完整推导 ⭐⭐

MINCO 的意义建立在微分平坦之上。理解"为什么规划一条光滑的位置轨迹就足够控制整个四旋翼"是理解 MINCO 的前提。

四旋翼刚体动力学模型

在世界坐标系 \(\{W\}\) 中，四旋翼的刚体动力学为：

\[\dot{p} = v \tag{位置动力学}$$ $$m\dot{v} = mge_3 - fRe_3 + f_{\text{drag}} \tag{平移动力学}$$ $$\dot{R} = R[\omega]_\times \tag{姿态运动学}$$ $$J\dot{\omega} = -\omega \times J\omega + \tau \tag{旋转动力学}\]

其中：\(p \in \mathbb{R}^3\) 为质心位置，\(v \in \mathbb{R}^3\) 为质心速度，\(R \in SO(3)\) 为机体到世界的旋转矩阵，\(\omega \in \mathbb{R}^3\) 为机体角速度，\(m\) 为质量，\(J\) 为惯性矩阵，\(f\) 为总推力（标量），\(\tau \in \mathbb{R}^3\) 为体力矩，\(e_3 = [0, 0, 1]^T\)，\(f_{\text{drag}}\) 为空气阻力（简单模型中忽略）。

定义平坦输出

定义**平坦输出**（Flat Output）\(\sigma = [x, y, z, \psi]^T = [p, \psi]^T\)，其中 \(\psi\) 是偏航角。

微分平坦（Differential Flatness）的定义：一个动力系统是微分平坦的，如果存在一组输出变量（平坦输出），使得系统的全部状态和输入都可以用平坦输出及其**有限阶导数**唯一确定。换言之，平坦输出的轨迹完全决定了系统的行为——不需要额外的微分方程求解。

步骤 1：从平坦输出恢复推力方向和大小

由平移动力学（忽略空气阻力）：

\[m\dot{v} = mge_3 - fRe_3 \implies fRe_3 = m(ge_3 - \ddot{p})\]

定义**期望推力向量**：

\[t_d = m(ge_3 - \ddot{p})\]

则：

\[f = \|t_d\| \quad \text{(总推力)} \qquad z_B = Re_3 = \frac{t_d}{\|t_d\|} \quad \text{(机体 z 轴方向)}\]

这告诉我们：位置的二阶导数（加速度）决定了推力大小和推力方向。加速度越大（急加速/急减速/急转弯），推力越大。加速度方向决定机体"倾向哪个方向"。

步骤 2：从偏航角恢复完整姿态 \(R\)

已知 \(z_B\)（机体 z 轴）和偏航角 \(\psi\)，可以构造完整旋转矩阵。首先定义偏航参考向量：

\[x_C = [\cos\psi, \sin\psi, 0]^T\]

然后依次构造机体坐标轴：

\[y_B = \frac{z_B \times x_C}{\|z_B \times x_C\|}, \quad x_B = y_B \times z_B, \quad R = [x_B \ | \ y_B \ | \ z_B]\]

步骤 3：从高阶导数恢复角速度 \(\omega\)

对 \(fRe_3 = t_d\) 两边关于时间求导：

\[\dot{f}Re_3 + fR[\omega]_\times e_3 = \dot{t}_d\]

其中 \([\omega]_\times e_3 = [-\omega_y, \omega_x, 0]^T\)。将 \(\dot{t}_d\) 投影到 \(x_B\) 和 \(y_B\) 方向：

\[\omega_x = -\frac{\dot{t}_d \cdot y_B}{f}, \quad \omega_y = \frac{\dot{t}_d \cdot x_B}{f}, \quad \omega_z = \dot{\psi} \cdot (z_B \cdot e_3)\]

这里 \(\dot{t}_d = m(ge_3 - \dddot{p})\) 涉及位置的**三阶导数**（jerk）。

步骤 4：从更高阶导数恢复力矩 \(\tau\)

继续对角速度求导得角加速度 \(\dot{\omega}\)，代入旋转动力学：

\[\tau = J\dot{\omega} + \omega \times J\omega\]

而 \(\dot{\omega}\) 涉及 \(\ddot{t}_d = m(-p^{(4)})\)，即位置的**四阶导数**（snap）。

平坦映射的导数阶次总表

物理量	需要平坦输出的导数阶数	说明
位置 \(p\)	0 阶	直接读取
速度 \(v\)	1 阶 (\(\dot{p}\))	位置求导
推力 \(f\)、推力方向 \(z_B\)	2 阶 (\(\ddot{p}\))	加速度决定推力
姿态 \(R\)	2 阶 (\(\ddot{p}\)) + \(\psi\)	加速度 + 偏航
角速度 \(\omega\)	3 阶 (\(\dddot{p}\)) + \(\dot{\psi}\)	jerk 决定角速度
力矩 \(\tau\)	4 阶 (\(p^{(4)}\)) + \(\ddot{\psi}\)	snap 决定力矩

关键结论：要使力矩 \(\tau\) 光滑（进而使电机指令平滑），需要 \(\sigma\) 的**至少四阶导数连续**。这就是 minimum snap（\(s=4\)）的物理动机——最小化 snap 的积分等价于最小化电机指令的"抖动"。

本质洞察：微分平坦将"12 维状态空间 + 4 维输入"的复杂动力学系统**坍缩**为"4 维平坦输出"的轨迹规划问题。这不仅是维度降低——更根本的是，它消除了显式的动力学约束（\(m\dot{v} = \ldots\) 等微分方程），将约束变成了平坦输出的代数函数。正因为如此，MINCO 才能把问题转化为"规划一条足够光滑的位置曲线"。

考虑空气阻力时的修正

实际高速飞行中（\(>5\) m/s），空气阻力不可忽略。GCOPTER 论文讨论了非线性阻力模型：

\[f_{\text{drag}} = -R \cdot \text{diag}(k_1, k_2, k_3) \cdot R^T v\]

此时平坦映射变得更复杂——\(t_d\) 的表达式中多了阻力项，推力和姿态的计算变为隐式方程。但核心思路不变：从 \(\sigma\) 及其导数仍可恢复全部状态和输入。GCOPTER 的 flatness.hpp 实现了包含阻力的完整平坦映射。

D5.1.3 MINCO 轨迹类的严格数学定义 ⭐⭐⭐

定义环境

考虑一个 \(s\) 阶积分链系统（\(s\)-th order integrator chain）：

\[z^{(s)}(t) = v(t)\]

其中 \(z(t) \in \mathbb{R}^m\) 是输出（\(m\) 维空间），\(v(t) \in \mathbb{R}^m\) 是控制输入。对四旋翼，\(m=3\)（三维位置），\(s=4\)（snap 是控制量）。

多段轨迹的时间结构

给定 \(M\) 段轨迹，时间戳序列为 \(t_0 < t_1 < \cdots < t_M\)。段时间向量 \(T = [T_1, T_2, \ldots, T_M]^T \in \mathbb{R}_+^M\)，其中 \(T_i = t_i - t_{i-1} > 0\)。

中间条件

在每个内部节点 \(t_i\)（\(i=1,\ldots,M-1\)），约束前 \(d_i\) 阶导数（\(d_i \le s\)）。最常见的情况是 \(d_i = 1\)——仅约束位置。

MINCO 的正式定义

定义（MINCO，Minimum Control）：给定边界条件 \(D_0, D_M\)（各 \(s\) 个导数值）、中间条件 \(\{\bar{q}_i\}\)（各 \(d_i\) 个导数值）和段时间 \(T\)，MINCO 轨迹是使**控制量积分最小**的分段多项式：

\[\min \int_{t_0}^{t_M} \|z^{(s)}(t)\|_W^2 \, dt\]

其中 \(W\) 是正定权重矩阵，满足上述所有边界条件和中间条件。

关键定理（Wang 2022 Theorem 2）

定理：\(z^*(t)\) 是上述多段控制量最小化问题的最优解，当且仅当：

1. 每段 \(z^*|_{[t_{i-1}, t_i]}\) 是**次数 \(2s-1\) 的多项式**
2. 满足边界条件
3. 满足中间条件
4. 在中间点处，\(z^*(t)\) 是 **\(\bar{d}_i - 1\) 次连续可微**的，其中 \(\bar{d}_i = 2s - d_i\)

且满足这些条件的轨迹**唯一存在**。

直觉理解

读到这里你可能会问："条件 4 的 \(\bar{d}_i = 2s - d_i\) 是从哪来的？为什么约束越多的中间点反而连续性越低？"

这不是人为设定的，而是**最优性的必要条件**。物理直觉是这样的：

• 每段 \((2s-1)\) 次多项式有 \(2s\) 个自由度（系数）
• 在内部节点处，\(d_i\) 个自由度被中间条件"用掉"了（约束了 \(d_i\) 个导数值）
• 剩下的 \(2s - d_i = \bar{d}_i\) 个自由度被最优性条件"花"在保证连续性上
• 所以约束越多（\(d_i\) 越大），留给连续性的自由度越少（\(\bar{d}_i\) 越小）

这个"约束+连续性 = 总自由度"的**守恒关系** \(d_i + \bar{d}_i = 2s\) 是 MINCO 理论的基石。

最常见特例：\(d_i = 1\)（仅约束位置）

在大多数实际应用中，中间点只约束位置。此时对 \(s=4\)（minimum snap）：

• 多项式次数 \(= 2 \times 4 - 1 = 7\)
• 中间点连续性 \(= 2 \times 4 - 1 - 1 = 6\) 次连续可微（\(C^6\) 连续，位置到 pop 连续）
• 决策变量：\((M-1)\) 个中间路点 \(q \in \mathbb{R}^{(M-1) \times 3}\) + \(M\) 个段时间 \(T \in \mathbb{R}_+^M\)

决策变量维度的三代对比

方法	决策变量维度（\(m=3, s=4\)）	说明
Mellinger 2011	\(24M\)	\(8M\) 系数 \(\times\) 3 维
Richter 2016	\(12(M+1)\)	\(4(M+1)\) 端点导数 \(\times\) 3 维
MINCO 2022	\(3(M-1) + M = 4M - 3\)	\((M-1)\) 路点 \(\times\) 3 维 + \(M\) 段时间

MINCO 的决策变量是**最紧凑的**——这不是近似，而是**精确的最小表示**。给定路点和时间后，多项式系数由带状方程唯一确定，没有任何冗余。

反事实推理：如果中间点也约束速度（\(d_i = 2\)）会怎样？ 决策变量增加（每个中间点多了 3 个速度值），但连续性降低（从 \(C^6\) 降到 \(C^4\)）。轨迹会在中间点处出现更明显的"拐角"。实际上，大多数场景下只约束位置就够了——MINCO 的最优性条件会自动选择最光滑的速度/加速度分布。

D5.1.4 带状线性系统 \(M(T)c = b(q)\) 的结构 ⭐⭐⭐

这是 MINCO 的核心数学——理解了这个带状方程，就理解了 MINCO 的计算效率从何而来。

单段多项式的表示

第 \(i\) 段（\(i=1,\ldots,M\)）在局部时间 \(\tau \in [0, T_i]\) 上的多项式为 \(p_i(\tau) = c_i^T \beta(\tau)\)，其中 \(\beta(\tau) = [1, \tau, \tau^2, \ldots, \tau^{2s-1}]^T\) 是单项式基。

各阶导数在起点 \(\tau=0\) 时有简洁形式：\(\beta^{(k)}(0) = k! \cdot e_{k+1}\)（仅第 \(k+1\) 个分量非零）。在终点 \(\tau=T_i\) 时，\(\beta^{(k)}(T_i)\) 是 \(T_i\) 的多项式——这正是矩阵 \(M(T)\) 依赖于段时间 \(T\) 的原因。

构建约束方程

MINCO 轨迹的约束分为三类：

A. 边界条件：起点 \(t_0\) 和终点 \(t_M\) 处各 \(s\) 个导数值。起点条件写成矩阵形式：\(E_0 \cdot c_1 = D_0\)，其中 \(E_0 \in \mathbb{R}^{s \times 2s}\) 是由 \(\beta^{(k)}(0)\) 组成的矩阵——实际上是一个对角矩阵的前 \(s\) 行。

B. 中间点条件：在第 \(i\) 个内部节点（\(d_i = 1\) 时）仅约束位置：\(p_i(T_i) = q_i\)。

C. 连续性条件：最优性要求 \(p_i^{(k)}(T_i) = p_{i+1}^{(k)}(0)\)，\(k = 0, 1, \ldots, \bar{d}_i - 1\)。当 \(d_i = 1\) 时，\(\bar{d}_i = 2s-1\)，意味着连续性从 0 阶到 \((2s-2)\) 阶，共 \(2s-1\) 个方程。加上 1 个位置约束，每个内部节点**恰好 \(2s\) 个方程**——等于每段的系数数。

完整的带状矩阵 \(M \in \mathbb{R}^{2sM \times 2sM}\)

       c₁         c₂         c₃        ...      cₘ
   ┌─────────┬─────────┬─────────┬────────┬─────────┐
   │  E₀     │    0    │    0    │  ...   │    0    │  ← 起点边界 (s 行)
   │  F₁(T₁) │  -E₁   │    0    │  ...   │    0    │  ← 节点 1 (2s 行)
   │    0    │  F₂(T₂) │  -E₂   │  ...   │    0    │  ← 节点 2 (2s 行)
   │   ...   │   ...   │   ...   │  ...   │   ...   │
   │    0    │    0    │   ...   │F_{M-1} │ -E_{M-1}│  ← 节点 M-1 (2s 行)
   │    0    │    0    │    0    │  ...   │ Eₘ(Tₘ) │  ← 终点边界 (s 行)
   └─────────┴─────────┴─────────┴────────┴─────────┘

关键性质：

1. 行数验证：\(s + (M-1) \times 2s + s = 2sM\) ✓
2. 列数验证：\(M \times 2s = 2sM\) ✓（方阵）
3. 带宽：每行最多涉及两个相邻系数块，有效半带宽为 \(2s\)
4. \(b\) 是 \(q\) 的线性函数：\(b = b_0 + B \cdot q\)
5. \(M\) 是 \(T\) 的函数：\(M = M(T)\)，因为 \(F_i(T_i)\) 依赖段时间
6. \(M\) 非奇异：由定理保证，\(c = M(T)^{-1} \cdot b(q)\) 唯一存在

多视角理解：带状矩阵的三种看法

• 数值线性代数视角：带状矩阵是稀疏矩阵中结构最好的一类——LU 分解的 fill-in 为零（不会产生新的非零元素），因此分解和回代的复杂度都是 \(O(n \cdot w^2)\)，其中 \(w\) 是带宽。
• 动力学视角：带状结构反映了轨迹的**时间局部性**——第 \(i\) 段的系数只取决于第 \(i\) 段和相邻段的约束，与远处的段无关。这和 SLAM 因子图的 Hessian 矩阵的稀疏结构异曲同工（见后文思考题）。
• 信息论视角：Mellinger 的方法用了 \(24M\) 个变量描述 \(4M-3\) 个自由度的信息——冗余了 \(\sim 20M\) 个变量。MINCO 消除了全部冗余，带状方程正是"用最少的信息精确描述轨迹"的数学体现。

D5.1.5 O(M) 带状 PLU 分解与求解 ⭐⭐

标准 LU 分解 vs 带状 LU 分解

	稠密矩阵 (\(n \times n\))	带状矩阵 (\(n \times n\), 带宽 \(w\))
LU 分解	\(O(n^3)\)	\(O(n \cdot w^2)\)
回代求解	\(O(n^2)\)	\(O(n \cdot w)\)

对 MINCO，\(n = 2sM\)，\(w = 2s\)，因此：

\[O(n \cdot w^2) = O(2sM \cdot (2s)^2) = O(4s^3 \cdot M) = O(M) \quad (s \text{ 是固定常数})\]

读到这里你可能会问："\(O(M)\) 是渐近复杂度，实际的常数因子有多大？"对 \(s=4\)（minimum snap），\(4s^3 = 256\)。一次 PLU 分解涉及 \(M\) 个 \(8 \times 8\) 块的操作——每个块需要约 \(8^3/3 \approx 170\) 次浮点运算（考虑 LU 的 \(n^3/3\) 公式）。\(M=20\) 段时，总计约 \(20 \times 170 = 3400\) 次浮点运算——在现代 CPU 上不到 1 微秒。这就是 MINCO "1 微秒/段"性能的数学基础。

块 Thomas 算法的详细步骤

由于 \(M\) 是块三对角的（每块 \(2s \times 2s\)），可以用块 Thomas 算法——三对角 LU 分解的块推广：

块 Thomas 算法:

初始化:
  D̃₁ = E₀                    (起点块, s × 2s)

前向消元 (i = 1, ..., M-1):
  // 目标: 消去第 i 个节点行的左下块 Fᵢ
  // 用 D̃ᵢ 的逆消去
  Lᵢ = Fᵢ(Tᵢ) · D̃ᵢ⁻¹        (乘数块, 2s × s 或 2s × 2s)
  D̃ᵢ₊₁ = -Eᵢ - Lᵢ · Uᵢ      (更新对角块)

  // 同时修改右端向量
  b̃ᵢ₊₁ = bᵢ₊₁ - Lᵢ · b̃ᵢ

终止:
  D̃ₘ = Eₘ(Tₘ) - Lₘ₋₁ · Uₘ₋₁  (终点块)

回代 (i = M, M-1, ..., 1):
  cᵢ = D̃ᵢ⁻¹ · (b̃ᵢ - Uᵢ · cᵢ₊₁)

每步涉及的操作都是 \(2s \times 2s\) 的块矩阵乘法和求逆——\(s\) 固定时为 \(O(1)\)。

GCOPTER 的实际实现策略

minco.hpp 没有使用 Eigen 的稀疏矩阵库（Eigen::SparseLU），理由有二：

1. Eigen::SparseLU 的 analyzePattern 步骤有不可接受的固定开销——对于 MINCO 这种每次 L-BFGS 迭代都要重新分解的场景，固定开销会主导总时间
2. MINCO 的带状结构比一般稀疏矩阵更规则，可以手写比通用稀疏求解器更快的特化代码

替代方案是**直接用 Eigen 的密集矩阵块操作**，但只操作非零块——利用块三对角结构做块 Thomas 算法：

// minco.hpp 的核心循环结构（简化）
// 前向消元阶段
for (int i = 0; i < M; ++i) {
    // 组装第 i 段的 Fᵢ(Tᵢ) 块 —— 使用编译期固定大小矩阵
    Eigen::Matrix<double, 2*S, 2*S> Fi;
    // 计算 T_i 的各次幂并填充 Fi
    // Fi 的结构：上半部分是 β^(k)(T_i) 评估，下半部分是连续性条件

    // 乘数块 L_i = F_i * D_tilde_i_inv
    // 这里的 D_tilde_i_inv 是前一步消元后的对角块的逆
    // 使用 Eigen 的 .solve() 避免显式求逆

    // 更新对角块和右端向量
    // D_tilde_{i+1} = -E_i - L_i * U_i
    // b_tilde_{i+1} = b_{i+1} - L_i * b_tilde_i
}

// 回代阶段
for (int i = M - 1; i >= 0; --i) {
    // c_i = D_tilde_i_inv * (b_tilde_i - U_i * c_{i+1})
    // 利用编译期固定大小，编译器可做 SIMD 向量化
}
// 关键：零 malloc —— 所有矩阵在优化开始前一次性预分配

为什么用固定大小矩阵而不是动态大小？

Eigen 的固定大小矩阵（如 Matrix<double, 8, 8>）在栈上分配，编译器可以将其放入 SIMD 寄存器做向量化运算。动态大小矩阵（如 MatrixXd）需要堆分配，且运行时才知道大小，编译器无法做同样的优化。对 \(8 \times 8\) 的块操作，固定大小比动态大小快 3-5 倍——这在 200 次 L-BFGS 迭代中累积为显著的差异。

内存预分配**是 GCOPTER 实时性能的关键——在 L-BFGS 的整个迭代过程中，**零 new/malloc。所有中间矩阵在优化开始前分配好，此后只做原地覆写。

理论-工程桥接：带状矩阵在数值线性代数中是经典概念，但 MINCO 是**第一个**将其应用于多项式轨迹优化的工作。之前的方法（Mellinger、Richter）之所以没有发现这个结构，是因为它们的参数化（系数或端点导数）产生的方程不是带状的——只有 MINCO 的"路点+时间"参数化才自然导出带状方程。这说明**好的参数化不仅降低变量维度，还能改善矩阵结构**。

D5.1.6 隐式微分——O(M) 梯度传播 ⭐⭐⭐

MINCO 的第二个核心技巧是通过**隐式微分**（implicit differentiation）计算代价函数对决策变量 \((q, T)\) 的梯度。

问题设定

代价函数的一般形式为：

\[J(q, T) = J_{\text{smooth}}(c(q,T), T) + J_{\text{penalty}}(c(q,T), T) + \rho(T)\]

其中 \(c(q,T)\) 通过 \(M(T) \cdot c = b(q)\) 隐式确定。链式法则给出：

\[\frac{\partial J}{\partial q} = \frac{\partial J}{\partial c} \cdot \frac{\partial c}{\partial q}, \quad \frac{\partial J}{\partial T} = \frac{\partial J}{\partial c} \cdot \frac{\partial c}{\partial T} + \frac{\partial J}{\partial T}\bigg|_{c \text{ 固定}}\]

关键问题是如何高效计算 \(\partial c / \partial q\) 和 \(\partial c / \partial T\)。

对 \(q\) 的梯度

从 \(M(T) \cdot c = b(q)\) 两边对 \(q\) 求导（\(M\) 不依赖 \(q\)）：

\[M \cdot \frac{\partial c}{\partial q} = \frac{\partial b}{\partial q}\]

代入链式法则并定义**伴随变量** \(\lambda = M^{-T} \cdot (\partial J / \partial c)^T\)：

\[\frac{\partial J}{\partial q} = \left(\frac{\partial b}{\partial q}\right)^T \lambda\]

\(\lambda\) 的计算：解线性方程 \(M(T)^T \cdot \lambda = (\partial J / \partial c)^T\)，利用已有的 PLU 分解做转置回代 → \(O(M)\)。\(\partial b / \partial q\) 是稀疏选择矩阵，乘法 \(O(M)\)。

对 \(T\) 的梯度

从 \(M(T) \cdot c = b(q)\) 对 \(T_i\) 求导：

\[\frac{\partial M}{\partial T_i} \cdot c + M \cdot \frac{\partial c}{\partial T_i} = \frac{\partial b}{\partial T_i}\]

因此：

\[\frac{\partial J}{\partial T_i} = \lambda^T \left(\frac{\partial b}{\partial T_i} - \frac{\partial M}{\partial T_i} \cdot c\right) + \frac{\partial J}{\partial T_i}\bigg|_c\]

关键观察：\(\partial M / \partial T_i\) 只影响 \(M\) 中包含 \(T_i\) 的块（即 \(F_i(T_i)\) 块），因此 \((\partial M / \partial T_i) \cdot c\) 的计算是 \(O(s^2)\) 的局部操作。

总梯度计算流程

输入: q, T, ∂J/∂c (代价函数对系数的梯度，通过前向评估获得)

1. [O(M)] 解伴随方程: M(T)ᵀ · λ = (∂J/∂c)ᵀ  (利用已有 PLU)
2. [O(M)] 计算 ∂J/∂q: 对每个 i，提取 λ 中对应行
3. [O(M)] 计算 ∂J/∂T: 对每个 i，局部计算 ∂Fᵢ/∂Tᵢ · cᵢ 并组合

总计: O(M)

与自动微分的对比

	隐式微分（MINCO）	自动微分（如 CasADi, JAX）
理论基础	利用 \(M \cdot c = b\) 的解析结构	通用的计算图反向传播
复杂度	\(O(M)\)	\(O(M \cdot s^3)\) 或更高
精度	精确到机器精度	可能有数值累积误差
代码复杂度	高（需手写推导）	低（自动生成）
维护性	需要数学功底	更易修改

本质洞察：MINCO 的 \(O(M)\) 梯度不是靠"省略计算"实现的，而是靠**利用数学结构**——带状矩阵的转置回代天然是 \(O(M)\) 的。自动微分不知道 \(M\) 是带状的，因此无法利用这个结构。这正是"领域知识 + 手写推导"在特定问题上胜过"通用工具"的典型案例。

D5.1.7 Diffeomorphic 参数化——全无约束化 ⭐⭐

MINCO 的第三个关键创新是将**所有约束**通过光滑映射消除，使整个问题变为**无约束优化**。

原始问题中的约束

1. 路点约束：\(q_i\) 必须在安全走廊的相邻多面体交集内：\(q_i \in P_i \cap P_{i+1}\)
2. 时间约束：\(T_i > 0\)（段时间必须为正）
3. 动力学约束：速度、加速度、推力等不超过限制

约束 1 的消除：路点的仿射映射

对凸多面体交集 \(P_i \cap P_{i+1}\)，GCOPTER 构造基于内切椭球的映射：

\[q_i = \text{center}(E) + \text{shape}(E) \cdot \tanh(\tilde{q}_i)\]

其中 \(\tilde{q}_i \in \mathbb{R}^m\) 是无约束变量，\(E\) 是 \(P_i \cap P_{i+1}\) 的内切椭球。\(\tanh\) 保证 \(q_i\) 严格在椭球内部（因而在多面体交集内部），且映射是可微的。

约束 2 的消除：时间的指数映射

\[T_i = \exp(\tau_i), \quad \tau_i \in \mathbb{R} \text{（无约束）}\]

\(\exp\) 保证 \(T_i > 0\)，且映射可微。梯度链式法则：\(\partial J / \partial \tau_i = (\partial J / \partial T_i) \cdot T_i\)。

约束 3 的消除：三次惩罚函数

动力学约束通过**光滑惩罚函数**处理：

\[J_{\text{dyn}} = \sum_i \int_0^{T_i} w_v \cdot \max(0, \|v(t)\| - v_{\max})^3 + w_a \cdot \max(0, \|a(t)\| - a_{\max})^3 \, dt\]

使用三次惩罚 \(\max(0, \cdot)^3\) 而非二次惩罚的原因：三次惩罚的梯度在约束边界处是 \(C^1\) 连续的（二阶可微），对 L-BFGS 的曲率估计更友好，收敛更稳定。

消除后的最终优化问题

\[\min_{\tilde{q}, \tau} J(q(\tilde{q}), T(\tau))\]

这是一个**完全无约束的**非线性优化问题，可以直接用 L-BFGS 求解。

为什么不用约束求解器（如 IPOPT/OSQP）？

	L-BFGS（GCOPTER 选择）	IPOPT/SQP
约束处理	惩罚 + 微分同胚 → 无约束	拉格朗日乘子 → 约束保持
每次迭代	\(O(M)\) 函数+梯度	\(O(M^2 \sim M^3)\) Hessian + KKT
中间可行性	不保证	可保证
实时性	~1 ms（\(M=10\sim20\)）	~10-100 ms

反事实推理：如果用 IPOPT 会怎样？ IPOPT 的 SQP 方法需要构造和求解 KKT 系统，复杂度至少 \(O(M^2)\)。对 \(M=20\) 段的典型场景，IPOPT 约需 10-50 ms，而 L-BFGS 约 1 ms——差一个数量级。在 50 Hz 的规划频率下，每周期只有 20 ms 的时间预算，L-BFGS 轻松满足而 IPOPT 很紧张。但反过来，在机械臂 TrajOpt 中，约束是非凸的（碰撞距离），且对中间可行性要求高，此时 IPOPT 的约束处理能力更重要——这就是"同一类问题在不同领域选择不同方法"的原因（见后文思考题）。

D5.1.8 L-BFGS 在 MINCO 中的适配 ⭐⭐

GCOPTER 使用 L-BFGS（Limited-memory BFGS）作为唯一的优化算法。每次迭代的流程：

1. 评估 \(J(q,T)\)：MINCO 前向求解 + 代价评估 → \(O(M)\)
2. 评估 \(\nabla J(q,T)\)：隐式微分 → \(O(M)\)
3. L-BFGS 搜索方向：两步循环递归 → \(O(m_{\text{lbfgs}} \cdot n)\)，其中 \(m_{\text{lbfgs}} \approx 5\sim10\)，\(n = 4M-3\)
4. 线搜索：Armijo 回溯，从 \(\alpha=1\) 开始逐步减半

总计：\(O(M)\) 每次迭代，收敛通常需要 50-200 次迭代。

GCOPTER 捆绑了一个来自 LBFGS-Lite 的实现（约 300 行 C++），纯头文件、无依赖、模板化、预分配内存——与 MINCO 的零 malloc 设计哲学一致。

⚠️ §D5.1 常见陷阱

💡 编程陷阱：用 Eigen::SparseLU 求解 MINCO 的带状方程 - 错误做法：把 \(M(T)\) 组装成 Eigen::SparseMatrix 然后调用 SparseLU::compute()。 - 现象：analyzePattern 步骤在每次 L-BFGS 迭代中耗费 ~0.5 ms 的固定开销（分析稀疏模式），而实际数值分解只需 ~0.01 ms。200 次迭代下，固定开销累计 100 ms，比手写带状求解慢 100 倍。 - 根本原因：Eigen::SparseLU 是为一般稀疏矩阵设计的，不知道 MINCO 的带状结构在每次迭代中**模式不变**（只有数值随 \(T\) 变化）。通用求解器无法跳过模式分析步骤。 - 正确做法：手写块 Thomas 算法，利用固定的块三对角结构。GCOPTER 的 minco.hpp 就是这样做的。 - 自检方法：对比手写求解和 SparseLU 的结果——数值应一致，但手写版快 10-100 倍。

💡 概念误区：以为 MINCO 的决策变量减少意味着"信息丢失" - 新手想法："Mellinger 用 24M 个变量，MINCO 只用 4M-3 个——是不是扔掉了某些自由度？MINCO 能表示的轨迹集合是不是更小？" - 正确理解：没有丢失任何信息。Mellinger 的 24M 个变量中，\(\sim 20M\) 个被连续性约束和最优性条件**唯一确定**——它们不是"自由的"决策变量，而是被约束锁死的。MINCO 只是把这些被锁死的变量从决策空间移到了求解过程中（通过 \(M(T)c = b(q)\) 隐式确定）。两者表示的轨迹集合**完全相同**。 - 类比：这类似于将 \(Ax = b\) 的自由变量和基变量分离——自由变量数量减少不是因为扔掉了解，而是因为基变量被方程唯一确定。

🧠 思维陷阱：以为 diffeomorphic 参数化可以处理任意约束 - 新手想法："既然 GCOPTER 把凸走廊约束和时间正性约束都消除了，是不是任何约束都可以用这种方法消除？" - 现实：diffeomorphic 参数化要求映射是**双射**（一一对应）且**光滑**。对凸集到全空间的映射（如 \(\tanh\)），这天然满足。但对非凸约束（如障碍物之间的缝隙）、不等式约束组合（如 OR 逻辑）、离散约束（如必须经过某些路点），diffeomorphic 映射可能不存在或极难构造。这也是 GCOPTER 的动力学约束仍用惩罚函数（而非参数化）处理的原因——推力约束是**全局**的（轨迹上每个点都要满足），无法简单地映射到无约束空间。 - 正确做法：凸约束 → 参数化消除；全局/非凸约束 → 惩罚函数。两者配合使用。

练习

练习 D5.1.1（手推带状矩阵，⭐⭐）：对 \(s=2\)（minimum jerk），\(M=2\) 段，\(d_i=1\)（仅位置约束）的情况，手动写出 \(M(T)\) 的完整矩阵（\(8 \times 8\)）。验证其带宽为 4，且非奇异（对任意 \(T_1, T_2 > 0\)）。

练习 D5.1.2（决策变量计数，⭐）：对 \(s=3\)（minimum crack），\(M=10\) 段，\(d_i=1\) 的情况，分别计算 Mellinger、Richter 和 MINCO 的决策变量维度（三维空间）。MINCO 相比 Mellinger 减少了多少百分比？

练习 D5.1.3（隐式微分验证，⭐⭐⭐）：用有限差分法验证 MINCO 的隐式微分梯度。具体地：(1) 给定 \(q, T\)，用 \(M(T)c=b(q)\) 求解 \(c\)，计算 \(J(q,T)\)；(2) 扰动 \(q_i \to q_i + \epsilon e_k\)，重新求解并计算 \(J(q+\epsilon e_k, T)\)；(3) 有限差分 \(\partial J / \partial q_{ik} \approx (J(q+\epsilon e_k, T) - J(q, T)) / \epsilon\)；(4) 与隐式微分结果对比。两者的相对误差应在 \(10^{-6}\) 以内（\(\epsilon = 10^{-7}\)）。

---

§D5.2 GCOPTER 的 header-only C++ 设计 ⭐⭐

D5.2.1 项目结构与设计哲学

GitHub: ZJU-FAST-Lab/GCOPTER，~1.2k stars，MIT License。

gcopter/
├── include/gcopter/
│   ├── minco.hpp          ← MINCO 轨迹类 (~400 行)
│   ├── firi.hpp           ← FIRI 走廊生成 (~600 行)
│   ├── flatness.hpp       ← 微分平坦映射 (~200 行)
│   ├── sfc_gen.hpp        ← 安全走廊拼接 (~300 行)
│   ├── gcopter.hpp        ← 外层优化循环 (~500 行)
│   ├── lbfgs.hpp          ← L-BFGS 优化器 (~300 行)
│   ├── geo_utils.hpp      ← 几何工具 (~400 行)
│   └── trajectory.hpp     ← 轨迹评估/采样
├── src/global_planning.cpp ← 主节点
├── launch/ / config/ / CMakeLists.txt

设计哲学：每个头文件一个完整算法——MINCO 的全部实现在 minco.hpp 中，FIRI 的全部实现在 firi.hpp 中。零外部依赖（除 Eigen），零运行时内存分配。这种极致的内聚设计使得集成到任何项目只需 #include 一行代码。

D5.2.2 minco.hpp 核心数据流

minco.hpp 是 GCOPTER 的灵魂。其核心数据流为：

q, T ──→ 组装 M(T), b(q) ──→ PLU 分解 ──→ 回代求 c
                                  │
                                  ▼ (保存 PLU 因子)
∂J/∂c ──→ 转置回代求 λ ──→ 提取 ∂J/∂q ──→ 返回给 L-BFGS
                          ──→ 局部计算 ∂J/∂T ──→ 返回给 L-BFGS

三个核心方法：

1. setConditions()：组装并求解 \(M(T)c = b(q)\)——组装带状矩阵、PLU 分解、回代求系数。这是前向过程。
2. getGradInnerP()：计算 \(\partial J / \partial q\)——解伴随方程 \(M^T \lambda = (\partial J / \partial c)^T\)，提取对应行。这是反向过程的第一部分。
3. getGradT()：计算 \(\partial J / \partial T\)——利用 \(\lambda\) 和 \(\partial F_i / \partial T_i\) 做局部计算。这是反向过程的第二部分。

D5.2.3 gcopter.hpp 外层优化循环

gcopter.hpp 将 MINCO 的前向/反向与 L-BFGS 连接：

// 伪代码
double optimize() {
    // 1. 初始化无约束变量
    //    q̃ = inverse_map(q₀)
    //    τ = log(T₀)
    Eigen::VectorXd x(dim);  // x = [q̃; τ]

    // 2. 调用 L-BFGS
    lbfgs::optimize(x, f_val, [&](const VectorXd& x, VectorXd& grad) {
        // 2a. 解包: x → q̃, τ
        // 2b. 映射: q = map(q̃), T = exp(τ)
        // 2c. MINCO 前向: setConditions(D₀, Dₘ, q, T) → c
        // 2d. 评估代价: J = J_obs(c) + J_dyn(c,T) + ρ(T)
        // 2e. MINCO 反向: getGradInnerP(), getGradT()
        // 2f. 链式法则: ∂J/∂q̃ = ∂J/∂q · ∂q/∂q̃
        //              ∂J/∂τ = ∂J/∂T · T (指数映射的链式)
        // 2g. 打包: grad = [∂J/∂q̃; ∂J/∂τ]
        return J;
    }, lbfgs_params);
}

D5.2.4 flatness.hpp 微分平坦映射

flatness.hpp 实现了 §D5.1.2 推导的平坦映射。核心函数 forward() 接受位置及其 0-4 阶导数和偏航信息，输出姿态 \(R\)、角速度 \(\omega\)、推力 \(f\)、力矩 \(\tau\)：

// flatness.hpp 的核心前向映射（简化）
void forward(const Eigen::Vector3d& pos,     // σ⁽⁰⁾ 位置
             const Eigen::Vector3d& vel,     // σ⁽¹⁾ 速度
             const Eigen::Vector3d& acc,     // σ⁽²⁾ 加速度
             const Eigen::Vector3d& jer,     // σ⁽³⁾ jerk
             const Eigen::Vector3d& sna,     // σ⁽⁴⁾ snap
             double yaw, double yaw_dot,
             // 输出:
             Eigen::Matrix3d& R,             // 姿态
             Eigen::Vector3d& omega,         // 角速度
             double& thrust,                 // 总推力
             Eigen::Vector3d& torque)        // 体力矩
{
    // 步骤 1: 推力向量 t = m(g·e₃ - acc)
    Eigen::Vector3d t_vec = mass * (gravity * e3 - acc);
    thrust = t_vec.norm();
    Eigen::Vector3d z_B = t_vec / thrust;

    // 步骤 2: 从 yaw 和 z_B 构造 R
    Eigen::Vector3d x_C(cos(yaw), sin(yaw), 0);
    Eigen::Vector3d y_B = z_B.cross(x_C).normalized();
    Eigen::Vector3d x_B = y_B.cross(z_B);
    R << x_B, y_B, z_B;   // 列拼接

    // 步骤 3: 角速度 (需要 jerk)
    Eigen::Vector3d t_dot = -mass * jer;
    omega.x() = -t_dot.dot(y_B) / thrust;
    omega.y() =  t_dot.dot(x_B) / thrust;
    omega.z() =  yaw_dot * z_B.dot(e3);

    // 步骤 4: 力矩 (需要 snap + 角速度导数)
    // ... (省略：涉及 snap 的复杂表达式)
    torque = J * omega_dot + omega.cross(J * omega);
}

反向函数 backward() 实现反向微分——给定动力学约束的梯度（如 \(\partial J / \partial f\)），反向传播到位置导数的梯度。这是计算动力学可行性惩罚梯度的关键——MINCO 需要知道"推力超限"这个事实应该如何修改轨迹的位置导数。

前向-反向的配合流程：

前向: pos, vel, acc, jer, sna ──→ forward() ──→ f, R, ω, τ
                                                   │
检查约束:                                          ▼
  f > f_max? → 计算 ∂J/∂f                    ∂J/∂f, ∂J/∂ω
  ‖ω‖ > ω_max? → 计算 ∂J/∂ω                       │
                                                   ▼
反向: ∂J/∂f, ∂J/∂ω ──→ backward() ──→ ∂J/∂acc, ∂J/∂jer, ∂J/∂sna
                                                   │
                                                   ▼
                                    累积到 ∂J/∂c（系数梯度）

D5.2.5 代价函数的详细分解

GCOPTER 的总代价函数由以下几项组成，每一项都值得深入理解：

1. 控制量代价（隐式包含）

\[J_{\text{ctrl}} = \int_0^T \|z^{(s)}(t)\|_W^2 \, dt\]

MINCO 轨迹**默认已最小化**此代价（给定 \(q\) 和 \(T\)）。当 GCOPTER 修改 \(q\) 和 \(T\) 时，MINCO 自动找到新的最优系数。因此 \(J_{\text{ctrl}}\) 不需要显式出现在优化目标中——它被 MINCO 的构造隐式处理了。

读到这里你可能会问："既然 MINCO 已经最小化了控制量，那 GCOPTER 优化的是什么？"答案是：GCOPTER 优化的是**在满足安全和动力学约束的前提下**的最优路点和时间分配。MINCO 保证了给定路点和时间后系数最优，GCOPTER 保证了路点和时间本身最优——两层优化嵌套。

2. 时间正则化

\[\rho(T) = k_\rho \cdot \sum_i T_i\]

线性形式鼓励轨迹尽量快完成。\(k_\rho\) 控制"速度-平滑度"的折中——\(k_\rho\) 越大，轨迹越快但控制量越大（更"激进"）。

为什么需要时间正则化？如果只最小化控制量 \(\int \|z^{(s)}\|^2 dt\)，最优解是 \(T_i \to \infty\)（无限慢地移动，控制量为零）。\(\rho(T)\) 防止时间爆炸。

3. 障碍物惩罚

\[J_{\text{obs}} = \sum_i \int_0^{T_i} \mu(p_i(t)) \, dt\]

其中 \(\mu(p)\) 是光滑距离惩罚函数。在使用安全走廊时，如果路点被约束在走廊交集内，障碍物惩罚可以作为额外保险——防止轨迹的中间部分（路点之间的曲线）伸出走廊。

4. 动力学可行性惩罚

\[J_{\text{dyn}} = \sum_i \int_0^{T_i} \left[ w_v \cdot P(\|v\|, v_{\max}) + w_a \cdot P(\|a\|, a_{\max}) + w_f \cdot P(f, f_{\max}) + w_f \cdot P(-f, -f_{\min}) \right] dt\]

其中 \(P(x, x_{\max}) = \max(0, x - x_{\max})^3\) 是三次惩罚。推力 \(f\) 和角速度 \(\omega\) 通过 flatness.hpp 的 forward() 从轨迹导数中获得。

为什么用三次惩罚而不是二次？

	二次惩罚 \(\max(0,\cdot)^2\)	三次惩罚 \(\max(0,\cdot)^3\)
在约束边界处	梯度 \(C^0\) 连续（有拐点）	梯度 \(C^1\) 连续（光滑）
对 L-BFGS 的影响	曲率估计在拐点处不准	曲率估计光滑，收敛更稳
惩罚强度	约束微小违反时惩罚较强	约束微小违反时惩罚较弱

三次惩罚的"约束微小违反时惩罚较弱"看似缺点，但实际上是优点——它允许 L-BFGS 在早期迭代中"穿过"约束边界探索更好的解，然后在后期迭代中被惩罚推回可行域。这比二次惩罚的"硬边界"更有利于全局探索。

5. 群飞碰撞惩罚（EGO-Planner-v2）

\[J_{\text{swarm}} = \sum_{j \ne \text{self}} \sum_i \int_0^{T_i} w_s \cdot P(d_{\text{safe}} - \|p_{\text{self}}(t) - p_j(t)\|, 0) \, dt\]

这个惩罚鼓励保持安全间距。\(p_j(t)\) 是第 \(j\) 架无人机广播的轨迹，通过 MINCO 的紧凑表示（路点+时间）完整重建。

6. 总代价汇总

\[J_{\text{total}} = \rho(T) + J_{\text{obs}} + J_{\text{dyn}} + J_{\text{swarm}}\]

所有项都是 \(c\)（因而是 \(q\) 和 \(T\)）的光滑函数，可以通过隐式微分计算梯度。

系统性分类：GCOPTER 的代价项按四维度穷举

维度	代价项	作用
时间	\(\rho(T)\)	防止时间爆炸，鼓励快速
安全	\(J_{\text{obs}}\)、走廊约束	避障
动力学	\(J_{\text{dyn}}\)	速度/加速度/推力可行
协调	\(J_{\text{swarm}}\)	多机避碰

这四个维度覆盖了四旋翼轨迹优化的全部关切。缺少任何一个都会导致问题——没有时间正则化则轨迹无限慢，没有安全项则撞墙，没有动力学项则推力超限，没有协调项则群飞碰撞。

⚠️ §D5.2 常见陷阱

💡 编程陷阱：在 L-BFGS 的代价函数 lambda 中调用 new/malloc - 错误做法：在每次 L-BFGS 迭代的代价评估中创建临时 Eigen::MatrixXd（动态大小矩阵），触发堆分配。 - 现象：200 次迭代 \(\times\) 每次 10+ 次堆分配 = 2000+ 次 malloc/free。在实时系统中，malloc 的延迟不确定性可达数十微秒，导致规划时间方差很大。 - 正确做法：所有中间矩阵用编译期固定大小 Eigen::Matrix<double, 2*S, 2*S> 或预分配的动态矩阵（在 lambda 外部分配，lambda 内部只做原地覆写）。

💡 概念误区：以为 getGradT() 和 getGradInnerP() 可以独立调用 - 错误做法：不调用 setConditions() 就直接调用 getGradInnerP()。 - 现象：梯度值是上一次 setConditions() 的残留数据，完全错误但不报错。 - 根本原因：getGradInnerP() 依赖 setConditions() 计算并存储的 PLU 因子和系数 \(c\)。调用顺序必须是 setConditions() → getGradInnerP() / getGradT()。

练习

练习 D5.2.1（源码精读，⭐⭐）：精读 minco.hpp 全部（约 400 行），标注：(1) 带状矩阵组装代码的起止行号；(2) PLU 分解的起止行号；(3) 伴随方程求解的起止行号。画出数据流图。

练习 D5.2.2（代价函数拆解，⭐⭐）：精读 gcopter.hpp 的 optimize() 函数，列出所有代价项（\(J_{\text{obs}}\)、\(J_{\text{dyn}}\)、\(\rho(T)\) 等）的计算位置和权重参数。修改时间正则化权重 \(k_\rho\)，观察轨迹速度如何变化。

---

§D5.3 安全飞行走廊（SFC）三代演进 ⭐⭐⭐

D5.3.1 动机：为什么需要安全飞行走廊

不用走廊的替代方案及其问题

直接在 ESDF（Euclidean Signed Distance Field）上做碰撞避让是最直观的方法——查询轨迹点的距离值，距离小于安全阈值就加惩罚。但这个方法有三个致命问题：

问题	具体表现	后果
ESDF 非凸	自由空间的 ESDF 有多个局部极值	优化器可能陷入"绕远路"的局部最优
ESDF 精度有限	离散网格的三线性插值	角点处距离值不精确，可能低估碰撞风险
ESDF 梯度不稳	在 Voronoi 面（等距面）梯度不连续	L-BFGS 的曲率估计在梯度不连续处失效

SFC 的作用是将碰撞避让从"连续的距离场查询"转化为"离散的凸多面体包含约束"。每段轨迹被约束在一个凸多面体内——这个约束是**凸的**（不会陷入局部最优）、精确的（不依赖网格插值）、光滑的（用 diffeomorphic 参数化消除后梯度连续）。

本质洞察：SFC 的核心价值不是"避障"——ESDF 也能避障。SFC 的核心价值是**把非凸的碰撞避让问题转化为凸约束**，使得 L-BFGS 几乎每次都能找到全局最优。这是 GCOPTER 能用纯局部优化算法（L-BFGS）获得高质量解的关键前提。

SFC 的形式定义

安全飞行走廊是一系列有序的凸多面体 \(\{P_1, P_2, \ldots, P_K\}\)，满足：

1. 无障碍：每个 \(P_i\) 完全在自由空间内
2. 连通：相邻多面体 \(P_i \cap P_{i+1} \ne \emptyset\)（有非空交集）
3. 覆盖：走廊覆盖从起点到终点的可行路径

每个凸多面体用**半空间交集**表示：\(P_i = \{x \in \mathbb{R}^3 : A_i x \le b_i\}\)。

SFC 在 MINCO 中的角色

在 MINCO 框架中，SFC 提供了路点约束：中间路点 \(q_i\) 必须在相邻走廊的交集内 \(q_i \in P_i \cap P_{i+1}\)。GCOPTER 通过 diffeomorphic 参数化将这个约束消除为无约束变量。

D5.3.2 第一代：DecompUtil（Liu 等 RA-L 2017） ⭐⭐

GitHub: sikang/DecompUtil，~273 stars，header-only C++。

算法：椭球膨胀法

从种子线段出发，迭代地将椭球膨胀到与障碍物接触，在接触点处创建切平面将障碍物切出。所有切平面围成的半空间交集就是凸多面体。

步骤 1: 初始椭球        步骤 2: 找障碍物         步骤 3: 切平面
   ╔════════╗           ╔════════╗              ╔════════╗
   ║        ║           ║   ×obs ║              ║  |     ║
   ║   E₀   ║    →      ║   E₀   ║     →       ║  |cut  ║
   ║        ║           ║        ║              ║  |     ║
   ╚════════╝           ╚════════╝              ╚════════╝

步骤 4: 重新膨胀        步骤 5: 收敛
   ╔═══╗                 ╔═══╗
   ║   ║                 ║   ║
   ║ E'║                 ║ P ║ (凸多面体)
   ║   ║                 ║   ║
   ╚═══╝                 ╚═══╝

DecompUtil 的三个问题

问题	后果
不保证种子包含	生成的凸多面体可能不包含原始种子点 → 后续优化可能找不到可行路径
体积不最大	椭球膨胀不追求最大体积 → 可能生成不必要地小的走廊 → 轨迹优化的可行域被过度压缩
参数敏感	初始椭球的形状影响最终结果 → 不同参数可能给出差异很大的走廊

D5.3.3 IRIS——SDP 方法的奠基者（Deits & Tedrake 2014）

在讨论 FIRI 之前，必须理解它改进的对象——IRIS（Iterative Region Inflation by Semidefinite programming）。

IRIS 的核心思想

IRIS 交替执行两个步骤：(1) 找分离超平面将当前椭球与每个障碍物分开，围成凸多面体 \(P\)；(2) 在 \(P\) 内找最大体积内切椭球（MVIE），用更大的椭球启动下一轮。

MVIE 的求解被建模为半正定规划（SDP）：

\[\max \log\det(C) \quad \text{s.t.} \quad \|Ca_j\| + a_j^T d \le b_j, \ \forall j; \quad C \succ 0\]

其中 \(E = \{Cx + d : \|x\| \le 1\}\) 是椭球的参数化，\(C \succ 0\) 是正半定约束。

IRIS 的问题：SDP 约束 \(C \succ 0\) 是计算瓶颈——内点法求解 SDP 在三维中约需 \(O(n_h^3 \cdot 3^3)\) 操作，典型耗时 ~10 ms/次。更严重的是，IRIS 不保证种子包含——收敛后的凸多面体可能不包含初始种子点。

D5.3.4 第二代：FIRI（Wang Q. 等 TRO 2025） ⭐⭐⭐

论文：Wang, Q. et al., "Fast Iterative Region Inflation for Computing Large 2-D/3-D Convex Regions of Obstacle-Free Space", IEEE T-RO, 2025.

位置：GCOPTER 内部 gcopter/include/gcopter/firi.hpp。

FIRI 在两个维度超越 IRIS：

改进 1：SOCP 替代 SDP

FIRI 的关键数学洞察：将 \(C\) 做 Cholesky 分解 \(C = LL^T\) 后，\(C \succ 0\) 等价于 \(L_{ii} > 0\)（对角元素为正），这是简单的不等式约束而非 SDP 约束。MVIE 问题变为 SOCP：

\[\max \sum_i \log(L_{ii}) \quad \text{s.t.} \quad \|L^T a_j\| + a_j^T d \le b_j \quad \text{(二阶锥约束)}\]

	SDP（IRIS）	SOCP（FIRI）
约束类型	正半定锥	二阶锥
内点法每步	\(O(n^3 \cdot \text{dim}^3)\)	\(O(n \cdot \text{dim}^2)\)
求解器	MOSEK（商业）	自研 SDQP（零依赖）
3D 典型耗时	~10 ms	~0.1 ms
加速比	1x	~100x

改进 2：Restrictive Inflation——保证种子包含

FIRI 引入了 Restrictive Inflation（RsI）子模块：在生成分离超平面时**显式约束种子集合在多面体内**。具体地，每个超平面 \((a, b)\) 不仅要将椭球与障碍物分离，还要满足 \(a^T q \le b\) 对所有种子 \(q \in Q\)。这个联合约束是一个严格凸 QP，FIRI 使用推广的 Seidel 线性规划算法（SDQP） 在期望线性时间内求解。

FIRI 的完整算法

输入: 障碍物集合 O, 种子集合 Q, 初始椭球 E₀, 精度 ρ

repeat:
  1. [Restrictive Inflation] 对每个与 E 相交的障碍物，
     解 RsI QP → 分离超平面 hᵢ（保证 Q ⊂ hᵢ）
     P_k = ∩ᵢ hᵢ

  2. [MVIE via SOCP] 在 P_k 内求最大体积内切椭球 E_k

until vol(E_k) ≤ (1 + ρ) · vol(E_{k-1})

收敛保证：可行性（\(Q \subseteq P_k\)）、单调性（\(\text{vol}(E_k) \ge \text{vol}(E_{k-1})\)）、收敛（体积有上界 + 单调递增）均有严格证明。

D5.3.5 IRIS vs FIRI 完整对比

特性	IRIS（Deits 2014）	FIRI（Wang Q. 2025）
MVIE 求解	SDP（半正定规划）	SOCP（二阶锥规划）
求解器	MOSEK/SeDuMi（外部）	SDQP（自研，期望线性）
种子包含	不保证	保证（Restrictive Inflation）
3D 单次耗时	~10 ms	~0.1 ms（100x 加速）
2D MVIE	\(O(n^3)\)	\(O(n)\) 解析（首次线性时间）
依赖	MOSEK（商业）	零依赖（header-only）
代码量	数千行 + 外部库	~600 行（firi.hpp）
适用场景	离线规划	实时在线规划

D5.3.6 第三代：CIRI（Ren 等 Science Robotics 2025）

CIRI（Configuration-space Iterative Region Inflation）将 FIRI 从工作空间扩展到构型空间。

传统方法的缺陷：传统 SFC 在工作空间生成走廊后，对走廊做 Minkowski 收缩 \(P' = P \ominus B(r)\)（\(r\) 为机器人半径）。这是**启发式的**——对非球形机器人不准确，收缩后可能不连通，体积过度缩小。

CIRI 的方法：先对每个障碍物做 Minkowski 膨胀 \(O' = O \oplus B(r)\)，然后在膨胀后的障碍物集合上运行 FIRI。生成的走廊对**机器人质心**是安全的——质心在走廊内等价于机体在原始自由空间内。

对比性思维："膨胀障碍物" vs "收缩走廊"。两者数学上等价（\(P \ominus B(r)\) 的质心安全 \(\iff\) \(P\) 对 \(O \oplus B(r)\) 安全），但工程效果截然不同。收缩走廊是后处理，可能破坏走廊的连通性和体积；膨胀障碍物是预处理，FIRI 在膨胀后的环境中生成的走廊天然具有正确的安全裕度。这是"在正确的地方做正确的事"的典型案例。

⚠️ §D5.3 常见陷阱

💡 编程陷阱：走廊交集为空但不检查 - 错误做法：直接用相邻走廊的半空间并集作为路点约束，不检查交集是否非空。 - 现象：MINCO 优化器尝试将路点放在空集中，diffeomorphic 映射返回 NaN，L-BFGS 发散。 - 根本原因：在障碍物密集区域，相邻走廊的交集可能退化为很小的区域甚至为空。 - 正确做法：在走廊生成后检查每对相邻走廊的交集体积。如果交集过小（体积 \(< \epsilon\)），插入额外的过渡走廊或调整走廊的生成种子。

🧠 思维陷阱：以为走廊越大越好 - 新手想法："走廊越大，轨迹优化的可行域越大，解一定越好。" - 现实：过大的走廊可能包含不必要的区域，导致路点在远离最优路径的地方"游荡"。更重要的是，过大的走廊需要更多半空间约束来描述，增加 firi.hpp 的计算时间和 MINCO 约束参数化的复杂度。 - 正确做法：走廊大小应匹配轨迹优化的需求——覆盖最优路径附近的合理区域即可，不必追求最大体积。

练习

练习 D5.3.1（SFC 可视化，⭐）：用 DecompUtil 在一个简单 3D 占据网格中生成安全走廊，在 RViz 中可视化凸多面体序列。增加障碍密度，观察走廊体积和面数的变化。

练习 D5.3.2（IRIS vs FIRI 耗时对比，⭐⭐）：在相同的 3D 环境中，分别用 IRIS（Drake 中的实现）和 FIRI（firi.hpp）生成走廊。对比：(1) 单次膨胀耗时；(2) 走廊体积；(3) 种子是否在走廊内。

---

§D5.4 SUPER——双轨迹安全范式 ⭐⭐⭐

D5.4.1 动机：单轨迹规划的安全盲区

论文：Ren, Zhu, Lu 等，"Safety-assured High-speed Navigation for MAVs", Science Robotics 10(98):eado6187, 2025.

GitHub: hku-mars/SUPER，~800 stars。

传统的单轨迹规划有一个致命假设：每个规划周期都能成功生成可行轨迹。在低速飞行中，这个假设通常成立——传感器有足够时间扫描环境，规划器有充裕的搜索空间。但在高速飞行中（\(>10\) m/s），这个假设频繁被打破：

1. 传感器来不及扫描前方空间——新障碍物"突然出现"
2. 规划器在有限时间内找不到可行解——搜索空间被新障碍物切割
3. 没有后备方案——如果重新规划失败，无人机正在高速飞向未知空间

反事实推理：如果单轨迹规划失败会怎样？ 答案取决于飞控的 fallback 策略。最常见的是"紧急刹车"——全推力反向制动。但在高速下，制动距离可能超过传感器的探测距离（"刹不住"），或者制动方向本身有障碍物。SUPER 的洞察是：安全后备必须在规划成功时提前准备好，而不是在规划失败时才去寻找。

D5.4.2 双轨迹范式的详细设计

SUPER 每个规划周期生成**两条**轨迹：

Primary 轨迹：追求速度和进度。它可以伸入**未知空间**——传感器尚未覆盖的区域。理由是：如果未知空间实际可通行，就赚了速度；如果不可通行，有 backup 兜底。

Backup 轨迹：追求安全。它**严格在已知自由空间内**，末端条件为悬停（\(v=0, a=0\)）。走廊使用 CIRI 生成构型空间走廊。

每个规划周期的详细步骤

周期 k:
┌──────────────────────────────────────────────────────────┐
│ 1. 生成 Primary 轨迹                                     │
│    - 允许伸入未知空间争取速度                              │
│    - 用 MINCO + L-BFGS 优化                              │
│                                                          │
│ 2. 生成 Backup 轨迹                                      │
│    - 严格在已知自由空间内                                  │
│    - 末端悬停 (v=0, a=0)                                 │
│    - 走廊: CIRI 生成                                      │
│                                                          │
│ 3. 提交 Backup                                           │
│    - 标记为"已提交"                                       │
│    - 如果下一周期 primary 失败 → 自动执行已提交 backup     │
│                                                          │
│ 4. 执行 Primary 的前 Δt 段                               │
└──────────────────────────────────────────────────────────┘

周期 k+1:
  primary(k) 后续仍安全 → 继续, 规划新 primary(k+1) + backup(k+1)
  primary(k) 不安全      → 执行 backup(k)! 安全减速到悬停

D5.4.3 递归可行性保证

SUPER 的安全保证是**递归的**——通过数学归纳法证明"任意时刻都有安全后备"：

不变量：在任意时刻 \(t\)，至少有一条已提交的 backup 轨迹 \(b(t)\)，满足：(1) \(b(t)\) 完全在已知自由空间 \(F(t)\) 内；(2) \(b(t)\) 的末端是悬停；(3) \(b(t)\) 对当前状态动力学可行。

归纳法：

• 基步：\(t=0\) 时，无人机静止 → 悬停本身就是 backup
• 归纳：在周期 \(k\)，backup\((k)\) 满足不变量。在周期 \(k+1\)：
• 情况 A：新 primary 安全 → 生成新 backup\((k+1)\)（满足不变量）
• 情况 B：新 primary 失败 → 执行 backup\((k)\) → 安全悬停

本质洞察：SUPER 将安全保证从"依赖重规划成功"变为"总有一条后备可用"。这是一个**范式级别**的提升——单轨迹方法的安全性取决于"最坏情况下规划器能否成功"，而 SUPER 的安全性取决于"上一次成功时准备的后备是否可靠"。前者随环境复杂度急剧恶化，后者是结构性的保证。

D5.4.4 SUPER 的硬件与性能

指标	数值
最高速度	20 m/s（密林环境）
机架尺寸	280 mm
推重比	\(> 5.0\)
传感器	Livox Avia LiDAR
规划频率	~50 Hz
失败率	比单轨迹基线低 ~36 倍
夜间飞行	成功（LiDAR 不依赖光照）

D5.4.5 ROG-Map 配套地图

GitHub: hku-mars/ROG-Map，~530 stars。

ROG-Map（Robot-centric Occupancy Grid Map）是 SUPER 的配套地图系统。核心设计：机器人中心滑窗（固定大小内存）+ 零拷贝滑动（通过索引映射）+ 增量式障碍膨胀（只处理新观测）。

	传统全局地图	ROG-Map
内存	\(O(V_{\text{total}})\)	\(O(V_{\text{local}})\)（固定）
更新	\(O(V_{\text{total}})\)	\(O(V_{\text{new}})\)（仅新观测）

⚠️ §D5.4 常见陷阱

💡 概念误区：以为 backup 轨迹是"紧急制动" - 新手想法："backup 就是全推力反向刹车吧？" - 正确理解：backup 是一条完整的 MINCO 轨迹——经过走廊约束、动力学约束优化，末端平滑悬停。它不是简单的制动指令，而是一条"优雅地停下来"的最优轨迹。"紧急制动"没有碰撞保证（制动路径可能穿过障碍物），而 backup 严格在已知自由空间内。

🧠 思维陷阱：以为双轨迹方法的计算量是单轨迹的两倍 - 新手想法："每周期算两条轨迹，计算量翻倍，规划频率减半？" - 现实：backup 轨迹通常比 primary 短得多（从当前位置减速到悬停，典型 3-5 段），计算量只占 primary 的 10-20%。总规划时间增加不到 30%，但安全性提升 36 倍——这是极高的性价比。

练习

练习 D5.4.1（双轨迹仿真，⭐⭐）：编译 SUPER，在提供的仿真场景中运行。故意在 primary 路径前方插入障碍物，观察 backup 接管过程。记录从"发现障碍"到"backup 激活"的延迟。

练习 D5.4.2（安全性极限分析，⭐⭐⭐）：分析 SUPER 的双轨迹方案在什么情况下会失败。考虑：(1) 所有方向同时出现障碍（如洞穴坍塌）；(2) backup 的悬停点被动态障碍物入侵；(3) 传感器盲区恰好在 backup 方向。

---

§D5.5 EGO-Planner-v2——MINCO 的群飞应用 ⭐⭐

D5.5.1 系统架构

论文：Zhou, Wen, Wang 等, "Swarm of Micro Flying Robots in the Wild", Science Robotics, 2022.

GitHub: ZJU-FAST-Lab/EGO-Planner-v2，~630 stars。

EGO-Planner-v2 是 MINCO 在多机群飞中的工程验证。系统架构为：全局目标分配（匈牙利算法）→ 前端路径搜索（A*）→ MINCO 后端优化（\(J = J_{\text{smooth}} + J_{\text{obs}} + J_{\text{dyn}} + J_{\text{swarm}}\)）→ 轨迹广播（UDP）→ 飞控跟踪。

关键设计：去中心化碰撞避让

每架无人机每 \(\Delta t_{\text{plan}}\) 秒重新规划一次，规划完成后通过 **UDP 广播**自己的 MINCO 轨迹（广播内容 = 中间路点 \(q\)、段时间 \(T\)、边界条件——MINCO 的紧凑表示使得每次广播仅需 ~200 bytes）。

接收方将其他无人机的广播轨迹作为"移动障碍物"，通过群飞碰撞惩罚项 \(J_{\text{swarm}}\) 避让：

\[J_{\text{swarm}} = \sum_{j \ne \text{self}} \sum_i \int_0^{T_i} w_s \cdot \max(0, d_{\text{safe}} - \|p_{\text{self}}(t) - p_j(t)\|)^3 \, dt\]

多视角理解：为什么用 UDP 不用 TCP？

• 延迟视角：UDP 无握手/无重传，延迟最低。轨迹信息有时效性——过时的轨迹不如不发。
• 鲁棒性视角：每 \(\Delta t\) 就有新轨迹广播，丢一个不影响大局（下一个很快就来）。TCP 的拥塞控制在 10 机场景中可能导致延迟飙升。
• 通信量视角：10 机 \(\times\) 30 Hz \(\times\) 200 bytes = ~60 KB/s——远低于无线带宽上限。

D5.5.2 实验结果

林地群飞实验：10 架 280mm 四旋翼，密林（树木间距 1-3 m），速度 3-5 m/s，规划频率 ~30 Hz/架，无人机间最小距离 0.5 m，碰撞次数 0（在报告的实验中）。

⚠️ §D5.5 常见陷阱

💡 编程陷阱：群飞碰撞惩罚的时间对齐错误 - 错误做法：用自己的段时间索引去采样别人的轨迹，不做时间转换。 - 现象：自己飞到第 3 段时，采样别人的"第 3 段第 \(t\) 秒"——但两架无人机的段时间不同，实际物理时刻对不上。碰撞惩罚基于错误的相对位置计算，形同虚设。 - 正确做法：用**全局时间戳**对齐。将段索引 + 段内时间转换为全局时间，在全局时间上采样自己和对方的轨迹。

练习

练习 D5.5.1（广播量计算，⭐）：对 20 架无人机、50 Hz 规划频率、MINCO 参数 \(M=10, s=4, m=3\) 的场景，计算每秒的总广播量（bytes/s）。是否超过 802.11n WiFi 的典型吞吐量？

---

§D5.6 MINCO 的后续生态与前沿 ⭐⭐

D5.6.1 衍生项目汇总

项目	GitHub	场景	MINCO 的角色
EGO-Planner-v2	ZJU-FAST-Lab/	10 机林地群飞	轨迹 + 群飞碰撞代价
Swarm-Formation	ZJU-FAST-Lab/	图拉普拉斯编队	编队中每机的轨迹
Fast-Racing	ZJU-FAST-Lab/	SE(3) 竞速（门约束）	穿越赛道门
DDR-opt	ZJU-FAST-Lab/	地面差速机器人	非完整约束下的 MINCO
Implicit-SVSDF	ZJU-FAST-Lab/	任意形状机器人（SIGGRAPH 2024）	扫掠体 SDF + MINCO

D5.6.2 DDR-opt：MINCO 推广到差速机器人

DDR-opt 将 MINCO 从四旋翼（\(s=4\), 全向运动）推广到差速驱动机器人（非完整约束——只能前后移动，不能横移）。平坦输出变为 \(\sigma = [x, y, \theta]\)，积分链阶数降为 \(s=2\)（minimum jerk 对应方向）。

对比性思维：四旋翼 vs 差速机器人的 MINCO 差异。四旋翼是全向的——位置的任何方向都可以独立控制，三个维度解耦。差速机器人是非完整的——速度方向被锁定在航向方向，位置和航向耦合。这导致 MINCO 的约束结构更复杂，但带状矩阵的基本思想不变。

D5.6.3 MIGHTY——后 MINCO 时代的候选者

论文：Kondo, Wu, Kumar, How, "MIGHTY: Hermite Spline-based Efficient Trajectory Planning", RA-L, 2025.

MIGHTY 用 Hermite 样条**替代 MINCO 的分段多项式，关键改进是**严格局部控制——修改一个节点只影响相邻两段，而 MINCO 理论上影响所有段（虽然影响指数衰减）。代价是决策变量增加（多了每个节点的速度变量）。

	MINCO	MIGHTY
决策变量	\(q + T\)	\(q + v + T\)（多了速度）
局部支持	弱（指数衰减）	严格
最优性	控制量最小	非最小（需额外平滑项）
实验性能	基线	计算时间 \(-9.3\%\)，飞行时间 \(-13.1\%\)

D5.6.4 学习型轨迹生成

DroneDiffusion（2024）和 DDAT（2025）尝试用扩散模型生成四旋翼轨迹。当前局限：无数学保证（可能生成不可行轨迹）、随机性（需采样+筛选）、较慢（~10 ms GPU 推理 vs MINCO 的 ~0.1 ms）。目前学习方法**无法匹配 MINCO 的速度和安全保证**，但在初始猜测生成中有潜力。

D5.6.5 开放问题

开放问题	现状	研究方向
Warm-start	MINCO 的隐式求解使得从上次解更新困难	增量式带状求解
多体系统	无人机+机械臂接触时微分平坦坍塌	混合平坦/非平坦方法
长程规划	段数 \(>100\) 时弱局部性导致数值问题	MIGHTY 的严格局部方案
形式验证	惩罚函数是软约束，不提供硬保证	SOS 多项式验证

练习

练习 D5.6.1（生态项目选型，⭐⭐）：你的项目是在仓库中用差速底盘导航，需要避障+时间最优。应该选择 GCOPTER 还是 DDR-opt？列出选型依据。

---

§D5.7 MINCO vs B 样条——系统对比与选型 ⭐⭐

D5.7.1 核心差异总表

特性	B 样条	MINCO
决策变量	\(N\) 个控制点	\((M-1)\) 路点 + \(M\) 段时间
局部控制	严格（修改一点只影响 \(k+1\) 段）	弱（指数衰减）
时间优化	困难（均匀节点距，需 NUBS）	原生（每段独立 \(T_i\)）
凸包性质	有（轨迹在控制点凸包内）	无（通过走廊约束补偿）
最优性	非最优（近似）	控制量最小（精确）
连续性	\(C^{k-1}\)（均匀 \(k\) 阶）	\(C^{2s-2}\)（可变）
求解方法	QP/SQP（有约束）	L-BFGS（无约束）
计算复杂度	\(O(NK)\)	\(O(M)\)
实时性	~ms	~0.1 ms

D5.7.2 选型指南

选 B 样条, 如果:
  ✓ 需要严格的局部支持（如在线增量式规划）
  ✓ 需要凸包保证（如安全关键应用的形式验证）
  ✓ 控制点可以直接对应物理意义

选 MINCO, 如果:
  ✓ 需要最优的时空联合规划
  ✓ 需要极致的计算速度
  ✓ 决策变量维度敏感（群飞等多机场景）
  ✓ 可以接受"事实局部性"代替严格局部控制

本质洞察：B 样条和 MINCO 的选择不是"好 vs 坏"，而是"不同约束/需求下的最优选择"。B 样条用**更多的决策变量**换取**严格的局部控制**和**凸包保证**；MINCO 用**更少的决策变量**和**最优性条件**换取**极致的计算速度**。在四旋翼这个特定领域，MINCO 的优势（时间优化原生 + \(O(M)\) 计算）压倒了 B 样条的优势（局部控制 + 凸包），所以 MINCO 成为了主流。但在其他领域（如需要增量式更新的在线规划），B 样条仍有不可替代的价值。

练习

练习 D5.7.1（定量对比，⭐⭐）：对同一个 3D 环境（10 个路点），分别用 EGO-Planner（B 样条后端）和 GCOPTER（MINCO 后端）规划轨迹。对比：(1) 规划时间；(2) 轨迹总时间；(3) snap 积分值；(4) 最大推力。

---

本章常见误解汇总

误解	正确理解
"MINCO 是一种新的多项式基"	MINCO 不是新的基函数，而是新的**参数化和求解框架**——用路点+时间作为决策变量，系数通过带状方程隐式确定
"MINCO 比 B 样条严格更好"	MINCO 在计算速度和最优性上更好，但在局部控制性和凸包保证上不如 B 样条——选择取决于应用需求
"安全走廊保证了碰撞自由"	安全走廊保证**路点**在走廊内，但 MINCO 轨迹不一定完全在走廊内——需要通过 diffeomorphic 参数化或惩罚函数保证
"FIRI 是 IRIS 的简单加速"	FIRI 不仅更快（SOCP vs SDP），更重要的是保证了**种子包含**——这是一个质的改进，解决了 IRIS 可能生成不包含种子的走廊的根本问题
"SUPER 的计算量是单轨迹的两倍"	backup 轨迹通常很短（3-5 段），计算量只增加 10-20%，安全性提升 36 倍
"L-BFGS 是因为 GCOPTER 作者喜欢无约束方法"	L-BFGS 是由**数学结构决定的最优选择**——MINCO 的 diffeomorphic 参数化使问题成为无约束，而 \(O(M)\) 的函数/梯度评估使 L-BFGS 的每次迭代代价最低

---

本章小结

术语速查表

术语	英文	一句话定义
MINCO	Minimum Control	给定路点+时间，使控制量积分最小的分段多项式类
微分平坦	Differential Flatness	系统的全部状态/输入可由平坦输出及其导数唯一确定
带状矩阵	Banded Matrix	非零元素集中在主对角线附近的稀疏矩阵
隐式微分	Implicit Differentiation	利用约束方程 \(M c = b\) 的结构计算梯度，避免显式构造 Jacobian
Diffeomorphic 参数化	Diffeomorphic Parameterization	通过光滑双射将约束空间映射到无约束空间
SFC	Safe Flight Corridor	一系列有序凸多面体，覆盖从起点到终点的可行路径
MVIE	Maximum Volume Inscribed Ellipsoid	凸多面体内的最大体积椭球
FIRI	Fast Iterative Region Inflation	SOCP 求 MVIE + 种子包含保证的走廊生成方法
CIRI	Configuration-space IRI	FIRI 在构型空间（膨胀障碍物后）的版本
RsI	Restrictive Inflation	FIRI 的子模块，在生成超平面时保证种子包含
SDQP	Seidel's Dimensionality-reducing QP	FIRI 的期望线性时间 QP 求解器

知识点总表

编号	知识点	核心要点	对应节	难度
1	MINCO 定义	路点+时间 → 带状方程 \(M(T)c=b(q)\) → 最优系数	§D5.1.3	⭐⭐⭐
2	微分平坦	平坦输出 \(\sigma\) 的 4 阶导数决定全部状态/输入	§D5.1.2	⭐⭐
3	带状 PLU	\(O(M)\) 分解和求解，零 fill-in	§D5.1.5	⭐⭐
4	隐式微分	伴随变量 \(\lambda\) + 转置回代 → \(O(M)\) 梯度	§D5.1.6	⭐⭐⭐
5	Diffeomorphic 参数化	\(\tanh\) 消路点约束，\(\exp\) 消时间约束	§D5.1.7	⭐⭐
6	GCOPTER 源码	minco.hpp / gcopter.hpp / flatness.hpp 的数据流	§D5.2	⭐⭐
7	SFC 三代	DecompUtil → IRIS → FIRI（100x 加速+种子保证）	§D5.3	⭐⭐⭐
8	FIRI	SOCP 替代 SDP + Restrictive Inflation	§D5.3.4	⭐⭐⭐
9	SUPER 双轨迹	primary（追速度）+ backup（保安全），递归可行	§D5.4	⭐⭐⭐
10	MINCO vs B 样条	局部控制 vs 计算速度 vs 最优性的权衡	§D5.7	⭐⭐

---

累积项目：本章新增模块

D5 模块：MINCO 后端 + FIRI 走廊

在累积项目的无人机规划测试台中，本章替换 D3 的 Mellinger QP 后端为 MINCO + L-BFGS，替换手工走廊为 FIRI 自动生成。

D3 模块（旧）:
  前端 A* → 手工走廊 → Mellinger QP → 7 次多项式轨迹

D5 模块（新）:
  前端 A* → FIRI 走廊 → MINCO + L-BFGS → 最优分段多项式轨迹

对比指标:
  - 规划时间: Mellinger ~10 ms → MINCO ~1 ms (10x 加速)
  - 走廊生成: 手工 ~50 ms → FIRI ~0.5 ms (100x 加速)
  - 时间优化: Mellinger 无 → MINCO 原生
  - 段数扩展: Mellinger M>20 数值崩溃 → MINCO M>100 稳定

实现步骤： 1. 编译 GCOPTER，运行 global_planning 示例，验证 FIRI 走廊和 MINCO 轨迹 2. 修改目标点和障碍物，观察走廊和轨迹的自适应变化 3. 用蒙特卡洛方法对比 Mellinger 和 MINCO 在 50 段场景下的求解成功率 4. 测量 L-BFGS 的收敛曲线（迭代次数 vs 代价值）

---

延伸阅读

必读（按顺序）：

优先级	资料	说明	难度
1	Wang 2022 TRO (GCOPTER/MINCO)	核心理论	⭐⭐⭐
2	minco.hpp 源码	核心实现	⭐⭐
3	Wang Q. 2025 TRO (FIRI)	走廊生成	⭐⭐⭐
4	firi.hpp 源码	FIRI 实现	⭐⭐
5	Zhou 2022 Sci. Rob. (群飞)	MINCO 应用	⭐⭐

推荐（按兴趣）：

资料	说明	难度
Ren 2025 Sci. Rob. (SUPER)	安全范式	⭐⭐⭐
gcopter.hpp 源码	完整优化框架	⭐⭐
Mellinger 2011 ICRA	历史根源	⭐⭐
Richter 2016 ISRR	闭式 QP 过渡	⭐⭐
Kondo 2025 RA-L (MIGHTY)	后 MINCO 方向	⭐⭐⭐

---

本章与后续章节的关系

后续章节	关系	本章铺垫的知识点
D6 基于学习的轨迹生成	MINCO 作为学习方法的对比基线	MINCO 的速度和安全保证是学习方法需要匹配的目标
D7 SE(3) 规划	MINCO 扩展到 SE(3) 空间（位置+姿态联合优化）	微分平坦映射、走廊约束的推广
U2 鲁棒规划	MINCO 轨迹 + Tube/CBF 安全层	MINCO 提供名义轨迹，Tube MPC 在其外围加管道

---

故障排查手册

故障 1：L-BFGS 不收敛，代价值振荡

维度	内容
症状	L-BFGS 迭代 200 次后代价值仍在 \(10^{3}\) 量级振荡，不下降
可能原因 A	惩罚权重 \(w_v, w_a\) 过大，梯度被惩罚项主导，L-BFGS 的曲率估计不准
排查 A	打印各代价项（\(J_{\text{smooth}}, J_{\text{obs}}, J_{\text{dyn}}, \rho\)），确认哪项主导。若惩罚项 \(>\) 平滑项的 100 倍，降低权重
可能原因 B	初始路点在障碍物内，惩罚梯度指向走廊外
排查 B	可视化初始路点和走廊，确认路点在走廊交集内。若不在，修正初始化逻辑
可能原因 C	段时间初始值过小，导致速度/加速度违反约束极严重
排查 C	用路径长度除以最大速度估计段时间下界，确保初始 \(T_i\) 不低于此下界
相关章节	§D5.1.7（惩罚函数）、§D5.1.8（L-BFGS 适配）

故障 2：轨迹在走廊边界处"抖动"

维度	内容
症状	轨迹在某些走廊边界附近高频振荡
可能原因	diffeomorphic 映射的 \(\tanh\) 在饱和区梯度趋零，L-BFGS 在边界附近步长不稳
排查	检查路点 \(q_i\) 是否贴近走廊边界（\(\tanh(\tilde{q}_i)\) 接近 \(\pm 1\)）。若是，适当缩小走廊或增加内切椭球的缩放因子
相关章节	§D5.1.7（diffeomorphic 参数化）、§D5.3（走廊生成）

故障 3：FIRI 生成的走廊面数过多

维度	内容
症状	单个凸多面体有 50+ 个半空间，MINCO 的约束参数化变慢
可能原因	环境中有大量细小障碍物，FIRI 为每个障碍物生成一个分离超平面
排查	增大障碍物的膨胀半径，将细小障碍物合并。或对多面体做后处理：移除冗余半空间（不影响多面体形状的半空间）
相关章节	§D5.3.4（FIRI 算法）

故障 4：群飞时两机"互锁"（都在等对方让路）

维度	内容
症状	两架无人机面对面飞行时，都大幅减速但不绕开，规划频率下降
可能原因	碰撞惩罚的对称性导致两机做对称的"让步"，但都不够大。加上 UDP 丢包导致两机看到的对方位置不一致
排查	(1) 增大安全距离 \(d_{\text{safe}}\)；(2) 引入对称打破机制（如 ID 小的机器人优先级高）；(3) 检查 UDP 丢包率
相关章节	§D5.5（EGO-Planner-v2 群飞）

故障 5：MINCO 求解的轨迹在中间段出现不物理的大加速度

维度	内容
症状	轨迹在某些段的加速度远超 \(a_{\max}\)，尽管惩罚权重已设置
可能原因	相邻走廊交集过小，路点被挤在很窄的区域，MINCO 为满足位置约束不得不生成急转弯
排查	可视化走廊交集体积。若交集过小，增加走廊密度（插入更多走廊）或用更大的 FIRI 膨胀
相关章节	§D5.1.3（MINCO 定义）、§D5.3（走廊生成）

---

API 速查表

MINCO 核心 API（minco.hpp）

方法	签名（简化）	功能	复杂度
setConditions	(headState, tailState, innerPts, timeAlloc)	组装 \(M(T)c=b(q)\) 并求解	\(O(M)\)
getGradInnerP	(gradInnerPs) → (M-1) × D 矩阵	计算 \(\partial J / \partial q\)	\(O(M)\)
getGradT	(gradT) → \(M \times 1\) 向量	计算 \(\partial J / \partial T\)	\(O(M)\)
getPos	(seg, t) → \(D\) 维向量	评估第 seg 段在 t 处的位置	\(O(s)\)
getVel / getAcc / getJer / getSnap	类似 getPos	评估各阶导数	\(O(s)\)

FIRI 核心 API（firi.hpp）

方法	签名（简化）	功能	复杂度
firi	(obstacles, seeds, E0, tol) → (A, b, E)	生成凸多面体 + 内切椭球	~\(O(n_{\text{obs}} \cdot n_{\text{iter}})\)
mvie3D	(A, b) → (C, d)	3D 最大体积内切椭球 (SOCP)	\(O(n_h)\)

L-BFGS 核心 API（lbfgs.hpp）

方法	签名（简化）	功能
lbfgs_optimize	(x, f, func_grad, params)	无约束优化，func_grad 同时返回代价值和梯度

---

研究实践建议

入门级（本科毕设/课程项目）： 1. 编译运行 GCOPTER 示例，在 RViz 中观察走廊和轨迹 2. 修改障碍物布局，感受 FIRI 走廊和 MINCO 轨迹的自适应性 3. 实现 2D 简化版 MINCO（minimum jerk, \(s=2\)），验证带状求解

中级（硕士研究/工程应用）： 1. 精读 minco.hpp 和 gcopter.hpp，理解完整数据流 2. 将 GCOPTER 集成到自己的规划框架（替换 QP 后端） 3. 在真实环境中测试 FIRI 走廊生成的鲁棒性

高级（博士研究/前沿探索）： 1. 研究 MINCO 的 warm-start 方法——如何利用上一次求解加速本次求解 2. 探索 MIGHTY 的严格局部控制在大规模场景（\(M>100\)）中的优势 3. 将 MINCO 与 CBF 安全滤波结合——MINCO 提供名义轨迹，CBF 在控制层提供实时安全保证

---

版本信息速查

软件/库	本章使用版本	关键依赖
GCOPTER	main branch (2022)	Eigen 3.3+, ROS Noetic
FIRI	集成在 GCOPTER 中	零外部依赖
DecompUtil	v1.0	Eigen 3.3+
SUPER	main branch (2025)	ROS1/ROS2, Livox driver
EGO-Planner-v2	main branch (2022)	ROS Noetic
ROG-Map	main branch (2025)	Eigen 3.3+

---

参考文献

1. D. Mellinger and V. Kumar, "Minimum snap trajectory generation and control for quadrotors," in Proc. IEEE ICRA, 2011, pp. 2520-2525.
2. C. Richter, A. Bry, and N. Roy, "Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments," in Proc. ISRR, Springer, 2016, pp. 649-666.
3. S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, "Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-D complex environments," IEEE Robot. Autom. Lett., vol. 2, no. 3, pp. 1688-1695, 2017.
4. R. Deits and R. Tedrake, "Computing large convex regions of obstacle-free space through semidefinite programming," in Proc. WAFR, Springer, 2014, pp. 109-124.
5. Z. Wang, X. Zhou, C. Xu, J. Chu, and F. Gao, "Alternating minimization based trajectory generation for quadrotor aggressive flight," IEEE Robot. Autom. Lett., vol. 5, no. 3, pp. 4836-4843, 2020.
6. Z. Wang, X. Zhou, C. Xu, and F. Gao, "Geometrically constrained trajectory optimization for multicopters," IEEE Trans. Robot., vol. 38, no. 5, pp. 3259-3278, 2022.
7. X. Zhou, J. Wen, Z. Wang, Y. Ma, H. Cheng, R. Duan, and F. Gao, "Swarm of micro flying robots in the wild," Sci. Robot., vol. 7, no. 66, eabm5954, 2022.
8. Q. Wang et al., "Fast iterative region inflation for computing large 2-D/3-D convex regions of obstacle-free space," IEEE Trans. Robot., 2025.
9. Y. Ren, Z. Zhu, H. Lu, Y. Cai, S. Yin, F. Kong, A. Lin, N. Chen, and F. Zhang, "Safety-assured high-speed navigation for MAVs," Sci. Robot., vol. 10, no. 98, eado6187, 2025.
10. K. Kondo, T. Wu, V. Kumar, and J. P. How, "MIGHTY: Hermite spline-based efficient trajectory planning," IEEE Robot. Autom. Lett., 2025.
11. L. Quan, Z. Wang, C. Xu, and F. Gao, "Distributed swarm trajectory optimization for formation flight in dense environments," in Proc. IEEE ICRA, 2022.
12. J. Wang et al., "Implicit swept volume SDF: Enabling continuous collision-free trajectory generation for arbitrary shapes," ACM Trans. Graph. (SIGGRAPH), vol. 43, no. 4, 2024.
13. F. Gao, W. Wu, Y. Lin, and S. Shen, "Online safe trajectory generation for quadrotors using fast Marching method and Bernstein basis polynomial," in Proc. IEEE ICRA, 2018, pp. 344-351.
14. F. Gao, X. Zhou, and S. Shen, "Teach-Repeat-Replan: A complete and robust system for aggressive flight in complex environments," IEEE Trans. Robot., vol. 36, no. 5, pp. 1557-1574, 2020.

---

附录 A：MINCO 带状矩阵的逐步手推示例 ⭐⭐

本附录的目的：§D5.1.4 给出了 \(M(T)c = b(q)\) 的一般结构，但对于初次接触带状矩阵的读者，抽象的块表达可能令人困惑。本附录用一个**最小完整示例**——\(s=2\)（minimum jerk）、\(M=3\) 段、一维空间——手把手地推导每一个矩阵元素，让你亲手验证"带状"结构从何而来。

A.1 问题设定

考虑一维空间中的 minimum jerk 轨迹（\(s=2\)，即最小化 jerk 的积分 \(\int \dddot{p}^2 \, dt\)）。每段多项式的阶数为 \(2s-1 = 3\)，即**三次多项式**。有 \(M=3\) 段，段时间分别为 \(T_1, T_2, T_3\)。

第 \(i\) 段轨迹写作：

\[p_i(\tau) = c_{i,0} + c_{i,1}\tau + c_{i,2}\tau^2 + c_{i,3}\tau^3, \quad \tau \in [0, T_i]\]

每段有 4 个系数，3 段共 12 个系数。这就是 Mellinger 方法的决策变量维度。

边界条件（给定的）： - 起点：\(p_1(0) = p_0\)，\(\dot{p}_1(0) = v_0\) - 终点：\(p_3(T_3) = p_f\)，\(\dot{p}_3(T_3) = v_f\)

中间路点（MINCO 的决策变量）： - \(q_1\)：第 1 段和第 2 段的连接点位置，\(p_1(T_1) = p_2(0) = q_1\) - \(q_2\)：第 2 段和第 3 段的连接点位置，\(p_2(T_2) = p_3(0) = q_2\)

连续性条件（由 MINCO 的最优性自动满足）： - 在 \(q_1\) 处：\(\dot{p}_1(T_1) = \dot{p}_2(0)\)（速度连续） - 在 \(q_2\) 处：\(\dot{p}_2(T_2) = \dot{p}_3(0)\)（速度连续）

为什么 \(s=2\) 时只需要速度连续？ 对于 minimum jerk（\(s=2\)），多项式阶数是 \(2s-1=3\)（三次）。每段有 4 个系数，但边界位置就消耗了 2 个条件（段首+段末位置），最优性条件自动确保了速度连续（\(C^1\) 连续）。更高阶的 \(C^{2s-2} = C^2\) 连续（加速度连续）由 minimum jerk 的最优性条件隐式保证。对比：\(s=4\)（minimum snap）时多项式是 7 次，每段 8 个系数，最优性条件保证到 \(C^6\) 连续。

A.2 逐段展开系数方程

第 1 段：段首由边界条件确定。

\[p_1(0) = c_{1,0} = p_0$$ $$\dot{p}_1(0) = c_{1,1} = v_0\]

段末位置是路点 \(q_1\)：

\[p_1(T_1) = c_{1,0} + c_{1,1}T_1 + c_{1,2}T_1^2 + c_{1,3}T_1^3 = q_1\]

这意味着只有 \(c_{1,2}\) 和 \(c_{1,3}\) 是"自由的"（\(c_{1,0}=p_0\), \(c_{1,1}=v_0\) 已确定）。段末位置约束给出一个方程，minimum jerk 的最优性条件给出另一个方程。两个方程确定两个未知数——\(c_{1,0}, c_{1,1}, c_{1,2}, c_{1,3}\) 全部由 \(p_0, v_0, q_1, T_1\) 唯一确定。

第 2 段：段首位置 \(p_2(0) = q_1\)，段末位置 \(p_2(T_2) = q_2\)，速度连续性 \(\dot{p}_1(T_1) = \dot{p}_2(0)\)。加上最优性条件，4 个方程确定 4 个系数。

第 3 段：段首位置 \(p_3(0) = q_2\)，段末位置 \(p_3(T_3) = p_f\)，段末速度 \(\dot{p}_3(T_3) = v_f\)，速度连续性 \(\dot{p}_2(T_2) = \dot{p}_3(0)\)。4 个方程确定 4 个系数。

A.3 带状结构的直觉

关键观察——第 2 段的系数只与 \(q_1, q_2, T_2\) 以及相邻段的连接信息有关。它不需要知道 \(p_0\)（起点，离它两段之远）的值。这就是带状结构的来源：第 \(i\) 段的系数只依赖第 \(i-1, i, i+1\) 段的信息——形成**块三对角**结构。

用矩阵语言写出来：

\[\begin{bmatrix} A_1 & B_1 & & \\ C_2 & A_2 & B_2 & \\ & C_3 & A_3 & \end{bmatrix} \begin{bmatrix} c_1 \\ c_2 \\ c_3 \end{bmatrix} = \begin{bmatrix} d_1(p_0, v_0, q_1) \\ d_2(q_1, q_2) \\ d_3(q_2, p_f, v_f) \end{bmatrix}\]

这里 \(A_i\) 是 \(4 \times 4\) 的对角块，\(B_i, C_i\) 是 \(4 \times 4\) 的上/下副对角块。整体矩阵是 \(12 \times 12\)，但带宽只有 4——只有对角线附近的元素非零。

对比性思维：稠密矩阵 vs 带状矩阵的求解代价

方法	矩阵结构	求解复杂度	\(M=3\)	\(M=100\)
Mellinger（直接 QP）	稠密 \((4M) \times (4M)\)	\(O(M^3)\)	\(12^3 = 1728\)	\(400^3 = 6.4 \times 10^7\)
MINCO（带状求解）	带状，带宽 4	\(O(M)\)	\(3 \times 4^2 = 48\)	\(100 \times 4^2 = 1600\)

从 \(M=3\) 到 \(M=100\)，Mellinger 的计算量增长了 \(\sim 37000\) 倍，MINCO 只增长了 \(\sim 33\) 倍。这就是带状结构的威力。

A.4 块 Thomas 算法的手推执行

块 Thomas 算法（块三对角系统的 LU 分解）分两步：前向消去**和**回代。

前向消去：从第 1 行开始，消去下三角元素。

\[\tilde{A}_1 = A_1$$ $$L_2 = C_2 \cdot \tilde{A}_1^{-1}$$ $$\tilde{A}_2 = A_2 - L_2 \cdot B_1$$ $$L_3 = C_3 \cdot \tilde{A}_2^{-1}$$ $$\tilde{A}_3 = A_3 - L_3 \cdot B_2\]

同时更新右端项：

\[\tilde{d}_1 = d_1$$ $$\tilde{d}_2 = d_2 - L_2 \cdot \tilde{d}_1$$ $$\tilde{d}_3 = d_3 - L_3 \cdot \tilde{d}_2\]

回代：从最后一行开始。

\[c_3 = \tilde{A}_3^{-1} \cdot \tilde{d}_3$$ $$c_2 = \tilde{A}_2^{-1} \cdot (\tilde{d}_2 - B_2 \cdot c_3)$$ $$c_1 = \tilde{A}_1^{-1} \cdot (\tilde{d}_1 - B_1 \cdot c_2)\]

每一步涉及 \(4 \times 4\) 矩阵的求逆和矩阵乘法——\(O(1)\) 的操作（因为块大小 \(2s\) 是常数）。总共 \(M=3\) 步前向消去 + \(M=3\) 步回代 = \(O(M)\)。

对比性思维：为什么不用 Cholesky 分解？ \(M(T)\) 不一定是对称正定的。MINCO 的连续性约束使得 \(M(T)\) 是**一般**的带状矩阵，不是对称的。因此不能用 Cholesky 分解（\(LL^T\)），必须用 PLU 分解（带行交换的 LU 分解）。GCOPTER 的实现确认了这一点——它使用的是带 pivot 的块 Thomas 算法。

A.5 自检练习

练习 A.1（矩阵元素填充，⭐⭐）：取 \(s=2, M=2, T_1=1, T_2=2, p_0=0, v_0=1, p_f=5, v_f=0, q_1=2\)。手动写出 \(M(T)\) 的完整 \(8 \times 8\) 矩阵和 \(b(q)\) 的 \(8 \times 1\) 右端向量。用 Python/MATLAB 求解 \(c = M^{-1}b\)，绘制轨迹 \(p(\tau)\) 并验证连续性和边界条件。

练习 A.2（带宽验证，⭐）：对 \(s=3\)（minimum snap）、\(M=5\) 段的情况，\(M(T)\) 是多大的矩阵？带宽是多少？非零元素占总元素的百分比是多少？与稠密矩阵对比，节省了多少存储空间？

---

附录 B：数值实现的工程细节 ⭐⭐

本附录的目的：将 §D5.1-D5.2 的数学和代码串联为**一个完整的实现指南**——从零搭建一个可运行的 2D minimum jerk MINCO 求解器。这是累积项目 D5 模块的理论基础。

B.1 数值稳定性：时间归一化

MINCO 矩阵 \(M(T)\) 的元素包含 \(T_i\) 的各次幂（\(T_i, T_i^2, \ldots, T_i^{2s-1}\)）。当 \(T_i\) 很大或很小时，矩阵条件数退化。

问题示例：\(s=4\)（minimum snap），\(T_i = 5\) 秒。\(M(T)\) 的元素范围从 \(T_i^0 = 1\) 到 \(T_i^7 = 78125\)——相差 5 个数量级。double 精度下，LU 分解的前向消去可能累积 \(\sim 10^{-11}\) 的误差，通常可接受。但当 \(M > 50\) 段时，误差可能累积到 \(10^{-8}\)，影响梯度精度，导致 L-BFGS 收敛变慢。

解决方案：时间归一化。将每段的时间归一化到 \([0, 1]\)：

\[\bar{\tau} = \tau / T_i, \quad \bar{\tau} \in [0, 1]\]

归一化后的多项式系数 \(\bar{c}_{i,k} = c_{i,k} \cdot T_i^k\)，矩阵元素不再包含 \(T_i\) 的高次幂。代价函数和梯度需要对应调整（乘以 \(T_i^{-(2s-1)}\) 等因子），但矩阵条件数大幅改善。

GCOPTER 的实现**使用了时间归一化**——这是它在 \(M > 100\) 段时仍然数值稳定的关键原因之一。

本质洞察：时间归一化的本质是**选择合适的坐标系**。未归一化时，矩阵元素的物理量纲混杂（位置、速度、加速度、jerk...的系数量纲不同）；归一化后，所有元素变为无量纲数，量级相近——这正是数值线性代数追求的"好条件数"。

B.2 梯度验证：有限差分 vs 隐式微分

在实现 MINCO 的隐式微分后，**必须**用有限差分法验证梯度的正确性。这不是可选步骤——隐式微分的推导和实现非常容易出错（符号、转置、链式法则的顺序），而梯度错误的后果是 L-BFGS 收敛到错误的点或不收敛。

有限差分验证协议：

输入: 任意可行的 (q, T)
参数: ε = 1e-7 (扰动步长)
容差: δ = 1e-5 (相对误差上界)

对于每个决策变量 x_k:
  1. 计算 J(x + ε·e_k) 和 J(x - ε·e_k)    // 中心差分
  2. 有限差分梯度: g_fd_k = (J(x+ε·e_k) - J(x-ε·e_k)) / (2ε)
  3. 隐式微分梯度: g_im_k = ∂J/∂x_k       // 你的实现
  4. 相对误差: err_k = |g_fd_k - g_im_k| / max(|g_fd_k|, |g_im_k|, 1e-10)
  5. 断言: err_k < δ

注意中心差分而非前向差分。前向差分 \((J(x+\epsilon) - J(x))/\epsilon\) 的精度是 \(O(\epsilon)\)；中心差分 \((J(x+\epsilon) - J(x-\epsilon))/(2\epsilon)\) 的精度是 \(O(\epsilon^2)\)。对于 \(\epsilon = 10^{-7}\)，中心差分的精度是 \(10^{-14}\)，前向差分只有 \(10^{-7}\)——差 7 个数量级。

多视角理解：梯度验证的价值

• 调试视角：隐式微分涉及矩阵求逆、链式法则、转置——任何一步出错，优化就不收敛。有限差分是独立的验证路径。
• 教学视角：有限差分验证让你确信"隐式微分确实给出了正确的梯度方向"——从"相信数学推导"升级为"看到数值证据"。
• 工程视角：梯度验证应作为**单元测试**永久保留。任何对代价函数或约束的修改，都必须重新通过梯度验证。GCOPTER 的测试代码中就包含这样的验证。

B.3 L-BFGS 超参数调优指南

L-BFGS 在 MINCO 中的表现对超参数敏感。以下是经验调优指南：

超参数	含义	推荐范围	过小的后果	过大的后果
m (历史长度)	存储的历史 \((s_k, y_k)\) 对数	5-15	曲率估计不准，收敛慢	内存浪费，改善微弱
maxIter	最大迭代次数	200-500	未收敛就停止	浪费计算（已收敛仍迭代）
ε_g (梯度容差)	\(\|\nabla J\|_\infty < \epsilon_g\) 时停止	\(10^{-5}\) - \(10^{-3}\)	过拟合（收敛到数值噪声）	未充分收敛
ε_f (函数值容差)	$	J_k - J_{k-1}	/	J_k
α_init (初始步长)	Armijo 线搜索的起始步长	1.0	线搜索迭代多	第一步过冲
c1 (Armijo 参数)	充分下降条件 \(J(x+\alpha d) \le J(x) + c_1 \alpha \nabla J^T d\)	\(10^{-4}\)	接受太差的步长	线搜索迭代多

GCOPTER 的默认配置：m=8, maxIter=200, ε_g=1e-4。在大多数场景下，L-BFGS 在 50-100 次迭代内收敛。若超过 200 次未收敛，通常是初始化问题（而非超参数问题）。

B.4 初始化策略

L-BFGS 是局部优化器——初始点的质量直接决定解的质量。GCOPTER 使用以下初始化策略：

路点初始化：前端路径搜索（A*）生成的路径节点直接作为 MINCO 的初始路点 \(q^{(0)}\)。

时间初始化：

\[T_i^{(0)} = \frac{\|q_i - q_{i-1}\|}{v_{\max}} \cdot \kappa\]

其中 \(v_{\max}\) 是最大速度，\(\kappa \in [1.2, 2.0]\) 是安全因子（给转弯预留时间）。

反事实推理：如果初始时间过小会怎样？ \(T_i^{(0)}\) 过小意味着初始轨迹要求"在极短时间内飞过这段距离"——需要极高的速度和加速度。动力学惩罚项 \(J_{\text{dyn}}\) 会极大，主导整个代价函数，使 L-BFGS 在早期迭代中只关注"降低速度/加速度"而忽略平滑性和避障。这会导致收敛到质量很差的局部最优——轨迹虽然满足动力学约束，但不够平滑或太绕远。

如果初始时间过大呢？轨迹过慢，但 MINCO 的时间优化会自然地将 \(T_i\) 压缩到合适值。因此，宁可初始时间偏大，不可偏小。

B.5 从零实现 2D Minimum Jerk MINCO 的步骤

以下是一个可操作的实现清单，用 C++ (Eigen) 或 Python (NumPy) 均可：

步骤 1: 定义数据结构
  - innerPts: (M-1) × 2 矩阵 (2D 路点)
  - timeAlloc: M × 1 向量 (段时间)
  - headState: [p0, v0] (起点位置+速度)
  - tailState: [pf, vf] (终点位置+速度)

步骤 2: 组装带状矩阵 M(T)
  - 对于 s=2 (minimum jerk), 每段多项式阶数 = 3
  - 每个块大小 = 2s = 4
  - M(T) 是 4M × 4M 的块三对角矩阵
  - 填入连续性条件 + 最优性条件的系数

步骤 3: 组装右端向量 b(q)
  - 第 1 块: 依赖 headState + q_1
  - 中间块: 依赖 q_{i-1} + q_i
  - 最后块: 依赖 q_{M-1} + tailState

步骤 4: 块 Thomas 算法求解 c = M(T)^{-1} b(q)
  - 前向消去: O(M)
  - 回代: O(M)

步骤 5: 计算代价 J = Σ_i ∫_0^{T_i} ||p_i'''(τ)||^2 dτ
  - 对三次多项式, jerk 是常数 → 积分是解析的

步骤 6: 隐式微分计算 ∂J/∂q 和 ∂J/∂T
  - ∂J/∂c 是 4M × 2 矩阵 (解析)
  - ∂c/∂q 通过 M(T) ∂c/∂q = ∂b/∂q 求解 (带状系统)
  - ∂c/∂T 通过 M(T) ∂c/∂T = ∂b/∂T - (∂M/∂T)c 求解

步骤 7: 有限差分验证梯度 (必做!)
  - 扰动 q 的每个分量, 对比有限差分和隐式微分

步骤 8: L-BFGS 优化
  - 传入 J(q,T) 和 ∇J(q,T)
  - 收敛后提取最优 q*, T*, c*

步骤 9: 可视化
  - 绘制轨迹 p(t), 速度 v(t), 加速度 a(t), jerk j(t)
  - 验证连续性、边界条件、平滑性

多视角理解：为什么推荐 2D minimum jerk 而非 3D minimum snap 作为入门？

• 调试视角：2D 可以直接绘图验证轨迹形状，3D 需要 3D 可视化工具。
• 复杂度视角：minimum jerk (\(s=2\)) 的块大小是 4，minimum snap (\(s=4\)) 的块大小是 8——手推矩阵的工作量差 4 倍。
• 概念完整性：从 \(s=2\) 到 \(s=4\) 的推广是直接的——只需增大块大小和多项式阶数，算法结构完全不变。先用简单的例子理解算法骨架，再推广到实际问题。

---

附录 C：安全走廊生成的几何直觉与退化分析 ⭐⭐⭐

本附录的目的：§D5.3 介绍了 SFC 的三代方法，但集中在算法层面。本附录从**几何直觉**出发，用具体场景解释走廊生成的成功和失败模式，帮助读者在调试时快速定位问题。

C.1 走廊生成的本质：凸集的最大膨胀

所有 SFC 方法的目标都可以统一为：从一个种子点出发，在障碍物之间膨胀出一个尽可能大的凸自由区域。不同方法的差异在于"膨胀"的定义和约束。

  方法         膨胀策略                 约束
  ─────────────────────────────────────────────────
  DecompUtil   椭球 → 多面体           种子必须在自由空间内
  IRIS         交替优化椭球+多面体      无种子包含保证 (!)
  FIRI         SOCP 最大椭球 + 限制性膨胀  种子包含保证 + 100x 加速
  CIRI         FIRI 推广到构型空间      考虑机器人几何形状

C.2 退化场景分析

场景 1：狭窄通道

  ████████████████████
  ████████████████████
                       ← 窄缝 (宽度 = 0.5m, 无人机直径 = 0.3m)
  ████████████████████
  ████████████████████

在狭窄通道中，走廊的体积极小——最大内切椭球可能退化为几乎一维的"细条"。后果： - MINCO 的路点被约束在极窄的空间内，自由度极低 - diffeomorphic 参数化中 \(\tanh\) 几乎永远饱和，梯度接近零 - L-BFGS 在此段几乎无法移动路点

解决方案：增加走廊密度——在窄缝中插入更多走廊（每 0.2m 一个种子），用多个"细条"走廊的序列覆盖窄缝。每个走廊虽然小，但路点的选择空间因为走廊数量增加而恢复。

场景 2：L 形拐角

  ████████████████
  ████████████████
  ███    ← 走廊 1 (水平段)
  ███ ████████████
  ███ ████████████
   ↓  走廊 2 (垂直段)
  ███
  ███

L 形拐角处，走廊 1 和走廊 2 的交集可能非常小（仅拐角处的一小块）。路点被挤在交集中，轨迹被迫急转弯。

解决方案：在拐角处插入一个额外的走廊——种子放在 L 的内角处，该走廊覆盖拐角区域，提供更大的过渡空间。

场景 3：IRIS 的种子丢失问题

IRIS 使用交替优化——先固定多面体优化内切椭球，再固定椭球优化多面体。这个交替过程**不保证种子点在最终多面体内**。

  初始: 种子点 ★ 在自由空间中
  迭代 1: 椭球膨胀, 多面体包围椭球
  迭代 2: 椭球偏移（向更大的自由区域）, 多面体跟随偏移
  ...
  迭代 N: 椭球和多面体都"漂"到了远离种子的位置!

FIRI 通过 Restrictive Inflation 解决了这个问题——每次膨胀都添加一个约束"种子必须在多面体内"。代价是走廊可能稍小（被约束限制了膨胀方向），但换来了**确定性**——种子一定在走廊内。

对比性思维：确定性 vs 最优性的权衡。IRIS 允许走廊"自由漂移"以找到更大的走廊，但牺牲了种子包含；FIRI 约束走廊必须包含种子，换来确定性但走廊可能更小。在实践中，确定性几乎总是更重要——因为"走廊不包含种子"意味着"路径上的这个点在走廊外面"，这是灾难性的规划失败（路点在走廊外意味着轨迹一定穿过障碍物）。FIRI 用走廊体积的轻微减少换取了零失败率——这是正确的工程权衡。

C.3 走廊序列的质量指标

在调试走廊问题时，以下指标有助于诊断：

指标	定义	健康范围	不健康的后果
走廊体积 \(V_i\)	多面体的体积	\(> 0.1 \text{m}^3\)	路点自由度不足，轨迹不平滑
交集体积 \(V_{i \cap i+1}\)	相邻走廊交集的体积	\(> 0.05 \text{m}^3\)	路点被挤在极小区域，急转弯
内切椭球轴比 \(\lambda_{\max}/\lambda_{\min}\)	椭球最长轴 / 最短轴	\(< 10\)	走廊退化为"细条"，参数化困难
走廊数量 \(N_c\)	总走廊数	路径长度 / 2m ~ 路径长度 / 0.5m	太少 → 走廊太大可能包含障碍；太多 → 优化维度高
面数 \(n_h\)	每个多面体的半空间数	\(6 \sim 20\)	太多 → MINCO 参数化变慢

C.4 可视化诊断流程

当 MINCO 优化不收敛或轨迹不合理时，按以下顺序检查走廊：

1. 可视化所有走廊 (半透明多面体) + 路径 (种子点连线)
   → 种子点是否都在对应走廊内? (FIRI 保证, IRIS 不保证)

2. 可视化相邻走廊的交集
   → 交集是否非空? 是否足够大?

3. 可视化内切椭球
   → 椭球是否退化 (一个轴极短)?
   → 椭球中心是否偏离路径太远?

4. 可视化 MINCO 路点在走廊内的位置
   → 路点是否贴着走廊边界? (tanh 饱和信号)

5. 如果问题在特定走廊: 增加该区域的走廊密度
   如果问题是全局性的: 检查前端路径质量

本质洞察：走廊质量是 MINCO 轨迹质量的**上界**——再好的优化器也无法在劣质走廊中生成好轨迹。调试 MINCO 轨迹问题时，50% 以上的情况根因在走廊而非优化器。

---

附录 D：MINCO 在群飞系统中的通信与同步细节 ⭐⭐

本附录的目的：§D5.5 概述了 EGO-Planner-v2 的群飞架构，但通信和同步是群飞系统中最容易出错的部分。本附录深入讨论 UDP 广播、时间同步和碰撞检测的实现细节。

D.1 轨迹广播协议

MINCO 的紧凑表示使得轨迹广播非常高效。一条 MINCO 轨迹只需要广播：

字段	大小	说明
drone_id	1 byte	无人机编号
timestamp	8 bytes	全局时间戳（规划完成时刻）
M (段数)	1 byte	通常 5-15 段
s (阶数)	1 byte	通常 4 (minimum snap)
headState	\(2s \times 3 \times 8\) bytes	起点状态（位置到 \(s-1\) 阶导数，3D）
tailState	\(2s \times 3 \times 8\) bytes	终点状态
innerPts	\((M-1) \times 3 \times 8\) bytes	中间路点
timeAlloc	\(M \times 8\) bytes	段时间

对于 \(M=10, s=4\)：\(1 + 8 + 1 + 1 + 192 + 192 + 216 + 80 = 691\) bytes。

对比性思维：如果广播多项式系数而非 MINCO 参数？ \(M=10\) 段、\(s=4\)（7 次多项式）、3D 的多项式系数有 \(10 \times 8 \times 3 = 240\) 个双精度数，共 \(240 \times 8 = 1920\) bytes——是 MINCO 参数的 2.8 倍。更重要的是，MINCO 参数的物理意义清晰（路点和时间），便于接收方做碰撞检测预判（只需比较路点距离，不需要完整的轨迹评估）。

D.2 时间同步的关键性

群飞碰撞避让依赖**时间对齐**——必须比较"在同一物理时刻，两架无人机分别在哪里"。时间不对齐的碰撞检测是无效的。

时间同步方案：

方案	精度	复杂度	适用场景
NTP	~1 ms	需网络	室内 WiFi
PTP (IEEE 1588)	~1 \(\mu\)s	需硬件支持	工业级
GPS PPS	~100 ns	需 GPS 信号	室外
本地同步	~10 ms	无额外硬件	原型验证

EGO-Planner-v2 在 Science Robotics 的实验中使用了基于 NTP 的时间同步，精度约 1-2 ms。对于 3-5 m/s 的飞行速度，1 ms 的时间误差对应 3-5 mm 的位置误差——远小于 0.5 m 的安全距离，因此可以忽略。

但在更高速度（\(>10\) m/s）或更紧密的编队（安全距离 \(<0.3\) m）中，时间同步精度变得关键。1 ms 误差在 15 m/s 下对应 15 mm 位置误差——仍可接受。但 10 ms 误差（本地同步）对应 150 mm——已经是安全距离的 50%，可能导致虚假碰撞告警或漏检。

D.3 去中心化碰撞检测的时间复杂度

\(N\) 架无人机的群飞系统中，每架无人机需要检测与其他 \(N-1\) 架的碰撞。每次碰撞检测需要在自己轨迹的每个段上采样并与对方轨迹对比。

每架无人机的碰撞检测计算量：

\[\text{Cost} = (N-1) \cdot M_{\text{self}} \cdot K_{\text{sample}} \cdot O(\text{eval})\]

其中 \(K_{\text{sample}}\) 是每段的采样点数（通常 5-10），\(O(\text{eval})\) 是评估 MINCO 轨迹在一个时间点的代价（\(O(s)\)，对 \(s=4\) 约 8 次乘法）。

\(N\)	\(M\)	\(K\)	每次碰撞检测的浮点运算数	30 Hz 下的计算负载
5	10	8	\(4 \times 10 \times 8 \times 8 = 2560\)	~77 KFLOP/s
10	10	8	\(9 \times 10 \times 8 \times 8 = 5760\)	~173 KFLOP/s
20	10	8	\(19 \times 10 \times 8 \times 8 = 12160\)	~365 KFLOP/s
50	10	8	\(49 \times 10 \times 8 \times 8 = 31360\)	~941 KFLOP/s

即使 50 架无人机，碰撞检测的计算量也不到 1 MFLOP/s——对现代处理器完全可忽略。瓶颈不在计算，而在通信（50 架 \(\times\) 30 Hz \(\times\) 691 bytes \(\approx\) 1 MB/s）。

D.4 对称打破机制

当两架无人机面对面飞行时，碰撞惩罚项会推动两机都"让路"。但由于对称性，两机可能做出完全对称的避让动作——结果是都偏向同一侧，然后再纠正，形成"互锁"（deadlock）。

解决方案：基于 ID 的优先级打破

规则: drone_id 较小的无人机拥有"路权"(right of way)
  - drone_id 小的: 碰撞惩罚权重 w_s 较小 → 轻微避让
  - drone_id 大的: 碰撞惩罚权重 w_s 较大 → 大幅避让

这打破了对称性——低 ID 的无人机只做微小调整，高 ID 的无人机主动让路。类似于交通规则中"右侧车辆优先"的规定。

多视角理解：为什么不用更复杂的避让协议？

• 简单性视角：基于 ID 的优先级是**无需协商**的——每架无人机独立决策，不需要额外通信。复杂协议（如拍卖、投票）需要多轮通信，增加延迟。
• 鲁棒性视角：在 UDP 丢包的环境下，任何依赖"收到对方消息后再决策"的协议都不可靠。基于 ID 的方法只依赖本地信息（自己的 ID 和对方的 ID），不依赖任何特定消息是否收到。
• 扩展性视角：对 \(N\) 架无人机，基于 ID 的优先级定义了一个**全序**（\(0 < 1 < 2 < \ldots\)），自动处理多机互锁（不限于两机）。

---

附录 E：跨章综合题与深度思考 ⭐⭐⭐

本附录的目的：提供跨越 D3-D5 多章的综合练习题，帮助读者建立知识之间的联系。这些题目不是"套公式"，而是需要综合多个知识点进行分析和设计。

综合题 E.1：端到端轨迹规划管线设计（⭐⭐⭐）

场景：你需要为一个仓库巡检无人机设计完整的轨迹规划管线。仓库大小 \(50\text{m} \times 30\text{m} \times 5\text{m}\)，有 200 个货架（静态障碍物），最大飞行速度 3 m/s，需要 10 分钟内完成巡检。

设计要求： 1. 画出完整的管线框图（前端搜索 → 走廊生成 → 轨迹优化 → 轨迹跟踪） 2. 对每个模块，说明你选择的方法和理由（为什么选 A* 不选 RRT？为什么选 FIRI 不选 DecompUtil？） 3. 估算各模块的计算时间，验证整条管线能否在 \(< 100\) ms 内完成 4. 分析最可能出问题的环节和对应的 fallback 策略

提示：回顾 D3 的 Mellinger QP（为什么在这个场景下不合适？），D4 的 B 样条（有哪些优势可以考虑？），D5 的 MINCO（为什么是最佳选择？）。

综合题 E.2：从 Mellinger 到 MINCO 的理论对比（⭐⭐⭐）

任务：对同一个 10 段 3D 轨迹优化问题，分别用 Mellinger QP 和 MINCO 求解。

1. 列出两种方法的决策变量、约束条件、目标函数
2. 计算两种方法的矩阵维度和理论浮点运算数
3. 实现两种方法（可用 Python），对比实际求解时间
4. 绘制两种方法的轨迹，验证它们是否一致（在相同边界条件下应该一致）
5. 将段数增加到 50、100、200，绘制求解时间 vs 段数的曲线，验证 \(O(M^3)\) vs \(O(M)\) 的理论预测

综合题 E.3：安全走廊失败模式分析（⭐⭐⭐⭐）

场景：在一个有 U 形障碍物的环境中，FIRI 走廊生成失败——生成的走廊序列不连通（相邻走廊交集为空）。

  ███████████████████
  █                 █
  █   ███████████   █
  █   █         █   █
  █   █  ★→→→→→ █   █    ★ = 起点, ● = 终点
  █   █         █   █
  █   █████ █████   █
  █         ●       █
  ███████████████████

1. 分析为什么直线种子点序列在 U 形环境中会导致走廊不连通
2. 设计一个种子点生成策略，使得走廊序列在 U 形环境中连通
3. 讨论如何自动检测"走廊不连通"并触发种子点重生成
4. 如果 U 形障碍物的内壁间距恰好等于无人机直径（极限情况），你的策略如何调整？

综合题 E.4：SUPER 安全性的形式化验证尝试（⭐⭐⭐⭐）

任务：SUPER 的递归可行性证明（§D5.4.3）依赖几个假设。识别并分析这些假设在实际中可能被打破的情况。

1. 列出递归可行性证明的所有假设（显式的和隐式的）
2. 对每个假设，构造一个使其不成立的具体场景
3. 对每个场景，分析 SUPER 的实际行为（是否仍然安全？安全性降级到什么程度？）
4. 提出至少一个改进方案（不一定要完整实现，给出思路即可）

提示：考虑以下因素——传感器延迟、计算延迟、执行延迟、环境动态变化、通信中断。

---

附录 F：MINCO 与控制层的接口——从规划到执行 ⭐⭐

本附录的目的：MINCO 生成的轨迹最终要由飞控系统执行。本附录讨论 MINCO 轨迹与控制器之间的接口，包括参考信号提取、跟踪误差分析和实时重规划策略。

F.1 从 MINCO 轨迹提取参考信号

飞控系统需要的参考信号通常包括：位置 \(p_{\text{ref}}(t)\)、速度 \(v_{\text{ref}}(t)\)、加速度 \(a_{\text{ref}}(t)\)、以及偏航角 \(\psi_{\text{ref}}(t)\)。MINCO 轨迹直接提供前三个（通过多项式评估），偏航角需要额外处理。

**位置/速度/加速度的提取**是直接的——给定全局时间 \(t\)，先确定它落在第几段（\(t \in [t_{\text{start}} + \sum_{j=1}^{i-1} T_j, \, t_{\text{start}} + \sum_{j=1}^{i} T_j]\)），然后计算段内时间 \(\tau = t - t_{\text{start}} - \sum_{j=1}^{i-1} T_j\)，最后用多项式系数评估：

\[p_{\text{ref}} = \sum_{k=0}^{2s-1} c_{i,k} \tau^k, \quad v_{\text{ref}} = \sum_{k=1}^{2s-1} k \cdot c_{i,k} \tau^{k-1}, \quad a_{\text{ref}} = \sum_{k=2}^{2s-1} k(k-1) \cdot c_{i,k} \tau^{k-2}\]

**偏航角的处理**有两种策略：

策略	做法	优点	缺点
速度方向	\(\psi_{\text{ref}} = \text{atan2}(v_y, v_x)\)	简单，无额外规划	悬停时未定义（\(v \to 0\)），转弯时偏航可能急变
独立规划	单独优化 \(\psi(t)\) 为一维 MINCO	平滑，可指定朝向	额外计算量

反事实推理：如果不提供加速度前馈会怎样？ 许多初学者只把 \(p_{\text{ref}}\) 传给 PID 控制器，不传 \(v_{\text{ref}}\) 和 \(a_{\text{ref}}\)。后果：(1) PID 需要靠积分项"追上"速度——跟踪误差大、响应慢；(2) 在急转弯处，PID 来不及响应，误差可能超过安全距离。正确做法是使用前馈+反馈控制——前馈项直接使用 \(a_{\text{ref}}\) 计算期望推力，反馈项仅修正跟踪误差。这使得跟踪误差从"与速度成正比"降为"与模型误差成正比"。

F.2 跟踪误差预算

MINCO 规划的轨迹与实际飞行的轨迹之间存在误差。这个误差来自多个来源：

误差来源	典型量级	可控性
控制器跟踪误差	5-20 cm	可通过控制器调参改善
模型误差（空气阻力等）	2-10 cm	需要辨识或自适应
状态估计误差	1-5 cm	取决于传感器和估计器
计算延迟	1-5 cm (= \(v \times \Delta t\))	可通过预测补偿
通信延迟	0-3 cm	取决于通信架构
总计	10-40 cm

安全裕度设计原则：MINCO 的障碍物膨胀半径应至少等于**跟踪误差预算的总和加上无人机半径**：

\[r_{\text{inflate}} = r_{\text{drone}} + e_{\text{tracking}} + e_{\text{model}} + e_{\text{estimation}} + e_{\text{delay}}\]

本质洞察：跟踪误差预算是**规划层和控制层之间的契约**——规划层保证轨迹距障碍物至少 \(r_{\text{inflate}}\)，控制层保证实际飞行不偏离轨迹超过 \(r_{\text{inflate}} - r_{\text{drone}}\)。如果任一方违约，碰撞就可能发生。SUPER 的双轨迹方案本质上是为这个契约加了一层**保险**——即使控制层违约（跟踪失败），backup 轨迹提供了二次保护。

F.3 实时重规划策略

MINCO 的 \(O(M)\) 求解速度使得高频重规划成为可能。以下是常见的重规划策略：

定频重规划：每 \(\Delta t_{\text{plan}}\) 秒重规划一次（如 30 Hz）。优点是简单可预测；缺点是资源浪费（环境没变也要重新规划）。

事件触发重规划：仅在以下事件发生时重规划： 1. 新障碍物被检测到（地图更新） 2. 跟踪误差超过阈值 3. 当前轨迹在剩余时间内可能违反约束（前瞻检查）

事件触发重规划的状态机:

  [执行中] ──── 无事件 ────→ [继续执行]
     │
     ├── 新障碍物检测 ──→ [重规划]
     ├── 跟踪误差 > ε ──→ [重规划]
     ├── 前瞻检查告警 ──→ [重规划]
     │
     └── 重规划失败 ──→ [执行 backup] (SUPER)
                         [紧急悬停] (单轨迹)

混合策略（GCOPTER 默认）：基础频率 10 Hz + 事件触发加速到 50 Hz。平时节省计算，紧急时快速响应。

F.4 Warm-start 与轨迹拼接

当重规划时，上一次的最优轨迹提供了一个很好的初始猜测。Warm-start 的核心思想是利用上次解来加速本次求解：

路点 Warm-start：取上次最优轨迹在新的走廊交叉点处的位置作为新的初始路点。比从 A* 路径节点初始化更接近最优解，L-BFGS 通常只需 10-30 次迭代即可收敛（而冷启动需要 50-200 次）。

时间 Warm-start：取上次的段时间作为新的初始时间分配。如果走廊变化不大，段时间也不会差太多。

多视角理解：Warm-start 的收益与风险

• 速度视角：Warm-start 可将收敛迭代数减少 50-80%，重规划时间从 ~1 ms 降到 ~0.3 ms。
• 质量视角：上次解是一个"好的"初始点，L-BFGS 更可能找到同一个局部最优——轨迹在重规划之间的变化更平滑。
• 风险视角：如果环境剧变（新障碍物切断了上次路径），上次解可能是一个很差的初始点——L-BFGS 可能被困在上次的局部最优附近，找不到绕过新障碍物的路径。因此 GCOPTER 在 warm-start 失败时自动回退到冷启动。

---

附录 G：术语对照表与符号总汇

G.1 中英术语对照

中文	英文	首次出现
最小控制量	Minimum Control (MINCO)	§D5.1.1
安全飞行走廊	Safe Flight Corridor (SFC)	§D5.3.1
带状矩阵	Banded Matrix	§D5.1.4
块三对角	Block Tridiagonal	§D5.1.4
隐式微分	Implicit Differentiation	§D5.1.6
微分同胚参数化	Diffeomorphic Parameterization	§D5.1.7
内切椭球	Inscribed Ellipsoid	§D5.3
限制性膨胀	Restrictive Inflation	§D5.3.4
递归可行性	Recursive Feasibility	§D5.4.3
半空间表示	H-representation (Half-space)	§D5.3.1
顶点表示	V-representation (Vertex)	§D5.3.1
二阶锥规划	Second-Order Cone Programming (SOCP)	§D5.3.4
半定规划	Semidefinite Programming (SDP)	§D5.3.3
构型空间	Configuration Space (C-space)	§D5.3.6
去中心化	Decentralized	§D5.5.1
前馈控制	Feedforward Control	附录 F
事件触发	Event-triggered	附录 F
热启动	Warm-start	附录 F

G.2 数学符号总汇

符号	含义	类型	首次出现
\(s\)	MINCO 阶数（\(s=4\) 对应 minimum snap）	标量	§D5.1.3
\(M\)	轨迹段数	标量	§D5.1.3
\(T_i\)	第 \(i\) 段的时间长度	标量	§D5.1.3
\(q_i\)	第 \(i\) 个中间路点	\(\mathbb{R}^3\)	§D5.1.3
\(c_i\)	第 \(i\) 段的多项式系数向量	\(\mathbb{R}^{2s}\)	§D5.1.3
\(M(T)\)	MINCO 的带状系统矩阵	\(\mathbb{R}^{2sM \times 2sM}\)	§D5.1.4
\(b(q)\)	MINCO 的右端向量	\(\mathbb{R}^{2sM}\)	§D5.1.4
\(J\)	总代价函数	标量	§D5.1.3
\(J_{\text{smooth}}\)	平滑代价（控制量积分）	标量	§D5.2.5
\(J_{\text{obs}}\)	障碍物惩罚代价	标量	§D5.2.5
\(J_{\text{dyn}}\)	动力学约束惩罚代价	标量	§D5.2.5
\(J_{\text{time}}\)	时间正则化代价	标量	§D5.2.5
\(J_{\text{swarm}}\)	群飞碰撞惩罚代价	标量	§D5.5.1
\(w_v, w_a, w_s\)	各惩罚项的权重	标量	§D5.2.5
\(\tilde{q}\)	diffeomorphic 映射前的无约束变量	\(\mathbb{R}^3\)	§D5.1.7
\(\rho\)	diffeomorphic 映射后的时间变量	\(\mathbb{R}\)	§D5.1.7
\(d_{\text{safe}}\)	群飞安全距离	标量	§D5.5.1
\(A, b\)	凸多面体的半空间表示 \(\{x : Ax \le b\}\)	矩阵/向量	§D5.3.1
\(C, d\)	内切椭球参数 \(\{Cu + d : \|u\| \le 1\}\)	矩阵/向量	§D5.3.4

---

附录 H：MINCO 技术演化时间线

本附录按时间顺序梳理 MINCO 及其生态的技术演化，帮助读者建立"从哪里来、到哪里去"的全局图景。

年份	事件	论文/代码	核心贡献
2011	Mellinger-Kumar minimum snap	ICRA 2011	首次将 minimum snap 引入四旋翼，\(24M\) 变量 QP
2014	IRIS：SDP 生成最大凸区域	Deits, WAFR 2014	交替优化椭球+多面体，奠基 SFC
2016	Richter-Bry-Roy 闭式 QP	ISRR 2016	端点导数参数化，\(12(M+1)\) 变量
2017	DecompUtil + SFC 管线	Liu, RA-L 2017	椭球膨胀生成走廊，首次完整 SFC+QP 管线
2018	Bernstein basis trajectory	Gao, ICRA 2018	用 Bernstein 多项式简化凸包约束
2021	大规模轨迹优化器	Wang, ICRA 2021	发现带状结构，\(O(M)\) 求解，MINCO 的前身
2020	Teach-Repeat-Replan	Gao, TRO 2020	完整系统集成，示教-重复-重规划
2022	GCOPTER/MINCO	Wang, TRO 2022	完整理论：一般 \(s\) 阶 + diffeomorphic + L-BFGS
2022	EGO-Planner-v2 群飞	Zhou, Sci. Rob. 2022	MINCO 在 10 机群飞中的验证
2022	编队飞行	Quan, ICRA 2022	图拉普拉斯编队 + MINCO
2024	Implicit SVSDF	Wang J., SIGGRAPH 2024	扫掠体 SDF + MINCO，任意形状机器人
2025	FIRI	Wang Q., TRO 2025	SOCP + 种子包含保证，100x 走廊加速
2025	SUPER	Ren, Sci. Rob. 2025	双轨迹安全范式 + CIRI + ROG-Map
2025	MIGHTY	Kondo, RA-L 2025	Hermite 样条替代 MINCO，严格局部控制

对比性思维：两条主线的交汇

MINCO 的发展有两条主线：轨迹表示（Mellinger → Richter → Wang 2021 → GCOPTER）和**安全走廊**（IRIS → DecompUtil → FIRI → CIRI）。两条主线在 GCOPTER (2022) 首次完整交汇——MINCO 轨迹表示 + SFC 走廊约束 + L-BFGS 无约束优化，形成了一个**闭合的优化框架**。SUPER (2025) 在此基础上加了**安全层**（双轨迹 + 递归可行性），标志着从"规划最优"到"规划安全"的范式转移。

---

附录 I：常见编译与运行问题

I.1 GCOPTER 编译问题

问题	原因	解决方案
Eigen/Dense: No such file	Eigen 未安装或路径未设置	sudo apt install libeigen3-dev，或在 CMakeLists.txt 中设置 EIGEN3_INCLUDE_DIR
catkin_make 报错找不到 gcopter 包	工作空间未正确初始化	source devel/setup.bash，确认 catkin_make 在工作空间根目录执行
链接错误 undefined reference to ros::init	ROS 版本不匹配	GCOPTER 依赖 ROS Noetic（Ubuntu 20.04），不兼容 ROS2。使用 Docker 容器或 Ubuntu 20.04 虚拟机
编译警告 -Wdeprecated-copy	Eigen 3.3 与 GCC 9+ 的兼容性	可忽略，不影响功能。升级到 Eigen 3.4 可消除警告

I.2 运行问题

问题	原因	解决方案
RViz 无显示	话题名不匹配	检查 rostopic list，确认 /trajectory 和 /corridor 话题存在
规划结果飞出边界	地图边界未正确设置	检查 launch 文件中的 map_size_x/y/z 参数
L-BFGS 迭代 500 次未收敛	初始化不良或权重不当	见故障排查手册故障 1
轨迹穿过障碍物	障碍物膨胀半径不足	增大 inflate_radius 参数，至少等于无人机半径 + 跟踪误差预算
段时间出现负值	diffeomorphic 映射的实现错误	检查 \(T_i = \exp(\rho_i)\) 的实现——\(\exp\) 的输出永远为正

I.3 性能调优

场景	瓶颈	调优方法	预期提升
段数 \(M > 50\)	带状求解	确认使用手写 Thomas 算法而非 Eigen SparseLU	10-100x
走廊数 \(> 30\)	走廊生成	减少走廊密度或使用 FIRI 替代 DecompUtil	10-100x
多机 \(N > 10\)	碰撞检测	用 KD-tree 做近邻筛选，只对近邻做碰撞检测	\(O(N) \to O(\log N)\)
高频重规划 \(> 50\) Hz	总体	Warm-start + 减少初始 L-BFGS 迭代上限	2-5x

---

附录 J：本章学习路径建议

J.1 按时间预算选择路径

时间预算: 1 天 (8 小时)
  → §D5.1 (4h) + §D5.3 (2h) + §D5.7 (1h) + 本章小结 (0.5h) + 故障排查 (0.5h)
  → 目标: 理解 MINCO 数学 + SFC 选型 + 工程快速上手

时间预算: 3 天 (24 小时)
  → 全部 7 个正文章节 + 附录 A + 附录 B
  → 目标: 全面理解 + 能独立实现 2D MINCO

时间预算: 1 周 (40 小时)
  → 全部内容 + 所有练习 + 综合题 E.1-E.2
  → 目标: 深入掌握 + 能修改 GCOPTER 源码

时间预算: 2 周 (80 小时)
  → 全部内容 + 所有练习 + 所有综合题 + 实现完整管线
  → 目标: 研究级理解 + 能发表相关论文

J.2 按目标选择重点

目标	重点章节	可跳过
快速在项目中使用 MINCO	§D5.1.1-1.3, §D5.2, §D5.3, 附录 I	§D5.1.4-1.8, §D5.4-5.6
理解 MINCO 的数学本质	§D5.1 全部, 附录 A	§D5.4-5.6
设计群飞系统	§D5.5, 附录 D, §D5.1 (概要)	§D5.6, §D5.7
研究安全规划	§D5.3, §D5.4, 附录 C	§D5.5, §D5.6
写相关方向的论文	全部 + 附录 H (时间线) + 综合题 E.3-E.4	无

J.3 自评检查清单

完成本章学习后，用以下问题自评。能回答 7/10 以上，说明已达到本章的学习目标。

#	问题	对应章节
1	MINCO 的决策变量是什么？为什么这是"最小表示"？	§D5.1.3
2	\(M(T)c = b(q)\) 中的 \(M(T)\) 为什么是带状的？带宽是多少？	§D5.1.4
3	隐式微分如何在不显式构造 \(\partial c/\partial q\) 的情况下计算 \(\partial J/\partial q\)？	§D5.1.6
4	diffeomorphic 参数化如何将凸约束消除？	§D5.1.7
5	GCOPTER 的 minco.hpp 中，setConditions 做了什么？	§D5.2.2
6	FIRI 相比 IRIS 的两个关键改进是什么？	§D5.3.4
7	SUPER 的 backup 轨迹如何保证"任意时刻都有安全后备"？	§D5.4.3
8	群飞中为什么用 UDP 而非 TCP 广播轨迹？	§D5.5.1
9	MINCO 和 B 样条各自的核心优势是什么？	§D5.7
10	MINCO 的 L-BFGS 不收敛时，最应该首先检查什么？	故障排查