Visual Servoing

Visual servoing controls the motion of a robot with feedback from a camera. The camera measures a set of visual features, such as the image positions of four marks on an object; the controller compares them with the values they should have when the task is done and computes a velocity that reduces the difference, frame after frame, until the robot reaches the goal. The camera can ride on the robot’s hand (eye-in-hand) or watch the robot from a fixed place (eye-to-hand). This article follows the formulation of Chaumette and Hutchinson’s two-part tutorial (2006, 2007): an error $\mathbf{e} = \mathbf{s} - \mathbf{s}^*$, a velocity control law $\mathbf{v}_c = -\lambda \widehat{\mathbf{L}_e^+}\mathbf{e}$, and an interaction matrix $\mathbf{L}$ that links feature motion to camera motion.

The idea is older than the formulation. Hutchinson, Hager and Corke (1996) cite the early work of Shirai and Inoue (1973), who described a visual feedback loop that corrects a robot’s position, and write that the term “visual servoing” appears to have been introduced by Hill and Park (1979), to distinguish their approach from earlier experiments that alternated between taking a picture and moving. They credit Weiss and colleagues with the image Jacobian, which Weiss et al. called the feature sensitivity matrix, and describe the taxonomy that Sanderson and Weiss introduced in 1980, which asks two questions: whether the error is defined in 3D task space or directly on image features (position-based against image-based), and whether the vision loop sends set-points to the robot’s joint controllers (dynamic look-and-move) or replaces them (direct visual servo). Nearly all systems implemented by 1996, they note, were of the look-and-move kind, and this article assumes it: the controller outputs a camera velocity, and the robot’s own controllers realize it.

Problem

A positioning task fixes where the camera should be relative to a target, usually by showing it: the robot is placed at the goal once, the image is recorded, and the features measured in it become $\mathbf{s}^*$. From any other starting pose, the robot must return to that goal using only what the camera sees. Chaumette and Hutchinson’s Part I treats the basic case: a motionless target, a constant $\mathbf{s}^*$, and a camera with six degrees of freedom whose velocity can be commanded directly. The difficulty is that the relation between camera pose and image features is nonlinear, depends on quantities the camera does not measure (the depth of each point), and is inverted only locally, at every control step.

Inputs and Outputs

• Input at each step: image measurements $\mathbf{m}(t)$ (for example, the pixel coordinates of tracked points), parameters $\mathbf{a}$ known in advance (camera intrinsics, and for PBVS a 3D model of the object), the desired features $\mathbf{s}^*$, and estimates of the 3D quantities the interaction matrix needs (depths, or the pose).
• Output at each step: a camera velocity $\mathbf{v}_c = (\mathbf{v}, \boldsymbol{\omega})$, the linear velocity of the camera’s origin and its angular velocity, both expressed in the camera frame. The robot converts it to joint velocities, or the control is written directly in joint space.
• At convergence: $\mathbf{e} = \mathbf{0}$, which puts the camera at the goal pose when the features determine the pose uniquely.

Intuition

Each feature tells the controller how the camera should move: a point left of its goal asks for a rotation or translation that moves it right. The interaction matrix collects these sensitivities, one row per feature coordinate and one column per camera degree of freedom, and its pseudo-inverse turns a desired change of the features into a camera velocity. Asking every error component to shrink exponentially at the same rate gives a proportional controller.

The choice of features decides the behaviour. Image-based servoing uses image coordinates directly, so each point heads straight for its goal in the image, but the camera may take a strange route through space. Position-based servoing estimates the camera pose and servos on it, so the camera follows a clean path, but the image is not controlled and the target can drift out of view. Hybrid schemes take some features from the image and some from a partial 3D reconstruction.

Algorithm

At each control step:

1. Acquire an image and extract the measurements $\mathbf{m}$: detect and track the features, and match them to the features of the desired image.
2. Compute the features $\mathbf{s}(\mathbf{m}, \mathbf{a})$: normalized image coordinates for IBVS (using the intrinsic parameters), or a pose estimate for PBVS (from a 3D model with a perspective-n-point solver, or from a homography between the current and desired images).
3. Form the error $\mathbf{e} = \mathbf{s} - \mathbf{s}^*$.
4. Build the approximation $\widehat{\mathbf{L}_e^+}$ of the pseudo-inverse of the interaction matrix, with current or desired depth estimates, or from a numerical estimate.
5. Compute $\mathbf{v}_c = -\lambda \widehat{\mathbf{L}_e^+}\mathbf{e}$ and send it to the robot, converted to joint velocities with the robot Jacobian and the hand-eye transform.
6. Repeat until $\|\mathbf{e}\|$ falls below a tolerance.

Mathematical Formulation

The control law

Chaumette and Hutchinson write the features as $\mathbf{s}(\mathbf{m}(t), \mathbf{a})$ and the error as $\mathbf{e}(t) = \mathbf{s}(\mathbf{m}(t), \mathbf{a}) - \mathbf{s}^*$. For $k$ features, the interaction matrix $\mathbf{L}_s \in \mathbb{R}^{k \times 6}$, also called the feature Jacobian or image Jacobian, relates their rate of change to the camera’s spatial velocity:

$$\dot{\mathbf{s}} = \mathbf{L}_s \mathbf{v}_c, \qquad \dot{\mathbf{e}} = \mathbf{L}_e \mathbf{v}_c, \quad \mathbf{L}_e = \mathbf{L}_s ,$$

for a motionless target and constant $\mathbf{s}^*$. Imposing an exponential, decoupled decrease $\dot{\mathbf{e}} = -\lambda\mathbf{e}$ gives $\mathbf{v}_c = -\lambda\mathbf{L}_e^+\mathbf{e}$, with the Moore–Penrose pseudo-inverse $\mathbf{L}_e^+ = (\mathbf{L}_e^\top\mathbf{L}_e)^{-1}\mathbf{L}_e^\top$ when $\mathbf{L}_e$ has full rank 6. The pseudo-inverse gives the velocity that comes closest, in the least-squares sense, to the requested error rate, and the smallest such velocity. When $k = 6$ and $\det\mathbf{L}_e \neq 0$, the inverse replaces the pseudo-inverse. Since $\mathbf{L}_e$ is never known exactly, the law actually implemented is

