An Efficient and Time-Optimal Trajectory Generation Approach forWaypoints under Kinematic Constraints and Error Bounds

Jianjie Lin, Nikhil Somani, Biao Hu, Markus Rickert, and Alois Knoll

Abstract— This paper presents an approach to generatethe time-optimal trajectory for a robot manipulator undercertain kinematic constraints such as joint position, velocity,acceleration, and jerk limits. This problem of generating atrajectory that takes the minimum time to pass throughspecified waypoints is formulated as a nonlinear constraintoptimization problem. Unlike prior approaches that model themotion of consecutive waypoints as a Cubic Spline, we modelthis motion with a seven-segment acceleration profile, as thistrajectory results in a shorter overall motion time while stayingwithin the bounds of the robot manipulator’s constraints. Theoptimization bottleneck lies in the complexity that increasesexponentially with the number of waypoints. To make theoptimization scale well with the number of waypoints, wepropose an approach that has linear complexity. This approachfirst divides all waypoints to consecutive batches, each withan overlap of two waypoints. The overlapping waypoints thenact as a bridge to concatenate the optimization results oftwo consecutive batches. The whole trajectory is effectivelyoptimized by successively optimizing every batch. We conductexperiments on practical scenarios and trajectories generatedby motion planners to evaluate the effectiveness of our proposedapproach over existing state-of-the-art approaches.

I. INTRODUCTION

In industrial applications, it is important to guarantee fastand accurate motions for robot manipulators to perform awell-defined task, such as assembly and welding. Limitingthe motion jerk is an essential requirement for avoidingthe manipulator’s mechanical resonances and improving thetrajectory accuracy. Once a task is defined, the robot istypically optimized for fast cycle times, therefore requiringa time-optimal trajectory with kinematic constraints. On theother side, the generated trajectory should also be as smoothas possible, especially for applications such as spray painting.

The most common motion profiles used during teach-in arepoint-to-point motions in configuration space, straight linesand circular motions in Cartesian space, and in a limitednumber of industrial controllers a spline interpolation inCartesian space. Controllers typically offer blending optionsin configuration or Cartesian space to smoothen the trajectoryand to avoid stops at each waypoint. With these differentmotion options, a programmer can impose constraints on thebehavior of the robot and its end effector, e.g., for followingan outline of a specific object. A robot with more degrees of

Jianjie Lin and Markus Rickert are with fortiss, An-Institut TechnischeUniversitat Munchen, Munich, Germany.

Nikhil Somani and Alois Knoll are with Robotics and Embedded Sys-tems, Department of Informatics, Technische Universitat Munchen, Munich,Germany.

Biao Hu is with College of Information Science and Technology, BeijingUniversity of Chemical Technology, Beijing, China.

freedom than required by this constraint is able to optimizeits motion according to other goals. Given an acceptabletolerance in following a straight line in Cartesian space, anoptimization algorithm can modify the trajectory even furtherto improve the cycle time of the process or the durability ofthe mechanical system.

With challenges arising from small lot sizes, where manualprogramming is no longer feasible, path planning algorithmscan be used to move from manual programming to anautomatic generation of robot motions. They consider ge-ometric information from the robot and its environment togenerate collision-free paths between specified start and goalconfigurations. This problem is already PSPACE-hard [1]when considering a state space solely based on positions andthat complexity increases for kinodynamic algorithms thatadd velocity information and generate a trajectory insteadof a path. The solution path is however only valid with thesame interpolation used during planning, typically a linearinterpolation in configuration space. Trajectory generationalgorithms based on the solution path have to performexpensive collision checking in order to prevent this. Anydeviation without an explicit upper bound error model in thecollision checking may result in a collision during executionof the motion and simply sending a list of waypoints toan industrial controller without a model of its trajectorygeneration can lead to undesired behavior.

This work presents a new approach to trajectory generationthat represents the trajectory between two adjacent waypointsin a different way and is able to explore the robot manipula-tor’s full potential in reducing the motion time. In the sameway as Haschke et al. [2] and Kroger et al. [3], the trajec-tory between two consecutive waypoints are generated withtrapezoidal acceleration profiles, which are also referred to asseven-segment acceleration profiles [4]. Compared to cubicsplines, seven-segment trajectories offer more optimizationpossibilities, while increasing the optimization complexity.

The main problem arises from an optimization complexitythat increases exponentially with the number of waypoints. Itwill become impossible to perform the optimization when thewaypoints exceed a certain amount. In this paper we developan approach that can efficiently reduce this problem to alinear complexity, thus making the optimization adaptableto any number of waypoints. This approach is inspired byModel Predictive Control [5], that predicts the system futurestate based on a formalized model. By decomposing allwaypoints into many consecutive batches, where each oneis bridged by two overlapping waypoints, the motion statesof these waypoints can be predicted and updated by the

optimization of two adjacent batches. In the end, the wholetrajectory is solved by successively optimizing every batch.On the other hand, the optimization performance is verysensitive to the initial point. We discuss in detail how to findan appropriate initial point that enables the optimal results tobe obtained in a short time and with a high success rate. Inaddition, the algorithm is extended to also consider trajectorybehavior in Cartesian space.

