See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/309291325

Continuous-Time Trajectory Optimization for Online UAV Replanning

Conference Paper · October 2016

DOI: 10.1109/IROS.2016.7759784

CITATIONS READS
289 1,035

6 authors, including:

Helen Oleynikova Michael Burri

Microsoft ETH Zurich
33 PUBLICATIONS 2,677 CITATIONS 30 PUBLICATIONS 4,890 CITATIONS

SEE PROFILE SEE PROFILE

Zachary Jeremy Taylor Juan I. Nieto

ETH Zurich Microsoft
32 PUBLICATIONS 1,848 CITATIONS 269 PUBLICATIONS 10,864 CITATIONS

SEE PROFILE SEE PROFILE

All content following this page was uploaded by Helen Oleynikova on 20 March 2018.

The user has requested enhancement of the downloaded file.

 Continuous-Time Trajectory Optimization for Online UAV Replanning
Helen Oleynikova, Michael Burri, Zachary Taylor, Juan Nieto, Roland Siegwart and Enric Galceran
Autonomous Systems Lab, ETH Zürich

Abstract— Multirotor unmanned aerial vehicles (UAVs) are

rapidly gaining popularity for many applications. However, safe
operation in partially unknown, unstructured environments re-
mains an open question. In this paper, we present a continuous-
time trajectory optimization method for real-time collision
avoidance on multirotor UAVs. We then propose a system where
this motion planning method is used as a local replanner, that
runs at a high rate to continuously recompute safe trajectories
as the robot gains information about its environment. We
validate our approach by comparing against existing methods
and demonstrate the complete system avoiding obstacles on a
multirotor UAV platform.

