기술 요약: 가장자리를 매끄럽게 만들기

문제 정의

본 논문은 이산적이고 비동기적일 수 있는 측정치 $y_k$를 시간 $t_k$에서 기반으로 하여, 연속 시간 상태 추정 (continuous-time state estimation) 의 과제를 다룹니다. 여기서의 목표는 $x(t)$를 시간의 연속 함수로 추정하는 것입니다. 이산 시간 추정은 모든 측정치나 시간 단계에 대해 명시적으로 상태 변수를 정의해야 하지만, 이 접근법은 LiDAR, IMU 와 같은 고주파수 센서, 비동기 데이터, 그리고 제어 또는 매핑과 같은 하위 작업을 위해 임의의 시간에 시스템 상태를 쿼리해야 하는 필요성으로 인해 어려움을 겪습니다.

기존의 연속 시간 접근법은 일반적으로 두 가지 패러다임으로 분류됩니다:

1. 매개변수화 (Parametric): 스플라인과 같은 기저 함수를 사용하여 궤적을 표현합니다. 이러한 방법은 과소적합이나 과적합을 피하기 위해 기저 함수의 신중한 선택이 필요합니다.
2. 비매개변수화 (Nonparametric): 가우시안 프로세스 (GP) 회귀를 사용합니다. 내재적인 정규화와 유연성으로 인해 기술적으로 유리하지만, 지배적인 팩터 그래프 (factor graph) 추정 패러다임과의 직관적인 통합 부재로 인해 로봇공학에서 GP 기반의 연속 시간 추정은 덜 채택되었습니다.

방법론

저자들은 리 군 (Lie groups, 예: SE(3)) 을 포함하는 로봇공학 응용 분야에 특화된, GP 회귀와 팩터 그래프를 통합하는 프레임워크를 제안합니다.

1. GP 회귀에 대한 팩터 그래프 관점

핵심 이론적 기여는 팩터 그래프의 렌즈를 통해 GP 연속 시간 추정을 재해석하는 것입니다.

• 분해: 추정 문제는 두 가지 명확한 단계로 분해됩니다:
  • 주해 (Main Solve): 표준 이산 시간 팩터 그래프 도구를 사용하여 이산 측정 시간 ($t_k$) 에서의 상태를 해결합니다.
  • 보간 (쿼리링): 주해가 완료된 후 임의의 쿼리 시간 ($\tau$) 에서 상태를 계산합니다.
• 소거 알고리즘: 저자들은 임의의 시간 $\tau$에서 궤적을 쿼리하는 것이 쿼리 상태 $x_\tau$를 제거하기 위해 팩터 그래프 소거 알고리즘을 적용하는 것과 동등함을 보여줍니다. 이는 쿼리가 $\tau$를 바로 앞뒤로 하는 "분리자 (separator)" 상태들 (측정 상태) 의 주변 분포에만 의존함을 드러냅니다.
• 계산 효율성: 선형 시불변 (LTI) 확률 미분 방정식 (SDE) 에서 유도된 커널 함수, 예를 들어 가속도 상의 백색 잡음 (White Noise on Acceleration, WNOA) 모델을 선택함으로써, 역커널 행렬이 **정확히 블록 삼대각 (block-tridiagonal)**이 됩니다. 이를 통해 주해는 측정 횟수의 $O(K^3)$가 아닌 $O(K)$ (선형) 시간 내에 수행될 수 있습니다. 또한, 쿼리 단계는 이웃 상태의 주변 통계만 필요로 하는 $O(1)$ 연산이 됩니다.

2. 리 군으로의 확장

리 군 (예: SE(3)) 같은 매니폴드에 존재하는 로봇 포즈 (회전 및 이동) 를 처리하기 위해, 저자들은 선형 GP 접근법을 적응시킵니다:

• 국소 GP 변수: 단일 전역 GP 대신, 각 측정 시간 주변의 리 대수 (Lie algebra) 에서 국소 변수를 정의합니다. 구체적으로, $\xi_k(t) = \text{Log}(T_k^{-1}T(t))$이며, 여기서 $T_k$는 측정 $k$에서의 포즈입니다.
• 선형화: 이러한 국소 변수에 선형 운동 모델 (예: WNOA) 을 적용합니다. 리 군 요소와 국소 변수 간의 비선형 관계는 반복 솔버 (예: 가우스 - 뉴턴 또는 레벤버그 - 마쿼트) 내의 표준 선형화 기법 (섭동 모델) 을 통해 처리됩니다.
• 공분산 보간: 리 군에서의 공분산 보간을 위한 새로운 방법이 소개되었으며, 이는 이전 방법 (예: Anderson, 2016) 보다 단순하고 다른 방법들 (예: Barfoot, 2024) 보다 효율적입니다.

3. 보간된 상태에서의 측정치 처리

명시적으로 최적화된 상태가 없는 시간에 발생하는 고주파수 측정치를 처리하는 것은 중요한 실용적 과제입니다.

