IEEE RA-L | 光鉴科技 ToF 定位研究成果发表于机器人权威期刊

近日，光鉴科技研究成果 ToF-Wheel Odometry: Navigating Degeneration of Localization With Limited Sensing Capabilities 发表于机器人领域权威期刊 IEEE Robotics and Automation Letters （RA-L）。

针对仅配备ToF 深度相机和轮编码器的地面机器人在运动异常与几何约束不足时面临的定位难题，该研究提出紧耦合滤波框架 ToF-Wheel Odometry（TWO）。通过退化感知的方向性协方差调整与帧内反馈两项关键设计，该方法无需依赖 IMU 提供运动先验，提升机器人在打滑、地形引起的姿态变化及几何退化等复杂条件下的定位稳健性。

研究背景

轮式里程计延迟低、计算量小、短时一致性好，是地面机器人最常用的运动先验。但它依赖平面运动、姿态局部一致等假设，经过坡道、地面材质切换或不平整路面时，打滑与倾斜会让预测轨迹与真实运动出现短时但显著的偏差。

消费级机器人通常用ToF 深度相机补充几何约束，因为它体积小、功耗低、成本可控。但 ToF 相机测距短、只有前向视场，几何可观测性有限。例如在长走廊中，墙面能约束横向位置，却难以约束沿走廊方向的位移。

左为湿滑地面、线缆门槛与坡道上的轮子打滑，右为长走廊实景与 ToF 视场的俯视点云

因此，这类系统的定位退化往往不是单一传感器失效，而是两种局限相互叠加的结果。这与激光惯性里程计（LIO）的处境不同：LIO 有大视场、远距离的激光雷达观测，还有 IMU 提供六自由度运动先验。

现有滤波方法大多采用固定或自适应的过程噪声协方差，自适应方法又多为标量或对角缩放。这对地面机器人并不适用：打滑和倾斜是突发的脉冲式扰动，不是平稳噪声。整体放大不确定性，会削弱本来可靠方向上的置信度；保持协方差不变，又会在关键时刻过度自信。观测端的鲁棒化也解决不了这个问题，因为不确定性来自运动模型本身失效。

TWO 把过程协方差的自适应拆成四个问题：加多少、加在哪个方向、几何观测是否支持、何时生效。

方法介绍

TWO 系统框图。轮式传播给出先验，ToF 更新给出几何修正，方向性协方差调整与帧内反馈在同一帧内修复过程噪声

TWO 基于 EKF，由四个环节组成：轮式传播、带可观测性分析的 ToF 更新、方向性协方差调整、帧内反馈。其中 ToF 更新沿用了光鉴科技 IROS 2024 工作C³P-VoxelMap的概率点到平面约束。

修正量的子空间分解

每一帧，轮式里程计先给出一个预测位姿（先验），ToF 点云与局部地图配准后再给出修正后的位姿（后验）。两者之差  就是 ToF 观测把机器人"拉回"了多少，它是TWO判断轮子是否出错的直接依据。但这份修正量里混着两类性质不同的误差。差速底盘只能前进后退和原地转向，轮编码器也只测得到这两个自由度；侧向平移、上下起伏、横滚和俯仰，轮式模型根本无法表达。因此，TWO 先把  投影到由底盘运动学决定的基  上，拆成两部分：

其中  是修正量在前进、偏航两个方向上的大小。

轮式可解释分量 ：前进与偏航方向的修正，说明轮子"量错了"，典型来源是打滑；

正交余量 ：侧向、垂直、横滚与俯仰方向的修正，对应轮式模型之外的运动，例如侧滑、越障时车身抬起或倾斜。但它也可能只是退化几何下的观测噪声。

前者可以直接用来衡量轮子出了多大问题；后者是否可信，要先看 ToF 在这些方向上有没有足够的几何约束。

退化感知的可观测性评分

