2021 American Control Conference (ACC)

New Orleans, USA, May 25-28, 2021

Efficient Robot Motion Planning via Sampling and Optimization

Jessica Leu, Ge Zhang, Liting Sun, and Masayoshi Tomizuka

Abstract— Robot motion planning is one of the important

elements in robotics. In environments full of obstacles, it is
always challenging to find a collision-free and dynamically-
feasible path between the robot’s initial configuration and goal
configuration. While many motion planning algorithms have
been proposed in the past, each of them has its pros and
cons. This work presents a benchmark which implements and
compares existing planning algorithms on a variety of problems
with extensive simulation. Based on that, we also propose
a hybrid planning algorithm, RRT*-CFS, that combines the
merits of sampling-based planning methods and optimization-
Fig. 1: A manipulator navigating through the obstacles.
based planning methods. The first layer, RRT*, quickly sam-
ples a semi-optimal path. The second layer, CFS, performs
sequential convex optimization given the reference path from are usually smooth. However, optimization-based algorithms
RRT*. The proposed RRT*-CFS has feasibility and convergence often fail to find a feasible solution if initial trajectories are
guarantees. Simulation results show that RRT*-CFS benefits
from the hybrid structure and performs robustly in various
naively chosen (e.g., a line segment from the initial to the
scenarios including the narrow passage problems. goal in the configuration space). The reason behind is that
most optimization-based algorithms rely on local gradient
I. INTRODUCTION information, so the final trajectories and the computational
Motion planning is one of the key challenges in robotics. time highly depend on the selection of the initial path.
It refers to the problem of finding a collision-free and In order to clearly see the effects of these pros and cons
dynamically-feasible path between the initial configuration when solving motion planning problems, this paper presents
and the goal configuration in environments full of obstacles. a benchmark that tests motion planning algorithms from
Existing motion planning algorithms fall into two categories: different categories. To the best knowledge of the authors,
planning-by-construction or planning-by-modification [1]. this is the first comprehensive benchmark that tests motion
Searching-based planning and sampling-based planning are planning algorithms from different categories.
two typical plan-by-construction algorithms. Algorithms In addition to the benchmark, this work also presents a
such as A* and D* search [2], [3] belong to searching-based hybrid planning algorithm, RRT*-CFS, which combines the
algorithms, whereas rapidly-exploring random tree (RRT) [4] merits of planning-by-construction algorithms and planning-
and probabilistic roadmap (PRM) [5] belong to sampling- by-modification algorithms. The algorithm has two layers.
based planning. Planning-by-modification refers to the algo- We first use RRT* [11] to quickly generate a feasible
rithms that reshape a reference trajectory to obtain optimality and semi-optimal path. This path then serves as an initial
regarding specific properties [6], [7], [8]. Optimization-based trajectory for the optimization layer that uses the convex
algorithms belong to this category. feasible set algorithm (CFS) [6] to quickly solve the non-
Both types of motion planning algorithms have pros and convex motion planning problem. RRT*-CFS is able to find
cons. Planning-by-construction algorithms are guaranteed to a globally-near-optimal solutions in complex environments,
generate collision-free trajectories and require short compu- even in scenarios with narrow passages [12], and has good
tational time. However, the constructed trajectories often do performance in terms of optimality and computational time.
not consider dynamic constraints and are not smooth. Some Our contributions are threefold as follows:
modifications remedy these problems [9], [10], but require
more computational resource. Optimization-based algorithms • A comprehensive benchmark that compares motion
often start with an initial trajectory that links the initial planning algorithms from different categories is pre-
configuration and the goal configuration [6], [7]. Next, the sented.
algorithms iteratively improve the trajectory to satisfy the • The proposed RRT*-CFS algorithm can solve planning
constraints (e.g., collision-free conditions) and minimize the problems that cannot be solved by many optimization-
cost. Note that the initial trajectory can be either feasible or based algorithms alone, has short computational time,
infeasible depending on the requirements of the algorithms. and has the lowest average-cost of all algorithms we
The trajectories generated by optimization-based algorithms compare in this work.
• We implement RRT*-CFS and demonstrate its success
All authors are with the Department of Mechanical Engineering, with extensive simulation.
University of California, Berkeley, CA 94720 USA jess.leu24,
ge zhang, litingsun, [email protected] The remainder of the paper is organized as fol-

978-1-6654-4197-1/$31.00 ©2021 AACC 4196

