TARE: A Hierarchical Framework for Efficiently Exploring

Complex 3D Environments
Chao Cao, Hongbiao Zhu, Howie Choset, and Ji Zhang

Global coarse path

Abstract— We present a method for autonomous exploration
in complex three-dimensional (3D) environments. Our method
demonstrates exploration faster than the current state-of-the-art
using a hierarchical framework – one level maintains data densely Global subspace
and computes a detailed path within a local planning horizon,
while another level maintains data sparsely and computes a
Local planning horizon Local detailed path
coarse path at the global scale. Such a framework shares the
insight that detailed processing is most effective close to the robot,
and gains computational speed by trading-off details far away Fig. 1. Illustration of our exploration framework. Inside the local planning
from the robot. The method optimizes an overall exploration horizon (green box), data is densely maintained and a local detailed path is
path with respect to the length of the path and produces a computed (dark-blue curve). At the global scale, data is sparsely maintained
kinodynamically feasible local path. In experiments, our systems in distant subspaces (solid green cubes) and a global coarse path is computed
autonomously explore indoor and outdoor environments at a high (light-blue curve). The local path and global path are connected on the
degree of complexity, with ground and aerial robots. The method boundary of the local planning horizon to form the exploration path.
produces 80% more exploration efficiency, defined as the average
explored volume per second through a run, and consumes less that requires detailed exploration, hence the need for a detailed
than 50% of computation compared to the state-of-the-art. path to guide the robot to explore locally (green box in Fig. 1).
I. I NTRODUCTION The method uses the path through the global representation to
move over large distances to areas throughout the free space
We consider the problem of exploring three-dimensional
(solid green cubes in Fig. 1). Combination of the paths at both
(3D) spaces unknown a priori with an autonomous robot. Such
levels allows the robot to explore areas in a sequence jointly
a problem remains challenging as the problem has to deal with
determined by local and global information.
two tasks simultaneously – 1) online updating a representation
Our method is evaluated using both ground and aerial
of the environment to keep track of explored areas, and 2)
robots. We show results where the ground robot explores
searching the representation for a continuous traversable path
the interior of a complex multi-storage building in physical
to guide the exploration. In cases where the environment
experiment and the aerial robot explores a large-scale outdoor
is large-scale, structurally complex, and 3D, the problem
environment in simulation. We compare the results to state-
becomes computationally complex, and ensuring complete
of-the-art methods and conclude that the proposed method
exploration of the environment can become a challenge.
produces 80% more exploration efficiently, defined as the
The benefit of our method is that it can explore 3D spaces
average explored volume per second, and uses computation
faster than the current state-of-the-art. The strength of the
less than 50% of the state-of-the-art methods.
approach is based on a hierarchical framework to separate
Further, we release a software environment for benchmark-
the processing at two levels. The first level maintains a high-
ing exploration algorithms and facilitating the development of
resolution representation of the environment surrounding the
complete autonomous navigation systems. The environment
robot, namely, the local planning horizon (green box in Fig.
contains representative simulation environment models, fun-
1). Within that, a kinodynamically feasible path is generated
damental navigation modules, e.g. collision avoidance, terrain
for the robot to follow. The second level maintains a low-
traversability analysis, way-point following, and visualization
resolution representation and computes a path connecting
tools. Our algorithm code is made publicly available1 .
distant areas, namely subspaces (solid green cubes in Fig. 1),
in the global environment. The insight of such a framework II. R ELATED W ORK
is that detailed processing is most effective in the vicinity of The problem of autonomous exploration has been tackled
the robot, while limited processing provides sufficient utility from multiple angles. The approach described in this paper is
far away from the robot. The framework performs a bulk of based on key results in information theory, frontier-based ex-
the processing inside the local planning horizon and trades-off ploration, topological exploration, and a few random sampling-
details for fast processing at the global scale. based methods briefly discussed in this section.
The method first plans a path through the global representa- Information Theory: A popular approach in solving the
tion. Such a path identifies the areas in the robot’s free space exploration problem is using information theory. The methods
TARE is named after an effort to develop Technologies for Autonomous
maximize the information gain over the next few actions [1]–
Robot Exploration. All authors are with the Robotics Institute at Carnegie Mel- [4]. The above-mentioned work are also extended to solving
lon University, Pittsburgh PA. Emails: {ccao1, hongbiaz, choset,
zhangji}@cmu.edu. 1 Sim. environment and source-code: www.cmu-exploration.com
the multi-robot exploration problem [5]–[7]. In summary, III. P ROBLEM D EFINITION
majority of these existing methods rely on greedy strategies, Define Q ⊂ R3 as the work space to be explored. Let
where efficiency is limited due to the methods being myopic. Qtrav ⊂ Q be the traversable subspace. Define viewpoint v ∈
In contrast, our method seeks the optimal exploration path as SE(3) to describe the pose of the sensor onboard the robot,
a whole other than maximizing the instant rewards. v = [pv , qv ] where pv ∈ Qtrav and qv ∈ SO(3) respectively
Frontier-based Exploration: A common formulation of the denote the position and orientation. Denote L ⊂ SE(3) as the
problem uses frontiers, i.e. boundary between mapped and set of viewpoints along the vehicle past trajectory. We use the
unmapped areas. When exploring, the vehicle keeps moving term “surface” to refer to the generalized boundary between
toward frontiers, which extends the boundary of the mapped free space and non-free space, the latter includes both occupied
areas until the entire environment is explored [8]–[14]. While and unknown spaces. Let Sv ⊂ Q be the surfaces perceived by
most of these methods greedily select frontiers to explore, the sensor at v. Note that the same surface can be perceived
Faigl and Kulich’s method determines a minimum set of from multiple viewpoints. The perceived surfaces so far are
viewpoints to cover the frontiers by solving a variant of the [
art gallery problem [15]. Similarly, our method also finds a S= Sv . (1)
v∈L
minimum set of viewpoints but does so by recursively and
randomly sampling the viewpoints. Here, S contains covered surfaces, denoted as Scov ⊂ S, and
yet uncovered surfaces, denoted as S̄ = S \ Scov . We would
Topological Exploration: Other approaches model the envi-
like to find the shortest path, which when followed by the
ronment with topological representations [16]–[18]. Most of
vehicle covers S̄. The path must meet kinodynamic constraints.
these approaches divide the environment into topologically
Let vcurrent be the viewpoint located at the vehicle’s current
distinct sections, where an exhaustive traversal through the
sensor pose. Our problem can be defined as follows.
sections entails complete exploration of the environment.
While these methods focus on topological completeness, our Problem 1: Given S̄ and vcurrent , find the shortest path
method aims at producing a detailed map of the environment. T ∗ formed by viewpoints v1 , v2 , ..., which when followed
Random Sampling-based Methods: Recent work develops by the vehicle covers S̄, such that vcurrent ∈ T ∗ , and T ∗ is
a few methods based on the Rapidly-exploring Random Tree kinodynamically feasible.
(RRT) [19] or Rapidly-exploring Random Graph (RRG) [20]. Problem 1 is solved repetitively at each planning cycle. We
The methods span RRTs or RRGs in the environment to use S̄ to compute the exploration path. When executing the
find the traversable space, within which the most informative path, we online update S with up-to-date sensor readings,
branch on the RRT or RRG is selected as the exploration path. processing both displaced and newly perceived surfaces. Then,
Specifically, Bircher et al. propose the Next-Best-View Planner we move surfaces become covered from S̄ to Scov , and use S̄
(NBVP) [21]. The method spans an RRT, the nodes of which in the next planning cycle, hence the exploration continues.
are modeled as viewpoints. The next viewpoint on the branch
IV. M ETHODOLOGY
that maximizes a reward function is chosen as the navigation
goal. Witting et al. [22] extend NBVP by seeding the RRT with A. Viewpoint Sampling
the vehicle’s trajectory, which allows the vehicle to explore We define the criteria for a surface point to be covered by
further in areas passed by previously. Dang et al. improve the sensor. Consider a surface patch centered at ps ∈ Q with
the scheme further by proposing the Graph-Based exploration normal ns ∈ R3 pointing toward the free-space side, the center
Planner (GBP) [23]. The method constructs a global RRG point on the surface patch is covered by viewpoint v, if
along the vehicle trajectory. When the local area is explored,
|ps − pv | ≤ D, (2)
the method plans routes based on the global RRG to distant
areas for further exploration. Note that in this method, the ns · (pv − ps )/|ns ||pv − ps | ≥ T, (3)
exploration mode and relocation mode are explicitly switched where D and T are two constants constraining the relative dis-
using heuristics. Recently, Dharmadhikari at al. propose the tance and orientation of the surface patch w.r.t. the viewpoint.
Motion primitives-Based exploration Planner (MBP) [24], a Such criteria encourage the surfaces to be perceived well. In
variant of GBP that constructs the RRT with motion primitives, practice, D is set to be shorter than the sensor range.
which produces smoother local paths spanning in constrained Define H ⊂ Q as the local planning horizon as shown in
directions. In essence, these methods adopt greedy strategies. Fig. 2. Let Htrav ⊂ H be the traversable subspace identified
Due to the randomness of RRT and RRG, these methods also H
by considering collision and connectivity, and let Ctrav be the
tend to overlook areas that have not been completely explored. corresponding configuration space considering rotation and
The main contribution of our work is a hierarchical frame- translation. Define S̄H ⊂ S̄ as the uncovered surfaces that
H
work to enable highly efficient exploration. The framework can be perceived from viewpoints in Ctrav . The problem of
does not involve heuristics, as GBP and MBP, for explicit viewpoint sampling is to select a minimum set of viewpoints
H
mode switch. Experiment comparisons to NBVP [21], GBP in Ctrav to cover S̄H . Let us use S̄v ⊂ S̄H to denote the
H
[23] and MBP [24] show that our method explores much more uncovered surfaces to be perceived from v ∈ Ctrav . The
completely and efficiently while consuming less computation. reward of v is defined as the area of S̄v , denoted as Av .
 Algorithm 1: Compute Local Path
H
input : traversable C-space Ctrav , uncovered surfaces
S̄H , current viewpoint vcurrent , boundary
1 2
viewpoints vboundary and vboundary
output: local path Tlocal
H
1 Generate a set of viewpoint candidates V in Ctrav ;
2 Initialize priority queue Q;
3 For every v ∈ V, estimate coverage S̄v , then push v
Fig. 2. Illustration of mathematical definitions. The green box represents
the local planning horizon H. The solid green squares represent the exploring
into Q with the priority set to its reward Av ;
subspaces Gh , h ∈ Z+ . The dark-blue curve is the local path Tlocal . The 4 Tlocal ← ∅, cbest ← +∞;
light-blue curve is the global path Tglobal . The dark-blue dot on Tlocal is the 5 for i := 1 to K do
1
current viewpoint vcurrent . The light-blue dots are viewpoints vboundary and
2
6 Q0 ← Q, V 0 ← {vcurrent , vboundary
1 2
, vboundary };
vboundary on the boundary of H where Tlocal and Tglobal are connected. 0 0
The orange dotes are sampled viewpoints vi , i ∈ Z+ . The orange lines are 7 while Q 6= ∅ and Q contains at least one
uncovered surfaces S̄H to be perceived by the viewpoints. The method uses non-zero priority do
an iterative random sampling process in determining vi to cover S̄H . 8 Probabilistically pick viewpoint v0 in Q0 , then
remove v0 from Q0 ;
Note that the problem exhibits submodularity [25], i.e. with 9 V 0 ← V 0 ∪ v0 ;
more viewpoints selected, the reward of selecting an additional 10 Update priorities for all viewpoints in Q0 ;
viewpoint decreases. This is because nearby viewpoints have 11 end
overlapping field-of-views, and the same surface can be per- 12 Compute smooth path Tsmooth 0
and cost c0smooth
ceived from multiple viewpoints. Consequently, the reward of using Algorithm 2;
a viewpoint is dependent on the viewpoints selected earlier. 13 if c0smooth < cbest then
Let vi , i ∈ Z+ , be the i-th viewpoint selected. The uncovered 14
0
Tlocal ← Tsmooth , cbest ← c0smooth ;
surfaces to be perceived from vi , S̄vi , needs to be adjusted to
Si−1 15 end
S̄vi − j=1 (S̄vi ∩ S̄vj ). Then, Avi is adjusted accordingly. 16 end
Algorithm 1 presents the process of viewpoint sampling. 17 return Tlocal ;
The algorithm first generates a set of viewpoint candidates V
uniformly from a lattice pattern in Htrav (line 1). Second, it
computes the rewards Av for all viewpoint candidates v ∈ V
by estimating their coverages S̄v with the updated environment like the path to be kindodynamically feasible, which can be
representation (line 3). Then, a process is iterated for K times followed by the vehicle at a high speed. Here, we focus on the
to determine the viewpoints. At each iteration, the algorithm curvature constraint such that the maximum curvature of the
randomly samples a subset of viewpoints from V that covers path is determined by the vehicle’s minimum turning radius
S̄H (line 6-11). Three viewpoints are pre-selected (line 6), when moving at the desired speed. Due to the distribution
one as the current viewpoint vcurrent , and the other two as the of the viewpoints in V 0 and structures in the environment,
1
viewpoints on the boundary of H, vboundary 2
and vboundary , a continuous path that meets the curvature constraint can
connecting the local path and global path. The process of be impossible. The method computes the path in smooth
1
determining vboundary 2
and vboundary is given in Section IV- segments as shown in Fig. 3. The vehicle stops at the end
0
C. A priority queue Q is used to manage the viewpoint of each segment before moving on to the next segment in
candidates. The priority of a viewpoint v is set to its reward a different direction. This requires that the vehicle can turn
Av . Viewpoints are selected from the priority queue with in one place. Considering the focus of this paper is not on
probabilities proportional to their rewards (line 8). Due to the path planning using sophisticated motion models, we adopt
submodularity, the rewards of the remaining viewpoints in the the generic differential motion model, and for aerial vehicles,
priority queue are reduced accordingly after a viewpoint is with independent altitude control. Let us denote the path as
0
selected, accounting for the overlapping field-of-views (line Tsmooth = [v11 , v21 , ...][v12 , v22 , ...]..., where [·] represents a
10). The viewpoint sampling process finishes when the priority segment and vkj is the k-th viewpoint on the j-th segment,
queue is empty or the marginal reward of adding a new j, k ∈ Z+ . Note that the last viewpoint on a segment and
viewpoint is negligible. The algorithm calls Algorithm 2 to the first viewpoint on the following segment share the same
generate a path through the sampled viewpoints (line 12), viewpoint. Let ñ ∈ Z+ be the number of segments, ñ ≥ j.
discussed in Section IV-B. Via iterations, paths with lower Define lj as the length of the j-th segment. We discourage
costs are found and the path with the lowest cost is returned stopping too frequently by applying a penalty p at each stop.
0
as the local path, denoted as Tlocal . The cost of Tsmooth is defined as
X ñ
B. Path Generation and Smoothing c0smooth = lj + p(ñ − 1). (4)
Given a set of sampled viewpoints V 0 , we want to generate j=1
a path through each of the viewpoints. Ideally, we would The problem of computing the path can be stated as follows.
 similar to [30], where the segment is modeled as a set of cubic
splines connected at the viewpoints. Boundary conditions are
applied at the viewpoints. Control points for the splines are
placed on the initial paths given by the A* search and adjusted
by a nonlinear optimization solver [31] to account for collision
clearance and path smoothness. To accelerate processing, the
nonlinear optimization is marginalized where only the splines
between the two adjacent viewpoints of vi are optimized. The
Fig. 3. Illustration of smoothed path Tsmooth . The dark-blue curves represent 0
path segments which satisfy the curvature constraint. The dark-blue dot is algorithm returns Tsmooth with c0smooth .
1
vcurrent . The light-blue dots are vboundary 2
and vboundary connecting to
Tglobal . The orange dots are sampled viewpoints and the hollow-centered C. Global Planning
orange dots are break-points between the path segments. The vehicle makes
a stop at each break-point before moving on to the next path segment.
We divide the space outside H into even cuboid subspaces.
Each subspace stores the covered and uncovered surfaces
developed during the exploration. Note that the data is kept in
Problem 2: Given viewpoints V 0 , find a path Tsmooth
∗
= the subspaces only for storage, while the data in H is actively
0
[v11 , v21 , ...][v12 , v22 , ...]... with the lowest
csmooth such that updated as the exploration proceeds. Each subspace holds a
∗
Tsmooth V 0 once and each
visits each of the viewpoints in status from “unexplored”, “exploring”, and “explored”. If a
∗
segment on Tsmooth satisfies the curvature
constraint. subspace does not contain any covered or uncovered surfaces,
Problem 2 is NP-hard given that it is an extended version the status is ”unexplored”. If a subspace contains only covered
of the Traveling Salesman Problem (TSP) [26]. Instead of surfaces, the status is ”explored”. If a subspace contains any
attempting to find an exact solution, we solve the problem uncovered surfaces, the status is ”exploring”. We only consider
using approximation algorithms in two steps. First, we solve the exploring subspaces in global planning. Denote Gh ⊂ Q,
for an order of the viewpoints with a standard TSP. Second, h ∈ Z+ , as an exploring subspace and Ĝ as the set of exploring
we separate the viewpoints into segments following the order. subspaces. The global planning problem is to find a global
This is to determine if a viewpoint, except the first and the path Tglobal that goes through the current viewpoint vcurrent
last ones, is an inner-point within a segment or a break- and the centroid of each subpace in Ĝ. During the course of
point between two segments. Let n0 ∈ Z+ be the number the exploration, we construct a sparse random roadmap in the
of viewpoints in V 0 . Define x = [x1 , x2 , ..., xn0 −2 ] as a traversable space expanded from the past trajectory. Similar to
sequence of boolean variables describing the status of the local planning, we use A* search on the roadmap for shortest
n0 − 2 viewpoints (excluding the first and last) and a function paths between the subspaces followed by solving a TSP.
f (x) = c0smooth . The problem of determining the viewpoint Algorithm 3 gives the overall procedure for computing the
status can be formulated as, exploration path. The algorithm constructs a distance matrix
Problem 3: Given a sequence of bolean variables x and a containing the length of the paths found on the roadmap
function f (x) = c0smooth , find x∗ such that (line 1) and solves a TSP (line 2). The two points on Tglobal
1
intersecting with the boundary of H are extracted as vboundary
2
x∗ = argmin c0smooth . (5) and vboundary (line 3). Then, Algorithm 1 is used to compute
x Tlocal (line 4). Finally, Tlocal and Tglobal are concatenated by
Problem 3 is a nonlinear integer programming problem and substituting the part of Tglobal inside H with Tlocal (line 5).
known to be NP-hard [27]. Especially, finding smooth paths Fig. 4 shows an example of the exploration where the dark-
involves time-consuming trajectory optimization. Instead of blue and light-blue paths represent Tlocal and Tglobal .
using an existing heuristic method such as [28] with a higher When the exploration completes in H (S̄H = ∅), Tlocal re-
computational complexity, our method applies a greedy strat- 1
duces to the shortest paths connect from vcurrent to vboundary
egy to check each viewpoint only once, maximally reducing 2
and vboundary , which are further connected to the adjacent
the runtime. Algorithm 2 gives the procedure of computing exploring subspaces on Tglobal . The vehicle follows the path to
0
Tsmooth from a set of viewpoints V 0 . The algorithm finds the transit to an exploring subspace to resume exploration. In other
shortest collision-free path between every viewpoint pair in V 0 words, the algorithm implicitly transitions between exploration
using the A* algorithm [29] and construct a distance matrix D0 and relocation to another area to explore further. If S̄H = ∅
containing the length of the paths (line 1). Next, the algorithm and Ĝ = ∅, the exploration terminates.
solves a TSP for a traversal order of the viewpoints (line
0
2). Upon determining the segments on Tsmooth , the algorithm D. Theoretical Analysis
initializes all n0 − 2 viewpoints as break-points (line 3) and 1) Computational Complexity: Let n ∈ Z+ be the number
smooths the segments between consecutive viewpoints (line of viewpoint candidates generated in Algorithm 1. For a fixed
4). Later, the algorithm attempts to reduce the cost c0smooth by number of iterations, the sampled viewpoints is no more than
sequentially setting each viewpoint vi as an inner-point and n. After selecting each viewpoint, adjusting rewards of the
re-smoothing the segment through vi (lines 5-11). Each path remaining viewpoints in the priority queue takes O(n) time.
segment is smoothed with a trajectory optimization method In Algorithm 2, we model the time of finding the shortest
 Algorithm 2: Compute Local Path from Viewpoints Algorithm 3: Compute Exploration Path
