Generalized Lazy Search for Robot Motion Planning: Interleaving Search and Edge Evaluation via Event-based Toggles

TL;DR

This paper introduces Generalized Lazy Search (GLS), a framework that dynamically balances search and edge evaluation to improve robotic motion planning efficiency, outperforming existing methods like LazySP especially in complex environments.

Contribution

The paper presents GLS, a novel framework that interleaves search and evaluation using event-based toggles, and incorporates prior edge probability knowledge to optimize planning time.

Findings

GLS outperforms LazySP in simulated environments.

Incorporating edge priors reduces planning time.

GLS is provably more efficient with certain toggle strategies.

Abstract

Lazy search algorithms can efficiently solve problems where edge evaluation is the bottleneck in computation, as is the case for robotic motion planning. The optimal algorithm in this class, LazySP, lazily restricts edge evaluation to only the shortest path. Doing so comes at the expense of search effort, i.e., LazySP must recompute the search tree every time an edge is found to be invalid. This becomes prohibitively expensive when dealing with large graphs or highly cluttered environments. Our key insight is the need to balance both edge evaluation and search effort to minimize the total planning time. Our contribution is two-fold. First, we propose a framework, Generalized Lazy Search (GLS), that seamlessly toggles between search and evaluation to prevent wasted efforts. We show that for a choice of toggle, GLS is provably more efficient than LazySP. Second, we leverage prior experience of edge probabilities to derive GLS policies that minimize expected planning time. We show that GLS equipped with such priors significantly outperforms competitive baselines for many simulated environments in R2, SE(2) and 7-DoF manipulation.

Tables

Table 1. Table 1: Equivalence of GLS to existing lazy algorithms

Algorithm	Event	Selector
LazyPRM (?)	ShortestPath	Any
LazySP (?)	ShortestPath	Any
LWA* (?)	ConstantDepth (1)	Forward
LRA* (?)	ConstantDepth ($\alpha$)	Forward

Table 2. Table 2: Planning Time(millisec.) and number of operations by algorithms under GLS . (Median reported on 100 tests.)

	SP + F	SP + A	SP + FF	CD + F	CD + A	CD + FF	SE + F	SE + A	SE + FF
Square
Total Planning Time	0.331	0.454	0.372	0.221	0.259	0.222	0.171	0.161	0.116
# Edge Evaluations	190	308	137	273	344.5	315.5	200.5	206	153
# Vertex Rewires	7058.5	8502.5	9859	1076.5	641.5	69.5	1104.5	603.5	318.5
Two Wall
Total Planning Time	0.419	0.394	0.377	0.262	0.301	0.290	0.220	0.169	0.161
# Edge Evaluations	224	242.5	144	310	393	407	287	224.5	202.5
# Vertex Rewires	9360	7997	9870	1594.6	914.5	165.5	697	435	711.5
Mazes
Total Planning Time	1.334	1.292	1.272	0.574	0.776	0.560	0.615	0.578	0.337
# Edge Evaluations	531	471	307.5	630	895	785.5	588	544.5	352.5
# Vertex Rewires	34359	34379.5	37750	4769	5357.5	326	7266.5	7039.5	3213
Forest
Total Planning Time	0.277	0.267	0.269	0.574	0.776	0.559	0.219	0.234	0.229
# Edge Evaluations	174.5	184.5	165.5	306.5	342.5	450.5	190	220	174.5
# Vertex Rewires	5524.5	4936	5467.5	579	346	95.5	3075	2855	3827.5

Table 3. Table 3: Mean Planning Times (in seconds) of GLS , LazySP and LRA* on S ​ E ​ ( 2 ) , ℝ 7 𝑆 𝐸 2 superscript ℝ 7 SE(2),~{}\mathbb{R}^{7} problems.

	GLS	LRA*	LazySP
Piano Movers’
Total Planning Time	1.00	1.13	1.25
Edge Evaluation Time	0.17	0.49	0.10
Vertex Rewire Time	0.83	0.64	1.15
HERB Task 1
Total Planning Time	1.17	1.81	1.53
Edge Evaluation Time	0.38	1.19	0.29
Vertex Rewire Time	0.79	0.62	1.24
HERB Task 2
Total Planning Time	1.64	2.77	2.02
Edge Evaluation Time	0.42	1.83	0.23
Vertex Rewire Time	1.22	0.94	1.79

Equations

$$\underset{\sigma }{\min }w(\sigma )\text{mbox}s.t.\forall e\in \sigma ,\varphi (e)=1$$

$$C({\mathcal{E}}_{\mathrm{eval}},{\mathcal{V}}_{\mathrm{rwr}})=c_e\left|{\mathcal{E}}_{\mathrm{eval}}\right|+c_r\left|{\mathcal{V}}_{\mathrm{rwr}}\right|$$

$${\sigma }^{i+1}\leftarrow \underset{\sigma }{\mathrm{argmin}}$$

$${\varphi }_{\mathrm{lazy}}(\sigma ,{\mathcal{E}}_{\mathrm{eval},\mathrm{LSP}}^{i+1})=1$$

$$v^{i+1}\leftarrow \underset{v}{\mathrm{argmin}}$$

$$h_{\mathcal{G}}(v,v_{\mathrm{t}})<h_{\text{min}}$$

$$v^{i+1}\leftarrow \underset{v}{\mathrm{argmin}}$$

$${\varphi }_{\mathrm{lazy}}({\sigma }_{v_{\mathrm{s}},v},{\mathcal{E}}_{\mathrm{eval},\mathrm{GLS}}^{i+1})=1$$

$${\varphi }_{\mathrm{lazy}}({\sigma }_{v,v_{\mathrm{t}}},{\mathcal{E}}_{\mathrm{eval},\mathrm{GLS}}^{i+1})=1$$

$$O(\left|\mathcal{V}\right|\log \left|\mathcal{V}\right|+\left|\mathcal{E}\right|)$$

$$O((N+l)\log (N+l)+3N+2l-1)$$

$$O((N+l)\log (N+l))$$

$$\min$$

$$\text{mbox}s.t.$$

$$K\left(c_e\frac{1}{(1-\delta )}+c_r\frac{b\log (\delta )}{\log (p_{\text{max}})}\right)$$

