Part 4/62026-10-30

Mobile robotics in simulation with ROS 2 - Part 4: Spatial awareness

RoboticsROS2ArUcoLocalization

Introduction

Part 3 set up a brake to keep the robot from colliding, yet the robot still does not know where it is. This part builds a localizer: every marker it reads through the camera gives it what it needs to work out where the robot is in the room.

The subject is:

  1. How a marker places the robot in the room.
  2. How much error that placement can have.
  3. How far all those placements can be trusted, checked against Gazebo.

The question behind this part is: how well does the robot know where it is with a single marker in view?

The chosen architecture

The localizer is a ROS 2 package of its own, aruco_localization. It takes the detections from Part 2 and, for every known marker closer than 4 m, publishes a fix on /aruco/robot_pose: where the robot is in the room and which way it faces according to that marker, with its covariance. In RViz, each fix is drawn as an arrow with its ellipse.

How a marker places the robot

Every marker has a known place in the room, kept in a map that is generated along with the world. With that map, the robot's position comes from chaining three references:

  1. Where the marker is in the room, according to the map.
  2. Where the camera is as seen from the marker, the inverse of what the detector measured.
  3. Where the robot is as seen from the camera, the inverse of where the camera is mounted on the robot.

Since the robot moves on a flat floor, it keeps only x, y and heading from the resulting pose.

Why a fix spreads sideways

A fix does not go wrong the same way in every direction:

  • Along the line of sight the error is small, because the distance to the marker comes from the size of the square in the image, and that is measured well.
  • Sideways the error grows fast. The orientation depends on how much perspective distorts the square, a distortion that fades far away. An error in the orientation turns the camera around the marker, and the fix moves sideways by the distance times that angle.

The farther the marker, the more the sideways error outgrows the error along the line of sight.

Decision 1: one fix per marker or one per image?

OptionWhat it means
Use only the nearest markerwastes the other markers in view, which could reduce the error
Average the fixes of each imagea single output, but one misread marker, an outlier, can drag the whole average
Solve a single pose from the corners of every markermore precise when several are in view, but the outlier can throw off the whole pose
One fix per markerthe filter in Part 5 weighs each one by its covariance and can reject each fix on its own

Decision 2: how far should each fix be trusted?

A fix's covariance says how far off it can be in each direction, and the filter uses it to decide how much the fix counts.

OptionWhat it means
The same covariance for every fixa marker 1 m away and one 4 m away would be trusted the same, and one of the two would be wrong
Growing with distance, the same in every directionfollows the distance, but if it fits along the line of sight it falls short sideways, and the filter rejects good fixes
Growing with distance, and faster sidewaysfollows the shape of the error: along the line of sight, a share of the distance, and sideways, the distance times an orientation error that also grows with it

This is the chosen covariance at one sigma, by distance to the marker:

DistanceOrientationAlong the line of sightSideways
1 m0.9°3 cm1.5 cm
2 m1.7°6 cm6 cm
3 m2.6°9 cm13.5 cm
4 m3.4°12 cm24 cm

The result

To check it, the experiment goes back to Part 2's grid:

  • 2,592 images, from 216 points across the room at 12 headings each.
  • Every marker read gives a fix through the localizer's own code.
  • The pose Gazebo computes says where the robot really was.

By true distance, this is where the fixes landed:

DistanceFixesPosition error (median)9 in 10 withinInside the 95% ellipse
1 to 2 m1133 cm5 cm97%
2 to 3 m2367 cm12 cm98%
3 to 4 m31013 cm24 cm97%
4 to 5 m32026 cm57 cm87%
5 to 6 m26541 cm3.3 m68%

Where each fix lands, along the line of sight and across it

In the plot, every dot is a fix and the band is what its covariance allows at two sigmas. Along the line of sight, the fixes land a little farther from the marker because the distance comes out long, and sideways they spread more and more with distance.

The localizer uses the fixes whose measured distance is 4 m or less, 622 in all:

  • Half of them landed about 8 cm from the robot or closer, and 9 in 10 less than 17 cm away.
  • 97.6% fell inside the 95% ellipse of their own covariance. That is a little more than the 95% expected, and it leaves the covariance slightly loose, on the safe side.

Past 4 m the error grows quickly. From 5 to 6 m, 21% of the fixes land more than 1 m from the robot and the ellipse covers only 68%, and that is why the localizer keeps Part 2's 4 m limit.

One image's fixes from above, with their ellipses

In this image the robot sees two markers:

  • Marker 4, 1.1 m away. Its ellipse is small, and its fix landed 2 cm from the robot.
  • Marker 5, 3.8 m away. Its ellipse is much bigger and lies sideways to its line of sight. Its fix landed 15 cm from the robot, mostly along the line of sight, and inside the ellipse.

Limitations

  • Mirrored fixes. A marker that looks small in the image allows two orientations, which means the detector can sometimes pick the wrong one. That said, even though the filter rejects them by how much it trusts its estimate of the pose, there is still a small chance that one slips through.
  • Far fixes have a bias. From 3 to 4 m the fix lands 3.4% of the distance farther from the marker than the robot is. The covariance is wide enough to cover it, and the bias stays in every fix.
  • An ideal map. In simulation, every marker is exactly where the map says. In a real room its place has to be measured, and the error of that measurement adds to every fix.
  • A flat floor. The localizer drops each fix's height and tilt, and assumes the robot is always perfectly level.

Wrapping up

With a marker closer than 4 m in view, the robot now knows where it is, half the time with an error of 3% of the distance or less, and every fix comes with a covariance that covers nearly all of its error. The few mirrored fixes left are a job for the filter in Part 5.

Coming up in Part 5

Fusion: a filter brings the fixes together with the wheels' motion and the gyro, for the robot to know where it is between one marker and the next as well.

The full code can be found at github.com/KevDP/ros2-aruco-world. To regenerate this part's numbers:

experiments/sim.sh -- python3 experiments/localization.py
python3 experiments/localization_report.py