Localización del Robot con MegaTag

Si la pose del espacio del robot de tu Limelight ha sido configurada en la interfaz web, y se ha cargado un mapa del campo a través de la interfaz web, entonces la ubicación del robot en el espacio del campo estará disponible a través de getBotpose() y el array "botpose" de networktables (x,y,z en metros, roll, pitch, yaw en grados).

Nuestro algoritmo de localización de robots se llama MegaTag. Si hay más de una etiqueta a la vista, es resistente a las ambigüedades individuales de las etiquetas y al ruido en la imagen. Si todos los puntos clave son coplanares, todavía existe algún riesgo de ambigüedad.

• Cilindro Verde: Pose individual del robot por etiqueta
• Cilindro Azul: BotPose Antigua
• Cilindro Blanco: BotPose MegaTag

Nota la obvia ambigüedad de pose aquí:

Observa cómo la nueva botpose (cilindro blanco) es extremadamente estable en comparación con la antigua botpose (cilindro azul). También puedes observar los valores tx y ty.

Esto no se limita a etiquetas planares. Se escala a cualquier número de etiquetas en 3D completo y en cualquier orientación. Las etiquetas de suelo y techo funcionan perfectamente.

Aquí hay un diagrama que demuestra un aspecto de cómo funciona esto con un caso planar simple. Los resultados son en realidad mejores de lo que se muestra, ya que el MegaTag representado tiene un error significativo aplicado a tres puntos en lugar de uno. A medida que el MegaTag 3D combinado aumenta en tamaño y en número de puntos clave, su estabilidad aumenta.

Usando el Estimador de Pose de WPILib

info

En 2024, la mayoría 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 y más allá, el origen de tu sistema de coordenadas siempre debe ser el origen "azul". Los equipos de FRC siempre deben usar botpose_wpiblue para funcionalidades relacionadas con la pose.

 LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
      
      if(mt1.tagCount == 1 && mt1.rawFiducials.length == 1)
      {
        if(mt1.rawFiducials[0].ambiguity > .7)
        {
          doRejectUpdate = true;
        }
        if(mt1.rawFiducials[0].distToCamera > 3)
        {
          doRejectUpdate = true;
        }
      }
      if(mt1.tagCount == 0)
      {
        doRejectUpdate = true;
      }

      if(!doRejectUpdate)
      {
        m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5,.5,9999999));
        m_poseEstimator.addVisionMeasurement(
            mt1.pose,
            mt1.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 si fueras a personificar el 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 pasar de la pose del objetivo en el espacio de la cámara -> pose del robot en el espacio del campo.