2018 15th International Conference on

Control, Automation, Robotics and Vision (ICARCV)

Singapore, November 18-21, 2018

Fast Robot Motion Planning

with Collision Avoidance and Temporal Optimization
Hsien-Chung Lin∗ , Changliu Liu∗ , and Masayoshi Tomizuka

Abstract— Considering the growing demand of real-time Trajectory planning to avoid collision
Reference Path/Points
motion planning in robot applications, this paper proposes a
fast robot motion planner (FRMP) to plan collision-free and Motion Reference Path
(Collision)
time-optimal trajectories, which applies the convex feasible set Planner
algorithm (CFS) to solve both the trajectory planning problem Trajectory Modified Path
(Collision Free)
and the temporal optimization problem. The performance of Planning
CFS in trajectory planning is compared to the sequential Collision-free
quadratic programming (SQP) in simulation, which shows Trajectory
Temporal optimization to reduce cycle time
a significant decrease in iteration numbers and computation Temporal
time to converge a solution. The effectiveness of temporal Optimization
Original
optimization is shown on the operational time reduction in the Time-optimal Horizon

experiment on FANUC LR Mate 200iD/7L. trajectory

Modified
Horizon
Robot Motion
I. I NTRODUCTION
Robot motion planning has been a popular topic for several
Fig. 1. The framework of fast robot motion planner (FRMP).
decades. Most researches address the two key factors: safety
and efficiency. Safety indicates the protection for the robot trajectory from a given reference trajectory or several way
system from any risk of collision. Several sampling-based points, whereas the temporal optimization is to minimize the
methods such as [1], [2] and optimization-based approaches cycle time over the planned trajectory. This paper utilizes
such as [3], [4] have been developed to plan collision-free optimization-based algorithms to deal with the problems
trajectories for robots. Efficiency refers the minimization of in both layers in FRMP. The optimization-based motion
the cost such as control input and operation time. Some planning methods, such as model-predictive control (MPC)
algorithms [5], [6] are proposed to find the time-optimal [7], often need to face highly nonlinear dynamics constraints
trajectory on a specified path. and highly non-convex constraints for obstacle avoidance,
However, this field is still open for research especially which make it difficult to solve the problem efficiently.
from the viewpoint of computationally efficient motion Convexification is a common technique to transform a
planning because of the increasing demand of mass cus- non-convex problem into a convex one. One of the most
tomization and the growing application of human-robot in- popular convexification method is the sequential quadratic
teraction (HRI). mass customization is unlike conventional programming (SQP) [8], [9], which iteratively solves a
mass production, where every product looks very similar but quadratic subproblem obtained by quadratic approximation
slightly different. However, the whole trajectory is required of the Lagrangian function and linearizion of all constraints.
to be reprogrammed due to these subtle changes, which The method has been successfully applied to robot motion
is nontrivial and time-consuming. On the other hand, HRI planning as discussed in [10] and [11]. However, SQP is
requires the robot to frequently re-plan its motion so that it designed for general purposes, and it often takes multiple
can safely interact with human in a dynamic environment. iterations to find the solution.
Both applications show the need of a fast robot motion Considering the specific geometric structure in motion
planning, which helps the robot to efficiently generate new planning problems, the convex feasible set algorithm (CFS)
motion trajectories to adapt to varying environment. [12] and the slack convex feasible set algorithm (SCFS) [13]
In this paper, the framework of fast robot motion planner are proposed for the real time motion planning, a successful
(FRMP) is proposed as shown in Fig.1. It has two layers: application to path planning for autonomous vehicles is given
the trajectory planning layer and the temporal optimization in [14]. FRMP follows the similar algorithmic structure
layer, where each layer deals with safety and efficiency to plan a collision-free trajectory for robot manipulators.
respectively. Trajectory planning is to plan a collision-free Moreover, in order to achieve the time optimality in a short
computational time, temporal optimization is formulated into
∗ These authors equally contributed to this work. another CFS problem and solved in a fast manner.
The authors are with Department of Mechanical Engineering, Uni-
versity of California, Berkeley, CA 94720, USA. Email: {hclin, The rest of this paper is organized as follow. Section II
changliuliu, tomizuka }@berkeley.edu reviews the convex feasible set algorithm for trajectory plan-
The current affiliation of H.-C. Lin is FANUC Advanced Research ning. Section III firstly formulates the temporal optimization
Laboratory, Union City, CA, 94587, USA.
The current affiliation of C. Liu is Stanford University, Stanford, CA, problem, then introduces how to apply CFS to this problem.
94305, USA. Section IV compares the performance between CFS and

