IEEE Transactions on Robotics

HDVIO2.0: Wind and Disturbance Estimation With Hybrid Dynamics VIO

作者：Giovanni Cioffi, Leonard Bauersfeld, Davide Scaramuzza · 机构：Robotics and Perception Group, Department of Informatics, University of Zurich, Zürich, Switzerland

会议/期刊：IEEE Transactions on Robotics · 发布日期：2025 · 页码：5396-5410

控制飞行机器人视觉定位建图状态估计

精读笔记

Problem Setting

论文标题：HDVIO2.0: Wind and Disturbance Estimation With Hybrid Dynamics VIO（IEEE Transactions on Robotics / 2025）。

这篇论文不是在做普通 VIO，也不是单独做风估计，而是在解决一个更具体的问题：如何在四旋翼 VIO 中引入足够强的动力学约束，使其在高速、风扰、控制输入不准、气动效应显著时仍能同时估计状态和外力。

关键矛盾是：动力学模型越强，越可能提高 VIO 在视觉退化或高动态下的可观性；但模型一旦不准，它又会成为强错误先验，把气动 drag、推力系数误差、持续风扰等错误解释成 accelerometer bias、外力或姿态漂移。VIMO / VID 这类方法的问题不是没有用动力学，而是用了过于理想化的平动动力学，并假设动力学噪声接近零均值。这在短时随机扰动下还行，在持续风、系统性推力误差、高速气动下会崩。

另一个真实难点是 rotational dynamics。平动动力学可以通过 thrust preintegration 写成相邻关键帧约束，但旋转动力学里 torque、angular velocity、orientation 纠缠在一起，很难满足预积分需要的“测量与优化变量可分离”。因此 prior 往往继续用 gyro 提供 orientation evolution，动力学因子只约束 position / velocity。这使得姿态在 dynamics 层面基本没有被真实执行器模型约束，也会重复使用 gyro 信息，带来一致性问题。

Motivation

作者的动机可以概括为：已有 model-based VIO 的 failure mode 很多不是来自视觉，而是来自“错误动力学被过度相信”。如果点质量模型忽略 drag，那么高速飞行中的空气阻力会被估计器看成外力；如果 thrust coefficient 有偏，z 方向力误差会被看成 accelerometer bias；如果风是持续的，zero-mean dynamics noise 假设会直接失效。

直接引入 NeuroBEM / BEM / polynomial aerodynamic model 这类高保真动力学也不自然，因为它们通常需要线速度、姿态、角速度等状态输入。在线 VIO 中这些状态正是待估变量，而且可能有误差。把估计状态喂给动力学模型再作为约束反馈给估计器，会形成自激式错误传播。作者要避免的不是高保真模型本身，而是 state-dependent learned dynamics 对 VIO 后端的反向污染。

关键缺口是：能否得到一个“足够强但不依赖待估状态”的动力学先验？作者的答案是利用控制输入和 IMU 历史。对四旋翼而言，短时 thrust / torque command、rotor speed、gyro history 本身包含了执行器动态、机体姿态变化、诱导气流、drag-like effect 的可预测结构。这个观察是全文最重要的前提。

Core Idea

核心思想是把动力学约束从“低保真物理模型”升级为“物理骨架 + learned residual”，但刻意限制 learned model 的输入，使它不依赖当前 VIO 估计的速度和姿态。TCN 只看控制输入和陀螺历史，输出 residual thrust / residual torque。这样 learned model 学到的是执行器-机体-短时气动系统的局部响应，而不是从状态估计中读取一个可能错误的全局 aerodynamic state。

更本质的变化是信息流被重新组织了：prior 方法让动力学模型根据 thrust 推出平动约束，未建模部分由 external force / bias 吸收；HDVIO2.0 先用学习残差把“可由平台自身气动和输入误差解释的部分”从外力通道中剥离，再把剩下的 motion divergence 解释成真正外部扰动。这对风估计尤其重要，因为否则 estimator 很容易把机体 drag 和风力混在一起。

旋转动力学的处理也很关键。作者没有强行把 torque dynamics 写成传统预积分形式，而是用连续时间 B-spline 表示 angular velocity，并通过 torque equation 拟合其导数。这样 rotational dynamics 被转成一个在线可优化的局部连续时间插值问题；采样出的 body rates 再进入 orientation preintegration。它不是完整地把所有旋转动力学变量联合进主 VIO 后端，而是用一个中间连续时间表示把不可预积分的问题工程化地变成可预积分。

Method

1. Hybrid translational dynamics：点质量模型仍是骨架，collective thrust 提供主要 z-body force；TCN 预测 residual thrust，用来补偿 drag、推力映射误差和短时气动效应。它解决的是“外力估计被系统性模型误差污染”的问题。核心变化是把原本进入 external force / bias 的一部分误差前置到 dynamics input correction。

