RAY-TOLD: Ray-Based Latent Dynamics for Dense Dynamic Obstacle Avoidance with TDMPC

1. Enunciato del Problema

I robot mobili autonomi affrontano sfide significative quando navigano in folle dense e dinamiche. Gli approcci esistenti soffrono di limitazioni specifiche:

• Metodi Puramente Reattivi (es. MPPI): Il controllo Model Predictive Path Integral (MPPI) è robusto per la fattibilità cinematica a breve termine e l'evitamento delle collisioni. Tuttavia, si basa su un orizzonte di previsione limitato. In scenari complessi (es. ostacoli a forma di U o folle convergenti), MPPI rimane spesso intrappolato in minimi locali perché manca di consapevolezza spaziale a lungo termine e non può anticipare i comportamenti futuri degli ostacoli oltre la sua finestra di rollout breve.
• Apprendimento per Rinforzo Puro (RL): Sebbene il RL eccella nel ragionamento a lungo orizzonte, gli approcci end-to-end senza modello spesso faticano a imporre vincoli di sicurezza rigidi e soffrono di un'esplorazione inefficiente in sistemi fisici critici per la sicurezza.
• Il Divario: C'è la necessità di un'architettura ibrida che combini la sicurezza e i vincoli cinematici degli MPC basati sulla fisica con la visione a lungo termine dei modelli appresi per navigare in ambienti stocastici ad alta densità.

2. Metodologia: RAY-TOLD

Gli autori propongono RAY-TOLD (Ray-based Task-Oriented Latent Dynamics), un framework di controllo ibrido che integra dinamiche latenti centrate sul LiDAR con MPPI basato sul campionamento.

A. Architettura Core

Il sistema opera in due modalità parallele:

1. Pianificatore Basato sulla Fisica (MPPI): Gestisce i rollout di traiettoria a breve orizzonte ($H$) utilizzando un modello cinematico differenziabile a bicicletta per garantire la fattibilità cinematica immediata e l'evitamento delle collisioni.
2. Modello di Dinamiche Latenti (TOLD): Un modello del mondo appreso che codifica dati sensoriali ad alta dimensionalità in uno spazio latente compatto per fornire una guida a lungo orizzonte.

B. Componenti Chiave

• Rappresentazione dello Stato: Il vettore di stato di input $x_t$ include la cinematica del veicolo ($s_t$), la posizione relativa dell'obiettivo ($s_{goal}$) e le misurazioni LiDAR ($s_{ray}$). I dati LiDAR sono "consapevoli dell'occlusione", il che significa che i raggi vengono troncati al primo ostacolo colpito, simulando i vincoli reali della linea di vista.
• Codificatore Latente e Dinamiche:
  • Un codificatore $h_\theta$ comprime gli input ad alta dimensionalità in uno stato latente a bassa dimensionalità $z_t$.
  • Un modello di dinamiche latenti $d_\phi$ prevede le transizioni di stato nello spazio latente.
  • Un predittore di ricompensa $R_\psi$ e una Funzione di Valore Terminale $C_\omega$ sono addestrati per stimare rispettivamente le ricompense immediate e i ritorni a orizzonte infinito.
• Priorità della Politica ($\pi_\xi$): Una rete politica addestrata a proporre azioni orientate all'obiettivo all'interno dello spazio latente.

C. Strategia di Campionamento Misto della Politica

L'innovazione principale risiede nel modo in cui RAY-TOLD modifica il processo di campionamento MPPI per superare i minimi locali:

• MPPI Standard: Campiona azioni puramente da una distribuzione Gaussiana centrata sulla soluzione precedente (rumore non distorto).
• RAY-TOLD: Utilizza una strategia di campionamento misto. Una frazione $\alpha$ delle $K$ traiettorie candidate viene seminata con azioni derivate dalla priorità della politica appresa $\pi_\xi$ (srotolata nello spazio latente), mentre le restanti $(1-\alpha)$ sono campionate tramite il rumore MPPI standard.
  • Formula: $a^{(k)} \sim \mathcal{N}(a^\pi, \Sigma)$ se $k < \alpha K$, altrimenti $\mathcal{N}(a^{MPPI}, \Sigma)$.
