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

MegaTagを使用したロボットの位置推定

Limelightのロボット空間ポーズがウェブUIで設定され、フィールドマップがウェブUIを通してアップロードされている場合、getBotpose()と"botpose"ネットワークテーブル配列（x,y,z：メートル単位、ロール、ピッチ、ヨー：度単位）を通してフィールド空間でのロボットの位置が利用可能になります。

私たちのロボット位置推定アルゴリズムはMegaTagと呼ばれています。複数のタグが視界に入っている場合、個々のタグのあいまい性や画像のノイズに対して耐性があります。 すべてのキーポイントが同一平面上にある場合でも、あいまい性の反転のリスクは依然として存在します。

  • 緑色の円柱：個々のタグごとのボットポーズ
  • 青色の円柱：古いBotPose
  • 白色の円柱：MegaTag Botpose

ここでの明らかなポーズのあいまい性に注目してください：

新しいbotpose（白色の円柱）が古いbotpose（青色の円柱）と比較して非常に安定していることに注目してください。txとty値も確認できます。

これは平面タグに限定されません。完全な3Dで任意の数のタグと任意の向きに対応します。床のタグと天井のタグも完璧に機能します。

これがシンプルな平面の場合でどのように機能するかを示す図です。 実際の結果は図で示されているものよりも優れています。図のMegaTagは1点ではなく3点に大きな誤差が適用されているためです。 3D結合MegaTagのサイズとキーポイント数が増えるにつれて、その安定性は向上します。

megatag botposeの例:

WPILibのPose Estimatorの使用​

備考

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

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

 LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
      
      if(mt1.tagCount == 1 && mt1.rawFiducials.length == 1)
      {
        if(mt1.rawFiducials[0].ambiguity > .7)
        {
          doRejectUpdate = true;
        }
        if(mt1.rawFiducials[0].distToCamera > 3)
        {
          doRejectUpdate = true;
        }
      }
      if(mt1.tagCount == 0)
      {
        doRejectUpdate = true;
      }

      if(!doRejectUpdate)
      {
        m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5,.5,9999999));
        m_poseEstimator.addVisionMeasurement(
            mt1.pose,
            mt1.timestampSeconds);
      }

Limelightのロボット空間ポーズの設定​

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