Motion Planning for Reconfigurable Mobile
Robots Using Hierarchical Fast Marching Trees
William Reid1 , Robert Fitch1,2 , Ali Haydar Göktoǧan1 and Salah Sukkarieh1
1
Australian Centre for Field Robotics, The University of Sydney, Australia
{w.reid,rfitch,a.goktogan,salah}@acfr.usyd.edu.au
2
Centre for Autonomous Systems, University of Technology Sydney, Australia
Abstract. Reconfigurable mobile robots are versatile platforms that
may safely traverse cluttered environments by morphing their physical
geometry. However, planning paths for these robots is challenging due to
their many degrees of freedom. We propose a novel hierarchical variant
of the Fast Marching Tree (FMT*) algorithm. Our algorithm assumes a
decomposition of the full state space into multiple sub-spaces, and begins
by rapidly finding a set of paths through one such sub-space. This set
of solutions is used to generate a biased sampling distribution, which is
then explored to find a solution in the full state space. This technique
provides a novel way to incorporate prior knowledge of sub-spaces to efficiently bias search within the existing FMT* framework. Importantly,
probabilistic completeness and asymptotic optimality are preserved. Experimental results are provided for a reconfigurable wheel-on-leg platform that benchmark the algorithm against state-of-the-art samplingbased planners. In minimizing an energy objective that combines the
mechanical work required for platform locomotion with that required
for reconfiguration, the planner produces intuitive behaviors where the
robot dynamically adjusts its footprint, varies its height, and clambers
over obstacles using legged locomotion. These results illustrate the generality of the planner in exploiting the platform’s mechanical ability to
fluidly transition between various physical geometric configurations, and
wheeled/legged locomotion modes.
1
Introduction
Reconfigurable mobile robots (RMRs) are platforms that gain versatility through
the ability to dynamically alter certain elements of their geometric structure. An
example RMR called the MAMMOTH rover [15] is shown in Fig. 1 in a variety of different poses. This wheel-on-leg platform is novel in how it combines
wheeled and legged locomotion modes to traverse over, under or around challenging obstacles. It may drive omni-directionally while raising and lowering its
body, packing or unpacking each of its legs between large and small footprints,
as well as lifting or lowering individual legs to clamber over terrain. One benefit
of this type of vehicle is in traversing unstructured terrain that conventional
rovers with passive suspension systems may find inaccessible. Even though the
(a)
(b)
(c)
Fig. 1. The MAMMOTH Rover: an example of a reconfigurable mobile robot. The
vehicle is capable of reconfiguring its footprint, changing its height, clambering over
obstacles and driving omni-directionally.
rover may need to reconfigure to achieve a successful traversal, the dominant
mode of motion remains the translation of its body frame relative to an inertial
frame. In this work we propose a hierarchical motion planning technique that
exploits this property by rapidly searching the dominant sub-space to find paths
that are then used to bias search within the full state space.
The main challenge in planning for RMRs is to efficiently find a feasible low cost
path given potentially many degrees of freedom. The problem is to generate a sequence of transitions between initial and terminal system states (configurations)
while both avoiding environmental obstacles and satisfying internal kinematic
constraints. The sequence of transitions is to be optimized with respect to a cost
function that is used to define the objectives of the robot’s overall motion. In
this work, the energy expenditure of the robot is minimized.
For RMR planning, techniques such as discrete grid-based dynamic programming [6] are not suitable due to the infeasibility of explicitly constructing and
searching occupancy grids over high-dimensional state spaces that include strict
kinematic constraints. Sampling-based planners are also not immune from such
plights, particularly because kinematic constraints may induce high-dimensional
narrow passageways. Hierarchical planning addresses these difficulties by exploiting prior knowledge of task structure. Although automatically discovering effective hierarchical decompositions remains an open problem, using prior knowledge
to inform the choice of hierarchy is known (in reinforcement learning) to be beneficial in taming high-DOF problems [11]. Typically, subroutine-like sequences
of actions known as macro-actions, or subtasks, are predefined for a given level of
the hierarchy and called by higher levels during planning. Hierarchical samplingbased planning is yet to be meaningfully explored.
We propose a novel hierarchical sampling-based planner for RMRs, the Hierarchical Bidirectional Fast Marching Tree (HBFMT*). Our approach enables the
use of prior knowledge of kinematic structure to inform the choice of hierarchical
decomposition, but defined in a way that is more flexible than in other hierarchi-
cal approaches. The state-space decomposition must still be provided a priori,
but the subtasks are continuous and generated probabilistically by the planner.
The extent of the state space explored by subtasks is defined by a continuous
parameter that may be automatically tuned.
A main technical challenge in developing a hierarchical sampling-based planner
is to define the information flow between levels in a way that does not sacrifice completeness and optimality guarantees. In our planner, we decompose the
state space as a hierarchy of increasingly larger sub-spaces, each searched by
a Fast Marching Tree (FMT*) variant. Solutions from adjacent levels are used
to bias search, thereby increasing path quality with equivalent computational
resources. This technique is suited to platforms that have intuitively identifiable
low-dimensional kinematic structure, such as the class of RMRs, but may also
generalise to other high-DOF robots such as redundant manipulator arms.
We show that this approach retains the completeness and asymptotic optimality guarantees of FMT* [8], and evaluate its performance in path planning for
RMRs. Experimental results in simulation show that HBFMT* outperforms
FMT*, BMFT*, RRT* and RRT-connect in a variety of environments.
2
Related Work
Over the past two decades, sampling-based algorithms have become the go-to
methodology for solving high-dimension planning problems. Ease of implementation, guarantees on probabilistic completeness, and more recently, asymptotic
optimality are attractive properties of these techniques. Multi-query samplingbased techniques include the probabilistic roadmap (PRM, PRM*), and variants [5]. Single-query techniques include the rapidly-exploring random tree (RRT)
and variants RRT* [10] and RRT# [1]. Additional asymptotically optimal singlequery planners that, unlike RRT planners, rely on sampling batches of states
prior to building a tree include (FMT*) [8] and Batch Informed Tree (BIT*) [7].
FMT* is advantageous in that it uses lazy collision detection to reduce the
number of expensive collision checking operations, and converges to optimal solutions more quickly than RRT* and PRM* (shown experimentally in [8, 19]).
The most significant advantage of FMT* and its variants, however, is its asymptotic optimality guarantees [8], which are amenable to non-uniform sampling
distributions. This property was recently exploited to handle narrow passageways [23]. Our work also exploits non-uniform sampling, and we show that the
conditions for asymptotic optimality are preserved.
Bidirectional search is a common method for improving performance of samplingbased planners and has been demonstrated with BFMT* [19], RRT-Connect [12]
and bidirectional RRT* [9]. In our work, bidirectional search is employed for its
performance boost as well and for finding alternative paths, which is a recent
popular topic in motion planning [14] and [22]. To the best of our knowledge,
ours is the first approach that leverages bidirectional sampling-based planning
to directly find multiple feasible solutions to the planning problem.
When planning for RMRs, or a reconfigurable team of robots, it is common that
a sub-space is initially searched to constrain the dimensionality of the problem.
In [18], a plan for a high-DOF four-legged robot is found by first finding a
footfall sequence and then finding paths between footfalls for the single swing
leg using a modified RRT. Hierarchical methods that first find a path over a
sub-space using discrete grid-based searches and then use this sub-space path
to guide a search of the full space with a sampling-based planner have been
demonstrated in various forms [17, 4, 3]. A similar state decomposition is also
used in [2] for the multi-robot planning problem, where obstacle free polytope
sub-regions are found within the configuration space and then route planning is
performed for each robot within each region. For these hierarchical planners, a
sub-space is selected by intuition. Automatic sub-space decomposition remains
open in general [11]; a recent robotics example does this by finding principal
cost variation components relative to an environment and robot model [21]. Our
algorithm assumes a given state variable decomposition, but is the first principled
hierarchical sampling-based planner that retains performance guarantees.
3
Problem Formulation and Background
In this section we formulate the path planning problem for RMRs. We have
attempted to choose notation that agrees with previous work relevant to FMT*.
3.1
Problem Definition
Let X = [0, 1]d be a state space with dimension d ∈ N constrained to d ≥ 2. Let
Xobs be the obstacle region such that X \ Xobs is an open set. This implies that
the free state space is a closed set Xf ree = cl(X \ Xobs ). We randomly sample
n points from Xf ree . Let X # = [0, 1]f be a sub-space that is embedded within
X where 2 ≤ f ≤ d. Any variable with the “#” super-script is used to denote a
variable related to the sub-space X # .
A path planning problem is denoted by a triplet (Xf ree , xinit , Xgoal ) where xinit
is the initial state and Xgoal is the goal region. In this work, the goal region is
considered as a single state, described by xgoal . A path is defined by a continuous
mapping π : [0, 1] → Rd such that 0 7→ x1 and 1 7→ x2 . Let Σ be the set of all
paths in X . A feasible path in the planning problem (Xf ree , xinit , Xgoal ) is a path
that is collision free with π(0) = xinit and π(1) = xgoal . Π = {π0 , π1 , ..., πk } is a
set of k feasible paths sorted according to path cost.
The optimal path planning problem is to find a path π ∗ , assuming problem
(Xf ree , xinit , Xgoal ) and an arc cost function c : Σ → R≥0 such that c(π ∗ ) =
min {c(π) : π is feasible}, or to report failure. The optimal path π ∗ is δ-robustly
feasible if every point along it is a minimum of δ away from Xobs .
3.2
The Fast Marching Tree
HBFMT* builds on FMT*, which is presented in detail in [8] and summarized
here for convenience. Tree T = (V, E, H, W ) is composed of states V , and edges
E. The open set of nodes is denoted as H, and W is the unvisited set. Tree T is
initialized with n nodes sampled from free space, Xf ree , in addition to the start
node, xstart and goal node, xgoal . The algorithm maintains an open set, H, and
an unvisited set, W . Initially, xstart is placed in H, while every other node as
well as the goal node is placed in W . The node set, V is initialized containing
xstart , while the edge set, E is initialized as empty. The algorithm then performs
a recursive dynamic programming procedure that propagates a wavefront of
state transitions outwards from the start node. This iterative procedure is called
Expand, and pseudocode can be found in [19].
Nodes that are within a radius rn of the minimum cost node in the open set are
considered as candidate transition nodes. For each candidate node, the locally
optimal transition between it and the open set are found. This transition is
tested for collision, and if it is collision free, then the transition is added to the
edge set of the tree and the candidate node is placed in a temporary open set.
After each candidate node has been considered all of the nodes in the temporary
open set are placed in the open set. Tree expansion terminates when the goal
node has been found or the open set is empty.
3.3
Bidirectional Fast Marching Tree
HBFMT* also builds on BFMT* [19], which is summarized here. BFMT* grows
two FMT* trees, T and T 0 , rooted at the start and goal states respectively.
BFMT* starts by sampling a set of states, S, from Xf ree . The two trees are then
initialized. The first tree, T is initialized with S along with the start state, xstart
at its root node and the goal state xgoal . The second tree T 0 is also initialized
with S, xstart and xgoal , with xgoal at its root. Each tree is alternately expanded
according to the FMT* expansion procedure until the two trees intersect.
The termination condition of BFMT* may occur in one of two separate conditions [19]. The first is when the two trees initially intersect, occurring when a
node is in the open set of both trees. This is the “first path” termination condition, however it does not guarantee an optimal solution. The second termination
condition occurs when a node is in the open set of one tree, while not inside the
open or unvisited set of the other. This second “best path” condition results in
a case where the node can no longer improve its cost-to-come value from either
tree root and is therefore the optimal solution given the sample nodes.
(a)
(b)
(c)
(d)
Fig. 2. (a) Initially, an exhaustive BFMT* search of a uniformly sampled sub-space
returns a set of feasible paths. (b) States in the full space are sampled from a biased
distribution focused around the sub-space paths. (c) A second BFMT* searches through
the full state samples. (d) A full state-space path is returned.
4
Hierarchical Bidirectional Fast Marching Tree
In this section we present the HBFMT* algorithm. HBFMT* consists of several
components, which are illustrated in Fig. 2.
Intuitively, HBFMT* biases search of the full state space based on knowledge
gathered from an initial rapid search of a sub-space. To introduce HBFMT* we
describe an example with an 8-DOF robot in a confined environment shown in
Fig. 2. The robot starts in an open-stance state at the bottom of the map and
its goal is to achieve a similar stance in the center of the map. Any feasible path
requires reconfiguration of the robot to fit through narrow corridors. Initially, as
in Fig. 2a, a sub-space is searched using BFMT*. The sub-space searched is R3 ,
the translation of the robot’s body with respect to the inertial frame. Collision
checking is performed between the environment and the translating body of the
robot without its legs. A set composed of feasible paths is returned. The full
8-DOF state space of the robot is then sampled with a bias around the set
of returned 3-DOF paths as shown in Fig. 2b. Another BFMT* instance then
searches this biased distribution as in Fig. 2c. The bias is implemented using a
tunable tunnel radius that defines how focussed the search of the solutions from
the previous level of the hierarchy is. A final path through the shorter of the two
narrow passageways is returned and shown in Fig. 2d.
Before we define HBFMT*, we first describe a modification to the termination condition of BFMT*. We introduce an “exhaustive” termination condition;
pseudo-code is provided in Alg. 1. When a “best path” termination condition
is found at a tree intersection node c, BFMT* returns the path πi , where i is
the path index, and adds it to the set of alternative paths Π. The planner then
“singes” the child nodes within a neighbourhood defined by a ball of radius rsinge
centered at c. When a node is singed, it is placed into the closed set to prevent
additional tree expansion through this node. The singeing procedure is described
Algorithm 1: Bidirectional Fast Marching Tree (BFMT*)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
function BFMT∗ (Xf ree ,xinit ,xgoal ,Π,`,rsinge ,σ 2 ,tc)
S ← xinit ∪ xgoal ∪ SampleFree (n, Π, `, σ 2 )
T ← Initialize(S, xinit ), T 0 ← Initialize(S, xgoal )
Π ← Ø, z ← xinit , c ← Ø
success = f alse
while success = f alse do
Expand(T, z, c)
if tc = BEST and z ∈ (V 0 \ H 0 ) then /* best termination cond. */
π ← Path(c,T ) ∪ Path(c,T’ ), Π ← Π ∪ π
success = true
else if tc = EXHAUSTIVE and z ∈ (V 0 \ H 0 ) then /* exhaustive */
π ← Path(c,T ) ∪ Path(c,T’ ), Π ← Π ∪ π
SingeBranch(c, T, rsinge ), SingeBranch(c, T 0 , rsinge )
if H = Ø and H 0 = Ø then
success = true
z ← argminx0 ∈H 0 {Cost(x’,T’ )}
Swap(T,T’ )
return Π
Algorithm 2: Singe Branch
1
2
3
4
5
6
function SingeBranch(z,T ,rsinge )
/* Perform a breadth first search of the branch of the sub-tree
with its root at node z.
*/
A ← BreadthFirstSearch(z,T ,T 0 )
A ← A∪ Near(T ,A,rsinge )
H ← H \ A, V ← V \ A
H 0 ← H 0 \ A, V 0 ← V 0 \ A
Algorithm 3: Hierarchical Bidirectional Fast Marching Tree (HBFMT*)
1
2
3
Data: Xf ree ,xinit ,xgoal ,rsinge ,σ 2
Result: Π
begin
#
2
Π # ← BFMT∗ (Xf#ree ,x#
init ,xgoal ,Ø,1,rsinge ,σ ,EXHAUSTIVE )
Π ← BFMT∗ (Xf ree ,xinit ,xgoal ,Π # ,`,Ø,σ 2 ,BEST )
in Alg. 2. BFMT* continues this search and singe procedure until both trees’
open sets are exhausted.
We now formalize HBFMT*. Pseudocode is listed in Alg. 3. The first step is
to plan a path through the sub-space Xf#ree using modified BFMT* sampling
uniformly from Xf#ree . Depending on the space Xf#ree and the performance of the
BFMT* search, there may be multiple unique paths Π # returned. An example
where two alternative paths are found is shown in Fig. 2 (a).
The set of paths Π # is then passed to a second BFMT* instance along with a
sampling variable `. This second BFMT* search starts by sampling n states from
Xf ree to create the tree nodes V . A collection of `n samples are taken from a
uniform distribution over Xf ree to comply with asymptotic optimality conditions
discussed in Sec. 5. The sub-space components of the remaining n(1−`) states are
2
sampled from a Gaussian distribution N (x#
j , σ ) around each of the unique nodes
#
in Π while the remaining components of these states are uniformly sampled.
The parameter σ 2 , the tunnel radius, determines how wide the biased search
region around the paths contained in Π # is. Currently, this variable is tuned
manually, however in future work it is desired that this variable be tuned as a
function of the results of previous planner executions from earlier stages of the
hierarchy. An example of a resulting set of nodes is shown in Fig. 2b. Once the
biased set of samples S has been found, the second BFMT* instance explores
the set and returns the best path through the resulting tree as shown in Fig. 2d.
5
Analysis
In this section we show the asymptotic optimality (AO) of HBFMT* and discuss
the effects of its tuning parameters. We prove AO (and thus, probabilistic completeness) of HBFMT* by observing that the terminal stage of the algorithm is
BFMT* with a non-uniform sampling distribution and a metric cost function.
The following theorem characterizes AO in terms of the number of sample nodes.
Theorem 1. Let π : [0, 1] be a feasible path with strong δ-clearance, δ > 0.
ζ is the Lebesgue measure of the unit-cost ball and µ(Xf ree ) is the Lebesgue
measure of the free space. Consider running HBFMT* by sampling n nodes from
a distribution ϕ and executing to completion using any termination criteria and
a radius
d1 1 1 1
1
µ(Xf ree ) d log(n`) d 1 d
rn = 2(1 + η)
,
(1)
d
ζ
n
`
for a parameter η ≥ 0 and n` > 1. Let cn denote the cost of the path returned by
HBFMT* and c∗ be the optimal path cost, then limn→∞ P(cn > (1 + ε)c∗ ) = 0
for all ε > 0 (which defines AO).
Proof. The terminal stage of HBFMT* implements BFMT* operating in Xf ree ,
using a metric cost function and samples from probability distribution ϕ that
is lower bounded by `. To prove AO of HBFMT* it therefore suffices to show
that BFMT* is AO under these two conditions. Firstly, any metric cost function
satisfies the AO conditions for FMT* (Sec. 5 of [8]). Secondly, we can represent ϕ as a mixture distribution, composed of a uniform distribution that is
sampled with probability ` and a non-uniform distribution that is sampled with
probability 1 − ` [8]. This results in a uniform distribution being sampled n`
times, and thus the proof of AO of FMT* is preserved. Likewise, for BFMT*,
limn→∞ P(cn > (1 + ε)c∗ ) = 0 for all ε > 0 [19].
The performance of HBFMT* compared with BFMT* and FMT* is largely
dependent on the sub-space Xf#ree that is initially sampled. It is generally the
case for reconfigurable mobile robots that Xf#ree is the workspace of the vehicle,
or the translational R3 Euclidean space. However, if the platform requires a
specific orientation or joint articulation motion that is not contained within
the biased search region in Xf ree , the algorithm will not perform as well as
FMT* and BFMT* and will be dependent on samples taken from the uniform
component of ϕ. Currently, the sub-space Xf#ree is chosen a priori, however a
topic of future work is incorporation of autonomous selection of these spaces
given a certain environment and robot model. Examples of such low-dimensional
structure identification can be found in [21].
An additional parameter that affects the performance of HBFMT* is the biased sampling distribution variance σ 2 . A small value for σ 2 results in focused
sampling around the nodes in Π # , thereby increasing the planner’s reliance on
the sub-state solutions Π # , and making the planner more susceptible to missing
necessary motions. A large value for σ 2 results in a distribution resembling uniformity, thereby removing any advantage over BFMT* or FMT*. Both of these
parameters require tuning given robot and environment models. An experiment
that varies ` and σ 2 is provided in Sec. 6 to empirically demonstrate the effect
that these parameters have on algorithm performance.
6
Experiments
Our experimental evaluation has two aims. First, we benchmark HBFMT*’s
performance against state-of-the-art sampling-based planners, including FMT*,
BFMT*, RRT* and RRT-connect. Second, we showcase the algorithm’s suitability to path planning for RMRs using the system described in Sec. 6.1 and
the three distinct environments described in Sec. 6.2. The experimental setup is
detailed in Sec. 6.3 and results are discussed in Sec. 6.4.
6.1
The MAMMOTH Rover
The RMR used in all experiments is a simulated version of the MAMMOTH
rover shown in Fig. 1. The simulated model has an 8-DOF state space. It may
W
translate its body frame B relative to its inertial frame W along the xW
B , yB
W
and zB directions. Its roll, pitch and yaw positions are denoted by φ, θ and
ψ respectively. In all experiments we hold φ and θ constant at 0◦ , while ψ is
free to rotate. Additionally, the four hip joints, qH1 ,...,qH4 may rotate, thereby
changing the robot’s contact footprint. A randomly sampled state x contains
values for each of the eight degrees of freedom. The state x also contains four
thigh joint variables, qU 1 ,..., qU 4 that are calculated as a function of the eight
degrees of freedom as well as the local terrain profile. Additional dependent
joints that are driven to meet a desired configuration are the wheel steering
joints, qS1 , ..., qS4 and wheel drive joints, qA1 , ..., qA4 . These joint positions and
rates are determined as a function of the desired velocity of the platform.
The physical MAMMOTH rover has a mass of 75 kg and 16 points of actuation.
Its maximum footprint is 1500 mm by 1500 mm, while its most compact footprint
is 650 mm by 650 mm. It may drive and steer each of its wheels independently
and continuously allowing for omnidirectional driving. Each hip actuator can be
moved between ±270◦ to change the footprint of the vehicle. Lastly, each leg is
a parallel structure. The legs can raise and lower between −20◦ and 70◦ relative
to the transverse plane of the rover’s body.
The 8-DOF state space does not characterize the capability of the MAMMOTH
rover to raise only one leg off the ground at a time. This ability is critical when
the rover needs to clamber over obstacles. To allow this clambering behaviour to
emerge, a sampling feedback method is used. If it is found that a sample from
the 8-DOF space is deemed invalid due to leg i colliding with an obstacle, the
sample is checked again with leg i raised. This is done by changing the leg’s qU i
value so that the leg is in its fully raised configuration. If this new leg-raised state
is not in collision with obstacles and obeys static stability constraints, then it is
inserted into Xf ree . Alg. 4 provides pseudocode for the sampling implementation
used for the MAMMOTH rover planning problem.
As part of the collision detection procedure, the rover’s static stability is checked
according to the model described in [13] and [16]. This characterizes static stability by finding the minimum angle between the force vector extending from
the rover’s center of mass and the vector connecting each wheel/ground contact
point. The minimum allowable stability margin is 3◦ .
The function that is used to evaluate state transition costs approximates the
amount of mechanical work in a state transition:
W
J(xi , xi+1 ) = µM gkδpxy k + (mbody + 2mleg )gδzB
+ µmgrleg δψ
+ µmleg grleg (δqH1 + δqH2 + δqH3 + δqH4 ) + (rleg /2)mleg gδqUj , (2)
where δ is the absolute difference over a single dimension and kδpxy k is the EuW
clidean vector norm along the xW
B and yB dimensions. Additionally, mbody =30 kg
is the mass of the rover’s central chassis, mleg =10 kg is the mass of a single leg,
M = (mbody + 4mleg ) is the total mass of the rover, rleg = 0.7 m is the average
reach of the center of mass of a single rover leg, and µ = 0.1 is an approximate
rolling resistance coefficient for the rover’s wheels. The constants used to generate J are an approximation; an item of future work is to incorporate a high
fidelity physical model of the rover into the planner to more accurately calculate
Algorithm 4: Sample Mammoth State
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
function SampleFree(n,Π,l,σ 2 )
m←n
while m > 0 do
if m < (1 − l)n then
x ← SampleBiased(Π,σ 2 )
else
x ← SampleUniform
qU ← MammothIK(x)
if IsValid(x) then
m ← m − 1, S ← S ∪ x
else
for i ← 1 to 4 do
if LegCollision(i) then
x.qUi ← qU .bounds.min
if IsValid(x) then
m ← m − 1, S ← S ∪ x
return S
energy expenditure.
Pb−1 The traversal cost of a path is the sum of its state transition
costs c(π) = a=0 J(xa , xa+1 ), where b is the number of waypoints in the path.
6.2
Environments
The three environments used are shown in Fig. 3. In the “Gauntlet” environment, the rover must navigate around/over box obstacles, through a tunnel and
through a narrow passageway to get to its goal. This environment contains a
single homotopy class of paths embedded within the Euclidean translational subspace of the rover. HBFMT* will be able to focus its search on this single family
of paths. The “Alternatives” environment is designed to contain two homotopy
classes of paths within the Euclidean translational sub-space. The robot may
choose to clamber over the four step obstacles, or drive around/over the many
block obstacles. Clambering over obstacles is energy intensive, so an optimal solution should lie in the region with the block obstacles. The “Steps” environment
contains a series of step obstacles that the rover must clamber over. This space
is designed to highlight the planner’s ability to find clambering maneuvers given
the collision feedback sampling strategy described above.
6.3
Experimental Setup
Paths are found for the three environments using a set of sampling-based planners: BFMT*, FMT*, RRT*, RRT-Connect, and HBFMT*. A batch of trials is
Fig. 3. The three environments used in our experiments. Each environment is contained
within a 15.5 m long, 7.5 m wide and 1.5 m high boundary. Successful traversals
generated by HBFMT* with 10,000 nodes are shown.
composed of 100 planner executions. For FMT* and BFMT*, distinct batches
are assigned between 500 and 2500 nodes with 500 node increments and between
2500 and 15000 nodes with 2500 node increments. HBFMT* additionally uses
1000 nodes to sample Xf#ree . For RRT*, batches are assigned an expiry time between 60 s and 360 s with 60 s increments. A goal-bias probability of 5% is used.
For RRT-connect (which terminates when it finds a solution) a single batch of
100 trials is used. All planners are limited to a maximum running time of 400 s.
Tuning parameters for HBFMT* are set as follows: sampling variance σ 2 =
0.2 kJ, focussed sampling density ` = 0.2 and singe radius rsinge = 0.8 kJ.
HBFMT* begins by sampling the translational R3 state space of the rover and
Gauntlet
Alternatives
Steps
5
4.5
5.5
4.5
4
3
4
Cost [kJ]
Cost [kJ]
Cost [kJ]
5
3.5
3.5
3
2.5
4.5
4
2.5
3.5
0
100
200
300
0
Mean Computation Time [s]
100
200
300
0
40
60
40
20
20
0
0
0
100
200
300
100
150
200
250
300
80
Success [%]
60
50
Mean Computation Time [s]
100
80
Success [%]
80
Success [%]
100
Mean Computation Time [s]
100
60
40
20
0
0
100
Mean Computation Time [s]
200
300
0
Mean Computation Time [s]
HBFMT*
BFMT*
FMT*
100
200
300
Mean Computation Time [s]
RRT*
Fig. 4. Experimental results for HBFMT*, BFMT*, FMT* and RRT* with the MAMMOTH rover over three environments. Each icon represents a single batch of 100 trials.
Horizontal and vertical error bars indicate one standard error for each batch.
uses the bounding box of the rover’s body (excluding the legs) for collision
checking. Based on observations of best practice [8], k-nearest neighbour versions
of FMT*, BFMT*, HBFMT* and RRT* are used. The value kn , which denotes
the number of nearest neighbour nodes, is calculated as kn,F M T ∗ = 2d e/d log(n)
for the FMT* variants, and kn,RRT ∗ = (e + e/d) log(n) for RRT*.
All planners are implemented in simulation within the Open Motion Planning
Library (OMPL) version 1.1.0 [20]. OMPL implementations for FMT*, RRT*
and RRT-Connect have been used; the BFMT* implementation is our own.
Experiments are run on a 3.4 GHz Intel i7 processor with 32 GB of RAM.
6.4
Experimental Results
The examples shown in Fig. 3 illustrate fluid transition between intuitively discrete locomotion modes such as driving, clambering and footprint reconfiguration. The experimental results shown in Fig. 4 characterize the performance,
including computation time, of all of the planning algorithms operating on the
three environments. Results for RRT-connect are omitted from the plots due to
large discrepancies between it and the other planners and are instead summarized in the text below. Videos showing feasible traversals returned by HBFMT*
for each environment are presented in the supplementary material.
Generally, HBFMT* is observed to improve path quality with equivalent computation time compared with its counterparts. Notably, HBFMT* returns a
greater number of lower-cost feasible solutions sooner than the other planners
for all cases. With 2500 nodes, in the Gauntlet environment, HBFMT* returns a
path that is 0.5 kJ lower on average than those returned by FMT* and BFMT*,
while in the Alternatives environment, the difference is a more significant 1 kJ.
In the Steps environment the margin is 0.4 kJ.
The Fast Marching Tree variants outperform RRT* in all environments. In
Gauntlet and Alternatives, RRT* begins to return feasible paths greater than
40% of the time at 240 s. This is a notable difference when compared to 11 s and
18 s for HBFMT*, and 24 s and 27 s for FMT* and BFMT*. For the Steps environment, RRT* failed to find a solution after an expiry time of 400 s. For Steps,
feasible paths begin to be returned more than 40% of the time for HBFMT* at
20 s, and 27 s for FMT* and BFMT*.
Like RRT*, RRT-connect failed to return a single successful result for the Steps
environment. For Gauntlet and Alternatives the mean costs returned are 16.85 kJ
and 14.13 kJ, the cost standard deviations are 2.06 kJ and 1.88 kJ, the mean
computation times are 147.1 s and 180.9 s, and the computation time standard
deviations are 78.27 s and 60.1 s respectively. For both of these environments
RRT-connect returns a feasible solution 100% of the time, outperforming all of
the the other planners. Given that the algorithm is not asymptotically optimal
and does not refine its solution once a feasible path has been found there is a
significant variation in its returned costs for all cases.
All of the FMT* variants including HBFMT* return stable or decreasing success
rates between 2500 and 15000 nodes for all environments. In some cases, as is
the case for BFMT* at 230 s (15000 nodes) in Alternatives, and in all FMT*
variants in Steps at 300 s (15000 nodes) we notice a drop in success rate by 5%
from the previous batch. This behaviour is most likely due to the choice of kn .
6.5
Parameter Tuning
An experiment investigating the robustness of the algorithm given varying parameter values of tunnel radius σ 2 and focused sampling density ` is performed.
In this experiment the HBFMT* planner is run for the MAMMOTH rover operating in the Alternatives environment with the same start and goal configurations
shown in Fig. 4. A total of 90 separate batches of trials are run, with 50 trials
per batch. Each batch has a different σ 2 and ` combination. ` is varied between
0.1 and 0.9, while σ 2 is varied between 0.1 and 1.0 kJ.
The results of this experiment are shown in Fig. 5 and demonstrate robustness to
parameter changes in the Alternatives environment. Each mesh vertex represents
the average of the 50 trials run for a specific ` and σ 2 combination. All of the
batches had a success rate greater than 68%. It is shown that the lowest average
costs are returned when ` is equal to 0.1 and the tunnel radius is equal to 0.5. It
is noted that in the worst cases where ` is large, the returned cost is equivalent
to the BFMT* planner’s cost shown in Fig. 4. Parameter tuning trials were run
for the Gauntlet and Steps environment with equivalent results.
4.4
4.5
4.2
4
4
3.5
3.8
3
3.6
3.4
2.5
3.2
2
1
3
0.8
2.8
0.6
2.6
0.4
0.2
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
2.4
Fig. 5. Average costs returned by HBFMT* in finding a path through the Alternatives
environment. Each mesh vertex is the average cost of 50 trials with the corresponding
` and σ 2 parameters given on the x and y-axes.
7
Conclusions and Future Work
In this paper, we proposed a hierarchical sampling-based motion planner that
biases search based on paths returned from a rapidly explored sub-space. The algorithm is based on the FMT* and BFMT* algorithms, and retains their asymptotic optimality guarantees. Numerical results showed that HBFMT* improves
convergence to an energy-optimal path compared with state-of-the-art samplingbased planners. This approach opens up a new methodology for building efficient
planners for RMRs and other high-DOF robots.
This work is motivated directly by a real-world RMR, and so the most important
area of future work from our perspective is to use HBFMT* to plan energy
efficient paths for the physical MAMMOTH rover in unstructured environments.
One avenue of future algorithmic work is to expand the number of levels in the
hierarchy to further reduce computational costs. Automatically tuning the tunnel
radius parameter is another promising idea. Finally, it would be interesting to
apply hierarchical planning to real-world systems with similar characteristics to
RMRs, such as high-DOF manipulators and teams of mobile robots.
Acknowledgements. This work was supported by the Australian Centre for Field
Robotics; the NSW Government; and the Faculty of Engineering & Information Technologies, The University of Sydney, under the Faculty Research Cluster Program. We
would like to thank Oliver Cliff and Jack Umenberger for their helpful assistance.
References
1. Arslan, O., Tsiotras, P.: Use of relaxation methods in sampling-based algorithms
for optimal motion planning. In: Proc. of IEEE ICRA. pp. 2421–2428 (2013)
2. Ayanian, N., Kumar, V.: Decentralized feedback controllers for multiagent teams
in environments with obstacles. IEEE Trans. Robot. 26(5), 878–887 (2010)
3. Belter, D., Labecki, P., Skrzypczynski, P.: Adaptive motion planning for autonomous rough terrain traversal with a walking robot. J. Field Robot. 33(3),
337–370 (2015)
4. Brunner, M., Bruggemann, B., Schulz, D.: Hierarchical rough terrain motion planning using an optimal sampling-based method. In: Proc. of IEEE ICRA. pp. 5539–
5544 (2013)
5. Dobson, A., Bekris, K.E.: Sparse roadmap spanners for asymptotically nearoptimal motion planning. Int. J. Rob. Res. 33(1), 18–47 (2014)
6. Ferguson, D., Stentz, A.: Using interpolation to improve path planning: The Field
D* algorithm. J. Field Robot. 23(2), 79–101 (2006)
7. Gammell, J.D., Srinivasa, S.S., Barfoot, T.D.: Batch informed trees (BIT*):
Sampling-based optimal planning via the heuristically guided search of implicit
random geometric graphs. In: Proc. of IEEE ICRA. pp. 3067–3074 (2015)
8. Janson, L., Schmerling, E., Clark, A., Pavone, M.: Fast marching tree: A fast
marching sampling-based method for optimal motion planning in many dimensions.
Int. J. Rob. Res. 34(7), 883–921 (2015)
9. Jordan, M., Perez, A.: Optimal bidirectional rapidly-exploring random trees. Tech.
Rep. TR-2013-021, MIT CSAIL (2013)
10. Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning.
Int. J. Rob. Res. 30(7), 846–894 (2011)
11. Kober, J., Bagnell, J.A., Peters, J.: Reinforcement learning in robotics: A survey.
Int. J. Rob. Res. 32(11), 1238–1274 (2013)
12. Kuffner, J.J., LaValle, S.M.: RRT-Connect: An efficient approach to single-query
path planning. In: Proc. of IEEE ICRA. pp. 995–1001 (2000)
13. Papadopoulos, E.G., Rey, D.A.: A new measure of tipover stability margin for
mobile manipulators. In: Proc. of IEEE ICRA. pp. 3111–3116 (1996)
14. Pokorny, F., Hawasly, M., Ramamoorthy, S.: Topological trajectory classification
with filtrations of simplical complexes and persistent homology. Int. J. Rob. Res.
35(1-3), 204–223 (2016)
15. Reid, W., Göktoǧan, A.H., Perez-Grau, F.J., Sukkarieh, S.: Actively articulated
suspension for a wheel-on-leg rover operating on a Martian analog surface. In:
Proc. of IEEE ICRA. pp. 5596–5602 (2016)
16. Reid, W., Göktoǧan, A.H., Sukkarieh, S.: Moving MAMMOTH: Stable motion for
a reconfigurable wheel-on-leg rover. In: Proc. of ARAA ACRA (2014)
17. Rickert, M., Brock, O., Knoll, A.: Balancing exploration and exploitation in motion
planning. IEEE T. Robot. 30(6), 2812–2817 (2014)
18. Satzinger, B.W., Lau, C., Byl, M., Byl, K.: Tractable locomotion planning for
robosimian. Int. J. Rob. Res. 34(13), 1541–1558 (2015)
19. Starek, J.A., Schmerling, E., Janson, L., Pavone, M.: Bidirectional fast marching
trees: An optimal sampling-based algorithm for bidirectional motion planning. In:
Proc. of WAFR (2014)
20. Sucan, I., Moll, M., Kavraki, L.E.: The Open Motion Planning Library. IEEE
Robot. Autom. Mag. pp. 72–82 (2012)
21. Vernaza, P., Lee, D.D.: Learning and exploiting low dimensional structure for efficient holonomic motion planning in high dimensional spaces. Int. J. Rob. Res.
31(14), 1739–1760 (2012)
22. Yi, D., Goodrich, M.A., Seppi, K.D.: Homotopy-aware RRT* : Toward humanrobot topological path-planning. In: Proc. of ACM/IEEE HRI. pp. 279–286 (2016)
23. Zhong, C., Liu, H.: A region-specific hybrid sampling method for optimal path
planning. Int. J. Adv. Robot. Syst. 13 (2016)