Resumen Técnico: Ensamblaje de Cuerpos Rígidos Robusto mediante Control Óptimo con Contacto Implícito y Derivadas de Segundo Orden Exactas

Planteamiento del Problema

La planificación eficiente de movimientos de ensamblaje sigue siendo un desafío significativo en la robótica. Los enfoques tradicionales dependen en gran medida del aprendizaje por refuerzo (RL) libre de derivadas y de métodos basados en muestreo, los cuales requieren simulaciones físicas extensas y a menudo carecen de eficiencia de muestreo. Si bien los métodos basados en derivadas han mostrado resultados prometedores en otros dominios (por ejemplo, locomoción), tienen dificultades en tareas de ensamblaje debido a la naturaleza intrínsecamente no suave de la dinámica de contacto.

La dificultad central radica en que los sistemas dinámicos ricos en contacto se formulan típicamente como Problemas de Complementariedad No Lineal (NCP) o Problemas de Complementariedad Lineal (LCP). Las soluciones a estos problemas son no diferenciables con respecto a los estados y las entradas de control porque cambios infinitesimales pueden causar cambios discretos en los conjuntos de contacto activos (por ejemplo, cuando los puntos de contacto se desplazan entre caras, aristas o vértices, o cuando cambian los modos de fricción). Esta falta de suavidad impide la aplicación directa de métodos de optimización de segundo orden eficientes, que dependen de derivadas exactas para lograr una convergencia rápida. Además, las simulaciones de física diferenciable existentes a menudo omiten la no diferenciabilidad introducida por la detección de colisiones o dependen de aproximaciones (diferencias finitas, suavizado aleatorio) que no coinciden con la evaluación nominal, lo que obstaculiza la convergencia de la optimización.

Metodología

El artículo propone un enfoque de control óptimo robusto y eficiente en términos de muestreo que utiliza una simulación de física diferenciable capaz de proporcionar derivadas analíticas de segundo orden exactas. La metodología integra tres componentes principales:

1. Detección de Colisiones Diferenciable y Suave

Los autores formulan la detección de colisiones como un Programa Lineal (LP) utilizando distancias de crecimiento (una variante de las funciones de distancia con signo).

• Formulación: En lugar de escalar los polítopos de forma estable en su forma, aplican un escalado uniforme en todas las direcciones para asegurar problemas de optimización bien escalados.
• Suavizado: Para manejar la no diferenciabilidad, emplean métodos de punto interior. Al resolver el sistema KKT del LP con un parámetro de barrera $\tau > 0$, obtienen una aproximación suave de la función de distancia con signo (SDF), $\Phi_\tau(q)$.
• Derivadas: Utilizando el Teorema de la Función Implícita (IFT), derivan las derivadas exactas de primer y segundo orden de la SDF y del vector normal de contacto.
• Aproximación de Gradiente: Se propone una aproximación específica para el vector normal de contacto, derivada de las variables primal-duales del LP. Esto evita la necesidad de derivadas de tercer orden de la SDF, que serían requeridas si se utilizara directamente el gradiente exacto.
• Robustez: Se introduce una estrategia de escalado heurística para manejar casos donde el solver falla al converger debido a matrices KKT mal condicionadas, asegurando la generación de información de contacto fiable.

2. Física de Multicontacto Diferenciable

El problema de resolución de contacto se modela utilizando una formulación de dinámica de contacto sin fricción basada en el esquema de paso de tiempo de Moreau, lo que resulta en un LCP de contacto.

• Suavizado/Relajación: Las condiciones de complementariedad se suavizan mediante el suavizado de Scholtes (reemplazando $a \perp b$ con $ab = \sigma$) o la relajación de Scholtes ($ab \leq \sigma$). Esto transforma el LCP no suave en un conjunto de restricciones de igualdad y desigualdad suaves.
• Integración: La dinámica de contacto suave se transcribe directamente al problema de control óptimo (OCP). El solver optimiza simultáneamente las entradas de control y la dinámica del sistema.
• Procedimiento de Homotopía: Para resolver la dinámica de contacto rígida (donde $\tau, \sigma \to 0$), se utiliza un procedimiento de homotopía. La optimización comienza con parámetros de suavizado grandes (física relajada) y los reduce secuencialmente, utilizando las soluciones previas para realizar un precalentamiento (warm-start) del solver. Esto permite que el algoritmo converja desde suposiciones iniciales triviales hacia soluciones que se aproximan estrechamente a las formulaciones originales no diferenciables.

