⚡ electrical engineering

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

FusionCore est un package ROS 2 open-source qui utilise un filtre de Kalman unscented à 23 états pour fusionner de manière robuste les données de l'IMU, des encodeurs de roues, du GPS et du SLAM visuel en un flux d'odométrie haute fréquence, atteignant des erreurs de trajectoire nettement inférieures et une stabilité numérique supérieure par rapport au package robot_localization standard sur diverses séquences du jeu de données NCLT.

Auteurs originaux : Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Publié 2026-05-26

📖 7 min de lecture🧠 Analyse approfondie

CC BY 4.0

Auteurs originaux : Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

Article original sous licence CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ Ceci est une explication générée par l'IA de l'article ci-dessous. Elle n'a pas été rédigée ni approuvée par les auteurs. Pour une précision technique, consultez l'article original. Lire la clause de non-responsabilité complète

Imaginez que vous essayez de guider un robot à travers une ville, mais que vous êtes bandé les yeux, que vos oreilles sont couvertes et que vous ne pouvez deviner votre position qu'en sentant les roues tourner. C'est essentiellement ce à quoi les robots sont confrontés lorsqu'ils tentent de déterminer leur localisation. Ils doivent combiner des données provenant de différents « sens » pour se faire une image claire de leur position.

Ce document présente FusionCore, un nouvel outil logiciel conçu pour être le « cerveau » qui combine ces sens pour les robots utilisant le système ROS 2. Voici comment il fonctionne, expliqué par de simples analogies.

Le Problème : L'Équipe « Bruyante »

Les robots disposent généralement de trois méthodes principales pour deviner leur position :

Encodeurs de roues (Le Podomètre) : Compte le nombre de rotations des roues. C'est excellent pour les courtes distances, mais il se trompe si les roues patinent sur la glace ou la boue.
GPS (La Carte Satellite) : Vous indique exactement où vous vous trouvez sur Terre. Mais il est lent, disparaît parfois (comme lorsque vous conduisez sous un pont), et peut occasionnellement vous donner un « bug » vous indiquant que vous êtes dans une autre ville.
IMU (L'Oreille Interne) : Un capteur qui ressent l'accélération et la rotation. Il est très rapide, mais avec le temps, il commence à « dériver » (comme une toupie qui penche lentement), faisant croire au robot qu'il bouge alors qu'il est immobile.

Les logiciels existants (appelés robot_localization) tentent de mélanger ces signaux, mais ils présentent quelques défauts :

Ils traitent la « dérive » des capteurs comme une erreur fixe, et non comme quelque chose qui change au fil du temps.
Ils se perdent lorsque les signaux GPS sautent par-dessus les limites des cartes.
Ils peinent à distinguer une véritable mise à jour GPS d'un bug fou.

La Solution : Le « Cerveau Intelligent » de FusionCore

FusionCore est un nouveau « cerveau » (un filtre mathématique) qui fusionne tous ces signaux en un seul flux lisse et haute fréquence de données de localisation (100 fois par seconde). Il utilise un « état » à 23 dimensions pour suivre tout, y compris certaines choses que d'autres systèmes ignorent.

Voici les astuces clés utilisées par FusionCore, expliquées par des métaphores :

1. Le « Détective de Dérive » (Estimation de biais en ligne)

Imaginez que vous marchiez avec un ami qui a une légère boiterie. Si vous ne remarquez pas la boiterie, votre carte de votre position deviendra lentement fausse.

Ancienne méthode : Le logiciel suppose que la boiterie est fixe et l'ignore.
Méthode FusionCore : Il possède un « Détective de Dérive » intégré. Il observe constamment les capteurs et se demande : « La roue patine-t-elle ? Le gyroscope est-il fatigué ? » Il calcule la quantité exacte de « boiterie » (biais) en temps réel et la soustrait. Cela empêche le robot de se perdre même lorsque le GPS est absent pendant un certain temps.

2. L'« Adresse Globale » (GPS natif ECEF)

Les coordonnées GPS sont généralement converties en cartes locales (comme les zones UTM). Si vous traversez une limite de zone, la carte peut buguer et planter.

Méthode FusionCore : Au lieu d'utiliser des adresses de rue locales, il parle le langage de toute la Terre (ECEF). Il traite la planète comme une seule sphère géante, de sorte qu'il ne se perd jamais à cause des limites de cartes. C'est comme utiliser la latitude et la longitude pour le monde entier au lieu de changer de carte de ville tous les 10 miles.

3. Le « Détecteur de Mensonges » (Gestion adaptative des valeurs aberrantes)

Parfois, le GPS envoie un message disant : « Je suis maintenant dans l'océan ! » alors que le robot est clairement sur une route.

Ancienne méthode : Le logiciel utilise une règle rigide : « Si l'erreur dépasse 5 mètres, ignorez-la. » Mais si le GPS n'est en réalité que légèrement bruyant, il pourrait ignorer de bonnes données.
Méthode FusionCore : Il utilise un « Détecteur de Mensonges » qui apprend. Il examine l'historique des erreurs. Si le GPS est généralement précis mais a soudainement une mauvaise journée, le détecteur relâche son emprise. Si le GPS agit de manière folle, il ferme la porte à double tour. Il ajuste automatiquement sa propre sensibilité, de sorte qu'il ne se laisse pas tromper par des bugs ni rejette de bonnes données.

4. Le « Voyageur dans le Temps » (Compensation de délai)

Les données GPS arrivent souvent avec un quart de seconde de retard car le satellite doit faire des calculs avant d'envoyer le signal.

Méthode FusionCore : Il maintient un « tampon mémoire » (un tampon circulaire) de la dernière seconde de données IMU. Lorsque le message GPS en retard arrive enfin, le logiciel ne l'applique pas simplement au moment actuel. Il « rembobine » jusqu'au moment exact où le signal GPS a été pris, applique la correction, puis « avance rapidement » jusqu'à maintenant. Cela garantit que la position du robot est parfaitement synchronisée, même avec des capteurs lents.

5. La « Mémoire Visuelle » (Fusion VSLAM)

Si le robot perd le GPS (comme en conduisant dans un tunnel), il peut utiliser une caméra (SLAM visuel) pour suivre des caractéristiques dans la pièce.

Méthode FusionCore : Il peut basculer de manière transparente vers la vue de la caméra. Si la caméra perd son chemin et doit « redémarrer » sa carte, FusionCore sait comment ramener la position du robot à la réalité sans que le robot ne panique ou ne plante.

Les Résultats : Le Grand Test

L'auteur a testé FusionCore contre le logiciel standard (robot_localization) en utilisant 12 séquences de conduite longues (jusqu'à 92 minutes chacune) issues d'un ensemble de données public.

