A Terrain-Adaptive epsilon-Constraint MPC for Uneven Terrain Kinodynamic Planning

Sintesi Tecnica: Un Controllo Predittivo di Modello (MPC) con Vincolo-$\epsilon$ Adattivo al Terreno per la Pianificazione Cinematica e Dinamica su Terreni Irregolari

Enunciato del Problema
La navigazione autonoma per veicoli di tipo automobilistico in ambienti non strutturati e irregolari (ad esempio, cantieri edili, zone di disastro, esplorazione planetaria) presenta una duplice sfida: generare percorsi privi di collisioni rispettando al contempo vincoli cinematici e dinamici complessi. I pianificatori basati su mesh tradizionali, come quelli che utilizzano il Metodo di Marcia Veloce (FMM), eccellono nel produrre percorsi geometrici consapevoli del terreno, ma spesso non riescono a incorporare la fattibilità dinamica, come i limiti di velocità e la stabilità del veicolo. Al contrario, i pianificatori cinematici e dinamici faticano frequentemente a bilanciare obiettivi in competizione, in particolare l'efficienza del percorso rispetto alla stabilità della posa (prevenzione del ribaltamento), senza cadere in ottimi locali o fare affidamento su ipotesi 2D eccessivamente semplificate. Gli approcci basati sui dati esistenti (ad esempio, l'Apprendimento per Rinforzo Profondo) spesso mancano di generalizzazione attraverso diverse configurazioni di partenza-arrivo e non riescono a integrare strutture di guida globale continue. Inoltre, i metodi standard di ottimizzazione multi-obiettivo fanno spesso affidamento sulla scalarizzazione pesata, che può permettere alla lunghezza del percorso di dominare la soluzione, trascurando vincoli di stabilità critici su terreni accidentati.

Metodologia
Gli autori propongono un framework di controllo a orizzonte scorrevole che integra un modello veicolo-terreno semi-parametrico con un Controllo Predittivo di Modello (MPC) a vincolo-$\epsilon$ adattivo al terreno.

1. Modellazione Veicolo-Terreno:

   • Approccio Semi-Parametrico: Il sistema combina un modello cinematico analitico per veicoli di tipo automobilistico con una correzione residua basata sui dati. Il modello nominale prevede l'evoluzione dello stato basandosi su velocità lineari e angolari.
   • Processo Gaussiano Sparsa (SGP): Per tenere conto degli effetti del terreno non modellati (ad esempio, slittamento, dinamiche di contatto complesse), un SGP viene addestrato su descrittori locali del terreno per prevedere l'errore residuo tra il modello nominale e il comportamento effettivo del sistema.
   • Descrittori del Terreno: Il modello utilizza un ricco insieme di descrittori, inclusi vettori normali ponderati per area, rugosità superficiale ($\sigma_z$), curvature gaussiana e media, e area totale del vicinato.
   • Funzione di Altezza: Un metodo basato sulla proiezione determina l'altezza del terreno $h(x,y)$ mappando i punti di interrogazione sulla faccia più vicina su una mesh triangolare, garantendo calcoli di orientamento accurati.

2. Framework di Ottimizzazione:

   • Formulazione Multi-Obiettivo: Il pianificatore ottimizza due obiettivi in competizione:
     • Efficienza del Percorso ($f_1$): Minimizza il disallineamento direzionale con un campo vettoriale globale FMM, pesato da un fattore di scala di prossimità all'obiettivo.
     • Stabilità della Posa ($f_2$): Minimizza le deviazioni cumulative di orientamento (rollio e beccheggio) rispetto alla normale locale del terreno, penalizzando le deviazioni oltre una soglia sicura ($\alpha_{safe}$) e imponendo un limite rigido ($\alpha_{max}$) per prevenire il ribaltamento.
   • Vincolo-$\epsilon$ Adattivo al Terreno: Invece della scalarizzazione pesata, il metodo converte il problema in un'ottimizzazione a singolo obiettivo vincolata. Il vincolo di stabilità ($f_2 \leq \epsilon_{stab}$) viene regolato dinamicamente in base alla rugosità e alla pendenza locale del terreno. In terreni impegnativi, il budget di stabilità si stringe; in terreni benigni, si allenta, permettendo un'esplorazione più aggressiva.
   • Strategia di Campionamento: Le sequenze di controllo candidate sono generate tramite un approccio ibrido:
     • Avvio a Caldo (Warm-starting): Spostamento delle sequenze di controllo precedenti di successo con l'aggiunta di rumore per garantire la continuità temporale.
     • Campionamento Biasato FMM: Fusione di controlli suggeriti dal campo vettoriale globale FMM con esplorazione casuale per sfuggire agli ottimi locali.