II. RELATED WORK

The problem of generating time-optimal and smooth tra-jectories has been studied extensively in previous work. Theproposed trajectory planning techniques can be generallycategorized into two categories: online real-time planningand offline planning. Online real-time trajectory planningtargets a dynamic and fast modification of the planned trajec-tories in case of unforeseen events. The online approachesoften rely on the manipulator’s current state and the goalstate to generate the motion trajectory. For example, byusing quintic splines, an online trajectory generation withjerk bound was shown in [6]. Besides, Haschke et al. [2]presented an online trajectory planner with an arbitrarystarting state and an end velocity of zero. Kroger et al. [3],[7] and Lange et al. [8] explored in more depth the onlinegeneration of trajectories for manipulators with arbitrary startand goal states. However, those online approaches are oftenlimited to two waypoints. For a complicated path with manyintermediate waypoints, it is often very difficult to applythese approaches for finding the time-optimal trajectory.

For a well-defined task, the offline trajectory planningaims at finding an optimal trajectory in space or time. Torepresent the trajectory, a polynomial curve is often used.Generally, the higher the degree of the polynomial curve, themore precise can the motion trajectory properties be tuned.A third-degree polynomial curve is necessary to providea limit on the jerk of the motion. In order to find theoptimal trajectory with a bounded jerk, cubic splines and B-splines are often used to represent the trajectory between twosuccessive waypoints. Thompson and Patel [9] proposed anapproach to approximately construct joint trajectories usingB-splines. Saravanan et al. [10] developed an optimal trajec-tory planning approach based on the evolutional theory usinguniform cubic B-splines. Gasparetto et al. [11], [12] adoptedcubic splines to optimize an objective function composed ofexecution time and squared jerk. Liu et al. [13] took furthersteps to optimize the motion time with the combination ofcubic splines in Cartesian space and septuple B-splines injoint space. Although cubic splines and B-splines simplifythe trajectory planning problem, they are not able to explorethe robot manipulator’s full capacity to minimize its cycletime.

Dahl et al. [14] address trajectories that are computed toreach the motor’s torque limits and introduce two methodsfor online scaling of calculated trajectories that allow com-pensating for modeling errors, disturbances, and uncertain-ties. The algorithm in [15] also considers online scaling witha focus on torque values for tracking an existing trajectory.

j1i,0a1i,0v1i,0p1i

j2i,0a2i,0v2i,0p2i

ti,0 ti,1 ti,2 ti,3 ti,4 ti,5 ti,6 ti+1,0

Fig. 1: The seven-segment motion profiles for two axes,where the segments of each dimensional motion are syn-chronized.

Antonelli et al. [16] present an offline algorithm that supportsseveral constraints on the trajectory, including limits on thejerk and torque values. They propose blending at waypointsby overlapping segments and superimposing them with aclotoid blend that fulfills the specified constraints.

III. PROBLEM FORMULATION

This paper assumes that a geometric path designed forthe robot manipulator to perform a task is available. Thispath consists of a sequence of waypoints defined in eitherCartesian or joint space. The studied problem is how togenerate a motion trajectory so that the robot manipulatorcan pass through those waypoints with the minimum timeand without violating its kinematic constraints.

A. Trajectory Model

Given the waypoints required for the task, we have toensure that the motion of each joint dimension is synchro-nized at every waypoint. In contrast with prior work [2], [3],[6] that only synchronize motions at every waypoint, thispaper requires that each dimensional motion is synchronizedin every segment as shown in Fig. 1, leading to two benefits.Firstly, the trajectory becomes more smooth by sharing thethree motion phases in every dimension. In particular, bysharing the same constant velocity phase, the robot manip-ulator is able to perform tasks that demand high-precisionand high-stability in this phase, e.g., gluing and painting.Secondly, the search space in the optimization is reduced aseach dimensional motion has the same time segments.

The waypoints in the k-th dimension (k < m) are indicatedas (pk0 , p

k1 , ..., p

kn−1), where m is the number of degrees of

freedom, n is the total number of waypoints, and pki indicatesthe position in the dimension k at the waypoint i. For a pathin configuration space, pki presents the joint position. In aCartesian space path, this refers to the Cartesian position.The trajectory between any two consecutive waypoints ismodeled as a trapezoidal acceleration or seven-segment ac-celeration profile.

We consider a trajectory between pki and pki+1. A typicaltrajectory profile is shown in Fig. 1, where the motion time is

divided into seven segments or three phases. The accelerationphase from ti,0 to ti,3 has increasing velocity. It is followedby the constant velocity phase from ti,3 to ti,4. The velocityis decreasing in the final deceleration phase from ti,4 toti,7 = ti+1,0. The jerk profile has a fixed value of zero in thethree time segments (ti,1, ti,2), (ti,3, ti,4), and (ti,5, ti,6) due toa constant acceleration in these segments.

