ETH Zurich2022#030측위·매핑논문 PDF

이중 그래프 건설로봇 위치추정

Graph-based multi-sensor fusion for consistent localization of autonomous construction robots

한줄 요약 — IMU 100 Hz의 저지연 전파와 GNSS·LiDAR가 촉발하는 receding-horizon 최적화를 결합하고, GNSS 열화 시 pseudo-global LiDAR factor를 쓰는 fallback graph로 전환해 두 Menzi Muck 굴착기에서 41 μs 평균 지연과 센티미터급 일관성을 달성한 위치추정 구조다.

1.연구 배경 및 동기

GNSS가 끊겨도 제어 연속성과 전역 좌표를 함께 지키는 굴착기 위치추정

대형 굴착기는 건설·자재 취급·임업뿐 아니라 지진 잔해 제거, 위험물 처리, 홍수 우회 수로 조성처럼 작업자를 위험에 노출시키는 임무에 투입된다. 이 장비를 자율화하려면 차체 균형 제어가 사용할 매끄럽고 고주기인 상태와, 항법·계획·매핑이 사용할 정확한 전역 위치가 동시에 필요하다. 그러나 실제 야외 현장에서는 수목, 벽, 터널, 통신 단절 때문에 RTK-GNSS가 열화하거나 사라졌다가 복구되며, 복구 시 큰 전역 보정이 그대로 제어 좌표에 전달되면 자세가 불연속적으로 뛸 수 있다.

필터는 IMU 도착마다 빠르게 상태를 갱신하기 좋지만 지연·비동기 비선형 측정을 과거 상태에 넣기 어렵고, smoothing·batch 최적화는 다양한 센서 factor를 유연하게 넣을 수 있으나 매 IMU 주기마다 전체 비선형 문제를 푸는 것은 부담스럽다. 논문은 이 양자택일을 그래프 기반 prediction-update loop로 풀고, GNSS가 있을 때의 전역 그래프와 없을 때의 LiDAR 기준 그래프를 병렬로 유지한다. 목표는 특정 굴착 작업 하나가 아니라 센서 손실과 복귀가 자연스럽게 발생하는 장시간 현장 운용에 공통으로 쓸 위치추정 기반을 만드는 것이다.

필터와 최적화의 장점을 결합한 구조는 두 Menzi Muck 실기계의 건설·주행·실시간 지도 작성에서 검증된다.

원문 Figure 1, PDF 1쪽
원문 Figure 1 · PDF p.1 — Fig. 1. A global map built online using the proposed approach’s estimate during a real-world construction task is shown. The colored robot map is overlaid on top of a groundtruth map created using a Leica RTC360 scanner.
선행 연구의 한계
이 연구의 기여

2.방법론 및 시스템 구성

상태는 world 기준 IMU pose·velocity·bias, world-to-odometry 변환, odometry 기준 base로 구성된다. 비동기 IMU·선회각·dual GNSS·LiDAR를 각 IMU 시간 노드에 연결하며 extrinsic은 주어졌다고 가정한다.

IMU callback은 최신 최적 상태를 즉시 전파하고, 별도 스레드는 GNSS·LiDAR 도착 때만 receding horizon을 최적화한 뒤 최신 IMU 시각까지 다시 전파한다. 낮은 latency와 지연 factor 처리를 함께 얻는다.

GNSS 신뢰도는 Leica covariance와 최대 속도 기반 delta transform을 연속 검사한다. 열화 시 마지막 신뢰 자세와 LiDAR 누적으로 pseudo-global pose factor를 만들어 fallback graph를 활성화한다.

처리 파이프라인

1

정적 초기화

기계를 수 초간 정지시켜 IMU와 두 GNSS 안테나 측정을 누적한다. 두 안테나 heading으로 초기 yaw, 중력·IMU 측정으로 초기 attitude와 bias, 왼쪽 안테나 위치로 world 기준 IMU 위치를 정해 임의 yaw보다 optimizer 수렴을 빠르게 한다.