input : traversable C-space H
Ctrav , viewpoints V0 input : local planning horizon H, traversable C-space
H
output: smooth path Tsmooth 0
, cost c0smooth Ctrav , uncovered surfaces S̄H , exploring
1 Compute shortest paths between viewpoint pairs in V 0 , subspaces Ĝ, current viewpoint vcurrent
then create distance matrix D0 ; output: exploration path T
2 Compute path T 0 by solving TSP using D0 ; 1 Compute shortest paths between centroids in Ĝ and
0
3 Initialize Tsmooth using T 0 , then set viewpoints v2 , v3 , vcurrent , then create distance matrix D;
..., vn−1 on Tsmooth as break-points; 2 Compute global path Tglobal by solving TSP using D;
1 2
4 Smooth path segments between consecutive viewpoints 3 Extract vboundary and vboundary as the intersections
0
on Tsmooth and compute cost c0smooth ; between Tglobal and the boundary of H;
5 for i := 2 to n0 − 1 do 4 Compute local path Tlocal using Algorithm 1;
6 Temporarily set viewpoint vi as an inner-point by 5 Concatenate Tlocal and Tglobal to generate T ;
connecting the two segments on both sides of vi ; 6 return T ;
7 Smooth the path segment through vi and compute
cost c0temp ;
8 if c0temp < c0smooth and Tsmooth 0
meets curvature The above relationship helps us draw Lemma 2 as follows.
constraint then
9 Finalize vi on Tsmooth0
as an inner-point; Lemma 2: The set function a(V) = AV is submodular.
10
0 0
csmooth ← ctemp ; Let SH ⊂ Q be the surfaces perceivable from viewpoints in
H
11 end Ctrav . As our method uses random sampling in selecting the
12 end viewpoints, Theorem 2 states the probabilistic completeness.
0
13 return Tsmooth , c0smooth ; Theorem 2: Algorithm 3 is probabilistically complete in
computing path T to cover SH .
Sketch Proof: Let SH,cov ⊂ SH be the covered surfaces and
path between two viewpoints and the time of smoothing a recall S̄H ⊂ SH as the uncovered surfaces. Since function
path segment as bounded by two constants (the trajectory a(V) = AV is monotone, as more viewpoints are selected,
optimization is marginalized where only a bounded number SH,cov → SH and correspondingly S̄H → ∅. The probability
of control points are optimized). The time complexity of that S̄H remains nonempty p(S̄H 6= ∅) → 0. 
constructing the distance matrix is O(n2 ). The TSP is solved The exploration process covers all surfaces in S at the end
using the Lin–Kernighan heuristic which consumes O(n2.2 ) since the termination criterion is S̄H = ∅ and Ĝ = ∅.
time [26]. The path smoothing processes each viewpoint once 3) Approximation Ratio: Let us analyze the approximation
and takes O(n) time. In Algorithm 3, computing the distance ratio introduced by the hierarchy. To simplify the problem,
matrix takes O(m2 ) time and solving the TSP runs in O(m2.2 ) we evaluate a path with its length and ignore its kinodynamic
time, where m ∈ Z+ is the number of exploring subspaces. feasibility. Define T ∗ as the shortest possible path to complete
Concatenating Tlocal and Tglobal takes constant time. The time the coverage. T ∗ is computed without using the hierarchy and
complexity of our algorithm is stated in Theorem 1. considered the theoretically optimal solution. Denote l∗ as the
Theorem 1: Algorithm 3 runs in O(n2.2 + m2.2 ) time. length of T ∗ . Let DH be the longest distance needed to travel
H
between any two viewpoints in Ctrav , i.e. for all the shortest
2) Probabilistic Completeness: Given a set of viewpoints paths between any two viewpoints in Ctrav H
, DH is the length
V, let AV be the corresponding area of covered surfaces. It of the longest path. As defined in (2), D is the distance limit
is our observation that AV monotonically increases as more for covering surfaces. Denote Dwidth , Dlength , and Dheight
viewpoints are added to V. Define a set function a(V) = AV . as the dimensions of a subspace in Ĝ. We consider a group
For any two sets of viewpoints V and V 0 , we have
V 0 ⊆ V ⇒ a(V 0 ) ≤ a(V). (6)
The above relationship helps us draw Lemma 1 as follows.
Lemma 1: The set function a(V) = AV is monotone.
Further, we observe that AV exhibits submodularity, i.e. as
more viewpoints are selected, the marginal reward of adding
a new viewpoint monotonically decreases. As discussed, this
is due to that the same surface is perceived by multiple
viewpoints with overlapping field-of-views. For any two sets Fig. 4. An example exploration process with real data. The figure uses the
of viewpoints V and V 0 , and a single viewpoint v, same color code as Fig. 2. The white points show lidar scan data, with which
the method extracts uncovered surfaces (red points). The yellow dots are the
V 0 ⊆ V ⇒ a(V 0 ∪ v) − a(V 0 ) ≥ a(V ∪ v) − a(V). (7) viewpoint candidates, from which viewpoints are sampled (orange dots).
of ([D/Dwidth ] + 2) × ([D/Dlength ] + 2) × ([D/Dheight ] + 2) or require the path being kinodynamically feasible. Tlocal is
connected subspaces where the sensor can stay in one subspace assumed to be the shortest possible path connecting to E, F
and “see through” to cover surfaces in another subspace. Let and fulfilling a coverage within H. we have
DG be the longest distance needed to travel between any two ∗
viewpoints in such a group of subspaces. The approximation llocal ≤ llocal + 2DH . (10)
ratio introduced by the hierarchy is stated in Theorem 3. ∗
Eq. (10) can be proved by forming a new path with Tlocal and
Theorem 3: The approximation ratio of the path length by the two short paths connecting between B, E and C, F . If
the hierarchy has σhier ≤ (l∗ + 4DH + 2mDG )/l∗ . (10) does not hold, the new path becomes shorter than Tlocal
Proof: As shown in Fig. 5, let Tlocal ∗
be the part of T ∗ and a contradiction occurs. Combining (8)-(10) gives
∗
inside H and let Tglobal be the part outside H. The length of llocal + lglobal ≤ l∗ + 4DH + 2mDG . (11)
∗ ∗ ∗ ∗
Tlocal and Tglobal are denoted as llocal and lglobal , respectively.
†
Define Tglobal as the shortest global path connecting to Tlocal ∗ Therefore, the approximation ratio is bounded by
at B, C and going through the centriod of each subspace in
† †
σhier ≤ (l∗ + 4DH + 2mDG )/l∗ . (12)
Ĝ. The length of Tglobal is denoted as lglobal . We have

