740 IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 20, NO.

2, APRIL 2015

Minimum-Time Trajectory Planning and Control

of a Pick-and-Place Five-Bar Parallel Robot
Francis Bourbonnais, Pascal Bigras, and Ilian A. Bonev, Senior Member, IEEE

Abstract—This paper presents trajectory planning optimization

and real-time control of a special five-bar parallel robot. Planning
is based on a cubic spline stochastic approach that minimizes tra-
jectory time and selects the best combination of working mode
regions to circumvent all parallel singularities, allowing the size of
the workspace to be increased. Identification of the dynamic model
of the robot and its actuators allows a precise implementation of
the trajectory planning and real-time control approach. The opti-
mization algorithm achieves a fast trajectory and a controller that
operates with an error of less than 0.7◦ for both actuated joints of
the robot.
Index Terms—Cubic spline stochastic optimization, five-bar
mechanism, identification, parallel robot, real-time controller, tra-
jectory planning, workspace.

I. INTRODUCTION
ARALLEL robots are more and more used as master piece
P in various applications [1], [2]. These robots are generally
faster, stronger, and more accurate than serial robots. However,
their workspaces are smaller and much more complex, since
these robots include not only serial singularities, but also parallel
singularities. Serial singularities are configurations in which the
end-effector loses one or more degrees of freedom. These singu-
larities constitute the working mode boundaries, each working
mode corresponding to one of the four solutions of the inverse
kinematics problem. Parallel singularities are configurations in
which the actuators cannot compensate for a force or moment
applied to the end-effector. These singularities reduce the size
of the workspace, since they are inside it.
Several methods have been proposed to increase the size of
the workspace of parallel robots. One of these consists of de-
signing the geometry of the robot in such a way that it contains
no parallel singularities [3]. It is also possible to remove them,
but only in the workspace of one working mode (a subspace)
[4], [5]. Even if this subspace does not contain any singular-
ities [6], the resulting workspace will be smaller than all the
Manuscript received May 28, 2013; revised October 25, 2013 and January
31, 2014; accepted March 26, 2014. Date of publication May 8, 2014; date of
current version October 24, 2014. Recommended by Technical Editor J. Berg.
This work was supported in part by Fonds québécois de la recherche sur la nature
et les technologies and in part the Natural Sciences and Engineering Research
Council of Canada.
F. Bourbonnais is with the Automation and Robotics Centre, GE Aviation,
Bromont, QC J2L 1S6, Canada (e-mail: francis.bourbonnais@ge.com).
P. Bigras and I. A. Bonev are with the Department of Automated Manufac-
turing Engineering, École de Technologie Supérieure, Montreal, QC H3C 1K3,
Canada (e-mail: pascal.bigras@etsmtl.ca; ilian.bonev@etsmtl.ca).
Color versions of one or more of the figures in this paper are available online
at http://ieeexplore.ieee.org.
Digital Object Identifier 10.1109/TMECH.2014.2318999
1083-4435 © 2014 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
Authorized licensed use limited to: ULAKBIM UASL
See - MIDDLE EAST TECHNICAL UNIVERSITY.
http://www.ieee.org/publications
standards/publications/rights/index.html 30,2023
Fig. 1. DexTAR robot.
working mode subspaces combined. Another way to increase
the size of the workspace of a parallel robot would be to use
redundant actuators [7], thus reducing or even eliminating paral-
lel singularities. However, adding motors and gearboxes would
significantly increase the cost of the robot.
When the task consists only of a pick-and-place operation,
there are other ways to increase the size of the workspace. One
of these is to pass through parallel singularities using an external
force, which cannot be provided by the actuators in that specific
case. The force of inertia of the robot itself is a common choice
for this external force [8], [ 9]. However, precision is lost around
the singularities, since the actuators cannot be used to control
the robot in these regions. Thus, the workspace becomes seg-
mented. Another approach is to pass through serial singularities,
in order to change the working mode. Since different working
modes generally have different parallel singularities [10], [11],
changing the working mode several times along the trajectory
can allow the robot to circumvent the parallel singularities to
the extent possible. Thanks to the actuated joints, these working
mode changes can be easily made in the joint space by passing
through the serial singularities [9].
We have built a new high-accuracy five-bar robot called Dex-
TAR (see Fig. 1) to validate this approach. The mechanical de-
sign and workspace analysis of this robot are presented in [12].
In this paper, we present our trajectory planning and real-time
control approach for DexTAR in detail. With respect to joint
space trajectory planning, the best combination of working
Downloaded on November for
moreatinformation.
17:18:29 UTC from IEEE Xplore. Restrictions apply.
 BOURBONNAIS et al.: MINIMUM-TIME TRAJECTORY PLANNING AND CONTROL OF A PICK-AND-PLACE FIVE-BAR PARALLEL ROBOT 741

Fig. 2. Procedure for solving the direct kinematics. Fig. 3. One DexTAR leg (i = 1, 2).

mode regions is selected to circumvent all parallel singularities, In Fig. 2, the solutions associated with the plus and minus
unfeasible regions, and mechanical interferences. Our approach, signs are represented by black and gray lines, respectively.
which is based on the cubic spline stochastic method presented
in [13], takes into account the velocity and torque constraints III. INVERSE KINEMATIC MODEL
of the robot, as well as its dynamic model, in order to minimize The inverse kinematic model will be used to determine the
the trajectory time. complete joint space and serial singularities. According to Fig. 3,
The remainder of the paper is organized as follows: Sections there is an inverse kinematic solution for leg i when the follow-
II and III present the kinematic and inverse kinematic model of ing Euclidian distance:
DexTAR, respectively. Section IV presents the workspace and 
joint space of the robot, including all the mechanical interfer- σi = rO C − rO O i  = (xO C − xO O i )2 + (yO C − yO O i )2
ences, singularities, and unfeasible regions. A model identifica- (6)
tion method for the robot and its actuators is presented in Section is less than 2l, for i = 1, 2. In this case, there are two solutions
V. This model is used in Section VI to formulate the trajectory for each active joint variable:
plan, and again in Section VII to design the controller.
qi = atan2 (yO A i − yO O i , xO A i − xO O i ) (7)
II. DIRECT KINEMATIC MODEL where, referring to Fig. 3,
 
