RK-MPC: Residual Koopman Model Predictive Control for Quadruped Locomotion in Offroad Environments

1. Il Problema

I robot quadrupedi che operano in ambienti non strutturati (fuoristrada) devono affrontare sfide significative legate all'incertezza dei contatti, alla variabilità del terreno e agli errori di modellazione.

• Limiti dei modelli fisici classici: I metodi di controllo basati su modelli fisici (come il MPC basato su dinamiche del corpo rigido singolo o SRB) sono robusti e gestiscono bene i vincoli, ma la loro efficacia dipende dalla fedeltà del modello. In terreni irregolari, con attriti variabili o impatti complessi, questi modelli semplificati soffrono di errori di previsione che degradano le prestazioni.
• Limiti dell'apprendimento puro: Gli approcci basati su Reinforcement Learning (RL) possono mostrare agilità, ma spesso mancano di garanzie di sicurezza, sono inefficienti dal punto di vista del campionamento dei dati e non forniscono una struttura predittiva esplicita per la gestione dei vincoli.
• Sfide dei metodi Koopman esistenti: L'uso diretto dell'operatore di Koopman (tramite EDMD - Extended Dynamic Mode Decomposition) per apprendere modelli lineari di sistemi non lineari ad alta dimensionalità (come i quadrupedi) presenta problemi di stabilità. I modelli basati su monomi puri tendono a divergere su orizzonti temporali lunghi, mentre le lifting basate su strutture geometriche (es. SE(3)) possono accumulare errori di rotazione (drift) che destabilizzano il controllo in tempo reale.

L'obiettivo è sviluppare un framework di controllo predittivo che sia data-driven (per adattarsi alle incertezze), strutturato (per mantenere la gestione dei vincoli del MPC) e computazionalmente efficiente per l'esecuzione in tempo reale su hardware.

2. Metodologia: RK-MPC

Gli autori propongono RK-MPC (Residual Koopman Model Predictive Control), un framework ibrido che combina un modello fisico nominale con un correttore residuo appreso dai dati.

A. Architettura Ibrida

Il sistema non sostituisce il modello fisico, ma lo potenzia:

1. Modello Nominale: Utilizza un modello di corpo rigido singolo (SRB) linearizzato e a tempo variante. Questo modello cattura la dinamica dominante e la struttura fisica di base (gravità, inerzia, cinematica del contatto).
2. Correttore Residuo Koopman: Viene appreso un modello lineare compatto che predice l'errore (residuo) tra lo stato reale misurato e la previsione del modello nominale.
   • Il residuo è definito principalmente sulle velocità (traslazionali e angolari): $e_k = [v_k - v^{nom}_k, \omega_k - \omega^{nom}_k]^T$.
   • Viene applicata una trasformazione di lifting (tramite polinomi di grado 2) allo stato del residuo per creare uno spazio osservabile in cui la dinamica è approssimativamente lineare.
   • L'operatore di Koopman (matrici $A_{res}, B_{res}, C_{res}$) viene identificato tramite EDMD con controllo (EDMDc) utilizzando dati raccolti in simulazione.

B. Formulazione del Controllo

Il correttore residuo è incorporato all'interno di un problema di ottimizzazione convessa (QP - Quadratic Program):

• Predizione: La dinamica predittiva nel MPC è data da: $\hat{x}_{k+1} = x^{nom}_{k+1} + \hat{e}_{k+1}$, dove il termine di correzione è calcolato linearmente dallo stato residuo elevato.
• Vincoli: Il problema mantiene i vincoli standard del MPC per i robot a zampe: coni di attrito (approssimati come piramidi lineari) e limiti di forza verticale.
• Efficienza: Poiché la dinamica del residuo è lineare e il modello nominale è lineare, l'intero problema di ottimizzazione rimane un QP convesso, risolvibile in tempo reale (500 Hz) su hardware embedded.

C. Garanzie Teoriche

Il paper fornisce un teorema che garantisce limiti sugli errori di previsione multi-step. Dimostra che, assumendo che la dinamica reale sia Lipschitziana e l'errore di approssimazione del residuo sia limitato, l'errore di previsione totale rimane limitato e cresce in modo controllato, a differenza dei modelli Koopman puri che possono divergere esponenzialmente.