$$\mathbf{v}_c = -\lambda\, \widehat{\mathbf{L}_e^+}\, \mathbf{e},$$

where $\widehat{\mathbf{L}_e^+}$ denotes either the pseudo-inverse of an approximation of $\mathbf{L}_e$ or an approximation of its pseudo-inverse. The rest of the method is the choice of $\mathbf{s}$, the form of $\mathbf{L}_s$, and how $\widehat{\mathbf{L}_e^+}$ is obtained.

The interaction matrix of an image point

A point with camera coordinates $\mathbf{X} = (X, Y, Z)$ projects, in the pinhole model, to normalized coordinates $x = X/Z = (u - c_u)/f\alpha$ and $y = Y/Z = (v - c_v)/f$, with $(u, v)$ in pixels, principal point $(c_u, c_v)$, focal length $f$ and pixel aspect ratio $\alpha$ (Part I’s notation). Differentiating the projection,

$$\dot{x} = \frac{\dot{X} - x\dot{Z}}{Z}, \qquad \dot{y} = \frac{\dot{Y} - y\dot{Z}}{Z}.$$

A point fixed in the world, seen by a camera moving with $(\mathbf{v}, \boldsymbol{\omega})$, moves in the camera frame with $\dot{\mathbf{X}} = -\mathbf{v} - \boldsymbol{\omega} \times \mathbf{X}$:

$$\dot{X} = -v_x - \omega_y Z + \omega_z Y, \quad \dot{Y} = -v_y - \omega_z X + \omega_x Z, \quad \dot{Z} = -v_z - \omega_x Y + \omega_y X.$$

Substituting, for example $\dot{x} = (-v_x - \omega_y Z + \omega_z Y + x v_z + x\omega_x Y - x\omega_y X)/Z = -v_x/Z + x v_z/Z + xy\,\omega_x - (1 + x^2)\,\omega_y + y\,\omega_z$, and stacking both coordinates,

$$\mathbf{L}_x = \begin{bmatrix} -\dfrac{1}{Z} & 0 & \dfrac{x}{Z} & xy & -(1 + x^2) & y \\[2mm] 0 & -\dfrac{1}{Z} & \dfrac{y}{Z} & 1 + y^2 & -xy & -x \end{bmatrix},$$

which is Part I’s equation (11). Three properties follow from its form. The depth $Z$ enters only the three translational columns, so any IBVS scheme needs a depth estimate or approximation, while the rotational columns depend on the image coordinates alone. The intrinsics enter through $x$ and $y$, so the true matrix is never known exactly. And for features measured in pixels, the rows are scaled by the focal lengths in pixels (by the chain rule, $\dot{u} = f\alpha\,\dot{x}$ and $\dot{v} = f\,\dot{y}$).

Sign conventions. Part I’s matrix maps the velocity of the camera. Hutchinson et al. (1996) derive their image Jacobian (their equation 47) for a fixed camera watching a point on the moving end-effector, so it maps the velocity of the point relative to the camera; with their focal length $\lambda$ set to 1, every entry is the negative of the corresponding entry above. Formulas from different sources should be checked against which velocity they describe before they are combined.

Each point gives two rows, so six degrees of freedom need at least three points. Three are not enough in practice: Part I notes, citing Michel and Rives (1993), that the stacked $6 \times 6$ matrix is singular for some configurations, and Chaumette (1998) lists the well-known ones: three collinear points, or three points on a cylinder that contains the optical center. Three points also admit four distinct camera poses with $\mathbf{e} = \mathbf{0}$, which cannot be told apart (Part I). Four or more points are therefore used, giving $k \geq 8$.

Choosing the approximation

Part I compares three choices for IBVS:

• Current matrix, $\widehat{\mathbf{L}_e^+} = \mathbf{L}_e^+$, evaluated with the current features and an estimate of the current depth of each point, obtained at every iteration (Part I cites Hutchinson et al. 1996 for this scheme).
• Desired matrix, $\widehat{\mathbf{L}_e^+} = \mathbf{L}_{e^*}^+$, evaluated at the goal, so that it is constant and only the desired depths are needed, which Part II notes are generally not difficult to obtain in practice (cited to Espiau, Chaumette and Rives, 1992).
• Mean, $\widehat{\mathbf{L}_e^+} = \left(\tfrac{1}{2}(\mathbf{L}_e + \mathbf{L}_{e^*})\right)^+$, proposed by Malis (2004), which again needs current depths.

In Part I’s simulation of a large rotation, all three converge, but differently. With $\mathbf{L}_{e^*}$, the image trajectories, velocities and camera path are poor for the first 30 or so iterations. With $\mathbf{L}_e$, the image points move along almost straight lines, but the camera path is far from straight and the initial velocities are large, a sign that the matrix is badly conditioned at the start. The mean gives smooth behaviour in both the image and in space, and the authors report that it performs well in practice.

The depths can come from a 3D model through pose estimation, from known camera motion, from a stereo pair by triangulation, or from the epipolar geometry or a homography between the current and desired images (Part II). Part II notes that the essential matrix becomes degenerate near convergence, when the two images are almost identical, so a homography is generally preferred; it gives the rotation, the translation divided by the plane’s distance, and the depth of every point up to one common scale factor.

The matrix can also be estimated numerically, without a model, from observed feature changes $\Delta\mathbf{s}$ under known camera motions $\Delta\mathbf{v}_c$. Stacking $N > 6$ motions in the columns of $A$ and the matching feature changes in $B$, the least-squares estimate is $\widehat{\mathbf{L}}_s = BA^+$; estimating the pseudo-inverse directly, $\widehat{\mathbf{L}_s^+} = AB^+$ with $N > k$, behaves better in practice (Part II, citing Lapresté, Jurie and Chaumette, 2004). Online, a Broyden update refines the estimate after each motion:

$$\widehat{\mathbf{L}}_s(t+1) = \widehat{\mathbf{L}}_s(t) + \frac{\alpha}{\Delta\mathbf{v}_c^\top\Delta\mathbf{v}_c}\left(\Delta\mathbf{s} - \widehat{\mathbf{L}}_s(t)\,\Delta\mathbf{v}_c\right)\Delta\mathbf{v}_c^\top .$$

Such estimates avoid modelling and calibration and work for features with no analytic interaction matrix, but, as Part II notes, they allow no theoretical analysis of stability or robustness.

Position-based servoing

PBVS uses the pose of the camera as the feature, so it needs the intrinsics and a way to compute the pose (a 3D model of the object, or the scaled reconstruction from a homography). With $\mathcal{F}_c$, $\mathcal{F}_{c^*}$ and $\mathcal{F}_o$ the current, desired and object frames, ${}^{c}\mathbf{t}_o$ the object origin in the current camera frame, $R = {}^{c^*}\!R_c$ the orientation of the current frame relative to the desired one, and $\theta\mathbf{u}$ its axis–angle vector (see rotation representations), Part I gives two schemes.

With $\mathbf{s} = ({}^{c}\mathbf{t}_o, \theta\mathbf{u})$ and $\mathbf{e} = ({}^{c}\mathbf{t}_o - {}^{c^*}\mathbf{t}_o, \theta\mathbf{u})$,

$$\mathbf{L}_e = \begin{bmatrix} -I_3 & [{}^{c}\mathbf{t}_o]_\times \\ 0 & \mathbf{L}_{\theta u} \end{bmatrix}, \qquad \mathbf{L}_{\theta u} = I_3 - \frac{\theta}{2}[\mathbf{u}]_\times + \left(1 - \frac{\operatorname{sinc}\theta}{\operatorname{sinc}^2(\theta/2)}\right)[\mathbf{u}]_\times^2,$$

and the exact inverse gives $\mathbf{v} = -\lambda\left(({}^{c^*}\mathbf{t}_o - {}^{c}\mathbf{t}_o) + [{}^{c}\mathbf{t}_o]_\times\theta\mathbf{u}\right)$, $\boldsymbol{\omega} = -\lambda\theta\mathbf{u}$, using $\mathbf{L}_{\theta u}^{-1}\theta\mathbf{u} = \theta\mathbf{u}$. The rotation follows a geodesic at an exponentially decreasing speed, and the image of the object origin moves on a straight line, but the camera does not.

With $\mathbf{s} = ({}^{c^*}\mathbf{t}_c, \theta\mathbf{u})$, so that $\mathbf{s}^* = \mathbf{0}$ and $\mathbf{e} = \mathbf{s}$, the interaction matrix is block diagonal, $\mathbf{L}_e = \operatorname{diag}(R, \mathbf{L}_{\theta u})$, and the control decouples:

$$\mathbf{v} = -\lambda R^\top\, {}^{c^*}\mathbf{t}_c, \qquad \boldsymbol{\omega} = -\lambda\theta\mathbf{u}.$$

With a perfect pose estimate, the camera then moves along a straight line, while the image trajectories are worse than with the first scheme, and for some configurations points leave the field of view (Part I).

Malis, Chaumette and Boudet (1999) give the determinant $\det\mathbf{L}_{\theta u} = 1/\operatorname{sinc}^2(\theta/2)$, so $\mathbf{L}_{\theta u}$ is singular only at $\theta = 2k\pi$, $k \neq 0$, outside the range of rotations a controller meets.

A frame convention to check. Part II writes the Euclidean homography from the desired to the current image as $H = R + \mathbf{t}\,\mathbf{n}^{*\top}/d^*$, with $\mathbf{n}^*$ the plane normal and $d^*$ its distance, both in the desired frame. For $H$ to map desired-frame points to current-frame points, it follows that this $R$ is ${}^{c}R_{c^*}$, the transpose of the $R = {}^{c^*}\!R_c$ used in the PBVS laws above, and this $\mathbf{t}$ is ${}^{c}\mathbf{t}_{c^*}$, so ${}^{c^*}\mathbf{t}_c = -R^\top\mathbf{t}$ in the homography’s notation. Part II describes $R$ only as relating the orientations of the two frames; the example below uses this reading, and its pose check confirms it.

Hybrid (2.5D) visual servoing

Malis, Chaumette and Boudet (1999) proposed 2 1/2 D visual servoing, which takes the rotation from a partial 3D reconstruction and the translation from extended image coordinates of one reference point. The homography between the current and desired images of a plane through three target points gives, without any 3D model, the rotation $R$, the ratio $r = d/d^* = \det H$ of the plane’s current and desired distances, and the depth ratio $\rho_Z = Z/Z^*$ of the reference point. The error is

$$\mathbf{e} = \left(x - x^*,\; y - y^*,\; \log\rho_Z,\; \theta\mathbf{u}\right),$$

and its interaction matrix is upper block triangular, as Part II writes it:

$$\mathbf{L}_e = \begin{bmatrix} \mathbf{L}_v & \mathbf{L}_\omega \\ 0 & \mathbf{L}_{\theta u}\end{bmatrix}, \quad \mathbf{L}_v = \frac{1}{Z^*\rho_Z}\begin{bmatrix} -1 & 0 & x \\ 0 & -1 & y \\ 0 & 0 & -1 \end{bmatrix}, \quad \mathbf{L}_\omega = \begin{bmatrix} xy & -(1 + x^2) & y \\ 1 + y^2 & -xy & -x \\ -y & x & 0 \end{bmatrix}.$$

The first two rows of $\mathbf{L}_v$ and $\mathbf{L}_\omega$ are those of $\mathbf{L}_x$; the third row is the derivative of $\log Z$, $\dot{Z}/Z = -v_z/Z - y\,\omega_x + x\,\omega_y$. The control is $\boldsymbol{\omega} = -\lambda\theta\mathbf{u}$ and $\mathbf{v} = -\mathbf{L}_v^{-1}(\lambda\mathbf{e}_t + \mathbf{L}_\omega\boldsymbol{\omega})$, where $\mathbf{e}_t$ is the first three components of $\mathbf{e}$: the translation cancels the image motion that the rotation will cause. The only unknown is the constant $d^*$ (or $Z^*$), which the 1999 paper sets by hand during the off-line learning stage; its authors report that the value has little influence on stability and affects the convergence time of the translation and a transient tracking error caused by imperfect compensation of the rotation; Part II notes that it can also be estimated online with adaptive techniques.

