AUTHOR’S VERSION

1

An Efficient B-spline-Based Kinodynamic
Replanning Framework for Quadrotors

arXiv:1906.09785v1 [cs.RO] 24 Jun 2019

Wenchao Ding, Wenliang Gao, Kaixuan Wang, and Shaojie Shen

Abstract—Trajectory replanning for quadrotors is essential
to enable fully autonomous flight in unknown environments.
Hierarchical motion planning frameworks, which combine path
planning with path parameterization, are popular due to their
time efficiency. However, the path planning cannot properly deal
with non-static initial states of the quadrotor, which may result
in non-smooth or even dynamically infeasible trajectories. In this
paper, we present an efficient kinodynamic replanning framework
by exploiting the advantageous properties of the B-spline, which
facilitates dealing with the non-static state and guarantees safety
and dynamical feasibility. Our framework starts with an efficient
B-spline-based kinodynamic (EBK) search algorithm which finds
a feasible trajectory with minimum control effort and time. To
compensate for the discretization induced by the EBK search,
an elastic optimization (EO) approach is proposed to refine
the control point placement to the optimal location. Systematic
comparisons against the state-of-the-art are conducted to validate
the performance. Comprehensive onboard experiments using two
different vision-based quadrotors are carried out showing the
general applicability of the framework.
Index Terms—Aerial systems: perception and autonomy, motion and path planning, collision avoidance, trajectory planning.

Fig. 1: Illustration of the motivating example. The initial state has
non-zero velocity (red arrow). The traditional geometric planner finds
the shortest path (red squares) and then parameterizes it using a piecewise polynomial (blue). However, the local path parameterization is
restricted to a homotopy class (blue area), and the resultant trajectory
is jerky (even infeasible) with respect to the given time allocation. In
contrast, our framework produces a dynamically feasible trajectory
(purple line) using kinodynamic planning.

I. I NTRODUCTION
UTONOMOUS navigation for quadrotors in unknown
environments has gained significant interest for its practical usage in various inspection and exploration tasks. To
fulfill the need of fully autonomous exploration in unknown
environments, trajectory replanning is of great significance.
Replanning requires a real-time response to unexpected obstacles to guarantee safety while satisfying the low-level
feasibility constraints induced by the non-trivial dynamics.
Many existing methods [1]–[5] tackle this challenging
problem using a hierarchical framework, which first finds
a geometric path and then locally optimizes the path to a
dynamically feasible trajectory with respect to a given time
allocation. Although this framework is efficient, inadequacy

A

Accepted final version. To Appear in IEEE Transactions on Robotics.
©2019 IEEE. Personal use of this material is permitted. Permission from
IEEE must be obtained for all other uses. This work was supported by Hong
Kong PhD Fellowship Scheme, HKUST-DJI Joint Innovation Laboratory.
(Corresponding author: Wenchao Ding.)
The authors are with the Department of Electronic and Computer Engineering, Hong Kong University of Science and Technology, Hong Kong,
China (email: wdingae@ust.hk; wenliang.gao@ust.hk; kwangap@ust.hk;
eeshaojie@ust.hk).
This paper has supplementary downloadable multimedia material available
at http://ieeexplore.ieee.org.
Color versions of one or more of the figures in this paper are available
online at http://ieeexplore.ieee.org.

exists between the path finding and the local path parameterization. Specifically, the parameterization may be restricted by
the geometric path to a homotopy class that does not contain
a globally optimal (or even feasible) solution, especially when
faced with non-static initial states (non-zero velocities or any
other higher-order derivatives), as shown in Fig. 1.
To address the limitations of the hierarchical methods,
it is essential to use kinodynamic motion planners, which
directly find time-parameterized trajectories that are globally
optimal with respect to control efforts and dynamical limits.
The incorporation of kinodynamic planners into replanning
facilitates dealing with non-static initial states and enhances
replanning consistency.
Sampling-based motion planning algorithms, such as
rapidly-exploring random trees (RRTs) [6] and their variants [7]–[11], are popular in the kinematic/kinodynamic planning literature. Asymptotical optimality has been proved for
some of them [7]–[10]. However, when applied to complex
kinodynamic systems, they typically require solving a computationally expensive non-linear two-point boundary value
problem (BVP) [12, 13] and cannot run in real-time. Liu et
al. [14] explored a search-based counterpart and proposed a
heuristic-guided resolution-complete (optimal with respect to

©2019 IEEE. IEEE Transactions on Robotics. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses.

2

AUTHOR’S VERSION

(a) Monocular Vision-Based Quadrotor Testbed

(b) Dual-fisheye Vision-Based Quadrotor Testbed

Fig. 2: Illustration of our (a) monocular vision-based quadrotor
testbed and (b) dual-fisheye vision-based quadrotor testbed.

discretization) search method using linear quadratic minimum
time control. However, for high-order kinodynamic systems or
a large dynamic range, the run-time efficiency is inadequate.
In this paper, we present a kinodynamic replanning framework which addresses the efficiency bottleneck. Our framework starts with an efficient B-spline-based kinodynamic
(EBK) search algorithm, which finds B-spline control points
on a spatial grid. We introduce the novel concept of vertex
tuple to keep the search problem simple and analyzable,
which enables a thorough theoretical characterization of the
problem. Built on top of the structure of the optimal solution,
a graph aggregation technique is proposed to minimize the
computation time through a controllable discretization of the
search space. An offline-computable minimum inflation is
adopted to avoid unnecessary collision checking and further
accelerate the online search. Compared to state-of-the-art
methods (Sect. VIII), our kinodynamic search finds the lowestcost dynamically feasible trajectories in real time.
To compensate for the discretization in the search, an
elastic optimization (EO) approach is proposed to refine the
control point placement to the optimal location, by solving
a convex quadratically constrained quadratic programming
(QCQP) problem. The two components are integrated into a
receding horizon replanner based on the local control property
of the B-spline.
Our replanning framework is not only theoretically analyzed, but also validated in simulated and onboard experiments. Superior performance is shown through comprehensive

comparisons against the state-of-the-art kinodynamic planning
and trajectory optimization methods. Moreover, the practical
impact of our framework is verified through onboard experiments using a monocular vision-based quadrotor and a dualfisheye vision-based quadrotor in unknown indoor and outdoor
environments. We summarize our contributions as follows:
• An EBK search algorithm which provides an initial-stateaware dynamically feasible time-parameterized trajectory.
• An EO approach that refines the control point placement
to the optimal location while preserving the safety and
dynamical feasibility.
• Systematic comparisons against the state-of-the-art showing
the superior performance of the proposed framework.
• Integration of our framework into a real monocular visionbased quadrotor and a dual-fisheye vision-based quadrotor as well as extensive experiments demonstrating fully
autonomous navigation in unknown, complex indoor and
outdoor environments.
A basic version of our framework was originally presented in [15], where we introduced the real-time B-splinebased kinodynamic (RBK) search. Although the RBK search
achieves high computational efficiency, the absence of theoretical optimality analysis in [15] limits confidence in its
solution quality and further limits its theoretical impact. In
this paper, instead of directly developing efficient methods, we
tackle the kinodynamic search problem in a systematic way:
1) We first characterize the complexity and optimal solution
of the search problem using a novel vertex tuple structure. 2)
We then establish the quality-efficiency tradeoff using a novel
graph aggregation technique, which provides a user-specified
parameter to control algorithm efficiency and solution quality.
The above two theoretical additions render the more flexible
and theoretically reliable EBK search. It also turns out that
the preliminary version in [15] is perfectly contained in
the EBK search. Apart from the theoretical additions, more
comprehensive experimental analyses in a wide variety of
environments are presented to support the new characteristics.
The relevant literature is discussed in Sect. II. An overview
of the proposed replanning framework is provided in Sect. III.
Mathematical background and advantageous properties of Bspline are introduced in Sect. IV. The problem formulation and
algorithm detail of the EBK search are elaborated in Sect. V.
The EO approach is presented in Sect. VI. Implementation
details are given in Sect. VII. Systematic comparisons against
the state-of-the-art methods are provided in Sect. VIII, and
onboard experimental results are illustrated in Sect. IX. Finally, a conclusion and further possible research directions are
provided in Sect. X.
II. R ELATED W ORKS
There is extensive literature on motion planning techniques
for quadrotors from various perspectives, such as controlbased methods [16]–[18], search-based methods [14, 19]–[21],
sampling-based methods [7, 9, 22]–[24] and optimizationbased methods [2, 5, 25, 26]. It is difficult to give a full
literature review of all these techniques, so in this section,

DING et al.: AN EFFICIENT B-SPLINE-BASED KINODYNAMIC REPLANNING FRAMEWORK FOR QUADROTORS

3

Fig. 3: A diagram of our kinodynamic replanning framework together with state estimation and mapping modules.

we choose the most relevant and organize them into two
categories, namely, hierarchical motion planning techniques
and kinodynamic motion planning techniques.

problem from the kinodynamic planning perspective, which
is discussed in Sect. II-B.
B. Kinodynamic Motion Planning

A. Hierarchical Motion Planning
Hierarchical motion planning refers to a high-level geometric path planner coupled with a low-level time parameterization scheme. The high-level geometric planner is concerned with finding an obstacle-free path, while the low-level
parameterization scheme takes care of the vehicle dynamical
constraints and generates a time-parameterized trajectory for
execution. For quadrotors which have non-trivial dynamics,
directly generating a trajectory in the high-dimensional state
space is time consuming, while smoothing a given geometric path is computationally efficient (with a suitable relaxation) [27]. As such, the hierarchical framework is popular for
quadrotors, and it enables a number of online methods [1]–[5].
Two pioneering works [1, 3] extract waypoints from the geometric path and formulate the trajectory generation problem
as quadratic programming (QP) on polynomial coefficients.
These methods are based on the differential flatness of the
quadrotor [1]. Due to the deviation of the polynomial trajectory from the straight-line collision-free path, an iterative
waypoint insertion scheme is adopted [3]. However, how many
additional waypoints are needed is not quantified. Chen et
al. [5] propose a corridor-based geometric planner based on
the octree-based map structure [28]. The control effort can be
reduced by generating the trajectory in a series of connected
cubes. Apart from that, they propose an iterative process of
adding constraints on polynomial extremas to cope with the
deviation from the corridor, and prove that a finite number of
iterations is needed to guarantee safety. Liu et al. [4] further
generalize the corridor representation to a series of connected
convex polygons.
Although the hierarchical methods have made significant
achievements, they suffer from the common problem that
the geometric planner is unaware of the vehicle dynamics,
resulting in inadequacy between the path planning and path
parameterization, especially when faced with non-static initial
states. The example in Fig. 1 motivates us to explore the

The kinodynamic motion planner directly explores the highdimensional state space, and outputs a time-parameterized
trajectory, which fundamentally avoids the inadequacy between path planning and parameterization. RRTs [6] and their
variants [7]–[11, 22] were originally designed for kinematic
systems and can be easily extended to kinodynamic systems.
These methods provide an efficient way of exploring the highdimensional state space, and some of them possess asymptotical optimality [7]–[10]. However, for robots with non-trivial
dynamics, the tree expansion typically involves solving the
BVP, which is non-linear and challenging. Webb et al. [7]
propose a fixed-final-state-free-final-time optimal controller
which solves the BVP for linear (or linearized) controllable
systems in closed form. Xie et al. [12] propose an efficient
BVP solver for general kinodynamic systems using sequential
quadratic programming (SQP). Li et al. [13] work in another
direction, namely, expanding the tree using random control
propagation, for cases where system models are complex and
BVP solvers are not available.
Despite the fact that the efficiency of the kinodynamic
planning techniques keeps improving [12, 13], it is still
prohibitively expensive for replanning. Allen et al. [23] work
towards a real-time kinodynamic planning framework by combining FMT* [10] with a support vector machine (SVM) for
the classification of the reachable set. This framework [23]
reduces the calling of the BVP solver to gain efficiency.
However, the solution quality largely depends on the number
of states pre-sampled. On the other hand, Liu et al. [14]
explore the search-based kinodynamic planning counterpart
and develop efficient heuristics by solving a linear quadratic
minimum time problem. Their solution is resolution-complete
with respect to the discretization on the control input, and
achieves near real-time performance. Note that both [14]
and [23] use a simplified system model, i.e., a double or triple
integrator, to reduce the computation complexity. However, the
resultant trajectory only has limited continuity. To improve the

4

AUTHOR’S VERSION

