ISSN 1590-8844

International Journal of Mechanics and Control, Vol. 15, No. 01, 2014

EXTENDING CONTINUOUS PATH TRAJECTORIES TO POINT-TO-

POINT TRAJECTORIES BY VARYING INTERMEDIATE POINTS

Hubert Gattringer* Matthias Oberherber* Klemens Springer*

* Johannes Kepler University Linz

ABSTRACT

This paper shows a method for the calculation of fast point-to-point movements for
industrial robots subject to various physical constraints. The main idea of the proposed
algorithm is to solve first the problem of time optimisation of continuous path trajectories.
A predefined path in world coordinates as well as the dynamical equations of motion are
projected onto a one dimensional path parameter, the time behaviour of which is optimised
with the dynamic programming approach (Bellman optimisation). The optimal path for the
point-to-point problem is found by varying additional intermediate points between start-
and endpoint of the predefined path in an optimal manner using a nonlinear solver. Each
path with varied intermediate points is optimised with the dynamic programming approach.
Simulation results as well as experimental results for an industrial robot are shown.
Keywords: Point-to-Point Motion, Optimisation, Industrial Robot, Robots Kinematic and Dynamics

1 INTRODUCTION
In contrast to that, we use Bernstein polynomials for the
Fast robot movements in highly automated production lines definition of the geometric path via splines. Motor torque
have become more and more important in the last few years constraints can be effectively included in the phase plane.
to sustain production locations in high wage countries. This The limiting curves are evaluated with a reformulation of
can be achieved by path optimisation. In a first step, the the Projection Equation, see [3] and [6]. Moreover we
optimisation of a continuous path (CP) trajectory (pre- calculate the time optimal solution using the dynamic
defined path) subject to mechanical constraints is programming approach (Bellman optimisation) to provide
performed. The problem is divided in the geometric path faster solutions and to be able to include additional terms in
planning and its optimisation using a scalar path parameter. the cost functional, see also [11]. This offers the possibility
The works of [12], [2] and [14] provide the basis for to calculate time/energy optimal solutions. A recursive log-
effective algorithms. [4], [8] and [10] extended this basic barrier method for time-optimal robot path tracking is
algorithms by additionally taking end-effector friction presented in [15]. As an extension to the CP trajectories,
constraints into account. The aim is to transport an object the point-to-point (PtP) optimisation is not limited to
that is loosely put onto the end-effector in shortest possible geometrically defined paths. It can, for instance, be
time on a predefined path (waiter-motion-problem). [8] use approached by formulating the task as two-point boundary
quintic B-splines to define the geometric path. The time value problem and solved with numerical software
optimal solution for the path parameter is obtained by packages, see e.g. [7]. However, in this work, a solution is
finding optimal switching points for acceleration and computed by varying the position and orientation of
deceleration in the phase plane (forward and backward intermediate points of the CP trajectory using a numerical
integration). solver. The paper is organized as follows: In section 2 the
optimisation of the CP trajectories is performed.
Constraints like motor torques, velocities and accelerations
Contact author: Hubert Gattringer1 are included. This is the basis for the optimisation of the
1 PtP trajectories in section 3. Experimental results and
Institute for Robotics, Johannes Kepler University Linz, comparisons to other optimisation approaches are part of
Altenbergerstr. 69, 4040 Linz, Austria section 4. The experiment is realized with a Stäubli
Email: [email protected] RX130L six axes industrial robot.

35
 ISSN 1590-8844
International Journal of Mechanics and Control, Vol. 15, No. 01, 2014

2 CONTINUOUS PATH TRAJECTORIES

2.1 GEOMETRIC PATH PLANNING

There are several methods to define a geometric path, e.g.
straight lines, curves, clothoids or splines. The latter ones
are used in this work to define the path for the end-effector

(1)

see Fig. 1.
Figure 2 Bernstein polynomial of degree d =4.

To define a path through interpolation points

, a chordal discretisation for is
performed. This means, that n+1 points are distributed over
the range of the path parameter with respect to
the distance of the defined points in space. With the length
of the polynomial-line

(3)

