Visual servoing

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

U11.6VisionUniversity35 min

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.

The two loops on this robot

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

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.

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.

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.

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 that is one tick; on a real camera pipeline it can be 100 ms 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?