Motion planning and trajectory generation problems are at the core of all autonomous and robotic systems. Although – at least in theory – such problems can be solved using optimal control or dynamic programming, the computational complexity for realizing these solutions is still prohibitive for many real-life problems, especially for high-dimensional systems. Recently, randomized algorithms have been proposed, which promise to remedy the “curse of dimensionality” associated with more traditional, deterministic methods.
In recent years, sampling-based motion planning algorithms (SBMPs) have become popular due to their ability to handle higher dimensions and kino-dynamic constraints. Incremental sampling based algorithms, in particular, Rapidly-exploring Random Trees (RRT), RRT* avoid apriori discretization of the search space and build a connectivity graph online by generating random samples from the search space. However, these randomized methods are hindered by three computational bottlenecks, namely:
- The need for an intelligent sampling strategy to avoid redundant exploration
- Avoiding an expensive collision checking module
- Efficient ways of implementing the so-called “local steering” problem connecting any two vertices in the connectivity graph.
In this research, we propose ideas from classical heuristics, dynamic programming and machine learning to address these issues.
The starting point is the RRT# algorithm, which rewires the graph globally using value iteration, so that every vertex is optimally connected to the root at the end of each iteration rather than only locally repaired. The planner was rewritten in C/C++ and contributed to the Open Motion Planning Library, where it can be compared directly against the other planners in the literature. We then went further and optimized the locations of the samples as well as the wiring between them. The resulting Deformable RRT (DRRT) treats each new sample as a local perturbation: the branch from the root to the new sample is optimized before the change is propagated through the tree, which avoids a global gradient descent at every iteration. To measure how these algorithms degrade with dimension we introduced the hypercube diagonal experiment, and on it DRRT converges orders of magnitude faster than comparable sampling-based planners for any problem of dimension greater than three.
RRT# on a planar problem. Global rewiring by value iteration keeps every vertex optimally connected, so the search concentrates on the region that can still improve the solution.
Because these planners work in configuration space rather than the plane, they apply directly to manipulators and other high-dimensional systems. The videos below show RRT# planning for anthropomorphic robots with six and twelve degrees of freedom, and DRRT planning for a manipulator.
Anthropomorphic robot motion planning with six degrees of freedom.
The same problem with twelve degrees of freedom.
Manipulator planning with the Deformable RRT algorithm.
Start and goal configurations for a dual-arm planning problem, where the two arms must reach their targets without colliding with each other or with the objects on the table.
The second thread attacks the exploration problem: where to draw the next sample. A good strategy has to find some initial solution quickly and then concentrate on the region that can still improve it. The non-parametric informed sampler does this by keeping a subset of well-dispersed kernel vertices and generating new samples in their vicinity, scoring each candidate by a combination of its heuristic value, the local density of the graph and a count-based exploration term, and steering away from directions where collisions have already been recorded. For planning on general cost-maps, where the cost varies continuously rather than being a binary obstacle test, the relevant region sampler uses the cost-to-come values already stored at the graph vertices, together with an admissible heuristic, to bound the cost of any solution passing through a given point, and samples only where that bound beats the current solution. Later, using ideas from reachability analysis, we defined a time-informed set containing every trajectory better than the incumbent, proved that exploring outside it is redundant, and built a library of reachable sets that makes sampling inside it cheap.
Non-parametric informed sampling. New samples are drawn near well-dispersed kernel vertices, inside the informed set, and away from directions in which collisions have already been seen.
Relevant region sampling on a terrain cost-map (left) and a potential cost-map (right). The tree spreads through the low-cost regions instead of the whole space.
The third bottleneck is local steering, the two-point boundary value problem that must be solved every time a node is expanded. It has a closed-form solution only for simple linear systems, and solving it numerically becomes the dominant cost as the dynamics grow more complex. We therefore learned it instead: optimal trajectories computed offline become training data for a goal-conditioned state-feedback neural network, which predicts the connecting control online and serves as the steering routine inside a kinodynamic RRT*. Separately, the policy-iteration variant of RRT# was parallelized on an NVIDIA GPU. Profiling showed that invoking the CUDA kernel every iteration is dominated by host-to-device transfer overhead, so the kernel is instead called at strategic intervals, solving a larger dynamic programming problem less often — which runs significantly faster than the serial planner while producing solutions of the same quality.
Kinodynamic planning through a three-dimensional reconstructed environment, where the connections between samples are produced by the learned steering controller.
The algorithms were tested both in simulation and on hardware. A fleet of ten 1/10-scale RACECAR ground robots was assembled, each carrying its own sensing and an NVIDIA Jetson processor, with a ROS navigation stack used to run the planners developed here. The closed-loop variant, CL-RRT#, was also implemented on a Mars rover testbed at NASA’s Jet Propulsion Laboratory as part of their next-generation autonomous navigation work.
Non-parametric informed sampling concentrating the search after an initial solution is found.
Autonomous navigation on the RACECAR platform in simulation.
CL-RRT# running within the next-generation AutoNav pipeline on a rover testbed at NASA JPL.
Sponsors
This project is sponsored by NSF.