the discretisation points are defined by

(4)
Figure 1 Path planning by interpolation of points.
With this chordal parameterisation for , the end-effector
Vector is the position of the end-effector while is path can be written as
the orientation in terms of Cardan angles, respectively. The
orientation matrix for this transformation
is defined by subsequent standard rotations about the x- (5)
(angle ), y-(angle ) and z-(angle ) direction. A set of
j pre-defined points is interpolated to the whole path
that can be parameterised by a (scalar) path parameter Note, the more interpolation points are chosen, the more
that is determined in the range (start point) and basis functions, see Fig. 2, enter calculations. The control
(end point). The spline within this range is defined points
by Bernstein polynomials
(6)

(2) are the result of the linear equation

(7)
resulting from a specific choice of number and location of
the B-spline nodes, see [13] for details. This kind of global
basis functions provides the possibility to calculate where is the matrix of Bernstein polynomials
derivatives with respect to the path parameter analytically
in a simple way. Furthermore the non-recursive definition
has a positive effect on the computation time. Parameter d
in Eq. (2) is the degree of the Bernstein polynomial. Figure (8)
2 shows them for degree d=4, that is chosen in the present
work.

and are the pre-defined points in space.

36
 ISSN 1590-8844
International Journal of Mechanics and Control, Vol. 15, No. 01, 2014

With the geometric definition at hand, the time behaviour 2.2.1 Torque Constraints
of can be optimised to calculate the trajectory. More The basis for taking motor torque constraints into account
details on spline parameterisations can be found in [5]. is an efficient dynamical modelling. We use the Projection
Equation (see [3])
2.2 PHYSICAL CONSTRAINTS
For trajectory planning, several physical constraints have to
be considered, namely motor torque-, velocity- and (18)
acceleration constraints

for this task. Linear momenta and angular

(9)
momenta are projected into the minimal space
(minimal velocities ) via the appropriate Jacobian
matrices. All the values like the translational velocity or
Since the trajectory is defined for the end-effector in world the rotational velocity of the centre of gravity can be
coordinates, a transformation into joint co-ordinates has to inserted in arbitrary coordinate systems R. In contrast to
be performed by inverse kinematic calculations is the velocity of the used reference system. Since
we use body fixed reference systems, holds in this
(10) case. Parameters are the mass and the inertia tensor,
respectively, while characterizes the vector product
A projection of velocities and accelerations onto the path . and are imposed forces and moments acting
coordinate yields
on the ith body. Evaluating the Projection Equation for N =
12 bodies (the industrial robot consists of six motors and
(11) six links) leads to the equations of motion

(12) (19)

when using the identity where is the position dependent, positive definite,
. However, the evaluation of the derivations with symmetric mass matrix, contains all nonlinear terms like
respect to are difficult to realize even for the case, when gravitational, Coriolis, centrifugal and friction forces, while
the inverse kinematics Eq. (10) is solved numerically. are the motor torques. Of course, the dynamical
Fortunately, the identity behaviour of the robot can be simulated with Eq. (19).
However in our case, it is needed to take motor torque
constraints into account. Therefore, Eq. (18) can be
(13) rewritten to

(14)

and therefore
(20)
(15)

(16)
by substituting the abbreviations of the momenta and for
holds, which is much less computational complex. The the identity matrix. The velocities and accelerations
generalized matrix inverse reads depending on the path parameter are

(17)

37
 ISSN 1590-8844
International Journal of Mechanics and Control, Vol. 15, No. 01, 2014

between and and therefore Eq. (30) can be

written as

(21) (31)

This torque constraints offer a geometrical interpretation in

the phase plane . For each joint, we get an upper and
(22) lower limiting curve in this plane, shown in Fig. 3. Only the
feasible region is valid for optimal path planning. Note, if
viscous friction is neglected ( ), Eq. (30) degenerates
The expression can further be evaluated as to straight limiting lines. However, this simplification
should be avoided for real systems.

(23)

(index i suppressed) and Eq. (20) equals

(24)

Figure 3 Limiting curves and feasible region.

The impressed torques can be divided into the friction

