Real-Time Trajectory Replanning for MAVs using Uniform

B-splines and a 3D Circular Buffer

Vladyslav Usenko, Lukas von Stumberg, Andrej Pangercic and Daniel Cremers
Technical University of Munich

Abstract—In this paper, we present a real-time approach

to local trajectory replanning for microaerial vehicles (MAVs).
Current trajectory generation methods for multicopters achieve
high success rates in cluttered environments, but assume that
the environment is static and require prior knowledge of the
map. In the presented study, we use the results of such planners
and extend them with a local replanning algorithm that can
handle unmodeled (possibly dynamic) obstacles while keeping the
MAV close to the global trajectory. To ensure that the proposed
approach is real-time capable, we maintain information about the
environment around the MAV in an occupancy grid stored in a
three-dimensional circular buffer, which moves together with a
drone, and represent the trajectories by using uniform B-splines.
This representation ensures that the trajectory is sufficiently
smooth and simultaneously allows for efficient optimization.

I. I NTRODUCTION
In recent years, microaerial vehicles (MAVs) have gained
popularity in many practical applications such as aerial pho-
tography, inspection, surveillance and even delivery of goods.
Most commercially available drones assume that the path
planned by the user is collision-free or provide only limited
obstacle-avoidance capabilities. To ensure safe navigation in
the presence of unpredicted obstacles a replanning method that
generates a collision-free trajectory is required.
Formulation of the trajectory generation problem largely
depends on the application and assumptions about the envi- Fig. 1: Example of local trajectory replanning algorithm run-
ronment. In the case where an MAV is required to navigate ning in the simulator. Global trajectory is visualized in purple
a cluttered environment, possibly an indoor one, we would and the local obstacle map is visualized in red. The local
suggest subdividing the problem into two layers. First, we trajectory is represented by a uniform quintic B-spline, and its
assume that a map of the environment is available and a control points are visualized in yellow for the fixed parts and
trajectory from a specified start point to the goal point is in green for the parts that can still change due to optimization.
planned in advance.
This task has been a popular research topic in recent years,
and several solutions have been proposed by Achtelik et al. [1]
and Richter et al. [21]. They used occupancy representation and simultaneously avoids unpredicted obstacles based on an
of the environment to check for collisions and searched for environment representation constructed from the most recent
the valid path in a visibility graph constructed using sam- sensor measurements. This replanning level should run in
pling based planners. Thereafter, they followed the approach several milliseconds to ensure the safety of MAVs operating
proposed by Mellinger and Kumar [14] to fit polynomial at high velocities.
splines through the points of the planned path to generate a The proposed approach solves a similar problem as that
smooth feasible trajectory. The best algorithms of this type solved by Oleynikova et al. [17], but instead of using poly-
can compute a trajectory through tens of waypoints in several nomial splines for representing the trajectory we propose the
seconds. use of B-splines and discuss their advantages over polynomial
To cope with any unmodeled, possibly dynamic, obsta- splines for this task. Furthermore, we propose the use of a
cle a lower planning level is required, which can generate robocentric, fixed-size three-dimensional (3D) circular buffer
a trajectory that keeps the MAV close to the global path to maintain local information about the environment. Even
though the proposed method cannot model arbitrarily large to find a feasible trajectory depends largely on the selected
occupancy maps, as some octree implementations, faster look- discretization scheme.
up and measurement insertion operations make it better suited
for real-time replanning tasks. B. Environment representation
We demonstrate the performance of the system in several To be able to plan a collision-free trajectory a representation
simulated and real-world experiments, and provide open- of the environment that stores information about occupancy
source implementation of the software to community. is required. The simplest solution that can be used in the
The contributions of the present study are as follows: 3D case is a voxel grid. In this representation, a volume is
• Formulation of local trajectory replanning as a B-spline
subdivided into regular grid of smaller sub-volumes (voxels),
optimization problem and thorough comparison with al- where each voxel stores information about its occupancy. The
ternative representations (polynomial, discrete). main drawback of this approach is its large memory-footprint,
• High-performance 3D circular buffer implementation for
which allows for maping only small fixed-size volumes. The
local obstacle mapping and collision checking and com- advantage, however, is very fast constant time access to any
parison with alternative methods. element.
• System design and evaluation on realistic simulator and
To deal with the memory limitation, octree-based represen-
real hardware, in addition to performance comparison tations of the environment are used in [9] [22]. They store
with existing methods. information in an efficient way by pruning the leaves of the
trees that contain the same information, but the access times
In addition to analyzing the results presented in the paper,
for each element become logarithmic in the number of nodes,
we encourage the reader to watch the demonstration video and
instead of the constant time as in the voxel-based approaches.
inspect the available code, which can be found at
Another popular approach to environment mapping is voxel
https://vision.in.tum.de/research/ hashing, which was proposed by Nießner et al. [16] and used
robotvision/replanning in [18]. It is mainly used for storing a truncated signed distance
function representation of the environment. In this case, only
II. R ELATED W ORK a narrow band of measurements around the surface is inserted
In this section, we describe the studies relevant to different and only the memory required for that sub-volume is allocated.
aspects of collision-free trajectory generation. First, we discuss However, when full measurements must be inserted or the
existing trajectory generation strategies and their applications dense information must be stored the advantages of this
to MAV motion planning. Thereafter, we discuss the state-of- approach compared to those of the other approaches are not
the-art approaches for 3D environment mapping. significant.