Authorized licensed use limited to: NORTH DAKOTA STATE UNIV. Downloaded on November 06,2024 at 06:13:47 UTC from IEEE Xplore. Restrictions apply.
lows. Section 2 discusses the related works. Section C. Optimization-based Algorithms and CFS
3 presents our proposed algorithm and the theoreti- There are also many optimization-based algorithms that
cal analysis. Section 4 presents the benchmark and can be used for motion planning, such as SQP [17], CHOMP
the simulation results (video is publicly available at [7], TrajOpt [18], and CFS [6]. SQP uses the Lagrangian of
jessicaleu24.github.io/ACC2021.html). Finally, we the original problem to formulate a transformed problem that
conclude the work in Section 5. solves for the Lagrange multipliers. The solution of these
Lagrange multipliers is then used to update the decision
II. RELATED WORKS variables of the original problem. CHOMP and TrajOpt both
formulate an unconstrained problem with a cost function
In this section, we first introduce our general formulation that penalizes the path’s smoothness and proximity to the
of the motion planning problem. We then present some key obstacles. However, the two have different approaches for
algorithms that we use or compare in our work. collision detection. In addition, TrajOpt uses SQP to solve
the problem; whereas CHOMP uses gradient descent.
Among these algorithms, CFS is a fast optimization-
A. Baseline Problem Formulation
based motion planning algorithm that can handle infeasible
In many scenarios, robot motion planning can be per- initialization under some assumptions. Here we give a brief
formed by solving an optimization problem with the fol- review of the CFS algorithm. We can rewrite the non-convex
lowing form: optimization problem as follows:
min f (x), (1) x∗ = arg max f (x). (3)
x∈Γ x∈Γ⊂Rn

where x ∈ Rn and Γ defines the feasible set: CFS solves the non-convex problem iteratively. The fol-
lowing information is required:
\ \
(0)
Γ= Γj = {x : hj (x) ≥ 0}. (2) • Initialization: An initial value of the state x , which
(0)
j j does not necessarily satisfy x ∈ Γ.
• Safety index and disjoint convex obstacles : Similar to
We assume that the constraint function hj (x) is a semi- Eq. (2), where hj (x) is the safety index for the jth
convex function [6]. For example, hj (x) can be the distance disjoint obstacle.
between a robot and the jth obstacle. The cost function, (k)
• Convex feasible set: The convex feasible set, χ :=
f : Rn → R, is strongly convex and smooth. Note that the (k)
χ(x ) ∈ Γ, is constructed corresponding to previous
motion planning problem is non-convex due to the existence states x(k) .
of the obstacles and the non-linear robot dynamics. Also, the The convex feasible set in our case is as follows:
dimension of the planning problem, n, depends on both the n
robot’s state space and the number of points from the initial (k)
\
χ(k) = χj ,
state to the final state. n is usually large in motion planning j=1
problems; therefore, solving motion planning problems is in n (4)
\
general hard. (k) > (k) (k)
= {x : hj (x ) + ∇ hj (x )(x − x ) ≥ 0}.
j=1

B. Sampling-based Algorithms and RRT* With CFS, a convex sub-optimization problem is formulated
and solved for the optimal value of x(k+1) :
In general, sampling-based algorithms are fast [4]. There
are many sampling-based motion planning algorithms, such x(k+1) = arg max f (x). (5)
as RRT*, variations of PRM [13], [14] and variations of x∈χ(k)

RRT [10], [11], [15], [16]. These variations modify the The algorithm solves the problem iteratively and results in a
original methods to obtain better planning performances, sequence of x(1) , x(2) , . . . , x(k) , . . . . It is guaranteed in [6]
e.g., handling dynamic constraints, smoothing trajectories, that this sequence will converge to a local optimal, x∗ .
short-cutting trajectories, etc. However, these modifications Notice that SQP and TrajOpt rely on the second-order
often require more computational time and resource and information of the original problem, while CHOMP and
the requirements grow quickly as the robot system becomes CFS rely on the gradient information. As mentioned previ-
more complex. As a result, these algorithms require longer ously, optimization-based algorithms rely on local gradient
computational time in complex systems. This weakens the information (or higher-order information); therefore the final
computation advantage of sampling-based methods. Since trajectories and the computational time highly depend on the
our goal is to construct a strong hybrid algorithm rather than choice of the initial path.
a strong stand-alone sampling-based algorithm, we value
computational time highly and choose RRT*, which can D. Hybrid Algorithms
provide a feasible and semi-optimal [11], [16] path with the There have been several researches focusing on hybrid
shortest computational time. planners [1], [19], [20], [21]. [1] used Lattice A* Search

