Riepilogo Tecnico: Un Benchmark Solo Propriocettivo per la Stima dello Stato di Quadrupedi

Enunciato del Problema

Una stima dello stato propriocettiva affidabile è fondamentale per l'andatura agile dei quadrupedi, in particolare in ambienti in cui la sensoristica esteroceptiva (ad esempio, visiva, LiDAR, GPS) è inaffidabile o non disponibile a causa di polvere, nebbia, corridoi privi di caratteristiche o interruzioni del segnale. In tali scenari, i robot devono fare affidamento esclusivamente su Unità di Misura Inerziale (IMU), encoder articolari e informazioni di contatto per stimare il movimento del corpo. La sfida principale consiste nel bilanciare due requisiti in competizione:

1. Coerenza a lungo termine: Mantenere l'accuratezza su traiettorie estese nonostante la deriva e gli errori di modellazione.
2. Fedeltà a breve orizzonte: Preservare l'accuratezza locale essenziale per il controllo in retroazione e la pianificazione dell'appoggio, nel rispetto di vincoli rigorosi di latenza e budget computazionale su piattaforme embedded.

Sebbene esistano diversi approcci, manca un confronto diretto, "mela con mela", tra paradigmi all'avanguardia diversi (filtraggio vs. smoothing, invariante vs. non invariante) su un dataset reale comune, per guidare i praticanti nella selezione dello stimatore appropriato per i vincoli specifici della loro applicazione.

Metodologia

Gli autori presentano un benchmark comparativo di tre stimatori di stato open-source rappresentativi per robot quadrupedi, tutti operanti in modalità solo propriocettiva sulla sequenza CYN-1 del Dataset GrandTour. Tutti i metodi stimano la posa del sistema a base flottante $(R, p) \in SO(3) \times \mathbb{R}^3$ e la velocità del corpo $v \in \mathbb{R}^3$.

1. MUSE (Stimatore di Stato Unificato Multi-sensore)

• Paradigma: Stack di fusione modulare in tempo reale (Filtraggio).
• Meccanismo: In questo studio, MUSE opera in modo puramente propriocettivo, disabilitando i blocchi esteroceptivi. Utilizza un Osservatore Non Lineare (NLO) a cascata e un Filtro di Kalman Esogeno (XKF) per la stima dell'assetto, garantendo stabilità globale. L'odometria delle zampe è derivata dalla cinematica diretta delle zampe in contatto stabile. Queste misurazioni sono fuse tramite un Filtro di Kalman per produrre posa e twist.
• Caratteristica Chiave: Progettato per robustezza e prestazioni in tempo reale, capace di gestire grandi errori iniziali di orientamento grazie al suo osservatore di assetto globalmente stabile.

2. Filtro di Kalman Esteso Invariante (IEKF)

• Paradigma: Filtraggio Invariante.
• Meccanismo: Formulato su un sistema gruppo-affine dove le dinamiche dell'errore invariante a destra sono indipendenti dallo stato. Questa struttura migliora la convergenza e la coerenza, in particolare durante gli eventi di contatto.
• Dettagli del Meccanismo: La propagazione integra i dati IMU, mentre gli aggiornamenti assumono contatto stabile (velocità del piede zero). Sfrutta strutture di gruppo di Lie per linearizzare le dinamiche dell'errore, incorporando costi di misurazione robusti e consapevolezza dello scivolamento.

3. Smoother Invariante (IS)

• Paradigma: Smoothing a ritardo fisso (Massima A Posteriori).
• Meccanismo: A differenza dell'IEKF, che aggiorna solo lo stato più recente, l'IS ottimizza su una finestra fissa di stati ($n$) e misurazioni. Formula il problema come un problema di minimi quadrati non lineare utilizzando residui consapevoli della struttura adattati dalle proprietà gruppo-affini dell'IEKF.
• Caratteristica Chiave: Ottimizzando su una finestra, raggiunge una migliore tolleranza ai contatti dinamici (ad esempio, scivolamenti) e una maggiore accuratezza, ma a scapito di un carico computazionale aumentato e di una latenza algoritmica proporzionale alla dimensione della finestra.

Metriche di Valutazione

Lo studio impiega metriche standard calcolate tramite il pacchetto Python evo:

• Errore di Traiettoria Assoluto (ATE): Misura l'accuratezza globale della posa e la deriva accumulata allineando la traiettoria stimata alla verità fondamentale tramite allineamento Umeyama SE(3).
• Errore di Posa Relativo (RPE): Misura l'accuratezza a breve orizzonte. Valutato secondo due schemi:
  • Finestra spaziale ($\Delta = 1$ m): Esplora la deriva su scala metrica.
  • Finestra temporale ($\Delta = 1$ frame): Esplora la coerenza frame-to-frame e la sensibilità al jitter delle misurazioni.
• RMSE della Velocità: Valuta l'accuratezza dei profili di velocità stimati.
• Tempo di Esecuzione: Tempo di calcolo per aggiornamento misurato su uno stack hardware/software fisso.

Risultati Chiave

Accuratezza

• Lungo termine (ATE): Sia l'IEKF che lo Smoother Invariante (IS) hanno ottenuto un ATE inferiore rispetto a MUSE. La differenza tra il migliore (IS) e il peggiore (MUSE) è stata di circa 0,91 m su una traiettoria di 300 m (circa 0,3%). Ciò suggerisce che la modellazione gruppo-affine e la regolarizzazione dello smoothing aiutano a ridurre la deriva accumulata.
• Breve termine (RPE):
  • A $\Delta = 1$ m: IEKF e IS hanno superato significativamente MUSE (riduzione di circa il 40% dell'RPE traslazionale), indicando una migliore gestione degli errori sistematici che si accumulano su più aggiornamenti.
  • A $\Delta = 1$ frame: IEKF e MUSE hanno mostrato un RPE inferiore rispetto a IS (riduzione di circa il 72% e il 66%, rispettivamente). Ciò suggerisce che la fusione a frequenza più elevata nei filtri sopprime meglio il rumore ad alta frequenza rispetto all'approccio di smoothing in questa specifica metrica.
  • RPE Rotazionale: I risultati sono stati ampiamente simili tra tutti i metodi.
• Velocità: Tutti e tre i metodi hanno fornito profili di velocità accurati con piccole differenze di RMSE.

Tempo di Esecuzione e Latenza

• MUSE: Il più veloce, con un tempo di esecuzione medio di $\approx 0,012$ ms per iterazione.
• IEKF: Leggermente più lento, $\approx 0,02$ ms per iterazione.
• Smoother Invariante: Dimostra un chiaro compromesso accuratezza-latenza dipendente dalla dimensione della finestra (WS).
  • WS = 1: $\approx 0,110$ ms.
  • WS = 5: $\approx 0,583$ ms.
  • Nota: Lo smoothing a ritardo fisso introduce una latenza algoritmica proporzionale al ritardo, che può ritardare le stime dello stato a seconda dell'implementazione della pipeline.

Comportamento di Convergenza

Sotto grandi errori iniziali di orientamento (disallineamento di 180 gradi nel rollio), tutti e tre gli stimatori sono convergenti verso l'orientamento corretto. Tuttavia, MUSE ha convergito più velocemente rispetto a IEKF e IS. Ciò è attribuito alla progettazione dell'osservatore di assetto globalmente stabile di MUSE, mentre IEKF e IS si basano su linearizzazioni locali che possono essere violate da grandi errori iniziali, portando a una convergenza più lenta.

Significato e Contributi

Il documento fornisce un'istantanea concisa e riproducibile dei compromessi tra accuratezza, latenza e costo computazionale per sistemi quadrupedi deployabili. I suoi principali contributi sono:

1. Confronto Diretto: Una valutazione "mela con mela" di tre paradigmi di stima distinti e ad alte prestazioni (MUSE, IEKF, IS) su un dataset reale condiviso.
2. Guida per Pratici: Dati chiari su come diverse metriche (ATE vs. RPE a diverse scale) e vincoli computazionali (latenza vs. accuratezza) influenzino la selezione dello stimatore.
3. Riproducibilità Open-Source: Il rilascio di una pipeline di benchmarking completa, inclusi codice, documentazione e strumenti di elaborazione dei dati, per facilitare futuri benchmark standardizzati.

Gli autori concludono che, sebbene lo smoothing (IS) offra una superiore accuratezza a lungo termine, i filtri (MUSE, IEKF) offrono una latenza inferiore e potenzialmente una migliore soppressione del rumore a breve termine. La scelta dello stimatore dovrebbe essere dettata dai requisiti specifici dell'applicazione riguardanti la coerenza globale, la frequenza del ciclo di controllo e le risorse di calcolo disponibili. Lo studio non rivendica di risolvere tutte le sfide della stima dello stato, ma mira a chiarire il panorama attuale per i praticanti.