I. I NTRODUCTION
Multirotor UAVs are gaining wide acceptance not only
as research platforms, but also for use in various real-
world applications. Despite recent progress in on-board state
estimation, planning, and control, many current UAV systems
still require either an empty environment or perfect knowl- Fig. 1: Results of an experiment showing our online local
edge of one a priori. This limits their safety and utility in replanning system. The UAV starts from behind the large
unstructured, unknown environments. mattress, which blocks its view of the iron obstacle (left)
In this paper, we focus on the problem of planning safe behind. The planning goal is set inside the iron obstacle, but
avoidance trajectories for a multirotor helicopter (multi- as soon as the UAV observes that it is occupied, it replans
copter) in partially known or unknown environments. For to stop in front (planned path seen in black). This shows the
example, use cases such as high-speed forest flight require ability of our system to avoid newly-detected obstacles and
low-latency motion planning, as these environments are find feasible goal positions online.
often densely populated and obstacles frequently occlude one
another [1].
Motion primitive methods have been a common choice for in discrete time, while dynamic constraints are more nat-
online replanning on fixed-wing and multirotor platforms, urally suited to continuous-time representations (and avoid
since they can be executed quickly and each “primitive” can unnecessary numerical differentiation errors).
be constructed to be dynamically feasible [2] [3]. However, For UAV flight, it is advantageous to use continuous-time
such methods require discretizing the state-space, which trajectory representations like polynomials [5]. These basis
requires a huge motion library or having the controller functions provide continuity up to a high derivative, fast
track from nearest start state, which [3] cites as being evaluation at any given time, and a very small number of
responsible for up to 20% of the failures of their fixed-wing parameters are needed to describe even long and complex
collision avoidance system. For our approach, we overcome trajectories. By varying only one parameter (segment time),
the need for state-space or time discretization by choosing it is possible to ensure that these trajectories are dynamically
a continuous-time basis function to express our trajectories, feasible given a simplified model of multicopter dynamics.
and plan from arbitrary points in the state space to allow We propose a method of continuous-time trajectory opti-
greater flexibility for online replanning. mization that allows the real-time generation of safe avoid-
We draw inspiration from trajectory optimization meth- ance trajectories. Our method uses a two-part objective
ods, such as CHOMP [4], which locally minimize collision function, minimizing both a derivative of position and cost
and smoothness costs on a discrete-time trajectory. These of collision with the environment. We also show this method
planners are most commonly used for solving manipulation as integrated into a local replanning system, where we use
problems, where most of the constraints are kinematic rather this local trajectory optimization to modify an initial plan in
than dynamic. Kinematic constraints can be simply expressed the presence of previously unknown obstacles.
Our approach runs in real-time (under 50 ms for a com-
The research leading to these results has received funding from the
European Communitys Seventh Framework Programme (FP7) under grant- plete planning cycle), produces continuous-time trajectories,
agreement n.608849 (EuRoC) and n.644128, (AEROWORKS). and is able to plan from and to arbitrary states without a
need for discretization in the workspace or state-space. candidates based on an objective function. This has been
The contributions of this work are as follows: done for finding good polynomial trajectories to enable
• A continuous-time polynomial formulation of a local quadrotor ball juggling [10], selecting locally lower-cost
trajectory optimization problem for collision avoid- trajectories to track a global plan in rough terrain [11], and
ance, capable of running in real-time on a real multi- choosing the safest trajectories for autonomous vehicles in
copter. traffic [12]. However, these approaches rely on randomly-
• A complete system with this as local replanning com- sampled trajectories finding collision-free paths, which is an
ponent, which continuously computes collision-free assumption that may not hold in very cluttered environments.
trajectories around any newly detected obstacles. An alternative is to solve the optimal control problem
using mixed-integer programming, where the workspace is
• Evaluation against existing trajectory optimization and again broken up into convex regions and a global optimum
planning algorithms, and experiments on a real world including some linearized or simplified version of the system
platform (Fig. 1). dynamics is found [13], [14]. These approaches generally
give dynamically-feasible and collision-free trajectories, and
II. R ELATED W ORK
it is even possible to make guarantees on their safety [15],
3D path planning approaches for UAVs can be broadly [16], however require a map representation which is very
classified into several categories, such as sampling-based costly to compute and generally have long runtimes (on the
methods (often followed by smoothing), trajectory optimiza- order of magnitude of minutes).
tion methods, and method based on motion primitives. Here A class of methods commonly used for replanning are
we discuss motion planning methods related to our work, those based on motion primitives. The state-space of the UAV
with emphasis in suitability for real-time replanning with a is discretized into a state lattice with motion primitives form-
dynamically updating map. ing edges in the graph, and standard graph search algorithms
Sampling-based planning followed by a smoothing step such as A* and AD* are used to find a feasible solution
which ensures dynamic constraints are met is commonly used through this graph. This has been shown in multicopters
for 3D global planning on UAVs [6]. Approaches such as for navigating through partially known environments [2],
running RRT-based methods to generate a visibility graph, [17] and on fixed-wing airplanes for navigating through
followed by fitting high-order polynomials through the way- a forest while always safely being able to perform an
points (graph vertices) are shown to outperform traditional emergency turn-around maneuver [18]. Another work shows
RRT methods in control space in terms of execution time [6]. flying high-speed through a forest using only on-board vision
Recent speed-ups to the polynomial optimization have also and planning, picking collision-free next maneuvers from a
allowed such combined planners to run in almost real-time, motion library [3], where they cite insufficient richness of
taking only a few seconds to generate long global plans [7]. the motion primitive library and discretization in the start
While these methods generally produce high-quality plans state as responsible for 50% of the experimental failures.
and are probabilistically complete, they are still too slow for A drawback of these approaches is the need to discretize
some online applications like real-time avoidance. both the workspace and state space (for example, motion
Closest to our proposed approach are discrete-time primitives can only be generated for a finite number of
trajectory optimization methods. CHOMP, a trajectory start velocities and end velocities), and the performance
optimization-based motion planner that revived interest in of the algorithm is tightly linked to how many motion
this class of planner in recent years, uses a two-part objective primitives are generated. Although we do use a discretized
function with a smoothness and collision cost, and performs workspace representation, our approach does not require
gradient descent with positions of discrete waypoints as such discretization, nor does it require discretization in time
parameters [4]. In order to speed up convergence time to or state-space, giving the possibility of a wider range of
a feasible plan, and ensure smoothness of the final solution, solutions to be found.
each gradient descent step is multiplied by a Riemennian
metric in order to ensure smooth, incremental updates. Also III. C ONTINUOUS -T IME T RAJECTORY O PTIMIZATION
based on trajectory optimization, STOMP is a gradient-free A LGORITHM
method that samples candidate trajectories and minimizes a Our approach focuses on optimizing high-degree poly-
cost function by creating linear combinations of the best- nomial trajectories made out of several segments, as in-
scoring candidates [8]. A more recent advent in trajec- spired by [6]. The trajectory is essentially a high-order
tory optimization for collision-free planning breaks up the polynomial spline, with C D continuity, where D is the
workspace into free convex regions and performs Sequential derivative we attempt to minimize. These high-order splines
Quadratic Programming (SQP) to converge to a solution are generally used for global trajectory generation, and have
faster than the previous two methods [9]. Unfortunately, this many advantages including the ability to specify velocities,
requires a pre-built map with pre-computed convex regions, accelerations, and lower derivatives at waypoints, very fast
which is difficult to achieve in real-time. evaluation times, and compact representation of long and
Another approach to finding a low-cost path is to cheaply complex trajectories. While a closed-form solution exists to
generate many path candidates, and choose the best of the minimizing the sum of squared derivatives of such a spline,
we expand the problem to also contain information about the The objective Jd can be calculated via the following:
environment to generate a locally optimal safe trajectory.
Jd = d> >
F RF F dF + dF RF P dP +
A. Problem Formulation d> >
P RP F dF + dP RP P dP (6)
Instead of considering the full dynamics of a multicopter, where R is the augmented cost matrix, and RXX denotes
we follow the work of Mellinger and Kumar [5] to plan in the appropriate blocks within this matrix.
a reduced space of differentially flat outputs. This allows us The Jacobian of Jd with respect to the parameter vector
to plan only in R3 and handle yaw separately. can be computed as follows:
Therefore, we will consider a polynomial trajectory in K
∂ Jd
dimensions, with S segments, and each segment of order N . = 2d> >
F RF P + 2dP RP P . (7)
Each segment has K dimensions, each of which is described ∂dP
by an N th order polynomial: The derivative costs are independent for each axis, and are
accumulated over all K dimensions of the problem.
fk (t) = a0 + a1 t + a2 t2 + a3 t3 . . . aN tN (1) To represent collision costs, we use a line integral of the
with the polynomial coefficients: potential function c(f (t)) over the arc length of the trajectory.
 > Since our environment is represented as a discrete voxel
