使用 MegaTag2 进行机器人定位

MegaTag2 于 2024 年推出，是一款精确且无歧义的基于 AprilTag 的移动机器人定位器。它的设计目标如下：

消除位姿歧义问题，提高对图像/角点噪声的鲁棒性
无论从什么角度，都能在单个标签的情况下提供出色的位姿估计
提高对 AprilTag 物理放置误差的鲁棒性
减少获得良好位姿估计结果所需的滤波量

MegaTag2 在任何距离下，仅凭单个标签就能提供出色的结果。这意味着只关注相关且在容差范围内的标签，并过滤掉所有其他标签是完全可行的。如果某个标签位置不正确，可以使用与 MegaTag2 一同推出的动态过滤功能将其过滤掉。

int[] validIDs = {3,4};
LimelightHelpers.SetFiducialIDFiltersOverride("limelight", validIDs);

与 MT1 不同，MT2 假设您知道机器人的航向角（yaw）。可选地，MegaTag2 还接受完整的机器人姿态和角速度。

要求：

您的 Limelight 的机器人空间位姿已在 webUI 或通过 API 配置
已上传场地地图（.fmap）
在机器人端代码中每帧调用 LimelightHelpers.SetRobotOrientation(robotYawInDegrees,0,0,0,0,0)
SetRobotOrientation 假设使用居中（参见地图生成器）或蓝色角落原点。逆时针为正，0 度 -> 在 FRC 中面向红色联盟墙

NetworkTables 键：

botpose_orb_wpiblue
botpose_orb_wpired
botpose_orb

JSON 键：

botpose_orb_wpiblue
botpose_orb_wpired
botpose_orb
（每个基准目标）t6r_fs_orb - 仅基于此标签使用 megatag2 计算的场地空间中的机器人位姿（非多标签）

注意在这个高度歧义的单标签情况下，MegaTag2（红色机器人）和 MegaTag（蓝色机器人）之间的差异

金色圆柱体 / 红色机器人：未过滤的 MegaTag2 botpose
黄色圆柱体：未过滤的单标签 MegaTag2 botpose

白色圆柱体/蓝色机器人：MegaTag1 Botpose
绿色圆柱体：单个标签的机器人位姿（MT1）
蓝色圆柱体：单个标签机器人位姿的平均值（MT1）

信息

在 2024 年，大部分 WPILib 生态系统过渡到了单一原点坐标系统。在 2023 年，您的坐标系统原点会根据您的联盟颜色而改变。

对于 2024 年及以后，您的坐标系统原点应始终为"蓝色"原点。FRC 团队应始终使用 botpose_orb_wpiblue 进行位姿相关功能

使用 WPILib 的位姿估计器

  LimelightHelpers.SetRobotOrientation("limelight", m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
  LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
   
  // 如果我们的角速度大于每秒 360 度，忽略视觉更新
  if(Math.abs(m_gyro.getRate()) > 360)
  {
    doRejectUpdate = true;
  }
  if(mt2.tagCount == 0)
  {
    doRejectUpdate = true;
  }
  if(!doRejectUpdate)
  {
    m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7,.7,9999999));
    m_poseEstimator.addVisionMeasurement(
        mt2.pose,
        mt2.timestampSeconds);
  }

配置您的 Limelight 的机器人空间位姿

LL Forward、LL Right 和 LL Up 表示沿机器人前向、右向和上向向量的距离，就像您置身于机器人中一样（单位：米）。 LL Roll、Pitch 和 Yaw 表示您的 Limelight 的旋转角度（单位：度）。您可以修改这些值并在 3D 查看器中观察 Limelight 3D 模型的变化。 Limelight 在内部使用此配置从相机空间中的目标位姿 -> 场地空间中的机器人位姿。

使用 Limelight 4 的内置 IMU 与 "imumode_set" / SetIMUMode()

Limelight 4 有一个内置 IMU。您可以将其与 MT2 一起使用，以在转弯时获得更准确的位姿估计。在此功能生效之前，您必须为 IMU 设置初始方向。这称为"播种"。一旦您的 Limelight 知道机器人的初始方向，它就能够自行更新机器人的方向以执行 MT2 计算。

要使用 LL4 IMU，目前您的 LL 必须以"横向"模式安装。

流程如下：

使用您的"外部"IMU（如 Pigeon 或 NavX）调用 SetRobotOrientation()。您可以根据需要继续调用此方法。
调用 SetIMUMode() 来配置您的 Limelight 如何利用来自内部和外部 IMU 的数据。
通常，在机器人等待自主阶段开始时使用模式 1。在启用时或在启用并转弯时切换到模式 2。

归零 / 播种

要将内部 IMU 的融合机器人 yaw 重置为通过 SetRobotOrientation() 提交的 yaw，请使用 LimelightHelpers.SetIMUMode() 将 Limelight 的 IMU 模式设置为 1。
在播种期间，MegaTag2 将继续使用通过 SetRobotOrientation() 提交的 yaw 值。

将内部 IMU 与 MegaTag2 一起使用

要允许 LL4 使用其内部 IMU 进行 MT2 定位，请使用 LimelightHelpers.SetIMUMode() 将 Limelight 的 IMU 模式设置为 2。

IMU 模式：

0 - 使用通过 SetRobotOrientation() 提交的外部 IMU yaw 进行 MT2 定位。内部 IMU 完全被忽略。
1 - 使用通过 SetRobotOrientation() 提交的外部 IMU yaw，并配置 LL4 内部 IMU 的融合 yaw 以匹配提交的 yaw 值。
2 - 使用内部 IMU 进行 MT2 定位。
3 -

使用 MegaTag2 进行机器人定位

使用 WPILib 的位姿估计器​

配置您的 Limelight 的机器人空间位姿​

使用 Limelight 4 的内置 IMU 与 "imumode_set" / SetIMUMode()​

归零 / 播种​

将内部 IMU 与 MegaTag2 一起使用​

IMU 模式：​