D3 多项式轨迹生成——Min-Snap 与 Richter QP¶
性质：算法工程教学 | 难度跨度：⭐⭐ ~ ⭐⭐⭐⭐ | 预计精读：10-14 小时
一句话定位：从 2011 年 Mellinger 的受约束 QP 到 2016 年 Richter 的无约束闭式解，本章完整讲透四旋翼多项式轨迹生成的数学核心——为什么 snap 是正确的代价、怎样把轨迹优化写成 QP、以及如何消除数值病态让方法在 50+ 段上仍然稳定。
前置自测¶
开始前先回答下面 5 个问题。答不出 2 题以上，建议先回前置章节补齐——D3 的每一步推导都建立在这些基础之上，欠了账会在第三章卡住。
-
微分平坦（Differential Flatness）的定义是什么？ 四旋翼的平坦输出是哪四个量？为什么平坦输出的维度必须等于控制输入的维度？ （答不出 → 回 D1 微分平坦，§D1.1）
-
二次型 \(x^\top A x\) 的最小化条件是什么？ 当 \(A\) 半正定时，最优解在什么条件下唯一？当加入线性等式约束 \(Cx = d\) 后，KKT 条件的形式是什么？ （答不出 → 回 Part 0 优化基础）
-
矩阵条件数（Condition Number）衡量什么？ 条件数 \(\kappa(A) = 10^{12}\) 意味着用 64 位浮点数求解 \(Ax = b\) 时，结果最多有几位有效数字？ （答不出 → 回 Part 0 数值线性代数基础）
-
Eigen 的固定大小矩阵（如
Matrix3d）和动态大小矩阵（MatrixXd）在内存分配上的核心区别是什么？ 为什么在 1kHz 控制循环中应优先使用固定大小矩阵？ （答不出 → 回 v8 Ch11 Eigen 深度） -
凸优化问题为什么保证全局最优？ 如果目标函数是凸的但约束集是非凸的，全局最优还保证吗？ （答不出 → 回 Part 0 优化基础）
参考答案要点（先自己答，再对照）：
-
微分平坦：存在输出映射 \(\sigma = h(x, u, \dot{u}, \ldots)\)，使 \(\dim(\sigma) = m\)（控制输入维度），且状态 \(x\) 和控制 \(u\) 可表示为 \(\sigma\) 及其有限阶导数的函数。四旋翼的平坦输出是 \(\sigma = (x, y, z, \psi)^\top\)，4 维匹配 4 个控制输入（总推力 + 3 个体力矩）。维度必须相等是因为平坦输出需要"参数化"全部控制自由度。
-
无约束时令梯度 \(2Ax = 0\)，需要 \(A\) 正定才有唯一解 \(x = 0\)。半正定时零空间非空，最优解不唯一。加约束后 KKT 条件为 \(\begin{pmatrix} A & C^\top \\ C & 0 \end{pmatrix} \begin{pmatrix} x \\ \lambda \end{pmatrix} = \begin{pmatrix} 0 \\ d \end{pmatrix}\)。
-
条件数衡量输入微小扰动被放大的最大倍数。\(\kappa = 10^{12}\) 时，64 位浮点（~16 位十进制精度）中约 12 位被条件数"吃掉"，结果仅有 ~4 位有效数字。
-
固定大小矩阵在栈上分配（零 malloc），编译器可自动 SIMD 向量化；动态大小矩阵每次构造都要堆分配（new/delete）。1kHz 循环中每帧数百次矩阵运算，堆分配的开销会累积成可观的延迟。
-
凸函数在凸集上最小化 → 任何局部最优都是全局最优。如果约束集非凸，即使目标凸也无法保证——因为可行域可能有多个不连通的"岛"，不同岛上的局部最优不一定是全局最优。
本章目标¶
学完本章后，你应该能够：
- **严格论证**为什么最小化 snap（四阶导数）积分是四旋翼轨迹优化的天然代价——从微分平坦映射的层级结构出发，而非凭直觉
- 从零推导 Mellinger & Kumar 2011 的受约束 QP 公式——包括 Hessian 矩阵 \(Q_i\) 的解析元素公式、约束矩阵的组装、KKT 系统的结构
- 理解并解释 Mellinger 方法的数值病态根源——单项式基的 Vandermonde 型映射矩阵条件数随 \(T^N\) 增长
- 从零推导 Richter, Bry & Roy 2013/2016 的端点导数变量替换——为什么约束"消失"了、\(R_{PP}\) 的条件数为什么小、闭式解 \(d_P^* = -R_{PP}^{-1} R_{FP}^\top d_F\) 的物理含义
- 理解 Bernstein 基的凸包性质（Convex Hull Property）如何将无穷维安全约束退化为有限维线性约束
- 掌握
mav_trajectory_generation的 Vertex→Segment→Trajectory 三层抽象和非类型模板参数（NTTP）设计 - **能用 Python 从零实现**一个完整的 3D 最小 snap 轨迹生成器（Richter 公式），并通过微分平坦逆映射生成电机指令
本章知识导航¶
本章的知识结构是一棵以"如何生成四旋翼最优轨迹"为根的树。树干是三个递进的核心问题，树枝是每个问题的技术细节和工程实现。
为什么 snap 是正确的代价？ (§D3.1 微分平坦回顾)
│
▼
怎样把轨迹优化写成数学问题？ (§D3.2 分段多项式表示)
│
├─→ Mellinger QP (§D3.3) ──→ 数值病态 (§D3.3.8)
│ │
│ ▼
├─→ Richter 无约束化 (§D3.4) ──→ 消除病态 + 闭式解
│
├─→ Bernstein 基与安全走廊 (§D3.5) ──→ 凸包性质
│
├─→ 时间分配优化 (§D3.6) ──→ 启发式 / 梯度优化
│
└─→ 工程实现 (§D3.7 mav_trajectory_generation)
│
▼
完整工作流 (§D3.8) ──→ 航路点 → 轨迹 → 电机指令
| 小节 | 主题 | 难度 | 一句话 |
|---|---|---|---|
| §D3.1 | 微分平坦回顾 | ⭐⭐ | snap 是电机平滑性的代理指标 |
| §D3.2 | 分段多项式表示 | ⭐⭐ | \(2r-1\) 阶多项式，端点求值矩阵 \(A_i\) |
| §D3.3 | Mellinger QP | ⭐⭐⭐ | 块对角 Hessian + 等式约束 + KKT 闭式 |
| §D3.4 | Richter 无约束化 | ⭐⭐⭐ | 端点导数替代系数，约束被吸收进变量定义 |
| §D3.5 | Bernstein 基与安全走廊 | ⭐⭐⭐ | 凸包性质将无穷维约束退化为有限维 |
| §D3.6 | 时间分配优化 | ⭐⭐⭐ | 时间是非凸变量，梯度+封包定理 |
| §D3.7 | mav_trajectory_generation 精读 | ⭐⭐ | NTTP + 三层抽象 + NLopt 两层优化 |
| §D3.8 | 完整工作流与数值实验 | ⭐⭐ | 端到端：航路点→多项式→微分平坦→电机 |
| §D3.9 | 大规模扩展与理论深入 | ⭐⭐⭐⭐ | am_traj / large_scale / 凸性与对偶 |
两条阅读线：
- 理论线（理解数学）：§D3.1→§D3.2→§D3.3→§D3.4→§D3.5→§D3.9，重点是推导和证明
- 工程线（会用就行）：§D3.1→§D3.2→§D3.4→§D3.7→§D3.8，重点是实现和接口
无论哪条线，§D3.1 和 §D3.2 都是必读——它们是所有后续内容的地基。
前置知识桥接¶
回顾 D1（微分平坦）：D1 建立了四旋翼的核心数学性质——给定一条平坦输出曲线 \(\sigma(t) = (x(t), y(t), z(t), \psi(t))^\top\)，全部 12 维状态和 4 维控制输入可以通过**纯代数运算**（无需积分）从 \(\sigma\) 及其前四阶导数恢复。这意味着：规划问题从"12 维状态空间的最优控制"坍缩为"4 维平坦输出空间的曲线设计"——维度降了 3 倍，且不需要显式处理非线性动力学。本章正是在这个坍缩后的空间里做优化。
回顾 v8 Ch11（Eigen 深度）：Mellinger/Richter 的 QP 核心运算是 \(8 \times 8\) 到 \(40 \times 40\) 左右的矩阵运算。mav_trajectory_generation 用 Eigen 的固定大小矩阵（NTTP PolynomialOptimization<N>）让这些矩阵全部栈分配、编译器自动 SIMD 向量化——这是该库在嵌入式 CPU 上实时运行的关键。如果你在 Ch11 学的"固定 vs 动态大小矩阵"的选型原则这里会直接用上。
前向预告：本章的 Mellinger/Richter 方法固定了段时间 \(T_i\)，仅优化空间轨迹。下一章（D4）的 B 样条方法和 D5 的 MINCO 将进一步**把时间也变成优化变量**，实现时空联合优化——那是 T3 章"时空轨迹优化"在无人机领域的具体体现。现在只需要知道：固定时间是一个简化假设，它让问题变成凸 QP（好解），但代价是轨迹的时间分配可能不是最优的。
如果跳过本章会怎样¶
跳过 D3，你会卡在两个具体的地方。
场景一："轨迹太软，飞不快"。 你让无人机沿 10 个航路点飞过。你用简单的三次样条插值——轨迹经过了所有点，但速度分布极不均匀，拐角处要么剧烈震荡要么极度迟缓。根本原因是三次样条最小化的是加速度（二阶导），而四旋翼的执行器平滑性由 snap（四阶导）决定。没有 D3 的微分平坦论证，你不知道为什么应该最小化 snap 而非 jerk 或加速度，只能凭试错调参。
场景二："5 段以上就崩了"。 你实现了 Mellinger 的原始 QP——5 个航路点时工作完美，但 20 个航路点时 KKT 矩阵的条件数飙到 \(10^{15}\)，求解器吐出的轨迹在段接合处出现米级跳变。你知道"数值不稳定"但不知道根源（单项式基的 Vandermonde 矩阵条件数随 \(T^N\) 增长），也不知道 Richter 的端点导数变量替换如何从根本上消除这个问题。没有 D3 的完整推导，你无法在工程中定位和修复这类数值陷阱。
预计阅读时间¶
| 模式 | 时长 | 适合 |
|---|---|---|
| 精读 | 10-14 小时 | 第一次系统学多项式轨迹生成：逐节读动机→推导→代码，亲手实现 Richter 求解器，做完每节练习。建议分 4-5 次。 |
| 速读 | 3-4 小时 | 有优化基础、想建立全局图景：读每节的"动机"和"理论"小标题、关键公式（框住的）、§D3.8 完整工作流，跳过代码细节和数值实验。 |
| 速查 | 30-60 分钟 | 已学过、回来查特定公式：直接定位到对应小节，看 boxed 公式 + 符号表 + API 速查。 |
科研发展脉络¶
在钻进具体方法前，先把这条研究线的来龙去脉理清——知道每个方法"从哪来、解决了前人什么痛点、又留下什么给后人"，比孤立地记名字有用得多。
| 年份 | 论文 | Venue | 核心贡献 |
|---|---|---|---|
| 2011 | Mellinger & Kumar | ICRA Best Paper | 奠基作：微分平坦 → 最小 snap QP；degree-7 多项式；受约束 QP |
| 2013/2016 | Richter, Bry, Roy | ISRR / Springer | 变量替换消除数值病态：端点导数作决策变量；闭式 KKT；50+ 段稳定 |
| 2017 | Bahnemann 等 (ETH ASL) | ROS 包 | 标准 C++ 实现：Vertex→Segment→Trajectory 抽象；NTTP 模板 |
| 2018 | Gao, Wu, Lin, Shen (HKUST) | ICRA | Bernstein 基：凸包性质 → 安全走廊约束线性化 |
| 2020 | Wang 等 (ZJU FAST Lab) | RA-L | 交替最小化：空间-时间解耦优化；MINCO 前身 |
| 2021 | Wang 等 (ZJU FAST Lab) | ICRA | 百万段轨迹：解析梯度 + L-BFGS；4 秒生成 \(10^6\) 段 |
关键实验室脉络：UPenn KumarRobotics（Mellinger/Kumar 原创）→ MIT ACL（Richter/Bry 数值修复）→ ETH ASL（mav_trajectory_generation 工程化）→ HKUST（Btraj 凸包安全）→ ZJU FAST Lab（am_traj/MINCO 统一）。
看这条线，有一条清晰的主线：决策变量从"稠密的多项式系数"走向"稀疏的端点导数/航路点"，每一步都在减少变量数、改善数值性质、扩大可处理规模。本章讲前两步（Mellinger→Richter），D5 讲终点（MINCO）。
本章符号约定¶
| 符号 | 含义 | 首见 |
|---|---|---|
| \(M\) | 轨迹段数 | §D3.2 |
| \(N\) | 多项式阶数（degree），\(N = 2r - 1\) | §D3.2 |
| \(r\) | 优化导数阶次（\(r=4\) 为 snap） | §D3.1 |
| \(w_i\) | 第 \(i\) 个航路点（Waypoint）位置 | §D3.2 |
| \(T_i\) | 第 \(i\) 段的持续时间 | §D3.2 |
| \(c_i\) | 第 \(i\) 段的多项式系数向量 | §D3.2 |
| \(d_i\) | 第 \(i\) 段的端点导数向量 | §D3.3 |
| \(d_F, d_P\) | 固定/自由导数 | §D3.4 |
| \(Q_i\) | 第 \(i\) 段的 Hessian 矩阵 | §D3.3 |
| \(A_i\) | 第 \(i\) 段的端点映射矩阵 | §D3.2 |
| \(R_i\) | 第 \(i\) 段的端点代价矩阵 \(A_i^{-\top} Q_i A_i^{-1}\) | §D3.4 |
| \(R_{PP}, R_{FP}\) | 全局代价矩阵的分块 | §D3.4 |
| \(\sigma(t)\) | 平坦输出轨迹 | §D3.1 |
| \(B_{j,N}(\tau)\) | Bernstein 基函数 | §D3.5 |
| \(b_j\) | Bernstein 控制点 | §D3.5 |
§D3.1 微分平坦回顾：为什么要最小化 snap ⭐⭐¶
动机¶
假设你已经有了一串航路点——规划器告诉你无人机要依次经过 \((x_1, y_1, z_1), (x_2, y_2, z_2), \ldots\)。现在的问题是：用什么曲线把它们连起来？
最朴素的想法是直线段——但直线段在航路点处有折角，速度方向不连续，无人机必须在每个航路点停下来再转向。这显然不是你想要的。
你需要一条**光滑曲线**。但"光滑"是一个模糊的词——三次样条也光滑（\(C^2\) 连续），五次多项式更光滑（\(C^4\) 连续）。你应该追求哪种程度的光滑？更高阶的光滑是否一定更好？
这个问题的答案不是凭直觉决定的——它由四旋翼的物理特性唯一确定。微分平坦映射精确地告诉我们：轨迹的四阶导数（snap）直接映射到电机指令的变化率。最小化 snap 等价于最小化电机转速的变化率——让电机行为最平滑、机械振动最小。
平坦映射的完整代数链¶
给定一条 \(C^4\) 连续的平坦输出曲线 \(\sigma(t) = (x(t), y(t), z(t), \psi(t))^\top\)，全部 12 维状态和 4 维控制输入通过以下**纯代数运算**逐层恢复。
理解这条链的关键不是记住每一步的公式，而是看到**每一层需要多一阶导数**——这正是 snap 成为正确代价的数学原因。
第 0-1 层：位置和速度（需要 \(\sigma, \dot{\sigma}\)）
这两层是平凡的——位置和速度就是平坦输出本身及其一阶导数。
第 2 层：加速度 → 推力方向 → 姿态（需要 \(\ddot{\sigma}\)）
其中 \(y_C = (-\sin\psi, \cos\psi, 0)^\top\) 由偏航角构造。关键洞察：推力方向 \(z_B\) 完全由加速度决定——无人机必须"倾斜"到推力方向恰好抵消重力并提供所需加速度。这就是为什么四旋翼不能做"侧飞"——它的水平加速度必须靠倾斜产生。
第 3 层：jerk → 角速度（需要 \(\dddot{\sigma}\)）
其中 \(\omega_z\) 是偏航角速度 \(\dot{\psi}\) 在机体 \(z_B\) 轴上的投影：悬停时 \(z_B = e_3\)，\(\omega_z = \dot{\psi}\)；机体倾斜角 \(\theta\) 时 \(\omega_z = \dot{\psi}\cos\theta\)。注意**不要**写成 \(\dot{\psi}\,(e_3\cdot z_B)/\lVert e_3\times z_B\rVert\) 这类形式——分母在悬停（\(z_B \parallel e_3\)）时趋零，会让 \(\omega_z\) 爆炸。
Jerk 是加速度的变化率——加速度变化了，推力方向就要转，转的速率就是角速度。如果 jerk 不连续，角速度会跳变——力矩会出现脉冲。
第 4 层：snap → 角加速度 → 力矩 → 电机转速（需要 \(\sigma^{(4)}\)）
最后通过混合矩阵（mixer）从 \((f, \tau)\) 解出四个电机转速 \(\omega_i^2\)。
阶段小结：到这里我们看到了完整的代数链。关键观察是：每"上"一层就需要多一阶导数——位置(0阶)→速度(1阶)→姿态(2阶)→角速度(3阶)→角加速度/力矩/电机(4阶)。所以 snap（4阶导数）是到达电机指令所需的最低阶导数。
为什么 snap 是正确的代价：严格论证¶
命题：在四旋翼的微分平坦映射下，最小化 \(\int_0^T \|\sigma^{(4)}(t)\|^2 \, dt\) 等价于最小化电机角加速度指令的加权 \(L^2\) 范数（在悬停近似下）。
论证：
-
电机转速 \(\omega_i^2\) 由 \((\sigma, \dot{\sigma}, \ddot{\sigma}, \dddot{\sigma}, \sigma^{(4)})\) 的代数函数决定——这是平坦映射的直接推论。
-
在悬停附近线性化：设 \(a \approx 0\), \(\omega \approx 0\), 则 \(f \approx mg\), \(R \approx I\)。此时：
其中 \(s_\perp = (s_x, s_y)\) 是 snap 的水平分量，\(s_z\) 是竖直分量。
-
力矩 \(\tau \approx J\alpha\)，而电机转速变化率 \(\dot{\omega}_i^2 \propto \dot{\tau}\)——其中 \(\dot{\tau}\) 包含 \(\alpha\)，即包含 snap。
-
因此，最小化 snap 的 \(L^2\) 范数，在悬停近似下等价于最小化电机指令的变化率——对应最平滑的电机行为。
为什么不是 jerk 或 acceleration？ 这不是"越高阶越好"——每升一阶导数，多项式的阶数就要增加 2，计算量和数值问题都会加重。选择哪一阶取决于"你想让什么物理量平滑"：
| 最小化阶次 | 物理含义 | 效果 | 多项式阶数 |
|---|---|---|---|
| \(r=2\)：最小加速度 | 最小推力变化 | 轨迹倾向直线，缺乏灵活性 | 3 (cubic) |
| \(r=3\)：最小 jerk | 最小角速度变化 | 轨迹较平滑，但角加速度不连续→力矩跳变 | 5 (quintic) |
| \(r=4\)：最小 snap | 最小角加速度/力矩变化 | 电机平滑，机械振动最小 | 7 (septic) |
| \(r=5\)：最小 crackle | 最小电机加加速度 | 过度平滑，多项式阶数过高 | 9 (nonic) |
本质洞察：\(r=4\)（最小 snap）是平滑性与多项式阶数之间的最佳折衷——它恰好覆盖到电机指令这一层，再高一阶（\(r=5\)）带来的平滑性改善微乎其微，但多项式阶数从 7 跳到 9，数值问题显著加重。Mellinger & Kumar 2011 选择此代价并非任意，而是微分平坦映射层级结构的直接产物。
最小 snap 轨迹的几何直觉¶
如果你在纸上画一条最小 snap 轨迹，它看起来是什么样？
- 局部接近匀速直线运动：snap 为零意味着 jerk 恒定、加速度线性变化、速度抛物线变化——接近于匀变速
- 拐弯处的"内切"行为：在航路点处轨迹自动取"最自然"的过渡，类似于人手画出的光滑曲线——不是锐角拐弯，也不是圆弧，而是一个让四阶导数积分最小的"最柔"过渡
- 首尾衰减：起止处由边界条件（速度/加速度为零）强制，轨迹从静止平滑加速，到终点平滑减速
这种行为可以用弹性力学类比理解：最小 snap 轨迹就像一根弹性梁——两端固定（起止点），中间被一系列支点（航路点）约束，梁自动取弯曲能最小的形状。弹性梁的弯曲能正好是四阶导数的积分（Euler-Bernoulli 梁理论）。但这个类比的边界在于：弹性梁是空间曲线，而轨迹还有时间维度——不同段的"时间长度"\(T_i\) 会影响"弹性"的分布。
⚠️ 常见陷阱¶
编程陷阱：混淆 snap 与 jerk
错误做法：实现了最小 jerk（r=3）却以为是最小 snap
现象：轨迹看起来"还行"，但力矩曲线在段接合处有尖刺
根本原因：r=3 只保证角速度连续，不保证角加速度连续；
段接合处 jerk 连续但 snap 可能跳变 → 力矩跳变
正确做法：确认 r=4，多项式阶数 N=7（不是 N=5）
自检方法：绘制 snap 曲线（四阶导数），检查段接合处是否连续
概念误区："snap 越小越好，所以应该用更高阶"
错误理解：既然最小 snap 好，那最小 crackle（r=5）岂不更好
现象：用 r=5 时多项式阶数升到 9，系数数从 8 增到 10，
Q 矩阵条件数恶化，5 段以上就数值不稳定
根本原因：微分平坦映射在第 4 层（snap）就到达了电机指令；
第 5 阶导数对电机平滑性的边际贡献远小于数值代价
正确理解：r=4 是物理意义与计算可行性的最佳折衷，不是"越高越好"
练习¶
练习 D3.1.1（实现题，⭐⭐）：用 Python 实现微分平坦的完整逆映射函数 flat_output_to_state_and_input(pos, vel, acc, jerk, snap, yaw, yaw_dot)。输入一条已知的悬停轨迹 \(p(t) = (0, 0, 1)\)（零速零加速），验证输出推力 \(f = mg\)、姿态 \(R = I\)、角速度 \(\omega = 0\)。然后输入一条匀速直线 \(p(t) = (t, 0, 1)\)，验证推力和姿态的变化。
练习 D3.1.2（思考题，⭐⭐⭐）：如果四旋翼的微分平坦映射只需要到 jerk（三阶导数）就能恢复电机指令——比如某种假想的飞行器——那么最优代价应该是最小化什么？多项式阶数应该选多少？
§D3.2 分段多项式表示 ⭐⭐¶
动机¶
§D3.1 回答了"优化什么"（snap 积分）。现在的问题是：用什么数学对象来表示轨迹？
我们需要一种表示方法，同时满足三个要求：(a) 足够灵活以经过任意航路点；(b) 导数容易计算（因为代价涉及四阶导数）；(c) 代价函数有解析形式（最好是二次型，这样问题是凸 QP）。
分段多项式完美地满足这三个要求：每段是一个多项式（导数就是降阶多项式），snap 代价对系数是二次型（积分多项式的平方仍是多项式，积分有解析公式），相邻段通过端点约束连接。
如果不用多项式，你能用什么？三角函数？——导数计算复杂且代价无闭式。神经网络？——导数计算昂贵且优化非凸。样条？——样条本质上就是分段多项式加上特定的连续性条件。所以分段多项式不是"恰好被选中"，而是**这类问题的天然表示**。
问题设定¶
给定 \(M+1\) 个有序航路点（Waypoints）：
和对应的时间分配（Time Allocation）：
其中 \(T_i\) 是第 \(i\) 段的持续时间。第 \(i\) 段使用**局部时间参数化**：\(t \in [0, T_i]\)。
目标：构造一条分段多项式轨迹 \(\sigma(t)\)，使得： 1. 轨迹经过所有航路点 2. 轨迹在航路点处足够光滑（连续性约束） 3. snap 积分最小
单段多项式的参数化¶
第 \(i\) 段轨迹用 \(N\) 阶多项式（degree \(N\)，\(N+1\) 个系数）表示：
其中 \(\phi(t) = (1, t, t^2, \ldots, t^N)^\top \in \mathbb{R}^{N+1}\) 称为**单项式基向量**（Monomial Basis Vector），\(c_i = (c_{i,0}, c_{i,1}, \ldots, c_{i,N})^\top\) 是系数向量。
多项式阶数的选择¶
关键原则：对于优化第 \(r\) 阶导数，每段多项式的阶数至少为 \(N = 2r - 1\)。
推导：在每段的两个端点（起点和终点），我们需要约束从第 0 阶到第 \((r-1)\) 阶的导数值——这样才能保证相邻段在接合处的导数连续到第 \((r-1)\) 阶。每个端点有 \(r\) 个约束条件，两个端点共 \(2r\) 个条件。\(N\) 阶多项式有 \(N+1\) 个系数（自由度），要满足 \(2r\) 个等式约束，需要 \(N + 1 \geq 2r\)，即 \(N \geq 2r - 1\)。
取等号 \(N = 2r - 1\) 时，自由度恰好被端点约束用完——多项式由端点导数唯一确定。这是"最经济"的选择。
| 优化目标 | \(r\) | 最小阶数 \(N=2r-1\) | 系数数 \(N+1\) |
|---|---|---|---|
| 最小加速度 | 2 | 3 (cubic) | 4 |
| 最小 jerk | 3 | 5 (quintic) | 6 |
| 最小 snap | 4 | 7 (septic) | 8 |
| 最小 crackle | 5 | 9 (nonic) | 10 |
阶段小结：对 \(r=4\)（最小 snap），我们使用 degree-7 多项式——每段 8 个系数。这 8 个系数恰好由起点和终点各 4 个导数值（位置、速度、加速度、jerk）唯一确定。
各阶导数的显式表达¶
第 \(i\) 段的 \(k\) 阶导数：
以 \(N=7\), \(r=4\) 为例，展开：
注意阶乘系数的快速增长：\(c_4\) 前面是 \(4! = 24\)，\(c_7\) 前面是 \(\frac{7!}{3!} = 840\)。这些阶乘系数在后面的 \(Q\) 矩阵中会反复出现。
端点求值矩阵 \(A_i\)¶
对第 \(i\) 段，在起点 \(t=0\) 和终点 \(t=T_i\) 处求值各阶导数，可以用一个映射矩阵 \(A_i\) 表示：
其中 \(d_i\) 是端点导数向量（\(2r\) 维），排列为起点的 \(r\) 个导数在前、终点的 \(r\) 个导数在后：
\(A_i\) 的上半部分（\(t=0\) 处求值）极其简单——\(t=0\) 使得所有 \(t^j = 0\)（除 \(j=0\)），所以上半部分是一个下三角矩阵，对角线元素为 \(k!\)：
下半部分（\(t=T_i\) 处求值）含 \(T_i\) 的幂次——这正是数值病态的根源（§D3.3.8 会详细分析）。
\(A_i\) 的通用元素公式：
其中 \(t_* = 0\) 对应上半部分，\(t_* = T_i\) 对应下半部分。
性质：\(A_i\) 是一个 Vandermonde 型矩阵，对合理的 \(T_i > 0\) 必然可逆，因此：
即多项式系数可由端点导数唯一确定——这是 Richter 变量替换（§D3.4）的数学基础。
⚠️ 常见陷阱¶
概念误区：认为多项式阶数可以任意选择
错误理解："degree-9 比 degree-7 更灵活，所以总是更好"
现象：用 degree-9 + r=4 时多出 2 个自由度（10 系数 - 8 约束 = 2 自由），
优化问题变成欠定的——需要额外约束或正则化，否则解不唯一
根本原因：N = 2r-1 是恰好让端点约束用完自由度的"经济"选择；
N > 2r-1 引入额外自由度，可用于走廊约束但增加实现复杂性
正确做法：初学者先用 N = 2r-1 = 7；需要走廊约束时再考虑更高阶
编程陷阱：局部时间 vs 全局时间的混淆
错误做法：用全局时间 t_global 直接代入段多项式求值
现象：第 2 段的起点位置 σ_2(t_global) 不等于 w_1
根本原因：每段多项式用局部时间 t ∈ [0, T_i] 参数化；
全局时间 t_global 需要先减去前面所有段的时间才能得到局部时间
正确做法：t_local = t_global - sum(T_1, ..., T_{i-1})
练习¶
练习 D3.2.1（手推题，⭐⭐）：对 \(r=4\), \(N=7\), \(T=2\) 秒，手动写出 \(A_i\) 的完整 \(8 \times 8\) 矩阵（数值），并用 Python 验证 \(A_i\) 的条件数。与 \(T=1\) 的情况对比。
练习 D3.2.2（实现题，⭐⭐）：实现函数 build_A(T, N, r) 构造端点映射矩阵，并验证 \(A_i \cdot A_i^{-1} = I\) 的残差在机器精度内。
§D3.3 Mellinger QP：受约束最小 snap 公式 ⭐⭐⭐¶
动机¶
§D3.1 确定了代价（最小 snap），§D3.2 确定了表示（分段多项式）。现在把它们组装成一个可以求解的优化问题。
Mellinger & Kumar 2011 的核心贡献是**把多项式轨迹生成写成一个等式约束的凸 QP**——这意味着问题有全局最优解，且可以用标准 QP 求解器一步求出。
但这个"一步求出"在数学上是完美的，在数值上却有致命缺陷——§D3.3.8 会揭示这个缺陷，§D3.4 会修复它。先理解 Mellinger 的公式，才能理解 Richter 修复了什么。
代价函数的矩阵形式¶
目标：最小化所有段的 \(r\) 阶导数平方积分之和：
对单维度问题，先推导一段的代价。将 \(\sigma_i^{(r)}(t) = \phi^{(r)}(t)^\top c_i\) 代入：
因此每段的代价为二次型：
其中 \(Q_i \in \mathbb{R}^{(N+1) \times (N+1)}\) 是该段的 Hessian 矩阵。
Hessian 矩阵 \(Q_i\) 的解析元素公式¶
\(Q_i\) 的第 \((l, m)\) 个元素（\(l, m \in \{0, 1, \ldots, N\}\)）：
关键性质： - \(Q_i\) 对称：\([Q_i]_{l,m} = [Q_i]_{m,l}\) - \(Q_i\) 半正定：它是积分的 Gram 矩阵 - 前 \(r\) 行和前 \(r\) 列全为零：低阶系数对 \(r\) 阶导数无贡献 - \(Q_i\) 依赖于段时间 \(T_i\)
以 \(r=4\), \(N=7\), \(T=1\) 为例的显式数值：
可以验证：\([Q]_{4,4} = \frac{4!}{0!} \cdot \frac{4!}{0!} \cdot \frac{1}{1} = 576\)，\([Q]_{7,7} = \frac{7!}{3!} \cdot \frac{7!}{3!} \cdot \frac{1}{7} = 100800\)。
全局代价函数：块对角 Hessian¶
将所有 \(M\) 段的系数拼接为一个长向量 \(c = (c_1^\top, \ldots, c_M^\top)^\top \in \mathbb{R}^{M(N+1)}\)，总代价：
\(Q\) 是**块对角**矩阵——各段之间通过代价函数不耦合。耦合仅通过约束引入。 这个结构很重要：它意味着如果没有约束，每段可以独立优化（但独立优化的结果不保证段间连续）。
等式约束的分类与计数¶
约束分三类，每类的物理含义不同：
类型 1：航路点位置约束 —— "轨迹必须经过指定位置"
相邻段共享端点，消去重复后实际有 \(M+1\) 个独立位置约束。
类型 2：连续性约束 —— "相邻段在接合处的导数必须匹配"
对 \(r=4\)：\(3(M-1)\) 个约束（速度、加速度、jerk 连续）。
类型 3：边界约束 —— "起止点的导数值给定"
对 \(r=4\)：6 个约束（起止点的速度、加速度、jerk 各指定，通常为零表示从静止出发到静止停止）。
约束总数（\(r=4\), \(N=7\)）：\((M+1) + 3(M-1) + 6 = 4M + 4\)
总变量数：\(8M\)
自由度：\(8M - (4M+4) = 4(M-1)\)——这些自由度恰好对应 \(M-1\) 个内部航路点各自的速度、加速度、jerk（每个 3 个自由导数），加上 1 个由内部位置约束冗余度产生的额外自由度。
KKT 系统与闭式解¶
所有约束可以统一写成 \(A_{\text{eq}} c = b_{\text{eq}}\)。Mellinger 方法的 QP 形式：
KKT 条件：
这是一个**鞍点系统**（Saddle-Point System），可用 \(LDL^\top\) 分解求解。Mellinger 的实际做法是调用通用 QP 求解器（如 MATLAB 的 quadprog）——在 \(M \leq 10\) 时完全可行。
数值病态问题：Mellinger 方法的阿喀琉斯之踵¶
问题核心：单项式基 \(\{1, t, t^2, \ldots, t^7\}\) 在 \(T_i > 1\) 秒时导致映射矩阵 \(A_i\) 的条件数急剧增长。
考虑终点求值矩阵的第一行 \((1, T, T^2, \ldots, T^7)\)。当 \(T = 5\) 秒时：
跨越 5 个数量级。\(A_i\) 的条件数 \(\kappa(A_i) \sim T^N\)，\(Q\) 矩阵的条件数更高（\(\sim T^{2N}\)）。多段拼接后，全局 KKT 矩阵的条件数可达 \(10^{15}\)——64 位浮点数的 ~16 位有效精度几乎被完全"吃掉"。
实际后果：5 段以上时结果开始不可靠，20 段以上时完全崩溃——段接合处的连续性残差可达米级。
本质洞察：数值病态的根源**不是 QP 公式本身**，而是**决策变量的选择**。多项式系数 \(c_{i,j}\) 的物理量级跨度极大（\(c_0 \sim O(1)\), \(c_7 \sim O(T^{-7})\)），导致 Hessian 矩阵的特征值跨度同样巨大。如果换一组量级相近的决策变量——比如端点导数（位置米，速度米/秒，加速度米/秒\(^2\)，jerk米/秒\(^3\)）——条件数问题就会消失。这正是 Richter 方法的核心思想。
"""验证条件数随段时间的增长"""
import numpy as np
from math import factorial
def build_Q(T: float, N: int, r: int) -> np.ndarray:
n = N + 1
Q = np.zeros((n, n))
for l in range(r, n):
for m in range(l, n):
cl = factorial(l) / factorial(l - r)
cm = factorial(m) / factorial(m - r)
val = cl * cm * T**(l + m - 2*r + 1) / (l + m - 2*r + 1)
Q[l, m] = val
Q[m, l] = val
return Q
def build_A(T: float, N: int, r: int) -> np.ndarray:
n = N + 1
A = np.zeros((2 * r, n))
for k in range(r):
for j in range(k, n):
coeff = factorial(j) / factorial(j - k)
if j == k:
A[k, j] = coeff
A[r + k, j] = coeff * T**(j - k)
return A
# 测试
for T in [1.0, 2.0, 5.0, 10.0]:
A = build_A(T, 7, 4)
Q = build_Q(T, 7, 4)
eig = np.linalg.eigvalsh(Q)
eig_nz = eig[eig > 1e-12]
cond_Q = eig_nz[-1] / eig_nz[0] if len(eig_nz) > 1 else 1
print(f"T={T:5.1f}s: cond(A)={np.linalg.cond(A):.2e}, cond(Q)={cond_Q:.2e}")
预期输出：
T= 1.0s: cond(A)=2.40e+02, cond(Q)=3.67e+04
T= 2.0s: cond(A)=3.07e+03, cond(Q)=6.01e+06
T= 5.0s: cond(A)=2.44e+05, cond(Q)=3.82e+10
T= 10.0s: cond(A)=3.12e+07, cond(Q)=6.24e+14
⚠️ 常见陷阱¶
编程陷阱：Q 矩阵中的阶乘溢出
错误做法：用 int 类型计算阶乘 factorial(7) * factorial(7) = 25401600
现象：Python 不会溢出（任意精度），但 C++ 的 int 在 13! 就溢出
根本原因：Q 元素公式中含 l! * m!，对 l=m=7 已达 ~2.5e7
正确做法：C++ 中用 double 计算阶乘，或预计算阶乘表避免重复计算
思维陷阱：认为条件数大就"不能用"
错误理解："条件数 10^4 就不稳定了"
现实情况：条件数 10^4 只是"吃掉" 4 位精度，64 位浮点还剩 ~12 位
对单段 Q 矩阵，cond ~ 10^4 完全可接受
问题是多段拼装后 KKT 矩阵条件数 *乘积* 性增长
正确理解：单段没问题，拼 20 段以上才会崩——
Mellinger 论文中的实验确实只用了 ≤10 段
练习¶
练习 D3.3.1（手推题，⭐⭐⭐）：对 \(r=4\), \(N=7\), \(T_i=1\)，手动验证 \([Q_i]_{4,5} = 1440\)、\([Q_i]_{6,7} = 50400\)。写出推导过程中的每一步。
练习 D3.3.2（实现题，⭐⭐⭐）：用 Python 从零实现 Mellinger 的受约束 QP——构造全局 \(Q\)、约束矩阵 \(A_{\text{eq}}\)、KKT 系统，用 np.linalg.solve 求解。对 \(M = 3, 5, 10, 20\) 段分别测试，记录 KKT 矩阵条件数和连续性残差，验证 20 段时的数值崩溃。
§D3.4 Richter 无约束化：端点导数变量替换 ⭐⭐⭐¶
动机¶
§D3.3 的 Mellinger QP 在数学上完美——凸 QP、全局最优、闭式 KKT。但在数值上 20 段就崩了。
Richter, Bry & Roy 2013/2016 的关键洞见可以用一句话概括：
不以多项式系数 \(c\) 为决策变量，改以端点导数 \(d\) 为决策变量。
这个变量替换同时解决了两个问题：(a) 约束自动消失——因为连续性约束变成了"相邻段共享同一个变量"；(b) 数值稳定——因为端点导数是物理量（位置米，速度米/秒），量级相近。
如果 Mellinger 是"先列方程、再解约束 QP"，那么 Richter 是"把约束吸收进变量定义，直接解无约束二次型"。约束没有被去掉，而是被"编织"进了变量本身。
单段变量替换：从 \(c_i\) 到 \(d_i\)¶
回顾 §D3.2 的映射关系 \(d_i = A_i c_i\)，即 \(c_i = A_i^{-1} d_i\)。将其代入代价函数：
定义端点代价矩阵：
\(R_i\) 的物理含义：在端点导数空间中衡量 snap 代价的 Hessian。
关键优势：\(R_i\) 是 \(2r \times 2r\) 矩阵（对 \(r=4\) 为 \(8 \times 8\)），其决策变量是端点的 position/velocity/acceleration/jerk 值——量级相近（位置米，速度米/秒，...），条件数远小于 \(Q_i\)。
阶段小结：到目前为止，我们完成了单段的变量替换：从 \(c_i^\top Q_i c_i\) 变成了 \(d_i^\top R_i d_i\)。代价函数的"长相"没变（仍然是二次型），变的是自变量——从"量级悬殊的系数"变成了"量级相近的导数值"。下一步是处理多段的拼接。
多段的端点导数拼接¶
对 \(M\) 段轨迹，每段有 \(2r\) 个端点导数，总共 \(2rM\) 个。但相邻段共享端点——第 \(i\) 段的终点就是第 \(i+1\) 段的起点——所以连续性约束被自动满足。实际独立的端点导数数量为：
对 \(r=4\)：\(n_d = 4(M+1)\)，对应 \(M+1\) 个航路点各自的 position, velocity, acceleration, jerk。
定义全局端点导数向量 \(d \in \mathbb{R}^{n_d}\)：
连续性约束的自动满足：第 \(i\) 段的终点 jerk 和第 \(i+1\) 段的起点 jerk 是**同一个变量** \(d_i^{(3)}\)——不需要写约束来强制相等，因为它们本来就是一个东西。
全局代价矩阵 \(R\)¶
通过选择矩阵 \(C_i\)（0-1 矩阵）将全局 \(d\) 映射到各段 \(d_i\)，得到：
\(R\) 的结构是**块三对角**的——因为每段只涉及相邻两个航路点的导数。这个带状结构意味着求解 \(R\) 的线性方程组只需 \(O(Mr^3)\) 时间——线性于段数。
固定/自由导数分割¶
将全局导数 \(d\) 分为两部分：
- \(d_F\)（fixed，固定导数）：已知值，不参与优化
- 所有航路点的**位置**：\(d_0^{(0)}, d_1^{(0)}, \ldots, d_M^{(0)}\)（给定的航路点位置）
- 起点的高阶导数：\(d_0^{(1)}, d_0^{(2)}, d_0^{(3)}\)（通常为零 = 从静止出发）
- 终点的高阶导数：\(d_M^{(1)}, d_M^{(2)}, d_M^{(3)}\)（通常为零 = 到静止停止）
共 \(n_F = (M+1) + 2(r-1) = M + 2r - 1\) 个（对 \(r=4\)：\(M + 7\)）
- \(d_P\)（free/pending，自由导数）：待优化的未知量
- 内部航路点（\(j = 1, \ldots, M-1\)）的**高阶导数**：每个点的 velocity, acceleration, jerk
共 \(n_P = (r-1)(M-1) = 3(M-1)\) 个（对 \(r=4\)）
验证：\(n_F + n_P = (M+7) + 3(M-1) = 4M + 4 = 4(M+1) = n_d\)。
无约束 QP 与闭式最优解¶
用置换矩阵重排 \(d\)，使 \(d_F\) 在前、\(d_P\) 在后：
\(d_F\) 已知固定，优化问题退化为关于 \(d_P\) 的**无约束二次型**：
对 \(d_P\) 求梯度令其为零：
这就是 Richter 的核心结果。 整个最小 snap 轨迹的最优解通过一次矩阵求逆（或等价地，一次线性方程组求解）获得。没有约束、没有迭代、没有求解器——一个公式搞定。
物理解读¶
\(d_P^* = -R_{PP}^{-1} R_{FP}^\top d_F\) 的物理含义值得仔细品味：
- \(R_{PP}\)：自由导数之间的"自代价"——内部航路点的速度/加速度/jerk 对 snap 积分的贡献
- \(R_{FP}\)：固定导数对自由导数的"交叉代价"——航路点位置和边界条件对内部导数的影响
- \(-R_{PP}^{-1} R_{FP}^\top\)：最优的自由导数是固定导数的**线性函数**——每个内部航路点的速度/加速度/jerk 取让整体 snap 最小的值
本质洞察：想象一串珠子（航路点）固定在空间中，连接它们的弹性线（多项式轨迹）会自动找到弹性势能（snap 积分）最小的形状——内部导数就是弹性线在珠子处的"斜率"和"曲率"，由弹性力学（二次型 \(R\)）决定。这个类比在"弹性梁的弯曲能 = 四阶导数积分"这一层是精确的，但在"弹性梁没有时间维度"这一点上失效——不同段的时间 \(T_i\) 会影响各段的"刚度"。
数值稳定性分析¶
| 指标 | Mellinger（\(c\) 为变量） | Richter（\(d_P\) 为变量） |
|---|---|---|
| 决策变量 | 多项式系数 \(c_{i,j}\) | 端点导数 \(d_j^{(k)}\) |
| 变量量级 | \(c_0 \sim O(1)\), \(c_7 \sim O(T^{-7})\) | \(d^{(0)} \sim O(1)\), \(d^{(3)} \sim O(1)\) |
| 量级跨度 | \(\sim T^N\) | \(\sim 1\) |
| \(\kappa(Q)\) 或 \(\kappa(R_{PP})\) | \(O(T^{2N})\) | \(O(1) \sim O(10^2)\) |
| 段数上限 | \(M \leq 10\) | \(M \leq 50+\) |
\(R_{PP}\) 的条件数之所以小，是因为自由导数都是"物理量"，它们的量级由飞行场景自然决定，不会像多项式系数那样出现指数级差异。
完整 Python 实现¶
"""
Richter 无约束最小 snap 轨迹生成的完整 Python 实现
参考: Richter, Bry, Roy, ISRR 2013 / Springer 2016.
"""
import numpy as np
from math import factorial
from typing import List, Tuple
# ============================================================
# 基础矩阵构造
# ============================================================
def build_Q(T: float, N: int, r: int) -> np.ndarray:
"""构造单段 Hessian 矩阵 Q_i."""
n = N + 1
Q = np.zeros((n, n))
for l in range(r, n):
for m in range(l, n):
cl = factorial(l) / factorial(l - r)
cm = factorial(m) / factorial(m - r)
val = cl * cm * T**(l + m - 2*r + 1) / (l + m - 2*r + 1)
Q[l, m] = val
Q[m, l] = val
return Q
def build_A(T: float, N: int, r: int) -> np.ndarray:
"""构造单段端点映射矩阵 A_i."""
n = N + 1
A = np.zeros((2 * r, n))
for k in range(r):
for j in range(k, n):
coeff = factorial(j) / factorial(j - k)
if j == k:
A[k, j] = coeff
A[r + k, j] = coeff * T**(j - k)
return A
def build_Ri(T: float, N: int, r: int) -> np.ndarray:
"""构造单段端点代价矩阵 R_i = A_i^{-T} Q_i A_i^{-1}."""
Q = build_Q(T, N, r)
A = build_A(T, N, r)
A_inv = np.linalg.inv(A)
return A_inv.T @ Q @ A_inv
# ============================================================
# 全局矩阵组装与求解
# ============================================================
def build_global_R(times: List[float], N: int, r: int) -> np.ndarray:
"""组装全局代价矩阵 R."""
M = len(times)
n_d = r * (M + 1)
R = np.zeros((n_d, n_d))
for i in range(M):
Ri = build_Ri(times[i], N, r)
s = r * i
e = r * (i + 1)
R[s:s+r, s:s+r] += Ri[:r, :r]
R[s:s+r, e:e+r] += Ri[:r, r:]
R[e:e+r, s:s+r] += Ri[r:, :r]
R[e:e+r, e:e+r] += Ri[r:, r:]
return R
def solve_min_snap_richter(
waypoints: np.ndarray, times: List[float],
r: int = 4, bc_start: dict = None, bc_end: dict = None,
) -> Tuple[np.ndarray, List[np.ndarray]]:
"""Richter 闭式最小 snap 求解器（单维度）."""
M = len(times)
N = 2 * r - 1
n_d = r * (M + 1)
if bc_start is None:
bc_start = {k: 0.0 for k in range(1, r)}
if bc_end is None:
bc_end = {k: 0.0 for k in range(1, r)}
R = build_global_R(times, N, r)
# 固定/自由索引
fixed_idx, fixed_val = [], []
for j in range(M + 1):
fixed_idx.append(r * j)
fixed_val.append(waypoints[j])
for k in range(1, r):
fixed_idx.append(k)
fixed_val.append(bc_start.get(k, 0.0))
fixed_idx.append(r * M + k)
fixed_val.append(bc_end.get(k, 0.0))
fixed_idx_sorted = sorted(set(fixed_idx))
free_idx = sorted(set(range(n_d)) - set(fixed_idx_sorted))
# 构建 d_F
d_F = np.zeros(len(fixed_idx_sorted))
for k, idx in enumerate(fixed_idx_sorted):
pos = [i for i, fi in enumerate(fixed_idx) if fi == idx]
d_F[k] = fixed_val[pos[0]]
R_PP = R[np.ix_(free_idx, free_idx)]
R_FP = R[np.ix_(fixed_idx_sorted, free_idx)]
# 闭式求解
d_P_opt = -np.linalg.solve(R_PP, R_FP.T @ d_F)
# 组装完整 d
d_opt = np.zeros(n_d)
for k, idx in enumerate(fixed_idx_sorted):
d_opt[idx] = d_F[k]
for k, idx in enumerate(free_idx):
d_opt[idx] = d_P_opt[k]
# 恢复多项式系数
coeffs = []
for i in range(M):
A_i = build_A(times[i], N, r)
d_i = np.zeros(2 * r)
d_i[:r] = d_opt[r*i : r*(i+1)]
d_i[r:] = d_opt[r*(i+1) : r*(i+2)]
c_i = np.linalg.solve(A_i, d_i)
coeffs.append(c_i)
return d_opt, coeffs
⚠️ 常见陷阱¶
编程陷阱：用 np.linalg.inv 而非 np.linalg.solve
错误做法：d_P_opt = -np.linalg.inv(R_PP) @ R_FP.T @ d_F
现象：结果相同，但速度慢一倍、数值精度略差
根本原因：inv 先计算整个逆矩阵再乘——O(n^3) + O(n^2)；
solve 直接分解后回代——O(n^3)，常数更小且不累积误差
正确做法：d_P_opt = -np.linalg.solve(R_PP, R_FP.T @ d_F)
更好做法：利用 R_PP 对称正定，用 Cholesky 分解：
from scipy.linalg import cho_solve, cho_factor
L, low = cho_factor(R_PP)
d_P_opt = -cho_solve((L, low), R_FP.T @ d_F)
概念误区：认为 Richter 方法"没有约束"
错误理解："Richter 是无约束优化，所以没有强制连续性"
现实情况：约束没有消失，而是被吸收进了变量定义——
相邻段共享同一个导数变量，连续性是自动满足的
航路点位置被固定在 d_F 中，不参与优化
正确理解：Richter 的无约束是"从优化器的角度无约束"，
物理约束全部被编码在变量的组织方式中
思维陷阱：认为 Richter 总是比 Mellinger 好
错误理解："Richter 方法全面优于 Mellinger，后者已过时"
反例：Mellinger 的约束 QP 框架可以直接加入不等式约束
（如安全走廊、速度上界），而 Richter 的无约束公式不行——
加约束就不再是无约束 QP 了
正确理解：两者适用场景不同。纯等式约束用 Richter（稳定）；
需要不等式约束用 Mellinger 框架或 Bernstein 基（§D3.5）
练习¶
练习 D3.4.1（实现题，⭐⭐⭐）：在同一组 5 个航路点上，分别用 Mellinger 和 Richter 方法求解，对比 Hessian/KKT 条件数、连续性残差、计算时间。段数从 3 增加到 50，绘制三个指标的变化曲线。
练习 D3.4.2（调试挑战，⭐⭐⭐）：以下代码有 bug——fixed_idx 的构造中，起点的高阶导数索引和第 0 个航路点的位置索引发生了重叠。找到 bug 并修复。
# 有 bug 的代码片段
fixed_idx = []
for j in range(M + 1):
fixed_idx.append(r * j) # 位置
for k in range(1, r):
fixed_idx.append(k) # 起点 vel/acc/jerk — 与位置重叠？
fixed_idx.append(r * M + k) # 终点 vel/acc/jerk
§D3.5 Bernstein 基与安全走廊 ⭐⭐⭐¶
动机¶
§D3.4 的 Richter 方法解决了数值稳定性问题，但它有一个重要的局限：不支持空间约束。现实中无人机必须避开障碍物——你需要一种方法来表达"轨迹必须在某个安全区域内"。
"轨迹必须在区域内"是一个**无穷维约束**——曲线上的每一个点（无穷多个）都必须满足。这看起来无法处理，但 Bernstein 基的凸包性质提供了一个优雅的解法：只要控制点在区域内，整条曲线就一定在区域内——无穷维约束退化为有限维约束。
Bernstein 基函数¶
Bernstein 基函数定义在 \([0, 1]\) 上：
多项式在 Bernstein 基下的表示：
其中 \(b_j\) 称为**控制点**（Control Points）。
核心性质：凸包性质（Convex Hull Property）¶
定理：对任意 \(\tau \in [0, 1]\)，\(\sigma(\tau) \in \text{conv}\{b_0, b_1, \ldots, b_N\}\)。
证明：Bernstein 基函数满足：(1) 非负性 \(B_{j,N}(\tau) \geq 0\)；(2) 归一性 \(\sum_{j=0}^{N} B_{j,N}(\tau) = 1\)。因此 \(\sigma(\tau)\) 是控制点的**凸组合**，必然在凸包内。\(\square\)
安全约束的线性化：若要求轨迹在凸多面体 \(\mathcal{P} = \{x : Hx \leq h\}\) 内，只需：
这是 \(N+1\) 个线性不等式约束，可以直接加入 QP。无穷维的"每个时刻都安全"被退化为有限个"每个控制点都安全"。
本质洞察：凸包性质的本质是"控制点包住了曲线"——管住有限个控制点就管住了曲线上无穷多的点。这和 Bezier 曲线在计算机图形学中的应用是同一个原理。但这个性质的代价是**保守性**——控制点的凸包通常比曲线本身大，所以约束比实际需要的更严格。如果走廊太窄，保守性可能导致无解。
Bernstein 基与单项式基的对比¶
| 特性 | 单项式基 | Bernstein 基 | 端点导数（Richter） |
|---|---|---|---|
| 数值稳定性 | 差（\(\kappa \sim T^{2N}\)） | 中等 | 好（\(\kappa \sim O(1)\)） |
| 安全约束 | 困难（非线性） | 容易（线性） | 困难（需转换） |
| 导数计算 | 简单 | 中等（差分） | 直接 |
| 适用场景 | 理论推导 | 安全走廊约束 | 数值稳定的无约束优化 |
三种基不是"谁更好"，而是各有适用场景。工程中常见的做法是：用 Richter 方法求解无约束问题（快且稳定），如果需要安全走廊约束则切换到 Bernstein 基或将端点导数转换为控制点后添加约束。
Bernstein 基的导数性质¶
Bernstein 多项式的 \(k\) 阶导数仍然是 Bernstein 多项式，阶数降为 \(N-k\)：
其中 \(\Delta^k b_j\) 是控制点的 \(k\) 阶前向差分：\(\Delta^1 b_j = b_{j+1} - b_j\)。
推论：导数曲线的凸包由差分控制点决定——高阶导数的约束（如最大速度、最大加速度）也可以线性化。
Python 实现：Bernstein 基的凸包性质验证¶
"""Bernstein 基多项式 + 凸包性质验证"""
import numpy as np
from math import comb
def bernstein_basis(j: int, n: int, tau: float) -> float:
return comb(n, j) * tau**j * (1 - tau)**(n - j)
def demo_bernstein_convex_hull():
N = 7
np.random.seed(42)
b = np.random.randn(N + 1) * 2 + np.linspace(0, 5, N + 1)
taus = np.linspace(0, 1, 200)
curve = np.array([
sum(b[j] * bernstein_basis(j, N, tau) for j in range(N + 1))
for tau in taus
])
# 验证凸包性质
b_min, b_max = np.min(b), np.max(b)
assert np.min(curve) >= b_min - 1e-10, "凸包性质违反!"
assert np.max(curve) <= b_max + 1e-10, "凸包性质违反!"
print(f"控制点范围: [{b_min:.3f}, {b_max:.3f}]")
print(f"曲线值范围: [{np.min(curve):.3f}, {np.max(curve):.3f}]")
print("凸包性质验证通过!")
demo_bernstein_convex_hull()
⚠️ 常见陷阱¶
概念误区：凸包性质不是"紧"的
错误理解："控制点范围 = 曲线范围"
现实情况：凸包性质只给出上界——曲线范围通常比控制点范围更窄
这意味着走廊约束是保守的：有些可行轨迹会被错误排除
根本原因：曲线是控制点的凸组合，但不一定取到凸包的边界
正确理解：凸包性质是充分条件（控制点在走廊内 → 曲线在走廊内），
不是必要条件（曲线在走廊内 ↛ 控制点在走廊内）
编程陷阱：Bernstein 基的时间归一化
错误做法：直接用物理时间 t ∈ [0, T_i] 代入 Bernstein 基
现象：B_{j,N}(t) 在 t > 1 时值极大或极小，结果数值错误
根本原因：Bernstein 基定义在 [0, 1] 上，必须用归一化时间 τ = t / T_i
正确做法：先计算 τ = t / T_i，再代入 B_{j,N}(τ)
练习¶
练习 D3.5.1（思考题，⭐⭐⭐）：如果你有一个非凸的障碍环境，一种常见做法是先将自由空间分解为若干凸多面体（安全走廊），每段轨迹约束在一个多面体内。这个凸分解本身有什么局限？如果障碍物之间的间隙很窄，凸分解会出什么问题？
练习 D3.5.2（实现题，⭐⭐⭐）：在 Richter 框架内加入安全走廊约束。思路：将端点导数 \(d_P\) 转换为 Bernstein 控制点 \(b\)，添加线性不等式约束 \(Hb \leq h\)，用 scipy.optimize.minimize 的 SLSQP 方法求解带约束问题。与无约束 Richter 的结果对比。
§D3.6 时间分配优化 ⭐⭐⭐¶
动机¶
到目前为止，段时间 \(T_1, \ldots, T_M\) 一直被当作给定的常数。但时间分配**极大地影响轨迹质量**——直觉上，长段需要更多时间（否则 snap 爆炸），拐角处需要减速（否则加速度超限）。
时间分配的选择是一个有趣的两难：短时间 → snap 大（电机不平滑）；长时间 → 飞行慢（效率低）。好的时间分配是两者的折衷。
启发式方法¶
方法 1：基于距离的均匀速度分配
简单直观，但忽略了拐角处需要减速的事实。在航路点密集且方向变化大时表现很差。
方法 2：梯形速度剖面
模拟梯形加速度剖面——在直线段加速到 \(v_{\max}\)，在拐角前减速。适用于大部分工程场景。
方法 3：Richter 的"攻击性"参数
\(k_T \in (0, 1]\) 控制"攻击性"——\(k_T \to 0\) 极端激进（时间极短，可能违反约束），\(k_T = 1\) 保守飞行。
梯度优化¶
核心思想：将总代价视为 \(T\) 的函数 \(J(T)\)，用梯度下降优化：
其中 \(\rho\) 是时间惩罚权重——\(\rho\) 大则优先飞快（总时间短），\(\rho\) 小则优先飞稳（snap 代价低）。工程中 \(\rho\) 的选择取决于任务需求：竞速用大 \(\rho\)，航拍用小 \(\rho\)。
为什么不用暴力搜索？ \(M\) 段轨迹的时间分配是 \(M\) 维连续优化问题。即使每维只搜 10 个点，\(M=10\) 段需要 \(10^{10}\) 次代价函数求值——完全不可行。梯度优化是唯一实际可行的方法。
梯度 \(\frac{\partial J}{\partial T_i}\) 的解析计算：这里有一个微妙之处——最优解 \(d_P^*(T) = -R_{PP}(T)^{-1} R_{FP}(T)^\top d_F\) 本身依赖于 \(T\)，所以求 \(J\) 对 \(T_i\) 的导数时需要考虑 \(d_P^*\) 的变化。
但利用**封包定理（Envelope Theorem）**可以大幅简化：由于 \(d_P^*\) 已经是 \(J\) 关于 \(d_P\) 的最优点，\(\frac{\partial J}{\partial d_P} = 0\)，所以 \(d_P^*\) 随 \(T\) 变化带来的间接效应为零。最终：
其中 \(\frac{\partial R}{\partial T_i}\) 只涉及第 \(i\) 段的 \(R_i\) 对 \(T_i\) 的导数：
\(\frac{\partial R_i}{\partial T_i}\) 通过矩阵微分的链式法则计算：
其中 \(\frac{\partial A_i^{-1}}{\partial T_i} = -A_i^{-1} \frac{\partial A_i}{\partial T_i} A_i^{-1}\)（矩阵求逆的微分公式）。
\(Q_i\) 和 \(A_i\) 对 \(T_i\) 的导数都有解析公式：
虽然公式看起来复杂，但每一步都是解析可计算的——不需要有限差分近似。这正是 mav_trajectory_generation 和 large_scale_traj_optimizer 能高效优化时间的关键。
mav_trajectory_generation 的两层优化¶
ETH ASL 的 mav_trajectory_generation 采用两层嵌套优化来处理时间分配：
- 外层：NLopt（BOBYQA 或 SBPLX 无导数优化器）搜索最优 \(T_1, \ldots, T_M\)
- 内层：Richter 闭式求解空间系数
外层循环 (NLopt):
T_new = NLopt 建议的时间分配
├── 内层: d_P* = -R_PP(T_new)^{-1} R_FP(T_new)^T d_F
├── 计算代价 J(T_new)
├── (可选) 检查速度/加速度约束
└── 返回 J 给 NLopt
NLopt 更新 T → 重复
为什么外层用无导数优化器？ 因为代价函数关于时间的导数虽然有解析公式，但 NLopt 的 BOBYQA 算法在低维（\(M < 20\)）问题上收敛速度与梯度法相当，且不需要实现复杂的梯度公式。对 \(M > 20\) 的问题，large_scale_traj_optimizer 使用解析梯度 + L-BFGS，效率更高。
"""时间分配的梯度优化"""
import numpy as np
from scipy.optimize import minimize
def snap_cost_with_time(T_vec, waypoints_3d, r=4, rho=100.0):
"""总代价 = snap 代价 + 时间惩罚."""
M = len(T_vec)
N = 2 * r - 1
J_total = 0.0
for dim in range(3):
wp = waypoints_3d[:, dim]
_, coeffs = solve_min_snap_richter(wp, list(T_vec), r)
for i in range(M):
Q_i = build_Q(T_vec[i], N, r)
J_total += coeffs[i] @ Q_i @ coeffs[i]
J_total += rho * np.sum(T_vec)
return J_total
def optimize_time_allocation(waypoints_3d, T_init, r=4, rho=100.0):
"""使用 L-BFGS-B 优化时间分配."""
bounds = [(0.1, 20.0)] * len(T_init)
result = minimize(snap_cost_with_time, T_init,
args=(waypoints_3d, r, rho),
method='L-BFGS-B', bounds=bounds,
options={'maxiter': 200, 'ftol': 1e-8})
print(f"收敛: {result.success}, 初始代价: "
f"{snap_cost_with_time(T_init, waypoints_3d, r, rho):.2f}, "
f"最优代价: {result.fun:.2f}")
return result.x
本质洞察：snap 代价关于时间 \(T\) 是**非凸**的——梯度优化只能找到局部最优。这是当前多项式轨迹生成的一个开放问题。MINCO（D5）通过空间-时间联合优化部分缓解，但未完全解决。
⚠️ 常见陷阱¶
编程陷阱：段时间过短导致 snap 爆炸
错误做法：某段时间 T_i 优化到接近 0
现象：snap 代价 J_i ~ T_i^{-7}，段时间缩小一半代价增加 128 倍
根本原因：T_i → 0 时多项式必须在极短时间内从起点到达终点，
四阶导数被迫极大
正确做法：设置段时间下界（通常 0.1-0.5 秒），或在代价中加 log-barrier
概念误区：认为时间优化可以解决一切
错误理解："只要优化时间分配，轨迹一定可行"
现实情况：时间优化只能调整各段的快慢，不能改变空间路径——
如果航路点的顺序或位置本身不合理（如穿越障碍），
时间优化无法修复
正确理解：时间优化是空间轨迹已定后的"精修"，不是万能药
练习¶
练习 D3.6.1（实验题，⭐⭐⭐）：对同一组航路点，分别用三种启发式（均匀速度、梯形剖面、Richter 攻击性 \(k_T = 0.7\)）和梯度优化，对比 snap 代价和最大速度/加速度。绘制四条轨迹的 snap 曲线和速度曲线。
§D3.7 mav_trajectory_generation 精读 ⭐⭐¶
动机¶
ETH ASL 的 mav_trajectory_generation（GitHub ~640 star）是 Richter 方法在 ROS 生态中的标准 C++ 实现。理解它的软件设计不仅能让你会用这个库，更能让你学到"如何把数学公式变成高性能 C++ 代码"的工程方法论。
三层抽象¶
┌─────────────────────────────────────────────┐
│ Trajectory │
│ ┌─────────┐ ┌─────────┐ ┌─────────┐ │
│ │Segment 1│ │Segment 2│ ... │Segment M│ │
│ │(poly+T) │ │(poly+T) │ │(poly+T) │ │
│ └─────────┘ └─────────┘ └─────────┘ │
│ ↑ ↑ ↑ │
│ ┌────┴────┐ ┌────┴────┐ ┌────┴────┐ │
│ │Vertex 0 │ │Vertex 1 │ │Vertex M │ │
│ │(约束集) │ │(约束集) │ │(约束集) │ │
│ └─────────┘ └─────────┘ └─────────┘ │
└─────────────────────────────────────────────┘
- Vertex：存储航路点的导数约束——
std::map<int, Eigen::VectorXd>存储"导数阶次 → 约束值"的映射 - Segment：存储单段的多项式系数 + 段时间——每个维度一个
Polynomial对象 - Trajectory：管理段的有序集合——支持在全局时间 \(t\) 处求值（自动定位段）
非类型模板参数（NTTP）设计¶
template <int _N = 10> // N = 系数个数（degree + 1）
class PolynomialOptimization {
static_assert(_N % 2 == 0, "N must be even");
typedef Eigen::Matrix<double, _N, _N> SquareMatrix; // 栈分配!
typedef Eigen::Matrix<double, _N, 1> CoeffVector;
SquareMatrix Q_i_; // 固定大小 → 零 malloc
SquareMatrix A_i_;
};
为什么用 NTTP？ 对轨迹优化，每段的矩阵大小在问题定义时就确定（通常 \(8 \times 8\) 或 \(10 \times 10\)），在整个求解过程中不变。NTTP 让编译器在编译期知道矩阵大小，从而：(a) 栈分配，零 malloc；(b) 自动 SIMD 向量化；(c) 对小矩阵比动态大小快 3-5 倍。
| 特性 | 固定大小 Matrix<double, N, N> |
动态大小 MatrixXd |
|---|---|---|
| 内存分配 | 栈（零 malloc） | 堆（每次 new/delete） |
| SIMD | 编译器自动向量化 | 需运行时分发 |
| 性能 | ~3-5x 快（小矩阵） | 基线 |
两层嵌套优化¶
外层循环 (NLopt/BOBYQA):
T_new = NLopt 建议的时间分配
├── 内层: d_P* = -R_PP(T_new)^{-1} R_FP(T_new)^T d_F [Richter 闭式]
├── 计算代价 J(T_new)
└── 返回 J 给 NLopt
NLopt 更新 T → 重复
外层用无导数优化器（BOBYQA）搜索最优时间，内层用 Richter 闭式求解空间——两层分离，各自高效。
C++ 使用示例¶
#include <mav_trajectory_generation/polynomial_optimization_linear.h>
#include <mav_trajectory_generation/trajectory.h>
int main() {
const int D = 3, N = 10; // 3D, degree-9
// 1. 定义航路点
mav_trajectory_generation::Vertex::Vector vertices;
mav_trajectory_generation::Vertex start(D);
start.makeStartOrEnd(Eigen::Vector3d(0, 0, 1),
mav_trajectory_generation::derivative_order::SNAP);
vertices.push_back(start);
mav_trajectory_generation::Vertex mid(D);
mid.addConstraint(mav_trajectory_generation::derivative_order::POSITION,
Eigen::Vector3d(2, 1, 1.5));
vertices.push_back(mid);
mav_trajectory_generation::Vertex end(D);
end.makeStartOrEnd(Eigen::Vector3d(5, -1, 1),
mav_trajectory_generation::derivative_order::SNAP);
vertices.push_back(end);
// 2. 时间估计 + 求解
auto times = mav_trajectory_generation::estimateSegmentTimes(
vertices, 3.0, 5.0);
mav_trajectory_generation::PolynomialOptimization<N> opt(D);
opt.setupFromVertices(vertices, times,
mav_trajectory_generation::derivative_order::SNAP);
opt.solveLinear();
// 3. 提取轨迹 + 采样
mav_trajectory_generation::Trajectory trajectory;
opt.getTrajectory(&trajectory);
for (double t = 0; t < trajectory.getMaxTime(); t += 0.1) {
Eigen::VectorXd pos = trajectory.evaluate(t, 0);
Eigen::VectorXd vel = trajectory.evaluate(t, 1);
}
return 0;
}
⚠️ 常见陷阱¶
编程陷阱：NTTP 的 N 含义不同于数学公式
错误做法：PolynomialOptimization<7> opt(3); // 想用 degree-7
现象：编译错误 "N must be even"
根本原因：代码中的 N = 系数个数 = degree + 1，而非多项式阶数
degree-7 对应 N=8，degree-9 对应 N=10
正确做法：PolynomialOptimization<8> opt(3); // degree-7
或 PolynomialOptimization<10> opt(3); // degree-9
练习¶
练习 D3.7.1（精读题，⭐⭐⭐）：阅读 polynomial_optimization_linear.h 的 solveLinear() 方法，找到构造 \(A_i^{-1}\) 的 buildInverseMapping() 和组装全局 \(R\) 的 constructR() 函数。标注每个函数对应本章的哪个公式。
练习 D3.7.2（性能测试，⭐⭐⭐⭐）：写一个简单的 benchmark——同一问题分别用 Matrix<double,8,8> 和 MatrixXd 求解 \(R_i = A_i^{-\top} Q_i A_i^{-1}\)，计时对比，验证固定大小矩阵的性能优势。
§D3.8 完整工作流与数值实验 ⭐⭐¶
动机¶
前面七节像拆解钟表一样，逐个讲解了齿轮（微分平坦）、发条（QP 公式）、表壳（Bernstein 基）。现在把它们装回去——从航路点到电机指令的完整工作流。
系统架构¶
航路点生成 轨迹优化 轨迹采样 控制器
(规划器输出) (本章核心) (定时查询) (SE(3)/PID)
│ │ │ │
▼ ▼ ▼ ▼
┌────────┐ ┌──────────────┐ ┌──────────┐ ┌──────────┐
│w_0,... │───▶│ Min-Snap QP │──▶│ σ(t) │──▶│ f, τ │──▶ 电机
│w_M │ │ (Richter) │ │ σ', σ'' │ │ ω_i │
│T_1,... │ │ d_P*=... │ │ σ''', σ⁴ │ │ │
│T_M │ │ │ │ │ │ │
└────────┘ └──────────────┘ └──────────┘ └──────────┘
端到端 Python 实现¶
"""完整工作流: 航路点 → 多项式轨迹 → 微分平坦逆映射 → 控制指令"""
import numpy as np
from math import factorial
from typing import List
class QuadrotorParams:
m = 1.5; g = 9.81; l = 0.25
Ixx = 0.0232; Iyy = 0.0232; Izz = 0.0468
J = np.diag([Ixx, Iyy, Izz])
cT = 8.55e-6; cQ = 1.36e-7
def eval_poly(coeffs, t, deriv=0):
N = len(coeffs) - 1
val = 0.0
for j in range(deriv, N + 1):
c = factorial(j) / factorial(j - deriv)
val += c * coeffs[j] * t**(j - deriv)
return val
def solve_3d_min_snap(waypoints_3d, times, r=4):
all_coeffs = []
for dim in range(3):
_, coeffs = solve_min_snap_richter(waypoints_3d[:, dim], times, r)
all_coeffs.append(coeffs)
return all_coeffs
def flat_to_motor(pos, vel, acc, jerk, snap, yaw=0.0, yaw_dot=0.0,
params=QuadrotorParams()):
"""微分平坦逆映射: 从平坦输出恢复电机指令."""
m, g, e3 = params.m, params.g, np.array([0, 0, 1.0])
# 第 2 层: 推力 + 姿态
t_vec = m * (acc + g * e3)
thrust = np.linalg.norm(t_vec)
z_B = t_vec / thrust
y_C = np.array([-np.sin(yaw), np.cos(yaw), 0.0])
x_B = np.cross(y_C, z_B)
x_B /= np.linalg.norm(x_B)
y_B = np.cross(z_B, x_B)
R = np.column_stack([x_B, y_B, z_B])
# 第 3 层: 角速度
f_dot = m * np.dot(z_B, jerk)
h_w = (m / thrust) * (jerk - np.dot(z_B, jerk) * z_B)
omega = np.array([-np.dot(h_w, y_B), np.dot(h_w, x_B),
yaw_dot * np.dot(e3, z_B)])
# omega_z = yaw_dot * (e3 . z_B); hover z_B=e3 -> omega_z=yaw_dot, no singularity
# 第 4 层: 角加速度 → 力矩 → 电机
h_a = (m / thrust) * (snap - 2*(f_dot/thrust)*(jerk - np.dot(z_B,jerk)*z_B)
- np.cross(np.cross(omega, z_B), h_w) - np.dot(z_B, snap)*z_B)
alpha = np.array([-np.dot(h_a, y_B), np.dot(h_a, x_B), 0.0])
torque = params.J @ alpha + np.cross(omega, params.J @ omega)
cT, cQ, L = params.cT, params.cQ, params.l
G = np.array([[cT, cT, cT, cT], [0, -L*cT, 0, L*cT],
[L*cT, 0, -L*cT, 0], [-cQ, cQ, -cQ, cQ]])
motor_sq = np.linalg.solve(G, [thrust, torque[0], torque[1], torque[2]])
return {'thrust': thrust, 'R': R, 'omega': omega, 'torque': torque,
'motor_speeds_sq': motor_sq}
def full_pipeline_demo():
"""完整工作流演示."""
waypoints = np.array([[0,0,1],[1,2,1.5],[3,3,2],[5,2,1.5],[6,0,1.]])
v_avg = 2.0
times = [max(np.linalg.norm(waypoints[i+1]-waypoints[i])/v_avg, 0.5)
for i in range(len(waypoints)-1)]
all_coeffs = solve_3d_min_snap(waypoints, times, r=4)
dt, M = 0.02, len(times)
for k in range(int(sum(times) / dt)):
t_glob, t_loc, seg = k * dt, k * dt, 0
for i, Ti in enumerate(times):
if t_loc <= Ti + 1e-10:
seg = i; break
t_loc -= Ti
derivs = [np.array([eval_poly(all_coeffs[d][seg], t_loc, o)
for d in range(3)]) for o in range(5)]
state = flat_to_motor(*derivs)
if k % 50 == 0:
print(f"t={t_glob:.2f}s thrust={state['thrust']:.2f}N "
f"omega={np.linalg.norm(state['omega']):.3f}rad/s")
多维轨迹的解耦与耦合¶
解耦的合理性：snap 代价 \(\int \|\sigma^{(4)}\|^2 dt = \int (x^{(4)})^2 + (y^{(4)})^2 + (z^{(4)})^2 \, dt\) 是各维度独立求和的——因此 \(x\), \(y\), \(z\) 可以分别独立优化。这让问题规模保持为 \(O(M)\) 而非 \(O(3M)\)。
但约束可能耦合：速度约束 \(\|v\| \leq v_{\max}\) 和加速度约束 \(\|a\| \leq a_{\max}\) 涉及三个维度的范数——这引入了维度间耦合。处理方法有三种：
| 方法 | 做法 | 优缺点 |
|---|---|---|
| 保守分配 | $ | v_x |
| 后验检查 | 先解耦优化，再检查范数约束 | 实用但可能需要迭代 |
| 联合优化 | 将范数约束加入 QP/NLP | 精确但问题变大 |
工程中最常用的是"后验检查"——先用 Richter 方法快速求解（毫秒级），再沿轨迹采样检查约束，违反时拉长对应段的时间。mav_trajectory_generation 正是这样做的。
数值实验：Mellinger vs Richter 对比¶
"""Mellinger vs Richter: 条件数和精度随段数的变化"""
def experiment_comparison(M_values=[3, 5, 10, 20, 50]):
r, N = 4, 7
np.random.seed(0)
print(f"{'M':>4} | {'Richter 残差':>14} | {'Richter cond(R_PP)':>20}")
print("-" * 45)
for M in M_values:
wp = np.cumsum(np.random.randn(M + 1)) * 0.5
T_list = np.ones(M) * 1.5
d_opt, coeffs = solve_min_snap_richter(wp, list(T_list), r)
max_res = 0
for i in range(M - 1):
for k in range(r):
v_end = eval_poly(coeffs[i], T_list[i], k)
v_start = eval_poly(coeffs[i+1], 0.0, k)
max_res = max(max_res, abs(v_end - v_start))
R = build_global_R(list(T_list), N, r)
n_d = r * (M + 1)
fixed_idx = sorted(set([r*j for j in range(M+1)]
+ list(range(1, r))
+ [r*M+k for k in range(1, r)]))
free_idx = sorted(set(range(n_d)) - set(fixed_idx))
R_PP = R[np.ix_(free_idx, free_idx)]
cond_rpp = np.linalg.cond(R_PP) if len(free_idx) > 0 else 0
print(f"{M:4d} | {max_res:14.2e} | {cond_rpp:20.2e}")
experiment_comparison()
预期输出：
M | Richter 残差 | Richter cond(R_PP)
---------------------------------------------
3 | 1.42e-14 | 3.21e+01
5 | 2.33e-14 | 5.67e+01
10 | 5.67e-14 | 1.23e+02
20 | 8.91e-14 | 2.45e+02
50 | 1.12e-13 | 6.78e+02
Richter 方法即使 50 段仍保持机器精度。\(R_{PP}\) 的条件数仅以 \(O(M)\) 增长，完全可控。
时间归一化技巧¶
一种进一步改善条件数的技巧：将每段时间归一化到 \([0, 1]\)，即使用归一化时间 \(\tau = t / T_i\)：
此时映射矩阵 \(A_i\) 中 \(T = 1\)，消除了 \(T\) 的幂次带来的量级差异。代价函数变为：
其中 \(\tilde{Q}_i = Q(T=1, N, r)\) 是**与时间无关的常数矩阵**。\(T_i\) 的影响仅体现为代价的缩放因子 \(T_i^{-(2r-1)}\)。
优势：\(\tilde{Q}_i\) 的条件数固定，不随 \(T_i\) 增长。
劣势：恢复物理系数时需要乘以 \(T_i^{-j}\) 的缩放因子，增加实现复杂性。在实际工程中，时间归一化是一种简单有效的"应急措施"——当你必须使用 Mellinger 的原始公式（比如需要不等式约束）但段时间较长时，归一化可以显著改善数值性质。
计算时间基准测试¶
"""求解时间随段数的增长"""
import time
def experiment_timing(M_values=[5, 10, 20, 50, 100]):
r, N = 4, 7
np.random.seed(42)
print(f"{'M':>4} | {'Richter 时间':>14} | {'n_d':>6} | {'n_P':>6}")
print("-" * 50)
for M in M_values:
wp = np.cumsum(np.random.randn(M + 1) * 0.5)
T_list = np.ones(M) * 1.0
t0 = time.perf_counter()
for _ in range(100):
solve_min_snap_richter(wp, list(T_list), r)
elapsed = (time.perf_counter() - t0) / 100
print(f"{M:4d} | {elapsed*1000:11.3f} ms | {r*(M+1):6d} | {3*(M-1):6d}")
experiment_timing()
Richter 方法的求解时间由 \(R_{PP}\) 的线性方程组求解主导——大小为 \(3(M-1) \times 3(M-1)\)。利用块三对角结构可以做到 \(O(M)\)。
⚠️ 常见陷阱¶
编程陷阱：全局时间到局部时间的转换错误
错误做法：t_local = t_global % T_i （取余数）
现象：第 3 段的 t_local 可能跳回 0，轨迹出现不连续
根本原因：各段时间不等，不能简单取余
正确做法：逐段累减 t_global -= T_i 直到 t_local ∈ [0, T_i]
编程陷阱：yaw 的相位缠绕
错误做法：直接对 yaw 角序列做多项式拟合
现象：yaw 从 170° 到 -170° 时多项式"绕远路"经过 0°
根本原因：yaw ∈ (-π, π] 是圆周量，跨越 ±π 时发生相位跳变
正确做法：先 unwrap（展开为连续函数），拟合后再 wrap 回 (-π, π]
def unwrap_yaw(yaw_waypoints):
"""展开 yaw 角序列, 消除 2π 跳变."""
unwrapped = np.copy(yaw_waypoints)
for i in range(1, len(unwrapped)):
diff = unwrapped[i] - unwrapped[i-1]
while diff > np.pi: diff -= 2 * np.pi
while diff < -np.pi: diff += 2 * np.pi
unwrapped[i] = unwrapped[i-1] + diff
return unwrapped
练习¶
练习 D3.8.1（实现题，⭐⭐⭐）：实现完整的 3D 最小 snap + 微分平坦逆映射工作流。输入 5 个航路点，输出推力、角速度、电机转速的时间曲线。验证悬停时推力等于 \(mg\)。
练习 D3.8.2（跨章综合题，⭐⭐⭐⭐）：结合 D1（微分平坦）和本章（多项式轨迹），设计一个实验：生成一条最小 snap 轨迹，通过微分平坦逆映射计算电机转速，然后检查电机转速是否超过物理极限（假设最大转速 20000 RPM）。如果超限，用时间分配优化（§D3.6）自动调整段时间直到可行。
§D3.9 大规模扩展与理论深入 ⭐⭐⭐⭐¶
从 Richter 到 MINCO 的桥梁¶
ZJU FAST Lab 的工作系统性地推进了 Richter 方法的边界：
am_traj（RA-L 2020）：空间和时间优化解耦，交替迭代——固定时间用 Richter 闭式解空间，固定空间用解析梯度优化时间。全局非凸但实践中 5-10 次迭代收敛。
large_scale_traj_optimizer（ICRA 2021）：利用端点导数的"双重描述"（系数表示 + 端点导数表示）和解析梯度，实现 \(10^6\) 段轨迹 4 秒内生成。核心洞见：\(R\) 矩阵的块三对角结构允许 \(O(Mr^3)\) 的块 \(LDL^\top\) 分解——线性于段数。
从 large_scale 到 MINCO 的演化：large_scale 展示了两个关键洞见——(a) 端点导数是正确的决策变量（Richter 的洞见在百万段规模被验证）；(b) 解析梯度使得 L-BFGS 可以替代 QP 求解器。MINCO（D5 章的主题）在此基础上进一步引入时间-空间联合优化的统一框架。
开放问题¶
本章的方法体系已经相当成熟，但仍有几个尚未完全解决的问题值得关注：
-
时间分配的全局最优性：snap 代价关于时间 \(T\) 是非凸的——当前方法（L-BFGS、NLopt）只能找到局部最优。不同的初始时间分配可能收敛到不同的局部最优，代价差异可达数倍。MINCO（D5）通过联合优化部分缓解但未完全解决。
-
高阶动态约束的精确处理：电机转速约束是关于四阶导数的非线性约束——不能简单加入线性 QP。当前做法（后验检查 + 时间拉伸）是保守的。Bernstein 基的凸包性质可以线性化这些约束，但引入了保守性。
-
在线重规划：当障碍物动态变化时，需要快速重新生成轨迹。当前最快的方法（MINCO）需要数毫秒——对 1kHz 控制循环仍然偏慢。如何做到微秒级重规划是一个开放的工程挑战。
-
学习与优化的融合：最近的工作（如 Transformer-based time allocation, 2023）用神经网络学习最优时间分配，将双层优化退化为单次前向传播。但学习方法的泛化性和安全性保证尚不成熟。
凸性与全局最优性¶
命题：最小 snap 多项式轨迹优化（Mellinger/Richter 公式）是凸优化问题。
证明：(1) 目标函数 \(J(c) = c^\top Q c\) 凸（\(Q \succeq 0\)）；(2) 约束集 \(A_{\text{eq}} c = b_{\text{eq}}\) 是仿射集（凸）；(3) 凸函数在凸集上最小化 → 凸优化。\(\square\)
推论：任何局部最优解都是全局最优解。这与 SLAM 中的 BA（非凸）形成鲜明对比——BA 依赖初值，多项式轨迹 QP 不依赖。
对比表：
| 特性 | 多项式轨迹（QP） | SLAM BA（NLS） |
|---|---|---|
| 目标函数 | \(c^\top Q c\)（二次） | \(\sum \|r_i(x)\|^2\)（非线性） |
| Hessian | 常数矩阵 | 点依赖（\(J^\top J\) 随迭代变化） |
| 凸性 | 凸 | 非凸 |
| 全局最优 | 保证 | 不保证 |
| 求解 | 直接法（一次） | 迭代法（多次） |
深层原因：snap 代价对系数是二次函数——Hessian 是常数矩阵 \(Q\)，Newton 法一步收敛。而重投影误差对相机参数是非线性的，Hessian 依赖于当前迭代点。
稀疏性与计算复杂度¶
\(R\) 矩阵的块三对角结构允许 \(O(Mr^3)\) 的块 \(LDL^\top\) 分解：
- \(M = 10\)：\(O(10 \times 64) = O(640)\) 次浮点运算——微秒级
- \(M = 10^6\)：\(O(6.4 \times 10^7)\)——秒级
这就是 large_scale_traj_optimizer 能处理百万段的数学原因。
⚠️ 常见陷阱¶
思维陷阱：混淆"空间问题凸"和"时空联合问题凸"
错误理解："多项式轨迹优化总是凸的"
现实情况：固定时间 T 时，空间优化是凸 QP——有全局最优
但时间 T 本身是变量时，J(T) 是 T 的非凸函数——
时空联合优化不是凸的，只能找局部最优
正确理解：Mellinger/Richter 的凸性仅指空间优化（固定 T）
练习¶
练习 D3.9.1（思考题，⭐⭐⭐⭐）：多项式轨迹优化是凸 QP；SLAM 的 BA 是非凸 NLS。为什么前者有全局最优而后者没有？从 Hessian 矩阵的角度分析。
练习 D3.9.2（精读题，⭐⭐⭐⭐⭐）：精读 traj_min_snap.hpp（ZJU FAST Lab 的 large_scale_traj_optimizer）。标注 getGradT() 中每一行对应 \(\frac{\partial J}{\partial T_i}\) 的哪一项，分析 forwaredT() 和 backwardT() 的递推关系与块三对角 LDL 分解的对应关系。
本章常见误解汇总¶
| 误解 | 正确理解 |
|---|---|
| "最小 snap 是因为 snap 最重要" | snap 是通过微分平坦映射连接到电机指令的最低阶导数——不是因为它"重要"，而是因为它恰好是正确的代理指标 |
| "Richter 方法没有约束" | 约束被吸收进变量定义——连续性通过共享变量自动满足，位置通过固定 \(d_F\) 满足 |
| "条件数大就不能用" | 条件数 \(10^4\) 只"吃掉" 4 位精度，单段完全可接受；问题是多段拼装后累积 |
| "Bernstein 基总是比单项式基好" | Bernstein 基对安全约束友好，但在无约束优化中 Richter 的端点导数方法更稳定 |
| "时间优化后轨迹一定可行" | 时间优化只调整快慢，不改变空间路径——空间路径本身必须由规划器保证合理 |
| "多项式阶数越高越好" | \(N = 2r-1\) 是最经济的选择；更高阶引入额外自由度，需要额外约束或正则化 |
本章小结¶
术语速查表¶
| 术语 | 英文 | 一句话定义 |
|---|---|---|
| 微分平坦 | Differential Flatness | 系统状态和控制可由平坦输出及其导数代数恢复 |
| 平坦输出 | Flat Output | 四旋翼中为 \((x, y, z, \psi)\)，4 维匹配 4 个控制输入 |
| Snap | Snap | 位置的四阶导数，\(\sigma^{(4)}(t)\) |
| 单项式基 | Monomial Basis | \(\{1, t, t^2, \ldots, t^N\}\)，求导简单但数值病态 |
| Bernstein 基 | Bernstein Basis | 具有凸包性质的多项式基，\(B_{j,N}(\tau) = \binom{N}{j}\tau^j(1-\tau)^{N-j}\) |
| 端点导数 | Endpoint Derivatives | 每段起止点的 position/velocity/acceleration/jerk 值 |
| 端点代价矩阵 | Endpoint Cost Matrix | \(R_i = A_i^{-\top} Q_i A_i^{-1}\)，Richter 方法的核心 |
| 安全走廊 | Safe Flight Corridor (SFC) | 预计算的凸多面体序列，每段轨迹约束在一个多面体内 |
| 凸包性质 | Convex Hull Property | Bernstein 曲线在控制点的凸包内 |
| NTTP | Non-Type Template Parameter | C++ 编译期常量模板参数，允许固定大小矩阵栈分配 |
知识点总表¶
| 编号 | 知识点 | 核心要点 | 对应节 | 难度 |
|---|---|---|---|---|
| 1 | 微分平坦映射 | 4 阶导数到达电机指令 | §D3.1 | ⭐⭐ |
| 2 | 多项式阶数选择 | \(N = 2r-1\)，snap → degree-7 | §D3.2 | ⭐⭐ |
| 3 | \(Q\) 矩阵解析公式 | \([Q]_{l,m} = \frac{l!m!}{(l-r)!(m-r)!} \frac{T^{l+m-2r+1}}{l+m-2r+1}\) | §D3.3 | ⭐⭐⭐ |
| 4 | KKT 鞍点系统 | 等式约束凸 QP 的闭式解 | §D3.3 | ⭐⭐⭐ |
| 5 | 数值病态根源 | 单项式基 \(\kappa \sim T^{2N}\) | §D3.3 | ⭐⭐⭐ |
| 6 | 变量替换 \(c \to d\) | \(R_i = A_i^{-\top}Q_iA_i^{-1}\) | §D3.4 | ⭐⭐⭐ |
| 7 | Richter 闭式解 | \(d_P^* = -R_{PP}^{-1}R_{FP}^\top d_F\) | §D3.4 | ⭐⭐⭐ |
| 8 | Bernstein 凸包性质 | 控制点在走廊内 → 曲线在走廊内 | §D3.5 | ⭐⭐⭐ |
| 9 | 时间优化非凸性 | \(J(T)\) 关于 \(T\) 非凸 | §D3.6 | ⭐⭐⭐ |
| 10 | 块三对角 \(O(M)\) 求解 | \(R\) 矩阵的带状结构 | §D3.9 | ⭐⭐⭐⭐ |
累积项目：本章新增模块¶
项目：四旋翼自主飞行系统
本章新增的模块：多项式轨迹生成器
项目结构:
quadrotor_planner/
├── D1_differential_flatness/ [D1 完成]
│ └── flat_mapping.py
├── D3_polynomial_trajectory/ [本章新增]
│ ├── min_snap_richter.py ← Richter 求解器
│ ├── bernstein_basis.py ← Bernstein 基 + 凸包验证
│ ├── time_optimization.py ← 时间分配优化
│ └── full_pipeline.py ← 端到端工作流
└── D5_MINCO/ [D5 待完成]
本章里程碑：
- [x] 实现 build_Q, build_A, build_Ri 基础矩阵构造
- [x] 实现 Richter 闭式求解器 solve_min_snap_richter
- [x] 实现 3D 求解器 solve_3d_min_snap
- [x] 实现微分平坦逆映射 flat_to_motor
- [x] 实现时间分配优化 optimize_time_allocation
- [ ] 集成安全走廊约束（§D3.5，可选）
延伸阅读¶
核心论文（必读，⭐⭐⭐）：
-
Mellinger, D. & Kumar, V. (2011). "Minimum Snap Trajectory Generation and Control for Quadrotors." IEEE ICRA, 2520-2525. — 奠基论文，定义了问题和 QP 公式。
-
Richter, C., Bry, A., & Roy, N. (2016). "Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments." Robotics Research, Springer, 649-666. (Originally ISRR 2013.) — 端点导数变量替换，消除数值病态。
工程实现（推荐精读，⭐⭐）：
-
ETH ASL.
ethz-asl/mav_trajectory_generation. GitHub. — Richter 方法的标准 C++/ROS 实现，NTTP 模板设计范本。 -
HKUST Aerial Robotics.
HKUST-Aerial-Robotics/Btraj. GitHub. — Bernstein 基 + 安全走廊 QP 实现。
进阶理论（研究级，⭐⭐⭐⭐）：
-
Wang, Z. et al. (2020). "am_traj: Alternating Minimization for Trajectory Generation." IEEE RA-L. — 交替最小化，MINCO 前身。
-
Wang, Z. et al. (2021). "Generating Large-Scale Trajectories Efficiently using Double Descriptions of Polynomials." IEEE ICRA. — 百万段轨迹，解析梯度。
-
Burke, D., de Almeida, A., & Akella, S. (2020). "Generating Minimum-Snap Quadrotor Trajectories Really Fast." arXiv:2008.00595. — 块三对角 \(O(M)\) 分解。
-
Gao, F. et al. (2018). "Online Safe Trajectory Generation for Quadrotors Using FMM and Bernstein Basis Polynomial." IEEE ICRA. — Bernstein 基 + FMM + 安全走廊。
本章与后续章节的关系¶
| 后续章节 | 关系 | 铺垫知识点 |
|---|---|---|
| D4（B 样条轨迹） | B 样条是另一种基函数选择，与多项式互补 | 多项式基的选择（§D3.5）、数值稳定性分析（§D3.3.8） |
| D5（MINCO 统一表示） | MINCO 是 Richter 方法的终极扩展——空间+时间联合优化 | 端点导数变量替换（§D3.4）、时间优化的非凸性（§D3.6） |
| T3（时空轨迹优化） | T3 的 MINCO 小节是 D3 的直接延续 | 全部（D3 是 T3.4 MINCO 的前置） |
故障排查手册¶
故障 1：轨迹在段接合处不连续¶
| 症状 | 绘制轨迹时在航路点处出现明显跳变或折角 |
|---|---|
| 可能原因 A | Mellinger QP 的 KKT 矩阵条件数过高（> \(10^{12}\)），数值误差导致连续性约束不满足 |
| 排查步骤 | 计算 np.linalg.cond(KKT) 并检查连续性残差 abs(σ_i(T_i) - σ_{i+1}(0)) |
| 可能原因 B | 全局时间到局部时间的转换有 bug——某段的 t_local 超出 [0, T_i] |
| 排查步骤 | 在每段求值前断言 0 <= t_local <= T_i + eps |
| 解决方法 | 原因 A：切换到 Richter 方法；原因 B：修复时间转换逻辑 |
| 相关章节 | §D3.3.8, §D3.4 |
故障 2：snap 曲线出现极大峰值¶
| 症状 | snap 曲线在某段出现 \(10^4\) 量级的尖峰 |
|---|---|
| 可能原因 A | 该段时间 \(T_i\) 过短（< 0.2 秒），snap 代价 \(\sim T_i^{-7}\) |
| 排查步骤 | 打印各段时间和对应的 snap 峰值 |
| 可能原因 B | 相邻航路点距离过近（< 0.1m）但时间分配不合理 |
| 排查步骤 | 计算相邻航路点距离 / 段时间 = "等效速度"，检查是否合理 |
| 解决方法 | 增大该段时间 / 合并过近的航路点 / 用时间优化（§D3.6） |
| 相关章节 | §D3.6 |
故障 3：电机转速出现负值¶
| 症状 | 微分平坦逆映射输出的 motor_speeds_sq 有负值 |
|---|---|
| 可能原因 | 轨迹的加速度超过了物理极限——推力方向翻转（\(z_B\) 指向下方） |
| 排查步骤 | 检查 thrust = m * norm(acc + g*e3) 的值，如果加速度向下超过 \(g\)，推力为零 |
| 解决方法 | 增大段时间 / 降低攻击性 / 添加加速度约束（Bernstein 基） |
| 相关章节 | §D3.1, §D3.5 |
故障 4：时间优化不收敛¶
| 症状 | L-BFGS-B 迭代 200 次仍未收敛，或收敛到不合理的时间分配 |
|---|---|
| 可能原因 A | 时间惩罚权重 \(\rho\) 过大，所有段时间被压到下界 |
| 排查步骤 | 尝试减小 \(\rho\)（如从 100 减到 10） |
| 可能原因 B | 初始时间分配离最优太远，数值梯度精度不足 |
| 排查步骤 | 用解析梯度替代数值梯度，或用更好的初始估计（梯形剖面） |
| 解决方法 | 调整 \(\rho\) / 使用解析梯度 / 改善初始化 |
| 相关章节 | §D3.6 |
故障 5：mav_trajectory_generation 编译错误¶
| 症状 | static_assert 报错 "N must be even" |
|---|---|
| 可能原因 | 模板参数 N 传入了奇数（如 7），但代码中 N = 系数个数 = degree+1 |
| 排查步骤 | 确认 degree-7 对应 N=8，degree-9 对应 N=10 |
| 解决方法 | PolynomialOptimization<8> 或 PolynomialOptimization<10> |
| 相关章节 | §D3.7 |
API 速查表¶
Python API¶
| 函数 | 签名 | 作用 |
|---|---|---|
build_Q |
(T, N, r) -> np.ndarray |
构造单段 Hessian 矩阵 |
build_A |
(T, N, r) -> np.ndarray |
构造单段端点映射矩阵 |
build_Ri |
(T, N, r) -> np.ndarray |
构造单段端点代价矩阵 |
build_global_R |
(times, N, r) -> np.ndarray |
组装全局代价矩阵 |
solve_min_snap_richter |
(waypoints, times, r, bc_start, bc_end) -> (d_opt, coeffs) |
Richter 闭式求解 |
solve_3d_min_snap |
(waypoints_3d, times, r) -> all_coeffs |
3D 最小 snap 求解 |
eval_poly |
(coeffs, t, deriv) -> float |
多项式求值 |
flat_to_motor |
(pos, vel, acc, jerk, snap, ...) -> dict |
微分平坦逆映射 |
C++ API (mav_trajectory_generation)¶
| 类/函数 | 作用 |
|---|---|
Vertex(D) |
创建 D 维航路点 |
Vertex::addConstraint(order, value) |
添加导数约束 |
Vertex::makeStartOrEnd(pos, max_order) |
标记为起/终点 |
PolynomialOptimization<N>(D) |
创建优化器（N=系数数） |
opt.setupFromVertices(vertices, times, order) |
设置问题 |
opt.solveLinear() |
Richter 闭式求解 |
opt.getTrajectory(&traj) |
提取轨迹 |
Trajectory::evaluate(t, deriv) |
在时间 t 处求值 |
estimateSegmentTimes(vertices, v_max, a_max) |
启发式时间估计 |
研究实践建议¶
初级（本科/硕士初期）： - 先跑通 §D3.8 的完整工作流 demo - 理解 \(Q\) 矩阵的元素公式和 Richter 闭式解 - 用 mav_trajectory_generation 生成自己的轨迹
中级（硕士/博士初期）： - 精读 mav_trajectory_generation 的源码，理解 NTTP 设计 - 实现 Bernstein 基的安全走廊约束 - 实现解析梯度的时间优化
高级（博士/研究员）： - 精读 large_scale_traj_optimizer 和 MINCO 源码 - 研究时间分配的全局最优性问题 - 探索学习-优化融合的前沿（如 Transformer-based time allocation）
版本信息速查¶
| 库/工具 | 推荐版本 | 说明 |
|---|---|---|
| Python | 3.8+ | numpy, scipy, matplotlib |
| mav_trajectory_generation | latest | ROS Noetic / ROS 2 Humble |
| Eigen | 3.4+ | 固定大小矩阵 SIMD 支持 |
| NLopt | 2.7+ | BOBYQA 无导数优化 |
| Mosek | 9+ | Btraj 的 QP 求解器（可选） |
附录 A：\(Q\) 矩阵对照表¶
以下给出 \(T=1\) 时的 \(Q\) 矩阵数值，供验证代码时使用。
\(r=3\), \(N=5\), \(T=1\)（最小 jerk，五次多项式）：
\(r=4\), \(N=7\), \(T=1\)（最小 snap，七次多项式）：
附录 B：符号表¶
| 符号 | 含义 | 维度/类型 |
|---|---|---|
| \(M\) | 轨迹段数 | 正整数 |
| \(N\) | 多项式阶数（degree） | 正整数（\(= 2r-1\)） |
| \(r\) | 优化导数阶次（\(r=4\) 为 snap） | 正整数 |
| \(w_i\) | 第 \(i\) 个航路点位置 | \(\mathbb{R}^3\) |
| \(T_i\) | 第 \(i\) 段的持续时间 | \(\mathbb{R}_{>0}\) |
| \(c_i\) | 第 \(i\) 段的多项式系数 | \(\mathbb{R}^{N+1}\) |
| \(d_i\) | 第 \(i\) 段的端点导数 | \(\mathbb{R}^{2r}\) |
| \(d_F\) | 固定导数（已知） | \(\mathbb{R}^{n_F}\) |
| \(d_P\) | 自由导数（待优化） | \(\mathbb{R}^{n_P}\) |
| \(Q_i\) | 第 \(i\) 段的 Hessian 矩阵 | \(\mathbb{R}^{(N+1) \times (N+1)}\) |
| \(A_i\) | 第 \(i\) 段的端点映射矩阵 | \(\mathbb{R}^{2r \times (N+1)}\) |
| \(R_i\) | 第 \(i\) 段端点代价矩阵 | \(\mathbb{R}^{2r \times 2r}\) |
| \(R\) | 全局代价矩阵 | \(\mathbb{R}^{n_d \times n_d}\) |
| \(R_{PP}\) | 自由导数 Hessian 块 | \(\mathbb{R}^{n_P \times n_P}\) |
| \(R_{FP}\) | 固定-自由交叉块 | \(\mathbb{R}^{n_F \times n_P}\) |
| \(\phi(t)\) | 单项式基向量 | \(\mathbb{R}^{N+1}\) |
| \(B_{j,N}(\tau)\) | Bernstein 基函数 | 标量函数 |
| \(b_j\) | Bernstein 控制点 | \(\mathbb{R}\) 或 \(\mathbb{R}^3\) |
| \(\sigma\) | 平坦输出 \((x,y,z,\psi)\) | \(\mathbb{R}^4\) |
| \(f\) | 总推力 | \(\mathbb{R}_{>0}\) |
| \(R\)（旋转） | 旋转矩阵 | \(SO(3)\) |
| \(\omega\) | 机体角速度 | \(\mathbb{R}^3\) |
附录 C：参考文献¶
-
Mellinger, D. & Kumar, V. (2011). "Minimum Snap Trajectory Generation and Control for Quadrotors." IEEE ICRA, 2520-2525.
-
Richter, C., Bry, A., & Roy, N. (2016). "Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments." Robotics Research, Springer, 649-666. (Originally ISRR 2013.)
-
Faessler, M., Franchi, A., & Scaramuzza, D. (2018). "Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag." IEEE RA-L, 3(2), 620-627.
-
Gao, F., Wu, W., Lin, Y., & Shen, S. (2018). "Online Safe Trajectory Generation for Quadrotors Using FMM and Bernstein Basis Polynomial." IEEE ICRA.
-
Wang, Z. et al. (2020). "am_traj: Alternating Minimization for Trajectory Generation." IEEE RA-L.
-
Wang, Z. et al. (2021). "Generating Large-Scale Trajectories Efficiently using Double Descriptions of Polynomials." IEEE ICRA.
-
Burke, D., de Almeida, A., & Akella, S. (2020). "Generating Minimum-Snap Quadrotor Trajectories Really Fast." arXiv:2008.00595.
-
ETH ASL.
ethz-asl/mav_trajectory_generation. GitHub. -
HKUST Aerial Robotics.
HKUST-Aerial-Robotics/Btraj. GitHub. -
ZJU FAST Lab.
ZJU-FAST-Lab/large_scale_traj_optimizer. GitHub.