We denote the acceleration, velocity and position at ti,ℎas ai,ℎ, vi,ℎ, and pi,ℎ, respectively. The jerk in the segment(ti,ℎ−1, ti,ℎ) is labeled as ji,ℎ with ℎ ∈ [0,… , 6]. Then, forthe time t ∈ (ti,ℎ, ti,ℎ+1), the time segment can be defined asΔt = t− ti,ℎ. The acceleration, velocity, and position profilescan be derived from the previous segment (ti,ℎ−1, ti,ℎ) as:

jki,ℎ+1(t) = jki,ℎ+1 ,

aki,ℎ+1(t) = aki,ℎ + j

ki,ℎ+1Δt ,

vki,ℎ+1(t) = vki,ℎ + a

ki,ℎΔt +

12ji,ℎ+1Δt2 ,

pki,ℎ+1(t) = pki,ℎ + v

ki,ℎΔt +

12aki,ℎΔt

2 + 16ji,ℎ+1Δt3 .

(1)

B. Kinematic Constraints

In any segment, the motion should not violate the kine-matic constraints of the robot. Note, that the accelerationis a monotonous function with time and piecewise smooth.Hence, to guarantee the kinematic constraints within a seg-ment, we only need to guarantee the kinematic constraintsat both ends of this segment.

Our aim is to find the time ti,ℎ and jerk ji,ℎ such thatthe motion time of the robot manipulator passing throughall waypoints can be minimized. Also, we aim to minimizethe overall time of the trajectory, i.e., tn,0. The kinematicconstraints ∀i ∈ [0,… , n − 1] are defined as:

pk(ti,7) = pk(ti+1,0) = pki+1vk(t0,0) = vk(tn,0) = 0ak(t0,0) = ak(tn,0) = ak(ti,3) = 0

jki,ℎ = 0 , ∀ℎ ∈ [1, 3, 5]|ak(ti,ℎ)| ≤ akmax , ∀ℎ ∈ [0,… , 6]|vk(ti,ℎ)| ≤ vkmax , ∀ℎ ∈ [0,… , 6]

|jki,ℎ| ≤ jkmax , ∀ℎ ∈ [0, 2, 4, 6]

(2)

At the initial and final points, i.e., at t0,0 and tn,0, theacceleration and velocity are equal to zero. Setting ak(ti,3) =0 guarantees that the velocity will remain constant duringthe constant velocity phase.

In (2), since vk(t0,0) and ak(t0,0) are equal to zero and pk1is known, the whole trajectory can be generated based on ti,ℎand jki,ℎ. As jki,ℎ is equal to zero ∀ℎ ∈ [1, 3, 5], the unknownvariables are ti,ℎ ∀i ∈ [0,… , n − 1] and ∀ℎ ∈ [0,… , 6], aswell as jki,ℎ ∀i ∈ [0,… , n−1] and ∀ℎ ∈ [0, 2, 4, 6]. Therefore,the total number of unknown variables is 7 (n − 1) for thetime variables and 4 (n − 1) for the jerk variables.

C. Constraints in Cartesian Space

In the previous section, a trapezoidal model is imple-mented in configuration space. However, in typical robot

p0

p1p2

p3

p4

�

Fig. 2: The error bound for the straight line deviation. Themaximum allowed line variation � is calculated based on thevariable � and the length of the individual path segment.

applications, robot arm motions in Cartesian Space areimportant as well. By only optimizing the trajectory inconfiguration space, the behavior in Cartesian space mightbe unpredictable. Hence, we propose an approach to includeCartesian space constraints in the optimization process. Weuse the forward kinematics function FK to get the Cartesianposition x ∈ SE(3) of the end effector for a given joint con-figuration q ∈ Rm : x = FK(q), where q = {q0,… , qm−1}.Here, the straight line interpolation between waypoints inthe Cartesian space will be considered. Since the trapezoidalmodel cannot guarantee a strict straight line, an approximatedstraight line behavior will be targeted. We introduce anadditional term in the objective function to regulate thedistance of the calculated trajectory to the straight line:

f1 =i=n−1∑

i=0

ℎ=6∑

ℎ=0g1(x)

d1 =‖Δxi,ℎ→0 × Δxi+1→i,0‖

‖Δxi+1→i,0‖

g1(x) =