pk = a0 a1 a2 . . . aN . (2) grid (see Section III-C), we need to sample the trajectory and
test at least one point within each voxel along the trajectory.
Given this trajectory representation, we seek to find the set
To do so, we transform the trajectory from end-derivatives
of coefficients p∗ that minimize an objective function J. In
into workspace coordinates. For each axis k at a time t:
our case, similar to CHOMP [4], our objective function has
two components: a part that attempts to minimize a derivative T = [t0 , t1 , t2 , ..., tN ] (8)
D, Jd , and a part that attempts to minimize collisions with fk (t) = Tpk (9)
the environment, Jc .  
f (t) = fx (t) fy (t) . . . (10)
p∗ = argmin wd Jd + wc Jc (3) We also compute the velocity at each time t, using a
p matrix V, which maps a vector of polynomial coefficients
The following sections will present our choices of ob- of a function to the polynomial coefficients of its derivative.
jective costs, Jd and Jc , optimization method, and map
vk (t) = f˙k (t) = TVpk (11)
representation to solve this problem in real-time.  
v(t) = vx (t) vy (t) . . . (12)
B. Method
The collision cost is then the line integral below, integrated
As described in [6], we express the polynomial not in over each segment m (where tm is the end time of the
terms of its N + 1 coefficients, but in terms of its end- segment):
derivatives to allow us to pose the derivative minimization Z
problem as an unconstrained quadratic program (QP), which Jc = c(f (t))ds
is significantly faster to solve than the constrained dual of ZStm
this problem. = c(f (t)) ḟ (t) dt
We can map between polynomial coefficients and end- t=0
derivatives using the A matrix, and rearrange the end- tm
X
derivatives into a free (dP ) and fixed (dF ) blocks using a = c(f (t)) kv(t)k ∆t (13)
mapping matrix M: t=0
 
d where c(f (t)) is the potential cost described in [4].
p = A−1 M F . (4) Finally, using the product and chain rules, we obtain the
dP
Jacobian for each axis k:
The construction of matrices A, M (and R below) is tm
∂ Jc X
addressed in [6]. The fixed derivatives dF are given from = kv(t)k ∇k c TLP P ∆t +
the fixed end-constraints, like start and end velocities and ∂dP k t=0
accelerations, while the free derivatives dP are the parame- vk (t)
ters we optimize. c(f (t)) TVLP P ∆t. (14)
kv(t)k
In order to incorporate costs from collisions with the
Here L = A−1 M, or the complete mapping matrix
environment, we use a minimization problem with two costs:
between end-derivatives and polynomial coefficients. LP P
d∗P = argmin wd Jd + wc Jc (5) refers to the block of the right-side columns of the matrix,
dP
corresponding to the columns which operate on the free
where Jd is the cost due to integrated squared derivative parameters dP .
terms (if minimizing snap, integral of squared snap along We use a heuristic to estimate the segment times, tm ,
the trajectory), Jc is the cost due to collisions, and wd and to meet dynamic constraints and we hold these times fixed
wc are the weighing terms for each part of the cost. during the optimization.
C. Map Representation When first constructing the map, we fill in the occupancy
Map representation and choice of potential cost function of all cells and compute the distances for the complete map,
is central to the algorithm described above. Naturally, the which is computationally expensive. In order to allow our
potential cost function must be smooth, but its gradient must algorithm to run in real-time, we track changed nodes in
also be able to push trajectories out of collision. We use the our Octomap representation and invalidate all voxels in the
potential described in [4], which is a function of an Euclidean ESDF that have those nodes as parents (i.e., nearest neighbor
Signed Distance Field (ESDF) value, d(x), at a point in 3D of a different state). This allows us to recompute the distance
space, x, and  is a constant value specifying the obstacle values of only a few tens to hundred voxels per map update,
clearance past which space is considered free. instead of having to recompute the full dense grid of millions
of voxels.