torques and the 2.2.2 Velocity/Acceleration Constraints
Beside torque constraints, velocity and acceleration
motor torques that are written on the right side of Eq. constraints have to be considered. Joint angular as well as
(24). Collecting appropriate terms in Eq. (24) yields path velocities

(25) (32)
(33)
with
can easily be transformed to (using )
(26)

(27) (34)

(28)
leading to vertical limiting curves on the position of in
(29) the phase plane, see again Fig. 3. An evaluation of joint
acceleration constraints
When using the abbreviation , the kth motor torque
equation yields (35)

(30) delivers additional straight lines in the phase plane

indicated by dashed lines in Fig. 3.
( ..gear ratio of kth joint). The input is constrained

38
 ISSN 1590-8844
International Journal of Mechanics and Control, Vol. 15, No. 01, 2014

2.3 OPTIMISATION feasible region in Fig. 5. At first we discretise the velocity

The global goal of the CP optimisation is to find a at each path point into values
minimum of the cost function with . The discretised cost function from Eq.
(39) equals as a consequence of the Bellman optimality
principle
(36)

(40)
The second part in this equation corresponds to the
minimization of the motor torques. Since we are only
where the discretisation step size of is .
interested in time-optimal solutions, the weighting factors
are used, and the equations of motion do
not enter the cost functional. The unknown cycle time is
the sought solution of the optimisation, so a transformation
of

(37)

and therefore

(38)
Figure 5 Feasible region with limits.

leads to a cost function depending on of The optimal values of can now be evaluated by
calculating the highest reachable point
and the lowest one , wherein the
(39) minimum of the cost function has to be sought, as shown in
Fig. 4. This minimum search is done with the method of the
golden ratio. With the found location of the minimum ,
The calculation of the optimal trend of the velocity profile the optimal value of can be calculated with
is done with the help of the Bellman optimality principle
[1]
(41)
”An optimal policy has the property that whatever the
initial state and initial decision are, the remaining decisions
must constitute an optimal policy with regard to the state As is calculated for each discretisation point
resulting from the first decision.” , can be evaluated by
solving

(42)

with a forward iteration, starting at , see Fig. 6

for a graphical interpretation.

Figure 4 Bellman optimisation.

Therefore the calculation of the optimal trend starts at

the last point by evaluating the maximum and minimum
allowed values of with the help of the Figure 6 Computing the solution.

39
 ISSN 1590-8844
International Journal of Mechanics and Control, Vol. 15, No. 01, 2014

As soon as the optimal trend is evaluated, the time . Summing up the above mentioned assumptions, the
behaviour of can be calculated by integrating Eq. (38) optimisation problem reads as

(43) (45)

and calculating the inverse function to get . Therefore subject to

the overall trajectory as well as the motor torques Eq. (25),
that can be used as model-based feed-forward torques, are (46)
defined. (47)

This can be solved e.g. with the BOBYQA algorithm of the

3 POINT-TO-POINT TRAJECTORIES
NLOPT optimisation package, see [16] and [17] for details.
The strategy to get optimal PtP trajectories is to use the Note, in the cost functional Eq. (45), the time optimal CP
results from the previous section and release intermediate trajectories have to be computed.
points as optimisation variables. However, the convergence The overall optimisation strategy for the PtP trajectories is
(time) to the optimal solution can drastically be increased if summarised in the flowchart Fig. 8.
not all 6 coordinates of the intermediate points are used as
optimisation variables, see also [8], where a similar
approach is used to constrain maximal accelerations of a
roller coaster system. Also lower (lb) and upper (ub)
bounds for the optimisation variables decrease convergence
time. For our time optimal PtP problem, positions should
only vary in (value n-normal) and (value b-
binormal) direction of the end-effector coordinate system,
while the orientations should only vary for the Cardan
angles (rotation around ) and (rotation around ),
see Fig. 7. This restriction increases the convergence and
does not influence the result significantly.

Figure 7 Optimisation variables.

Figure 8 Flowchart of PtP optimisation approach.
The whole set of optimisation variables is

