🌐 3D Spatial Pose Tracking AprilTags

1. Course Overview

The function implemented in this lesson can be simply understood as: hold the AprilTag and move it anywhere in space, the robotic arm end-effector follows omnidirectionally in 3D like a magnet.

The system uses visual computation to obtain the AprilTag's 3D spatial coordinates in the camera frame in real-time. Through three-channel independent PID (X-axis yaw, Y-axis pitch, Z-axis depth distance), it closed-loop controls the robotic arm's base rotation, up/down lifting, and front/rear pushing/pulling, achieving omnidirectional dynamic following of the gripper end-effector in 3D space.

2. Preparation and Running

This case requires opening 2 terminals to start the system sequentially:

2.1 Start Camera Driver and Kinematics Solver Environment

Run in Terminal 1:

ros2 launch dofbot_pro_driver A1pro_camera_arm.launch.py

2.2 Start 3D Spatial Tracking Node

Run in Terminal 2:

​x
# Depth camera version (Nuwa)
ros2 run dofbot_pro_driver apriltag_follow

# Or USB camera version
ros2 run dofbot_pro_usb apriltag_follow

3. Operation Demo and Interaction Flow

1. Initial standby state: After node starts, the image upper-left corner shows STANDBY, robotic arm is stationary at patrol pose;

2. Start 3D following:

   • Keep the image window active, press the spacebar (Space);

   • Status switches to highlighted green TRACKING, robotic arm starts 3D spatial closed-loop tracking;

3. Spatial block movement guidance:

   • Hold the AprilTag block and move it slowly and smoothly in 3D space in front of the robotic arm:

     • Left/right translation: Joint 1 base servo rotates left/right smoothly for following;

     • Up/down lifting: Joints 2/3/4 pitching coordinate to lift or press down;

     • Front/rear pushing/pulling: Robotic arm automatically adjusts link extension to maintain constant lens-to-block distance;

4. Pause and reset: Press spacebar again, tracking pauses immediately, robotic arm automatically returns to initial pose and resets each channel PID integral.

4. Core Source Code Analysis

The 3D servo tracking system is driven by underlying vision recognition and three-channel PID algorithm:

The main technical implementation comparison:

4.1 Three-Channel PID Independent Decoupling Control

Compared with 2D plane tracking, 3D tracking adds independent closed-loop control of the Z-axis depth channel, aligning the target center across three dimensions separately:

xxxxxxxxxx
# apriltag_follow.py —— Three-axis PID initialization
self.pid_x_params = (0.35, 0.0, 1.0)  # X axis (left/right rotation adjustment)
self.pid_y_params = (1.00, 0.0, 1.0)  # Y axis (pitch up/down adjustment)
self.pid_z_params = (3.00, 0.0, 0.5)  # Z axis (front/rear push/pull adjustment)

# Error input and calculation
raw_x = self.pid_x.compute(center_x, TARGET_X)
raw_y = self.pid_y.compute(center_y, TARGET_Y)
raw_z = self.pid_z.compute(depth_val, TARGET_DEPTH)

4.2 Multi-Joint Feed-Forward Fusion Mapping Algorithm

To prevent the robotic arm's lens pitch perspective from changing during front/rear extension, the depth error calculated angular_z is simultaneously superimposed onto Joint2, Joint3, and Joint4, achieving a "front/rear push/pull" motion similar to a parallelogram mechanism:

xxxxxxxxxx
# apriltag_follow.py —— XYZ_track() joint fusion
# X yaw: controls joint 1 base left/right rotation
target_j1 = base_j1 + (10 * angular_x * self.gain_j1)

# Y pitch + Z extension: multi-axis coordinated linear superimposition
target_j2 = base_j2 + (angular_y * self.gain_pitch_j2) + (angular_z * self.gain_reach_j2)
target_j3 = base_j3 + (angular_y * self.gain_pitch_j3) + (angular_z * self.gain_reach_j3)
target_j4 = base_j4 + (angular_y * self.gain_pitch_j4) + (angular_z * self.gain_reach_j4)

4.3 Independent Slope Limiting and Safe Reset

Each axis is equipped with an independent per-frame maximum change slope limiter (max_step_change_xy = 0.015, max_step_change_z = 0.02), effectively filtering abrupt impacts from image detection frame drops or hand jitter; when the user presses spacebar again to exit, the system automatically clears integral cache and smoothly returns to position:

xxxxxxxxxx
# apriltag_follow.py —— Spacebar safe switching and reset
if key == 32:
    self.tracking_active = not self.tracking_active
    if not self.tracking_active:
        self.pubSixArm(self.init_joints)  # Reset to safe pose
        self.pid_x.reset()
        self.pid_y.reset()
        self.pid_z.reset()

Through this 3D decoupling control architecture, whether using USB monocular lens or 3D depth camera, the robotic arm can present magnet-like tightly smooth dynamic following in 3D space.