Malis et al. state that this interaction matrix has no singularity in the whole task space (it is singular only when the camera centre lies on the reference plane; other degenerate cases are zero or infinite depths and $\theta = \pm 2\pi$), which gives convergence from any initial pose when the intrinsics are known. With calibration errors, the rotation angle and the ratios $r$ and $\rho_Z$ are still recovered exactly, the rotation axis is distorted, and the triangular structure allows the paper to derive necessary and sufficient conditions for local asymptotic stability and sufficient conditions for global asymptotic stability. The paper also lists the costs: a non-planar target needs at least eight points to estimate the homography (three to define the plane and five others), and the method is more sensitive to image noise than IBVS, which uses the image features with no estimation step. The reference point’s image path is a straight line, so it stays in view if a perfect model is available, but the other points are not guaranteed to.

Part II describes further hybrids: Chaumette and Malis (2000) use $({}^{c^*}\mathbf{t}_c, \mathbf{x}_g, \theta u_z)$, which in perfect conditions moves the camera on a straight line and the image of the object’s centre of gravity on a straight line as well.

Stability

Part I analyzes stability with the Lyapunov function $\mathcal{L} = \tfrac{1}{2}\|\mathbf{e}\|^2$, whose derivative under the control law is $\dot{\mathcal{L}} = -\lambda\,\mathbf{e}^\top\mathbf{L}_e\widehat{\mathbf{L}_e^+}\mathbf{e}$. Global asymptotic stability holds under the sufficient condition

$$\mathbf{L}_e\widehat{\mathbf{L}_e^+} > 0 ,$$

which is met for $k = 6$ features when both matrices have full rank 6 and the approximation is not too coarse.

IBVS. With $k > 6$, the $k \times k$ product $\mathbf{L}_e\widehat{\mathbf{L}_e^+}$ has rank at most 6 and a non-trivial null space, so the condition can never hold. Configurations with $\mathbf{e} \in \operatorname{Ker}\widehat{\mathbf{L}_e^+}$ and $\mathbf{e} \neq \mathbf{0}$ are local minima: the computed velocity is zero although the task is not done. Part I’s Figure 9 shows one reached with four points and the current matrix: every error component decreases exponentially and the image paths are straight, yet the error does not reach exactly zero and the camera stops far from the goal. What can be shown is local asymptotic stability. Defining $\mathbf{e}' = \widehat{\mathbf{L}_e^+}\mathbf{e}$ gives $\dot{\mathbf{e}}' = -\lambda(\widehat{\mathbf{L}_e^+}\mathbf{L}_e + \mathbf{O})\mathbf{e}'$, with $\mathbf{O} = \mathbf{0}$ at $\mathbf{e} = \mathbf{0}$, so the system is locally asymptotically stable near the goal if

$$\widehat{\mathbf{L}_e^+}\mathbf{L}_e > 0$$

(a $6 \times 6$ condition), and Part I argues that no local minimum exists in a small enough neighbourhood of the goal pose. The size of that neighbourhood is, in Part I’s words, still an open issue, though in practice it is “surprisingly quite large”.

Chaumette (1998) gives the mechanism behind the local minima. Image motions $\dot{\mathbf{s}}^\perp$ in $\operatorname{Ker}\mathbf{L}_s^\top$ are not in the range of $\mathbf{L}_s$, so no camera motion produces them; with four points this kernel has dimension 2. Since $\operatorname{Ker}\mathbf{L}_s^\top = \operatorname{Ker}\mathbf{L}_s^+$, a controller using the exact current matrix stops wherever the error lies in that space. In his four-point example, the current matrix ends in a local minimum with a residual of about one pixel on each coordinate, while the constant desired matrix, from the same start, reaches the goal.

Singularities and the rotation about the optical axis. Chaumette (1998) also showed that, with points as features, the interaction matrix can become singular during servoing whatever the number and configuration of the points. When the camera must rotate by 180° about its optical axis, the desired image is the current one mirrored through the principal point. With the current matrix, each point moves on a straight line toward the principal point and all arrive at the same instant; the corresponding camera motion is a pure backward translation along the optical axis, with no rotation at all, which takes the camera towards infinity, where the matrix has rank 2. With the desired matrix, the motion is instead a pure forward translation, toward the target and a singularity of $\mathbf{L}_s$ at $Z = 0$; in practice, Chaumette notes, the features leave the image before a collision. Part I explains the milder version: for any rotation about the optical axis, coupling between the third ($v_z$) and sixth ($\omega_z$) columns makes the current matrix add a retreat and the desired matrix add an advance, both growing with the angle and vanishing for small rotations, while the mean matrix gives a pure rotation in all cases except a rotation of $\pi$. Chaumette (1998) showed that straight lines as features, parameterized by $(\rho, \theta)$, give a pure rotation in the 180° case.

PBVS. Since $\mathbf{L}_{\theta u}$ is nonsingular for $\theta \neq 2k\pi$, both PBVS schemes give $\mathbf{L}_e\widehat{\mathbf{L}_e^{-1}} = I_6$ and are globally asymptotically stable, under the strong assumption that the pose is estimated perfectly. The true condition involves the Jacobian of the pose estimate with respect to the image features, which Chaumette (1998) notes is unknown. Small image errors can cause large pose errors, especially for planar targets: Part I’s Figure 10 shows two quite different poses that give almost the same image of four coplanar points, and Chaumette (1998) notes that the control can become completely unstable in such cases, while non-coplanar objects give satisfactory results in practice. With no control in the image, he adds, nothing keeps the object in view, especially with large calibration errors.

