整体式融合：与任务和平台无关的机器人定位与状态估计Holistic Fusion (HF)

01 动机Motivation

同一台机器人的不同任务对状态估计的要求互相冲突：legged locomotion 与 tracking control 需要低延迟、平滑、局部一致的估计（尤其是速度），而 global planning 与户外施工需要全局精确的位姿。更麻烦的是，测量分散在互不相连的坐标系里——GNSS 表达在 world frame，外部 SLAM 的 scan-to-map registration 位姿表达在各自的 map frame，而这些 map frame 本身还在随时间漂移；同时 GNSS 会因植被或多径而退化，LiDAR 会遇到 geometric degeneracy。既有融合系统要么为某个平台硬编码，要么把绝对测量手工改写成 between factor，要么在核心优化之外单独做一次刚性对齐，都难以原样搬到新任务上。Different tasks on one and the same robot impose conflicting requirements on state estimation: legged locomotion and tracking control need low-latency, smooth and locally consistent estimates (velocities above all), whereas global planning and outdoor construction need globally accurate poses. Worse, the measurements are scattered across mutually disconnected coordinate frames — GNSS is expressed in the world frame, the scan-to-map registration poses of an external SLAM system are expressed in their own map frames, and those map frames themselves drift over time; at the same time GNSS degrades under vegetation or multipath, and LiDAR runs into geometric degeneracy. Existing fusion systems are either hard-coded for one platform, or rewrite absolute measurements into between factors by hand, or perform a separate rigid alignment outside the core optimization — none of which carries over to a new task unchanged.

Holistic Fusion formulates sensor fusion as a combined estimation problem of i) the local and global robot state and ii) a (theoretically unlimited) number of dynamic variables, including automatic alignment of reference frames; this formulation fits countless real-world applications without conceptual modifications, offering a comprehensive solution beyond hard-coded/task-specific approaches.

02 方法Method

HF 把待估变量分成两部分：固定尺寸的机器人导航状态（位姿、速度、IMU bias），以及按需动态创建的上下文变量——全局时不变状态、参考系对齐状态、landmark 状态。三类变量与机器人状态在同一个 factor graph 里被联合优化，因此任何测量都能以「它自己的类型、它自己的参考系」直接进图，无需预处理；反过来，机器人状态也能在当前存在的任意参考系里以全速率同步取出，等价于内置了一个 localization manager。HF splits the variables to be estimated into two parts: a fixed-size robot navigation state (pose, velocity, IMU biases) and context variables created dynamically on demand — global time-invariant states, reference-frame alignment states and landmark states. These three kinds of variables are optimized jointly with the robot state in one and the same factor graph, so any measurement can enter the graph directly "with its own type, in its own reference frame", without preprocessing; conversely, the robot state can be read out synchronously and at full rate in any currently present reference frame, which amounts to a built-in localization manager.

整体状态：机器人状态 + 动态上下文变量Holistic state: robot state plus dynamic context variables

论文指出，HF 与其它融合框架的一个主要区别在于：被估计的状态不只包含机器人运动的定尺寸信息，还引入了一组随 setup 变化的动态上下文变量并整体优化——它们按需分配，包括 i) 全局时不变状态、ii) 时变（漂移）或时不变（不漂移）的参考系对齐状态、iii) landmark 状态。相比之下，既有的融合公式只把机器人状态与 IMU bias（可能再加 landmark 与标定量）放进状态向量，实践中能支持的测量模型也就被限死在这些状态上。所有测量函数按统一的 Algorithm 1 拼装：先做参考系对齐，再并入 landmark 状态，然后做传感器坐标系变换与外参标定。The paper notes that one major distinction between HF and other fusion frameworks is that the estimated state does not only contain fixed-size information about the robot motion, but also introduces a setup-dependent set of dynamic context variables that are optimized holistically — allocated as needed, they consist of i) global time-invariant states, ii) time-variant (drifting) or time-invariant (non-drifting) reference-frame alignment states, and iii) landmark states. By contrast, existing fusion formulations include only the robot state and IMU biases (and possibly landmarks and calibration terms) in their state vector, which limits the measurement models supported in practice to those states. Every measurement function is assembled by the same Algorithm 1: reference-frame alignment first, then incorporation of landmark states, followed by the sensor-frame transformation and extrinsic calibration.

