Goal-Conditioned Neural ODEs with Guaranteed Safety and Stability for Learning-Based All-Pairs Motion Planning

1. Problem Statement

The paper addresses the challenge of all-pairs motion planning in robotics, where a robot must navigate from an arbitrary initial state ($x_0$) to an arbitrary goal state ($x^\star$) within a safe set ($X_{safe}$) that may have complex geometry (e.g., narrow corridors, obstacles).

Key Challenges:

• Limitations of Classical Methods: Sampling-based methods (like RRT and PRM) are finite, lack smooth feedback policies for arbitrary start/goal pairs, and are difficult to integrate with learning-from-demonstration (LfD).
• Limitations of Existing Learning Methods: Many learning-based dynamical systems lack formal guarantees of safety (forward invariance of the safe set) and stability (convergence to the goal) simultaneously, especially when the goal state changes dynamically.
• Goal: To learn a smooth, goal-conditioned dynamical system $\dot{x} = f(x, x^\star)$ that guarantees:
  1. Safety: The trajectory never leaves $X_{safe}$ for any $x_0, x^\star \in X_{safe}$.
  2. Stability: Global exponential convergence to the goal $x^\star$.
  3. Generalization: The ability to handle unseen goal locations without retraining.
  4. Boundedness: Explicit bounds on velocity and tracking error.

2. Methodology

The proposed approach constructs a Goal-Conditioned Neural Ordinary Differential Equation (Neural ODE) using bi-Lipschitz diffeomorphisms.

A. Core Concept: Geometric Transformation

The method relies on learning a diffeomorphism $g: \mathbb{R}^n \to \mathbb{R}^n$ that maps the geometrically complex safe set $X_{safe}$ in the state space ($X$-space) to a simple, convex unit ball $B^n$ in a latent space ($Z$-space).

• Transformation: $z = g(x)$, where $z \in B^n$.
• Inverse: $x = g^{-1}(z)$.
• Property: $g$ is bi-Lipschitz, meaning it preserves topological structure and provides bounded distortion (Lipschitz constants $\mu$ and $\nu$).

B. Natural Gradient Flow Dynamics

Instead of standard gradient flow, the authors define the dynamics in the $X$-space as a natural gradient flow derived from a Lyapunov function defined in the $Z$-space.

1. Lyapunov Function in Z-space: $V(z, z^\star) = \frac{\lambda}{2} |z - z^\star|^2$, where $z^\star = g(x^\star)$.
2. Dynamics in Z-space: Simple linear convergence: $\dot{z} = \lambda(z^\star - z)$.
3. Pullback to X-space: The dynamics in the original space are constructed using the inverse of the Riemannian metric induced by $g$:
   $$\dot{x} = f(x, x^\star) := -M(x)^{-1} \nabla_x V(x, x^\star)$$
   where $M(x) = G(x)^\top G(x)$ and $G(x)$ is the Jacobian of $g$.

C. Learning Framework

• Model: The diffeomorphism $g$ is parameterized by a BiLipNet (a neural network architecture designed to enforce certified bi-Lipschitz bounds).
• Training Data:
  • Safety Data: Samples from $X_{safe}$ and $X_{unsafe}$.
  • Task Data: Demonstration trajectories or cost-to-go fields (computed via RRT/PRM) to guide the shape of the mapping inside the safe set.
• Loss Function:
  • Safety Loss: Ensures the sublevel sets of the learned Lyapunov function encompass all safe points and exclude unsafe points (effectively learning the boundary of $X_{safe}$).
  • Task Loss: Minimizes the error between the learned vector field and demonstration data (or cost-to-go gradients).

3. Key Theoretical Contributions

The paper provides rigorous mathematical proofs for the following properties:

1. Global Exponential Stability: The system is proven to be globally equilibrium-independent exponentially stable. The convergence rate is explicitly bounded by the parameter $\lambda$ and the bi-Lipschitz constants.
   $$|x(t) - x^\star| \leq \frac{\nu}{\mu} e^{-\lambda t} |x_0 - x^\star|$$
2. Safety (Forward Invariance): The safe set $X_{safe}$ is proven to be forward invariant for any goal $x^\star \in X_{safe}$. This is achieved because the straight-line trajectory in the convex $Z$-space (unit ball) is mapped back to a safe trajectory in $X$-space.
3. Explicit Bounds:
   • Velocity Bound: $|f(x, x^\star)| \leq \frac{2\lambda}{\mu}$.
   • Tracking Error: Explicit bounds are provided even when the goal is time-varying with bounded velocity.
4. Barrier Function Construction: An explicit barrier function $h(x) = 1 - |g(x)|^2$ is derived to certify safety, distinguishing this approach from standard Lyapunov-based methods that fail to guarantee safety for arbitrary goals.

4. Experimental Results

The method was validated on a 2D corridor navigation task with complex geometric obstacles.

• Setup: The safe set was a narrow, winding corridor. The model was trained using data from a single fixed goal (via RRT cost-to-go) and demonstration data.
• Performance:
  • All-Pairs Capability: The system successfully generated smooth, collision-free trajectories from arbitrary start points to arbitrary goal points within the safe set.
  • Generalization: Crucially, the model generalized to previously unseen goal locations without retraining. Trajectories converged smoothly to new goals while respecting obstacle boundaries.
  • Geometric Simplification: Visualizations confirmed that complex curved paths in the $X$-space corresponded to straight lines in the $Z$-space, validating the diffeomorphic transformation.

5. Significance and Impact

• Solving the "All-Pairs" Problem: Unlike traditional methods that require re-planning for every new goal, this approach learns a single feedback policy valid for all start-goal pairs within the safe set.
• Certifiable Safety in Learning: It bridges the gap between data-driven learning and formal safety guarantees. By using bi-Lipschitz constraints, the method avoids the "black box" nature of standard neural networks regarding safety.
• Robustness: The explicit bounds on velocity and convergence rates make the approach suitable for real-time, safety-critical robotic applications where predictability is paramount.
• Efficiency: The use of natural gradient flow and fast inversion algorithms (available for BiLipNets) allows for efficient computation, making it viable for real-time control.

In summary, this paper presents a mathematically rigorous framework that transforms complex motion planning problems into simple linear dynamics via learned diffeomorphisms, achieving a rare combination of learning flexibility, global stability, and formal safety guarantees.