Planning, Fast and Slow: A Framework for Adaptive Real-Time Safe Trajectory Planning

Planning, Fast and Slow:
A Framework for Adaptive Real-Time Safe Trajectory Planning

David Fridovich-Keil*, Sylvia L. Herbert*, Jaime F. Fisac*, Sampada Deglurkar, and Claire J. Tomlin *These authors contributed equally. This research is supported by NSF under the CPS Frontiers VehiCal project (1545126), by the UC-Philippine-California Advanced Research Institute under project IIID-2016-005, and by the ONR MURI Embedded Humans (N00014-16-1-2206). The research of S. Herbert and D. Fridovich-Keil has received funding from the NSF GRFP. S. Herbert is also funded through the UC Berkeley Chancellor’s Fellowship Program. {dfk, sylvia.herbert, jfisac, tomlin},

Motion planning is an extremely well-studied 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 “meta-planning” 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 real-time 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 meta-planning algorithm both in simulation and a hardware experiment using a small Crazyflie 2.0 quadrotor.

I Introduction

Fig. 1: (a) A dynamical system (black, dotted) may not be able to track the output of a geometric planner (blue, solid), resulting in collision with an obstacle. (b) Often planners account for tracking error by augmenting obstacles heuristically, which does not guarantee that the system will deviate from the planned path by less than this margin. (c) Schematic of meta-planner operation using fast (blue, dashed) and slow (red, solid) planners with correspondingly large (blue) and small (red hatched) TEB-augmented obstacles.

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 sampling-based planners such as rapidly-exploring 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(a-b).

In practice, this safety margin is almost always a conservative heuristic chosen by the operator. However, the recently-developed 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 lower-dimensional 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 real-time planning using a fast, potentially low-dimensional planning model, and quickly computable robust optimal tracking of the planned trajectory using a higher-dimensional tracking model.

While FaSTrack makes no significant assumptions about the specific type of low-dimensional 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 difficult-to-track 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 “meta-planning,” 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 “meta-planning” scheme can quickly and safely adapt to the presence of obstacles detected at motion time.

The main contributions of this paper are the aforementioned real-time meta-planning 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. Sampling-based 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, Hamilton-Jacobi (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 meta-planning 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 error-prone. Thinking with the “slow system” is less error-prone, 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 meta-planning 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 real-time 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.111In 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 re-planning. In this section we will define the tracking and planning models and their relation to one another, and present a brief overview of FaSTrack.

Iii-a Tracking Model

The tracking model should be a realistic representation of the real system dynamics, and in general may be nonlinear and high-dimensional. Let represent the state variables of the tracking model. The evolution of the dynamics satisfies the ordinary differential equation (ODE):


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 double-integrator with control and disturbances :


Iii-B 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:


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.

Iii-C 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.


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 double-integrator tracking a 1D point mass as:


Iii-D The FaSTrack Algorithm

The FaSTrack algorithm, explored in detail in [2], consists of both an offline precomputation algorithm and an online algorithm for the real-time 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 collision-checks planned trajectories using a precomputed guaranteed safe TEB. This bound is a safety margin that guarantees robustness despite worst-case 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 worst-case 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 real-time generation of trajectories.

Iv Meta-Planning

Iv-a General Framework

The main contribution to the FaSTrack framework in this paper is the addition of the meta-planning 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 meta-planner is a random tree inspired by RRT-style sampling-based 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 collision-free trajectory, connected to their nearest neighbor in (dashed blue lines). If the fast planner fails to find a collision-free 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 “meta-plan” is found from start to goal, the meta-planner continues building until a user-specified 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.222In 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 meta-planning 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.

Iv-B Offline Reachability Analysis

There are two major components to the offline precomputation for the meta-planner. The first step is in computing the TEB and optimal control look-up 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 double-integrator can remain within despite worst-case 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.

Fig. 2: Invariant set that the double-integrator can remain in despite worst-case disturbance and planning control for the both numerical solution (dotted) and analytic solution (solid)

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 meta-planner 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.

Fig. 3: Example of a Dubins car that must leave its invariant set in order to move into the set. This example illustrates why the switching safety bound may be larger than the tracking error bound.

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 pursuit-evasion 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 worst-case 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 non-anticipative 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 worst-case disturbances. Formally, the BRT is defined here as333Similar definitions of BRTs and their relationships can be found in, for example, [18]


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 look-up table. Continuing our double-integrator 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.

Fig. 4: Visualizations of the numerical (left) and analytic (right) invariant sets for two different planners. Projecting these sets onto the position dimension yields a rectangular TEB. The safe switching set to transition from large to small invariant sets is also shown. Projection onto position similarly provides the SSB.

Iv-C Online Meta-Planning

At runtime, the meta-planner 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 re-planning whenever new information about the environment becomes available, i.e. when obstacles are detected.

The precomputed safety sets allow the meta-planner to reason quickly about dynamic tracking feasibility as it builds . Using the precomputed TEBs, the meta-planner can determine what planners are safe to use in different regions of the environment. In addition, the SSBs allow the meta-planner to decide on the validity of planner-to-planner transitions. The meta-planner’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 mid-trajectory, 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 meta-planner 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 meta-planner always tries to connect waypoints using the fastest planner , which is also associated to the largest TEB. If does not find a collision-free trajectory, the meta-planner then attempts to use the second-fastest planner , which has a smaller TEB. The meta-planner 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 meta-planner 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 meta-planner 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 meta-planner does this is by checking what planner was used to reach ’s parent , and if , using the safe switching bound to collision-check the already-computed 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 more-slowly-reached is a new node in , and is added to as a child of this new node.

If the check is unsuccessful, the meta-planner does not add to the tree. At this point, different logics are possible. We describe them briefly here noting their practical implications:

  1. [a)]

  2. Discard: is discarded and the meta-planner moves on to sample a new candidate point.

  3. Recursive Virtual Backtrack: the meta-planner 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.

