Geometric Visual Servo Via Optimal Transport

1. 문제 제기 (Problem Statement)

기존의 로봇 매니퓰레이터 비주얼 서보 (Visual Servoing) 제어는 주로 두 가지 방식인 **포지션 기반 비주얼 서보 (PBVS)**와 **이미지 기반 비주얼 서보 (IBVS)**로 나뉩니다.

• PBVS: 추출된 특징을 특정 3D 포지션으로 변환하여 현재와 목표 포지션의 차이를 계산합니다.
• IBVS: 추출된 이미지 특징을 직접 추적하여 목표 특징 집합으로 이동시킵니다.

기존 방법의 한계:

1. 수동 특징 추출 의존성: 현재 알고리즘들은 손으로 조정된 (hand-tuned) 특징 추출 방법에 의존하며, 추출된 특징의 확률적 특성을 고려하지 않습니다.
2. 노이즈 및 정밀도 문제: 카메라 파라미터에 기반한 특징 추출은 포지션 추정에 노이즈를 유발하며, 고정밀 작업을 위해 필요한 세밀한 특징 집합을 제공하지 못합니다.
3. 동역학 손실: 많은 제어 동역학 유도 과정에서 로봇 플랫폼의 물리적 특성 (기하학적 구조, 에너지 보존 등) 이 손실됩니다.
4. 역운동학 및 PD 제어의 제약: 기존 접근법은 역운동학 (Inverse Kinematics) 과 좌표 레벨의 PD 제어를 사용하여 성능 보장을 저해할 수 있습니다.

이 논문은 카메라 입력을 **3 차원 특수 유클리드 군 (SE(3)) 상의 확률 측도 (Probability Measure)**로 정의하고, 이를 최적 수송 (Optimal Transport, OT) 이론을 통해 처리하여 위와 같은 한계를 극복하는 새로운 기하학적 제어 법칙을 제안합니다.

---

2. 방법론 (Methodology)

제안된 제어 아키텍처는 포트-해밀토니안 (Port-Hamiltonian) 시스템 이론과 **동적 최적 수송 (Dynamic Optimal Transport)**을 결합하여 구성됩니다.

2.1. 시스템 모델링: 포트-해밀토니안 동역학

• 로봇 매니퓰레이터의 동역학을 SE(3) 리 군 (Lie Group) 위에서 정의하여 기하학적 특성을 유지합니다.
• 작업 공간 (Task-space) 과 관절 공간 (Joint-space) 간의 관계를 유지하며, 포트-해밀토니안 형태로 재구성합니다.
  • 상태 변수: $x = [g, p]^T$ (포지션 $g$와 켤레 운동량 $p$)
  • 해밀토니안 $H(g, p)$: 운동 에너지와 위치 에너지의 합.
• 이 구조는 에너지 보존 법칙을 따르며, 수동성 (Passivity) 기반 제어 (PBC) 를 적용하기 용이하게 합니다.

2.2. 깊이 맵을 확률 측도로 표현

• RGB-D 카메라에서 얻은 깊이 맵 (Depth Map) 을 **점 구름 (Point Cloud)**으로 간주하고, 이를 SE(3) 군 위의 확률 측도 $\mu$로 정의합니다.
• 초기 깊이 맵 ($\mu_0$) 과 목표 깊이 맵 ($\mu_1$) 간의 거리를 계산하기 위해 Wasserstein 거리를 사용합니다.
• 깊이 값의 불균형 (Unbalanced nature) 을 처리하기 위해 **불균형 카antorovich 완화 (Unbalanced Kantorovich relaxation)**를 적용하여, 질량 보존이 완벽하지 않은 경우에도 수송 경로를 계산할 수 있도록 합니다.

2.3. 제어 입력 구성

제어 입력 $u$는 세 가지 요소의 합으로 구성됩니다:
$$u = u_{ES} + u_{DI} + u_{DC}$$

1. 에너지 형상화 (Energy Shaping, $u_{ES}$):
   • 목표 포지션 ($g^*$) 에서 해밀토니안 에너지가 최소가 되도록 시스템을 재형상화합니다.
   • 기하학적 오차 (위치 및 회전) 를 기반으로 한 비례 - 미분 (PD) 제어와 중력 보상 (Gravity Compensation) 을 포함합니다.
2. 감쇠 주입 (Damping Injection, $u_{DI}$):
   • 시스템의 안정성을 보장하고 에너지를 소산시키기 위한 감쇠 항입니다.