Robustness to calibration. Part I’s conclusion summarizes the trade-off. In IBVS, poor 3D estimates can destabilize the system, but coarse ones only perturb the trajectory and leave the final accuracy unchanged, because the error is measured in the image; IBVS is thereby robust to calibration errors and image noise. In PBVS, the pose enters both the error and the matrix, so estimation errors also change the pose that is reached.

Eye-in-hand and eye-to-hand systems

When the robot cannot realize an arbitrary camera velocity (for example, with fewer than six joints), the control is written in joint space. Part II gives the same form for both set-ups, $\dot{\mathbf{s}} = \mathbf{J}_s\dot{\mathbf{q}} + \partial\mathbf{s}/\partial t$, with $n$ joints and a feature Jacobian $\mathbf{J}_s \in \mathbb{R}^{k \times n}$:

• Eye-in-hand: $\mathbf{J}_s = \mathbf{L}_s\,{}^{c}\mathbf{V}_N\,\mathbf{J}(\mathbf{q})$, with $\mathbf{J}(\mathbf{q})$ the robot Jacobian in the end-effector frame and ${}^{c}\mathbf{V}_N$ the constant spatial motion transform between camera and end-effector. Thanks to the closed loop, a coarse approximation of it usually suffices; hand-eye calibration gives an accurate one when needed. Here $\partial\mathbf{s}/\partial t$ is due to target motion.
• Eye-to-hand: $\mathbf{J}_s = -\mathbf{L}_s\,{}^{c}\mathbf{V}_N\,{}^{N}\mathbf{J}(\mathbf{q}) = -\mathbf{L}_s\,{}^{c}\mathbf{V}_0\,{}^{0}\mathbf{J}(\mathbf{q})$. The sign changes because the camera sees the robot move, rather than the scene move against the camera, so the same robot motion produces opposite image motion. In the first form ${}^{c}\mathbf{V}_N$ changes throughout the servo and must be estimated at each iteration by pose estimation; in the second, with the Jacobian in the robot base frame, ${}^{c}\mathbf{V}_0$ is constant while the camera stays put, and a coarse approximation usually suffices.

The spatial motion transform for a rigid transformation $(R, \mathbf{t})$ is $\mathbf{V} = \begin{bmatrix} R & [\mathbf{t}]_\times R \\ 0 & R\end{bmatrix}$ (Part I). The joint-space law $\dot{\mathbf{q}} = -\lambda\widehat{\mathbf{J}_e^+}\mathbf{e} - \widehat{\mathbf{J}_e^+}\widehat{\partial\mathbf{e}/\partial t}$ has the same stability conditions as before with $\mathbf{J}_e$ in place of $\mathbf{L}_e$. Part II notes that it is not equivalent to computing $\mathbf{v}_c$ first and then inverting the robot Jacobian: the two minimize different norms ($\|\mathbf{v}_c\|$ against $\|\dot{\mathbf{q}}\|$) and the robot Jacobian can be singular where the feature Jacobian is not, so the choice of space changes the robot’s trajectory.

Hutchinson et al. (1996) draw a related distinction: an endpoint open-loop system observes only the target, while an endpoint closed-loop system observes both the target and the end-effector, which makes it less sensitive to calibration but requires tracking both and keeping both in view.

Parameters

• Features $\mathbf{s}$ and the approximation $\widehat{\mathbf{L}_e^+}$. The main design choices, discussed above; most of Part II’s advanced schemes are new feature sets (see Variants).
• Gain $\lambda$. It sets the time constant of the exponential decrease. With a sampled loop of period $\Delta t$ and a first-order update, the ideal error recursion is $\mathbf{e}_{k+1} = (1 - \lambda\Delta t)\,\mathbf{e}_k$, which, by our reasoning and ignoring delays, decreases monotonically for $0 < \lambda\Delta t < 1$ and diverges for $\lambda\Delta t > 2$. Real loops add latency: Hutchinson et al. (1996) stress that a visual servo system is a closed-loop discrete-time system with delays from exposure, pixel transfer, feature extraction and communication, that it becomes unstable as the gain rises, and that many systems are tuned empirically. The timing side of the loop is the subject of real-time vision pipelines.
• Target motion compensation. For a moving target, Part II adds a feedforward term, $\mathbf{v}_c = -\lambda\widehat{\mathbf{L}_e^+}\mathbf{e} - \widehat{\mathbf{L}_e^+}\widehat{\partial\mathbf{e}/\partial t}$. If the estimate of $\partial\mathbf{e}/\partial t$ is off by $b$, the scalar model $\dot{e} = -\lambda e + b$ converges to $b/\lambda$, a tracking error that a higher gain reduces but a too high gain destabilizes; an integral term cancels it for constant target velocity, and a Kalman filter can smooth the estimate.

Complexity

The control computation is small: building $\mathbf{L}$ is $O(k)$, and its pseudo-inverse costs $O(36k)$ through a $k \times 6$ singular value decomposition, so for tens of points it is negligible next to image acquisition and feature extraction. PBVS adds a pose estimate per frame (a PnP solve with a model, or a homography fit and decomposition), and 2.5D servoing adds a homography estimate, linear for a planar target with at least four points. The loop rate is set by the camera and the image processing: Hutchinson et al. (1996) cite video sample intervals of 33 or 40 ms for RS-170 and CCIR video, against 14 or 28 ms for the position loops of a common industrial arm of that time.

Implementation

The NumPy and SciPy example below servos a simulated pinhole camera (800-pixel focal length, $640 \times 480$ image) toward a planar target of four points on a 20 cm square, with the goal 0.5 m in front of the target and parallel to it. Each run integrates the commanded camera velocity with a fixed step $\Delta t = 0.04$ s and gain $\lambda = 1$ for at most 300 steps (12 s), applying each step as an exact rigid motion in the camera frame. Four laws are compared: IBVS with the current, desired and mean interaction matrices (the current depths are taken from the simulation, as if measured), and the second PBVS scheme, $\mathbf{s} = ({}^{c^*}\mathbf{t}_c, \theta\mathbf{u})$. The PBVS pose comes from a homography between the current and desired images, estimated by the direct linear transform, scaled so that its middle singular value is 1, and decomposed using the known desired normal $\mathbf{n}^* = (0, 0, 1)$ and distance $d^* = 0.5$ m: for any vector $\mathbf{a}$ orthogonal to $\mathbf{n}^*$, $H\mathbf{a} = R\mathbf{a}$, so the first two columns of $H$ are those of $R$.

