Visual Servoing for Robotics

Visual Servoing Overview

Visual servoing uses camera feedback to control robot motion, closing the loop between vision and actuation. The system extracts visual features from the current image, compares them to desired features, and computes velocity commands that drive the robot to reduce the feature error. This enables precise manipulation and navigation without explicit 3D reconstruction, working directly in image space or through estimated 3D positions.

Theory: Image-Based Visual Servoing (IBVS)

IBVS computes velocity commands directly in image space without estimating 3D structure. The feature vector represents 2D image coordinates of tracked points, and the desired features define the target configuration. The interaction matrix (image Jacobian) relates feature velocities to camera velocities, enabling the controller to compute the camera velocity that drives the feature error to zero.

The image Jacobian for a point feature relates pixel velocity to camera twist:

Where each parameter means:

• is the velocity of the feature point in image coordinates (2D)
• is the 2x6 interaction matrix (image Jacobian) for that feature
• is the 6-DoF camera velocity (3 linear + 3 angular)
• The interaction matrix depends on the feature depth and camera intrinsics

IBVS control law computes velocity to minimize feature error:

Where each parameter means:

• is the commanded camera velocity
• is the control gain (typically 0.5 to 2.0)
• is the pseudo-inverse of the interaction matrix
• is the current feature error vector

Theory: Position-Based Visual Servoing (PBVS)

PBVS estimates the 3D pose of the target relative to the camera and performs servoing in Cartesian space. The pose is computed using PnP algorithms or known target geometry. The controller computes a velocity that drives the pose error to zero in SE(3).

The pose error in SE(3):

Where each parameter means:

• are the desired translation and rotation
• are the current estimated translation and rotation
• maps the rotation error to axis-angle representation
• The error is a 6-vector combining position and orientation errors

Mathematical Foundations

The interaction matrix for point features at depth :

Where each parameter means:

• is the depth of the feature point relative to the camera
• are the normalized image coordinates of the feature
• The matrix relates 3D camera motion to 2D image feature velocities
• Singular values indicate degenerate configurations

Lyapunov stability analysis ensures convergence:

Where each parameter means:

• is the Lyapunov candidate function (positive definite)
• is its time derivative (negative semi-definite)
• The derivative is negative when is positive definite
• Global asymptotic stability requires persistently exciting features

Architecture Design

Implementation

import numpy as np
import cv2


class VisualServoingController:
    def __init__(self, camera_matrix, dist_coeffs):
        self.K = camera_matrix
        self.dist = dist_coeffs
        self.feature_extractor = cv2.goodFeaturesToTrack
        self.tracker = cv2.KLT_create()
        self.lambda_gain = 0.5
        self.desired_features = None
        self.prev_gray = None

    def set_desired(self, image):
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if len(image.shape) == 3 else image
        corners = self.feature_extractor(gray, 4, 0.01, 10)
        self.desired_features = corners.reshape(-1, 2)

    def extract_features(self, gray):
        if self.prev_gray is None:
            corners = self.feature_extractor(gray, 4, 0.01, 10)
            self.prev_gray = gray
            return corners.reshape(-1, 2) if corners is not None else None
        new_pts, status, _ = self.tracker.calcOpticalFlowPyrLK(
            self.prev_gray, gray, self.desired_features.astype(np.float32).reshape(-1, 1, 2))
        self.prev_gray = gray
        return new_pts[status.flatten() == 1].reshape(-1, 2)

    def compute_interaction_matrix(self, features, depths):
        Le = np.zeros((2 * len(features), 6))
        fx, fy = self.K[0, 0], self.K[1, 1]
        cx, cy = self.K[0, 2], self.K[1, 2]
        for i, (u, v) in enumerate(features):
            x = (u - cx) / fx
            y = (v - cy) / fy
            Z = depths[i]
            Le[2*i] = [-1/Z, 0, x/Z, x*y, -(1+x**2), y]
            Le[2*i+1] = [0, -1/Z, y/Z, 1+y**2, -x*y, -x]
        return Le

    def compute_velocity(self, current_features, desired_features, depths):
        s = current_features.flatten()
        s_star = desired_features.flatten()
        e = s - s_star
        Le = self.compute_interaction_matrix(current_features, depths)
        Le_pinv = np.linalg.pinv(Le)
        v = -self.lambda_gain * Le_pinv @ e
        return np.clip(v, -0.5, 0.5)

    def step(self, image):
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if len(image.shape) == 3 else image
        current = self.extract_features(gray)
        if current is None or self.desired_features is None:
            return np.zeros(6)
        depths = np.ones(len(current)) * 0.5
        v = self.compute_velocity(current, self.desired_features, depths)
        error = np.linalg.norm(current - self.desired_features)
        return v, error


def simulate_servoing():
    K = np.array([[525, 0, 319.5], [0, 525, 239.5], [0, 0, 1]])
    controller = VisualServoingController(K, None)
    cap = cv2.VideoCapture(0)
    ret, frame = cap.read()
    if ret:
        controller.set_desired(frame)
    for step in range(300):
        ret, frame = cap.read()
        if not ret:
            break
        v, error = controller.step(frame)
        if error < 1.0:
            print(f"Converged at step {step}")
            break
    cap.release()

Comparison Table

Common Challenges

1. Local Minima: Feature configurations can get stuck in non-zero error equilibria
2. Visibility Constraints: Camera field-of-view limits may cause features to leave the image
3. Depth Estimation: IBVS requires depth for interaction matrix computation
4. Camera Calibration: PBVS requires accurate intrinsics and extrinsics
5. Dynamic Environments: Moving objects disrupt feature tracking and servoing stability

Case Study: Peg-in-Hole Assembly

A visual servoing system for peg-in-hole assembly task achieves 0.1mm positioning accuracy using IBVS with 4 corner features. The system runs at 30 Hz on an industrial robot arm with 6-DoF. Convergence from initial offset of 5cm and 15 degrees takes approximately 8 seconds with gain lambda=0.5. The interaction matrix is computed using estimated depth from the known peg geometry. Feature tracking uses KLT with sub-pixel refinement achieving 0.01 pixel accuracy. The control loop maintains stability with maximum angular velocity of 30 deg/s and linear velocity of 0.1 m/s, successfully completing 500 insertion trials with 99.8% success rate.

Key Takeaways

• Visual servoing closes the control loop using camera feedback without explicit 3D reconstruction
• IBVS works in image space making it robust to camera calibration errors
• PBVS provides intuitive Cartesian control but requires accurate 3D pose estimation
• The image Jacobian relates camera velocity to feature velocity in the interaction matrix
• Lyapunov stability analysis guarantees convergence for well-conditioned configurations
• Gain tuning balances convergence speed against oscillation and overshoot
• Hybrid approaches combine IBVS robustness with PBVS trajectory predictability