State and Trajectory Estimation of Tensegrity Robots via Factor Graphs and Chebyshev Polynomials

논문 요약: 텐서그리티 로봇을 위한 팩터 그래프 및 체비셰프 다항식을 활용한 상태 및 궤적 추정

1. 문제 정의 (Problem Statement)

• 배경: 텐서그리티 (Tensegrity) 로봇은 압축을 받는 막대 (rods) 와 인장력을 받는 케이블 (cables) 로 구성되어 있어 유연성 (compliance) 과 적응성이 뛰어나지만, 비선형적이고 제약이 부족한 (underconstrained) 동역학 특성을 가집니다.
• 도전 과제:
  • 폐루프 제어, 시스템 식별, 머신러닝을 위해서는 모든 강체 링크의 연속적인 시간 (continuous-time) 상태 추정이 필수적입니다.
  • 기존 방법들은 고주파수 센서 데이터 처리나 중간 상태의 정밀한 추정에 한계가 있으며, 특히 텐서그리티의 복잡한 구조와 비선형성으로 인해 정확한 상태 추정이 어렵습니다.
  • 기존 ICP(Iterative Closest Point) 기반 방법들은 안정적인 초기화 (manual initialization) 를 필요로 하거나, 높은 주파수 데이터 처리에 취약합니다.
• 목표: RGB-D 카메라와 온보드 케이블 길이 센서 데이터를 융합하여, 텐서그리티 로봇의 강체 막대 (rigid links) 의 위치, 자세, 속도 및 궤적을 고충실도 (high-fidelity) 로 추정하는 새로운 프레임워크 개발.

2. 제안된 방법론 (Methodology)

논문은 **온라인 상태 추정 (Online State Estimation)**과 **오프라인 궤적 정제 (Offline Trajectory Refinement)**의 두 단계로 구성된 접근법을 제시합니다.

A. 데이터 전처리: 마할라노비스 거리 기반 클러스터링

• 문제: RGB-D 센서에서 얻은 포인트 클라우드에는 노이즈, 자기 가림 (self-occlusion), 유사 색상 오검출 등이 존재합니다.
• 해결: 마할라노비스 거리 (Mahalanobis distance) 를 기반으로 한 클러스터링 알고리즘을 적용합니다.
  • 포인트 클라우드를 색상 (빨강, 초록, 파랑 엔드캡, 검은색 막대) 에 따라 분류합니다.
  • 베이지안 프레임워크를 사용하여 관측치와 사전 추정을 비교하고, 통계적 검정 ($\chi^2$ 분포) 을 통해 노이즈가 심하거나 일관성이 없는 관측치를 제거 (reject) 합니다.
  • 이를 통해 최적화 과정에 입력되는 데이터의 품질을 높이고 불필요한 검출을 줄입니다.

B. 온라인 상태 추정: 팩터 그래프 (Factor Graphs)

• 구조: 로봇의 구조적 제약을 활용하는 확률적 프레임워크인 팩터 그래프를 도입합니다.
• 변수 (Variables): 각 막대 (bar) 의 6 자유도 (6-DOF) 자세 ($SE(3)$) 를 나타냅니다.
• 인자 (Factors):
  • 엔드캡 팩터 ($f_{endcap}$): 카메라로 관측된 엔드캡 포인트와 막대 추정치 간의 기하학적 관계.
  • 케이블 팩터 ($f_{cable}$): 온보드 센서로 측정한 케이블 길이와 두 엔드캡 간의 거리 관계.
  • 막대 팩터 ($f_{bar}$): 검은색 막대 포인트 클라우드와 막대 추정치 간의 거리 최소화.
  • 사이 팩터 ($f_{between}$): 시간 단계 간의 연속성을 유지하며, 구동 모델 (actuation model) 이 없더라도 관측 간 큰 변화를 억제합니다.
  • 대칭성 팩터 ($f_{R0}, f_{R1}, f_{R2}$): 엔드캡의 대응 문제 (correspondence problem) 를 해결하기 위해 막대의 대칭성을 강제합니다.
• 특징: 명시적인 구동 모델이 없어도 구조적 제약과 센서 융합을 통해 정확한 상태를 추정할 수 있습니다.

C. 오프라인 궤적 추정: 체비셰프 다항식 (Chebyshev Polynomials)