†
lglobal ∗
≤ lglobal + 2mDG . (8) In practice, the subspaces in Ĝ are often located at intersec-
tions where the vehicle takes one branch and leaves exploring
∗
Eq. (8) can be proved by contradiction. On Tglobal , we add subspaces at the entrances of the other branches. These explor-
two pieces of short paths to connect it to the centroid of each ing subspaces are converted to “explored” when the vehicle
subspace in Ĝ. One short path is for traveling to the centroid revisits the intersections and completes the coverage. When the
∗
and the other for coming back to Tglobal . Since T ∗ completes environment is large-scale, the exploring subspaces are usually
∗
the coverage, Tglobal has to approach each subspace in Ĝ for sparsely distributed through the environment. With a large l∗ ,
the underlying surfaces to be covered. These short paths can be m/l∗ is relatively small and σhier is close to one. Finally,
∗
no longer than DG . This way, we form a new path with Tglobal considering that Theorem 3 does not take into account the
and a number of 2m short paths. The new path connects to segmented path smoothing, we analyze it below. Recall that p
B, C and visits the centroid of each subspace in Ĝ. If (8) is the penalty for stopping the vehicle at a break-point and n0 is
† 0
does not hold, the new path becomes shorter than Tglobal and the number of viewpoints on Tsmooth . We reuse llocal and refer
a contradiction occurs. Let lglobal be the length of the global to the length of the local path where all n0 − 2 viewpoints are
path Tglobal found by our method. Note that Theorem 3 states set as break-points. This path has the shortest length since no
the approximation ratio introduced by the hierarchy only. Here, boundary conditions are applied at the viewpoints as compared
we do not consider the effect of the approximation algorithm to that any viewpoints are set as inner-points. We have
used to solve the TSP. In other words, we assume an exact Theorem 4: The approximation ratio of cost c0smooth by the
solution is obtained from solving the TSP and Tglobal is the path smoothing has σsmooth ≤ (llocal + p(n0 − 2))/llocal .
shortest possible path connecting to E, F and going through
the centroid of each subspace in Ĝ. We can show that Proof: Since llocal is the length of the shortest local path,
the cost of the theoretically optimal local path has
†
lglobal ≤ lglobal + 2DH . (9)
c∗smooth ≥ llocal . (13)
Again, we use proof by contradiction. We find two short paths
connecting between B, E and C, F , respectively. Each short In Algorithm 2, the algorithm starts with initializing all n0 −
path can be no longer than DH . This way, we form a new path 2 viewpoints as break-points and reduces the cost by setting
† each viewpoint as an inner-point and re-smoothing the segment
with Tglobal and the two short paths. The new path connects to
E, F and visits the centroid of each subspace in Ĝ. If (9) does through the viewpoint. The resulting path has a cost
not hold, the new path becomes shorter than Tglobal . Therefore, c0smooth ≤ llocal + p(n0 − 2). (14)
a contradiction occurs. Further, let llocal be the length of the
local path Tlocal found by our method. We do not consider Therefore, the approximation ratio is bounded by
the effect of the approximation algorithm solving the TSP
σsmooth ≤ (llocal + p(ñ − 2))/llocal . (15)

