# Adaptively Informed Trees (AIT*): Fast Asymptotically Optimal Path Planning through Adaptive Heuristics

## Abstract

Informed sampling-based planning algorithms exploit problem knowledge for better search performance. This knowledge is often expressed as heuristic estimates of solution cost and used to order the search. The practical improvement of this informed search depends on the accuracy of the heuristic.

Selecting an appropriate heuristic is difficult. Heuristics applicable to an entire problem domain are often simple to define and inexpensive to evaluate but may not be beneficial for a specific problem instance. Heuristics specific to a problem instance are often difficult to define or expensive to evaluate but can make the search itself trivial.

This paper presents Adaptively Informed Trees (AIT*), an almost-surely asymptotically optimal sampling-based planner based on BIT*. AIT* adapts its search to each problem instance by using an asymmetric bidirectional search to simultaneously estimate and exploit a problem-specific heuristic. This allows it to quickly find initial solutions and converge towards the optimum. AIT* solves the tested problems as fast as RRT-Connect while also converging towards the optimum.

IEEEexample:BSTcontrol

## I Introduction

Path planning is the problem of finding a continuous sequence of valid states between a start and goal specification. Sampling-based planners, such as Probabilistic Roadmaps (PRM) [kavraki1996], approximate the state space by sampling discrete states and connecting them with edges. The resulting structure can then be processed by graph-search algorithms to find a sequence of states that connects the start to the goal.

Informed graph-search algorithms, such as A* [hart1968], use knowledge about a problem domain to increase their efficiency. This knowledge is often captured in the form of a heuristic function, , which estimates cost-to-go, i.e., the cost to go from any state in the state space to the goal.

The properties of this heuristic directly affect the performance of the search algorithms. An admissible heuristic never overestimates the actual cost-to-go. A consistent heuristic satisfies a triangle-inequality, such that for any two states, , it satisfies , where is the best cost of any path from to . Note that by definition all consistent heuristics are also admissible. A* is guaranteed to find the optimal solution when provided with an admissible heuristic. If the provided heuristic is also consistent, then A* expands the minimum number of states of any informed graph-search algorithm using that heuristic (i.e., it is optimally efficient [hart1968]).

Improving the accuracy of a heuristic directly improves the performance of informed search algorithms [korf1997, culberson1998, felner2004, paden2017], and the search becomes trivial when a perfect heuristic is available.

Designing and selecting effective heuristics is difficult for many problem domains. This is because heuristics are most effective when they are both accurate and computationally inexpensive to evaluate. Heuristics that are applicable to an entire problem domain are often inexpensive to evaluate but may not be accurate for a specific problem instance. Accurate heuristics can be designed for a specific problem instance during the search, but this can be computationally expensive and may diminish the overall search performance.

Computational cost directly influences the real-world performance of planning algorithms. Sampling-based planners contain a number of computationally expensive basic operations, including state expansions and edge evaluations [kleinbort2016]. State expansions often require nearest neighbor searches that increase in computational cost with the number of samples. Edge evaluations require local planning between two states and detecting collisions on the resulting path.

Lazy sampling-based planners, such as Lazy PRM [bohlin2000], reduce this computational cost by avoiding the evaluation of every edge. These algorithms first perform an inexpensive search on a simplified approximation without collision detection. This allows them to only evaluate the edges that are believed to be on an optimal path, and reduce the number of evaluated edges. This improves performance, especially for problems with computationally expensive edge evaluations, such as those considered in this paper.

This paper presents Adaptively Informed Trees (AIT*), a lazy, almost-surely asymptotically optimal sampling-based planner that uses an asymmetric bidirectional search to simultaneously estimate and exploit an accurate, problem-specific heuristic. AIT* estimates this heuristic by performing a lazy reverse search on the current sampling-based approximation. This heuristic is then used to order the forward search of this approximation while considering complete edge evaluations. The results of the computationally expensive edge evaluations performed by this forward search inform the reverse search, which creates increasingly accurate heuristics. This allows AIT* to efficiently share information between the two individual searches.

Efficiently estimating and exploiting a problem-specific heuristic allows AIT* to outperform existing sampling-based planning algorithms when edge evaluations are expensive. AIT* finds initial solutions to the tested problems at least as fast as RRT-Connect while still almost-surely converging to the optimal solution, which RRT-Connect does not.

## Ii Background

Creating and adapting heuristics to improve informed graph-search algorithms is an active area of research (Sec. II-A). Heuristics are applied in sampling-based planning to reduce search effort by ordering the search and focus the approximation to the relevant region of the state space (Sec. II-B). Lazy search algorithms separately focus on reducing search effort by avoiding collision detection (Sec. II-C).

### Ii-a Improved Heuristics for Informed Search Algorithms

Improving the heuristic for informed graph-search algorithms has been shown to be effective for many problem domains, including path planning [paden2017].

Pattern Databases [culberson1998] are precomputed tables of exact solution costs to simplified subproblems of a problem domain. An informed algorithm can use this database during the search to create admissible heuristics. Additive Pattern Databases [felner2004] extend this approach to combine database entries into more accurate heuristics that are still admissible. This approach speeds up the search of problems for which simplified subproblems can be created and solved, but requires creating databases for every problem domain a priori to the search.

Heuristic accuracy can alternatively be improved by using the error in the heuristic values of states as they are discovered. Thayer et al. [thayer2011] use the error of each state expansion to update the heuristic during the search. This can be applied to any problem domain and does not require any preprocessing, but the resulting heuristic is not guaranteed to be admissible.

Adaptive A* [koenig2005, koenig2006a] is an incremental search algorithm that updates its heuristic function based on the cost-to-come values of previous searches of similar problems. This results in ever more accurate and admissible heuristics but can not be used for the initial search of a graph.

