Riepilogo Tecnico: DynoJEPP – Stima, Previsione e Pianificazione Congiunte in Ambienti Dinamici

1. Enunciato del Problema

L'autonomia robotica in ambienti reali è fondamentalmente messa alla prova dalla presenza di oggetti dinamici con moti complessi e imprevedibili. Sebbene recenti avanzamenti abbiano unito con successo la stima dello stato del robot e la pianificazione del percorso all'interno di framework basati su grafi dei fattori per ambienti statici, l'estensione di questi approcci a scenari dinamici rimane limitata.

Le soluzioni esistenti tipicamente affrontano una delle due limitazioni seguenti:

1. Assunzione di Conoscenza Perfetta: Esse assumono che le traiettorie future degli oggetti siano note a priori, il che è irrealistico in contesti dinamici.
2. Strategie Reattive: Esse si affidano a una semplice evitamento reattivo, privo della lungimiranza offerta dalla previsione delle traiettorie.

Una questione critica, spesso trascurata nei framework di ottimizzazione simultanea, è il flusso bidirezionale di informazioni. Nelle formulazioni congiunte convenzionali, le informazioni dai moduli di pianificazione e previsione retroagiscono nel modulo di stima dello stato. Questo ciclo di feedback corrompe le stime dello stato con bias "ottimistici" derivanti da piani futuri, portando a una ridotta accuratezza della stima, comportamenti indesiderati e, in definitiva, a piani non sicuri e collisioni.

2. Metodologia: Il Framework DynoJEPP

Gli autori propongono DynoJEPP, un framework unificato basato su grafi dei fattori che ottimizza simultaneamente la stima dello stato del robot e degli oggetti, la previsione delle traiettorie e la pianificazione locale. L'innovazione fondamentale risiede nell'introduzione di un fattore diretto per controllare il flusso delle informazioni.

A. Fattori Diretti

Per impedire che la pianificazione e la previsione degradino la stima, DynoJEPP introduce un fattore diretto che impone un flusso unidirezionale di informazioni:

• Stima $\to$ Previsione/Pianificazione: I risultati della stima (posizione attuale del robot e moti degli oggetti) si propagano in avanti per informare la previsione e la pianificazione.
• Previsione/Pianificazione $\nrightarrow$ Stima: Gli stati futuri e i vincoli di pianificazione sono impediti dall'influenzare la stima degli stati attuali o passati.

Tecnicamente, ciò è ottenuto mediante manipolazione dello Jacobiano. Azzerando le voci dello Jacobiano relative alla variabile sorgente (lo stato di stima) nelle equazioni normali ($J^\top J \delta = -J^\top r$), il framework garantisce che il gradiente del costo di pianificazione/previsione non aggiorni le variabili di stima. Questo disaccoppia strutturalmente il sottografo di stima dal sottografo di pianificazione, mantenendo al contempo la dipendenza necessaria affinché la pianificazione possa utilizzare le stime correnti.

B. Componenti della Formulazione Congiunta

Il framework integra tre moduli in un singolo grafo dei fattori:

1. Stima dello Stato: Si basa su Hybrid Dynamic SLAM per stimare la posizione del robot ($W X_k$) e i moti rigidi SE(3) degli oggetti dinamici ($W eH_k$). Utilizza misurazioni puntuali, odometria e fattori di smoothing del moto per tracciare i centri di massa degli oggetti.
2. Previsione del Moto degli Oggetti: Utilizza i moti SE(3) stimati per prevedere le traiettorie future degli oggetti su un orizzonte $N$. Impiega un modello a velocità costante (imposto da fattori di smoothing), ma rilassa questa assunzione vicino a ostacoli statici utilizzando fattori consapevoli della scena statica (basati su ESDF).
3. Pianificazione Locale: Impiega un Controllo Predittivo basato su Modello (MPC) basato su grafi dei fattori per ottimizzare le future posizioni, velocità e accelerazioni del robot. Incorpora:
   • Consapevolezza della Scena Statica: Utilizza Campi di Distanza Segnati Euclidei (ESDF) per evitare ostacoli statici.
   • Consapevolezza della Scena Dinamica: Utilizza fattori diretti per ostacoli dinamici per penalizzare la prossimità agli oggetti in movimento basandosi sulle loro traiettorie previste.