978-1-5386-9582-1/18/$31.00 ©2018 IEEE 29

 (a) (b)
Fig. 3. Illustration of (a) the nonlinear equality constraint and (b) the
(a) (b) nonlinear inequality constraints with slack variables.
Fig. 2. Illustration of (a) The geometric structure of the trajectory planning
problem (b) The convex feasible set in the trajectory space. Equation (1a) is designed to be a quadratic cost function
for the task performance, where kx − xr k2Q := (x −
SQP in trajectory planning and provides the experimental
xr )T Q(x − xr ) penalizes the deviation of the planned
results of FRMP conducted on FANUC LR Mate 200iD/7L.
trajectory from the reference trajectory, and kuk2R = uT Ru
Section V concludes the paper.
penalizes the control effort over the trajectory. Note that
II. T RAJECTORY P LANNING the matrices Q, R are designed to be positive definite.
A. Problem Formulation Equation (1b) are the feasible constraints of the state and the
input, e.g. joint limits, singularity points, and saturation of
Denote the state of the robot as x ∈ X ⊂ Rn where the control input. Equation (1c) is the dynamics constraints.
X is the feasible state in the n-dimensional state space; Equation (1d) describes the collision avoidance constraints,
u ∈ U ⊂ Rm is the control input of the robot, where where dmin ∈ R+ is the safety distance margin.
U is the constraints of u in the m-dimensional space1 . In
general robot trajectory planning, the input constraint is often B. The Geometric Structure of Trajectory Planning
formulated as a box constraint, e.g. −umax ≤ u ≤ umax ,
where umax ∈ Rm Combining the feasible state constraint in 1b and the safety
+ is the maximum magnitude of the control
input. constraint 1d together, the feasible trajectory constraint in the
Suppose the robot needs to move from the initial position trajectory space is given by x ∈ Γ = X N +1 ∩ D, where Γ
to the goal denotes the constraint on the augmented state space X N +1
 position, its discrete-time
T trajectory is denoted
and D describes the safety set in the trajectory space. The
as x = xT0 , xT1 , · · · , xTN ∈ X N +1 ⊂ Rn(N +1) , where
xt ∈ X is the robot state at time step t and N is the dynamics constraint in (1c) can be rewritten as G(x, u) = 0
horizon. The sampling time is defined as dt. The reference in the trajectory space, where G : X N +1 × U N → Rm×N
trajectory and the reference state at time step t are denoted represents the dynamic relationship between states and in-
as xr ∈ X N +1 and xrt ∈ X , respectively. Similarly, u = puts. Hence, the robot trajectory planning problem can be
 T T T rewritten as
u0 , u1 , · · · , uTN −1 ∈ U N = Ω ⊂ RmN is the control
input for the whole trajectory. min J(x, u) (2a)
The Cartesian space occupied by the robot body at the x,u

state xt is denoted as C(xt ) ∈ R3 , whereas the area s.t. x ∈ Γ, u ∈ Ω, (2b)

