D8 敏捷飞行平台与集成规控——Agilicious / CPC / IPC / MPCC

性质：算法工程教学 | ❌ 纯无人机——敏捷飞行的问题结构(单刚体+推力极限+气动耦合+高速传感器延迟)在机械臂/足式中不存在 | 难度跨度：⭐⭐ ~ ⭐⭐⭐⭐ | 预计精读：20-28 小时

一句话定位：当四旋翼的速度从 5 m/s 推到 100 km/h,气动残差、电机饱和、传感器延迟**从可忽略的二阶噪声变成主导一阶效应。本章完整讲透这个极限区间的工程答卷——**Agilicious 怎样用 Pilot/Bridge/Guard 三层分离让 30+ 篇论文共享一个安全平台、CPC 怎样把"飞最快"形式化为互补约束的 bang-bang 最优、IPC 怎样把规划和控制塞进同一个 QP、MPCC 怎样用"进度最大化"替代"轨迹跟踪"实现快慢自适应。

---

本部分定位

Part 4(敏捷飞行与 RL)开篇:Part 2-3 的轨迹规划器假设飞行速度在 2-10 m/s 范围,动力学约束通过微分平坦和安全走廊"软化"处理。当速度推向 20+ m/s 乃至 100+ km/h(竞速),气动残差、电机饱和、传感器延迟从可忽略的二阶效应变为一阶效应——此时必须要么用更精确的模型(MPC+BEM)、更紧密的规划-控制耦合(IPC/CPC/MPCC),要么干脆用 RL 端到端学。本章(D8)讲经典优化方法逼近极限,下一章(D9)讲 RL 如何跨越。对已有足式 RL 背景的读者而言,D9 是**最短的认知路径**——PPO/TD3 + Isaac Gym 的工具链几乎可以直接复用。

---

前置自测

开始前先回答下面 5 个问题。答不出 2 题以上,建议先回前置章节补齐——本章的每个控制器都建立在这些基础之上,欠了账会在 CPC/MPCC 的公式推导处卡住。

1. SE(3) 几何控制器的姿态误差 \(e_R = \frac{1}{2}(R_d^\top R - R^\top R_d)^\vee\) 为什么定义在李代数 \(\mathfrak{so}(3)\) 上,而不是简单地对欧拉角做差? 它解决了欧拉角的什么病态? (答不出 → 回 D1 SE(3) 控制,§D1.3)

2. 模型预测控制(MPC)的"滚动时域(Receding Horizon)"是什么意思? 为什么 MPC 每个控制周期只执行求得的最优控制序列的**第一个**元素 \(u_0\),然后丢弃其余、重新求解? (答不出 → 回 D2 MPC,§D2.1)

3. 微分平坦(Differential Flatness)让四旋翼的规划问题降到几维? 平坦输出 \(\sigma = (x,y,z,\psi)\) 的前几阶导数能代数地恢复出全部状态和控制? (答不出 → 回 D1 微分平坦 / D3 §D3.1)

4. 安全飞行走廊(Safe Flight Corridor, SFC)用什么数学对象表示自由空间? 一个凸多面体约束 \(A p \le b\) 中,每一行 \(a_i^\top p \le b_i\) 的几何含义是什么? (答不出 → 回 D5 MINCO/SFC)

5. 二次规划(QP)与一般非线性规划(NLP)的根本区别是什么? 为什么 QP 能在毫秒级求得**全局最优**,而 NLP 通常只能保证局部最优? (答不出 → 回 Part 0 优化基础 / D3 §D3.3)

参考答案要点(先自己答,再对照):

1. 欧拉角在大倾角(接近 ±90° pitch)处有万向锁(gimbal lock)——两个旋转轴重合,丢失一个自由度,姿态误差的导数发散。敏捷飞行倾角常超 60°,必须用旋转矩阵/李代数表示。\(e_R\) 定义在 \(\mathfrak{so}(3)\) 上,对任意大姿态偏差都良定义且光滑,这是 SE(3) 控制在敏捷飞行中不可替代的根本原因。

2. 滚动时域:在当前时刻 \(t\),MPC 用当前状态做初值,在未来 \(N\) 步预测区间上求解一个最优控制问题(OCP),得到控制序列 \(u_0, u_1, \ldots, u_{N-1}\);只执行 \(u_0\),下一周期用新的实测状态重新求解。这样做的原因是**反馈**——丢弃 \(u_1\ldots\) 并用新测量重算,等于把开环最优控制变成闭环,自动吸收模型误差和扰动。

3. 降到 4 维(平坦输出空间)。\(\sigma\) 及其前 4 阶导数(到 snap)可代数恢复全部 12 维状态 + 4 维控制——这就是为什么 D3 最小化的是 snap(四阶导)。规划在 4 维曲线空间做,而非 12 维状态空间的最优控制。

4. SFC 用一串**凸多面体**(convex polytopes)的并集表示自由空间走廊。每行 \(a_i^\top p \le b_i\) 是一个**半空间**(half-space):位置 \(p\) 必须落在以 \(a_i\) 为法向、过某点的平面的"内侧"。多个半空间的交集 \(Ap \le b\) 就是一个凸多面体。

5. QP 的目标函数是凸二次型、约束是线性的 → 可行域是凸多面体、目标是凸函数 → 任何局部最优即全局最优,且有多项式时间算法(内点法/ADMM)。一般 NLP 的目标或约束非凸 → 可行域可能有多个不连通的"岛",梯度法只能收敛到某个局部最优。CPC 的互补约束正是把问题推回非凸 NLP,这是它必须离线求解的根源。

---

本章目标

学完本章后,你应该能够:

1. **系统论证**为什么敏捷飞行(推力利用率 >70%)从根本上不同于常规飞行——从气动残差、感知延迟、执行器饱和三个一阶效应出发,定量说明每个效应如何随速度从"可忽略"变为"主导"
2. 解释 Agilicious 的工程纪律:为什么 Pilot/Bridge/Guard 三层分离是 30+ 篇论文能在同一平台上做实验的关键——尤其 Guard 为什么必须用**独立估计器**,以及它是 70 km/h 下唯一的安全网
3. 从物理含义理解 CPC 的互补进度约束 \(\dot{\pi}_i \cdot (\|p - w_i\| - r_{wp}) = 0\):为什么它能让优化器自动分配时间、为什么时间最优必然是 bang-bang、为什么这个公式让问题变成非凸 NLP(只能离线)
4. 理解 IPC 如何把走廊约束和 MPC 动力学放进同一个 OSQP QP @ 100 Hz——从根本上消除"规划器交给控制器一条不可行参考"的经典管线病
5. 掌握 MPCC 的轮廓跟踪范式:为什么"最大化沿路径进度 \(\Delta\theta\) + 最小化轮廓误差 \(e_{con}\)"比"跟踪时间参数化参考"更适合竞速,以及它如何自然实现弯道减速、直道加速
6. 能区分并选用四种敏捷飞行范式:DFBC(前馈+PD)、NMPC(约束优化)、CPC(时间最优离线)、MPCC(进度最大化在线)——清楚各自的计算成本、约束处理能力、适用场景
7. 理解 sim-to-real 的关键工程:为什么精确的电机延迟模型、气动残差(NeuroBEM)、域随机化是高速飞行从仿真迁移到真机的决定性因素

---

本章知识导航

本章的知识结构是一棵以"如何让四旋翼飞到物理极限还安全"为根的树。树干是**先理解极限从哪来(D8.0),再看支撑实验的平台(D8.1),然后是四种攻克极限的范式(D8.2-8.4),最后是底层控制器、感知与系统集成(D8.5-8.9)**。

敏捷飞行为什么难? (§D8.0 三大一阶效应:气动/延迟/饱和)
    │
    ▼
谁来托住 70 km/h 的实验安全? (§D8.1 Agilicious:Pilot/Bridge/Guard)
    │
    ├─→ 飞最快(离线) ── CPC 时间最优 (§D8.2) ── 互补约束 → bang-bang
    │
    ├─→ 规控一体(在线) ── IPC 统一 QP (§D8.3) ── SFC + 动力学 同一 OSQP
    │
    ├─→ 竞速自适应(在线) ── MPCC 轮廓跟踪 (§D8.4) ── 进度最大化 + 隧道
    │
    ├─→ 底层跟踪器 ── DFBC / INDI (§D8.5) ── 平坦前馈 vs 传感器增量
    │
    ├─→ 高频状态从哪来 ── 感知流水线 (§D8.6) ── VIO/MSCKF/门检测
    │
    ▼
   把它们拼成一个能赢人类的系统 (§D8.7 范式对照 → §D8.8 系统集成 → §D8.9 AC-MPC)

小节	主题	难度	一句话
§D8.0	敏捷飞行的定义与物理边界	⭐⭐	气动/延迟/饱和——三个二阶效应在高速下变一阶
§D8.1	Agilicious 全栈平台	⭐⭐	Pilot/Bridge/Guard 三层分离;Guard 是唯一安全网
§D8.2	CPC 时间最优飞行	⭐⭐⭐	互补进度约束 → bang-bang → 跑赢职业飞手
§D8.3	IPC 规控统一 QP	⭐⭐⭐	SFC + MPC 动力学塞进同一 OSQP @ 100 Hz
§D8.4	MPCC 进度最大化	⭐⭐⭐	弧长参数化 + 轮廓误差 + 隧道约束
§D8.5	DFBC 与 INDI 控制器	⭐⭐	平坦前馈 vs 传感器驱动增量;扰动如何被消去
§D8.6	敏捷感知流水线	⭐⭐⭐	高频低延迟状态估计;高速下帧相机为何失效
§D8.7	四种范式对照与选型	⭐⭐	计算成本/约束处理/频率的决策树
§D8.8	竞速系统集成	⭐⭐⭐	AlphaPilot / Swift / Agilicious 端到端链路
§D8.9	Actor-Critic MPC	⭐⭐⭐⭐	可微 MPC 作 RL 的 actor;结构与学习的中间点

两条阅读线:

• 理论线(理解数学):§D8.0→§D8.2→§D8.3→§D8.4→§D8.9,重点是 CPC/MPCC 的优化公式和互补/轮廓的数学结构
• 工程线(会用就行):§D8.0→§D8.1→§D8.5→§D8.7→§D8.8,重点是平台架构、控制器选型和系统集成

无论哪条线,§D8.0 都是必读——不理解三大一阶效应,后面所有"为什么要这么复杂"的问题都无解。

---

前置知识桥接

回顾 D1(SE(3) 几何控制):D1 建立了在旋转矩阵流形 \(SO(3)\) 上直接定义控制律的方法——姿态误差 \(e_R = \frac{1}{2}(R_d^\top R - R^\top R_d)^\vee\) 不依赖欧拉角,因此在任意大倾角下都不发散。本章的 DFBC(§D8.5.1)的内环就是 SE(3) 几何控制的姿态环;CPC/MPC 用四元数 \(q\) 表示姿态也是同一动机——避开欧拉角的万向锁。一句话:敏捷飞行倾角常 >60°,姿态表示必须用流形而非欧拉角,这是 D1 给本章的地基。

回顾 D2(MPC / acados):D2 讲了 MPC 的滚动时域原理和 acados 的实时迭代(SQP-RTI)架构——在每个控制周期把一个非线性最优控制问题(OCP)线性化、用 QP 求解、只执行第一步。本章的 NMPC、IPC、MPCC、AC-MPC 全部是 MPC 的变体:IPC 把 SFC 约束加进 QP(§D8.3),MPCC 把进度变量加进状态(§D8.4),AC-MPC 让 RL 调 MPC 的权重(§D8.9)。如果你在 D2 学的"OCP→QP→只执行 \(u_0\)"的循环这里会反复出现。

回顾 D3 / D5(轨迹生成与 SFC):D3 讲了最小 snap 多项式轨迹——为什么 snap 是四旋翼的天然代价(微分平坦的层级结构);D5 讲了 MINCO 和安全飞行走廊(SFC)。本章中:CPC(§D8.2)用 min-snap 轨迹做初始猜测;IPC(§D8.3)的走廊约束直接来自 D5 的 SFC 生成。CPC 的"为什么 min-snap 不是时间最优"这个论证,前提就是你已经懂 D3 的 min-snap 在最小化什么。

前向预告:本章讲经典优化方法(CPC/MPCC/IPC/AC-MPC)如何把四旋翼推到 ~21 m/s 真机竞速,但它们都依赖**精确模型**或**精确门位置**。下一章(D9)的 Swift(Nature 2023)用端到端 RL 跳过这两个依赖——不需要预先知道门在哪,也不需要精确动力学。现在只需要知道:经典方法的天花板是"模型有多准";RL 的代价是"sim-to-real gap 有多大"。本章 §D8.9 的 AC-MPC 正是站在这两者中间的折中点。

---

如果跳过本章会怎样

跳过 D8,你会卡在三个具体的地方。

场景一:"低速调好的控制器,一加速就发散。" 你用 D1 的 SE(3) 控制器 + D3 的 min-snap 轨迹,在 5 m/s 下跟踪误差几厘米,完美。但当你把目标速度提到 18 m/s,无人机在弯道处突然飘出去半米,推力指令疯狂震荡。你不知道这是**旋翼阻力前馈缺失**(§D8.0 挑战一)+ 推力饱和后控制律失效(§D8.0 挑战三)。没有 D8,你只会盲目调 PD 增益,越调越糟——因为问题不在增益,在模型和约束。

场景二:"规划器说能飞,控制器一飞就撞。" 你的 A* + min-snap 规划器在 t=0 生成了一条避开所有已知障碍的漂亮轨迹,交给 MPC 跟踪。但飞到一半,一个行人突然走进轨迹——MPC 只看下一个参考点,不知道走廊在哪,径直撞上去。你不知道这是**分离式管线的根本病**(§D8.3.1),也不知道 IPC 把 SFC 约束塞进 MPC 的 QP 就能让控制器自动绕行。没有 D8,你会以为"加个紧急刹车逻辑"就行——治标不治本。

场景三:"我能算出时间最优轨迹,但飞不出来。" 你听说 CPC 能跑赢人类,自己用 CasADi 写了个最小化总时间的 NLP,但求解器要么不收敛、要么吐出一条物理上无法跟踪的轨迹。你不知道**互补约束的非凸性**让 NLP 有海量局部最优、对初始化极度敏感(§D8.2.4),也不知道为什么时间最优一定是 bang-bang(Pontryagin 最大值原理)。没有 D8 对 CPC 数学结构的拆解,你会在数值不收敛的泥潭里耗上几周。

---

预计阅读时间

模式	时长	适合
精读	20-28 小时	第一次系统学敏捷飞行规控:逐节读动机→公式→代码,亲手跑 Agilicious 仿真、实现 2D MPCC,做完每节练习与思考题。建议分 5-6 次。
速读	6-8 小时	有 MPC/优化基础、想建立全局图景:读每节的"动机/本质洞察",§D8.0 三大挑战表、§D8.7 范式对照、§D8.8 系统集成,跳过代码细节。
速查	30-60 分钟	已学过、回来查特定方法:直接定位到对应小节,看核心公式 + 范式对照表 + 故障排查手册。

---

科研发展脉络

年份	论文	Venue	核心贡献
2017	Mellinger, Michael, Kumar, "Trajectory Generation and Control for Precise Aggressive Maneuvers with Quadrotors"	IJRR	精确激进机动:翻转、穿环、着陆垫——min-snap + SE(3) 控制的极限演示
2021	Foehn, Romero, Scaramuzza, "Time-Optimal Planning for Quadrotor Waypoint Flight" (CPC)	Science Robotics	互补进度约束(CPC):单推力极限下的时间最优航点轨迹;职业飞手对比
2022	Foehn, Kaufmann 等(UZH RPG), "Agilicious: Open-Source and Open-Hardware Agile Quadrotor"	Science Robotics	敏捷飞行全栈:Pilot/Bridge/Guard 架构;MPC/DFBC/INDI 多控制器;C++17
2023	Liu, Ren, Zhang(HKU MaRS), "IPC: Integrated Planning and Control for Quadrotor Navigation in Tight Environments"	RA-L	规划+控制统一 QP:SFC 约束 + 线性 MPC 在同一 OSQP 中 100 Hz 求解
2023	Romero, Sun, Foehn, Scaramuzza, "MPCC for Quadrotor Racing"	T-RO	MPCC/MPCC++:进度最大化+轮廓误差最小化;Frenet-Serret 隧道约束
2024	Krinner 等(UZH RPG), "MPCC++: Model Predictive Contouring Control with BEM Aerodynamics"	arXiv	学到的气动残差 + TuRBO 贝叶斯超参数调优;100 Hz 实时
2025	Romero 等(UZH RPG), "Actor-Critic MPC"	T-RO	可微 MPC 作为 actor 层;RL 优化 MPC 参数;21 m/s 真机竞速

---

看这条线的走向:从 2011 Mellinger 到 2025 AC-MPC,有一条清晰主线——模型从"凑合能用"走向"精确到气动残差",规划与控制从"分离的两个模块"走向"融合的一个优化",再走向"用 RL 调优的混合体"。每一步都在逼近物理极限的同时,把"人为分割"的接口缝合掉。本章讲前五步(Mellinger→CPC→Agilicious 工程化→IPC/MPCC 规控融合→AC-MPC),Swift 的纯 RL 终点留给 D9。

---

本章符号约定

符号	含义	首见
\(v\)	飞行器速度(惯性系)	§D8.0
\(f\)	集体推力(collective thrust),四电机推力之和	§D8.0
\(f_{\max}, f_{\min}\)	单电机/集体推力的上下限	§D8.0
\(\tau = (\tau_x, \tau_y, \tau_z)\)	机体力矩	§D8.0
\(\Omega_i\)	第 \(i\) 个电机转速	§D8.0
TWR	推重比(Thrust-to-Weight Ratio)\(= f_{\max}/(mg)\)	§D8.1
\(R\)	机体到惯性系的旋转矩阵 \(\in SO(3)\)	§D8.0
\(q\)	姿态四元数	§D8.2
\(\omega\)	机体角速度	§D8.0
\(D\)	旋翼阻力系数矩阵 \(\mathrm{diag}(d_x,d_y,d_z)\)	§D8.0
\(w_i\)	第 \(i\) 个航点(waypoint)位置	§D8.2
\(\pi_i(t)\)	CPC 的第 \(i\) 个航点进度变量	§D8.2
\(r_{wp}\)	航点容差半径	§D8.2
\(\theta\) (或 \(s\))	MPCC 的路径进度变量(弧长)	§D8.4
\(e_{con}, e_{lag}\)	轮廓误差 / 滞后误差	§D8.4
\(r_{tunnel}\)	MPCC 隧道半径	§D8.4
\(A_{sfc} p \le b_{sfc}\)	安全飞行走廊(SFC)的半空间约束	§D8.3
\(N\)	MPC/MPCC 预测步长(horizon)	§D8.3

---

§D8.0 敏捷飞行的定义、挑战与物理边界 ⭐⭐

动机

在进入具体算法和平台之前,必须先回答一个根本问题:为什么不能把 D1-D5 学的控制器和规划器直接拿来,把目标速度调高就完事? 答案是——常规飞行下被你合理忽略的三件事(空气、延迟、推力上限),在高速下会同时反扑。本节系统阐述**什么是敏捷飞行**、它在工程上为什么困难,以及那些原本可忽略的物理效应如何从噪声变成控制系统必须正面对抗的一阶效应。

反面:如果忽略这些效应会怎样

设想你把 D3 的 min-snap 轨迹生成器目标速度从 5 m/s 改成 20 m/s,控制器原封不动。会发生三件事:(1) 轨迹要求的推力在弯道处超过 \(f_{\max}\),控制器悄悄裁剪,实际轨迹偏离参考且无法恢复;(2) 你的 VIO 在 50 ms 延迟下交给控制器的是"无人机 1 米之前在哪",控制器据此算出的指令已经过时;(3) 旋翼阻力在 20 m/s 下产生 0.5-1 m 的稳态跟踪误差,而你的模型里 \(f_{ext}=0\)。三个误差叠加,无人机要么撞墙要么翻滚。这正是"敏捷飞行需要专门方法"的实证起点——不是因为算法不够花哨,而是因为低速的简化假设全部失效。

§D8.0.1 定义与速度边界 ⭐⭐

