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

TL;DR

FASTER is a trajectory planning algorithm that enables high-speed, safe flights in unknown environments by optimizing in both known and unknown spaces and ensuring safety with backup trajectories.

Contribution

It introduces a novel mixed-integer quadratic programming approach that optimizes trajectory interval allocation and guarantees safety with backup plans.

Findings

Achieves agile flights up to 3.6 m/s in cluttered environments.

Enables fast reaction times with real-time optimization.

Provides safety guarantees through backup trajectories.

Abstract

High-speed trajectory planning through unknown environments requires algorithmic techniques that enable fast reaction times while maintaining safety as new information about the operating environment is obtained. The requirement of computational tractability typically leads to optimization problems that do not include the obstacle constraints (collision checks are done on the solutions) or use a convex decomposition of the free space and then impose an ad-hoc time allocation scheme for each interval of the trajectory. Moreover, safety guarantees are usually obtained by having a local planner that plans a trajectory with a final "stop" condition in the free-known space. However, these two decisions typically lead to slow and conservative trajectories. We propose FASTER (Fast and Safe Trajectory Planner) to overcome these issues. FASTER obtains high-speed trajectories by enabling the local planner to optimize in both the free-known and unknown spaces. Safety guarantees are ensured by always having a feasible, safe back-up trajectory in the free-known space at the start of each replanning step. Furthermore, we present a Mixed Integer Quadratic Program formulation in which the solver can choose the trajectory interval allocation, and where a time allocation heuristic is computed efficiently using the result of the previous replanning iteration. This proposed algorithm is tested extensively both in simulation and in real hardware, showing agile flights in unknown cluttered environments with velocities up to 3.6 m/s.

Tables

Table 1. TABLE I: Distances obtained in 10 random forest simulations. Improvement percentages are computed for the minimum and the maximum of each column. Some results were provided by the authors of [ 10 ] .

Method	Number of	Distance (m)
	Successes	Avg	Std	Max	Min
Incremental	0	-	-	-	-
Rand. Goals	10	138.0	32.0	210.5	105.6
Opt. RRT⋆	9	105.3	10.3	126.4	95.5
Cons. RRT⋆	9	155.8	52.6	267.9	106.2
NBVP [17]	6	159.3	45.6	246.9	123.6
SL Expl. [10]	8	103.8	21.6	148.3	86.6
Mult-Fid [8]	10	84.5	11.7	109.4	73.2
FASTER	10	77.6	5.9	88.0	70.7
Min/Max improvement (%)		8/51	43/89	20/67	3/43

Equations

$${\mathbf{x}}_n(\tau )={\mathbf{a}}_n{\tau }^3+{\mathbf{b}}_n{\tau }^2+{\mathbf{c}}_n\tau +{\mathbf{d}}_n,\tau \in [0,dt]$$

$$\bm{x}_{n}(\tau)=\sum_{j=0}^{3}\left(\begin{array}[]{c}3\\ j\end{array}\right)\left(1-\frac{\tau}{dt}\right)^{3-j}\left(\frac{\tau}{dt}\right)^{j}\bm{r}_{nj},\;\;\tau\in[0,dt],$$

$${\mathbf{r}}_{n0}={\mathbf{d}}_n,{\mathbf{r}}_{n1}=\frac{{\mathbf{c}}_{n}dt+3{\mathbf{d}}_n}{3}$$

$${\mathbf{r}}_{n2}=\frac{{\mathbf{b}}_{n}d{t}^2+2{\mathbf{c}}_{n}dt+3{\mathbf{d}}_n}{3}$$

$${\mathbf{r}}_{n3}={\mathbf{a}}_{n}d{t}^3+{\mathbf{b}}_{n}d{t}^2+{\mathbf{c}}_{n}dt+{\mathbf{d}}_n$$

$$\underset{{\mathbf{j}}_n,b_{np}}{\min }$$

$${\mathbf{x}}_0(0)={\mathbf{x}}_{init}$$

$${\mathbf{x}}_{N-1}(dt)={\mathbf{x}}_{final}$$

$${\mathbf{x}}_n(\tau )={\mathbf{a}}_n{\tau }^3+{\mathbf{b}}_n{\tau }^2+{\mathbf{c}}_n\tau +{\mathbf{d}}_n\forall n,\forall \tau \in [0,dt]$$