$${\mathbb{E}}_{\mathbf{p}}\left[{\mathcal{E}}_{\mathrm{eval}}\right]$$

$$\le (1-\delta )\left(1+2\delta +3{\delta }^2+\dots \right)$$

$$\le (1-\delta )\frac{1}{(1-\delta {)}^2}$$

$$\le \frac{1}{(1-\delta )}$$

$$p_{\text{max}}^{L(\delta )}\ge \delta$$

$$L(\delta )$$

$${\mathbb{E}}_{\mathbf{p}}\left[{\mathcal{V}}_{\mathrm{rwr}}\right]$$

$$\le (1-\delta )bL(\delta )\left(1+\delta +{\delta }^2+\dots \right)$$

$$\le (1-\delta )bL(\delta )\frac{1}{(1-\delta )}$$

$$\le bL(\delta )$$

$$\le \frac{b\log (\delta )}{\log (p_{\text{max}})}$$

$${\mathbb{E}}_{\mathbf{p}}\left[C({\mathcal{E}}_{\mathrm{eval}},{\mathcal{V}}_{\mathrm{rwr}})\right]$$

$$=c_{e}K\frac{1}{(1-\delta )}+c_{r}K\frac{b\log (\delta )}{\log (p_{\text{max}})}$$

$$=K\left(c_e\frac{1}{(1-\delta )}+c_r\frac{b\log (\delta )}{\log (p_{\text{max}})}\right)$$

$$\frac{\partial }{\partial \delta }\left(K\left(c_e\frac{1}{(1-\delta )}+c_r\frac{b\log (\delta )}{\log (p_{\text{max}})}\right)\right)$$

$$\frac{c_e}{(1-\delta {)}^2}+\frac{c_r}{\log (p_{\text{max}})}\frac{b}{\delta }$$

$$(1-\delta {)}^2-\frac{c_e}{b{c}_r}\log \left(\frac{1}{p_{\text{max}}}\right)\delta$$

$${\delta }^2-\left(\frac{c_e}{b{c}_r}\log (\frac{1}{p_{\text{max}}})+2\right)\delta +1$$

$$\delta =\frac{\eta -\sqrt{{\eta }^2-4}}{2}$$

$$\delta$$

$$=\frac{\eta \left(1-\sqrt{1-\frac{4}{{\eta }^2}}\right)}{2}$$

$$=\frac{\eta \left(1-\left(1-\frac{4}{2{\eta }^2}\right)\right)}{2}$$

$$=\frac{1}{\eta }$$

$$\displaystyle=\frac{1}{\left(\frac{c_{e}}{bc_{r}}\log(\frac{1}{p_{\text{max}}})+2\right)}\$$

$${\mathbb{E}}_{\mathbf{p}}\left[{\mathcal{E}}_{\mathrm{eval}}(S)\right]$$

$$=\sum _{l=1}^n\left(\prod _{m=1}^{l-1}{p}_m\right)\left(1-p_l\right)l$$

$${\mathbb{E}}_{\mathbf{p}}\left[{\mathcal{E}}_{\mathrm{eval}}(S)\right]-{\mathbb{E}}_{\mathbf{p}}\left[{\mathcal{E}}_{\mathrm{eval}}(S^{\prime })\right]$$

$$=\dots +\prod _{m=1}^{i-1}{p}_m\left[(1-p_i)i+p_i(1-p_{i+1})(i+1)\right]+\dots$$

Full text

1 Introduction

We focus on the problem of finding the shortest path on a graph while minimizing total planning time. This is critical in applications such as robotic motion planning (?), where collision-free paths must be computed in real time. A typical search algorithm expands a wavefront from the start, evaluating edges discovered until it finds the shortest feasible path to the goal. The planning time then becomes the sum of the time spent in two phases – search effort and edge evaluation. While edge evaluation is generally more expensive in motion planning (?), the actual ratio of these times varies with problem instances and graph sizes. Our goal is to design a framework of algorithms that let us balance this trade-off.

Unfortunately, current shortest path algorithms do not provide a framework flexible enough to traverse the pareto curve between search effort and edge evaluation. On one end of the spectrum, A* and its variants (?; ?; ?) evaluate edges as soon as they are discovered. Hence although A* is optimal in terms of search effort, it is at the cost of excessive edge evaluations. On the other hand, LazySP (?) amongst other lazy search techniques (?; ?; ?), expands the search wavefront all the way to the goal before evaluating edges. Hence LazySP is optimal in terms of edge evaluation but has to replan everytime an edge is invalidated.

In this work, we propose a framework for algorithmically toggling between search effort and edge evaluation. We are guaranteed to find the shortest path as long as the following holds true; the search tree must always be repaired to be consistent, and edge evaluation must be restricted to the shortest subpath in the tree. Our framework, Generalized Lazy Search (GLS), has two modules - Event and Selector (Fig. 1). The algorithm expands a lazy search tree without evaluating any edges till the Event is triggered. A Selector is then invoked to evaluate an edge on the shortest subpath in the lazily expanded search tree. We show that by choosing different Event and Selector pairs, we can recover several existing lazy search algorithms such as LazySP (?), LWA* (?) and LRA* (?).

What constitutes an optimal trade-off and can this be captured by GLS? Consider the ideal scenario, one with an omniscient oracle (?) that knows ahead of time which edges are valid or invalid. In fact, the oracle can compute the minimal set of invalid edges $\mathcal{I}$ that must be invalidated to arrive at the shortest feasible path. How can we utilize such an oracle in GLS? A simple strategy is as follows; as the search wavefront expands from start to goal, the oracle monitors the new edges that are discovered and triggers an Event if it belongs to $\mathcal{I}$. A Selector then evaluates that edge. This minimizes edge evaluation and curtails wasted search effort.

This insight extends to the more practical setting where we have priors on edge validity that are learned from experience. We derive Event and Selector that minimize the expected planning time. This produces behaviors similar to the omniscient oracle (Fig. 2); the search proceeds until the Event is triggered due to the appearance of low probability edges on the current subpath; the Selector then selects these edges to invalidate the subpath; and the process continues until the shortest feasible path is found.

We make the following contributions:

