Probabilistic Homotopy Optimization for Dynamic Motion Planning

TL;DR

This work addresses the difficulty of optimization-based motion planning in dynamic, non-convex landscapes by introducing Probabilistic Homotopy Optimization (PHO), which searches a multidimensional homotopy parameter space to traverse the Solution Manifold of related problems. PHO maintains an Optimization Tree that records solved minima and leverages alternating solve and sample phases to explore multiple local minima, avoiding the limitations of single-branch continuation methods. The approach is shown to handle bifurcation, folding, and disconnected manifolds and is compared against Linear Interpolation HO and RRT HO on Cart-Pole and MIT Humanoid tasks, demonstrating faster attainment of feasible solutions and the ability to discover higher-quality minima with additional time. The results suggest PHO's practical potential for robust, anytime motion planning in complex robots, with future work aimed at richer parameterizations and discrete constraint toggling.

Abstract

We present a homotopic approach to solving challenging, optimization-based motion planning problems. The approach uses Homotopy Optimization, which, unlike standard continuation methods for solving homotopy problems, solves a sequence of constrained optimization problems rather than a sequence of nonlinear systems of equations. The insight behind our proposed algorithm is formulating the discovery of this sequence of optimization problems as a search problem in a multidimensional homotopy parameter space. Our proposed algorithm, the Probabilistic Homotopy Optimization algorithm, switches between solve and sample phases, using solutions to easy problems as initial guesses to more challenging problems. We analyze how our algorithm performs in the presence of common challenges to homotopy methods, such as bifurcation, folding, and disconnectedness of the homotopy solution manifold. Finally, we demonstrate its utility via a case study on two dynamic motion planning problems: the cart-pole and the MIT Humanoid.

Figures (9)

• Figure 1: Framewise animation of the back flip motion planned using Probabilstic Homotopy Optimization.
• Figure 2: Illustration of the algorithm: (a) Solution Manifold ${\mathcal{M}_S}$ (blue) and the associated Basin of Attraction $\mathcal{B}_{\bm{\theta}}\left(\mathbf{x}\right)$ for each point on ${\mathcal{M}_S}$ (yellow). (b) Highlights the range of $\mathcal{X}$ in $\mathcal{B}_{\Gamma\left({\bm{\lambda}}_1\right)}\left(\mathbf{x}^{*}_3\right)$ (purple) and the points on the homotopy curve from which $\mathbf{N}_{3,1}$ is visible (red). (c) A sequence of nodes on ${\mathcal{M}_S}$ and their projections on the Basins of attraction of the next node in the sequence (dashed line). (d) Potential solution path found by satisfying \ref{['eqn:param_seq_requirements']}.
• Figure 3: Visualization of common pitfalls encountered by Homotopy Methods.
• Figure 4: Illustration of how and deal with bifurcation (left) and multiple local minima (right). After the green path has been found, only can also find the red path.
• Figure 5: Result of solving Cart-Pole swing up. For each parameter $\bm{\theta}$, a green dot represents the algorithm solving $\mathcal{P}_{\bm{\theta}}$ successfully within the iteration limit, and a red dot represents an unsuccessful attempt. Parameters are uniformly drawn from $1kg \leq m_{pole} \leq 60kg$, $0.6m \leq l_{pole} \leq 2m$, $x_{max}=1.6m$, $F_{max}=100N$, $m_{cart}=20kg$ (SI units). With an initial guess of zero, none of the $\mathcal{P}_{\bm{\theta}}$ in this range are solvable.

Theorems & Definitions (3)

• Definition 1: Basin of Attraction
• Definition 2: Solution Manifold
• Definition 3: Optimization Tree