1 One important point about the map representation is that
−d(x) + 2 
 if d(x) < 0
although Octomap allows three states (free, unknown, and
1 2
c(x) = 2 (d(x) − ) if 0 ≤ d(x) ≤  (15)
occupied) with full probabilities, in order to construct a

0 otherwise

distance field, we must discretize to only two states —
free and occupied. How to deal with unknown voxels is a
We use a voxel-based map representation, as they can be
question of safety: we cannot safely plan through them unless
built and maintained quickly. To ensure that a trajectory does
the robot is able to stop in known free space. Therefore,
not collide with the environment, we must check each voxel
we choose to treat unknown as occupied, creating a very
along the trajectory. Note that our continuous-time approach
conservative planner. A more thorough discussion of this
still presents advantages over discrete-time methods, as we
choice is offered in Section VI.
are flexible in how often we sample the trajectory for
collisions, and can change this interval between iterations. B. Global Planning
We choose to evaluate the function along every arc length Next, we built a global plan to an end point in the original
point ∆s equal to the map voxel resolution. This significantly Octomap, while treating unknown space as free. This creates
speeds up computation without compromising safety. a high-level optimistic planner, while the local replanner is
conservative and therefore safer. This plan will then be used
D. Optimization as a prior for the replanner, and also allows us to use a
In any nontrivial environment, the optimization problem replanner that is not complete – in case no solution is found
in (5) is likely to be non-convex and highly nonlinear. For by the local trajectory optimization, we simply stop and wait
minimizing the function, we choose to use a quasi-Newton for the global planner to find a new path.
method like BFGS [19] (though other simpler methods like We use a 2-stage global planner: first, we find a topo-
gradient descent can also be used). logically feasible straight-line path using Informed RRT*
However, all solutions found with such methods are in- [21], and then we plan a dynamically feasible polynomial
herently local solutions – and they are prone of falling into trajectory through it [7].
local minima depending on the initialization. Therefore, in C. Local Replanning
order to increase the chances of finding a feasible (non-
To perform the local replanning, we start with the global
colliding) solution, we do several random restarts where
plan (if available) or a straight-line plan to the next waypoint
we perturb the initial state by a random quantity, and then
as prior, and incrementally update the ESDF.
select the lowest-cost trajectory as the final solution. A more
We then select appropriate start and end points for the
thorough discussion on the necessity of random restarts for
replanning algorithm. As start point, we choose the point
local trajectory optimization is offered in [9].
on the current trajectory tR seconds in the future, where
IV. R EPLANNING S YSTEM tR is the update rate of the replanner. Since our planner
allows continuity and smoothness even in low derivatives,
In this section, we introduce the complete system that we are able to use the full state of the UAV at the start point,
makes it possible to run our local replanning method online, including velocity and acceleration, guaranteeing a smooth
in real-time on dynamically updating map data. First, we path even with changing plans.
introduce how to build the map and its companion distance The goal point is chosen as a point on the global trajectory
field, then we discuss using a global plan as input into the that is h meters ahead of the start point, where h is a planning
local replanner, and finally, how to select start and end points horizon. If unoccupied, we accept that point as the goal,
for replanning. otherwise we attempt to find the nearest unoccupied neighbor
in the ESDF, and as a final fallback we shorten the planning
A. Incremental Mapping horizon until a free goal point is found.
As mentioned in Section III-C, we require an Euclidean We can then run the local optimization procedure between
Signed Distance Field (ESDF) to compute the collision these two points. Either the optimization succeeds in finding
potentials. Our map representation is an Octomap [20], which a collision-free path, or we attempt random restarts until
contains voxels in one of three states: free, unknown, or either a collision-free trajectory is found or the vehicle stops
occupied. and waits for the global planner to select a new path.
 V. E XPERIMENTAL R ESULTS Mean Mean