We propose a class of algorithms, GLS (Section 4), that minimize computational effort, defined as a function of both edge evaluation and vertex rewiring (Section 3). 2. 2.

We recover different lazy search algorithms as instantiations of GLS. We further prove that one such instantiation is edge optimal and causes fewer rewires than LazySP (Section 4, Theorem 4.3). 3. 3.

We derive instantiations of GLS that exploit the availability of edge priors to minimize expected computational effort (Section 5, Theorem 5.2). 4. 4.

We show that GLS informed with edge priors can outperform competitive baselines on a spectrum of planning domains (Section 6).

2 Related Work

Graphs lend powerful tractability to robotic motion planning (?). They can be explicit, i.e., constructed as part of a pre-processing stage (?; ?; ?), or implicit, i.e., discovered incrementally during search (?; ?; ?).

A* (?) and its variants have enjoyed widespread success in finding the shortest path with an optimal number of vertex expansions. However, in domains where edge evaluations are expensive and dominate the planning time, a lazy approach is often employed (?; ?; ?). In this approach, the graph is constructed without testing if edges are collision-free. Only a subset of edges are evaluated to save computation time. LazySP (?) extends the graph up to the goal before checking edges. LWA* (?) extends the graph a single step before evaluation. LRA* (?) trades off these approaches, allowing the search to proceed to an arbitrary lookahead. We generalize this further by introducing an event-based toggle.

Several works have explored the use of priors in search. FuzzyPRM (?) evaluates paths that minimize the probability of collision. The Anytime Edge Evaluation (AEE*) framework (?) uses an anytime strategy for edge evaluation informed by priors. POMP (?) defines surrogate objectives using priors to improve anytime planning. BISECT (?) and DIRECT (?) cast search as Bayesian active learning to derive edge evaluation. E-graphs (?) uses priors in heuristics. We focus on using priors to find the shortest path while minimizing expected planning time.

Several alternate approaches speed up planning by creating efficient data structures (?), modeling belief over the configuration space (?), sampling vertices in promising regions (?; ?) or using specialized hardware (?). Other approaches forego optimality and computing near-optimal paths (?; ?). Our work also draws inspiration from approaches that interleave planning and execution, such as LRTA* (?) and LSS-LRTA* (?).

3 Problem Formulation

Our goal is to design an algorithm that can solve the Single Source Shortest Path (SSSP) problem while minimizing computational effort. We begin with the SSSP problem. Let $\mathcal{G}=\left(\mathcal{V},\mathcal{E}\right)$ be a graph, where $\mathcal{V}$ denotes the set of vertices and $\mathcal{E}$ the set of edges. Given a pair of source and target vertices $\left(v_{\rm s},v_{\rm t}\right)\in\mathcal{V}$, a path $\sigma$ is represented as a sequence of vertices $\left(v_{1},v_{2},\ldots,v_{l}\right)$ such that $v_{1}=v_{\rm s},v_{l}=v_{\rm t},\forall i,~{}\left(v_{i},v_{i+1}\right)\in\mathcal{E}$. We define a world $\phi:\mathcal{E}\rightarrow\{0,1\}$ as a mapping from edges to valid ($1$) or invalid ([math]). A path is said to be feasible if all edges are valid, i.e., $\forall e\in\sigma,\phi(e)=1$. Let $w:\mathcal{E}\rightarrow\mathbb{R}^{+}$ be the length of an edge. The length of a path is the sum of edge costs, i.e., $w(\sigma)=\sum_{e\in\sigma}w(e)$. The objective of the SSSP problem is to find the shortest feasible path:

[TABLE]

Given an SSSP, we define a shortest path algorithm $\textsc{Alg}(\mathcal{G},v_{\rm s},v_{\rm t},\phi)$ that takes as input the graph $\mathcal{G}$, the source-target pair $\left(v_{\rm s},v_{\rm t}\right)$, and the underlying world $\phi$. The algorithm typically solves the problem by building, verifying and rewiring a shortest path tree from source to target.

Maintaining the search tree and verifying the shortest feasible path are primarily characterized by two atomic operations: edge evaluation and vertex rewiring.

Definition 3.1** (Edge Evaluation).**

The operation of querying the world $\phi(e)$ to check if an edge $e$ is valid.

Definition 3.2** (Vertex Rewiring).**

The operation of finding and assigning a new parent for a vertex $u$ when an invalid edge is discovered.

The algorithm returns three terms, i.e, $\sigma^{*},~{}\mathcal{E}_{\rm eval},~{}\mathcal{V}_{\rm rwr}=\textsc{Alg}(\mathcal{G},v_{\rm s},v_{\rm t},\phi)$. Here, $\sigma^{*}$ is the shortest feasible path, $\mathcal{E}_{\rm eval}$ is the set of edges evaluated during the search, and $\mathcal{V}_{\rm rwr}$ is the multiset111$\mathcal{V}_{\rm rwr}$ is a multiset since a vertex can potentially be rewired multiple times during the planning cycle. of vertices rewired. Alg ensures the following certificate:

Returned path $\sigma^{*}$ is verified to be feasible, i.e., $\forall e\in\sigma^{*},~{}e\in\mathcal{E}_{\rm eval},~{}\phi(e)=1$ 2. 2.

All paths shorter than $\sigma^{*}$ are verified to be infeasible, i.e., $\forall\sigma_{i},~{}w(\sigma_{i})\leq w(\sigma^{*}),~{}\exists e\in\sigma_{i},~{}e\in\mathcal{E}_{\rm eval},~{}\phi(e)=0$

We now define the computational cost (planning time), of solving the SSSP problem as a function of $\mathcal{V}_{\rm rwr}$ and $\mathcal{E}_{\rm eval}$. Let $c_{e}$ be the average cost of evaluating an edge, and $c_{r}$ be the average cost of rewiring a vertex. We approximate the total planning time as a linear combination:

[TABLE]

Our motivation for defining the cost will become clearer in the following section, where we propose a general framework for Alg. This framework lets us explicitly reason about the terms $\mathcal{E}_{\rm eval}$ and $\mathcal{V}_{\rm rwr}$ in order to balance them.

4 Generalized Lazy Search