3. Contributi Chiave

1. Modellazione Residua Koopman: Un framework che apprende una correzione lineare compatta su un modello fisico, garantendo coerenza geometrica e riducendo la sensibilità alla scelta delle osservabili rispetto ai metodi EDMD completi.
2. Formulazione Convessa RK-MPC: Un MPC che integra il predittore residuo mantenendo la struttura QP, permettendo un'esecuzione in tempo reale a 500 Hz su hardware reale.
3. Validazione Estensiva: Test completi in simulazione (Gazebo) e su hardware reale (Unitree Go1) che dimostrano la capacità di locomozione "alla cieca" (senza sensori esterni del terreno) su terreni complessi.

4. Risultati Sperimentali

A. Simulazione (Gazebo)

• Confronto Predittivo: Il modello RK-MPC ha mostrato errori di previsione (RMSE) significativamente inferiori rispetto a un EDMD basato su monomi (che divergeva) e a un EDMD basato su SE(3) (che soffriva di drift di rotazione).
• Tracking in Chiuso: In un compito di tracciamento di un percorso circolare di 10 giri, RK-MPC ha mantenuto una stabilità eccellente, paragonabile al MPC basato su modelli fisici non lineari (che è più lento da risolvere) e molto superiore all'MPC basato su SE(3) che falliva dopo pochi secondi a causa del drift di rotazione.
• Tempo di Calcolo: RK-MPC ha risolto il QP in media in 0.89 ms, più veloce sia del MPC non lineare (1.57 ms) che dell'approccio SE(3) (1.57 ms).

B. Validazione Hardware (Unitree Go1)

Il sistema è stato eseguito interamente a bordo su un computer NVIDIA Jetson Xavier NX a 500 Hz.

• Tracking di Velocità: Il robot ha seguito comandi di velocità planare variabili nel tempo su terreno piano con alta precisione.
• Robustezza a Disturbi:
  • Terreni con Detriti: Il robot ha attraversato con successo un campo di detriti (legno, ostacoli) mantenendo la stabilità, dimostrando che il correttore residuo compensa lo slittamento e le variazioni di altezza.
  • Spinte Esterne: Il robot è riuscito a recuperare l'equilibrio dopo spinte laterali e longitudinali ripetute senza cadere.
• Generalizzazione delle Andature: Sebbene addestrato solo con dati di andatura "Trot" (trotto), il controller ha funzionato stabilmente anche con l'andatura "Crawl" (strisciamento), dimostrando che il residuo appreso è trasferibile tra diversi schemi di contatto.
• Terreni Fuoristrada: Il robot ha operato con successo su erba, ghiaia, neve e ghiaccio. Su ghiaccio, ha mostrato di adattarsi allo slittamento aumentando la forza normale richiesta, mantenendo la stabilità dinamica.

5. Significato e Impatto

Il lavoro di RK-MPC rappresenta un passo avanti significativo nel controllo dei robot a zampe per ambienti non strutturati:

• Ponte tra Fisica e Dati: Offre una via pratica per integrare l'apprendimento dati nei controllori basati su ottimizzazione, superando i limiti dei modelli puramente fisici senza sacrificare la stabilità e la velocità di calcolo.
• Affidabilità Hardware: Dimostra che tecniche di apprendimento avanzate (Koopman) possono essere implementate con successo su robot reali in tempo reale, gestendo incertezze di contatto e variabilità del terreno senza bisogno di sensori esterni complessi (lidar/camere per la mappatura del terreno).
• Scalabilità: La struttura residua riduce la complessità dell'apprendimento rispetto all'apprendimento di un modello completo, rendendo il metodo più robusto ai cambiamenti di distribuzione e più efficiente dal punto di vista dei dati.

In sintesi, RK-MPC fornisce una soluzione pratica e validata sperimentalmente per il controllo predittivo di quadrupedi in ambienti ostili, combinando la robustezza della fisica con l'adattabilità dei dati.