Algorithm Success Norm. Path Compute
In this section we first evaluate the proposed continuous- Fraction Length Time [s]
time local planner and compare it to existing planning Inf. RRT* + Poly 0.9778 1.1946 2.2965
algorithms. Then, we validate our complete system in both a RRT Connect + Poly 0.9444 1.6043 0.5444
long, realistic simulation scenario where the robot only has CHOMP N = 10 0.3222 1.0162 0.0032
CHOMP N = 100 0.5000 1.0312 0.0312
local information about the environment, and then in a real- CHOMP N = 500 0.3333 1.0721 0.5153
world test on an UAV avoiding newly detected objects in a Ours S = 2 jerk 0.4889 1.1079 0.0310
room. Ours S = 3 vel 0.4778 1.1067 0.0793
Ours S = 3 jerk 0.5000 1.0996 0.0367
A. Evaluation Ours S = 3 jerk + Restart 0.6333 1.1398 0.1724
Ours S = 3 snap + Restart 0.6222 1.1230 0.1573
We validate our approach as a local start-to-goal point Ours S = 3 snap 0.5000 1.0733 0.0379
planner in simulation on 100 random 2D forest environments. Ours S = 4 jerk 0.5000 1.0917 0.0400
Ours S = 5 jerk 0.5000 1.0774 0.0745
To analyze how our algorithm behaves in different den-
sities of clutter, we generate 5 × 5 m Poisson forests [1] TABLE I: A table showing comparison of RRT variants
of densities between 0.2 trees/m2 to 0.8 trees/m2 . We then with polynomial smoothing, CHOMP, and our approach on
analyze the success fraction of our algorithm versus CHOMP a set of 90 forest planning problems, as shown in Fig. 3.
[4], a discrete-time local optimization method. We compare the success fraction, normalized path length
We initialize both algorithms with a straight-line path (solution path length divided by straight-line path length),
between opposite corners of the map. For our continuous- and computation time. As can be seen, adding random
time algorithm, we use between 1 and 5 segments of 11th restarts significantly improves success fraction but at the cost
order polynomials (N = 11) minimizing snap, and optionally of higher computation time. RRT* and RRT Connect are able
use 10 random restarts. For CHOMP, we use a fixed N of to solve a higher percentage of problems, but at the cost of
100 points and minimize velocity. slower performance.
Fig. 2a shows typical paths generated by the algorithms.
As can be seen, 1 segment does not have sufficient degrees
of freedom to solve this planning problem, but 5 segments number of problems, Informed RRT* takes too long to run in
are able to find a short, smooth, feasible solution. CHOMP real-time at a high rate, and RRTConnect, while significantly
is also able to find a solution for this problem, but falls into faster, is still exceeding the time budget and producing much
a different local minima from our approach. longer paths.
We can further analyze the behavior of the algorithms For N = 100 (where N is the number of discretized way-
at different forest densities (number of obstacles in the points) in the CHOMP algorithm, the results are comparable
environment), as seen in Fig. 2b. As the density of the both in run time, success rate, and path length. However,
environment increases, the chance of all methods finding a in order to fully safely verify the trajectory, there should
valid solution decreases. However, there is a large increase on be a waypoint for every voxel in the 3D occupancy grid.
success rate if random restarts are used, and a larger number Therefore, N = 500 is a more appropriate comparison from a
of segments is able to handle denser environments (as in the safety perspective (as the mean path length is approximately
case in Fig. 2a). Fig. 2c shows the effect of increasing the 5 meters), and since the execution time grows approximately
number of segments on success across all test cases. with O(N 2 ), this method performs much slower in such
For a more representative evaluation, we simulate arealis- cases. It also has a lower success rate, as it does not converge
tic 3D forest environment using real tree models, as shown in to a collision-free solution within the limited iteration steps.
Fig. 3. The environments are 10 × 10 × 10 m, populated with On the other hand, our method has a fixed number of
a density of 0.2 trees/m2 . The trees are of random scale parameters for a given number of segments, regardless of
and height, adding the additional complexity of navigating trajectory length or map resolution. Therefore, we are able
in 3D and avoiding the tree canopies. We generate 9 such to keep a low computation time, as long as the number
environments, and select 10 random start and goal points at of segments chosen is appropriate for the density of the
least 4 meters apart, for a total of 90 test cases. environment, and our results show that 3 segments is enough
We evaluate several parameter settings of our algorithm for the realistic forest scenario tested. As a result, our
and compare to CHOMP (which minimizes velocity), and approach has only 10 free parameters where CHOMP has
sampling-based visibility graph search (RRT-based methods) 500 per axis.
with polynomial smoothing using 9th order minimum snap We chose to use 3 segments and minimize jerk in our final
polynomials. For CHOMP and our method, since both fea- real-world experiments, as this has the smallest number of
ture a derivative cost term and a collision cost term, we use free parameters for the highest success rate in our compari-
the same weights (wd = 0.1, wc = 10) to make as fair of a son.
comparison as possible. Both algorithms are allowed to run
for up to 50 iterations. B. System Simulation
The results are shown in Table I. Though RRT-based Next, we validate our local replanning in the context of a
algorithms with smoothing are clearly able to solve a larger complete system and only a partially known map.
 Typical Solution Paths
5 1 Planning Success vs. Tree Density in Poisson Forest Fraction of Successful Plans by Number of Segments
1
4.5 0.9
1
0.9
4 0.8
0.8 0.8

