Visual servoing

Closing the loop on pixels instead of converting to metres first, and why that is more robust.

U11.6VisionUniversity35 min

Do this lesson in the simulator

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

Guide: HSV colour detection explained

The obvious way to use a camera is to convert every detection into metres, put the metres into a world model, and plan in the world model. There is another way, often better: leave the measurement in pixels and close the loop there.

Two families

Position based visual servoing (PBVS) estimates the pose of the target in the world, computes the error in the world, and controls to it. It needs good calibration and a good pose estimate, and it inherits every error in both.

Image based visual servoing (IBVS) defines the error directly in the image: the target features are here and should be there, in pixels. The controller maps image error to actuator command. Nothing is ever converted to metres.

IBVS is usually the more robust of the two for a simple task, for one blunt reason. The quantity you actually care about (the ball is centred, the ball is the right size) is a statement about the image. Converting to metres and back is an opportunity to be wrong that adds nothing.

Position based servoing turns pixels into metres before comparing; image based servoing compares in pixelsPosition based (PBVS)cameraframedetection,in pixelspose,in metreserror,in metresdrivecommandneeds calibration andthe object's true sizeImage based (IBVS)cameraframedetection,in pixelserror,in pixelsdrivecommandnothingconverted
Both loops start and end in the same place. PBVS takes a detour through metres, and every error in the calibration or in the assumed size of the object enters there. IBVS never leaves pixels.

The two loops on this robot

For driving up to a ball, two errors and two commands, and they barely interact:

error, in pixels command why
cx - 160 rotation the ball is off to one side, so turn
want_width - width forward speed the ball is too small, so it is too far away

The second is the interesting one. The forward loop never computes a distance. It works in apparent width, and it stops when the ball looks the right size. If the ball turned out to be a different size than you thought, the robot stops at a different distance and the loop does not care, because the loop was never about distance. That is the robustness IBVS buys: errors in calibration and in your assumptions about the object show up as a slightly wrong setpoint, not as an unstable controller.

Balls of three sizes, each at the distance where it looks as wide as the loop's setpointrobot4 cm ballstops at 16.7 cm6 cm ballstops at 25.0 cm8 cm ballstops at 33.3 cmevery one of them looks 22.2 px wide
The forward loop stops when the ball looks 22.2 px wide, the width of a 6 cm ball at 25 cm. If the ball is really bigger or smaller, the robot simply stops further off or closer in, and the loop itself behaves exactly the same.

The interaction matrix

Formally, IBVS needs the Jacobian relating camera motion to feature motion in the image, called the interaction matrix or image Jacobian. For a point feature at normalised coordinates (x, y) at depth Z, with camera translation v and rotation w:

xdot = -vx/Z + x*vz/Z + x*y*wx - (1 + x*x)*wy + y*wz

You do not need to implement that here, and you should know it exists, because it explains two things you will meet. First, every translational term has a 1/Z in it, so an IBVS controller needs a rough depth estimate for its gains even though it does not need one for its setpoint, and a crude constant depth is usually good enough. Second, the rotational terms have no Z at all, which is why rotating to centre a target is the most reliable visual control there is.

Pixels moved by a centred feature for 1 cm of sideways travel and for 1 degree of turn, against its depth02550751001251500246810depth Z of the feature, cmpixels moved1 cm sideways: f / Z1 degree of turn: 1.61 px at any depthequal at 57 cm
The translation term shrinks as 1 / Z: a sideways step moves a near feature a lot and a far one hardly at all, so its gain depends on a depth you may not know. The rotation term does not change with depth at all.

Gains, dead bands and losing sight

from bugbot import *
connect()

F, R = 92.4, 3.0
WANT = 2 * R * F / 25.0          # how wide a 6 cm ball looks at 25 cm
set_cv("blob", "red")
wait(0.3)

# the rotation half of the loop on its own: turn until the ball sits in the middle of the frame
for tick in range(40):
    seen = blobs()
    if not seen:
        drive(0, 0, 25)                       # nothing in view: sweep
        wait(0.1)
        continue
    cx, cy, area, x0, y0, x1, y1, aspect = seen[0]
    err = cx - 160
    rot = max(-55, min(55, 0.35 * err))
    if abs(err) < 6:
        rot = 0
    elif abs(rot) < 17:                       # under the dead band nothing turns at all
        rot = 17 * (1 if rot > 0 else -1)
    drive(0, 0, rot)
    plot("cx", cx)
    print("cx", cx, " err", err, " rot", round(rot), " width", x1 - x0, "of", round(WANT, 1))
    wait(0.1)
stop()

Run this in the simulator

Three practical points that decide whether this works.

The dead band. The drive commands do nothing below about 15 percent. A pure proportional law therefore stalls short of the target: the error shrinks, the command shrinks, and at some point the robot simply stops while the error is still there. Either add integral action or, more simply, push any non-zero command up to the dead band and accept a small limit cycle.

The rotation command against the column error: zero near the middle, then never less than 17, capped at 55-160-80080160-60-3003060column error, cx - 160rotation commandthe motors do nothing in this banddashed: 0.35 x error alonepushed up to 17within 6: no turn
The lesson's law. Within 6 pixels of the middle it asks for nothing. Anywhere else it asks for at least 17, just above the band where the motors would not move, so a small error still turns the robot. The price is a small back and forth round the middle. The cap of 55 only bites in the last few pixels at each edge.

Losing the target. The blob list comes back empty and the naive loop drives on blindly. Always have a behaviour for "no detection": here, keep turning the way the target was last seen to go. A robot that remembers which side it lost something on recovers in a fraction of a second. One that does not, spins.

Latency. Every frame is stale by the time you act on it. On this simulator the frame itself is fresh, so the delay is the tick you wait before looking again plus the motors' quarter of a second to respond; on a real camera pipeline the frame alone can be 100 ms old or more, which at 20 cm/s is 2 cm of motion. Latency in a feedback loop is phase lag, and phase lag is what makes loops oscillate, so a vision loop tolerates much less gain than a loop closed on an encoder. If it wobbles, lower the gain before you blame the detector.

Task: servo on the pixels

A red ball, 6 cm across, is off to the robot's right and the robot is not facing it. Drive up and stop about 25 cm away, controlling rotation from the blob's column and forward speed from its apparent width. Plot cx and width, and print width:, the apparent width you stopped at. Neither distance() nor position() is allowed.

from bugbot import *
connect()

F, R = 92.4, 3.0
STANDOFF = 25.0
set_cv("blob", "red")
wait(0.3)

Challenges

  1. Double the rotation gain and look at the cx plot. Where does it start to ring?
  2. Delay your measurement by one tick on purpose, using the previous frame's cx. How much gain can the loop take now?
  3. Change the standoff to 40 cm without touching the control law. Which line did you have to edit, and what does that say about where the units live?