非凸前瞻 | TIO-Former:不用相机和光流,Crazyflie级微型无人机如何靠六向 ToF + IMU 做 6DoF 里程计

发布时间:2026.09.16
分享:

对微型无人机而言,定位算法从整体架构来看,首先面对的并不是“精度能做到多高”,而是传感器、算力和功耗是否装得下。

视觉惯性里程计和激光雷达能提供丰富几何约束,但相机、镜头、LiDAR 以及对应计算链路都会挤占严格的 SWaP-C(尺寸、重量、功耗与计算)预算;向下光流虽然轻量,却容易在低纹理、弱光和剧烈运动中失效;纯 IMU 又会随着时间持续积累漂移。

TIO-Former 给出了一条更“稀疏”的路线:只使用一枚 IMU 和 六个正交布置的 8×8 多区域 ToF 阵列,每帧总共只有 384 个距离值,ToF 载荷仅 15 g,却直接学习连续 6DoF 相对运动。

论文的关键不是简单把 ToF 与 IMU 拼在一起,而是分别解决三个工程难题:无效测距如何不制造假运动、六个方向该如何按当前运动状态动态取舍、Transformer 的历史记忆如何在长时间飞行中保持固定开销。

图片

图 1|TIO-Former 总览:15 g 六向 ToF 载荷、开放环 3D 轨迹对比,以及 15 Hz 流式推理的精度与延迟

图片

ToF 的价值在于直接输出带尺度的距离。六个传感器分别朝向前、右、后、下、左、上,使机体周围形成极低分辨率但覆盖全向的几何观测。系统以 15 Hz 接收六张 8×8 距离网格,同时把 200 Hz 的 IMU 角速度和线加速度重采样到当前 ToF 周期。网络最终预测机体系下的增量平移与增量旋转,再递推积分成全局轨迹。

因为平移会通过估计姿态旋转到世界坐标,姿态的小误差也会转化为位置漂移,这也是论文后续引入多时间尺度轨迹监督的原因。

与单个向下测距不同,六向布局提供了横向和纵向结构约束。例如前向加速时,前方量程变化最明显;沿走廊飞行时,某些侧向视图可能因墙面平行而退化;垂直运动时,下方地面往往比上方天花板提供更稳定的回波。

换句话说,稀疏 ToF 并不是“低配 LiDAR”,它的可观测性强烈依赖运动方向与场景几何,算法必须学会选择当前最有用的方向。

图片

图 2|六向TOF空间示意图与轻量TOF传感器

图片

TIO‑Former的数据流分为四个阶段。

多模态编码阶段,ToF‑Encoder借助因果填补、双边门控差分提升测距稳定性,将六向ToF观测编码为空间特征Token;IMU‑Encoder从短序列提取加速度、角速度信息,提供运动先验。

IMU引导的ToF融合阶段,以IMU运动状态作为Query,通过交叉注意力动态筛选有效ToF特征,实现自适应几何融合。 

Streaming Causal Transformer时序建模阶段,采用两级有界KV记忆:Local KV Cache保存近期细节,Chunk‑FIFO Memory压缩久远历史,在有限算力下建模长时序上下文。

最终时序特征送入轻量化Pose Head,经Pose MLP回归平移、旋转增量,完成端到端6DoF位姿估计。

图片

图 3|TIO-Former 整体架构:可靠性前端、六向空间编码、IMU 引导方向融合、两级流式时间记忆与位姿输出

图片

多区域 ToF 在真实飞行里会遇到镜面反射、多径、量程外目标和弱回波。最危险的情况不是单纯缺一个值,而是某个像素从“无效/默认值”突然变成“有效距离”,如果直接做帧差,网络会把这种有效性切换误认为物体或机体发生了大幅运动。因此,TIO-Former 把“数值连续性”和“测量可信度”分开处理。

前端从整体架构来看,首先根据量程与传感器状态构造可靠性权重:正常有效回波权重为 1,新出现但缺少上一帧连续跟踪的目标权重为 0.5,其余异常回波为 0;有效量程限制在约 0.015–4.0 m。无效像素可以借助当前有效邻域和历史观测进行因果填补,让 CNN 仍能看到连续网格,但这些填充值的可靠性权重继续保持为 0,明确告诉后续网络“这里是估计值,不是真实回波”。

真正抑制假运动的是双边门控差分:只有前后两个时刻都可信时,帧间距离变化才被保留;任意一端无效,差分就被压到 0。

于是网络的三通道输入同时包含“上一帧绝对距离、当前帧绝对距离、可靠的距离变化”,既保留场景尺度,又显式暴露与速度和转动相关的一阶变化。训练阶段还以 0.6 的概率注入离散、空间块和时间突发缺失,并叠加相关噪声,让模型提前见过现实中的测距劣化。

图片

图 4|因果填补与双边门控差分:缺失项可被补齐用于卷积,但可靠性仍为 0;只有相邻两帧都可信的位置才产生运动差分