Fig. 5: Illustration of the online meta-planning algorithm.

One alternative option for handling planner-switching 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 tracker-planner dynamics, namely on how much larger is than .

Remark 1

In the case of a point-mass 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 meta-planner algorithm can be safely followed by the tracking system. {proof} The proof is by construction of the meta-planner, based on FaSTrack guarantees; we provide an outline here. A point is only added to the meta-planning 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, re-planning succeeds (at worst, a geometric planner can always reverse or stop) in time for the system to switch to the new plan before colliding.

(a) LQR controller.
(b) Optimal FaSTrack controller.
Fig. 6: Simulated autonomous flight in a cluttered environment. Notice that when using LQR control the quadrotor leaves the TEB, but under optimal control it remains within the TEB. This is particularly important in the vicinity of obstacles.

We demonstrate our algorithm on a 6D near-hover 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 tracking444Note 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. IV-C (tracker on the left, planner on the right):


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:


There is assumed to be an external acceleration disturbance in each dimension with bounds

Fig. 7: TEB vs. planner speed in each subsystem.

Note that Eq. 9 can be split into three 2D subsystems with states and that are of the same form as the double-integrator example fro Sec. IV-B. Since the double-integrator 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. IV-B, the TEB for is identical to the SSB for switching from .

Iv-D Simulation

We implemented the meta-planning 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 meta-planner’s internal environment model and used during re-planning.

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. IV-B. 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.

Iv-E 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 meta-plan.

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.

Fig. 8: A Crazyflie 2.0 flying during our hardware experiment. Two OptiTrack cameras are visible in the background.
Fig. 9: Position vs. time for in each spatial dimension recorded during a hardware experiment. Note that the TEB changes size, indicating a planner switch.

V Conclusions

We have proposed a novel meta-planning algorithm for using FaSTrack with multiple planners. The algorithm adaptively selects the planner with the largest error bound that finds locally collision-free 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 rapidly-exploring random trees and moving at different top speeds.

We are excited to explore several future directions of meta-planning 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.


  • [1] S. M. LaValle, “Rapidly-exploring 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:
  • [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 collision-free 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 real-time 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 Multi-Vehicle 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. 1-3, pp. 361–395, 1991.
  • [13] S. Milli, F. Lieder, and T. L. Griffiths, “When does bounded-optimal 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 sampling-based 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 capture-the-flag,” in Proc. IEEE Int. Conf. Robotics and Automation, 2011, pp. 1451–1456.
  • [16] M. Chen, Z. Zhou, and C. J. Tomlin, “Multiplayer Reach-Avoid 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 time-dependent Hamilton-Jacobi 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, “Reach-avoid problems with time-varying 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*): Sampling-based 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 open-source 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,
Comments 0
Request Comment
You are adding the first comment!
How to quickly get a good reply:
  • Give credit where it’s due by listing out the positive aspects of a paper before getting into which changes should be made.
  • Be specific in your critique, and provide supporting evidence with appropriate references to substantiate general statements.
  • Your comment should inspire ideas to flow and help the author improves the paper.

The better we are at sharing our knowledge with each other, the faster we move forward.
The feedback must be of minimum 40 characters and the title a minimum of 5 characters
Add comment
Loading ...
This is a comment super asjknd jkasnjk adsnkj
The feedback must be of minumum 40 characters
The feedback must be of minumum 40 characters

You are asking your first question!
How to quickly get a good answer:
  • Keep your question short and to the point
  • Check for grammar or spelling errors.
  • Phrase it like a question
Test description