4 RESULTS
(44)
4.1 PRESENT APPROACH
where l and k are the number of intermediate points for The proposed method is applied for a specific PtP
position and orientation, respectively. Geometrical trajectory planning problem. The test case is chosen in such
constraints like barriers in the workspace can be defined as a way, such that the initial trajectory passes near the wrist
additional nonlinear constraints. For instance, the distance singularity of the robot. For a PtP solution this is not time-
between the end-effector and a disturbing object optimal due to the velocity constraint being active near the
should be greater than a safety distance singular position. It is shown that the PtP time-optimal
trajectory diverges from this singularity in an optimal

40
 ISSN 1590-8844
International Journal of Mechanics and Control, Vol. 15, No. 01, 2014

manner such that process time decreases. So the start- and

endpoint are defined as

(52)
(48)

(49) These points are interpolated by the spline curves,

described in section 2. Figure 9 presents the optimised
trajectory in world coordinates, while Fig. 10 shows the
see Fig. 9. For the optimisation, two equidistant time behaviour of the Cardan angles and of the end-
effector.

Figure 9 Initial and optimised trajectory

Figure 10 Optimised Cardan angles for end-effector.
in Cartesian coordinates.
For the particular chosen start- and endpoint, the optimised
intermediate points between start and endpoint,
angle is nearly zero. The resulting joint angles,
that are located on a straight line, are chosen, yielding normalized joint velocities and normalized motor torques
are presented in Figs. 11, 12 and 13.

(50)

A similar procedure is chosen for the orientation

description. For this case three intermediate Cardan angles
are chosen. Applying the optimisation algorithm from
section 3 yields the optimal intermediate points. The upper
and lower bounds for the variations of positions and
orientations are given in Tab. I.

Table I - Upper and lower bounds for optimisation

variables Figure 11 Joint angles: present approach.
n1, n2 b1, b2
lb -0.2m -0.5m rad  rad
ub 0.4m 0.2m  rad  rad

The optimised result for the positions is

(51)

while the optimal intermediate Cardan angles are

Figure 12 Normalised joint velocities: present approach.

41
 ISSN 1590-8844
International Journal of Mechanics and Control, Vol. 15, No. 01, 2014

Figure 13 Normalised torques: present approach. Figure 15 Normalised joint velocities:

MUSCOD-II solution.
4.2 MUSCOD-II SOLUTION
An alternative solution for the same PtP problem is solved
with the numerical solver MUSCOD-II, see [9] for details.
The two point boundary value problem

(53)

subject to

(54)
(55)
Figure 16 Normalised torques: MUSCOD-II solution.
(56)
(57) 4.3 COMPARISON
(58) A comparison between both methods shows, that joint
(59) positions start at the same values, while the end position of
the joints angles is different, cp. Figs. 11 and 14.
(60)
Obviously, this effects the velocities. Regarding the motor
torques (Figs. 13, 16), one can mention that in both cases,
is solved with the well known multiple shooting method. In
is most of the time either in positive or negative
contrast to the dynamic programming approach, presented
boundary. In the MUSCOD-II solution, the maximum
in section 2.3 here additionally the motor torques are
torques are more often at their boundaries, while the joint
included to the cost functional, to achieve good
velocities are in a quite similar range. An interesting and
convergence. However the weighting
important comparison point is the total energy consumption
is chosen to get (nearly) time optimal
solutions. The results for the MUSCOD-II optimisation are
presented in Figs. 14, 15 and 16. (61)

for this specific task. Table II shows the summed up

comparison for both algorithms.

Table II - Comparison: present approach,

MUSCOD-II solution
Present Approach MUSCOD-II Solution
tcpu 27 sec 150 sec
te 0.69 sec 0.66 sec
Energy 2270 Ws 2840 Ws

Figure 14 Joint angles: MUSCOD-II solution.

42
 ISSN 1590-8844
International Journal of Mechanics and Control, Vol. 15, No. 01, 2014