图片

六个 ToF 视图先通过同一个 CNN,减少参数与存储带宽;再加入物理方向嵌入恢复“前后左右上下”的身份,并用 Cross-view Transformer 交换全局空间信息。随后,IMU 特征作为 Query,对六个方向的 ToF Token 计算多头交叉注意力。

图片

图 5|IMU引导ToF Cross-Attention融合:系统以 IMU 提供的运动状态作为查询,动态选择并加权六个方向的 ToF 观测,从而保留更有信息量的几何特征

与当前主要运动轴一致、几何视差更强的方向会得到更高权重,退化或噪声方向则被压低。同时保留IMU残差通路,即使六个方向都退化,系统仍不会切断惯性信息。

论文的attention可视化实验验证了这种机制:水平向前、向后、向左、向右运动时,对应方向通常取得行内最高权重;上下运动时,下视传感器反而更受重视,作者认为这是因为地面通常距离更近、回波更稳定。

图片

图 6|训练退化增强使模型在 70% 像素缺失这一训练外条件下仍保持较低误差

这个结果说明 Cross-Attention 学到的并非固定传感器优先级,而是一种与机体运动和环境结构共同相关的动态观测路由。

图片

在线里程计必须严格因果:当前时刻只能使用过去信息。普通 Transformer 如果保留全部历史 KV,缓存和每步注意力成本会随飞行时间线性增长;只保留滑动窗口虽然省资源,却会完全丢掉更早的运动背景。TIO-Former 用两级记忆折中:最近的 Local KV 保持原始细节,较早的 KV 被分组压缩成摘要,再放入固定长度 Chunk-FIFO。

论文设置 Local KV 容量 C=8;满载后每次淘汰最老的 G=4 个 KV,经均值与轻量 MLP 压成一个摘要;Chunk-FIFO 保存 S=4 个摘要。这样网络实际覆盖 24 帧历史,在 15 Hz 下约 1.6 秒,但每步只需要检索 12 个 Token。因为两个缓存容量和模型维度都固定,推理开销和内存占用不再随着飞行时长增长,这正是这一设计能够真正放到边缘 RISC-V 平台持续运行的核心。

消融结果也说明“压缩历史”比“盲目增大窗口”更划算。纯 KV 从 8 个增到 32 个,ATE 只从 0.134 m 小幅降到 0.130 m;而 KV8+FIFO4 在同样只检索 12 个 Token 的情况下覆盖 24 帧,最终达到 0.118 m,并比未压缩 KV24 进一步改善约 11.3%,同时减少一半活跃检索量。

图片

表 1|时间记忆消融:固定 12-Token 检索预算下,Local KV + Chunk-FIFO 兼顾更长历史、较低误差与边缘部署成本

图片

图 7|有界时序记忆机制:TIO-Former 不再无限累积历史,而是保留高分辨率近期状态、压缩更早运动趋势,让微型无人机在有限算力下持续理解自身运动

图片

如果只监督单步相对位姿,一个很小的平移偏差或旋转偏差经过连续积分仍会快速放大。

因此训练目标同时包含单步 Smooth L1、多个时间跨度上的位置与旋转组合误差、全局路径长度约束,以及相邻预测残差的平滑项。

平移监督跨 4/8/16/32 帧,旋转监督跨 4/8/16 帧。这样既约束局部瞬时估计,也直接惩罚累积后的尺度偏移和高频抖动。论文报告,多时间尺度监督相对单步训练可将 ATE 改善约 25.8%。

图片

图 8|多尺度轨迹监督:除了约束每一步的相对位姿预测,TIO-Former 还在不同时间跨度上监督积分后的轨迹一致性,并通过尺度与残差约束抑制长期漂移和局部抖动

图片

真实飞行基准使用 Crazyflie Crazyflie级微型无人机:六个 TOFSense-M 模块每个 2.5 g,总 ToF 载荷 15 g;IMU 以 200 Hz 采样,六向 8×8 ToF 以 15 Hz 更新。Crazyflie 数据共 9,492.01 秒、2,195.22 m,包含圆、8 字、方形、直线与随机 3D 飞行。论文还在办公室、茶水间、长走廊和会议室采集额外 2,362 m,多环境总飞行距离超过 4.5 km。

图片

图 9|实机平台:Crazyflie 搭载六向 TOFSense-M、200 Hz IMU 与 LicheeRV Nano 边缘计算板

在开放环测试中,TIO-Former 的位置 ATE 为 0.118 m,相比 Crazyflie Flow 的 0.259 m 下降 54.4%;相比 AirIO 和 TLIO 等学习式惯性基线,误差降低约 66.4%–89.1%。Endpoint Drift Rate 也降到 7.18%。