Le Gagnant : FusionCore a gagné dans 10 courses sur 12.
La Marge : Dans certains cas, il était 22 fois plus précis que le logiciel standard.
Les Perdants : La version avancée du logiciel standard (UKF) a planté (divergé) sur toutes les 12 séquences, ce qui signifie qu'elle a complètement perdu la trace de la position du robot.
Pourquoi FusionCore a perdu 2 courses :
1. Le Long Blackout : Dans une course, le signal GPS a disparu pendant près de 8 minutes. Même le meilleur détective ne peut pas deviner parfaitement sans carte pendant aussi longtemps, et le robot a légèrement dérivé.
2. Le Bug « Adversarial » : Dans une autre course, le GPS a envoyé un groupe de mensonges si convaincants qu'ils ont trompé le filtre. Le logiciel standard a heureusement rejeté ces données par accident car ses règles étaient trop strictes, tandis que FusionCore a essayé d'être trop serviable et s'est fait piéger. L'auteur note qu'il s'agit de problèmes spécifiques et corrigibles.

Résumé

FusionCore est une méthode plus intelligente et plus adaptable pour aider les robots à savoir où ils se trouvent. Il traite les erreurs des capteurs comme des êtres vivants qui changent au fil du temps, apprend à ignorer les mensonges et se souvient du passé pour corriger le présent. Bien que imparfait, il surpasse considérablement la norme industrielle actuelle dans la plupart des scénarios réels.

Résumé Technique : FusionCore

Énoncé du Problème

L'estimation précise de la pose est fondamentale pour la navigation autonome des robots, mais le déploiement pratique des systèmes de fusion de capteurs reste difficile. Les solutions existantes font face à plusieurs limitations critiques :