For the present approach, the task requires 20% lesser [4] Constantinescu D. and Croft E. A., Smooth and Time-
energy. Of course, this is a specific result for the chosen optimal Trajectory Planning for Industrial
PtP problem and has to be evaluated for appropriate test Manipulators Along Specified Paths, Journal of
scenarios to make a general statement. The task time is Robotic Systems, 17(5):233–249, 2000.
for both methods nearly the same. The time indicates [5] De Boor C., A Practical Guide to Splines, Springer
the computational time for solving the optimisation Verlag, 1978.
problem on a standard PC. This time is about a factor of 5 [6] Gattringer H., Starr-elastische Robotersysteme:
lower for the present approach. There is also a code Theorie und Anwendungen, Springer Verlag, 2011.
optimisation potential, since at the moment only non-
[7] Gattringer H., Riepl R. and Neubauer M., Optimizing
optimised C- code is used.
Industrial Robots for Accurate High-Speed
Applications, Journal of Industrial Engineering,
5 CONCLUSION 2013:1–12, 2013.
This paper shows a method to get time optimal PtP [8] Geu Flores F. and Kecskemethy A., Time-Optimal
trajectories for industrial robots. The main idea is to Path Planning Along Specified Trajectories, In
calculate optimal CP trajectories and vary in a second step Gattringer H. and Gerstmayr J., eds, Multibody System
intermediate points with a nonlinear solver. For the CP Dynamics, Robotics and Control, pages 1–16,
trajectories, we suggest a dynamic programming approach, Springer Verlag, 2012.
since it delivers fast solutions. However, it is not important, [9] Leineweber D., Bauer I., Schäfer A., Bock H. and
how the time-optimal CP trajectories are computed. So the Schlöder J., An Efficient Multiple Shooting Based
approach may also be interesting for robot manufacturers Reduced SQP Strategy for Large-Scale Dynamic
that have a solution for the CP problem and want to extend Process Optimization (Parts I and II), Computers and
it to the PtP problem. A comparison with results evaluated Chemical Engineering, 27:157–174, 2003.
with the multiple shooting method shows the effectivity of [10] Oberherber M., Gattringer H. and Springer K., A
the approach and nearly the same end-time for the Time Optimal Solution for the Waiter Motion
trajectory. However, what has to be done in future is a Problem with an Industrial Robot, Proceedings of
proof of optimality which is not part of this paper. Austrian Robotics Workshop 13, Vienna, pp. 55–60,
Including disturbing objects for specific cases will also be 2013.
done in future works. [11] Pfeiffer F., Mechanical System Dynamics, Springer
Verlag, 2008.
ACKNOWLEDGMENTS [12] Pfeiffer F. and Johanni R., A Concept for Manipulator
Trajectory Planning, IEEE Journal of Robotics and
This work has been supported by the Austrian COMET-K2 Automation, 3(2):115–123, 1987.
program of the Linz Center of Mechatronics (LCM), and
[13] Piegl L. A. and Tiller W., The NURBS Book, Springer
was funded by the Austrian federal government and the
Verlag, 1995.
federal state of Upper Austria.
[14] Shin K. and McKay N., Minimum-Time Control of
Robotic Manipulators with Geometric Path
REFERENCES Constraints, IEEE Transactions on Automatic
[1] Bellman R. E. and Dreyfus S. E., Applied Dynamic Control, 30(6):531–541, 1985.
Programming, Princton Univ. Press, 1962. [15] Verscheure D., Diehl M., De Schutter J. and Swevers
[2] Bobrow J. E., Dubowsky S. and Gibson J. S., Time- J., Recursive Log-barrier Method for On-line Time-
Optimal Control of Robotic Manipulators Along optimal Robot Path Tracking, Proceedings of 2009
Specified Paths, International Journal Robotics American Control Conference, pp. 4134–4140, 2009.
Research, 4:3–17, 1985. [16] Johnson St. G., The nlopt Nonlinear-Optimization
[3] Bremer H., Elastic Multibody Dynamics: A Direct Ritz Package, URL http://ab-initio.mit.edu/nlopt.
Approach, Springer Verlag, 2008. [17] Powell M. J. D., The BOBYQA Algorithm for Bound
Constrained Optimization without Derivatives,
Technical Report NA2009/06, 2009.

43