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は中央配置（マップジェネレーターを参照）またはブルーコーナー原点を想定しています。反時計回りが正、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 botpose

ホワイトシリンダー/青いロボット：MegaTag1 Botpose
グリーンシリンダー：個別のタグごとのbotポーズ（MT1）
ブルーシリンダー：個別のタグごとのbotポーズの平均（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");
   
  // 角速度が毎秒360度を超える場合、ビジョン更新を無視
  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);
  }

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

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

Limelight 4の内蔵IMUを"imumode_set" / SetIMUMode()で使用する

Limelight 4には内蔵IMUがあります。これをMT2と組み合わせて使用することで、旋回中でもより正確なポーズ推定を得ることができます。これを機能させるには、IMUの初期方向をシードする必要があります。これを「シーディング」と呼びます。Limelightがロボットの初期方向を認識すると、MT2計算を実行するためにロボットの方向を自動的に更新できるようになります。

LL4 IMUを使用するには、現在LLを「ランドスケープ」モードでマウントする必要があります。

フローは次のようになります：

PigeonやNavXなどの「外部」IMUでSetRobotOrientation()を呼び出します。このメソッドは好きなだけ頻繁に呼び出し続けることができます。
SetIMUMode()を呼び出して、Limelightが内部および外部IMUからのIMUデータをどのように利用するかを設定します。
一般的に、ロボットが自律期間の開始を待っている間はモード1を使用します。有効化中、または有効化中かつ旋回中はモード2に切り替えます。

ゼロイング / シーディング

内部IMUの融合ロボットヨーをSetRobotOrientation()で送信されたヨーにリセットするには、LimelightHelpers.SetIMUMode()でLimelightのIMUモードを1に設定します。
シーディング中、MegaTag2はSetRobotOrientation()で送信されたヨー値を引き続き使用します。

MegaTag2での内部IMUの使用

LL4がMT2ローカライゼーションに内部IMUを使用できるようにするには、LimelightHelpers.SetIMUMode()でLimelightのIMUモードを2に設定します。

IMUモード：

0 - MT2ローカライゼーションにSetRobotOrientation()で送信された外部IMUヨーを使用。内部IMUは完全に無視されます。
1 - SetRobotOrientation()で送信された外部IMUヨーを使用し、LL4内部IMUの融合ヨーを送信されたヨー値に一致するように設定します。
2 - MT2ローカライゼーションに内部IMUを使用。
3 -

MegaTag2によるロボットローカライゼーション

WPILibのPose Estimatorの使用​

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

Limelight 4の内蔵IMUを"imumode_set" / SetIMUMode()で使用する​

ゼロイング / シーディング​

MegaTag2での内部IMUの使用​

IMUモード：​