8. AprilTag识别与位姿显示

本章使用ROS2、AprilTag和OpenCV完成机器人视觉标签识别。程序从深度相机获取RGB图像和相机标定信息，识别tag36h11标签，计算标签相对相机的三维位置和姿态，并在RViz2中同时显示机器人模型、标签坐标系、标签标记和实时检测画面。

课程程序已经集成机器人控制器、机械臂初始动作、Aurora深度相机、AprilTag检测节点和RViz2。使用一条Launch命令即可启动完整实验。

8.1.1 本章能完成什么

完成本章学习后，用户可以：

• 使用一条ROS2 Launch命令启动机器人、相机、AprilTag检测和RViz2。
• 识别tag36h11标签的ID、中心点和四个角点。
• 在实时图像中查看标签边框、ID、标签族和检测质量。
• 在RViz2中观察机器人模型以及tag_1、tag_2、tag_3坐标系。
• 使用ROS2话题和TF2命令读取标签检测结果与三维位姿。
• 调整标签尺寸、降采样倍数和汉明距离参数，并比较识别效果。

8.1.2 实验学习路径

AprilTag基础概念 → 准备并测量标签 → 启动基础识别 → 查看图像与RViz2 → 使用命令行读取检测结果 → 调整检测参数 → 恢复默认配置。

8.2.1 什么是AprilTag

AprilTag是一种用于机器人视觉定位的黑白人工标记。每张标签由黑白方格组成，程序能够从图像中获得以下信息：

• 标签所属的标签族，例如tag36h11。
• 标签ID，例如1、2和3。
• 标签中心点和四个角点在图像中的像素坐标。
• 标签相对摄像头的三维位置和旋转姿态。

AprilTag不仅可以表示编号，还可以作为机器人定位、自动停靠、机械臂抓取和相机标定的空间基准。

8.2.2 标签族与ID

标签族规定了标签内部的数据位数量、编码方式和可纠正的错误数量。本课程使用：

tag36h11

其中：

• 36表示标签包含36个数据位。
• h11表示标签编码之间具有较大的汉明距离，有利于降低误识别概率。
• 同一标签族内的每张标签使用不同ID进行区分。

课程默认使用下表中的标签：

8.2.3 从二维图像到三维位姿

相机图像只能直接提供二维像素位置。程序结合以下数据计算标签三维位姿：

1. 标签四个角点的像素坐标。
2. 相机内参，包括焦距和主点位置。
3. 相机畸变参数。
4. 标签的实际边长。

程序使用PnP方法估计标签相对相机的平移和旋转，并通过TF2发布标签坐标系。

标签四角像素坐标 + 相机内参 + 标签实际尺寸
                    ↓
                  PnP
                    ↓
         标签相对相机的三维位姿
                    ↓
             /tf中的tag_1等坐标系

8.2.4 标签打印与测量

1. 使用tag36h11标签族生成或下载ID为1、2和3的标签。

2. 打印时关闭“适应页面”“缩放到纸张”等自动缩放选项。

3. 标签四周保留完整的白色区域，不要紧贴黑色外框裁剪。

4. 打印后测量黑色外边框的实际边长。本课程默认值为：

33 mm = 0.033 m

5. 将标签固定在平整、坚硬的表面，避免标签弯曲、反光或出现折痕。

8.3.1 你将完成什么

启动课程程序后，机器人机械臂首先执行初始动作，随后启动深度相机、AprilTag检测节点和RViz2。

将ID为1、2或3的标签放入摄像头视野后：

• 实时图像使用绿色线条标记标签四边。
• 红色圆点标记标签中心。
• 图像显示标签ID和decision_margin。
• RViz2显示绿色标签平面和标签TF坐标轴，并可选显示ID文字。
• 标签移动或旋转时，RViz2中的标签坐标系同步变化。

图：RViz2同时显示机器人模型与tag_1标签TF坐标系。

8.3.2 实验目标

完成本实验后，用户可以：

• 正确启动和关闭AprilTag课程程序。
• 使用ID 1、2、3标签完成单标签和多标签识别。
• 区分二维检测结果和三维TF位姿。
• 使用ROS2命令检查图像、检测消息和TF。
• 根据画面现象排查标签族、标签尺寸和光照问题。

8.3.3 实验准备

开始实验前，请确认：

• 机器人已正常启动，并通过NoMachine连接远程桌面。
• 相机连接正常，摄像头镜头无遮挡。
• 已准备ID为1、2和3的tag36h11标签。
• 标签黑色外边框边长与配置值33 mm一致。
• 机械臂周围没有人员和障碍物。
• 没有运行其他会占用深度相机或机器人控制器的程序。

8.3.4 开始实验

1. 启动机器人，并使用NoMachine连接机器人远程桌面。关于连接方法，可参考 1 教程资料/1 机器人快速使用指南/ROSOrin Pro使用手册/7.2直连模式连接步骤。

2. 点击系统桌面的终端图标，打开命令行终端。

3. 输入以下命令，停止APP自启服务：

sudo systemctl stop start_app_node.service