smoothness, both [14] and [23] adopt trajectory reparameterization using the unconstrained QP formulation [3], which may
break the dynamical feasibility and safety.
In contrast, the proposed EBK search adopts a high-order
B-spline parameterization with continuity up to snap, which
can be directly used to control the quadrotor. Moreover, the
advantageous properties of the B-spline facilitate the kinodynamic replanning as follows:
• Local control property for incrementally constructing the
B-spline trajectory in the kinodynamic search and local
refinement of the trajectory during replanning.
• Convex hull property for enforcing collision-free constraints and providing a dynamical feasibility guarantee
for the entire trajectory.
Given the same run-time budget according to the real-time
requirement, the EBK search finds a lower-cost trajectory,
as validated by the comprehensive comparisons against the
state-of-the-art in Sect. VIII. Apart from this, the proposed
refinement module, namely, the EO approach, can preserve
the safety and dynamical feasibility by taking advantage of
the convex hull property of the B-spline.
III. OVERVIEW
The structure of the proposed framework is shown in
Fig. 3. The replanning framework is built on top of the state
estimation and dense mapping module, which are discussed
in Sec. VII. The frequency of the grid map update is 10
Hz. As such, the real-time requirement in this paper refers
to a run-time of less than 100 ms. The updated map and
the initial state of the quadrotor are fed to the EBK search
module (Sect. II-B), and the replanning strategy is elaborated
in Sect. VII. The control points are constantly refined by the
proposed EO approach (Sect. VI), which consists of an elastic
tube expansion module (Sec. VI-A) for free space characterization and a convex optimization formulation (Sec. VI-B)
for trajectory refinement. The confirmed control points are
evaluated, and position commands are generated accordingly.
IV. B- SPLINE C URVE AND R EPLANNING
For the proposed kinodynamic planning framework, we
adopt a B-spline parameterization for its advantageous properties, namely, local control and convex hull property, and we
further adopt the uniform B-spline for its convenient closedform evaluations. In this section, we elaborate these properties
and explain how they can be applied to the replanning system.
Given n + 1 control points p0 , p1 , . . . , pn and knot vector
{t0 , t1 , . . . , tm }, the B-spline curve s(t) of degree k is defined
as follows:
n
X
s(t) =
pi Ni,k (t),
(1)
i=0

where Ni,k (t) is the B-spline blending function of degree k,
which can be evaluated recursively as follows:
(
1 if ti ≤ t < ti+1
Ni,0 (t) =
0 otherwise
ti+k+1 − t
t − ti
Ni,k−1 (t) +
Ni+1,k−1 (t).
Ni,k (t) =
ti+k − ti
ti+k+1 − ti+1
(2)

The total number of knots should satisfy m+ 1 = n+k + 2.
The uniform B-spline is a special type of B-spline whose
knot vector is uniformly distributed. Suppose the knot vector
is separated with equidistance ∆t . The half-open interval
[ti , ti+1 ) is called the i−th knot span. We normalize each
knot span using u = (t − ti )/∆t , and for the i-th knot span,
only k + 1 blending functions are non-zero, corresponding
to k + 1 control points pi−k , . . . , pi . We stack the k + 1
control points and call the stacked coordinate matrix a control
|
point span Pi−k := [pi−k pi−k+1 · · · pi ] ∈ R(k+1)×3 . Since
the blending functions Ni,k (t) are shifted versions of each
other for the uniform B-spline, we have closed-form matrix
representations [29] for parametric evaluation. Let j = i − k,
and the position and the derivatives of the B-spline curve
corresponding to the j-th control point span can be evaluated
as follows:
1 db|
dsj (u)
=
Mk Pj ,
(3)
l
dl u
(∆t) dl u
where l denotes the order of the derivative
| (l = k+10
means the position), b = 1 u u2 · · · uk
∈ R
denotes the basis vector, and Mk = (mi,j ) ∈
R(k+1)×(k+1)
the blending matrix, where mi,j =
 Pk denotes
s−j k+1
k−i
k
1
(−1)
. According to Eq. 3,
s=j
k! k−i
s−j (k − s)
the evaluation of the derivatives of the B-spline curve can
be expressed by a linear matrix multiplication in terms of
the control point span Pj . The paper uses a quintic uniform
B-spline (k = 5) to ensure the continuity up to snap for
controlling quadrotors.
As described by Mellinger [1], the control cost of a quadrotor is closely related to the integral over squared derivatives of
the planned trajectory, which can also be evaluated in closed
form in the case of the uniform B-spline. The total control
cost Ejl of the j-th control point span can be expressed by the
integral over the squared derivatives of degree l (e.g., for the
min-snap trajectory, l = 4) as follows:
2
Z 1
dsj (u)
du = P|j M|k Ql Mk Pj ,
(4)
Ejl =
lu
d
0
 db |
R1
2l−1
where Ql = 0 ddb
du/(∆t )
is the Hessian
lu
dl u
matrix of the l-th squared derivative, which is constant for the
uniform B-spline. The control cost Ejl is quadratic with respect
to the control point span Pj . Note that the cost evaluation of
a span only depends on the stacked control point coordinates
of this span.
A. Local Control Property and Replanning
The local control is one of the important properties of
B-spline, making it suitable for replanning. Specifically, the
evaluation of any point of the B-spline curve is controlled by
a single control point span containing k +1 control points, and
any control point only affects k + 1 control point spans. We
incorporate the local control property into a receding horizon
(re-)planner, and we divide the planned trajectory into three
types, namely, executed trajectory, executing trajectory and
optimizing trajectory. The executed trajectory means the part
of the trajectory which has already been executed, the executing trajectory means the part of the trajectory corresponding

DING et al.: AN EFFICIENT B-SPLINE-BASED KINODYNAMIC REPLANNING FRAMEWORK FOR QUADROTORS

5

1
2
0.8
1
0.6
0
0.4
-1

replan

0.2
-2
0
0

Fig. 4: Illustration of the B-spline local control property and its
application to the replanning system. The control points are shown by
squares. The control points corresponding to the executing trajectory
are shown in green. The original locations of the control points ahead
of the executing control point are in orange, and their subsequently
modified positions are marked in red. Due to the locality of the Bspline, the replan causes no perturbation to the executing trajectory.

0.5

1

-2

-1

0

1

2

-2

-1

0

1

2

1
2
0.8
1
0.6
0
0.4
-1
0.2
-2

to the control point span being executed, and the optimizing
trajectory means the part of the trajectory whose supporting
control points are potentially under optimization.
Thanks to the local control property, modification of the
supporting control points of the optimizing trajectory will not
affect the evaluation of the executing trajectory, as shown
in Fig. 4. Unlike [30] and [31] where local reshaping may
cause the violation of dynamical constraints, the dynamical
feasibility of the executing trajectory can be preserved by
leveraging the local control property. Moreover, the locality
also makes it possible to optimize any subset of control points
without re-generation of the whole trajectory, which is computationally efficient. The locality also helps to preserve a smooth
trajectory since the next executing control point span always
shares k control points with the current executing control point
span, yielding k-th-order continuity and a consistent trajectory.

B. Convex Hull Property and Dynamical Feasibility
Another important property of the B-spline is the convex
hull property. A B-spline (or Bèzier [32]) trajectory is strictly
bounded inside the convex hull supported by the corresponding
control point span. Strictly speaking, dynamical feasibility
should be induced by the robot’s kinematic and dynamical constraints. Given polynomial/spline parameterization, a common
practice to enforce dynamical feasibility is to use maximum
velocity and maximum acceleration bounds [4, 33, 34]. We
follow this practice in this paper. Note that for piecewise
polynomial parameterization methods [2, 4], the dynamical
feasibility constraints are enforced on a finite number of
checkpoints. Denser checkpoints will enhance the robustness
but yield higher computation complexity. In contrast, by using
the convex hull property, the entire velocity and acceleration
profile can be strictly bounded.
We utilize the fact that the derivative of the B-spline of
degree k is a B-spline of degree k − 1, which also enjoys
the convex hull property. Therefore, if the supporting control
points are bounded inside the convex hull expanded by the
allowed maximum derivative, the derivative spline is subsequently bounded, as elaborated in Prop. 1. Note that Prop. 1
is a sufficient but not necessary condition. A toy example

0
0

0.5

1

Fig. 5: Illustration of the convex hull property. The dashed red box
shows the feasible velocity hull (1.2 m/s for each axis). Applying
Prop. 1 under the objective of minimum change to control point
positions (bottom left figure), the resulting velocity profile is shown
at the bottom right, where the velocity profile is strictly bounded.

illustrating the relation between the convex hull property and
dynamical feasibility is shown in Fig. 5.
Proposition 1. Given a uniform B-spline of degree k and
knot separation ∆t , there exists a constant linear combinal
(k+1)×(k+1)
tion Sl = M−1
such that
k Cl Mk /(∆t ) ∈ R
min
D
max
ul,D 1(k+1)×1 ≤ Sl P ≤ ul,D 1(k+1)×1 is a sufficient condition for the derivative along coordinate D to be thoroughly
dsD (u)
bounded; i.e., umin
≤ umax
l,D ≤
l,D , ∀u ∈ [0, 1], where
dl u
PD ∈ Rk+1 is coordinate D ∈ {x, y, z} of the control point
span P, and Cl ∈ R(k+1)×(k+1) is a constant mapping matrix
1
of the l-th derivative satisfying ddb
l u = Cl b.
Proof. Please refer to Appendix A for the detailed proof.



V. B- SPLINE -BASED K INODYNAMIC S EARCH
A. Motivating Example
As shown in the motivating example in Fig. 1, hierarchical
motion planners may produce sub-optimal or even dynamically infeasible trajectories given a non-static initial state of
the quadrotor. The reason is that the geometric planner has
no knowledge about the vehicle dynamics and restricts the
solution space of path parameterization to a homotopy class
of the geometric shortest path. The inadequacy motivates us
to propose an efficient kinodynamic planning algorithm which
can work in real-time. However, kinodynamic planning is
typically time consuming [14, 23]. The major computation
of the traditional kinodynamic planning lies in three tasks,
namely, covering the large state space, solving the BVP and
collision checking.
1 S and C are fixed given the degree k and the derivative order l.
l
l

6

Given the advantageous properties of the B-spline introduced in Sect. IV, we propose using uniform B-spline parameterization in kinodynamic planning, which facilitates reducing
the computation time for the above three tasks. Specifically,
we propose a spatial-grid-based deterministic graph search to
place B-spline control points. The proposed search algorithm
has three major features as follows:
• Controllable discretization of the state space: Due to the
locality of the B-spline, it is possible to incrementally
sample the B-spline control points during the search. A
vertex tuple structure is proposed to recover the Markovian assumption and make the problem analyzable. A
novel graph aggregation technique is proposed to control
the discretization of the state space, which achieves a
speed-quality tradeoff.
• Closed-form evaluations of control cost and dynamical
feasibility: The control cost and feasibility of the B-spline
can be evaluated in closed forms efficiently.
• Offline-computable inflation to avoid collision checking:
The maximum deviation of uniform B-spline from the
free-cells can be characterized offline and compensated
for by workspace inflation.
The kinodynamic search accounts for the total control efforts
and dynamical limits, which is a systematic way to deal with
the non-static initial states in replanning.

AUTHOR’S VERSION

Since the B-spline is evaluated in terms of the control
point span, we re-organize π by combining neighboring k + 1
vertices as one vertex tuple. Combining the sequence π
with πs and πg , the overall sequence can be formalized
as π̃ = (vs0 , . . . , vsk , v0 , . . . , vT , vg0 , . . . , vgk ). We define the
ordered sub-sequence containing consecutive k + 1 vertices
k
of π̃ as a k-degree vertex tuple, which is denoted by [π̃] . We
provide a toy example in Fig. 6 showing how 3-degree vertex
tuples can be constructed.
k
We denote by [π̃]j the j-th k-degree tuple in π̃, and two
k
k
neighboring tuples, [π̃]j−1 and [π̃]j , overlap for k vertices.
k
k
According to this convention, [π̃]0 = πs and [π̃]J = πg , where
J + 1 = k + T + 3. Each vertex tuple represents a short trajectory, and a sequence of neighboring vertex tuples represents a
continuous trajectory. We associate with each k-degree tuple
k
[π̃] a strictly positive cost function fk,∆t : [π̃] → R+ . Note
that the cost function has to be strictly positive to cope with
the possible repetition in π. We state the problem as follows:
Problem 1. Given a uniform B-spline of degree k and knot
separation ∆t , initial state πs and goal state πg , find admissible
control point placement π = (v0 , v1 , . . . , vT ) on G such that
the following cost function is minimized:
J
X
k
fk,∆t ([π̃]j ),
Jk,∆t (π̃) =
j=0
k

B. Problem Formulation
In this section, we formally present the problem of the
B-spline-based kinodynamic search on a spatial grid. The
problem is formalized as a deterministic graph search where
the action is the placement of the control points. Suppose the
topological graph associated with the grid map is denoted
as G := (V, E), where V is the set of vertices denoting
the collection of free cells and E denotes the set of edges
(i, j) ⊂ V × V between all adjacent vertices i and j. The
adjacency of the vertices depends on the grid connectivity
adopted. In this paper, every cell in the 3-D grid has 26
neighbors which are cells connected to this cell at a Chebyshev
distance of 1.2
Given uniform B-spline parameterization of degree k and
knot separation ∆t , the proposed search method finds a
finite sequence π = (v0 , v1 , . . . , vT ) of vertices representing an admissible control point placement which satisfies
vi ∈ V, (vi−1 , vi ) ∈ E for each i = 1, . . . , T and connects
the given initial state πs = (vs0 , vs1 , . . . , vsk ) and goal state
πg = (vg0 , vg1 , . . . , vgk ), i.e., (vsk , v0 ) ∈ E and (vT , vg0 ) ∈ E.
The sequence π possibly contains repetition since we allow
placing the control points at the same cell. πs and πg are
two tuples, both containing k + 1 vertices which form the
control point span according to the definition of the B-spline
in Sect. IV. Therefore, πs and πg actually represent two short
trajectories, different from the initial and goal positions used in
geometric planners and the position-velocity-acceleration state
vector used in these kinodynamic planners [7, 14, 23].
2 Note that the connectivity can also be defined based on a Chebyshev
distance larger than one, which will result in a non-uniform control point
placement and a higher computational complexity.

where [π̃]j is a k-degree vertex tuple, whose corresponding
trajectory should satisfy collision-free and dynamical feasibility requirements.
We adopt a cost function following the idea of the linear
quadratic minimum time control problem [14, 35], where the
control cost is represented by Eq. 4 with a tradeoff penalty
on the execution time ∆t . Mathematically, the cost function
fk,∆t is represented as follows:
Z 1 ds k (u) !2
[π̃]j
k
du,
(5)
fk,∆t ([π̃]j ) = λ∆t +
dl u
0
k

where [π̃]j can be rewritten into matrix form, as in Sect. IV;
the integral can be evaluated according to Eq. 4; and λ is the
weight for the trajectory execution time. The criterion to check
the collision-free and dynamical feasibility of the vertex tuple
is introduced in Sect. V-D.
C. Optimal B-spline-Based Kinodynamic Search
The difficulty of solving Prob. 1 is that the placement of
any control point depends on the placed k control points
(not only the predecessor) within the vertex tuple. Regarding
the placement of a single control point as an action, the
Markovian assumption does not hold, which makes traditional
graph search algorithms inapplicable. However, we find that
Problem 1 can be transformed into an equivalent standard
shortest path problem on a higher dimensional directed graph
GH = (VH , EH ) induced by G by regarding the whole
vertex tuple as a “vertex”. Since the vertex tuple is the basic
evaluation unit of the B-spline, the placement of the next
vertex tuple will only depend on its predecessor, which follows
the Markovian assumption. The transformation enables the

DING et al.: AN EFFICIENT B-SPLINE-BASED KINODYNAMIC REPLANNING FRAMEWORK FOR QUADROTORS

Fig. 6: Illustration of the construction process of 3-degree vertex
tuples from an admissible path. Each vertex tuple is formed by
combining four consecutive control points. [π̃]30 and [π̃]31 are two
neighboring vertex tuples since they overlap for three vertices.

usage of well-characterized shortest path search algorithms,
such as Dijkstra’s [36] and A* [37]. In the following, we
elaborate the transformation.
Similar to the construction process of the vertex tuple in
Sect. V-B, we construct GH as follows: 1) each vertex tuple
v̂i = (vi , vi+1 , . . . , vi+k ) ∈ VH is created by combining a
sequence of adjacent vertices of G satisfying (vj−1 , vj ) ∈ E
for j = i + 1, . . . , i + k; and 2) two vertices v̂i and v̂j on VH
are adjacent if and only if the last k vertices of v̂i overlap with
the first k vertices of v̂j . The construction of GH groups the
associated control point coordinates into a high-dimensional
state. Note that each dimension of the combined state has
the same physical meaning, i.e., the spatial coordinates of the
control point. This observation motivates us to come up with
a low-dispersion search algorithm, as presented in Sect. V-E.
In Fig. 7(a), we show an example of how the trace of control
points on G can be mapped to the path on GH . The initial
vertex tuple v̂s is shown by squares. Starting from v̂s , we
consider two directions of the next-step placement, which form
v̂0 and v̂1 , respectively. In a similar way, starting from v̂0 , two
expansion directions are considered, forming v̂3 and v̂4 , while
for v̂1 , one direction (v̂5 ) is considered. Note that although
the last vertex of v̂4 and v̂5 are in the same spatial cell, in
the induced high-dimensional graph GH , they are two distinct
vertex tuples. Following the expansion of control points, we
form a tree of vertex tuples on GH . From the expansion
process, we observe that, given the initial and goal vertex tuple,
the problem of finding the optimal control point placement is
equivalent to finding the shortest path on the graph GH . We
refer interested readers to Appendix B for further details.
Recall that in Sect. V-B, we allow the repetition of vertices
since some necessary vertex tuples rely on repetition; for
instance, the same vertex being repeated k + 1 times actually
represents a static state (for ∆t ). According to the definition
k
of fk,∆t ([π̃] ), the cost of GH is defined on vertices VH
instead of the edges. Although repetition is allowed, each
vertex v̂ ∈ VH of GH is associated with a strictly positive
cost so that repetition is properly penalized.
Note that any path on GH is a time-parameterized Bspline trajectory instead of a geometric path. The dynamical
feasibility and control cost are taken into account by evaluating
the corresponding short trajectories of the nodes of GH .
Problem 1 can be solved optimally using traditional labelcorrecting algorithms such as Dijkstra’s [36] and A* [37] on