C. Estensione Cooperativa (C-DynoJEPP)

Il documento introduce Cooperative DynoJEPP (C-DynoJEPP), che permette al robot egoista di incorporare comportamenti cooperativi nella sua pianificazione. In questa estensione, i fattori per ostacoli dinamici sono diretti dalla pianificazione alla previsione. Ciò crea un ciclo diretto tra previsione e pianificazione, modellando l'influenza reciproca in cui il robot reagisce all'oggetto e l'oggetto è previsto reagire alla traiettoria intenzionale del robot. Ciò consente una navigazione meno conservativa e più assertiva in scenari interattivi.

3. Contributi Chiave

1. Framework DynoJEPP: Il primo framework basato su grafi dei fattori che stima congiuntamente il moto degli oggetti dinamici, ne prevede l'evoluzione e incorpora queste previsioni nelle azioni di controllo del robot all'interno di un singolo problema di ottimizzazione.
2. Nuovo Fattore Diretto: Un meccanismo per specificare la direzione dell'influenza nei grafi dei fattori, consentendo un flusso di informazioni controllato tra stima, previsione e pianificazione. Ciò previene la "corruzione" delle stime dello stato da parte dei vincoli di pianificazione futura.
3. Ottimizzazione Cooperativa: Un'estensione (C-DynoJEPP) che parametrizza il livello di cooperazione degli oggetti dinamici, permettendo un'influenza reciproca tra il piano del robot e la previsione dell'oggetto senza compromettere l'indipendenza della stima dello stato.

4. Risultati Sperimentali

Gli autori hanno valutato il framework in un ambiente di magazzino simulato (Gazebo) con robot a trazione differenziale e ostacoli dinamici. Hanno confrontato tre configurazioni: Non Diretto (flusso bidirezionale), Diretto (DynoJEPP) e Disaccoppiato (stima separata seguita da previsione/pianificazione congiunta).

• Sicurezza e Tasso di Successo: In 10 esperimenti di navigazione con ostacoli dinamici, la configurazione Non Diretta ha fallito nella maggior parte dei casi (collisione in 8 casi su 10 tentativi dinamici) a causa di una pianificazione eccessivamente ottimistica che corrompeva le stime. Al contrario, la configurazione Diretta ha raggiunto un tasso di successo del 100% in tutti i tentativi.
• Accuratezza della Stima: Il metodo Diretto ha mantenuto un'accuratezza di stima paragonabile alla baseline Disaccoppiata, superando significativamente l'approccio Non Diretto. Il metodo Non Diretto ha mostrato un degrado significativo sia nelle stime della traiettoria del robot che di quella degli oggetti.
• Navigazione Cooperativa: In scenari con ostacoli dinamici reattivi, C-DynoJEPP ha dimostrato la capacità di navigare in modo più assertivo rispetto al DynoJEPP standard. Ha raggiunto gli obiettivi più velocemente (82 passi contro 92 passi della corsa più rapida di DynoJEPP) sfruttando pesi cooperativi per influenzare le previsioni.

5. Significato e Affermazioni

Il documento afferma che controllare la direzionalità del flusso di informazioni è critico per un funzionamento sicuro in ambienti dinamici. Senza fattori diretti, l'accoppiamento tra pianificazione e stima porta a un ciclo di feedback in cui gli obiettivi di pianificazione distorcono le stime dello stato, risultando in comportamenti non sicuri.

Gli autori sottolineano che, sebbene la stima non tragga intrinsecamente beneficio dall'ottimizzazione congiunta con la pianificazione, la previsione e la pianificazione ne traggono beneficio dall'essere ottimizzate congiuntamente con la stima, in particolare quando si modellano comportamenti interattivi. Il framework proposto fornisce una soluzione robusta che abilita un'ottimizzazione simultanea sicura senza sacrificare l'integrità della stima dello stato.

Il lavoro conclude notando che la ricerca futura si concentrerà sull'implementazione di una versione efficiente, incrementale e multi-thread di DynoJEPP per raggiungere prestazioni in tempo reale, confrontandosi con standard industriali come Nav2.