occupied by the obstacles in the environment at time t G(x, u) = 0 (2c)
is denoted as Ot ∈ R3 . Suppose the Euclidean distance
between pA and pB in the Cartesian space is denoted as The geometry of this problem in the trajectory space
dE (pA , pB ) : R3 × R3 → R, then the minimum distance is illustrated in Fig. 2a, where M is the manifold of the
between the robot and the obstacles is given by d(xt , Ot ) := robot dynamics, and the gray patch on the manifold is the
minpR ∈C(xt ),pO ∈Ot dE (pR , pO ). infeasible region. Notice that there are two important features
With the notation above, the robot trajectory planning in this problem.
problem can be formulated as a general non-convex opti- Feature 1 (Symmetry): The cost function given in (1a) is
mization problem, designed to be J(x, u) = J1 (x)+J2 (u), where the minimum
of J2 (u) is achieved at u = 0. Moreover, the box constraint
min J(x, u) = kx − xr k2Q + kuk2R (1a)
x,u of Ω, −umax ≤ u ≤ umax , is also symmetric to u = 0.
s.t. xt ∈ X , ut ∈ U, (1b) Feature 2 (Affine Dynamics): Considering the robot dynam-
ics equation is written as M (x)ẍ + N (x, ẋ) = u, it can
xt = f (xt−1 , ut−1 ), (1c)
be regarded as an affine dynamics equation, i.e. G(x, u) =
d(xt , Ot ) ≥ dmin , ∀t = 1, · · · , N (1d) F (x) + H(x)u = 0. For the kinematic model, the affine
1 The control input u is not necessarily a physical input (such as the joint
dynamics is also valid since it can be regarded as a linear
torque for a robot). It can be any parameter that needs to be considered in control system, i.e. xt = Axt−1 + But−1 = 0.
the trajectory optimization (such as the acceleration of a trajectory). Due to these two features, the problem can be relaxed by

30
 convex subproblems by obtaining convex feasible sets within
the non-convex inequality constraints and linear equality
constraints, then iteratively solve the quadratic program-
ming (QP) subproblems until convergence. Note that CFS

…
is applied to the problem under the following assumption:
1) The cost function J is assumed to be smooth, strictly
convex. 2) The constraint Γe is assumed as the intersection
of N supersets Γi which can be represented by continuous,
semi-convex and piecewise smooth functions φi , e.g. Γe =
∩i {z : φi (z) ≥ 0}. The semi-convexity of φi implies
that the Hessian of φi is lower-bounded. i.e. there exists a
positive semi-definite matrix Hi such that for any z and v,
φi (z + v) − 2φi (z) + φi (z − v) ≥ −v T Hi v.
Fig. 4 shows how CFS computes the solution iteratively,
where the space Γe is filled contour plots of J(z), and the
infeasible sets are the gray polygons on the plot. At iteration
Fig. 4. The illustration of the convex feasible set algorithm, where the k, given a reference point z(k) , a convex feasible set F (k) :=
yellow polygons are the convex feasible set obtained at each iteration and
the gray areas are the infeasible set in the space Γe . F (z(k) ) ⊂ Γe is computed around z(k) , which are the yellow
polygons in Fig. 4. Then a new reference point z(k+1) will
Algorithm 1 The Convex Feasible Set Algorithm be obtained by solving the following QP problem
z(0) = zr
z(k+1) = arg min J(z) (4)
while kz(k+1) − zk k >  do z∈F (k)
Find the convex feasible set: F(z(k) ) ⊂ Γe
Solve QP: z(k+1) = arg minz∈F (k) J(z) The iteration will be terminated until the solution converges,
zr ← z(k+1) , k = k + 1 i.e. kz(k+1) − zk k ≤ .
end while Given a reference point zr , the desired convex feasible set
F(zr ) is obtained by F(zr ) := ∩i Fi (zr ), where Fi (zr ) ⊂
Γi . The different cases of Fi (zr ) are illustrated in Fig. 5,
introducing the slack variable y, where the gray shaded areas represent the infeasible set. The
mathematical definition of Fi (zr ) is stated below,
min J(x, y) (3a) Case 1: Γi is convex.
x,y
s.t. x ∈ Γ, y ≤ umax , (3b) Define Fi = Γi .
Case 2: The complementary of Γi is convex.
F (x) + H(x)y ≥ 0, F (x) − H(x)y ≥ 0 (3c)
In this case, φi can be designed to be convex, then φi (z) ≥
It is proven in [13] that the optimizer of this relaxed φi (zr ) + ∇φi (zr )(z − zr ). At the point where φi is not
problem is equivalent to that of the original problem. The differentiable, ∇φi is a sub-gradient which should be chosen
intuition behind the relaxation is that: by introducing the as such that the steepest descent of J in the set of Γe is
slackness, the feasible region is augmented from the original always included in the convex set F. the convex feasible set
nonlinear manifold M in Fig. 2a to the augmented feasible Fi with respect to a reference point zr is defined as
volume Γe in Fig. 2b. Due to Feature 1, as J2 achieves
the minimum at u = 0, the algorithm will pull the optimal Fi (zr ) := {z : φi (zr ) + ∇φi (zr )(z − zr ) ≥ 0} (5)
solution down to M, which is on the “bottom” of Γe , so that
we may still get the same optimal solution as in the original Case 3: neither Γi nor its complementary is convex.
problem. In this case, φi is neither convex nor concave, but we can
The difference between these two problems is illustrated define a new convex function as φ̃i (z) := φi (z) + 12 (z −
in Fig. 3, where the curve in Fig. 3a represents the nonlinear zr )T Hi (z − zr ). Then φ̃i (z) ≥ φ̃i (zr ) + ∇φ̃i (zr )(z − zr ) +
equality constraint (2c) and the shaded area in Fig. 3b repre- ∇φi (zr )(z − zr ), where ∇φi is identified with the sub-
sents the nonlinear inequality constraints (3c).By introducing gradient of φ̃i at points that are not differentiable. The convex
the slack variable y, the nonlinear equality constraint is feasible set with respect to zr is then defined as
successfully removed. Then, let z = [xT yT ]T , and the
problem becomes minz∈Γe J(z), which will be solved by Fi (zr ) := {z :φi (zr ) + ∇φi (zr )(z − zr )
the CFS algorithm. 1
≥ (z − zr )T Hi (z − zr )} (6)
C. The Convex Feasible Set Algorithm 2

