A Sliding-Window Filter for Online Continuous-Time Continuum Robot State Estimation

Resumen Técnico: Un Filtro de Ventana Deslizante para la Estimación de Estado en Tiempo Continuo y en Línea de Robots Continuos

Enunciado del Problema
Los robots continuos (CR) son manipuladores flexibles capaces de navegar en espacios confinados, pero su control depende de una estimación de estado precisa. Los métodos probabilísticos existentes enfrentan una dicotomía:

• Los métodos de filtrado (por ejemplo, Filtros de Kalman Extendidos) son computacionalmente eficientes y adecuados para operaciones en línea, pero a menudo sufren una reducción en la precisión debido a aproximaciones de linealización que no capturan completamente la estructura de Markov de los sistemas no lineales.
• Los métodos de suavizado por lotes ofrecen una precisión superior al optimizar conjuntamente todos los estados, pero generalmente se restringen a operaciones fuera de línea, ya que requieren acceso retrospectivo a todas las mediciones y no escalan bien para el control en tiempo real.
• Los enfoques en tiempo continuo han demostrado ser prometedores para los CR, pero previamente se han limitado a la optimización por lotes fuera de línea. Además, los intentos existentes de ventana deslizante para CR se han basado en modelos simplificados y no probabilísticos (por ejemplo, aproximaciones de curvatura constante o eslabones rígidos) o carecían de representaciones estocásticas para cuantificar la incertidumbre.

Metodología
Los autores proponen un Filtro de Ventana Deslizante (SWF) diseñado específicamente para la estimación de estado en tiempo continuo y en línea de CR. El método se basa en un marco de gráfico de factores derivado de técnicas recientes de optimización por lotes.

• Marco de Estimación: El sistema estima la pose $T(s,t)$, la velocidad $\varpi(s,t)$ y la deformación $\epsilon(s,t)$ del robot de manera continua a lo largo de la longitud de arco $s$ y el tiempo $t$. El problema se formula como una estimación de Máxima A Posteriori (MAP), minimizando el logaritmo negativo de la verosimilitud de los factores previos y de medición.
  • Priors: Utilizan un modelo aproximado de varilla de Cosserat basado en un prior de movimiento de "ruido blanco sobre aceleración".
  • Mediciones: Incorporan mediciones de la pose de la punta y datos de giroscopio.
• Formulación de Ventana Deslizante: El filtro mantiene una ventana de tiempo de longitud fija que contiene los estados $x_{a:k}$.
  • Factorización: La posterior conjunta se descompone en factores previos marginalizados (que encapsulan la historia antes de la ventana), factores espaciales (a lo largo de la longitud de arco del robot) y factores de movimiento/medición temporales (dentro de la ventana).
  • Marginalización: Para mantener un tamaño de ventana fijo mientras se propaga la información, el paso de tiempo más antiguo se marginaliza utilizando el complemento de Schur en forma de información. Esto crea un prior efectivo para la nueva ventana, preservando la propiedad de Markov mientras mitiga los errores inherentes a las suposiciones estrictas de Markov.
• Detalles de Implementación:
  • Expansión de Ventana: Los nuevos estados se inicializan promediando una estimación consistente con el prior espacial y una estimación consistente con el prior temporal.
  • Extracción de Estado: La media y la covarianza se extraen del fondo de la ventana para utilizar todas las mediciones disponibles, introduciendo una pequeña latencia (típicamente $\leq 0.1$s).
  • Manejo de Covarianza: Para abordar las discontinuidades causadas por la re-linealización, las estimaciones conjuntas de covarianza entre pasos de tiempo vecinos se almacenan a medida que se bloquean los estados.

Contribuciones Clave

1. Primer SWF Probabilístico para CR: Este trabajo presenta el primer filtro de ventana deslizante estocástico diseñado específicamente para robots continuos, permitiendo la estimación de formas de curvatura variable tanto en el espacio como en el tiempo.
2. Operación en Tiempo Continuo en Línea: El método permite que la estimación de estado en tiempo continuo se ejecute en línea, superando la restricción fuera de línea de los métodos anteriores de tiempo continuo por lotes.
3. Rendimiento Equilibrado: Logra un compromiso entre la eficiencia computacional del filtrado y la precisión del suavizado por lotes, operando a velocidades más rápidas que el tiempo real mientras mantiene representaciones estocásticas de incertidumbre.
4. Disponibilidad de Código Abierto: Se proporciona una implementación de código abierto a la comunidad para facilitar la investigación futura.

Resultados
El método se validó en un robot continuo real accionado por tendones e impreso en 3D a través de cinco trayectorias distintas (incluyendo movimiento en espacio libre, contacto rápido e interacciones por impulso).

• Precisión: El SWF (con una ventana de 0.1s) superó significativamente a la línea base de filtro (ventana de 0s) en términos de Error Cuadrático Medio (RMSE) de Posición y Rotación de la Punta. Por ejemplo, en la trayectoria "Contacto Rápido", el SWF redujo el RMSE de posición del 1.75% al 1.29%.
• Comparación con Lotes: El SWF logró una precisión comparable a la línea base de optimización por lotes completa de 10s (por ejemplo, 1.29% frente a 1.29% de RMSE en Contacto Rápido), pero con un costo computacional drásticamente reducido.
• Eficiencia Computacional: El SWF operó a aproximadamente 10ms por paso de tiempo (100Hz), muy dentro de las restricciones de tiempo real (umbral de 33.3ms), mientras que el método por lotes requirió ~1.3 segundos por paso.
• Sensibilidad al Tamaño de Ventana: Los resultados indicaron rendimientos decrecientes en precisión para tamaños de ventana mayores a 0.1s. Cabe destacar que incluso ventanas muy pequeñas (0.033s) proporcionaron mejoras significativas en la precisión sobre el filtro estándar.
• Robustez: El filtro demostró la capacidad de rastrear el robot durante movimientos rápidos y recuperarse rápidamente de caídas temporales de sensores, con estimaciones de incertidumbre que se expandieron correctamente durante la pérdida de datos.

Significado y Afirmaciones
Los autores afirman que este trabajo representa una "dirección prometedora" para el campo al cerrar la brecha entre el filtrado y el suavizado. El significado principal radica en habilitar la estimación de estado estocástica en tiempo continuo y en línea para robots continuos, una capacidad previamente no disponible.

El artículo nota modestamente que, aunque el método introduce una pequeña latencia y una suavidad ligeramente reducida en comparación con los métodos completos por lotes, mantiene las propiedades de tiempo continuo del marco original. Los autores sugieren que el margen computacional restante (ejecutándose a >200Hz equivalente para la variante solo de filtro) podría soportar aplicaciones aguas abajo como controladores y planificadores. Reconocen que el rendimiento puede variar con diferentes configuraciones de sensores y perfiles de ruido, y que el aumento inesperado del RMSE en una trayectoria específica ("Fuera de Límites") con ventanas más grandes podría deberse a la degradación del sensor en los límites, lo que sugiere que los tamaños de ventana óptimos dependen del contexto.