4197
Authorized licensed use limited to: NORTH DAKOTA STATE UNIV. Downloaded on November 06,2024 at 06:13:47 UTC from IEEE Xplore. Restrictions apply.
to generate initial trajectories for CFS and illustrated its one RRT* thread as nsamples , and the obstacles as O. The
performance on a mobile robot. The sampling space of these RRT*-CFS algorithm is summarized as in Algorithm 1.
experiments is 2D. Since the number of points in the lattice As shown in Algorithm 1, given the inputs,
grid grows exponentially as the dimension grows in the θ0 , θgoal , nsamples , and O, each thread of the multi-
configuration space, the effectiveness of this work for high thread RRT* starts to find a feasible path that connects
dimensional applications (e.g., 6-DoF manipulators) is highly the initial configuration and the goal configuration. If
doubtful. [19] adopted the Bidirectional Rapidly-exploring more than one thread find a path, we choose the shortest
Random Tree (BiRRT) [15] to generate an initial feasible path and set it as θ RRT . If no thread finds a solution,
guess for the TrajOpt trajectory optimizer and demonstrated we repeat the multi-thread RRT* until we find a path. By
the success of their approach on the Atlas robot. However, setting up the nsamples properly, we can find a solution
only a limited number of testing scenarios were presented; in the first batch almost every time. Let x ∈ Rn and the
the robots only needed to avoid no more than two obsta- planning horizon be H, we generate the initial reference
0> >
cles. [20] presented a planning algorithm that combined x0 := [x0> 0>
0 , x1 , · · · , xH ] for CFS using the sampled
RRT
a roadmap and TrajOpt. Besides generating a collision- path θ . This process can be done by feeding the θ RRT
free and dynamically-feasible trajectory, they focused on to a motion generator that outputs a motion plan, x0 , which
avoiding singularities in redundant manipulators and meet- is a trajectory that follows the RRT* path. Then, the convex
ing Cartesian constraints. However, the algorithm required feasible set, χ(k) , is generated by linearizing the constraints
long planning time. [21] combined a sparse roadmap with at the reference point x(k) for k = 0, 1, . . . . The algorithm
TrajOpt. However, their methods did not consider complex terminates when the change of the cost at each iteration is
environments. smaller than a threshold, i.e., kf (x(k−1) ) − f (x(k) ))k ≤ .
A key challenge in motion planning problem is the narrow
passage problem, which refers to planning problems that B. Theoretical Analysis
have a very narrow region in between the initial and the Both the feasibility and the global convergence of RRT*-
goal in the feasible configuration space. Motion planning CFS rely on the fact that the motion planning problem
algorithms often take too much time or even cannot find a (Eq. (1) and (2)) satisfies the following assumption:
solution when encountering a narrow passage problem even
though the solution does exit [12], [22]. This type of problem Assumption (Problem formulation). The cost function f (x)
is one of the target scenarios in our simulation, which was is strongly convex and smooth. The constraint function hj (x)
not tested in [1], [19], [18], [20], [21]. is continuous, piece-wise smooth, and convex. The state
constraint Γ is non-convex and its complement is a collection
III. THE PROPOSED ALGORITHM of disjoint convex sets, i.e., each of the obstacle-region is
itself convex.
In this section, we introduce the proposed RRT*-CFS
algorithm as well as its feasibility and global convergence Let xr ∈ Rn be a feasible reference point, i.e., xr ∈ Γ.
guarantees.
Lemma 1 (Feasibility). If xr ∈ Γ, then xr ∈ χr and
A. The RRT*-CFS Algorithm Int(χr ) 6= ∅, where Int(χr ) is the interior of the set χr .