We propose a framework, Generalized Lazy Search (GLS), to solve the problem defined in Section 3. The general concept idea is to toggle between lazily searching to a horizon and evaluating edges along the current estimated shortest path. This toggle must be chosen appropriately to balance the competing computational costs of edge evaluation and vertex rewiring.

4.1 The Algorithm

Algorithm 1 describes the GLS framework for the shortest path algorithm $\textsc{Alg}(\mathcal{G},v_{\rm s},v_{\rm t},\phi)$ referred to in Section 3. This framework requires two functions: Event and Selector.

To solve the SSSP problem, we maintain a shortest path search tree over $\mathcal{G}$. We assume that every call to $\phi$, which populates $\mathcal{E}_{\rm eval}$, is expensive. Therefore, we initially assume that all edges in $\mathcal{G}$ are valid and maintain this search tree lazily. Our algorithm initializes the search tree $\mathcal{T}_{\rm lazy}$ rooted at $v_{\rm s}$ (Line 1). It begins by iteratively extending $\mathcal{T}_{\rm lazy}$ into $\mathcal{G}$ (Line 4). The search is guided with an admissible heuristic $h(v,v_{\rm t})$.

The procedure ExtendTree additionally takes as input a function Event. Extending $\mathcal{T}_{\rm lazy}$ triggers the Event by definition. The algorithm, at this point, discontinues the extension of $\mathcal{T}_{\rm lazy}$ and switches to validate the already constructed search tree. Therefore, the Event acts as a toggle between lazy seach and edge evaluation.

Definition 4.1** (Event).**

A function that defines the toggle between extending the lazy search tree and validating it.

To solve the SSSP problem and validate $\mathcal{T}_{\rm lazy}$, the algorithm picks the path, $\sigma_{\rm sub}$, to a leaf vertex with the lowest estimated total cost to reach the goal (Line 5). It then evaluates an edge along $\sigma_{\rm sub}$ to validate the search tree (Line 6). In addition to $\sigma_{\rm sub}$, the procedure EvaluateEdge also takes as input a function Selector. The Selector acts on $\sigma_{\rm sub}$ and returns an edge belonging to it that the algorithm evaluates.

Definition 4.2** (Selector).**

A function that defines the strategy to select an edge along a subpath to evaluate.

Edge evaluation is followed by the extension of $\mathcal{T}_{\rm lazy}$ until the Event is triggered again. If the edge were invalid, the subtree emanating from the edge has to be rewired. We can do this efficiently using the mechanics of LPA* (?).

This process of interleaving search with edge evaluation continues until the algorithm terminates with the shortest feasible path from source to goal, if one exists. While the algorithm is guaranteed to return the shortest path, the framework permits the design of Event and Selector to reduce the total computation cost of solving the SSSP problem.

4.2 Role of Event and Selector

Since the lazy search paradigm operates based on the concept of optimism under uncertainty, the search tree is extended assuming edges are collision free. However, extending the search tree beyond edges that are in collision can waste computational effort. The Event acts as a toggle to halt a search deemed wasteful. The Selector aims to quickly invalidate the path. Fig. 2 illustrates the ideal behavior of such an algorithm. Interestingly, the framework can also capture existing lazy search algorithms as different combinations of event and selectors, as shown in Table. 1.

Event.

When triggered, events must ensure that the shortest subpath $\sigma_{\rm sub}$ in $\mathcal{T}_{\rm lazy}$ has at least one unevaluated edge (Theorem 4.1). Algorithm 2 defines some candidate events.

ShortestPath (SP) is triggered when a shortest path to $v_{\rm t}$ has been determined during the lazy extension of $\mathcal{T}_{\rm lazy}$. Therefore, in every iteration, this Event presents the Selector with the candidate shortest path from $v_{\rm s}$ to $v_{\rm t}$ on $\mathcal{G}$. Note that ShortestPath exhibits algorithmic behavior similar to LazySP and LazyPRM.

ConstantDepth (CD) is triggered when the procedure ExtendTree chooses to extend a leaf vertex $v\in\mathcal{T}_{\rm lazy}{}$ such that the subpath from $v_{\rm s}$to $v$ has exactly $\alpha$ number of unevaluated edges. Therefore, in every iteration, this Event presents the Selector with $\sigma_{\rm sub}$ that is characterized by a constant number of unevaluated edges.

HeuristicProgress (HP) is triggered whenever the search expands a vertex whose heuristic value is lower than any vertex whose incident edge has been evaluated. It does so by recording the minimum heurisitic value of a vertex with a parent that has been evaluated, i.e., $h_{\text{min}}\leftarrow\min_{(u^{\prime},v^{\prime})\in\mathcal{E}_{\rm eval}}h(v^{\prime},v_{\rm t})$. The event is triggered whenever ExtendTree chooses to extend a leaf vertex $v\in\mathcal{T}_{\rm lazy}{}$ with a heuristic value smaller than $h_{\text{min}}$.

Selector.

Selectors must ensure that they select at least one unevaluated edge (Theorem 4.1). Algorithm 3 defines some candidate selectors.

Given $\sigma_{\rm sub}$, Forward (F) evaluates the first unevaluated edge on $\sigma_{\rm sub}$ that is closest to $v_{\rm s}$. Given a forward search, this constitutes one of the most natural Selectors available. Alternate (A) toggles between evaluating the first unevaluated edge closest to $v_{\rm s}$ and $v_{\rm t}$ in every iteration. This approach is motivated by bi-directional search algorithms. Both Selectors were first used in (?).

4.3 Analysis

For any choice of Event and Selector, GLS is complete and correct.

Theorem 4.1** (Completeness).**

Let Event be a function that on halting ensures there is at least one unevaluated edge on the current shortest path or that the goal is reached. Let Selector be a function that evaluates at least one unevaluated edge (if it exists). GLS implemented using ExtendTree(Event) and EvaluateEdges(Selector) on a finite graph $\mathcal{G}$ is complete.

**Proof ** In each iteration, ExtendTree(Event) ensures there is atleast one unevaluated edge on the shortest path (unless the goal has been reached). The EvaluateEdges(Selector) evaluates atleast one edge. Since there are a finite number of edges, the algorithm will eventually terminate. ∎

Theorem 4.2** (Correctness).**

If the heuristic $h(v,v_{\rm t})$ is admissible, then GLS terminates with the shortest feasible path.