Theorem 4 implies that our path smoothing is most suitable
Short path when p is relatively small as compared to llocal .
F Tglobal
B Tlocal
*
†
Tglobal V. E XPERIMENTS
Tlocal C
Gh E Tglobal
* We conduct experiments in both simulation and real-world
H experiments. Fig. 6 shows our simulated aerial vehicle model
and ground vehicle platform. The maximum speed is set to
Fig. 5. Illustration of mathematical definitions in the proof of Theorem 3. 5m/s for the aerial vehicle and 2m/s for the ground vehicle.
Both vehicles are equipped with a Velodyne Puck lidar, used
for exploration and mapping. In addition, the ground vehicle
has a camera at 640 × 360 resolution and a MEMS-based
IMU, coupled with the lidar for state estimation [32]. The
onboard autonomy system incorporates navigation modules
from our benchmark environment, e.g. collision avoidance,
terrain traversability analysis, way-point following, as the mid-
layer, while the exploration algorithm is at the top layer.
The exploration algorithm runs on a 4.1GHz i7 computer
using a single CPU thread. Our method re-plans at 1Hz.
The configuration space in our method is evenly divided into (a)
blocks, where each block represents a subspace. For aerial
vehicle experiments, the block size is set to 16m×16m×10m,
and for ground vehicle experiments, the block size is set to
8m×8m×5m. The local planning horizon consists of 5×5×3
blocks with the vehicle in the center block. Point clouds are
used to model surfaces, which is kept at 1.2m and 0.2m
resolutions for the aerial and ground vehicle experiments. We
(b)
compare our method against three state-of-the-art methods,
all using open-source code adapted to the specific evaluation
environments.
• NBVP [21]: A method spans an RRT in the free space
and finds the most informative branch in the RRT as the
path to the next viewpoint.
• GBP [23]: An extension of NBVP, where the method con-
structs an RRG in the global space and searches the RRG (c)
for routes to relocate the vehicle. The method explicitly Fig. 7. Results of Test 1 with the aerial vehicle in simulation. (a) shows
switches between exploration mode and relocation mode. the resulting map and trajectory of our method from a representative run.
• MBP [24]: A variant of GBP, which constructs the local Trajectories from NBVP, GBP, and MBP are shown on the left. The blue dots
indicate the start point. (b-c) are exploration metrics for all methods.
RRT using motion primitives. The resulting paths are
smoother but only span in constrained directions.
Test 1 uses the aerial vehicle to explore a university campus second over the entire runs, and the relative efficiency r is
in simulation. Fig. 7(a) shows the resulting point cloud map defined as the ratio compared to our method. Our method
and trajectory of our method in a representative run, with produces an efficiency more than 7 times of the other methods.
the trajectories of the other methods shown on the left. Fig. 7(c) shows the algorithm runtime. The average runtime
Each method is run 10 times starting at the same position for NBVP is 4.6s, for GBP is 7.4s, for MBP is 6.3s, and for
indicated by blue dots. Our method is able to cover the entire ours is 0.21s, where ours is a magnitude lower.
environment using 1318m of travel on average and 366s in the Test 2 uses the ground vehicle to explore a four-storage
longest run. The time limit for the other methods is set to four garage and a connected patio in real-world experiments. The
times of our longest run. Within the time limit, none of the start point is set at the entrance of the garage. During experi-
three methods is able to completely explore the environment. ments, a run is terminated if the exploration algorithm reports
Fig. 7(b) gives results on the explored volume over time. The completion, the vehicle almost stops moving (less than 10m
dotted lines represent the upper-bound and lower-bound, and of movement within 300s), or the time limit is met (set as
the solid lines represent the mean over the 10 runs. Table I twice of our method). Only our method is able to explore
compares the exploration efficiency for all methods. Here, the the whole environment and report completion after 1839m
efficiency  is defined as the average explored volume per of travel in 1907s. All other methods terminate inside the
garage and miss the patio connected to the garage from the
top floor. In particular, NBVP reaches the time limit, and GBP
and MBP are terminated due to they almost stop moving.
Fig. 8(a) shows the resulting point cloud map and trajectory
from our method, with trajectories from the other methods
on the bottom. Fig. 8(b) compares the explored volume. Note
that compared to Test 1, the topology in Test 2 is simpler,
with fewer intersections and branches. Especially, all methods
(a) (b) exhibit similar performance during the first half of the run,
Fig. 6. (a) Simulated aerial and (b) Ground experiment platforms. where the vehicle follows the main spiral driveway in the
 TABLE I
