ETH Zurich2026#002측위·매핑논문 PDF

Holistic Fusion 범용 센서융합

Holistic Fusion: Task- and Setup-Agnostic Robot Localization and State Estimation with Factor Graphs

한줄 요약 — Holistic Fusion은 임의의 sensor/reference frame을 factor graph 변수로 승격하고 drift를 random walk와 지역 keyframe으로 추적해, GNSS 복귀나 LiDAR 퇴화가 있어도 전역 정확도와 제어용 smooth odometry를 한 최적화에서 유지하는 공개 범용 상태추정 프레임워크다.

1.연구 배경 및 동기

센서 수보다 어려운 문제, 서로 다른 좌표계와 시간의 일관된 결합

산림·광산·건설 현장의 로봇은 GNSS, LiDAR odometry, IMU, landmark처럼 성질이 다른 센서를 쓴다. 전역 센서는 drift를 누르지만 끊기거나 jump하고, 지역 odometry는 매끄럽지만 누적 오차가 생긴다. 제어에는 연속적인 지역 상태가, 임무 기록에는 전역 정합이 동시에 필요하다.

기존 estimator는 특정 센서 조합과 고정 frame에 맞춰져 센서를 추가하거나 reference frame을 바꾸기 어렵다. 지연된 absolute 측정이 들어오면 과거 상태를 고쳐야 하며, 그 보정이 곧바로 제어 frame에 나타나면 명령이 튀는 문제가 있다.

Holistic Fusion은 모든 측정을 factor graph의 변수·factor로 표현하고 World와 jump-free Odom frame을 함께 유지한다. 고정지연 최적화와 graph marginalization으로 계산을 제한하며, 임의 frame의 state와 covariance를 질의하는 공통 인터페이스를 제공한다.

원문 Figure 1, PDF 1쪽
원문 Figure 1 · PDF p.1 — Fig. 1. Overview of a fully autonomous hiking experiment using the ANYmal quadrupedal robot in a forest environment. Online and offline motion estimation are conducted by fusing i) IMU, ii) noisy and degraded GNSS measurements, iii) the online-aligned (drifting) absolute pose of a LiDAR registration (LR) SLAM system expressed in the map frame, and iv) leg odometry. A: Overview of the different trajectories with highlighted drift and GNSS loss. B: The aligned LR trajectory at three keyframes (indicated by ×). At each keyframe, the LR trajectory locally perfectly aligns. C: 2D and 3D visualizations of the random walk are used to illustrate the evolution of the map frame location. Yel- low highlights the correspondence between the spikes of the 2D and 3D plots.
원문 Figure 3, PDF 5쪽
원문 Figure 3 · PDF p.5 — Fig. 3. An illustrative scenario of HF. During the mission, different measurements are fused directly without preprocessing (in the shown example: IMU, GNSS, LR, local velocity). To fuse global and non-global absolute measurements, which drift against each other (blue path), HF explicitly estimates the shift between the reference coordinate frames, aligns the measurements, and models them as a random walk. This alignment is not conducted at the global origin but at local keyframes. The graph at the top shows a simplified version of the resulting graph with factors and states depicted in the same colors.
선행 연구의 한계
이 연구의 기여

2.방법론 및 시스템 구성

상태 graph에는 body pose·velocity·IMU bias와 센서별 동적 변수를 넣는다. absolute, landmark, local odometry, IMU 측정을 각기 residual factor로 연결해 MAP 추정을 수행한다.

World graph의 보정과 별도로 최신 body velocity를 적분한 Odom frame을 유지해 제어 출력의 jump를 없앤다. 비동기 측정은 timestamp 위치에 삽입하고 fixed-lag window 밖 상태는 marginalize한다.

처리 파이프라인

1

센서·기준 좌표계 선언

센서 timestamp와 frame tree를 기준으로 측정을 absolute, landmark, local 또는 IMU factor로 변환한다.

2

동적 변수 생성

Body pose·velocity·bias와 GNSS offset 등 필요한 동적 변수를 생성하고 random-walk prior로 시간축을 연결한다.

3

Holistic measurement function

측정 함수와 covariance를 factor로 추가해 fixed-lag MAP를 갱신하며 늦게 도착한 값도 과거 timestamp에 삽입한다.