Success [fraction]
3.5 0.7
y position [meters]

0.7

Success [fraction]
Collision Cost
3 0.6
0.6 0.6
2.5 0.5 0.5
2 0.4 0.4 0.4
1.5 0.3 0.3

1 0.2 0.2
CHOMP 0.2
1 segment Without Random Restarts
CHOMP 5 segments
0.5 Our method - 1 segment 0.1 0.1 1 segments + restarts
With Random Restarts
5 segments + restarts
Our method - 5 segments 0
0 0 0 1 2 3 4 5
0 1 2 3 4 5 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8
x position [meters] Tree Density [trees/m2 ] Number of Segments

(a) (b) (c)

Fig. 2: Results of 2D evaluations of our algorithm against CHOMP. (a): Typical paths generated by our algorithm with
different number of segments, compared to CHOMP. With only one segment (red), there are not enough degrees of freedom
to avoid all the obstacles, but it is able to find a solution (and one different from CHOMP) with 5 segments. The potential
cost map is in gray, with the original obstacle edges in blue. (b): Success rate of the different local planners vs. density of
the environment. As density increases, success rate decreases, but more so for a smaller number of segments, and some of
this decrease can be counteracted by doing 10 random restarts. (c): Fraction of successful plans (taken over environments
of all densities) by number of segments. This also shows a significant increase in success rate by doing random restarts,
which allows the algorithm to avoid local minima that are in collision.

RRT* and polynomial smoothing.

As can be seen, both algorithms produce similar paths,
with the local replanning finding a solution that is only
0.5m longer than the global path. The RRT* plus smoothing
algorithm ran with complete knowledge of the map and took
30 seconds to compute, 20 of which were spent on finding the
visibility graph and 10 were spent on finding a collision-free
polynomial path. On the other hand, our algorithm was able
to find a comparable path while considering only a 4 meter
region around itself and continuous replanning at 4 Hz.
C. Real World Experiments
Finally, we show our complete system running in real-
Fig. 3: Figure showing the simulation setup for evaluations. time on a multicopter, starting from a completely blank map
The forest is 10 × 10 × 10 meters, with a density of 0.2 and filling it from sensor data. The experiment is done in an
tree/m2 . The paths are planned between two random points indoor environment with two obstacles: a large one directly
in the space, at least 4 m apart. Yellow is our method, cyan in front of the robot, obscuring the robot’s vision, and a
is Informed RRT* + polynomial optimization (discussed in smaller second obstacle behind the first. A goal point is
global planner section), and purple is CHOMP. placed inside the second obstacle, and the UAV must avoid
the large obstacle, fly behind it, detect the second obstacle,
and stop short of collision. The physical setup is shown in
We set up a realistic simulation experiment in RotorS [22] Fig. 1.
simulator, using a model of our multicopter platform. We Our platform is an Asctec Firefly1 using a visual-inertial
approximate filling a blank map from sensor data by only stereo sensor [23], running at 20 Hz, for both state estimation
giving the UAV access to a small radius of the map around and perception, both of which are done entirely on-board on
itself while flying through a large forest environment. The an 2.1 GHz Intel i7 CPU.
map is 50 × 50 m with a density of 0.1 trees/m2 . The UAV starts with a blank map and builds it online
We use 4 meters as a planning horizon for our local from dense stereo reconstruction data. The map is updated
replanning and give the algorithm access to 5 meters around at 5 Hz, and replanning is done at 4 Hz, and we use the same
its current position (to emulate a stereo system with a 5 parameters as in the previous section. The key difference is
meter maximum range). A new plan, minimizing jerk in a that due to the narrow field of view of the camera, we treated
3-segment trajectory, is generated at 4 Hz as the UAV is unknown space as free. A further discussion of this decision
flying. is offered in Section VI.
Fig. 4 shows the results of our experiment, compared to 1 http://www.asctec.de/en/uav-uas-drones-rpas-roav/
a global plan made from a fully-known map using Informed asctec-firefly/
 Fig. 5: Local plans over time for the real-world experiments.
