Safe Learning Predictive Control for Ego-World Robotic Systems

Riepilogo Tecnico: SOWL-MPC (Safe Online World Policy Learning Model Predictive Control)

Definizione del Problema
Il documento affronta la sfida della navigazione autonoma sicura per un robot "ego" che opera in un ambiente condiviso con un robot "world" la cui politica di controllo è sconosciuta e potenzialmente non stazionaria. Questo scenario, denominato framework EGO–WORLD, presuppone che il robot ego abbia accesso solo a misurazioni di stato rumorose del robot world ($\tilde{z}_t = z_t + w_t$) e una conoscenza pregressa della dinamica nominale del robot world (vettore di deriva $g(z)$ e matrice di controllo $H(z)$), ma manchi della conoscenza della reale politica di controllo $\pi(z)$ del robot world. I metodi reattivi tradizionali (ad esempio, Dynamic Window Approach, Control Barrier Functions) sono spesso miopi, mentre gli approoli predittivi esistenti si basano tipicamente su modelli addestrati offline o assunzioni di moto fisse che non riescono ad adattarsi a comportamenti imprevisti. Il problema centrale è progettare un framework di controllo online capace di apprendere iterativamente la politica latente del mondo attraverso dati in streaming e utilizzare tale apprendimento per eseguire manovre sicure e prive di collisioni tramite Model Predictive Control (MPC).

Metodologia
La soluzione proposta, SOWL-MPC, integra l'apprendimento online con uno schema di controllo a orizzonte scorrevole attraverso tre fasi principali:

1. Modellazione della Politica e Pre-addestramento Offline:
   La politica ignota del mondo $\pi(z)$ è modellata come un insieme di processi gaussiani sparse variazionali (SVGP) indipendenti, uno per ogni componente del vettore di politica. Inizialmente, gli iperparametri vengono selezionati tramite pre-addestramento offline su un dataset di coppie stato-input rumorose. I dati di input (input di controllo del mondo) vengono ricostruiti online dalle consecutive osservazioni di stato rumorose utilizzando un approccio di minimi quadrati basato sulla dinamica nominale nota.

2. Apprendimento Online tramite Online Variational Conditioning (OVC):
   Per adattarsi a nuovi dati in tempo reale senza l'oneroso costo computazionale del ri-addestramento, gli autori impiegano la Online Variational Conditioning (OVC). Invezione di memorizzare l'intera cronologia dei dati, l'OVC mantiene statistiche sufficienti (posizioni inducenti, medie variazionali e covarianze) per aggiornare ricorsivamente i parametri SVGP. L'algoritmo proietta la distribuzione posteriore corrente in "pseudo-dati", li combina con la nuova osservazione e aggiorna i punti inducenti e i parametri variazionali. Ciò consente al sistema di apprendere la politica latente direttamente da misurazioni rumorose in streaming.

3. Previsione della Traiettoria e Controllo Consapevoli dell'Incertezza:
   La politica appresa viene propagata attraverso le dinamiche non lineari del mondo usando uno schema di propagazione dei momenti gaussiani del primo ordine (analogo a un passo di predizione di un Filtro di Kalman Esteso). Questo genera una previsione probabilistica multi-step della traiettoria futura del robot world, includendo una misura dell'incertezza predittiva.
   Queste previsioni vengono fornite a un controllore MPC non lineare. La formulazione MPC include:

   • Funzione di Costo: Un costo di inseguimento standard con penalità per lo sforzo di controllo e la fluidità dell'input.
   • Vincoli di Sicurezza: L'evitamento delle collisioni è garantito dall'inflazione della distanza di sicurezza tra i robot. Il margine di sicurezza è regolato dinamicamente in base all'incertezza predittiva (traccia della matrice di covarianza) scalata da un parametro di sicurezza $n_\sigma$ definito dall'utente. Sono applicati vincoli rigidi al passo immediatamente successivo, mentre variabili di scorrimento (slack variables) consentono un rilassamento temporaneo nell'orizzonte di predizione successivo per garantire la fattibilità.

