Aller au contenu principal

Localisation du Robot avec MegaTag2

Introduit en 2024, Megatag2 est un localisateur précis et sans ambiguïté basé sur AprilTag pour les robots mobiles. Il a été conçu avec les objectifs suivants :

  • Éliminer le problème d'ambiguïté de pose et augmenter la robustesse contre le bruit d'image/de coins.
  • Fournir d'excellentes estimations de pose avec un seul tag, quelle que soit la perspective.
  • Augmenter la robustesse contre les imprécisions de placement physique des AprilTags
  • Réduire la quantité de filtrage nécessaire pour de bons résultats d'estimation de pose

Megatag2 fournit d'excellents résultats à n'importe quelle distance avec un seul tag. Cela signifie qu'il est parfaitement viable de se concentrer uniquement sur les tags qui sont à la fois pertinents et dans la tolérance, et de filtrer tous les autres tags. Si un tag n'est pas au bon emplacement, filtrez-le avec la fonctionnalité de filtre dynamique introduite avec MegaTag2.

int[] validIDs = {3,4};
LimelightHelpers.SetFiducialIDFiltersOverride("limelight", validIDs);

Contrairement à MT1, MT2 suppose que vous connaissez l'orientation (yaw) de votre robot. Optionnellement, Megatag2 accepte une orientation complète du robot et des vitesses angulaires.

Prérequis :

  • La pose de votre Limelight dans l'espace robot a été configurée dans l'interface web ou via l'API
  • Une carte de terrain (.fmap) a été téléchargée
  • LimelightHelpers.SetRobotOrientation(robotYawInDegrees,0,0,0,0,0) est appelé à chaque frame dans le code côté robot
  • SetRobotOrientation suppose une origine centrée (voir le générateur de carte) ou coin bleu. Positif dans le sens antihoraire, 0 degrés -> face au mur de l'alliance rouge en FRC.

Clés NetworkTables :

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb

Clés JSON :

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb
  • (Par cible fiducielle) t6r_fs_orb - pose du robot dans l'espace terrain utilisant megatag2 basé sur ce tag seul (pas de multitag)

Remarquez la différence entre MegaTag2 (robot rouge) et Megatag (robot bleu) dans ce cas très ambigu avec un seul tag

  • Cylindre Doré / Robot Rouge : Botpose Megatag2 non filtré
  • Cylindres Jaunes : Botposes Megatag2 single-tag non filtrés
  • Cylindre Blanc/Robot Bleu : Botpose MegaTag1
  • Cylindre Vert : Pose bot individuelle par tag (MT1)
  • Cylindre Bleu : Moyenne des poses bot individuelles par tag (MT1)
info

En 2024, la majeure partie de l'écosystème WPILib a transitionné vers un système de coordonnées à origine unique. En 2023, l'origine de votre système de coordonnées changeait en fonction de la couleur de votre alliance.

Pour 2024 et au-delà, l'origine de votre système de coordonnées devrait toujours être l'origine "bleue". Les équipes FRC devraient toujours utiliser botpose_orb_wpiblue pour les fonctionnalités liées à la pose

Utilisation de l'Estimateur de Pose WPILib​

  LimelightHelpers.SetRobotOrientation("limelight", m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
  LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
   
  // si notre vitesse angulaire est supérieure à 360 degrés par seconde, ignorer les mises à jour de vision
  if(Math.abs(m_gyro.getRate()) > 360)
  {
    doRejectUpdate = true;
  }
  if(mt2.tagCount == 0)
  {
    doRejectUpdate = true;
  }
  if(!doRejectUpdate)
  {
    m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7,.7,9999999));
    m_poseEstimator.addVisionMeasurement(
        mt2.pose,
        mt2.timestampSeconds);
  }

Configuration de la Pose Espace-Robot de votre Limelight​

LL Forward, LL Right et LL Up représentent les distances le long des vecteurs avant, droite et haut du robot comme si vous incarniez le robot. (en mètres). LL Roll, Pitch et Yaw représentent la rotation de votre Limelight en degrés. Vous pouvez modifier ces valeurs et observer le modèle 3D du Limelight changer dans le visualiseur 3D. Limelight utilise cette configuration en interne pour passer de la pose de la cible dans l'espace caméra -> pose du robot dans l'espace terrain.

Utilisation de l'IMU Intégré du Limelight 4 avec "imumode_set" / SetIMUMode()​

Le Limelight 4 possède un IMU intégré. Vous pouvez l'utiliser avec MT2 pour obtenir des estimations de pose encore plus précises lors des rotations. Vous devez initialiser l'orientation initiale de l'IMU avant que cela fonctionne. C'est ce qu'on appelle le "seeding". Une fois que votre limelight connaît l'orientation initiale de votre robot, il sera capable de mettre à jour l'orientation de votre robot par lui-même pour effectuer les calculs MT2.

Pour utiliser l'IMU du LL4, actuellement votre LL doit être monté en mode "paysage".

Le flux ressemble à ceci :

  1. Appelez SetRobotOrientation() avec votre IMU "externe" comme un Pigeon ou un NavX. Vous pouvez continuer à appeler cette méthode aussi souvent que vous le souhaitez.
  2. Appelez SetIMUMode() pour configurer comment votre Limelight utilise les données IMU des IMUs internes et externes.
  3. En général, utilisez le mode 1 pendant que votre robot attend le début de la période autonome. Passez au mode 2 lorsqu'il est activé ou lorsqu'il est activé et en rotation.

Remise à Zéro / Initialisation​

  • Pour réinitialiser le yaw fusionné du robot de l'IMU interne à la valeur de yaw soumise via SetRobotOrientation(), réglez le mode IMU de votre Limelight sur 1 avec LimelightHelpers.SetIMUMode().
  • Pendant l'initialisation, MegaTag2 continuera à utiliser la valeur de yaw soumise via SetRobotOrientation().

Utilisation de l'IMU Interne avec MegaTag2​

Pour permettre au LL4 d'utiliser son IMU interne pour la localisation MT2, réglez le mode IMU de votre Limelight sur 2 avec LimelightHelpers.SetIMUMode().

Modes IMU :​

  • 0 - Utiliser le yaw de l'IMU externe soumis via SetRobotOrientation() pour la localisation MT2. L'IMU interne est entièrement ignoré.
  • 1 - Utiliser le yaw de l'IMU externe soumis via SetRobotOrientation(), et configurer le yaw fusionné de l'IMU interne du LL4 pour correspondre à la valeur de yaw soumise.
  • 2 - Utiliser l'IMU interne pour la localisation MT2.
  • 3 -