Riepilogo Tecnico: Terminal Matters: Pianificazione Cinodinamica con Costo Terminale e Incertezza Appresa nello Spazio Stato-Costo di Credenza

Enunciato del Problema

I sistemi robotici che operano in ambienti reali devono generare movimenti dinamicamente fattibili che soddisfino i vincoli cinodinamici, tenendo conto dell'incertezza esecutiva derivante da errori di modellazione, rumore di percezione e interazioni di contatto complesse. Sebbene i pianificatori cinodinamici basati sul campionamento esistenti (ad es. AO-RRT, SST) offrano completezza probabilistica e ottimalità asintotica, essi ottimizzano tipicamente solo il costo di esecuzione (accumulato lungo la traiettoria, come lunghezza del percorso o energia) e trattano il raggiungimento dell'obiettivo meramente come un controllo di fattibilità.

Questo approccio presenta due limitazioni critiche:

1. Qualità dello Stato Terminale: Molti compiti dipendono dalla qualità dello stato finale, non solo dal percorso seguito. Ad esempio, nella spinta planare, un percorso più breve potrebbe risultare in un'alta incertezza terminale, riducendo la probabilità di raggiungere con successo l'obiettivo. In scenari multi-obiettivo (ad es. parcheggio di automobili), i pianificatori potrebbero selezionare un obiettivo geometricamente più vicino che è meno desiderabile o più difficile da abbandonare, ignorando le preferenze per specifiche regioni terminali.
2. Propagazione dell'Incertezza: La pianificazione nello spazio di credenza (dove lo stato è una distribuzione di probabilità) è essenziale per la robustezza. Tuttavia, i metodi esistenti spesso si affidano a soglie di probabilità rigide o a surrogati a livello di traiettoria piuttosto che ottimizzare esplicitamente lo stato terminale di credenza. Inoltre, sistemi complessi (come la spinta ricca di contatti) spesso mancano di modelli analitici di incertezza, rendendo necessari approcci basati sui dati che raramente sono integrati strettamente con i pianificatori nello spazio di credenza.

Metodologia

Gli autori propongono KiTe (Pianificazione Cinodinamica con Costo Terminale), un framework che potenzia l'Albero Casuale Rapido di Esplorazione Asintoticamente Ottimale (AO-RRT) con un obiettivo esplicito di costo terminale. La metodologia opera su tre livelli principali:

1. KiTe con Costo Terminale nello Spazio degli Stati

L'algoritmo centrale riformula il problema di pianificazione cinodinamica ottimale in uno spazio stato-costore accresciuto $\mathcal{Y} = \mathcal{X} \times \mathbb{R}^+$.

• Dinamica Accresciuta: Lo stato è esteso da $x$ a $y = (x, c)$, dove $c$ accumula il costo di esecuzione $\ell(x, u)$. Le dinamiche sono estese per includere il tasso di costo: $\dot{y} = (f(x, u), \ell(x, u))$.
• Integrazione del Costo Terminale: A differenza delle formulazioni precedenti in cui il costo terminale viene ignorato durante la ricerca, KiTe valuta il costo totale $C_T = \int \ell dt + \phi(x(T))$ solo al momento del raggiungimento della regione obiettivo.
• Strategia di Ricerca: L'algoritmo mantiene un limite di costo $c_{best}$. Quando viene trovata una soluzione, il limite viene aggiornato a $c_{new} + \phi(x_{new})$. La strategia di campionamento si concentra quindi sul sottoinsieme di livello definito da questo limite, potendo i rami che non possono migliorare il costo totale (esecuzione + terminale).
• Garanzia Teorica: Gli autori forniscono una prova rigorosa che AO-RRT con un costo terminale preserva l'ottimalità asintotica, a condizione che le dinamiche del sistema, il costo di esecuzione e il costo terminale soddisfino condizioni di continuità di Lipschitz.

2. Estensione allo Spazio di Credenza

La formulazione è estesa allo spazio stato-costore di credenza $\mathcal{Y}_b = \mathcal{B} \times \mathbb{R}^+$, dove $\mathcal{B}$ è lo spazio delle distribuzioni di probabilità sullo stato.

• Metrica: La distanza di Wasserstein-2 è adottata come metrica per lo spazio di credenza, poiché cattura naturalmente i cambiamenti sia nella media che nella covarianza delle credenze gaussiane.
• Limite Inferiore per il Raggiungimento dell'Obiettivo: Un contributo teorico chiave è la dimostrazione che la probabilità di raggiungere una regione obiettivo $G$ (definita come una sfera di raggio $r$) è limitata inferiormente dalla distanza di Wasserstein tra la credenza terminale $b(T)$ e la misura di Dirac sull'obiettivo $\nu_g$:
  $$\text{Pr}(x(T) \in G) \geq 1 - \frac{W_2^2(b(T), \nu_g)}{r^2}$$
  Ciò permette al pianificatore di utilizzare la distanza di Wasserstein come costo terminale $\psi(b(T))$, ottimizzando direttamente un limite inferiore per il successo del raggiungimento dell'obiettivo senza richiedere soglie di probabilità rigide e sintonizzate manualmente.
