What is FaSTrack?

FaSTrack is a tool that is used in conjunction with any model-based motion planner to provide safety guarantees while planning and executing trajectories in real time. FaSTrack allows the planner to find trajectories using simple and easily computed planning models; this allows for the planner to operate in real time.  To guarantee safety, a tracking error bound is precomputed to provide bounds on the largest deviation possible between the simple planner and the more complicated autonomous system.

 
A nonlinear, ten-dimensional quadrotor simulation navigating an obstacle course in real-time with guaranteed safety.  The planner is a 3D RRT planner with a tracking error bound (think safety bubble) based on the speed of the planner.  Note that the planner mode changes throughout the course to trade off speed with maneuverability.  As the planner senses its environment the local obstacles turn red and the planner replans.

A nonlinear, ten-dimensional quadrotor simulation navigating an obstacle course in real-time with guaranteed safety.  The planner is a 3D RRT planner with a tracking error bound (think safety bubble) based on the speed of the planner.  Note that the planner mode changes throughout the course to trade off speed with maneuverability.  As the planner senses its environment the local obstacles turn red and the planner replans.

 

The Thorough Explanation

On the left we have a high-dimensional vehicle moving through an obstacle course to a goal. Computing the optimal safe trajectory is a slow and sometimes intractable task, and replanning is nearly impossible.  In the center we simplify our model of the vehicle (in this case assuming it can move in straight lines connected at points).  This allows us to plan very quickly, but when we execute the planned trajectory we may find that we cannot actually follow the path exactly, and end up crashing. On the right we have our proposal: plan using the simplified model, but precompute a tracking error bound that captures all potential deviations of the trajectory due to model mismatch.

On the left we have a high-dimensional vehicle moving through an obstacle course to a goal. Computing the optimal safe trajectory is a slow and sometimes intractable task, and replanning is nearly impossible.  In the center we simplify our model of the vehicle (in this case assuming it can move in straight lines connected at points).  This allows us to plan very quickly, but when we execute the planned trajectory we may find that we cannot actually follow the path exactly, and end up crashing. On the right we have our proposal: plan using the simplified model, but precompute a tracking error bound that captures all potential deviations of the trajectory due to model mismatch.

Guaranteed safe real-time motion planning is a difficult challenge, particularly for systems with complicated dynamics, external disturbances (like wind), and a priori unknown environments.  Using the reachability techniques developed in our lab, safe motion planning can be accomplished for decomposable and/or low dimensional systems; however even in these situations our techniques can't process safe trajectories for systems of more than about two dimensions in real time.  For real life systems like cars, planes, and quadrotors, fast planning is intractable.  On the other hand, motion planners like rapidly-exploring random trees (RRT) or model-predictive control (MPC) can plan in real time by using simplified models of system dynamics, which sacrifice guarantees of safety and feasibility.  Our tool allows users to implement a fast motion planner for simplified dynamics while maintaining safety by providing a precomputed bound on the maximum possible tracking error between the planner and the autonomous system.  This precomputation also results in an optimal control look-up table which provides the optimal control for the autonomous system to pursue the online planner in real time.

Offline Precomputation

We precompute this bound by viewing the problem as a pursuit-evasion game between a planner and a tracker.  The planner uses a simplified model of the system that is necessary for fast planning; the tracker uses a model of the true autonomous system.  We assume that the tracker is always pursuing the planner; in other words, the autonomous system is always trying to follow the planner. We want to know what the maximum relative distance (i.e. maximum tracking error) could be in the worst case scenario: when the planner is actively attempting to evade the tracker.  If we have an upper limit on this bound then we know the maximum tracking error that can occur during the online planning.

 
 

To solve this pursuit-evasion game we first find the relative dynamics between the two systems. We then set a cost function as the distance of the tracking system to the planning system.  The tracking system will try to minimize this cost/distance, and the planning system will do the opposite.  While evolving these optimal trajectories over time, we capture the highest cost that occurs of the time period.  If the tracking system can always eventually reach the planning system, this cost converges to a fixed cost for all time.  That converged cost is the tracking error bound.  For a more thorough explanation of the optimization, please see [1] in the list of related papers below.

 
The value function initializes at the cost function (normed distance to the origin) and evolves over time to convergence

The value function initializes at the cost function (normed distance to the origin) and evolves over time to convergence

 
An example of a 3D set at the slice V = 0.75 meters of the value function. We can think of this as a "candidate" tracking error bound.  As the set evolves we see the initial states we must start in (purple) in order to stay within the desired bound (blue)

An example of a 3D set at the slice V = 0.75 meters of the value function. We can think of this as a "candidate" tracking error bound.  As the set evolves we see the initial states we must start in (purple) in order to stay within the desired bound (blue)

 

Online Planning

In the online phase, we sense obstacles within a given radius and expand these obstacles by the tracking error bound through minkowski addition. Using these padded obstacles, the planner decides the next desired state.  Based on the relative state between the tracker and planner, the optimal control is determined from the look-up table.  The autonomous system executes the optimal control, and the process repeats.  For more details, please see reference [1] in the related papers section below.

 
online_comp.png
 

Related papers

[1] Sylvia Herbert*, Mo Chen*, SooJean Han, Somil Bansal, Jaime F. Fisac, and Claire J .Tomlin, "FaSTrack: a Modular Framework for Fast and Guaranteed Safe Motion Planning." IEEE Conference on Decision and Control, 2017 (submitted). cite