Abstract-Rapidly-exploring random trees (RRTs) are popular in motion planning because they find solutions efficiently to single-query problems. Optimal RRTs (RRT*s) extend RRTs to the problem of finding the optimal solution, but in doing so asymptotically find the optimal path from the initial state to every state in the planning domain. This behaviour is not only inefficient but also inconsistent with their single-query nature.For problems seeking to minimize path length, the subset of states that can improve a solution can be described by a prolate hyperspheroid. We show that unless this subset is sampled directly, the probability of improving a solution becomes arbitrarily small in large worlds or high state dimensions. In this paper, we present an exact method to focus the search by directly sampling this subset.The advantages of the presented sampling technique are demonstrated with a new algorithm, Informed RRT*. This method retains the same probabilistic guarantees on completeness and optimality as RRT* while improving the convergence rate and final solution quality. We present the algorithm as a simple modification to RRT* that could be further extended by more advanced path-planning algorithms. We show experimentally that it outperforms RRT* in rate of convergence, final solution cost, and ability to find difficult passages while demonstrating less dependence on the state dimension and range of the planning problem.

In this paper, we present Batch Informed Trees (BIT*), a planning algorithm based on unifying graph-and sampling-based planning techniques. By recognizing that a set of samples describes an implicit random geometric graph (RGG), we are able to combine the efficient ordered nature of graph-based techniques, such as A*, with the anytime scalability of sampling-based algorithms, such as Rapidly-exploring Random Trees (RRT).BIT* uses a heuristic to efficiently search a series of increasingly dense implicit RGGs while reusing previous information. It can be viewed as an extension of incremental graphsearch techniques, such as Lifelong Planning A* (LPA*), to continuous problem domains as well as a generalization of existing sampling-based optimal planners. It is shown that it is probabilistically complete and asymptotically optimal.We demonstrate the utility of BIT* on simulated random worlds in R 2 and R 8 and manipulation problems on CMU's HERB, a 14-DOF two-armed robot. On these problems, BIT* finds better solutions faster than RRT, RRT*, Informed RRT*, and Fast Marching Trees (FMT*) with faster anytime convergence towards the optimum, especially in high dimensions.

Anytime almost-surely asymptotically optimal planners, such as RRT*, incrementally find paths to every state in the search domain. This is inefficient once an initial solution is found as then only states that can provide a better solution need to be considered. Exact knowledge of these states requires solving the problem but can be approximated with heuristics.This paper formally defines these sets of states and demonstrates how they can be used to analyze arbitrary planning problems. It uses the well-known L 2 norm (i.e., Euclidean distance) to analyze minimum-path-length problems and shows that existing approaches decrease in effectiveness factorially (i.e., faster than exponentially) with state dimension. It presents a method to address this curse of dimensionality by directly sampling the prolate hyperspheroids (i.e., symmetric n-dimensional ellipses) that define the L 2 informed set.The importance of this direct informed sampling technique is demonstrated with Informed RRT*. This extension of RRT* has less theoretical dependence on state dimension and problem size than existing techniques and allows for linear convergence on some problems. It is shown experimentally to find better solutions faster than existing techniques on both abstract planning problems and HERB, a two-arm manipulation robot.Index Terms-path planning, sampling-based planning, optimal path planning, informed sampling.The only states that need to be considered in single-query scenarios are those that can provide a better solution [7]. While exact knowledge of these states requires solving the planning problem, they can often be approximated with heuristics (Fig. 1). These heuristics have previously been used to focus almost-surely asymptotically optimal search [8,9] but can also provide insight into the optimal planning problem. This paper uses the set of states that can provide a better solution to analyze incremental almost-surely asymptotically optimal planning. It formally defines this shrinking set as the omniscient set and shows that sampling it is a necessary condition for RRT*-style planners to improve a solution. It defines estimates of this set as informed sets and provides metrics to quantify them in terms of their compactness (i.e., precision) and completeness (i.e., recall). It uses these results to bound the probability of improving a solution to a holonomic planning problem by the probability of sampling an informed set with 100% recall.The L 2 norm (i.e., Euclidean distance) is a well-known heuristic for problems seeking to minimize path length. It describes the omniscient set exactly in the absence of obstacles This document consolidates the published paper [40] with its supplementary material and presents it as reviewed. arXiv:1706.06454v5 [cs.RO] 17 Aug 2018 P lim sup q→∞ c (σ q ) = c (σ * ) = 1,This document consolidates the published paper [40] with its supplementary material and presents it as reviewed.

Estimating motion from images is a well-studied problem in computer vision and robotics. Previous work has developed techniques to estimate the motion of a moving camera in a largely static environment (e.g., visual odometry) and to segment or track motions in a dynamic scene using known camera motions (e.g., multiple object tracking).It is more challenging to estimate the unknown motion of the camera and the dynamic scene simultaneously. Most previous work requires a priori object models (e.g., trackingby-detection), motion constraints (e.g., planar motion), or fails to estimate the full SE (3) motions of the scene (e.g., scene flow). While these approaches work well in specific application domains, they are not generalizable to unconstrained motions. This paper extends the traditional visual odometry (VO) pipeline to estimate the full SE (3) motion of both a stereo/RGB-D camera and the dynamic scene. This multimotion visual odometry (MVO) pipeline requires no a priori knowledge of the environment or the dynamic objects. Its performance is evaluated on a real-world dynamic dataset with ground truth for all motions from a motion capture system.