ToF 能否可靠地纠正某个方向，取决于当前视野里的几何结构。在长走廊里，两侧墙面能锁定横向位置，沿走廊方向却几乎没有约束；换到家具较多的房间，各个方向都能被约束住。TWO 用点到平面残差的雅可比  和观测协方差  构造测量信息矩阵 ，再转成信息增益算子 ：

的每个特征值都落在0-1之间，表示观测在对应方向上提供了多少信息：约束充分时接近 1，几乎没有约束时接近 0。

为了便于在滤波中使用，TWO 把 压缩成一个标量：取 Frobenius 范数下最接近 的 。它的闭式解就是各方向增益的平均值：

其中  为位姿维数， 为  的特征值。 可以理解为当前帧的"几何可信度"：六个自由度都被锁定时接近 1；进入长走廊、沿走廊方向失去约束后随之下降，弱约束的方向越多，下降越多。把同样的计算限定在正交余量对应的四个方向上，就得到余量可观测性评分 。 它专门回答一个问题：侧向、垂直和姿态方向的修正，有没有几何证据支撑。

方向性协方差调整

有了修正量的分解和两个评分，TWO 分别决定过程噪声"加多少"和"加在哪"。

加多少。如果轮式模型的不确定性设置得合理，轮式可解释分量  的大小应当和滤波器的预期相符。TWO 假设  服从协方差被放大  倍的高斯分布，通过最小化负对数似然得到放大系数的闭式解：

其中  是  的马氏距离平方，衡量这次修正相对滤波器预期有多大； 是有效维数。修正超出预期时，，过程噪声被放大；在预期范围内时保持基准值，不会被调得更小。几何退化时，修正量本身可能不可靠，因此再用  对  做平滑：

几何可信度高时， 快速跟随当前帧的估计；可信度低时，主要沿用上一帧的值，避免被退化观测带偏。

加在哪。正交余量先按  缩放，再与可解释分量合成，作为放大不确定性的方向：

其中  是  归一化后的方向， 是本帧内的轮速读数个数， 是经 SE(3) 伴随变换转到第  个传播步下的同一方向。也就是说，额外的过程噪声只沿 这一个方向注入，并平均分摊到本帧的每个轮式传播步上。

由此，余量的去留由几何证据决定：

几何能支撑侧滑或姿态变化时（），余量被采纳，不确定性朝侧滑或倾斜方向放大；

几何退化时（），余量被当作传感器噪声抑制，不确定性只在前进、偏航方向放大。

此外，只有轮式可解释分量能驱动  增大，余量只影响方向。这样可以避免退化几何产生的虚假修正单独推高不确定性。

帧内反馈

标准卡尔曼滤波中，过程噪声在传播之前就已确定，当前帧发现的异常只能影响下一帧。打滑恰恰是突发的：发生打滑的那一帧，先验协方差仍然很小，滤波器过度相信轮式预测，ToF 的纠正作用被压得很弱。误差最大时，滤波器对误差的估计反而最小。

TWO 在当前帧内完成修复：

1.用当前的过程噪声做轮式传播，完成一次ToF 更新；

2.根据得到的修正量，按上文方法计算新的  ；

3.从上一帧的后验协方差出发，用新的过程噪声重新传播本帧的协方差，再做一次ToF 更新；

4.先验与后验收敛，或达到迭代上限时结束。

反馈期间，传播的均值和局部地图保持不变，只修复协方差；后验被接受后才更新地图。迭代次数有上限，额外延迟因此被限制在少数几次EKF 更新以内。协方差修复在当前帧内完成，不用等多帧自适应窗口慢慢收敛。

实验结果

实验平台为差速底盘，机载计算单元为树莓派4B，测试相机为光鉴科技自研的Nebula 280。该相机以sToF技术为核心，融合 3D 结构光与mToF 方案，同时具备近距离和中远距离的高精度深度感知能力，支持 H98° × V60°的视场角，室内测距最高可达 12 m。采集序列的真值由独立安装的Livox Mid-360 激光惯性系统生成，TWO 不使用该数据。所有参数在全部序列中保持一致。