A. Trajectory generation III. T RAJECTORY R EPRESENTATION USING UNIFORM

B- SPLINES
Trajectory generation strategies can be subdivided into
three main approaches: search-based path planning followed We use uniform B-spline representation for the trajectory
by smoothing, optimization-based approaches and motion- function p(t). Because, as shown in [14] and [1], the trajectory
primitive-based approaches. must be continuous up to the forth derivative of position
In search-based approaches, first, a non-smooth path is (snap), we use quintic B-splines to ensure the required smooth-
constructed on a graph that represents the environment. This ness of the trajectory.
graph can be a fully connected grid as in [6] and [11], or be A. Uniform B-splines
computed using a sampling-based planner (RRT, PRM) as in
[21] and [3]. Thereafter, a smooth trajectory represented by a The value of a B-spline of degree k − 1 can be evaluated
polynomial, B-spline or discrete set of points is computed to using the following equation:
closely follow this path. This class of approaches is currently n
X
the most popular choice for large-scale path planning problems p(t) = pi Bi,k (t), (1)
in cluttered environments where a map is available a priory. i=0

Optimization-based approaches rely on minimizing a cost where pi ∈ Rn are control points at times ti , i ∈ [0, .., n] and
function that consists of smoothness and collision terms. The Bi,k (t) are basis functions that can be computed using the De
trajectory itself can be represented as a set of discrete points Boor – Cox recursive formula [5] [4]. Uniform B-splines have
[25] or polynomial segments [17]. The approach presented a fixed time interval ∆t between their control points, which
in the present work falls into this category, but represents a simplifies computation of the basis functions.
trajectory using uniform B-splines. In the case of quintic uniform B-splines, at time t ∈
Another group of approaches is based on path sampling [ti , ti+1 ) the value of p(t) depends only on six control points,
and motion primitives. Sampling-based approaches were suc- namely [ti−2 , ti−1 , ti , ti+1 , ti+2 , ti+3 ]. To simplify calcula-
cessfully used for challenging tasks such as ball juggling tions we transform time to a uniform representation s(t) =
[15], and motion primitives were successfully applied to flight (t − t0 )/∆t, such that the control points transform into
through the forest [19], but the ability of both approaches si ∈ [0, .., n]. We define function u(t) = s(t) − si as time
 500
Ring buffer 120
Octomap

100
400

80
300
60
200
40

100
20

0 0
0 2 4 6 8 10 0 20 40 60 80 100
Insertion time [ms] Insertion time [ms]

(a) (b) (c) (d)

