Pular para o conteúdo principal

Localização de Robô com MegaTag2

Introduzido em 2024, o Megatag2 é um localizador preciso e sem ambiguidade baseado em AprilTag para robôs móveis. Foi construído com os seguintes objetivos:

  • Eliminar o problema de ambiguidade de pose e aumentar a robustez contra ruído de imagem/cantos.
  • Fornecer excelentes estimativas de pose dado uma única tag, independentemente da perspectiva.
  • Aumentar a robustez contra imprecisões físicas no posicionamento de AprilTags
  • Reduzir a quantidade de filtragem necessária para bons resultados de estimativa de pose

O Megatag2 fornece excelentes resultados a qualquer distância dada uma única tag. Isso significa que é perfeitamente viável focar apenas em tags que são relevantes e estão dentro da tolerância, e filtrar todas as outras tags. Se uma tag não está na localização correta, filtre-a com o recurso de filtro dinâmico introduzido junto com o MegaTag2.

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

Diferente do MT1, o MT2 assume que você conhece a orientação (yaw) do seu robô. Opcionalmente, o Megatag2 aceita uma orientação completa do robô e velocidades angulares.

Requisitos:

  • A pose do seu Limelight no espaço do robô foi configurada na webUI ou via API
  • Um mapa de campo (.fmap) foi carregado
  • LimelightHelpers.SetRobotOrientation(robotYawInDegrees,0,0,0,0,0) é chamado a cada frame no código do robô
  • SetRobotOrientation assume uma origem centralizada (veja o gerador de mapas) ou origem no canto azul. Positivo no sentido anti-horário, 0 graus -> voltado para a parede da aliança vermelha na FRC.

Chaves NetworkTables:

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb

Chaves JSON:

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb
  • (Por alvo fiducial) t6r_fs_orb - pose do robô no espaço do campo usando megatag2 baseado apenas nesta tag (sem multitag)

Note a diferença entre MegaTag2 (robô vermelho) e Megatag (robô azul) neste caso altamente ambíguo de tag única

  • Cilindro Dourado / Robô Vermelho: Botpose Megatag2 não filtrado
  • Cilindros Amarelos: Botposes Megatag2 de tag única não filtrados
  • Cilindro Branco/Robô Azul: Botpose MegaTag1
  • Cilindro Verde: Pose individual do bot por tag (MT1)
  • Cilindro Azul: Média das poses individuais do bot por tag (MT1)
info

Em 2024, a maior parte do Ecossistema WPILib fez a transição para um sistema de coordenadas de origem única. Em 2023, a origem do seu sistema de coordenadas mudava com base na cor da sua aliança.

Para 2024 e além, a origem do seu sistema de coordenadas deve ser sempre a origem "azul". Equipes FRC devem sempre usar botpose_orb_wpiblue para funcionalidades relacionadas a pose

Usando o Estimador de Pose da WPILib​

  LimelightHelpers.SetRobotOrientation("limelight", m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
  LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
   
  // se nossa velocidade angular for maior que 360 graus por segundo, ignorar atualizações de visão
  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 a Pose do seu Limelight no Espaço do Robô​

LL Forward, LL Right e LL Up representam distâncias ao longo dos vetores frontal, direito e superior do Robô como se você incorporasse o robô. (em metros). LL Roll, Pitch e Yaw representam a rotação do seu Limelight em graus. Você pode modificar esses valores e observar o modelo 3D do Limelight mudar no visualizador 3D. O Limelight usa essa configuração internamente para ir da pose do alvo no espaço da câmera -> pose do robô no espaço do campo.

Usando a IMU Integrada do Limelight 4 com "imumode_set" / SetIMUMode()​

O Limelight 4 tem uma IMU integrada. Você pode usar isso com o MT2 para obter estimativas de pose ainda mais precisas durante rotações. Você deve inicializar a orientação inicial da IMU antes que isso funcione. Isso é chamado de "seeding". Uma vez que seu limelight conhece a orientação inicial do seu robô, ele será capaz de atualizar a orientação do seu robô por conta própria para realizar cálculos MT2.

Para usar a IMU do LL4, atualmente seu LL deve estar montado no modo "paisagem".

O fluxo é assim:

  1. Chame SetRobotOrientation() com sua IMU "externa" como um Pigeon ou NavX. Você pode continuar chamando este método quantas vezes quiser.
  2. Chame SetIMUMode() para configurar como seu Limelight utiliza dados da IMU de IMUs internas e externas.
  3. Em geral, use o modo 1 enquanto seu robô está esperando o período autônomo começar. Mude para o modo 2 enquanto habilitado ou enquanto habilitado e girando.

Zerando / Inicializando​

  • Para resetar o yaw fundido do robô da IMU interna para o yaw enviado via SetRobotOrientation(), defina o modo IMU do seu Limelight para 1 com LimelightHelpers.SetIMUMode().
  • Durante a inicialização, o MegaTag2 continuará usando o valor de yaw enviado via SetRobotOrientation().

Usando a IMU Interna com MegaTag2​

Para permitir que o LL4 use sua IMU interna para localização MT2, defina o modo IMU do seu Limelight para 2 com LimelightHelpers.SetIMUMode().

Modos IMU:​

  • 0 - Usar yaw da IMU externa enviado via SetRobotOrientation() para localização MT2. A IMU interna é completamente ignorada.
  • 1 - Usar yaw da IMU externa enviado via SetRobotOrientation(), e configurar o yaw fundido da IMU interna do LL4 para corresponder ao valor de yaw enviado.
  • 2 - Usar IMU interna para localização MT2.
  • 3 -