SE(3) Linear Parameter Varying Dynamical Systems for Globally Asymptotically Stable End-Effector Control

TL;DR

The paper addresses the challenge of stable end-effector pose control by extending the LPV-DS framework to SE(3) through a Quaternion-DS for orientation and an integrated SE(3) LPV-DS that couples position and orientation. It leverages Riemannian geometry (log/exp maps, tangent spaces) to model quaternion data and uses a GMM-based, Lyapunov-constrained learning pipeline to guarantee global asymptotic stability. A stability analysis, conversion from quaternion outputs to angular velocity, and a quaternion mixture model are developed alongside a semidefinite optimization to learn the linear dynamics. Empirical validation on RoboTasks9 and four real-robot tasks demonstrates accurate trajectory reproduction, robustness to perturbations, and favorable computational efficiency relative to neural baselines, with a compact, explainable model:** $V(oldsymbol{q})$ and the associated Lyapunov constraints underpin GAS in the discrete-time setting. The proposed SE(3) LPV-DS thus provides a principled, efficient approach to joint pose control applicable to real-world robotic manipulation tasks.

Abstract

Linear Parameter Varying Dynamical Systems (LPV-DS) encode trajectories into an autonomous first-order DS that enables reactive responses to perturbations, while ensuring globally asymptotic stability at the target. However, the current LPV-DS framework is established on Euclidean data only and has not been applicable to broader robotic applications requiring pose control. In this paper we present an extension to the current LPV-DS framework, named Quaternion-DS, which efficiently learns a DS-based motion policy for orientation. Leveraging techniques from differential geometry and Riemannian statistics, our approach properly handles the non-Euclidean orientation data in quaternion space, enabling the integration with positional control, namely SE(3) LPV-DS, so that the synergistic behaviour within the full SE(3) pose is preserved. Through simulation and real robot experiments, we validate our method, demonstrating its ability to efficiently and accurately reproduce the original SE(3) trajectory while exhibiting strong robustness to perturbations in task space.

Figures (6)

• Figure 1: The schematic of the SE(3) LPV-DS formulation which is composed of an ordinary LPV-DS PC-GMM for position control and Quaternion-DS for orientation control; the architecture of Quaternion-DS consists of the clustering of the orientation trajectory and the optimization to minimize the prediction error; the resulting SE(3) LPV-DS takes the end-effector states: position $\bold{p}$ and orientation $\bold{q}$ as inputs, and generates the estimated desired linear velocity $\Hat{v}$ and angular velocity $\Hat{\omega}$, which are then passed down to command the robot via a low-level feedback controller.
• Figure 2: Illustrative examples of the operations on Riemannian geometry: exponential/logarithmic mapping (left) and parallel transport (right), which are depicted on a $\mathbb{S}^2$ manifold embedded in $\mathbb{R}^3$
• Figure 3: The quaternion mixture model ($K=4$) with the mean orientation of each Gaussian (left); note the color is only indicative of the clustering results on the quaternion trajectory, not position. The simulation result of quaternion-DS (right) shows the evolution of the quaternions in its 4 coordinates, where the thin lines are demonstration trajectory, and the thick lines are reproduction.
• Figure 4: The simulation results of SE(3) LPV-DS in the event of perturbation (left), where the demonstration data and clustering results are scatter points in color and the reproduction is the black curve; the value of mixing function $\gamma(\cdot)$ corresponding to each Gaussian ($K=4$) during the simulation (right), where the solid lines are computed in SE(3) LPV-DS by Eq. \ref{['eq:se3_gamma']}, and the dashed lines are computed in Quaternion-DS by Eq. \ref{['eq:quat_gamma']}.
• Figure 5: Comparison of parameter size and computation time with data size (top), and DTW error and quaternion error (lower is better) over 9 tasks in three scenarios. Solid bars represent our approach, and hatched bars depict the baseline. Bars range from lower quartile to upper quartile, with whiskers indicating extreme values.