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

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

Limelightのロボット空間姿勢がWeb UIで設定され、フィールドマップがWeb UI経由でアップロードされている場合、 フィールド空間におけるロボットの位置はgetBotpose()および"botpose" NetworkTablesの配列（x,y,zはメートル、roll, pitch, yawは度）で取得できます。

私たちのロボット位置推定アルゴリズムは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はこの設定を内部で使用して、カメラ空間のターゲット姿勢からフィールド空間のロボット姿勢に変換します。