**Proof ** Let $\sigma^{*}$ be the shortest feasible path with respect to $w(\cdot)$ and world $\phi$. For any vertex $v^{*}\in\sigma^{*}$, we denote its f-value to be $f(v^{*})=w(\sigma_{v_{\rm s},v^{*}})+h(v^{*},v_{\rm t})$, where $\sigma_{v_{\rm s},v^{*}}$ is the subpath from the start to vertex $v^{*}$. As our heuristic function is admissible, we have that $f(v^{*})\leq w(\sigma^{*})$. Recall that in each iteration, the inner GetShortestPathToLeaf () returns a vertex $v_{\rm ret}$ with the smallest f-value among all the leaves of the tree $\tau_{\rm lazy}$. Let $v^{*}_{\rm leaf}$ be the leaf vertex on $\tau_{\rm lazy}$ that lies on the shortest feasible path $\sigma^{*}$. Hence, $f(v_{\rm ret})\leq f(v^{*}_{\rm leaf})\leq w(\sigma^{*})$. If GLS terminates with $v_{\rm ret}$, this implies $\sigma_{v_{\rm s},v_{\rm ret}}$ is verified to be feasible and $v_{\rm ret}=v_{\rm t}$. Let the verified path that is returned be $\sigma_{\rm ret}$ such that $\sigma_{\rm ret}=\sigma_{v_{\rm s},v_{\rm t}}$. In that case $w(\sigma_{\rm ret})=f(v_{\rm t})\leq w(\sigma^{*})$. ∎

LazySP with the Forward selector was proved to be edge optimal222See (?) for the formal computational model in the class of all shortest path algorithms that use a Forward selector (?). We now show that GLS lets us derive another algorithm that is also edge-optimal but reduces number of vertex rewires.

Theorem 4.3** (Edge Optimality).**

GLS evaluates the same number of edges $\mathcal{E}_{\rm eval}$ as LazySP, i.e., is edge optimal, while having a smaller number of vertex rewires $\mathcal{V}_{\rm rwr}$ under the following setting:

Heuristic: Distance on the unevaluated graph $h_{\mathcal{G}}(v,v_{\rm t})$ 2. 2.

Event*: HeuristicProgress* 3. 3.

Selector*: Forward*

**Proof ** We are going to prove this via induction over iterations of LazySP and GLS. In each iteration cycles through algorithm by invoking EvaluateEdges (Selector), ExtendTree (Event) and SelectShortestSubpath ().

At iteration $i$, let $\mathcal{E}_{\rm eval,LSP}^{i}$ and $\mathcal{V}_{\rm rwr,LSP}^{i}$ be the edges evaluated and vertex rewired respectively by LazySP. Let $\sigma^{i}$ be the candidate shortest path.

Let $\mathcal{E}_{\rm eval,GLS}^{i}$ and $\mathcal{V}_{\rm rwr,GLS}^{i}$ be the edges evaluated and vertices rewired, respectively, by GLS at iteration $i$. Let $h_{\mathcal{G}}(v,v_{\rm t})$ be the heuristic used by the search which corresponds to the distance on the graph $\mathcal{G}$. Let $v^{i}$ be the leaf vertex corresponding to the current shortest subpath from the start $\sigma_{v_{\rm s},v^{i}}$. This implies $v^{i}$ corresponds to the vertex with the smallest f-value $w(\sigma_{v_{\rm s},v^{i}})+h_{\mathcal{G}}(v^{i},v_{\rm t})$.

We also introduce the lazy edge status function $\phi_{\rm lazy}(\sigma,\mathcal{E}_{\rm eval})$ which determines if a path $\sigma$ is valid depending on edges evaluated thus far in $\mathcal{E}_{\rm eval}$.

Following are the conditions for the induction:

1. A

Both algorithms have the same set of evaluated edges $\mathcal{E}_{\rm eval,GLS}^{i}=\mathcal{E}_{\rm eval,LSP}^{i}$. 2. B

Both algorithms share the same subpath $\sigma_{v_{\rm s},v}^{i}\subseteq\sigma^{i}$.

For $i=1$, $\mathcal{E}_{\rm eval,GLS}^{1}=\mathcal{E}_{\rm eval,LSP}^{1}$ because no edges have been evaluated. Hence (A) is true. Since $h_{\mathcal{G}}(v,v_{\rm t})$ is the distance on the unevaluated graph, the leaf vertex $v^{i}$ considered by GLS lies on $\sigma^{1}$, i.e. $\sigma_{v_{\rm s},v}^{1}\subseteq\sigma^{1}$. Hence (B) is true.

We will show these conditions hold for $i+1$.

Since both LazySP and GLS use Forward, share the same subpath (A) and have the same evaluation status (B) - they both evaluate the same edge $e$. Both algorithms increase their evaluated set $\mathcal{E}_{\rm eval,LSP}^{i+1}=\mathcal{E}_{\rm eval,GLS}^{i+1}\leftarrow\mathcal{E}_{\rm eval,GLS}^{i}\cup e$. Hence (A) holds.

If $e$ is valid, neither algorithms rewire vertices. However, if an edge is in collision, LazySP rewires at least the remainder of the path $\sigma^{i+1}$. GLS does not have to rewire the remainder of the subpath $\sigma_{v^{i},v_{\rm t}}$ as it was never expanded during the search. Hence GLS can only result in smaller rewires, i.e. $|\mathcal{V}_{\rm rwr,GLS}^{i+1}|\leq|\mathcal{V}_{\rm rwr,LSP}^{i+1}|-|\sigma_{v^{i},v_{\rm t}}|$.

We will now show that $\sigma_{v_{\rm s},v^{i+1}}\subseteq\sigma^{i+1}$.

LazySP finds the next candidate shortest path $\sigma^{i+1}$ by solving the following search problem

[TABLE]

GLS invokes the ExtendTree (Event) which proceeds till HeuristicProgress toggles off the search. The search stops at vertex $v^{i+1}$ which satisfies the following:

[TABLE]

Note that $\phi_{\rm lazy}(\sigma_{v_{\rm s},v},\mathcal{E}_{\rm eval,GLS}^{i+1})=1$, i.e. the subpath from the start to any vertex is valid according to the lazy estimate.