Fig. 2: Comparison between octomap and circular buffer for occupancy mapping on fr2/pioneer slam2 sequence of [23]. Being
able to map only a local environment around the robot (3 m at voxel resolution of 0.1 m) circular buffer is more than an order
of magnitude faster when inserting point cloud measurements from a depth map subsampled to a resolution of 160 × 120.
Subplots (a) and (b) show the histograms of insertion time, and (c) and (d) show the qualitative results of the circular buffer
(red: occupied, green:free) and the octomap, respectively.

elapsed since the start of the segment. Following the matrix The integral over squared time derivatives can be computed
representation of the De Boor – Cox formula [20], the value in the closed form. For example, the integral over squared
of the function can be evaluated as follows: acceleration can be computed as follows:
Z ti+1
T   Eq = p00 (u(t))2 dt (6)
1 pi−2 ti
u pi−1   T  
 2   pi−2 pi−2
u   pi  pi−1  pi−1 
p(u(t)) =  3  M6 
   , (2)
u4  pi+1 
   
 pi 
 M6T QM6  pi  ,
  
u  pi+2  =
pi+1  pi+1  (7)
u5 pi+3
   
pi+2  pi+2 
pi+3 pi+3
 
1 26 66 26 1 0 (8)
 −5 −50 0 50 5 0
  where
1  10 20 −60 20 10 0 T
M6 =  . (3)
 
0 0
−10 20
5!  0 −20 10 0
Z 1 0  0 

 5 −20 30 −20 5 0
  
−1 5 −10 10 −5 1 1  2  2 
Q=    du (9)
∆t3 0  6u   6u 
  
Given this formula, we can compute derivatives with respect 12u2  12u2 
to time (velocity, acceleration) as follows: 20u3 20u3
 
 T   0 0 0 0 0 0
0 pi−2 0 0 0 0
 1  pi−1   0 0 

    1 0 0 8 12 16 20 
0 1  2u   pi  = 
0 0 12 24 36
. (10)
p (u(t)) =  2  M6 
  , (4) ∆t3 48 
∆t 3u3  pi+1 
  
0 0 16 36 57.6 80 
4u  pi+2 
0 0 20 48 80 114.286
5u4 pi+3
Please note that matrix Q is constant in the case of uniform
 T   B-splines. Therefore, it can be computed in advance for
0 pi−2 determining the integral over any squared derivative (see [21]
 0  pi−1  for details).
   
00 1  2   pi 
p (u(t)) =   M6 
 . (5) B. Comparison with polynomial trajectory representation
 6u2 
∆t2  pi+1 
 