7

Algorithm 1 Optimal B-spline-Based Kinodynamic Search
1: function I NITIALIZE (πs , πg , k, ∆t )
2:
O ← ∅; L ← ∅
3:
n ← I NDEX(πs );
4:
h(πs ) ← H EURISTIC(πs , πg ); g(πs ) ← fk,∆t (πs );
5:
O ← I NSERT(O, g(πs ) + h(πs ), πs );
6:
L ← I NSERT(L, n, πs )
7: function M AIN (πs , πg , G, k, ∆t )
8:
(O, L) ← I NITIALIZE(πs , πg , k, ∆t );
9:
while O =
6 ∅ do
10:
(m, v̂i ) ← P OP(O);
11:
if m = I NDEX(πg ) then
12:
return success
13:
for v̂j ∈ F EASIBLE S UCCS(v̂i , G, k, ∆t ) do
14:
n ← I NDEX(v̂j );
15:
if not V ISITED(n, L) then
16:
g(v̂j ) ← ∞
17:
if g(v̂j ) > g(v̂i ) + fk,∆t (v̂j ) then
18:
g(v̂j ) ← g(v̂i ) + fk,∆t (v̂j )
19:
h(v̂j ) ← H EURISTIC(v̂j , πg )
20:
O ← I NSERT(O, g(v̂j ) + h(v̂j ), v̂j )

the induced graph GH . The optimal control point placement
π ∗ can then be re-constructed. The optimal B-spline-based
kinodynamic (OBK) search algorithm is outlined in Algo. 1.
Typically, label-correcting algorithms maintain one or multiple sets of vertices as so-called fringes [38]. For example,
A* [37] maintains two fringes (known as the OPEN set and
the CLOSED set) to reduce the expansion of nodes and save
computation. Similarly, Algo. 1 maintains two fringes, namely,
the OPEN set (as denoted by O) and the VISITED set (as
denoted by L). The visited set L provides query and retrieving
functions for vertices. We use I NDEX(v̂) (Algo. 2) to assign a
unique integer index to each distinct vertex tuple. Specifically,
Algo. 2 collects the extracted coordinates I(v̂) (using the
C OORD(v) function) for each vertex v in the tuple v̂, and
uses the U NIQUE E NCODE(·) function to generate a unique
hash encoding for a series of integer coordinates I(v̂).
We construct the nodes of GH only on demand during the
search process as the graph size may be prohibitively large.
At the beginning of the search, the whole GH is not explicitly
constructed, and the visited set L and the open set O are
both initialized to empty. After the insertion of the initial
state πs , a tree of nodes is gradually expanded based on the
F EASIBLE S UCCS(·) function and the priority queue structure
maintained by O. Every time a new node is found (whether
or not a successor node is a new node is identified by L,
which is implemented using a hash map structure), the node
is added to L to trace its open/closed status. In practice, only a
small proportion of the nodes of GH are constructed before the
search succeeds. A toy example3 which compares the number
of actual expanded nodes with the estimated graph size is
shown in Tab. I. Note that the EBK method used in Tab. I is
an efficient version of Algo. 1, which is discussed in detail in
3 The setup of this example is the same as the qualitative experiment in
Fig. 10, where the grid size is 51 × 51 × 5 and the B-spline degree is five.

8

AUTHOR’S VERSION

Algorithm 2 Given a k-degree vertex tuple v̂ ∈ VH , assign
a unique index to each distinct tuple.
1: function I NDEX (v̂)
2:
I(v̂) = ∅;
3:
for all v ∈ v̂ do
4:
I(v̂) ← I(v̂) ∪ C OORD(v)
5:
return U NIQUE E NCODE(I(v̂))
(a) Mapping from the traces on G to GH

Sect. V-E. The estimated graph size is computed based on the
graph aggregation technique introduced in Sect. V-F.
Note that there are three functions making Algo. 1 different from the traditional geometric path search: 1) the cost
function fk,∆t (·) evaluates the control effort of the k-degree
vertex tuple, instead of a simple path length measure; 2)
the function I NDEX(·) regards the k-degree vertex tuple as
the “state”, which expands the high-dimensional state space
supported by the control point coordinates, while the geometric
path search typically regards positions as states; and 3) the
function F EASIBLE S UCCS(·) will expand to the neighboring
k-degree tuples with dynamical feasibility checking, while
the geometric path search cannot check feasibility without
parameterization. As for the heuristic function H EURISTIC(·),
we adopt the admissible minimum time heuristic in [14].
It is worth noting that the design of Algo. 1 heavily relies
on the properties of the B-spline. Thanks to the local control
property, the cost evaluation and feasibility checking can be
done locally based on the k-degree vertex tuple. Instead of
solving the BVP, the proposed search method expands to new
states on the high-dimensional graph GH by expanding lowdimensional control point coordinates, which are associated
with closed forms for cost evaluation. Moreover, checking
for collision is time consuming in traditional kinodynamic
planners [7, 14, 23]. By using B-spline parameterization,
the process can be avoided by characterizing the B-spline
deviation, as introduced in Sect. V-D. The limitation of our
method is that the expansion of control points is restricted
by the resolution and connection of the grid, which results in
limited representations of B-spline trajectories.
D. Feasibility Condition
The dynamical feasibility of the k-degree vertex tuple can
be validated via checking the extrema of the derivative of
the B-spline. Since the derivative of the B-spline is another
B-spline with decreasing degree, the velocity spline is of
degree k − 1 and the acceleration spline is of degree k − 2.

(b) Graph aggregation

Fig. 7: Illustration of the mapping process to GH and the graph
aggregation process used by the EBK search.

Considering that the convex hull property in Prop. 1 is a
sufficient but not necessary condition, directly using Prop. 1
for feasibility checking may be conservative. Actually, there is
a non-conservative approach for feasibility checking by using
the closed-form solutions of the extremas of the uniform Bspline. Take the fifth-degree uniform B-spline as an example.
The B-spline can be rewritten in a monomial basis according
to Eq. 3. The velocity profile is a degree-4 polynomial whose
extremas can be checked by finding the roots of its derivative
(degree-3) in closed form.
For traditional kinodynamic planners [7, 14, 23], collision
checking is an expensive process and may become the computation bottleneck of the algorithm [39]. Position-only shortest
path search on the graph of the cell decompositions does not
require collision checking since the piecewise linear connection between cell centers is restricted to collision-free cells.
For the proposed method, given the B-spline parameterization
of degree k and cell size of the decomposed environment, the
B-spline may deviate from the piecewise linear connection
due to the fact that it does not exactly pass through the
control points. However, the maximum distance that the Bspline curve deviates from the piecewise linear collection can
be characterized offline, which is compensable by moderate
obstacle inflation. The inflation needed is characterized in
Appendix C. In practice, since the degree of the B-spline is
fixed and the cell size is not tuned frequently, the inflation can
be calculated once and then used for many experiments.

TABLE I: Illustration of the on-demand graph construction.
Method

Run
Time (s)

Actual # of
Exp. Nodes

Total # of
Nodes (Esti.)

Percentage

EBK-D1

0.034

2, 334

13, 005

17.9

EBK-D2

0.383

36, 818

351, 135

10.5

EBK-D3

7.422

460, 469

9, 480, 645

4.9

EBK-D4

173.710

9, 096, 338

255, 977, 415

3.6

(%)

E. Efficient Low Dispersion Search
Before proposing the efficient methods, we present a complexity analysis of the OBK search. According to the definition
of the k-degree vertex tuple, the total number of vertices VH
of the graph GH grows exponentially w.r.t. the degree k of
B-spline parameterization, i.e., |VH | = O(|V |k+1 ). Note that

DING et al.: AN EFFICIENT B-SPLINE-BASED KINODYNAMIC REPLANNING FRAMEWORK FOR QUADROTORS

