דלג לתוכן הראשי

לוקליזציית רובוט עם MegaTag

אם תנוחת הרובוט-מרחב של ה-Limelight שלך הוגדרה בממשק האינטרנט, ומפת שדה הועלתה דרך ממשק האינטרנט, אז מיקום הרובוט במרחב השדה יהיה זמין דרך getBotpose() ומערך ה-networktables "botpose" (x,y,z במטרים, roll, pitch, yaw במעלות).

אלגוריתם הלוקליזציה שלנו נקרא MegaTag. אם יותר מתג אחד נמצא בשדה הראייה, הוא עמיד לאי-בהירויות של תגים בודדים ולרעש בתמונה. אם כל נקודות המפתח נמצאות על מישור אחד, עדיין קיים סיכון מסוים להיפוך אי-בהירות.

  • גליל ירוק: תנוחת רובוט לכל תג בנפרד
  • גליל כחול: BotPose ישן
  • גליל לבן: MegaTag Botpose

שימו לב לאי-בהירות התנוחה הברורה כאן:

שימו לב כמה ה-botpose החדש (גליל לבן) יציב במיוחד בהשוואה ל-botpose הישן (גליל כחול). אתם יכולים לצפות גם בערכי tx ו-ty.

זה לא מוגבל לתגים מישוריים. זה מתרחב לכל מספר של תגים בתלת-ממד מלא ובכל כיוון. תגים על הרצפה ותגים על התקרה עובדים בצורה מושלמת.

הנה דיאגרמה המדגימה היבט אחד של איך זה עובד עם מקרה מישורי פשוט. התוצאות למעשה טובות יותר ממה שמתואר, מכיוון שה-MegaTag המתואר כולל שגיאה משמעותית שהוחלה על שלוש נקודות במקום נקודה אחת. ככל שה-MegaTag התלת-ממדי המשולב גדל בגודל ובמספר נקודות המפתח, היציבות שלו עולה.

דוגמת megatag botpose:

שימוש ב-Pose Estimator של WPILib​

info

ב-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 שלכם במעלות. אתם יכולים לשנות ערכים אלה ולצפות במודל התלת-ממדי של ה-Limelight משתנה בצופה התלת-ממדי. Limelight משתמש בהגדרה זו באופן פנימי כדי לעבור מתנוחת המטרה במרחב המצלמה -> תנוחת הרובוט במרחב השדה.