Kaindl. et al [kaindl1997] use the Add method to inform a forward search with a partial reverse search. The reverse search reveals errors in the heuristic values, the minimum of which is added to all unexpanded states. This results in a more informed but still admissible heuristic, but requires the user to specify how many states to expand during the reverse search and increases the heuristic uniformly for all unexpanded states. Wilt et al. [wilt2013] present an updated version of this method which does not require a user-specified parameter.

Unlike these approaches, AIT* does not need a predefined database, creates a consistent heuristic during the search, can be used on the initial search of a graph, and adaptively estimates the heuristic for each state individually.

### Ii-B Sampling-Based Planning with Heuristics

Heuristics have been used in sampling-based planning to guide the search and focus the approximation. RRT-Connect [kuffner2000] builds on Rapidly-exploring Random Trees (RRT) [lavalle2001] by incrementally growing two trees, one rooted in the start state and one in the goal state. These trees each explore the state space around them but are also guided towards each other by a connect heuristic. This approach can result in very fast initial solution times but is not almost-surely asymptotically optimal and does not improve the solution quality with more computational time.

Informed RRT* [gammell2018] incorporates an ellipsoidal heuristic into the almost-surely asymptotically optimal RRT* [karaman2011]. This improves the convergence rate by focusing the incremental approximation to the relevant region of the state space but does not guide the search.

Sakcak et al. [sakcak2019a] incorporate a heuristic into a version of RRT* which is based on motion-primitives [sakcak2019b]. This can improve the performance for kinodynamic systems but requires preprocessing and relies on an a priori discretization which suffers from the curse of dimensionality [bellman1957].

Batch Informed Trees (BIT*) [gammell2015, gammell2020] samples batches of states and views these sampled states as an increasingly dense edge-implicit random geometric graph (RGG) [penrose2003]. This allows BIT* to use a series of informed graph-searches to process the states in order of potential solution quality. BIT* efficiently reuses information from both previous searches and approximations by using incremental search techniques but does not update its heuristic during the search.

Unlike these approaches, AIT* improves its solution quality with more computational time, uses its heuristic to guide the search, does not rely on an a priori discretization, and updates its heuristic during the search.

### Ii-C Sampling-Based Planning with Lazy Collision Detection

Path planning algorithms employ lazy collision detection to avoid spending computational resources on edges that are unlikely to be on an optimal path. Lazy PRM [bohlin2000] approximates the entire state space with an RGG without collision detection and searches this RGG for a path from the start state to a goal state. This path is then checked for collisions. If collisions are detected, then the corresponding edges and vertices are removed from the graph and a new search must be started from scratch. There also exist almost-surely asymptotically optimal variants of Lazy PRM [hauser2015, kim2018].

Lazy Shortest Path (LazySP) [dellin2016] is a class of algorithms that reduces the number of edges checked for collisions. It first finds a path from the start to the goal using an inexpensive estimate of the edge costs. Once a path is found, it uses an edge selector function which determines the order in which these edges are checked for collision. An example of a LazySP algorithm is Lazy Receding Horizon A* (LRHA*) [mandalika2018].

Unlike these approaches, AIT* does not restart its search from scratch upon detecting collisions, and uses admissible heuristics to focus its approximation.

## Iii Adaptively Informed Trees (AIT*)

BIT* is an almost-surely asymptotically optimal sampling-based planner that builds a discrete approximation of a state space by sampling batches of states. This approximation can be focused to the region of the state space that can possibly improve a current solution with informed sampling [gammell2018].

BIT* views the states it samples as an increasingly dense, edge-implicit RGG. It processes the implicit RGG edges in order of potential solution quality, similar to an edge-queue version of Lifelong Planning A* (LPA*) [koenig2004]. The true edge costs are evaluated lazily by maintaining a queue ordered by the sum of the current cost-to-come from the start to the edge’s parent state, a heuristic of the edge cost, and a heuristic of the cost-to-go from the edge’s child state to the goal state. Full details of BIT* are in [gammell2017, gammell2020].

AIT* builds on BIT* by improving the accuracy of the used heuristic, which improves performance on problems with expensive edge evaluations. It uses an asymmetric bidirectional search to efficiently estimate and exploit an accurate heuristic for each problem instance (Fig. 2). The forward search is the same as in BIT* but uses the heuristic provided by a computationally inexpensive reverse search. This heuristic can be updated efficiently when the forward search reveals that it contains errors by using an incremental algorithm, such as LPA*, on the reverse search. Algorithm LABEL:alg:conceptual presents a conceptual overview of BIT* and AIT*. The full algorithmic details are provided in Algorithms LABEL:alg:technical–LABEL:alg:updatestate.

Since BIT* is almost-surely asymptotically optimal when given an admissible heuristic [gammell2020] and the reverse search of AIT* results in an admissible heuristic for each approximation, AIT* is almost-surely asymptotically optimal as well.

### Iii-a Notation

The state space of the planning problem is denoted by , the start by , and the goals by . The sampled states are denoted by .

The forward and reverse search trees are denoted by , and , respectively. The vertices in these trees, denoted by and , are associated with valid states. The edges in the forward tree, , represent valid connections between states, while the edges in the reverse tree, , can lead through invalid regions of the problem domain. An edge consists of a parent state, , and a child state, , and is denoted as .

Let denote the union of the nonnegative real numbers with infinity. The function represents an admissible heuristic of the cost-to-come from the start to a state. The function represents the cost-to-come from the start state to a state through the current forward tree. This cost is taken to be infinite for any state without an associated vertex in the forward tree.

The function represents an admissible heuristic of the cost-to-go from a state to a goal. The function represents an admissible estimate of the cost of a path from the start to a goal constrained to go through a state, e.g., . This estimate defines the informed set of states that could provide a better solution,