Contributi Chiave
Il documento rivendica quattro contributi principali:

1. Un modello semi-parametrico per il movimento di veicoli di tipo automobilistico su mesh triangolari irregolari, che combina dinamiche analitiche con apprendimento residuo basato su SGP.
2. Una funzione di costo per la navigazione su mesh derivata da un campo vettoriale FMM continuo.
3. Un approccio di soluzione che utilizza l'ottimizzazione a vincolo-$\epsilon$ adattivo al terreno all'interno di un framework a orizzonte scorrevole per generare traiettorie non dominate che bilanciano efficienza e stabilità.
4. Un pianificatore compatibile con ROS (Move Base Flex) progettato per il dispiegamento in tempo reale.

Risultati Sperimentali
Il metodo proposto ($\epsilon$-MPC) è stato valutato in simulazione (Gazebo) e in esperimenti nel mondo reale utilizzando un robot AgileX Scout Mini su due distinte mesh 3D del terreno. È stato confrontato con le linee di base Model Predictive Path Integral (MPPI) e Genetic Algorithmic Kinodynamic (GAKD).

• Metriche di Prestazione: Il metodo proposto ha raggiunto un tasso di successo nella navigazione del 94% in simulazione, superando MPPI (87%) e GAKD (89%).
• Stabilità: Ha ridotto la massima deviazione di orientamento del 24% rispetto a MPPI e ha migliorato la metrica di compromesso multi-obiettivo ($T$) del 23%.
• Studi di Ablazione:
  • Bias FMM: Una forza di bias FMM di $\alpha_{FMM}=0.7$ ha prodotto risultati ottimali, migliorando i tassi di successo dell'11–27% rispetto al campionamento puramente casuale.
  • Adattività: L'approccio a vincolo adattivo al terreno ha eliminato tutti gli incidenti di ribaltamento osservati con vincoli epsilon fissi e ha ridotto la massima deviazione di orientamento del 32%.
  • Dispiegamento nel Mondo Reale: Negli esperimenti fisici, il metodo ha mantenuto un tasso di successo dell'80% (contro il 67% di MPPI), navigando con successo sezioni ripide dove le linee di base fallivano a causa di angoli di rollio eccessivi. I tempi di pianificazione sono rimasti entro i vincoli di tempo reale (~35–48 ms per iterazione).

Significato e Rivendicazioni
Il documento afferma che il suo significato principale risiede nel fornire un framework principiato che affronta esplicitamente la natura multi-obiettivo della pianificazione cinematica e dinamica su terreni irregolari. Regolando dinamicamente i vincoli di stabilità in base ai descrittori locali del terreno, l'approccio raggiunge un equilibrio superiore tra ottimalità del percorso e sicurezza fisica, senza fare affidamento su ricerche evolutive complesse e computazionalmente costose (come NSGA-II, che è risultato troppo lento per l'uso in tempo reale). Gli autori concludono che, mentre i metodi di scalarizzazione pesata possono ottimizzare obiettivi individuali, il metodo a vincolo-$\epsilon$ adattivo al terreno è più efficace per garantire una navigazione fattibile e sicura in ambienti esterni non strutturati, offrendo una soluzione pratica che scala dalla simulazione al dispiegamento nel mondo reale.