Localización del Robot con MegaTag

Si la pose del robot en el espacio del robot de tu Limelight ha sido configurada en la interfaz web, y se ha subido 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 de networktables "botpose" (x,y,z en metros, roll, pitch, yaw en grados).

Nuestro algoritmo de localización de robots se llama MegaTag. Si más de una etiqueta está visible, 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 cierto riesgo de inversión por ambigüedad.

• Cilindro Verde: Pose del bot individual por etiqueta
• Cilindro Azul: BotPose antiguo
• Cilindro Blanco: Botpose MegaTag

Observa la obvia ambigüedad de pose aquí:

Nota cómo el nuevo botpose (cilindro blanco) es extremadamente estable comparado con el antiguo botpose (cilindro azul). También puedes observar los valores de tx y ty.

Esto no está restringido a etiquetas planares. Escala a cualquier número de etiquetas en 3D completo y en cualquier orientación. Las etiquetas en el suelo y en el 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 un punto. A medida que el MegaTag combinado 3D aumenta en tamaño y en cantidad de puntos clave, su estabilidad aumenta.

Usando el Estimador de Pose de WPILib

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_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 Robot en el 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 estuvieras encarnando 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 el modelo 3D del Limelight cambia 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.