3. 동적 수송 보정 (Dynamic Transport Compensation, $u_{DC}$):
   • 핵심 혁신: 현재 깊이 맵과 목표 깊이 맵 간의 **최적 수송 매핑 (Optimal Transport Map, $T^*$)**을 계산합니다.
   • 이 매핑을 통해 깊이 맵의 변위를 최소화하는 흐름 (Flow) 을 생성하고, 이를 보정 입력으로 사용하여 이미지 공간의 오차를 직접 줄입니다.
   • 이는 IBVS 의 특징 추적 능력을 포트-해밀토니안 프레임워크에 통합한 것입니다.

---

3. 주요 기여 (Key Contributions)

1. 포트-해밀토니안 형태의 시스템 동역학 공식화:
   • 작업 공간의 기하학적 특성과 관절 공간의 제어를 결합한 로봇 매니퓰레이터 동역학을 SE(3) 리 군 위에서 포트-해밀토니안 형태로 재정의했습니다.
2. 깊이 맵의 확률 측도 표현:
   • RGB-D 카메라의 출력을 단순한 특징점이 아닌, 리 군 포지션 상의 확률 측도로 모델링하여 깊이 맵을 생성했습니다.
3. 기하학적 오차 기반의 새로운 제어 신호:
   • 리 군의 기하학을 활용하여 참조 깊이 맵과 현재 깊이 맵 간의 오차를 재정의하고, 이를 통해 목표 깊이 맵으로 유도하는 최소 에너지 작업 공간 제어기를 개발했습니다.
4. 하이브리드 비주얼 서보 프레임워크:
   • PBVS(포지션 기반) 의 안정성과 IBVS(이미지 기반) 의 특징 추적 능력을 통합한 새로운 제어 아키텍처를 제시했습니다.

---

4. 실험 결과 (Results)

• 실험 설정: 실제 하드웨어 (로봇 매니퓰레이터) 를 사용하여 'peg-in-hole' 시나리오와 유사한 테스트를 수행했습니다. 다양한 초기 포지션 (4 가지 무작위 포지션) 에서 목표 포지션으로 이동하는 능력을 평가했습니다.
• 성능:
  • 수렴성: 로봇은 초기에는 큰 오차로 인해 빠르게 반응하다가 목표에 가까워질수록 부드럽게 수렴했습니다.
  • 최단 경로: 로봇 엔드 이펙터가 이동한 경로는 SE(3) 공간에서의 **측지선 (Geodesic)**과 일치하여 최소 에너지 경로를 따르는 것을 확인했습니다.
  • 강건성: 다양한 초기 위치와 회전 오차에서도 목표 포지션에 안정적으로 도달했으며, 자코비안 행렬의 랭크 결손 (Rank deficiency) 으로 인한 일시적 불안정성에서도 회복 능력을 보였습니다.
  • 실시간성: 제어 루프 시간 (300ms) 내에서 회전 오차 및 수송 맵 변동을 처리하며 실시간 성능을 입증했습니다.
  • 에너지 보존: 제어 작동 중 시스템의 운동 에너지가 목표 상태에 도달함에 따라 0 으로 수렴하는 것을 확인하여 에너지 효율적인 제어가 이루어짐을 보였습니다.

---

5. 의의 및 결론 (Significance & Conclusion)

이 논문은 비주얼 서보 문제를 단순한 "오차 최소화"가 아닌 "질량 이동 (Mass-movers)" 문제로 재해석했다는 점에서 의의가 큽니다.

• 이론적 통합: 기하학적 제어 (리 군, 포트-해밀토니안) 와 최적 수송 이론 (Wasserstein 거리, 동적 수송) 을 성공적으로 융합하여, 기존 PBVS 와 IBVS 의 단점을 보완한 통합 프레임워크를 제시했습니다.
• 확장성: 이 접근법은 고정된 목표뿐만 아니라, 미래에는 예측 제어 (MPC) 와 결합하여 움직이는 목표 (Non-static target) 를 추적하는 데에도 적용 가능한 잠재력을 가집니다.
• 실용적 가치: 복잡한 기하학적 환경이나 불규칙한 물체 조작과 같은 고난도 작업에서, 수동 특징 추출 없이 깊이 정보의 확률적 분포를 직접 활용하여 더 정밀하고 강건한 제어를 가능하게 합니다.

결론적으로, 이 연구는 로봇 제어 분야에서 기하학적 구조와 데이터 기반의 확률적 모델링을 결합하여 차세대 비주얼 서보 제어의 새로운 패러다임을 제시했습니다.