Planning, Fast and Slow:
A Framework for Adaptive
RealTime Safe Trajectory Planning
Abstract
Motion planning is an extremely wellstudied problem in the robotics community, yet existing work largely falls into one of two categories: computationally efficient but with few if any safety guarantees, or able to give stronger guarantees but at high computational cost. This work builds on a recent development called FaSTrack in which a slow offline computation provides a modular safety guarantee for a faster online planner. We introduce the notion of “metaplanning” in which a refined offline computation enables safe switching between different online planners. This provides autonomous systems with the ability to adapt motion plans to a priori unknown environments in realtime as sensor measurements detect new obstacles, and the flexibility to maneuver differently in the presence of obstacles than they would in free space, all while maintaining a strict safety guarantee. We demonstrate the metaplanning algorithm both in simulation and a hardware experiment using a small Crazyflie 2.0 quadrotor.
I Introduction
The navigation of autonomous dynamical systems through cluttered environments is a hard problem, particularly when there is a need for both speed and safety. Often, elements of the environment (such as obstacle locations) are also unknown a priori, further complicating the problem. Many popular methods exist for planning trajectories in such scenarios, but a common challenge lies in accounting for dynamic feasibility in real time while providing a safety guarantee. Some of the most common approaches in this space are samplingbased planners such as rapidlyexploring random trees (RRT) [1]. Typically, these planners fall into one of two categories: geometric planners only attempt to find a path the system can take from its current position to the goal, while kinodynamic planners find a dynamically feasible trajectory, i.e. a path with associated time stamps that adheres to some known system dynamics.
Since the output of a geometric planner is not usually dynamically feasible, a common practice is to apply a feedback controller, e.g. a linear quadratic regulator (LQR), to attempt to track a geometric plan. Since the controller will not follow the plan perfectly, geometric plans are usually generated by assuming a conservative safety margin. This idea is illustrated in Fig. 1(ab).
In practice, this safety margin is almost always a conservative heuristic chosen by the operator. However, the recentlydeveloped Fast and Safe Tracking (FaSTrack) algorithm provides a rigorous way to precompute a safety margin offline given a model of the true system dynamics [2] while using a lowerdimensional model for online planning. In the FaSTrack algorithm, a guaranteed maximum possible tracking error is computed between the tracking system model and the planning model. This tracking error bound (TEB) can also accommodate deviations due to external disturbances such as wind and time delays. The TEB is used to expand obstacles by a margin that guarantees safety. The offline precomputation also provides a computationally efficient robust optimal controller that maps the relative state between the tracking system and the planned trajectory at any given time to the most effective control action for the tracking system to remain within the TEB. Hence, the online algorithm involves realtime planning using a fast, potentially lowdimensional planning model, and quickly computable robust optimal tracking of the planned trajectory using a higherdimensional tracking model.
While FaSTrack makes no significant assumptions about the specific type of lowdimensional planner, in this work we focus our attention on geometric planners operating in with a fixed maximum speed in each dimension. We emphasize that this restriction is purely pedagogical; like FaSTrack, our contributions are much more general and easily extend to more complex planning models.
One key drawback of FaSTrack is that the TEB can be overly conservative if the system is tracking a particularly difficulttotrack planner. In this paper we propose an extra layer to the core algorithm that allows combining multiple planning models with different maximum speeds, and hence different TEBs. We call this process “metaplanning,” and it effectively generates a tree of trajectories that switch between “faster” and “slower” planners, as illustrated in Fig. 1(c). Faster planners are able to navigate through the environment quickly, but their larger TEBs prevent them from threading narrow passages between obstacles. Slower planners take more time to traverse the environment, but the correspondingly smaller TEBs allow them to maneuver more precisely near obstacles. By adaptively selecting the planner in real time, our algorithm can trade off between speed of navigation and size of the TEB. Crucially, our “metaplanning” scheme can quickly and safely adapt to the presence of obstacles detected at motion time.
The main contributions of this paper are the aforementioned realtime metaplanning algorithm for Fast and Safe Tracking, a constructive proof of safety, and a demonstration of the full algorithm both in simulation and hardware using a small quadrotor vehicle.
Ii Related Work
Robust motion planning and trajectory optimization have been active areas of research in recent years. However, navigation that is both robust and fast is still a challenge. Samplingbased motion planners can be very computationally efficient, but attempts to make them robust are generally heuristic. Other techniques for online dynamic navigation include model predictive control (MPC), which is extremely useful, particularly for linear systems. MPC is harder to use in real time for nonlinear systems due to the computational costs of solving for dynamic trajectories, though work to speed up computation is ongoing [3, 4]. Robustness can be achieved in linear systems [5, 6], and there is work on making MPC for nonlinear systems robust by using algorithms based on minimax formulations and tube MPCs that bound output trajectories with a tube around a nominal path (see [7] for references).
There are other techniques for robust navigation that take advantage of precomputation. Safety funnels can be constructed around motion primitives that can then be pieced together in real time [8]. Given a precomputed nominal dynamically feasible trajectory, contraction mapping can be used to make this nominal trajectory more robust to external disturbances in real time [9]. Finally, HamiltonJacobi (HJ) reachability analysis has been used for offline robust trajectory planning in fully known environments, providing guaranteed tracking error bounds under external disturbances [10].
The metaplanning aspect of this paper was inspired by behavioral economist Daniel Kahneman’s Nobel Prize winning work on “fast” (intuitive) and “slow” (deliberative) modes of cognitive function in the brain [11]. Thinking with the “fast system” is efficient, but more errorprone. Thinking with the “slow system” is less errorprone, but is slower. The brain adaptively chooses which mode to be in to operate efficiently while minimizing error in scenarios where error can be disastrous. This act of deciding how much cognitive effort to expend for a given task is called metareasoning [12], and can be useful for robotics. It may be desirable for a robot to plan and move swiftly whenever possible, but to operate more carefully when approaching a challenging region in the environment. Research in psychology has suggested that selecting between a limited number of discrete cognitive modes is computationally advantageous [13], which inspires the use of discrete set of faster and slower planners in our metaplanning algorithm. Our algorithm is able to trade off planner speed and tracking conservativeness in a modular way while providing a strong theoretical safety guarantee.
Iii Background
The FaSTrack algorithm can be used to plan and track a trajectory online and in real time. The realtime planning is done using a set of kinematic or dynamic planning models, and the physical system is represented by a dynamic tracking model that will attempt to follow the current planning model. The environment can contain static a priori unknown obstacles provided they can be observed by the system within a limited sensing range.^{1}^{1}1In order to provide safety guarantees, the minimum allowable sensing distance in any dimension is twice the TEB in that dimension added to the largest spatial movement the planner can make during replanning. In this section we will define the tracking and planning models and their relation to one another, and present a brief overview of FaSTrack.
Iiia Tracking Model
The tracking model should be a realistic representation of the real system dynamics, and in general may be nonlinear and highdimensional. Let represent the state variables of the tracking model. The evolution of the dynamics satisfies the ordinary differential equation (ODE):
(1)  
The trajectories of (1) will be denoted as , where and . Under standard technical assumptions [2], these trajectories will satisfy the initial condition and the ODE (1) almost everywhere. For a running example we will consider a tracking model of a simple doubleintegrator with control and disturbances :
(2) 
IiiB Planning Model
The planning model defines the class of trajectories generated by the motion planner. Let represent the state variables of the planning model, with control . The planning states are a subset of the tracking states . FaSTrack is agnostic to the type of planner, as long it can be represented using a kinematic or dynamic model as follows:
(3) 
This paper focuses on geometric planners; however, note that although geometric planners may not directly use a dynamical model, the paths they generate can be described by a point with direct velocity control. For example, a 1D geometric planner could be described as a point moving with a direct velocity controller: . Also note that the planning model does not need a disturbance input. The treatment of disturbances is only necessary in the tracking model, which is modular with respect to any planning method, including those that do not account for disturbances.
IiiC Relative Dynamics
The FaSTrack algorithm relies on using the relative dynamics between the tracking and planning models. The relative system is found by fixing the planning model to the origin and finding the dynamics of the tracking model relative to the planning model, as shown below.
(4) 
where is matrix that matches the common states of and by augmenting the state space of the planning model. The relative states now represent the tracking states relative to the planning states. Using our tracking and planning model examples from above we can define the relative dynamics of a doubleintegrator tracking a 1D point mass as:
(5) 
IiiD The FaSTrack Algorithm
The FaSTrack algorithm, explored in detail in [2], consists of both an offline precomputation algorithm and an online algorithm for the realtime navigation of a nonlinear dynamic autonomous system through an a priori unknown environment with static obstacles.
In the online algorithm, the system senses obstacles in the environment and collisionchecks planned trajectories using a precomputed guaranteed safe TEB. This bound is a safety margin that guarantees robustness despite worstcase bounded disturbances. The relative state between the tracking system and the planning system is computed at each time step. Based on this relative state, a hybrid tracking controller outputs a control signal to the autonomous system. In this paper the hybrid controller always uses the precomputed safety controller. This safety controller is computed offline via HJ reachability analysis, and guarantees that the TEB is not violated despite worstcase bounded disturbances. Note that the TEB and safety controller depend only on the current relative state between the tracking system and planning system, and is agnostic to the absolute states of the autonomous system in the environment. This cycle is repeated until the autonomous system reaches its goal.
To precompute the TEB and the safety controller, the relative dynamics are used in a HJ reachability computation to determine the tracking error function over a discretized state space. The minimum level set of this function represents the smallest invariant set that the tracker can stay within relative to the planner, and its projection onto the position/configuration space is the TEB. The gradients of this function inform the safety controller. These functions depend only on the relative dynamics and not absolute states or dynamics of the systems at execution time. This is what allows FaSTrack to be used for realtime generation of trajectories.
Iv MetaPlanning
Iva General Framework
The main contribution to the FaSTrack framework in this paper is the addition of the metaplanning algorithm to choose between a selection of planners with different maximum speeds and hence different TEBs at runtime. We first assume that planners are sorted in order of decreasing speed and hence TEB size, and that the overall objective is to minimize the time to reach a specified goal point. This objective implies a preference for planners that can move faster, but also for planners that can safely navigate a more direct route even if they must do so at lower velocity.
The core of the metaplanner is a random tree inspired by RRTstyle samplingbased planners [1], as shown in Fig. 1. The obstacles are shown in blue, and are augmented by the TEBs for two different planners. As in RRT, waypoints in are sampled randomly from the environment. If the fast planner (with the large blue TEB) finds a collisionfree trajectory, connected to their nearest neighbor in (dashed blue lines). If the fast planner fails to find a collisionfree path, then the slow planner (with smaller red dashed TEB and solid red lines) attempts to connect to the nearest neighbor. Upon success, the waypoint is inserted into , along with the trajectory generated by the planner to reach that waypoint from the nearest neighbor, and the associated optimal controller to remain inside the TEB. If a waypoint is successfully inserted near the goal, a similar process ensues to attempt to find a trajectory between it and the goal point.
Once a valid “metaplan” is found from start to goal, the metaplanner continues building until a userspecified maximum runtime has elapsed, always retaining the best (shortest time) sequence of waypoints to the goal. However, similar to Informed RRT* [14], we reject samples which the fast planner could not reach (in straight lines from/to start and goal) in less than the time associated with the best available trajectory.^{2}^{2}2In Informed RRT*, planner velocities lie on the sphere leading to an elliptical geometry for valid samples. Since we assume a maximum speed in each dimension, valid samples lie in a distorted rhombicuboctahedron.
The key to metaplanning lies in ensuring safe switching between planners. This guarantee requires an offline computation to determine a safety margin for switching into successively slower planners (with smaller TEBs), as well as an optimal switching control law. Online, we must be sure to plan with the appropriate safety margin at each step, and to “backtrack” if we detect the need for a switch to a slower planner. We will next explore the offline and online steps in detail.
IvB Offline Reachability Analysis
There are two major components to the offline precomputation for the metaplanner. The first step is in computing the TEB and optimal control lookup tables for each planner. This is done following the normal FaSTrack precomputation algorithm [2]. Using the relative dynamics from (5) we show the level set of the value function representing the set of states that the doubleintegrator can remain within despite worstcase disturbance and planning control in Fig. 2. The projection of this set onto the position dimension comprises the TEB. Note that in this particular example an analytic solution can be found as the intersection of two parabolas defined by (6), where the overbars represent the maximum value of the parameters. This analytic solution is also shown in Fig. 2.
(6) 
The second major component in the offline computation is in computing the switching bounds and optimal control between each successive pair of planners. In the online algorithm, transitioning from a planner with a smaller TEB to a larger TEB is simple; by construction the autonomous system is already within the larger error bound, and the metaplanner only allows for planners that would not be in collision with obstacles. However, switching from a larger TEB to a smaller one is more complicated.
To transition from a larger error bound to a smaller one we must ensure that the relative state between the autonomous system and the planned path is within the smaller TEB (e.g. the autonomous system is close enough to the planned path to switch into a smaller “safety bubble”). FaSTrack provides the optimal control for staying within the larger tracking error bound, but does not necessarily provide the controller and bound required for moving closer to the planned path and reducing the tracking error. It is possible that in order to move closer, the system must first exit the larger bound. For general intuition on why a system might have to leave a given invariant set in order to move into the set, see Fig. 3. In this figure, a Dubins car moving at a fixed speed is remaining within radius R of the origin by turning at its maximum rate of rotation. The car cannot move closer to the origin while remaining inside of the circle because it is already at its maximum turning rate. In order for the car to reduce its distance to the origin, it must first exit the original circle to reorient itself towards the origin.
In general we must precompute the set of states that the system may enter when transitioning from a larger error bound to a smaller one, and the optimal control to achieve this transition. To do this we use HJ reachability analysis.
HJ reachability analysis provides a rigorous method for analyzing the liveness of a system, and can be used to determine the backward reachable tube (BRT): the set of all allowable initial states of a system such that the system can enter a set of goal states within a given time period. HJ reachability analysis can also be used in the context of a pursuitevasion game [15, 16]. Here we assume there is such a game between the tracking system and the planning system. In this game, the tracking system will try to “capture” the planning system, while the planning system is doing everything it can to avoid capture. In reality the planner is typically not actively trying to avoid the tracking system, but this allows us to account for worstcase scenarios. We want to determine the set of states that the tracking system may enter when transitioning from the value function level set associated with the larger TEB to the value function level set associated with the smaller TEB.
Before constructing the differential game we must first determine the method each player must use for making decisions. We define a strategy for planning system as the mapping that determines a control for the planning model based on the control of the tracking model. We restrict to draw from only nonanticipative strategies , as defined in [17]. We similarly define the disturbance strategy , .
For the system in the form of (4), we would like to compute the BRT of time horizon , denoted . Intuitively, is the set of states from which there exists a control strategy to drive the system into a target set within a duration of despite worstcase disturbances. Formally, the BRT is defined here as^{3}^{3}3Similar definitions of BRTs and their relationships can be found in, for example, [18]
(7)  
Standard HJ formulations exist for computing the BRT in the full dimensionality [19, 17, 20, 21], and for decomposable systems as in [22]. Here the goal is the set of states represented by the smaller tracking error bound. Using the relative dynamics between the tracking model and the planning model associated with the smaller TEB, we evolve this set backwards in time. We stop the computation when the tube contains the set of states associated with the larger tracking error bound. This BRT represents the set of states from which the system will enter the small tracking error bound, as well as the states that the trajectories may enter along the way. By projecting this set down to position dimensions we have the switching safety bound (SSB). The gradients of the resulting value function from this computation inform the switching controller lookup table. Continuing our doubleintegrator example, Fig. 4a shows the level set of the value functions associated with the larger and smaller TEBs. The black set represents the level set of the value function associated with the SSB. The same information computed analytically can be seen in Fig. 4b.
IvC Online MetaPlanning
At runtime, the metaplanner is in charge of constructing and maintaining a tree, , of waypoints, connected via trajectories generated by the set of available planners. It is also responsible for replanning whenever new information about the environment becomes available, i.e. when obstacles are detected.
The precomputed safety sets allow the metaplanner to reason quickly about dynamic tracking feasibility as it builds . Using the precomputed TEBs, the metaplanner can determine what planners are safe to use in different regions of the environment. In addition, the SSBs allow the metaplanner to decide on the validity of plannertoplanner transitions. The metaplanner’s logic is detailed below and illustrated in Fig. 5.
Step 0: Root. The root node of is originally set at the starting position of the tracking system. Since the system has an initial tracking error equal to zero, it is by definition inside of all the available TEBs. Later, if an obstacle has just been detected midtrajectory, the new root node will be placed at the predicted position of the planning system after some allowed computation time (typically s) and the tracking system will only be guaranteed to be inside the TEB associated to the current edge of .
Step 1: Sample. The metaplanner constructs its tree by sequentially sampling points uniformly at random from the environment and attempting to connect them to the nearest existing waypoint in the tree.
Step 2: Plan. By default, the metaplanner always tries to connect waypoints using the fastest planner , which is also associated to the largest TEB. If does not find a collisionfree trajectory, the metaplanner then attempts to use the secondfastest planner , which has a smaller TEB. The metaplanner keeps cycling through available planners in order of decreasing TEB size until one succeeds or all have failed (in which case it abandons this candidate waypoint and samples a new one).
Step 3: Virtual Backtrack. When a planner succeeds in reaching a new point from the nearest waypoint , the metaplanner checks what planner was previously used to reach waypoint from its parent . If this preceding planner had a larger TEB than the new planner (that is, if ), then cannot be immediately added to . Instead, the metaplanner first needs to ensure that the tracking system will be able to safely transition into before reaching , so that it can then track ’s plan from to while remaining inside its TEB. The metaplanner does this is by checking what planner was used to reach ’s parent , and if , using the safe switching bound to collisioncheck the alreadycomputed path . If , there is no need to use a SSB and the path is guaranteed to be safe under , since it was already deemed safe under the larger by .
If the check is successful, this means that, instead of getting from to tracking the faster planner , the system can follow an alternative trajectory, skipping altogether and transitioning from the speed of to the speed of . This path is added to as an alternative to the original path: the moreslowlyreached is a new node in , and is added to as a child of this new node.
If the check is unsuccessful, the metaplanner does not add to the tree. At this point, different logics are possible. We describe them briefly here noting their practical implications:

