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

2024年に導入されたMegatag2は、モバイルロボット向けの精密で曖昧さのないAprilTagベースの位置推定システムです。以下の目標を持って開発されました：

ポーズの曖昧さ問題を排除し、画像/コーナーノイズに対する堅牢性を向上させる
視点に関係なく、単一のタグから優れたポーズ推定を提供する
物理的なAprilTagの配置の不正確さに対する堅牢性を向上させる
良好なポーズ推定結果に必要なフィルタリングの量を削減する

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

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

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

要件：

LimelightのロボットスペースポーズがウェブUIまたは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
緑の円柱：個々のタグごとのボットポーズ（MT1）
青い円柱：個々のタグごとのボットポーズの平均（MT1）

備考

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

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

WPILibのポーズ推定器を使用する

  LimelightHelpers.SetRobotOrientation("limelight", m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
  LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
   
  // 角速度が1秒あたり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ビューアでのライムライトの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に切り替えます。

ゼロ設定/シーディング

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

内部IMUをMegaTag2で使用する

LL4が内部IMUをMT2位置推定に使用できるようにするには、LimelightHelpers.SetIMUMode()でLimelightのIMUモードを2に設定します。

IMUモード：

0 - MT2位置推定にはSetRobotOrientation()を介して送信された外部IMUヨーを使用します。内部IMUは完全に無視されます。
1 - SetRobotOrientation()を介して送信された外部IMUヨーを使用し、LL4内部IMUの融合ヨーを送信されたヨー値に一致するように設定します。
2 - MT2位置推定には内部IMUを使用します。
3 -

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

WPILibのポーズ推定器を使用する​

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

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

ゼロ設定/シーディング​

内部IMUをMegaTag2で使用する​

IMUモード：​