Saltar al contenido principal

Localización de Robot con MegaTag2

Introducido en 2024, Megatag2 es un localizador preciso y sin ambigüedades basado en AprilTag para robots móviles. Fue construido con los siguientes objetivos:

  • Eliminar el problema de ambigüedad de pose y aumentar la robustez contra ruido de imagen/esquinas.
  • Proporcionar excelentes estimaciones de pose dado un solo tag, sin importar la perspectiva.
  • Aumentar la robustez contra imprecisiones en la colocación física de AprilTags
  • Reducir la cantidad de filtrado necesario para buenos resultados de estimación de pose

Megatag2 proporciona excelentes resultados a cualquier distancia dado un solo tag. Esto significa que es perfectamente viable enfocarse solo en tags que sean relevantes y estén dentro de tolerancia, y filtrar todos los demás tags. Si un tag no está en la ubicación correcta, fíltralo con la función de filtro dinámico introducida junto con MegaTag2.

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

A diferencia de MT1, MT2 asume que conoces la orientación (yaw) de tu robot. Opcionalmente, Megatag2 acepta una orientación completa del robot y velocidades angulares.

Requisitos:

  • La pose del robot en el espacio del robot de tu Limelight ha sido configurada en la interfaz web o a través de la API
  • Un mapa de campo (.fmap) ha sido subido
  • LimelightHelpers.SetRobotOrientation(robotYawInDegrees,0,0,0,0,0) se llama cada frame en el código del lado del robot
  • SetRobotOrientation asume un origen centrado (ver el generador de mapas) o en la esquina azul. Positivo en sentido antihorario, 0 grados -> mirando hacia la pared de la alianza roja en FRC.

Claves de NetworkTables:

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb

Claves JSON:

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb
  • (Por objetivo fiducial) t6r_fs_orb - pose del robot en espacio de campo usando megatag2 basado solo en este tag (sin multitag)

Observa la diferencia entre MegaTag2 (robot rojo) y Megatag (robot azul) en este caso de un solo tag altamente ambiguo

  • Cilindro Dorado / Robot Rojo: Botpose de Megatag2 sin filtrar
  • Cilindros Amarillos: Botposes de Megatag2 de un solo tag sin filtrar
  • Cilindro Blanco/Robot Azul: Botpose de MegaTag1
  • Cilindro Verde: Pose del bot individual por tag (MT1)
  • Cilindro Azul: Promedio de poses del bot individuales por tag (MT1)
info

En 2024, la mayor parte del Ecosistema WPILib hizo la transición a un sistema de coordenadas de origen único. En 2023, el origen de tu sistema de coordenadas cambiaba según el color de tu alianza.

Para 2024 en adelante, el origen de tu sistema de coordenadas siempre debe ser el origen "azul". Los equipos de FRC siempre deben usar botpose_orb_wpiblue para funcionalidad relacionada con pose

Usando el Estimador de Pose de WPILib​

  LimelightHelpers.SetRobotOrientation("limelight", m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
  LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
   
  // si nuestra velocidad angular es mayor a 360 grados por segundo, ignorar actualizaciones de visión
  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);
  }

Configurando la Pose del Espacio del Robot de tu Limelight​

LL Forward, LL Right, y LL Up representan distancias a lo largo de los vectores hacia adelante, derecha y arriba del Robot como si encarnaras al robot. (en metros). LL Roll, Pitch, y Yaw representan la rotación de tu Limelight en grados. Puedes modificar estos valores y ver cómo cambia el modelo 3D del Limelight en el visor 3D. Limelight usa esta configuración internamente para ir de la pose del objetivo en espacio de cámara -> pose del robot en espacio de campo.

Usando el IMU Integrado del Limelight 4 con "imumode_set" / SetIMUMode()​

Limelight 4 tiene un IMU integrado. Puedes usar esto con MT2 para obtener estimaciones de pose aún más precisas mientras giras. Debes sembrar la orientación inicial del IMU antes de que esto funcione. Esto se llama "seeding" (sembrado). Una vez que tu limelight conoce la orientación inicial de tu robot, podrá actualizar la orientación de tu robot por sí solo para realizar cálculos de MT2.

Para usar el IMU del LL4, actualmente tu LL debe estar montado en modo "landscape" (horizontal).

El flujo se ve así:

  1. Llama a SetRobotOrientation() con tu IMU "externo" como un Pigeon o un NavX. Puedes continuar llamando este método tan frecuentemente como desees.
  2. Llama a SetIMUMode() para configurar cómo tu Limelight utiliza datos del IMU de IMUs internos y externos.
  3. En general, usa el modo 1 mientras tu robot está esperando que comience el período autónomo. Cambia al modo 2 mientras está habilitado o mientras está habilitado y girando.

Puesta a Cero / Sembrado​

  • Para resetear el yaw fusionado del robot del IMU interno al yaw enviado vía SetRobotOrientation(), configura el modo IMU de tu Limelight a 1 con LimelightHelpers.SetIMUMode().
  • Mientras se siembra, MegaTag2 continuará usando el valor de yaw enviado vía SetRobotOrientation().

Usando el IMU Interno con MegaTag2​

Para permitir que LL4 use su IMU interno para localización MT2, configura el modo IMU de tu Limelight a 2 con LimelightHelpers.SetIMUMode().

Modos de IMU:​

  • 0 - Usar yaw del IMU externo enviado vía SetRobotOrientation() para localización MT2. El IMU interno se ignora completamente.
  • 1 - Usar yaw del IMU externo enviado vía SetRobotOrientation(), y configurar el yaw fusionado del IMU interno del LL4 para que coincida con el valor de yaw enviado.
  • 2 - Usar IMU interno para localización MT2.
  • 3 -