By definition, the heuristic $h_{\mathcal{G}}(v,v_{\rm t})=w(\sigma_{v,v_{\rm t}})$ is the weight of the shortest path on the unevaluated graph $\sigma_{v,v_{\rm t}}$. The heuristic progress threshold $h_{\text{min}}$ is by definition the minimum heuristic value of the child vertex of any evaluated edge, i.e. $h_{\text{min}}=\min_{(u^{\prime},v^{\prime})\in\mathcal{E}_{\rm eval,GLS}^{i+1}}h_{\mathcal{G}}(v^{\prime},v_{\rm t})$. Since $h_{\mathcal{G}}(v,v_{\rm t})$ is consistent, $h_{\mathcal{G}}(v,v_{\rm t})<h_{\text{min}}<\min_{(u^{\prime},v^{\prime})\in\mathcal{E}_{\rm eval,GLS}^{i+1}}h_{\mathcal{G}}(v^{\prime},v_{\rm t})$ implies that none of the edges $(u^{\prime},v^{\prime})\in\sigma_{v,v_{\rm t}}$ belong to $\mathcal{E}_{\rm eval,GLS}^{i+1}$ have been evaluated. This means that the subpath to goal is valid according to the lazy estimate, i.e. $\phi_{\rm lazy}(\sigma_{v,v_{\rm t}},\mathcal{E}_{\rm eval,GLS}^{i+1})=1$.

Hence (4) can be re-written as

[TABLE]

Since $\mathcal{E}_{\rm eval,GLS}^{i+1}=\mathcal{E}_{\rm eval,LSP}^{i+1}$, (3) and (5) are the same optimization. Hence $\sigma_{v_{\rm s},v^{i+1}}\subseteq\sigma^{i+1}$ and (B) holds. As a result, the induction holds.

This process continues till both algorithms discover the shortest feasible path $\sigma^{*}$ at the end of iteration $N$. Both evaluate the same number of edges $\mathcal{E}_{\rm eval,LSP}^{N+1}=\mathcal{E}_{\rm eval,GLS}^{N+1}$. But GLS saves on more vertices being rewired than LazySP, i.e. $|\mathcal{V}_{\rm rwr,GLS}^{N}|\leq|\mathcal{V}_{\rm rwr,LSP}^{N}|-\sum_{i=1}^{N}|\sigma_{v^{i},v_{\rm t}}|$. ∎

Corollary 4.1**.**

There is a graph $\mathcal{G}$ for which the number of vertex rewires $\mathcal{V}_{\rm rwr}$ for LazySP over GLS is linear over logarithmic.

**Proof **

We are going to construct a counter example that shows a particularly bad case of vertex rewiring undertaken by LazySP.

Scenario.

Consider the graph in Fig. 3. It has a set of $l$ vertices connected to the start. The upper half of the $l$ vertices are connected to vertex $A$. The lower half is connected to $B$. Each of $A$ and $B$ is connected to a chain of $N$ vertices going to the goal. $\left|\mathcal{V}\right|=N+l$, $\left|\mathcal{E}\right|=3N+2l-1$.

The graph is such that one of $l$ edges connected to the start is valid, the rest is invalid. The remaining edges are all valid.

The weights of the graph are such that the shortest path alternates between the top and bottom halves of the graph. Assume that all $l-1$ shortest paths are invalid and the last one is valid. Finally assume that $A$ and $B$ alternate being the optimal parent to the $N$ vertices.

LazySP computation

.

For LazySP, the only computation is vertex rewiring. The graph is such that successive shortest paths alternate between the upper and lower halves. The shortest paths in the upper half pass through A and lower half pass through B. Hence every edge that is invalidated, causes all $N$ vertices to rewire to either vertex $A$ or $B$. This is the optimistic thrashing scenario explained in LazyPRM*. Since $l$ edges have to be invalidated, the number of rewires is $O(Nl)$

GLS computation

There are two computation steps to account for - heuristic computation and vertex rewiring.

The heuristic computation is a Djikstra operation.

[TABLE]

Since the search never proceeds beyond the first set of edges, the amount of vertex rewiring is [math].

Hence the complexity of LazySP is $O(Nl)$ while GLS is $O((N+l)\log(N+l))$. The ratio is linear over logarithmic growth.

∎

5 Leveraging Edge Priors in GLS

The GLS framework is powerful because one can optimize Event and Selector to minimize computational costs while still retaining guarantees. Here, we show its expressive power in a scenario where we have additional side information, such as priors on the validity of edges. Such information can be collected from datasets of prior experience or generated from approximations of the world representation.

5.1 Modified Problem Formulation

We assume that the validity of each edge is an independent Bernoulli random variable. We are given a vector of probabilities $\mathbf{p}\in[0,1]^{\left|\mathcal{E}\right|}$, such that $P(\phi(e)=1)=\mathbf{p}(e)$, i.e., for each edge $e$, we have access to $\mathbf{p}(e)$, which defines the probability of the edge being valid in the current world $\phi$.

We allow the shortest path algorithm $\textsc{Alg}(\mathcal{G},v_{\rm s},v_{\rm t},\phi,\mathbf{p})$ to leverage knowledge of edge probabilities $\mathbf{p}$ to minimize the expected computation cost as follows:

[TABLE]

5.2 Event and Selector Design

Event.

The Event restricts lazy search from proceeding beyond a point when the search is likely to be ineffective, i.e. to a point that potentially increases the amount of rewires $\mathcal{V}_{\rm rwr}$. One such case is when the current shortest subpath is likely to be in collision, i.e., the probability of being valid drops below a threshold $\delta$. We describe this event, SubpathExistence (SE), in Algorithm 2. We show that we can bound the performance of this event.

Theorem 5.1**.**

For any Selector, the expected planning time of SubpathExistence ($\delta$) can be upper bounded as:

[TABLE]

where $K$ is the number of shortest-paths that are infeasible, $b$ is the maximum branching factor, and $p_{\text{max}}$ is the maximum value of an edge prior.