Algorithm 3 Given a k-degree vertex tuple v̂ ∈ VH , assign an
integer index based on the selected coordinates of the tuple.
1: function I NDEX (v̂, d)
2:
I(v̂) = ∅;
3:
for all i ∈ {k − d + 1, . . . , k} do
4:
I(v̂) ← I(v̂) ∪ C OORD(v̂[i])
5:
return U NIQUE E NCODE(I(v̂))

according to Prop. 2, Algo. 1 shares a similar complexity with
the execution of Dijkstra’s algorithm on the graph GH if the
heuristic is set to zero. According to the known result that
each vertex is expanded at most once for Dijkstra’s algorithm
(see [40, 41]), the maximum number of iterations of Algo. 1
is upper bounded by |VH | = O(|V |k+1 ), which characterizes
the worst-case execution time of the OBK search. Actually,
since |VH | and |EH | grow exponentially with k, the worstcase execution time of any algorithm that optimally solves
Problem 1 scales exponentially with k.
Given the observation that the state in the OBK search
is homogeneous (i.e., all the dimensions are control point
coordinates), we can aggregate the nodes of GH based on the
proximity of the coordinates to gain efficiency. Specifically,
according to Algo. 2, each vertex v̂ ∈ VH is marked with
a unique integer index. In the modified I NDEX(·) function in
Algo. 3, we encode the vertex v̂ only based on the coordinates
for the last d vertices such that the vertices which share the
same partial coordinates will be regarded as the same node.
By aggregating the nodes of GH , the dimension of the search
space is directly controlled by the user-specified parameter d.
The modification essentially conducts a low dispersion search
on the high-dimensional graph GH with a control on the
number of expanded nodes.
Different d values will determine how the vertex tuples
are aggregated, which in turn affects the solution quality and
algorithm efficiency. Note that although the search space is
reduced, the continuity and smoothness of the resultant trajectory is maintained since the modification preserves the sharing
of the B-spline coordinates. The resultant search method is
called EBK search and a formal analysis of the EBK search
is provided in Sect. V-F.
F. Analysis of the EBK Search
As introduced in Sect. V-E, the user-specified parameter d
determines how vertex tuples in the graph GH are aggregated.
We provide a toy example of d = 1 in Fig. 7(b) to understand
the graph aggregation. When choosing d = 1, as in Fig. 7(b),
vertex tuples are aggregated based on the last vertex of the
tuple. For instance, v̂4 and v̂5 share the same last vertex and
are aggregated into the same node, as marked in purple. In the
same way, the three distinct nodes v̂6 , v̂7 and v̂8 are aggregated
into the same cyan node.
The edges of GH are also reduced accordingly. For example,
the edges (v̂4 , v̂7 ) and (v̂5 , v̂8 ) are aggregated since they are
connecting the same two aggregated nodes. Note that once a
path to the aggregated node is determined, such as the path

9

blue-yellow-purple-cyan, the vertex tuple associated with each
aggregated node is determined. In this example, the purple
node is associated with v̂5 and the cyan node is associated
with v̂8 . Therefore, given the initial vertex tuple, any path
on the aggregated graph, can be uniquely transformed to a
path on the graph GH . And, apparently, a path on the graph
GH can be transformed to a path of aggregated nodes. The
equivalence states that we are actually conducting a graph
search on the aggregated graph with a controllable number
of vertices, i.e., a low-dispersion search on the original high
dimensional graph GH . The resultant path on the aggregated
graph can be reconstructed as an admissible path on GH .
For the simple case of d = 1, as illustrated in Fig. 7(b), the
size of the aggregated graph is the same as the original graph
G. Therefore, the EBK search can be as efficient as a shortest
path search on the spatial grid by choosing a small d. And
the advantage of the EBK search is that it directly outputs
a time-parameterized dynamically feasible trajectory. It turns
out that the EBK search is resolution complete with respect
to the aggregated graph, and we refer interested readers to a
detailed analysis of the EBK search in Appendix D.
By choosing a small d < k + 1, a large number of vertex
tuples are aggregated into one group, and the “resolution”
of the graph becomes large. Due to the aggregation, the
representation of the trajectory is limited. An intuitive example
is that, when choosing d = 1, the search process will never
choose to place the same control point in the same grid cell due
to the strictly positive cost. As a result, the trajectory obtained
may fail to reach the exact end state, such as a static state.
However, the issue can be addressed by choosing a larger d
and sacrificing efficiency.
Compared to the preliminary version, i.e., the RBK search
in [15], the EBK search is more flexible and allows for
control of the algorithm efficiency and solution quality. The
connection is that the RBK search is essentially the EBK
search using d = 1.
VI. E LASTIC O PTIMIZATION
To compensate for the discretization introduced by the
EBK search and further improve the trajectory quality, we
present the EO approach, which refines the control point
placement to the optimal location w.r.t. the free space. Our
approach is motivated by the seminal work [42], in which a
collision-free “tube” around the initial path is identified and
the path is “stretched” within the tube so that the shape is
optimized. Mathematically, the tube is defined as a series of
balls, with the ball centers denoted as P := {p0 , p1 , . . . , pT }
and corresponding radiuses denoted as R := {r0 , r1 , . . . , rT },
where pi denotes the ball center and ri denotes the radius.
The tube is defined to be “well-connected” if and only if
kpi − pi+i k2 ≤ ri + ri+1 , ∀i ∈ {0, . . . , T − 1}. Compared
to [42] which cannot handle dynamical feasibility constraints
for complex kinodynamic systems such as quadrotors, we
propose a convex optimization formulation based on B-spline
parameterization, which uses the convex hull property to
enforce feasibility.
Note that Zhu et al. [43] also propose a convex elastic
smoothing formulation for car-like robots. There are two

10

Algorithm 4 Expand elastic tube in configuration space C ELAS .
1: function T UBE E XPANSION (P, C ELAS )
max
0
2:
Initializes: dmin
infl , dinfl , dthres . R = R = Q = ∅;
3:
for all pi ∈ P do
4:
(ni , ri ) ← NNS EARCH(pi , C ELAS );
→
−
5:
n = (pi − ni )/NORM(pi − ni );
min
tol
6:
while dmax
infl − dinfl > d
infl do
−
max
min
7:
d ← dinfl + dinfl /2, pi,infl ← pi + d · →
n;
ELAS
0 0
);
8:
(ni , ri ) ← NNS EARCH(pi,infl , C
9:
if A BS(ri0 − d − ri ) > dthres then
10:
dmax
infl ← d
11:
else
12:
dmin
infl ← d
13:
Q ← Q ∪ pi,infl , R0 ← R0 ∪ ri0
14:
return Q, R0

major differences: 1) The formulation in [43] is based on the
dynamics of car-like robots and cannot be applied to complex
dynamic systems, while our formulation uses high-order Bspline parameterization, which can be directly used to control
quadrotors. 2) In [43], the smoothed trajectory may collide
with obstacles due to the geometric incompleteness of the tube
constraint (as shown in Fig. 9(a)) and only a heuristic waypoint
insertion/obstacle inflation scheme is provided, while the EO
approach has a theoretical safety guarantee, which is achieved
by a two-level inflation scheme to ensure the connectivity of
the tube and a finite iterative control point insertion process.
A. Elastic Tube Expansion
In [42] and [43], the elastic tube is a series of connected
balls which are centered at the waypoints of the reference path.
Intuitively, the tube generated in this way cannot fully utilize
the free space around it, as shown in Fig. 8(a). We therefore
propose a lightweight tube expansion algorithm so that the
tube can roughly represent the locally largest free space. Given
the initial control point placement π = (v0 , . . . , vT ) provided
by Algo. 1, we first extract the coordinates of π, and denote the
collection of coordinates as P := {p0 , p1 , . . . , pT } following
the notation used in Sect. IV.
The elastic tube expansion algorithm (Algo. 4) can be
divided into two steps: First, we construct the initial tube,
by conducting a radius search for the initial placement P,
and obtain the nearest obstacle position ni . Second, we push
−
the center of the bubbles in the direction →
n (away from the
nearest obstacle) while satisfying the criterion that the new
bubble contains the original bubble, as required by condition
A BS(ri0 − d − ri ) ≤ dthres , as shown in Fig. 8(a). The inflation
process is implemented in a binary search manner. Algo. 4
will finally find a series of local maximum volume bubble
centers Q := {q0 , q1 , . . . , qT } based on the initial tube P. For
min
the parameter settings, dmax
infl and dinfl are the maximum and
minimum inflation distance, respectively; dthres is the threshold
for checking whether the new bubble contains the original
one, and should be set to a small value, e.g., less than the
map resolution; and dtol
infl is the binary search end condition,

AUTHOR’S VERSION

EBK Control Points
Start Span
End Span
EBK Traj.
Inflate Direction
Support Position

(a)

EBK Control Points
EO Control Points
Start Span
End Span
EBK Traj.
EO Traj.

(b)

Fig. 8: Illustration of the EO approach: (a) shows the elastic tube
expansion process (Sect. VI-A); (b) shows the optimization process
(Sect. VI-B). In (a), the original tube is marked in green, while the
inflated tube is marked in yellow.

which can be set to the resolution of the map. The function
NNS EARCH is the nearest neighborhood search, which can be
done efficiently if a KD-tree is maintained. The efficiency of
Algo. 4 is verified in Section. VIII.
B. Elastic Optimization Formulation
Given the inflated ball centers Q = {q0 , q1 , . . . , qT }
and corresponding radiuses R0 = {r00 , r10 , . . . rT0 }, the EO
formulation minimizes the total control effort by finding the
optimal placement P ∗ = {p∗0 , p∗1 , . . . , p∗T } while satisfying
the safety and dynamical feasibility constraints. The safety
constraints are enforced by constraining the control point
position inside the 3-D balls, and in Sect. VI-C, we will discuss
how to theoretically guarantee the safety of the resultant
trajectory. The dynamical feasibility constraints are enforced
using Prop. 1.
Note that like the EBK search, we consider the initial and
goal state of the
of πs and

 quadrotor. Denote the coordinates
πg as P s = p0s , p1s , . . . , pks and P g = p0g , p1g , . . . , pkg ,
respectively. These coordinates are fixed during optimization.
Similar to the construction of the k-degree vertex tuple in
Sect. V-B, we concatenate P s , P and P g and formalize the
concatenation in terms of the control point spans. Following
k
the similar notation in Sect. V-B, we denote by [P̃]j the jth control point span of the concatenated coordinates P̃. It
k
k
follows that [P̃]0 = P s and [P̃]J = P g , where J = k + T + 2.
k
We denote by Pj the stacked coordinates matrix of [P̃]j ,
with PD
j denoting the D ∈ {x, y, z} axis. The optimization
problem can be expressed as follows:
min
s.t

J
X

k

fk,∆t ([P̃]j )

j=0
k
k
[P̃]0 = P s , [P̃]J = P g
kpi − qi k2 ≤ ri0 ,
max
|SPD
j | ≤ ul,D 1(k+1)×1 ,

(6)
∀i ∈ {0, . . . , T }
∀j, D ∈ {x, y, z},

where the first constraint expresses that the initial and goal
states need to be fixed. And the second constraint (quadratic)

DING et al.: AN EFFICIENT B-SPLINE-BASED KINODYNAMIC REPLANNING FRAMEWORK FOR QUADROTORS

(a)

(b)

(c)

Fig. 9: Illustration of (a) the geometric incompleteness of the ball
constraint, (b) the two-level inflation scheme, and (c) the iterative
convex hull shrinking process for enforcing the safety guarantee. It
can be observed in (a) that even the straight-line segments between
the balls may collide with the obstacle.

restricts the control points inside the expanded tube. The
third constraint (linear) is to ensure the dynamical feasibility
using the sufficient condition in Prop. 1. Since the control
points can be adjusted in continuous free space, the potential
conservativeness brought by Prop. 1 is minor in practice. The
overall formulation is a QCQP which can be solved efficiently
using off-the-shelf convex solvers. The time cost is fixed given
the initial placement P, so it is not included in the objective.
C. Enforcing Safety Guarantee
Restricting control points inside the balls is not a sufficient
condition for safety, for the following two reasons: 1) the
balls are placed with a finite density and constraining the
control points in the balls cannot preserve the collision-free
characteristic even for straight-line segments between control
points (Fig. 9(a)), and 2) the B-spline does not exactly pass
the control points and deviates from the straight-line segments.
The first issue is also observed in [43], which proposes using
waypoint insertion/obstacle inflation to handle the problem.
However, there is no quantification of how much inflation or
how many insertions are needed and only a heuristic is given.
The second issue is inherently similar to one common issue
faced by piecewise polynomial parameterization [2]–[5]: the
polynomial may deviate from the collision-free straight-line
segments between waypoints or exceed the safe flight corridor.
Chen et al. [5] propose a finite iterative process by adding
constraints on polynomial extremas based on a cube corridor
(linear constraints), but this is not directly applicable to Bspline parameterization with quadratic constraints.
In our case, the issues can be resolved using a two-level
inflation scheme and an iterative convex hull shrinking process.
The two-level inflation scheme is to ensure the connectivity

11