12u  pi+2  In this subsection, we discuss the advantages and disad-
20u3 pi+3 vantages of B-spline trajectory representation compared to
polynomial-splines-based representation [21] [17].
The computation of other time derivatives and derivatives To obtain a trajectory that is continuous up to the forth
with respect to control points is also straightforward. derivative of position, we need to use B-splines of degree five
or greater and polynomial splines of at least degree nine (we
need to set five boundary constraints on each endpoint of the
segment). Furthermore, for polynomial splines we must ex-
plicitly include boundary constraints into optimization, while
the use of B-splines guarantees the generation of a smooth
trajectory for an arbitrary set of control points. Another useful
property of B-splines is the locality of trajectory changes
due to changes in the control points, which means that a
change in one control point affects only a few segments
in the entire trajectory. All these properties result in faster
optimization because there are fewer variables to optimize and
fewer constraints.
Evaluation of position at a given time, derivatives with
respect to time (velocity, acceleration, jerk, snap), and inte-
grals over squared time derivatives are similar for both cases
because closed-form solutions are available for both cases.
The drawback of B-splines, however, is the fact that the
Fig. 3: Example of online trajectory replanning using proposed
trajectory does not pass through the control points. This makes
optimization objective. The plot shows a global trajectory com-
it difficult to enforce boundary constraints. The only constraint
puted by fitting a polynomial spline through fixed waypoints
we can enforce is a static one (all time derivatives are zero),
(red), voxels within 0.5 m of the obstacle (blue), computed B-
which can be achieved by inserting the same control point
spline trajectory with fixed (cyan) and still optimized (green)
k + 1 times, where k is the degree of the B-spline. If we need
segments and control points. In the areas with no obstacles,
to set an endpoint of the trajectory to have a non-zero time
the computed trajectory closely follows the global one, while
derivative, an iterative optimization algorithm must be used.
close to an obstacle, the proposed method generates a smooth
These properties make polynomial splines more suitable
trajectory that avoids the obstacle, and rejoins the global
for the cases where the control points come from planning
trajectory.
algorithms (RRT, PRM), which means that the trajectory must
pass through them, else the path cannot be guaranteed to be
collision-free. For local replanning, which must account for
find its address in the stored array:
unmodeled obstacles, this property is not very important; thus,
the use of B-spline trajectory representation is a better option. insideV olume(x) = 0 ≤ x − o < N, (11)
address(x) = (x − o) mod N. (12)
IV. L OCAL E NVIRONMENT M AP USING 3D C IRCULAR
p
B UFFER If we restrict the size of the array to N = 2 , we can rewrite
these functions to use cheap bitwise operations instead of
To to avoid obstacles during flight, we need to maintain divisions:
an occupancy model of the environment. On one hand, the
model should rely on the most recent sensor measurements,
and on the other hand it should store some information over insideV olume(x) = ! ((x − o) & (∼ (2p − 1))), (13)
time because the field of view of the sensors mounted on the p
address(x) = (x − o) & (2 − 1). (14)
MAV is usually limited.
We argue that for local trajectory replanning a simple where & is a ”bitwise and,” ∼ is a ”bitwise negation,” and !
solution with a robocentric 3D circular buffer is beneficial. In is a ”boolean not.”.
what follows, we discuss details pertaining to implementation To ensure that the volume is centered around the camera,
and advantages from the application viewpoint. we must simply change the offset o and clear the updated part
of the volume. This eliminates the need to copy large amounts
A. Addressing of data when the robot moves.

To enable addressing we discretize the volume into voxels B. Measurement insertion

of size r. This gives us a mapping from point p in 3D space We assume that the measurements are performed using
to an integer valued index x that identifies a particular voxel, range sensors, such as Lidar, RGB-D cameras, and stereo
and the inverse operation that given an index outputs its center cameras, and can be inserted into the occupancy buffer by
point. using raycast operations.
A circular buffer consists of a continuous array of size N We use an additional flag buffer to store a set of voxels
and an offset index o that defines the location of the coordinate affected by insertion. First, we iterate over all points in our
system of the volume. With this information, we can define measurements, and for the points that lie inside the volume,
the functions to check whether a voxel is in the volume and we mark the corresponding voxels as occupied. For the points
 (a) (b) (c)
Fig. 4: Real-world experiment performed outdoors. The drone (AscTec Neo) equipped with RGB-D camera (Intel Realsense
R200) is shown in (a). In the experiment, the global path is set to a straight line with the goal position 30 m ahead of the
drone, and trees act as unmapped obstacles that the drone must avoid. Side view of the scene is shown in (b), and visualization
with the planned trajectory is shown in (c).