The direct kinematic model will be used to determine the xO A i
= rO A i = rO O i + fi ± hi (8)
complete workspace and parallel singularities of the robot. In yO A i
order to avoid voids in the workspace of the robot around points
O1 and O2 (on Fig. 2), the proximal and the distal links have rO C − rO O i
fi = (9)
the same length (l). Referring to Fig. 2, the following vectors 2
are defined: and
      
xO A i xO O i + l cos qi 4l2 − σi2 0 −1
rO A i = = , i = 1, 2. (1) hi = fi . (10)
yO A i yO O i + l sin qi σi 1 0

The direct kinematic problem is solved when the following The two solutions for each active joint variable can be com-
Euclidian distance: bined, for a total of four solutions, corresponding to the four
 working modes.
σ = rO A 1 − rO A 2  = (xO A 1 −xO A 2 )2 +(yO A 1 −yO A 2 )2
(2) IV. WORKSPACE AND JOINT SPACE
is less than 2l. When σ is equal to 2l, a parallel singularity The four working mode regions of DexTAR are illustrated in
occurs. Assuming that σ ≤ 2l, the vectors f and h, illustrated in Fig. 4. Each working mode region covers a different portion of
Fig. 2, are defined as follows: the complete workspace, and its boundary can be determined
rO A 2 − rO A 1 by calculating the subregions corresponding to the serial and
f= (3) parallel singularities. To pass from one working mode region to
2
another one, it is necessary for the robot to cross a serial singu-
and
√   larity (in this study, parallel singularities are avoided). For these
4l2 − s2 0 −1 reasons, the DexTAR workspace can be extended by allowing
h= f. (4)
s 1 0 working mode changes. The complete workspace is therefore
the union of all working mode regions (see this Java applet:
The two solutions, corresponding to the two assembly modes,
http://www.parallemic.org/Java/FiveBar.html).
are then given by
The joint space corresponding to each working mode can
rO C = rO A 1 + f ± h. (5) be obtained using the inverse kinematic model. There may be

Authorized licensed use limited to: ULAKBIM UASL - MIDDLE EAST TECHNICAL UNIVERSITY. Downloaded on November 30,2023 at 17:18:29 UTC from IEEE Xplore. Restrictions apply.
 742 IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 20, NO. 2, APRIL 2015

Fig. 4. Working mode regions (light red, light blue, light green, and light
yellow); parallel singularity loci (red lines); serial singularity loci (dark green
lines); and unfeasible regions for the current assembly mode (gray).

several corresponding positions in the joint space (possibly one

for each working mode) for every desired Cartesian position in
the workspace. The trajectory planning presented in Section VI
consists of finding the best joint space trajectory from an initial
joint position to one of the several possible final joint positions
that avoids all unfeasible regions and mechanical interferences.
The obtained trajectory often crosses serial singularities, which,
fortunately, have no impact for the control in the joint space. The
unfeasible regions are bordered by parallel singularities, which
Fig. 5. Working mode regions in the enlarged joint space (light red, light blue,
cannot be crossed, and by positions that are beyond the robot’s light green, and light yellow); parallel singularities (red lines); serial singularities
reach. (dark green lines), and unfeasible regions (white).
In DexTAR (the actual prototype), the motion range of the
actuated joints is unlimited and they can rotate more than 360◦ .
To take this feature into account in trajectory planning, the
joint space must be enlarged by considering the existence of
a maximum of 36 neighboring joint positions for each end-
effector position. In fact, for each end-effector position, there are
four working modes, but also nine neighboring joint positions
for each of them, since each of the two joints can be: 1) in its
current position or 2) one turn forward or 3) one turn backward.
Fig. 5 shows a partial view of the enlarged joint space in the Fig. 6. Reduced working mode regions (light red, light blue, light green, and
−360◦ to +360◦ range for both joints. light yellow); and mechanical and virtual interferences (black).
In practice, each working mode region must be constrained to
avoid mechanical interferences. As we explain in detail in [12], bined with viscous terms. The magnetic effects [14] and com-
interferences occur when DexTAR’s two proximal links are too plex friction terms are ignored, since the robot is only used at
close and when the angle between its two distal links is too high speed (when the torques are dominated by a simple friction
small. Moreover, to avoid the problem of poor conditioning, model and the dynamics of the robot). Accordingly, the model
additional virtual interferences have been introduced to ensure of actuator i (i = 1, 2) can be approximated as follows:
adequate performance of the robot controller. These virtual in-
terferences, which represent the positions where the parallel τi = ki vi = ji q̈i + bi q̇i + ci sign(q̇i ) (11)
singularities are closed, were determined experimentally, and
where vi is the input voltage of drive i; ki is the motor gain com-
occur when the angle between the two distal links is less than
bined with the drive gain (which transforms the input voltage vi
18◦ or greater than 162◦ . The workspace regions, constrained
to torque τi ); ji is the inertia of the rotor; bi is the viscous coef-
by these interferences, are illustrated in Fig. 6.
ficient; ci is the Coulomb coefficient; sign() is the sign function;
The trajectory planning approach presented in Section VI will
and q̇i and q̈i are the angular velocity and acceleration of the
take these constrained regions into account. The dynamic model
rotor of motor i, respectively. This model can be rewritten in the
presented in the next section will also be taken into account, in
following compact form:
order to respect the physical limits of the actuators and minimize
the overall task time. τi = ki vi = Ymi (q̇i , q̈i )pmi (12)
T
where Ymi = [q̈i q̇i sign(q̇i )] and pmi = [jj bi ci ] . Since the
V. DYNAMIC MODEL AND ITS IDENTIFICATION parameters provided by the manufacturer are not accurate, the
DexTAR’s two actuators are direct drive brushless motors model must be identified. However, the identification problem
combined with electronic drives which are used in current mode. (12) does not have a unique solution when ki , ji , bi , and ci are
The friction of the robot is the Coulomb approximation com- unknown, because both sides of the equation are multiplied by

