Geometric Visual Servo Via Optimal Transport

1. Planteamiento del Problema

El control de servocontrol visual (visual servoing) en robótica busca mover un efector final o un objeto desde una pose inicial a una pose objetivo utilizando retroalimentación visual. Sin embargo, los enfoques actuales presentan limitaciones significativas:

• Dependencia de características manuales: Los algoritmos tradicionales dependen de la extracción de características visuales (como bordes o esquinas) que a menudo requieren ajuste manual y son ruidosas.
• División entre PBVS e IBVS: Existen dos enfoques principales: Pose-Based Visual Servo (PBVS), que calcula el error en la pose 3D, e Image-Based Visual Servo (IBVS), que minimiza el error en las características de la imagen. La mayoría de los sistemas eligen uno u otro, perdiendo la sinergia entre la geometría física y la información de la imagen.
• Pérdida de propiedades geométricas: Muchos métodos de control ignoran la estructura geométrica inherente del espacio de tareas (el grupo de Lie $SE(3)$) y las dinámicas físicas del robot, lo que puede llevar a inestabilidades o garantías de rendimiento deficientes.
• Incertidumbre: La estimación de la pose basada en cámaras puede ser inexacta, y los métodos actuales no tratan las características visuales como distribuciones probabilísticas robustas.

El objetivo de este trabajo es desarrollar una ley de control geométrica unificada que combine la información de pose y de imagen, tratando las características visuales como medidas de probabilidad y utilizando la teoría del transporte óptimo para minimizar la energía necesaria para alcanzar el objetivo.

2. Metodología

La propuesta se basa en una reformulación del problema de servocontrol visual como un problema de control óptimo en el espacio de tareas, utilizando tres pilares fundamentales:

A. Dinámica Port-Hamiltoniana en $SE(3)$

Los autores modelan la dinámica del manipulador robótico utilizando la mecánica Hamiltoniana en el grupo de Lie $SE(3)$ (que representa rotaciones y traslaciones en 3D).

• Se deriva una dinámica en forma Port-Hamiltoniana, lo que permite preservar las propiedades geométricas y de conservación de energía del sistema.
• Se utiliza un controlador basado en pasividad (PBC) que inyecta energía en el sistema para estabilizarlo en un punto de equilibrio deseado $(g^*, p^*)$.
• El control se divide en:
  • Forma de Energía ($u_{ES}$): Moldea el Hamiltoniano para crear un mínimo en la pose deseada.
  • Inyección de Amortiguamiento ($u_{DI}$): Asegura la estabilidad asintótica disipando energía.
  • Esto equivale a un control PD con compensación de gravedad, pero formulado geométricamente.

B. Representación de Nubes de Puntos como Medidas de Probabilidad

En lugar de extraer características discretas, el sistema utiliza mapas de profundidad (de una cámara RGB-D) para representar el entorno.

• La nube de puntos se modela como una medida de probabilidad $\mu$ sobre el grupo $SE(3)$.
• Se define el objetivo como mover la distribución de la nube de puntos inicial ($\mu_0$) a la distribución objetivo ($\mu_1$).

C. Transporte Óptimo (Optimal Transport - OT)

Para cerrar el lazo de control entre la imagen y la acción, se utiliza el Transporte Óptimo:

• Se calcula la distancia entre la nube de puntos actual y la objetivo utilizando la distancia de Wasserstein ($W_p$). Esta distancia actúa como una métrica geométrica análoga a la geodésica en el espacio de tareas.
• Dado que las nubes de puntos pueden tener masas desiguales (debido a oclusiones o límites del sensor), se emplea una relajación de Kantorovich no balanceada (Unbalanced Optimal Transport).
• Se introduce un término de control de compensación de perturbaciones ($u_{DC}$) que se deriva del mapa de transporte óptimo dinámico. Este término corrige el error basándose en el flujo de masa necesario para alinear las nubes de puntos, actuando como una corrección de error en el espacio de la imagen.

Ecuación de Control Final:
El control total es la suma de tres componentes:
$$u = u_{ES} + u_{DI} + u_{DC}$$
Donde $u_{DC}$ es el término innovador que utiliza el mapa de transporte óptimo para guiar al robot hacia la distribución de profundidad deseada.

3. Contribuciones Clave

1. Formulación Unificada: Se presenta un marco que combina el control basado en pose (PBVS) y basado en imagen (IBVS) en un solo esquema geométrico, eliminando la necesidad de extraer características manuales específicas.
2. Dinámica Port-Hamiltoniana en $SE(3)$: Se deriva la dinámica del manipulador en el grupo de Lie $SE(3)$, preservando las propiedades geométricas y permitiendo el uso de controladores basados en energía.
3. Integración de Transporte Óptimo: Se utiliza el transporte óptimo dinámico para generar señales de control que minimizan la energía requerida para alinear mapas de profundidad, tratando las características visuales como medidas probabilísticas.
4. Estabilidad Garantizada: Se demuestra teóricamente (Teorema 12) que el sistema es asintóticamente estable bajo condiciones de rango completo de la Jacobiana, utilizando una función de Lyapunov basada en el Hamiltoniano.

4. Resultados

Los autores validaron su enfoque en un manipulador robótico real utilizando una cámara RGB-D:

• Generalización: El controlador logró converger suavemente a la pose objetivo desde cuatro poses iniciales aleatorias diferentes, demostrando robustez ante variaciones en la distancia y rotación inicial.
• Comportamiento Geodésico: La trayectoria del efector final siguió la geodésica más corta en el espacio $SE(3)$, confirmando que el control genera movimientos de mínima energía.
• Rendimiento en Tiempo Real: El bucle de control operó con un tiempo de ciclo de 300 ms, incluso con perturbaciones rotacionales y variaciones en el mapa de transporte.
• Conservación de Energía: Los gráficos mostraron que la energía cinética del manipulador tiende a cero a medida que se alcanza el objetivo, y la energía total se estabiliza en el vector de gravedad, lo que indica un comportamiento físico coherente.
• Recuperación de Inestabilidades: El sistema fue capaz de recuperarse de fluctuaciones menores en el error que surgieron cuando la matriz Jacobiana se volvió casi singular (puntos singulares cinemáticos).

5. Significado e Impacto

Este trabajo representa un avance significativo en la teoría de control robótico al:

• Puentear la brecha entre visión y dinámica: Al tratar las características visuales como medidas de probabilidad y utilizar el transporte óptimo, se integra la percepción directamente en la dinámica del sistema sin depender de heurísticas de extracción de características.
• Robustez Geométrica: Al operar directamente en el grupo de Lie $SE(3)$, el controlador evita problemas de singularidades y representaciones inadecuadas (como ángulos de Euler) que suelen afectar a los métodos tradicionales.
• Aplicabilidad Futura: La metodología es general y puede aplicarse a cualquier sistema autónomo que utilice control basado en visión. Los autores sugieren que el uso de métodos de transporte óptimo neuronal podría mejorar la velocidad de cálculo para nubes de puntos de alta dimensión, abriendo puertas a aplicaciones en manufactura aeroespacial y ensamblaje electrónico de alta precisión.

En resumen, el paper propone una solución elegante y matemáticamente rigurosa que transforma el problema de servocontrol visual en un problema de flujo de masa óptimo, garantizando estabilidad, eficiencia energética y una integración natural entre la percepción visual y la acción motora.