Proprioceptive Invariant State Estimation for Humanoid Robots on Non-Inertial Ground

Sintesi Tecnica: Stima dello Stato Invariante Propriocezione per Robot Umanoidi su Terreno Non Inerziale

Definizione del Problema
La stima accurata e in tempo reale dello stato è fondamentale per la pianificazione e il controllo basati su modello dei robot leggati. Sebbene gli stimatori esistenti, come gli Invariant Extended Kalman Filters (InEKF), abbiano ottenuto prestazioni elevate su terreni statici sfruttando la dinamica group-affine e le condizioni di osservazione invarianti, essi si basano sull'assunto che il terreno di supporto sia stazionario. Questo assunto fallisce in ambienti non inerziali (ad esempio, treni, navi o aeroplani in movimento), dove i moti traslazionali e rotazionali indotti dal terreno introducono significative non linearità nelle dinamiche di sistema e nei modelli di misura. Ignorare tali moti porta a errori di modellazione che degradano l'accuratezza della stima.

Gli approcci attuali alla stima dello stato non inerziale affrontano limitazioni pratiche:

1. Stima dello Stato Assoluto: I metodi che stimano lo stato assoluto del robot in un sistema di riferimento inerziale richiedono spesso la conoscenza diretta e accurata della posa e della velocità assoluta del terreno, informazione che raramente è disponibile senza strumentazione esterna.
2. Stima dello Stato Relativo: I metodi recenti che stimano lo stato del robot rispetto a un frame di terreno mobile richiedono tipicamente un IMU esterno montato fisicamente sul terreno per misurarne il moto. Ciò è spesso impraticabile nelle implementazioni nel mondo reale a causa di vincoli di strumentazione, ritardi di comunicazione e perdita di segnale in ambienti non strutturati.
3. Metodi Basati sull'Apprendimento: Sebbene promettenti, gli stimatori basati sull'apprendimento richiedono spesso ingenti dati di addestramento e la loro capacità di generalizzazione in ambienti non inerziali rimane incerta.

Esiste la necessità di uno stimatore completamente propriocezione capace di tracciare accuratamente il moto del robot rispetto a un terreno in movimento senza fare affidamento su sensori esterni o misurazioni esplicite del moto del terreno.

Metodologia
Il documento propone un Proprioceptive Invariant Extended Kalman Filter (InEKF) progettato per stimare la posizione, l'orientamento e la velocità della base del robot rispetto a un frame di terreno dinamico utilizzando solo sensori onboard (encoder di giunto e IMU montati sul torso e sui piedi).

• Rappresentazione dello Stato: Il filtro stima l'orientamento ($R_B^D$), la posizione ($p_t^D$) e la velocità ($v_t^D$) relativi della base rispetto al frame di terreno dinamico $\{D\}$. Lo stato è incorporato in un gruppo di Lie $G$, composto da una doppia isometria spaziale $SE_3(3)$ e dal gruppo speciale ortogonale $SO(3)$.
• Modellazione del Processo:
  • Il filtro deriva le dinamiche in tempo continuo per l'orientamento, la velocità e la posizione relativa della base.
  • Crucialmente, elimina la dipendenza dalle misurazioni dirette del moto del terreno (che non sono disponibili) sfruttando le vincoli cinematici al piede in appoggio. Utilizzando le misure provenienti da un IMU montato sul piede, il filtro calcola esplicitamente le accelerazioni e le velocità angolari indotte dal terreno necessarie per il modello di processo.
  • Il modello tiene conto degli effetti non lineari indotti dal moto del terreno e tratta lo scivolamento del piede come rumore gaussiano bianco.
  • Gli eventi discreti, come l'atterraggio del piede e il cambio di appoggio, sono gestiti tramite un passaggio di propagazione discreta che aggiorna la posa del piede in appoggio utilizzando la cinematica diretta, mantenendo la continuità nello stato della base.
• Modellazione della Misura:
  • Il filtro utilizza l'odometria delle gambe derivata dagli encoder di giunto come sorgente di misura.
  • Il modello di misura è formulato per soddisfare una condizione di osservazione destra-invariante. Questa struttura assicura che la dinamica dell'errore sia lineare e indipendente dalla traiettoria nel caso deterministico, promuovendo una convergenza robusta anche sotto grandi incertezze iniziali.