2

IMU 노드와 고주기 예측

100 Hz IMU마다 SO(3)×R 상태 노드를 만들고 preintegration factor로 과거 노드에 연결한다. callback에서는 최신 최적 상태와 bias로 회전·위치·속도를 전파해 제어에 바로 제공한다.

3

GNSS·LiDAR factor 삽입

20 Hz RTK-GNSS 위치는 안테나-IMU lever arm을 반영해 world 기준 IMU 위치 residual로 넣는다. 5 Hz Ouster scan은 CompSLAM이 계산한 상대 odometry를 6-DoF between factor로 넣어 GNSS만으로 관측되지 않는 yaw를 제약한다.

4

비동기 그래프 갱신

GNSS나 LiDAR 도착이 optimization thread를 깨우면 새 factor를 복사하고 receding horizon 전체를 최적화한다. 완료된 상태와 바이어스를 최신 IMU 시각까지 전파하므로 지연 측정은 과거 node에 들어가면서도 현재 출력은 중단되지 않는다.

5

GNSS dropout 전환

수신 covariance와 속도 기반 변화량 검사가 연속으로 실패하면 fallback을 활성화한다. LiDAR 누적 자세를 dropout 직전 world 자세에 고정한 pseudo-global unary factor로 변환해 local map의 연속성을 유지한다.

6

GNSS recovery 정렬

복구 시 main graph가 GNSS로 교정한 world 자세와 fallback의 odometry 자세로 world-to-odometry 변환을 다시 계산한다. 그 뒤 main으로 출력원을 바꾸고 fallback graph를 main 상태로 복사·flush해 다음 중단에 대비한다.

원문 수식과 의미

GNSS 중단 중 pseudo-global LiDAR 자세

T~WI,tk=TWI,tj(TLt0Ltj)−1TLt0Ltk\tilde{T}_{WI,t_k}=T_{WI,t_j}(T_{L_{t_0}L_{t_j}})^{-1}T_{L_{t_0}L_{t_k}}

t_j는 마지막 신뢰 GNSS 시각이다. 그때의 전역 IMU 자세에 LiDAR odometry의 상대 이동을 이어 붙여 fallback graph의 unary pose factor를 만든다.

GNSS 복구 시 world-odometry 재정렬

TWO=TWI(TOI)−1T_{WO}=T_{WI}(T_{OI})^{-1}

main graph의 GNSS 교정 world 자세와 fallback graph의 local odometry 자세 차이를 좌표변환으로 흡수해 제어 출력에 위치 점프가 나타나지 않게 한다.

원문 Figure 2, PDF 3쪽
원문 Figure 2 · PDF p.3 — Fig. 2. Structural overview of the factor graph design proposed in this work. As depicted in this figure, IMU measurements are the driving factor in the architecture. Furthermore, a dual graph design is introduced in order to remain operational also in absence of RTK GNSS.

3.핵심 기술

필터처럼 예측하고 smoother처럼 갱신하는 factor graph

비선형 측정 residual을 factor로 표현하는 GTSAM 그래프가 IMU·GNSS·LiDAR의 서로 다른 주기와 도착 지연을 수용한다. 핵심은 최적화를 IMU 주기와 분리한 것이다. IMU는 가벼운 propagation만 수행하고, 저주기 절대·상대 측정이 도착할 때만 horizon을 다시 풀어 최적화 비용이 제어 loop를 막지 않는다.

실험에서 propagated 출력은 100 Hz, optimizer 갱신은 GNSS·LiDAR trigger에 따라 약 25 Hz였다.

  • 센서별 callback과 optimization을 다중 스레드로 분리
  • IMU node를 비동기 factor의 시간 anchor로 사용
  • delayed factor를 과거 node에 넣을 수 있는 receding horizon

주·fallback 이중 그래프의 센서 손실 상태기