与主流方法对比

对比方法为VoxelMap、AKF-LIO、Faster-LIO 和 LIWO。在自采集序列中，各方法使用同一份ToF 点云，唯一的区别是运动先验：对比方法使用模组内置IMU，TWO 只使用轮编码器。

在大厅、办公室、长走廊三个序列中，TWO 的 ATE 分别为0.072 m、0.177 m、0.249 m，均为最低。五个序列的平均ATE 为 0.190 m，次优方法为0.411 m。

三个自采集序列的轨迹对比，(d) 为 (a)(b) 中虚线框处的局部放大

从轨迹上看，TWO 在大厅中能沿打滑转弯的折线轨迹前进，在办公室中保持了矩形回环的形状，在走廊中避免了纵向过冲。对比方法则出现了路径扭曲、旋转漂移或纵向过冲。

在M3DGR 公开数据集（轮子悬空、急转弯）上，由于数据集不含 ToF 相机，TWO 改用激光雷达点云作为几何输入。使用陀螺仪的 AKF-LIO 与 Faster-LIO 在短时 RPE 上更优，但 ATE 差距较小（0.190 vs 0.209 m，0.200 vs 0.242 m）。在轮子悬空序列上，TWO 的精度优于同为轮式融合的 LIWO。

与不同协方差调节机制对比

为了排除传感器配置的影响，研究在相同的ToF 与轮编码器输入下，对比了固定协方差和标量自适应协方差：

TWO 相比固定协方差基线，ATE 降低约 82%。值得注意的是，标量自适应反而比固定协方差更差：每次修正都各向同性地放大不确定性，把不确定性加进了轮式先验本来约束良好的方向。这说明只调幅度不仅不够，还可能适得其反。

研究还把基线过程噪声  在 0.1× 到 10× 之间缩放。在每一个尺度下，TWO 相比静态配置都将 ATE 降低 74.3%–83.2%。可见这部分收益无法靠整体调大过程噪声获得。

消融实验

余量门控：强制抑制余量（η=0）时，侧滑和抬升/姿态扰动的 ATE 分别升至 0.164 m 和 0.115 m（完整方案为 0.072 m 和 0.042 m）；强制全部采纳余量（η=1）时，前向打滑叠加弱几何场景的误差上升。余量应当只在其子空间可观测性不足时被抑制，而不是只要违反轮式模型就抑制。

幅度膨胀：固定 β=0 时，四个序列的平均 ATE 从 0.046 m 升至 0.219 m，是退化最大的变体。

帧内反馈：去掉反馈后，前向打滑叠加弱几何、抬升/姿态扰动两个场景的 ATE 分别增至完整方案的 1.60 倍和 1.50 倍。

消融实验中的选择性门控行为

嵌入式实时性

大多数帧在首次更新后就满足收敛条件，额外迭代集中在运动异常时刻，平均每帧不到0.5 次。一次反馈就能取得大部分精度收益。

结语

继此前围绕 C³P-VoxelMap 开展三维地图表示与更新效率研究后，此次提出的 TWO则进一步聚焦复杂环境下的机器人定位难题，通过评估环境几何约束的可靠性，动态调整运动估计中的不确定性，提升机器人在运动异常与几何退化叠加场景下的定位稳健性。该方法无需依赖 IMU 提供运动先验，并在树莓派平台完成运行验证，为资源受限条件下的稳健定位提供了新的技术思路。

从三维地图构建到稳健定位，光鉴科技持续探索机器人三维视觉技术的创新与应用。目前，TWO 相关定位技术已实现规模化应用。未来，光鉴科技将继续推动技术研究与实际应用深度结合，为机器人在复杂环境中的自主移动与空间理解提供更轻量、可靠的感知支持。