Dérive des biais : Les biais des gyroscopes et accéléromètres IMU dérivent continuellement, et les biais de vitesse angulaire de lacet des encodeurs de roues (décalages systématiques dans la transmission différentielle) sont rarement traités comme des états du filtre, entraînant une accumulation de l'erreur de cap non observable lors des interruptions GPS.
Fragilité architecturale : Les packages ROS dominants comme robot_localization reposent sur des nœuds séparés pour la transformation des coordonnées GPS (UTM), introduisant des défaillances aux limites des zones. Ils utilisent également des covariances de bruit fixes et des seuils de rejet des valeurs aberrantes scalaires qui ne tiennent pas compte de la dimensionalité des mesures.
Lacunes opérationnelles : Les systèmes actuels peinent avec le fonctionnement sans GPS, la récupération après réinitialisation de la carte, et l'intégration de capteurs divers tels que la SLAM visuelle (VSLAM) et le radar sans coordination complexe.

Méthodologie

FusionCore est un package ROS 2 open-source implémentant un Filtre de Kalman Unscented (UKF) à 23 états qui fusionne l'IMU, l'odométrie des encodeurs de roues, le GPS et la pose VSLAM en un seul flux d'odométrie à 100 Hz.

Architecture de Base

Vecteur d'état : Le filtre maintient un vecteur d'état à 23 dimensions incluant la position ( $p$ ), l'orientation ( $q$ ), la vitesse linéaire/angulaire ( $v, \omega$ ), l'accélération linéaire ( $a$ ), le biais du gyroscope ( $b_g$ ), le biais de l'accéléromètre ( $b_a$ ), et un biais critique de vitesse angulaire de lacet des encodeurs de roues ( $b_{ewz}$ ).
Estimation des biais : Contrairement aux approches précédentes, les biais du gyroscope, de l'accéléromètre et du lacet des encodeurs sont des états de filtre de première classe avec un bruit de processus dédié. Cela permet au filtre de converger vers des valeurs de biais spécifiques aux capteurs en ligne sans phase de calibration séparée.
Fusion GPS : Le système fusionne les données GPS nativement dans le repère Terre-Centrée, Terre-Fixe (ECEF), ne convertissant en ENU que pour l'état du filtre. Cela élimine le besoin d'un nœud navsat_transform séparé et évite les défaillances aux limites des zones UTM.
Modèles de mesure :
- IMU : Gère les données brutes à 6 DOF et les mises à jour d'orientation à 2/3 DOF, tenant compte de la gravité et des biais.
- Encodeurs de roues : Soustrait le biais $b_{ewz}$ estimé de la vitesse angulaire rapportée avant l'intégration. Impose des contraintes non holonomes (vitesse et accélération verticales nulles) avec un bruit adaptatif.
- VSLAM : Accepte les mesures de pose à 6 DOF de n'importe quel système d'odométrie visuelle/SLAM, incluant la récupération automatique après réinitialisation de la carte en réancrant la transformation après des rejets consécutifs de valeurs aberrantes.
- Vitesse : Prend en charge la vitesse Doppler GPS et la vitesse propre Doppler Radar pour la détection de glissement.

Innovations de Conception du Filtre

Filtrage Chi-deux par Capteur : Implémente un rejet des valeurs aberrantes par distance de Mahalanobis avec des seuils calibrés spécifiquement pour les degrés de liberté (DOF) de chaque chemin de capteur (par exemple, GPS 3-DOF contre cap de 1-DOF), plutôt que d'utiliser un seuil scalaire unique.
Adaptation de la Covariance de Bruit : Ajuste automatiquement la covariance de bruit du capteur ( $R$ ) en temps réel en utilisant une fenêtre glissante de séquences d'innovation et une moyenne mobile exponentielle, éliminant le besoin de réajustement manuel dû aux changements environnementaux.
Compensation de Délai : Utilise un tampon circulaire IMU d'une seconde pour effectuer une rétrodiction, appliquant les mesures GPS ou VSLAM retardées à l'état historique correct avant de re-propager vers l'avant.
Mises à jour à Vitesse Nulle (ZUPT) : Détecte les périodes stationnaires pour supprimer la dérive des biais.
Mode de Dérive : Pendant les interruptions GPS, le biais de lacet de l'encodeur estimé est soustrait des données de l'encodeur pour réduire la dérive de cap.