of the elastic tube, and furthermore, the inflation needed is
quantified in the scheme. For simplicity, we first consider the
original tube before applying the tube expansion algorithm
(Algo. 4). The two-level obstacle inflation scheme is as
follows: the configuration space in which we conduct kinodynamic search with larger obstacle inflation δ BK is C BK , while
we generate the elastic tube and optimize the control points in
the configuration space C ELAS with smaller obstacle inflation
δ ELAS , as shown in Fig. 9(b). The difference adds additional
clearance to any point in the configuration space C BK . Without
the difference, the minimum radius of the ball at the grid center
is min(cx , cy , cz ), where cx , cy and cz are the grid size. Denote
the maximum separation distance of two neighboring control
points in the 3-D grid as hmax . It follows that, if the difference
δ BK − δ ELAS > hmax /2 − min(cx , cy , cz ) holds, the additional
clearance will ensure that the two neighboring balls overlap,
thus ensuring the connectivity of the elastic tube. Note that
Algo. 4 maintains the connectivity of the tube by ensuring that
the inflated ball contains the original ball while keeping the
same support point on √the obstacle. For the low-level inflation,
given hmax , δ ELAS ≥ 2−1
2 hmax is sufficient for the straightline segments to be contained in the free space [15].
Based on the two-level inflation scheme, we propose an
iterative convex hull shrinking process, which pulls the Bspline trajectory back to the free space in the case of collision.
The idea of the process is to iteratively add control points
to the original B-spline control point sequence. The newly
added control points are constrained in the intersection of
two consecutive balls. 4 The process is built upon the convex
hull property of the B-spline and constrains the B-spline
trajectory by shrinking the convex hull. We highlight that only
a finite number of control points are needed to enforce the
safety of the B-spline trajectory. This could save a significant amount of computation power compared to the methods
which apply dense constraint points based on a conservative
heuristic [1, 43]. We conclude this feature in Theorem 1.
Theorem 1. Given a well-connected elastic tube, a Bspline trajectory that fits within the tube can be generated by
iteratively adding constrained control points to the original Bspline control point sequence. The newly added control point
is constrained inside the intersection of neighboring balls. The
iterative process succeeds in a finite number of iterations, or
infeasibility is reported when the dynamical feasibility cannot
be satisfied, given the current tube and control point sequence.
Proof. Please refer to Appendix E for the detailed proof.



In Fig. 9(c), we provide a toy example of the convex hull
shrinking process. The initial placement is constrained in four
balls, and the convex hull envelope exceeds the free space.
If collision is detected, we add one additional control point
(blue dot) which is constrained to be inside the intersection of
the first and second ball. The extended EO is expected to be
executed again, and the convex hull shrinks. Similarly, if the
4 According to the introduction in Sect. IV, the total number of knots should
satisfy m + 1 = (n + 1) + k + 1. Since uniform B-spline is used, when a
new control point is inserted, the number of knots is increased by one, while
the knot separation ∆t remains the same. The new control point sequence
still matches the new knot vector.

12

collision is still not resolved, we iteratively add control points
to the intersection space. It can be shown that at most nine
iterations are needed to resolve the collision in this case.
VII. I MPLEMENTATION D ETAILS
A. Receding Horizon Replanner Using Local Control
As shown in Fig. 4, B-spline has the local control property
which facilitates the receding horizon (re)planning. Specifically, all the control points are organized in a sliding window. The control points corresponding to the executed and
executing trajectory are committed and fixed. The disturbance
caused by the optimization will not affect the feasibility of
the executing trajectory due to the local control. A stopping
policy will be activated if no feasible solution is found before
the end of the executing trajectory.
When replanning is activated, the EBK search is called to
update the placement for the control points under optimization.
Note that the initial and goal state of the EBK search can be
determined by the control points inside the window according
to the local planning range. Note that the control points from
the sliding window are in the continuous space after reshaping.
However, the EBK search should use the discretized control
points as the reference initial/ goal state to preserve optimality.
The strategy of getting the reference states for the EBK search
are to find the closest span pattern in terms of position and
velocity error while matching the last control point to the grid
cell. We constantly gather a fixed number of control points
(e.g., twelve) for EO as the window moves forward.
There are two modes for the activation of replanning,
namely, active mode and passive mode. For the passive mode,
the EBK search is only called when collision is detected,
while for the active mode, the EBK search is constantly
activated as the sliding window moves forwards. Since the
active mode can constantly improve the trajectory quality, it
is more robust when the mapping quality is limited, but it is
more computationally expensive.
B. Monocular Vision-Based Testbed
The monocular quadrotor testbed is equipped with a monocular camera (30 Hz), one IMU (100 Hz), an Intel i7 processor
and an NVIDIA Jetson TX1 (Fig. 2(a)). The localization,
mapping and planning modules are all running onboard. The
localization module is based on our Monocular Visual Inertial Navigation System (VINS-Mono) [44], and the mapping
module is based on our monocular dense mapping method [45]
and truncated signed distance field (TSDF) fusion. No prior
knowledge of the environment is required.
C. Dual-Fisheye Vision-Based Testbed
The dual-fisheye quadrotor testbed is equipped with two
fisheye cameras (30 Hz), one IMU (100 Hz), an Intel i7
processor and an NVIDIA Jetson TX2 (Fig. 2(b)). All modules
are running onboard. It is worth noting that by using the
two fisheye cameras, the system can provide omnidirectional
perception and the quadrotor is able to fly a round-trip with
a fixed yaw angle. The mapping module is based on our
dual-fisheye omnidirectional stereo system [46]. Also no prior
knowledge of the environment is required.

AUTHOR’S VERSION

VIII. A NALYSIS
In this section, we present an analysis of the proposed kinodynamic planning framework. We begin with two individual
analyses for the EBK search and the EO approach, respectively. To analyze the EBK search, we compare the proposed
method with two kinodynamic planning algorithms, namely,
search-based motion primitive (SMP) [14] and kinodynamic
RRT* (kRRT*) [7], representing both the search-based method
and sampling-based method, respectively. For the EO, we
compare the EO approach with two state-of-the-art trajectory
optimization techniques, namely, continuous trajectory (CT)
optimization [25] and gradient-based safe (GS) trajectory
optimization [33], which are popular non-linear optimization
techniques for trajectory refinement. After the individual tests,
we analyze the run-time efficiency of the proposed replanning
framework, and compare the whole replanning system with
the SMP [14] method. We run all the simulations on a desktop
computer equipped with an Intel I7-8700K CPU.
A. Analysis of the B-spline-Based Kinodynamic Search
Recall that the motivation for introducing kinodynamic
search to replanning is to facilitate dealing with the nonstatic initial states of the quadrotors. To this end, we analyze
the performance of the kinodynamic search by planning from
a given non-static initial state to varying goal states in a
10×10×2 m test field. We compare our results with SMP [14]
and kinodynamic RRT* [7] in terms of the trajectory quality
and time efficiency, under the same planning setup. At the
same time, we also illustrate the results of a hierarchical
geometric planner as a common baseline. Specifically, the
geometric planner first uses A* under the Euclidean distance
measure to find the shortest path, and then parameterizes the
path using the unconstrained QP formulation introduced in [3].
The kinodynamic planners from [7], [14] and our method all
have a tuning parameter to control the algorithm complexity
and solution quality. For instance, SMP [14] can control
the discretization resolution for the control input. To further
investigate the optimality-efficiency tradeoff achieved by each
algorithm, we also demonstrate the results for these algorithms
under different parameter setups. Note that we focus on the
real-time replanning scenario, so we are concerned with the
operation region where the run-time efficiency is close to realtime. Specifically, we compare the following methods:
†
• Geometric : A path finder using A* and a fifth-degree
polynomial parameterization using the unconstrained QP
formulation [3] by minimizing the average integral of acceleration.
†
• kRRT*-T100 : The kinodynamic RRT* planner [7] using a
3-D acceleration-controlled double integrator system under
a 100 ms termination condition for the sampling.
• kRRT*-T600: The kinodynamic RRT* planner under a 600
ms termination condition.
†
• SMP-U3 : The SMP planner [14] using a 3-D accelerationcontrolled double integrator system with three discrete control inputs for each axis.
• SMP-U5: The SMP planner with five discrete control inputs
for each axis.

DING et al.: AN EFFICIENT B-SPLINE-BASED KINODYNAMIC REPLANNING FRAMEWORK FOR QUADROTORS

13

TABLE II: Comparison of different kinodynamic planning approaches.

(a) Geometric method

(b) Kinodynamic RRT*-T100

(c) SMP-U3

(d) SMP-U5

(e) EBK-D1

(f) EBK-D2

Fig. 10: Comparisons of different kinodynamic planning approaches.

EBK-D1† : Our EBK planner using fifth-degree B-spline
parameterization and d = 1.
• EBK-D2: The EBK planner using d = 2.
•

The reason for using the acceleration-controlled double
integrator system for kinodynamic RRT* and SMP is that
if a higher-order system were used, the run-time would be
too large, making it inapplicable to replanning. For a similar
reason, a termination time larger than 600 ms for kRRT* and
number of control inputs larger than five for SMP are not
considered. The methods marked with † are amenable to realtime, and the other methods serve as showcases illustrating
how the trajectory quality can be improved given a larger
computation time budget. All the kinodynamic planners have
a similar form of the cost function according to Eq. 4. Since
both kRRT* and SMP adopt the acceleration-controlled double
integrator system, the order of the derivative l we can take is 2
for all the experiments. The weight λ of the trajectory time is
set to 20. We consider two additional metrics, namely, average
acceleration and maximum acceleration, which represent the
smoothness and feasibility of the resultant trajectory.
For the methods where cell decomposition is needed, such
as A* for the geometric method and EBK, the environment
is decomposed into cells with a fixed size of 0.2 m for each
dimension. The geometric planner requires a time allocation
module since the path does not contain any time information.
Generating the optimal time profile for a path is actually not a
trivial problem [47], and the common practice is based on
a heuristic allocation method, such as a trapezoid velocity

Method

Run
Time (s)

Traj.
Dur. (s)

Acc. Cost

Ave. Acc.

Max. Acc.

(m2 /s3 )

(m/s2 )

(m/s2 )

kRRT*-T600
SMP-U5
EBK-D2

0.661
2.878
0.383

4.04
3.86
4.52

30.6
12.8
14.4

2.42
1.36
0.95

4.18
2.97
4.60

Geometric†
kRRT*-T100†
SMP-U3†
EBK-D1†

0.018
0.110
0.111
0.034

4.34
4.60
4.47
4.31

108.8
38.8
32.3
15.2

4.60
2.54
1.70
1.10

9.48
4.28
4.70
4.56

profile [4, 25]. We follow this practice and set the average
speed to achieve a similar trajectory duration to the kinodynamic planners. Time allocation is not needed for kinodynamic
planners since they directly produce time-profiled trajectories.
The initial state is set to a fixed position with non-zero velocity
1.2 m/s and zero acceleration, as shown in Fig. 10. The goal
state is static, and its position is regularly sampled with a
distance separation of 0.7 m. In total, there are 136 collisionfree goals available. The velocity and acceleration limit are set
to 2 m/s and 4.7 m/s2 for each axis. The ∆t for the EBK
method is set to 0.17 s. The statistics averaged over the 136
rounds of planning are shown in Tab. II, and the qualitative
results for the planning to the same goal state are shown in
Fig. 10.
According to the qualitative results in Fig. 10, there is a
significant difference in the performance. For the geometric
method (Fig. 10(a)), since the shortest path diverges from the
initial velocity direction, the parameterization is jerky at the
beginning. Moreover, as the unconstrained QP cannot enforce
the dynamical feasibility, the generated trajectory is infeasible
due to the non-static initial state. As for the kRRT* with a 100
ms time budget (Fig. 10(b)), it can respect the initial state of
the quadrotor since the control effort is directly considered
in the sampling process. It also guarantees the dynamical
feasibility of the resultant trajectory by constraining the control
input and states along the tree edges. However, the trajectory
quality is unsatisfactory due to the limited sampling. We also
observe some unpredictable randomized behavior as shown by
the three rounds of planning with exactly the same initial state
and goal state in Fig. 10(b). Meanwhile, for the SMP method,
the shape of the trajectory of SMP-U3 is not natural since
the resolution of the control input is large.5 Some maneuvers
such as flying over the little step in the middle are not
included in the solution space of SMP-U3. SMP-U5 performs
better than SMP-U3 due to finer discretization. Finally, EBKD1 and EBK-D2 both generate an initial-state-aware smooth
trajectory. EBK-D2 finds a slightly better trajectory than EBKD1 according to the acceleration profile.
It is notable that the trajectory provided by the EBK method
has continuity up to snap, which is good for controlling
quadrotors. We can observe from Fig. 10 that the kRRT*
trajectory only has continuity up to acceleration and that of
5 The acceleration bound we use is larger than that in [14].

14

AUTHOR’S VERSION

TABLE III: Comparison of different trajectory optimization approaches.
Map

Method

CT [25]