2. Hybrid rotational dynamics：torque command 加 learned residual torque 后，通过刚体转动方程约束 angular acceleration。由于直接把 rotational dynamics 放进预积分不方便，作者用 B-spline 表示 body rate，并最小化 spline derivative 与 torque dynamics 的差异。它解决的是 prior dynamics factor 不约束姿态的问题。核心变化是姿态增量不再完全依赖 gyro，而被 actuator torque model 间接约束。

3. Dynamics preintegration factor： corrected thrust 和 B-spline sampled body rate 被预积分为相邻状态之间的 position / velocity / orientation change，作为滑窗 VIO 的 dynamics residual。这里的设计目标不是替代 IMU preintegration，而是提供一个与 actuation 一致的 motion prior，使视觉/IMU/动力学三者在 backend 中互相校验。

4. External force estimate：外力变量仍在状态中优化，并使用类似 VID-Fusion 的 acceleration-thrust difference 先验。它承担的是“hybrid model 解释不了的 motion divergence”。这点很重要：HDVIO2.0 并不是让网络直接输出 wind，而是让网络减少内部模型误差，外力由剩余不一致性估计出来。

5. Training without force labels：网络用相对 position / velocity / orientation change 的监督训练 residual dynamics，而不需要 ground-truth force。机制上等价于让 residual model 学会让 dynamics rollout 对齐观测轨迹。这降低了数据采集门槛，但也意味着 residual 的物理可解释性依赖训练环境中“无外力”的假设。

Key Insight / Why It Works

最核心的有效性来自 representation alignment，而不是网络规模。作者没有让网络端到端回归 pose，也没有把 VIO 后端黑盒化，而是让网络只预测动力学模型中最容易系统性出错的局部残差：force / torque correction。这种残差与控制输入、gyro history 在时间尺度上高度对齐，学习问题比直接学习 aerodynamic force as function of full state 更稳定，也更适合在线 estimator。

它有效的第二个原因是误差归因被改善。VIMO / VID 的问题是只有 thrust model 和 external force / bias 两个解释通道；当 thrust model 有偏或 drag 持续存在时，优化器会把错误塞进 bias 或 external force。HDVIO2.0 增加了 learned residual 这个中间解释层，使“平台内生气动/标定误差”和“外部扰动”更可分。这是 force estimation 提升的主要来源，尤其是 z 轴 thrust bias 和风场实验中的持续扰动。

第三个原因是短时 history 隐式编码了部分状态。论文强调 learned model 不需要 velocity / attitude 输入，但这不等于它完全 state-free。gyro history、control history 和 thrust/torque 序列在短窗口内包含了姿态变化、机动模式、加速度响应的代理信息。换句话说，网络可能是在利用局部动态轨迹片段做 implicit state inference。这个设计是优点也是边界：它避免了显式 VIO state feedback，但仍依赖训练数据覆盖足够多的局部动态模式。

B-spline rotational dynamics 是实质贡献，但更偏 estimator engineering + representation trick。它解决了“旋转动力学难以预积分”的在线可计算性，而不是提出新的动力学理论。选择 angular velocity spline 而非 orientation spline 的理由主要是优化收敛快；这是合理的工程判断。真正的 conceptual contribution 是用局部连续时间 latent trajectory 作为 torque dynamics 与 discrete VIO factor 之间的桥。

哪些可能只是辅助？TCN 的具体结构、层数、GELU、窗口长度等大概率不是本质；增益主要来自把 residual learning 放在 dynamics input correction 这个位置，以及训练数据中覆盖了目标平台的气动/执行器误差。所谓 generalization 到更高速值得注意，但不能过度解读为跨域泛化；它更像同一平台动力学流形上的外推。

文中一个未充分说明的问题是 learned residual 与 wind force 的可辨识性。训练在无外力数据上进行时，网络会学习“正常气动残差”；测试遇到风时，理论上风导致的额外 force 应留给 external force estimate。但如果训练数据中存在未标注气流、地效、轻微扰动，或者测试风产生的响应与训练中的高速 drag 模式相似，网络和 external force 之间可能出现解释竞争。论文通过实验显示效果好，但理论 identifiability 并没有完全解决。

Relation To Prior Work

最接近的是 VIMO、VID / VID-Fusion 和作者自己的 HDVIO。VIMO 开启了把 actuation dynamics 作为 VIO factor 的路线，但使用简化平动模型，并把外力当作随机变量。VID 改进外力先验，能更好处理 constant load，但仍无法处理系统性动力学误差和复杂气动。HDVIO 引入 learned residual thrust，但仍停留在平动 3-DoF。HDVIO2.0 的新增点是 residual torque + rotational dynamics + 连续时间 body-rate 表示。

和 NeuroBEM / BEM / PolyFit 这类 drone dynamics modeling 的差别在于目标不同。那些模型追求 standalone aerodynamic force prediction，通常可以使用 full state；HDVIO2.0 的 learned dynamics 是为 VIO backend 服务的，关键约束是不能依赖待估状态，否则会污染估计闭环。因此它牺牲了一部分物理完整性，换取 estimator compatibility。