The idea of the CFS algorithm proposed in [12] is to The CFS algorithm is summarized in Algorithm 1, and the
transform the original non-convex problem into a sequence of detail of its convergence and feasibility are proven in [12].

31
 (a) (b) (c)
Fig. 5. The geometry illustration of the feasible set F at the reference
point zr . (a) Case 1:Γi is convex. (b) Case 2: The infeasible set is convex.
(c) Case 3: Neither Γi nor the infeasible set is convex.

Fig. 6. Illustration of the temporal optimization, where the green line

is represents original trajectory, whereas the orange line represents the
III. T HE T EMPORAL O PTIMIZATION modified trajectory.
A. Concept
In the previous section, a collision-free trajectory with
Robot
fixed time step is optimized in the feasible set. However, the
operational time of this trajectory has not been optimized yet. Kinect
In order to achieve the time optimality, the time step should
be considered as a variable as well. For example, suppose
there is a N -step trajectory planning problem as the green
line shown in Fig. 6. Since thePsampling time dt is fixed, the
N Path
operational time is given by k=1 dt = N · dt. On the other
hand, the same trajectory planning problem with variable
time step is shown as the orange line in Fig. 6. Its operational
time
PN is determined by the summation of time steps, i.e.
k=1 τk . In short, the idea of the temporal optimization is
to penalize the time variables over the horizon of the defined Obstacles
path.
B. Problem Formulation Fig. 7. The experimental setup, where the robot is a FANUC LR Mate
Denote that the time variable at time step t as τk ∈ R+ , 200iD/7L, the Microsoft Kinect is used to detect the pink and black
T obstacles, and the red line represents the reference path.
and τ = [τ1 , τ2 , · · · , τN ] ∈ RN + is denoted as the time
profiles over the horizon. Considering the smoothness of the acceleration with a positive definite matrix S. Note that
the trajectory in the defined path, the acceleration should the cost function can be decoupled as JT (τ , a) = JT 1 (τ ) +
be limited, i.e. −amax ≤ at ≤ amax , where at , amax ∈ JT 2 (a), and the minimum of the second term is achieved
A ⊂ Rm are denoted as the acceleration and the acceleration at a = 0. Furthermore, GT is affine with respect to a. For
bound. Suppose the initial velocity and the final velocity are example, for t = 2, · · · , N − 1, (7) can be reformulated as,
given as v0 , vN ∈ Rm respectively, then acceleration are
computed by τt (xt − xt−1 ) − τt−1 (xt+1 − xt ) + τt2 τt−1 at = 0 (9)
| {z } | {z }
FTt (τt ) t (τ )
   HT
1 x1 −x0 t

 τ τ − v 0 t=1
 1 1
  where FTt (τ ) ∈ R → Rm and HTt (τt ) ∈ R → Rm×m .
