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

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では赤アライアンスの壁に向かって facing。

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
  • 緑の円柱：個々のタグごとのボットポーズ（MT1）
  • 青い円柱：個々のタグごとのボットポーズの平均（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) // 角速度が1秒あたり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の変化を確認できます。 Limelightは、カメラ空間におけるターゲットポーズからフィールド空間におけるロボットポーズへの内部変換にこの設定を使用します。