用 random walk 显式建模参考系漂移Modelling reference-frame drift as a random walk

把外部 SLAM 的绝对位姿融进来的老办法是 Umeyama 刚性对齐，但论文的野外数据表明：漂移一旦发生，根本不存在一个刚性变换能同时对齐两条轨迹（图 13、图 14 中，仅用一次全局对齐连平移都对不上）。HF 转而把每个参考系相对 world frame 的变换建模成 random walk——这一思路借鉴了估计中把 IMU bias 建成 random walk 的成功经验，实现简单，且主要依赖时间：间隔越久，允许该变换变化得越多。论文强调，random walk 要「按需要尽量松、但能多紧就多紧」，因为在缺少全局测量的时段它同时充当运动先验。The classical way to fuse in the absolute poses of an external SLAM system is a rigid Umeyama alignment, but the paper's field data show that once drift occurs there simply is no rigid transformation that aligns the two trajectories (in Fig. 13 and Fig. 14 a single global alignment fails even in translation). HF instead models the transformation of each reference frame w.r.t. the world frame as a random walk — an idea borrowed from the successful modelling of IMU biases as a random walk in estimation: it is simple to implement and depends primarily on time, in the sense that the longer the elapsed duration, the more the transformation is allowed to change. The paper stresses that the random walk should be as loose as necessary but as strict as possible, since during phases without global measurements it doubles as a motion prior.

In practice the random walk should be set as loosely as necessary (to account for the occurring drift), but as strictly as possible, to serve as a valid motion prior needed for some estimation problems.

在局部 keyframe 而非全局原点处对齐Aligning at local keyframes rather than at the global origin

若把对齐变量参数化在 world frame 原点，则机器人离原点越远，姿态误差引起的杠杆臂越长，旋转敏感性随距离线性增长。HF 因此沿路径不断新建 keyframe，把已知的 keyframe 位置从测量中减去，使对齐在一个很小的杠杆臂下进行，优化变量变成数值上更稳定的 T_WK；需要回看整体漂移时，再把 keyframe 位置加回去换算成 T_WR。论文称这个「新建 keyframe」的参数化不只是设计选择，而是让自动对齐在实践中真正可用的关键组件，也是支撑长距离任务的前提。If the alignment variable is parametrized at the origin of the world frame, then the farther the robot travels from that origin, the longer the lever arm induced by an attitude error, so the rotational sensitivity grows linearly with distance. HF therefore keeps creating new keyframes along the path and subtracts the known keyframe position from the measurements, so that the alignment happens under a very small lever arm and the optimization variable becomes the numerically more stable T_WK; to trace back the overall drift, the keyframe position is added again to obtain T_WR. The paper calls this "new keyframe" parametrization not merely a design choice but the crucial component that makes the automatic alignment work in practice, and the prerequisite for missions spanning large distances.

不跳变的平滑里程计A smooth, jump-free odometry

新信息（例如 GNSS 长时间缺失后回归）会让 world frame 中的位姿与速度信念显著跳变；这对定位是好事，对 tracking control 与点云去畸变却是灾难。HF 的做法是不在 world frame 里积分，而是把当前的位姿与线速度信念转到 body frame 增量 T_IkIk+1 上做递推，再映射回 odom frame；每次更新都用优化后的机体系速度覆盖里程计速度以抑制积分漂移，并把 roll 与 pitch 用 world frame 的值覆盖，供依赖重力方向的模块使用。由此 world frame 的估计尽可能准，odom frame 的估计尽可能平滑，正对应机器人里常见的 map → odom 分工。New information — for instance GNSS returning after a prolonged absence — can make the pose and velocity belief in the world frame jump significantly; that is good for localization but disastrous for tracking control and point-cloud undistortion. Rather than integrating in the world frame, HF converts the current pose and linear-velocity belief into the body-frame increment T_IkIk+1, increments it there, and maps it back into the odom frame; at every update the odometry velocity is overwritten by the optimized body-frame velocity to counter integration drift, and roll and pitch are overwritten with those of the world frame for gravity-dependent modules. The estimate in the world frame is thus as accurate as possible and the estimate in the odom frame as smooth as possible, mirroring the familiar map-to-odom division of labour in robotics.