at = τ1t xt+1τt−xt − xtτ−x t−1
t = 2, · · · , N − 1 (7)
  t−1 Therefore, this problem has the same geometric features
x −x
 1 −1
τ N vN −

 N
τN
N
t=N as (2). Hence, (8) can be solved by CFS.
In the fast robot motion planning framework, both the
 TDenote the acceleration profile in the horizon by a = trajectory planning and the temporal optimization can be
T T
T
 N
a1 , a2 , · · · , aN ∈ A = A. a and τ define a nonlinear translated to CFS-solvable problems, which are formulated
dynamics, i.e. GT (τ , a) = 0. Then, the temporal optimiza- as several QP subproblems and solved iteratively. This results
tion problem can be formulated as in a significant reduction of the computation time, comparing
min JT (τ , a) = kτ k1 + kak2S (8a) to the conventional motion planning methods.
τ ,a
IV. E XPERIMENT
s.t. τ > 0, a ∈ A (8b)
The simulation and the experiment were performed on the
GT (τ , a) = 0 (8c)
experimental setup shown in Fig. 7. The robot was a FANUC
where JT is designed as the convex and smooth cost PNfunc- LR Mate 200iD/7L, and the Microsoft Kinect was used to
tion of the temporal optimization, where kτ k1 = t=1 τt detect the pink and black obstacles. The red line represented
penalizes the operational time, and kak2S = aT Sa penalizes the reference path that the robot followed. To validate the

32
Fig. 8. The reference path that robot plan to move, where the red dots are Fig. 9. The simulation result of the trajectory planning with SQP and CFS.
the way points.

CFS Trajectory without Temporal Optimization

1

performance of FRMP, we compared the proposed algorithm 0.5

J1
with the benchmark algorithms, SQP, where both algorithms 0 J2

Pos (rad)
were implemented in MATLAB on a Windows desktop with -0.5
J3
J4
a Intel Core i5 CPU and 16GB RAM. The robot controller J5
-1
was deployed on a Simulink RealTime target. J6

-1.5
The robot reference path was shown in Fig. 8. The red
points were the way points that the robot needed to pass and -2
0 5 10 15
stop by. The cyan lines represented the desired path, and Time (sec)

path segments were sequentially numbered. Table I showed CFS Trajectory with Temporal Optimization
the performance of both algorithms on the planning of a 1

trajectory with 95 steps. Paths 1,2, and 5 were collision- 0.5

J1
free, while Paths 3 and 4 were occupied by obstacles. On 0 J2
Pos (rad)

the collision-free paths, there were not significant difference -0.5

J3
J4
between two algorithms. On the blocked paths, the CFS al- J5
-1
gorithm exhibited much less computation time and iterations J6

than SQP to converge to local optima. This was because -1.5

SQP was developed for general purposes, where it was more -2

0 1 2 3 4 5 6 7
conservative the step size selection. On the other hand, CFS Time (sec)

considered the specific geometric structure of the trajectory

planning problem, and the computational efficiency was sig- Fig. 10. The CFS trajectories before and after the temporal optimization.
nificantly improved. The results of the simulation was shown
in Fig. 9, where the red line and the yellow line represented the reference trajectory, which required a motion planner
the SQP and CFS trajectory respectively. Although these to modify the path. In this scenario, CFS, ITP and SQP
algorithms converged to different solution, both of them were implemented in Knitro [15] in C++. In order to further
achieved the collision avoidance motion. analyze the performance among various methods, the same
We used the same path to evaluate the performance of problem was solved with different planning horizons, where
FRMP, which was the CFS trajectory planning with the the number of steps went from 10 to 100. Figure 13 and
temporal optimization. The computation time of FRMP was Fig. 14 show the total computation time and the computation
shown in Table II, where the average computation per time per iteration respectively, and Table IV provides the
path of the temporal optimization was 31 ms. Hence, the detailed numerical result. The computation time of SQP
temporal optimization would not become a burden in FRMP; was untraceable when the planning horizon increased to 80;
moreover, it could significantly reduce the operational time, hence, the corresponding results were not listed.
where the improvement by FRMP could be found in Table III In terms of total computation time, CFS always outper-
and Fig. 10. formed ITP and SQP, since it required less time per iteration
To better illustrate the advantage of CFS, we also im- and fewer iterations to converge. This was due to the fact that
plemented CFS in C++ and compared it with SQP and CFS did not require additional line search after solving (4) as
interior point (ITP) in a simplified 2D mobile robot motion was needed in ITP and SQP, hence saving time during each
planning problem. As shown in Fig. 12, a mobile robot iteration. CFS required fewer iterations to converge since it
started at (0, 0) and tried to follow the straight purple dash could take unconstrained step length ||z(k+1) − z(k) || in the
reference trajectory. However, the green obstacle blocked convex feasible set. Moreover, CFS scaled much better than

