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 key 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 (RRTs) [lavalle1998rapidly]. Typically, these planners fall into one of two broad 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 an ad hoc 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) framework [Herbert2017] provides a rigorous way to precompute a safety margin offline, given a model of the true system dynamics and a (possibly lowerdimensional) model of the online planner’s dynamics. In the FaSTrack framework, 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 safety 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 the robot’s configuration space. We observe that the resulting geometric paths can be interpreted as kinematic trajectories with a fixed maximum speed in each dimension. We emphasize that the restriction to geometric planners is pedagogical; like FaSTrack, our proposed metaplanning approach is more general and extends 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 planning model. In this paper we propose an extra layer to the core framework 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” planning models, as illustrated in Fig. 1(c). Faster planning models are able to navigate through the environment quickly, but their larger TEBs prevent them from threading narrow passages between obstacles. Slower planning models take more time to traverse the environment, but the correspondingly smaller TEBs allow them to maneuver more precisely near obstacles. By adaptively selecting the planning model in real time, our framework 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 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 [Diehl2009, Neunert2016]. Robustness can be achieved in linear systems [Richards2006, DiCairano2016], 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 [Hoy2015] 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 [Majumdar2017]. 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 [Singh2017]. 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 [Bansal2017].
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 [kahneman2011thinking]. Thinking with the “fast system” is efficient, but more errorprone. Thinking with the “slow system” is less errorprone, but 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 [russell1991principles], 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 [milli2017does], which inspires the use of discrete set of faster and slower planning models in our metaplanning algorithm. Our algorithm is able to trade off planner velocity and tracking conservativeness in a modular way while providing a strong theoretical safety guarantee.
Iii Background
The FaSTrack framework 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 direction is the length of the TEB’s projection onto that direction, added to the largest distance the current planning reference could move while a new metaplan is generated. 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 [Herbert2017], 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. 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: . Note that the planning model does not need a disturbance input. Disturbances need only be considered in the tracking model and not the planning model, since the latter only exists in the abstract as a reference for the former.
IiiC Relative Dynamics
The FaSTrack framework relies on using the relative dynamics between the tracking and planning models. The relative system may be derived by lifting the planner’s state from to and subtracting:
(4) 
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 dynamics of a doubleintegrator tracking a 1D point mass as:
(5) 
IiiD The FaSTrack Framework
The FaSTrack framework, explained in detail in [Herbert2017], consists of both an offline precomputation algorithm and an online planning algorithm. Together, these allow a nonlinear dynamic system to navigate through an a priori unknown environment with static obstacles, safely and in real time.
Offline, FaSTrack computes a tracking error bound (TEB) and a safety controller to stay inside this bound. The TEB is a safety margin that, when using the safety controller, guarantees robust tracking despite worstcase planner behavior and bounded disturbances. The safety controller operates on the relative state between tracker and planner, and is computed offline via HJ reachability analysis in free space. This is possible because the relative dynamics do not depend on the absolute state of the tracking system in the environment. Since the tracker will always remain inside the TEB, as long as the TEB never intersects any obstacles, the free space relative dynamics will always apply.
Online, both at the start and whenever a new obstacle is sensed, an offtheshelf planning algorithm—equipped with the precomputed TEB for collisionchecking—generates a new trajectory. The tracking system may then apply the precomputed safety controller to track this planned trajectory in real time.
Iv MetaPlanning
Iva General Framework
In this work, we use the term planner to denote the conjunction of a planning algorithm and an associated planning model that it uses to generate timed trajectories.This paper’s main contribution to the FaSTrack framework is the introduction of a 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 maximum 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 speed.
The core of the metaplanner is a random tree inspired by RRTstyle samplingbased planners [lavalle1998rapidly], as shown in Fig. 1.^{2}^{2}2The choice of a tree topology is for convenience; any directed graph including the robot’s current state would suffice. The obstacles are shown in black, and are augmented by the TEBs for two different planners. As in RRT, waypoints in are sampled randomly from the environment and (potentially) connected with their nearest neighbor in . If the fast planner^{3}^{3}3Note that by a faster planner we mean one with a higher associated maximum velocity, rather than smaller computation time. (with the large blue TEB) finds a collisionfree trajectory, the connection is established (dashed blue lines). Otherwise, the slow planner (with smaller red striped 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 safety 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. Similar to Informed RRT* [gammell2014informed], the metaplanner immediately rejects samples which could not possibly improve upon the best available trajectory.^{4}^{4}4In 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 a safe 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 to compute the TEB and safety control lookup tables for each planner. This is done following the standard FaSTrack precomputation algorithm [Herbert2017]. Fig. 2 shows the set of relative states in the subsystem that the tracker can remain within despite worst case planner behavior and external disturbance. The projection of this controlled invariant set onto the position axis comprises the TEB. For the doubleintegrator dynamics in (5), an analytic solution can also be found by applying the equations of constantacceleration motion under the worstcase disturbance and the best associated control effort. The analytic controlled invariant set, consisting of two parabolic curves, is superimposed in Fig. 2. Such analytic solutions do not exist in general.
The second major component of the offline precomputation is to find the corresponding tracking bound and optimal controller for transitioning between planners. For the dynamics in (5), switching from a planner with a small TEB to one with a large TEB is safe by construction, because the large TEB contains the small TEB. Switching from a large TEB to a small one is more complicated.
To transition from a large TEB to a small TEB we must ensure that the relative state between the autonomous system and the planned path is within the small TEB by the time of the planner switch. FaSTrack provides the optimal control for staying within each bound individually, but does not provide the controller and bound required for reducing the tracking error prior to a switch. Perhaps surprisingly, in general the tracker may first need to exit the large TEB before converging to the small TEB. Fig. 3 provides an intuitive example of this phenomenon. Here, a Dubins car moving at a fixed speed remains within radius of the origin by turning at its maximum steering angle. 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 visit when transitioning from a large TEB to a small TEB, and the optimal control to achieve this transition. To do this we use HJ reachability analysis.
HJ reachability analysis provides a rigorous mechanism for analyzing the goal satisfaction of a system, and can be used to determine the backward reachable tube (BRT). The BRT is the set of all allowable initial states of a system such that it can enter a set of goal states within a given time interval. HJ reachability analysis can also be used in the context of differential pursuitevasion games [Huang11, Chen17]. Here, as in FaSTrack [Herbert2017], 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 attempting to avoid capture. In practice, the planner is not actively trying to avoid the tracker, but this assumption accounts for unexpected, worstcase planner behavior. We want to determine the set of states that the tracking system may visit when transitioning from the larger TEB to the smaller TEB.
Before constructing the differential game we must first determine its information structure, i.e. how and when each player makes decisions. Since the relative dynamics between the tracker and planner are decoupled in their respective inputs, and we assume an additive disturbance, it is in fact irrelevant who “plays first” at each time instant, and the value of the game is well defined under feedback strategies.
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
(6)  
where denote the sets of feedback strategies for the tracker, planner and disturbance.
Standard HJ formulations exist for computing the BRT in general [Barron90, Mitchell05, Bokanowski10, Fisac15], and more efficiently for decomposable systems [Chen2016DecouplingJournal]. Here the target 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 TEB. This BRT represents the set of states from which the system can enter the small TEB, as well as the states that the trajectories may enter along the way. By projecting this set onto the position dimensions we obtain a switching safety bound (SSB). We note that this is an overapproximation of the minimal SSB because it includes trajectories that do not originate inside the larger TEB’s controlled invariant set. The SSB precomputation also generates the switching controller. Continuing our doubleintegrator example, Fig. 4a shows the controlled invariant sets associated with the larger and smaller TEBs, and the overapproximated set associate with the SSB. The same information computed analytically is shown in Fig. 4b, where the minimal SSB may be computed exactly.
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 which planners are safe to use in different regions of the environment. In addition, the SSBs allow the metaplanner to determine 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 initially 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 continues trying 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. Two different options for handling this possibility are as follows:

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. 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 .
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. 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.
V Results
We demonstrate our algorithm on a 6D nearhover quadrotor model tracking a suite of 3D geometric planners running BIT* [gammell2015batch] in the cluttered environment depicted in Fig. 6 with different maximum speeds in each dimension. The tracking^{5}^{5}5Note that we have assumed a zero yaw angle for the quadrotor. This is enforced in practice 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. V (tracker at left, planner at 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. Due to the form of (V), the optimal safety controller will be bangbang. However, it is only critical to apply the safety control at the boundary of the TEB. A smooth linear controller may be used in the interior, following a leastrestrictive supervisory control law. The relative dynamics between the tracking and planning models are:
(8) 
Equation (8) can be split into three 2D subsystems with states and that are of the same form as the doubleintegrator example from Section IVB. Note that the dynamics of the and subsystems are identical, and thus can be solved once and applied to each subsystem. By using decomposable HJ reachability [Chen2016DecouplingJournal] we compute the set in 2 min 15 s and the set in 2 min, for a total of a 4 min 15 s precomputation time. Fig. 7 shows the growth of in each subsystem’s position state as the planner speed in that dimension increases. Moreover, as explained in Section IVB, the TEB for is identical to the SSB for switching from .
Va Simulation
We implemented the metaplanning online algorithm within the robot operating system (ROS) [ros] framework. We used the BIT* [gammell2015batch] geometric planner from the Open Motion Planning Library (OMPL) [sucan2012theopenmotionplanninglibrary]. Code is written in C++ and is available as an open source ROS package.^{6}^{6}6https://github.com/HJReachability/meta_fastrack Metaplanning typically runs in well under one second in a moderately cluttered environment.
Fig. 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 Section 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 Section 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.
VB Hardware Demonstration
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 accompanying video,
^{7}^{7}7https://youtu.be/lPdXtR8ArE the quadrotor successfully avoids the obstacles while remaining inside the TEB for each planner the metaplan.Fig. 9 shows the quadrotor’s position over time recorded during a hardware demonstration. Note that the quadrotor stays well within the TEB throughout the flight even when the TEB changes size during planner switches.
Vi Conclusions
We have proposed a novel metaplanning algorithm for using FaSTrack with multiple planners. The algorithm adaptively selects the fastestmoving planner that finds locally collisionfree paths, and guarantees safe online transitions between these planners. The resulting metaplans use more aggressive, fastermoving planners in open areas and more cautious, slowermoving planners near obstacles. We demonstrate metaplanning in simulation and in a hardware demonstration, using a quadrotor to track piecewiselinear trajectories at different top speeds.
The theory we develop here is general and can be applied to a wide variety of systems, including manipulators and other mobile robots. However, computing the TEB and SSB using HJ reachability can be challenging for these highdimensional coupled systems. Ongoing work seeks to alleviate this challenge using other methods of computation such as sum of squares programming and neural network function approximators. Other promising directions include incorporating timevarying obstacle avoidance, further integration with OMPL and other planning libraries, providing adaptable error bounds based on external disturbances, and updating the tracking error bound online based on learned information about the tracking model.
Comments
There are no comments yet.