KoopMotion: Learning Almost Divergence Free Koopman Flow Fields for Motion Planning

TL;DR

KoopMotion addresses the challenge of learning motion plans from demonstrations with convergence guarantees by combining Koopman operator theory with Fourier-feature lifting to produce flow fields for motion planning. It introduces two novel losses: an almost divergence-free flow constraint and a goal-convergence constraint, incorporated into a unified loss $L_{Koopman}$, $L_{FlowDivergence}$, and $L_{Goal}$, and demonstrates that the learned discrete-time Koopman model yields stable, convergent trajectories that track desired motions even from outside the training set. The approach achieves strong performance on 2D LASA handwriting data, 3D end-effector trajectories, and hardware experiments with a miniature autonomous surface vehicle, while requiring as little as 3% of the LASA data. Spectral analysis of the learned Koopman operator confirms asymptotic stability, and the method shows favorable improvements in the swept area metric over baselines, indicating better spatiotemporal fidelity and robustness to disturbances. Overall, KoopMotion provides a sample-efficient framework for motion planning with convergence properties, offering practical impact for robust robot motion under disturbances and in dynamic environments.

Abstract

In this work, we propose a novel flow field-based motion planning method that drives a robot from any initial state to a desired reference trajectory such that it converges to the trajectory's end point. Despite demonstrated efficacy in using Koopman operator theory for modeling dynamical systems, Koopman does not inherently enforce convergence to desired trajectories nor to specified goals - a requirement when learning from demonstrations (LfD). We present KoopMotion which represents motion flow fields as dynamical systems, parameterized by Koopman Operators to mimic desired trajectories, and leverages the divergence properties of the learnt flow fields to obtain smooth motion fields that converge to a desired reference trajectory when a robot is placed away from the desired trajectory, and tracks the trajectory until the end point. To demonstrate the effectiveness of our approach, we show evaluations of KoopMotion on the LASA human handwriting dataset and a 3D manipulator end-effector trajectory dataset, including spectral analysis. We also perform experiments on a physical robot, verifying KoopMotion on a miniature autonomous surface vehicle operating in a non-static fluid flow environment. Our approach is highly sample efficient in both space and time, requiring only 3\% of the LASA dataset to generate dense motion plans. Additionally, KoopMotion provides a significant improvement over baselines when comparing metrics that measure spatial and temporal dynamics modeling efficacy. Code at: \href{https://alicekl.github.io/koop-motion/}{\color{blue}{https://alicekl.github.io/koop-motion}}.

Figures (11)

• Figure 1: Overview of KoopMotion. Demonstration trajectories are acquired, representing desired nonlinear motions that a robot should follow. Trajectories are inputs to the KoopMotion model, which estimates an almost divergence-free flow field for motion planning. This flow field drives a robot from locations away from the desired motion towards the desired reference trajectory and towards convergence to a desired goal. After scaling to the vehicle's velocity limits, learnt KoopMotion flow fields can be used for motion planning on real robots. In this work, we train KoopMotion flow fields on demonstrations of human-drawn trajectories from the LASA handwriting dataset, describing the desired robot path in time. We evaluate the efficacy of using the learnt flow fields to produce reference trajectories for miniature autonomous surface vehicles navigating in a flow field tank with non-static conditions.
• Figure 2: Qualitative performance of KoopMotion for learning motion planning flow fields for 30 nonlinear planar demonstrations from the LASA handwriting dataset. Training data, temporally sub-sampled, using less than $3\%$ of original training data (red), learnt motion plans from same initial conditions as training data (black), motion plan from all other initial conditions in the domain (gray). Eigenfunctions of some shapes are shown in Supplementary \ref{['sec:supp_eigenfunctions']}.
• Figure 3: Qualitative experiments
• Figure 4: Performance evaluation of mean time dynamic time warping distance (DTWD) and mean swept error area (SEA) over 7 demonstrations for 4 nonlinear demonstrations of the LASA handwriting dataset. For both metrics, the lower the value, the better. Results have been recreated from Figure 3 found in ravichandar2017learning. KoopMotion (ours) has overall comparable DTWD metrics compared to baselines. This can also be qualitatively observed when comparing the trajectories as shown in Fig. \ref{['fig:lasa_demos']}, and those in trajectories in ravichandar2017learning and in khansari2014learning. On the other hand, KoopMotion significantly outperforms other baselines when comparing SEA metrics, which captures the spatiotemporal differences in the trajectories, and is difficult to visualize qualitatively otherwise.
• Figure 5: Experimental verification of KoopMotion with miniature autonomous surface vehicles operating in fluid flow tank on LASA left leaf (Leaf_1), center letter 'z' (ZShape), and right multi modal (Multi_Models_4) trajectory with two starting points. top row shows training data motion demonstrations in red, learnt motions for same initial conditions as training data in black, attracting KoopMotion flow field in gray. Other colors show experimental runs from different various conditions. middle row shows an example of snapshots of the autonomous surface vehicle in time, with convergence to the goal. White arrows indicate direction of motion. For the multi modal trajectories right, we show the trajectories of a robot starting from the left (segment 1), and starting from the right (segment 2), and separate the two real-world experiment trajectories to emphasize this. This experiment demonstrates that the KoopMotion flow fields can be used for multi-robot rendezvous. We note that we did not fine-tune the existing velocity controller for these experiments. bottom row shows the spectral analysis of the system, showing the eigenvalues of the discrete-time Koopman operator. The magnitude of all eigenvalues are less than 1, showing system stability.