3D Spatial Tracking AprilTags

1. Course Content

The feature implements the program acquiring images through the camera and recognizing the AprilTag. When the AprilTag is moved up/down, left/right, and forward/backward, the robotic arm follows the AprilTag's movement.

2. Preparation

The wooden block used in this section is: 40x40x40mm AprilTag wooden block

xxxxxxxxxx
ros2 launch yahboomcar_bringup arm_demo.launch.py demo:=apriltag_follow

3. Case Demonstration

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

• Left/right panning: base servo No. 1 rotates smoothly left and right to follow;

• Up/down raising and lowering: pitch joints No. 2/3/4 cooperate to raise or press down;

• Forward/backward pushing and pulling: the robotic arm automatically adjusts the linkage extension amount, keeping the distance between the camera and the wooden block constant;

4. Core Source Code Analysis

• Program path:

xxxxxxxxxx
~/yahboomcar_ros2_ws/yahboomcar_ws/src/common_demo/common_demo/arm_demo/apriltag_follow.py

xxxxxxxxxx

    def track(self, center_x, center_y):
        """Three-channel PID: pixel X -> J1, pixel Y -> J2/J3/J4 pitch, depth Z -> J2/J3/J4 reach + J6 gripper"""

        # --- Channel 1: left/right (pixel X -> J1) ---
        raw_x = self.angular_x_PID.compute(center_x, TARGET_X)

        # --- Channel 2: up/down (pixel Y -> J2/J3/J4 pitch) ---
        raw_y = self.angular_y_PID.compute(center_y, TARGET_Y)

        # --- Channel 3: forward/backward (depth -> J2/J3/J4 reach + J6) ---
        raw_z = self.depth_pid.compute(self.cur_depth, self.target_depth)

        # ==== Smoothing (Slew Rate Limiter) ====
        # X axis
        diff_x = raw_x - self.last_angular_x
        if diff_x > self.max_step_change_xy:
            angular_x = self.last_angular_x + self.max_step_change_xy
        elif diff_x < -self.max_step_change_xy:
            angular_x = self.last_angular_x - self.max_step_change_xy
        else:
            angular_x = raw_x
        self.last_angular_x = angular_x

        # Y axis
        diff_y = raw_y - self.last_angular_y
        if diff_y > self.max_step_change_xy:
            angular_y = self.last_angular_y + self.max_step_change_xy
        elif diff_y < -self.max_step_change_xy:
            angular_y = self.last_angular_y - self.max_step_change_xy
        else:
            angular_y = raw_y
        self.last_angular_y = angular_y

        # Z axis
        diff_z = raw_z - self.last_angular_z
        if diff_z > self.max_step_change_z:
            angular_z = self.last_angular_z + self.max_step_change_z
        elif diff_z < -self.max_step_change_z:
            angular_z = self.last_angular_z - self.max_step_change_z
        else:
            angular_z = raw_z
        self.last_angular_z = angular_z

        base_j1, base_j2, base_j3, base_j4 = 90, 150.0, 12.0, 20.0

        # Pitch gains (Y channel -> J2/J3/J4)
        gain_pitch_j2 = -150.0
        gain_pitch_j3 = -60.0
        gain_pitch_j4 = -100.0

        # J1: pure yaw
        gain_j1 = -90.0
        target_j1 = base_j1 + (10 * angular_x * gain_j1)

        # J2/J3/J4: pitch (Y) + reach (Z) combined
        target_j2 = base_j2 + (angular_y * gain_pitch_j2) + (angular_z * self.gain_reach_j2)
        target_j3 = base_j3 + (angular_y * gain_pitch_j3) + (angular_z * self.gain_reach_j3)
        target_j4 = base_j4 + (angular_y * gain_pitch_j4) + (angular_z * self.gain_reach_j4)

        base_j6 = 30
        target_j6 = base_j6 + (angular_z * self.gain_j6)

        joints_0 = [
            int(max(0, min(180, target_j1))),
            int(max(0, min(180, target_j2))),
            int(max(0, min(180, target_j3))),
            int(max(0, min(180, target_j4))),
            90,
            int(max(10, min(180, target_j6)))
        ]
        self.pubSixArm(joints_0)
        self.cur_joints = joints_0

• Parameter file

xxxxxxxxxx
~/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_bringup/param/common.yaml

Parameter Description:

​x
apriltag_follow:                   
  ros__parameters:
    tag_size: 0.025                        # 4*4*4 cm wooden block
    observation_pose: [90, 150, 12, 20, 90, 30]       # robotic arm observation pose (degrees)
    camera_measure: [0.0, 0.0, 0.0]        # measure coordinate frame measurement compensation (meters)
    pixel_tolerance: 15                    # pixel alignment tolerance
    target_depth: 0.3                      # target hold (meters)