C OMPARISON OF EXPLORATION EFFICIENCY

NBVP GBP MBP Ours

Test  r  r  r  r
Test 1 15.7 0.079 23.9 0.12 26.9 0.13 199.7 1.0
Test 2 3.0 0.24 6.8 0.55 5.3 0.43 12.3 1.0

TABLE II
RUNTIME BREAKDOWN
Local Planning
Test Update Sample Find/Opt. Global Overall
Representation Viewpoints Path Planning
Test 1 129.7ms 2.1m 45.8ms 28.6ms 206.2ms
Test 2 382.9ms 7.3ms 5.4ms 24.8ms 420.4ms

TABLE III
RUNTIME WITH DIFFERENT H FOR T EST 1
H (m) 80×80×30 160×160×60 320×320×120
Average Local Global Local Global Local Global
Runtime (ms) 177.6 28.6 2197.6 27.9 5340.7 23.25

equivalent to our method reconfigured to not use the hierar-

chy. The result consolidates the strength of our hierarchical
(a) framework in producing high-efficiency processing.
Finally, our method is used by the CMU-OSU team in
attending the DARPA Subterranean Challenge. Fig. 9 shows
a representative result from a competition that takes place in
Satsop Nuclear Plant, WA. Our vehicle fully autonomously
explores the entire floor traveling over 886m in 1458s. Due to
space issue, we eliminate the details of this result.
(b)
VI. C ONCLUSION
We propose a method for highly efficient exploration of
large and complex environments. Our method uses a hierarchi-
cal framework to plan detailed paths within the local planning
horizon and coarse paths at the global scale. The method opti-
mizes the full exploration path rather than greedily maximizes
the marginal rewards. We provide theoretical analysis on the
(c) approximation ratio introduced by the hierarchy. Our method
Fig. 8. Results of Test 2 with the ground vehicle in physical experiments. is evaluated against state-of-the-art methods in simulation
The figure shares the same layout as Fig. 7. and physical environments. Experiment results show that our
method is 80% more efficient in covering spaces and consumes
garage from the top floor to the bottom floor. As indicated in less than 50% of computation compared to the state-of-the-art.
Table I, our method is still 80% more efficient than the other
methods for the overall run. Fig. 8(c) presents the algorithm
runtime. The average runtime for NBVP is 0.88s, for GBP is
2.64s, for MBP is 10.13s, and for our method is 0.42s. Our
runtime is 50% less than the other methods.
Table II presents the runtime breakdown for our method.
One can see that majority of the processing is spent on local
planning. Despite different scales and complexity levels of
the environments in the two tests, the overall runtime of our
method has the same scale. Further, we conduct tests with
different H to inspect the corresponding algorithm runtime.
Table III shows the result. As expected, the computation in
local planning increases dramatically as H increases from the Fig. 9. Result from DARPA Subterranean Challenge in Satsop Nuclear Plant,
default value on the left. The largest H on the right covers WA. The photo shows the exterior of the building where the event takes place.
the entire environment, i.e. the runtime for local planning is Our vehicle travels over 886m in 1458s to explore the entire floor.
 R EFERENCES [22] C. Witting, M. Fehr, R. Bähnemann, H. Oleynikova, and R. Siegwart,