The proposed RRT*-CFS inherits the merits and avoids the Proof: When xr is feasible, xr ∈ χrj for all j according
shortcomings of each of the two algorithms. The RRT*-CFS to (4). Therefore, xr ∈ χr . [6] proved that χ(0) has nonempty
algorithm solves the non-convex motion planning problem interior if the assumption is satisfied. The problems studied
first by quickly finding a feasible and semi-optimal path, and in this work satisfy the assumption; therefore the proof holds
then iteratively refining the solution using CFS. The RRT*- true.
CFS has three main features. With Lemma 1, we obtain the following theorem:
• First, the RRT* layer can be implemented with multi- Theorem 2 (Feasibility of RRT*-CFS). Under Algorithm 1,
thread computation. This allows us to significantly the sequence {x(k) } satisfies x(k) ∈ Γ for k = 0, 1, 2, ....
reduce the computational time.
• Second, RRT*-CFS has stochasticity due to the random
Proof: RRT* generates x(0) such that x(0) ∈ Γ, i.e.,
sampling process in RRT*. This helps RRT*-CFS to feasibility holds when k = 0. According to Lemma 1, χ(0)
avoid bad local optima that optimization-based algo- has nonempty interior, then x(1) ∈ Γ can be attained by
rithms may get stuck in. solving the convex optimization problem (5). By induction,
• Finally, RRT*-CFS inherits the properties of CFS so
we conclude that x(k) ∈ χ(k−1) ⊂ Γ for k = 1, 2, 3....
that feasibility, smoothness and convergence of the final The following shows the convergence of RRT*-CFS.
solution are guaranteed if the problem satisfies the Given Theorem 2, the remainder of the proof is similar to
description in Section II-A. that in [6], Theorem 4.1; therefore, we have the following
corollary.
Denote the configuration of a d-degree-of-freedom (d-DoF)
robot as θ ∈ Rd , the initial configuration as θ0 , the goal Corollary 2.1 (Global convergence of RRT*-CFS). Under
configuration as θgoal , the maximum number of samples in Algorithm 1, the sequence {x(k) } will always converge to

4198
Authorized licensed use limited to: NORTH DAKOTA STATE UNIV. Downloaded on November 06,2024 at 06:13:47 UTC from IEEE Xplore. Restrictions apply.
Algorithm 1 RRT*-CFS
procedure RRT*-CFS(θ0 , θgoal , nsamples , O)
while ! ∃ θ RRT do
θ RRT ← Multi thread RRT*(θ0 , θgoal , nsamples , O) . Get the shortest RRT* path among all threads.
0 RRT
x ← generate reference(θθ ) . Generat the initial reference x0 for CFS based on θ RRT .
while Stop Tcriterion is not satisfied do . k = 0, 1, 2, . . .
n
χ(k) = j=1 {x : hj (x(k) ) + ∇> hj (x(k) )(x − x(k) ) ≥ 0}
x(k+1) = arg min f (x).
x∈χ(k)
(k+1)
return x

B. The Motion Planning Problem

In this paper, the goal of the motion planning problem

is to plan the command that brings the robot to the goal
configuration while avoiding obstacles. We first solve for a
path using the multi-thread-RRT* with the configuration, θ,
defined previously. After getting the path θ RRT , an optimiza-
tion problem can be formulated. The decision variable for
Fig. 2: A mobile robot (left) and a manipulator (right). each time step is ut , and the input vector that the algorithm
optimizes is denoted as u := [u> > > >
0 , u1 , · · · , uH ] , where H
some x∗ ∈ Γ. x∗ is a strong local optimum of (1) if the is the planning horizon. Similarly, the resulting state vector
limit is reached. x∗ is at least a weak local optimum of (1) >
is z := [z1> , z2> , · · · , zH+1 ]> . Given the initial state, z0 ,
if the limit is not reached. we obtain z = fki (z0 , u) by concatenating the kinematic
function (Eq. (6) or (7)) throughout the planning horizon.
IV. SIMULATION SETUP AND RESULTS For simplicity, denote the kinematic function as fki,z0 (u) :=
A. Robot Models fki (z0 , u). In order to obtain the optimal solution u∗ given
the constrained feasible set Γ and the input constraint umax ,
We used two different robot platforms (Fig. 2) to test the
the following optimization problem needs to be solved:
RRT*-CFS and other algorithms for comparison.
1) Mobile robot: We use a simple kinematic model to
u∗ = arg min fz0 (u),
model a mobile robot on a 2D-plan. Denote the states of u
the mobile robot at time step t as zt = [xt , yt ]> , the input s.t. fki,z0 (u) ∈ Γ, (8)
velocity as ut = [vx,t , vy,t ]> , and the robot configuration as − umax ≤ u ≤ umax .
θ = [x, y]> . The linear kinematic model, is
The cost function is quadratic that has the form: fz0 (u) =
       
