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

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は、カメラ空間内のターゲットポーズからフィールド空間内のロボットポーズへの変換を内部的に行うために、この設定を使用します。