4

Drift random walk

window 밖 상태를 marginalize해 정보 prior로 압축하고 graph 크기와 온라인 계산량을 제한한다.

5

Local keyframe alignment

전역 최적화의 pose correction은 World에 반영하되 최신 body velocity를 적분해 연속 Odom 상태를 출력한다.

6

비동기 fixed-lag update

사용자는 임의 source-target frame의 pose·velocity와 covariance를 같은 API로 요청해 planner와 controller에 공급한다.

7

전역·로컬 이중 출력

센서가 끊기면 남은 factor로 추정하고 복귀 측정은 graph를 재정렬하되 Odom 제어 출력은 불연속 없이 유지한다.

원문 수식과 의미

Holistic MAP 추정

Θ∗=arg⁡max⁡Θ p(IX,LX,GX,RX∣Z)\Theta^*=\arg\max_{\Theta}\,p(I X,L X,G X,R X\mid Z)

robot navigation state와 landmark, global/extrinsic, reference alignment 상태를 모든 측정 Z에 조건부로 함께 추정하고 Gaussian residual의 least-squares factor graph로 푼다.

Reference frame SE(3) 진화

TWRi,k+1≈TWRi,kexp⁡([Δt γi]∧)T_{WR_{i,k+1}}\approx T_{WR_{i,k}}\exp([\Delta t\,\gamma_i]^\wedge)

reference frame의 local group velocity γ_i를 적분하여 LiDAR map 등 외부 frame이 world에 대해 천천히 drift하도록 허용한다.

Discrete-time random walk

γi∼N(0,Σi2),diag(Σi)=[σi,1,…,σi,6]⊤\gamma_i\sim\mathcal{N}(0,\Sigma_i^2),\quad \mathrm{diag}(\Sigma_i)=[\sigma_{i,1},\ldots,\sigma_{i,6}]^\top

회전·이동 각 축의 drift 허용량을 noise로 지정한다. 0은 고정 관계, 큰 값은 빠르게 변하는 map frame을 의미한다.

Absolute pose holistic factor

h(x)=TRWTWITISTSScorr∈SE(3)h(x)=T_{RW}T_{WI}T_{IS}T_{SS_{corr}}\in SE(3)

world robot state, reference alignment, sensor extrinsic를 연결해 해당 센서·기준 좌표계에서 기대되는 절대 pose를 직접 만든다.

원문 Figure 4, PDF 7쪽
원문 Figure 4 · PDF p.7 — Fig. 4. Structural overview of the factor graph design of HF proposed in this work. As depicted, IMU measurements drive the creation of the robot state. All other states are created dynamically (setup-specific) based on the provided measurements and framework configuration: a) Reference frame alignment states. b) Landmark states. c) Global states. d) Example measurement types depicted as factors in the graph. Right side: Steps from Algorithm 1.
원문 Figure 5, PDF 9쪽
원문 Figure 5 · PDF p.9 — Fig. 5. Visual summary of the state transformation steps in Algorithm 1. Top row: absolute measurement pipeline. Bottom row: landmark measurement pipeline. Circled letters denote the corresponding algorithm steps.
원문 Figure 8, PDF 12쪽
원문 Figure 8 · PDF p.12 — Fig. 8. Effect of the limited FL window of the smoother leading to variables being marginalized out. The handling of the different dynamic state variables in HF and the differences between its online and offline graphs are highlighted.

3.핵심 기술

좌표계를 상태로 다루는 계층형 factor graph

Frame-agnostic factor 설계로 World, map, odometry, body, sensor frame을 같은 graph에서 조합한다. 새 센서는 측정 함수·noise·frame 연결만 정의하면 되고 출력도 임의 frame으로 변환된다.

  • absolute, landmark, local/relative, raw IMU 측정 지원
  • 임의 수의 reference/sensor frame을 이론상 수용
  • Huber·Cauchy·Tukey robust norm으로 outlier 처리 가능

Random walk와 지역 keyframe을 결합한 drift 정렬