Random
Map

GS [33]
EO (proposed)
GS [33]
EO (proposed)
GS [33]
EO (proposed)

Density
(pillars/m2 )

Success.
Frac. (%)

RunTime (s)

Traj.
Dura. (s)

Jerk Cost
(m2 /s5 )

Ave. Vel.
(m/s)

Ave. Acc.
(m/s2 )

Max. Acc.
(m/s2 )

Traj. Len.
(m)

0.1
0.2
0.4
0.1
0.1
0.2
0.2
0.4
0.4

95
68
46
100
100
100
100
100
100

0.030
0.082
0.073
0.062
0.012
0.075
0.012
0.206
0.013

8.1
8.0
7.9
11.1
10.6
12.8
11.3
12.9
12.4

70.2
91.6
105.6
186.8
174.0
205.9
181.2
190.8
132.4

1.73
1.72
1.69
1.39
1.43
1.28
1.38
1.33
1.36

0.93
1.01
1.05
0.87
0.38
0.85
0.49
0.89
0.55

2.63
2.90
3.10
2.99
1.35
2.89
1.85
3.10
1.34

14.3
13.9
13.7
15.6
14.1
15.9
14.5
17.4
16.2

the SMP has continuity up to velocity, due to the restriction
of computation complexity and order of the system model.
From the quantitative results in Tab. II, among the real-time
methods (with †), EBK-D1 finds the lowest cost trajectory,
achieving a 15.2 m2 /s3 total cost within 0.034 s. Our method
shows superior performance given the real-time requirement.
It is of interest to examine the situation where we have a time
budget in the range of seconds. In that case, SMP-U5 achieves
a lower cost than EBK-D1 and EBK-D2. The reason is that the
trajectory duration of EBK-D2 cannot be efficiently reduced
since the control points have to be expanded step by step on
the discrete grid. This illustrates the limitation induced by the
discretization for the EBK method. However, the difference
between the SMP method and EBK method is minor, meaning
the EBK method can provide competitive solutions given a
run-time budget of seconds.
B. Analysis of the Elastic Optimization
To evaluate the performance of EO, we compare our method
with two state-of-the-art trajectory optimization methods: the
CT method [25] and GS method [33]. Two test environments
are provided, namely, a random map (shown in Fig. 11(c))
and a Perlin noise map6 (shown in Fig. 11(d)). The map
size is fixed to 20 m × 20 m × 4 m for both maps. The start
location is fixed to the center of the test field for the qualitative
experiments in Fig. 11(a) and Fig. 11(b), and is fixed to the
left bottom corner for all the experiments in Tab. III to test
the performance for long trajectories. The goal location is
regularly sampled in the test field with a distance separation
of 1 m. We vary the obstacle density of the random map
from 0.1 pillars/m2 to 0.4 pillars/m2 , where the pillar size
is set to 0.5 m × 0.5 m. The qualitative results are provided
in Fig. 11 and the quantitative statistics of the optimization
performance are organized in Tab. III. Note that we omit the
statistics for the Perlin noise map in Tab. III since the trend
is similar to that of the random map. All the optimization
methods can handle high-order parameterization and they are
set to minimizing the integral of the squared jerk. According
to Eq. 4, the jerk cost listed in Tab. III has unit m2 /s5 and
represents the accumulated integral of the squared change rate
of the acceleration, which represents the total control efforts.
The CT method uses a gradient-based non-linear optimization process to optimize the polynomial coefficients such
6 https://github.com/HKUST-Aerial-Robotics/mockamap

that the resultant trajectory is collision free. It requires a
Euclidean signed distance field (ESDF) to evaluate the collision cost. Following the practice in [25], we provide an
initial polynomial trajectory as an initial guess, which is
parameterized by a straight-line guiding path. The number of
segments is determined by a 3 m distance separation. The
GS method shares a similar formulation to the CT method,
with the difference being that the GS method starts from a
collision-free initial guess, which is provided by A* search
in the experiment. Both the CT method and GS method
require time allocation, and in the experiment, we use the
trapezoid velocity profile and scale the total allocated time
such that different optimization methods achieve a similar
average velocity, as shown in Tab. III. The CT method has
a larger average velocity due to the cases where it fails to
resolve collision and the trajectory does not contain necessary
deceleration. The success fraction is calculated by counting
the collision-free and dynamically feasible trajectories among
the total number of rounds.
Firstly, we examine the optimization reliability, i.e., the
success fraction for the different methods. As shown in
Tab. III, the CT method is sensitive to the obstacle density.
For a density of 0.1 pillars/m2 , the success fraction of the
CT method is 95%, which means that in obstacle-sparse
environments, the CT method can resolve collision efficiently.
However, when the density increases to 0.4 pillars/m2 , we
observe that the success fraction drops significantly to 46%.
The reason is that the CT method easily gets stuck in the
infeasible local minimum when the trajectory is inside the
cluttered obstacle. As shown in Fig. 11(c), the trajectory of
the CT method gets stuck between two pillars where there is
not enough clearance considering the quadrotor size. On the
other hand, we do not observe a drop in the success fraction
for either the GS method or EO method. The reason is that
the GS method starts from a collision-free initial guess and
has a dominant collision penalty compared to its smoothness
cost. The EO method has a high success fraction according to
its theoretical guarantee.
Secondly, we focus on the two reliable methods, namely,
the GS method and EO method, and further investigate the
trajectory statistics. As shown in Tab. III, the average trajectory
durations of the two methods are close, so the comparison of
the jerk cost is fair. For an obstacle density of 0.2 pillars/m2 ,

DING et al.: AN EFFICIENT B-SPLINE-BASED KINODYNAMIC REPLANNING FRAMEWORK FOR QUADROTORS

(a) Random map (0.2 pillars/m2 )

(b) Perlin noise map

15

(a) Replan on the random map

(b) Replan on the Perlin noise map

Fig. 12: Illustration of our replanning system on different maps.

C. Analysis of the Run-Time Efficiency

(c) Random map (0.2 pillars/m2 )

(d) Perlin noise map

Fig. 11: Comparisons of different trajectory optimization methods
on two different maps. The EO method is shown in purple, the CT
method is shown in red and the GS method is shown in blue.

averaged over the 135 rounds, the jerk cost of the EO method
is 181.2 m2 /s5 , which is smaller than that of the GS method.
Similar results are also observed for different densities. The
reason is that the GS method is sensitive to the time allocation,
since it also starts with an unparamterized path. However,
for unknown environments, the shape of the initial path may
vary and there is no systematic way to allocate the time. The
heuristic trapezoid velocity profile may fail to provide a good
initial guess when the initial path is not in a regular shape.
For the EO method, since it starts from a time-parameterized
B-spline trajectory, there is no need for time allocation.
From the efficiency perspective, for the GS method to
achieve a similar jerk cost to the EO method, it needs runtimes of 0.062 s, 0.075 s and 0.206 s on average for the
three densities, which are significantly longer than the EO
method. Note that the GS method is based on a non-linear
optimization formulation. In the experiments, the non-linear
optimization of the GS method is terminated when the nonlinear solver (NLOPT [48]) reports convergence. For the GS
method to converge to a compatible solution to that of our EO
method, it requires a significantly longer run-time. Moreover,
the run-time of the GS method is sensitive to the density, since
for cluttered environments, more segments of the polynomial
are needed, which may affect the convergence rate of the GS
method. However, the EO method has lower run-times for the
three different densities. The efficiency of the EO method
is affected by the total number of control points, but we
observe that the efficiency difference is minor for the different
densities.

In this section, we test our replanning system, combining
the EBK search with EO refinement. For the EBK search, we
adopt EBK-D1 since it is the most efficient and the trajectory
quality is satisfactory as verified in Sect. VIII-A. We provide
two different maps, namely, the random map and Perlin noise
map. We evaluate the run-time efficiency of our replanning
system, and list the statistics of all components in Tab. IV.
The overall trajectory illustrating the whole round trip is
shown in Fig. 12. As shown in Tab. IV, on the random map,
the EBK-D1 method consumes an average computing time
of 0.017 s with a standard deviation of 0.01 s. The elastic
tube expansion method can be finished in 0.002 s, and the
optimization can be done in 0.021 s. On the Perlin noise map,
which contains unstructured 3D obstacles, our method has a
similar performance, showing that our method works well in
complex 3-D environments.
D. Comparison of the Replanning Framework
In this section, we conduct a system comparison with the
SMP method [14]. We use SMP-U3 since SMP-U5 cannot
work in real-time. Moreover, we conduct an ablation test,
which excludes the initial-state aware EBK search and adopts
the naive position-only A* search combined with EO local
reshaping. We call the method for the ablation test A*-EO.
The ablation test validates the critical role of the kinodynamic
search in replanning.
We set up a challenging obstacle-cluttered 3-D complex
simulation environment containing walls, 3-D steps (free space
below) and pillars, as shown in Fig. 13. The replanning
strategy is choosing a local goal state on a given straightline guiding path with a local replanning range of 5 m.
The simulated quadrotor is equipped with a depth camera
which has a sensing range of 4 m. The maximum velocity
and acceleration bound are set to 2 m/s and 3.2 m/s2 for
TABLE IV: Run-time analysis on different maps
Maps

#
Replans

Random map
( 0.25 pillars/m2 )

76

Perlin Map

19

Time (s)

EBK
Search

Avg
Max
Std
Avg
Max
Std

0.017
0.049
0.010
0.014
0.026
0.006

#
Opt.
993

1044

Time (s)

Tube
Expan.

Traj.
Opt.

Total
Opt.

Avg
Max
Std
Avg
Max
Std

0.002
0.009
0.001
0.002
0.008
0.001

0.021
0.043
0.010
0.028
0.058
0.010

0.023
0.044
0.010
0.030
0.061
0.011

16

each axis. For SMP-U3, we use a conservative bound with
maximum acceleration 1.0 m/s2 for each axis since SMPU3 can only work with a narrow velocity and acceleration
range. Using a large dynamic range for SMP-U3 will result
in a very sparse primitive graph, which does not even contain
one feasible solution. For the EBK search of our method, we
use EBK-D1 with a 60 × 60 × 20 uniform grid. For the EO
approach, 12 control points are refined as the window moves
forward. We evaluate the replanning system from the trajectory
statistics and time efficiency perspectives, as shown in Tab. V.

Fig. 13: Illustration of the simulated environment for benchmarking.

Critical snapshots of the three methods are shown in Fig. 14.
For SMP-U3 (Conservative) in Fig. 14(a), when the quadrotor
observes the 3-D step, it chooses to circle around instead
of directly flying through the space below since the latter
action requires a large acceleration range. This phenomenon
demonstrates that SMP-U3 (Conservative) sacrifices maneuverability due to the restriction of the dynamic range. SMPU3 takes the initial state into account, as shown in the middle
snapshot in Fig. 14(a). The necessity of using the kinodynamic
search instead of the position-only A* search is identified by
the totally different maneuvers in Fig. 14(b) and Fig. 14(c).
When the quadrotor enters the region of the pillars, it has
two distinct choices, namely, “pass on the left” or “pass on
the right”. For the A*-EO method, as shown in the middle
snapshot in Fig. 14(b), the quadrotor tends to choose the
direction purely based on the shortest path. So there are cases
where the quadrotor is passing in one direction and suddenly
switches to the opposite direction due to finding a new
shortest path, which results in an inconsistent and non-smooth
replanning trajectory. Compared to A*-EO, the kinodynamic
EBK search provides an initial-state-aware trajectory for the
local reshaping, as shown in Fig. 14(c). With the EBK search,
the overall trajectory is clearly more natural and the replanning
is more consistent.
As shown in Tab. V, SMP-U3 (Conservative) is efficient and
has an average computation time of 0.010 s. However, since
the acceleration bound is conservative, the maneuverability is
sacrificed and the trajectory duration is 33.3 s, longer than the
other two methods. Note that we use acceleration-controlled
SMP [14] with unconstrained QP [3] reparameterization. Since
the unconstrained QP has no dynamical feasibility guarantee,
the maximum acceleration is 1.3 m/s2 , which exceeds its
designed maximum acceleration (1 m/s2 ). Compared to SMPU3 and A*-EO, our method has the lowest total jerk cost and
the improvement is achieved by incorporating the kinodynamic
search. The average velocity of our method is 1.37 m/s,
slightly higher than the A*-EO, due to the fact that the kinody-

AUTHOR’S VERSION

TABLE V: Performance of different replanning systems
Method

Traj.
Dura.(s)

Trajectory Statistics
Jerk Cost Mean Vel.
(m2 /s5 )
(m/s)

Time Efficiency(s)
Max Acc.
(m/s2 )

Ave

Max

Std

SMP-U3 (Conservative)

33.3

956.3

1.14

1.3

0.010

0.027

0.004

A*-EO

25.2

723.0

1.31

3.18

0.014

0.062

0.011

Our method

24.4

648.1

1.37

3.18

0.019

0.080

0.012

