







































INTERNATIONAL JOURNAL OF COMPUTERS COMMUNICATIONS & CONTROL
ISSN 1841-9836, 10(1):123-135, February, 2015.

Tractable Algorithm for Robust Time-Optimal Trajectory
Planning of Robotic Manipulators under Confined Torque

Q. Zhang, S.-R. Li, J.-X. Guo, X.-S. Gao

Qiang Zhang, Shu-Rong Li
Automation Department
China University of Petroleum(East China)
E-mail:zhangqiangupc@gmail.com, lishuron@upc.edu.cn

Jian-Xin Guo, Xiao-Shan Gao*
Academy of Mathematics and Systems Science
Chinese Academy of Sciences
E-mail:guojianxin@amss.ac.cn
*Corresponding author: xgao@mmrc.iss.ac.cn

Abstract: In this paper, the problem of time optimal trajectory planning under
confined torque and uncertain dynamics and torque parameters along a predefined
geometric path is considered. It is shown that the robust optimal solution to such a
problem can be obtained by solving a linear program. Thus a tractable algorithm is
given for robust time-optimal path-tracking control under confined torque.
Keywords: robust optimal control, time minimum trajectory planning, parameter
uncertainties, tractable algorithm.

1 Introduction

In order to maximize productivity, the problem of minimum time motion planning of robotic
manipulators is widely studied. In practical applications, the motion planning problem can be
solved by two kinds of methods: the coupled method and the decoupled method [1]. As the
name implies, the coupled methods determine the geometric path and the velocity along the
path simultaneously. On the other hand, the decoupled methods determine the geometric path
first according to certain criteria, and then determine the velocity along the path to minimize
the path traversal time. This paper follows the decoupled approach due to the following reasons.
Simultaneous optimization of the geometric path and the velocity is computationally difficult [2],
especially when one wants to find a tractable algorithm. Also, in many industrial applications,
the geometric path is predefined and does not need to be optimized.

Many efficient approaches for the time minimum trajectory planning (TMTP) problem along
a predefined geometric path have been proposed, including the phase plane analysis approaches
[3–6], the Pontryagin maximum principle and shooting algorithms [7], the convex optimiza-
tion [2, 10], the path reshaping [8], and the direct search method [9]. The recent works [2, 11]
are significant in which general and efficient methods are proposed. In [2], the second-order cone
program is applied to solve the corresponding convex optimization problem. In [11], the sequen-
tial linear programming is used to solve the TMTP, which has better computational complexity
than that of the phase plane method given in [4].

The above approaches assume that the dynamics model of the robotic manipulator is precisely
known and equal to some nominal values. In practice, the dynamic model always has uncertain-
ties. For example, to simplify the solution procedure, the viscous friction is often neglected and
the uncertainties caused by the payload variation are often ignored. Many robust trajectory plan-
ning methods were proposed to handle these uncertainties. One of the commonly used approach
is the min-max optimization approach which obtains the time minimum solution in the case of
worst disturbances [12–14]. Due to its bi-level structure, min-max optimization problems pose

Copyright © 2006-2015 by CCC Publications



124 Q. Zhang, S.-R. Li, J.-X. Guo, X.-S. Gao

challenges for efficient numerical solution in many cases. Other interesting approaches include
the stochastic optimization approach [15] and the predictive control [16, 17].

In this paper, a tractable offline algorithm is given for the robust TMTP of robotic manipula-
tors under torque limits and bounded dynamic parameter uncertainties and torque disturbances
based on the min-max optimization approach. To solve the problem, it is shown that the TMTP
problem for robotics under confined torque is equivalent to a linear optimal control problem
meaning that both the objective function and the constraints are linear in the control and state
variables. When disturbances are added, a robust linear optimal control problem is obtained,
which is formulated as a min-max optimization problem. Furthermore, it is shown that when the
trajectory is approximated with a quadratic B-spline curve, the min-max optimization problem
is reduced to a min-max linear optimization problem which in turn can be reduced to a plain
linear programming problem and thus is tractable, meaning that the problem can be solved in
polynomial time. Numerical simulations are used to show that the method gives high precision
approximate solutions to the robust TMTP very efficiently.

Comparing to existing approaches [12–14], the proposed method seems to be the first to
convert the robust TMTP for robotics into a linear program. Linear program was also used
in [12], where the objective function is not equivalent to time-minimality.

The rest of the paper is organized as follows. In Section 2, the nominal TMTP problem
is reduced to a linear control problem. In Section 3, methods of parametrization is given. In
Section 4, a tractable algorithm for the robust TMTP problem is given. In Section 5, numerical
examples are presented. Conclusions are presented in Section 6.

2 Linear Formulation for TMTP under Confined Torque

In this section, we will show that the TMTP problem can be formulated as a linear optimal
control problem meaning that the objective function and the constraints of the problem are linear
in the control and state variables.

Consider an n DOF robotic manipulator satisfying the following dynamic equation [2]