Contributions Clés

L'article présente dix contributions spécifiques :

Une formulation UKF à 23 états avec estimation continue en ligne des biais du gyroscope, de l'accéléromètre et de la vitesse angulaire de lacet des encodeurs de roues.
Fusion GPS native ECEF, éliminant les dépendances de projection UTM.
Filtrage Chi-deux de Mahalanobis par capteur avec des seuils calibrés selon la dimensionalité.
Adaptation automatique de la covariance de bruit à partir des séquences d'innovation.
Mises à jour automatiques à Vitesse Nulle (ZUPT) pour les périodes stationnaires.
Rétrodicttion par tampon circulaire IMU pour la compensation de délai.
Fusion de pose VSLAM avec récupération automatique après réinitialisation de la carte.
Fusion de vitesse GPS pour la détection de glissement inter-capteurs.
Fusion de vitesse propre Doppler Radar pour les plateformes de radar d'imagerie 4D.
Une évaluation quantitative contre robot_localization sur douze séquences complètes NCLT.

Évaluation et Résultats

Le système a été évalué sur douze séquences complètes (55 à 92 minutes chacune) issues du jeu de données public NCLT, couvrant diverses conditions saisonnières. L'évaluation a comparé FusionCore à l'EKF et l'UKF de robot_localization en utilisant l'erreur RMSE de la trajectoire absolue (ATE).

Performance : FusionCore a obtenu un ATE inférieur à celui de l'EKF robot_localization sur 10 séquences sur 12. Les améliorations sur les séquences gagnantes variaient de 1,2× à 22,2×.
Stabilité : L'UKF robot_localization a divergé numériquement (NaN) sur les douze séquences dans les 30 premières secondes, confirmant les problèmes d'instabilité connus avec les entrées à haut débit.
Analyse des Défaillances :
- 2012-06-15 (Perte FC) : FusionCore a perdu face à l'EKF robot_localization (49,2 m contre 18,2 m) en raison d'une interruption GPS de 7,7 minutes. L'article attribue cela aux limitations de la navigation à l'estime basée sur les encodeurs sur de telles durées sans référence de cap absolue (par exemple, magnétomètre).
- 2012-08-20 (Perte FC) : FusionCore a perdu de manière significative (98,3 m contre 10,6 m) en raison d'un « cluster GPS adversaire » de 105 fixes apparaissant à 720–840 m de la vérité terrain à une limite d'interruption. L'article note que le seuil mal calibré de robot_localization a par hasard rejeté ces valeurs aberrantes, tandis que le seuil relâché de FusionCore (nécessaire à la récupération) les a acceptées. Les auteurs identifient une pré-vérification de cohérence de vitesse comme solution.
Inadéquation de la Covariance : L'article souligne que la dépendance de robot_localization à des rapports de covariance GPS fixes, souvent inexacts, conduit au rejet de données valides et à un effondrement vers la navigation à l'estime. Le modèle de bruit adaptatif de FusionCore atténue cela en gonflant le modèle de bruit lorsque les erreurs d'innovation dépassent les covariances rapportées.

Importance et Revendications

L'article revendique que FusionCore répond aux limitations pratiques des piles de fusion de capteurs ROS 2 existantes en fournissant un nœud de cycle de vie unique ne nécessitant aucun coordinateur externe. Son importance réside dans :

Robustesse : Atteindre une précision de trajectoire supérieure dans des scénarios réels de longue durée où les implémentations UKF existantes échouent numériquement.
Adaptabilité : Gérer la qualité dynamique des capteurs et les inadéquations de covariance GPS grâce au bruit adaptatif et au filtrage conscient de la dimensionalité.
Continuité Opérationnelle : Permettre le fonctionnement sans GPS et la récupération transparente après réinitialisation de la carte VSLAM.
Reproductibilité : Fournir une implémentation entièrement open-source avec des scripts de référence et des configurations pour des plateformes standard (TurtleBot3, Husky) et l'intégration avec rtabmap_ros.

Les auteurs concluent que bien que des modes de défaillance spécifiques (interruptions longues, clusters GPS adverses) existent, ils sont diagnostiquables et qu'ils ont identifié des correctifs architecturaux (intégration de magnétomètre, pré-vérifications de vitesse) pour les travaux futurs.