“History-aware autonomous exploration in confined environments using
[1] F. Bourgault, A. A. Makarenko, S. B. Williams, B. Grocholsky, and mavs,” in IEEE/RSJ International Conference on Intelligent Robots and
H. F. Durrant-Whyte, “Information based adaptive robotic exploration,” Systems (IROS), Madrid, Spain, Oct. 2018.
in IEEE/RSJ International Conference on Intelligent Robots and Systems [23] T. Dang, M. Tranzatto, S. Khattak, F. Mascarich, K. Alexis, and
(IROS), Lausanne, Switzerland, Oct. 2002. M. Hutter, “Graph-based subterranean exploration path planning using
[2] C. Stachniss, G. Grisetti, and W. Burgard, “Information gain-based aerial and legged robots,” Journal of Field Robotics, vol. 37, no. 8, pp.
exploration using rao-blackwellized particle filters.” in Robotics: Science 1363–1388, 2020.
and Systems, Cambridge, MA, June 2005. [24] M. Dharmadhikari, T. Dang, L. Solanka, J. Loje, H. Nguyen,
[3] W. Tabib, M. Corah, N. Michael, and R. Whittaker, “Computation- N. Khedekar, and K. Alexis, “Motion primitives-based path planning
ally efficient information-theoretic exploration of pits and caves,” in for fast and agile exploration using aerial robots,” in IEEE International
IEEE/RSJ International Conference on Intelligent Robots and Systems Conference on Robotics and Automation (ICRA), Paris, France, May
(IROS), Daejeon, Korea, Oct. 2016. 2020.
[4] S. Bai, J. Wang, F. Chen, and B. Englot, “Information-theoretic explo- [25] M. Roberts, D. Dey, A. Truong, S. Sinha, S. Shah, A. Kapoor, P. Han-
ration with bayesian optimization,” in IEEE/RSJ International Confer- rahan, and N. Joshi, “Submodular trajectory optimization for aerial 3D
ence on Intelligent Robots and Systems (IROS), Daejeon, Korea, Oct. scanning,” in Proceedings of the IEEE International Conference on
2016. Computer Vision, Venice, Italy, Oct. 2017.
[5] W. Burgard, M. Moors, C. Stachniss, and F. E. Schneider, “Coordinated [26] C. H. Papadimitriou, “The complexity of the lin–kernighan heuristic for
multi-robot exploration,” IEEE Transactions on robotics, vol. 21, no. 3, the traveling salesman problem,” SIAM Journal on Computing, vol. 21,
pp. 376–386, 2005. no. 3, pp. 450–465, 1992.
[6] C. Nieto-Granda, J. G. Rogers III, and H. I. Christensen, “Coordination [27] D. Li and X. Sun, Nonlinear integer programming. Springer Science
strategies for multi-robot exploration and mapping,” The International & Business Media, 2006.
Journal of Robotics Research, vol. 33, no. 4, pp. 519–533, 2014. [28] D. Bergman, A. A. Cire, W.-J. van Hoeve, and T. Yunes, “BDD-based
[7] M. Corah and N. Michael, “Efficient online multi-robot exploration heuristics for binary optimization,” Journal of Heuristics, vol. 20, no. 2,
via distributed sequential greedy assignment.” in Robotics: Science and pp. 211–234, 2014.
Systems, Cambridge, MA, July 2017. [29] S. Russell and P. Norvig, “Artificial intelligence: a modern approach,”
[8] B. Yamauchi, “A frontier-based approach for autonomous exploration,” 2002.
in IEEE International Symposium on Computational Intelligence in [30] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and effi-
Robotics and Automation, 1997, pp. 146–151. cient quadrotor trajectory generation for fast autonomous flight,” IEEE
[9] D. Holz, N. Basilico, F. Amigoni, and S. Behnke, “Evaluating the Robotics and Automation Letters, vol. 4, no. 4, pp. 3529–3536, 2019.
efficiency of frontier-based exploration strategies,” in 41st International [31] S. G. Johnson, “The nlopt nonlinear-optimization package,” http://github.
Symposium on Robotics (ISR) and 6th German Conference on Robotics com/stevengj/nlopt, 2014.
(ROBOTIK), Munich, Germany, 2010. [32] J. Zhang and S. Singh, “Laser-visual-inertial odometry and mapping
[10] M. Kulich, J. Faigl, and L. Přeučil, “On distance utility in the exploration with high robustness and low drift,” Journal of Field Robotics, vol. 35,
task,” in IEEE International Conference on Robotics and Automation no. 8, pp. 1242–1264, 2018.
(ICRA). Shanghai, China: IEEE, May 2011.
[11] C. Dornhege and A. Kleiner, “A frontier-void-based approach for au-
tonomous exploration in 3D,” Advanced Robotics, vol. 27, no. 6, pp.
459–468, 2013.
[12] L. Heng, A. Gotovos, A. Krause, and M. Pollefeys, “Efficient visual
exploration and coverage with a micro aerial vehicle in unknown
environments,” in IEEE International Conference on Robotics and
Automation (ICRA), Seattle, WA, May 2015.
[13] T. Cieslewski, E. Kaufmann, and D. Scaramuzza, “Rapid exploration
with multi-rotors: A frontier selection method for high speed flight,” in
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), Vancouver, Canda, Sept. 2017.
[14] M. Kulich, J. Kubalı́k, and L. Přeučil, “An integrated approach to goal
selection in mobile robot exploration,” Sensors, vol. 19, no. 6, p. 1400,
2019.
[15] J. Faigl and M. Kulich, “On determination of goal candidates in frontier-
based multi-robot exploration,” in European Conference on Mobile
Robots, Barcelona, Spain, Sept. 2013.
[16] H. Choset, S. Walker, K. Eiamsa-Ard, and J. Burdick, “Sensor-based
exploration: Incremental construction of the hierarchical generalized
voronoi graph,” The International Journal of Robotics Research, vol. 19,
no. 2, pp. 126–148, 2000.
[17] E. U. Acar and H. Choset, “Sensor-based coverage of unknown en-
vironments: Incremental construction of morse decompositions,” The
International Journal of Robotics Research, vol. 21, no. 4, pp. 345–
366, 2002.
[18] S. Kim, S. Bhattacharya, R. Ghrist, and V. Kumar, “Topological
exploration of unknown and partially known environments,” in 2013
IEEE/RSJ International Conference on Intelligent Robots and Systems.
Tokyo, Japan: IEEE, November 2013, pp. 3851–3858.
[19] S. M. LaValle, “Rapidly-exploring random trees: A new tool for path
planning,” 1998.
[20] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal
motion planning,” The International Journal of Robotics Research,
vol. 30, no. 7, pp. 846–894, 2011.
[21] A. Bircher, M. Kamel, K. Alexis, H. Oleynikova, and R. Siegwart,
“Receding horizon” next-best-view” planner for 3D exploration,” in
IEEE international conference on robotics and automation (ICRA),
Stockholm, Sweden, May 2016.