xt+1 1 0 xt Ts 0 vx,t
= + , (6) kfki,z0 (u) − zgoal k22 + λkuk22 , which is convex and regular.
yt+1 0 1 yt 0 Ts vy,t
The first term penalizes the deviation from the goal and the
where Ts is the sampling time. second term penalizes the input.
2) Manipulator: We use a 5-degree-of-freedom manip-
ulator. Denote the states of the manipulator as zt =
[θ1 , θ2 , θ3 , θ4 , θ5 , ω1 , ω2 , ω3 , ω4 , ω5 ]>
t , where θi and ωi , i ∈ C. Implementation
{1, 2, 3, 4, 5} are the angle and the angular velocity of ith
joint, respectively. The input contains the angular accelera- CFS updates z(k) = fki,z0 (u(k−1) ) at iteration k =
tion at each joint, denoted as ut = [α1 , α2 , α3 , α4 , α5 ]> t . The
2, 3 . . . . Notice that z(1) is determined by θ RRT and u(0)
robot configuration is θ = [θ1 , θ2 , θ3 , θ4 , θ5 ]> . The linear is initialized as a zero vector. (A more sophisticated way
kinematic model is as follows: is to initialize u(0) with a motion generator that commands
the robot to track θ RRT .) The convex feasible set,Tχ(k) , is
zt+1 = Azt + But , (7) determined by z(k) . Given the feasible set Γ = j,t {z :
hj (zt ) ≥ 0} (j numerates over obstacles and t numerates
where,   over time steps), the results of the previous iteration (u(k−1)
I5×5 Ts I5×5
A= , and z(k) ), and the function fki,z0 (u), we can construct the
05×5 I5×5
convex feasible set as:
and,
(k)
0.5Ts2 I5×5
  \
B= . χz(k)
0
= {u : h0j,t (u, zt , u(k−1) ) ≥ 0}, (9)
Ts I5×5 j,t

4199
Authorized licensed use limited to: NORTH DAKOTA STATE UNIV. Downloaded on November 06,2024 at 06:13:47 UTC from IEEE Xplore. Restrictions apply.
 Obstacles CHOMP CFS Initial
Goal PRM path PRM-CFS
0.1 Obstacles CHOMP SQP CFS
Initial Goal RRT* samples RRT* path
PRM-CFS RRT*-CFS
y [m]

0 0.2
0.1

y [m]
-0.1 0
-0.1 0 0.1 0.2 0.3 0.4 0.5
x [m] −0.1
(a) Planning result using CFS, CHOMP, and PRM-CFS.
−0.2
−0.1 0 0.1 0.2 0.3 0.4 0.5
x [m]
Obstacles RRT* samples RRT* path
RRT*-CFS Initial Goal
Fig. 4: A simulation result of motion planning in the narrow passage
0.1 (close to y = 0.1) scenario.

see that all four trajectories are smooth and can successfully
y [m]

0 bring the robot to the goal (at (0, 0)). More importantly,
RRT*-CFS converges to the global optimum, while others
-0.1 stuck in local optima. TABLE I shows the result averaging
-0.1 0 0.1 0.2 0.3 0.4 0.5 over 100 trials (obstacles are randomly located in each trials).
x [m] Benefiting from the first layer, the proposed RRT*-CFS has
(b) Planning result using RRT*-CFS. a success rate of 100% and also requires less iterations
Fig. 3: Simulation results of a 2D motion planning. to converge comparing to CFS, SQP, CHOMP, and PRM-
CFS. RRT*-CFS also has the lowest average cost, which is
(k) (k)
where h0j,t = hj (zt ) + ∇> hj (zt )∇fki,z0 ,t (u(k−1) )(u − contributed by the semi-optimal initial path given by RRT*
u(k−1) ). Therefore, the iterative sub-problem is as follows: and the optimization process by CFS. This also indicates
that RRT*-CFS is more likely to converge to nearly-global
u∗(k) = arg min fz0 (u),
u optima. Comparing RRT*-CFS and RRT*-SQP, we see that
s.t. u ∈ χz(k) (u(k−1) , z(k) ), (10) RRT*-CFS requires less computational time. This is due
0

− umax ≤ u ≤ umax . to the fact that CFS exploits the geometry of the motion
planning problem. Also, RRT*-CFS is faster than PRM-CFS
D. Simulation Setup due to the fact that RRT* is faster than PRM in these motion
We show the motion planning simulation results in the planning problems. Even though CFS is the fastest algorithm
following sections. The simulation is conducted in Matlab besides RRT* in both 4-obstacle and 1-obstacle cases, RRT*-
R2020a on a desktop with 3.2GHz Intel Core i7-8700 CPU. CFS is faster than CFS in the 10-obstacle complex scenarios
The stopping criteria for optimization-based methods (i.e., due to a better initialization and faster convergence rate. Also
CFS, SQP, CHOMP, and the CFS layer of RRT*-CFS) are notice that CFS is more vulnerable to be trapped in bad local
the same. In other words, if either (1) the algorithm reaches optima and cannot find a path to reach the goal in complex
the maximum number of iterations (i.e., 40 iterations) or (2) scenarios; whereas RRT*-CFS can always find a path (the
the change of the cost is smaller than the threshold (i.e., failure cases indicate the resulting solutions do not bring the
 = 10−3 ), the algorithm terminates. robot to the goal).