4. 确认其他视觉课程和相机Launch已经停止。若某个程序仍在运行，请返回对应终端按“Ctrl+C”。

5. 清除机械臂周围障碍物，然后输入以下命令启动课程程序：

/home/ubuntu/ros2_ws/src/apriltag_course/scripts/run_course.sh

当前命令行终端需要保持运行。

6. 等待RViz2窗口打开。第一次启动时，模型和相机可能需要数秒完成初始化。

7. 手持ID为1的标签，使标签正面朝向摄像头，并放置在摄像头视野中央。

8. 缓慢改变标签的距离、位置和角度，观察检测图像和RViz2中的变化。

9. 依次更换ID为2和3的标签，然后尝试同时放入两张或三张标签。

8.3.5 查看实验结果

实时检测图像

检测图像的话题为：

/apriltag/detections_image

程序正常识别后，检查以下内容：

• 标签四周出现绿色边框。
• 标签中心出现红色圆点。
• 标签旁显示id=1、id=2或id=3。
• ID旁显示margin=检测质量数值。
• 画面左上角显示Detected tags: N，其中N为当前识别数量。

decision_margin越大，通常表示当前标签黑白区域区分越明显。该值适合用于同一环境下的相对比较，不建议作为所有环境通用的固定合格线。

RViz2三维界面

RViz2默认包含以下显示项：

RViz2的Fixed Frame默认为：

base_footprint

当标签在摄像头前移动时，tag_1等坐标系会相对机器人同步移动。标签绕自身旋转时，标签坐标轴方向也会发生变化。

图：RViz2同时显示检测图像、机器人模型以及tag_1标签TF坐标系。

8.3.6 观察识别结果

课程启动后，主要通过RViz2观察AprilTag识别结果：

• 在Detection Image中查看标签数量、ID、margin检测质量、标签边框和中心点。
• 在三维区域中查看机器人模型以及tag_1、tag_2、tag_3坐标系的位置和方向。
• 缓慢移动或旋转标签，观察检测图像和三维坐标系是否同步变化。

8.3.7 结束实验

1. 返回运行run_course.sh的命令行终端。

2. 按“Ctrl+C”停止程序。

3. 等待AprilTag检测、RViz2、相机和机器人控制节点退出。

4. 输入以下命令恢复APP自启服务：

sudo systemctl restart start_app_node.service

APP自启服务恢复后，机械臂可能再次执行系统初始动作，请保持机械臂周围区域空旷。

8.3.8 完成标准

满足以下条件即表示完成本节实验：

• 能够使用一条命令启动机器人、相机、AprilTag检测和RViz2。
• ID 1、2、3至少能够分别完成一次稳定识别。
• 检测图像能够显示绿色边框、红色中心点、标签ID和检测数量。
• RViz2能够同时显示机器人模型、标签Marker和标签TF。
• 移动标签时，图像标记和RViz2坐标系能够同步变化。
• 能够使用tf2_echo查看标签相对相机的三维位姿。
• 能够使用“Ctrl+C”结束程序并恢复APP自启服务。

本节只介绍课程程序的主要数据流和输出。检测参数的调整方法可参考8.4节。

8.5.1 程序工作流程

运行课程脚本后，Launch文件会启动机器人和摄像头、AprilTag检测节点、可视化节点以及RViz2：

run_course.sh
    ↓
apriltag_course.launch.py
    ├─ 启动机器人并执行初始动作
    ├─ 启动深度摄像头
    ├─ 识别AprilTag并计算位姿
    ├─ 绘制检测图像和标签Marker
    └─ 在RViz2中显示机器人与检测结果

摄像头提供RGB图像和标定信息。AprilTag节点根据图像中的四角位置、标签实际边长和相机参数，计算标签ID及其相对于摄像头的三维位姿。

8.5.2 主要文件

课程程序位于/home/ubuntu/ros2_ws/src/apriltag_course，主要文件如下：

图像经过检测和可视化后的数据关系如下：

摄像头图像与标定信息
        ↓
camera_frame_relay
        ↓
AprilTag检测节点 ──> 标签ID与三维位姿
        ↓
tag_visualizer ────> 检测图像与标签Marker

8.5.3 检测结果与坐标系

课程主要使用以下三个输出：

标签位姿以摄像头坐标系depth_cam_frame为参考。课程涉及的主要坐标关系如下：

base_footprint
    └─ ...机器人与相机安装坐标系...
           └─ depth_cam_frame
                  ├─ tag_1
                  ├─ tag_2
                  └─ tag_3

tag_1、tag_2和tag_3分别对应ID为1、2和3的标签。只有摄像头识别到相应标签时，该标签坐标系才会出现在RViz2和TF中。

• AprilTag官方实现：https://github.com/AprilRobotics/apriltag
• AprilTag官方标签图片：https://github.com/AprilRobotics/apriltag-imgs
• ROS2 Humble apriltag_ros文档：https://docs.ros.org/en/ros2_packages/humble/api/apriltag_ros/
• RViz2用户文档：https://docs.ros.org/en/ros2_packages/humble/api/rviz2/