⚡ electrical engineering

Geometric Visual Servo Via Optimal Transport

This paper proposes a geometric visual servoing control law for robotic manipulators that models camera inputs as probability measures on the Special Euclidean group and utilizes Wasserstein distance-based geodesic flows to minimize pose and feature errors, thereby enabling robust tracking across various initial positions.

Original authors: Ethan Canzini, Simon Pope, Ashutosh Tiwari

Published 2026-04-02

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Ethan Canzini, Simon Pope, Ashutosh Tiwari

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to teach a robot arm to pick up a specific object, like a screwdriver, and place it perfectly into a hole. Traditionally, you would tell the robot: "Move your hand to these exact coordinates." But what if the robot's camera is blurry, or the lighting changes, or the object is slightly different than expected? The robot might get confused and miss the target.

This paper proposes a smarter, more flexible way to guide the robot. Instead of giving it a rigid set of coordinates, the authors treat the robot's vision like a cloud of floating particles and use a mathematical concept called Optimal Transport to guide it.

Here is a breakdown of their idea using simple analogies:

1. The Problem: The "Pixel-by-Pixel" Struggle

Usually, robots use "Visual Servoing" (using eyes to control movement). They do this in two ways:

Pose-Based: "Move your hand to X, Y, Z." (Like following a GPS pin).
Image-Based: "Move until the red dot in your camera matches the red dot on the screen." (Like lining up a picture).

The Flaw: Both methods rely on picking specific points (like the tip of a screw). If the camera is noisy or the object is weirdly shaped, the robot gets lost. It's like trying to navigate a city by only looking at one specific street sign; if that sign is covered in mud, you're stuck.

2. The Solution: The "Cloud of Dust" Analogy

The authors suggest a different approach. Instead of looking at specific points, imagine the robot's camera sees the object as a cloud of dust or a fog.

Current State: The robot sees a cloud of dust in one shape and location.
Target State: The robot wants that cloud to look exactly like a different shape in a different location.

The goal isn't to move one specific grain of dust; it's to move the entire cloud so it matches the target shape perfectly.

3. The Magic Tool: Optimal Transport (The "Moving Company")

How do you move a cloud of dust efficiently? You use Optimal Transport.
Think of this as a super-smart moving company.

You have a pile of boxes (the current cloud) in one room.
You have a target arrangement of boxes (the desired cloud) in another room.
Optimal Transport calculates the absolute most energy-efficient way to move every single box from the pile to the target arrangement without wasting a single joule of energy.

In the paper, they use a specific metric called the Wasserstein distance. Think of this as the "cost of moving the cloud." The robot's brain calculates the path that minimizes this cost, ensuring the movement is smooth and direct.

4. The Engine: Geometric Control (The "River Flow")

Once the robot knows where the cloud needs to go, it needs to know how to move its arm to get there.

The authors use a concept from physics called Port-Hamiltonian systems. Imagine the robot arm is a boat floating on a river.
The "river" is the mathematical space where the robot moves (called SE(3), which is just a fancy way of saying "3D space with rotation").
The robot doesn't just jerk its arm around; it flows along the "geodesic" (the shortest, smoothest path) of this river, just like water flowing downhill.

5. The Hybrid Approach: The "Best of Both Worlds"

The paper combines two types of control into one super-controller:

The "GPS" part (Pose Control): It ensures the robot arm physically moves to the right spot, compensating for gravity (so the arm doesn't drop).
The "Cloud" part (Image Control): It constantly checks the "cloud of dust" (the depth map from the camera). If the cloud is slightly off, the Optimal Transport math tells the robot exactly how to nudge the arm to fix the whole cloud shape, not just one point.

Why is this cool?

It's Robust: If the camera sees a slightly different shape or has some noise, the "cloud" method is forgiving. It doesn't care if one specific pixel is wrong; it cares about the overall shape.
It's Efficient: By using the "moving company" math, the robot takes the most direct, energy-saving path to the goal.
It's Smooth: The robot doesn't jitter. It flows like water toward the target.

The Real-World Test

The authors tested this on a real robot arm. They gave it a task to move a "peg" into a "hole" from four different starting positions.

Result: The robot successfully moved the "cloud" of the peg into the "cloud" of the hole every time, even when starting from weird angles.
Speed: It did this in real-time (about 300 milliseconds per calculation), which is fast enough for a human to watch it happen smoothly.

Summary

Imagine you are trying to fold a blanket into a suitcase.

Old Way: You try to match the corners of the blanket to the corners of the suitcase exactly. If the blanket is wrinkled, you fail.
This Paper's Way: You look at the blanket as a whole shape. You gently push and pull the fabric (using the most efficient energy) until the entire shape of the blanket perfectly matches the shape of the suitcase, regardless of where the corners started.

This paper gives robots a "feel" for the whole shape of an object, allowing them to move more naturally, safely, and accurately in a messy, unpredictable world.

1. Problem Statement

The paper addresses the limitations of current Visual Servoing (VS) control laws for robotic manipulators. Traditional approaches generally fall into two categories:

Pose-Based Visual Servo (PBVS): Relies on extracting specific geometric features to estimate the camera pose relative to a target. This often requires hand-tuned feature extraction and can be noisy.
Image-Based Visual Servo (IBVS): Tracks image features directly to minimize error but often lacks a rigorous geometric framework for the underlying system dynamics.

Key Challenges Identified:

Feature Dependency: Current methods rely on hand-crafted features, which may lack the granularity required for high-accuracy tasks in complex or cluttered environments.
Dynamic Disconnect: Many control algorithms ignore the probabilistic nature of extracted features (e.g., point clouds) and fail to integrate system dynamics (inertia, gravity) with the visual error signal effectively.
Coordinate Issues: Standard approaches often rely on inverse kinematics and coordinate-level PD control, which can compromise performance guarantees and lose the physical characteristics of the robotic platform.

The authors propose a unified framework that treats the visual servoing problem as a geometric optimal control problem, utilizing the full depth map (point cloud) as a probability measure rather than discrete features.

2. Methodology

The proposed approach integrates Lie Group theory, Port-Hamiltonian systems, and Optimal Transport (OT) to create a geometric control law.

A. Geometric System Dynamics (Port-Hamiltonian on SE(3))

The authors model the robot manipulator dynamics on the Special Euclidean group SE(3), which represents rigid body motions (rotations and translations).

Formulation: They derive the system dynamics in Port-Hamiltonian (pH) form. This structure preserves the geometric properties of the system (energy conservation, passivity) and allows for the separation of energy storage (kinetic/potential) and dissipation.
State Representation: The state is defined on the cotangent bundle $T^*SE(3)$ , comprising the pose $g$ and conjugate momenta $p$ .
Control Structure: A standard passivity-based controller (PBC) is designed to reshape the Hamiltonian energy function. This involves:
- Energy Shaping ( $u_{ES}$ ): Modifying the potential energy to create a stable equilibrium at the desired pose.
- Damping Injection ( $u_{DI}$ ): Adding damping to ensure asymptotic stability.
- This results in a geometric PD controller with gravity compensation.

B. Optimal Transport for Visual Error

Instead of tracking discrete features, the authors treat the depth map from an RGB-D camera as a probability measure on the SE(3) group.

Unbalanced Optimal Transport: Since point clouds from cameras may have varying densities or missing data (mass is not strictly conserved), the authors use Unbalanced Optimal Transport. This relaxes the strict mass conservation constraint of standard Wasserstein distance.
Dynamic Transport Map: The control problem is framed as finding a flow map that transports the current depth distribution ( $\mu$ ) to the target depth distribution ( $\nu$ ) with minimum energy.
Disturbance Compensation ( $u_{DC}$ ): The optimal transport map ( $T^*$ ) is computed to generate a control input that minimizes the Wasserstein distance between the current and target depth clouds. This term acts as a dynamic error correction, effectively steering the system toward the target distribution in the image space.

C. The Unified Control Law

The final control input $u$ is a combination of three components:
$u = u_{ES} + u_{DI} + u_{DC}$

$u_{ES}$ (Energy Shaping): Drives the system toward the desired pose using geometric PD control.
$u_{DI}$ (Damping Injection): Stabilizes the system dynamics.
$u_{DC}$ (Dynamic Transport): Corrects errors based on the optimal transport of the depth map, bridging the gap between pose and image-based control.

3. Key Contributions

Port-Hamiltonian Formulation on SE(3): The authors provide a rigorous derivation of robot manipulator dynamics in port-Hamiltonian form on the SE(3) Lie group, combining task-space geometry with joint-space control while preserving geometric invariants.
Probabilistic Depth Map Representation: They redefine the visual error by treating the RGB-D camera output as a probability measure on the Lie group, eliminating the need for hand-crafted feature extraction.
Geometric Optimal Transport Controller: A novel control law is developed that uses Unbalanced Optimal Transport to generate a dynamic transport map. This map injects energy into the system to steer the depth cloud toward the target, effectively unifying PBVS and IBVS.
Stability Proof: The paper provides a Lyapunov stability proof (Theorem 12) demonstrating that the closed-loop system is asymptotically stable, provided the Jacobian is full rank and gain matrices are positive definite.

4. Results and Evaluation

The method was validated on a real-world robotic manipulator using a "peg-in-hole" style task with randomized initial poses.

Convergence: The controller successfully guided the robot from various initial poses to the target pose. The trajectory followed the geodesic (shortest path) in SE(3), demonstrating minimum-energy control.
Robustness: The system handled rotational and positional errors smoothly. Even when encountering local instabilities (e.g., near kinematic singularities where the Jacobian rank drops), the controller recovered.
Performance: The system achieved real-time performance with a control loop time of 300ms, even while computing complex transport maps and handling rotational disturbances.
Energy Analysis: The results showed that the kinetic energy of the manipulator trended toward zero as it approached the target, confirming the system's ability to settle at the desired equilibrium under gravity compensation.

5. Significance and Future Work

Significance:

Unified Framework: This work bridges the gap between pose-based and image-based visual servoing, offering a single framework that leverages the full richness of depth data without manual feature engineering.
Geometric Rigor: By grounding the control in Lie group theory and Optimal Transport, the method offers stronger theoretical guarantees regarding stability and energy efficiency compared to heuristic approaches.
Scalability: The approach is applicable to any autonomous system using vision, particularly in complex environments where defining specific features is difficult.

Future Directions:

Dynamic Targets: Extending the method to handle moving targets (where $\dot{g}_1 \neq 0$ ), which would require reformulating the boundary conditions for the transport problem.
Computational Efficiency: Addressing the computational cost of real-time transport map generation for high-resolution point clouds, potentially via Neural Optimal Transport methods.
Singularity Handling: Developing strategies (e.g., gain scheduling or null-space projection) to explicitly handle kinematic singularities where the Jacobian becomes rank-deficient.

In conclusion, this paper presents a mathematically rigorous and practically effective method for robotic visual servoing, transforming the problem from simple feature tracking into a geometric mass-transport optimization problem.