更重要的是,它在四类环境都取得最低 ATE:走廊 0.142 m、办公室 0.158 m、茶水间 0.091 m、会议室 0.114 m。纯粹把 ToF 和 IMU 做静态拼接的 Naive ToF+IMU 明显更差,说明提升并不来自“多装了几个距离传感器”,而来自可靠性建模、方向选择和时间上下文的共同作用。

图片

图 10|开放环轨迹对比:TIO-Former 在圆、方形、8 字与随机飞行中更好保持闭环形状与急转弯结构

图片

表 2|主结果与多环境结果:TIO-Former 在位置 ATE、相对运动误差和终点漂移率上均取得更优表现

图片

方向缺失实验最能说明 IMU 引导路由的意义。正常传感时,Cross-Attention 相比通道拼接的 ATE 优势只有约 7.8%;但当 1–5 个方向持续、随机或突发丢失时,优势迅速扩大。跨不同缺失数量,Cross-Attention 的平均相对 ATE 增幅约 57.19%,而通道拼接达到 151.84%。在最极端的“五个方向同时缺失”下,前者仍保持 0.307 m ATE、22.12% 失败率,后者恶化到 0.569 m 和 53.31%。

图片

图 11|方向盲区鲁棒性:缺失方向越多,动态 Cross-Attention 相比平均池化和直接拼接的优势越明显

随机退化训练同样显著提高了外推能力。训练只见过最高 50% 的缺失比例,但测试把缺失提高到 70% 时,增强模型仍只有 0.245 m ATE、12.0% 失败率;未做退化增强的模型则达到 0.760 m 和 74.3%。

架构消融还显示:IMU-only ATE 为 0.816 m,ToF-only 为 0.156 m;只看下方会恶化到 0.937 m,而四个水平侧向视图已接近完整六向的 0.129 m。因果填补与双边门控差分在成对距离输入基础上再降低约 25.3% ATE。

图片

图 12|方向注意力热图:水平运动时权重沿对应运动方向形成明显对角结构;垂直运动中下视方向更稳定

图片

论文没有停留在离线 GPU 评测,而是把模型部署到 SG2002 SoC。ToF CNN 在 CV181x TPU 上以 BF16 运行,IMU 编码、方向融合与流式记忆在 850 MHz RISC-V C906 CPU 上以 FP32 RVV 执行。

模型只有 1.117M 参数,存储约 3.958 MiB;峰值常驻内存 6.324 MiB,仅占 128 MiB RAM 的 4.94%。平均单步延迟 10.038 ms,P95 为 10.466 ms,最大观测延迟 20.609 ms,5000 个周期中没有一次超过 66.7 ms 的 15 Hz 截止时间。

图片

表 3|板端资源与时延:固定记忆让持续飞行不会把 KV 缓存越堆越大,实测 5000 个周期无截止时间超限

图片

TIO-Former 展示出一种很有工程意味的思路:当Crazyflie级微型无人机无法承担相机视觉或 LiDAR 的重量与算力时,不必放弃外部几何信息,而可以用极稀疏、全向、带尺度的 ToF 去持续锚定惯性积分。

其完整闭环由四个机制共同成立——可靠性掩码避免无效回波制造假运动,IMU 引导 Cross-Attention 让六向观测按运动状态动态分工,两级 Streaming Transformer 用固定成本保留近期细节和更早摘要,多时间尺度监督则直接约束积分后的轨迹一致性。

另外,论文也明确指出了边界:ToF 的有限量程与几何退化仍会限制开放环长期估计,当前系统也没有闭环修正。因此作者把轻量级 loop closure 和自监督预训练作为后续方向。换言之,TIO-Former 解决的并不是“永久无漂移”的最终定位,而是验证了384 个稀疏距离值 + IMU,也能够在极端资源预算下形成实时、可部署、对退化具有韧性的 6DoF 里程计。对于真正受重量、功耗和边缘算力限制的微型飞行机器人,这一点比单纯追求离线精度更重要。

论文链接(点击左下角“阅读原文”):

http://arxiv.org/abs/2609.17198

作者介绍

作者: 刘洋,浙江大学控制科学与工程学院23级本科生,研究兴趣学习式里程计、深度强化学习、机器人多模态感知。

邮箱地址:1211175035@qq.com

关于我们

微分智飞(杭州)科技有限公司,以让飞行机器人“连接千行百业,飞入千家万户”为使命,致力于打造全球领先的飞行具身大脑及通用群体智能系统。公司依托自主导航、具身决策、端到端控制及群体智能四大核心技术,突破环境限制,打造面向多样复杂场景的飞行机器人基座模型,驱动构建应用于家庭、工业、教育、城市与自然空间的“空天智能终端”。

非凸空间是微分智飞旗下教育子品牌,以通用飞行平台产品为核心,致力于为高校、科研院所及应用单位提供全方位科研教育解决方案,涵盖实验室建设、专业建设、赛事支持、课程培训、人才培养及基金课题合作等领域,助力飞行机器人科研与教学更专业、更高效。

  END

图片

获取更多内容 请关注公众号