⚡ electrical engineering

FusionCore: A 23-State Unscented Kalman Filter for IMU, Wheel Encoder, GPS, and Visual SLAM Fusion in ROS 2

FusionCore è un pacchetto ROS 2 open-source che impiega un Filtro di Kalman Unscented a 23 stati per fondere in modo robusto i dati IMU, degli encoder delle ruote, GPS e Visual SLAM in un flusso di odometria ad alta frequenza, ottenendo errori di traiettoria significativamente inferiori e una maggiore stabilità numerica rispetto al pacchetto standard robot_localization su diverse sequenze del dataset NCLT.

Autori originali: Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Pubblicato 2026-05-26

📖 6 min di lettura🧠 Approfondimento

CC BY 4.0

Autori originali: Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Articolo originale sotto licenza CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ Questa è una spiegazione generata dall'IA dell'articolo qui sotto. Non è stata scritta né approvata dagli autori. Per precisione tecnica, consulta l'articolo originale. Leggi il disclaimer completo

Immagina di dover guidare un robot attraverso una città, ma sei bendato, le orecchie tappate, e puoi solo indovinare dove ti trovi sentendo girare le ruote. È essenzialmente ciò che i robot affrontano quando cercano di capire la propria posizione. Devono combinare dati provenienti da diversi "sensi" per costruire un quadro chiaro della loro collocazione.

Questo articolo presenta FusionCore, un nuovo strumento software progettato per essere il "cervello" che combina questi sensi per i robot che utilizzano il sistema ROS 2. Ecco come funziona, spiegato attraverso semplici analogie.

Il Problema: Il Team "Rumoroso"

I robot hanno solitamente tre modi principali per indovinare la propria posizione:

Encoder delle ruote (Il Podometro): Conta quante volte le ruote girano. È ottimo per brevi distanze, ma si confonde se le ruote slittano su ghiaccio o fango.
GPS (La Mappa Satellitare): Ti dice esattamente dove ti trovi sulla Terra. Ma è lento, a volte scompare (come quando guidi sotto un ponte) e può occasionalmente darti un "glitch" che ti dice di essere in una città diversa.
IMU (L'Orecchio Interno): Un sensore che percepisce accelerazione e rotazione. È molto veloce, ma col tempo inizia a "derivare" (come una trottola che lentamente si inclina), facendo credere al robot di muoversi anche quando è fermo.

Il software esistente (chiamato robot_localization) cerca di mescolare questi segnali, ma presenta alcuni difetti:

Tratta la "deriva" nei sensori come un errore fisso, non come qualcosa che cambia nel tempo.
Si confonde quando i segnali GPS saltano attraverso i confini delle mappe.
Fatica a distinguere tra un vero aggiornamento GPS e un glitch folle.

La Soluzione: Il "Cervello Intelligente" di FusionCore

FusionCore è un nuovo "cervello" (un filtro matematico) che fonde tutti questi segnali in un unico flusso continuo e ad alta velocità di dati di posizione (100 volte al secondo). Utilizza uno "stato" a 23 dimensioni per tenere traccia di tutto, inclusi alcuni aspetti che altri sistemi ignorano.

Ecco i trucchi chiave utilizzati da FusionCore, spiegati con metafore:

1. Il "Detective della Deriva" (Stima Online del Bias)

Immagina di camminare con un amico che zoppica leggermente. Se non noti la zoppia, la tua mappa di dove ti trovi diventerà lentamente errata.

Vecchio Metodo: Il software assume che la zoppia sia fissa e la ignora.
Metodo FusionCore: Ha un "Detective della Deriva" interno. Osserva costantemente i sensori e chiede: "La ruota sta slittando? Il giroscopio si sta stancando?" Calcola l'esatta quantità di "zoppia" (bias) in tempo reale e la sottrae. Questo mantiene il robot da perdersi anche quando il GPS scompare per un po'.

2. L'"Indirizzo Globale" (GPS Nativo ECEF)

Le coordinate GPS vengono solitamente convertite in mappe locali (come le zone UTM). Se guidi attraverso un confine di zona, la mappa può glitchare e bloccarsi.

Metodo FusionCore: Invece di usare indirizzi stradali locali, parla la lingua dell'intera Terra (ECEF). Tratta il pianeta come un'unica grande sfera, quindi non si confonde mai per i confini delle mappe. È come usare latitudine e longitudine per tutto il mondo invece di cambiare mappa di città ogni 10 miglia.

3. Il "Sondatore di Bugie" (Gating Adattivo degli Outlier)

A volte il GPS invia un messaggio che dice: "Ora sono nell'oceano!" quando il robot è chiaramente su una strada.

Vecchio Metodo: Il software usa una regola rigida: "Se l'errore è maggiore di 5 metri, ignoralo". Ma se il GPS è solo un po' rumoroso, potrebbe ignorare dati buoni.
Metodo FusionCore: Usa un "Sondatore di Bugie" che impara. Esamina la storia degli errori. Se il GPS è solitamente accurato ma ha improvvisamente una brutta giornata, il rilevatore allenta la presa. Se il GPS sta agendo in modo folle, sbatte la porta in faccia. Regola automaticamente la propria sensibilità, così non viene ingannato dai glitch né rifiuta dati buoni.

4. Il "Viaggiatore nel Tempo" (Compensazione del Ritardo)

I dati GPS arrivano spesso un istante in ritardo perché il satellite deve fare calcoli prima di inviare il segnale.

Metodo FusionCore: Mantiene un "buffer di memoria" (un buffer ad anello) dell'ultimo secondo di dati IMU. Quando il messaggio GPS in ritardo arriva finalmente, il software non lo applica semplicemente al momento corrente. "Riavvolge" fino al momento esatto in cui il segnale GPS è stato acquisito, applica la correzione e poi "avvolge velocemente" fino ad ora. Questo garantisce che la posizione del robot sia perfettamente sincronizzata, anche con sensori lenti.

5. La "Memoria Visiva" (Fusione VSLAM)

Se il robot perde il GPS (come guidando in un tunnel), può usare una telecamera (Visual SLAM) per tracciare le caratteristiche nella stanza.

Metodo FusionCore: Può passare senza soluzione di continuità alla vista della telecamera. Se la telecamera perde la strada e deve "riavviare" la sua mappa, FusionCore sa come riportare la posizione del robot alla realtà senza che il robot vada in panico o si schianti.

I Risultati: Il Grande Test

L'autore ha testato FusionCore contro il software standard (robot_localization) utilizzando 12 lunghe sequenze di guida (fino a 92 minuti ciascuna) da un dataset pubblico.

Il Vincitore: FusionCore ha vinto in 10 su 12 gare.
Il Margine: In alcuni casi, è stato 22 volte più accurato del software standard.
I Perdenti: La versione avanzata del software standard (UKF) si è bloccata (divergenza) su tutte le 12 sequenze, il che significa che ha completamente perso la traccia della posizione del robot.
Perché FusionCore ha perso 2 gare:
1. Il Blackout Lungo: In una gara, il segnale GPS è stato assente per quasi 8 minuti. Anche il miglior detective non può indovinare perfettamente senza una mappa per così tanto tempo, e il robot è deriverato leggermente.
2. Il Glitch "Adversarial": In un'altra gara, il GPS ha inviato un gruppo di bugie così convincenti da ingannare il filtro. Il software standard, per caso, le ha rifiutate perché le sue regole erano troppo rigide, mentre FusionCore ha cercato di essere troppo utile e si è fatto ingannare. L'autore nota che questi sono problemi specifici e risolvibili.

Riepilogo

FusionCore è un modo più intelligente e adattabile per aiutare i robot a sapere dove si trovano. Tratta gli errori dei sensori come esseri viventi che cambiano nel tempo, impara a ignorare le bugie e ricorda il passato per correggere il presente. Sebbene non sia perfetto, supera significativamente lo standard industriale attuale nella maggior parte degli scenari del mondo reale.

Riepilogo Tecnico: FusionCore

Enunciato del Problema

La stima accurata della posa è fondamentale per la navigazione autonoma dei robot, tuttavia il dispiegamento pratico dei sistemi di fusione sensoriale rimane impegnativo. Le soluzioni esistenti presentano diverse limitazioni critiche:

Deriva del Bias: I bias dei giroscopi e degli accelerometri IMU derivano continuamente, e i bias del tasso di imbardata degli encoder delle ruote (offset sistematici nella guida differenziale) sono raramente trattati come stati del filtro, portando all'accumulo di errori di imbardata non osservabili durante le interruzioni del GPS.
Fragilità Architetturale: I pacchetti ROS dominanti come robot_localization si affidano a nodi separati per la trasformazione delle coordinate GPS (UTM), introducendo guasti ai confini delle zone. Utilizzano inoltre covarianze di rumore fisse e soglie di rifiuto degli outlier scalari che non tengono conto della dimensionalità della misurazione.
Lacune Operative: I sistemi attuali faticano con le operazioni in assenza di GPS, il recupero dalla reinizializzazione della mappa e l'integrazione di sensori diversificati come Visual SLAM (VSLAM) e radar senza un coordinamento complesso.

Metodologia

FusionCore è un pacchetto ROS 2 open-source che implementa un Filtro di Kalman Unscented (UKF) a 23 stati che fonde IMU, odometria degli encoder delle ruote, GPS e posa Visual SLAM in un unico flusso di odometria a 100 Hz.

Architettura Principale

Vettore di Stato: Il filtro mantiene un vettore di stato a 23 dimensioni che include posizione ( $p$ ), orientamento ( $q$ ), velocità lineare/angolare ( $v, \omega$ ), accelerazione lineare ( $a$ ), bias del giroscopio ( $b_g$ ), bias dell'accelerometro ( $b_a$ ) e un critico bias del tasso di imbardata dell'encoder delle ruote ( $b_{ewz}$ ).
Stima del Bias: A differenza degli approcci precedenti, i bias del giroscopio, dell'accelerometro e dell'encoder di imbardata sono stati di filtro di prima classe con rumore di processo dedicato. Ciò permette al filtro di convergere su valori di bias specifici per il sensore online senza una fase di calibrazione separata.
Fusione GPS: Il sistema fonde i dati GPS nativamente nel riferimento Earth-Centered, Earth-Fixed (ECEF), convertendo in ENU solo per lo stato del filtro. Ciò elimina la necessità di un nodo navsat_transform separato ed evita i guasti ai confini delle zone UTM.
Modelli di Misurazione:
- IMU: Gestisce dati grezzi a 6 DOF e aggiornamenti di orientamento a 2/3 DOF, tenendo conto di gravità e bias.
- Encoder delle Ruote: Sottrae il $b_{ewz}$ stimato dalla velocità angolare riportata prima dell'integrazione. Impone vincoli non olonomi (velocità e accelerazione verticale nulle) con rumore adattivo.
- VSLAM: Accetta misurazioni di posa a 6 DOF da qualsiasi sistema di odometria visiva/SLAM, incluso il recupero automatico dalla reinizializzazione della mappa ri-ancorando la trasformazione dopo il rifiuto consecutivo di outlier.
- Velocità: Supporta la velocità Doppler GPS e la velocità propria Doppler del radar per il rilevamento dello slittamento.

Innovazioni nella Progettazione del Filtro

Porta Chi-Quadrato per Sensore: Implementa il rifiuto degli outlier tramite distanza di Mahalanobis con soglie calibrate specificamente per i gradi di libertà (DOF) di ciascun percorso del sensore (ad es. GPS a 3 DOF contro imbardata a 1 DOF), anziché utilizzare una singola soglia scalare.
Covarianza del Rumore Adattiva: Regola automaticamente la covarianza del rumore del sensore ( $R$ ) in tempo reale utilizzando una finestra scorrevole delle sequenze di innovazione e una media mobile esponenziale, eliminando la necessità di un ritaraggio manuale dovuto a cambiamenti ambientali.
Compensazione del Ritardo: Utilizza un buffer ad anello IMU di 1 secondo per eseguire la retrodizione, applicando misurazioni GPS o VSLAM ritardate allo stato storico corretto prima di ripropagare in avanti.
Aggiornamenti a Velocità Zero (ZUPT): Rileva i periodi di stazionarietà per sopprimere la deriva del bias.
Modalità di Costeggio: Durante le interruzioni del GPS, il bias dell'encoder di imbardata stimato viene sottratto dai dati dell'encoder per ridurre la deriva di imbardata.

Contributi Chiave

Il documento presenta dieci contributi specifici:

Una formulazione UKF a 23 stati con stima continua online dei bias del giroscopio, dell'accelerometro e del tasso di imbardata dell'encoder delle ruote.
Fusione GPS nativa ECEF, rimuovendo le dipendenze dalla proiezione UTM.
Porta chi-quadro Mahalanobis per sensore con soglie calibrate per la dimensionalità.
Adattamento automatico della covarianza del rumore dalle sequenze di innovazione.
Aggiornamenti automatici a Velocità Zero (ZUPT) per i periodi di stazionarietà.
Retrodizione con buffer ad anello IMU per la compensazione del ritardo.
Fusione della posa Visual SLAM con recupero automatico dalla reinizializzazione della mappa.
Fusione della velocità GPS per il rilevamento dello slittamento tra sensori.
Fusione della velocità propria Doppler del radar per piattaforme radar di imaging 4D.
Una valutazione quantitativa contro robot_localization su dodici sequenze NCLT a lunghezza completa.

Valutazione e Risultati

Il sistema è stato valutato su dodici sequenze a lunghezza completa (da 55 a 92 minuti ciascuna) dal dataset pubblico NCLT, coprendo diverse condizioni stagionali. La valutazione ha confrontato FusionCore con l'EKF e l'UKF di robot_localization utilizzando l'Errore di Traiettoria Assoluta (ATE) RMSE.

Prestazioni: FusionCore ha ottenuto un ATE inferiore rispetto all'EKF di robot_localization su 10 sequenze su 12. I miglioramenti sulle sequenze vincenti sono variati da 1,2× a 22,2×.
Stabilità: L'UKF di robot_localization ha diverso numericamente (NaN) su tutte le dodici sequenze entro i primi 30 secondi, confermando noti problemi di instabilità con input ad alta frequenza.
Analisi dei Guasti:
- 2012-06-15 (Perdita FC): FusionCore ha perso contro l'EKF di robot_localization (49,2m contro 18,2m) a causa di un'interruzione GPS di 7,7 minuti. Il documento attribuisce ciò alle limitazioni della navigazione stimata basata su encoder per durate così lunghe senza un riferimento di imbardata assoluto (ad es. magnetometro).
- 2012-08-20 (Perdita FC): FusionCore ha perso significativamente (98,3m contro 10,6m) a causa di un "cluster GPS avversario" di 105 correzioni apparse a 720–840m dalla verità al suolo al confine di un'interruzione. Il documento nota che la porta mal calibrata di robot_localization ha casualmente rifiutato questi outlier, mentre la porta rilassata di FusionCore (necessaria per il recupero) li ha accettati. Gli autori identificano un pre-controllo di coerenza della velocità come soluzione.
Disallineamento della Covarianza: Il documento evidenzia che la dipendenza di robot_localization da report di covarianza GPS fissi, spesso inaccurati, porta al rifiuto di dati validi e al collasso nella navigazione stimata. Il modello di rumore adattivo di FusionCore mitiga ciò inflazionando il modello di rumore quando gli errori di innovazione superano le covarianze riportate.

Significato e Affermazioni

Il documento afferma che FusionCore affronta le limitazioni pratiche degli stack di fusione sensoriale ROS 2 esistenti fornendo un singolo nodo di ciclo di vita che non richiede coordinatori esterni. La sua rilevanza risiede in:

Robustezza: Raggiungere una precisione di traiettoria superiore in scenari reali a lunga durata dove le implementazioni UKF esistenti falliscono numericamente.
Adattabilità: Gestire la qualità dinamica dei sensori e le discrepanze di covarianza GPS attraverso rumore adattivo e porte consapevoli della dimensionalità.
Continuità Operativa: Abilitare operazioni in assenza di GPS e un recupero senza soluzione di continuità dalla reinizializzazione della mappa VSLAM.
Riproducibilità: Fornire un'implementazione completamente open-source con script di benchmark e configurazioni per piattaforme standard (TurtleBot3, Husky) e integrazione con rtabmap_ros.

Gli autori concludono che, sebbene esistano modalità di guasto specifiche (interruzioni prolungate, cluster GPS avversari), queste sono diagnosticabili e hanno identificato correzioni architetturali (integrazione del magnetometro, pre-controlli di velocità) per lavori futuri.