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

Sintesi Tecnica: Un Filtro a Finestra Scorrevole per la Stima dello Stato in Tempo Continuo Online di Robot Continui

Enunciato del Problema
I robot continui (CR) sono manipolatori flessibili capaci di navigare in spazi confinati, il cui controllo dipende tuttavia da una stima accurata dello stato. I metodi probabilistici esistenti affrontano una dicotomia:

• I metodi di filtraggio (ad esempio, Filtri di Kalman Estesi) sono computazionalmente efficienti e adatti al funzionamento online, ma spesso soffrono di una ridotta accuratezza a causa di approssimazioni di linearizzazione che non riescono a catturare appieno la struttura di Markov dei sistemi non lineari.
• I metodi di smoothing batch offrono una superiorità di accuratezza ottimizzando congiuntamente tutti gli stati, ma sono tipicamente limitati al funzionamento offline, poiché richiedono l'accesso retrospettivo a tutte le misurazioni e non scalano bene per il controllo in tempo reale.
• Gli approcci in tempo continuo hanno mostrato promesse per i CR, ma sono stati finora limitati all'ottimizzazione batch offline. Inoltre, i tentativi esistenti di finestre scorrevoli per i CR si sono basati su modelli semplificati e non probabilistici (ad esempio, approssimazioni a curvatura costante o a link rigidi) o hanno mancato di rappresentazioni stocastiche per quantificare l'incertezza.

Metodologia
Gli autori propongono un Filtro a Finestra Scorrevole (SWF) progettato specificamente per la stima dello stato in tempo continuo online dei CR. Il metodo è costruito su un framework a grafo fattorizzato derivato da recenti tecniche di ottimizzazione batch.

• Framework di Stima: Il sistema stima la posa $T(s,t)$, la velocità $\varpi(s,t)$ e la deformazione $\epsilon(s,t)$ del robot in modo continuo lungo la lunghezza dell'arco $s$ e il tempo $t$. Il problema è formulato come una stima del Massimo a Posteriori (MAP), minimizzando il log-verosimiglianza negativa dei fattori a priori e di misurazione.
  • Priori: Utilizzano un modello approssimato di asta di Cosserat basato su un priori di moto "rumore bianco sull'accelerazione".
  • Misurazioni: Incorporano misurazioni della posa della punta e dati del giroscopio.
• Formulazione a Finestra Scorrevole: Il filtro mantiene una finestra temporale di lunghezza fissa contenente gli stati $x_{a:k}$.
  • Fattorizzazione: La posterior congiunta è decomposta in fattori a priori marginalizzati (che racchiudono la storia precedente alla finestra), fattori spaziali (lungo la lunghezza dell'arco del robot) e fattori temporali di moto/misurazione (all'interno della finestra).
  • Marginalizzazione: Per mantenere una dimensione fissa della finestra mentre si propaga l'informazione, il passo temporale più vecchio viene marginalizzato utilizzando il complemento di Schur in forma di informazione. Ciò crea un priori efficace per la nuova finestra, preservando la proprietà di Markov mentre mitiga gli errori intrinseci nelle assunzioni di Markov strette.
• Dettagli di Implementazione:
  • Espansione della Finestra: I nuovi stati sono inizializzati mediando una stima coerente con il priori spaziale e una stima coerente con il priori temporale.
  • Estrazione dello Stato: La media e la covarianza sono estratte dalla fine della finestra per utilizzare tutte le misurazioni disponibili, introducendo una piccola latenza (tipicamente $\leq 0.1$s).
  • Gestione della Covarianza: Per affrontare le discontinuità causate dalla ri-linearizzazione, le stime congiunte della covarianza tra passi temporali adiacenti vengono memorizzate man mano che gli stati vengono bloccati.

Contributi Chiave

1. Primo SWF Probabilistico per CR: Questo lavoro presenta il primo filtro a finestra scorrevole stocastico specificamente progettato per robot continui, abilitando la stima di forme a curvatura variabile sia nello spazio che nel tempo.
2. Funzionamento Online in Tempo Continuo: Il metodo abilita la stima dello stato in tempo continuo a funzionare online, superando la restrizione offline dei precedenti metodi batch in tempo continuo.
3. Prestazioni Bilanciate: Raggiunge un compromesso tra l'efficienza computazionale del filtraggio e l'accuratezza dello smoothing batch, operando a velocità superiori al tempo reale mantenendo al contempo rappresentazioni stocastiche dell'incertezza.
4. Disponibilità Open-Source: Viene fornita un'implementazione open-source alla comunità per facilitare ulteriori ricerche.

Risultati
Il metodo è stato validato su un robot continuo reale ad azionamento tendineo stampato in 3D su cinque traiettorie distinte (inclusi movimenti nello spazio libero, contatto rapido e interazioni a impulso).

• Accuratezza: Lo SWF (con una finestra di 0.1s) ha superato significativamente la baseline del filtro (finestra 0s) in termini di Errore Quadratico Medio (RMSE) di Posizione e Rotazione della Punta. Ad esempio, nella traiettoria "Contatto Rapido", lo SWF ha ridotto l'RMSE di posizione dal 1.75% al 1.29%.
• Confronto con il Batch: Lo SWF ha raggiunto un'accuratezza paragonabile alla baseline completa di ottimizzazione batch di 10s (ad esempio, 1.29% vs 1.29% RMSE nel Contatto Rapido) ma con un costo computazionale drasticamente ridotto.
• Efficienza Computazionale: Lo SWF ha operato a circa 10ms per passo temporale (100Hz), ben entro i vincoli di tempo reale (soglia di 33.3ms), mentre il metodo batch richiedeva ~1.3 secondi per passo.
• Sensibilità alla Dimensione della Finestra: I risultati hanno indicato rendimenti decrescenti in accuratezza per dimensioni della finestra superiori a 0.1s. Notabilmente, anche finestre molto piccole (0.033s) hanno fornito miglioramenti significativi di accuratezza rispetto al filtro standard.
• Robustezza: Il filtro ha dimostrato la capacità di tracciare il robot durante movimenti rapidi e di recuperare rapidamente da interruzioni temporanee dei sensori, con stime di incertezza che si espandevano correttamente durante la perdita di dati.

Significato e Affermazioni
Gli autori affermano che questo lavoro rappresenta una "direzione promettente" per il settore colmando il divario tra filtraggio e smoothing. Il significato principale risiede nell'abilitare la stima dello stato stocastica, in tempo continuo e online per i robot continui, una capacità precedentemente non disponibile.

Il documento nota con modestia che, sebbene il metodo introduca una piccola latenza e una lieve riduzione della fluidità rispetto ai metodi batch completi, mantiene le proprietà in tempo continuo del framework originale. Gli autori suggeriscono che la capacità computazionale residua (funzionando a >200Hz equivalente per la variante solo filtro) potrebbe supportare applicazioni a valle come controllori e pianificatori. Riconoscono che le prestazioni possono variare con diverse configurazioni di sensori e profili di rumore, e che l'aumento inaspettato dell'RMSE in una specifica traiettoria ("Fuori dai Limiti") con finestre più grandi potrebbe essere dovuto al degrado dei sensori ai confini, suggerendo che le dimensioni ottimali della finestra dipendono dal contesto.