33
 (a)

(b)
Fig. 11. The sequential of the figures shows the robot motion in the experiment, where the executive motion of the robot planned by FRMP is shown as
the orange line (a) The robot avoids the pink obstacle. (b) The robot avoids the black obstacle.

2 TABLE II
T HE COMPUTATION TIME OF FRMP
1
0 Fast Robot Motion Planner (FRMP)
unit: sec Path Planning Temporal Optimization
−1
Path 1 0.0033 0.0279
−2 Path 2 0.0033 0.0272
0 2 4 6 8 10 Path 3 0.8938 0.0272
Path 4 0.8397 0.0485
Fig. 12. A 2D mobile robot motion planning problem. Path 5 0.0031 0.0252
TABLE I Total 1.7434 0.1561
T HE COMPARISON OF SQP AND CFS
TABLE III
SQP CFS T HE COMPARISON OF BEFORE AND AFTER TEMPORAL OPTIMIZATION
Total Horizon: 95
unit: sec Computation Iterations Computation Iterations unit: sec before after
Path 1 0.0043 0 0.0033 0 Operational Time 15.00 6.63
Path 2 0.0031 0 0.0033 0 Computation Time 1.74 1.90
Path 3 46.8098 94 0.8938 2
Path 4 48.8767 96 0.8397 5
Path 5 0.0032 0 0.0031 0
Total 95.725 190 1.7434 7 convex feasible set (CFS) algorithm.
In the trajectory planning problem, the CFS algorithm was
efficient because we explicitly exploits the unique geometric
ITP and SQP, as the computation time and time per iteration structure of the problem by relaxation and convexification.
in CFS went up almost linearly with respect to number of One consequence was that the steps size of the algorithm was
steps (or the number of variables). unconstrained, hence the number of iterations was greatly
reduced. Another consequence was that we did not need
V. C ONCLUSION
to do line search in CFS, hence the computation time
This paper proposed the fast robot motion planner (FRMP) during each iteration also decreased. Most importantly, as
by formulating trajectory planning and temporal optimization we directly searched in the feasible set, the solution was
as two optimization problems and solving them by the good enough even before convergence. Good-enough meant

34
 TABLE IV
C OMPUTATION C OMPARISON AMONG DIFFERENT ALGORITHMS IN C++

No. Iteration Total Computation Time Computation Per Iteration

No. step
CFS ITP SQP CFS ITP SQP CFS ITP SQP
10 5 42 86 4.74 8.53 201.41 0.95 0.20 2.34
20 4 46 366 4.80 17.37 1108.02 1.20 0.38 3.03
30 7 61 734 8.45 32.84 3164.40 1.21 0.54 4.31
40 4 87 1534 6.57 61.16 6717.45 1.64 0.70 4.38
50 4 139 1907 7.37 199.96 9633.93 1.84 1.44 5.05
60 4 140 3130 8.58 329.75 17123.95 2.14 2.36 5.47
70 4 165 2131 8.78 309.09 12546.80 2.19 1.87 5.89
80 4 169 9.96 307.55 2.49 1.82
90 4 223 11.01 838.63 2.75 3.76
100 4 253 11.80 1500.96 2.95 5.93
1,000 1,500

scenario. In future, FRMP will be applied to more practical

CFS ITP SQP scenarios.
Time [ms]

ACKNOWLEDGMENT
The work is supported by National Science Foundation
500

under Grant No. 1734109.

R EFERENCES
0

