Synchronous Observer Design for Landmark-Inertial SLAM with Magnetometer and Intermittent GNSS Measurements

Cet article propose un observateur non linéaire pour la SLAM basée sur des repères et l'inertie, qui surmonte les contraintes d'observabilité grâce à des mesures magnétiques et GNSS intermittentes, garantissant une stabilité asymptotique presque globale et une stabilité exponentielle locale.

L'essentiel

Imaginez que vous êtes un robot explorateur perdu dans une grande forêt inconnue. Votre mission est double : se repérer (savoir où vous êtes) et cartographier (savoir où se trouvent les arbres et les rochers autour de vous). C'est ce qu'on appelle le SLAM (Localisation et Cartographie Simultanées).

Voici comment les auteurs de cet article ont résolu le problème de la navigation de ce robot, expliqué simplement :

1. Le Problème : Le Robot "Amnésique"

Votre robot est équipé de deux outils principaux :

• Un gyroscope et un accéléromètre (l'IMU) : Comme un senseur d'équilibre humain, il sait comment il bouge, tourne et accélère.
• Une caméra : Il voit les arbres (les "points de repère") et peut dire : "L'arbre A est à 2 mètres devant moi".

Le souci ? Si le robot tourne sur lui-même sans bouger, ou s'il se déplace dans le vide, il perd le fil. Il sait comment il bouge, mais il ne sait pas où il est par rapport au monde réel, ni dans quelle direction il regarde (le "cap" ou le "yaw"). C'est comme si vous fermiez les yeux, tourniez en rond, puis ouvriez les yeux : vous voyez les meubles, mais vous ne savez plus si vous êtes face à la porte ou face au mur du fond. De plus, la boussole (magnétomètre) et le GPS sont souvent indisponibles ou intermittents (le signal coupe).

2. La Solution : Le "Jumeau" et le "Guide"

Les auteurs proposent une nouvelle méthode intelligente, qu'ils appellent un "Observateur Synchrone".

Imaginez que le robot a un jumeau virtuel dans son cerveau.

• Le vrai robot vit dans la réalité.
• Le jumeau virtuel essaie de deviner la position du vrai robot.

Le secret de cette méthode, c'est que le jumeau ne se contente pas de deviner. Il a un guide (une variable mathématique appelée $Z$) qui l'aide à se corriger en temps réel.

3. Les Trois Aides Magiques (Les Capteurs)

Pour que le jumeau ne se perde pas, on lui donne trois types d'indices, même si certains arrivent par intermittence :

• A. La Caméra (Les Arbres) : Le robot voit les arbres. Le jumeau compare ce qu'il voit avec ce qu'il pense voir. S'il y a une différence, il ajuste sa carte.
• B. Le GPS (La Carte du Monde) : Parfois, le GPS s'allume et dit : "Tu es exactement ici !". Mais attention, le signal coupe souvent (comme dans un tunnel).
  • L'astuce : Le jumeau est conçu pour accepter ces pauses. Il sait que le GPS est "temporairement indisponible" mais qu'il reviendra. Il continue de naviguer avec ses autres sens en attendant, et dès que le GPS revient, il se recadre instantanément. C'est comme un marin qui navigue à l'estime entre deux repères de phare.
• C. La Boussole (Le Nord) : Pour savoir exactement où est le "Nord" (le cap), le robot utilise un magnétomètre. Cela aide à corriger la direction (le "yaw") beaucoup plus vite que si on attendait seulement le GPS.

4. La Magie Mathématique (Stabilité)

Le plus impressionnant de ce papier, c'est la garantie de sécurité.
Les auteurs ont prouvé mathématiquement que peu importe où le robot commence (même avec une très mauvaise estimation au départ), son jumeau virtuel finira toujours par se synchroniser parfaitement avec la réalité.

• Quasi-globalement stable : Cela signifie que sauf dans des cas très rares et bizarres (comme si le robot était parfaitement aligné d'une manière mathématiquement impossible), le robot trouvera son chemin.
• Exponentiellement stable : Une fois qu'il commence à se rapprocher de la vérité, il accélère sa convergence. C'est comme un aimant qui attire le métal : plus il est proche, plus il colle fort et vite.

En Résumé

Cet article présente un système de navigation ultra-robuste pour les robots.

1. Il combine la vision (caméra), le mouvement (IMU), le GPS (même s'il coupe) et la boussole.
2. Il utilise une structure mathématique élégante (des "groupes de Lie", imaginez des formes géométriques complexes qui permettent de faire des rotations sans erreur) pour garder le robot sur la bonne voie.
3. Il garantit que le robot ne restera jamais perdu, même si le GPS saute et que le robot commence avec une mauvaise idée de sa position.

C'est comme donner à un explorateur une boussole, une carte, et un ami invisible qui le corrige doucement à chaque instant, même si la carte ne s'affiche que par intermittence. Le résultat ? Une navigation fluide, précise et fiable.

Résumé technique

1. Problématique

Le problème traité est la Localisation et Cartographie Simultanées (SLAM) basées sur des repères et l'inertie (LI-SLAM). Dans ce contexte, un robot mobile utilise des mesures de repères (landmarks) provenant d'une caméra (stéréo, RGB-D ou Lidar) et des données d'une centrale inertielle (IMU : accéléromètres et gyroscopes) pour estimer sa pose et la carte de l'environnement.

Cependant, le système LI-SLAM standard souffre d'un problème fondamental d'observabilité :

• Les états observables sont : le roulis et le tangage (attitude), la vitesse dans le repère du corps, et les positions relatives des repères par rapport au robot.
• Les états non observables sont : le cap (yaw) du robot, ainsi que les positions absolues du robot et des repères dans le repère inertiel.

Pour des applications pratiques (comme la navigation en extérieur en milieu urbain), il est impératif d'estimer l'attitude complète (y compris le cap) et les positions absolues. L'article propose de résoudre ce problème en intégrant des mesures supplémentaires :

1. GNSS (Global Navigation Satellite System) : Pour fournir des positions absolues, mais avec une contrainte majeure : les mesures sont intermittentes (disponibles par intermittence).
2. Magnétomètre : Pour fournir une référence de direction magnétique, aidant à la convergence rapide du cap (yaw).

2. Méthodologie

Les auteurs proposent un observateur non linéaire basé sur la théorie des observateurs synchrones et la géométrie des groupes de Lie.

A. Modélisation sur les Groupes de Lie

Le système est formulé sur le groupe de Lie étendu spécial euclidien $SE_{n+2}(3)$.

• L'état du système $X$ regroupe l'orientation $R$, la vitesse $v$, la position du robot $x$ et les positions des $n$ repères $p_i$ dans une matrice unique.
• La dynamique du système est exprimée de manière compacte en utilisant l'algèbre de Lie associée, intégrant les entrées de l'IMU, la gravité et la dynamique de la position.
• Les modèles de mesure (repères, GNSS, magnétomètre) sont également reformulés dans cette structure géométrique.

B. Architecture de l'Observateur Synchrone

L'observateur utilise une architecture synchrone composée de deux états :

1. L'estimation de l'état $\hat{X} \in SE_{n+2}(3)$.
2. Un état auxiliaire $Z \in SIM_{n+2}(3)$ (groupe de Lie des similitudes).

L'erreur d'observation est définie comme $\bar{E} = Z^{-1} X \hat{X}^{-1} Z$. Une propriété clé de cette architecture est que la dynamique de l'erreur $\bar{E}$ est indépendante de l'entrée du système et de l'estimation de l'état, ne dépendant que linéairement des termes de correction.

C. Conception des Termes de Correction

La force de la méthode réside dans sa modularité. Les auteurs conçoivent des termes de correction distincts pour chaque type de capteur, puis les somment :

• Correction GNSS : Gère la position intermittente. Le signal de disponibilité $\sigma(t)$ (valide 0 ou 1) est intégré directement dans la conception. Le terme de correction est activé uniquement lorsque le signal GNSS est disponible ($\sigma=1$).
• Correction par Repères : Utilise les positions relatives des repères pour corriger la carte et la position relative.
• Correction Magnétomètre : Utilise la direction du champ magnétique pour corriger spécifiquement le cap (yaw), accélérant ainsi la convergence de l'attitude.

Les gains de l'observateur sont choisis pour garantir la stabilité d'une fonction de Lyapunov candidate $L(\bar{E}) = |V_{\bar{E}}|^2 + \text{tr}(I_3 - R_{\bar{E}})$.

3. Contributions Clés

1. Résolution du problème d'observabilité : L'article démontre comment l'ajout de mesures GNSS intermittentes et de magnétomètres rend le système LI-SLAM entièrement observable (y compris le cap et les positions absolues).
2. Gestion de l'intermittence du GNSS : Contrairement aux approches classiques qui supposent des mesures continues, cette conception intègre théoriquement l'intermittence du signal GNSS. La convergence est prouvée sous l'hypothèse que le signal GNSS est persistamment excitant temporellement (TPE) (c'est-à-dire disponible pendant des intervalles réguliers d'une durée minimale).
3. Stabilité Globale Presque : L'observateur garantit une stabilité asymptotique presque globale (AGAS) et une stabilité exponentielle locale pour l'erreur d'estimation. Cela signifie que l'observateur converge vers la solution vraie pour presque toutes les conditions initiales, évitant les minima locaux souvent rencontrés dans les filtres de type EKF.
4. Approche Modulaire : La séparation des termes de correction par capteur simplifie la conception et l'analyse de stabilité, tout en permettant une intégration transparente de capteurs hétérogènes.

4. Résultats de Simulation

Les auteurs valident leur théorie par des simulations numériques :

• Scénario : Un robot volant en trajectoire circulaire (rayon 1m, hauteur 1m) avec 5 repères au sol.
• Conditions : Les mesures GNSS sont disponibles par intermittence (fenêtres de 5 secondes toutes les 10 secondes).
• Résultats :
  • Les erreurs d'estimation (attitude, vitesse, position du robot, position des repères) convergent vers zéro.
  • La fonction de Lyapunov est strictement décroissante.
  • La convergence de la position absolue se produit uniquement lorsque le signal GNSS est actif, confirmant la dépendance au signal intermittent.
  • L'observateur converge même avec de mauvaises estimations initiales (ex: position initiale à l'origine alors qu'elle est loin).

5. Signification et Impact

Ce travail est significatif pour la robotique mobile et la navigation inertielle pour plusieurs raisons :

• Robustesse théorique : Il offre des garanties de stabilité globales (contrairement aux méthodes EKF ou d'optimisation non linéaire qui ne garantissent que la convergence locale).
• Pragmatisme : Il adresse le défi réel de l'intermittence du GNSS (bâtiments, tunnels, arbres), un problème fréquent en navigation urbaine, sans nécessiter de modèles de bruit complexes.
• Efficacité computationnelle : En tant qu'observateur déterministe, il évite la complexité quadratique associée aux filtres de Kalman étendus (EKF) lorsque le nombre de repères augmente.
• Fondation pour l'avenir : Cette approche ouvre la voie à des systèmes de navigation plus fiables pour les drones et robots autonomes opérant dans des environnements où le GNSS n'est pas fiable en continu.

En résumé, l'article propose une solution mathématiquement rigoureuse et pratiquement applicable pour la navigation SLAM en extérieur, combinant inertie, vision, magnétométrie et GNSS intermittent avec des garanties de stabilité supérieures aux méthodes existantes.