敏捷飞行(Agile Flight)是指四旋翼在接近甚至达到其**物理极限**的条件下执行高速机动的飞行模式。与常规飞行(2-5 m/s、低倾角、线性化动力学有效)相比,敏捷飞行具有以下特征:

指标	常规飞行	敏捷飞行	竞速极限
速度	2-5 m/s	10-20 m/s	20-30+ m/s (>100 km/h)
加速度	<1g	2-4g	5g+
倾斜角	<15°	30-60°	>60°, 含翻滚
推力利用率	30-50%	70-90%	90-100% (饱和)
角速度	<1 rad/s	2-5 rad/s	>10 rad/s
轨迹类型	直线/缓弯	S弯/急转	赛道竞速/极限穿越

速度的物理含义:当速度从 5 m/s 增加到 20 m/s 时: - 空气动力:阻力 ∝ v^2,20 m/s 时阻力是 5 m/s 时的 16 倍 - 旋翼气动效应:旋翼前行桨叶和后行桨叶的来流不对称(blade flapping)从可忽略变为显著 - 感知延迟影响:50 ms 的感知延迟在 5 m/s 下对应 0.25 m 位置偏差,在 20 m/s 下变为 1.0 m——可能直接撞上障碍 - 控制带宽需求:要在曲率半径 3 m 的弯道以 20 m/s 转弯,需要 >13g 的向心加速度——超过大多数飞行器推重比

本质洞察:敏捷飞行的难度不是线性增长,而是在某个速度阈值附近**相变**。三个原本独立的小效应(气动、延迟、饱和)在高速下不仅各自变大,还**相互耦合放大**——气动残差让模型变差,模型变差让 MPC 预测变差,预测变差又遇上感知延迟的滞后,三者叠加使误差超过安全裕度。这就是为什么不存在"把低速控制器调激进一点"的渐进路径,而必须换一整套方法论。

正式定义:我们将**敏捷飞行**定义为推力利用率持续超过 70%、飞行器需要在推力/力矩饱和边界附近运行的飞行状态。在此状态下: 1. 线性化动力学模型不再有效 2. 气动残差不可忽略 3. 执行器饱和是约束优化的常态而非异常 4. 感知系统延迟成为安全的决定性因素

类比:敏捷飞行 vs F1 赛车过弯。敏捷四旋翼在物理极限上飞,像 F1 赛车在抓地力极限上过弯——两者都在"再多一点就失控"的边界上运行,都要求精确预判而非事后反应,都用 bang-bang 式的"全力加速/全力刹车"而非匀速巡航。但**不像** F1 的地方在于:赛车的极限是轮胎与地面的**摩擦力**(横向约束),四旋翼的极限是电机推力的**饱和**(纵向+姿态耦合约束)——赛车打滑了还能修正,四旋翼推力饱和后会同时丢失推力和力矩自由度(§D8.0 挑战三),更接近"悬崖"而非"滑坡"。这个边界类比仅在"极限运行"层面成立,具体的失效机制完全不同。

§D8.0.2 敏捷飞行的应用场景 ⭐⭐

敏捷飞行不仅仅是竞速游戏——它代表了无人机在真实环境中生存和执行任务的极端需求:

搜救与灾害响应: - 在倒塌建筑内穿越狭小缝隙:需要在 <1 m 宽的通道中保持精确位姿控制 - 快速搜索大面积区域:高速飞行 + 实时避障 = 敏捷飞行 - 时间关键任务:被困人员的生存窗口要求飞行器以最短时间到达目标

工业检测: - 桥梁/风电塔筒检测:需要在强风(阵风 10+ m/s)中保持稳定 - 管道巡检:在狭窄管道内飞行,空间约束与气动干扰同时存在 - 电力线检测:近距离飞行要求毫秒级的避障反应

军事/安全: - 城市环境穿越:建筑物之间的快速穿梭 - 对抗性环境:需要规避威胁的高机动飞行 - 蜂群协同:多机高速编队要求每架飞行器都具备敏捷能力

科研基准: - 无人机竞速(Drone Racing):AlphaPilot、Swift 等以竞速作为敏捷飞行的标准化基准 - 算法验证:CPC、MPCC 等算法需要在极限条件下验证

---

§D8.0.3 敏捷飞行的三大核心挑战 ⭐⭐⭐

敏捷飞行的难度可以归结为三个在低速飞行中可以忽略、但在高速下变为主导的挑战:

挑战一:气动效应——模型不确定性的爆发

低速模型:在 5 m/s 以下,四旋翼可以用简单的刚体+四力模型:

mẍ = R·[0, 0, f]^T - mge₃ + f_ext
Jω̇ = τ - ω × Jω

其中 f_ext ≈ 0(外部力可忽略)。

高速现实:在 15+ m/s 下,多种气动效应必须建模:

a) 旋翼阻力(Rotor Drag)

旋翼在前飞时,前行桨叶(advancing blade)的相对气流速度增加,后行桨叶(retreating blade)的相对气流速度降低。这导致:

f_drag = -D · R^T · v_body

其中 D = diag(d_x, d_y, d_z) 是阻力系数矩阵。Faessler 2018 的关键发现:考虑旋翼阻力后,四旋翼的微分平坦性仍然保持,但平坦映射需要修改——推力方向不再是 (ẍ + ge₃) 的方向,而需要解一个耦合方程。

具体来说,传统平坦映射:

z_B = (ẍ + ge₃) / ‖ẍ + ge₃‖    (无阻力)

带旋翼阻力的修正映射:

z_B = (mẍ + mge₃ + D·R^T·v) / ‖mẍ + mge₃ + D·R^T·v‖    (有阻力)

由于 R 本身依赖 z_B,这成为一个**隐式方程**,Faessler 用迭代法求解。

DFBC 中的阻力前馈:

f_ff = ‖mẍ_ref + mge₃ + D·v_ref‖       // 推力前馈
R_ref = reconstruct_R(ẍ_ref, v_ref, ψ_ref)  // 姿态前馈(带阻力修正)
ω_ref = extract_ω(R_ref, jerk_ref, v̇_ref)   // 角速度前馈

工程影响:不建模旋翼阻力时,15 m/s 的跟踪误差可达 0.5-1.0 m;加入阻力前馈后降至 0.1-0.2 m。

b) 桨叶挥舞(Blade Flapping)

柔性桨叶在不对称气流下上下摆动,导致有效推力平面倾斜:

θ_flap ≈ (v_∞ / Ω·R) · (3/2) · (1 / (1 + 3μ²/2))

其中 v_∞ 是前飞速度,Ω 是转速,R 是桨叶半径,μ = v_∞/(Ω·R) 是前进比。

对于小型竞速四旋翼(R ≈ 0.065 m, Ω ≈ 2000 rad/s): - 在 5 m/s 下:μ ≈ 0.038,θ_flap ≈ 0.06° → 可忽略 - 在 20 m/s 下:μ ≈ 0.15,θ_flap ≈ 0.23° → 开始显著 - 在 30 m/s 下:μ ≈ 0.23,θ_flap ≈ 0.35° → 必须补偿

挥舞效应在横向(y_B 方向)产生额外力矩,导致偏航耦合——这是纯 PD 控制器在高速下跟踪性能急剧下降的主要原因之一。

c) 地面效应(Ground Effect)

飞行器距地面高度 h < 2R(旋翼直径)时,下洗气流被地面反射,旋翼效率增加:

T_IGE / T_OGE ≈ 1 / (1 - (R/(4h))²)

其中 T_IGE 是地面效应内推力,T_OGE 是地面效应外推力。

在敏捷飞行中,地面效应主要在起飞/降落和低空穿越阶段产生影响。如果控制器不考虑地面效应,低空急加速时实际推力比期望大 10-20%,可能导致过冲。

d) 气流相互干扰(Propeller-Propeller Interaction)

在大攻角(>30°)飞行时,前排旋翼的下洗气流影响后排旋翼的入流条件。这种干扰在传统 BEM(Blade Element Momentum)理论中难以精确建模,是 NeuroBEM 用神经网络学习残差的主要动机。

e) NeuroBEM:混合气动建模

Bauersfeld, Kaufmann, Foehn, Sun, Scaramuzza (RSS 2021) 提出 NeuroBEM:

f_aero = f_BEM(v, ω, Ω_motors) + f_NN(v, ω, Ω_motors, history)
                ↑ 解析部分                ↑ 学到的残差

核心思想: 1. BEM 部分用经典的桨叶元素-动量理论计算**已知的气动效应**(推力、扭矩与转速/来流的关系) 2. 神经网络(TCN, Temporal Convolutional Network)学习**BEM 无法捕捉的残差**(旋翼间干扰、非定常效应、机体阻力的非线性部分) 3. 输入:50 ms 的状态历史(位置、速度、姿态、角速度、电机转速) 4. 训练数据:96 次飞行,1.8M 数据点,最高速度 65 km/h,加速度达 46.8 m/s^2

性能提升: - 纯 BEM vs NeuroBEM:轨迹仿真位置 RMSE 降低 >50% - 纯 NN vs NeuroBEM:在训练分布外的泛化性显著优于纯数据驱动方法 - 在 MPCC++ 中的应用:NeuroBEM 使控制器能在 80+ km/h 下保持稳定

// NeuroBEM 伪代码结构
class NeuroBEM {
  BEMModel bem_;           // 解析 BEM 模型
  TCNNetwork residual_nn_; // 残差网络

  Vec6 predict(const StateHistory& hist, const MotorCommands& cmd) {
    Vec6 f_bem = bem_.compute(hist.current(), cmd);
    Vec6 f_res = residual_nn_.forward(hist.window(50ms), cmd);
    return f_bem + f_res;
  }
};

挑战二:感知延迟——高速下的致命 blind spot

延迟分解:

t_total = t_capture + t_readout + t_transfer + t_process + t_estimate + t_control

组件	典型延迟	说明
图像采集(t_capture)	1-10 ms	曝光时间,高速运动导致模糊
传感器读出(t_readout)	1-5 ms	全局快门 vs 卷帘快门差异大
数据传输(t_transfer)	1-3 ms	USB3/CSI 接口
视觉处理(t_process)	10-30 ms	特征提取/匹配/CNN 推理
状态估计(t_estimate)	1-5 ms	EKF/优化
控制计算(t_control)	0.1-5 ms	PD 到 MPC 差异大
总延迟	15-60 ms

位置偏差计算:

Δp = v · t_total + 0.5 · a · t_total²

速度 (m/s)	延迟 30 ms	延迟 50 ms	延迟 80 ms
5	0.15 m	0.25 m	0.40 m
10	0.30 m	0.50 m	0.80 m
15	0.45 m	0.75 m	1.20 m
20	0.60 m	1.00 m	1.60 m
25	0.75 m	1.25 m	2.00 m

在 20 m/s 下,50 ms 延迟导致 1 m 位置偏差——对于穿过 0.7 m 宽的竞速门,这意味着**控制器必须预测未来状态**而不是跟踪当前感知。

延迟补偿策略:

方法一:IMU 前向传播(IMU Forward Propagation)

// 在收到视觉更新后,用 IMU 数据前向传播到当前时刻
State compensate_latency(const State& visual_state,
                         const IMUBuffer& imu_buf,
                         double t_visual, double t_now) {
  State s = visual_state;
  for (auto& imu : imu_buf.between(t_visual, t_now)) {
    // 状态传播:用 IMU 的加速度和角速度积分
    s.position += s.velocity * imu.dt + 0.5 * (s.R * imu.acc - g) * imu.dt * imu.dt;
    s.velocity += (s.R * imu.acc - g) * imu.dt;
    s.R = s.R * exp_SO3(imu.gyro * imu.dt);
  }
  return s;
}

在 Agilicious 中,IMU 以 400-1000 Hz 运行,视觉以 30-100 Hz 运行。IMU 前向传播可以将有效延迟从 50 ms 降低到 ~2 ms(最近一次 IMU 读数的延迟)。

方法二:MPC 预测补偿

// MPC 在 t_now 求解时,初始状态取 t_now + t_latency 的预测值
State x0_shifted = propagate_dynamics(x_current, u_current, t_latency);
solve_MPC(x0_shifted, reference);

MPC 天然具有预测能力——通过将预测区间的起点偏移 t_latency,MPC 的控制指令在到达执行器时恰好对应正确的时间点。

方法三:事件相机(Event Camera)

事件相机不按固定帧率采集完整图像,而是在每个像素亮度变化时异步触发事件:

event = (x, y, t, polarity)   // 像素坐标、微秒时间戳、明暗变化

优势: - 微秒级时间分辨率(vs 帧相机的毫秒级) - 无运动模糊(每个事件是瞬时的) - 高动态范围(>120 dB vs 帧相机 60 dB) - 低数据率(只传输变化)

在 Scaramuzza/Delmerico 的研究中,事件相机已成为研究级竞速无人机的标准传感器,因为在 100+ km/h 下帧相机的运动模糊使特征跟踪完全失效。

挑战三:执行器饱和——推力极限上的控制

电机模型:

f_i = k_f · Ω_i²    // 推力与转速平方成正比
τ_i = k_τ · Ω_i²    // 反扭矩与转速平方成正比

每个电机有物理转速上限 Ω_max(由 KV 值和电池电压决定):

Ω_max = KV · V_battery    // 例:1750 KV × 14.8V = 25,900 RPM
f_max_single = k_f · Ω_max²
f_max_total = 4 · f_max_single

推重比(Thrust-to-Weight Ratio, TWR):

TWR = f_max_total / (m · g)

竞速四旋翼典型 TWR = 4-6:1,意味着最大加速度约 3-5g。Agilicious 平台的 TWR ≈ 4.5:1。

饱和问题的工程表现:

当 MPC 或 DFBC 请求的推力超过 f_max 时:

u_requested = [f, τ_x, τ_y, τ_z]    // 控制器请求
u_actual = allocate_and_clip(u_requested, Ω_min, Ω_max)  // 实际执行

推力饱和导致: 1. 推力不足:无法产生足够的向心力完成急弯 2. 力矩耦合丧失:推力饱和后,力矩分配自由度减少——4 个电机只剩 <4 个自由度 3. 积分器饱和(Integrator Windup):如果控制器有积分项,持续饱和导致积分值爆炸

解决策略对比:

策略	方法	优缺点
控制分配裁剪	将 u 映射到电机转速后裁剪	简单但可能丢失力矩方向
优先级分配	推力>roll/pitch>yaw 的优先级	保证姿态稳定但牺牲跟踪
MPC 约束	在优化中显式加入 f ∈ [f_min, f_max]	全局最优但计算量大
CPC 互补性	f·(f_max - f) = 0 → bang-bang	时间最优但非实时
INDI	增量控制,隐式处理饱和	鲁棒但依赖角加速度估计

电机延迟建模:

电机不是瞬时响应——从指令到实际转速有一阶延迟:

Ω̇_i = (Ω_cmd_i - Ω_i) / τ_motor    // τ_motor ≈ 15-30 ms

在敏捷飞行中,这 15-30 ms 的电机延迟叠加感知延迟,使得从"决策"到"执行"的总延迟可达 50-80 ms。SimpleFlight (Chen, Yu 等, 2024) 的关键发现之一就是:精确的电机延迟模型是 sim-to-real 成功的五大关键因素之一。

电池电压跌落:

随着飞行进行,锂电池电压从 16.8V(满电)降至 14.0V(低电量),Ω_max 下降 17%,最大推力下降 31%:

f_max(V) / f_max(V_full) = (V / V_full)²

这意味着控制器在飞行后期的可用推力显著减少——CPC 和 MPCC 等追求极限的方法必须考虑电池状态。

本质洞察:三大挑战表面上是三个独立的物理问题(气动、延迟、饱和),但它们在工程上指向**同一个统一答案——预测**。气动残差要靠**预测模型**(BEM/NeuroBEM)前馈补偿;感知延迟要靠**预测未来状态**(IMU 前向传播 / MPC 区间偏移)抵消;执行器饱和要靠**预测约束**(MPC 显式 \(f \in [f_{\min}, f_{\max}]\))提前规避。这就解释了为什么敏捷飞行的主流方法全是**基于模型的预测控制(MPC 家族)**而非反应式 PID——PID 只能对"已经发生的误差"做反应,而高速飞行的误差一旦发生就来不及修正。本章后面的 CPC/IPC/MPCC/AC-MPC,本质上都是在回答"如何更好地预测"这一个问题的不同侧面。

反事实:如果四旋翼的推重比无限大(电机永不饱和)、传感器零延迟、空气不存在,那么 D1 的 SE(3) 几何控制器配 D3 的 min-snap 轨迹就足以飞任意快——本章 90% 的内容都不需要。敏捷飞行这门学问之所以存在,完全是因为这三个理想化假设在 20+ m/s 下全部破产。把这句话记住,你就能理解后面每个方法**到底在补哪个假设的窟窿**:CPC 补"推力有限"(互补约束逼近饱和边界),DFBC 补"空气存在"(旋翼阻力前馈),延迟补偿补"传感器有延迟"。

---

§D8.1 Agilicious 全栈平台——从仿真到 70 km/h 真机 ⭐⭐

动机

讲完"敏捷飞行为什么难",一个尖锐的现实问题立刻浮现:这些极限实验在哪里、怎么安全地做? 一架 70 km/h 的四旋翼撞到人会造成严重伤害,而 CPC/MPCC/Swift 这些方法在成熟前必然反复失控。如果每个研究组都从零搭建飞控、估计器、安全监控,不仅重复劳动,而且每次新算法上真机都是一次拿安全赌运气。Agilicious 要解决的正是这个**平台问题**——它不发明任何控制算法,而是提供一套硬件+软件+安全的基座,让 30+ 篇论文能在同一个经过验证的平台上安全地迭代。

反面:没有统一平台会怎样

设想没有 Agilicious。研究组 A 用 PX4 + 自写 MPC,研究组 B 用 BetaFlight + ROS,两者的安全逻辑、坐标系约定、电机标定全不一样。结果是:(1) A 组的 MPCC 论文别人无法复现,因为底层栈不同;(2) 每个新博士生入组第一年都在重写飞控胶水代码而非做研究;(3) 最危险的是——安全监控往往是事后补的、和控制器耦合的,一旦控制器崩溃,安全网也跟着崩溃。Agilicious 的 Pilot/Bridge/Guard 三层分离正是对这三个痛点的针对性回应:Bridge 统一硬件接口(可复现),Pilot 可插拔算法(专注研究),Guard 独立于 Pilot 运行(控制器崩了安全网还在)。

历史

Agilicious 自 2016 年起在苏黎世大学机器人与感知组(UZH RPG)开发与使用,2022 年作为开源开硬件框架正式发表于 Science Robotics。它是 RPG 一系列里程碑工作的共同底座——CPC(2021,跑赢职业飞手)、NeuroBEM(2021,混合气动建模)、Swift(2023,Nature 击败世界冠军)、MPCC/MPCC++(2023-2024)、AC-MPC(2025)全部跑在这个平台上。可以说,理解 Agilicious 的架构,就是理解 UZH RPG 过去十年敏捷飞行研究得以工程化落地的"操作系统"。

引用:Foehn, Kaufmann, Romero, Penicka, Sun, Bauersfeld, Laengle, Cioffi, Song, Loquercio, Scaramuzza, "Agilicious: Open-Source and Open-Hardware Agile Quadrotor for Vision-Based Flight", Science Robotics 7(67), 2022.

GitHub: uzh-rpg/agilicious, ~574 stars, GPL-3.0。 文档站: https://agilicious.readthedocs.io

Agilicious 不是一个算法——它是一个**硬件+软件+安全**平台,使后续算法研究(Swift/CPC/MPCC/Actor-Critic MPC)在工程上成为可能。自 2016 年以来在 UZH RPG 开发和使用,支撑了 30+ 篇论文的实验。

§D8.1.1 硬件平台详解

硬件规格:

组件	选型	规格	选择理由
机架	250 mm 碳纤维 X 型	~200g 裸机架	刚性高、振动传导低
电机	2207/1750KV	最大推力 ~12N/个	TWR 4.5:1,兼顾推力和响应速度
螺旋桨	5" 三叶桨	-	高推力密度,适合 250mm 轴距
FCU	BetaFlight F7	STM32F7, 8K PID 循环	成熟的飞控固件,PID 内环
机载计算	Intel NUC / Jetson TX2	i7/ARM + GPU	NUC 用于纯 CPU 算法,TX2 用于 CNN 推理
VIO 传感器	Intel RealSense T265	双鱼眼 + IMU, 200Hz	自包含 VIO,6-DOF 位姿
外置相机	全局快门相机	分辨率/帧率可配置	避免卷帘快门伪影
IMU	FCU 内置 + T265 IMU	400-1000 Hz	高速率状态估计
电池	4S LiPo (14.8V)	1300-1550 mAh	约 3-5 分钟激进飞行时间
通信	UART (FCU) + WiFi/Ethernet	低延迟控制链路

推重比计算:

总重量 ≈ 机架(200g) + 电机×4(120g) + 螺旋桨×4(20g) + FCU(10g)
       + NUC/TX2(~260g) + 电池(180g) + 传感器(60g) + 杂项(50g)
       ≈ 900g

最大推力 ≈ 4 × 12N = 48N
推重比 = 48 / (0.9 × 9.81) ≈ 5.4:1
有效推重比(考虑电池电压) ≈ 4.5:1

为什么 TWR > 4 是敏捷飞行的门槛:

最大可用加速度 = (TWR - 1) × g
TWR = 4.5 → a_max = 3.5g ≈ 34.3 m/s²

在曲率半径 5 m 的弯道中:

v_max = sqrt(a_max × r) = sqrt(34.3 × 5) ≈ 13.1 m/s ≈ 47 km/h

要在 3 m 曲率半径弯道达到 70 km/h (19.4 m/s),需要:

a_req = v² / r = 19.4² / 3 ≈ 125 m/s² ≈ 12.8g

这超过了 Agilicious 的物理极限——所以 MPCC 的弯道减速策略不是保守,而是物理必然。

§D8.1.2 软件三层架构深度解析

Agilicious 的软件架构是其最重要的工程贡献——三层分离使得 30+ 篇论文能在同一平台上安全地做实验,而不需要每次都从头搭建安全机制。

层次结构:

┌─────────────────────────────────────────────────────────┐
│                       Guard 层                           │
│  独立估计器 + 地理围栏 + 位姿偏差监控 + 心跳检测           │
│  违规 → 立即接管 → 紧急悬停/降落                          │
├─────────────────────────────────────────────────────────┤
│                       Pilot 层                           │
│  Pipeline = Estimator → Sampler → Reference → Controller │
│  Controller 通过 Factory 从 YAML 热切换                   │
│  支持: MPC / DFBC / INDI / RL / 自定义                   │
├─────────────────────────────────────────────────────────┤
│                       Bridge 层                          │
│  统一 API → RotorS / Flightmare / 真机 UART              │
│  同一份二进制不重编译切换目标                              │
└─────────────────────────────────────────────────────────┘

Guard 层——70 km/h 下唯一的安全网

Guard 是独立于 Pilot 运行的安全监控层,其设计原则是**即使 Pilot 完全失控,Guard 也能保证飞行器不飞出安全区域**。

Guard 的安全检查清单:

// Guard 安全条件检查(概念化代码)
struct GuardConfig {
  Eigen::Vector3d fence_min;      // 地理围栏最小角 [x,y,z]
  Eigen::Vector3d fence_max;      // 地理围栏最大角 [x,y,z]
  double max_position_error;      // 最大位姿偏差(与 Guard 自身估计比较)
  double max_velocity;            // 最大速度限制
  double heartbeat_timeout;       // 心跳超时
  double max_tilt_angle;          // 最大倾斜角
};

enum class GuardDecision {
  PASS,          // 安全,不干预
  WARN,          // 警告但不接管
  TAKEOVER_HOVER, // 接管并悬停
  TAKEOVER_LAND,  // 接管并紧急降落
  KILL            // 直接关闭电机(最后手段)
};

GuardDecision check_safety(const State& pilot_state,
                            const State& guard_state,
                            const GuardConfig& cfg) {
  // 1. 心跳检查:Pilot 是否还活着?
  if (time_since_last_pilot_heartbeat() > cfg.heartbeat_timeout)
    return GuardDecision::TAKEOVER_HOVER;

  // 2. 地理围栏检查:是否在安全区域内?
  if (!is_inside_fence(guard_state.position, cfg.fence_min, cfg.fence_max))
    return GuardDecision::TAKEOVER_HOVER;

  // 3. 位姿偏差检查:Pilot 的估计和 Guard 的估计是否一致?
  //    (大偏差可能意味着 Pilot 的 VIO 漂移了)
  double pos_error = (pilot_state.position - guard_state.position).norm();
  if (pos_error > cfg.max_position_error)
    return GuardDecision::TAKEOVER_HOVER;

  // 4. 速度检查
  if (guard_state.velocity.norm() > cfg.max_velocity)
    return GuardDecision::TAKEOVER_HOVER;

  // 5. 姿态检查:倾斜角是否过大?
  double tilt = acos(guard_state.R.col(2).dot(Eigen::Vector3d::UnitZ()));
  if (tilt > cfg.max_tilt_angle)
    return GuardDecision::WARN;

  return GuardDecision::PASS;
}

Guard 的独立估计器:

Guard 不信任 Pilot 的状态估计——它运行自己独立的估计器: - 在 Motion Capture 环境中:直接使用 Vicon/OptiTrack 数据(外部真值) - 在 GPS 环境中:使用独立的 GPS+IMU 融合 - 在纯视觉环境中:使用独立的 VIO 实例

关键设计:Guard 的估计器必须比 Pilot 的更保守、更可靠——它可以精度更低,但绝不能漂移或崩溃。

Guard 接管后的行为:

接管触发 → 切断 Pilot 的控制输出 → 用 Guard 自身的估计器和简单 PD 控制器
         → 执行紧急悬停(减速到零)→ 如果悬停稳定 → 保持悬停等待操作员
         → 如果无法稳定悬停 → 紧急降落 → 如果降落也不行 → 关闭电机(KILL)

思考题:Guard 依赖什么信息?如果 VIO 和 mocap 同时失效(例如光照突变+mocap 遮挡),Guard 只剩 IMU 的惯性推算——在 3-5 秒内误差积累到米级。此时唯一安全的选择是 KILL(关闭电机让飞行器自由落体),因为带着错误估计继续飞可能比坠落更危险(飞出围栏撞人)。

Pilot 层——可插拔的算法流水线

Pilot 是敏捷飞行算法运行的主要层,其核心是一个可配置的 Pipeline:

// Pipeline 组合模式(概念化代码)
class Pipeline {
  std::unique_ptr<Estimator>   estimator_;    // 状态估计
  std::unique_ptr<Sampler>     sampler_;      // 参考轨迹采样
  std::unique_ptr<Reference>   reference_;    // 参考轨迹生成
  std::unique_ptr<Controller>  controller_;   // 控制器

public:
  Command run(const SensorData& data) {
    State state = estimator_->update(data);
    Reference ref = sampler_->sample(reference_->get(), state);
    return controller_->run(state, ref);
  }
};

Controller 工厂模式:通过 YAML 配置文件在运行时切换控制器:

# config/controller_mpc.yaml
controller:
  type: "MPC"
  horizon: 20
  dt: 0.05
  solver: "acados"
  weights:
    position: [10.0, 10.0, 10.0]
    velocity: [1.0, 1.0, 1.0]
    thrust: 0.01
    body_rate: 0.01

# config/controller_dfbc.yaml
controller:
  type: "DFBC"
  gains:
    position: [6.0, 6.0, 6.0]
    velocity: [4.0, 4.0, 4.0]
    attitude: [0.52, 0.52, 0.15]
  drag_compensation: true
  drag_coefficients: [0.4, 0.4, 0.1]

切换控制器只需更改配置文件路径——同一份编译好的二进制可以用完全不同的控制算法:

# 运行 MPC 控制器
roslaunch agilicious pilot.launch config:=config/controller_mpc.yaml

# 运行 DFBC 控制器(不需要重新编译)
roslaunch agilicious pilot.launch config:=config/controller_dfbc.yaml

C++17 技术细节:

Agilicious 大量使用 C++17 特性:

// std::variant 处理多消息类型
using SensorMessage = std::variant<ImuData, ImageData, PoseData, MotorData>;

void process(const SensorMessage& msg) {
  std::visit(overloaded{
    [this](const ImuData& imu)     { estimator_.processImu(imu); },
    [this](const ImageData& img)   { estimator_.processImage(img); },
    [this](const PoseData& pose)   { estimator_.processPose(pose); },
    [this](const MotorData& motor) { controller_.processMotor(motor); }
  }, msg);
}

// overloaded 辅助结构(C++17 聚合初始化 + 可变参模板)
template<class... Ts> struct overloaded : Ts... { using Ts::operator()...; };
template<class... Ts> overloaded(Ts...) -> overloaded<Ts...>;

// 组合模式的 Pipeline
class Pipeline {
  // 用 std::unique_ptr 持有多态组件
  std::unique_ptr<EstimatorBase> estimator_;
  std::unique_ptr<ControllerBase> controller_;

  // Factory 根据 YAML 配置创建具体实例
  static std::unique_ptr<ControllerBase> createController(const YAML::Node& cfg) {
    const auto type = cfg["type"].as<std::string>();
    if (type == "MPC")  return std::make_unique<MpcController>(cfg);
    if (type == "DFBC") return std::make_unique<DfbcController>(cfg);
    if (type == "INDI") return std::make_unique<IndiController>(cfg);
    throw std::runtime_error("Unknown controller type: " + type);
  }
};

Bridge 层——一份代码,多个目标

Bridge 是 Agilicious 的硬件抽象层,用统一接口对接不同的执行后端:

┌─────────────────────────────────────────────────┐
│                  Pilot (统一接口)                 │
│        Command = {thrust, body_rates}            │
└────────┬──────────────┬──────────────┬───────────┘
         ▼              ▼              ▼
   ┌──────────┐  ┌──────────┐  ┌──────────────┐
   │ RotorS   │  │Flightmare│  │  真机 UART   │
   │ Bridge   │  │  Bridge  │  │   Bridge     │
   │ (Gazebo) │  │ (Unity)  │  │ (BetaFlight) │
   └──────────┘  └──────────┘  └──────────────┘

// Bridge 基类
class BridgeBase {
public:
  virtual void sendCommand(const Command& cmd) = 0;
  virtual SensorData receiveSensors() = 0;
  virtual bool isConnected() const = 0;
};

// 真机 Bridge
class UartBridge : public BridgeBase {
  SerialPort serial_;
public:
  void sendCommand(const Command& cmd) override {
    // 将 [thrust, ω_x, ω_y, ω_z] 编码为 BetaFlight MSP 协议
    MSPPacket pkt = encode_msp_set_raw_rc(cmd);
    serial_.write(pkt.data(), pkt.size());
  }
};

// RotorS 仿真 Bridge
class RotorsBridge : public BridgeBase {
  ros::Publisher cmd_pub_;
public:
  void sendCommand(const Command& cmd) override {
    // 发布 ROS 消息到 RotorS 仿真器
    mav_msgs::Actuators msg;
    msg.angular_velocities = cmd_to_motor_speeds(cmd);
    cmd_pub_.publish(msg);
  }
};

// Flightmare 仿真 Bridge
class FlightmareBridge : public BridgeBase {
  zmq::socket_t socket_;
public:
  void sendCommand(const Command& cmd) override {
    // 通过 ZeroMQ 发送到 Flightmare Unity 仿真器
    FlightmareMsg msg = pack_command(cmd);
    socket_.send(zmq::message_t(msg.data(), msg.size()));
  }
};

统一 Bridge 的价值: 1. 算法开发流程:先在 RotorS(快速迭代)→ 再在 Flightmare(逼真渲染)→ 最后真机(验证) 2. 同一份代码:控制器/估计器代码完全相同,只切换 Bridge 配置 3. 自动化测试:CI/CD 中用 RotorS Bridge 做回归测试,无需真机 4. 安全验证:新算法必须先在仿真中通过 Guard 检查,才允许上真机

§D8.1.3 Agilicious 支撑的代表性研究

Agilicious 平台支撑了 UZH RPG 的一系列突破性工作:

项目	成果	在 Agilicious 上的角色
CPC (2021)	跑赢职业 FPV 飞手	提供 DFBC/MPC 跟踪 CPC 离线轨迹
NeuroBEM (2021)	混合气动建模	96 次飞行数据采集平台
Swift (2023)	Nature:击败世界冠军	RL 策略的真机部署平台
MPCC (2023)	60+ km/h 竞速	MPC 控制器 + Guard 安全保护
MPCC++ (2024)	80+ km/h, 100% 安全率	BEM 气动模型 + Guard
AC-MPC (2025)	可微 MPC + RL	acados MPC 嵌入 RL actor

---

§D8.2 CPC——推力极限下的时间最优飞行 ⭐⭐⭐

动机

有了 Agilicious 这个安全平台,第一个值得追问极限的问题是:给定一串航点,什么是物理上可能的最快轨迹? 这不是学术好奇——无人机竞速、灾害搜救的时间窗口、巡检效率,全都归结为"在推力极限内最快通过一串点"。但这个问题有一个反直觉的陷阱:你在 D3 学的 min-snap 轨迹很平滑、很优雅,却**远不是最快的**。CPC(互补进度约束)要给出的,是这个时间最优问题的第一个严格数学形式化。

反面:为什么不能直接用 min-snap,也不能简单"预先分配时间"

两条朴素路线都行不通。路线一(沿用 min-snap):min-snap 最小化 snap 积分,本质是追求"平滑/省力",它会让推力远离极限(利用率 ~50%)——而最快恰恰要求推力**贴着极限**。平滑和最快是两个方向相反的目标(见 §D8.2.1 的详细论证)。路线二(预先指定每个航点的到达时刻 \(t_1, t_2, \ldots\),然后在固定时间网格上优化):问题是**最优时间分配本身就是未知量**——你怎么知道第 3 个航点应该在第 2.7 秒还是第 3.1 秒到达?如果时间分配错了,在它上面求出的"最优"轨迹也是次优的。CPC 的突破就是:把时间分配也变成优化变量,用互补约束让优化器自己决定何时通过每个航点。

历史

时间最优控制的理论根基是 1950 年代 Pontryagin 的最大值原理——它早就告诉我们,带执行器上下限的时间最优解必然是 bang-bang(控制量在边界间切换)。但把这个原理应用到四旋翼这种 12 维非线性系统、还要处理"通过一串航点"的离散约束,长期没有可数值求解的形式。Foehn, Romero, Scaramuzza 在 2020(CPC 形式化,arXiv 2007.06255)/2021(Science Robotics)提出用**互补进度约束**把"通过航点"这个离散事件嵌入连续优化,并用世界最大的动作捕捉系统验证——CPC 生成的轨迹在真实赛道上**跑赢了职业 FPV 飞手**,这是机器人首次在竞速中击败人类专家。

引用:Foehn, Romero, Scaramuzza, "Time-Optimal Planning for Quadrotor Waypoint Flight", Science Robotics 6(56), 2021(早期 CPC 形式化见 arXiv 2007.06255, 2020)。

§D8.2.1 问题定义:时间最优航点飞行

目标:给定一组有序航点 {w₁, w₂, ..., w_N},求通过所有航点的**总时间最短**的轨迹。

这个问题在机器人学中极为基础——赛车、无人机竞速、时间关键任务都归结为此。但它的难度远超直觉:

为什么 min-snap 不是时间最优?

Min-snap 轨迹最小化 ∫‖σ⁽⁴⁾(t)‖²dt(snap 的积分),这产生平滑的多项式轨迹。但平滑 ≠ 最快:

min-snap 思维:    尽量平滑 → 电机指令缓和 → 远离推力极限
时间最优思维:      尽量激进 → 电机在极限上切换 → bang-bang 控制

考虑一个简单的一维加速问题(从静止到目标位置): - min-snap 解:平滑加速→巡航→平滑减速,推力利用率 ~50% - 时间最优解:最大推力加速→突然切换→最大反推减速(bang-bang),推力利用率 100%

CPC 的核心贡献是将这个直觉形式化为一个可以数值求解的优化问题。

§D8.2.2 CPC 的数学公式

决策变量: - 状态轨迹 x(t) = [p(t), v(t), q(t), ω(t)] — 位置、速度、姿态四元数、角速度 - 控制输入 u(t) = [f(t), τ(t)] — 集体推力、机体力矩 - 航段时间 T₁, T₂, ..., T_N — 每段的持续时间(优化变量!) - 进度变量 π_i(t) — 表示是否已通过航点 i 的二值变量(松弛为连续)

优化问题:

min  T_total = Σᵢ Tᵢ
s.t.
  // 动力学约束
  ṗ = v
  mv̇ = R·[0, 0, f]^T - mge₃              (牛顿方程)
  q̇ = 0.5 · q ⊗ [0, ω]                   (四元数运动学)
  Jω̇ = τ - ω × Jω                         (欧拉方程)

  // 执行器约束
  f_min ≤ f ≤ f_max                        (推力范围)
  |τ_i| ≤ τ_max                            (力矩范围)

  // 航点约束(互补性形式)
  ‖p(t) - wᵢ‖ ≥ r_wp · (1 - πᵢ(t))        (只有 πᵢ=1 时才强制到达)
  π̇ᵢ(t) ≥ 0                                (进度只增不减)
  πᵢ(t_final) = 1                           (最终必须通过)

  // 互补进度约束(CPC 的核心创新)
  π̇ᵢ(t) · (‖p(t) - wᵢ‖ - r_wp) = 0         (进度变化只在航点附近发生)

§D8.2.3 互补性约束的物理含义

互补性条件(Complementarity Condition):

π̇ᵢ(t) · (‖p(t) - wᵢ‖ - r_wp) = 0

这个条件说:两个量**不能同时非零**: - 如果 π̇ᵢ > 0(进度在变化):必须 ‖p - wᵢ‖ = r_wp(飞行器在航点附近) - 如果 ‖p - wᵢ‖ > r_wp(飞行器远离航点):必须 π̇ᵢ = 0(进度不变)

为什么需要互补性?

传统时间最优方法需要预先指定**时间分配**:航点 1 在 t₁ 秒到达,航点 2 在 t₂ 秒到达...但最优时间分配本身是未知的!

CPC 的创新:用进度变量 πᵢ 取代固定时间分配——优化器自动决定何时以何种顺序通过航点。互补性约束确保"通过航点"这个离散事件被嵌入到连续优化中。

推力互补性:

在时间最优问题中,最优控制理论(Pontryagin 最大值原理)告诉我们:最优推力必须在边界上:

f*(t) ∈ {f_min, f_max}    (bang-bang 控制)

等价地:

(f - f_min) · (f_max - f) = 0

物理含义:时间最优的飞行器**要么全力加速(f = f_max),要么最小推力/反推减速(f = f_min)**,不存在"中间推力巡航"的阶段——任何中间推力都意味着"我还有没用完的推力,所以我可以飞得更快"。

类比:CPC 的 bang-bang vs 职业赛车手的油门/刹车。CPC 的最优推力剖面**像**职业赛车手过赛道——直道上一脚油门到底(f = f_max),弯前在"最晚刹车点"全力制动(f = f_min),弯心一闪而过立刻重新全油门,绝不匀速巡航。两者都是 bang-bang,都源于同一个数学原理(在约束边界间切换才能时间最优)。但**不像**的地方在于:赛车手凭几千圈的肌肉记忆**逐弯反应**,而 CPC 是**全局**求解——它一次性优化所有航段的时间分配,知道第 5 个弯应该多快是为了第 7 个弯的最优进入姿态。这个类比仅在"控制信号形态(bang-bang)"上成立;赛车手的局部贪心与 CPC 的全局最优在决策范围上根本不同,这也正是 CPC 能比人类更快的原因之一。

本质洞察:CPC 的互补约束 \(\dot{\pi}_i \cdot (\|p - w_i\| - r_{wp}) = 0\) 在做一件深刻的事——把一个组合优化问题(航点的访问时刻是离散决策)松弛成连续优化。传统做法要么枚举所有时间分配(组合爆炸),要么固定时间分配(次优)。互补约束的巧妙在于:它用一个连续的乘积等于零的条件,精确编码了"进度只能在航点附近改变"这个逻辑事件,使得连续优化器(IPOPT)能间接搜索离散的访问安排。代价是——乘积等于零的约束**非凸且非光滑**(可行集是两条相交直线的并,而非凸集),这正是 CPC 必须离线、且对初始猜测极度敏感的数学根源(§D8.2.4)。天下没有免费的午餐:把离散问题塞进连续优化,换来的是非凸性。