Authorized licensed use limited to: ULAKBIM UASL - MIDDLE EAST TECHNICAL UNIVERSITY. Downloaded on November 30,2023 at 17:18:29 UTC from IEEE Xplore. Restrictions apply.
 BOURBONNAIS et al.: MINIMUM-TIME TRAJECTORY PLANNING AND CONTROL OF A PICK-AND-PLACE FIVE-BAR PARALLEL ROBOT 743

unknown parameters. In this case, as proposed in [15] for the the last column of Yr is eliminated, so that the dimension of pr
identification of the dynamics of a robot combined with actua- is 7. From this reduction, the last term of pr is also eliminated,
tors, it is possible to combine the model of the complete system but can be expressed as a function of its other terms. It would
with another model that includes an additional known load. also be possible to reduce again the number of parameters based
This combination makes it possible to transform the problem on physical feasibility conditions [19]. In the context of a direct
into a form that provides a unique solution. For DexTAR, it was adaptive controller, a different linear form of the dynamic model
simpler to reformulate this approach and add the known load can also be obtained. In [16], such a formulation is proposed for
directly to the actuators alone (i.e. the robot arms are removed). a five-bar mechanism.
In fact, to ensure that DexTAR’s actuators can turn more than To identify the model of the actuators with and without the
360◦ , many restrictions had to be respected in order to fix an additional known load combined with the dynamic model of the
additional load at an appropriate position on the robot. Thus, by robot, five datasets must be collected:
adding a known load directly to the actuators alone, the model 1) Ai : Inputs vi and outputs q̇i and q̈i of actuator i, for i = 1,
given by (12) becomes 2, obtained by a sufficiently rich input trajectory without
the additional known load.
ki vi = Ymi (q̇i , q̈i )pmi + jL i q̈i (13)
2) Li : Inputs vi and outputs of q̇i , and q̈i actuator i, for i =
where jL i is the additional inertia, which is assumed known. 1, 2, obtained by a sufficiently rich input trajectory with
With this scheme, it is possible to collect experimental data the additional known load.
with and without additional inertia, and combine models (12) 3) M: Inputs v and output q, q̇, and q̈ of the robot obtained
and (13) to form an identification problem that can yield a by a sufficiently rich input trajectory.
unique solution for all the actuator parameters. However, since Datasets Ai and Li are obtained from the actuators alone (i.e.,
the trajectory planning and controller need the dynamic model not assembled with the robot) while the dataset M is obtained
of the robot, the actuator model has to be combined with the from the actuators assembled with the robot arms. With these
robot model in order to identify parameters of the robot as well. five datasets, the model for identifying the complete system
The dynamic model of the DexTAR robot can be expressed in (actuators and robot) for joint i can be obtained by combining
the following common form: models (12), (13), and (15)
⎡ (Ai ) ⎤ ⎡ (A ) ⎤⎡ ⎤
M(q)q̈ + F(q, q̇) = τ (14) vi 0 Ymi i 0 1/ki
⎢ (Li ) ⎥ ⎢ ⎥⎢
where M is the mass matrix, F is the vector of the Coriolis, cen- ⎢ vi ⎥ = ⎢ jk i q̈(Li) (L )
Ymi i 0 ⎥ 1/ki pmi ⎥
⎣ ⎦ ⎣ i ⎦⎣ ⎦ (16)
trifugal, gravity, and friction forces, and τ is the vector of the (R) (R) (R) 1/k p
vi 0 Ymi Yr i i r
input torque, while q, q̇, and q̈ are the generalized coordinates
and their corresponding velocities and accelerations, respec- where
tively. This model takes into account the constraint associated ⎡ ⎤ ⎡ ⎤
(Ai 1 ) (Ai 1 ) (Ai 1 )
with the mechanical loop closure. Accordingly, the generalized vi Ymi (q̇i , q̈i )
⎢ ⎥ ⎢ ⎥
=⎢ .. ⎥ , Y(Ai ) = ⎢ .. ⎥
(Ai )
coordinates are only the angle of each of the two actuated joints vi ⎣ . ⎦ mi ⎣ . ⎦
at the base of the robot. For more details about the dynamics (Ai # ) (A ) (A )
vi Ymi (q̇i i # , q̈i i # )
model of a five bars mechanism, see for example [16]. Gen-
⎡ ⎤ ⎡ ⎤
erally, the dynamic model of the robot can be expressed as a (L )
vi i 1
(L )
q̈i i 1
linear function of a set of dynamic parameters which are simple ⎢ . ⎥ (Li ) ⎢ . ⎥
=⎢ ⎥ =⎢ ⎥
(Li )
functions of the physical parameters of each member, like mass, vi ⎣ .. ⎦ , q̈i ⎣ .. ⎦
(Li # ) (Li # )
position of the center of mass, and inertia [17]. So, the model vi q̈i
given by (14) can be rewritten as ⎡ ⎤ ⎡ ⎤
(L ) (L ) (M )
Ymi (q̇i i 1 , q̈i i 1 ) vi 1
Yr (q, q̇, q̈)pr = τ (15) ⎢ ⎥ (M) ⎢ . ⎥
=⎢ .. ⎥,v =⎢ ⎥
(L )
Ymi i ⎣ . ⎦ i ⎣ .. ⎦ ,
where Yr is a matrix, in which each term is a nonlinear function (L ) (L ) (M )
of q, q̇, and q̈, and pr is the vector of the dynamic parameters. Ymi (q̇i i # , q̈i i # ) vi #
The friction parameters of the actuated joints were omitted, ⎡ ⎤
(M ) (M )
Ymi (q̇i 1 , q̈i 1 )
since they are considered in the actuator model. Moreover, the ⎢ ⎥
=⎢ .. ⎥ , and
(M)
friction of the passive joints were not considered, since they Ymi ⎣ . ⎦
were negligible compared to those of the actuators. Then, since (M ) (M )
Ymi (q̇i # , q̈i # )
DexTAR is a parallel planar robot, the vector pr should be ⎡ ⎤
formed by combining the four parameters of the two serial Yr i (q(M1 ) , q̇(M1 ) , q̈(M1 ) )
planar arms that compose the robot [18]. If this is the case, (M) ⎢ .. ⎥
Yr i =⎣ . ⎦
the dimension of pr should be 8. However, it is not, since the
Yr i (q(M# ) , q̇(M# ) , q̈(M# ) )
constraint formed by the mechanical loop closure can reduce the
rank of the matrix Yr [17]. This reduction is generally difficult and where S1 and S# designate the indices of the first and last
to calculate analytically, and is usually obtained by numerical element of the set S, respectively. This system includes all in-
simulation combined with QR decomposition. In this process, formation to simultaneously identify the parameters of both the

Authorized licensed use limited to: ULAKBIM UASL - MIDDLE EAST TECHNICAL UNIVERSITY. Downloaded on November 30,2023 at 17:18:29 UTC from IEEE Xplore. Restrictions apply.
 744 IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 20, NO. 2, APRIL 2015

actuators and the robot. However, although the vector of the

robot parameters pr is the same for every joint, since each actu-
ator gain ki is assumed to be different, every complete vector of
parameters pi = [1/ki 1/ki pTmi 1/ki pTri ]T must be assumed
to be different as well. This overparameterization implies a pos-
sible elimination of particular terms in pr i , since the rank of
Yr i may be less than that of Yr . For this reason, one column
of Yr 1 and one of Yr 2 were eliminated. Consequently, the cor-
responding elements of pr i were eliminated too. The model for
identifying joint i, defined by (16), can then be rewritten in the
following compact form:
vi = Yi pi . (17) Fig. 7. Position trajectory of the actuated joints 1 and 2 that was used for the
identification of the robot parameters.
In order to filter data and eliminate the acceleration terms
included in Yi , a low-pass filter can be applied on both sides of
(17). In fact, as explained in [17], it is possible to combine the
derivative operator with the filter to obtain a causal operation that
calculates the filtered accelerations from the velocities (i.e.,g ⊗
q̈i = ġ ⊗ q̇i , where g is the impulse response of the filter and ⊗
is the convolution operator). However, a sufficiently low cutoff
frequency for the filter must be chosen to reduce the noise, but
sufficiently large so as not to eliminate critical information. The
filter is often a first-order low-pass filter, which can be expressed
as the following Laplace transfer function:
wc
G(s) = (18)
s + wc
where s is the Laplace variable and wc is the cutoff frequency.
Assuming that the parameter vector pi is constant, after the
filtering operation, the identification model is rewritten as
vf i = Yf i pi (19)
where vf i and Yf i are the filtered version of vi and Yi , re-
spectively. The least squares method [17] can then be applied to
identify the vector pi Fig. 8. Input voltage v of the acquired datasets A i , L i , and M, and the corre-
sponding estimation error (in black) for actuated joint 1.
−1
p̂i = YfT i Yf i YfT i vf i (20)
where p̂i is, as stated previously, the vector of estimated param-
eters that respects the following partition:
T
p̂i = [ 1/k̂i 1/k̂i p̂Tmi 1/k̂i p̂Tri ] . (21)
It is also possible to apply the weighted least squares ap-
proach, in order to ensure better numerical conditioning [17].
For the identification process, 20 000 samples were acquired
to make up set Ai , 20 000 samples to make up set Li , and 14 000
samples to make up set M. For the set M, the joint trajectories
were obtained using the trajectory planning presented in Sec-
tion VI in order to cover the complete workspace excluding
interferences and parallel singularities. For this, approximate
dynamic model obtained from the CAD model of the robot was
used. Also, a PID controller was used to ensure an approximate
tracking. The obtained trajectory is shown in Fig. 7 and the
corresponding input voltages are shown in Figs. 8 and 9.
For the acquisition, the sampling time was 1 ms, and the cutoff
frequency of the filter was fixed to wc = 100 rad/s. To acquire
the samples of set Li , a known load jL i of 0.00336 kg · m2 was Fig. 9. Input voltage v of the acquired datasets A i , L i , and M, and the corre-
considered. In order to take into account the nonsymmetrical sponding estimation error (in black) for actuated joint 2.

Authorized licensed use limited to: ULAKBIM UASL - MIDDLE EAST TECHNICAL UNIVERSITY. Downloaded on November 30,2023 at 17:18:29 UTC from IEEE Xplore. Restrictions apply.
 BOURBONNAIS et al.: MINIMUM-TIME TRAJECTORY PLANNING AND CONTROL OF A PICK-AND-PLACE FIVE-BAR PARALLEL ROBOT 745

Fig. 10. Pick-and-place points (green circles) and optimal trajectories (blue
lines) in the joint space obtained in [21].

friction terms, two parameters, associated with the positive part
of the velocity and the positive part of its sign, respectively,
were added to the actuator model described by (12). The input
trajectories for sets Ai , Li , and M are illustrated in Figs. 8 and 9.
The RMS estimation error for joint 1 was 0.0274 V, while it was
0.0283 for joint 2. These results could be improved by optimiz-
ing the identification trajectory as proposed in [20]. However,
the constraints associated with interferences and parallel singu-
larities of DexTAR would have to be taken into account in this
optimization process.
Fig. 11. Pick-and-place points (green circles) and optimal trajectories (blue
lines) in the workspace obtained in [21].
programming (NP) methods performed significantly worse than
the stochastic cubic spline method (SCS).
In fact, trajectory times were 0.27 and 0.236 s for SL and NP,
respectively, when it was 0.176 s for SCS. For this reason, just
the SCS approach [13] is presented in this paper.
According to (12), (15), and (21), the torque of joint i corre-
sponding to the desired trajectory can be expressed as

τid = Ymi (q̇id (t), q̈id (t))p̂mi + Yr i (qd (t), q̇d (t), q̈d (t))p̂r i
(22)
VI. TRAJECTORY PLANNING
where qd (t), q̇d (t), and q̈d (t) are the position vector, the veloc-
Pick-and-place trajectories are usually generated so that the ity vector, and the acceleration vector of the desired trajectory
robot tool picks up an object at an initial position and moves it, in the joint space, respectively. By defining a normalized time
as quickly as possible, to a final position in the robot workspace. 1
scale t̄ = (t − t0 ), such as t̄ ∈ [0, 1], this dynamics can be
Using the inverse kinematic model, the initial and final positions T
can be transformed into the robot joint space, where the trajec- rewritten as [13]
 
tory can be planned without any restriction concerning serial d 1 d 1  d
singularities, since they have no impact in the joint space. τi = Ymi q̄ (t̄), 2 q̄i (t̄) p̂mi
T i T
The first objective of trajectory planning, which we present  
in this section, is to calculate a smooth joint space trajectory 1  1 
+Yr i q̄d (t̄), q̄ d (t̄), 2 q̄ d (t̄) p̂r i (23)
starting at the initial position and finishing at one of the fi- T T
nal positions qf j , where j indicates the final position in the
where the normalized trajectory is defined by
extended joint space. In fact, as explained in Section IV, Dex-
TAR is a parallel robot in which a final Cartesian position can be q̄d (t̄) = qd (T t̄ + t0 )
achieved through multiple joint positions, of which we must test
36 (four modes multiplied by nine possibilities in the extended  dqd (T t̄ + t0 )
q̄ d (t̄) = = T q̇d (T t̄ + t0 )
joint space). Another objective is to minimize the trajectory time dt̄
T under the constraints of maximal torque τim ax and maximal  d2 qd (T t̄ + t0 )
velocity q̇im ax . Finally, the trajectory should completely avoid q̄ d (t̄) = = T 2 q̈d (T t̄ + t0 ). (24)
dt̄2
crossing over parallel singularities and regions of interference.
By convenience, for the following formulation, the grouping Since the dynamic model is linear according to the joint ac-
of all the regions that should be avoided for working mode j celerations, and quadratic according to the joint velocities, (23)
is defined by Π(j). In [21], we compared different optimiza- can always be rewritten in the following form:
tion methods applied to the trajectory planning of DexTAR. As α1i (t̄) α2i (t̄)
shown in Figs. 10 and 11, the straight line (SL) and nonlinear τid = α0i (t̄) + + . (25)
T T2

Authorized licensed use limited to: ULAKBIM UASL - MIDDLE EAST TECHNICAL UNIVERSITY. Downloaded on November 30,2023 at 17:18:29 UTC from IEEE Xplore. Restrictions apply.
 746
Then, according to (24) and (25), when the normalized desired
trajectory q̄d (t̄) is assumed to be known, the optimization time
problem can be formulated as follows:
P q̄d : T ∗ = min(T )
under the constraints
1   d 
q̄ (t̄) < q̇im ax ∀t̄ ∈ [0, 1]
T i
1   d 
q̄ (t̄) < q̈im ax ∀t̄ ∈ [0, 1]
T2 i
 
 
α0i (t̄) + α1i (t̄) + α2i (t̄)  < τim ax ∀t̄ ∈ [0, 1]
 T T 
2 TABLE III
q (t̄) ∈
d
/ Π̄(t̄) ∀t̄ ∈ [0, 1]
where Π̄(t̄) = Π(j(T t̄ + t0 )), with j(t) being the working
mode in the extended joint space at actual time t. In order to
solve the optimization problem defined by (26)–(29), the nor-
malized time can be discretized. Then, for each discretization
point, where the solution exists, the minimization problem (26)
under constraints (27)–(29) can be solved easily, since (26)–(29)
are linear and quadratic forms of T . From the results at each dis-
cretization point, the optimization in the complete discretized
space can then be obtained by choosing the worst case. For
further details, see [13]. The last constraint of problem P(q̄d )
defined by (30) is, however, independent of T . It can be re-
spected or not, depending on the normalized trajectory q̄d (t̄),
which is assumed to be known for this optimization problem.
This constraint can be verified by the discretization of the 2-D
joint space. Now, when q̄d (t̄) is not known, the optimization
problem not only depends on the trajectory time T , as explained
previously, but also on the normalized trajectory itself. Since
the optimal choice of q̄d (t̄) defines a nonlinear problem, it
may not be easy to find. Moreover, it is coupled with the opti-
mization problem P(q̄d ). This is why, for simplicity, a subopti-
mal stochastic cubic spline approach [13] can be used to deter-
mine the normalized trajectory q̄d (t̄), as well as the trajectory
time T .
To consider this approach, let the normalized trajectory q̄d (t̄)
be defined by the following cubic spline:
q̄d (t̄) = CS (q0 , qf , {q1 , ..., qk } , {t̄1 , ..., t̄k })
where CS is a cubic spline parameterized by the initial posi-
tion q0 , the final position qf , and a set of intermediate nodes
q1 , q2 ,. . ., qk with t̄1 , t̄2 , ..., t̄k being corresponding normal-
ized node times. The times associated with q0 and qf are 0 and
1, respectively, whereas t̄1 , t̄2 , ..., t̄k are distributed within the
open interval]0, 1[. (Node times are generally equidistant.) The
approach proposed in [13] consists of adding random points to
the set of nodes sequentially, calculating the corresponding cu-
bic spline, and solving the optimization problem P(q̄d ) at each
time, in order to obtain the best trajectory. The following algo-
rithm details this approach for the final point qf j corresponding
to working mode j in the extended joint space.
Step0: Set k = 0, Tinner = ∞, Touter = ∞, and choose nm ax ,
the maximum number of nonimproving iterations of the inner
Authorized licensed use limited to: ULAKBIM UASL - MIDDLE EAST TECHNICAL UNIVERSITY. Downloaded on November 30,2023 at 17:18:29 UTC from IEEE Xplore. Restrictions apply.
IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 20, NO. 2, APRIL 2015
TABLE I
CONSTRAINTS FOR TRAJECTORY OPTIMIZATION
(26)
TABLE II
(27) PARAMETERS OF THE OPTIMIZATION ALGORITHM
(28)
(29)
POINTS ON THE PICK-AND-PLACE TRAJECTORY
(30)
loop, and Q, the size of the square used to determine the distri-
bution of the intermediate nodes.
Step1: Set k=k +1 and n = 0.
Step2: Set the node qk to belong to a random set distributed
uniformly in the smallest rectangle that includes the nodes ql
and qm corresponding to tl and tm , the two closest neighbors
on each side of tk .
Step3: For each qr , for r = 1,. . ., k − 1, set a new value for qr
to belong to a random set distributed uniformly in the rectangle
of qr previously obtained from Step2, for which the size is
multiplied by Q and the center is fixed at the current value
of qr .
Step4: Calculate the new normalized trajectory q̄d (t̄) by using
(31) with the initial node q0 , the final node qf =qf j , and the
intermediate nodes obtained in Steps2 and 3.
Step5: Solve the optimization problem P(q̄d ). If there is no
solution, then set n=n + 1 and go to Step7.
Step6: If T ∗ < Tinner , then set Tinner = T ∗ , qinner =
{q1 , ..., qk } and go to Step2. Else, set n=n + 1.
Step7: If n < nm ax , then go to Step2.
Step8: If Tinner < Touter , then set Touter = Tinner , qouter =
qinner , and go to Step1. Else, the best solution is given by the
(31) normalized trajectory (31) with {q1 , ..., qk } =qouter combined
with the trajectory time T = Touter . END.
To obtain the optimal result, the algorithm is applied for each
final point qf j corresponding to working mode j in the extended
joint space. The best trajectory is then chosen. Even though this
algorithm has demonstrated very good results in practice [13], it
may not converge. If this is the case, new feasible starting nodes
of the cubic spline can be obtained with a more deterministic
algorithm like A∗ . For more details, see [21]. In order to execute
a pick-and-place task composed of several points, the algorithm
can be applied as many times as there are points. For each point,
a pause can be added to allow the pick-and-place operation to
take place.
Using this algorithm with the constraints in Table I and the
parameters in Table II, a trajectory is planned for a pick-and-
place operation composed of the seven points given in Table III.
 BOURBONNAIS et al.: MINIMUM-TIME TRAJECTORY PLANNING AND CONTROL OF A PICK-AND-PLACE FIVE-BAR PARALLEL ROBOT
Fig. 12. Pick-and-place points (green circles), optimal trajectory (blue line)
and interference regions (black) in the extended joint space.
Fig. 13. Optimal trajectory for joint 1 and 2, and the angle between the two
distal links (in red).
These points and the optimal trajectory are shown in Figs. 12–
14. Figs. 12 and 13 show the trajectory in the joint space, while
Fig. 14 shows it in the workspace. Moreover, Fig. 13 shows the
angle between the two distal links, which is outside the virtual
interferences (defined in Section IV), i.e., inside the range [18◦ ,
162◦ ]. As can be seen, the trajectory completely avoids the inter-
ference and unfeasible regions (in black and white, respectively)
by crossing several serial singularities to pass through several
working mode regions. The time minimization function has al-
lowed the complete trajectory to be executed in 2.7 s, including
all pauses. Also, even though points 0, 4, and 6 are identical in
the workspace and are in the same working mode region, they
are not the same in the extended joint space. For example, the
algorithm determined that it was quicker to go from point 5 to
point 6, which is point 4 shifted by one turn forward for each
motor, than to go back to point 4.
Authorized licensed use limited to: ULAKBIM UASL - MIDDLE EAST TECHNICAL UNIVERSITY. Downloaded on November 30,2023 at 17:18:29 UTC from IEEE Xplore. Restrictions apply.
747
Fig. 14. Pick-and-place points (green circles), optimal trajectory (blue line),
unfeasible regions (black and white), and feasible region (light brown) in the
workspace.
Fig. 15. Hardware used to implement the controller.
VII. CONTROLLER
The hardware used to implement the controller is shown in
Fig. 15. The electrical drives and an IO card constitute the
interface between the robot’s motor and a real-time computer
(XPC OS). Another computer, running Windows, is used with
Simulink software to edit the controller program. The Windows
computer is linked to the XPC computer by a TCPIP port. For
further details, see [12].
To ensure fast tracking performance of the trajectory planned
using the approach presented in Section VI, the dynamic model
of DexTAR must be taken into account in the control law design.
For that, it would be possible to control the five-bar mechanism
 748
Fig. 16. Trajectory error for joints 1 and 2.
with, for example, adaptive control [16], [22] or iterative learn-
ing control [23]. However, since the load of DexTAR is assumed
to be approximately constant and the trajectory is not repetitive,
a simple computer torque control law has been chosen. Based
on the model given by (14), this control law has the following
very well-known form [17]:
τ = M(q)u + F(q, q̇)
where u is given by the following PID control law:
 t
u = q̈d + Kd q̃˙ + Kp q̃ + Ki q̃(τ )dτ
0 [2] J. A. Saglia, N. G. Tsagarakis, J. S. Dai, and D. G. Caldwell, “Control
where q̃ = qd − q is the joint position error, and Kp , Ki , and
Kd are the gain matrices of the controller, which are generally
diagonal positive. According to the following error dynamics,
the controller gains can easily be chosen to ensure stability and
performance:
...
¨ + Kp q̃˙ + Ki q̃ = 0.
q̃ + Kd q̃ (34)
For the implementation presented here, the gains were chosen
to place all the eigenvalues at −60. In order to take into account
the combined identification of the robot and actuators presented
in Section V, the control law (32) can be modified according to
(12), (15), and (21)
1 pp. 2295–2300.
vi = (Ymi (q̇i , ui )p̂mi + Yr i (q, q̇, u)p̂r i )
k̂i
where the estimated parameters k̂i , p̂mi , and p̂r i are easily cal-
culated from the estimation vector given by (21).
The trajectory obtained in Section VI was used to validate the
controller performances. The error between the desired trajec-
tory (shown in Fig. 13) and the actual trajectory obtained by the
real-time implementation of the controller is shown in Fig. 16.
For both joints, the error is always less than 0.7◦ . The robot
was also validated with a PID controller without computer
torque control and dynamic model identification. The joint error
in this case was approximately ten times greater than the error
for our controller, which justifies the identification process and
nonlinear control law implementation.
A video demonstrating DexTAR and its controller is available
at http://youtu.be/dnixuCu49o4.
Authorized licensed use limited to: ULAKBIM UASL - MIDDLE EAST TECHNICAL UNIVERSITY. Downloaded on November 30,2023 at 17:18:29 UTC from IEEE Xplore. Restrictions apply.
IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 20, NO. 2, APRIL 2015
VIII. CONCLUSION
The trajectory planning and real-time control of DexTAR, a
special five-bar robot, was presented in this paper. Direct and in-
verse kinematic models were used to find the workspace and the
joint space, as well as the singularities and interference regions.
The dynamic model of the robot and its actuators were then
identified, and that model was used for trajectory planning and
real-time controller implementation. In planning the trajectory,
a stochastic optimization method was used to minimize the tra-
jectory time under the constraints of velocity, and torque, as well
as the parallel singularities, the interferences, and the unfeasi-
ble region. For trajectory optimization, the algorithm selects the
best working mode regions along the trajectory, allowing the
workspace to be enlarged.
ACKNOWLEDGMENT
The authors would like to thank S. Briot, a researcher at
IRCCyN, CNRS, France, for his valuable help concerning the
identification method.
(32)
REFERENCES
[1] O. Bebek, H. Myun Joong, and M. C. Cavusoglu, “Design of a paral-
lel robot for needle-based interventions on small animals,” IEEE/ASME
(33) Trans. Mechatronics, vol. 18, no. 1, pp. 62–73, Feb. 2013.
strategies for patient-assisted training using the ankle rehabilitation robot
(ARBOT),” IEEE/ASME Trans. Mechatronics, vol. 18, no. 6, pp. 1799–
1808, Dec. 2013.
[3] M. Arsenault and R. Boudreau, “The synthesis of three-degree-of-freedom
planar parallel mechanisms with revolute joints (3-RRR) for an optimal
singularity-free workspace,” J. Robot. Syst., vol. 21, pp. 259–274, 2004.
[4] S. Sen, B. Dasgupta, and A. K. Mallik, “Variational approach for
singularity-free path-planning of parallel manipulators,” Mech. Mach.
Theory, vol. 38, pp. 1165–1183, 2003.
[5] A. K. Dash, I. M. Chen, S. H. Yeo, and G. Yang, “Workspace generation
and planning singularity-free path for parallel manipulators,” Mech. Mach.
Theory, vol. 40, pp. 776–805, 2005.
[6] X.-J. Liu, J. Wang, and G. Pritschow, “Performance atlases and optimum
design of planar 5R symmetrical parallel mechanisms,” Mech. Mach.
Theory, vol. 41, pp. 119–144, 2006.
[7] S. Kock and W. Schumacher, “Parallel x-y manipulator with actuation
redundancy for high-speed and active-stiffness applications,” in Proc.
IEEE Int. Conf. Robot. Autom., Leuven, Belgium, May 16–20, 1998,
(35) [8] T. Yasuda, D. N. Nenchev, K. Aida, and H. Tamura, “Experiments with
a parallel robot with singularity-perturbed design,” in Proc. 26th Annu.
Conf. IEEE Electron. Soc., Nagoya, Japan, Oct. 22–28, 2000, pp. 217–222.
[9] J. Hesselbach, M. B. Helm, and S. Soetebier, “Connecting assembly
modes for workspace enlargement,” in Advances in Robot Kinematics,
J. Lenarcic and F. Thomas, Eds. Dordrecht, The Netherlands: Kluwer,
2005, pp. 347–356.
[10] D. Chablat and P. Wenger, “Regions of feasible point-to-point trajec-
tories in the Cartesian workspace of fully-parallel manipulators,” pre-
sented at the ASME Des. Tech. Conf., Las Vegas, NV, USA, 1999, Paper
DETC99/DAC-8645.
[11] I. A. Bonev and C. M. Gosselin, “Singularity loci of planar parallel manip-
ulators with revolute joints,” in Proc. 2nd Workshop Comput. Kinematics,
Seoul, South Korea, 2001, pp. 291–299.
[12] L. Campos, F. Bourbonnais, I. A. Bonev, and P. Bigras, “Development of
a five-bar parallel robot with large workspace,” in Proc. ASME Int. Des.
Eng. Tech. Conf. Comput. Inf. Eng. Conf., Montreal, QC, Canada, Aug.
15–18, 2010, pp. 917–922.
[13] T. Chettibi, M. Haddad, and S. Rebai, “A stochastic off line planner of
optimal dynamic motions for robotic manipulators,” Informat. Control,
Autom. Robot., vol. 1, pp. 73–80, 2006.
 BOURBONNAIS et al.: MINIMUM-TIME TRAJECTORY PLANNING AND CONTROL OF A PICK-AND-PLACE FIVE-BAR PARALLEL ROBOT 749

[14] G. Ferretti, G. Magnani, and P. Rocco, “Modeling, identification, and
compensation of pulsating torque in permanent magnet AC motors,” IEEE
Trans. Ind. Electron., vol. 45, no. 6, pp. 912–20, Dec. 1998.
[15] M. Gautier and S. Briot, “Global identification of drive gains parameters
of robots using a known payload,” in Proc. IEEE Int. Conf. Robot. Autom.,
2012, pp. 2812–2817.
[16] C. Long, L. Yingzi, H. Zeng-Guang, T. Min, H. Jian, and W. J. Zhang,
“Adaptive tracking control of hybrid machines: A closed-chain five-
bar mechanism case,” IEEE/ASME Trans. Mechatronics, vol. 16, no. 6,
pp. 1155–1163, Dec. 2011.
[17] W. Khalil and E. Dombre, Modeling, Identification and Control of Robots.
London, U.K.: HPS, 2002.
[18] J. J. E. Slotine and W. Li, Applied Nonlinear Control. Englewood Cliffs,
NJ, USA: Prentice-Hall, 1991.
[19] M. Diaz-Rodriguez, A. Valera, V. Mata, and M. Valles, “Model-based
control of a 3-DOF parallel robot based on identified relevant parameters,”
IEEE/ASME Trans. Mechatronics, vol. 18, no. 6, pp. 1737–1744, Dec.
2013.
[20] J. Swevers, C. Ganseman, D. B. Tukel, J. De Schutter, and H. Van Brussel,
“Optimal robot excitation and identification,” IEEE Trans. Robot. Autom.,
vol. 13, no. 5, pp. 730–740, Oct. 1997.
[21] F. Bourbonnais, “Utilisation optimale de l’espace de travail des
robots parallèles en affrontant certains types de singularités [ressource
électronique],” Master’s dissertation, École de Technologie Supérieure,
Montréal, QC, Canada, 2012.
[22] T. Le, H.-J. Kang, and Y.-S. Suh, “An online self gain tuning computed
torque controller for a five-bar manipulator,” in Advanced Intelligent Com-
puting, vol. 6838, D.-S. Huang, Y. Gan, V. Bevilacqua, and J. Figueroa,
Eds. Berlin, Germany: Springer, 2012, pp. 538–543.
[23] Z. Bin, C. Jianbin, and Z. Zhencai, “Dynamic simulation of hybrid-driven
planar five-bar parallel mechanism based on simmechanics and tracking
control,” Int. J. Adv. Robot. Syst., vol. 8, pp. 28–33, 2011.
Pascal Bigras received the B.Eng. and M.Eng. de-
grees from the Department of Electrical Engineering,
École de Technologie Supérieure (ÉTS), Montréal,
QC, Canada, in 1991 and 1993, respectively, and the
Ph.D. degree in automatic control from the École
Polytechnique de Montréal, Montreal, in 1997.
He is currently an Associate Professor in the De-
partment of Automated Manufacturing Engineering,
ÉTS. His research interests include nonlinear and ro-
bust control and their applications.
Ilian A. Bonev (SM’10) received the B.Eng. degree
in manufacturing engineering from TU-Sofia, Sofia,
Bulgaria, in 1996, the M.Sc. degree in mechatronics
from the Gwangju Institute of Science and Technol-
ogy, Gwangju, Korea, in 1999, and the Ph.D. degree
in mechanical engineering from Laval University
Quebec, QC, Canada, in 2002.
He joined the École de Technologie Supérieure,
Montréal, QC, Canada, as an Assistant Professor in
2004 and was recently promoted to the rank of Pro-
fessor. He holds a Canada Research Chair in precision
robotics. His research interests include the field of modeling and design of paral-
lel robots, as well as the area of robot calibration. He is a Fellow of the Canadian
Society for Mechanical Engineering.

Francis Bourbonnais received the B.Eng. and

M.Eng. degrees from the Department of Automated
Manufacturing Engineering, École de Technologie
Supérieure, Montréal, QC, Canada, in 2009 and 2012,
respectively.
He is currently a Member of the Global Robotics,
Automation and Instrumentation R&D Center, GE
Aviation, Bromont, QC, Canada. His research inter-
ests include parallel robots, singularities, and path
planning.

Authorized licensed use limited to: ULAKBIM UASL - MIDDLE EAST TECHNICAL UNIVERSITY. Downloaded on November 30,2023 at 17:18:29 UTC from IEEE Xplore. Restrictions apply.