• 목적: 이산적인 상태 추정값을 연속적인 시간 함수로 변환하여 속도 (velocity) 와 중간 상태를 고충실도로 복원합니다.
• 방법:
  • 엔드캡의 궤적을 체비셰프 다항식으로 파라미터화합니다.
  • 체비셰프 점은 (Chebyshev points) 을 사용하여 다항식 계수를 구하고, 이를 통해 임의의 시간에서의 상태를 계산합니다.
  • 막대의 궤적은 엔드캡 다항식을 기반으로 유도됩니다.
  • 장점: 스플라인 (Splines) 과 달리 리 군 (Lie group) 위에서 정의하기 쉬우며, 미분 (속도 계산) 이 용이하고 최소 제곱 (least-square) 의미에서 최적의 근사치를 제공합니다.
  • 최적화: 다항식의 차수 (degree) 를 자동으로 탐색하여 주어진 데이터에 가장 잘 맞는 모델을 선택합니다.

3. 주요 기여 (Key Contributions)

1. 최초 적용: 텐서그리티 로봇 분야에서 팩터 그래프를 상태 추정에 적용한 최초의 연구입니다.
2. 센서 융합 프레임워크: RGB-D 카메라 데이터와 케이블 길이 센서 데이터를 통합하여, 구동 모델 없이도 로봇의 형태와 운동을 정확히 추정합니다.
3. 연속 시간 추정: 이산적인 상태 추정에서 체비셰프 다항식을 사용하여 연속 시간 궤적과 속도를 복원함으로써, 시스템 식별 및 데이터 기반 모델링에 필수적인 고충실도 데이터를 제공합니다.
4. 강건한 전처리: 마할라노비스 거리 기반 클러스터링을 통해 노이즈와 가림 현상을 효과적으로 처리합니다.

4. 실험 결과 (Results)

• 데이터셋: 시뮬레이션 (Mujoco 기반) 데이터와 3 개의 실제 로봇 데이터셋 (기존 데이터셋 2 개, 새로운 데이터셋 1 개) 을 사용했습니다.
• 비교 대상: 기존 ICP 기반 알고리즘 [4] 과 비교했습니다.
• 성능:
  • 정확도: 제안된 방법은 ICP 기반 방법과 유사하거나 특정 조건 (새로운 데이터셋의 회전 오차 등) 에서 더 나은 성능을 보였습니다. 특히 회전 (Rotation) 오차 측면에서 개선되었습니다.
  • 초기화: 기존 ICP 방법은 안정적인 초기 자세를 수동으로 지정해야 했으나, 제안된 방법은 자동 초기화가 가능하여 적용성이 높습니다.
  • 주파수: 고주파수 센서 업데이트를 효과적으로 처리할 수 있습니다.
  • 체비셰프 다항식: 시뮬레이션 실험에서 다양한 노이즈 레벨과 관측 주파수 (5Hz~30Hz) 에서 체비셰프 다항식이 200Hz 의 고해상도 궤적을 잘 복원함을 입증했습니다.
• 결론: 제안된 접근법은 복잡한 텐서그리티 로봇 운동에 대해 고충실도의 연속 시간 상태 및 궤적 추정을 제공합니다.

5. 의의 및 의의 (Significance)

• 제어 및 학습: 이 연구는 텐서그리티 로봇의 폐루프 제어, 시스템 식별, 그리고 머신러닝 기반 모델 학습을 위한 고품질 레이블 데이터 생성을 가능하게 합니다.
• 확장성: 팩터 그래프 구조는 다양한 텐서그리티 구조 (예: 6-바 플랫폼) 로 확장 가능하며, 추가 센서도 쉽게 통합할 수 있습니다.
• 미래 작업: 추후 상태 추정과 제어의 동시 수행 (simultaneous estimation and control) 으로 확장될 수 있으며, 이는 소프트/하이브리드 로봇의 자율성 향상에 중요한 기여를 할 것으로 기대됩니다.

이 논문은 텐서그리티 로봇의 복잡한 동역학 문제를 해결하기 위해 기하학적 구조와 확률적 추론을 결합한 혁신적인 접근법을 제시하며, 로봇의 상태 인식 및 제어 분야에서 중요한 이정표가 됩니다.