E. 2D Motion Planning Benchmark Narrow Passages. We also test RRT*-CFS in the narrow
The 2D scenarios are designed to simulate general 2D passage scenario. The goal of the robot is again to plan a path
motion planning scenes of mobile platforms. Notice that to the goal point. However the robot has to navigate through
the obstacles in these scenarios are convex and the robot a narrow pathway in order to reach the goal point. Fig. 4
kinematics is simplified as a point mass. Therefore, the shows a planning result in one of these scenarios. RRT*-
motion planning problems satisfy the conditions mentioned CFS successfully explores the narrow passage and plans
in Section II-A. The benchmark has two categories, which a feasible trajectory (blue-circle-line). On the other hand,
are collision avoidance planning in multiple obstacles scenes CFS, SQP, and CHOMP failed to find a solution. The CFS
and in narrow passage scenes. In both cases, the planning trajectory (blue-star-line) and the SQP trajectory (gray-star-
horizons of optimization-based algorithms are set to be H = line, under the CFS trajectory) stop in front of the wall while
30. the CHOMP trajectory (red-star-line) directly penetrates the
Multiple Obstacles. One exemplar simulation environment wall. These two failures are due to the lack of local gradient
is shown in Fig. 3, where Fig. 3a shows the planning result information, which is crucial for optimization-based methods
of CHOMP (red-star-line), PRM-CFS (blue-circle), and CFS that solve each iteration by calculating the gradient using
(blue-star-line) and Fig. 3b shows the planning result of the result of the previous iteration. The 99% success rate in
RRT*-CFS (blue-line). By comparing the two figures, we TABLE I for CFS and CHOMP is due to the same reason,

4200
Authorized licensed use limited to: NORTH DAKOTA STATE UNIV. Downloaded on November 06,2024 at 06:13:47 UTC from IEEE Xplore. Restrictions apply.
 TABLE I: Simulation comparison of 2D planning. (Average of 100 trials.)
Algorithm # obstacles Computational time [s] # iterations Cost Success rate (%)
RRT* 1 0.006 N/A 1.266 100
PRM 1 1.1859 N/A 1.3547 100
CFS 1 0.038 7.40 1.007 99
SQP 1 1.73 7.40 1.006 100
CHOMP 1 0.053 137.2 1.100 99
PRM-CFS 1 1.261 7.22 1.009 100
RRT*-SQP 1 0.994 7.01 1.001 100
RRT*-CFS 1 0.038 7.01 1.001 100
RRT* 4 0.03 N/A 1.272 100
PRM 4 4.24 N/A 1.379 100
CFS 4 0.12 12.63 1.048 99
SQP 4 4.29 12.80 1.031 100
CHOMP 4 0.23 199.42 1.110 99
PRM-SQP 4 4.35 11.04 0.102 100
RRT*-SQP 4 2.63 10.58 1.011 100
RRT*-CFS 4 0.15 10.58 1.011 100
RRT* 10 0.042 N/A 1.266 100
CFS 10 0.695 14.46 1.107 87
RRT*-CFS 10 0.510 12.69 1.061 100

