Generating and Optimizing Topologically Distinct Guesses for Mobile Manipulator Path Planning with Path Constraints

TL;DR

This work addresses nonconvex constrained motion planning for mobile manipulators with end-effector path constraints by introducing a four-step pipeline that first generates homotopically distinct path guesses on a low-dimensional configuration graph using a modified Neighborhood Augmented Graph Search (NAGS), then refines each guess with trajectory optimization to produce multiple locally optimal paths. By augmenting NAGS with path-neighborhood based equivalence, handling tiny obstacles, nonuniform discretization, and robust visiting-order rules, the method reliably identifies multiple distinct local optima and reduces dependence on any single initial guess. Empirical results across two planning problems and randomized tests show faster discovery of diverse, high-quality initial guesses, higher NLP success rates, and lower final costs compared with constrained sampling-based planners, especially in environments with large obstacles. The approach thus provides a practical, topology-aware alternative that complements existing sampling-based methods for high-dimensional constrained planning.

Abstract

Optimal path planning is prone to convergence to local, rather than global, optima. This is often the case for mobile manipulators due to nonconvexities induced by obstacles, robot kinematics and constraints. This paper focuses on planning under end effector path constraints and attempts to circumvent the issue of converging to a local optimum. We propose a pipeline that first discovers multiple homotopically distinct paths, and then optimizes them to obtain multiple distinct local optima. The best out of these distinct local optima is likely to be close to the global optimum. We demonstrate the effectiveness of our pipeline in the optimal path planning of mobile manipulators in the presence of path and obstacle constraints.

Figures (11)

• Figure 1: Mobile manipulator executing three homotopically distinct locally optimal paths given a desired end effector path. Blue shows the desired end effector path, green and red shows the computed elbow and base paths respectively.
• Figure 2: (a) Given the grey obstacle, $p_1$ and $p_2$ belong to the same $\mathcal{H}$-class (homotopically equivalent) while $p_2$, $p_3$, $p_4$ each belong to a different $\mathcal{H}$-class (homotopically distinct). (b) Illustration of a 2D cross section ($y$ axis omitted) of the configuration graph.
• Figure 3: The planning pipeline
• Figure 4: Top row: (a) shows a simple CG with the green start vertex and yellow goal vertex. The edge weight corresponds to the length depicted. Note that this CG only has one homotopically unique path from start to goal. (b)-(f) corresponds to the successive iterations as the NAG grows using $r = 1$. The subscript indicates the PNS of the NAG vertex. Notice in (f) that since NAG vertex $D_C$ and $D_A$ have disjoint PNS, NAGS incorrectly identifies them as homotopically distinct. Bottom row: (g) shows a simple CG with an obstacle in the middle. Notice that there are two homotopically distinct paths from the start to goal. (h)-(l) corresponds successive iterations with $r = 2$. Notice in (l) that the NAG vertices $D_{BA}$ and $D_{CA}$ have overlapping PNS, thus NAGS incorrectly identifies them as homotopically equivalent.
• Figure 5: Successive iterations of the NAG. Notice that obstacles cause the yellow wavefront to split and then merge.

Theorems & Definitions (2)

• Definition 4.1: Equivalence between NAG vertices
• Proposition 1