namic search can reduce sharp decelerations. The maximum
acceleration of our method is 3.18 m/s2 , which obeys the
dynamical feasibility constraint. Compared to the positiononly A*-EO method, the jerk cost is reduced by 10 % by using
the kinodynamic search. Considering that the advantages of the
EBK search are outstanding for part of the trajectory where
potential inconsistency exists, this quantitative improvement
still faithfully identifies the gain of using the kinodynamic
search. Note that navigating through this challenging environment with enough agility already requires considerable control
efforts, and the 10 % cost reduction represents a reasonable
overall improvement.
IX. E XPERIMENTAL R ESULTS
We conduct onboard experiments7 with the two visionbased testbeds to show the general applicability of the proposed framework. For onboard testing, the parameters are
as follows: the time step ∆t is set to 0.35 s; the maximum
velocity and maximum acceleration are set to 1.2 m/s and
2.0 m/s2 , respectively; 8 and the local planning range is set to
10m×6m×1.1m. (The corresponding grid size is 55×35×6.)
A. Indoor Replanning Performance
1) Monocular-vision-based indoor navigation: As shown
in Fig. 15, our replanning system works in complex 3D environments with only a local map. The quadrotor is
commanded to navigate to a 3-D position where the environment is previously unknown. The whole trajectory and the
final accumulated map is shown in Fig. 16. The trajectory
length of the final trajectory is 18.6 m and total trajectory
execution time is 43.4 s. The average velocity of the quadrotor
is 0.45 m/s with a maximum velocity of 0.79 m/s. The
maximum acceleration of the trajectory is 0.58 m/s2 , the
whole trajectory is dynamically feasible, and there are a total
125 calls of the EBK search (active mode), with an average
computation time of 0.010 s. There are 105 calls of EO. The
average computation times of the elastic tube expansion and
the optimization are 0.001 s and 0.031 s, respectively.
2) Dual-fisheye-based indoor round-trip navigation: As
shown in Fig. 17 and Fig. 18, with omnidirectional perception,
our quadrotor testbed is able to fly a round-trip without controlling the yaw angle. Online mapping using the dual-fisheye
cameras is challenging due to the high distortion of the images
acquired from the fisheye cameras. Although the uncertainty
of the map is larger than the monocular case, our replanning
system is still able to robustly avoid the unexpected obstacles
and navigate in the unstructured cluttered environment.
7 https://www.youtube.com/watch?v=sg46XT9-o1k
8 The speed limit and acceleration limit are set to be slightly conservative
considering the perception delay in onboard experiments.

DING et al.: AN EFFICIENT B-SPLINE-BASED KINODYNAMIC REPLANNING FRAMEWORK FOR QUADROTORS

(a) SMP-U3 (Conservative)

(b) A*-EO

17

(c) Our method

Fig. 14: Illustration of different replanning methods in the same simulated environment. The trajectory is shown in purple, and the acceleration
profile is marked in green. For the replanning cases where the shortest path direction is inconsistent with the initial state, we mark the initial
velocity with a red arrow and the shortest path direction in brown.

Fig. 15: Illustration of the snapshot of the indoor replanning with the
monocular vision. In (a), the control points found by EBK (blue),
executed control points (pink), committed control points (green),
and control points under optimization (yellow) are marked. The
corresponding indoor environment is shown in (b).

Fig. 18: Illustration of the whole trajectory for the replanning
using the dual-fisheye omnidirectional perception system. Unlike the
experiments using the monocular testbed, we can achieve round-trip
flight in an unknown complex indoor environment.

B. Outdoor Replanning Performance

Fig. 16: Illustration of the whole trajectory and final accumulated map
for the indoor replanning using the monocular perception system.

As shown in Fig. 19, we demonstrate outdoor experiments
using the monocular vision testbed. For Fig. 19(a), the trajectory length is 19.6 m and total execution time is 41.3 s. The
average velocity of the quadrotor is 0.49 m/s. The maximum
acceleration of the trajectory is 1.06 m/s2 , and the whole
trajectory is dynamically feasible. There are a total 35 calls
of EBK search (passive mode) with an average computation
time of 0.025 s. The average computation times of the elastic
tube expansion and optimization are 0.001 s and 0.030 s,
respectively. For the experiment shown in Fig. 19(b), the
performance and trajectory statistics are similar.
X. C ONCLUSION AND F UTURE W ORK

Fig. 17: Illustration of the snapshot of the indoor replanning with the
omnidirectional vision. With a fixed yaw angle, the obstacles around
the quadrotor can be mapped.

In this paper, we present an efficient kinodynamic replanning framework by exploiting the advantageous properties of
the B-spline. The proposed EBK search algorithm is flexible and provides a user-specified parameter which can be
used to control the algorithm efficiency and solution quality.
The problem of the B-spline-based kinodynamic search on
a spatial grid is characterized in detail, and the theoretical
performance of the EBK search is analyzed. To compensate for

18

AUTHOR’S VERSION

Proposition 2. Given a strictly positive cost function fk,∆t :
k
[π̃] → R+ , Problem 1 is equivalent to the shortest path
problem on the graph GH , where the cost is defined on the
vertices according to the function fk,∆t (·).
C. Characterization of the inflation for the B-spline-based
kinodynamic search
(a)

(b)

Fig. 19: Illustration of the outdoor experiments using the monocular
vision: A flight through a pavilion is shown in (a) (part of the map
on the top is cut for visualization purposes); a flight avoiding trees
is shown in (b).

the discretization, we propose an elastic optimization process.
We combine the two components into a receding horizon
framework. Detailed analysis and comprehensive experiments
are carried out to validate the performance. Systematic comparisons against the state-of-the-art are provided to verify the
claims. The replanning framework is efficient and complete,
and can be used in various kinds of exploration tasks and
different kinds of quadrotor testbeds. The current limitation of
the framework lies in the fact that for EBK search we are using
the uniform B-spline on the spatial grid, which will result in
limited B-spline patterns. In the future, we expect to explore
NURBS for kinodynamic search, which will allow for various
motion patterns in the kinodynamic search.

We denote by C BK the configuration space in which we
conduct the B-spline-based kinodynamic search. The configuration space C BK is generated by inflating all the obstacles
by δ BK in the workspace. We take a 26-connected 3-D grid
with fixed cell size dx × dy × dz and a fifth-degree Bspline as an example. There is a finite number of possible
span patterns (275 in total). The minimum clearance cBK
min
of the configuration space C BK can be expressed by the
cell size and obstacle inflation δBK according
to cBK
min =

BK
BK
BK
min dx /2 + δ , dy /2 + δ , dz /2 + δ
. The problem of
finding the sufficient condition such that the overall trajectory
is collision free is equivalent to finding the minimum inflation
δ BK such that the trajectories for all the B-spline patterns are
completely bounded inside the inflated cells. Since the total
number of patterns is finite, δ BK can be found by enumerating
all the possible span patterns and picking out the one with the
largest deviation. The script is available.9 Note that the process
of finding the sufficient inflation is one-time work prior to the
planning process. Therefore, it does not affect the EBK search
efficiency. Typically, for a 26-connected 3-D grid with fixed
cell size 0.16 m×0.16 m×0.16 m, the inflation needed is less
than 0.03 m, which is easy to satisfy in practice.

A PPENDIX
A. Proof of Prop. 1

D. Performance analysis of the EBK search

The correctness of the proposition follows from the fact that
the derivative of the B-spline of degree k is another B-spline
of degree k − 1, which enjoys the convex hull property.
Specifically, for the l-th derivative, let Cl map the basis b
to the derivatives; i.e., ddb
l u = Cl b. It follows that

To analyze the performance of the EBK search, we show
than the modified I NDEX function in Algo. 3 induces another
search graph. The characterization of the search graph unveils
the complexity of the EBK search. In the following, we give
a formal definition of the search graph given by the modified
I NDEX function.
We begin with the definition of the nodes which are
called virtual nodes. Specifically, given the encoding level
d and encoding index e, we denote by Hed := {v̂ ∈
VH |I NDEX(v̂, d) = e} the virtual node aggregating all the
vertex tuples which share the same encoding, i.e., the same last
d coordinates. It follows that each vertex tuple v̂ ∈ VH belongs
to exactly one virtual node due to the uniqueness induced
by the function U NIQUE E NCODE(·). For each vertex tuple
v̂i ∈ Hedi , we can obtain the set of neighboring virtual nodes
{Hedj |ej = I NDEX(v̂j , d), ∀(v̂i , v̂j ) ∈ EH }. The interesting
part is that every vertex tuple of Hedi has exactly the same set
of neighboring virtual nodes. We denote by E the set of edges
between virtual nodes, and we denote by GD = (Hd , E) the
virtual graph formed by the virtual nodes, where Hd denotes
the set of all virtual nodes.
There are several unique transformations between GD
and GH . Given the initial state πs ∈ Heds and the goal state

dsj (u)
1 db|
1
=
Mk Pj =
b| C|l Mk Pj .
l
l
l
l
du
(∆t ) d u
(∆t)

(7)

Plug in Sl = Mk−1 Cl Mk /(∆t )l . It follows that
dsj (u)
= b| Mk (Sj Pj ),
(8)
dl u
where Sj Pj is the control point span of the derivative spline.
By applying the convex hull property, we complete the proof.
B. Relationship between G and GH
Lemma 1. Given the initial state πs and goal state πg , let
π = (v0 , v1 , . . . , vT ) be an admissible control point placement
of Problem 1. The extended sequence π̃ uniquely corresponds
to an admissible path Φ = (v̂0 , v̂1 , . . . , v̂Q ) on the graph GH ,
with v̂0 = πs , v̂Q = πg and Q = K + T + 2.
Lemma 2. Any admissible path Φ = (v̂0 , v̂1 , . . . , v̂Q ) on the
graph GH which satisfies v̂0 = πs , v̂Q = πg and (v̂j−1 , v̂j ) ∈
EH for j = 1, . . . , Q uniquely corresponds to an admissible
control point placement π of Problem 1.

9 The corresponding script can be found at https://github.com/WenchaoDing/
kinodynamic replanning.git.

DING et al.: AN EFFICIENT B-SPLINE-BASED KINODYNAMIC REPLANNING FRAMEWORK FOR QUADROTORS

πg ∈ Hedg , an admissible path Θ = (Hed0 , Hed1 , . . . , HedQ ) on
the virtual graph GD should satisfy Hed0 = Heds , HedQ = Hedg
and (Hedj−1 , Hedj ) ∈ E for j = 1, . . . , Q. By the construction,
any admissible path Φ on GH uniquely corresponds to an
admissible path Θ on GD due to the uniqueness of encoding.
Given a known initial state πs , any admissible path Θ on
GD also uniquely corresponds to an admissible path Φ on
GH .10 The reason is that the encoding is based on the last
d > 1 coordinates, and with the known initial virtual node,
the original vertex tuple can be reconstructed.
We define the cost to the virtual node to be c[Hed ] =
min{c[πs , v̂]|∀v̂ ∈ Hed }, where c[πs , v̂] is the minimum cost
from the start vertex tuple πs to v̂ according to Eq. 5. The
EBK search cannot reach the exact goal state πg , and instead
it can only reach the virtual node Hedg . In other words, the
EBK search can only reach the relaxed goal state. Given the
start and goal virtual nodes, the EBK search is complete (finds
the optimal admissible path if one exists) with respect to the
aggregated graph GD , as stated below:
Theorem 2. Given the graph GD = (Hd , E), the start virtual
node Heds ∈ Hd and the goal virtual node Hedg ∈ Hd ,
the EBK search finds the optimal admissible path Θ =
(Heds , Hed1 , . . . , Hedg ) on GD if one exists.
Proof. We prove by induction and contradiction. The proof
follows a similar reasoning process to proving the correctness
of Dijkstra’s algorithm [36]. And the difference is that for one
virtual node, there is one vertex tuple picked out to associate
with the virtual node, and the association will be updated
during the search process. For the expanded virtual node, the
association will be fixed and is supposed to yield the minimum
cost to the virtual node among all the aggregated vertex tuples.
Suppose the following hypothesis holds: for each expanded
virtual node Hvd , c[Hvd ] is the lowest cost from the source
virtual node to Hvd ; and for unexpanded virtual node Hud , c[Hud ]
is the lowest cost from the source virtual node to Hud via
expanded virtual nodes only. The base case is that there is just
the initial virtual node, and the hypothesis holds obviously.
Assume there are n − 1 expanded virtual nodes, and the
hypothesis holds, in which case, the lowest costs to the n − 1
virtual nodes are known, and given the source virtual node,
the vertex tuple associations for the n − 1 virtual nodes are
fixed accordingly. We choose Hvd from the n − 1 nodes such
that it has the least c[Hud ] = c[Hvd ] + f [Hud ] while satisfying
(Hvd , Hud ) ∈ Ed , where f [Hud ] is the cost of the virtual node.
First, c[Hud ] should be the shortest path from the source
node to Hud . Since if there is a shorter path reaching Hud via the
d
nodes other than the n−1 expanded nodes, and Hw
is the first
d
unexpanded node on that path, it follows that c[Hw
] > c[Hud ],
which yields a contradiction.
d
d
Second, for any of the remaining unvisited nodes Hw
, c[Hw
]
should still be the shortest path via the expanded nodes. Since
d
if a lower cost of c[Hw
] is found by adding Hud to the expanded
nodes, Line 17 of Algo. 1 will have updated it. Note that the
d
d
update of c[Hw
] will update the association with Hw
, so the
cost and association are consistent.