Before servoing, the script checks the analytic interaction matrix against finite differences of the projection, checks that the centroids of the four dots in a rendered image of the target match the projected points that serve as features, and checks the homography pose against the true pose. For each run it prints the largest distance of any image point from the straight segment between its first and last image positions (dev), the largest image coordinate reached relative to the half-width or half-height of the image (extent, above 1 means a point left the image), the length of the camera path and the distance between its ends (path, net), the range of distances from the camera to the target centre (dist), $\|\mathbf{e}\|$ in normalized coordinates after 1 s and 12 s, and the final position and rotation errors. A run stops if the camera comes within 5 cm of the target plane, retreats past 5 m, or a velocity command exceeds 10 m/s or rad/s.

import numpy as np
from scipy.linalg import expm
from scipy.ndimage import center_of_mass, label
from scipy.spatial.transform import Rotation

F, W, H = 800.0, 640, 480                      # focal length (px), image size (px)
P = np.array([[-.1, -.1, 0], [.1, -.1, 0], [.1, .1, 0], [-.1, .1, 0]])  # target (m)
t_d = np.array([0, 0, -0.5])                   # desired camera: 0.5 m in front, R = I
LAM, DT, STEPS = 1.0, 0.04, 300                # gain (1/s), time step (s), 12 s

def project(R, t):
    """Normalized image points (x, y) = (X/Z, Y/Z) and depths, camera pose (R, t)."""
    Xc = (P - t) @ R                           # X_c = R^T (X_w - t)
    return Xc[:, :2] / Xc[:, 2:], Xc[:, 2]

def L_point(x, y, Z):
    """Interaction matrix of an image point for the camera velocity (v, w)."""
    return np.array([[-1 / Z, 0, x / Z, x * y, -(1 + x * x), y],
                     [0, -1 / Z, y / Z, 1 + y * y, -x * y, -x]])

def L_stack(s, Z):
    return np.vstack([L_point(x, y, z) for (x, y), z in zip(s, Z)])

def move(R, t, xi):
    """Apply the camera-frame twist xi = (v, w) dt with the SE(3) exponential."""
    T = expm(np.block([[np.cross(np.eye(3), xi[3:]), xi[:3, None]], [np.zeros((1, 4))]]))
    return R @ T[:3, :3], t + R @ T[:3, 3]

def render(s, k=4):
    """Image of the four target dots (radius 6 px), 4x4 supersampled."""
    v, u = (np.mgrid[0:H * k, 0:W * k] + 0.5) / k - 0.5
    img = np.zeros((H * k, W * k))
    for cu, cv in s * F + [W / 2 - 0.5, H / 2 - 0.5]:
        img[(u - cu) ** 2 + (v - cv) ** 2 < 36] = 1
    return img.reshape(H, k, W, k).mean(axis=(1, 3))

def centroids(img):
    lab, n = label(img > 0)
    c = np.array(center_of_mass(img, lab, range(1, n + 1)))[:, ::-1]
    return (c - [W / 2 - 0.5, H / 2 - 0.5]) / F

def homography_pose(s, s_star, d_star=0.5):
    """Desired-to-current homography x ~ H x*, decomposed with the known normal n* = z."""
    A = []
    for (x, y), (xs, ys) in zip(s, s_star):
        A += [[xs, ys, 1, 0, 0, 0, -x * xs, -x * ys, -x],
              [0, 0, 0, xs, ys, 1, -y * xs, -y * ys, -y]]
    Hm = np.linalg.svd(np.array(A))[2][-1].reshape(3, 3)
    Hm /= np.linalg.svd(Hm, compute_uv=False)[1]          # scale so that sigma_2 = 1
    Hm *= np.sign(np.r_[s[0], 1] @ Hm @ np.r_[s_star[0], 1])  # positive depth ratio
    R_cd = np.c_[Hm[:, 0], Hm[:, 1], np.cross(Hm[:, 0], Hm[:, 1])]  # H a = R a, a ⟂ n*
    t_cd = (Hm[:, 2] - R_cd[:, 2]) * d_star               # H = R + t n*^T / d*
    return R_cd.T, -R_cd.T @ t_cd                         # c*R_c and c*t_c

s_star, Z_star = project(np.eye(3), t_d)
L_star = L_stack(s_star, Z_star)

def control(law, R, t):
    s, Z = project(R, t)
    if law == "PBVS":                          # s = (c*t_c, theta u), Part I eq. (18)
        Rr, tr = homography_pose(s, s_star)
        return np.r_[-LAM * Rr.T @ tr, -LAM * Rotation.from_matrix(Rr).as_rotvec()]
    L = {"IBVS L(s)": L_stack(s, Z), "IBVS L*": L_star,
         "IBVS mean": 0.5 * (L_stack(s, Z) + L_star)}[law]
    return -LAM * np.linalg.pinv(L) @ (s - s_star).ravel()

def servo(law, R, t):
    errs, cams, imgs = [], [t], [project(R, t)[0]]
    for k in range(STEPS + 1):
        s, Z = project(R, t)
        errs.append(np.linalg.norm(s - s_star))
        if Z.min() < 0.05 or Z.max() > 5:
            why = "at the target plane" if Z.min() < 0.05 else "retreated past 5 m"
            return errs, cams, imgs, R, t, f"stopped at step {k}: {why}"
        if k < STEPS:
            v = control(law, R, t)
            if np.abs(v).max() > 10:
                return errs, cams, imgs, R, t, f"stopped at step {k}: command > 10"
            R, t = move(R, t, v * DT)
            cams.append(t); imgs.append(project(R, t)[0])
    return errs, cams, imgs, R, t, ""