{

0 if d1 < �1,d1 otherwise.

(3)

where Δxi,ℎ→0 = xi,ℎ − xi,0, Δxi+1→i,0 = xi+1,0 − xi,0, andxi,ℎ is the Cartesian position at the waypoint i and at thetime segment ti,ℎ. The variable �1 describes the error boundfor a path segment (Fig. 2) and is defined by a scalar � andthe length of a path segment:

�1 = ‖�Δxi+1→i,0‖ . (4)

For n waypoints, the optimization function has to consider 7nadditional scalar terms.

D. Linearity Constraints in Configuration Space

Here, the straight line interpolation li,i+1 between way-points in the configuration space will be considered. Similarto Section III-C, we introduce an additional term in theobjective function to minimize the distance of the calculatedtrajectory to the straight line in configuration space:

f2 =i=n−1∑

i=0

ℎ=6∑

ℎ=0g2(x)

d2 = distance(pti,ℎ , li,i+1)

g2(x) =

{

0 if d2 < �2,d2 otherwise.

(5)

where li,i+1 is the straight line connecting pi and pi+1 anddistance(pti,ℎ , li,i+1) is calculated as

‖

‖

‖

(pti,ℎ − pi) −(pti,ℎ − pi) (pi+1 − pi)2

‖pi+1 − pi‖‖

‖

‖

. (6)

IV. OPTIMIZATION APPROACHES

The trajectory generation can be formulated as a nonlinearand non-convex constraint optimization problem, which canbe solved by means of the sequential quadratic programmingalgorithm that decomposes the non-convex problem into se-quential convex problems. We use the the SLSQP [17] solverfrom NLOpt [18] for our implementation. With the highdimensionality of the configuration space and a large numberof waypoints, the complexity of the optimization problemand the optimization space can increase significantly. A goodinitial estimate of the trajectory noticeably affects the con-vergence of the optimization routine. To handle this problem,we apply concepts from Model Predictive Control (MPC).

A. Optimization Problem Formulation

The purpose of the constrained optimization problem isto optimize the time of the whole trajectory by makinguse of the trapezoidal acceleration model. The optimizationparameters X for this problem consist of the time andjerk values at each segment of the trajectory. Using thesevalues, the acceleration, velocity, and position can be derivedautomatically using (1). For the time optimization part of theobjective function, we try to minimize Δti,ℎ = ti,ℎ − ti,ℎ−1 forevery value of ℎ and i. The objective function is then definedas

f (X) =i=n−1∑

i=0

ℎ=6∑

ℎ=0Δti,ℎ , (7)

where

X = ({t0,… , ti,… , tn−1}, {j0,… , ji,… , jℎ}) ,ti = {Δti,0,… ,Δti,6} ,ji = {{j0i,0, j

0i,2, j

0i,4, j

0i,6},… , {jm−1i,0 , jm−1i,2 , jm−1i,4 , jm−1i,6 }} .

Besides the object function, the constraints also playan important role in solving this problem. Roughly, theconstraints comprise nonlinear inequality and equality con-straints as well as lower and upper bounds.

minimizeX

f (X)

subject to lb ≤ X ≤ ubc(X) ≤ 0ceq(X) = 0

(8)

The multidimensional trapezoid model consists of 7n+4nmoptimization variables, a total of 14mn nonlinear inequalityconstraints, and mn + nm nonlinear equality constraints.

Algorithm 1 Trajectory optimization based on Model Pre-dictive ControlInput: p, vmax, amax, jmaxOutput: Ω time and jerk profile

1: Ttentative ← ∅2: Jtentative ← ∅3: for i = 0 to n − ℎ do ℎ: receding horizon4: if i == 0 then5: (ti, {ti+1,… , ti+ℎ}, ji, {ji+1,… , ji+ℎ}) ←InitialPoint({pi,… ,pi+ℎ})

6: Xinit ← (ti, {ti+1,… , ti+ℎ}, ji, {ji+1,… , ji+ℎ})7: else8: (ti+ℎ, ji+ℎ) ← InitialPoint({pi+ℎ−1,pi+ℎ})9: Xinit ← (Ttentative, ti+ℎ, Jtentative, ji+ℎ)

10: end if11: Xoptimized ← JointSolver(Xinit , vmax,amax, jmax) Eq. 812: if optimize Cartesian constraints then13: Xoptimized ← OptimizeCartesian(Xoptimized, vmax,amax, jmax)14: end if15: (toptimized, Ttentative, joptimized, Jtentative)← Xoptimized16: Ω ← Ω ∪ {toptimized, joptimized} Output optimized profile for

the part between pi to pi+117: end for18: return Ω

Algorithm 2 OptimizeCartesian() function for optimizingCartesian linearity constraintsInput: Xoptimized, vmax,amax, jmaxOutput: Xoptimized

1: for j = 1 to 7ℎ do trajectory profile with 7 segments2: Xoptimized ← CartesianSolver(Xoptimized, vmax,amax, jmax)

Eq. 93: end for4: return Xoptimized

B. Optimization for Linearity Constraints

In order to include linearity constraints in the Cartesian orconfiguration space (Algorithm 2), the optimization problemfrom (8) is updated as:

minimizeX

f (X) +w1f1(X) +w2f2(X)

subject to lb ≤ X ≤ ubc(X) ≤ 0ceq(X) = 0

(9)

The weights w1 and w2 denote the relative importance of theCartesian or configuration space linearity constraints. Theseparameters affect the optimization problem and need to betuned according to the use case.

C. MPC-based Optimization Approach

With a high number of degrees of freedom and multiplewaypoints, the optimization problem is a highly nonlinearand non-convex constraint problem. It is not realistic tooptimize all waypoints within one optimization step. How-ever, the waypoints can be divided into consecutive batches.The remaining problem is then to connect all the individualbatches. Directly connecting all batches will result in azero velocity at all connecting points. In order to avoidthis situation, we adopt the idea behind Model Predictive

p0 p1 p2 p3 p4 pi−1 pi pi+1pi+2 pnk = 0 p0 p1 p2k = 1 p1 p2 p3

k = i − 1 pi−1 pi pi+1k = i pi pi+1pi+2

waypoints

deco

mpo

sitio

npi

eces

Fig. 3: The waypoints have been decomposed into n − 2batches. Each batch consists of three waypoints, where theformer and last two waypoints (bold lines) are respectivelyoverlapped with their former and subsequent batches.

Control approaches [19]. As shown in Fig. 3, we first needto set a receding horizon. We use a value of two for this,therefore only the next two waypoints pi and pi+1 will betaken into account at waypoint pi−1 and the batch containsthe waypoint set {pi−1, pi, pi+1}. The batch including thesethree waypoints is used as an input for the optimizationsolver, but only the result between pi−1 and pi will beused and the result between pi and pi+1 will be discarded.However, we can still use this result as initial guess forthe next optimization step, since the next batch will containthe waypoints {pi, pi+1, pi+2}. Hence, every batch shares twowaypoints with its former and subsequent batch. In this way,the trajectory profile between two overlapping waypoints canbe predicted by optimizing the former batch and updated byoptimizing the subsequent batch.

As shown in Fig. 3, the whole set of waypoints has beensuccessively decomposed into n − ℎ batches, where ℎ is thereceding horizon. The optimization problem for the com-plete set of waypoints is then posed as multiple (i = n − ℎ)successive optimization problems, one for each batch. Theoverlapping waypoints act as a bridge, connecting the opti-mization results from one batch to the next one. The detailedoptimization procedure is presented in Algorithm 1. Now weanalyze the optimization of batch i, where the overlappingpart with its former batch is between waypoints pi and pi+1.After the optimization of batch i−1, a tentative accelerationprofile is predicted on the overlapping part. Then, afterthe optimization towards the batch i, the tentative profilewill become the optimized profile and the unknown profilebetween pi+1 and pi+2 becomes the tentative accelerationprofile, as illustrated in Fig. 4. Here, the overlapping partis optimized twice so that the trajectories of two successivebatches can be bridged efficiently.

In order to keep the consistency at pi, the acceleration andvelocity at pi that have been optimized from optimizationstep i − 1 will act as initial conditions of optimizationstep i. The trajectory of batch i is required to end with zerovelocity and acceleration. There are two benefits out of thisassumption. Firstly, the static state improves the optimizer

beforepi pi+1 pi+2

tentative profile unknown profile

after pi pi+1 pi+2

optimized profile tentative profile

Fig. 4: The conceptual acceleration profile of batch i beforeand after its optimization.

flexibility to predict a good tentative profile. For example, thetrajectory is assumed to be static at pi+1 after the optimizationon batch i−1. Then, when optimizing the batch i, the staticpoint at pi+1 leaves the optimizer with the full possibility ofgenerating a good tentative profile between pi+1 and pi+2.Secondly, the initial point for the unknown profile will bechosen by means of the solution without blending, whichrequires zero velocity and acceleration as initial conditions.Therefore, the initial point can be consistent—a very impor-tant property for optimization.

After this optimization on the n− ℎ decomposed batches,the whole trajectory is generated and optimized. Thereare ℎ + 1 waypoints in every batch and the optimization ofeach batch has the same computational complexity. There-fore, the computation of the whole trajectory scales linearlywith n.

D. Initial Point

Although the optimization complexity has been loweredto a linear scale, the optimization on a specific batch isstill complex. As an example, for a robot manipulator withsix degrees of freedom and a receding horizon of two, theoptimization problem has 62 variables, 48 nonlinear equalityand 168 nonlinear inequality constraints (Section III-A).Hence, to efficiently find the optimal result, choosing a goodinitial point is a very important step.

For the optimization of batch i, the tentative profile is avery promising starting point for the part between pi and pi+1as this profile has already been optimized tentatively. Regard-ing the starting point for the other part, i.e., from pi+1 to pi+2,a simple way is to derive a profile where the trajectories in alldimensions are synchronized at the endpoint with the shortesttime, which is also a non-smooth trajectory. This profile canbe derived analytically as follows. First, we independentlycompute the profile with the shortest motion time for eachdimension. Suppose

tki+1 = f (pki+1, p

ki+2, v

kmax, a

kmax, j

kmax) (10)

represents the shortest time for the k-th dimension trajectory,which is a sum of seven time segments. Secondly, the

0 s 3.33 s−57◦

170◦

(a) Linear Parabolic

0 s 3.58 s−58◦

170◦

(b) Cubic Spline

0 s 4.07 s−58◦

170◦

(c) Trapezoidal Acceleration

0 s 3.33 s

−59 ◦∕s

71 ◦∕s

(d) Linear Parabolic0 s 3.58 s

−81 ◦∕s

90 ◦∕s

(e) Cubic Spline

0 s 4.07 s−78 ◦∕s

83 ◦∕s

(f) Trapezoidal Acceleration

0 s 3.33 s−686 ◦∕s2

690 ◦∕s2

(g) Linear Parabolic

0 s 3.58 s

−288 ◦∕s2

318 ◦∕s2

(h) Cubic Spline

0 s 4.07 s

−257 ◦∕s2

320 ◦∕s2

(i) Trapezoidal Acceleration

0 s 3.33 s−∞ ◦∕s3

∞ ◦∕s3

(j) Linear Parabolic

0 s 3.58 s−795 ◦∕s3

674 ◦∕s3

(k) Cubic Spline0 s 4.07 s

−900 ◦∕s3

900 ◦∕s3

(l) Trapezoidal Acceleration

Fig. 5: A comparison of three different trajectory profiles for the 7-DOF Kuka LWR example in Fig. 8a–8d. The individualplots show the respective (a)–(c) position, (d)–(f) velocity, (g)–(i) acceleration, and (j)–(l) jerk profiles.

0◦ 2◦ 4◦ 6◦ 8◦0

10,000

20,000

30,000

40,000

(a) Joint dev. w/ Linear Parabolic

0◦ 2◦ 4◦ 6◦ 8◦0

1,000

2,000

3,000

4,000

(b) Joint dev. w/ Cubic Spline

0◦ 2◦ 4◦ 6◦ 8◦0

1,000

2,000

3,000

4,000

(c) Joint dev. w/ Trapezoidal Acceleration

Fig. 6: A comparison of the deviations from straight lines in configuration space for the Kuka LWR example of Fig. 8a–8d.This property is especially important for ensuring collision free paths calculated by a motion planner. In this example, theerror bound �2 = 0.1 and optimization weights are w1 = 0.1 and w2 = 0.1.

0.0m 0.01m 0.02m 0.03m 0.04m 0.05m

0

2,000

4,000

6,000

8,000

(a) Cartesian dev. w/ Linear Parabolic

0.0m 0.01m 0.02m 0.03m 0.04m 0.05m

0

2,000

4,000

6,000

8,000

(b) Cartesian dev. w/ Cubic Spline

0.0m 0.01m 0.02m 0.03m 0.04m 0.05m

0

2,000

4,000

6,000

8,000

(c) Cartesian dev. w/ Trapezoidal Acc.

Fig. 7: A comparison of the deviations from straight lines in Cartesian space for the Kuka LWR example of Fig. 8a–8d. Thisproperty is especially important for following paths of a Cartesian task. In this example, the error bound �1 is calculatedbased on a value of � = 0.1 and optimization weights are set as w1 = 0.1 and w2 = 0.1.

(a) Linear Parabolic (b) Linear Parabolic (c) Cubic Spline (d) Trapezoidal Acc. (e) Trapezoidal Acc.

Fig. 8: Trajectories for different interpolation models on path planning use cases for (a)–(d) a Kuka LWR next to a tablewith a parallel gripper and (e) a Kuka KR60-3 next to a wall and three columns with a vacuum gripper.

maximum time among all dimensions is chosen as

tmaxi+1 = max tki+1 , ∀k ∈ [0,… , m − 1] . (11)

Then, in order to scale up the motion time of all dimensionsto tmaxi+1 , the maximum jerk of other dimensions is adjusted sothat kinematic constraints at each dimension are not violated:

jk′max = (t

ki+1∕t

maxi+1 )

3jkmax , ∀k ∈ [0,… , m − 1] . (12)

Lastly, the time profile of the dimension that has the maxi-mum time and the modified jerk of all dimensions are usedas the initial point for the optimization.

We have to mention that there are two exceptions thatare not optimized twice. In Algorithm 1, the parts {p1, p2}of the first batch and {pn−1, pn} of the last batch will onlybe optimized once as they do not overlap with any otherbatch. In addition, even after the initial point is chosen asdescribed above, this does not guarantee that a solution canbe found. Therefore, after the chosen initial point fails, auniform random noise is added to this initial point and theoptimization is restarted to avoid getting stuck in a localminimum [20].

V. EXPERIMENTAL EVALUATIONS

We evaluate the performance of the proposed optimiza-tion approaches and two state-of-the-art approaches: LinearParabolic interpolation [21] and Cubic Splines [22]. TheLinear Parabolic model divides each interpolation step intothree parts: two parabolic blends with the previous and nexttrajectory segments and a linear interpolation with constantvelocity in the middle. We test these approaches on two pathplanning examples and present a detailed analysis of theirperformance and properties. For both the Cubic Spline andLinear Parabolic models, we ensured that the accelerationand velocity limits of each joint are satisfied by scalingthe timescales of the trajectory segments. For the TrapezoidAcceleration model, this is handled intrinsically by ouroptimization routine.

We show how we fulfill two key requirements of pathplanning algorithms, i.e., reaching waypoints exactly andmaintaining near-linear trajectories in the configuration spacebetween successive waypoints. Fig. 8b–8d shows a pathplanning example for the Kuka LWR consisting of sevenwaypoints indicated by green spheres. The interpolated tra-jectories are shown by green lines. For the Cubic Spline

and our approach, the calculation will consider all kinematicconstraints, as described in Algorithm 1. For the LinearParabolic model, we calculate the time segment for eachdimension by only taking into account the velocity andacceleration constraints. The time for each segment is chosenaccording to the most constrained dimension/joint, ensuringthat all robot joints are within their velocity and accelerationlimits. The collision-free paths in the examples are computedusing the Robotics Library [23], which is also used forkinematics calculations and simulation. All evaluations wereperformed on a laptop with a 2.60GHz Intel Core i7-6700HQand 16GB of RAM.

The trajectory calculated by the Cubic Splinemodel (Fig. 8c) deviates significantly from a straightline interpolation between successive waypoints. Thetrajectory from the Linear Parabolic method (Fig. 8b)follows a near-linear interpolation but is clearly non-smooth.Our method (Fig. 8d) satisfies both requirements. Weplot the configuration space positions, velocities, andaccelerations for each of these models in Fig. 5. CubicSpline and Trapezoidal Acceleration methods are guaranteedto pass through all waypoints exactly. Linear Parabolicinterpolation blends around the waypoints and fails to hitinner waypoint as demonstrated in Fig. 8a. The percentageof the trajectory segment that is blended influences thisdeviation from the waypoints. We set this to 20% for ourexperiments, resulting in deviations of 0.0◦, 2.3◦, 1.3◦, 3.0◦,4.5◦, 2.2◦, and 0.0◦ for the seven waypoints.

Cubic Spline has a movement time of 3.58 s, while theapproach presented in this paper is slightly longer with 4.07 s.The Linear Parabolic one only takes 3.33 s and is the shortestone. Note, that Linear Parabolic cannot consider the jerklimits, which are very important to prevent any damage tothe motors. In contrast to our approach, Cubic Spline doesnot start and end with an acceleration value of zero and itmathematically only allows one point at the maximum valueat each path segment. The trapezoidal acceleration modelcan hold the maximum velocity and acceleration for a longertime. Over 20 runs, the optimization for the Linear Paraboliceach takes 1.45 ± 0.03 s, Cubic Spline 0.07 ± 0.01 s, and themore complex trapezoidal acceleration model 9.58 ± 2.05 s.

The simulation of these three trajectories is shown inFig. 8b–8d. A key property from the underlying path plan-

ning algorithm is that collision-free paths are only guaranteedas long as the interpolation between the waypoints followsa straight line in configuration space. In practice, the pathplanning algorithms usually consider a minimum distancefrom obstacles as a safety margin. Hence, the straight lineinterpolation requirement can also be relaxed according tothis distance. We evaluate the deviations in configurationspace, as shown in Fig. 6a–6c. Note, that in our approachwe add an additional term to minimize the deviation error.It can be considered as a soft constraint and the problemwill be optimized to achieve minimum deviation. However,it cannot be guaranteed to stay within the error bound.

In Fig. 7a–7c, the deviation from the straight line betweenwaypoints in Cartesian space is illustrated. It can be seen thatthe Linear Parabolic shows the smallest error as it consists oflinear movement and a blending around the waypoints. Thetrapezoidal model cannot guarantee straight line movement.However, by adjusting the duration of the constant velocityphase, the motion can be made closer to a straight lineinterpolation. In our approach, we employ an error boundobject function to minimize the deviation from the straightline. From the results, we can observe that the Cartesianerrors are lowest for Linear Parabolic, and highest for CubicSpline while ours lies in the middle. Our approach allows usto adjust this deviation by tuning �, w1, and w2.

In Fig. 8e, we evaluate our approach in a more com-plex example consisting of 31 waypoints. Optimizing thetrajectory using the whole set of waypoints in one optimiza-tion problem requires several hours, whereas our approachcan find each solution in 222.50 ± 49.75 s over a total of10 attempts. The robot is guaranteed to pass through allwaypoints while exhibiting a smooth 16.63 s trajectory andobeying all kinematic constraints. The optimization of theLinear Parabolic’s 11.61 s trajectory requires 6.05 ± 0.41 sand the Cubic Spline’s 13.89 s solution takes 0.33 ± 0.01 s.

VI. CONCLUSION

In this paper, we have presented an approach to finda time-optimal trajectory passing through given waypointsunder kinematic constraints. Unlike prior approaches thatmodel the trajectory between two adjacent waypoints as aCubic Spline or Linear Parabolic segments, this approachadopts a trapezoidal acceleration profile to represent thetrajectory. We require this trajectory to move through allwaypoints while exploiting the manipulator’s capabilities inorder to reduce the motion time and ensuring that the kine-matic limitations are always satisfied. Our proposed bridgedoptimization approach has linear complexity with the numberof waypoints compared to a full trajectory optimization.Evaluations of two practical examples from path planninghave shown how the three approaches compare to each other.

There are also some limitations that we observed inour experiments. In cases where there are large rotationsbetween successive waypoints, our method can take longerto converge and sometimes not provide an optimal solution.Additionally, the Cartesian linearity constraint is only in-cluded as a soft constraint and might not work in applications

with more stringent linearity requirements such as weldingor deburring. In such cases, the Cartesian linearity constraintshould be formulated as an inequality constraint instead.Future work includes handling more complicated cases andexamples from planners that support Cartesian constraints.

REFERENCES

[1] J. Reif, “Complexity of the mover’s problem and generalizations,”in Proceedings of the IEEE Symposium on Foundations of ComputerScience, 1979, pp. 421–427.

[2] R. Haschke, E. Weitnauer, and H. Ritter, “On-line planning of time-optimal, jerk-limited trajectories,” in Proceedings of the IEEE/RSJ Int.Conf. on Intelligent Robots and Systems, 2008, pp. 3248–3253.

[3] T. Kroger and F. M. Wahl, “Online trajectory generation: Basicconcepts for instantaneous reactions to unforeseen events,” IEEETransactions on Robotics, vol. 26, no. 1, pp. 94–111, 2010.

[4] R. H. Castain and R. P. Paul, “An on-line dynamic trajectory gen-erator,” The Int. J. of Robotics Research, vol. 3, no. 1, pp. 68–72,1984.

[5] E. F. Camacho and C. B. Alba, Model predictive control, ser. AdvancedTextbooks in Control and Signal Processing. Springer, 2007.

[6] S. Macfarlane and E. A. Croft, “Jerk-bounded manipulator trajectoryplanning: Design for real-time applications,” IEEE Transactions onRobotics and Automation, vol. 19, no. 1, pp. 42–52, 2003.

[7] T. Kroger, On-line trajectory generation in robotic systems, ser.Springer Tracts in Advanced Robotics. Springer, 2010, vol. 58.

[8] F. Lange and A. Albu-Schaffer, “Path-accurate online trajectory gen-eration for jerk-limited industrial robots,” IEEE Robotics and Automa-tion Letters, vol. 1, no. 1, pp. 82–89, 2016.

[9] S. E. Thompson and R. V. Patel, “Formulation of joint trajectories forindustrial robots using B-splines,” IEEE Transactions on IndustrialElectronics, no. 2, pp. 192–199, 1987.

[10] R. Saravanan, S. Ramabalan, and C. Balamurugan, “Evolutionary op-timal trajectory planning for industrial robot with payload constraints,”The International Journal of Advanced Manufacturing Technology,vol. 38, no. 11, pp. 1213–1226, 2008.

[11] A. Gasparetto and V. Zanotto, “A technique for time-jerk optimalplanning of robot trajectories,” Robotics and Computer-IntegratedManufacturing, vol. 24, no. 3, pp. 415–426, 2008.

[12] ——, “Optimal trajectory planning for industrial robots,” Advances inEngineering Software, vol. 41, no. 4, pp. 548–556, 2010.

[13] H. Liu, X. Lai, and W. Wu, “Time-optimal and jerk-continuoustrajectory planning for robot manipulators with kinematic constraints,”Robotics and Computer-Integrated Manufacturing, vol. 29, no. 2, pp.309–317, 2013.

[14] O. Dahl and L. Nielsen, “Torque-limited path following by online tra-jectory time scaling,” IEEE Transactions on Robotics and Automation,vol. 6, no. 5, pp. 554–561, 1990.

[15] J. Moreno-Valenzuela, “Tracking control of on-line time-scaled trajec-tories for robot manipulators under constrained torques,” in Proc. ofthe IEEE Int. Conf. on Robotics and Automation, 2006, pp. 19–24.

[16] G. Antonelli, C. Curatella, and A. Marino, “Constrained motionplanning for open-chain industrial robots,” Robotica, vol. 29, no. 3,pp. 403–420, 2011.

[17] D. Kraft, “A software package for sequential quadratic program-ming,” Deutsche Forschungs- und Versuchsanstalt fur Luft- und Raum-fahrt Koln, Cologne, Germany, Forschungsbericht DFVLR-FB–88-28,1988.

[18] S. G. Johnson, “The NLopt nonlinear-optimization package.” [Online].Available: http://ab-initio.mit.edu/nlopt

[19] R. S. Sutton, A. G. Barto, et al., Reinforcement learning: An intro-duction. MIT press, 1998.

[20] Z. Ugray, L. Lasdon, J. Plummer, F. Glover, J. Kelly, and R. Martı,“Scatter search and local NLP solvers: A multistart framework forglobal optimization,” INFORMS Journal on Computing, vol. 19, no. 3,pp. 328–340, 2007.

[21] L. Biagiotti and C. Melchiorri, Trajectory planning for automaticmachines and robots. Springer, 2008.

[22] C. de Boor, A Practical Guide to Splines. Springer, 1978.[23] M. Rickert and A. Gaschler, “Robotics Library: An object-oriented

approach to robot applications,” in Proceedings of the IEEE/RSJ Int.Conf. on Intelligent Robots and Systems, 2017, pp. 733–740.