• Continuità di Lipschitz: Gli autori dimostrano che, sotto ipotesi lievi sulle dinamiche stocastiche (specificamente riguardo alla continuità di Lipschitz dell'incertezza di processo e dell'iniezione di rumore), le dinamiche di credenza e le funzioni di costo rimangono continue di Lipschitz rispetto alla metrica di Wasserstein. Ciò garantisce che l'ottimalità asintotica di KiTe si estenda allo spazio di credenza.

3. Apprendimento delle Dinamiche di Credenza

Per affrontare sistemi in cui i modelli analitici di incertezza non sono disponibili (ad es. spinta ricca di contatti), KiTe integra un modello appreso di dinamiche di credenza.

• Architettura del Modello: Una rete neurale (MLP) viene addestrata per prevedere la transizione stocastica locale. Dato uno stato $x_i$ e un controllo $u_i$, la rete produce una transizione media $\Delta x_i$ e un parametro di covarianza di processo parametrizzato da un vettore di varianze $\sigma_i^2$.
• Obiettivo di Addestramento: Il modello viene addestrato utilizzando la funzione di perdita Negative Log-Likelihood (NLL) su dati di interazione. Ciò permette alla rete di apprendere sia le dinamiche medie che l'incertezza aleatoria (rumore di processo) direttamente dai dati.
• Propagazione: La media e la covarianza apprese sono utilizzate per propagare lo stato di credenza (media e covarianza) in avanti nel pianificatore, supportando sia spazi di stato euclidei che di gruppo di Lie (ad es. SE(2)).

Contributi Chiave

1. Estensione Algoritmica e Dimostrazione: Implementazione di AO-RRT con un costo terminale esplicito, accompagnata da una prova rigorosa che l'ottimalità asintotica è preservata sia nello spazio degli stati che in quello di credenza.
2. Collegamento Teorico alla Probabilità di Successo: Dimostrazione che la minimizzazione della distanza di Wasserstein tra la credenza terminale e l'obiettivo induce un limite inferiore sulla probabilità di successo nel raggiungimento dell'obiettivo, fornendo un obiettivo terminale fondato per l'affidabilità.
3. Propagazione di Credenza Basata sui Dati: Dimostrazione di come apprendere modelli di dinamiche di credenza (media e incertezza) direttamente dai dati di interazione utilizzando la perdita NLL e integrarli nel framework KiTe per sistemi privi di modelli analitici.
4. Validazione Empirica: Valutazione completa su tre compiti diversificati (Flappy Bird, Parcheggio Auto, Spinta Planare) sia in simulazione che nel mondo reale, confrontando KiTe con baseline all'avanguardia (AO-RRT, SST, Gaussian Belief Tree).

Risultati Sperimentali

Il documento convalida KiTe su tre compiti:

• Flappy Bird (Deterministico): Dimostra che KiTe può codificare preferenze per l'obiettivo (ad es. centrare l'uccellino nel varco) ottimizzando il costo terminale, mentre le baseline smettono di affinare una volta raggiunto la regione obiettivo. KiTe raggiunge traiettorie più vicine al centro dell'obiettivo con un modesto aumento del costo di esecuzione.
• Parcheggio Auto (SE(2) con Incertezza): In un compito con molteplici posti auto fattibili, KiTe prioritizza con successo una regione obiettivo "preferita" (posto anteriore) e migliora i tassi di successo nel raggiungimento dell'obiettivo rispetto alle baseline che ottimizzano solo la lunghezza del percorso o utilizzano vincoli probabilistici. La pianificazione nello spazio di credenza con costo terminale ha prodotto i tassi di successo esecutivo più elevati.
• Spinta Planare (Ricco di Contatti, Mondo Reale): Utilizzando dinamiche apprese per oggetti come un Camion della Spazzatura e una Scatola di Biscotti, KiTe ha superato le baseline (inclusa la Pianificazione Attiva con incertezza del modello) nei tassi di successo esecutivo. Il pianificatore ha selezionato percorsi geometrici più lunghi che hanno risultato in un'incertezza terminale inferiore e probabilità di successo più elevate, dimostrando il valore dell'ottimizzazione della qualità della credenza terminale.

Significato e Affermazioni

Il documento afferma che KiTe affronta una lacuna fondamentale nella pianificazione cinodinamica: la separazione dell'ottimizzazione della traiettoria dalla qualità dello stato terminale. Ottimizzando esplicitamente lo stato terminale, il metodo:

• Migliora l'Affidabilità: Aumenta direttamente la probabilità di raggiungere l'obiettivo in condizioni di incertezza minimizzando la distanza di Wasserstein verso l'obiettivo.
• Codifica le Preferenze: Permette ai pianificatori di distinguere tra obiettivi multipli fattibili in base alla desiderabilità (ad es. facilità di partenza, stabilità) piuttosto che alla sola prossimità geometrica.
• Collega Apprendimento e Pianificazione: Fornisce un framework in cui i modelli di incertezza appresi non sono utilizzati solo per influenzare il campionamento, ma sono strettamente integrati nel ciclo di propagazione della credenza e di ottimizzazione.

Gli autori concludono che, sebbene il metodo introduca parametri di progettazione (pesi terminali) e si basi su modelli appresi, offre un approccio fondato e teoricamente solido per generare movimenti che non sono solo dinamicamente fattibili, ma anche affidabili e consapevoli delle preferenze in ambienti incerti.