[1] S. M. LaValle and J. J. Kuffner Jr, “Randomized kinodynamic plan-

20 40 60 80 100
ning,” The international journal of robotics research, vol. 20, no. 5,
Number of Sampled Points on the Path pp. 378–400, 2001.
[2] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Prob-
Fig. 13. Total computational time in the 2D motion planning problem abilistic roadmaps for path planning in high-dimensional configuration
spaces,” IEEE transactions on Robotics and Automation, vol. 12, no. 4,
pp. 566–580, 1996.
6

[3] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradi-

ent optimization techniques for efficient motion planning,” in Robotics
and Automation, 2009. ICRA’09. IEEE International Conference on.
IEEE, 2009, pp. 489–494.
Time [ms]
4

CFS ITP SQP [4] C. Park, J. Pan, and D. Manocha, “Itomp: Incremental trajectory
optimization for real-time replanning in dynamic environments.” in
ICAPS, 2012.
2

[5] D. Costantinescu and E. Croft, “Smooth and time-optimal trajectory

planning for industrial manipulators along specified paths,” Journal of
robotic systems, vol. 17, no. 5, pp. 233–249, 2000.
0

20 40 60 80 100 [6] P. Reynoso-Mora, W. Chen, and M. Tomizuka, “A convex relaxation

for the time-optimal trajectory planning of robotic manipulators along
Number of Sampled Points on the Path
predetermined geometric paths,” Optimal Control Applications and
Methods, 2016.
Fig. 14. The computational time per iteration in the 2D motion planning [7] E. F. Camacho and C. B. Alba, Model predictive control. Springer
problem Science & Business Media, 2013.
[8] P. Spellucci, “A new technique for inconsistent qp problems in the
sqp method,” Mathematical Methods of Operations Research, vol. 47,
feasible and safe. Hence we could safely stop the iteration no. 3, pp. 355–400, 1998.
before convergence and execute the suboptimal trajectory. [9] J. Nocedal and S. J. Wright, Sequential quadratic programming.
In temporal optimization problem, time variables were Springer, 2006.
[10] T. A. Johansen, T. I. Fossen, and S. P. Berge, “Constrained nonlinear
introduced to achieve the time optimality on a defined path. control allocation with singularity avoidance using sequential quadratic
The problem was also formulated to be a CFS-solvable programming,” IEEE Transactions on Control Systems Technology,
problem. It was shown by the experimental that the average vol. 12, no. 1, pp. 211–216, 2004.
[11] J. Schulman, J. Ho, A. X. Lee, I. Awwal, H. Bradlow, and P. Abbeel,
computation time of the temporal optimization only took 31 “Finding locally optimal, collision-free trajectories with sequential
ms, and the operational time was reduced from 15 second convex optimization.” in Robotics: science and systems, vol. 9, no. 1.
to 6.6 second. The experiment demonstrated that FRMP can Citeseer, 2013, pp. 1–10.
[12] C. Liu, C.-Y. Lin, and M. Tomizuka, “The convex feasible set algo-
plan a time-optimal trajectory on a 95th-step horizon within rithm for real time optimization in motion planning,” SIAM Journal
2 second. on Control and Optimization, vol. 56, no. 4, pp. 2712–2733, 2018.
There are several directions to further improve this work [13] C. Liu and M. Tomizuka, “Real time trajectory optimization for
nonlinear robotic systems: Relaxation and convexification,” Systems
in the future. In this work, we only consider the kinematic & Control Letters, vol. 108, pp. 56–63, 2017.
level problems such as acceleration-bounded, collision-free; [14] C. Liu, C.-Y. Lin, Y. Wang, and M. Tomizuka, “Convex feasible set
however, the actual physical limits result from the system algorithm for constrained trajectory smoothing,” in American Control
Conference (ACC), 2017. IEEE, 2017, pp. 4177–4182.
dynamics. Hence, the torque limit and the jerk limit will [15] R. H. Byrd, J. Nocedal, and R. A. Waltz, “Knitro: An integrated pack-
be deployed to the planning problem as well. Also, the age for nonlinear optimization,” in Large-scale nonlinear optimization.
experimental validation only performed in a preliminary Springer, 2006, pp. 35–59.

35