高频预测 + 异步优化，容忍延迟与乱序High-rate propagation plus asynchronous optimization, tolerant to delays and out-of-order data

后端用 GTSAM 的 fixed-lag smoother 配 iSAM2，在滑窗内做完整的 MAP 估计并对旧变量做概率边缘化；IMU 以全速率做状态推演，非 IMU 测量到达时才在后台跑较慢的窗口优化，最新信念随后用缓存的 IMU 数据重新推演出来。多数现代因子图方法以低频建状态并依赖 IMU 预积分，乱序测量到达时要重连图、插入新时间戳状态，代价很高；HF 反其道而行，维持稠密状态，只在确定不会再有测量到来时才做预积分，因此在笔记本级 CPU 上也能稳定运行。被边缘化的全局变量会保存最后的信念与不确定度并标记为 inactive，对应测量重新出现时用一个 virtual prior factor 把它重新激活。The backend uses the GTSAM fixed-lag smoother together with iSAM2, performing full MAP estimation inside the window with probabilistic marginalization of old variables; IMU-based state propagation runs at full rate, the slower windowed optimization runs in the background only when a non-IMU measurement arrives, and the latest belief is then re-propagated from buffered IMU data. Most modern factor-graph methods create states at a low rate and rely on IMU pre-integration, which makes out-of-order measurements expensive because the graph has to be rewired and new timestamped states inserted; HF does the opposite, maintaining dense states and pre-integrating only when no further measurements are expected, which keeps it reliable on laptop-grade CPUs. Marginalized global variables are stored with their last belief and uncertainty and labelled inactive; when the corresponding measurement reappears, the variable is reactivated with a virtual prior factor.

离线批量优化与 pseudo ground truthOffline batch optimization and pseudo ground truth

同一套公式还能离线跑批量优化：图结构与在线一致，每个变量都用在线信念初始化，因此迭代很少就能收敛；没有边缘化就不需要 virtual prior，参考系对齐与标定量在离线图里本就可观。结果是一条可达全 IMU 频率的平滑轨迹，可直接当作事后 pseudo GT，用来评估实时估计或训练学习型估计器。论文给出的规模：32.5 min 的高山徒步任务对应 238,216 个优化变量、64.1 s 的离线优化时间。The same formulation also runs offline as a batch optimization: the graph structure mirrors the online one and every variable is initialized from the online belief, so it converges in few iterations; without marginalization no virtual priors are needed, and reference-frame alignment and calibration are observable in the offline graph anyway. The result is a smooth trajectory at up to full IMU frequency that directly serves as post-mission pseudo GT, for evaluating the real-time estimates or training learning-based estimators. The scale reported in the paper: the 32.5 min mountain hike corresponds to 238,216 optimization variables and 64.1 s of offline optimization time.

03 实验Experiments

评测覆盖 3 个平台、5 个场景：ANYmal 四足机器人（自主徒步、parkour、室内动捕数据集）、DARPA RACER 的越野车、以及 HEAP 液压挖掘机。ATE / ARE / RTE / RRE 用 EVO 库计算，全部评测跑在一台 Intel i9 13900K 的 PC 上。有真值的场景用 Qualisys 动捕；徒步、RACER 与 HEAP 没有真值，改用带 GNSS 的离线批量优化轨迹作为 PGT。在这些平台上，HF 已经是默认的定位与状态估计方案。The evaluation spans 3 platforms and 5 scenarios: the ANYmal quadrupedal robot (autonomous hikes, parkour, an indoor motion-capture dataset), the DARPA RACER off-road vehicle, and the HEAP hydraulic excavator. ATE / ARE / RTE / RRE are computed with the EVO library, and all evaluations run on a PC with an Intel i9 13900K CPU. Where ground truth is available it comes from a Qualisys motion-capture system; the hikes, RACER and HEAP have no ground truth, so the offline batch-optimized trajectories with GNSS are used as PGT instead. On these platforms HF is already the default localization and state-estimation solution.

自主徒步：全局精度Autonomous hiking: global accuracy