World는 GNSS·landmark 보정을 수용하고 Odom은 최신 body velocity를 적분해 jump-free 상태를 낸다. 전역 일관성과 제어 연속성을 한 estimator에서 분리해 제공하는 핵심 장치다.

  • Loop closure jump는 외부 correction measurement로 흡수
  • drift noise는 가능한 작되 실제 drift를 수용하도록 설정
  • keyframe 변화 mean은 측정 자체의 알려진 위치 이동을 사용

전역 정확도와 로컬 smoothness의 분리

Fixed-lag smoothing은 비동기·지연 측정을 과거 상태에 넣고 오래된 변수를 marginalize한다. 정확한 smoothing을 유지하면서 장기 임무의 graph 크기를 제한한다.

  • W: global planning과 시공 기준
  • O: feedback control·point-cloud undistortion·local elevation map
  • 두 frame의 관계와 모든 map transform을 IMU rate로 동기화 출력

온라인·오프라인 공용 graph 관리

GNSS offset, landmark, bias 같은 변수를 운전 중 생성·제거할 수 있어 센서 가용성과 임무 단계가 바뀌는 현장 로봇에 대응한다.

  • iSAM2 fixed-lag smoother와 완전 marginalization
  • online belief를 offline 초기값으로 재사용
  • C++ core는 ROS middleware와 분리
원문 Figure 7, PDF 11쪽
원문 Figure 7 · PDF p.11 — Fig. 7. Motivation and illustration of the smooth odometry generation by integrating the body frame state estimated in the current smoother window.

4.실험 결과

실험 환경·장비·데이터

실험 프로토콜

비교 기준

평가 지표

원문 Figure 10, PDF 13쪽
원문 Figure 10 · PDF p.13 — Fig. 10. Overview of the evaluated robotic platforms. Due to its advanced capabilities and sensor measurement setup, three ANYmal datasets (Sec. VI-C.1/2/3) are investigated. Moreover, the RACER vehicle (Sec. VI-D) is a case study for high-speed off-road driving and demanding tracking control. HEAP (Sec. VI-E) operates in mixed environments and requires high accuracy for global position and orientation in the presence of geometric degeneracy. SMB, a robot used for education, and Boxi [79], an integrated sensor box for GT generation, are not in this work but are available in the open-source examples.

핵심 결과 해석

Hiking에서 HF World의 forest ATE/ARE는 0.42 m/1.42°, mountain은 0.38 m/2.63°였다. Filtered World mountain 결과는 0.12 m/1.22°까지 개선됐다. World 보정은 정확하지만 jump 수 2,482와 jitter 59,382가 나타났고, Odom은 jump 0과 jitter 3,470으로 제어 연속성을 지켰다.

40 Hz state 생성에서 평균 상태 생성 25.31 μs, 최적화 3.86 ms, ATE 0.373 m였다. 100 Hz는 최적화가 8.96 ms로 늘었지만 정확도 이득은 없어 센서율보다 과도한 state 증가는 비효율적이었다.

23.6분 hiking graph 167,020 변수는 56.7초, 17.9분 HEAP graph 161,217 변수는 62.4초에 처리됐다. GNSS 차단·LiDAR 퇴화 뒤 absolute 정보가 돌아오면 World를 복구하면서 Odom 기반 제어 trajectory는 계속 매끄럽게 유지했다.

정량 결과

평가 항목정량 결과조건·맥락원문 근거
Forest HF-World ATE/ARE0.42 m / 1.42°GNSS+IMU+absolute LR+leg odometryPDF p.14
Mountain HF-World ATE/ARE0.38 m / 2.63°Seealpsee autonomous hikePDF p.14
Mountain GNSS-filtered HF ATE/ARE0.12 m / 1.22°GNSS covariance 1 m thresholdPDF p.14
HF-Odom local smoothnessNOJ 0, jitter 3,470 m/s³두 ANYmal hike 평균PDF p.14
40 Hz online computation25.31 µs latency, 3.86 ms optimizationForest missionPDF p.15
HEAP offline optimization161,217 variables / 62.4 s17.9 min missionPDF p.13
실패 사례와 경계조건
원문 Figure 12, PDF 14쪽
원문 Figure 12 · PDF p.14 — Fig. 12. Qualitative result for a local elevation map expressed in O vs. W for the forest hike dataset. The return of GNSS leads to an update jump of the estimate in W, creating a corrupted elevation map. The odometry estimate in O does not jump (Sec. IV-D3), rendering it suitable for local mapping and navigation.
원문 Figure 23, PDF 18쪽
원문 Figure 23 · PDF p.18 — Fig. 23. Created maps for the Oberglatt, Switzerland experiment using the offline, online and raw LR estimates. The drift occurs due to LiDAR degeneracy.
원문 Figure 24, PDF 18쪽
원문 Figure 24 · PDF p.18 — Fig. 24. Estimated translation drift for the Oberglatt, Switzerland experiment. As expected, the framework estimates larger drift during geometric degeneracy.
원문 Figure 25, PDF 18쪽
원문 Figure 25 · PDF p.18 — Fig. 25. Translation/orientation alignment error for the ETH Zurich campus for i) alignment around the global W origin and ii) around the local keyframe. When far away from the origin, the alignment error is significantly larger for case ii).