看似新的部分里，hybrid model、residual learning、TCN sequence modeling、B-spline continuous-time trajectory 都不是全新思想；真正的新意在于组合方式：把 residual dynamics 设计成可预积分的测量修正，并用 CT angular velocity spline 打通 rotational dynamics 与 sliding-window VIO。它属于 model-based estimation 与 learned residual dynamics 融合的谱系，而不是纯 learning-based odometry。

实质创新是两个：一是证明不输入 velocity / attitude 也能学到足够有用的四旋翼残余力/力矩，用于 VIO；二是给出一个在线可运行的 6-DoF dynamics factor 方案，特别是旋转动力学进入 VIO 后端的工程路径。

Dataset / Evaluation

评估覆盖面比较强：NeuroBEM 用来单独验证 learned dynamics；Blackbird 验证高速无外扰下 pose improvement；VID 验证外力估计和无 mocap 训练；自建风场验证持续风扰；闭环飞行验证在线部署。这比单一 benchmark 更能支撑论文 claim。

NeuroBEM 实验支持“无需 full state 的 learned residual 仍能逼近气动模型”这一点，但也要注意该评估仍是同类四旋翼、同数据分布内比较，不等价于跨平台泛化。Blackbird 高速序列上提升明显，比较好地验证了 drag / dynamics residual 对 VIO drift 的帮助。VID 和风场实验更直接支持 external force estimation claim，尤其能看出 HDVIO2.0 减少了 bias-divergence 问题。

真实风场实验是论文比较有说服力的部分，因为持续 wind 正是 VIMO / VID 假设最容易失效的地方。作者还构建了 wind speed map 和 dragboard 配置，试图给 force ground truth。这个设置比只看 trajectory ATE 更有价值。不过 wind force ground truth 仍来自气动模型和离线测量，不是完全直接测量；虽然有 load cell 校验，但精度上限受风场非定常、空间插值、机体姿态和相对风速建模影响。

闭环实验说明系统能实时跑并能控制，但它更像 deployment sanity check，不足以证明复杂场景下长期鲁棒性。整体评估支持核心机制有效，但对各组件贡献的归因仍有空缺：rotational dynamics、learned torque、learned thrust、external force model、视觉前端选择之间的耦合没有被完全拆开。

Limitation

第一，方法强依赖平台特定动力学。网络学习的是某个机体、桨叶、执行器映射、载荷配置、传感器时延和控制接口下的 residual。换机型、换桨、换质量分布、硬件老化、推力映射漂移，都可能需要重新采集数据。作者说数据采集容易，这是真的，但也说明泛化并不是免费的。

第二，learned residual 和 external disturbance 的边界并不严格。训练时假设无外力，部署时把模型解释不了的部分当外力；但如果外力模式和训练中的气动残差模式相似，网络可能吸收一部分真实 disturbance，或者 external force 吸收平台内生误差。这个 identifiability 问题文中未充分说明。

第三，所谓不依赖状态输入并不等于没有隐式状态。gyro/control history 是一种短时 latent state carrier。它能工作说明局部历史足够预测许多气动残差，但在长时依赖、强风切变、地效、复杂相对风、非平稳执行器状态下，这个短窗口记忆可能不够。这里的泛化可能主要来自训练数据覆盖，而不是模型真正理解了 aerodynamic physics。

第四，旋转动力学部分虽有创新，但可能有相当工程成分。B-spline 优化、采样、预积分、协方差传播的在线实现很重要，但其增益在不同场景下未完全隔离。部分实验显示 rotation error 改善明显，但是否主要来自 learned torque、B-spline smoothing、gyro denoising，还是额外 torque prior，归因不清。

第五，force ground truth 的评估仍有模型依赖。VID 中有 force sensor 的序列较有价值；风场实验的 ground truth 则通过风速图和气动系数计算得到，虽然合理但不是绝对真值。对于 force estimation claim，这是一个需要保留的 caveat。

第六，系统复杂度增加带来的调参面更大：dynamics noise、residual covariance、external force prior、bias random walk、TCN 训练数据、B-spline fitting 都会影响最终归因。论文强调实时性，但没有充分讨论这些权重错配时的稳定性。

Takeaway

  • 1. 对 model-based VIO 来说，关键不是“加入更强动力学”本身，而是让动力学误差有正确的解释通道。
  • HDVIO2.0 的价值在于把平台内生残差和外部扰动分离得更好。
  • 2. 不依赖待估状态的 learned residual dynamics 是一个值得迁移的设计原则。
  • 对于闭环 estimator，避免把当前估计状态反馈进 learned dynamics 可以显著降低 compounding error 风险。

一句话总结

HDVIO2.0 是 model-based VIO 向 learned residual dynamics 演化的一步：它真正贡献的不是一个更大的网络，而是把平台气动残差、旋转动力学和外力估计重新组织成一个在线可预积分、较少污染状态估计的 6-DoF hybrid dynamics factor。