[Paper Review] A Homotopy Method for Motion Planning

Shenyu Liu, Mohamed-Ali Belabbas|arXiv (Cornell University)|Jan 29, 2019

Robotic Path Planning Algorithms15 references4 citations

TL;DR

This paper introduces a homotopy-based motion planning method that deforms an initial, unconstrained trajectory (a 'sketch') into an admissible path satisfying holonomic, non-holonomic, and obstacle constraints by encoding constraints into a Riemannian metric. The method uses a geometric heat flow to evolve the path, enabling efficient, scalable solutions for complex robotic motion planning problems including multi-vehicle collision avoidance.

ABSTRACT

We propose a novel method for motion planning and illustrate its implementation on several canonical examples. The core novel idea underlying the method is to define a metric for which a path of minimal length is an admissible path, that is path that respects the various constraints imposed by the environment and the physics of the system on its dynamics. To be more precise, our method takes as input a control system with holonomic and non-holonomic constraints, an initial and final point in configuration space, a description of obstacles to avoid, and an initial trajectory for the system, called a sketch. This initial trajectory does not need to meet the constraints, except for the obstacle avoidance constraints. The constraints are then encoded in an inner product, which is used to deform (via a homotopy) the initial sketch into an admissible trajectory from which controls realizing the transfer can be obtained. We illustrate the method on various examples, including vehicle motion with obstacles and a two-link manipulator problem.

Motivation & Objective

• To develop a novel, computationally efficient motion planning method that integrates holonomic, non-holonomic, and obstacle constraints into a unified framework.
• To address the challenge of generating admissible trajectories for systems with complex dynamics and environmental constraints without requiring the initial path to satisfy kinematic or dynamic constraints.
• To demonstrate the method’s effectiveness on canonical robotic motion planning problems, including vehicle motion and multi-vehicle coordination with collision avoidance.
• To show that the method avoids the curse of dimensionality by ensuring the PDE solution domain remains two-dimensional, enabling scalability to high-dimensional configuration spaces.

Proposed method

• The method encodes all constraints—holonomic, non-holonomic, and obstacle-related—into a Riemannian metric via an inner product, which defines a geometry where minimal-length paths are admissible.
• It begins with an initial sketch (a piecewise differentiable curve from start to end state) that need not satisfy constraints, enabling easy generation, especially in convex configuration spaces.
• A geometric heat flow (GHF) is applied to deform the initial sketch into an admissible path by evolving the curve in the configuration space under the constraint-induced metric.
• The GHF is formulated as a system of parabolic PDEs on a two-dimensional domain (parameterized by time and homotopy parameter), ensuring polynomial scaling with configuration space dimension.
• Obstacle avoidance is implemented by multiplying the base metric with a barrier function that penalizes proximity to forbidden regions, including diagonal subspaces in multi-vehicle scenarios.
• The final admissible path yields a control input via the metric-induced velocity, enabling practical trajectory tracking for the system.

Experimental results

Research questions

• RQ1 Can a homotopy method efficiently generate admissible trajectories for non-holonomic systems with obstacle constraints by deforming an unconstrained initial sketch?
• RQ2 How can holonomic, non-holonomic, and obstacle constraints be unified into a single Riemannian metric to guide path deformation?
• RQ3 Does the proposed method avoid the curse of dimensionality in high-dimensional configuration spaces due to its fixed two-dimensional PDE domain?
• RQ4 Can the method be extended to multi-vehicle motion planning with collision avoidance by embedding constraints in a product metric with diagonal avoidance?
• RQ5 What is the computational efficiency of solving the geometric heat flow for practical robotic motion planning tasks?

Key findings

• The method successfully deforms an initial sketch into an admissible path for a unicycle with a turning radius constraint, producing a smooth, feasible trajectory that respects kinematic constraints.
• In a two-unicycle swap problem, the algorithm deforms an initial circular sketch into two V-shaped paths that allow both vehicles to swap positions without violating orientation or collision constraints.
• For multi-vehicle motion planning, the method achieves collision avoidance by embedding a diagonal obstacle in the product configuration space using a barrier function on the block-diagonal metric.
• The geometric heat flow solver scales efficiently, with computation times ranging from seconds to minutes on a standard laptop using non-optimized MATLAB code, indicating strong practical feasibility.
• The method avoids the curse of dimensionality because the PDE domain is fixed at two dimensions, and the number of interpolation points grows linearly with configuration space dimension, not exponentially.
• The approach enables direct computation of control inputs from the metric-induced velocity of the final path, providing a practical route to trajectory tracking.