main graph는 GNSS가 전역 위치를 고정하고 LiDAR between factor가 yaw와 단기 motion을 보강한다. fallback graph는 dropout 순간 main과 동일하지만 이후 LiDAR를 pseudo-global unary pose로 바꾸어 CompSLAM의 drift가 짧은 중단 동안 제어 좌표계의 불연속보다 우선하도록 한다.

복구 시 두 해의 상대변환만 갱신해 누적 drift 교정이 제어 자세의 jump로 전달되지 않게 한다.

  • covariance와 최대속도 delta를 연속 검사해 GNSS outlier 판정
  • dropout 시 pseudo-global unary LiDAR factor
  • recovery 시 T_WO 갱신 후 fallback reset

굴착기 다중 프레임과 온보드 센서 통합

world, local odometry, IMU, LiDAR, GNSS, chassis base, cabin 프레임을 분리해 전역 위치와 제어용 국소 운동을 한 상태 안에서 다룬다. 두 IMU와 cabin turn 관절 측정은 보행 굴착기의 회전 가능한 상부체와 차체를 연결하고, 두 안테나 RTK-GNSS는 초기 heading과 centimeter급 절대 위치를 제공한다.

Ouster OS0-128 scan은 CompSLAM odometry로 처리하고 비동기 global·relative pose source를 graph factor로 수용한다.

  • GTSAM C++ 온보드 구현
  • Ouster OS0-128와 CompSLAM
  • Leica iCON iXE3 dual-antenna RTK-GNSS

4.실험 결과

실험 환경·장비·데이터

실험 프로토콜

비교 기준

평가 지표

원문 Figure 3, PDF 5쪽
원문 Figure 3 · PDF p.5 — Fig. 3. Global accuracy for the real-world construction task. The top plot shows the relative position errors with respect to GNSS measurements, whereas the lower plot underlines the benefit of the dual-graph design in absence of GNSS from second 105 to 150. Time is expressed in [s].
원문 Figure 4, PDF 5쪽
원문 Figure 4 · PDF p.5 — Fig. 4. Global accuracy evaluation for the navigation task. Global accuracy is maintained as compared to CompSLAM, while local smoothness is also maintained in absence of GNSS. Time is expressed in [s].

핵심 결과 해석

건설 작업의 정상 GNSS 구간에서 제안법 상대 위치 오차는 평균 0.18 cm, 표준편차 1.28 cm였다. TSIF는 0.20/1.54 cm, MSF는 0.10/1.60 cm여서 평균만 보면 MSF가 작지만 제안법은 가장 작은 분산을 보였다. 더 중요한 dropout 구간에서는 TSIF가 GNSS를 잃고 불안정해졌고 MSF는 손실·복구 뒤 drift가 남은 반면, 제안법은 fallback LiDAR로 CompSLAM만큼 매끄럽게 유지하면서 GNSS가 있는 main graph 덕분에 전역 좌표를 회복했다.

주행 실험에서도 수목 인접 220~380 s에 TSIF와 MSF가 흔들렸고, 제안법과 CompSLAM은 부드러웠다. 다만 CompSLAM은 전역 drift를 누적했고 제안법은 RTK 복구로 이를 교정했다. 제안 추정으로 만든 지도는 수목 근처에서 찌그러진 TSIF 지도와 달리 CompSLAM 지도와 유사한 국소 형상을 유지하면서 Leica ground-truth에 전역 정렬되었다.

100 Hz prediction과 약 25 Hz optimization 사이의 translation consistency는 평균 0.58 cm, 표준편차 3.16 cm였다. 평균 latency는 41 μs, 표준편차 15 μs로 TSIF의 102/40 μs보다 낮고 MSF의 32.9/18.7 μs와 같은 규모였다. factor graph가 필터보다 본질적으로 느리다는 통념과 달리 최적화를 callback에서 분리하면 실시간 제어용 출력이 가능함을 보인 결과다.