两段全自主徒步分别为苏黎世附近森林的 23.6 min 与瑞士阿尔卑斯 Seealpsee 的 32.5 min，融合 GNSS、IMU、腿部运动学与 Open3D SLAM 的 LiDAR scan-to-map registration 位姿。表中可见：把 LR 位姿当作绝对测量融合（HF-World）优于把它写成 between factor（HF-World (LR-between)）；GNSS filtered 指仅在协方差低于 1 m 阈值时才融合 GNSS。The two fully autonomous hikes last 23.6 min in a forest near Zurich and 32.5 min at Seealpsee in the Swiss Alps, and fuse GNSS, IMU, leg kinematics and the LiDAR scan-to-map registration poses of Open3D SLAM. The table shows that fusing the LR poses as absolute measurements (HF-World) outperforms formulating them as between factors (HF-World (LR-between)); GNSS filtered means the GNSS measurement is only fused in when its covariance is below a 1 m threshold.

需要说明的是，MINS 在高山徒步中途发散，表中的 split 一行是把该数据集拆开后重评的结果；即便如此，它在森林的 ATE 0.44 m 与 HF-World 的 0.42 m 相当，而在拆分后的高山段 ATE 0.23 m 反而优于 HF-World 的 0.38 m——HF 的优势体现在姿态误差（ARE 1.42° / 2.63° 对 2.14° / 4.10°）与不拆分即可跑完全程的鲁棒性上。It should be said that MINS diverged mid-way through the mountain hike, so the split row reports a re-evaluation on the split dataset; even then, its forest ATE of 0.44 m is on par with the 0.42 m of HF-World, and on the split mountain segment its ATE of 0.23 m is actually better than the 0.38 m of HF-World — the advantage of HF lies in the rotational error (ARE 1.42° / 2.63° versus 2.14° / 4.10°) and in running the whole mission without splitting it.

局部平滑性与一致性Local smoothness and consistency

RTE 与 RRE 定义为所有 1 m 行进距离对的平均漂移；NOJ 把两次估计之间超过 10 cm 的运动记为一次跳变（IMU 速率 400 Hz），jitter 取位置的三阶时间导数。HF-Odom 把跳变彻底压到 0 次，jitter 也比 ANYmal 出厂的腿式里程计 TSIF 低一个量级。RTE and RRE are defined as the average drift over all pairs at 1 m of traversed distance; NOJ counts a motion of more than 10 cm between two estimates as one jump (at the 400 Hz IMU rate), and jitter is the third time derivative of the position. HF-Odom pushes the number of jumps all the way down to 0 and its jitter is an order of magnitude below that of TSIF, the leg odometry shipped with ANYmal.

这里也有不利于本文的一格：TSIF 的 RRE 0.57°/m 仍是全表最好，HF 只在 RTE、NOJ 与 jitter 上大幅领先。One cell here does not favour this paper: the RRE of 0.57 deg/m from TSIF is still the best in the table, and HF leads by a large margin only in RTE, NOJ and jitter.

高动态 parkour 与室内动捕Highly dynamic parkour and indoor motion capture

ANYmal parkour 是目前最动态的四足运动控制器之一，含跳跃越沟与多点接触，总速度可达 2.07 m/s、有效加速度 59.43 m/s²。只用 IMU 与腿部运动学时，HF World 的 ATE 33.2 cm 不及打磨多年的商用 TSIF 的 24.9 cm；一旦把 LR 融进来，HF World 降到 7.0 cm、离线优化到 4.9 cm，其 RTE 也从 13.1% 降到 8.3%。室内五段动捕序列上同样如此：TSIF 为 7.9 cm、松耦合速度的 HF World 为 13.7 cm，而紧耦合腿部运动学并加入 LR 后 HF World 与 HF Offline 都是 2.8 cm，优于只用低频 LR 的 Open3D-SLAM 的 3.0 cm。ANYmal parkour is one of the most dynamic quadrupedal motion controllers to date, with jumps over gaps and multi-contact interactions, reaching up to 2.07 m/s total speed and 59.43 m/s² effective acceleration. With IMU and leg kinematics alone, the ATE of HF World is 33.2 cm and does not match the 24.9 cm of the long-engineered commercial TSIF; as soon as LR is fused in, HF World drops to 7.0 cm and the offline optimization to 4.9 cm, and its RTE falls from 13.1% to 8.3%. The same holds on the five indoor motion-capture sequences: TSIF reaches 7.9 cm and HF World with loosely fused velocity 13.7 cm, whereas with tightly fused leg kinematics plus LR both HF World and HF Offline reach 2.8 cm, better than the 3.0 cm of low-rate LR-only Open3D-SLAM.

