使用 MegaTag2 进行机器人定位

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

消除姿态歧义问题并增强对图像/角点噪声的鲁棒性
仅凭单个标签就能提供出色的姿态估计，无论视角如何
增强对物理AprilTag放置不准确的鲁棒性
减少获得良好姿态估计结果所需的滤波量

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

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

与MT1不同，MT2假设您已知机器人的航向（偏航角）。可选地，Megatag2接受完整的机器人方向和角速度。

要求：

您的Limelight的机器人空间姿态已在网页UI或通过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 botposes

白色圆柱/蓝色机器人：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前向、LL右向和LL上向表示如果您站在机器人位置，沿机器人的前进、右侧和上方向量的距离（以米为单位）。 LL横滚、俯仰和偏航表示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的融合机器人偏航重置为通过SetRobotOrientation()提交的偏航值，请使用LimelightHelpers.SetIMUMode()将Limelight的IMU模式设置为1。
在种子设定期间，MegaTag2将继续使用通过SetRobotOrientation()提交的偏航值。

将内部IMU与MegaTag2一起使用

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

IMU模式：

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

使用 MegaTag2 进行机器人定位

使用WPILib的姿态估计器​

配置Limelight的机器人空间姿态​

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

归零/种子设定​

将内部IMU与MegaTag2一起使用​

IMU模式：​