def report(law, R0, t0):
    errs, C, S, R, t, stop = servo(law, R0, t0)
    C, S = np.array(C), np.array(S)
    seg, rel = S[-1] - S[0], S - S[0]          # straight image segment of each point
    dev = np.abs(rel[..., 0] * seg[:, 1] - rel[..., 1] * seg[:, 0])
    dev /= np.linalg.norm(seg, axis=1)         # distance from the straight segment
    ext = max(np.abs(S[..., 0]).max() * F / (W / 2), np.abs(S[..., 1]).max() * F / (H / 2))
    path = 1000 * np.linalg.norm(np.diff(C, axis=0), axis=1).sum()
    dist = np.linalg.norm(C, axis=1)           # camera distance to the target centre
    rot = np.degrees(Rotation.from_matrix(R).magnitude())
    end = (f"{stop}, R err {rot:.0f} deg" if stop else
           f"{errs[25]:8.1e} {errs[-1]:7.0e} {1000 * np.linalg.norm(t - t_d):9.1e} {rot:9.1e}")
    print(f"  {law:9s} {F * dev.max():6.1f} {ext:6.2f} {path:7.0f} "
          f"{1000 * np.linalg.norm(C[-1] - C[0]):6.0f}  {dist.min():.2f}-{dist.max():.2f}  {end}")

R0 = Rotation.from_rotvec(np.radians([0, 30, 60])).as_matrix()
t0 = -R0 @ np.array([0.1, 0, 0.7])             # target centre at (0.1, 0, 0.7) m in camera
s0, Z0 = project(R0, t0)
num = np.array([(project(*move(R0, t0, 1e-7 * np.eye(6)[j]))[0] - s0).ravel() / 1e-7
                for j in range(6)]).T
c = centroids(render(s0))
print(f"L vs finite differences: {np.abs(L_stack(s0, Z0) - num).max():.1e}")
print(f"dot centroids vs projections: "
      f"{F * np.linalg.norm(c[:, None] - s0, axis=2).min(axis=0).max():.3f} px")
Rr, tr = homography_pose(s0, s_star)
print(f"homography pose error: {np.abs(Rr - R0).max():.1e}, "
      f"{np.abs(tr - (t0 - t_d)).max():.1e} m")

def rot_z(deg):
    return Rotation.from_rotvec([0, 0, np.radians(deg)]).as_matrix()

for title, R_init, t_init in [("30 deg about y, 60 deg about z, target off-centre", R0, t0),
                              ("150 deg about the optical axis", rot_z(150), t_d),
                              ("180 deg about the optical axis", rot_z(180), t_d)]:
    s_i, Z_i = project(R_init, t_init)
    rank = np.linalg.matrix_rank(0.5 * (L_stack(s_i, Z_i) + L_star), tol=1e-10)
    v0 = np.abs(control("IBVS mean", R_init, t_init)).max()
    print(f"\n{title}: |e(0)| = {np.linalg.norm(s_i - s_star):.3f}, "
          f"mean L: rank {rank}, first command {v0:.0e}")
    print("            dev px extent path mm net mm  dist m      |e| 1 s |e| 12 s"
          "  t err mm R err deg")
    for law in ["IBVS L(s)", "IBVS L*", "IBVS mean", "PBVS"]:
        report(law, R_init, t_init)

Output (NumPy 2.3.5, SciPy 1.16.3; two runs gave identical output):

L vs finite differences: 5.0e-08
dot centroids vs projections: 0.017 px
homography pose error: 3.9e-15, 2.6e-15 m

30 deg about y, 60 deg about z, target off-centre: |e(0)| = 0.582, mean L: rank 6, first command 1e+00
            dev px extent path mm net mm  dist m      |e| 1 s |e| 12 s  t err mm R err deg
  IBVS L(s)    5.1   0.72     511    420  0.50-0.74   2.1e-01   3e-06   2.5e-03   2.3e-04
  IBVS L*    127.6   1.19     591    420  0.47-0.71   3.1e-01   4e-06   2.6e-03   4.1e-04
  IBVS mean   66.1   0.91     450    420  0.50-0.71   2.6e-01   4e-06   1.7e-03   2.9e-04
  PBVS       139.2   1.24     420    420  0.50-0.71   3.5e-01   6e-06   2.0e-03   3.2e-04

150 deg about the optical axis: |e(0)| = 1.093, mean L: rank 6, first command 7e+00
            dev px extent path mm net mm  dist m      |e| 1 s |e| 12 s  t err mm R err deg
  IBVS L(s)    4.8   0.91    3086      0  0.50-2.04   4.1e-01   5e-06   4.5e-03   1.2e-04
  IBVS L*    137.4  22.61     520    520  0.02-0.50  stopped at step 10: at the target plane, R err 116 deg
  IBVS mean  167.7   0.94       0      0  0.50-0.50   3.8e-01   5e-06   1.1e-13   5.1e-04
  PBVS       167.7   0.94       0      0  0.50-0.50   5.1e-01   7e-06   1.4e-13   7.2e-04

180 deg about the optical axis: |e(0)| = 1.131, mean L: rank 4, first command 1e-14
            dev px extent path mm net mm  dist m      |e| 1 s |e| 12 s  t err mm R err deg
  IBVS L(s)    0.0   0.67    1553   1553  0.50-2.05  stopped at step 13: command > 10, R err 180 deg
  IBVS L*      0.0   4.66     588    588  0.07-0.50  stopped at step 9: at the target plane, R err 180 deg
  IBVS mean    0.0   0.67       0      0  0.50-0.50  stopped at step 28: command > 10, R err 180 deg
  PBVS       226.3   0.94       0      0  0.50-0.50   6.1e-01   9e-06   3.8e-13   8.6e-04

The checks confirm the pieces: the analytic matrix agrees with finite differences to the size of the finite-difference error, so the derivation and its signs hold for the camera-velocity convention; the dot centroids in the rendered image match the projected features to a fiftieth of a pixel; and the homography recovers the pose to rounding error, confirming the frame convention discussed above.