Contributi Chiave
Il documento delinea tre contributi primari:

1. Framework SOWL-MPC: Una nuova strategia di controllo per sistemi EGO–WORLD che apprende la politica di feedback stato-input latente di un robot world non cooperativo online, anziché apprendere la dinamica del sistema o fare affidamento su predittori offline. Questa formulazione sfrutta la minore dimensionalità dello spazio di input rispetto allo spazio di stato per l'efficienza dei dati.
2. Applicazione di OVC alla Robotica: La prima applicazione della Online Variational Conditioning all'identificazione della politica online e alla previsione della traiettoria in sistemi robotici. Ciò consente ai modelli SVGP di aggiornarsi continuamente su dati in streaming con complessità computazionale fissa ($O(M^3)$ per aggiornamento, dove $M$ è il numero di punti inducenti).
3. Validazione in Tempo Reale: Un'implementazione full-stack in ROS 2 (integrata con il toolbox CHOIRBOT) che dimostra la fattibilità in tempo reale. L'approccio è stato validato attraverso estesi esperimenti virtuali Monte Carlo e test su hardware reale coinvolgenti manovre di sorpasso e attraversamento.

Risultati Sperimentali
La strategia è stata valutata sia in ambienti virtuali (Webots/ROS 2) che reali (arena interna con auto NVIDIA JetRacer):

• Accuratezza della Predizione: In esperimenti virtuali sulle piste ETHZ MOBIL e LEMNISCATE, SOWL-MPC ha superato significativamente un baseline GP addestrato offline e un baseline a Velocità Costante (CV). L'Average Displacement Error (ADE) per SOWL-MPC si è stabilizzato a circa 0,05 m dopo la fase iniziale di apprendimento, rispetto a errori superiori a 0,4 m per il baseline CV e picchi periodici sopra 0,7 m per il baseline offline.
• Garanzie di Sicurezza: Simulazioni Monte Carlo hanno dimostrato che la calibrazione del parametro di sicurezza $n_\sigma$ permette un compromesso tra aggressività e sicurezza. Con $n_\sigma = 3$, il metodo ha ottenuto un tasso di successo del 100% nell'evitare collisioni in 50 prove, mentre un approccio deterministico ($n_\sigma = 0$) è fallito nel 36% delle prove.
• Prestazioni in Tempo Reale: Il tempo di calcolo medio per passo di controllo è rimasto ben al di sotto del tempo di campionamento di 50 ms anche per orizzonti di predizione fino a $T=40$, confermando la fattibilità in tempo reale.
• Validazione su Hardware Reale: Su hardware fisico, il sistema ha eseguito con successo manovre di evitamento anticipatorio, con l'ADE che si è stabilizzato intorno a 0,05 m, confermando la trasferibilità dei risultati virtuali alle dinamiche del mondo reale.

Significatività e Rivendicazioni
Il documento sostiene che SOWL-MPC rappresenti un avanzamento significativo colmando il divario tra sicurezza reattiva e pianificazione predittiva in ambienti con agenti sconosciuti e adattivi. Modellando ed apprendendo esplicitamente la politica del robot world piuttosto che solo la sua dinamica, e utilizzando l'OVC per aggiornamenti online efficienti, il framework raggiunge garanzie di sicurezza che si adattano al comportamento osservato del robot world. Gli autori sottolineano che questa è la prima applicazione dell'OVC a questo specifico problema robotico, consentendo l'apprendimento efficiente dei dati di politiche latenti che possono essere integrate direttamente in loop di controllo consapevoli dell'incertezza. Il lavoro dimostra che una navigazione anticipatoria sicura è raggiungibile anche quando la logica di controllo del robot world è completamente sconosciuta e sono disponibili solo misurazioni di stato rumorose.