Path planning in robotics often requires finding high-quality solutions to continuously valued and/or high-dimensional problems. These problems are challenging and most planning algorithms instead ...solve simplified approximations. Popular approximations include graphs and random samples, as used by informed graph-based searches and anytime sampling-based planners, respectively.
Informed graph-based searches, such as A*, traditionally use heuristics to search a priori graphs in order of potential solution quality. This makes their search efficient, but leaves their performance dependent on the chosen approximation. If the resolution of the chosen approximation is too low, then they may not find a (suitable) solution, but if it is too high, then they may take a prohibitively long time to do so.
Anytime sampling-based planners, such as RRT*, traditionally use random sampling to approximate the problem domain incrementally. This allows them to increase resolution until a suitable solution is found, but makes their search dependent on the order of approximation. Arbitrary sequences of random samples approximate the problem domain in every direction simultaneously, but may be prohibitively inefficient at containing a solution.
This article unifies and extends these two approaches to develop Batch Informed Trees (BIT*), an informed, anytime sampling-based planner. BIT* solves continuous path planning problems efficiently by using sampling and heuristics to alternately approximate and search the problem domain. Its search is ordered by potential solution quality, as in A*, and its approximation improves indefinitely with additional computational time, as in RRT*. It is shown analytically to be almost-surely asymptotically optimal and experimentally to outperform existing sampling-based planners, especially on high-dimensional planning problems.
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 L2 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 ndimensional ellipses) that define the L2 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.
This paper presents a Robust Constrained Learning-based Nonlinear Model Predictive Control (RC-LB-NMPC) algorithm for path-tracking in off-road terrain. For mobile robots, constraints may represent ...solid obstacles or localization limits. As a result, constraint satisfaction is required for safety. Constraint satisfaction is typically guaranteed through the use of accurate, a priori models or robust control. However, accurate models are generally not available for off-road operation. Furthermore, robust controllers are often conservative, since model uncertainty is not updated online. In this work our goal is to use learning to generate low-uncertainty, non-parametric models in situ. Based on these models, the predictive controller computes both linear and angular velocities in real-time, such that the robot drives at or near its capabilities while respecting path and localization constraints. Localization for the controller is provided by an on-board, vision-based mapping and navigation system enabling operation in large-scale, off-road environments. The paper presents experimental results, including over 5 km of travel by a 900 kg skid-steered robot at speeds of up to 2.0 m/s. The result is a robust, learning controller that provides safe, conservative control during initial trials when model uncertainty is high and converges to high-performance, optimal control during later trials when model uncertainty is reduced with experience.
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.
This paper presents a Learning‐based Nonlinear Model Predictive Control (LB‐NMPC) algorithm to achieve high‐performance path tracking in challenging off‐road terrain through learning. The LB‐NMPC ...algorithm uses a simple a priori vehicle model and a learned disturbance model. Disturbances are modeled as a Gaussian process (GP) as a function of system state, input, and other relevant variables. The GP is updated based on experience collected during previous trials. Localization for the controller is provided by an onboard, vision‐based mapping and navigation system enabling operation in large‐scale, GPS‐denied environments. The paper presents experimental results including over 3 km of travel by three significantly different robot platforms with masses ranging from 50 to 600 kg and at speeds ranging from 0.35 to 1.2 m/s (associated video at http://tiny.cc/RoverLearnsDisturbances). Planned speeds are generated by a novel experience‐based speed scheduler that balances overall travel time, path‐tracking errors, and localization reliability. The results show that the controller can start from a generic a priori vehicle model and subsequently learn to reduce vehicle‐ and trajectory‐specific path‐tracking errors based on experience.
Visual navigation is a key enabling technology for autonomous mobile vehicles. The ability to provide large‐scale, long‐term navigation using low‐cost, low‐power vision sensors is appealing for ...industrial applications. A crucial requirement for long‐term navigation systems is the ability to localize in environments whose appearance is constantly changing over time—due to lighting, weather, seasons, and physical changes. This paper presents a multiexperience localization (MEL) system that uses a powerful map representation—storing every visual experience in layers—that does not make assumptions about underlying appearance modalities and generators. Our localization system provides real‐time performance by selecting online, a subset of experiences against which to localize. We achieve this task through a novel experience‐triage algorithm based on collaborative filtering, which selects experiences relevant to the live view, outperforming competing techniques. Based on classical memory‐based recommender systems, this technique also enables landmark‐level recommendations, is entirely online, and requires no training data. We demonstrate the capabilities of the MEL system in the context of long‐term autonomous path following in unstructured outdoor environments with a challenging 100‐day field experiment through day, night, snow, spring, and summer. We furthermore provide offline analysis comparing our system to several state‐of‐the‐art alternatives. We show that the combination of the novel methods presented in this paper enable full use of incredibly rich multiexperience maps, opening the door to robust long‐term visual localization.
The Boreas dataset was collected by driving a repeated route over the course of 1 year, resulting in stark seasonal variations and adverse weather conditions such as rain and falling snow. In total, ...the Boreas dataset includes over 350 km of driving data featuring a 128-channel Velodyne Alpha-Prime lidar, a 360° Navtech CIR304-H scanning radar, a 5MP FLIR Blackfly S camera, and centimetre-accurate post-processed ground truth poses. Our dataset will support live leaderboards for odometry, metric localization, and 3D object detection. The dataset and development kit are available at boreas.utias.utoronto.ca.
In this paper, we revisit batch state estimation through the lens of
Gaussian process
(GP) regression. We consider continuous-discrete estimation problems wherein a trajectory is viewed as a ...one-dimensional GP, with time as the independent variable. Our continuous-time prior can be defined by any nonlinear, time-varying stochastic differential equation driven by white noise; this allows the possibility of smoothing our trajectory estimates using a variety of vehicle dynamics models (e.g. ‘constant-velocity’). We show that this class of prior results in an inverse kernel matrix (i.e., covariance matrix between all pairs of measurement times) that is exactly sparse (block-tridiagonal) and that this can be exploited to carry out GP regression (and interpolation) very efficiently. When the prior is based on a linear, time-varying stochastic differential equation and the measurement model is also linear, this GP approach is equivalent to classical, discrete-time smoothing (at the measurement times); when a nonlinearity is present, we iterate over the whole trajectory to maximize accuracy. We test the approach experimentally on a simultaneous trajectory estimation and mapping problem using a mobile robot dataset.