**Proof ** We first describe the GLS algorithm with SubpathExistence ($\delta$). The algorithm searches till the probability of the current shortest subpath drops below $\delta$. It toggles edge evaluation which will either eliminate the subpath or check an edge such that the probability rises above $\delta$. The search continues forward. This repeats till the shortest path has been found. For this proof, we assume we have an oracular selector that can invalidate a subpath if it is truly invalid.

We begin by upper bounding the number of edge evaluations. Let $\sigma^{*}$ be the shortest feasible path. Let there be $K$ shorter paths than $\sigma^{*}$ that are infeasible and that the algorithm has to eliminate. Since we are showing an upper bound, we can relax the condition that the paths have overlapping edges since they will only reduce edge evaluations (eliminating one implies the other is eliminated).

Consider one of $K$ paths that we have to eliminate. If we pick an edge from the subpath, with probability $1-\delta$ we will find a witness that the path is invalid. A selector either invalidates a subpath with probability $1-\delta$ or results in a wasted edge evaluation. This process is repeated till a path is eventually eliminated. The expected number of edge evaluated to eliminate the path is:

[TABLE]

Hence the total expected cost of edge evaluation is bounded by $c_{e}K\frac{1}{(1-\delta)}$. Note as $\delta\rightarrow 1$, this term goes to $\infty$. This is backed by the intuition that triggering the event often results in increased edge evaluation.

We will now upper bound the number of vertex rewiring. We assume that the search is using as heuristic the distance on the graph $h_{\mathcal{G}}(v,v_{\rm t})$. Hence when one of the $K$ subpaths are eliminated, only the vertices of that subpath is rewired. Since we are deriving an upper bound, we will ignore overlap between subpaths (which can only help).

Consider one of $K$ paths that we have to eliminate. Let $p_{\text{max}}$ be the maximum probability of an edge being valid. Then the maximum length of any subpath $L(\delta)$ is

[TABLE]

When the subpath is eliminated, the rewiring is restricted only to vertices belonging to the subpath. Hence the maximum vertex rewire that can occur is $\mathcal{V}_{\rm rwr}=bL(\delta)$ where $b$ is the maximum branching factor. A selector either invalidates a subpath with probability $1-\delta$ and results in rewiring or the process continues without any penalty. The expected number of vertices rewired before the path is eliminated can be upper bounded:

[TABLE]

Hence the total expected cost of vertex rewiring is upper bounded by $c_{r}Kb\frac{\log(\delta)}{\log(p_{\text{max}})}$. Note as $\delta\rightarrow 0$, this term goes to $\infty$. This is backed by the intuition that triggering the event less often results in increased vertex rewiring. ∎

Low values of $\delta$ result in lower edge evaluations but more edge rewiring, and vice-versa.

Corollary 5.1**.**

There exists a critical threshold $\delta\in(0,1)$ that upper bounds the expected computational cost.

**Proof ** The total expected planning time can be bounded as:

[TABLE]

We will now show that there exists a critical point $\delta$ that minimizes this. Solving for that critical point, we have:

[TABLE]

Let $\eta=\left(\frac{c_{e}}{bc_{r}}\log(\frac{1}{p_{\text{max}}})+2\right)$. The critical point is:

[TABLE]

When $c_{e}\approx c_{r}$, $\eta$ is close to $2$ and $\delta\rightarrow 1$. When $c_{e}\gg c_{r}$, we have $\eta\gg 2$. Hence, the critical point is:

[TABLE]

The critical point is inversely proportional to the ratio $\frac{c_{e}}{c_{r}}$.

∎

Selector.

The Selector invalidates as many subpaths as quickly as possible, which restricts the size of $\mathcal{E}_{\rm eval}$. One strategy for doing so is to invalidate the current subpath as quickly as possible. We describe a selector, FailFast (FF), in Algorithm 3 that evaluates the edge on the subpath with the highest probability of being in collision. We show that this selector is the optimal strategy to invalidate a subpath.

Theorem 5.2**.**

Given a path $\sigma$, FailFast minimizes the expected number of edges from $\sigma$ that must be evaluated to invalidate $\sigma$.

**Proof ** Given a path $\sigma$, and a sequence of edges $S=\{e_{1},e_{2},\ldots,e_{n}\}$ belonging to the path, and the corresponding priors of the edges being valid $(p_{1},p_{2},\ldots,p_{n})$, let the expected number of edge evaluations to invalidate the $\sigma$ be $\mathcal{E}_{\rm eval}(S)$ which is given by

[TABLE]

Without loss of generality, let $p_{i}>p_{i+1}$ for a given $i$. Consider the alternate sequence of evaluations $S^{\prime}=\{e_{1},e_{2},\ldots,e_{i+1},e_{i}\ldots,e_{n}\}$ where the positions of the edges $e_{i},~{}e_{i+1}$ are swapped. Consider the difference:

[TABLE]

Since each such swap results in monotonic decrease in the objective, there exists an unique fixed point, i.e., the optimal sequence $S^{*}$ has $p_{1}\leq p_{2}\leq\ldots\leq p_{n}$. ∎

5.3 Hypotheses

Based on our theoretical analysis and insight, we state three hypotheses that we intend to test:

H 1**.**

For any Selector, the event SubpathExistence requires less planning time compared to ShortestPath and ConstantDepth.

This follows from Theorem 5.1, which upper bounds the planning time for SubpathExistence. ShortestPath corresponds to $\delta=0$ and can increase planning time. ConstantDepth has a fixed lookahead and does not adapt as priors change.

H 2**.**

For any Event, FailFast evaluates fewer edges than Forward and Alternate.

This follows from Theorem 5.2, which shows that FailFast is optimal in expectation for eliminating a path. From H 1 and H 2, we hypothesize that the combination of SubpathExistence and FailFast will have the lowest planning time.

H 3**.**

The performance gain of SubpathExistence over ShortestPath increases with both graph size and problem difficulty.

ShortestPath assumes that $\mathcal{V}_{\rm rwr}$ is negligible. As graph size increases, the size of vertices $\mathcal{V}_{\rm rwr}$ that ShortestPath rewires also increases. Similarly, as problem difficulty increases, so does the number of shortest paths that ShortestPath must invalidate, which also increases $\mathcal{V}_{\rm rwr}$. SubpathExistence, on the other hand, makes no such assumption.

6 Experiments

Algorithm Details.