TABLE II: Simulation comparison of 2D-narrow-passage moves to the goal configuration without colliding with the
planning. (Average of 20 trials.) obstacles. Notice that CFS fails to plan a trajectory that
Algorithm Computational time [s] # iterations Cost brings the manipulator to the goal configuration in this
RRT* 0.27 N/A 1.22 scenario. With the initial path from RRT*, RRT*-CFS plans
PRM 20.05 N/A 1.22
PRM-CFS 20.64 6.6 1.02
a smoother trajectory that navigates through the complex en-
RRT*-CFS 0.43 8.26 1.01 vironment comparing to the original RRT* trajectory. Notice
where the initial point and the goal point are both lying on that these planning problems do not satisfy the descriptions
the symmetric axis of the obstacle. Even though using local in Section II-A, however, RRT*-CFS still converges well
higher-order information can enable the algorithms to deal empirically.
with some of these scenarios (e.g., SQP), solving the narrow V. CONCLUSION
passage problem is still hard for optimization-based methods
alone; because sometimes there is no information contained This paper presented a comprehensive benchmark that
in the higher-order terms in these scenarios (e.g., when facing compares motion planning algorithms from different cat-
the wall, the second order derivative of a line is zero). egories and introduced a fast motion planning algorithm,
This observation again demonstrates the need of a sampling RRT*-CFS, that combined the merits of sampling-based
mechanism in the motion planning algorithm so that the planning methods and optimization-based planning methods.
algorithm can explore beyond the local gradient information. The RRT*-CFS quickly found a feasible and semi-optimal
The simulation result of narrow passage environments is path using RRT* and iteritavly refined the solution using
summarized in TABLE II. We see that RRT*-CFS improves CFS. RRT*-CFS had feasibility and global convergence
the cost without sacrificing too much computational time. guarantees inherited from CFS in scenarios where obstacles
Although PRM-CFS also finds a solution and requires fewer could be represented by disjoint convex objects. Simulation
iterations, its computational time is two magnitudes larger results showed that RRT*-CFS benefited from the hybrid
than RRT*-CFS. structure. Comparing to RRT*, PRM, CFS, SQP, CHOMP,
and PRM-CFS, RRT*-CFS had the lowest cost and con-
F. 5D Motion Planning for a Manipulator verged with less number of iterations. RRT*-CFS could
also solve planning problems in complex scenarios such as
In 5D motion planning simulations, the goal of the ma- the narrow passage problem, in which CHOMP, SQP, and
nipulator is to reach the goal configuration from the initial CFS failed. Even though RRT*-CFS has two layers, the
configuration while avoiding obstacles. TABLE III shows computational time is still competitive in simple scenarios
the result averaging over 20 trials. Similar to the trend in and outperforms other algorithms (except RRT* in terms of
2D cases, RRT*-CFS has lower average cost comparing to time) in complex scenarios. We concluded that the hybrid
CFS. In terms of computational time, CFS performs better structure indeed brought strong performance. The future
in scenarios with one or two obstacles, while RRT*-CFS work is to improve both the sampling-based layer and the
shows its strength in complex environment and performs optimization-based layer as well as the connection between
better in scenarios with three or four obstacles. Benefiting them.
from the first layer, the proposed RRT*-CFS has a success
rate higher than that of CFS. RRT*-CFS also requires less ACKNOWLEDGEMENT
iterations to converge comparing to CFS. One of the simula- The authors thank Changliu Liu for helpful discussions.
tion environments is shown in Fig. 5, where the manipulator This work was supported by the National Science Foun-

4201
Authorized licensed use limited to: NORTH DAKOTA STATE UNIV. Downloaded on November 06,2024 at 06:13:47 UTC from IEEE Xplore. Restrictions apply.
 TABLE III: Simulation comparison of 5D Motion planning. (Average of 20 trials.)
Algorithm # obstacles Computational time [s] # iterations Cost Success rate (%)
RRT* 1 or 2 5.7 N/A N/A 100
CFS 1 or 2 4.74 20 1.94 95
RRT*-CFS 1 or 2 12.42 15.56 1.66 99
RRT* 3 or 4 8.45 N/A N/A 100
CFS 3 or 4 114.10 20 3.78 85
RRT*-CFS 3 or 4 59.675 18.58 3.39 89

Fig. 5: Planning results using RRT*-CFS in 5D manipulation planning problems.