Fig. 4: Here we show a comparison of our local replanning The original goal point is embedded in the second obstacle
method (cyan - 67.9 m), operating with a planning horizon (pink), which is not visible from the start position. Over
of 4 meters, compared to a single global plan (black - time, the paths stop short of collision with the obstacle and
67.5 m) generated with prior knowledge of the entire map. the final path (blue) is a shorter, lower curvature path than
The environment is 50×50 m with a density of 0.1trees/m2 , many of the plans earlier in the experiment.
and we show only the tree trunks of the obstacles for clarity.
The local replanning algorithm is running at a rate of 4
Hz, while the global planner, using Informed RRT* with
seen in Table I, the number of waypoints CHOMP requires
polynomial smoothing, runs in 30 seconds.
to check every voxel along a 5 m long path with a map
resolution of 10 cm is N = 500, and with this many
variables, the convergence rate is significantly slower and the
Fig. 5 shows the path evolution over time of the trajectory, execution time is significantly longer than for our algorithm,
with the color of the trajectory going from red to blue with even with 5 polynomial segments.
time. As can be seen, though initial trajectory candidates are Our approach allows better control of end derivatives,
both in collision and often very far from obstacles, but as which makes it much easier to integrate this into a continuous
the UAV approaches the goal, its path gets smoother and out replanning framework, as shown in Section V-B and Section
of collision. Also note that until the trajectory color reaches V-C. We are able to continue planning from the exact current
cyan, the trajectory goal is still placed in collision since the (or future) state of the UAV, leading to smooth, continuous
UAV has not seen the obstacle yet. This experiment can be paths. This is an advantage over both motion primitive
seen in the video attachment. methods, which must discretize the state, and discrete-time
We also show average timings for the complete system methods, which can only encode lower-derivative continuity
in Table II. As can be seen, the complete system is fast as a cost rather than a hard constraint.
enough to run in real-time at 4 Hz, and has a mean latency However, the main drawback of this approach is the
of only 40 ms between acquiring depth data from the sensors required map representation (ESDF) in which space is treated
to generating a feasible collision-free trajectory. as either occupied or unoccupied, and unknown space must
be treated as one of the two. The obvious choice is to treat
VI. D ISCUSSION
unknown space as impassable. While this can work well in
Our experiments have shown that our approach is able to simulation, real sensors often have measurements which are
find solutions to local path-planning problems successfully, not completely dense, leading to blocks of unknown space
at a comparable rate to existing trajectory optimization even in areas that have been observed. Treating these as
methods. While sampling-based methods are still able to occupied leads to the UAV rarely being able to find areas
solve a much larger percentage of the problems posed, they where the entire bounding box of the UAV contains no
are prohibitively slow for our target application. unoccupied voxels. Treating these as free, on the other hand,
The main advantage of our method compared to discrete- encourages the UAV to travel into unknown space to avoid
time trajectory optimization methods lies in the inherently obstacles. This can have disastrous consequences depending
smooth, compact representation. For example, as can be on the sensor configuration; for example, flying straight into
 Step Time [ms] [5] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and