§D8.2.4 CPC 的数值求解

CPC 使用 CasADi + IPOPT 离线求解:

import casadi as ca

# CasADi 符号变量定义
opti = ca.Opti()

# 决策变量
N_segments = 5       # 航段数
N_knots = 50         # 每段配点数
dt = opti.variable(N_segments)  # 每段时间(优化变量)

# 状态和控制轨迹
p = opti.variable(3, N_segments * N_knots)   # 位置
v = opti.variable(3, N_segments * N_knots)   # 速度
q = opti.variable(4, N_segments * N_knots)   # 四元数
w = opti.variable(3, N_segments * N_knots)   # 角速度
f = opti.variable(1, N_segments * N_knots)   # 推力
tau = opti.variable(3, N_segments * N_knots) # 力矩

# 进度变量
pi_var = opti.variable(N_waypoints, N_segments * N_knots)

# 目标函数:最小化总时间
opti.minimize(ca.sum1(dt))

# 约束:动力学(用直接配点法)
for k in range(N_segments * N_knots - 1):
    # 状态传播
    h = dt[k // N_knots] / N_knots
    # 位置导数 = 速度
    opti.subject_to(p[:, k+1] == p[:, k] + h * v[:, k])
    # 速度导数 = 推力/质量 - 重力
    R_k = quat_to_rotmat(q[:, k])
    opti.subject_to(v[:, k+1] == v[:, k] + h * (R_k @ ca.vertcat(0, 0, f[k]) / m - g_vec))
    # ... 四元数和角速度动力学类似

# 约束:推力范围
opti.subject_to(opti.bounded(f_min, f, f_max))

# 约束:互补进度
for i in range(N_waypoints):
    for k in range(N_segments * N_knots):
        dist = ca.norm_2(p[:, k] - waypoints[:, i])
        pi_dot = (pi_var[i, k+1] - pi_var[i, k]) / h if k < N_segments * N_knots - 1 else 0
        # CPC 互补性约束
        opti.subject_to(pi_dot * (dist - r_wp) == 0)
        opti.subject_to(pi_dot >= 0)
    # 最终进度 = 1
    opti.subject_to(pi_var[i, -1] == 1)

# 求解
opti.solver('ipopt', {'ipopt.max_iter': 5000, 'ipopt.tol': 1e-6})
sol = opti.solve()

求解特性: - 非实时:求解时间 5-60 秒(取决于航点数和配点密度) - 高度非凸:互补性约束使 NLP 有大量局部最优 - 初始化敏感:需要好的初始猜测(通常用 min-snap 轨迹作为初始化) - 收敛策略:先放松互补性约束(penalty method),逐步收紧到精确互补

与 min-snap 的对比:

特性	Min-Snap	CPC
代价函数	∫‖snap‖²dt(平滑)	T_total(最快)
推力分布	远离极限,平滑	bang-bang,在极限切换
时间分配	预先指定或启发式	由优化器自动分配
航点约束	硬约束在固定时间	互补约束,灵活时间
求解速度	毫秒级(QP)	秒级(NLP)
实时性	可在线	离线
轨迹特征	平滑多项式	激进,有急转弯

§D8.2.5 CPC 的实验验证:跑赢职业飞手

UZH RPG 在 Motion Capture 环境中设置了包含多个航点的赛道,让 CPC 生成的时间最优轨迹与**职业 FPV 飞手**的手飞轨迹进行对比:

实验设置: - 赛道:7 个航点(门),室内 mocap 环境 - 飞行器:Agilicious 平台,TWR ≈ 4.5:1 - 对比对象:经验丰富的 FPV 竞速飞手 - CPC 轨迹离线生成,由 DFBC/MPC 在线跟踪

结果: - CPC 轨迹单圈时间:比职业飞手快约 0.5-1 秒 - CPC 达到的最高速度:~19 m/s (~70 km/h) - CPC 的推力分布:明显的 bang-bang 特征——加速段推力接近 f_max,减速段推力接近 f_min

为什么 CPC 能赢人类?

人类飞手的限制: 1. 反应时间:~200 ms 视觉-运动延迟(vs CPC 预先计算好的最优轨迹) 2. 非最优操控:人类本能追求"平滑"而非"极限"——很少真正把推力推到 100% 3. 保守弯道:人类会在弯前提前减速留安全裕度,CPC 精确计算最晚减速点 4. 缺乏全局优化:人类逐弯决策,CPC 全局优化所有航段时间分配

CPC 的限制: 1. 离线计算:不能在线适应(门位置变化需要重新求解) 2. 完美模型假设:CPC 假设精确的动力学模型(实际需要 MPC/DFBC 补偿) 3. 需要精确航点:依赖 mocap 提供精确的门位置

这些限制正是 Swift(Nature 2023)用 RL 端到端学习来解决的——RL 不需要预先知道门位置,也不需要精确模型。

---

§D8.3 IPC——规划与控制的统一 QP ⭐⭐⭐

动机

CPC(§D8.2)解决的是"已知航点、离线、最快"。但真实自主飞行往往是**在线**的——障碍可能突然出现,环境在变。这暴露出经典自主飞行架构的一个根本裂缝:规划器和控制器是两个分离的模块,各自优化不同目标、运行在不同频率,中间靠一条"参考轨迹"传话。当环境一变,这条传话的参考就可能不可行,而控制器对此一无所知。IPC(集成规划与控制)要做的,是把这道裂缝从架构层面缝死——让"往哪飞"和"怎么飞"在同一个优化问题里同时决定。

反面:分离式管线在动态环境下会怎样

设想经典的"感知→规划→控制"流水线在飞行中遇到一个突然走进航线的行人。规划器在 t=0 给出的参考是安全的,但它要 100-200 ms 后才重规划;在这之间,控制器(传统 MPC)只盯着下一个参考点,它根本不知道走廊边界在哪,于是忠实地朝着已经穿过行人的参考点飞过去——直到碰撞。即便重规划及时,规划器优化的是"路径短/平滑"(不管动力学),控制器优化的是"跟踪准"(不管环境),两个目标不一致导致全局次优。根本病在于:安全约束(走廊)只活在规划器里,而真正下指令的控制器看不到它。IPC 的解法直截了当——把 SFC 走廊约束**下沉**进控制器的 QP,让每个 100 Hz 的控制周期都亲自保证"不出走廊"。

历史

把约束"下沉"到底层优化器并非无人机独创——足式机器人的全身控制器(WBC)早就把接触力、关节限位、末端任务塞进一个 QP 同时求解(§D8.3.5 会详细对比)。IPC(Liu, Ren, Zhang,香港大学 MaRS 实验室,RA-L 2023)把这个"约束下沉"思想用到无人机导航:它复用 D5 的安全飞行走廊(SFC)生成,把走廊的半空间约束和线性化 MPC 动力学放进**同一个 OSQP**,在 100 Hz 求解。它的标志性能力是应对"突然出现的物体和扰动"——这正是分离式管线最脆弱的场景。

引用:Liu, Ren, Zhang(HKU MaRS), "Integrated Planning and Control for Quadrotor Navigation in Presence of Suddenly Appearing Objects and Disturbances", RA-L 2023.

GitHub: hku-mars/IPC, ~244 stars.

§D8.3.1 传统管线的根本问题

传统无人机自主飞行的系统架构是**分离式管线**:

感知 → 建图 → 规划器(生成参考轨迹) → 控制器(跟踪参考) → 执行器
         ↑         └─────────↓───────────┘           ↓
      环境模型          模块间接口             电机指令

问题 1:参考不可行

规划器在 t=0 时基于当前环境生成参考轨迹。但在执行过程中: - 环境变化:突然出现的障碍物使参考轨迹穿过障碍 - 控制器只看到下一个参考点,不知道走廊约束——它会试图"穿过"障碍

时间 t=0:规划器生成安全轨迹(避开已知障碍)
时间 t=0.5s:新障碍出现在参考轨迹上
控制器此时:
  传统 MPC → 继续跟踪参考(直到碰撞)
  IPC → 在走廊内自动偏离参考(安全避障)

问题 2:频率不匹配

规划器通常运行在 5-20 Hz(重新规划),控制器运行在 100-500 Hz。在两次重规划之间的 50-200 ms 内,控制器只能跟踪"可能已经不安全"的旧参考。

问题 3:全局次优

规划器和控制器各自优化不同的代价函数: - 规划器:最小化路径长度/时间/平滑度(不考虑动力学限制) - 控制器:最小化跟踪误差(不考虑环境约束)

两个模块的**目标不一致**导致全局次优。

§D8.3.2 IPC 的核心思想:统一 QP

IPC 的关键洞见:把安全走廊约束和 MPC 动力学约束放进同一个 QP,让优化器同时考虑"往哪飞"(规划)和"怎么飞"(控制)。

系统架构:

感知 → 建图 → A* 路径 → SFC 生成 → ┌─────────────────────┐
                                     │ 统一 QP (OSQP)      │
                                     │                     │
                    参考 →            │  min MPC 代价       │
                                     │  s.t. 动力学约束    │ → 电机指令
                                     │       SFC 约束      │
                                     │       执行器约束    │
                                     └─────────────────────┘
                                           100 Hz

IPC 的 QP 公式:

min  Σ_{k=0}^{N-1} [‖x_k - x_ref,k‖²_Q + ‖u_k‖²_R] + ‖x_N - x_ref,N‖²_P
s.t.
  x_{k+1} = A·x_k + B·u_k           (线性化动力学)
  A_sfc,i · p_k ≤ b_sfc,i    ∀k ∈ segment_i    (安全走廊约束)
  u_min ≤ u_k ≤ u_max                 (执行器约束)

变量定义: - 状态 x = [p, v, a]^T ∈ R⁹ (位置、速度、加速度)——注意用加速度而非姿态,利用微分平坦! - 控制 u = jerk ∈ R³ (加加速度)——对应 snap 的积分,直接关联电机 - 预测步长 N ≈ 20-40 步,dt = 0.01-0.05s

SFC 约束的物理含义:

安全飞行走廊(Safe Flight Corridor)由一系列凸多面体组成,每个多面体用半平面表示:

A_sfc · p ≤ b_sfc

其中每行 aᵢ^T · p ≤ bᵢ 表示一个半平面约束(位置在某个平面的一侧)。

IPC 把这些约束**直接注入到 MPC 的 QP 中**——MPC 在优化跟踪误差的同时,自动保证飞行器不离开走廊。

本质洞察:IPC 的核心不是发明新算法,而是一次**职责重新划分**。传统架构把"决定走廊"(规划器)和"在走廊内跟踪"(控制器)切成两个模块,接口是一条可能过期的参考轨迹;IPC 认识到——既然控制器本来就在解一个 QP,而 SFC 约束也是线性的(半空间),那就**没有理由把它们分开**,合进一个 QP 反而更简单、更安全、还消除了接口不一致。这揭示了一个反复出现的工程模式:当两个模块的数学形式兼容(都是线性约束的凸优化),分模块往往是历史包袱而非必要设计。但这个洞察有边界——IPC 用的是**线性化**动力学和**线性** SFC 才能保持 QP 凸性;如果约束本质非线性(如 CPC 的互补约束),硬塞进一个问题就会破坏凸性、丢掉实时性。融合的前提是"数学形式相容"。

§D8.3.3 IPC 的 OSQP 实现细节

QP 标准形式:

IPC 的统一 QP 可以写成 OSQP 的标准形式:

min  0.5 · z^T · H · z + g^T · z
s.t. l ≤ A · z ≤ u

其中 z = [x₀, u₀, x₁, u₁, ..., x_N] 是所有时步的状态和控制的拼接。

H 矩阵(Hessian):块对角,Q/R/P 权重矩阵

H = diag(Q, R, Q, R, ..., Q, R, P)
      ↑  ↑  ↑  ↑       ↑  ↑  ↑
      x₀ u₀ x₁ u₁     x_{N-1} u_{N-1} x_N

A 矩阵(约束):包含三类约束:

     ┌ 动力学: x_{k+1} = A·x_k + B·u_k  (等式约束) ┐
A =  │ SFC:    A_sfc · p_k ≤ b_sfc       (不等式)    │
     └ 执行器: u_min ≤ u_k ≤ u_max        (box 约束) ┘

稀疏结构:

A 矩阵的稀疏模式:
┌ I  0  0 ... -A -B  0  0 ... ┐  ← 动力学 k=0
│ 0  I  0 ...  0  0 -A -B ... │  ← 动力学 k=1
│ ...                          │
│ S₀ 0  0 ...  0  0  0  0 ... │  ← SFC 段 0 的约束
│ 0  0  S₁ ... 0  0  0  0 ... │  ← SFC 段 1 的约束
│ ...                          │
│ 0  I  0 ...  0  0  0  0 ... │  ← u₀ box 约束
│ ...                          │
└                              ┘

OSQP 求解性能:

配置	变量数	约束数	求解时间
N=20, nx=9, nu=3	240+60=300	~600-800	0.5-2 ms
N=40, nx=9, nu=3	480+120=600	~1200-1600	2-5 ms

在 Intel NUC i7 上,100 Hz 运行完全可行。

§D8.3.4 IPC vs 传统 MPC 的行为差异

场景:突然出现的障碍

时间 t=0:
  参考轨迹: ----→----→----→  (直线飞向目标)
  环境:     [   空旷   ]

时间 t=0.5s:
  障碍出现: ----→--[X]→----→  (参考穿过障碍)

传统 MPC 行为:
  控制器: ----→--[X]💥  (试图跟踪不安全的参考,碰撞)

IPC 行为:
  控制器: ----→--↗----→  (在 SFC 内自动绕行)
                 ↑ SFC 约束迫使轨迹偏离参考

关键差异:IPC 的 MPC 知道走廊在哪——当参考轨迹指向走廊外时,SFC 约束强制 MPC 寻找走廊内的最优路径。传统 MPC 没有这个信息,只能盲目跟踪参考。

§D8.3.5 IPC 与足式机器人 WBC 的类比

IPC 把规划约束(SFC)放进 MPC;足式机器人的 WBC(全身控制器)把任务约束(末端位置)放进 QP。两者在"约束集成到优化问题"这一设计模式上的共通点:

对比维度	IPC (无人机)	WBC (足式)
约束类型	SFC(空间安全约束)	末端位姿、接触力、关节限位
动力学	线性化飞行动力学	浮动基座刚体动力学
QP 求解器	OSQP	qpOASES / EIQUADPROG
频率	100 Hz	200-1000 Hz
核心思想	同一个 QP 里同时处理"去哪里"和"怎么去"
好处	避免模块间接口的不一致性

类比:IPC 之于无人机,像 WBC 之于足式机器人。IPC 把"规划层的安全走廊"下沉进控制器的 QP,像**足式 WBC 把"任务层的末端目标、接触约束、关节限位"全塞进一个 QP 同时求解——两者都遵循"约束下沉、消除模块接口"的同一设计哲学,都用 QP 作为统一求解器。但**不像**的地方在于:WBC 处理的是**浮动基座刚体动力学 + 接触力锥(约束更丰富、含非线性摩擦锥,常需分层 QP 或松弛),而 IPC 处理的是**线性化飞行动力学 + 线性半空间 SFC**(约束更简单、保持严格凸 QP)。这个类比在"设计模式"层面高度一致,但在"约束复杂度和动力学维度"上差异显著——四旋翼是单刚体无接触,足式是多接触切换的混合系统。把握这一点,就能理解为什么 IPC 能稳定跑 100 Hz 而 WBC 往往要更精巧的求解器工程。

---

§D8.4 MPCC——进度最大化替代轨迹跟踪 ⭐⭐⭐

动机

CPC 给出离线最优轨迹,但它**不能在线**(秒级求解)。如果想在飞行中实时地"自适应地快慢有度"——直道狂飙、弯道收油、还能应对门位置的小扰动——就需要一个在线方法。直接想法是"用 MPC 跟踪 CPC 算好的时间最优轨迹",但这里藏着一个微妙的失败:时间参数化的跟踪在飞行器跟不上参考时会崩。MPCC(模型预测轮廓控制)换了一个根本视角——不跟踪"某时刻该在哪",而是最大化"沿路径走了多远",从而把"该多快"这件事交给优化器自己决定。

反面:为什么时间参数化的轨迹跟踪在竞速中会崩

设想你让 MPC 跟踪一条时间参数化参考 \(p_{ref}(t)\)。竞速中飞行器在弯道因推力饱和而短暂落后,但**时间 \(t\) 不会等它**——参考点 \(p_{ref}(t)\) 继续沿轨迹前进,跑到了飞行器前方很远。于是跟踪误差 \(\|p(t) - p_{ref}(t)\|\) 急剧增大,MPC 拼命想"追上"这个跑掉的参考点,可能抄近道切弯(撞墙)或下达饱和指令(失控)。问题的根源是:时间参数化把"位置"和"时间"硬绑在一起,而竞速恰恰需要让飞行器自由调整速度。MPCC 的解法是解绑——用**弧长 \(\theta\)**(走了多远)参数化路径,\(\theta\) 是优化变量而非外部时钟,飞行器慢下来时 \(\theta\) 也跟着慢,参考点永远在飞行器"应该在"的地方,绝不会跑掉。

历史

MPCC 不是无人机原生的——它 2014 年由 Liniger, Domahidi, Morari 在 CDC 提出,用于**自动驾驶微缩赛车**(1:43 模型车),核心是"沿赛道中心线最大化进度 + 惩罚偏离"。Romero, Sun, Foehn, Scaramuzza(T-RO 2023)把它从 2D 自行车模型移植到 3D 四旋翼——把赛道中心线换成 3D 参考路径、赛道边界换成 Frenet-Serret 管状隧道,实现 60+ km/h 竞速。Krinner 等(MPCC++,2024)进一步加上 NeuroBEM 气动模型和 TuRBO 贝叶斯自动调参,把成功率做到 100%、速度推到 80+ km/h。

引用: - Romero, Sun, Foehn, Scaramuzza, "Model Predictive Contouring Control for Time-Optimal Quadrotor Flight", T-RO 2023. - Krinner, Romero, Sun, Scaramuzza, "MPCC++: Model Predictive Contouring Control for Time-Optimal Flight with Safety Constraints", arXiv 2024. - (思想源头)Liniger, Domahidi, Morari, "Optimization-Based Autonomous Racing of 1:43 Scale RC Cars", CDC 2014 / Optimal Control Applications and Methods 2015.

§D8.4.1 轨迹跟踪 vs 轮廓跟踪:范式转换

传统 MPC 轨迹跟踪:

参考轨迹是**时间参数化**的:p_ref(t),控制器跟踪时间对应的参考点。

跟踪误差:e(t) = p(t) - p_ref(t)
代价函数:min Σ ‖p_k - p_ref(k·dt)‖²

问题:如果飞行器比参考慢(比如弯道减速了),时间继续前进,参考点跑到前面去了——误差积累,控制器试图"追赶"参考,可能导致走捷径或失控。

MPCC 轮廓跟踪:

参考路径是**弧长参数化**的:p_ref(s),进度 s(t) 是优化变量:

e_lag = 沿路径方向的滞后误差(进度偏差)
e_con = 垂直于路径方向的轮廓误差(偏离路径)

代价函数:min −w_s·Δs + w_c·e_con² + w_u·‖u‖²
              ↑ 最大化进度  ↑ 最小化偏离  ↑ 平滑控制

物理含义:MPCC 不关心飞行器在**什么时候**到达哪个点——它只关心: 1. 尽量快地沿路径前进(最大化 Δs) 2. 不要偏离路径太远(最小化 e_con) 3. 控制指令平滑(最小化 ‖u‖²)

§D8.4.2 MPCC 的完整公式

弧长参数化:

给定路径 p_ref(s),s ∈ [0, S_total],MPCC 引入进度变量 θ(t) 表示飞行器沿路径的位置:

参考点:  p_ref(θ(t))
切向量:  t(θ) = dp_ref/ds |_{s=θ}
法向量:  n(θ) = dt/ds / ‖dt/ds‖ (Frenet-Serret 框架)

误差分解:

e_total = p(t) - p_ref(θ(t))
e_lag = e_total · t(θ)         // 沿路径方向的误差(滞后/超前)
e_con = e_total - e_lag · t(θ)  // 垂直路径方向的误差(轮廓偏离)

完整 MPCC 优化问题:

min  Σ_{k=0}^{N-1} [−w_θ·Δθ_k + w_c·‖e_con,k‖² + w_l·e_lag,k²
                     + ‖u_k‖²_R + w_Δu·‖Δu_k‖²]
s.t.
  // 飞行器动力学(全非线性)
  x_{k+1} = f_dyn(x_k, u_k, dt)

  // 进度动力学(θ 也是状态)
  θ_{k+1} = θ_k + v_θ,k · dt
  v_θ,k ≥ 0                    (进度不能后退)

  // 执行器约束
  f_min ≤ f_k ≤ f_max
  |ω_k| ≤ ω_max

  // 隧道约束(Frenet-Serret 框架)
  ‖e_con,k‖ ≤ r_tunnel(θ_k)    (必须在隧道内)

隧道约束(Tunnel Constraint):

MPCC 在参考路径周围构建一个**管状安全区域**——飞行器必须在管内飞行。管半径 r_tunnel 可以沿路径变化:

直道段:r_tunnel 较大(允许切弯)
弯道段:r_tunnel 较小(防止偏离太远)
门通过段:r_tunnel = 门的半径(精确穿过)

§D8.4.3 MPCC 的自适应快慢机制

MPCC 的核心优势是**无需预先时间分配即可自适应快慢**:

直道行为: - 轮廓误差 e_con 小(路径直,容易跟踪) - 优化器将更多权重分配给进度最大化 −w_θ·Δθ - 结果:自动加速

弯道行为: - 轮廓误差 e_con 大(弯道跟踪困难) - 优化器压低进度 Δθ 以减少轮廓误差 - 结果:自动减速

直道:  Δθ ↑↑   e_con ↓     → 飞得快
弯道:  Δθ ↓    e_con ↓↓    → 飞得慢(保证不偏离)
出弯:  e_con 减小 → Δθ 恢复 → 重新加速

这完全类似于赛车中的**赛道线优化**——车手在直道上全油门,在弯前刹车,弯心低速,出弯加速。MPCC 用数学优化自动实现了这个策略。

本质洞察:MPCC 的"快慢自适应"不是一条额外加的规则,而是**两个相互拉扯的代价项自动平衡的涌现结果**。进度项 \(-w_\theta \Delta\theta\) 想让飞行器尽量快(贪婪前进),轮廓项 \(w_c \|e_{con}\|^2\) 想让飞行器贴紧路径(不许偏离)。在直道上贴紧路径很容易,进度项主导 → 加速;在弯道上想快就会偏离,轮廓项的二次惩罚迅速增大并压制进度项 → 减速。没有任何地方写"弯道要减速"——它是优化器在两个矛盾目标间求平衡时自然涌现的。这和 CPC 形成有趣对照:CPC 用硬约束(推力边界)逼出 bang-bang 的"硬"最优;MPCC 用软代价的权衡逼出"软"的快慢自适应。前者求得离线全局最优但不能实时;后者牺牲全局最优性换来在线实时与对扰动的鲁棒。调权重 \(w_\theta/w_c\) 的比值,本质就是在"激进"和"安全"之间设定飞行器的性格——这也是为什么 MPCC++ 要用 TuRBO 自动调参,因为这个比值的最优值随赛道而变。

§D8.4.4 MPCC 的实际性能

原始 MPCC (Romero 2023): - 最高速度:60+ km/h - 加速度:>5g - 求解频率:50-100 Hz(使用 acados) - 圈速:超过职业 FPV 飞手

MPCC++ (Krinner 2024):

在 MPCC 基础上增加三个关键组件:

a) BEM 气动模型(NeuroBEM):

