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

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