Redundancy Resolution in Minimum-time Path Tracking of Robotic
Manipulators
Alexander Reiter, Hubert Gattringer and Andreas M¨uller
Institute of Robotics, Johannes Kepler University Linz, Altenberger Straße 69, Linz, Austria
Keywords:
Robotics, Optimal Control, Trajectory Planning, Redundant Robots, Inverse Kinematics.
Abstract:
Minimum-time trajectories for applications where a geometric path is followed by a kinematically redundant
robot’s end-effector may yield economical improvements in many cases compared to conventional manipula-
tors. While for non-redundant robots the problem of finding such trajectories has been solved, the redundant
case has not been treated exhaustively. In this contribution, the problem is split into two interlaced parts:
inverse kinematics and trajectory optimization. In a direct optimization approach, the inverse kinematics
problem is solved numerically at each time point. Therein, the manupulator’s kinematic redundancy is ex-
ploited by introducing scaled nullspace basis vectors of the Jacobian of differential velocities. The scaling
factors for each time point are decision variables, thus the inverse kinematics is solved optimally w.r.t. the
trajectory optimization goal, i.e. minimizing end time. The effectiveness of the presented method is shown by
means of the example of a planar 4R manipulator with two redundant degrees of freedom.
1 INTRODUCTION
In industrial applications such as painting, welding or
gluing a geometric end-effector path is defined leav-
ing only the problem of finding a suitable time evolu-
tion of the joints of the executing robot. Introduc-
ing minimum-time trajectories may yield economi-
cal advantages as a shorter trajectory duration results
in a lower task cycle time. Kinematically redun-
dant manipulators provide favorable properties such
as increased workspace dexterity and improved task-
specific adaptiveness compared to conventional, non-
redundant manipulators. The topic of time optimal
trajectory planning has been studied in a large num-
ber of publications. While for non-redundant ma-
nipulators this problem has been solved, e.g. (Bo-
brow et al., 1985; Shin and McKay, 1985; Pfeif-
fer and Johanni, 1986), for redundant robots no sat-
isfying methods have been proposed yet. Concepts
of minimum-time trajectory planning for kinemati-
cally redundant manipulators can be largely separated
into two method families: joint space and workspace-
based techniques. Members of the former group as-
sume, that the path following problem is solved as an
equality constraint to the trajectory optimization pro-
cess. Alternatively, a joint space parametrization is
assumed to be available (Pham, 2014). In the lat-
ter group, methods incorporate inverse kinematics.
Due to the mathematical representation of redundant
robots’ kinematics, solutions are often obtained nu-
merically (Li´egeois, 1977). Redundancy allows to
augment such solutions by adding objectives such as
maximizing performance measures, e.g. directional
dynamic manipulability in (Chiacchio, 1990). How-
ever, the choice of a performance measure is crucial
as it must act as a local proxy for the superseding time
minimization. Methods relying on joint space decom-
position (Wampler, 1987) offer computation of ana-
lytic inverse kinematics for certain manipulator struc-
tures. There are various joint space decomposition ap-
proaches, some have drawbacks such as the inability
to process certain paths, or boundary conditions, c.f.
(Ma and Watanabe, 2004). Others rely on diffeomor-
phisms that may be difficult to obtain, c.f. (Galicki,
2000). Summarizing, the unsolved problem is often a
kinematic one, particularly the redundancy resolution
in the inverse kinematics problem poses difficulties.
In Section 2 of this paper, the problem of com-
puting minimum-time joint trajectories for tracking
a kinematically redundant serial manipulator’s pre-
scribed end-effector path is formulated. Section 3
discusses methods to fulfill the path tracking con-
straints. The main contribution of this paper is pre-
sented in Section 4. A method is introduced wherein
the path following requirement is treated using an
inverse kinematics scheme underlying the trajectory
Reiter, A., Gattringer, H. and Müller, A.
Redundancy Resolution in Minimum-time Path Tracking of Robotic Manipulators.
DOI: 10.5220/0005975800610068
In Proceedings of the 13th International Conference on Informatics in Control, Automation and Robotics (ICINCO 2016) - Volume 2, pages 61-68
ISBN: 978-989-758-198-4
Copyright
c
2016 by SCITEPRESS Science and Technology Publications, Lda. All rights reserved
61
optimization problem. The numerical inverse kine-
matics scheme is augmented by optimally scaled
nullspace basis vectors of the instantaneous differen-
tial velocity Jacobian in order to obtain time-optimal
trajectories. This new approach is applied to the ex-
ample of a planar manipulator with two redundant de-
grees of freedom in Section 5. Therein, the trajectory
planning problem is formulated using direct multiple
shooting (Bock and Plitt, 1984) and solved using a
modern interior-point method. Unfavorable proper-
ties of time integration and appropriate countermea-
sures are also discussed. Section 6 concludes this con-
tribution and gives insight in possible enhancements
to the proposed method.
The algorithms are presented using an accelera-
tion level inverse kinematics for the sake of brevity
but can be similarly formulated for higher-order
derivatives.
2 PROBLEM DESCRIPTION
2.1 Kinematically Redundant
Manipulators
The configurationof a robotic manipulatoris uniquely
defined using coordinates q
i
,i = 1... n in the configu-
ration space V
n
, i.e. q
= (q
1
,...,q
n
) V
n
. The end-
effector pose z
E
can be described with its Cartesian
position r
E
R
3
and its orientation, denoted by the
rotation matrix R
E
, i.e. z
E
= (R
E
,r
E
) SO(3) × R
3
instead of SE(3). The forward (or direct) kinematics
mapping f : V
n
7→ SO(3) × R
3
maps joint configura-
tions to end-effector poses.
The robot’s workspace W is given as the im-
age of the direct kinematics mapping f, i.e. W =
{C imf|h(q) 0} SO(3) × R
3
wherein only
geometrically admissible configurations (inequality
constraints h) are considered. The workspace dimen-
sion is given as m = dimW.
A serial manipulator is considered kinematically
redundant if n > m, i.e. the configuration space is
of higher dimension than the workspace. For non-
redundant manipulators (where n = m holds) the in-
verse mapping of the forward kinematics, f
1
, is
well-defined, and in special cases evengiven in closed
form, e.g. for standard 6R robots with a spherical
wrist. In the case of redundant robots, one has to re-
sort to different, mostly numerical or iterative, meth-
ods.
2.2 Optimal Trajectory Planning for
Prescribed End-effector Paths
The goal of minimum-time trajectory optimization
is to find the shortest possible time evolution of the
considered manipulator’s joint positions such that a
given end-effector path is tracked while being re-
stricted to technological limitations. In the follow-
ing it is assumed that the path is given by means of
a series of desired poses, continuously parametrized
with a scalar path parameter s [0, 1], i.e. z
E,d
(s) =
(R
E,d
(s),r
E,d
(s)) : R 7→ SO(3) × R
3
. The index d
denotes desired quantities. This yields a non-linear
optimization problem (NLP) of the form
min
x
t
f
Z
0
1dt (1)
s.t. M(q)
¨
q+ g(q,
˙
q) = Q (2)
q
min
q q
max
(3)
˙
q
min
˙
q
˙
q
max
(4)
¨
q
min
¨
q
¨
q
max
(5)
Q
min
Q Q
max
(6)
0 s 1 (7)
s(0) = 0,s(t
f
) = 1 (8)
˙s 0 (9)
z
E,d
(s) = f (q) (10)
q(0) = q
0
,q(t
f
) = q
f
(11)
˙
q(0) =
˙
q
0
,
˙
q(t
f
) =
˙
q
f
(12)
wherein x represents the vector of optimization vari-
ables describing the time evolution of the joint posi-
tions q. Declarations of dependencies of x will be
omitted below. The optimization problem is subjected
to the manipulator’s (in general non-linear) dynamics
denoted as the equations of motion (2) with the vec-
tor of minimal coordinates q and its time derivatives
˙
q and
¨
q. M is the system’s inertia matrix, g represents
non-linear terms in the equations of motion, consist-
ing of the Coriolis, centrifugal, gravitational and dis-
sipative effects. Q indicates the vector of generalized
forces and torques. Limitations of the manipulators
joint positions in (3), joint velocities (4) and possi-
bly higher derivatives such as joint accelerations (5)
may also be incorporated. Further bounds are applied
to the generalized forces Q in (6). The desired end-
effector path is prescribed using a monotonically in-
creasing (9), bounded (7) path parameter. (10) repre-
sents the aforementioned path tracking requirement.
Typically, there are also initial and final (11), (12)
constraints of the robot’s joint positions and their time
derivatives. In addition, constraints for cyclic tasks
can be formulated as q
0
= q
f
and
˙
q
0
=
˙
q
f
.
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
62
This optimization problem can be decomposed
into the trajectory optimization problem and an un-
derlying path-tracking subproblem.
3 PATH FOLLOWING
In a minimum-time path tracking optimization prob-
lem, the path following constraint can be imposed as
an equality constraint (10). Other approaches assume
a known joint space parametrization to be known,
e.g. (Pham, 2014). Alternatively, an inverse kine-
matics mapping f
1
: W 7→ V
n
can be applied to ob-
tain joint quantities from workspace quantities. As
mentioned in Section 2.1, no closed-form solution
to the inverse kinematics problem exists for kine-
matically redundant manipulators. However, there
are other approaches such as joint space decompo-
sition (Wampler, 1987; Ma and Watanabe, 2004)
or Jacobian-based numeric methods (Whitney, 1969;
Li´egeois, 1977).
3.1 Path Following and Inverse
Kinematics
In order to resolve the path following requirement us-
ing inverse kinematics, divide-and-conquer as well
as unite-and-conquer methods can be applied. Joint
space decomposition can be used as a divide-and-
conquer type approach. Therein, a manipulators
structure has to be explicitly separated into two or
more parts. Then the inverse kinematics problem
can be solved based on a loop closure condition.
Joint space decomposition makes direct use of kine-
matic redundancy as operations are performed on
joint level. The choice of decomposition may not be
straight-forward and thus a result of an superseding
integer program. The inverse kinematics solution can
be performed analytically only in cases with suitable
geometry but not in general. Also, the enforcement
of the aforementioned loop closure condition is non-
trivial.
In unite-and-conquer methods such as differen-
tial inverse kinematics, firstly introduced in (Whit-
ney, 1969), a least-squares solution (w.r.t. an end-
effector error quantity) yields all joint quantities at
once. However, this family of methods needs to
be augmented in order to exploit kinematical redun-
dancy. In this paper, the latter type of inverse kine-
matics methods is used.
The derivations of differential inverse kinematics
schemes below are well-known but reproduced here
as an introduction to and a motivation for the main
contribution of this paper presented in Section 4.
The most simple case is first-order differential in-
verse kinematics,
˙
r
E
= J(q)
˙
q, (13)
wherein J =
˙
r
E
˙
q
R
m×n
denotes the forward kine-
matics Jacobian, a non-square, wide matrix. Thus it
is not invertible, but an approximate solution for the
joint velocities
˙
q can be computed minimizing the er-
ror in the least-squares sense, i.e.
˙
q = J
+
˙
r
E,d
(14)
wherein J
+
= J
JJ
1
denotes the right
Moore-Penrose pseudoinverse. Alternatively,
the dynamically consistent pseudoinverse
J
+
M
= M
1
J
JM
1
J
1
can be used (Khatib,
1988). Compared to (13), for (14) the index d was
added to the end-effector velocity as it is now a given,
desired quantity. For computing the matrix inverse
of
JJ
in singular configurations, a regularization
term can be introduced, i.e. J
+
= J
JJ
+ κI
1
with a small κ > 0. In general, non-singular config-
urations the nullspace of J has dimension n m > 0,
i.e. the manipulator is capable of internal motion
that does not affect the end-effector motion. This
property can be exploited using an inverse kinematics
scheme (Li´egeois, 1977) that is augmented to pursue
additional goals. Scalar performance measures w
such as kinematic manipulability (Yoshikawa, 1985b)
or dynamic (Yoshikawa, 1985a) manipulability can
be maximized by adding a velocity term to (14).
This velocity points in the direction of v =
w
q
and is
projected into the nullspace of the Jacobian, i.e.
˙
q = J
+
˙
r
E,d
+ Nv (15)
with the nullspace projector N = (I JJ
+
). I denotes
the identity matrix of appropriate size. Substituting
(15) in (13) shows that no end-effector motion re-
sults from the additional term. Pose-dependent per-
formance measures such as kinematic or dynamic ma-
nipulability suffer from the fact that they only rep-
resent a local, instantaneous property. As a result,
they can hardly be exploited in the course of an super-
seding trajectory optimization problem minimizing a
global property such as a trajectory time.
Similar inverse kinematics approaches can be set
up for higher time derivatives simply by deriving (13)
w.r.t. time and isolating the highest time derivative
of the joint positions q, e.g. an acceleration-level ap-
proach yields
¨
q = J
+
¨
r
E,d
˙
J
˙
q
+ Nv (16)
wherein v can again represent a performance measure
gradient projected into the Jacobian nullspace.
Redundancy Resolution in Minimum-time Path Tracking of Robotic Manipulators
63
3.2 Numerical Inverse Kinematics in
Trajectory Planning
In trajectory planning tasks, constraints regarding
derivatives of the joint positions q may be imposed,
e.g. zero joint velocities at the end-effector final po-
sition. By close examination of (16), it can be found
that such a constraint is not necessarily fulfilled as a
vector v may not have the appropriate magnitude to
cancel all internal accelerations. Thus, a scaling fac-
tor γ needs to be introduced to fulfill such require-
ments, i.e. for the case of an acceleration-level ap-
proach
¨
q = J
+
¨
r
E,d
˙
J
˙
q
+ γNv. (17)
The nullspace scaling factor γ introduced in (17)
needs to vary over time in order to stick to multiple
constraints across the trajectory, i.e. γ = γ(t).
4 NULLSPACE BASIS SCALING
Using the above methods, the joint state is changed
such that a performance measure w is maximized lo-
cally, i.e. following the instantaneous gradient
w
q
,
projected into the current Jacobian nullspace. Even
if the step size γ is adjusted properly, this may not
yield an optimal joint state evolution across the path.
If a manipulator provides more than one redundant
degree of freedom, the projection of the gradient will
always lie in a subspace of the nullspace. To make use
of remaining free nullspace directions, task priority-
based methods (Nakamura et al., 1987) can be used to
pursue additional (ideally non-conflicting) goals with
lower priorities.
The key idea of the present approach is to com-
pute a basis for the Jacobian nullspace, i.e. kerJ =
span{a
i
},i = 1,.. .,(n m). The basis vectors a
i
are
then scaled by factors γ
i
,i = 1,..., (n m) and added
to the inverse kinematics solution (17), i.e.
¨
q = J
+
¨
r
E,d
˙
J
˙
q
+
nm
i=1
γ
i
(t)a
i
. (18)
For manipulators with a kinematic redundancy of
n m = 1, there is only one basis vector of the
nullspace. Thus, redundancy is fully exploited by
both approaches, performance measure-based meth-
ods and nullspace basis scaling. However, for higher
degrees of redundancy n m > 1, exploiting the full
nullspace as in (18) enables an superseding optimiza-
tion process to directly modify the joint trajectories
according to the criteria to be minimized. In contrast
to other approaches, there is no need of a projected
x in m
y in m
0
0.5
1
1.5
2
2.5
3
1
0.5
0
0.5
1
Figure 1: Planar manipulator with four revolute joints in
initial configuration.
performance measure gradient acting as a proxy func-
tion to pursue the optimization goals. As for both,
performance measure gradients and the nullspace ba-
sis vectors, symbolic expressions can be obtained be-
forehand by means of computer algebra systems, the
difference in computational cost is negligible.
Section 5 shows that this approach can be easily
applied to problems with readily available kinematic
and dynamical models.
In the method development (13) to (18), only po-
sition coordinates were treated as workspace coordi-
nates for simplicity. Adding a prescribed end-effector
orientation increases the complexity of the problem.
The differential inverse kinematics needs also to be
computed for the end-effector’s angular velocity or its
time derivatives. Care has to be taken in order to es-
tablish consistency in the physical units of products
of the Jacobian.
5 EXAMPLE
5.1 Kinematic and Dynamic Model
The method presented in Section 4 is illustrated us-
ing the simple example of the planar manipulator de-
picted in Figure 1, moving along straight line paths.
The robot consists of four revolute joints (4R), its
links have masses m
L,i
and moments of inertia C
L,i
(about their respectivecenters of mass). The joints are
directly actuated by means of motors without mass
and inertia. Damping coefficients d
i
account for joint
friction. The system is not influenced by gravity. Nu-
merical values used for the simulation can be obtained
from Table 1.
A minimal coordinate representation of the ma-
nipulators configuration yields q
= (q
1
,q
2
,q
3
,q
4
)
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
64
Table 1: Parameters of planar manipulator.
symbol description value
m
L,i
link mass 10 kg
l
i
link length 1 m
C
L,i
link moment of inertia m
L,i
l
2
i
/12
d
i
damping coefficient 0.1 Nm/rad
q
i,max/min
joint position limits ±π rad
˙q
i,max/min
joint velocity limits ±2 rad/s
M
i,max/min
joint torque limits ±10 Nm
V = S
4
wherein S represents the 0-sphere of each
joint’s admissible range of positions (q
i,min
,q
i,max
).
The forward kinematics mapping can be easily de-
rived by hand and is given by f. The equations of mo-
tion M(q)
¨
q+g(q,
˙
q) = Q can be derived using well-
known techniques such as the LAGRANGE formalism
or the Projection Equation (Bremer, 1988). The vec-
tor of generalized forces Q
= (M
1
,M
2
,M
3
,M
4
) con-
sists of the motor torques.
In this example, the end-effector position, but
not its orientation is considered as workspace coordi-
nates, i.e. r
E
= (x, y) W = {C imf|q V} R
2
.
Thus, the degree of kinematic redundancy is n m =
2.
5.2 Inverse Kinematics
Applying (18) to the present example yields the in-
verse kinematics law
¨
q = J
+
¨
r
E,d
˙
J
˙
q
+
2
i=1
γ
i
(t)a
i
. (19)
The inverse kinematics laws (19) and above have only
instantaneous, point-wise characteristics. In order to
obtain joint trajectories for a given end-effector path,
(19) has to be evaluated for all
¨
r
E,d
along the path
and time integrated using numerical methods to ob-
tain lower time derivatives, i.e.
¨
q
ˆ
˙
q =
t
f
Z
0
¨
qdt
ˆ
q =
t
f
Z
0
ˆ
˙
qdt. (20)
Numerical time integration introduces workspace
drift errors e = r
E,d
f (
ˆ
q),
˙
e =
˙
r
E,d
J(
ˆ
q)
˙
ˆ
q. This is-
sue can be avoided by adding stabilizing terms to (19)
such that
¨
e+ K
1
˙
e+K
0
e = 0. Re-writing the error dy-
namics in terms of single-order ordinary differential
equations by e
1
= e and e
2
=
˙
e yields
˙
e
1
˙
e
2
=
0 I
K
0
K
1
e
1
e
2
(21)
whose structure can be exploited for pole-placement.
Incorporating the error dynamics scheme (21) into
(19) yields
¨
q = J
+
¨
r
E,d
˙
J
˙
q+ K
1
˙
e+ K
0
e
+
2
i=1
γ
i
(t)a
i
. (22)
In the pseudoinverse J
+
= J
JJ
+ κI
1
regular-
ization is conducted with κ = 10
10
. The matrices K
0
and K
1
were chosen such that all poles of the error dy-
namics are at 2. In this simple case, the nullspace
basis vectors a
i
kerJ(q) can be computed analyti-
cally as functions of the system’s parameters and the
current joint configuration.
5.3 Task
For this example, the robot’s task is to move its end-
effector along straight line paths
r
E,d
= r
0
+ sL
cosϕ
sinϕ
(23)
of length L = 1 m wherein s [0,1] denotes the path
parameter. The task is to be performed for slopes
ϕ = 0, π/4, π/2,.. .,7π/4, c.f. Figure 1. At the ini-
tial point r
0
= (2,0) m the manipulator’s configura-
tion is chosen to be
q
0
= (π/3,2π/3, 0,2π/3) rad.
The task is performed as a minimum-time rest-to-rest
maneuver, i.e.
˙
q(t = 0) =
˙
q(t = t
f
) = 0.
5.4 Direct Multiple Shooting Trajectory
Optimization
In this section, the optimization problem posed in
Section 2.2 is reformulated incorporatingthe specifics
of the used manipulator from Section 5.1, its inverse
kinematics scheme from Section 5.2 and the task de-
fined in Section 5.3. Using direct multiple shooting,
originally developed in (Bock and Plitt, 1984), the
time domain is discretized into N uniform intervals
and scaled with the final trajectory time t
f
as a de-
cision variable. In each interval [t
k
,t
k+1
] the system’s
state is integrated using a fourth-order explicit Runge-
Kutta scheme, denoted as the function f(x
k
(t
f
),u
k
).
Further declaration of dependencies of t
f
are omit-
ted for the sake of brevity. Therein the optimization
system’s state consists of the path parameter and its
time derivative as well as the manipulator’s joint po-
sitions and velocities, i.e. x
k
=
s
k
, ˙s
k
,q
k
,
˙
q
k
. u
k
=
( ¨s
k
,γ
k,1
,γ
k,2
) represents the control input of the opti-
mization problem as piecewise constant functions.
The vector of decision variables x consists of con-
catenations of the intermediate states x
k
,k = 0,.. .,N
and the controls u
k
,k = 0,...,N 1, as well as of the
trajectory’s final time t
f
, i.e.
x
=
x
0
,u
0
,...,x
N1
,u
N1
,x
N
,t
f
. (24)
Redundancy Resolution in Minimum-time Path Tracking of Robotic Manipulators
65
The NLP posed in Section 2.2 can be thus reformu-
lated as
min
x
t
f
(25)
s.t. 0 s
k
1 (26)
˙s
k
0 (27)
q
min
q
k
q
max
(28)
˙
q
min
˙
q
k
˙
q
max
(29)
s
0
= 0,s
N
= 1 (30)
˙s
0
= ˙s
N
= 0 (31)
q
0
=
q
0
(32)
˙
q
0
=
˙
q
N
= 0 (33)
¨
q
k
= J
+
k
¨
r
E,d
(s
k
)
˙
J
k
˙
q
k
+ K
1
˙
e
k
+K
0
e
k
) +
2
i=1
γ
i,k
a
i,k
(34)
Q
min
M(q
k
)
¨
q
k
+ g(q
k
,
˙
q
k
) Q
max
(35)
x
k+1
f(x
k
,u
k
) = 0 (36)
wherein kerJ
k
= span
a
i,k
. The state is constrained
by (26) to (29). There are also initial and final condi-
tions of the state, (30) to (33), but the final joint posi-
tions q
N
are free and obtained as a result of the NLP.
Equation (34) describes the inverse kinematics reso-
lution law used in the state integration and the com-
putation of the inverse dynamics constrained by (35).
As the NLP is implemented as direct multiple shoot-
ing, (36) is required to close the state integration gaps
between adjacent shooting intervals.
For direct multiple shooting, the structure of the
Jacobian matrix of the (in)equality constraints w.r.t.
the decision variables x is blockdiagonal if the order-
ing of x
k
and u
k
is as described in (24). Block diago-
nal matrices enable efficient solution algorithms to be
applied. However, in this special case where the final
time t
f
is also a decision variable, an additional dense
column reduces the matrix sparsity as all constraints
depend on this variable. The corresponding sparsity
pattern for N = 5 is depicted in Figure 2.
As this NLP is non-convex,globally optimal solu-
tions cannot be guaranteed. Solutions obtained from
this problem depend on the initial guess. For the ini-
tial guess of the path parameter s
k
, a linear time evo-
lution was assumed, resulting in a constant velocity
profile for ˙s
k
. For these s
k
and ˙s
k
, an initial guess for
the time evolution of the joint positions q
k
is obtained
using an numerical inverse kinematics scheme on ve-
locity level. The controls u
k
are initialized with zeros.
For efficient numerical solution the NLP was im-
plemented using MATLAB interface to the optimiza-
tion framework Casadi 3.0rc3 (Andersson, 2013) and
solved with Ipopt 3.12.3 (HSL MA27 for linear sub-
problems).
Figure 2: Sparsity pattern of the constraint Jacobian for N =
5 uniform time intervals.
The trajectories obtained by the method described
above are continuously differentiable once w.r.t. time,
i.e. q(t) C
1
as a second-order inverse kinemat-
ics resolution scheme with piecewise constant inputs
u
k
was used. The method can be easily generalized
to higher levels of continuity by simply deriving the
inverse kinematics scheme (22) and adding further
states to the NLP. This is useful for system that require
continuous torques (q(t) C
2
), especially differen-
tially flat elastic systems (q(t) C
4
), c.f. (Springer
et al., 2013).
5.5 Results
The optimization problem posed in the previous sec-
tion was solved with a time discretization of N = 100
uniform intervals. As an example, the obtained trajec-
tories for ϕ = π/2 are shown in Figure 3 for the path
parameter, and in Figure 4 and Figure 5 for the joint
positions and velocities, respectively. The results sat-
isfy the constraints posed in (26) to (36) and provide
a minimum for t
f
. However, this minimum is only lo-
cal as the optimization problem is non-convex. The
resulting joint torques are depicted in Figure 6. It can
be seen that at all times, at least two of the constrained
joint velocities or joint torques are saturated, which is
one of the characteristics of a minimum-time property
of the obtained trajectories. Corresponding snapshots
of the manipulators optimal motion are depicted in
Figure 7.
Iteration counts, computation times obtained us-
ing an INTEL XEON E3-1246 V3 processor as well
as the resulting trajectory end times for all slopes ϕ
can be obtained from Table 2.
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
66
s
˙s
¨s
time in s
path parameter, vel., acc.
0
0.5
1
1.5
2
1
0.5
0
0.5
1
1.5
2
Figure 3: Path parameter s, velocity ˙s, acceleration ¨s (ϕ =
π/2).
q
1
q
2
q
3
q
4
time in s
joint angles in rad
0
0.5
1
1.5
2
3
2
1
0
1
2
3
Figure 4: Joint positions q (ϕ = π/2).
˙q
1
˙q
2
˙q
3
˙q
4
time in s
joint velocities in rad/s
0
0.5
1
1.5
2
1.5
1
0.5
0
0.5
1
1.5
2
Figure 5: Joint velocities
˙
q (ϕ = π/2).
M
1
M
2
M
3
M
4
time in s
Motor torques in Nm
0
0.5
1
1.5
2
10
5
0
5
10
Figure 6: Motor torques Q (ϕ = π/2).
6 CONCLUSION
This study has presented a contribution to the solution
of the time-optimal path following problem for kine-
matically redundant manipulators. In this approach,
the problem is divided into the trajectory optimiza-
tion and an underlying inverse kinematics problem.
x in m
y in m
0
0.5
1
1.5 2
1
0.5
0
0.5
1
Figure 7: Time evolution along the path with ϕ = π/2 (10
snapshots equally distributed in time).
Table 2: Optimization results for all ϕ.
ϕ t
f
in s #it. CPU time in s
0 2.1262 206 29
π/4 2.4746 290 41
π/2 2.4073 206 30
3π/4 1.7304 213 30
π 1.7583 157 22
5π/4 2.5625 222 31
3π/2 1.5966 239 34
7π/4 1.1505 134 19
The former is solved using a numerical computation
scheme, augmented to fully exploit redundancy in an
optimal way such that the latter problem yields op-
timal results. It was discussed that this method is
valuable for robots with multiple redundant degrees
of freedom.
The method was successfully applied to a pla-
nar manipulator with two redundant joints moving its
end-effector along prescribed straight line paths.
In future work, the method proposed in this pa-
per will be applied to more complex, spatial examples
also incorporating prescribed orientations. Regard-
ing the implementation, the multiple shooting method
from Section 5.4 can be refined to use non-uniform
shooting intervals to be included as decision variables
instead of varying t
f
and using uniform time intervals.
This would allow for local adjustment of the time res-
olution and will yield a purely blockdiagonalstructure
in the Jacobian of constraints, c.f. Figure 2.
ACKNOWLEDGEMENTS
This work has been supported by the Austrian
COMET-K2 program of the Linz Center of Mecha-
Redundancy Resolution in Minimum-time Path Tracking of Robotic Manipulators
67
tronics (LCM), and was funded by the Austrian fed-
eral government and the federal state of Upper Aus-
tria.
REFERENCES
Andersson, J. (2013). A General-Purpose Software Frame-
work for Dynamic Optimization. PhD thesis, Aren-
berg Doctoral School, KU Leuven.
Bobrow, J., Dubowsky, S., and Gibson, J. (1985). Time-
optimal control of robotic manipulators along spec-
ified paths. International Journal of Robotics Re-
search, 4(3):3–17.
Bock, H. G. and Plitt, K. J. (1984). A multiple shooting
algorithm for direct solution of optimal control prob-
lems. In Proceedings of the IFAC World Congress,
pages 242–247. Pergamon Press.
Bremer, H. (1988). Dynamik und Regelung mechanischer
Systeme. Teubner Studienb¨ucher.
Chiacchio, P. (1990). Exploiting redundancy in minimum-
time path following robot control. In Proc. American
Control Conf., pages 2313–2318.
Galicki, M. (2000). Time-optimal controls of kinemat-
ically redundant manipulators with geometric con-
straints. IEEE Transactions on Robotics and Automa-
tion, 16(1):89–93.
Khatib, O. (1988). Augmented object and reduced effective
inertia in robot systems. In American Control Confer-
ence 1988, pages 2140 – 2147. IEEE.
Li´egeois, A. (1977). Automatic supervisory control of the
configuration and behavior of multibody mechanisms.
IEEE Transactions on Systems Man and Cybernetics,
12:868–871.
Ma, S. and Watanabe, M. (2004). Time optimal path-
tracking control of kinematically redundant manipu-
lators. JSME International Journal, 47(2):582–590.
Nakamura, Y., Hanafusa, H., and Yoshikawa, T. (1987).
Task-priority based redundancy control of robot ma-
nipulators. International Journal of Robotics Re-
search, 6(2):3–15.
Pfeiffer, F. and Johanni, R. (1986). A concept for manip-
ulator trajectory planning. In International Confer-
ence on Robotics and Automation, pages 1399 – 1405.
IEEE.
Pham, Q.-C. (2014). A general, fast, and robust implemen-
tation of the time-optimal path parameterization algo-
rithm. IEEE Transactions on Rob, 30(6):1533–1540.
Shin, K. and McKay, N. (1985). Minimum-time control of
robotic manipulators with geometric path constraints.
IEEE Transactions on Automatic Control, 30(6):531–
541.
Springer, K., Gattringer, H., and Staufer, P. (2013). On
time-optimal trajectory planning for a flexible link
robot. Journal of Systems and Control Engineering,
227(10):751–762.
Wampler, C. (1987). Inverse kinematic functions for redun-
dant manipulators. In IEEE International Conference
on Robotics and Automation, pages 610 – 617.
Whitney, D. E. (1969). Resolved motion rate control of ma-
nipulators and human prostheses. IEEE Transactions
on Man-Machine Systems, 10(2):47 – 53.
Yoshikawa, T. (1985a). Dynamic manipulability of robot
manipulators. In IEEE International Conference on
Robotics and Automation, volume 2, pages 1033–
1038.
Yoshikawa, T. (1985b). Manipulability of robotic mecha-
nisms. International Journal of Robotics Research,
4(2):3–9.
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
68