メインコンテンツまでスキップ

MegaTag2によるロボット位置推定

2024年に導入されたMegatag2は、AprilTagベースの高精度で曖昧さのない移動ロボット用位置推定システムです。以下の目標を掲げて開発されました：

  • ポーズの曖昧さの問題を解消し、画像/コーナーノイズに対する堅牢性を向上
  • 視点に関係なく、単一のタグで優れたポーズ推定を提供
  • 物理的なAprilTagの設置誤差に対する堅牢性を向上
  • 良好なポーズ推定結果を得るために必要なフィルタリングを削減

Megatag2は、単一のタグでも任意の距離で優れた結果を提供します。 これは、関連性があり許容範囲内にあるタグのみに焦点を当て、他のすべてのタグをフィルタリングすることが完全に実用的であることを意味します。タグが正しい位置にない場合は、MegaTag2と共に導入された動的フィルター機能でフィルタリングできます。

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

MT1とは異なり、MT2はロボットの方位（ヨー）を既知としています。オプションで、Megatag2は完全なロボットの向きと角速度を受け入れます。

要件：

  • LimelightのロボットスペースポーズがwebUIまたはAPIで設定されていること
  • フィールドマップ（.fmap）がアップロードされていること
  • LimelightHelpers.SetRobotOrientation(robotYawInDegrees,0,0,0,0,0)がロボット側コードで毎フレーム呼び出されること
  • SetRobotOrientationは中央（マップジェネレーターを参照）またはブルーコーナーを原点とします。CCW正、0度→FRCの赤アライアンスウォールに向かって

NetworkTablesキー：

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb

JSONキー：

  • botpose_orb_wpiblue
  • botpose_orb_wpired
  • botpose_orb
  • （フィデューシャルターゲットごとに）t6r_fs_orb - このタグのみを使用したmegatag2ベースのフィールドスペースでのロボットポーズ（マルチタグなし）

この非常に曖昧な単一タグのケースにおける、MegaTag2（赤ロボット）とMegatag（青ロボット）の違いに注目してください

  • 金色の円柱/赤ロボット：フィルタリングされていないMegatag2 botpose
  • 黄色の円柱：フィルタリングされていない単一タグMegatag2 botposes
  • 白い円柱/青ロボット：MegaTag1 Botpose
  • 緑の円柱：タグごとの個別botpose（MT1）
  • 青い円柱：タグごとの個別botposeの平均（MT1）
備考

2024年、WPILibエコシステムの大部分が単一原点座標系に移行しました。 2023年では、アライアンスカラーに基づいて座標系の原点が変更されていました。

2024年以降、座標系の原点は常に「青」の原点であるべきです。FRCチームはポーズ関連の機能に常にbotpose_orb_wpiblueを使用する必要があります。

WPILibのPose Estimatorの使用​

  LimelightHelpers.SetRobotOrientation("limelight", m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
      LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
      if(Math.abs(m_gyro.getRate()) > 720) // 角速度が720度/秒を超える場合、ビジョンの更新を無視
      {
        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);
      }

Limelightのロボットスペースポーズの設定​

LL Forward、LL Right、LL Upは、ロボットに視点を置いた場合のロボットの前方、右、上方向のベクトルに沿った距離（メートル単位）を表します。 LL Roll、Pitch、Yawは、Limelightの回転を度単位で表します。これらの値を変更すると、3Dビューアでライムライトの3Dモデルの変化を確認できます。 Limelightはこの設定を内部的に使用して、カメラ空間でのターゲットポーズからフィールド空間でのロボットポーズへの変換を行います。