기술 요약: 분산형 ToF 센서를 이용한 연속체 로봇의 위치 추정

문제 정의

연속체 로봇(Continuum Robots, CRs)은 순응성(compliance)과 소형화 능력 덕분에 제약이 많고 구조화되지 않은 환경에서의 검사 및 조작에 매우 효과적입니다. 그러나 이러한 로봇의 정밀한 온보드(onboard) 위치 추정을 달성하는 것은 여전히 큰 과제입니다. 기존의 고해解상도 센서(예: 표준 LiDAR)는 직경이 작은 CR에 통합하기에는 너무 크기 때문입니다. 결과적으로 연구자들은 작고 저해상도인 Time-of-Flight(ToF) 센서에 의존해야 합니다.

핵심적인 어려움은 이러한 소형 센서들이 **퇴화된 측정값(degenerate measurements)**을 생성한다는 점에 있습니다. 즉, 단일 센서가 자신의 위치를 완전히 구속할 수 있을 만큼 충분한 환경적 특징을 관측하지 못합니다. 또한, 기존의 위치 추정 방식은 일반적으로 밀집된 센싱, 강체 가정 또는 외부 참조 시스템(예: 모션 캡처)에 의존하는데, 이는 변형 가능하고 온보드 전용으로 작동해야 하는 복잡한 환경의 연속체 로봇에는 적합하지 않습니다. 사전 지도(prior map)와 실제 환경 사이에 중간 정도의 편차가 발생하더라도 표준 추정기는 실패할 수 있습니다.

방법론

저자들은 희소한 온보드 센서만을 사용하여 변형 가능한 연속체 로봇을 위치 추정하기 위해 설계된 연속 시간 기반의 파라미터 그래프(factor-graph) 기반 최대 사후 확률(MAP) 추정 프레임워크를 제안합니다.

1. 센서 퓨전 및 상태 추정

시스템은 로봇 본체를 따라 분산된 세 가지 소스의 데이터를 융합합니다:

• 분산형 ToF 센서: 로봇의 링을 따라 장착된 저해상도 센서(구체적으로 VL53L5CX).
• 자이로스코프: 방향 보조를 위한 관성 측정.
• 변형 센서 (시뮬레이션 전용): 형상 사전 정보를 제공하기 위한 굽힘 각도 및 곡률 데이터 (Fiber Bragg Grating 시스템을 모델링함).

상태 벡터 $x(s, t)$는 호 길이(arc length) $s$와 시간 $t$에 따른 연속적인 로봇 포즈 $T_{ib}(s, t) \in SE(3)$, 본체 중심의 일반화된 변형 $\epsilon(s, t)$, 그리고 속도 $\varpi(s, t)$를 포함합니다.

2. 강건한 측정 팩터(Robust Measurement Factors)

ToF 데이터의 희소하고 노이즈가 많은 특성을 처리하기 위해, 저자들은 Cauchy 손실 함수가 적용된 점-대-평면(point-to-plane) 측정 팩터를 구현했습니다.

• 점-대-평면 오차: 측정된 ToF 점들을 사전 계산된 지도(법선 벡터가 포함된 포인트 클라우드)에 투영하여 오차를 계산합니다.
• 강건성: IRLS(Iteratively Reweighted Least Squares) 기법을 통해 Cauchy 손실이 적용됩니다. 이는 이상치(outlier) 측정값과 기하학적 퇴화의 영향을 완화하여, 추정기가 복잡한 장면에서의 잘못된 매칭에 과도하게 영향을 받는 것을 방지합니다.
• 노이즈 모델링: ToF 노이즈는 거리 의존적 가우시안 노이즈로 모델링되며, 표준 편차는 측정 거리가 멀어짐에 따라 증가합니다 (VL53L5CX의 경험적 데이터 기반).

3. 최적화

프레임워크는 가중 제곱 오차 비용 함수를 최소화하기 위해 Gauss-Newton 솔버를 활용합니다. 연속 상태는 이산적인 시간 단계와 호 길이를 기준으로 추정되며, 전체 연속 궤적을 재구성하기 위해 보간법(interpolation)이 사용됩니다. 시스템은 슬라이딩 윈도우(sliding-window) 방식으로 작동합니다.

4. 실험 설정

• 시뮬레이션: 기하학적 특징이 추가되거나 제거된 10가지 환경 시나리오(S0–S9)를 가진 MuJoCo 모델 기반의 1.5m 길이 로봇.
• 실제 환경: 세 개의 센서 링(각 링당 3개의 ToF 센서와 1개의 자이로스코프 포함)을 갖춘 53cm 길이의 신축 가능한 소프트 연속체 로봇. 실험은 복잡도와 지도 편차를 시뮬레이션하기 위해 다양한 오브젝트 구성(R0–R4)이 있는 70cm 큐브 환경에서 수행되었습니다.

주요 기여

본 논문은 두 가지 주요 기여를 주장합니다:

1. 강건한 추정 프레임워크: 희소한 ToF 측정값과 로봇 형상 사전을 통합하는 연속 시간, 파라미터 그래프 기반의 MAP 추정기입니다. 이를 통해 빈번한 기하학적 퇴화와 부분적인 사전 지도 불일치 상황에서도 안정적인 변형 연속체 로봇의 전신(full-body) 위치 추정이 가능합니다.
2. 최초의 실세계 시연: 분산된 저해상도 ToF 및 관성 센싱을 사용하여, 복잡한 환경에서 소프트 신축형 연속체 로봇의 최초 온보드 전용 위치 추정을 성공하였으며, 이를 시뮬레이션과 물리적 플랫폼 모두에서 검증하였습니다.

결과

시뮬레이션 성능

제트 엔진 검사 시나리오 시뮬레이션에서, 시스템은 지도 편차에 대해 높은 강건성을 보여주었습니다.

• 정확도: 모든 10개 장면(오브젝트가 추가되거나 제거된 장면 포함)에 대해 평균 위치 오차 약 0.78 cm (MAE) 및 회전 오차 **0.96° (MAE)**를 달ach했습니다. 이에 대응하는 RMSE 값은 각각 0.99 cm와 0.98°였습니다.
• 관찰 사항: 시뮬레이션 결과는 강력한 비용 함수와 변형 데이터의 포함 덕분에 사전 지도와 실제 환경이 일치하지 않을 때도 성능 저하가 거의 없음을 보여주었습니다. 다만, 저자들은 시뮬레이션 결과가 이상적인 조건과 모델링되지 않은 오차의 부재로 인해 실제 환경보다 훨씬 더 정확하다는 점을 언급했습니다.

실제 환경 성능

물리 실험에서 시스템은 환경 변화에도 불구하고 위치 추정 능력을 유지했으나, 오차 범위는 더 높았습니다.

• 기본 정확도: 사전 지도가 장면과 일치하는 환경에서, 시스템은 평균 위치 오차 1.2 cm ~ 2.5 cm, 회전 오차 **3.7° ~ 10.3°**를 기록했습니다.
• 이상 현상의 영향:
  • 오브젝트를 추가했을 때 평균 위치 오차는 약 1.7 cm, 회전 오차는 약 1.0° 증가했습니다.
  • 오브젝트를 제거했을 때 위치 오차는 약 0.5 cm, 회전 오차는 약 1.5° 증가했습니다.
  • 가장 큰 성능 저하는 대형 "젖은 바닥 표지판(wet floor sign)"이 제거되었을 때 발생했으며, 이때 위치 오차는 3.6 cm, 회전 오차는 **14.6°**에 달했습니다.
• 전체 평균: 모든 실험 조건에서 시스템은 위치에서 2.5 cm, 회전에서 **7.2°**의 평균 위치 추정 오차를 달성했습니다.
• 오차 분포: 위치 오차는 로봇의 길이를 따라 누적되었으며, 베이스에 비해 팁 링(tip ring)에서 가장 높은 오차(변위 3.6 cm, 회전 9.9°)를 보였습니다.

장면 재구성

저자들은 추정된 로봇 궤적과 ToF 측정값을 사용하여 환경을 재구성할 수 있음을 입증했습니다. 위치 추정이 정확할 때 재구성된 장면은 사전 지도와 밀접하게 일치했습니다 (RMSE ~0.8–1.2 cm). 위치 추정이 큰 이상 현상으로 인해 실패했을 때는 재구성 품질이 크게 저하되었습니다.

의의 및 한계

본 논문은 이 연구가 변형 가능한 연속체 로봇을 위해 희소하고 저해상도인 깊이 센서만을 사용하여 전신(full-body), 온보드 전용 위치 추정의 실현 가능성을 보여준다고 주장하며, 이를 통해 외부 추적 시스템의 필요성을 제거합니다.

그러나 저자들은 현재의 한계점에 대해 다음과 같이 명시했습니다:

• 지도 의존성: 정확도는 현재 사전 지도의 충실도에 묶여 있습니다. 지도와 실제 장면 사이의 차이가 클 경우 추정기가 실패하거나 지역 최솟값(local minima)에 빠질 수 있습니다.
• 지역 최솟값: 초기값이 좋지 않거나, 로봇의 갑작스러운 움직임, 또는 심각한 지도 불일치가 발생할 경우 지역 최솟값에 갇히기 쉽습니다.
• 향ate 연구: 저자들은 사전 지도 의존성을 완화하고(예: 실시간 지도 업데이트), 지도 부정확성에 대한 강건성을 개선하며, 가속도계 데이터를 통합하여 지역 최솟값을 탈출하는 데 도움을 주는 연구가 필요하다고 제안합니다. 또한, 이상 탐지가 잠재적인 응용 분야이지만, 현재의 실제 실험 노이즈 수준은 오탐(false positive)을 유발하므로, 검사 도구로서 기능하기 위해서는 위치 추정 정확도의 추가적인 개선이 필요하다고 언급했습니다.