$${\mathbf{v}}_n(\tau )={\dot{\mathbf{x}}}_n(\tau )\forall n,\forall \tau \in [0,dt]$$

$${\mathbf{a}}_n(\tau )={\dot{\mathbf{v}}}_n(\tau )\forall n,\forall \tau \in [0,dt]$$

$${\mathbf{j}}_n=6{\mathbf{a}}_n(0)\forall n$$

$$\displaystyle b_{np}=1\implies\left\{\begin{array}[]{c}\bm{A}_{p}\bm{r}_{n0}\leq\bm{c}_{p}\\ \bm{A}_{p}\bm{r}_{n1}\leq\bm{c}_{p}\\ \bm{A}_{p}\bm{r}_{n2}\leq\bm{c}_{p}\\ \bm{A}_{p}\bm{r}_{n3}\leq\bm{c}_{p}\end{array}\right.\quad\forall n,\forall p$$

$$\sum _{p=0}^{P-1}{b}_{np}\ge 1\forall n$$

$$b_{np}\in \{0,1\}\forall n,\forall p$$

$${\mathbf{x}}_{n+1}(0)={\mathbf{x}}_n(dt)n=0:N-2$$

$${\Vert {\mathbf{v}}_n(0)\Vert }_{\infty }\le v_{max},\forall n$$

$${\Vert {\mathbf{a}}_n(0)\Vert }_{\infty }\le a_{max},\forall n$$

$${\Vert {\mathbf{j}}_n\Vert }_{\infty }\le j_{max},\forall n$$

$$dt=f\cdot \max \{T_{v_x},T_{v_y},T_{v_z},T_{a_x},T_{a_y},T_{a_z},T_{j_x},T_{j_y},T_{j_z}\}/N$$

Full text

Accepted for the 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China

Supplementary material

Code: FASTER (https://github.com/mit-acl/faster) and simulation worlds (https://github.com/jtorde)

Video: https://youtu.be/gwV0YRs5IWs.

I INTRODUCTION

Navigating through unknown environments entails repeatedly generating collision-free, dynamically feasible trajectories that are executed over a finite horizon. Similar to that in the Model Predictive Control (MPC) literature, safety is guaranteed by ensuring a feasible solution exists indefinitely. If we consider $\mathbb{R}^{3}=\mathcal{O}\cup\mathcal{F}\cup\mathcal{U}$ where $\mathcal{F}$, $\mathcal{O}$, $\mathcal{U}$ are disjoint sets denoting free-known, occupied-known, and unknown space respectively, safety is guaranteed by constructing trajectories that are entirely contained in $\mathcal{F}$ with a final stop condition. This can be achieved by generating motion primitives that do not intersect $\mathcal{O}\cup\mathcal{U}$ [1, 2, 3, 4], or by constructing a convex representation of $\mathcal{F}$ to be used in an optimization [5, 6, 7]. However, both approaches lead to slow trajectories in scenarios where $\mathcal{F}$ is small compared to $\mathcal{U}\cup\mathcal{O}$. This paper presents an optimization-based approach that reduces the aforementioned limitations by solving for two optimal trajectories at every planning step (see Fig. 1): one in $\mathcal{U}\cup\mathcal{F}$, and another one in $\mathcal{F}$.

Decomposing the free space into $P$ overlapping polyhedra along a path connecting a start $A$ to goal $E$ location (see Fig. 1), the usual approach is to divide the total trajectory into $N=P$ intervals [5]. On one hand, this simplifies the problem because no integer variables are needed, as each interval is forced to be in one specific polyhedron. On the other hand, the time allocation problem becomes much harder, as there are $N$ different $dt_{n}$ (time allocated for each interval $n$). The trajectory is also more conservative since the optimizer is only allowed to move the end points of each interval of the trajectory in the overlapping areas. To overcome these two problems, we propose the use of the same $dt$ for all the intervals, and use $N>P$ intervals, encoding the optimization problem as a Mixed Integer Quadratic Program (MIQP). Moreover, and as the minimum feasible $dt$ depends depends on the state of the UAV and on the specific shape of $\mathcal{F}$ and $\mathcal{U}$ at a specific replanning step, we also propose an efficient way to compute a heuristic of this $dt$ using the result obtained in the previous replanning iteration.

In summary, this work has the following contributions:

• •

A framework that ensures feasibility of the entire collision avoidance algorithm and guarantees safety without reducing the nominal flight speed by allowing the local planner to plan in $\mathcal{F}\cup\mathcal{U}$ while always having a safe trajectory in $\mathcal{F}$.

• •

Reduced conservatism of the MIQP formulation for the interval and time allocation problem of the flight trajectories compared to prior work.

• •

Extension of our previous work [8], (where we considered the interaction between the global and local planners) by proposing a way to compute very cheaply a heuristic of the cost-to-go needed by the local planner to decide which direction is the best one to optimize towards.

• •

Simulation and hardware experiments showing agile flights in completely unknown cluttered environments, with velocities up to $3.6$ m/s.

II RELATED WORK

Trajectory planning strategies for UAVs can be classified according to the operating space of the local planner and the specific formulation of the optimization problem.

With regard to the planning space of the local planner, several approaches have been developed. One approach is to use only the most recent perception data [4, 3, 2], which requires the desired trajectory to remain within the perception sensor field of view. An alternative strategy is to create and plan trajectories in a map of the environment using a history of perception data. Within this second category, some works [9, 8, 10] limit the local planner to generate trajectories only in free-known space ($\mathcal{F}$ in Fig. 1), which guarantees safety if the local planner has a final stop condition. However, limiting the planner to operating in free-known space and enforcing a terminal stopping condition can lead to conservative, slow trajectories (especially when much of the world is unknown). While allowing the local planner to optimize in both the free-known and unknown space ($\mathcal{F}\cup\mathcal{U}$), higher speeds can be obtained but with no guarantees that the trajectory is safe or will remain feasible.

As far as the optimization formulation is concerned, two approaches can be highlighted. The first does not include the obstacles in the optimization problem [8], leading to a closed-form solution for the trajectory [1, 3, 4] or in general to very small computation times [8]. The computation time for these approaches are very low since obstacles are not explicitly considered in the trajectory generation. This enables multiple candidate trajectories to be generated (via sampling) and evaluated for collisions (using nearest-neighbor search) at each planning stage. While these approaches are computationally efficient, they are unable to construct sophisticated maneuvers due to the discretization of the candidate trajectories, leading to slower trajectories in cluttered environments. The second approach is to include obstacle constraints directly in the optimization. This is usually done describing the free space by a set of overlapping polyhedra (also known as convex decomposition) [5, 6, 7]. The trajectory can then be parameterized by a sequence of third (or higher)-degree polynomials. Bézier Curves [7], [11] or the sum-of-squares condition [6, 12] can be used to guarantee that the trajectory remains in the overlapping polyhedra. Subsequently, there will be an interval (which polytope each polynomial is in) and a time allocation (how much time is assigned to each interval) problem. For the interval allocation, a typical solution is to use the same number of trajectory segments as number of polyhedra, and then each polynomial segment is forced to be inside its corresponding polyhedron [7]. However, this can be very restrictive since the solver only has the freedom to select where the two endpoint points of each interval are placed in the overlapping regions. Another option, but with higher computation times, is to use binary decision variables [12, 6] to allow the solver to choose the specific interval allocation. For the time allocation, one can either use a fixed time allocation [5] or formulate a bi-level optimization to find the times [13], [7]. However, the first approach can be very conservative and can cause infeasibility in the optimization problem while the seconds leads to longer replanning times.

III FAST AND SAFE TRAJECTORY PLANNER

III-A Planning

The Fast and Safe Trajectory Planner (FASTER) uses hierarchical architecture where a long-horizon global planner guides a short-horizon local planner to a desired goal location. The global planner used in this work is Jump Point Search (JPS). JPS finds the shortest piecewise linear path between two points in a 3D uniformly-weighted voxel grid, guaranteeing optimality and completeness but running an order of magnitude faster than A* [14], [5].

For the local planner, we distinguish these three different jerk-controlled trajectories (some of the points will be precisely defined later, see Fig. 3):

• •

Whole Trajectory: This trajectory goes from a start location $A$ to goal location $E$, and it is contained in $\mathcal{F}\cup\mathcal{U}$. It has a final stop condition.

• •

Safe Trajectory: It goes from $R$ to $F$, where $R$ is a point in the Whole Trajectory, and $F$ is any point inside the polyhedra obtained by doing a convex decomposition of $\mathcal{F}$. It is completely contained in $\mathcal{F}$ (free-known space), and it has also a final stop condition to guarantee safety.

• •

Committed Trajectory: This trajectory consists of two pieces: The first part is the interval $A\rightarrow R$ of the Whole Trajectory. The second part is the Safe Trajectory. It is also guaranteed to be inside $\mathcal{F}$ (see explanation below). This trajectory is the one that the UAV will execute in case no feasible solutions are found in the next replanning steps.

The quadrotor is modeled using triple integrator dynamics with state vector $\mathbf{x}^{T}=\left[\bm{{x}}^{T}~{}\bm{{\dot{x}}}^{T}~{}\bm{{\ddot{x}}}^{T}~{}\right]=\left[\bm{{x}}^{T}~{}\bm{{v}}^{T}~{}\bm{{a}}^{T}\right]$ and control input $\mathbf{u}=\bm{{\dddot{x}}}=\bm{{j}}$ (where $\bm{{x}}$, $\bm{{v}}$, $\bm{{a}}$, and $\bm{{j}}$ are the vehicle’s position, velocity, acceleration, and jerk, respectively).

Let $n=0:N-1$ denote the specific interval of the trajectory and $p=0:P-1$ the specific polyhedron. If $\bm{{j}}(t)$ is constrained to be constant in each interval $n=0:N-1$, then the whole trajectory will be a spline consisting of third degree polynomials. Matching the cubic form of the position for each interval

[TABLE]

with the expression of a cubic Bézier curve

[TABLE]

we can solve for the four control points $\bm{{r}}_{nj}$ $(j=0:3)$ associated with each interval $n$:

[TABLE]

Let us denote the sequence of $P$ overlapping polyhedra as $\{(\bm{{A}}_{p},\bm{{c}}_{p})\},\;p=0:P-1$, and introduce binary variables $b_{np}$ ($P$ variables for each interval $n=0:N-1$). As a Bézier curve is contained in the convex hull of its control points, we can ensure that the whole trajectory will be inside this convex corridor by forcing that all the control points are in the same polyhedron [7, 11] with the constraint [b_{np}=1\implies\bm{r}_{nj}\in\text{polyhedron p}\;\;\forall j], and at least in one polyhedron with the constraint $\sum_{p=0}^{P-1}b_{np}\geq 1$. The optimizer is free to choose in which polyhedron exactly. The complete MIQP solved in each replanning step (using Gurobi, [15]) for both the Safe and the Whole trajectories is this one:

[TABLE]

In the optimization problem above, $dt$ (same for every interval $n$) is computed as

[TABLE]

where $T_{v_{i}}$, $T_{a_{i}}$, $T_{j_{i}}$ are solution of the constant-input motions in each axis $i={x,y,z}$ by applying $v_{max}$, $a_{max}$ and $j_{max}$ respectively. $f\geq 1$ is a factor that is obtained according to the solution of the previous replanning step (see Fig. 2): The optimizer will try values of $f$ (in increasing order) in the interval $[f_{worked,k-1}-\gamma,f_{worked,k-1}+\gamma^{\prime}]$ until the problem converges. Here, $f_{worked,k-1}$ is the factor that made the problem feasible in the previous replanning step. Note that, if $f=1$, then $dt$ is a lower bound on the minimum time per interval required for the problem to be feasible.

Algorithm 1 summarizes the full approach (see also Fig. 3). Let $L$ be the current position of the UAV. The point $A$ is chosen in the Committed Trajectory of the previous replanning step with an offset $\delta t$ from $L$. This offset $\delta t$ is computed by multiplying the total time of the previous replanning step by $\alpha\geq 1$ (typically $\alpha\approx 1.25$). The idea here is to dynamically change this offset to ensure that most of the times the solver is able to find the next solution in less than $\delta t$. Then, the final goal $G_{term}$ is projected into the sliding map $\mathcal{M}$ (centered on the UAV) in the direction $\overrightarrow{G_{term}A}$ to obtain the point $G$ (line 1). Next, we run JPS from $A$ to $G$ (line 1) to obtain $JPS_{a}$.

The local planner then must decide if the current JPS solution should be used to guide the optimization (lines 1-1). Instead of blindly trusting the last JPS solution ($JPS_{a}$) as the best direction for the local planner to optimize (note that JPS is a zero-order model, without dynamics encoded), we take into account the dynamics of the UAV in the following way: First of all, we modify the $JPS_{k-1}$ so that it does not collide with the new obstacles seen (Fig. 4): we find the points $I_{1}$ and $I_{2}$ (first and last intersections of $JPS_{k-1}$ with $\mathcal{O}$) and run JPS three times, so $A\rightarrow I_{1}$, $I_{1}\rightarrow I_{2}$ and $I_{2}\rightarrow I_{G}$. Hence, the modified version, denoted by $JPS_{b}$, will be the concatenation of these three paths. Then, we compute a lower bound on $dt$ using Eq. 7 for both $A\rightarrow C$ and $A\rightarrow D$, where $C$ and $D$ are the intersections of the previous JPS paths with a sphere $\mathcal{S}$ of radius $r$ centered on $A$. Next, we find the cost-to-go associated with each direction by adding this $dt_{a}$ (or $dt_{b}$) and the time it would take the UAV to go from $C$ (or $D$) to $G$ following the JPS solution and flying at $v_{max}$. Finally, the one with lowest cost is chosen, and therefore $JPS_{k}\leftarrow\underset{\{JPS_{a,}JPS_{b}\}}{\mathop{\mbox{argmin}}}\{J_{a},J_{b}\}$. This will be the direction towards which the local planner will optimize.

The Whole Trajectory (lines 1-1) is obtained as follows. We do the convex decomposition [5] of $\mathcal{U}\cup\mathcal{F}$ around $JPS_{in}$, which is the part of $JPS_{k}$ that is inside the sphere $\mathcal{S}$. This gives a series of overlapping polyhedra that we denote as $Poly_{whole}$. Then, the MIQP in (1) is solved using these polyhedral constraints to obtain the Whole Trajectory.

The Safe Trajectory is computed as in lines 1-1. First we choose the point $R$ along the Whole Trajectory with an offset $\delta t^{\prime}$ from $A$ (this $\delta t^{\prime}$ is computed by multiplying the previous replanning time by $\beta\geq 1$), and run convex decomposition in $\mathcal{F}$ using the part of $JPS_{in}$ that is in $\mathcal{F}$, obtaining the polyhedra $Poly_{safe}$. Then, we solve the MIQP from $R$ to any point $F$ inside $Poly_{safe}$ (this point $F$ is chosen by the optimizer).

In both of the convex decompositions presented above, one polyhedron is created for each segment of the piecewise linear paths. To obtain a less conservative solution (i.e. bigger polyhedra), we first check the length of segments of the JPS path, creating more vertexes if this length exceeds certain threshold $l_{max}$. Moreover, we truncate the number of segments in the path to ensure that the number of polyhedra found does not exceed a threshold $P_{max}$. This helps reduce the computation times (see Sec. IV).

Finally (line 1), we compute the Committed Trajectory by concatenating the piece $A\rightarrow R$ of the Whole Trajectory, and the Safe Trajectory. Note that in this algorithm we have run two decoupled optimization problems per replanning step: (1) one for the Whole Trajectory, and (2) one for the Safe Trajectory. This ensures that the piece $A\rightarrow R$ is not influenced by the braking maneuver $R\rightarrow F$, and therefore guarantees a higher nominal speed on this first piece. The intervals $L\rightarrow A$ and $A\rightarrow R$ have been designed so that, with high probability, at least one replanning step can be solved within that interval. Moreover, to prevent the (very rare) cases where both $A$ and $R$ are in $\mathcal{F}$, but the piece $A-R$ is not, we check that piece $A-R$ against collision with $\mathcal{U}$. If any of the two optimizations in this algorithm fails, or the piece $A-R$ intersects $\mathcal{U}$, or the replanning step takes longer than $\delta t$, the UAV does not commit to a new trajectory in that replanning step, and continues executing the Committed Trajectory of the previous replanning step. Thus safety is guaranteed by construction: the UAV will only fly Committed Trajectories, which are always guaranteed to be in $\mathcal{F}$ with a terminal stopping condition.

III-B Mapping

For the mapping, we use a sliding map centered on the UAV that moves as the UAV flies. We fuse a depth map into the occupancy grid using the 3D Bresenham’s line algorithm for ray-tracing [16], and $\mathcal{O}$ and $\mathcal{U}$ are inflated by the radius of the UAV to ensure safety.

IV RESULTS

IV-A Simulation

We first test FASTER in 10 random forest environments with an obstacle density of $0.1$ obstacles/m2 (see Fig. 5) and compare the flight distances achieved against the following seven approaches: Incremental approach (no goal selection), random goal selection, optimistic RRT⋆ (unknown space = free), conservative RRT⋆ (unknown space = occupied), “next-best-view” planner (NBVP) [17], Safe Local Exploration [10], (see [10] for details of all these approaches), and Multi-Fidelity [8].

The results are shown in Table III, which highlights that FASTER achieves a $8-51\%$ improvement in the distance. Completion times are compared in Table III to our previous proposed algorithm [8] (time values are not available for all other algorithms in Table III). FASTER achieves an improvement of $52\%$ in the completion time. The dynamic constraints imposed for the results of this table are (per axis) $v_{max}=5$ m/s, $a_{max}=5$ m/s2, and $j_{max}=8$ m/s3.

We also test FASTER using the bugtrap environment shown in Fig. 5, and obtain the results that appear on Table III. Both algorithms have a similar total distance, but FASTER achieves an improvement of $63\%$ on the total flight time. For both cases the dynamic constraints imposed are $v_{max}=10$ m/s, $a_{max}=10$ m/s2, and $j_{max}=40$ m/s3.

The timing breakdown of Alg. 1 as a function of the maximum number of polyhedra $P_{max}$ is shown in Fig. 6. The number of intervals $N$ was 10 for the Whole Trajectory and 7 for the Safe Trajectory. Note that the runtime for the MIQP of the Safe Trajectory is approximately constant as a function of $P_{max}$. This is due to the fact that the Safe Trajectory is planned only in $\mathcal{F}$, and therefore most of the times $P<P_{max}$. For the simulations and hardware experiments presented in this paper, $P_{max}=2-3$ was used. The runtimes for JPS as a function of the voxel size of the map for the forest simulation are available in Fig. 7 of [8]. All these timing breakdowns were measured using an Intel Core i7-7700HQ 2.8GHz Processor.

IV-B Hardware

The UAV used in the hardware experiments is shown in Fig. 7. The perception runs on the Intel® RealSense, the mapper and planner run on the Intel® NUC, and the control runs on the Qualcomm® SnapDragon Flight. The attitude, IMU biases, position and velocity are estimated by fusing (via a Kalman filter) propagated IMU measurements with an external motion capture system.

The first and second experiments (Fig. 11 and 11) were done in similar obstacle environments with the same starting point, but with different goal locations. In the first experiment (Fig. 11), the UAV performs a 3D agile maneuver to avoid the obstacles on the table. In the second experiment (Fig. 11) the UAV flies through the narrow gap of the cardboard boxes structure, and then flies below the triangle-shaped obstacle. In these two experiments, the maximum speed was $2.1$ m/s.

In the third and fourth experiments (Fig 11 and 11), the UAV must fly through a space with poles of different heights, and finally below the cardboard boxes structure to reach the goal, achieving a maximum speed of $3.6$ m/s.

For $P_{max}=2$, the boxplots of the runtimes achieved on the forest simulation (measured on an Intel Core i7-7700HQ) and on the hardware experiments (measured on the onboard Intel NUC with the mapper and the RealSense also running on it) are shown in Fig. 12. For the runtimes of the MIQP for the Whole and the Safe Trajectories, the 75th percentile is always below $32$ ms.

V CONCLUSIONS

This work presented FASTER, a fast and safe planner for agile flights in unknown environments. The key properties of this planner is that it leads to a higher nominal speed than other works by planning both in $\mathcal{U}$ and $\mathcal{F}$, and ensures safety by having always a Safe Trajectory planned in $\mathcal{F}$ at the beginning of every replanning step. FASTER was tested successfully both in simulated and in hardware flights, achieving velocities up to $3.6$ m/s.

ACKNOWLEDGMENT

Thanks to Boeing Research & Technology for support of the hardware, to my brother Pablo Tordesillas (ETSAM-UPM) for his great help with some figures of this paper and to Parker Lusk (ACL-MIT) for his help with the hardware. The authors would also like to thank John Carter and John Ware (CSAIL-MIT) for their help with the mapper used in this paper. Supported in part by Defense Advanced Research Projects Agency (DARPA) as part of the Fast Lightweight Autonomy (FLA) program, HR0011-15-C-0110. Views expressed here are those of the authors, and do not reflect the official views or policies of the Dept. of Defense or the U.S. Government.