Visual servoing

Steer from cx, speed from distance: docking at a tag.

4.3VisionRobot club20 min

Do this lesson in the simulator

Print it: worksheet (PDF) · answers (PDF) · as a web page

Steering by what the camera sees, while driving, is called visual servoing. It is how a drone follows a person, how a robot arm picks up a cup, and how BugBot is about to park in front of a tag.

Two errors from one tag

A tag gives you two errors at once. cx says how far off-centre it is: that becomes rotation. distance says how far away it is: that becomes speed.

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

def bearing_of(cx):
    return (cx - 160) * 120 / 320

# camera: the tag detector
set_cv("apriltag")
tag = apriltags()[0]
print("bearing:", round(bearing_of(tag[1]), 1), "distance:", tag[3])

Run this in the simulator

Marker 1 is up the mat and a little to the right. The bearing is small, 7.5 degrees, and the distance is large, 71.6 cm.

Steer while driving

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

def bearing_of(cx):
    return (cx - 160) * 120 / 320

# camera: the tag detector
set_cv("apriltag")
# do this 30 times (tick counts from 0)
for tick in range(30):
    # every tag in view: [id, cx, cy, distance], nearest first
    tags = apriltags()
    if not tags:
        # leave the loop
        break
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(50, 0, bearing_of(tags[0][1]) * 3)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()
tag = apriltags()[0]
print("distance now", tag[3], "bearing", round(bearing_of(tag[1]), 1))

Run this in the simulator

The robot curves towards the tag: the rotation controller pulls the tag to the middle while the forward speed closes the distance.

Speed from distance

Flat out until the last moment is the bang-bang parking from lesson 3.4. Make the speed fall with the distance, and clamp it:

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

def bearing_of(cx):
    return (cx - 160) * 120 / 320

# camera: the tag detector
set_cv("apriltag")
stop_at = 13
# again and again, for ever
while True:
    # every tag in view: [id, cx, cy, distance], nearest first
    tags = apriltags()
    if not tags:
        # leave the loop
        break
    cx, dist = tags[0][1], tags[0][3]
    if dist <= stop_at:
        # leave the loop
        break
    speed = max(20, min(70, (dist - stop_at) * 3))
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(speed, 0, bearing_of(cx) * 3)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()
# pause 0.3 s (the robot keeps doing what it was told)
wait(0.3)
tag = apriltags()[0]
print("docked at", tag[3], "cm, facing", round(heading()))

Run this in the simulator

That loop is the whole lesson: read, two errors, two outputs, repeat.

When the tag disappears

if not tags: break is the lazy answer. Near a tag the camera can lose it for a frame, and stopping dead is wrong. Better: remember the last command and keep going for a few ticks, or turn slowly towards where it last was. Lesson 4.5 does the searching properly.

Task: dock at the marker

Drive to marker 1 and stop about 12 cm in front of it, square to it: the card faces straight down the mat, so that means heading 0, within 12 degrees. The docking loop above arrives at an angle and ends facing 17, so square up at the end. The starter drives at a fixed speed and only stops once it has lost sight of the tag, by then on top of the card.

# the two lines every program starts with: the commands, then the robot
from bugbot import *
connect()

def bearing_of(cx):
    return (cx - 160) * 120 / 320

# camera: the tag detector
set_cv("apriltag")
# again and again, for ever
while True:
    # every tag in view: [id, cx, cy, distance], nearest first
    tags = apriltags()
    if not tags:
        # leave the loop
        break
    # forward, sideways, rotation: -100 to 100 each, until the next command
    drive(50, 0, bearing_of(tags[0][1]) * 3)
    # pause 0.1 s (the robot keeps doing what it was told)
    wait(0.1)
# all motors off
stop()

Challenges

  1. Dock at 20 cm, then at 8 cm. What is the closest the camera can still read the tag?
  2. Add a sideways controller so the robot ends up square in front of the tag, not just facing it, without a turn at the end. Hint: the tag's bearing and the robot's heading together tell you which side you are on.
  3. Time the dock. Make it faster without touching the tag.