Robust Autonomy for Uncertain Systems using Randomized Search

Motion planning, decision-making and trajectory generation sit at the core of every autonomous and robotic system, and two bottlenecks continue to limit them: the dimensionality of the search space, and the uncertainty that is always present in a real deployment. This project combined two bodies of work that are usually kept apart — randomized graph representations, which scale to high dimensions where grid-based abstractions do not, and stochastic optimal control, which quantifies and mitigates uncertainty. The result is a family of algorithms that plan in high-dimensional spaces while accounting for motion, sensing and model uncertainty.

The first thread concerns where to draw the next sample. Planning on a cost-map, where the cost varies continuously rather than being a binary obstacle test, makes uninformed sampling especially wasteful. 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 candidate point, and samples only where that bound improves on the current solution. For time-optimal kinodynamic problems we defined instead a time-informed set containing every trajectory better than the incumbent, showed that exploring outside it is redundant, and precomputed a library of reachable sets that makes sampling inside it cheap. A later refinement exploits the structure of the incumbent solution locally rather than globally.

Planning on a terrain cost-map with the relevant region sampling strategy. The tree concentrates in the low-cost corridors rather than spreading uniformly over the map.

The second thread reduces the dimension of the space being sampled. Sampling-based planners converge slowly for dynamical systems, where the sampling space is the full state space rather than the configuration space. We introduced a partial-final-state-free optimal controller that computes part of the terminal state rather than sampling it, so the planner samples in a reduced-dimension space and converges faster; the idea admits an analytical solution for linear systems. Extending it to nonlinear dynamics required replacing the analytical controller with one learned offline by supervised learning, which in turn required a planner tolerant of inexact steering, since a learned controller cannot be relied on to hit a state exactly. The resulting algorithm needs no rewiring step and produces dynamically feasible plans in cluttered environments with nonconvex obstacles.

The third thread plans in belief space, where the state of the robot is a distribution rather than a point. Earlier belief-space roadmaps could not achieve or maintain a specified covariance at a node, which forced conservative plans. We used covariance steering to design the edge controllers of the roadmap, which guarantees that terminal belief constraints are met in finite time and removes the convergence phase previously required at every node — so several velocities can be sampled at the same position node, and the belief space is explored far better. For nonlinear dynamics we introduced compatible nominal trajectories, computed by successive linearization, together with an output-feedback covariance steering method built on them. A separate line of work grew belief trees by batch sampling, interleaving nominal trajectory graph construction with an informed cost-to-go heuristic and an ordered graph search, and reusing results between iterations.

The fourth thread addresses incremental search when evaluating an edge is expensive, as it is for nonholonomic systems where each edge requires a steering computation. We combined the vertex efficiency of Lifelong Planning A* with the edge efficiency of Generalized Lazy Search: a lazily evaluated LPA* repairs the cost-to-come inconsistencies of the relevant part of the search tree from the previous episode, and expensive edge evaluations are restricted to the current shortest subpath. The algorithm is complete and finds the optimal solution in the current graph, with an order of magnitude faster replanning than LPA* over consecutive episodes in a dynamic environment. Related work extends the same idea to weighted colored graphs and to bounded-suboptimality search.

Comparison of replanning algorithms. Bold lines are edges of the current search tree, with the solution path drawn boldest. The lazy variants reach the same path having evaluated a fraction of the edges.

The fifth thread uses random graphs to solve stochastic optimal control problems posed as coupled forward–backward stochastic differential equations. FBSDEs avoid solving the Hamilton-Jacobi-Bellman partial differential equation, but a naive application produces an estimator with high variance, because the sampled trajectories fall away from the optimal ones. Invoking Girsanov’s theorem allows the forward sampling measure to be changed without introducing bias into the estimator, which is what makes it possible to import a rapidly-exploring random tree into the forward pass: McKean-Markov branched sampling spreads samples across the reachable space instead of drawing independent trajectories. In the backward pass, a local entropy weighting concentrates the accuracy of the value function approximation where optimal trajectories are likely to lie. The combination converges considerably faster than previous FBSDE methods and produced value functions and policies for problems as hard as the double inverted pendulum and a five-dimensional vehicle reachability problem.

Measure distributions for a one-dimensional stochastic optimal control problem. RRT-based sampling together with local entropy weighting recovers the optimal distribution much sooner than independent parallel sampling.

In the final phase the same concerns were carried into sampling-based model predictive control. Monte Carlo estimators such as MPPI degrade when most sampled rollouts are infeasible, so we trained a neural approximation of a discrete-time control barrier function offline and used it online to resample the rollouts, steering the sampling distribution towards the safe set before the weighted average that produces the control is taken.

Offline, a neural approximation of a discrete-time control barrier function is trained from data. Online, it drives a resampling-based rollout that improves the sampling distribution of the Monte Carlo estimator before the weighted average is taken.

Alongside the graduate research, more than twenty-five undergraduate students from aerospace, mechanical and electrical engineering and computer science built and maintained a fleet of 1/10-scale autonomous ground robots as part of a Vertically Integrated Projects team on active safety for autonomous and semi-autonomous vehicles. The platforms carry their own sensing and computing and were used to test the perception, planning and control algorithms developed here on hardware. Several undergraduates took part through an REU supplement.

Sponsors

This project is sponsored by NSF.