越野车与挖掘机Off-road vehicle and excavator

RACER 的改装 Polaris RZR 在美国 Helendale 沙漠跑了 4.1 km、最高速度 9.66 m/s，HF 同时融合 IMU、GNSS、LiDAR、RADAR 与轮速编码器五种模态，并作为 NASA JPL 车辆的主估计器参加 RACER 挑战赛。这一数据集最有说服力的结果不是轨迹误差，而是 localization manager 的对比：常规做法用两个不同步的变换相减得到 T_WM，会引入噪声与跳变；HF 由于把它与机器人状态联合优化，输出的是同步且平滑的变换。HEAP 液压挖掘机则展示长时任务：在 Oberglatt 工地两栋厂房之间的走廊里，GNSS 拒止与几何退化同时发生，HF 通过把 CompSLAM 的 scan-to-map 位姿与 Coin-LIO 的输出一起作为绝对测量融合（共三个参考系 W、M_comp.、M_coin.），在丢掉两路核心测量的情况下跑完了整个任务。The customized Polaris RZR of RACER traverses 4.1 km of the Helendale desert in the USA at up to 9.66 m/s, with HF fusing five modalities at once — IMU, GNSS, LiDAR, RADAR and a wheel encoder — and serving as the primary estimator of the NASA JPL vehicle in the RACER challenge. The most telling result on this dataset is not a trajectory error but the localization-manager comparison: the usual practice of subtracting two unsynchronized transformations to obtain T_WM introduces noise and jumps, whereas HF, which optimizes it jointly with the robot state, outputs a synchronized and smoothed transformation. The HEAP hydraulic excavator then demonstrates long-duration operation: in the corridor between two industrial buildings at the Oberglatt site, GNSS denial and geometric degeneracy occur simultaneously, and by fusing the scan-to-map poses of CompSLAM together with the output of Coin-LIO as absolute measurements (three reference frames in total, W, M_comp. and M_coin.), HF completes the entire mission despite losing two core measurement streams.

消融实验Ablations

论文的消融集中在三处。其一，把 random walk 置零（等价于不允许 map frame 漂移）后，即便走出森林、GNSS 信号良好，对齐后的 LR 位姿与融合轨迹仍因 LiDAR 地图累积的漂移而对不上；在线与离线优化估出的漂移曲线则彼此一致，说明边缘化与信念传播工作正常。其二，把对齐放在 world frame 原点而非局部 keyframe，在 ETH Zurich 校园这类长距离数据上会因敏感性随距离线性增长，在约 150 s 后出现振荡。其三，状态创建率的权衡：10 / 40 / 100 Hz 对应平均延迟 22.41 / 25.31 / 29.63 µs 与异步优化时间 1.29 / 3.86 / 8.96 ms，而 40 Hz 与 100 Hz 之间的 ATE 几乎没有差别（0.373 m 与 0.374 m），只有降到 10 Hz 才让平移与姿态误差变大。The ablations of the paper concentrate on three points. First, setting the random walk to zero (that is, not letting the map frame drift) means that even outside the forest with a good GNSS signal, the aligned LR pose and the fused trajectory cannot be brought into agreement because of the drift accumulated in the LiDAR map; the drift estimated online and offline, in contrast, behaves consistently, showing that marginalization and belief propagation work as intended. Second, aligning at the origin of the world frame instead of at a local keyframe makes the sensitivity grow linearly with distance on long datasets such as the ETH Zurich campus, producing oscillations after roughly 150 s. Third, the state-creation-rate tradeoff: 10 / 40 / 100 Hz correspond to mean latencies of 22.41 / 25.31 / 29.63 µs and asynchronous optimization times of 1.29 / 3.86 / 8.96 ms, while ATE barely differs between 40 Hz and 100 Hz (0.373 m versus 0.374 m) and only dropping to 10 Hz increases the translation and orientation errors.

04 局限性Limitations