The general displacement shows the trade-off Part I describes. IBVS with the current matrix keeps every point within a few pixels of a straight image path, and its image error falls by the factor $e^{-1}$ in the first second, as the control law asks; but the camera first moves away from the target and travels about a fifth further than the straight line. PBVS moves the camera exactly along the straight line, while an image point goes past the image border by about a quarter of the half-image size, so a real camera would lose it. The desired matrix also loses a point and takes the longest camera path, and the mean matrix lies between the extremes. All four reach the goal to micrometres: the difference is in the route, not the destination.

The 150° rotation about the optical axis reproduces Part I’s geometric interpretation. The current matrix converges, but only after the camera retreats to four times the goal distance and comes back. The desired matrix drives the camera forward into the target plane while the points spread far outside the image. The mean matrix and PBVS rotate the camera in place, with the points on circular arcs.

At 180° the runs reproduce Chaumette’s (1998) cases exactly: straight image paths and a pure retreat with the current matrix, a pure advance into the target plane with the desired one, and no rotation in either. For the current matrix, in continuous time the points would reach the principal point together when $e^{-\lambda t} = 1/2$, after $\ln 2/\lambda \approx 0.69$ s (17 steps), with the retreat speed growing without bound, so the run hits the command limit first. The mean matrix has rank 4 here: with every point mirrored through the centre, the third and sixth columns of $\mathbf{L}(\mathbf{s})$ are exactly the negatives of those of $\mathbf{L}^*$ and cancel, and the first command is zero to rounding. The error lies in $\operatorname{Ker}\widehat{\mathbf{L}_e^+}$ and the camera does not move measurably; the later stop is numerical, rounding errors amplified through the rank-deficient matrix until one command is oversized. PBVS, which servos on $\theta\mathbf{u}$, rotates through 180° in place. The example uses exact features, depths and calibration, so it shows the routes, not the robustness differences discussed under Stability.

Properties and Behavior

The three families trade the same properties against each other:

	IBVS	PBVS	2.5D
Error defined in	image	3D pose	image point, depth ratio, rotation
Proven stability (perfect conditions)	local only for $k > 6$	global	global
Image trajectories	nearly straight	uncontrolled	straight for the reference point
Camera trajectory	can retreat or advance	straight (second scheme)	near the first PBVS scheme
3D information needed	depths (current or desired)	intrinsics and a model, or a scaled reconstruction	homography and an approximate $d^*$
Final accuracy under calibration errors	unaffected	that of the pose estimate	unaffected

For 2.5D servoing, Malis et al. (1999) show that the error estimated with wrong intrinsics vanishes if and only if the true error does, since the rotation angle and $\rho_Z$ are recovered exactly and the image point is compared in the same pixels.

As the error decreases, the current and desired matrices approach each other and all IBVS variants tend to the same behaviour (Part I). Chaumette and Hutchinson conclude Part I that neither IBVS nor PBVS is superior in general; there is only a set of performance trade-offs, which the hybrid and advanced schemes try to improve.

Limitations

• Features must be tracked and matched throughout the motion. Part I places feature tracking and pose estimation outside its scope; a lost track or a mismatch corrupts the error directly.
• Kinematics only. The tutorial considers velocity controllers. Part II notes that robot dynamics must be modelled for high-speed tasks and for nonholonomic or underactuated robots, and that other sensors (fisheye or catadioptric cameras, ultrasound probes) require new feature models.

Variants

Part II surveys schemes designed to fix the weaknesses of the basic laws:

• Partitioned IBVS. Corke and Hutchinson (2001) separated the optical-axis motions $(v_z, \omega_z)$ from the others, controlling $\omega_z$ from the angle $\alpha$ of the segment joining two feature points and $v_z$ from the area $\sigma^2$ of the polygon they form, and computing the remaining four velocities with $\mathbf{L}_{xy}^+$ while compensating for the motion along the axis.
• Cylindrical coordinates. Iwatsuki and Okiyama (2005) used the polar coordinates $(\rho, \theta)$ of the image points: $\rho$ is invariant to $\omega_z$, and $\theta$ is invariant to $v_z$ and linear in $\omega_z$. With the constant desired matrix, Part II reports behaviour as good as the mean matrix with Cartesian points, including the rotation about the optical axis.
• Other geometric features and image moments. Interaction matrices exist for segments, lines, spheres, circles and cylinders, and for the image moments of planar objects of any shape (Chaumette, 2004; Tahri and Chaumette, 2005); combinations of moments give nearly decoupled systems, but, with few exceptions, their translational columns still depend on depth.
• Switching schemes. Gans and Hutchinson (2003) switch between IBVS and PBVS whenever the Lyapunov function of the inactive scheme exceeds a threshold; for a 160° rotation about the optical axis, the camera starts with straight image paths, switches to PBVS as it begins to retreat, and finishes with a rotation and a forward motion.
• Optimal control and redundancy. Linear quadratic designs trade tracking error against motion; the gradient projection method adds secondary tasks, such as joint-limit avoidance, in the null space of the visual task, which exists only when that task does not constrain all six degrees of freedom.
• Feature trajectory planning. Planning a feature trajectory $\mathbf{s}^*(t)$ offline, for example with potential fields that account for visibility, joint limits and obstacles, keeps the error small along the way, which also limits the effect of modelling errors; the control law becomes $\mathbf{v}_c = -\lambda\widehat{\mathbf{L}_e^+}\mathbf{e} + \widehat{\mathbf{L}_e^+}\dot{\mathbf{s}}^*$.
• Stereo IBVS. Stacking the coordinates of a point in both images of a calibrated stereo pair gives a $4 \times 6$ matrix of rank 3, because of the epipolar constraint, so three points are again needed (Part I).

Visual servoing relies on camera calibration, camera pose estimation and feature tracking for its inputs. Hutchinson et al. (1996) list applications proposed or prototyped by then in manufacturing (grasping objects on conveyor belts, part mating), teleoperation, fruit picking, car steering and aircraft landing, and Part II names mobile robots indoors and outdoors, aerial, space and underwater robots, and medical robotics as fields for further applications.