정량 결과

평가 항목정량 결과조건·맥락원문 근거
건설 작업 상대 위치 오차제안법 μ=0.18 cm, σ=1.28 cm정상 RTK-GNSS를 ground truth로 한 건설 task; TSIF 0.20/1.54 cm, MSF 0.10/1.60 cmPDF p.5
GNSS 비신뢰 구간105~150 sFig. 3 caption의 dropout 구간; fallback graph를 사용PDF p.5
translation consistencyμ=0.58 cm, σ=3.16 cmnavigation task의 100 Hz propagated pose와 약 25 Hz optimized pose 차이PDF p.6
상태추정 latency제안법 41±15 μsIMU 도착부터 propagated estimate 출력; TSIF 102±40 μs, MSF 32.9±18.7 μsPDF p.6
실패 사례와 경계조건
원문 Figure 5, PDF 6쪽
원문 Figure 5 · PDF p.6 — Fig. 5. Maps that are built using the estimates from the proposed approach, TSIF, and CompSLAM (left to right). While the TSIF map gets corrupted close to trees, the proposed approach makes use of the estimates provided by the lidar odometry during these intervals.
원문 Figure 6, PDF 6쪽
원문 Figure 6 · PDF p.6 — Fig. 6. State-estimation consistency results for the navigation task compute but calculating the deviation between propagated and optimized estimates. In this plot, time is expressed in [s].

5.한계 및 향후 연구

현재 한계

  • 센서 간 extrinsic calibration이 주어졌다고 가정하며 그래프 안에서 공동 추정하지 않는다.
  • GNSS 신뢰 판정이 수신기 covariance와 최대 속도 threshold에 의존해 다중 센서의 동시 열화나 잘못된 낮은 covariance를 직접 검증하지 않는다.
  • 차체 orientation, encoder bias, sensor time offset을 상태로 공동 추정하지 않아 큰 장비의 설치·동기 오차가 consistency에 남을 수 있다.
  • 두 스위스 현장과 두 보행 굴착기에서 검증했으나 터널처럼 장시간 GNSS가 전혀 없거나 LiDAR도 퇴화하는 환경은 실험하지 않았다.

향후 연구

  • 논문이 명시한 후속 과제대로 chassis orientation과 encoder bias를 factor graph 상태에 포함해 관절·차체 추정을 함께 보정한다.
  • sensor extrinsic calibration과 time offset을 온라인 공동 추정해 장비 장착 변화와 비동기 오차를 줄인다.
  • GNSS 외의 global source와 sensor residual 기반 fault detection을 추가해 LiDAR·GNSS 동시 이상에서도 graph 전환 근거를 확보한다.
  • 장시간 음영, 먼지·진동, 반복 구조에서 fallback drift와 잘못된 GNSS recovery를 포함한 stress test를 수행한다.

6.한마디로 정리

핵심 방법은?

IMU 전파와 factor-graph 최적화를 비동기로 분리하고 GNSS 가용·중단용 그래프를 병렬 유지한다.

핵심 결과는?

두 실물 굴착기에서 GNSS 중단·복귀 중 부드러운 궤적과 전역 지도를 유지하며 41 μs 평균 출력 지연을 보였다.

한계는?

외부보정·시간오프셋·차체 자세는 주어진 값이며 LiDAR와 GNSS가 함께 열화하는 장기 상황은 다루지 않았다.

#측위·매핑#시스템통합·현장실증#factor graph#IMU-LiDAR-GNSS 융합#fallback graph

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

world에는 설계·elevation map을, smooth odometry에는 arm·chassis 제어를 두면 GNSS 보정에도 작업 좌표를 유지하고 제어 jump를 막는다. factor residual과 fallback 지속시간은 작업 속도·workspace를 제한하는 localization integrity 지표로 쓸 수 있다.

7.전체 그림·표

섹션별 원문 근거 페이지

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

추출·교차검증 근거