We implemented 3 Events and 3 Selectors described in Algorithms 2 and 3 to get a total of $9$ algorithms. To analyze the trade-offs, we test on a diverse set of $\mathbb{R}^{2}$ datasets. We then finalize on 3 algorithms: LazySP (ShortestPath, FailFast), LRA* (ConstantDepth, FailFast) and GLS (SubpathExistence, FailFast). We evaluate these on a Piano Movers’ problem in $SE(2)$ and manipulation problems in $\mathbb{R}^{7}$ using HERB (?), a mobile robot with 7DoF arms. 333Code is publicly available as an OMPL Planner at:

https://github.com/personalrobotics/gls

Analysis on $\mathbb{R}^{2}$ datasets.

We use $5$ datasets of $\mathbb{R}^{2}$ problems from (?). Each dataset corresponds to different parametric distribution of obstacles from which we sample $1000$ worlds. A graph of 2000 vertices is sampled using a low dispersion sampler (?) with an optimal connection radius (?). Priors are computed by collision checking the graph on the training data and averaging edge outcomes. The prior and some samples from $\mathbb{R}^{2}$ datasets are shown in Fig. 4(a). We pick one representative dataset, TwoWall, to show detailed plots.

We choose evaluation metrics (a) number of edge evaluations (b) number of vertex rewires and c) total planning time: weighted combination of (a), (b) (see Eq. 2). Since $\mathbb{R}^{2}$ problems are not expensive to evaluate, we choose weights based on empirical data from manipulation planning problems in $\mathbb{R}^{7}$ (avg. eval time: $3.35\times 10^{-4}$s, avg. rewire time $1.1\times 10^{-5}$s, ratio $29.04$).

Finally, for parameter selection, we choose $\delta$ in SubpathExistence from the pareto curve of vertices rewired vs edges evaluated computed on the training data. The slope of the line is the ratio of their relative cost – the point of interesection corresponds to the $\delta:0.01$ that minimizes planning time. For ConstantDepth, we use the recommended value from (?).

Table 2 shows the planning times of various algorithms under GLS. The planning times are the median quantities obtained from experiments over 100 different worlds sampled within the environment type. Fig. 4(c) shows the ranking of the planning times of the algorithms across the 400 worlds considered across the four datasets (lower plannning time in a problem translates to a better rank). We note that GLS with SubpathExistence and FailFast consistently outperforms remaining algorithms on a majority of environments.

We found strong evidence to support H 1 - SubpathExistence exhibits lowest planning times in 99% of the problems (Fig. 4(b, left)) Corresponding median planning times supporting the hypothesis are reported in Table 2. Fig. 5 shows a comparison of the events ShortestPath, ConstantDepth and SubpathExistence (for the Forward selector) on a problem from the TwoWall dataset. We can see that ShortestPath checks small number of edges but rewires significant portion of the search tree. The trend is reversed in ConstantDepth when using a depth of $1$. SubpathExistence is able to balance both by exploiting priors - it triggers events when the search reaches the walls thus reducing rewires.

We also found strong evidence to support H 2 - FailFast exhibits the lowest planning times in 83% (Fig. 4(b, right)) of the problems across the four datasets. In Table 2, we note that for a given event, FailFast has the lowest planning time in majority of the datasets. Fig. 6 shows a comparison of Forward and FailFast (for ShortestPath event) - FailFast quickly eliminates paths by checking the weakest link (supporting Theorem 5.2).

We found strong evidence to support H 3. Fig. 7(b) shows that as graphs get larger, planning times of ShortestPath grows at a faster rate than SubpathExistence. Fig. 7(c) shows that as the density of obstacles increase, the planning times of ShortestPath grows linearly while SubpathExistence eventually saturates.

Analysis on $SE(2)$ problems and $\mathbb{R}^{7}$ problems.

We consider the Piano Movers’ problem in $SE(2)$ from the Apartment scenario in OMPL (?). For the $\mathbb{R}^{7}$ environment, we consider two manipulation tasks with a 7-DoF arm (?) in a cluttered kitchen environment (Fig. 8). We used graphs of 8000 vertices and 30,000 vertices for the $SE(2)$ and $\mathbb{R}^{7}$ problems respectively.

In Table 3, we report the mean planning times across 100 problems each. We see that GLS(SubpathExistence, FailFast) outperforms the other algorithms in planning time on all three problems. Additionally, Fig. 7(d) shows a breakdown of the planning time for each of the three events on HERB Task 2. GLS significantly lowers rewiring time while having a minimal increase in evaluation time.

Figures 9, 10 compare the performance of LazySP and GLS with FailFast selector. They illustrate the savings of GLS on the Piano Movers’ problem (Fig. 9) and on a simplified manipulation scene (Fig. 10). In both cases, LazySP has to rewire a large search tree everytime a path is found to be in collision. GLS, on the other hand, halts the search as soon as it enters a region of low probability, eliminates the paths and hence drastically minimizes rewiring time at the cost of few additional edge evaluations over LazySP.

7 Discussion

We presented a general framework for lazy search (GLS). The staple framework interleaves two phases, search and evaluation. In the search phase, it extends a lazy shortest-path tree forward without evaluating any edges until an Event is triggered. It then switches to evaluation phase. It finds the shortest subpath to a leaf node of the tree and invokes a Selector to evaluate an edge on it. Careful choice of Event and Selector allows the balance of search effort with edge evaluation to minimize overall planning time.

The framework, quite expressive, lets us capture a range of lazy search algorithms (Table 1). While it draws inspiration from prior work interleaving search and evaluation, such as LRA* (?), the key difference lies in our definition of the Event, which makes the algorithm adaptive. This lets us derive new algorithms that are edge optimal while saving on search effort (Theorem 4.3).

In future work, we plan to examine more sophisticated Selector policies (?) that exploit correlations amongst edges to minimize evaluation cost. We also plan to extend GLS to an anytime paradigm; this would let us use heuristics that exploit edge priors to guide the search through regions of high probability (?), for significant speed-ups. Finally, we plan to explore problems where multiple lazy estimates of weight functions are available, e.g., in kinodynamic planning, where different relaxations of the boundary value problem can be obtained. We believe GLS can interleave search efficiently over multiple resolutions of approximation.