Geometry-Aware Sampling-Based Motion Planning on Riemannian Manifolds

TL;DR

This work addresses motion planning on configuration spaces endowed with a Riemannian metric, where costs are intrinsic to the manifold rather than Euclidean. It introduces a geometry-aware, sampling-based framework that replaces expensive geodesic solves with a cheap midpoint-based distance approximation and a local planner that uses retractions and the Riemannian natural gradient. The key contributions are a cubic-accurate midpoint distance estimator, a geometry-consistent vertex expansion mechanism, and extensive validation on serial manipulators and SE(2) with nonholonomic constraints, demonstrating lower geodesic length and energy than Euclidean baselines and classical solvers. The approach advances scalable, manifold-aware planning with practical impact for high-DoF robots by aligning planning costs with intrinsic geometric structure without requiring explicit geodesic computations.

Abstract

In many robot motion planning problems, task objectives and physical constraints induce non-Euclidean geometry on the configuration space, yet many planners operate using Euclidean distances that ignore this structure. We address the problem of planning collision-free motions that minimize length under configuration-dependent Riemannian metrics, corresponding to geodesics on the configuration manifold. Conventional numerical methods for computing such paths do not scale well to high-dimensional systems, while sampling-based planners trade scalability for geometric fidelity. To bridge this gap, we propose a sampling-based motion planning framework that operates directly on Riemannian manifolds. We introduce a computationally efficient midpoint-based approximation of the Riemannian geodesic distance and prove that it matches the true Riemannian distance with third-order accuracy. Building on this approximation, we design a local planner that traces the manifold using first-order retractions guided by Riemannian natural gradients. Experiments on a two-link planar arm and a 7-DoF Franka manipulator under a kinetic-energy metric, as well as on rigid-body planning in $\mathrm{SE}(2)$ with non-holonomic motion constraints, demonstrate that our approach consistently produces lower-cost trajectories than Euclidean-based planners and classical numerical geodesic-solver baselines.

Figures (4)

• Figure 1: Geodesic motion planning for a 7-DoF Franka manipulator in a cluttered environment. Conventional sampling-based methods compute joint-space shortest paths under the ambient Euclidean metric (red), often ignoring the intrinsic geometry of the configuration manifold (left). Our geodesic formulation instead recovers minimum-energy trajectories under the kinetic-energy Riemannian metric (green), with path thickness indicating relative energy consumption (right).
• Figure 2: Midpoint-based geodesic distance between configurations $q_x$ and $q_y$ on the manifold $\mathcal{M}$. The geodesic midpoint $q_{\textrm{mid}}$ is constructed by interpolating halfway along the tangent vector connecting $q_x$ and $q_y$ in either of their tangent spaces and mapping the result back to the manifold (left). The distance is then computed in the tangent space at $q_{\textrm{mid}}$ as the Riemannian norm of the difference between corresponding tangent vectors (right).
• Figure 3: Geodesics found by various motion planning methods for the $2$-DoF planar manipulator experiment in Section \ref{['sec:exp-manipulators']}. Configuration space paths from start ($\bullet$) and goal ($\blacksquare$) are shown (left), with shaded regions indicating kinetic-energy ellipsoids at different configurations. Corresponding task space motions with end-effector trajectories are shown in yellow (right). The Euclidean approach minimizes joint space shortest distance, yielding straight-line geodesics (). Numerical methods optimize the energy functional, producing lower-cost paths () but often converge to local minima. Our geometry-aware sampling-based approach recovers globally optimal geodesics under the intrinsic kinetic-energy Riemannian metric (), achieving lower geodesic length and energy.
• Figure 4: Comparison of collision-free geodesics produced by the benchmarked methods in the Willow Garage environment for the $\mathrm{SE}(2)$ planning experiment described in Section \ref{['sec:exp-se2-planning']}. The proposed method () is compared against the variational method () and a sampling-based planner using a Euclidean metric (). Arrows along each path represent the $\mathrm{SE}(2)$ pose at discrete intervals; for visual clarity, only the body-frame $x$-axis is shown to illustrate orientation along the trajectory. Start and goal configurations are indicated by green and red dots, respectively, with their full orientation frames.

Theorems & Definitions (12)

• definition thmcounterdefinition: Geodesics Midpoint
• lemma thmcounterlemma
• proof
• definition thmcounterdefinition: Retraction
• theorem thmcountertheorem
• proof
• definition thmcounterdefinition: Riemannian gradient
• lemma thmcounterlemma
• proof
• lemma thmcounterlemma