dation under Grant No.1734109. Any opinion, finding, and [12] S. Rodrı́guez, X. Tang, J. Lien, and N. Amato, “An obstacle-based
conclusion expressed in this paper are those of the authors rapidly-exploring random tree,” Proceedings 2006 IEEE International
Conference on Robotics and Automation, 2006. ICRA 2006., pp. 895–
and do not necessarily reflect those of the National Science 900, 2006.
Foundation. [13] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and D. Vallejo,
“Obprm: An obstacle-based prm for 3d workspaces,” in Robotics:
R EFERENCES The Algorithmic Perspective: 1998 Workshop on the Algorithmic
Foundations of Robotics, 1998, pp. 155–168.
[1] C. Liu, C. Lin, Y. Wang, and M. Tomizuka, “Convex feasible set [14] R. Bohlin and L. E. Kavraki, “Path planning using lazy prm,” in
algorithm for constrained trajectory smoothing,” in 2017 American Proceedings 2000 ICRA. Millennium Conference. IEEE International
Control Conference (ACC), May 2017, pp. 4177–4182. Conference on Robotics and Automation. Symposia Proceedings (Cat.
[2] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the No. 00CH37065), vol. 1. IEEE, 2000, pp. 521–528.
heuristic determination of minimum cost paths,” IEEE Transactions [15] J. J. Kuffner and S. M. LaValle, “Rrt-connect: An efficient approach to
on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968. single-query path planning,” in Proceedings 2000 ICRA. Millennium
[3] A. Stentz, “Optimal and efficient path planning for partially-known Conference. IEEE International Conference on Robotics and Automa-
environments,” in IN PROCEEDINGS OF THE IEEE INTERNA- tion. Symposia Proceedings (Cat. No.00CH37065), vol. 2, 2000, pp.
TIONAL CONFERENCE ON ROBOTICS AND AUTOMATION, 1994, 995–1001 vol.2.
pp. 3310–3317. [16] F. Islam, J. Nasir, U. Malik, Y. Ayaz, and O. Hasan, “Rrt-smart: Rapid
[4] S. M. Lavalle, “Rapidly-exploring random trees: A new tool for path convergence implementation of rrt towards optimal solution,” in 2012
planning,” Tech. Rep., 1998. IEEE International Conference on Mechatronics and Automation.
[5] L. E. Kavraki, P. Svestka, J. . Latombe, and M. H. Overmars, “Proba- IEEE, 2012, pp. 1651–1656.
bilistic roadmaps for path planning in high-dimensional configuration [17] P. Spellucci, “A new technique for inconsistent qp problems in the sqp
spaces,” IEEE Transactions on Robotics and Automation, vol. 12, method,” Mathematical Methods of Operations Research, vol. 47, pp.
no. 4, pp. 566–580, Aug 1996. 355–400, 1998.
[6] C. Liu, C.-Y. Lin, and M. Tomizuka, “The convex feasible set algo- [18] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan,
rithm for real time optimization in motion planning,” SIAM Journal S. Patil, K. Goldberg, and P. Abbeel, “Motion planning with sequential
on Control and Optimization, vol. 56, no. 4, pp. 2712–2733, 2018. convex optimization and convex collision checking,” The International
Journal of Robotics Research, vol. 33, pp. 1251–1270, 08 2014.
[7] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp:
[19] L. Li, X. Long, and M. A. Gennert, “Birrtopt: A combined sam-
Gradient optimization techniques for efficient motion planning,” in
pling and optimizing motion planner for humanoid robots,” in 2016
2009 IEEE International Conference on Robotics and Automation,
IEEE-RAS 16th International Conference on Humanoid Robots (Hu-
2009, pp. 489–494.
manoids), 2016, pp. 469–476.
[8] J. Leu, R. Lim, and M. Tomizuka, “Safe and coordinated hierarchical
[20] C. Park, F. Rabe, S. Sharma, C. Scheurer, U. E. Zimmermann, and
receding horizon control for mobile manipulators,” in 2020 American
D. Manocha, “Parallel cartesian planning in dynamic environments
Control Conference (ACC). IEEE, 2020, pp. 2143–2149.
using constrained trajectory planning,” in 2015 IEEE-RAS 15th In-
[9] J. Van Den Berg and M. Overmars, “Kinodynamic motion planning on
ternational Conference on Humanoid Robots (Humanoids), 2015, pp.
roadmaps in dynamic environments,” in 2007 IEEE/RSJ International
983–990.
Conference on Intelligent Robots and Systems. IEEE, 2007, pp. 4253–
[21] S. Dai, M. Orton, S. Schaffert, A. Hofmann, and B. Williams,
4258.
“Improving trajectory optimization using a roadmap framework,” 10
[10] D. J. Webb and J. Van Den Berg, “Kinodynamic rrt*: Asymptotically
2018, pp. 8674–8681.
optimal motion planning for robots with linear dynamics,” in 2013
[22] Liangjun Zhang and D. Manocha, “An efficient retraction-based rrt
IEEE International Conference on Robotics and Automation. IEEE,
planner,” in 2008 IEEE International Conference on Robotics and
2013, pp. 5054–5061.
Automation, 2008, pp. 3743–3750.
[11] 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.

4202
Authorized licensed use limited to: NORTH DAKOTA STATE UNIV. Downloaded on November 06,2024 at 06:13:47 UTC from IEEE Xplore. Restrictions apply.