τ = M (q) q̈+q̇T C (q) q̇+G (q) , (1)

where τ ∈ Rn is the vector of actuator torques, q ∈ Rn is the vector of joint angular positions,
M (q) ∈ Rn×n is the inertia matrix of the manipulator, C (q) ∈ (Rn)n×n is a matrix, whose
elements are in Rn, representing the coefficients of the centrifugal and Coriolis forces, and G (q) ∈
Rn is the vector of gravitational torques.

In this paper, the given path to be tracked in task space is denoted by

r (s) = [x(s) ,y (s) ,z (s)]
T
,s ∈ [0,1] , (2)

and the corresponding joint path q (s) in the joint space is also assumed to be determined by
considering task requirement, obstacle avoidance, singularity avoidance, etc. Hence, a kinematics
map between r (s) and q (s) exists. Here the path singularity problem is not considered since it
can be avoided by properly adjusting the joint path.

Following [4], each element of r (s) is assumed to be a piecewise analytic real-valued function
and has at least C1 continuity at the connection points. Furthermore, r is assumed to be non-
singular, that is, for any s ∈ [0,1] there exists at least one l ∈ {x,y,z} such that dl(s)

ds
̸= 0.

Denote ′ = d
ds

and ˙ = d
dt

, where t represents time. Then the joint velocity and joint
acceleration can be formulated as

q̇ = q′ṡ, q̈ = q′s̈+q′′ṡ2. (3)



Tractable Algorithm for Robust Time-Optimal Trajectory Planning of Robotic Manipulators
under Confined Torque 125

Following (1) and (3), the dynamics model can be written as a linear function in s̈ and ṡ2:

τ = m (s) s̈+c (s) ṡ2+g (s) , (4)

where m (s) = M (q (s)) q′ (s) ∈ Rn,c (s) = M (q (s)) q′′ (s) +q′(s)T C (q (s)) q′ (s) ∈ Rn,
g (s) = G (q (s)) ∈ Rn.

The TMTP problem can be described as the following optimal control problem [2]:

min
b(s)

JT =
∫ 1
0

1√
a(s)

ds

s.t. a′ (s) = 2b(s) , ∀s ∈ [0,1],
a(s) > 0, ∀s ∈ (0,1) ,a(0) = a0,a(1) = a1,
m (s)b+c (s)a+g (s) = τ (s) ,

τmin ≤ τ (s) ≤ τmax,

(5)

where a = ṡ2, b = s̈, τmin and τmax are the bound vectors for the torque, and the boundary
values a(0) and a(1) can be obtained from (3) and the boundary values of the joint velocities:
q̇ (0) = q̇0, q̇ (1) = q̇f. When b(s) = s̈ is known, ṡ and q(t) can be uniquely determined.

Problem (5) has the following important property.

Theorem 1. The optimal solution ṡo(s),s ∈ [0,1] of problem (5) is unique and is maximum at
any parameter value. That is, let ṡf(s) be another parameter velocity satisfying the constraints
of (5). Then, ṡf(s) ≤ ṡo(s) for all s ∈ [0,1].

Theorem 1 follows from the proof of Theorem 2 in [3] and the proof of Theorem 3 in [4]. In
both cases, an admissible parameter velocity ṡo(s),s ∈ [0,1] is first constructed. To show ṡo(s) is
a solution to problem (5), the authors actually proved that ṡo(s) is maximum at any parameter
value.

The constraints of problem (5) are linear in a and b. It will be shown that the nonlinear
objective function of problem (5) can be replaced with a linear function in a. The idea is to
maximize the total feedrate for the end effector of the robots instead of minimizing the traversing
time. The feedrate of the end effector of the robot is Fd (s) = ∥r′ (s)∥

√
a(s), where ∥r′ (s)∥ =√

x′2 (s) + y′2 (s) + z′2 (s).
Consider the optimal control problem.

max
b(s)

JF =
∫ 1
0
∥r′ (s)∥2a(s)ds

s.t. a′ (s) = 2b(s) , ∀s ∈ [0,1],
a(s) > 0, ∀s ∈ (0,1) ,a(0) = a0,a(1) = a1,
m (s)b+c (s)a+g (s) = τ (s) ,

τmin ≤ τ (s) ≤ τmax.

(6)

Since the objective function of the above problem is linear in a(s), problem (6) is a linear
optimal control problem meaning that both the objective function and the constraints are linear
in a and b. We thus have

Theorem 2. Problem (6) and problem (5) have the same unique optimal solution.

Proof: From [3] and [4], problem (5) has an optimal solution ao(s) = ṡo(s)2,s ∈ [0,1]. Since the
two problems have the same constraints, ao(s) is also an admissible pseudo feedrate to problem
(6). From Theorem 1, ao(s) achieves maximum values among all a(s) satisfying the constraints
of (6) at any parametric value. Since the parametric path r(s) is assumed to be non-singular,
∥r′(s)∥2 > 0 for all s ∈ (0,1). Thus ao(s) is also the optimal solution to problem (6). 2