3. Control Óptimo Multi-Escenario Robusto

Para asegurar la ejecución exitosa en hardware real a pesar de los desajustes entre el modelo y la realidad, los autores formulan un OCP basado en múltiples escenarios.

• Seguimiento de Impedancia: Se optimiza una trayectoria de referencia, la cual es rastreada por múltiples trayectorias "compliantes" mediante un controlador de impedancia cartesiana.
• Perturbación: Cada escenario corresponde a una trayectoria compliante con un desplazamiento posicional aleatorio respecto a la referencia. El optimizador debe encontrar una trayectoria de referencia que complete con éxito la tarea de ensamblaje para todos los escenarios simultáneamente.
• Objetivo: Este enfoque optimiza efectivamente la robustez, asegurando que el movimiento planeado sea factible incluso si el robot real se desvía ligeramente del modelo.

Contribuciones Clave

1. Formulación de Distancia de Crecimiento Modificada: Una nueva formulación de funciones de distancia con signo basada en LP que asegura derivadas bien escaladas y aproxima estrechamente la distancia euclidiana, incluyendo una aproximación de gradiente adecuada para las normales de contacto.
2. Implementación de Derivadas Eficiente: Una implementación utilizando el solver de alto rendimiento para puntos interiores HPIPM y la librería de álgebra lineal BLASFEO para computar derivadas exactas de primer y segundo orden de la detección de colisiones y la resolución de contacto, integradas con la librería de control óptimo CasADi.
3. Planificación Multi-Escenario Robusta: Una formulación que combina el control óptimo con contacto implícito y robustez multi-escenario, asegurando movimientos ejecutables en robots reales mediante control de impedancia cartesiana.
4. Validación Empírica: Experimentos en el mundo real que demuestran altas tasas de éxito (>99%) en tareas de ensamblaje tipo perno-agujero (peg-in-hole) con holgura casi nula, y resultados de simulación que muestran los beneficios de las Hessianas exactas sobre las aproximaciones (Gauss-Newton, L-BFGS) y el impacto de los parámetros de suavizado.

Resultados

• Desempeño en el Mundo Real: Los experimentos con un robot UR10e realizando tareas de ensamblaje perno-agujero lograron más del 99% de ejecuciones exitosas. El método manejó con éxito desajustes sustanciales entre el modelo y la realidad sin necesidad de ajustes adicionales.
• Eficiencia Computacional: La implementación propuesta de la SDF computa las derivadas de manera eficiente. Aunque es ligeramente más lenta que los algoritmos Guro de estado del arte para la evaluación nominal, la capacidad de computar derivadas exactas de segundo orden hace que el proceso de optimización general sea significamente más eficiente en términos de muestreo.
• Convergencia: Los resultados numéricos indican que el uso de Hessianas exactas mejora significamente la velocidad de convergencia en comparación con las aproximaciones de la Hessiana como Gauss-Newton o L-BFGS.
• Impacto del Suavizado: El estudio investiga la compensación entre los parámetros de suavizado y las tasas de éxito, mostrando que el procedimiento de homotopía cierra eficazmente la brecha entre las aproximaciones suaves y fáciles de resolver y la dinámica de contacto precisa y rígida requerida para la ejecución en el mundo real.

Significado

El artículo afirma cerrar la brecha entre la optimización basada en derivadas y las tareas de ensamblaje complejas. Al proporcionar un marco que utiliza derivadas exactas de segundo orden tanto para la detección de colisiones como para la resolución de contacto, el método permite el uso de solvers eficientes basados en Newton para problemas de contacto rico. Esto conduce a:

• Mejor Eficiencia de Muestreo: Se requieren significativamente menos pasos de simulación física durante la planificación en comparación con los métodos libres de derivadas.
• Robustez: La formulación multi-escenario asegura que las trayectorias generadas sean robustas a las incertidumbres, lo que las hace directamente ejecutables en hardware real mediante control de impedancia cartesiana.
• Generalidad: El enfoque es aplicable a cualquier problema de ensamblaje de cuerpos rígidos donde las formas se representen como uniones de polítopos convexos.

El trabajo demuestra que, con un suavizado cuidadoso y el cálculo de derivadas exactas, el control óptimo puede resolver eficazmente problemas de ensamblaje desafiantes que tradicionalmente han sido el dominio del aprendizaje por refuerzo y de los métodos basados en muestreo.