在 20+ m/s 下,气动力是推力的 20-30%:
标准 MPCC:不建模气动 → 模型失配 → 控制器补偿滞后 → 性能下降
MPCC++:  NeuroBEM 提供精确气动预测 → 模型匹配 → 前馈补偿 → 性能提升

b) TuRBO 贝叶斯超参数调优:

MPCC 有大量超参数:w_θ, w_c, w_l, R, r_tunnel, ...

手动调参: - 耗时:数天到数周 - 依赖经验:不同赛道需要不同参数 - 容易过拟合:在一条赛道上调好的参数换赛道就不行

TuRBO(Trust Region Bayesian Optimization): - 自动在真机上测试不同参数组合 - 用高斯过程建模"参数→圈速"映射 - 在置信区域内搜索最优参数 - 通常 50-100 次试验找到接近最优的参数

c) 安全约束强化:

MPCC++ 添加了门碰撞约束(不仅是隧道约束):

‖p(θ_gate) - gate_center‖ ≤ gate_radius    (通过门时的精确约束)

MPCC++ 实验结果: - 最高速度:>80 km/h - 100% 真机成功率(无碰撞通过所有门) - 比 MPCC 平均快 5-10%

§D8.4.5 MPCC 与自动驾驶赛车的联系

MPCC 最初来自**自动驾驶赛车**领域(Liniger, Domahidi, Morari, CDC 2014),后被 Romero 等移植到四旋翼:

对比	赛车 MPCC	四旋翼 MPCC
动力学	自行车模型(2D)	6-DOF 刚体(3D)
路径参数化	赛道中心线	3D 参考路径
隧道约束	赛道边界	3D 管状安全区域
进度变量	沿赛道弧长	沿 3D 路径弧长
关键差异	地面摩擦限制	推力/力矩限制

---

§D8.5 DFBC 与 INDI 控制器详解 ⭐⭐

动机

CPC(§D8.2)产出一条离线轨迹,MPCC(§D8.4)在线优化"该往哪走",但它们最终都得**有人把指令真正送到电机**——这是底层控制器(low-level controller)的活。一个自然的问题是:既然 MPC 已经能输出控制,为什么还需要 DFBC、INDI 这些"额外"的控制器?答案是**计算预算和鲁棒性的分工**。MPC 求一次 QP/NLP 要 0.5-10 ms,在 Crazyflie 这种巴掌大的飞行器上跑不动;而强扰动/未知载荷下,再精确的模型也会失配。DFBC 用微分平坦把前馈算到极快(<0.1 ms),INDI 用传感器读数把扰动**直接消掉**——它们各自补的是 MPC 的短板。

反面:只用一个"全能 MPC"会怎样

设想你固执地只用 NMPC 做所有控制,不要任何底层控制器。在资源受限平台(Crazyflie,微控制器)上,MPC 的 QP 求解器根本塞不进 192 KB RAM,飞机直接起不来。在强阵风/抓取未知重物时,MPC 依赖的标称模型失配,它的预测偏离现实,控制性能骤降——而它没有任何机制去**在线消除**这个未建模扰动(标准 MPC 只能靠加积分项或扰动观测器打补丁)。这就是为什么实践中常是**分层**的:外环用 MPC/规划器决定"去哪",内环用 DFBC(快)或 INDI(抗扰)把指令落地。理解这两个控制器,才理解 Agilicious 的 Pilot 为什么把 controller 做成可插拔的工厂(§D8.1.2)。

§D8.5.1 DFBC:微分平坦前馈控制器

DFBC (Differential Flatness-Based Controller) 的核心思想:利用微分平坦性,从参考轨迹**直接计算前馈控制量**(推力、姿态、角速度),再加上 PD 反馈补偿误差。

引用:Faessler, Franchi, Scaramuzza, "Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag for Accurate Tracking of High-Speed Trajectories", RA-L 2018.

控制器结构:

参考轨迹 σ_ref(t) = [p_ref, v_ref, a_ref, j_ref, s_ref, ψ_ref, ψ̇_ref]
                      位置   速度   加速度  jerk   snap  偏航  偏航率

                     ┌──────────────┐
σ_ref(t) ──────────→ │  平坦映射     │ → f_ff, R_ff, ω_ff    (前馈)
                     └──────────────┘
                            ↓
当前状态 x ─→ ┌──────────────────┐
              │  PD 反馈 + 前馈    │ → f_cmd, ω_cmd  → 电机指令
              └──────────────────┘

数学公式:

外环(位置控制):

// 期望加速度 = 前馈加速度 + PD 反馈
a_des = a_ref + K_v·(v_ref - v) + K_p·(p_ref - p)

// 考虑旋翼阻力的推力方向
z_B_des = (m·a_des + m·g·e₃ + D·v) / ‖m·a_des + m·g·e₃ + D·v‖

// 推力大小
f_cmd = (m·a_des + m·g·e₃ + D·v)^T · R·e₃

内环(姿态控制):

// 期望姿态由 z_B_des 和 ψ_ref 确定
R_des = reconstruct_R(z_B_des, ψ_ref)

// 姿态误差(SE(3) 几何控制风格)
e_R = 0.5 · vee(R_des^T · R - R^T · R_des)

// 角速度前馈(从 jerk 计算)
ω_ff = flatness_to_body_rates(R_des, j_ref, f_cmd)

// 角速度指令
ω_cmd = ω_ff + K_R · e_R

DFBC 的计算成本:

平坦映射:~10 次矩阵乘法 + 1 次归一化 → <0.01 ms
PD 反馈:~5 次向量运算 → <0.001 ms
总计:~0.02 ms → 可在 >10 kHz 运行

对比 MPC 的 0.5-5 ms,DFBC 快 50-500 倍——这是它在资源受限平台(如 Crazyflie)上的关键优势。

DFBC 的局限:

DFBC 假设:参考轨迹在执行器约束内可行
现实:高速飞行时,参考轨迹可能要求超过 f_max 的推力

DFBC 的应对:裁剪 f_cmd 到 [f_min, f_max]
问题:裁剪后的控制不再"最优"——可能偏离参考越来越远,无法恢复

MPC 的应对:在优化中显式处理约束 → 自动寻找约束下的最优解

Sun, Romero, Foehn, Scaramuzza (T-RO 2022) 的系统对比结论: - 中低速(<10 m/s):DFBC 和 MPC 性能接近,DFBC 计算量优势明显 - 高速(>15 m/s):MPC 显著优于 DFBC,因为 MPC 能预见并处理推力饱和 - 不可行参考:DFBC 跟踪失败,MPC 自动偏离到最近可行轨迹

§D8.5.2 INDI:增量非线性动态逆

INDI (Incremental Nonlinear Dynamic Inversion) 是一种**传感器驱动**的控制方法,通过使用**当前传感器读数**(特别是角加速度)来减少对模型的依赖。

传统 NDI vs INDI:

传统 NDI(模型驱动):
  u = f_model⁻¹(ẍ_des)     // 需要精确的完整模型

INDI(传感器驱动):
  Δu = f_model⁻¹(ẍ_des - ẍ_measured)  // 只需要模型的增量关系
  u = u_prev + Δu                       // 在上一步控制的基础上增量调整

INDI 的数学推导:

从角动力学出发:

Jω̇ = τ - ω × Jω + d_ext    (d_ext = 外部扰动)

在时间步 Δt 内,假设 d_ext 变化缓慢:

Jω̇_des = τ_des - ω × Jω + d_ext
Jω̇_0   = τ_0   - ω × Jω + d_ext    (上一步的实际动力学)

相减:

J(ω̇_des - ω̇_0) = τ_des - τ_0

即:

Δτ = J · (ω̇_des - ω̇_measured)
τ_cmd = τ_prev + Δτ

关键洞见:INDI 不需要知道 d_ext(外部扰动),因为扰动在增量方程中被消去了!这就是 INDI 对模型不确定性鲁棒的原因。

INDI 的实现要求:

输入:
  - ω̇_measured: 角加速度(需要从陀螺仪差分或通过加速度计估计)
  - ω̇_des: 期望角加速度(从外环控制器)
  - J: 惯量矩阵(仍需要合理精度)
  - τ_prev: 上一步的力矩指令

输出:
  - τ_cmd = τ_prev + J · (ω̇_des - ω̇_measured)

角加速度估计的挑战:

INDI 需要**角加速度 ω̇** 的实时估计,这通常通过以下方法获得:

方法一:陀螺仪有限差分

ω̇ ≈ (ω(t) - ω(t-Δt)) / Δt    // 噪声大,需要低通滤波

方法二:基于模型的估计

ω̇_est = J⁻¹ · (τ_cmd_prev - ω × Jω)    // 依赖模型,但可用传感器修正

方法三:专用传感器(高端 IMU)

一些高端 IMU 直接输出角加速度,但成本高且重量大

INDI 的性能特征:

特性	INDI	DFBC	MPC
模型依赖	低(只需 J)	中(需完整模型)	高(需全动力学)
扰动抑制	强(隐式消除)	弱(依赖前馈精度)	中(需要扰动模型)
计算成本	<0.1 ms	<0.1 ms	0.5-5 ms
约束处理	无(需外部裁剪)	无(需外部裁剪)	有(显式约束)
适用场景	未知扰动环境	已知模型+温和飞行	约束活跃飞行

在 Agilicious 中的 INDI 配置:

controller:
  type: "INDI"
  gains:
    outer_loop:
      position: [5.0, 5.0, 5.0]
      velocity: [3.0, 3.0, 3.0]
    inner_loop:
      attitude: [200.0, 200.0, 50.0]   # 角加速度增益
  angular_accel_filter:
    type: "butterworth"
    order: 2
    cutoff_freq: 30.0   # Hz, 陀螺仪差分的低通滤波
  inertia: [0.0015, 0.0015, 0.003]   # Ixx, Iyy, Izz

---

§D8.6 感知流水线:敏捷飞行中的高频状态估计 ⭐⭐⭐

动机

前面所有控制器(DFBC/MPC/CPC/MPCC)都假设一件事:它们能拿到准确的当前状态(位置、速度、姿态)。但这个状态从哪来?在实验室靠动作捕捉(mocap)作弊,在真实世界必须靠机载感知。而敏捷飞行对感知的要求是**双重的、且常常矛盾的**——既要高精度,又要低延迟、高频率。本节讲清楚:为什么常规 VIO 在 20+ m/s 下会失效、高频状态估计如何用 EKF/MSCKF 实现、以及竞速场景怎么检测和利用"门"。

反面:用常规飞行的感知去做敏捷飞行会怎样

设想你把消费级无人机的感知栈(30 Hz 帧相机 + 100 ms 端到端延迟的视觉里程计)直接用于 20 m/s 竞速。三件事会同时崩:(1) 运动模糊——1 ms 曝光内飞行器移动 20 mm,特征点在图像上拖尾 20 像素,角点检测器直接失效,VIO 丢失跟踪;(2) 延迟致命化——100 ms 延迟在 20 m/s 下等于 2 米位置误差,控制器拿到的是"两米前的我",据此下的指令必然过时;(3) 更新太稀——30 Hz 意味着每 33 ms 才更新一次状态,而高速下 33 ms 足以让飞行器姿态变化很大。结论是:敏捷飞行的感知不是"把常规感知调快一点",而是要从传感器(全局快门/事件相机)、估计器(高频 IMU 传播 + MSCKF)、到延迟补偿(IMU 前向传播)全链路重新设计——这正是本节要拆解的。

§D8.6.1 状态估计需求分析

需求	常规飞行	敏捷飞行	理由
位置精度	<0.3 m	<0.1 m	穿越狭小空间
速度精度	<0.5 m/s	<0.2 m/s	高速下微小速度误差累积快
姿态精度	<5°	<2°	大倾角下推力分配对姿态敏感
更新频率	30-50 Hz	100-400 Hz	高速下需要更密集的状态更新
端到端延迟	<100 ms	<30 ms	20 m/s × 30 ms = 0.6 m 偏差
鲁棒性	光照/遮挡	+运动模糊+振动+IMU饱和	极限条件下传感器退化

§D8.6.2 Motion Capture 方案(实验室环境)

在实验室环境中,Vicon/OptiTrack 提供近乎完美的状态估计:

精度:  亚毫米级位置,<0.5° 姿态
频率:  100-360 Hz(取决于型号和反射点数量)
延迟:  ~5-10 ms(采集到输出)
覆盖:  仅限室内,需要多相机设置

在 Agilicious 中的角色: - Guard 层使用 mocap 作为安全参考(独立估计器) - Pilot 层可以选择使用 mocap(纯控制实验)或 VIO(感知实验) - 所有 CPC、MPCC 等高速实验的初始验证都在 mocap 环境中完成

局限:mocap 只是实验室工具——真实世界没有 mocap。这就是为什么 VIO 和端到端感知如此重要。

§D8.6.3 VIO 方案(机载自主)

Intel RealSense T265:Agilicious 默认的 VIO 传感器

传感器组成:
  - 2× 鱼眼相机(OV9282, 全局快门, 163° FOV)
  - 1× BMI055 IMU (6-DOF, 200 Hz)
  - 1× Intel Movidius Myriad 2 VPU(片上 VIO 处理)

输出:
  - 6-DOF 位姿(位置 + 四元数), 200 Hz
  - 位姿置信度估计
  - 可选:鱼眼图像流

内部算法:
  - 基于滤波器的 VIO(类似 MSCKF)
  - IMU 高频传播 + 视觉低频校正
  - 特征跟踪 + 三角化 + 优化

VIO 在高速飞行中的退化:

速度 5 m/s:  VIO 正常工作,特征跟踪稳定
速度 10 m/s: 开始出现特征丢失,需要全局快门相机
速度 15 m/s: 运动模糊显著(即使全局快门),特征匹配质量下降
速度 20 m/s: VIO 频繁丢失跟踪,需要 IMU 前向传播桥接
速度 25+ m/s: 纯 VIO 几乎不可用,需要事件相机或其他补充

为什么帧相机在高速下失效:

曝光时间:1 ms(典型值)
20 m/s 飞行:1 ms 内运动 20 mm = 20 像素(在 VGA 分辨率下)
结果:特征点在图像上拖尾 20 像素 → 角点检测器失效 → 特征跟踪崩溃

解决方案: - 缩短曝光时间:降低模糊但增加噪声 - 全局快门:避免卷帘快门导致的行间偏移,但不解决运动模糊 - 事件相机:根本性解决运动模糊问题

§D8.6.4 EKF/MSCKF 状态估计架构

EKF 融合架构:

┌──────────┐ 400 Hz  ┌──────────────┐ 400 Hz  ┌──────────┐
│   IMU    │────────→│  EKF 传播    │────────→│ 状态输出  │
└──────────┘         │              │         │ (控制器用) │
                     │  x̂_{k+1|k} =│         └──────────┘
┌──────────┐ 30 Hz   │  f(x̂_k, u_k)│
│  相机    │────→┐   │              │
└──────────┘     │   │  EKF 更新    │
                 │   │  (低频)      │
┌──────────┐     │   └──────────────┘
│VIO/特征  │────→┘
└──────────┘

EKF 状态向量:

x = [p, v, q, b_a, b_g, ...]
     位置 速度 姿态 加速度计偏置 陀螺仪偏置

IMU 传播(高频):

// 在每次 IMU 读数到来时(400-1000 Hz)
void EKF::propagate(const Eigen::Vector3d& accel,
                     const Eigen::Vector3d& gyro,
                     double dt) {
  // 去偏置
  Eigen::Vector3d a = accel - state_.b_a;
  Eigen::Vector3d w = gyro - state_.b_g;

  // 状态传播
  state_.p += state_.v * dt + 0.5 * (state_.R * a - g) * dt * dt;
  state_.v += (state_.R * a - g) * dt;
  state_.R = state_.R * exp_SO3(w * dt);

  // 协方差传播
  Eigen::MatrixXd F = compute_jacobian(state_, a, w, dt);
  P_ = F * P_ * F.transpose() + Q_ * dt;
}

视觉更新(低频):