126 Q. Zhang, S.-R. Li, J.-X. Guo, X.-S. Gao

3 Parametrization and Tractable Algorithm for Nominal TMTP

In this section, a method of parametrization is given to reduce the infinite dimensional optimal
control problem (6) to a linear program. The parametrization method will also be used in the
next section to solve the robust TMTP.

The first step is to use a special type quadratic B-spline function to approximate the function
a(s).

We first define the B-splines used in this paper. Let K and p be positive integers and
si =

i
K−p+2, i = 0, . . . ,K − p + 2. Consider the following knot sequence with the first p + 1 and

last p + 1 knots “clamped".

s =


0, . . . ,0︸ ︷︷ ︸

p+1

,s1, . . . ,si, . . . ,sK−p+1,1, . . . ,1︸ ︷︷ ︸
p+1


. (7)

The B-spline curve of degree p in the parameter interval [0,1] is defined as

ã(s) =

K+1∑
i=0

Ni,p (s)âi, (8)

where âi are parameters and Ni,p (s) is the i-th spline basis function of degree p defined as

Ni,0 (s) =

{
1 if s(i) ≤ s < s(i + 1)
0 otherwise

, i = 0, . . . ,K + p + 1

Ni,k (s) =
s − s(i)

s(i + k) − s(i)
Ni,k−1 (s) +

s(i + k + 1) − s
s(i + k + 1) − s(i + 1)

Ni+1,k−1 (s) ,

k = 1, . . . ,p;i = 0, . . . ,K + p − k + 1.

The derivative of the B-spline basis function is given below

N ′i,p (s) =
d

ds
Ni,p (s) =

p

s(i + p) − s(i)
Ni,p−1 (s) −

p

s(i + p + 1) − s(i + 1)
Ni+1,p−1 (s) . (9)

Now, function a(s) is approximated by a quadratic B-spline function a(s) ≈ ã(s) where ã(s)
is from (8) for p = 2. Set â0 = a0, âK+1 = a1. Due to the multiple knots in the B-splines,
we have have ã(0) = â0 and ã(1) = âK+1 [18]. The control points X = [â1, . . . , âK]T will be
optimized. The variable b(s) is

b(s) = a′ (s)/2 ≈
1

2

K+1∑
i=0

N ′i,p (s)âi, (10)

where N ′i,p (s) is given in (9).
The second step is to reduce the continuous optimal control problem (6) to a finite state

optimal problem by using pointwise constraints to replace the continuous torque constraints.
The constraint knot points are

s̄j =




1
2
s1,j = 1

1
2
(sj−1+sj) ,j = 2, . . . ,K − 1

1
2
(sK−1+1) ,j = K

(11)

With these knot points, the objective function can be approximated as

JF =

∫ 1
0

∥r′ (s)∥2a(s)ds ≈ BX (12)



Tractable Algorithm for Robust Time-Optimal Trajectory Planning of Robotic Manipulators
under Confined Torque 127

where B = (B1, . . . ,BK) with Bi =
K∑
j=1

∥r′ (s̄j)∥2Ni,p (s̄j) ∆s̄j for ∆s̄j = sj − sj−1 = 1/K.

From (4), (8), and (10), the torques of the l-th joint at the knot points are

τ̂l = AlX + ĝl, l = 1, . . . ,n, (13)

where τ̂l = [τl (s̄1) , . . . ,τl (s̄K)]
T with τl (s̄j) = 12ml (s̄j)

K+1∑
i=0

N ′i,p (s̄j) âi +cl (s̄j)
K+1∑
i=0

Ni,p (s̄j)

âi + gl (s̄j), Al ∈ RK×K with Alji = 12ml (s̄j)N
′
i,p (s̄j) + cl (s̄j)Ni,p (s̄j), and ĝl = [ gl (s̄1) +

ξ1, . . . , gl (s̄K) + ξK]T with ξj = 12ml (s̄j) (N
′
0,p (s̄j)a0 + N

′
K+1,p (s̄j)a1) + cl (s̄j) (N0,p (s̄j) a0 +

NK+1,p (s̄j)a1).
With the above parametrization, problem (6) is reduced to the following linear program

max
X

BX

s.t. ã(s̄j) = NjX + N0,p(s̄j)a0 + NK+1,p(s̄j)a1 > 0,j = 1, . . . ,K,

τl,minI ≤ AlX + ĝl ≤ τl,maxI, l= 1, . . . ,n,
(14)

where I = (1, . . . ,1)T ∈ RK, B is from (12), Nj = [N1,p(s̄j), . . . ,NK,p(s̄j)], and Al, ĝl are from
(13). Note that the boundary conditions a(0) = â0 = a0 and a(1) = âK+1 = a1 are automatically
satisfied. Properties of the above problem will be discussed in the next section.

4 Tractable Algorithm for Robust TMTP

In this section, we will show that the robust TMTP can be reduced to a linear program by
using proper parametrization and methods of robust optimization.

Suppose that the dynamics model (4) has uncertainties and the real dynamics model is

τ (s) = (m (s) + ∆m (s)) s̈+ (c (s) + ∆c (s)) ṡ2 + (g (s) + ∆g (s)) − ∆d (s) , (15)

where ∆m (s) ,∆c (s) ,∆g (s) , and ∆d (s) are the disturbances for m (s) ,c (s) ,g (s), and the
torque, respectively. The disturbances are assumed to satisfy the following constraints:

D(∆m,∆c,∆g,∆d) :




|∆ml (s)| ≤ Bm (l,s) , l = 1, . . . ,n
|∆cl (s)| ≤ Bc (l,s) , l = 1, . . . ,n
|∆gl (s)| ≤ Bg (l,s) , l = 1, . . . ,n
|∆dl (s)| ≤ Bd (l,s) , l = 1, . . . ,n


 (16)

where ∆ml (s) ,∆cl (s) ,∆gl (s), and ∆dl (s) are the elements of the vectors ∆m (s) ,∆c (s) ,∆g (s),
and ∆d (s), respectively.

Using the min-max approach [12, 13], the robust TMTP can be formulated as the following
bi-level optimization problem:

max
b(s)

JF =

∫ 1
0

∥r′ (s)∥2a(s)ds (17)

s.t. a′ (s) = 2b(s) ,a(s) > 0,s ∈ (0,1), a(0) = a0, a(1) = a1,
max

D(∆m,∆c,∆g,∆d)

(
(m (s) + ∆m (s))b(s)+ (c (s) + ∆c (s))a(s)+ (g (s) + ∆g (s)) − ∆d (s)

)
≤ τmax

min
D(∆m,∆c,∆g,∆d)

(
(m (s) + ∆m (s))b(s)+ (c (s) + ∆c (s))a(s)+ (g (s) + ∆g (s)) − ∆d (s)

)
≥ τmin

where D(∆m,∆c,∆g,∆d) is defined in (16).



128 Q. Zhang, S.-R. Li, J.-X. Guo, X.-S. Gao

Using the parametrization procedure presented in Section 3, the robust TMTP problem (17)
is reduced to the following bi-level optimization problem

max
X

BX

s.t. NjX + N0,p(s̄j)a0 + NK+1,p(s̄j)a1 > 0,j = 1, . . . ,K

max
∆Aml,∆Acl,∆gl,∆dl

AlX + ĝl + ∆AmlY + ∆AclX + ∆ĝl − ∆d̂l ≤ τl,maxI,

min
∆Aml,∆Acl,∆gl,∆dl

AlX + ĝl + ∆AmlY + ∆AclX + ∆ĝl − ∆d̂l ≥ τl,minI,

l = 1, . . . ,n,

(18)

where Y = [N′1, . . . ,N
′
K]

T
X is a newly defined variable with N′j =

[
N ′1,p (s̄j) , . . . ,N

′
K,p (s̄j)

]
,

B, Nj, Al, and ĝl are the same as in (14), ∆ĝl = [∆gl (s̄1) , . . . ,∆gl (s̄K)]
T is the disturbance of

the gravitational torque, ∆d̂l = [∆dl (s̄1) , . . . ,∆dl (s̄K)]
T is the torque disturbances, ∆Aml ∈

RK×K and ∆Acl ∈ RK×K are the uncertainty matrixes of the l-th joint with elements ∆Amlji =
1
2
∆ml (s̄j) and ∆Aclji = ∆cl (s̄j)Ni,p (s̄j) respectively. And the uncertainty constraints are

∆Aml : |∆ml(s̄j)| ≤ Bm (l, s̄j) , (19)
∆Acl : |∆cl(s̄j)| ≤ Bc (l, s̄j) , (20)
∆gl : |∆gl(s̄j)| ≤ Bg(l, s̄j),
∆dl : |∆dl(s̄j)| ≤ Bd(l, s̄j),j = 1, . . . ,K.

Based on a result of Soyster about robust optimization [19], the bi-level optimization problem
(18) can be relaxed to the following linear program

max
X,Z1,Z2

BX

s.t. NjX + N0,p(s̄j)a0 + NK+1,p(s̄j)a1 > 0,j = 1, . . . ,K,

AlX + ĝl + ∆BmlZ1 + ∆BclZ2 + ∆Bgl + ∆Bdl ≤ τl,maxI,
AlX + ĝl − ∆BmlZ1 − ∆BclZ2 − ∆Bgl − ∆Bdl ≥ τl,minI,
−Z2 ≤ X ≤ Z2,−Z1 ≤ Y ≤ Z1,Z1 ≥ 0,Z2 ≥ 0,
Y = [N′1, . . . ,N

′
K]

T
X,

l = 1, . . . ,n,

(21)

where Z1 ∈ RK and Z2 ∈ RK are new sets of variables, ∆Bml ∈ RK×K and ∆Bcl ∈ RK×K
are matrixes with elements ∆Bmlji = 12Bm (l, s̄j) and ∆Bclji = Ni,p (s̄j)Bc (l, s̄j) respectively,
∆Bgl = [Bg(l, s̄1), . . . ,Bg(l, s̄K)]

T , and ∆Bdl = [Bd(l, s̄1), . . . ,Bd(l, s̄K)]T .
The number of variables in problem (21) is 3K and the number of constraints is (2n + 7)K.

According to Karmarkar’s result [20], problem (21) can be solved with O((nK)3.5) floating point
arithmetic operations.

We briefly discuss the convergency of the method, that is, whether the solutions of the linear
programs (14) and (21) convert to the optimal solutions of the optimal control problems (6) and
(17), respectively, when K becomes sufficiently large. From the approximation theory, quadratic
splines can approximate the solution of optimal control problems to any given precision when
K is large enough [21]. Notice that the method of using quadratic splines to approximate the
control variable is a special case of the Ritz method, the convergence of which has been proved
only for special types of constraints [21–23]. Convergency for problems like the one in this paper
seems open. In the next section, numerical results will be used to demonstrate that the method
gives the optimal solution with very high precision in less than one second.



Tractable Algorithm for Robust Time-Optimal Trajectory Planning of Robotic Manipulators
under Confined Torque 129

5 Numerical Examples

The experimental robot is a six-DOF Puma 560 manipulator which is modeled using the
Robotics Toolbox for Matlab [24]. The torque limits of the six joints are set to be [97.6; 186.4;
89.4; 24.2; 20.1; 21.3]N.m. Initial and end feedrates of the end effecter are zeros. The path to be
traced in the robot work space is shown in Fig.1(a) with the following parametric formula




x(s) = 0.5,

y (s) = 0.25
(
cos (2aπs) − cos (2bπs)3

)
,

z (s) = 0.25
(
sin (2aπs) − sin (2bπs)3

)
,

s ∈ [0,1] .

where a = 2,b = 1. The corresponding joint path is shown in Fig.1(b).

−0.4 −0.2 0 0.2 0.4 0.6
−0.4

−0.2

0

0.2

0.4

Y(m)

Z
(m

)

Start Point

(a)

0 200 400 600 800 1000
−1.5

−1

−0.5

0

0.5

1

1.5

Path parameter(−)

D
is

pl
ac

em
en

t 
(r

ad
)

 

 
joint 1
joint 2
joint 3
joint 4
joint 5
joint 6

(b)

Figure 1: (a): The task path on Y-Z plane. (b): The corresponding joint path.

First, problem (14) is used to show that the proposed method is efficient and correct. The
proposed linear program (LP) approach will be compared to the convex optimization (CP) ap-
proach [2] for efficiency. The approach in [2], which is realized by executing the second order
cone program (SOCP), was the fastest existing numerical method by now. Both algorithms are
implemented by using optimization software package Sedumi [25] in Matlab on a PC with a
2.6GHz CPU and 2G memory.

Table 1: Comparison of computation times for different K

Number K
CPU time(s) Optimal motion time(s)

LP CP in [2] LP CP in [2]
100 0.3964 1.2460 1.6473 1.6449
200 0.6830 2.2970 1.6495 1.6491
500 0.4630 3.6260 1.6506 1.6506
700 0.9048 4.2620 1.6508 1.6507
1000 0.9300 3.4430 1.6508 1.6508
2000 1.1960 6.3440 1.6508 1.6508



130 Q. Zhang, S.-R. Li, J.-X. Guo, X.-S. Gao

0 0.2 0.4 0.6 0.8
0

0.2

0.4

0.6

0.8

1

Path parameter(−)

P
ar

am
et

er
 v

el
oc

it
y(

−
)

 

 

VLC

Nominal

Case1

Case2

Case3

Figure 2: Optimized parameter velocity curves of all four cases for K=1000.

0 0.2 0.4 0.6 0.8 1

−200

0

200

Jo
in

t 
to

rq
ue

(N
.m

)

 

 joint 1 joint 2 joint 3

0 0.2 0.4 0.6 0.8 1

−20

0

20

Path parameter(−)

Jo
in

t 
to

rq
ue

(N
.m

)

 

 
joint 4 joint 5 joint 6

Figure 3: Joint torques of the planned nominal
minimum time motion for K=1000.

0 0.2 0.4 0.6 0.8 1

−200

0

200
Jo

in
t 

to
rq

ue
(N

.m
)

 

 joint 1 joint 2 joint 3

0 0.2 0.4 0.6 0.8 1

−20

0

20

Path parameter(−)

Jo
in

t 
to

rq
ue

(N
.m

)

 

 
joint 4 joint 5 joint 6

Figure 4: Joint torques of robust minimum
time motion in case 1 for K=1000.

In Table 1, the computation time and the robot motion time are given for different values of K.
From Table 1, we can see that the optimal motion times based on formulation (6) and formulation
(5) are almost the same, which is consistent with Theorem 2. However, the computation time of
our approach is faster than that of [2]. Also note that the actually computational complexity of
our approach is sub-linear, meaning less than O(K), which might be due to the special structure
of Al in (14). The practical computational complexity of this approach is better than the
complexity O(K2) of the method given in [11].

The planned joint torques are given in Fig. 3. From [3], the optimal solution of problem (5)
is bang-bang-singular in the sense that for any s ∈ [0,1], the torque of at least one joint reaches
its boundary values in the constraint τmin ≤ τ (s) ≤ τmax. This property is often used to verify
whether a numerical solution is the optimal solution and whether K is large enough. In Fig.
3, the torque is clearly bang-bang-singular: the torques of joints 2, 1, 2, 1, and 2 reach their
maximal or minimal values in the intervals [0,0.17], [0.17,0.35], [0.35,0.41], [0.41,0.78], [0.78,1],
respectively. This indicates that for K = 1000, the solution obtained with our method most
probably gives high precision approximation to the optimal solution of the original problem (5).

Now, it will be shown that the solution of problem (21) is indeed robust. Suppose that the
payload holden by the end effector is the only uncertain parameter. As mentioned in [24], the
maximum payload capability of the Puma 560 is 2.5 kg. In this section, the robust trajectory



Tractable Algorithm for Robust Time-Optimal Trajectory Planning of Robotic Manipulators
under Confined Torque 131

0 0.2 0.4 0.6 0.8 1

−200

0

200

Jo
in

t 
to

rq
ue

(N
.m

)

 

 joint 1 joint 2 joint 3

0 0.2 0.4 0.6 0.8 1

−20

0

20

Path parameter(−)

Jo
in

t 
to

rq
ue

(N
.m

)

 

 
joint 4 joint 5 joint 6

Figure 5: Joint torques of robust minimum
time motion in case 2 for K=1000.

0 0.2 0.4 0.6 0.8 1

−200

0

200

Jo
in

t 
to

rq
ue

 (
N

.m
)

 

 joint 1 joint 2 joint 3

0 0.2 0.4 0.6 0.8 1

−20

0

20

Path parameter(−)

Jo
in

t 
to

rq
ue

 (
N

.m
)

 

 
joint 4 joint 5 joint 6

Figure 6: Joint torques of robust minimum
time motion in case 3 for K=1000.

planning is implemented under three cases of maximum payload uncertainty assumptions, re-
spectively. The assumptions are 0.5kg for case 1, 1.25kg for case2, and 2.5kg for case 3. The
disturbed ∆m, ∆c, and ∆g are estimated by using the dynamics computation function of the
Robotic Toolbox for Matlab.

The computation performance of the proposed robust trajectory planning algorithm is tested
by implementing case 2 under different values of K. The results are listed in Table 2. From this
table, we can see that the calculated optimal solution is convergent when K increases. Also in
this example, the actually computational complexity of our approach is less than O(K).

Table 2: Computation performance of robust algorithm for Case 2 with different K

Number (K)
100 200 500 700 1000 2000

CPU time 0.4330 0.7080 1.0720 1.1290 1.6920 2.6680
Optimal motion time 1.7355 1.7395 1.7407 1.7408 1.7409 1.7409

The planned joint torques of the robust minimum time motions in case 1, case 2, and case
3 are given in Fig. 4, Fig. 5, and 6, respectively. The optimized parameter velocities ṡ for all
four cases are shown in Fig. 2. Comparing to Fig. 3, the torques for the robust time-minimum
motion are reduced to reserve enough space to reject uncertainties and disturbances without
causing torque saturation.

To demonstrate the robustness of the obtained trajectories compared to the nominal one,
consider 10 equidistantly distributed mass values of payload in [0,2.5]kg. The planned feedrates
in the cases of Fig. 3, Fig. 4, Fig. 5, and Fig. 6 are used to traverse the path, and the
corresponding torques are given in Fig. 7, Fig. 8, Fig. 9, and Fig. 10, respectively.

Note that the torque bounds are [97.6; 186.4; 89.4; 24.2; 20.1; 21.3] N.m. From Fig. 7, Fig. 8
and Fig. 9, we can see that the actual torques pass the given bounds for about 25%, 19% and
11%, respectively. From Fig. 10, it can be seen that the actual torques are all within the given
bound, which is consistent with the fact that the robust trajectory in this case is planned under
the condition of maximum 2.5kg payload.

The robust trajectories for the three cases of uncertainty assumptions are used to traverse



132 Q. Zhang, S.-R. Li, J.-X. Guo, X.-S. Gao

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

−200

0

200

S
im

ul
at

ed
 j

oi
nt

 t
or

qu
e 

(N
.m

)

 

 joint 1 joint 2 joint 3

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

−20

0

20

Time (s)

S
im

ul
at

ed
 j

oi
nt

 t
or

qu
e 

(N
.m

)

 

 
joint 4 joint 5 joint 6

Figure 7: Joint torques for 10 difference pay-
load masses along the nominal reference tra-
jectory.

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

−200

0

200

S
im

ul
at

ed
 j

oi
nt

 t
or

qu
e 

(N
.m

)

 

 joint 1 joint 2 joint 3

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

−20

0

20

Time (s)

S
im

ul
at

ed
 

jo
in

t 
to

rq
ue

 (
N

.m
)

 

 
joint 4 joint 5 joint 6

Figure 8: Joint torques for 10 difference pay-
load masses along the robust reference trajec-
tory of case 1.

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

−200

0

200

S
im

ul
at

ed
 

jo
in

t 
to

rq
ue

 (
N

.m
)

 

 joint 1 joint 2 joint 3

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

−20

0

20

Time (s)

S
im

ul
at

ed
 j

oi
nt

 t
or

qu
e 

(N
.m

)

 

 
joint 4 joint 5 joint 6

Figure 9: Joint torques for 10 difference pay-
load masses along the robust reference trajec-
tory of case 2.

0 0.5 1 1.5

−200

0

200

S
im

ul
at

ed
 j

oi
nt

 t
or

qu
e 

(N
.m

)

 

 joint 1 joint 2 joint 3

0 0.5 1 1.5

−20

0

20

Time (s)

S
im

ul
at

ed
 j

oi
nt

 t
or

qu
e 

(N
.m

)

 

 
joint 4 joint 5 joint 6

Figure 10: Joint torques for 10 difference pay-
load masses along the robust reference trajec-
tory of case 3.

the path using the ten values of the payload. Detailed data of the experiments are given in Table
3, where Ctime stands for the computation time, Mtime stands for the optimal motion time,
RTI stands for the rate of time increase compared with the nominal case, MTV stands for the
maximum torque violation, ATV stand for the average torque violation, and RTV stands for the
rate of torque violation.

From Table 3, we can see that when the 2.5kg payload uncertainty assumption is used, along
the generated robust trajectory, the actual torques do not pass the given bounds. By contrast,
along the non robust nominal trajectories, at almost all times at least one actual torque passes
the given bound and the maximum torque violation reaches 25% of the given bound. The optimal
motion time of this robust trajectory increases about 10.7% compared with the nominal case,
which is a reasonable price to pay for robustness.



Tractable Algorithm for Robust Time-Optimal Trajectory Planning of Robotic Manipulators
under Confined Torque 133

Table 3: Results of trajectory robustness test for K=1000

Ctime(s) Mtime(s) RTI(%) MTV(N.m) ATV(N.m) RTV(%)
Nominal 1.4508 1.6508 0 46.39 3.99 99.16
Case 1 1.9810 1.6872 2.2 35.65 2.87 98.96
Case 2 1.6920 1.7409 5.5 21.04 1.75 82.35
Case 3 1.8600 1.8281 10.7 None None 0

6 Conclusions

In this paper, a linear program based robust time minimum trajectory planning approach is
presented. The robust version of the TMTP problem is constructed to overcome the parameter
uncertainties and torque disturbances of the dynamic system. To make our approach tractable,
the TMTP problem is proven to be equivalent to a linear optimal control problem by considering
the total motion feedrate of the end-effector of the robot as the objective function. Quadratic
B-spline curves are used to parameterize the linear optimal control problem into a linear program
which can be solved in polynomial time. The approach theoretically ensures that the generated
velocity is time optimal and for every possible realization of the uncertainties and disturbances
within the bound, the robust trajectory will not violate any joint torque constraints. Though
the proposed algorithm is designed to execute offline, online use is also possible because of the
good computation performance.

Acknowledgment

This research was partially supported by a National Key Basic Research Project of China
(2011CB302400), a grant from NSFC (60821002) and a grant from UPC (120501A).

Bibliography

[1] Katzschmann, R.; Kroger, T.; Asfour, T.; Khatib O.(2013); Towards Online Trajectory
Generation Considering Robot Dynamics and Torque Limits, in Intelligent Robots and
Systems (IROS), 2013 IEEE/RSJ International Conference on, ISSN 2153-0858, Tokyo,
5644 - 5651.

[2] Verscheure, D.; Demeulenaere, B.; Swevers, J.; De Schutter, J.; Diehl, M.(2009); Time-
optimal path tracking for robots: a convex optimization approach, IEEE Trans. on Auto-
matic Control, ISSN 0018-9286, 54(10): 2318-2327.

[3] Bobrow, J.E.; Dubowsky, S.; Gibson, J.(1985); Time-optimal control of robotic manipu-
lators along specified paths, International Journal of Robotics Research, ISSN 0278-3649,
4(3): 3-17.

[4] Shin, K.; McKay, N.(1985); Minimum-time control of robotic manipulators with geometric
path constraints, IEEE Trans. on Automatic Control, ISSN 0018-9286, 30(6): 531-541.

[5] Timar, S.D.; Farouki, R.T.(2007); Time-optimal traversal of curved paths by Carte-
sian CNC machines under both constant and speed-dependent axis acceleration bounds,
Robotics and Computer-Integrated Manufacturing, ISSN 0736-5845, 23(5): 563-579.



134 Q. Zhang, S.-R. Li, J.-X. Guo, X.-S. Gao

[6] Yuan, C.; Zhang, K.; Fan, W.(2013); Time-optimal Interpolation for CNC Machining
along Curved Tool Pathes with Confined Chord Error, Journal of Systems Science and
Complexity, ISSN 1559-7067, 26(5): 836-870.

[7] Chen, Y.; Desrochers, A.A.(1989); Structure of minimum-time control law for robotic
manipulators with constrained paths, in Robotics and Automation, IEEE International
Conference on, ISBN 0-8186-1938-4, Scottsdale, USA, 971-976.

[8] Guo, J.X.; Zhang, Q.; Gao, X.S.(2013); Tracking Error Reduction in CNC Machining by
Reshaping the Kinematic Trajectory, Journal of Systems Science and Complexity, ISSN
1559-7067, 26(5), 800-817.

[9] Zhang, K.; Yuan, C.M.; Gao, X.S.(2013); Efficient algorithm for feedrate planning and
smoothing with confined chord error and acceleration for each axis, The International
Journal of Advanced Manufacturing Technology, ISSN 0268-3768 , 66(9): 1685-1697.

[10] Ardeshiri, T.; Norrlof, M.; Lofberg, J.; Hansson, A.(2011); Convex optimization approach
for time-optimal path tracking of robots with speed dependent constraint, in Proceedings
of the 18th IFAC World Congress, ISSN 1474-6670, Milano, Italy, 14648-14653.

[11] Hauser, K.(2013); Fast Interpolation and Time-Optimization on Implicit Contact Subman-
ifolds, in Proceedings of Robotics: Science and Systems, ISSN 2330-765X, Berlin, Germany.

[12] Shin, K.G.; McKay, N.D.(1987); Robust trajectory planning for robotic manipulators under
payload uncertainties, IEEE Trans. on Automatic Control, ISSN 0018-9286, 32(12): 1044-
1054.

[13] Kieffer, J.; Cahill, A.J.; James, M.R.(1997); Robust and accurate time-optimal path-
tracking control for robot manipulators, IEEE Trans. on Robotics and Automation, ISSN
1042-296X, 13(6): 880–890.

[14] Diehl, M.; Gerhard, J.; Marquardt, W.; Monnigmann, M.(2008); Numerical solution ap-
proaches for robust nonlinear optimal control problems, Computers & Chemical Engineer-
ing, ISSN 0098-1354, 32(6): 1279-1292.

[15] Marti, K.; Aurnhammer, A.(2002); Robust optimal trajectory planning for robots by
stochastic optimization, Mathematical and Computer Modelling of Dynamical Systems,
ISSN 1744-5051, 8(1): 75-116.

[16] Chisci, L.; Rossiter, J.A.; Zappa, G.(2001); Systems with persistent disturbances: Predic-
tive control with restrictive constraints, Automatica, ISSN 0005-1098, 37(7): 1019-1028.

[17] Mayne, D.Q.; Seron, M.M.; Rakovic, S.V.(2005); Robust model predictive control of con-
strained linear systems with bounded disturbances, Automatica, ISSN 0005-1098, 41(2):
219-224.

[18] Patrikalakis, N.M.; Maekawa, T.(2010); Shape Interrogation for Computer Aided Design
and Manufacturing, ISBN 978-3-642-04074-0, Springer Berlin, Heidelberg.

[19] Bertsimas, D.; Brown, D.B.; Caramanis, C.(2011); Theory and applications of robust
optimization, SIAM Review, ISSN 0036-1445, 53(3): 464–501.

[20] Karmarkar, N.(1984); A new polynomial time algorithm for linear programming, Combi-
natorica, ISSN 0209-9683, 4(4): 373-395.



Tractable Algorithm for Robust Time-Optimal Trajectory Planning of Robotic Manipulators
under Confined Torque 135

[21] Sirisena, H.R.; Chou, F.S.(1979); Convergence of the control parameterization Ritz method
for nonlinear optimal control problems, Journal of Optimization Theory and Applications,
ISSN 0022-3239, 29(3): 369-382.

[22] Daniel, J.W.(1973); The Ritz-Galerkin method for abstract optimal control problems,
SIAM Journal on Control, ISSN 0036-1402, 11(1): 53-63.

[23] Schwartz, A.L.(1996); Theory and Implementation of Numerical Methods Based on Runge-
Kutta Integration for Solving Optimal Control Problems, Ph.D. Thesis, Univ. of California
at Berkeley.

[24] Corke, P.(1996); A robotics toolbox for MATLAB, IEEE Robotics and Automation Maga-
zine, ISSN 1070-9932, 3(1): 24-32.

[25] Sturm, J.F.(1999); Using SeDuMi 1.02, A Matlab toolbox for optimization over symmetric
cones, Optimization Methods and Software, ISSN 1055-6788, 11(1-4): 625-653.