• Valutazione: Il costo di ogni traiettoria è calcolato come la somma delle ricompense a breve orizzonte più un valore terminale appreso $C_\omega(z_{t+H})$, che stima il costo residuo oltre l'orizzonte di pianificazione.

3. Contributi Chiave

1. Architettura Ibrida: Integrazione fluida di dinamiche latenti centrate sul LiDAR con MPPI, permettendo al robot di navigare in folle dense e dinamiche sfruttando sia i vincoli basati sulla fisica che l'intento appreso.
2. Campionamento Misto della Politica: Una tecnica innovativa che arricchisce la popolazione di candidati MPPI con traiettorie di politica appresa. Questo elimina la vulnerabilità di MPPI ai minimi locali senza richiedere euristiche manuali.
3. Integrazione del Valore Terminale: L'uso di una funzione di valore terminale appresa permette al pianificatore di "guardare avanti" oltre l'orizzonte fisico di rollout, guidando efficacemente il robot fuori dalle strade senza uscita.
4. Validazione Empirica: Test estensivi in ambienti altamente stocastici che dimostrano riduzioni significative dei tassi di collisione rispetto alle baseline standard.

4. Risultati Sperimentali

Il metodo è stato valutato in un ambiente di 20m $\times$ 10m con 40–60 ostacoli dinamici in movimento secondo un Social Force Model (SFM) modificato.

• Prestazioni della Baseline: MPPI standard ha raggiunto un tasso di successo dell'89,0% con un tasso di collisione dell'11,0%. Ha fallito frequentemente in scenari che coinvolgono ostacoli convergenti o riflessioni sui muri a causa della scarsità dei campioni.
• Prestazioni di RAY-TOLD:
  • RAY-TOLD ($\alpha=0$): (Solo funzione di valore, nessuna guida della politica) Ha raggiunto lo stesso tasso di successo di MPPI ma ha migliorato il margine di sicurezza, validando la capacità della funzione di valore di valutare gli stati terminali.
  • RAY-TOLD ($\alpha=0.1$): (Mix Ottimale) Ha raggiunto il più alto tasso di successo del 94,0% e ridotto il tasso di collisione al 6,0% (un miglioramento di circa il 45% rispetto alla baseline). Questa configurazione ha bilanciato la guida a lungo orizzonte con la reattività a breve orizzonte.
  • RAY-TOLD ($\alpha=0.2$): (Guida Forte) Il tasso di successo è sceso leggermente al 91,0%, suggerendo che un'eccessiva dipendenza dalla priorità della politica può introdurre distorsioni in casi limite altamente stocastici.
• Analisi Qualitativa:
  • Fig. 3: Le visualizzazioni mostrano RAY-TOLD che naviga con successo ostacoli convergenti e anticipa le riflessioni sui muri (dove MPPI falliva).
  • Fig. 4 (t-SNE): La visualizzazione dello spazio latente rivela una transizione topologica fluida dagli stati "pericolosi" (rosso) agli stati "sicuri" (blu), dimostrando che il modello impara rappresentazioni fisicamente significative del rischio di collisione piuttosto che memorizzare dati sensoriali grezzi.

5. Significato

RAY-TOLD rappresenta un avanzamento significativo nella navigazione robotica risolvendo il problema dei "minimi locali" intrinseco nei pianificatori reattivi senza sacrificare la sicurezza.

• Sicurezza e Affidabilità: Radicando la ricerca nei rollout basati sulla fisica mentre inietta un'intenzione a lungo termine appresa, il sistema garantisce la fattibilità cinematica evitando decisioni miopi.
• Generalizzazione: L'uso di un modello del mondo latente permette al robot di generalizzare a configurazioni di ostacoli non viste e comportamenti dinamici (es. riflessioni) piuttosto che memorizzare percorsi specifici.
• Praticità: Il metodo opera a 50 Hz in una simulazione parallelizzata, dimostrando la fattibilità per il dispiegamento in tempo reale in ambienti centrati sull'uomo.

In conclusione, RAY-TOLD colma efficacemente il divario tra la robustezza del Controllo Predittivo Modello e la visione a lungo termine dell'Apprendimento per Rinforzo, offrendo una soluzione robusta per la navigazione autonoma negli ambienti più impegnativi, densi e dinamici.