// 在每次视觉测量到来时(30-100 Hz)
void EKF::update_vision(const Eigen::Vector3d& p_meas,
                         const Eigen::Quaterniond& q_meas) {
  // 创新(测量残差)
  Eigen::VectorXd z(6);
  z.head<3>() = p_meas - state_.p;
  z.tail<3>() = quaternion_error(q_meas, state_.q);

  // 卡尔曼增益
  Eigen::MatrixXd H = measurement_jacobian();
  Eigen::MatrixXd S = H * P_ * H.transpose() + R_vision_;
  Eigen::MatrixXd K = P_ * H.transpose() * S.inverse();

  // 状态更新
  Eigen::VectorXd dx = K * z;
  state_.p += dx.head<3>();
  state_.v += dx.segment<3>(3);
  state_.q = state_.q * exp_quaternion(dx.segment<3>(6));
  state_.b_a += dx.segment<3>(9);
  state_.b_g += dx.segment<3>(12);

  // 协方差更新
  P_ = (I - K * H) * P_;
}

MSCKF(Multi-State Constraint Kalman Filter):

MSCKF 比标准 EKF 更适合 VIO,因为它不将特征点加入状态向量(避免状态维度爆炸):

标准 EKF-SLAM:状态 = [机器人状态, 特征1, 特征2, ...] → O(n²) 复杂度
MSCKF:       状态 = [机器人状态, 过去 M 帧的位姿] → O(M²) 复杂度(M≈20)

MSCKF 将特征点观测转化为对多帧位姿的约束,在特征点被边缘化时一次性处理所有观测——这是**紧耦合 VIO** 的高效实现。

§D8.6.5 竞速场景的门检测感知

在 AlphaPilot 和 Swift 等竞速场景中,除了自身状态估计外,还需要**检测和定位竞速门**:

AlphaPilot 系统架构(Foehn 等, Autonomous Robots 2021):

┌──────────┐     ┌──────────────┐     ┌──────────────┐
│  立体相机  │────→│ 门检测 (CNN) │────→│  状态估计融合  │
└──────────┘     │ 2D 检测      │     │  EKF: VIO +   │
                 │ + 位姿估计    │     │  门检测 +     │
┌──────────┐     └──────────────┘     │  IMU          │
│   IMU    │─────────────────────────→│              │
└──────────┘                          └──────┬───────┘
                                             │
                                      ┌──────▼───────┐
                                      │  轨迹规划     │
                                      │  (CPC/时间   │
                                      │   最优)       │
                                      └──────┬───────┘
                                             │
                                      ┌──────▼───────┐
                                      │  MPC 控制器   │
                                      └──────────────┘

门检测方法演进:

方法	频率	精度	鲁棒性	平台
颜色分割	>100 Hz	低	对光照敏感	CPU
YOLO/SSD	30-50 Hz	中	较好	GPU
PencilNet	83 Hz (GPU)	高	Sim-to-real	GPU
关键点回归	50-80 Hz	高	需要训练	GPU

PencilNet(Foehn 等): - 轻量级 backbone + pencil 滤波器 - 同时预测门的 2D 位置、距离、朝向 - 零样本 sim-to-real:仅用仿真数据训练,直接在真机上工作 - 关键:使用**全局快门相机**避免检测框变形

Swift 的感知流水线(Kaufmann 等, Nature 2023):

Swift 的创新是将**学习型感知与卡尔曼滤波器结合**:

1. CNN 检测器:从图像中检测门的 2D 角点
2. PnP 求解器:从 2D-3D 对应关系估计门的 6-DOF 位姿
3. EKF 融合:将门检测与 VIO 融合
   - 单次检测精度低 → EKF 时间序列融合提高精度
   - 多门同时检测 → 利用门间几何关系校正 VIO 漂移
4. 关键创新:利用多门检测构建全局门地图,
   持续校正 VIO 漂移——类似于 SLAM 中的闭环

这种混合方法在真实世界中的性能远超纯端到端(像素到控制)方案——后者在光照变化和门位偏移时迅速退化。

---

§D8.7 四种敏捷飞行范式对照与选型 ⭐⭐

动机

走到这里,你手上已经有了一整套工具:DFBC、NMPC、CPC、MPCC、IPC。但真正的工程难题不是"会用某一个",而是"面对一个具体任务,该选哪个"。选错了代价很大——在 Crazyflie 上硬上 MPCC 会跑不动,在动态避障场景用离线 CPC 会撞墙,在资源充足的竞速场景用 DFBC 会因为不处理饱和而发散。本节把五种范式按**计算成本、约束处理能力、控制频率、实现复杂度**四个维度横向对照,并给出一棵可操作的选型决策树。这是把零散知识收敛成工程判断力的关键一步。

范式	代表	计算成本	约束处理	适用场景
DFBC	Faessler RA-L 2018	<0.1 ms	无	可行参考 + 温和飞行
NMPC	rpg_mpc/acados	0.5-5 ms	推力/体率上下限	中速敏捷 + 已知扰动
CPC	Foehn Sci.Rob. 2021	离线秒级	推力极限互补性	时间最优 + 离线规划
MPCC	Romero T-RO 2023	1-10 ms	隧道+进度	竞速 + 自适应快慢
IPC	Liu RA-L 2023	1-5 ms	SFC + 动力学	复杂环境 + 规控一体

§D8.7.1 选择决策树

你的应用场景是什么?

├── 离线规划 + 已知环境
│   ├── 需要时间最优? → CPC
│   └── 不需要最快? → Min-snap + DFBC
│
├── 在线竞速 + 已知赛道
│   ├── 速度 < 15 m/s? → NMPC
│   └── 速度 > 15 m/s? → MPCC / MPCC++
│
├── 未知/动态环境 + 避障
│   └── IPC (规控一体)
│
├── 资源极度受限(Crazyflie)
│   └── DFBC (计算最轻)
│
├── 强扰动/未知载荷
│   └── INDI (鲁棒性最好)
│
└── 追求极限性能 + 有 GPU
    └── RL (Swift/SimpleFlight) → 见 D9

§D8.7.2 各范式的控制回路频率要求

范式	最低频率	推荐频率	瓶颈
DFBC	100 Hz	500 Hz	无(计算极快)
INDI	200 Hz	500-1000 Hz	角加速度估计需要高频 IMU
NMPC	50 Hz	100 Hz	QP 求解时间
IPC	50 Hz	100 Hz	OSQP 求解时间
MPCC	30 Hz	50-100 Hz	NLP 求解时间
CPC	N/A	N/A	离线求解

§D8.7.3 各范式的代码实现复杂度

范式	核心代码量	依赖库	实现难度
DFBC	~200 行 C++	Eigen	低——理解平坦映射即可
INDI	~300 行 C++	Eigen + 滤波器	中——角加速度估计是关键
NMPC	~500 行 C++ + 配置	acados/CasADi/Eigen	中高——需要 OCP 知识
IPC	~800 行 C++	OSQP/Eigen	中高——SFC 生成是额外复杂度
MPCC	~1000 行 C++	acados/CasADi	高——弧长参数化 + 隧道约束
CPC	~500 行 Python	CasADi/IPOPT	高——互补约束数值困难

---

§D8.8 竞速规控系统集成——从感知到执行的完整链路 ⭐⭐⭐

动机

前面的小节把敏捷飞行拆成了独立的零件:感知(§D8.6)、规划(§D8.2-8.3)、控制(§D8.4-8.5)、安全(§D8.1)。但**一个能赢人类的系统不是零件的简单堆叠**——它要解决零件之间的接口、时序、失败传播问题。本节通过三个真实系统(AlphaPilot 冠军方案、Swift 的 Nature 端到端 RL、Agilicious 的 MPC 路线)展示这些零件如何拼成端到端链路,以及不同的拼法(传统分层 vs 端到端学习)各自的取舍。这是把"懂每个模块"升级为"懂整个系统"的必经一步。

反面:把模块拼起来"应该就能飞"——为什么往往不行

一个常见的天真预期是:"我有了 VIO、有了 CPC 轨迹、有了 MPC 跟踪器,接起来无人机就该自己飞赛道了。"现实是它通常飞不起来,因为模块**接口处的隐性假设**会互相打架:CPC 假设精确的门位置,但 VIO 有累积漂移,飞到第 5 个门时漂了半米,CPC 的离线轨迹已经对不上真实门了;门检测器单帧精度低,直接喂给控制器会让指令抖动;感知 30 Hz、控制 100 Hz、内环 8 kHz 三个频率如何对齐?这些都不是单个模块的问题,而是系统集成的问题。AlphaPilot 用"多门检测校正 VIO 漂移"(类似 SLAM 闭环)解决漂移,用 EKF 时序融合解决单帧抖动——本节正是要展示这些集成层面的关键工程决策。

§D8.8.1 AlphaPilot 系统集成案例

AlphaPilot 竞赛背景:

2019 年,Lockheed Martin 与 Drone Racing League 联合举办 AlphaPilot 竞赛: - 奖金:$1M 大奖 - 规则:全自主(无人干预)、纯机载感知(无 GPS/mocap)、赛道未知 - 飞行器:统一平台(DRL 赛车级,配 NVIDIA Xavier GPU) - UZH RPG 团队参赛:并最终赢得冠军

UZH RPG 的 AlphaPilot 系统架构(Foehn 等, Autonomous Robots 2021):

传感器层:
  立体相机(30 Hz) + IMU(200 Hz)

感知层:
  ├── VIO 状态估计(视觉惯性里程计)
  │     输出:6-DOF 位姿,30 Hz(视觉)→ IMU 传播到 200 Hz
  │
  ├── 门检测(CNN 回归)
  │     输出:每个可见门的 3D 位姿估计
  │     特点:可同时检测多个门
  │
  └── EKF 融合
        输入:VIO + 门检测 + IMU
        输出:飞行器全局位姿 + 门地图
        关键:多门检测校正 VIO 累积漂移

规划层:
  ├── 全局路径:A* / 门序列确定
  │
  └── 轨迹生成:
        方案 A:CPC 时间最优(如果门位置已知且稳定)
        方案 B:MINCO/min-snap(实时重规划)

控制层:
  ├── 外环:MPC 或 DFBC(跟踪参考轨迹)
  │     频率:50-100 Hz
  │
  └── 内环:BetaFlight PID(姿态/角速度)
        频率:8 kHz
        输入:来自外环的推力+角速度指令

安全层:
  └── 紧急减速 + 重规划逻辑

关键工程决策:

1. 学习型感知 + 传统估计:CNN 门检测提供语义信息,EKF 提供时间一致性——两者互补
2. 利用多门检测:同时看到多个门时,门间的已知几何关系可以作为额外约束,大幅改善定位精度
3. 分层控制:外环 MPC(50-100 Hz)负责轨迹跟踪,内环 BetaFlight PID(8 kHz)负责姿态稳定——利用成熟的飞控固件
4. 无先验赛道知识:系统必须在飞行中逐步发现门位置并构建赛道地图

§D8.8.2 Swift 系统集成(Nature 2023)

Swift 的系统架构与 AlphaPilot 有根本性差异——它用**端到端 RL** 替代了传统的分层规划控制:

传感器 → CNN 特征提取 → RL 策略 → 控制指令(推力 + 角速度)
  ↑          ↑              ↑
  │          │              │
相机+IMU   视觉编码器    PPO 训练的策略网络

对比 AlphaPilot:
  传感器 → 感知 → 状态估计 → 规划 → 控制 → 指令
  (5个独立模块)   vs   Swift(3个模块,规划和控制被 RL 合并)

Swift 的关键创新:

1. 仿真训练:
   - 环境:Flightmare(Unity 渲染 + 物理引擎)
   - 奖励:r = r_progress + r_collision + r_gate_pass
   - 域随机化:门位偏移、光照变化、气动参数变化
   - 训练量:数千万步(数小时 GPU 时间)

2. 感知流水线:
   - CNN 检测门角点 → PnP 估计门位姿
   - EKF 融合 VIO + 门检测
   - 关键:不是纯端到端像素到控制——中间有结构化的状态估计!

3. RL 策略:
   - 输入:估计的飞行器状态 + 下一门的相对位姿
   - 输出:集体推力 + 机体角速度(CTBR 动作空间)
   - 网络:MLP,可在机载 CPU 上 100+ Hz 推理

4. Sim-to-Real:
   - 系统识别(SysID):精确辨识电机延迟、旋翼阻力系数
   - 域随机化:在辨识参数周围添加随机噪声
   - 关键发现:少量真机数据微调可以大幅改善 transfer

Swift vs 人类冠军实验结果:

比赛设置:
  - 3 位世界冠军级 FPV 飞手(含 Alex Vanover, 2019 DRL 冠军)
  - 室内赛道,7 个门
  - 每位飞手和 Swift 各飞多圈
  - 使用同一架物理飞行器

结果:
  - Swift 赢得 15/25 场比赛
  - Swift 创下最快单圈记录
  - 最高速度 >100 km/h
  - 人类在适应性上更好(环境变化时)
  - Swift 在一致性上更好(每圈时间方差更小)

§D8.8.3 Agilicious 上的完整竞速集成(MPC 路线)

对于使用 MPC(非 RL)路线的竞速集成,Agilicious 提供了以下参考架构:

┌────────────────────────────────────────────────────────────┐
│                    Guard 层(独立运行)                       │
│   Mocap/VIO → 围栏检查 + 偏差监控 + 心跳 → 接管/紧急悬停    │
├────────────────────────────────────────────────────────────┤
│                    Pilot 层                                │
│                                                            │
│   ┌──────────┐  ┌──────────┐  ┌──────────┐  ┌──────────┐ │
│   │Estimator │→│ Sampler  │→│Reference │→│Controller│ │
│   │          │  │          │  │          │  │          │ │
│   │ EKF:     │  │ 采样当前  │  │ CPC 离线  │  │ MPCC:    │ │
│   │ IMU+VIO  │  │ 参考点    │  │ 轨迹存储  │  │ acados   │ │
│   │ +门检测   │  │          │  │ 或 MPCC  │  │ SQP-RTI  │ │
│   │ 400 Hz   │  │ 100 Hz   │  │ 在线规划  │  │ 100 Hz   │ │
│   └──────────┘  └──────────┘  └──────────┘  └──────────┘ │
├────────────────────────────────────────────────────────────┤
│                    Bridge 层                                │
│   Command → MSP 编码 → UART → BetaFlight F7 → 电机 PWM     │
└────────────────────────────────────────────────────────────┘

acados 在 MPCC 中的使用:

acados 工作流:
1. 用 CasADi (Python) 定义 OCP:
   - 状态:x = [p, v, q, ω, θ, v_θ]  (飞行器 + 进度)
   - 控制:u = [f, τ_x, τ_y, τ_z, a_θ]  (推力+力矩+进度加速度)
   - 代价:MPCC 代价(进度+轮廓+平滑)
   - 约束:动力学 + 执行器 + 隧道

2. acados 代码生成:
   - 从 Python OCP 描述生成 C 代码
   - SQP-RTI 求解器 + HPIPM QP 求解器
   - 生成的 C 代码无外部依赖,可直接嵌入

3. C++ 运行时:
   - 加载生成的求解器
   - 每 10 ms 设置初始状态和参考
   - 调用 solve() 获取最优控制
   - 发送 u[0] 到 Bridge

// acados MPCC 求解器接口(概念化)
class AcadosMPCC {
  AcadosOcp ocp_;
  AcadosSolver solver_;

public:
  Command solve(const State& state, const PathRef& path_ref) {
    // 设置初始状态
    ocp_.set_initial_state(state);

    // 设置参考路径(MPCC 需要弧长参数化路径)
    for (int k = 0; k <= N; ++k) {
      double theta_k = state.theta + k * dt * state.v_theta;
      auto ref = path_ref.evaluate(theta_k);
      ocp_.set_reference(k, ref);
    }

    // 求解
    int status = solver_.solve();

    // 提取第一步控制
    auto u0 = solver_.get_u(0);
    return Command{u0.thrust, u0.body_rates};
  }
};

§D8.8.4 仿真环境:Flightmare 与 RotorS

Flightmare(Song 等, CoRL 2020):

Agilicious 的主要仿真后端之一,专为敏捷飞行和 RL 研究设计:

架构:
  ┌───────────────┐              ┌───────────────┐
  │  物理引擎      │  ZeroMQ     │  渲染引擎      │
  │  (C++ / Eigen) │ ←────────→ │  (Unity / C#)  │
  │  200,000 Hz   │              │  230 Hz       │
  │  多机并行      │              │  多摄像头      │
  └───────────────┘              └───────────────┘

特性:
  - 物理:最高 200,000 Hz 仿真(远超实时)
  - 渲染:Unity 高质量渲染,230 Hz
  - RL:OpenAI Gym 接口,支持数百架四旋翼并行
  - 传感器:RGB / 深度 / 分割 / 光流 / 点云
  - 解耦:物理和渲染独立运行(可只用物理做 RL)

RotorS(Furrer 等, Springer 2016):

架构:
  - 基于 Gazebo + ROS
  - 物理:ODE 刚体仿真,1 kHz
  - 传感器:IMU / 相机 / 激光(Gazebo 插件)
  - 多机:多 namespace 支持

优势:与 ROS 生态完全兼容
劣势:渲染质量和速度远不如 Flightmare

在 Agilicious 中的仿真流程:

# 步骤 1:在 RotorS 中快速验证(不需要 GPU)
roslaunch agilicious simulation.launch bridge:=rotors controller:=dfbc

# 步骤 2:在 Flightmare 中做视觉相关实验(需要 GPU)
roslaunch agilicious simulation.launch bridge:=flightmare controller:=mpc

# 步骤 3:切换到真机(确保 Guard 已配置)
roslaunch agilicious real_flight.launch bridge:=uart controller:=mpc \
  guard:=enabled fence_size:=5.0

---

§D8.9 Actor-Critic MPC:可微优化遇见强化学习 ⭐⭐⭐⭐

本节定位

到 §D8.8 为止,我们看到了一条光谱:最左端是纯结构(DFBC/MPC,有约束保证但要手调、不会学习),最右端是纯学习(Swift 端到端 RL,会学习但是黑盒、无约束保证)。AC-MPC(Actor-Critic MPC)站在这条光谱的**正中间**——它把一个可微的 MPC 当作 RL 的 actor,让 RL 在线学着调 MPC 的权重/参考,既保留 MPC 的约束满足保证,又获得 RL 从经验中自适应的能力。这是本章的收尾,也是通往下一章(D9 纯 RL)的桥梁:理解了"为什么要在 MPC 外面再套一层 RL",才能理解 D9 为什么敢把整个 MPC 都丢掉换成神经网络。本节难度为研究级(⭐⭐⭐⭐),涉及隐函数定理对 KKT 系统求导,可在初读时只把握思想、跳过梯度推导细节。

引用:Romero, Aljalbout, Song, Scaramuzza, "Actor-Critic Model Predictive Control: Differentiable Optimization meets Reinforcement Learning for Agile Flight", T-RO 2025.

GitHub: uzh-rpg/acmpc_public

§D8.9.1 动机:MPC 和 RL 各自的不足

MPC 的问题:
  ├── 超参数调优困难(权重、约束、预测步长)
  ├── 模型失配在高速下放大
  ├── 只优化短期(有限预测区间)
  └── 不能从经验中学习

RL 的问题:
  ├── 黑盒策略缺乏可解释性
  ├── 无法处理状态/输入约束(只有惩罚)
  ├── 样本效率低(需要大量交互)
  └── Sim-to-real gap

AC-MPC 的解决方案:把 MPC 嵌入 RL 的 actor-critic 框架:

┌──────────────────────────────────────────────┐
│  RL Actor = 可微 MPC                         │
│                                              │
│  策略网络 → MPC 参数(权重/参考/约束)          │
│  可微 MPC → 控制指令(保证约束满足)            │
│                                              │
│  RL Critic = 价值网络(标准 MLP)              │
│  评估长期回报(MPC 看不到的长期效果)           │
│                                              │
│  训练:PPO / TD3,梯度通过可微 MPC 反传        │
└──────────────────────────────────────────────┘

§D8.9.2 可微 MPC 的实现

关键思想:acados MPC 的求解过程可以看作一个**可微函数**:

u* = MPC(x_0, params)

其中 params 包括 MPC 的权重 Q, R、参考轨迹 x_ref、约束边界等。通过**隐函数定理**或 KKT 条件的微分,可以计算:

∂u*/∂params = -(∂²L/∂u∂u)⁻¹ · (∂²L/∂u∂params)

其中 L 是 MPC 的 Lagrangian。

实际实现:acados 提供了 sensitivity 接口,可以计算控制动作对参数的梯度——这使得 RL 的梯度可以通过 MPC 反向传播到策略网络。

§D8.9.3 训练流程

for episode in range(num_episodes):
  state = env.reset()
  for t in range(max_steps):
    # 策略网络输出 MPC 参数
    mpc_params = policy_network(state)

    # 可微 MPC 求解(前向传播)
    action = differentiable_mpc.solve(state, mpc_params)

    # 执行动作
    next_state, reward = env.step(action)

    # 存储经验
    buffer.add(state, action, reward, next_state, mpc_params)

    state = next_state

  # 更新策略网络和价值网络
  policy_loss, value_loss = compute_losses(buffer)

  # 关键:梯度通过可微 MPC 反传到 policy_network
  policy_loss.backward()  # ∂loss/∂params → ∂params/∂network_weights
  optimizer.step()

§D8.9.4 实验结果

仿真结果: - 在多条赛道上,AC-MPC 比纯 MPC 快 10-20% - 比纯 RL(MLP 策略)有更好的 out-of-distribution 泛化 - 样本效率比纯 RL 高 ~5 倍(MPC 提供了良好的结构先验)

真机结果: - 最高速度 21 m/s (75.6 km/h) - 在 Agilicious 平台上验证 - 模型失配鲁棒性:改变飞行器质量 20%,AC-MPC 仍然稳定(纯 MPC 性能显著下降)

§D8.9.5 AC-MPC 的意义

AC-MPC 代表了**规划控制融合**的新范式:

传统层级:   感知 → 规划 → 控制    (固定接口)
IPC:        感知 → [规划+控制]    (同一 QP)
MPCC:       感知 → [规划+控制]    (同一 NLP)
AC-MPC:     感知 → [学习+规划+控制]  (RL 调优 MPC)
端到端 RL:   [感知+规划+控制]     (全部学习)

↓ 从左到右:更多学习,更少结构 ↓
↓ 从右到左:更多结构,更多保证 ↓

AC-MPC 在中间位置:保留 MPC 的约束保证,获得 RL 的自适应能力

---

本章常见陷阱汇总 ⭐⭐⭐

下面按"概念误区 / 思维陷阱 / 编程陷阱"三类汇总本章最常踩的坑。每个陷阱按**错误描述 → 现象/后果 → 根本原因 → 正确做法**四要素拆解。这些不是假想的——它们是初学者在敏捷飞行规控里实实在在卡住的地方。

概念误区类

陷阱 1:以为"飞得平滑"就等于"飞得快"

• 错误描述:想让无人机尽快通过一串航点,于是直接用 D3 的 min-snap 轨迹,以为最优平滑轨迹就是最快轨迹。
• 现象/后果:无人机确实平滑地飞完了,但圈速比时间最优慢 20-40%;推力利用率只有 ~50%,大量推力余量没用上;和 CPC 轨迹对比时明显"慢半拍"。
• 根本原因:min-snap 最小化的是 snap 积分(平滑性/省力),而时间最优要求推力**贴着边界**做 bang-bang 切换(§D8.2.3)。平滑和最快是方向相反的两个优化目标——平滑要远离极限,最快要逼近极限。
• 正确做法:明确区分两个目标。要平滑/省力/远离极限 → min-snap;要最快 → CPC(离线)或 MPCC(在线)。不要指望调 min-snap 的时间分配就能逼近时间最优——目标函数错了,再调也是次优。

陷阱 2:把 MPCC 的进度变量 \(\theta\) 当成时间 \(t\)

• 错误描述:实现 MPCC 时,把进度 \(\theta\) 理解成"飞行时间",于是像传统跟踪一样按固定时钟推进 \(\theta\)。
• 现象/后果:飞行器在弯道落后时,\(\theta\) 仍按时钟前进,参考点跑到前方,轮廓误差爆炸,控制器抄近道切弯或失控——完全退化回时间参数化跟踪的失败模式(§D8.4 反面)。
• 根本原因:没理解 MPCC 的核心是**把 \(\theta\) 变成优化变量**(状态的一部分),由优化器根据"能跟多紧"自己决定 \(\theta\) 推进多快,而不是外部时钟驱动。
• 正确做法:把 \(\theta\) 和它的推进速度 \(v_\theta\) 都纳入状态/控制,在代价里用 \(-w_\theta \Delta\theta\) 鼓励前进、用轮廓误差 \(w_c\|e_{con}\|^2\) 约束偏离,让两者博弈自动决定快慢(§D8.4.3)。\(\theta\) 推进慢是优化器的合理决策,不是 bug。

陷阱 3:以为 Guard 可以复用 Pilot 的状态估计器

• 错误描述:为省计算资源,让安全层 Guard 直接用 Pilot 的 VIO/EKF 输出来做围栏和偏差检查。
• 现象/后果:当估计器漂移或崩溃(高速下 VIO 丢跟踪)时,Pilot 和 Guard **同时**失效——Guard 拿着同样错误的状态,以为一切正常,安全网形同虚设,飞行器带着错误估计飞出围栏。
• 根本原因:违反了安全冗余的基本原则——监控者必须独立于被监控者。共享估计器引入了**共因失效**(common-cause failure),单点故障同时击穿控制和安全。
• 正确做法:Guard 必须有**独立**的估计器(§D8.1.2),可以精度更低但绝不能和 Pilot 同源。在 mocap 环境用外部真值,在外场用独立 GPS/IMU 或独立 VIO 实例。类比民航的三套独立惯导投票——独立性比单套精度更重要。

思维陷阱类

陷阱 4:把"约束当惩罚项"和"约束当硬约束"混为一谈

• 错误描述:看到 RL 用奖励惩罚碰撞、MPC 用硬约束限制推力,以为两者等价,可以随意互换。
• 现象/后果:用 RL 软惩罚替代 MPC 硬约束时,策略偶尔会"违规换取高回报"(轻微撞门但进度奖励更高),真机上表现为偶发碰撞;反过来把本该是软偏好的项写成硬约束,导致 QP 无解(infeasible)、求解器报错。
• 根本原因:软惩罚是"尽量满足"(可被其他项交易掉),硬约束是"必须满足"(不可违反)。安全相关的(推力上限、不撞墙)必须是硬约束;偏好性的(平滑、省电)适合做软惩罚。混淆二者会在安全性和可解性上各踩一个坑。
• 正确做法:分清哪些是**不可违反的物理/安全约束**(→ 硬约束,如 \(f\le f_{\max}\)、SFC),哪些是**可权衡的偏好**(→ 软代价,如轮廓误差、控制平滑)。这也是 §D8.9 AC-MPC 的价值——它保留 MPC 的硬约束,只用 RL 调软代价的权重。

陷阱 5:忽视延迟,以为"当前感知"就是"当前状态"

• 错误描述:控制器直接拿 VIO/视觉输出的位姿当作飞行器**此刻**的状态,不做任何延迟补偿。
• 现象/后果:低速下没事,高速下控制器实际在跟踪"飞行器几十毫秒之前的位置"(20 m/s × 50 ms = 1 m 误差),指令系统性滞后,表现为过冲、振荡,穿门时偏移撞框。
• 根本原因:感知有 15-60 ms 端到端延迟(§D8.0 挑战二),这个延迟在低速可忽略、高速下变成米级位置误差。把"延迟的感知"当"即时状态"是把二阶效应当不存在。
• 正确做法:做延迟补偿——用高频 IMU 前向传播把视觉状态推到当前时刻(§D8.0 方法一),或让 MPC 的预测区间起点偏移一个延迟量(方法二)。把有效延迟从 ~50 ms 压到 ~2 ms。

编程陷阱类

陷阱 6:CPC 的互补约束直接硬求解,不做松弛

• 错误描述:在 CasADi 里直接写下精确互补约束 \(\dot{\pi}_i(\|p-w_i\|-r_{wp})=0\) 就调 IPOPT 求解。
• 现象/后果:求解器要么不收敛、要么卡在很差的局部最优、要么报告问题不可行;换个初始猜测结果完全不同,毫无稳定性。
• 根本原因:互补约束的可行集是"两条相交直线的并"——非凸且非光滑,梯度类求解器在 \(=0\) 的尖点处行为恶劣(§D8.2.4)。直接硬求几乎必然失败。
• 正确做法:用**松弛策略**——先把互补约束写成惩罚项或放松成 \(\le \epsilon\),从大 \(\epsilon\) 开始求解,逐步收紧 \(\epsilon \to 0\)(同伦/续延法);并用 min-snap 轨迹做初始猜测(§D8.2.4)。这是数值求解互补约束 NLP 的标准手段。

陷阱 7:在 1 kHz 控制循环里用动态大小 Eigen 矩阵 / 频繁堆分配

• 错误描述:实现 MPC/IPC 的 QP 组装时,用 Eigen::MatrixXd(动态大小)存储已知维度的 Hessian/约束矩阵,每个控制周期 new/delete。
• 现象/后果:控制循环出现不规律的延迟尖刺(jitter),偶尔单周期超时,在高频(>500 Hz)下尤其明显;实时性被破坏,严重时触发 Guard 的心跳超时。
• 根本原因:维度在编译期已知时仍用动态矩阵,每帧触发 malloc/free,在 1 kHz 循环里累积成可观且不可预测的延迟(回顾 v8 Ch11 / D3 §D3.7 的固定 vs 动态矩阵选型)。
• 正确做法:维度编译期已知 → 用固定大小矩阵(Matrix<double, N, N>)栈分配,编译器可 SIMD 向量化;维度运行期才知道但循环内不变 → 在循环外预分配、循环内复用。OSQP/acados 的 warm-start 也依赖这种内存稳定性。

陷阱 8:INDI 用原始陀螺仪差分估角加速度,不加滤波

• 错误描述:实现 INDI 时直接用 \(\dot{\omega}\approx(\omega(t)-\omega(t-\Delta t))/\Delta t\) 得到角加速度就喂进控制律。
• 现象/后果:控制指令高频抖动、电机啸叫、姿态环发散——尤其在有机架振动时。
• 根本原因:有限差分会**放大高频噪声**(微分是高通操作),陀螺仪噪声 + 机架振动经差分后淹没真实信号(§D8.5.2)。INDI 对角加速度估计质量极度敏感。
• 正确做法:差分后必须接低通滤波(如 2 阶 Butterworth,截止 ~30 Hz,见 §D8.5.2 的 YAML 配置);或用基于模型的角加速度估计 + 传感器修正。滤波的截止频率是 INDI 调参的关键——太低引入延迟,太高滤不掉噪声。

---

前沿工作与开放问题

已有前沿工作

• Actor-Critic MPC(Romero 等, T-RO 2025):把 acados MPC 嵌为 RL 的 actor 层——RL 在线调 MPC 权重/参考,21 m/s 真机竞速
• DDR-opt(ZJU FAST Lab, ~360 stars):把 MINCO 扩展到地面差速机器人——MINCO 框架的**跨平台泛化**
• SimpleFlight(Chen, Yu 等, 2024-2025):识别 sim-to-real 的五大关键因素,在 Crazyflie 上实现 >50% 误差降低
• Splat-Nav(Chen, Shorinwa, Schwager 等, T-RO 2025):高斯散点地图 + 安全走廊,>2 Hz 在线重规划

开放问题

数值问题: - MPC 在推力饱和边界上的数值条件恶化:Hessian 的条件数在约束活跃时急剧增大 - CPC 的互补性约束使 NLP 高度非凸:大量局部最优,对初始化极度敏感 - MPCC 的隧道约束在三维空间生成困难:如何从稀疏门位置自动生成光滑的三维隧道?

感知问题: - VIO 在 20+ m/s 下的可靠性:帧相机特征跟踪在高速下严重退化 - 事件相机的算法成熟度:事件驱动的 SLAM/VIO 仍不如帧相机成熟 - 动态光照下的门检测:竞速场景中的光照变化可能导致 CNN 检测器失效

系统问题: - 电池电压下降对控制性能的影响:飞行后期推力下降 30%,CPC/MPCC 的轨迹不再可行 - 飞行器到飞行器的气动干扰:多机竞速时,前方飞行器的尾流影响后方飞行器 - 端到端系统延迟的精确建模:从传感器到执行器的每个环节都有延迟,如何精确建模和补偿?

理论问题: - MPC/RL 融合的理论保证:AC-MPC 目前缺乏收敛性和稳定性的理论证明 - 时间最优控制的在线求解:CPC 目前是离线的——能否做到实时时间最优? - 约束主动推理(Active Inference):RL 策略能否学会预测并主动利用约束(而非被动满足)?

---

项目精读清单

Agilicious 精读路径

序号	文件/目录	精读目标
1	agilib/include/agilib/pilot/pilot.hpp	Pipeline 组合模式:组件如何串联
2	agilib/include/agilib/controller/mpc/	acados MPC 控制器的 C++17 实现
3	agilib/include/agilib/controller/indi/	INDI 控制器:角加速度估计 + 增量控制
4	agilib/include/agilib/bridge/	Bridge 适配层:统一接口设计模式
5	agilib/include/agilib/guard/	Guard 安全层:围栏检查 + 接管逻辑
6	agilib/include/agilib/estimator/	估计器:EKF/mocap/VIO 切换
7	agilib/yaml/	YAML 配置示例:理解工厂模式的配置驱动
8	agilib/tests/	Google Test 覆盖:关键路径的测试策略

精读策略:

阶段 1 (4h):通读 pilot.hpp,理解 Pipeline 的组合模式
  → 画出类图:Pipeline 如何持有 Estimator/Controller/Bridge 的 unique_ptr
  → 理解 run() 的调用链:数据如何从传感器流到电机

阶段 2 (4h):对比 MPC 和 DFBC 控制器的实现
  → 读 controller_base.hpp 的虚函数接口
  → 读 mpc_controller.cpp 的 acados 调用
  → 读 dfbc_controller.cpp 的平坦映射计算
  → 对比两者的计算复杂度和代码量

阶段 3 (4h):精读 Guard 和 Bridge
  → 理解 Guard 如何独立于 Pilot 运行
  → 理解 Bridge 如何用多态切换仿真/真机
  → 思考:如果你要添加一个新的仿真后端(如 Isaac Sim),需要改什么?

IPC 精读路径

序号	文件/目录	精读目标
1	src/planner/traj_opt/	SFC + MPC 统一 QP 的 OSQP 接口
2	src/planner/sfc_gen/	安全飞行走廊生成算法
3	src/controller/	MPC 后端的具体实现
4	launch/	ROS launch 文件:参数配置

CPC 精读路径

序号	文件/目录	精读目标
1	uzh-rpg/rpg_time_optimal	CPC 问题的 CasADi + IPOPT 公式
2	论文中的 Supplementary Material	互补约束的松弛策略

MPCC 精读路径

序号	文件/目录	精读目标
1	论文中的 OCP 公式	弧长参数化 + Frenet-Serret 框架
2	acados 代码生成的 OCP 定义	如何用 CasADi 定义 MPCC 的代价和约束

AC-MPC 精读路径

序号	文件/目录	精读目标
1	uzh-rpg/acmpc_public	可微 MPC + RL 的 PyTorch 实现
2	论文 Section III	可微 MPC 的梯度计算(隐函数定理)

---

实战练习

A 型练习:动手实验

[A 型·Agilicious 仿真]

在 Agilicious + RotorS/Flightmare 中运行 MPC 控制器。用 YAML 切换到 DFBC 控制器,在同一条参考轨迹上对比跟踪性能——预期:MPC 在高速/大角度时更好。

详细步骤:

# 1. 安装 Agilicious(需要 ROS Noetic + Ubuntu 20.04)
cd ~/catkin_ws/src
git clone https://github.com/uzh-rpg/agilicious.git
cd ..
catkin build

# 2. 运行 MPC 控制器实验
roslaunch agilicious pilot.launch \
  config:=$(rospack find agilib)/yaml/mpc_sim.yaml

# 3. 发送参考轨迹(lemniscate 八字形,最高速度 10 m/s)
rosrun agilicious trajectory_publisher _type:=lemniscate _speed:=10.0

# 4. 记录数据
rosbag record /pilot/state /pilot/reference /pilot/command -O mpc_exp.bag

# 5. 切换到 DFBC 控制器(只改 YAML,不重编译!)
roslaunch agilicious pilot.launch \
  config:=$(rospack find agilib)/yaml/dfbc_sim.yaml

# 6. 重复步骤 3-4,记录 dfbc_exp.bag

# 7. 分析对比
python3 compare_tracking.py mpc_exp.bag dfbc_exp.bag

预期观察: - 速度 < 8 m/s:两者跟踪误差接近 - 速度 10-15 m/s:MPC 误差增长缓慢,DFBC 误差在弯道明显增大 - 速度 > 15 m/s:DFBC 可能出现推力饱和导致的跟踪发散

[A 型·IPC 运行]

编译 IPC,在提供的仿真场景中运行。观察:当障碍突然出现时,IPC 如何在 SFC 内自动偏离参考(而传统 MPC 会试图穿过障碍)。

# 1. 安装 IPC
cd ~/catkin_ws/src
git clone https://github.com/hku-mars/IPC.git
cd ..
catkin build

# 2. 运行 IPC 仿真(包含动态障碍)
roslaunch ipc simulation_dynamic_obs.launch

# 3. 观察 RViz 中的行为:
#    - 绿色:参考轨迹
#    - 红色:SFC 边界
#    - 蓝色:IPC 实际飞行轨迹
#    → 当红色障碍出现时,蓝色轨迹自动在 SFC 内偏离

# 4. 对比:运行传统 MPC(无 SFC 约束)
roslaunch ipc simulation_dynamic_obs.launch use_sfc:=false
# → 预期:传统 MPC 试图穿过新出现的障碍

[A 型·MPCC 概念实现]

用 CasADi Python 实现一个 2D 简化 MPCC:圆形赛道 + 单积分器动力学。优化进度 s(t) + 轮廓误差。观察:弯道自动减速、直道自动加速。

import casadi as ca
import numpy as np
import matplotlib.pyplot as plt

# 2D MPCC 简化实现
# 圆形赛道:p_ref(theta) = [R*cos(theta), R*sin(theta)]
R_track = 5.0  # 赛道半径

# 动力学:双积分器
# 状态:x = [px, py, vx, vy, theta]  (位置、速度、进度)
# 控制:u = [ax, ay, v_theta]  (加速度、进度速度)

N = 30    # 预测步长
dt = 0.1  # 时间步长

opti = ca.Opti()

# 决策变量
X = opti.variable(5, N+1)  # 状态
U = opti.variable(3, N)    # 控制

# 权重
w_theta = 10.0   # 进度权重(最大化)
w_con = 50.0     # 轮廓误差权重
w_u = 0.1        # 控制平滑权重
w_lag = 1.0      # 滞后误差权重

# 代价函数
cost = 0
for k in range(N):
    # 当前进度
    theta_k = X[4, k]

    # 参考点
    p_ref = ca.vertcat(R_track * ca.cos(theta_k), R_track * ca.sin(theta_k))

    # 切向量
    t_vec = ca.vertcat(-ca.sin(theta_k), ca.cos(theta_k))

    # 误差分解
    e = X[:2, k] - p_ref
    e_lag = ca.dot(e, t_vec)      # 滞后误差
    e_con = e - e_lag * t_vec      # 轮廓误差

    # 进度增量
    delta_theta = U[2, k] * dt

    # MPCC 代价
    cost += -w_theta * delta_theta            # 最大化进度
    cost += w_con * ca.dot(e_con, e_con)      # 最小化轮廓误差
    cost += w_lag * e_lag**2                   # 最小化滞后误差
    cost += w_u * ca.dot(U[:2, k], U[:2, k])  # 控制平滑

    # 动力学约束
    opti.subject_to(X[0, k+1] == X[0, k] + X[2, k] * dt)
    opti.subject_to(X[1, k+1] == X[1, k] + X[3, k] * dt)
    opti.subject_to(X[2, k+1] == X[2, k] + U[0, k] * dt)
    opti.subject_to(X[3, k+1] == X[3, k] + U[1, k] * dt)
    opti.subject_to(X[4, k+1] == X[4, k] + U[2, k] * dt)

    # 约束
    opti.subject_to(opti.bounded(-5, U[0, k], 5))   # 加速度限制
    opti.subject_to(opti.bounded(-5, U[1, k], 5))
    opti.subject_to(U[2, k] >= 0)                     # 进度不后退

opti.minimize(cost)

# 初始条件
opti.subject_to(X[:, 0] == ca.vertcat(R_track, 0, 0, 2, 0))

# 求解
opti.solver('ipopt')
sol = opti.solve()

# 可视化
# ... (绘制赛道、实际轨迹、速度剖面)
# 预期:在曲率大的弯道处速度自动降低,直道处自动加速

B 型练习:分析理解

[B 型·Agilicious Guard 精读]

精读 Guard 的实现:安全条件检查(围栏/位姿偏差/心跳超时)、接管逻辑、紧急悬停指令。

分析要点: 1. Guard 依赖什么信息? 2. 如果 VIO 和 mocap 同时失效会怎样? 3. Guard 的接管延迟是多少?在 70 km/h 下对应多少米? 4. Guard 能否检测到"慢性"问题(如估计器缓慢漂移)?

[B 型·IPC vs rpg_mpc]

对比 IPC 的"SFC 约束 MPC"和 rpg_mpc 的"纯动力学 MPC"。画出两者的 QP 问题结构差异:

rpg_mpc QP:
  变量:N × (nx + nu) ≈ 20 × 16 = 320
  约束:动力学等式 + 执行器 box ≈ 20 × 12 + 20 × 4 = 320
  结构:带状稀疏

IPC QP:
  变量:N × (nx + nu) ≈ 40 × 12 = 480
  约束:动力学 + 执行器 + SFC ≈ 40 × 9 + 40 × 3 + ~200 = 680
  结构:带状 + SFC 引入的额外稀疏块

问题:SFC 约束如何影响 QP 的求解时间?
答案:SFC 约束增加约束数量 ~50%,但 OSQP 的稀疏求解器
     利用问题结构,实际求解时间增加 ~30%。

[B 型·CPC 推力剖面分析]

从 CPC 论文中提取时间最优轨迹的推力剖面,验证 bang-bang 特性:

期望观察:
  时间 ─→ ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
  推力     ┌──────┐      ┌──────┐      ┌────
  f_max    │      │      │      │      │
           │      │      │      │      │
  f_min ───┘      └──────┘      └──────┘
                弯道减速    弯道减速

  → 推力在 f_max 和 f_min 之间切换(bang-bang)
  → 中间推力值几乎不存在(互补性条件的直接结果)

思考题

[思考题 1] IPC 把规划约束(SFC)放进 MPC;足式机器人的 WBC(全身控制器)把任务约束(末端位置)放进 QP。两者在"约束集成到优化问题"这一设计模式上有什么共通点?

参考思路: - 共同模式:将原本属于上层模块的约束下沉到下层优化器中,消除模块间接口 - 好处:避免上层给出不可行的参考(规划器不知道动力学 / 任务规划器不知道关节限制) - 代价:优化问题变大(更多约束),可能影响实时性 - 差异:IPC 是线性 QP(线性化动力学 + 线性 SFC);WBC 通常也是 QP 但约束更复杂(接触力锥)

[思考题 2] MPCC 的"进度最大化"代价 −w_s·Δs 在 RL 中对应什么?

参考思路: - Swift 的奖励函数中有 r_progress:沿向门方向的速度分量 - r_progress = v · d_gate / ‖d_gate‖:其中 d_gate 是指向下一门的方向 - 这与 MPCC 的 −w_θ·Δθ 在结构上完全对应:都是**最大化沿期望方向的进度** - 关键差异:MPCC 的进度是沿预定义路径的弧长;RL 的进度是沿门序列的自由路径进度 - 思考:RL 的进度奖励是否隐含了 MPCC 的轮廓误差惩罚?不完全——RL 的碰撞惩罚起到了类似作用

[思考题 3] 为什么 Guard 层需要独立的估计器?如果 Guard 复用 Pilot 的估计器会怎样?

参考思路: - 安全层的核心设计原则:独立性——Guard 不应信任 Pilot 的任何输出 - 如果复用估计器:当估计器故障(VIO 漂移、崩溃)时,Pilot 和 Guard 同时失效 - 独立估计器的代价:额外计算资源、额外传感器(或传感器共享的复杂性) - 类比:航空中的冗余设计——飞机有 3 套独立的惯导系统,投票决定

[思考题 4] NeuroBEM 的混合架构(BEM + NN 残差)与 L1 自适应控制的"名义模型 + 自适应项"有什么结构上的相似性?

参考思路: - 两者都是"已知部分用解析模型 + 未知部分用自适应/学习方法" - BEM ↔ 名义模型:基于第一性原理的近似 - NN 残差 ↔ L1 自适应项:补偿模型与现实的差距 - 关键差异:NeuroBEM 离线训练,L1 自适应在线调整 - 融合可能:用 NeuroBEM 提供更好的名义模型给 L1 自适应控制器(减少在线需要补偿的量)

[思考题 5] 从 min-snap → CPC → MPCC → AC-MPC → Swift(RL) 的演进中,每一步放弃了什么、获得了什么?

参考思路:

过渡	放弃	获得
min-snap → CPC	平滑性	时间最优性
CPC → MPCC	离线全局最优	在线实时自适应
MPCC → AC-MPC	手动调参	RL 自动调优
AC-MPC → Swift	MPC 结构保证	端到端任务优化
每一步都是**结构先验 vs 学习自由度**的权衡

---

补充材料:关键数学推导

补充 A:微分平坦映射(带旋翼阻力)的完整推导

以下推导基于 Faessler, Franchi, Scaramuzza (RA-L 2018):

起点:考虑旋翼阻力的四旋翼动力学:

mẍ = f·R·e₃ - mge₃ - R·D·R^T·ẋ

其中 D = diag(d_x, d_y, 0) 是体坐标系下的阻力系数矩阵(z 方向的阻力被推力方程吸收)。

步骤 1:推力方向

定义 α = mẍ + mge₃ + R·D·R^T·ẋ,则:

f·R·e₃ = α
→ R·e₃ = α / ‖α‖ = z_B
→ f = ‖α‖

注意:α 包含 R(因为 R·D·R^T·ẋ),所以 z_B 是 R 的函数——这是一个隐式方程。

步骤 2:迭代求解

初始化:R⁰ 由无阻力平坦映射给出(z_B⁰ = (ẍ + ge₃)/‖ẍ + ge₃‖)
迭代 k:
  α^k = mẍ + mge₃ + R^k·D·(R^k)^T·ẋ
  z_B^{k+1} = α^k / ‖α^k‖
  R^{k+1} = construct_R(z_B^{k+1}, ψ)
收敛条件:‖z_B^{k+1} - z_B^k‖ < ε

通常 2-3 次迭代即可收敛(阻力项相对推力较小)。

步骤 3:角速度从 jerk 计算

对 α = f·z_B 求时间导数:

α̇ = ḟ·z_B + f·(ω × z_B)

分解到体坐标系:

ω_x = -z_B · ẏ_B = (-z_B^T · α̇) / f  的 y 分量
ω_y =  z_B · ẋ_B = ( z_B^T · α̇) / f  的 x 分量
ω_z =  x_B · ẏ_B = 从 ψ̇ 计算

其中 α̇ 需要 jerk(三阶导数)的信息——这就是为什么 DFBC 需要参考轨迹的**三阶导数**作为前馈。

补充 B:MPCC 的 Frenet-Serret 框架

Frenet-Serret 公式:

给定 3D 参考曲线 p_ref(s),Frenet-Serret 框架定义了沿曲线的局部坐标系:

切向量:   T = dp/ds / ‖dp/ds‖
法向量:   N = dT/ds / ‖dT/ds‖
副法向量: B = T × N

曲率和挠率:

κ(s) = ‖dT/ds‖     (曲率:曲线弯曲程度)
τ(s) = -dB/ds · N   (挠率:曲线扭转程度)

Frenet-Serret 方程:

dT/ds = κ·N
dN/ds = -κ·T + τ·B
dB/ds = -τ·N

在 MPCC 中的应用:

飞行器位置 p 在 Frenet-Serret 框架中分解:

p = p_ref(θ) + e_n·N(θ) + e_b·B(θ)

其中: - e_n:法向偏差(轮廓误差的法向分量) - e_b:副法向偏差(轮廓误差的副法向分量)

隧道约束:

e_n² + e_b² ≤ r_tunnel(θ)²

这定义了一个沿参考曲线的**圆柱形安全隧道**。

补充 C:OSQP 求解器原理简介

OSQP (Operator Splitting QP) 用 ADMM (Alternating Direction Method of Multipliers) 求解 QP:

min  0.5·x^T·P·x + q^T·x
s.t. l ≤ Ax ≤ u

ADMM 迭代:

1. x^{k+1} = (P + σI + ρA^T·A)⁻¹ · (σx^k - q + A^T·(ρz^k - y^k))
2. z^{k+1} = clip(Ax^{k+1} + y^k/ρ, l, u)
3. y^{k+1} = y^k + ρ(Ax^{k+1} - z^{k+1})

IPC 选择 OSQP 的理由: - 无矩阵分解:只需一次 KKT 矩阵的 LDL 分解(预计算,O(n) warm-start) - 热启动:利用上一步的解作为初始化,大幅加速收敛 - 稀疏利用:IPC 的 QP 有特殊的带状稀疏结构,OSQP 自动利用 - 数值稳定:ADMM 对约束条件恶化的问题比活跃集法更鲁棒

---

本章小结

本章的主线是一个问题:当四旋翼飞到物理极限,经典的"分层规控 + 简化模型"为什么不够,以及怎么补。三个原本可忽略的二阶效应(气动残差、感知延迟、执行器饱和)在 20+ m/s 下变成主导一阶效应,而它们指向同一个工程答案——预测(基于模型的预测控制)。围绕这个核心:

• 平台层(§D8.1):Agilicious 用 Pilot/Bridge/Guard 三层分离,让算法可插拔、硬件可切换、安全可独立——Guard 的独立估计器是 70 km/h 下唯一的安全网。
• 范式层(§D8.2-8.4):CPC 把"飞最快"形式化为互补约束的 bang-bang 时间最优(离线、全局最优);IPC 把走廊约束和动力学融进同一个 OSQP(在线、规控一体);MPCC 用"进度最大化 vs 轮廓误差"的博弈实现快慢自适应(在线、竞速)。
• 控制层(§D8.5):DFBC 用微分平坦做极快前馈,INDI 用传感器增量消除扰动——它们补 MPC 在计算预算和抗扰上的短板。
• 感知层(§D8.6):高频低延迟状态估计 + 延迟补偿是一切控制的前提;高速下帧相机因运动模糊失效,需全局快门/事件相机/MSCKF。
• 融合层(§D8.7-8.9):从纯结构(MPC)到纯学习(RL)是一条光谱,AC-MPC 用可微 MPC 作 RL 的 actor,站在保证与自适应的中间点,也为 D9 的纯 RL 埋下伏笔。

一句话总括:敏捷飞行的全部技术演进,都是在"逼近物理极限"和"维持工程可控(实时、安全、可复现)"这对矛盾之间寻找更好的平衡点。

核心速查表

三大一阶效应与对策

效应	低速(<5 m/s)	高速(20+ m/s)	对策	小节
气动残差	\(f_{ext}\approx 0\) 可忽略	阻力达推力 20-30%	旋翼阻力前馈 / NeuroBEM	§D8.0 挑战一
感知延迟	50 ms → 0.25 m,可忍	50 ms → 1 m,致命	IMU 前向传播 / MPC 区间偏移	§D8.0 挑战二
执行器饱和	利用率 <50%,余量大	利用率 ~100%,常饱和	MPC 显式约束 / CPC bang-bang	§D8.0 挑战三

四种规控范式选型

范式	计算	约束处理	在线?	最适场景	小节
DFBC	<0.1 ms	无(需外裁剪)	是	可行参考+温和飞行+资源受限	§D8.5.1
INDI	<0.1 ms	无(需外裁剪)	是	强扰动/未知载荷	§D8.5.2
NMPC	0.5-5 ms	推力/体率上下限	是	中速敏捷+已知扰动	§D8.7
CPC	离线秒级	推力极限互补性	否	时间最优+已知航点	§D8.2
IPC	1-5 ms	SFC+动力学	是	动态环境+规控一体	§D8.3
MPCC	1-10 ms	隧道+进度	是	竞速+快慢自适应	§D8.4

关键公式速查

概念	公式	含义	小节
推重比	\(\text{TWR}=f_{\max}/(mg)\)	TWR>4 是敏捷门槛	§D8.1
延迟位置误差	\(\Delta p = v t + \tfrac{1}{2}a t^2\)	高速下线性主导	§D8.0
CPC 互补约束	\(\dot{\pi}_i(\|p-w_i\|-r_{wp})=0\)	进度只在航点附近变	§D8.2.3
CPC 推力 bang-bang	\((f-f_{\min})(f_{\max}-f)=0\)	时间最优在边界切换	§D8.2.3
MPCC 代价	\(-w_\theta\Delta\theta + w_c\|e_{con}\|^2 + w_u\|u\|^2\)	进度 vs 轮廓博弈	§D8.4.2
INDI 增量律	\(\tau_{cmd}=\tau_{prev}+J(\dot{\omega}_{des}-\dot{\omega}_{meas})\)	扰动被增量消去	§D8.5.2
电池推力跌落	\(f_{\max}(V)/f_{\max}(V_0)=(V/V_0)^2\)	飞行后期推力骤减	§D8.0

---

故障排查手册

下表针对敏捷飞行规控中最常见的现场故障,给出"症状 → 可能原因 → 排查步骤 → 相关章节"的结构化诊断流程。遇到问题时按症状定位,逐条排查。

场景一:高速跟踪误差大(>0.5 m),低速正常

项	内容
症状	5 m/s 下跟踪误差几厘米,提速到 15+ m/s 后稳态误差达 0.5-1 m,尤其在持续前飞段
可能原因	(1) 旋翼阻力未建模/未前馈;(2) 推力或姿态前馈缺高阶项;(3) 模型质量/惯量标定不准
排查步骤	① 检查控制器是否开启旋翼阻力补偿(DFBC 的 drag_compensation),对比开/关的误差;② 用辨识工具核对质量、惯量、\(k_f\)、阻力系数 \(D\);③ 检查前馈是否用了参考轨迹的 jerk(角速度前馈需要三阶导);④ 若仍大,考虑上 NeuroBEM 气动残差模型
相关章节	§D8.0 挑战一(气动)、§D8.5.1(DFBC 阻力前馈)、补充 A(带阻力平坦映射)

场景二:穿门系统性偏移、过冲或振荡

项	内容
症状	高速穿门时总是偏向同一侧或冲过头;控制指令呈现滞后/振荡;速度越高越严重
可能原因	(1) 感知延迟未补偿,控制器在跟"过去的状态";(2) 状态估计更新频率太低;(3) 门检测单帧噪声直接进控制
排查步骤	① 测量端到端延迟(从曝光到指令),估算 \(v\times t_{delay}\) 是否接近观测偏移量;② 启用 IMU 前向传播,把视觉状态推到当前时刻;③ 确认 EKF 以 IMU 频率(400+ Hz)输出,而非视觉频率;④ 检查门检测是否经 EKF 时序融合而非单帧直用
相关章节	§D8.0 挑战二(延迟)、§D8.6.4(EKF/MSCKF)、§D8.6.5(门检测融合)

场景三:CPC / 时间最优 NLP 求解器不收敛或乱解

项	内容
症状	IPOPT 报不可行或迭代到上限;换初始猜测结果剧变;吐出的轨迹物理上无法跟踪
可能原因	(1) 互补约束直接硬求,没做松弛;(2) 初始猜测太差;(3) 配点数/段数与求解器容差不匹配
排查步骤	① 改用同伦/续延法:从松弛的互补约束(\(\le\epsilon\) 或惩罚项)开始,逐步收紧 \(\epsilon\to 0\);② 用 min-snap 轨迹作初始猜测;③ 先减少航点/配点数验证流程,再逐步加密;④ 检查动力学约束尺度,必要时做变量归一化
相关章节	§D8.2.3(互补约束非凸性)、§D8.2.4(数值求解与松弛策略)

场景四:控制循环出现延迟尖刺 / 偶发超时

项	内容
症状	平均求解时间达标,但偶尔单周期超时;高频(>500 Hz)下 jitter 明显;偶尔触发 Guard 心跳超时接管
可能原因	(1) 循环内频繁堆分配(动态 Eigen 矩阵 new/delete);(2) QP 未 warm-start;(3) 求解器最大迭代/容差设置不当导致偶发慢收敛
排查步骤	① 用性能分析工具看是否有 malloc/free 热点;② 维度编译期已知的矩阵改固定大小、循环外预分配;③ 确认 OSQP/acados 启用了 warm-start(用上周期解初始化);④ 给求解器设迭代上限,宁可次优也要保实时;⑤ 检查是否有日志/IO 阻塞控制线程
相关章节	§D8.3.3(OSQP 实现)、陷阱 7(固定 vs 动态矩阵)、§D8.7.2(频率要求)

场景五:推力饱和后姿态失稳 / 无法恢复

项	内容
症状	急弯或大机动时无人机突然翻滚或失控;事后看推力指令长时间贴在 \(f_{\max}\);有积分项的控制器更易发作
可能原因	(1) 控制分配在饱和后丢失力矩方向;(2) 积分器 windup;(3) 参考轨迹本身超出推力可行域(规划没考虑约束)
排查步骤	① 检查控制分配策略,饱和时是否保姿态优先级(推力>roll/pitch>yaw);② 加抗 windup(饱和时停止积分);③ 核对参考轨迹的峰值需求是否超 TWR 物理极限(\(a_{req}=v^2/r\) vs \((\text{TWR}-1)g\));④ 改用显式处理约束的 MPC 替代裁剪式 DFBC
相关章节	§D8.0 挑战三(饱和)、§D8.1.1(TWR 与曲率极限)、§D8.5.1(DFBC 局限)

场景六:仿真完美,真机一飞就发散(sim-to-real gap)

项	内容
症状	控制器/RL 策略在仿真里表现优异,部署到真机后性能骤降甚至发散
可能原因	(1) 电机延迟未建模;(2) 气动残差仿真过于理想;(3) 无域随机化导致过拟合仿真;(4) 传感器延迟/噪声特性不匹配
排查步骤	① 辨识真机电机一阶延迟 \(\tau_{motor}\)(15-30 ms)并加入仿真模型;② 用 NeuroBEM 或飞行数据校正气动模型;③ 训练时对质量、阻力、延迟做域随机化;④ 用少量真机数据微调;⑤ 对照 SimpleFlight 的五大关键因素逐项核查
相关章节	§D8.0 挑战一/三(电机延迟、气动)、§D8.8.2(Swift 的 sim-to-real)

场景七:动态障碍突现时控制器径直撞上去

项	内容
症状	静态环境正常,但有物体突然进入航线时,控制器不避让、直冲障碍
可能原因	(1) 用的是分离式管线,控制器看不到走廊约束;(2) 重规划频率太低,两次规划间参考已过期;(3) 走廊未随感知更新
排查步骤	① 确认控制器 QP 里是否含 SFC 约束(IPC 模式)而非仅跟踪参考;② 检查 SFC 是否随新障碍实时更新;③ 提高重规划频率或改用 IPC 让 100 Hz 控制周期自带避障;④ 验证感知到走廊生成的延迟
相关章节	§D8.3.1(分离管线的病)、§D8.3.2(IPC 统一 QP)、§D8.3.4(行为差异)

---

预计学习时间

2 周(20-28 小时)

建议时间分配:

内容	时间	优先级
D8.0 敏捷飞行挑战理解	3h	必读
D8.1 Agilicious 架构精读	5h	必读
D8.2 CPC 数学公式理解	3h	必读
D8.3 IPC 统一 QP 理解	3h	必读
D8.4 MPCC 轮廓跟踪	3h	推荐
D8.5 DFBC/INDI 控制器	2h	推荐
D8.6 感知流水线	2h	推荐
D8.7 范式对比与选型	1h	必读
D8.8 系统集成	2h	推荐
D8.9 AC-MPC	2h	选读
实战练习	4-6h	按兴趣选择

速成路线(8h):D8.0 → D8.1 (架构概览) → D8.2 (CPC 概念) → D8.4 (MPCC 概念) → D8.7 (范式对比)

深入路线(28h):全部内容 + 至少完成 2 个 A 型练习 + 全部思考题

---