10 Without π , the uniqueness no longer holds.
s

19

E. Proof of Theorem 1
The correctness of the theorem follows from the convex
hull property of the B-spline. For brevity, we only consider
one control point span which consists of k + 1 control points,
but can be generalized to a long control point sequence
without any difficulty. The original tube of one control point
span consists of k + 1 balls, and the connectivity is already
guaranteed by the two-level inflation scheme. As such, there
are k intersection areas for the sequence of k+1 control points.
Note that here we only consider the intersection between
the two balls associated with two neighboring control points.
In the extreme case, we add k control points to each of
the intersection areas, and the overall control point sequence
consists of k + 1 + k 2 control points, i.e., k 2 + 1 control
point spans. By applying the convex hull property to each
of the spans, the trajectory for each control point span is
bounded inside one of the balls. As such, the iterative process
can succeed in k 2 iterations if no violation of the dynamical
feasibility is reported.
R EFERENCES
[1] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and
control for quadrotors,” in Proc. of the IEEE Intl. Conf. on Robot. and
Autom., 2011, pp. 2520–2525.
[2] F. Gao and S. Shen, “Online quadrotor trajectory generation and autonomous navigation on point clouds,” in IEEE International Symposium
on Safety, Security, and Rescue Robotics (SSRR), 2016, pp. 139–146.
[3] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for
aggressive quadrotor flight in dense indoor environments,” in Intl. J.
Robot. Research. Springer, 2016, pp. 649–666.
[4] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor,
and V. Kumar, “Planning dynamically feasible trajectories for quadrotors
using safe flight corridors in 3-D complex environments,” IEEE Robotics
and Automation Letters, vol. 2, 2017.
[5] J. Chen, T. Liu, and S. Shen, “Online generation of collision-free
trajectories for quadrotor flight in unknown cluttered environments,” in
Proc. of the IEEE Intl. Conf. on Robot. and Autom. IEEE, 2016, pp.
1476–1483.
[6] S. M. LaValle and J. J. Kuffner Jr, “Randomized kinodynamic planning,”
Intl. J. Robot. Research, vol. 20, no. 5, pp. 378–400, 2001.
[7] D. J. Webb and J. van den Berg, “Kinodynamic RRT*: Asymptotically
optimal motion planning for robots with linear dynamics,” in Proc. of
the IEEE Intl. Conf. on Robot. and Autom., 2013, pp. 5054–5061.
[8] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Batch informed trees
(bit*): Sampling-based optimal planning via the heuristically guided
search of implicit random geometric graphs,” in Proc. of the IEEE Intl.
Conf. on Robot. and Autom., 2015, pp. 3067–3074.
[9] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal
motion planning,” Intl. J. Robot. Research, pp. 846–894, 2011.
[10] L. Janson, E. Schmerling, A. Clark, and M. Pavone, “Fast marching tree:
A fast marching sampling-based method for optimal motion planning in
many dimensions,” Intl. J. Robot. Research, pp. 883–921, 2015.
[11] Y. Kuwata, J. Teo, S. Karaman, G. Fiore, E. Frazzoli, and J. How, “Motion planning in complex environments using closed-loop prediction,” in
AIAA Guidance, Navigation and Control Conference and Exhibit, 2008,
p. 7166.
[12] C. Xie, J. van den Berg, S. Patil, and P. Abbeel, “Toward asymptotically
optimal motion planning for kinodynamic systems using a two-point
boundary value problem solver,” in Proc. of the IEEE Intl. Conf. on
Robot. and Autom., 2015.
[13] Y. Li, Z. Littlefield, and K. E. Bekris, “Asymptotically optimal samplingbased kinodynamic planning,” Intl. J. Robot. Research, vol. 35, no. 5,
pp. 528–564, 2016.
[14] S. Liu, N. Atanasov, K. Mohta, and V. Kumar, “Search-based motion
planning for quadrotors using linear quadratic minimum time control,”
in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst., 2017.
[15] W. Ding, W. Gao, K. Wang, and S. Shen, “Trajectory replanning for
quadrotors using kinodynamic search and elastic optimization,” in Proc.
of the IEEE Intl. Conf. on Robot. and Autom. IEEE, 2018, pp. 7595–
7602.

20

[16] J. Van Den Berg, D. Wilkie, S. J. Guy, M. Niethammer, and D. Manocha,
“LQG-obstacles: Feedback control with collision avoidance for mobile
robots with motion and sensing uncertainty,” in Proc. of the IEEE Intl.
Conf. on Robot. and Autom. IEEE, 2012, pp. 346–353.
[17] D. Zhou and M. Schwager, “Vector field following for quadrotors using
differential flatness,” in Proc. of the IEEE Intl. Conf. on Robot. and
Autom. IEEE, 2014, pp. 6567–6572.
[18] D. Bareiss, J. Van Den Berg, and K. K. Leang, “Stochastic automatic
collision avoidance for tele-operated unmanned aerial vehicles,” in Proc.
of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst. IEEE, 2015,
pp. 4818–4825.
[19] S. Liu, K. Mohta, N. Atanasov, and V. Kumar, “Search-based
motion planning for aggressive flight in SE (3),” arXiv preprint
arXiv:1710.02748, 2017.
[20] M. Likhachev and D. Ferguson, “Planning long dynamically feasible
maneuvers for autonomous vehicles,” Intl. J. Robot. Research, vol. 28,
2009.
[21] S. Aine, S. Swaminathan, V. Narayanan, V. Hwang, and M. Likhachev,
“Multi-heuristic A*,” Intl. J. Robot. Research, pp. 224–243, 2016.
[22] S. Karaman and E. Frazzoli, “Incremental sampling-based algorithms
for optimal motion planning,” Proc. of Robot.: Sci. and Syst., vol. 104,
p. 2, 2010.
[23] R. E. Allen and M. Pavone, “A real-time framework for kinodynamic
planning with application to quadrotor obstacle avoidance,” Ph.D. dissertation, Stanford University, 2016.
[24] M. Pivtoraiko, D. Mellinger, and V. Kumar, “Incremental micro-UAV
motion replanning for exploring unknown environments,” in Proc. of
the IEEE Intl. Conf. on Robot. and Autom. IEEE, 2013, pp. 2452–
2458.
[25] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran, “Continuous-time trajectory optimization for online UAV replanning,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.,
2016, pp. 5332–5339.
[26] R. Deits and R. Tedrake, “Efficient mixed-integer planning for UAVs in
cluttered environments,” in Proc. of the IEEE Intl. Conf. on Robot. and
Autom., 2015, pp. 42–49.
[27] S. K. Kannan, W. M. Sisson, D. A. Ginsberg, J. C. Derenick, X. C. Ding,
T. A. Frewen, and H. Sane, “Close proximity obstacle avoidance using
sampling-based planners,” in AHS Specialists’ Meeting on Unmanned
Rotorcraft and Network-Centric Operations, 2013.
[28] J. Chen and S. Shen, “Improving octree-based occupancy maps using
environment sparsity with application to aerial robot navigation,” in
Proc. of the IEEE Intl. Conf. on Robot. and Autom. IEEE, 2017,
pp. 3656–3663.
[29] K. Qin, “General matrix representations for b-splines,” The Visual
Computer, vol. 16, no. 3, pp. 177–186, 2000.
[30] K. Yang and S. Sukkarieh, “An analytical continuous-curvature pathsmoothing algorithm,” IEEE Transactions on Robotics, vol. 26, no. 3,
pp. 561–568, 2010.
[31] L. Yang, D. Song, J. Xiao, J. Han, L. Yang, and Y. Cao, “Generation of
dynamically feasible and collision free trajectory by applying six-order
Bezier curve and local optimal reshaping,” in Proc. of the IEEE/RSJ
Intl. Conf. on Intell. Robots and Syst. IEEE, 2015, pp. 643–648.
[32] W. Ding, L. Zhang, J. Chen, and S. Shen, “Safe trajectory generation for
complex urban environments using spatio-temporal semantic corridor,”
IEEE Robot. and Auto. Letters, 2019.
[33] F. Gao, Y. Lin, and S. Shen, “Gradient-based online quadrotor safe
trajectory planning in 3d complex environments,” in Proc. of the
IEEE/RSJ Intl. Conf. on Intell. Robots and Syst., 2017.
[34] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers, “Real-time
trajectory replanning for mavs using uniform b-splines and 3d circular
buffer,” arXiv preprint arXiv:1703.01416, 2017.
[35] E. Verriest and F. Lewis, “On the linear quadratic minimum-time
problem,” IEEE Transactions on Automatic Control, vol. 36, no. 7, pp.
859–863, 1991.
[36] E. W. Dijkstra, “A note on two problems in connexion with graphs,”
Numerische mathematik, vol. 1, no. 1, pp. 269–271, 1959.
[37] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic
determination of minimum cost paths,” IEEE transactions on Systems
Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968.
[38] S. J. Russell and P. Norvig, Artificial Intelligence: A Modern Approach.
Malaysia; Pearson Education Limited,, 2016.
[39] M. Kleinbort, O. Salzman, and D. Halperin, “Collision detection or
nearest-neighbor search? on the computational bottleneck in samplingbased motion planning,” arXiv preprint arXiv:1607.04800, 2016.

AUTHOR’S VERSION

[40] D. P. Bertsekas, D. P. Bertsekas, D. P. Bertsekas, and D. P. Bertsekas,
Dynamic Programming and Optimal Control. Athena Scientific Belmont, MA, 1995, vol. 1, no. 2.
[41] T. H. Cormen, Introduction to Algorithms. MIT press, 2009.
[42] S. Quinlan and O. Khatib, “Elastic bands: Connecting path planning and
control,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. IEEE,
1993, pp. 802–807.
[43] Z. Zhu, E. Schmerling, and M. Pavone, “A convex optimization approach
to smooth trajectories for motion planning with car-like robots,” in Proc.
of the IEEE Control and Decision Conf., 2015, pp. 835–842.
[44] T. Qin, P. Li, and S. Shen, “VINS-mono: A robust and versatile monocular visual-inertial state estimator,” arXiv preprint arXiv:1708.03852,
2017.
[45] K. Wang, W. Ding, and S. Shen, “Quadtree-accelerated real-time monocular dense mapping,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell.
Robots and Syst. IEEE, 2018.
[46] W. Gao and S. Shen, “Dual-fisheye omnidirectional stereo,” in Proc. of
the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst. IEEE, 2017, pp.
6715–6722.
[47] F. Gao, W. Wu, J. Pan, B. Zhou, and S. Shen, “Optimal time allocation
for quadrotor trajectory generation,” in Proc. of the IEEE/RSJ Intl. Conf.
on Intell. Robots and Syst. IEEE, 2018.
[48] S. G. Johnson, The NLopt nonlinear-optimization package, 2011.
[Online]. Available: http://ab-initio.mit.edu/nlopt
Wenchao Ding received his B.Eng. degree in Electronic and Information Engineering from Huazhong
University of Science and Technology, China, in
2015. He is currently pursuing his PhD degree in the
Hong Kong University of Science and Technology
under the supervision of Prof. Shaojie Shen.
His research interests include decision making,
prediction, motion planning and autonomous navigation for aerial robots and autonomous vehicles.

Wenliang Gao received his B.Eng. degree in optical
engineering from Beijing Institute of Technology,
Beijing, China, in 2016. He received his M.Phil.
degree in robotics from Hong Kong University of
Science and Technology, Hong Kong, in 2018. He is
currently working as an algorithm engineer in DJI.
His research interests include state estimation,
navigation, and mapping with the visual-inertial system of autonomous robots.

Kaixuan Wang received his B.Eng. degree in automation from Southeast University, Nanjing, China,
in 2016. He then joined HKUST Aerial Robotics
Group at the Hong Kong University of Science and
Technology, under the supervision of Prof. Shaojie
Shen.
His research interests cover real-time 3D perception and mapping for robotics navigation, especially
for UAV autonomous flight and autonomous vehicles.

Shaojie Shen received his B.Eng. degree in Electronic Engineering from the Hong Kong University
of Science and Technology (HKUST) in 2009. He
received his M.S. in Robotics and Ph.D. in Electrical
and Systems Engineering in 2011 and 2014, respectively, all from the University of Pennsylvania. He
joined the Department of Electronic and Computer
Engineering at the HKUST in September 2014 as an
Assistant Professor.
His research interests are in the areas of robotics
and unmanned aerial vehicles, with focus on state
estimation, sensor fusion, localization and mapping, and autonomous navigation in complex environments. He is currently serving as associate editors for
T-RO and AURO.