that lie outside the volume, we compute the closest point penalizing position and velocity deviation at the end of the
inside the volume and mark the corresponding voxels as free optimized trajectory segment from the desired values that
rays. Second, we iterate over all marked voxels and perform come from the global trajectory. Because the property is
raycasting toward the sensor origin. We use a 3D variant of formulated as a soft constraint, the targeted values might not
Bresenham’s line algorithm [2] to increase the efficiency of be achieved, for example, because of obstacles blocking the
the raycasting operation. path. The function is defined as follows:
Thereafter, we again iterate over the volume and update
the volume elements by using the hit and miss probabilities,
Eep = λp (p(tep ) − pep )2 + λv (p0 (tep ) − p0ep )2 , (16)
similarly to the approach described in [9].
where tep is the end time of the segment, p(t) is the trajectory
C. Distance map computation to be optimized, pep and p0ep are the desired position and
To facilitate fast collision checking for the trajectory, we velocity, respectively, λp and λv are the weighting parameters.
compute the Euclidean distance transform (EDT) of the oc-
cupancy volume. This way, a drone approximated by the B. Collision cost function
bounding sphere can be checked for collision by one look- Collision cost penalizes the trajectory points that are within
up query. We use an efficient O(n) algorithm written by the threshold distance τ to the obstacles. The cost function is
Felzenszwalb and Huttenlocher [7] to compute EDT of the computed as the following line integral:
volume, where n = N 3 is the number of voxels in the grid Z tmax
(the complexity is cubic in the size of the volume along a Ec = λc c(p(t))||p0 (t)||dt, (17)
tmin
single axis). For querying distance and computing gradient,
trilinear interpolation is used. where the cost function for every point c(x) is defined as
follows:
V. T RAJECTORY O PTIMIZATION (
1
(d(x) − τ )2 if d(x) ≤ τ
The local replanning problem is represented as an optimiza- c(x) = 2τ (18)
0 if d(x) > τ,
tion of the following cost function:
where τ is the distance threshold, d(x) is the distance to the
Etotal = Eep + Ec + Eq + El , (15)
nearest obstacle, and λc is a weighting parameter. The line
where Eep is an endpoint cost function that penalizes position integral is computed using the rectangle method, and distances
and velocity deviations at the end of the optimized trajectory to the obstacles are obtained from the precomputed EDT by
segment from the desired values that usually come from the using trilinear interpolation.
global trajectory; Ec is a collision cost function; Eq is the cost C. Quadratic derivative cost function
of the integral over the squared derivatives (acceleration, jerk,
snap); and El is a soft limit on the norm of time derivatives Quadratic derivative cost is used to penalize the integral
(velocity, acceleration, jerk and snap) over the trajectory. over square derivatives of the trajectory (acceleration, jerk,
and snap), and is defined as follows:
A. Endpoint cost function 4 Z tmax
X
The purpose of the endpoint cost function is to keep the Eep = λqi (p(i) (t))2 dt. (19)
tmin
local trajectory close to the global one. This is achieved by i=2
 Soft Limit Cost Function
Mean
1000 Mean
Success Norm.
Algorithm Compute
Fraction Path
800 time [s]
Length
600 Inf. RRT* + Poly 0.9778 1.1946 2.2965
RRT Connect + Poly 0.9444 1.6043 0.5444
400 CHOMP N = 10 0.3222 1.0162 0.0032
CHOMP N = 100 0.5000 1.0312 0.0312
200 CHOMP N = 500 0.3333 1.0721 0.5153
[17] S = 2 jerk 0.4889 1.1079 0.0310
0 [17] S = 3 vel 0.4778 1.1067 0.0793
0 2 4 6 8 10
[17] S = 3 jerk 0.5000 1.0996 0.0367
[17] S = 3 jerk + Restart 0.6333 1.1398 0.1724
Fig. 5: Soft limit cost function l(x) proposed in Section V-D [17] S = 3 snap + Restart 0.6222 1.1230 0.1573
for pmax equals three (red), six (green), and nine (blue). This [17] S = 3 snap 0.5000 1.0733 0.0379
function acts as a soft limit on the time derivatives of the [17] S = 4 jerk 0.5000 1.0917 0.0400
trajectory (velocity, acceleration, jerk, and snap) to ensure they [17] S = 5 jerk 0.5000 1.0774 0.0745
Ours C = 2 0.4777 1.0668 0.0008
are bounded and are feasible to execute by the MAV. Ours C = 3 0.4777 1.0860 0.0011
Ours C = 4 0.4888 1.1104 0.0015
Ours C = 5 0.5111 1.1502 0.0021
Ours C = 6 0.5555 1.1866 0.0028
The above function has a closed-form solution for trajectory Ours C = 7 0.5222 1.2368 0.0038
segments represented using B-splines. Ours C = 8 0.4777 1.2589 0.0054
Ours C = 9 0.5777 1.3008 0.0072
D. Derivative limit cost function
TABLE I: Comparison of different path planning approaches.
To make sure that the computed trajectory is feasible, we
All results except thouse of the presented study are taken
must ensure that velocity, acceleration and higher derivatives
from [17]. Our approach performs similarly to approaches
of position remain bounded. This requirement can be included
using polynomial splines without restarts, which indicates that
into the optimization as a constraint ∀t : p(k) (t) < pkmax , but
B-splines can represent trajectories similar to those repre-
in the proposed approach, we formulate it as a soft constraint
sented by polynomial splines. Lower computation times of
by using the following function:
our approach can be explained by the fact that unconstrained
X4 Z tmax optimization occurs directly on the control points, unlike other
Eep = l(p(i) (t))dt, (20) approaches where the problem must first be transformed into
i=2 tmin
an unconstrained form.
where l(x) is defined as follows:
(
exp((p(k) (x))2 − (pkmax )2 ) − 1 if p(k) (x) > pkmax After optimization, the first control point from the points
l(x) =
0 if p(k) (x) ≤ pkmax that were optimized is fixed and sent to the MAV position
(21) controller. Another control point is added to the end of the
This allows us to minimize this cost function by using any spline, which increases tep and moves the endpoint further
algorithm designed for unconstrained optimization. along the global trajectory.
For optimization we use [10], which provides an interface
E. Implementation details to several optimization algorithms. We have tested the MMA
To run the local replanning algorithm on the drone, we first [24] and BFGS [13] algorithms for optimization, with both
compute a global trajectory by using the approach described in algorithms yielding similar performance.
[21]. This gives us a polynomial spline trajectory that avoids
VI. R ESULTS
all mapped obstacles. Thereafter, we initialize our replanning
algorithm with six fixed control points at the beginning of In this section, we present experimental results obtained
the global trajectory and C control points that need to be using the proposed approach. First, we evaluate the mapping
optimized. and the trajectory optimization components of the system
In every iteration of the algorithm we set the endpoint separately for comparison with other approaches and justify
constraints (Sec. V-A) to be the position and velocity at tep their selection. Second, we evaluate the entire system in
of the global trajectory. The collision cost (Sec. V-B) of the a realistic simulator in several different environments, and
trajectory is evaluated using a circular buffer that contains finally, present an evaluation of the system running on real
measurements obtained using the RGB-D camera mounted hardware.
on the drone. The weights of quadratic derivatives cost (Sec.
V-C) are set to the same values as those used for global A. Three-dimensional circular buffer performance
trajectory generation, and the limits (Sec. V-D) are set 20% We compare our implementation of the 3D circular buffer
higher to ensure that the global trajectory is followed with the to the popular octree-based solution of [9]. Both approaches
appropriate velocity while laterally deviating from it. use the same resolution of 0.1 m. We insert the depth maps
 Inserting
Computing SDF Trajectory
Moving mea-
Operation 3D compu- opti-
volume sure-
points tation mization
ments
Time
0.265 0.025 0.518 9.913 3.424
[ms]

TABLE II: Mean computation time for operations involved in

trajectory replanning in the simulation experiment with depth
map measurements sub-sampled to 160×120 and seven control
points optimized.

Fig. 6: Result of local trajectory replanning algorithm running C. System simulation

in a simulator on the forest dataset. The global trajectory To further evaluate our approach, we perform a realistic
is visualized in purple, local trajectory is represented as a simulation experiment by using the Rotors simulator [8]. The
uniform quintic B-spline, and its control points are visualized main source of observations of the obstacles is a simulated
in cyan. Ground-truth octomap forest model is shown for RGB-D camera that produces VGA depth maps at 20 FPS.
visualization purposes. To control the MAV, we use the controller developed by Lee
et al. [12], which is provided with the simulator and modified
to receive trajectory messages as control points for uniform B-
sub-sampled to the resolution of 160 × 120, which come splines. When there are no new commands with control points,
from a real-world dataset [23]. The results (Fig. 2) show that the last available control point is duplicated and inserted into
insertion of the data is more than an order of magnitude the B-spline. This is useful from the viewpoint of failure case
faster with the circular buffer, but only a limited space can because when an MAV does not receive new control points,
be mapped with this approach. Because we need the map of a it will slowly stop at the last received control point.
bounded neighborhood around the drone for local replanning, We present the qualitative results of the simulations shown
this drawback is not significant for target application. in Figures 1 and 6. The drone is initialized in free space
and a global path through the world populated with obstacles
B. Optimization performance is computed. In this case, the global path is computed to
To evaluate the trajectory optimization we use the forest intersect the obstacles intensionally. The environment around
dataset from [17]. Each spline configuration is tested in 9 the drone is mapped by inserting RGB-D measurements into
environments with 10 random start and end positions that the circular buffer, which is then used in the optimization
are at least 4 m away from each other. Each environment procedure described above.
is 10 × 10 × 10 m3 in size and is populated with trees In all presented simulation experiments, the drone can
with increasing density. The optimization is initialized with compute a local trajectory that avoids collisions and keeps it
a straight line and after optimization, we check for collisions. close to the global path. The timings of the various operations
For all the approaches, the success fraction, mean normalized involved in trajectory replanning are presented in Table II.
path length, and computation time are reported (Table I).
The results of the proposed approach are similar in terms D. Real-world experiments
of success fraction to those achieved with polynomial splines
from [17] without restarts, but the computation times with the We evaluate our system on a multicopter in several outdoor
proposed approach are significantly shorter. This is because the experiments (Fig. 4). In these experiments, the drone is initial-
unconstrained optimization employed herein directly optimizes ized without prior knowledge of the map and the global path
the control points, while in [17], a complicated procedure to is set as a straight line with its endpoint in front of the drone
transform a problem to the unconstrained optimization form 1 m above the ground. The drone is required to use onboard
[21] must be applied. sensors to map the environment and follow the global path
Another example of the proposed approach for trajectory avoiding trees, which serve as obstacles.
optimization is shown in Figure 3, where a global trajectory We use the AscTec Neo platform equipped with a stereo
is generated through a pre-defined set of points with an camera for estimating drone motion and an RGB-D camera
obstacle placed in the middle. The optimization is performed (Intel Realsense R200) for obstacle mapping. All computations
as described in Section V-E, with the collision threshold τ set are performed on the drone on a 2.1 GHz Intel i7 CPU.
to 0.5 m. As can be seen in the plot, the local trajectory in In all presented experiments, the drone can successfully
the collision free regions aligns with the global one, but when avoid the obstacles and reach the goal position. However, the
an obstacle is encountered, a smooth trajectory is generated robustness of the system is limited at the moment owing to
to avoid it and ensure that the MAV returns to the global the accuracy of available RGB-D cameras that can capture
trajectory. outdoor scenes.
 VII. C ONCLUSION [10] Steven G. Johnson. The nlopt nonlinear-optimization
package. URL http://ab-initio.mit.edu/nlopt.
In this paper, we presented an approach to real-time local
[11] Dongwon Jung and Panagiotis Tsiotras. On-line path
trajectory replanning for MAVs. We assumed that the global
generation for small unmanned aerial vehicles using B-
trajectory computed by an offline algorithm is provided and
spline path templates. In AIAA Guidance, Navigation
formulated an optimization problem that replans the local
and Control Conference and Exhibit, 2008.
trajectory to follow the global one while avoiding unmodeled
[12] Taeyoung Lee, Melvin Leoky, and N Harris McClam-
obstacles.
roch. Geometric tracking control of a quadrotor uav on
We improved the optimization performance by representing
SE(3). In Conference on Decision and Control, 2010.
the local trajectory with uniform B-splines, which allowed us
[13] Dong Liu and Jorge Nocedal. On the limited memory
to perform unconstrained optimization and reduce the number
BFGS method for large scale optimization. Mathematical
of optimized parameters.
Programming, 1989.
For collision checking we used the well-known concept
[14] D. Mellinger and V. Kumar. Minimum snap trajectory
of circular buffer to map a fixed area around the MAV,
generation and control for quadrotors. In International
which improved the insertion times by an order of magnitude
Conference on Robotics and Automation, 2011.
compared to those achieved with an octree-based solution.
[15] Mark Mueller, Markus Hehn, and Raffaello D’Andrea.
In addition, we presented an evaluation of the complete
A computationally efficient algorithm for state-to-state
system and specific sub-systems in realistic simulations and
quadrocopter trajectory generation and feasibility verifi-
on real hardware.
cation. In Intelligent Robots and Systems, 2013.
ACKNOWLEDGMENTS [16] Matthias Nießner, Michael Zollhöfer, Shahram Izadi, and
Marc Stamminger. Real-time 3d reconstruction at scale
This work has been partially supported by grant CR 250/9- using voxel hashing. Transactions on Graphics, 2013.
2 (Mapping on Demand) of German Research Foundation [17] Helen Oleynikova, Michael Burri, Zachary Taylor, Juan
(DFG) and grant 608849 (EuRoC) of European Commission Nieto, Roland Siegwart, and Enric Galceran. Continuous-
FP7 Program. time trajectory optimization for online UAV replanning.
We also thank the authors of [17] for providing their dataset In International Conference on Intelligent Robots and
for evaluation of the presented method. Systems, 2016.
[18] Helen Oleynikova, Zachary Taylor, Marius Fehr, Juan
R EFERENCES Nieto, and Roland Siegwart. Voxblox: Building 3d
[1] Markus W Achtelik, Simon Lynen, Stephan Weiss, signed distance fields for planning. arXiv preprint
Margarita Chli, and Roland Siegwart. Motion-and arXiv:1611.03631, 2016.
uncertainty-aware path planning for micro aerial vehicles. [19] Aditya Paranjape, Kevin C Meier, Xichen Shi, Soon-Jo
Journal of Field Robotics, 2014. Chung, and Seth Hutchinson. Motion primitives and
[2] John Amanatides and Andrew Woo. A fast voxel traver- 3d path planning for fast flight through a forest. The
sal algorithm for ray tracing. In Eurographics 87, 1987. International Journal of Robotics Research, 2015.
[3] Michael Burri, Helen Oleynikova, , Markus W. Achtelik, [20] Kaihuai Qin. General matrix representations for B-
and Roland Siegwart. Real-time visual-inertial mapping, splines. In Sixth Pacific Conference on Computer Graph-
re-localization and planning onboard MAVs in unknown ics and Applications, 1998.
environments. In Intelligent Robots and Systems, 2015. [21] Charles Richter, Adam Bry, and Nicholas Roy. Polyno-
[4] Maurice G Cox. The numerical evaluation of B-splines. mial trajectory planning for aggressive quadrotor flight in
IMA Journal of Applied Mathematics, 1972. dense indoor environments. In Robotics Research. 2016.
[5] Carl De Boor. On calculating with B-splines. Journal of [22] Frank Steinbrücker, Jürgen Sturm, and Daniel Cremers.
Approximation theory, 1972. Volumetric 3D mapping in real-time on a cpu. In IEEE
[6] Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo, Conference on Robotics and Automation, 2014.
and James Diebel. Practical search techniques in path [23] Jürgen Sturm, Nikolas Engelhard, Felix Endres, Wolfram
planning for autonomous driving. Ann Arbor, 2008. Burgard, and Daniel Cremers. A benchmark for the
[7] Pedro F Felzenszwalb and Daniel P Huttenlocher. Dis- evaluation of RGB-D SLAM systems. In Intelligent
tance transforms of sampled functions. Theory of Com- Robots and Systems, 2012.
puting, 2012. [24] Krister Svanberg. A class of globally convergent opti-
[8] Fadri Furrer, Michael Burri, Markus Achtelik, and mization methods based on conservative convex separa-
Roland Siegwart. Robot operating system (ROS). Studies ble approximations. Journal on Optimization, 2002.
in Computational Intelligence, 2016. [25] Matt Zucker, Nathan Ratliff, Anca D Dragan, Mihail Piv-
[9] Armin Hornung, Kai Wurm, Maren Bennewitz, Cyrill toraiko, Matthew Klingensmith, Christopher M Dellin,
Stachniss, and Wolfram Burgard. OctoMap: An efficient J Andrew Bagnell, and Siddhartha S Srinivasa. CHOMP:
probabilistic 3D mapping framework based on octrees. Covariant hamiltonian optimization for motion planning.
Autonomous Robots, 2013. The International Journal of Robotics Research, 2013.