5.한계 및 향후 연구

현재 한계

  • 초기값이 실제 상태에서 멀면 비선형 optimizer가 잘못된 local minimum에 수렴할 수 있어 고불확실도 prior와 초기 정렬이 필요하다.
  • Single-antenna GNSS에서는 초기 yaw observability가 이동에 의존하며 정지 상태에서 즉시 전역 heading을 얻지 못한다.
  • 높은 state creation rate는 정확도 이득이 작아도 optimization time과 offline memory를 증가시킨다.
  • 범용성이 커진 만큼 drift level, alignment uncertainty, factor noise 등 새 사용자 tuning 파라미터가 많다.
  • 실험의 extrinsic은 horizon 동안 상수로 두며 온도·진동에 따른 sub-mm/sub-degree 변화와 시간 동기화 자체는 주제가 아니다.

향후 연구

  • Global state의 변화도 random process로 모델링해 장시간 calibration 변화와 context variable drift를 다룬다.
  • Gaussian process나 spline 기반 continuous-time backend를 도입해 arbitrary timestamp와 dense state 비용을 줄인다.
  • 로봇 동역학과 다음 measurement 도착을 예측해 state creation rate를 adaptive하게 조절한다.
  • Ill-posed 초기화와 outlier filtering에 학습된 probabilistic prior 또는 ML quality estimator를 결합한다.
  • Factor residual 기반 sensor integrity monitoring으로 GNSS multipath·LiDAR degeneracy에 따라 noise와 안전속도를 자동 조정한다.

6.한마디로 정리

핵심 방법은?

센서와 기준 좌표계를 graph state로 만들고 SE(3) random walk·local keyframe으로 drift를 추정하며, W와 O 출력을 분리한다.

핵심 결과는?

세 플랫폼·다섯 임무에서 absolute sensor fusion의 전역 정확도와 NOJ 0의 smooth odometry, IMU-rate 출력을 동시에 입증했다.

한계는?

초기 observability와 noise tuning, state rate 계산비용에 민감하며 시간변 calibration·continuous-time 처리와 자동 integrity 판단은 남아 있다.

#측위·매핑#시스템통합·현장실증#GTSAM factor graph#다중 기준좌표계 자동 정렬#동적 외부보정#local keyframe

나의 의견 / 우리 연구와의 연관성

자율굴착기는 오랜 정지 작업과 cabin 회전, 건물 가림, 먼지·평면 벽의 LiDAR 퇴화를 동시에 겪는다. HF는 dual GNSS를 world 기준으로, CompSLAM/CoinLIO를 각각의 map frame으로, IMU와 encoder를 local motion으로 직접 융합해 특정 GNSS 유무에 따라 graph를 전환하던 이전 HEAP 구조를 하나의 graph로 대체한다.

설계면과 굴착 지점은 전역 W에서 유지하되 버킷 제어와 local terrain map은 jump-free O에서 수행할 수 있다. 또한 map-to-world transform이 최적화와 동기화되어 나오므로, 굴착 후 지형 재스캔·작업계획 갱신이 reference-frame mismatch 없이 이어진다.

7.전체 그림·표

섹션별 원문 근거 페이지

원문·기존 요약·관련 자료

추출·교차검증 근거