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

10/12/2017
by   David Fridovich-Keil, et al.
berkeley college
0

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.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 1

page 7

02/05/2020

Reachable Sets for Safe, Real-Time Manipulator Trajectory Design

For robotic arms to operate in arbitrary environments, especially near p...
03/08/2019

FaSTraP: Fast and Safe Trajectory Planner for Flights in Unknown Environments

Planning high-speed trajectories for UAVs in unknown environments requir...
01/22/2020

A Real-Time Approach for Chance-Constrained Motion Planning with Dynamic Obstacles

Uncertain dynamic obstacles, such as pedestrians or vehicles, pose a maj...
12/29/2020

Alternative Paths Planner (APP) for Provably Fixed-time Manipulation Planning in Semi-structured Environments

In many applications, including logistics and manufacturing, robot manip...
08/02/2018

Robust Tracking with Model Mismatch for Fast and Safe Planning: an SOS Optimization Approach

In the pursuit of real-time motion planning, a commonly adopted practice...
01/09/2020

FASTER: Fast and Safe Trajectory Planner for Flights in Unknown Environments

Planning high-speed trajectories for UAVs in unknown environments requir...
10/25/2021

Online Strategy Synthesis for Safe and Optimized Control of Steerable Needles

Autonomous systems are often applied in uncertain environments, which re...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

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 heuristically augmenting obstacles; however, the system may still deviate from the planned path by more than this margin. (c) Schematic of meta-planner operation using fast (blue, dashed) and slow (red, solid) planning models with correspondingly large (blue, solid) 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 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 sampling-based planners such as rapidly-exploring 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(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) framework [Herbert2017] provides a rigorous way to precompute a safety margin offline, given a model of the true system dynamics and a (possibly lower-dimensional) 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 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 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 meta-planning 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 difficult-to-track 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 meta-planning, 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 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 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, 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 [Bansal2017].

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 [kahneman2011thinking]. Thinking with the “fast system” is efficient, but more error-prone. Thinking with the “slow system” is less error-prone, 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 meta-planning 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 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 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 meta-plan 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.

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):

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

(2)

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:

(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.

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

(5)

Iii-D 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 worst-case 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 off-the-shelf planning algorithm—equipped with the precomputed TEB for collision-checking—generates a new trajectory. The tracking system may then apply the precomputed safety controller to track this planned trajectory in real time.

Iv Meta-Planning

Iv-a 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 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 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 meta-planner is a random tree inspired by RRT-style sampling-based planners [lavalle1998rapidly], as shown in Fig. 1.222The 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 planner333Note 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 collision-free 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 “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. Similar to Informed RRT* [gammell2014informed], the meta-planner immediately rejects samples which could not possibly improve upon the best available trajectory.444In 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 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.

Iv-B Offline Reachability Analysis

There are two major components to the offline precomputation for the meta-planner. The first step is to compute the TEB and safety control look-up 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 double-integrator dynamics in (5), an analytic solution can also be found by applying the equations of constant-acceleration motion under the worst-case 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.

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

Fig. 3: Example of a Dubins car that must leave its tight orbit in order to eventually move closer to the origin. This example illustrates why the switching safety bound may generally be larger than the tracking error bound.

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 pursuit-evasion 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, worst-case 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 worst-case 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 over-approximation 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 double-integrator example, Fig. 4a shows the controlled invariant sets associated with the larger and smaller TEBs, and the over-approximated set associate with the SSB. The same information computed analytically is shown in Fig. 4b, where the minimal SSB may be computed exactly.

Fig. 4: Visualizations of the -subsystem’s numerical (left) and analytic (right) controlled invariant sets for two different planners. The numerical SSB is guaranteed to over-approximate the minimal 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 which planners are safe to use in different regions of the environment. In addition, the SSBs allow the meta-planner to determine 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 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 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 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 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. Two different options for handling this possibility are as follows:

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

  2. 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. 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 .

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

V Results

(a) LQR controller.
(b) Safety controller.
Fig. 6: Simulated autonomous flight in a cluttered environment. Notice that when using LQR control the quadrotor leaves the TEB, but under optimal safety 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* [gammell2015batch] in the cluttered environment depicted in Fig. 6 with different maximum speeds in each dimension. The tracking555Note 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):

(7)

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 bang-bang. 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 least-restrictive supervisory control law. The relative dynamics between the tracking and planning models are:

(8)
Fig. 7: TEB vs. planner speed in each subsystem.

Equation (8) can be split into three 2D subsystems with states and that are of the same form as the double-integrator example from Section IV-B. 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 IV-B, the TEB for is identical to the SSB for switching from .

V-a Simulation

We implemented the meta-planning online algorithm within the robot operating system (ROS) [ros] framework. We used the BIT* [gammell2015batch] geometric planner from the Open Motion Planning Library (OMPL) [sucan2012the-open-motion-planning-library]. Code is written in C++ and is available as an open source ROS package.666https://github.com/HJReachability/meta_fastrack Meta-planning 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 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 Section 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.

V-B 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,

777https://youtu.be/lPdXtR8Ar-E the quadrotor successfully avoids the obstacles while remaining inside the TEB for each planner the meta-plan.

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.

Fig. 8: A Crazyflie 2.0 flying during our hardware demonstration. Two OptiTrack cameras are visible in the background.
Fig. 9: Position vs. time during a hardware demonstration.

Vi Conclusions

We have proposed a novel meta-planning algorithm for using FaSTrack with multiple planners. The algorithm adaptively selects the fastest-moving planner that finds locally collision-free paths, and guarantees safe online transitions between these planners. The resulting meta-plans use more aggressive, faster-moving planners in open areas and more cautious, slower-moving planners near obstacles. We demonstrate meta-planning in simulation and in a hardware demonstration, using a quadrotor to track piecewise-linear 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 high-dimensional 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 time-varying 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.