Geometric Visual Servo Via Optimal Transport

Abstract

When developing control laws for robotic systems, the principle factor when examining their performance is choosing inputs that allow smooth tracking to a reference input. In the context of robotic manipulation, this involves translating an object or end-effector from an initial pose to a target pose. Robotic manipulation control laws frequently use vision systems as an error generator to track features and produce control inputs. However, current control algorithms don't take into account the probabilistic features that are extracted and instead rely on hand-tuned feature extraction methods. Furthermore, the target features can exist in a static pose thus allowing a combined pose and feature error for control generation. We present a geometric control law for the visual servoing problem for robotic manipulators. The input from the camera constitutes a probability measure on the 3-dimensional Special Euclidean task-space group, where the Wasserstein distance between the current and desired poses is analogous with the geometric geodesic. From this, we develop a controller that allows for both pose and image-based visual servoing by combining classical PD control with gravity compensation with error minimization through the use of geodesic flows on a 3-dimensional Special Euclidean group. We present our results on a set of test cases demonstrating the generalisation ability of our approach to a variety of initial positions.

Figures (5)

• Figure 1: Our proposed control architecture for geometric visual servoing. The computation of the optimal transport map $T^\ast$, shown in red, is performed during the initialisation of the control loop using the initial depth map and pose (Section \ref{['sec:sub:features']}). The transport map is then used online during the computation of the disturbance correction input $\mathbf{u}_{\text{DC}}$ using the current camera depth map, highlighted in blue (Section \ref{['sec:sub:energy-ot']}). The input is then combined with the passivity-based gains (Section \ref{['sec:sub:closed-loop']}) to compute the input to the manipulator.
• Figure 2: Example of the depth maps supported on the $\mathtt{SE}(3)$ Lie group. The initial depth map $\rho_0 \in \mathcal{P}(\mathtt{SE}(3))$ is supported on the initial pose $\mathfrak{g}_0 \in \mathtt{SE}(3)$, and the target depth map $\rho_1 \in \mathcal{P}(\mathtt{SE}(3))$ is supported on the target pose $\mathfrak{g}_1 \in \mathtt{SE}(3)$. The geodesic distance $d(\mathfrak{g}_0, \mathfrak{g}_1)$ maps the shortest distance in the group space whilst the $p$-Wasserstein distance $\mathbb{W}_\tau^2$ maps the initial to target depth maps in probability space. The colours in the depth maps correspond to the distance from the camera, with blue particles indicating close-range measurements and green particles indicating the distance measurements within the limit of the camera range.
• Figure 3: Plots of the feature being evaluated, taken at the target pose $\mathfrak{g}_1$.
• Figure 4: Segment of the trajectory from pose 1 to the target pose $\mathfrak{g}^\ast$. As compared with Figure \ref{['fig:results']}, there is a fast response that slows as the robot manipulator trends towards the goal position.
• Figure 5: Plots of the results from 4 different initial poses to the goal pose: (a) The geodesic distance $d(\mathfrak{g}_t, \mathfrak{g}_1)$ in (\ref{['eq:geodesic']}); (b) The position error $\delta_\mathbf{p}(\mathfrak{g}_t, \mathfrak{g}_1)$ in (\ref{['eq:delta-p']}); (c) The rotational error $\delta_\mathbf{R}((\mathfrak{g}_t, \mathfrak{g}_1)$ in (\ref{['eq:delta-R']}); (d) The wrench input $\mathbf{u}_t$ from (\ref{['eq:wrench']}); (e) The kinetic energy flow $E_\mathcal{H}[t]$ from (\ref{['eq:hamil-KE']}).

Theorems & Definitions (15)

• Definition 1
• Definition 2
• Definition 3
• Definition 4
• Definition 5
• Definition 6
• Definition 7
• Definition 8
• Definition 9
• Definition 10