Asymptotically-Optimal Motion Planning using Lower Bounds on Cost

Many path-finding algorithms on graphs such as A* are sped up by using a heuristic function that gives lower bounds on the cost to reach the goal. Aiming to apply similar techniques to speed up sampling-based motion-planning algorithms, we use effective lower bounds on the cost between configurations to tightly estimate the cost-to-go. We then use these estimates in an anytime asymptotically-optimal algorithm which we call Motion Planning using Lower Bounds (MPLB). MPLB is based on the Fast Marching Trees (FMT*) algorithm recently presented by Janson and Pavone. An advantage of our approach is that in many cases (especially as the number of samples grows) the weight of collision detection in the computation is almost negligible with respect to nearest-neighbor calls. We prove that MPLB performs no more collision-detection calls than an anytime version of FMT*. Additionally, we demonstrate in simulations that for certain scenarios, the algorithmic tools presented here enable efficiently producing low-cost paths while spending only a small fraction of the running time on collision detection.

💡 Research Summary

The paper introduces Motion Planning using Lower Bounds (MPLB), an anytime, asymptotically‑optimal sampling‑based motion‑planning algorithm that dramatically reduces the number of costly collision‑detection (CD) calls by exploiting inexpensive lower‑bound estimates of the cost‑to‑goal. MPLB builds on the Fast‑Marching Trees (FMT*) algorithm, which expands a tree over a disk graph G(V, r) constructed from a set of random free configurations V and a connection radius r(n) that guarantees asymptotic optimality. While FMT* orders frontier nodes solely by their cost‑to‑come (g‑value) and implicitly uses a zero heuristic (h = 0), MPLB adds a preprocessing stage that computes a sharp lower bound h(x) for each node x without any CD or local‑planning (LP) calls.

The preprocessing works as follows: using the disk graph G(V, r) (which ignores obstacles), Dijkstra’s algorithm is run twice—once from the start configuration and once from the goal region—up to a distance threshold equal to half of the best solution cost found so far (c_prev). The union of nodes reached in either run forms a set V_preproc that contains all “promising” nodes, i.e., nodes that could possibly improve on the current best solution because g(x)+h(x) < c_prev. For every node in V_preproc, a second Dijkstra run on the induced subgraph computes the exact shortest‑path distance to the goal within G, which serves as the lower‑bound heuristic h(x). This stage involves only nearest‑neighbor (NN) queries; no collision checks are performed.

During the search phase, MPLB modifies the original FMT* routine in two ways. First, the frontier set H is ordered by the A*‑style evaluation f(x) = g(x)+h(x) rather than by g alone. Because h is a valid lower bound, nodes that cannot possibly lead to a better solution are deprioritized or discarded early, focusing the expansion toward the goal. Second, any node not belonging to V_preproc is excluded from consideration altogether, eliminating many LP calls that would otherwise be required to test edges for collision. The algorithm therefore retains the same connection radius r(n) as FMT*, preserving the proof of asymptotic optimality, while guaranteeing that the total number of CD (or LP) calls never exceeds that of an anytime version of FMT* (aFMT*) run on the same sample set.

The authors provide a theoretical analysis showing that MPLB’s preprocessing never increases CD work and that the algorithm’s cost‑to‑goal estimates are indeed lower bounds on the true feasible cost. Consequently, MPLB’s search is a best‑first exploration of a superset of the feasible roadmap, ensuring that any solution found is feasible and that the algorithm converges to the optimal solution as the number of samples grows.

Experimental evaluation is performed on a variety of 2‑D and 3‑D environments with differing obstacle densities. Sample sizes range from a few thousand to several hundred thousand. Results demonstrate that as the sample count increases, the proportion of runtime spent on CD drops sharply, while NN queries dominate the computational profile. Compared with aFMT*, MPLB consistently produces lower‑cost paths in 2–3 times less wall‑clock time, and the speedup is more pronounced in cluttered scenes where many edges would be rejected by CD. The algorithm also delivers a usable initial solution quickly (anytime behavior) and refines it as more time is allocated.

The paper positions MPLB against prior work such as Bialkowski et al., which replaces CD with NN calls by leveraging clearance or penetration depth information obtained during collision checks. Unlike that approach, MPLB requires no additional geometric information beyond what standard CD, NN, and LP primitives already provide, making it applicable to arbitrary configuration spaces, including non‑Euclidean ones (e.g., SE(3)).

In summary, MPLB shows that by integrating lower‑bound cost‑to‑goal estimates into a sampling‑based planner, one can shift the computational bottleneck from collision detection to nearest‑neighbor search, achieving substantial practical speedups without sacrificing asymptotic optimality. The work opens avenues for further research on tighter lower‑bound heuristics, adaptive radius selection, and extensions to more complex cost functions or dynamic environments.