• 상태 소거: 저자들은 최적화 그래프에서 측정과 관련된 상태를 완전히 소거하는 방법을 보여줍니다.
• 잔차 팩터: 측정치가 있는 보간된 상태를 소거하면 경계 상태 사이에 두 가지 팩터가 생성됩니다: **운동 사전 팩터 (motion prior factor)**와 새로운 측정 팩터입니다.
• 독립성 근사: 희소 그래프 구조를 유지하기 위해, 저자들은 여러 보간된 상태 간의 교차 공분산을 제거하는 근사법을 도입합니다. 이를 통해 여러 측정치를 경계 상태에 대한 독립적인 팩터로 처리할 수 있어 최적화 문제 크기가 크게 감소합니다. 또한 저자들은 정확도와 신뢰도 간의 트레이드오프를 언급하며, 특정 공분산 항을 생략하여 속도를 더 높이는 "단순화된 잡음 모델"에 대해서도 논의합니다.

구현

이 방법론은 팩터 그래프 최적화를 위한 인기 있는 C++ 라이브러리인 GTSAM(Georgia Tech Smoothing and Mapping) 에 구현되었습니다.

• 새로운 팩터: 저자들은 WnoaMotionFactor(운동 사전용) 와 WnoaInterpFactor(보간된 상태에 정의된 팩터를 경계 상태에 정의된 팩터로 변환하는 래퍼) 를 도입했습니다.
• 워크플로우: 사용자는 "경계 상태"(최적화됨) 와 "보간 상태"(소거됨) 가 포함된 그래프를 정의할 수 있습니다. 라이브러리는 자동으로 동등한 그래프를 구성하고 이를 해결한 후, 전체 연속 궤적과 공분산을 복원하는 후처리 단계 (updateInterpValues) 를 제공합니다.
• 최적화: 반복 중 중복 계산을 방지하기 위해 보간된 상태의 평균과 야코비 행렬을 캐싱하는 전용 WnoaFactorGraph 클래스가 구현되었습니다.

결과 및 평가

이 프레임워크는 1 차원부터 3 차원까지의 세 가지 데이터셋에서 평가되었습니다:

1. 1 차원 (거대한 우유 한 잔): 레이저 거리계와 주행계 (odometry) 를 사용하여 직선으로 이동하는 로봇. 결과는 희소 측정치 (최대 7.5 초 간격) 가 있더라도 연속 시간 접근법이 매끄럽고 정확한 궤적을 생성함을 보여주며, 선형 사례에서 "축소된" 해 (보간 포함) 가 "완전한" 해와 동일한 결과를 산출함을 보여줍니다.
2. 2 차원 (숲속에서 길을 잃다): 바퀴 달린 로봇과 랜드마크를 포함하는 SLAM 문제. 이 연구는 상태의 30 분의 1 만 최적화하고 나머지를 보간함으로써 완전한 해와 거의 동일한 궤적을 복원할 수 있음을 보여줍니다. 정량적 분석은 트레이드오프를 보여줍니다: "단순화된 잡음 모델"은 상당한 속도 향상을 제공하지만, "완전한 잡음 모델"에 비해 과신된 추정치와 약간 저하된 평균 정확도를 초래할 수 있습니다.
3. 3 차원 (별이 빛나는 밤): 스테레오 카메라와 IMU 데이터를 사용하여 SE(3) 에서의 위치 추정 문제. 선형화 포인트를 80% 줄여도 (5 번째 상태마다 최적화), 비선형성이 포함됨에도 불구하고 보간된 해는 완전한 해와 비교할 수 있는 정확도와 일관성을 유지합니다.

중요성과 주장

이 논문은 GP 기반의 연속 시간 추정이 로봇공학에서 미활용되는 상황을 시정하기 위해 다음과 같은 것을 제공한다고 주장합니다:

• 새로운 통찰: GP 회귀와 팩터 그래프 간의 명확한 연결을 제시하여, 궤적 쿼리가 단순히 소거 알고리즘의 적용임을 보여줍니다. 이는 공분산 보간을 수행하는 새로운 직관적인 방법을 제공합니다.
• 실용적 접근성: 복잡한 커널 행렬이나 보간 방정식을 처음부터 유도할 필요 없이 연속 시간 추정을 쉽게 채택할 수 있도록 GTSAM 에 구현된 작동 가능한 구현체를 제공합니다.
• 효율성: 궤적의 매끄러움이나 정확도를 희생하지 않고 최적화 변수의 수를 줄임으로써, 연속 시간 추정이 고주파수 비동기 센서를 효율적으로 처리할 수 있음을 보여줍니다.

저자들은 이 작업을 튜토리얼이자 실용적인 가이드로 위치시켜, 로봇공학에서의 연속 시간 추정 진입 장벽을 낮추는 것을 목표로 합니다. 그들은 추정 시간의 휴리스틱 선택 필요성과 리 군에서의 국소 GP 사용 (아티팩트를 유발할 수 있음) 과 같은 한계를 인정하지만, 이 접근법이 매개변수화 방법에 비해 견고하고 유연한 대안을 제공한다고 주장합니다.