control for quadrotors,” in IEEE International Conference on Robotics
Mapping
and Automation (ICRA), pp. 2520–2525, IEEE, 2011.
Octomap Insert 10 [6] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning
ESDF Initial Map Creation* 110 for aggressive quadrotor flight in dense indoor environments,” in
ESDF Incremental Update <1 Proceedings of the International Symposium on Robotics Research
(ISRR), 2013.
Local Replanning [7] M. Burri, H. Oleynikova, , M. W. Achtelik, and R. Siegwart, “Real-
Select Start and End 1 time visual-inertial mapping, re-localization and planning onboard
Optimization (Total) 28 mavs in unknown environments,” in IEEE/RSJ International Confer-
Compute Der. Gradient (per 100 evals) 0.6 ence on Intelligent Robots and Systems (IROS), Sept 2015.
Computer Col. Gradient (per 100 evals) 16 [8] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal,
“Stomp: Stochastic trajectory optimization for motion planning,” in
Total Time per Planner Iteration 40 IEEE International Conference on Robotics and Automation (ICRA),
pp. 4569–4574, IEEE, 2011.
TABLE II: Timings for our complete replanning system, [9] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan,
taken from the real-world experiment. We present mean S. Patil, K. Goldberg, and P. Abbeel, “Motion planning with sequential
convex optimization and convex collision checking,” The International
timings over the entire experiment, which is why the total Journal of Robotics Research, vol. 33, no. 9, pp. 1251–1270, 2014.
optimization time is shorter than the maximum time with [10] M. W. Mueller, M. Hehn, and R. D’Andrea, “A computationally
10 restarts (most planner iterations find a feasible solution efficient motion primitive for quadrocopter trajectory generation,”
IEEE Transactions on Robotics, vol. 31, no. 6, pp. 1294–1310, 2015.
without any restarts). Gradients timings are given over 100 [11] P. Krüsi, B. Bücheler, F. Pomerleau, U. Schwesinger, R. Siegwart, and
evaluations of the cost function. *Initial map creation runs P. Furgale, “Lighting-invariant adaptive route following using iterative
only once and is not included in the total. closest point matching,” Journal of Field Robotics, vol. 32, no. 4,
pp. 534–564, 2015.
[12] U. Schwesinger, M. Rufli, P. Furgale, and R. Siegwart, “A sampling-
based partial motion planning framework for system-compliant navi-
a ceiling that the sensors can not observe. There is also the gation along a reference path,” in Intelligent Vehicles Symposium (IV),
pp. 391–396, IEEE, 2013.
additional cost of computing a dense distance field over each [13] A. Richards and J. P. How, “Aircraft trajectory planning with collision
voxel of the original map, which does not scale to very large avoidance using mixed integer linear programming,” in Proceedings
environments. of the American Control Conference, vol. 3, pp. 1936–1941, IEEE,
2002.
However, our algorithm can be adapted to use other map [14] B. Landry, R. Deits, P. R. Florence, and R. Tedrake, “Aggressive
representations, as long as a smooth, continuous penalty for quadrotor flight through cluttered environments using mixed integer
collisions can be defined. Future work will focus on finding programming,” 2016.
[15] A. J. Barry, A. Majumdar, and R. Tedrake, “Safety verification of
more compact potential cost representations without these reactive controllers for uav flight in cluttered environments using
drawbacks. barrier certificates,” in IEEE International Conference on Robotics and
Automation (ICRA), pp. 484–490, IEEE, 2012.
VII. C ONCLUSIONS [16] R. Tedrake, I. R. Manchester, M. Tobenkin, and J. W. Roberts, “Lqr-
trees: Feedback motion planning via sums-of-squares verification,” The
We presented a motion planning method that uses trajec- International Journal of Robotics Research, 2010.
tory optimization in continuous time to find collision-free [17] B. MacAllister, J. Butzke, A. Kushleyev, H. Pandey, and M. Likhachev,
“Path planning for non-circular micro aerial vehicles in constrained
paths between obstacles. We then constructed a complete environments,” in IEEE International Conference on Robotics and
replanning system, from mapping to trajectory generation, Automation (ICRA), pp. 3933–3940, IEEE, 2013.
which allows us to replan at a high rate and respond to [18] A. A. Paranjape, K. C. Meier, X. Shi, S.-J. Chung, and S. Hutchin-
son, “Motion primitives and 3d path planning for fast flight
previously unknown or unseen obstacles with low delay. through a forest,” The International Journal of Robotics Research,
We verified that our method runs comparably to discrete- p. 0278364914558017, 2015.
time trajectory optimization, while having the advantages [19] D. F. Shanno, “On broyden-fletcher-goldfarb-shanno method,” Journal
of Optimization Theory and Applications, vol. 46, no. 1, pp. 87–94,
of continuous-time representation to minimize the number 1985.
of parameters and allow arbitrary start and goal states. Our [20] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Bur-
experiments showed the system running both in simulation gard, “Octomap: An efficient probabilistic 3d mapping framework
based on octrees,” Autonomous Robots, vol. 34, no. 3, pp. 189–206,
and on a real multicopter platform at 4 Hz, though timing 2013.
analysis shows that it could run at upwards of 25 Hz. [21] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed rrt*:
Optimal sampling-based path planning focused via direct sampling
R EFERENCES of an admissible ellipsoidal heuristic,” in IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), pp. 2997–3004,
[1] S. Karaman and E. Frazzoli, “High-speed flight in an ergodic forest,” in IEEE, 2014.
IEEE International Conference on Robotics and Automation (ICRA), [22] F. Furrer, M. Burri, M. Achtelik, and R. Siegwart, Robot Operating
pp. 2899–2906, IEEE, 2012. System (ROS): The Complete Reference (Volume 1), ch. RotorS—A
[2] M. Pivtoraiko, D. Mellinger, and V. Kumar, “Incremental micro- Modular Gazebo MAV Simulator Framework, pp. 595–625. Springer
uav motion replanning for exploring unknown environments,” in International Publishing, 2016.
IEEE International Conference on Robotics and Automation (ICRA), [23] J. Nikolic, J. Rehder, M. Burri, P. Gohl, S. Leutenegger, P. T. Furgale,
pp. 2452–2458, IEEE, 2013. and R. Siegwart, “A synchronized visual-inertial sensor system with
[3] A. J. Barry, High-Speed Autonomous Obstacle Avoidance with Pushb- fpga pre-processing for accurate real-time slam,” in IEEE International
room Stereo. PhD thesis, Massachusetts Institute of Technology, 2016. Conference on Robotics and Automation (ICRA), pp. 431–437, IEEE,
[4] M. Zucker, N. Ratliff, A. D. Dragan, M. Pivtoraiko, M. Klingensmith, 2014.
C. M. Dellin, J. A. Bagnell, and S. S. Srinivasa, “Chomp: Covariant
hamiltonian optimization for motion planning,” The International
Journal of Robotics Research, vol. 32, no. 9-10, pp. 1164–1193, 2013.

View publication stats