[a)]

Discard: is discarded and the metaplanner moves on to sample a new candidate point.

Recursive Virtual Backtrack: the metaplanner marks as a waypoint that needs to be reached from its parent using a slower planner than the original , so that safe transition into will be possible (in particular, this will always be the case if is reached using , since is safe under ). Step 3 can then be repeated on , and recursively applied (at worst) until the root of is reached.
One alternative option for handling plannerswitching failures is to prevent them altogether by always using SSBs instead of TEBs for the planning in Step 2. In particular, replacing with will ensure that planners will only attempt to add a candidate point to the tree if it would not only be possible to reach under this planner but also, if later deemed necessary, to do so while transitioning to the smallest TEB (so that subsequent nodes can be connected to it by any planner without the need for the backtracking verification in Step 3). The additional conservativeness introduced by this substitution depends on the relative trackerplanner dynamics, namely on how much larger is than .
Remark 1
In the case of a pointmass tracking model following a kinematic planner, we have , , and therefore this substitution does not need to be done explicitly nor does it introduce any additional conservativeness. The backtracking check in Step 3 is always guaranteed to succeed.
Proposition 1
Any plan generated by the metaplanner algorithm can be safely followed by the tracking system. {proof} The proof is by construction of the metaplanner, based on FaSTrack guarantees; we provide an outline here. A point is only added to the metaplanning tree if there exists a sequence of planned trajectories that reach the point such that (a) each planned trajectory can be tracked by the system with an error bounded by the associated TEB, and is clear of known obstacles by at least TEB, (b) each transition between planners can be followed by the system with an error bounded by the corresponding SSB, and is clear of known obstacles by at least SSB, and (c) if new obstacles are detected, replanning succeeds (at worst, a geometric planner can always reverse or stop) in time for the system to switch to the new plan before colliding.
We demonstrate our algorithm on a 6D nearhover quadrotor model tracking a suite of 3D geometric planners running BIT* [23] in the cluttered environment depicted in Fig. 6 with different maximum speeds in each dimension. The tracking^{4}^{4}4Note that we have assumed a zero yaw angle for the quadrotor. This is enforced experimentally by using a strict feedback controller on yaw rate to regulate yaw to zero. and planning models (for the planner ) are given below in Eq. IVC (tracker on the left, planner on the right):
(8) 
Here and correspond to roll, pitch, and thrust. In all experiments, we set and . Planner ’s controls are , each representing a fixed maximum speed in the given dimension.
The relative dynamics between the tracking and planning models are:
(9) 
There is assumed to be an external acceleration disturbance in each dimension with bounds
Note that Eq. 9 can be split into three 2D subsystems with states and that are of the same form as the doubleintegrator example fro Sec. IVB. Since the doubleintegrator affords an analytic solution for all quantities of interest (TEB, SSB, optimal controllers, etc.) we skip the offline computation step and simply compute these quantities in closed form online. Fig. 7 shows the growth of in each subsystem’s position state as the planner speed in that dimension increases. Moreover, as explained in Sec. IVB, the TEB for is identical to the SSB for switching from .
IvD Simulation
We implemented the metaplanning online algorithm within the robot operating system (ROS) [24] framework. We used the BIT* [23] geometric planner from the Open Motion Planning Library (OMPL) [25]. Code is written in C++ and will be available as an open source ROS package.
Figure 6 shows a snapshot of a simulated autonomous quadrotor flight in an artificial environment with spherical obstacles using trajectories generated by our algorithm. Initially, the obstacle locations and sizes are unknown to the quadrotor, but as soon as they come within the sensing radius (the size of which must adhere to the constraint discussed in Sec. III) they are added to the metaplanner’s internal environment model and used during replanning.
In Fig. 5(a) we show what happens when the tracking controller is a standard LQR controller, while in Fig. 5(b) everything remains the same except that we apply the optimal controllers derived from the offline analysis in Sec. IVB. Note that the LQR controller makes no guarantee about staying within the TEB, and hence it is unable to remain inside the TEB in the vicinity of the obstacle. The optimal controller, conversely, is guaranteed to remain in the TEB.
IvE Experiment
We replicated the simulation on a hardware testbed using the Crazyflie 2.0 open source quadrotor platform, shown in Fig. 8. We obtained position and orientation measurements at Hz from an OptiTrack infrared motion capture system. Given state estimates, we send control signals over a radio to the quadrotor at 100 Hz. As shown in our video submission, the quadrotor successfully avoids the virtual obstacles while remaining inside the TEB for each planner along the metaplan.
Figure 9 shows the quadrotor’s position over time recorded during a hardware experiment. Note that the quadrotor stays well within the TEB throughout the flight. Also note that the TEB changes size, indicating a planner switch.
V Conclusions
We have proposed a novel metaplanning algorithm for using FaSTrack with multiple planners. The algorithm adaptively selects the planner with the largest error bound that finds locally collisionfree paths. This allows using more aggressive planners in open areas, defaulting to more cautious planners in cluttered environments. We introduced a method for guaranteed safe online switching between different planners. We demonstrated the algorithm in simulation and experimentation for a quadrotor vehicle tracking a reference generated using rapidlyexploring random trees and moving at different top speeds.
We are excited to explore several future directions of metaplanning with FaSTrack in the future, including integration with the Open Motion Planning Library (OMPL) and other planning libraries to allow for a greater variety of planning method. Other promising directions include providing adaptable error bounds based on external disturbances, and updating the tracking error bound based on learned information about the tracking model.
References
 [1] S. M. LaValle, “Rapidlyexploring random trees: A new tool for path planning,” 1998.
 [2] S. Herbert*, M. Chen, S. Han, S. Bansal, J. Fisac, and C. J. Tomlin, “Fastrack: a modular framework for fast and guaranteed safe motion planning,” IEEE Conference on Decision and Control, to appear, 2017. [Online]. Available: https://arxiv.org/pdf/1703.07373.pdf
 [3] M. Diehl, H. J. Ferreau, and N. Haverbeke, “Efficient numerical methods for nonlinear mpc and moving horizon estimation,” in Nonlinear Model Predictive Control, 2009, pp. 391–417.
 [4] M. Neunert, C. de Crousaz, F. Furrer, M. Kamel, F. Farshidian, R. Siegwart, and J. Buchli, “Fast nonlinear model predictive control for unified trajectory optimization and tracking,” in Proc. IEEE Int. Conf. Robotics and Automation, 2016.
 [5] A. Richards and J. P. How, “Robust variable horizon model predictive control for vehicle maneuvering,” Int. J. Robust and Nonlinear Control, vol. 16, no. 7, pp. 333–351, 2006.
 [6] S. Di Cairano and F. Borrelli, “Reference tracking with guaranteed error bound for constrained linear systems,” IEEE Trans. Automatic Control, vol. 61, no. 8, pp. 2245–2250, 2016.
 [7] M. Hoy, A. S. Matveev, and A. V. Savkin, “Algorithms for collisionfree navigation of mobile robots in complex cluttered environments: a survey,” Robotica, vol. 33, no. 03, pp. 463–497, 2015.
 [8] A. Majumdar and R. Tedrake, “Funnel libraries for realtime robust feedback motion planning,” Int. J. Robotics Research, pp. 947–982, Jun. 2017.
 [9] S. Singh, A. Majumdar, J.J. Slotine, and M. Pavone, “Robust online motion planning via contraction theory and convex optimization,” in Proc. IEEE Int. Conf. Robotics and Automation, 2017.
 [10] S. Bansal, M. Chen, J. F. Fisac, and C. J. Tomlin, “Safe Sequential Path Planning of MultiVehicle Systems Under Presence of Disturbances and Imperfect Information,” Proc. American Control Conference, 2017.
 [11] D. Kahneman, Thinking, fast and slow. Macmillan, 2011.
 [12] S. Russell and E. Wefald, “Principles of metareasoning,” Artificial intelligence, vol. 49, no. 13, pp. 361–395, 1991.
 [13] S. Milli, F. Lieder, and T. L. Griffiths, “When does boundedoptimal metareasoning favor few cognitive systems?” in AAAI, 2017, pp. 4422–4428.
 [14] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed rrt*: Optimal samplingbased path planning focused via direct sampling of an admissible ellipsoidal heuristic,” in Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on. IEEE, 2014, pp. 2997–3004.
 [15] H. Huang, J. Ding, W. Zhang, and C. Tomlin, “A differential game approach to planning in adversarial scenarios: A case study on capturetheflag,” in Proc. IEEE Int. Conf. Robotics and Automation, 2011, pp. 1451–1456.
 [16] M. Chen, Z. Zhou, and C. J. Tomlin, “Multiplayer ReachAvoid Games via Pairwise Outcomes,” IEEE Trans.Automatic Control, vol. 62, no. 3, pp. 1451–1457, Mar. 2017.
 [17] I. Mitchell, A. Bayen, and C. Tomlin, “A timedependent HamiltonJacobi formulation of reachable sets for continuous dynamic games,” IEEE Trans. Automatic Control, vol. 50, no. 7, pp. 947–957, 2005.
 [18] I. M. Mitchell, “Comparing forward and backward reachability as tools for safety analysis,” in Proc. ACM Int. Conf. on Hybrid Systems: Computation and Control, 2007.
 [19] E. Barron, “Differential Games with Maximum Cost,” Nonlinear analysis: Theory, methods & applications, pp. 971–989, 1990.
 [20] O. Bokanowski, N. Forcadel, and H. Zidani, “Reachability and minimal times for state constrained nonlinear problems without any controllability assumption,” SIAM J. on Control and Optimization, pp. 1–24, 2010.
 [21] J. F. Fisac, M. Chen, C. J. Tomlin, and S. S. Sastry, “Reachavoid problems with timevarying dynamics, targets and constraints,” in Proc. ACM Int. Conf. Hybrid Systems: Computation and Control, 2015.
 [22] M. Chen, S. L. Herbert, M. S. Vashishtha, S. Bansal, and C. J. Tomlin, “Decomposition of reachable sets and tubes for a class of nonlinear systems,” arXiv preprint arXiv:1611.00122, 2016.
 [23] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Batch informed trees (bit*): Samplingbased optimal planning via the heuristically guided search of implicit random geometric graphs,” in Robotics and Automation (ICRA), 2015 IEEE International Conference on. IEEE, 2015, pp. 3067–3074.
 [24] M. Quigley, K. Conley, B. P. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng, “Ros: an opensource robot operating system,” in ICRA Workshop on Open Source Software, 2009.
 [25] I. A. Şucan, M. Moll, and L. E. Kavraki, “The Open Motion Planning Library,” IEEE Robotics & Automation Magazine, vol. 19, no. 4, pp. 72–82, December 2012, http://ompl.kavrakilab.org.