Visual servoing

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

4.3VisionRobot club20 min

Do this lesson in the simulator

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 and to the right of the robot. Both errors are large.

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, facing it within 12 degrees. The starter drives at a fixed speed and never stops.

# 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.