Entry 4: Robot in Motion and Final Solution

🤖 Robot Trajectory and Tag-Based Localization

Now that we have a reliable foundation for computing the robot’s position, we can set the robot in motion and observe how it self-localizes using detected AprilTags. However, there are many moments when no tag is visible in the image, so odometry must be used in those situations.

To control the robot's movement, I created an alternating loop of linear motion and rotation. First, the robot moves forward for a specified time, followed by a rotational phase for another time interval. This pattern results in a circular trajectory.

Additionally, to avoid tracing a perfect circle, I implemented logic to invert the rotation direction every few cycles. It’s not the most advanced motion pattern, but it is sufficient to demonstrate how the localization system functions.

The following video fragment shows the results:

âś… Final Version of the AprilTag-Based Visual Localization System

The final implementation accurately and stably estimates the robot’s position on the map using AprilTags, relying on odometry only when no tags are detected. The operational workflow is as follows:

  1. Autonomous Robot Movement: The robot moves in cycles alternating between forward movement and in-place rotations. After completing a full loop detecting four different tags and seeing the first tag again, it inverts its rotation direction to avoid continuous circular paths.
  2. AprilTag Detection: Using the pyapriltags library, the system detects tags in the captured image and extracts their corners and center, overlaying them on the image for visualization.
  3. Pose Estimation via solvePnP: With the detected corners and the known 3D model of the AprilTag, cv2.solvePnP is used to estimate the position and orientation of the robot relative to the tag’s coordinate system. This output includes rotation (as a Rodrigues vector) and translation in the camera space.
  4. Transformation to World Coordinates: The relative pose estimated in the previous step is transformed into global coordinates by chaining a series of homogeneous transformations:
    • From physical tag to optical tag
    • From optical tag to optical camera (from solvePnP)
    • From optical camera to real camera
    • From real camera to robot center
    • From physical tag to world (from apriltags_poses.yaml)
    This process allows the robot to be positioned precisely on the global map.
  5. Result Visualization: The estimated pose (whether from vision or odometry) is shown on the simulator map using GUI.showEstimatedPose, allowing real-time visualization of the robot's estimated path.
  6. Fallback to Odometry: When no tag is detected in the image, the system automatically switches to odometry to keep estimating the robot’s movement, preventing gaps or interruptions in the trajectory trace.

This design successfully combines the accuracy of computer vision with the stability of odometry, enabling robust robot localization even when tags are temporarily occluded.

⬅ Back to Exercise 3