• Analisi dell'Osservabilità:
  • Un'analisi di osservabilità locale delle dinamiche dell'errore logaritmico linearizzato rivela che la velocità relativa della base è sempre osservabile.
  • La posizione relativa della base è osservabile sotto moti del terreno generici, a condizione che l'IMU del piede in appoggio rilevi il moto angolare su almeno due assi.
  • L'analisi indica che l'orientamento della base è non osservabile sotto una configurazione di sensing nominale (un IMU sul torso e un IMU sul piede). Tuttavia, dimostra che aumentando la configurazione di sensing con molteplici IMU non collineari sul torso è possibile ricostruire l'orientamento della base, ottenendo così l'osservabilità completa dello stato.

Contributi Chiave

1. Formulazione InEKF Completamente Propriocezione: Un nuovo InEKF che stima la posa e la velocità della base di un robot umanoide rispetto a un frame di terreno non inerziale utilizzando solo encoder di giunto e IMU montati su torso e piedi, eliminando la necessità di sensori del terreno esterni.
2. Modellazione Esplicita delle Non Linearità: I modelli di processo e di misura tengono esplicitamente conto delle non linearità indotte dal moto del terreno. Il modello di misura soddisfa la condizione di osservazione destra-invariante, consentendo dinamiche di errore favorevoli sotto grandi errori iniziali.
3. Caratterizzazione dell'Osservabilità: Un'analisi che stabilisce le condizioni in cui la posizione e la velocità relativa della base sono osservabili. Inoltre, propone una semplice strategia di aumento dei sensori (molteplici IMU sul torso) per recuperare l'osservabilità dell'orientamento della base.
4. Validazione Sperimentale: Esperimenti hardware sul robot umanoide Digit durante compiti di stazione eretta, squat e camminata su un tapis roulant oscillante e con beccheggio.

Risultati Sperimentali
Gli esperimenti sono stati condotti sul robot Digit in cinque casi di test che coinvolgono vari moti del robot (stazione eretta, squat, camminata) e moti del terreno (oscillazione, beccheggio). Il filtro proposto (denominato FRS) è stato confrontato con due baseline: SRS (uno standard InEKF per terreno statico) e DRS (un InEKF per terreno dinamico che richiede un IMU esterno montato sul terreno).

• Velocità di Convergenza: Il proposto FRS ha ottenuto un incremento della velocità di convergenza del 96% rispetto agli InEKF esistenti. Nello specifico, FRS è convergito alla verità di base entro 0,1 secondi per la stima della posizione, mentre la baseline DRS richiedeva 3–5 secondi.
• Accuratezza:
  • FRS ha dimostrato una riduzione dell'80% degli errori di stima della posizione rispetto agli InEKF esistenti.
  • In regime stazionario, FRS ha ottenuto errori di posizione inferiori a 4 cm per i compiti di stazione eretta e squat (Casi T1–T4), rispetto a circa 10 cm della baseline DRS.
  • Durante la camminata su un terreno con beccheggio (Caso T5), FRS ha mantenuto un errore di stima medio inferiore a 9 cm nonostante errori iniziali fino a 1 metro.
• Recupero dell'Orientamento: Gli esperimenti con tre IMU montati sul torso hanno ricostruito con successo l'orientamento della base con errori assoluti medi inferiori a 0,4° in rollio, pitch e yaw, validando la proposta strategia di aumento.
• Prestazioni in Tempo Reale: Tutti i filtri, incluso il metodo proposto, hanno supportato l'implementazione in tempo reale con tempi di calcolo inferiori a 3 ms per ciclo su un laptop standard.

Significatività e Rivendicazioni
Il documento afferma che l'approccio proposto consente una stima dello stato accurata e in tempo reale per i robot umanoidi in ambienti non inerziali senza il sensing esplicito del moto del terreno. Sfruttando gli IMU montati sui piedi per inferire la dinamica del terreno, il metodo supera i limiti pratici che richiederebbero strumentazione esterna. Gli autori sostengono che il filtro fornisca una soluzione robusta per compiti di pianificazione e controllo definiti rispetto a un ambiente in movimento, ottenendo una convergenza e un'accuratezza superiori rispetto alle baseline allo stato dell'arte, anche sotto significative incertezze iniziali e grandi moti del terreno. Il lavoro valida che l'osservabilità completa dello stato può essere raggiunta attraverso un semplice aumento dei sensori, rendendo l'approccio utilizzabile per il deployment in ambienti dinamici e non strutturati.