multi-objective off-line programming of parallel kinematic ... · multi-objective off-line...
TRANSCRIPT
Multi-Objective Off-Line Programming
of Parallel Kinematic Machines
Amar Khoukhi, Luc Baron and
Marek Balazinski
TTeecchhnniiccaall RReeppoorrtt CDT-P2877-05
Abstract
This report deals with the optimal multi-objective off-line programming of Parallel Kinematic
Machines (PKM) under several constraints, including manipulator, workspace, and task models. After
introducing the continuous-time kinematic and dynamic models including actuators model of the robot
based on Euler-Lagrange formalism, the associated discrete-time model is developed. Then a new approach
to PKM trajectory planning problem is considered. It is formulated in a variational calculus framework as an
optimal control problem. It minimizes electrical and kinetic energy and robot traveling time, and maximizes
a measure of manipulability allowing singularity avoidance. Several technological constraints such as
actuator, link length, and workspace limitations, and some task requirements, such as passing through
imposed poses are simultaneously satisfied. The discrete augmented Lagrangian method is used to
transform the resulting non-linear and non-convex constrained multi-objective optimal control to an
unconstrained problem. A new decoupled and linearized formulation is proposed in order to cope with some
difficulties arising from dynamic parameters computation. A systematic implementation procedure is
provided along with some numerical issues. Simulation results proving the effectiveness of the proposed
approach on a two degrees of freedom planar parallel manipulator are then given and discussed.
Keywords ParallelKinematic Machines, Gough-Stewart Platform, Time-Energy Optimal Planning, Dynamic
Off-line Programming, Constrained Optimization, Augmented Lagrangian, Decoupling.
ii
Nomenclature
B : Reference frame attached to the center of mass of the base
A : Reference frame attached to the center of mass of the end-effector (EE)
, : ith attachment point of leg i on body A and B iA iB
: Unit vector along the ith joint axis ie
: Position vector of the origin of frame A relative to frame B in B Tzyx ] [ =p
iA a : Constant position vector of attachment point Ai in frame A.
Tzyx ] [ &&&& =p : Velocity vector of the origin of frame A relative to frame B
ABR : Rotation matrix of frame A with respect to frame B
TT ] [ ψθϕpq = : Position and orientation of A in B = 1x
TT ] [ E ψθϕ &&&&& pq = : Time derivatives of )(1 tx
TTT ] [ ωp&& =q=2x : Cartesian and angular velocity of the EE
T] [ 21 xxx = : Continuous-time state of the PKM
= kx Tkk ] [ 21 xx : Discrete-time state of the PKM
T] ... [ 621 τττ=τ : Cartesian force/torques wrench
Tlll ] ... [ 621=l : Vector of the link lengths
: Jacobian matrix of the PKM J
xJ : Forward Jacobian matrix of the PKM
lJ : Inverse Jacobian matrix of the PKM
, : Inertia matrix expressed in joint and Cartesian space )(qMc)(qM j
iii
),(.qqN j , : Coriolis and centrifugal force/torque in joint and Cartesian space ) ,(
.qqNc
, : Gravity force in joint and Cartesian space )(qGc)(qG j
M , aaM : Actuator inertia matrix and its component
V , aaV : Actuator viscous damping coefficient matrix and its component
K , aaK : Actuator gain matrix and its component
K : Control law gain matrix
mτ : Joint torque vector produced by the DC motors
F : 6-directional column of the generalized forces
p : Ballscrew pitch
n : Gear ratio
sJ , : Ballscrew and motor mass moments of inertia mJ
sb , : Ballscrew and motor viscous damping coefficients mb
λ : Lagrange multipliers associated to state variables
kdf : Approximated function of the discretized robot dynamic model
Ddk
f : Approximated function of the discretized robot decoupled dynamics
: Imposed lth position and orientation )( ll R ,p
maxΘ : Maximum limit of the joint position
μL : Augmented Lagrangian function
DLμ : Augmented Lagrangian function of the decoupled problem
dE : Discrete objective function
DdE : Discrete objective function of the decoupled problem
iv
)( ρσ , : Lagrange multipliers related to equality and inequality constraints
) ,( ΦΨ : Penalty functions associated to equality and inequality constraints
: Penalty coefficients associated to equality and inequality constraints ) ,( gS μμ
) ,( gs : Generic functions defining equality and inequality constraints
C : Admissible domain of input torques
: Admissible domain of sampling periods Η
QU , : Weight matrices associated to electric and kinetic energy criteria
( ,δ ) : Weight factors for traveling time and singularity avoidance criteria 1ι
: Measure of manipulability function w
ϖ : Singularity avoidance function
: Total number of discretizations of the trajectory N
: Total number of iterations of the optimization algorithm *T
: Feasible tolerance for cost minimization tw
: Feasible tolerances for equality and inequality constraints satisfaction t1η
: Feasible tolerance for for equality constraints satisfaction tη
*w : Optimal tolerance for cost minimization
*1η : Optimal tolerance for inequality constraints satisfaction
*η : Optimal tolerance for equality constraints satisfaction
: Tolerance on passage satisfaction through the lth position PlPassTh
T
: Tolerance on passage satisfaction through the lth orientation RlPassTh
T
v
List of Abbreviations
PKM : Parallel Kinematic Machine
GSP : Gough-Stewart Platform
DOF : Degree Of Freedom
FK : Forward Kinematics
IK : Inverse Kinematics
FD : Forward Dynamics
ID : Inverse Dynamics
MOM : Measure of Manipulability
TP : Trajectory Planning
MTE-SF-TP : Minimum Time-Energy Singularity-Free Trajectory Planning
AL : Augmented Lagrangian
List of Figures
Figure 1. Robot Off-line Programming Framework …………………..........................................................15
Figure 2. Example of a of a serial robot……………………………………………………..……………..17
Figure 3. Example of a parallel robot……………………………………………………..………………..17
Figure 4. Example of a parallel robot Flight Simulator: Series 500 by CAE……………….…..….……....17
Figure 5. A schematic representation of a parallel manipulator……… ………………….....……………....20
Figure 6. Illustration of EE passage through imposed poses……...………………………………..…….…27
Figure. 7 Trapezoidal profile for a kinematic trajectory in the operation space…………..……….………..47
Figure 8. Flowchart for the architecture and operation of AL algorithm………………...…...……………..49
Figure 9. A geometric representation of a planar 2-DOF parallel manipulator..…… ………....……………55
Figure 10. Initial solution, velocity profile………………………………….………………………………56
Figure 11. Kinematic simulation results…………….………………………………………………………56
Figure 12. Simulation results with the ALD (Minimum Time-Energy) ….…………………….……..…...57
vi
Figure 13. Simulation results with the ALD (Minimum Energy) ………………………….…....................57
Figure 14. Simulation results of ALD (Minimum Time-Energy) with imposed passage
satisfaction through Cartesian position (0.0, -1.4, (0.4, -1.1), (0.5, -1)………………………..……………57
Figure 15. ALD Simulation with disturbed trajectory (Min Time-Energy) …………………………58
List of Tables
Table 1. Limits of workspace, actuator torques and sampling periods. …....................................................56
Table 2. Convergence history of Minimum Time-Energy Planning with Augmented Lagrangian……..…56
vii
Table of Contents
Abstract…...………………………………………………..…………………………………………..….i
Nomenclature………………………………………………………………………………………….….ii
List of Abbreviations…………………………………………………………………………………..….v
List of Figures…………………………………………………………………………………..…………v
List of Tables……………………………………………………………………………………..……….vi
Table of Contents…………………………………………………………………………………….......vii
1. Introduction ……………………….……………………………………………………………..…...10
2. Offline Programming for Robotic Systems ………………………………………………………....11
2. 1. Approaches to Offline Programming………………………………………………………….11
2. 2. Advantages of Offline Programming………………………………………………………….12
2. 3. Disadvantages of Offline Programming……………………………………………………….13
2. 4. Offline Programming Systems for Parallel Kinematic Machines……………………………..13
3. Modelling…………………………………………….………………………………………………...18
3. 1. Kinematic Modelling ………….………..…….......…............................................................18
3. 1. 1. Displacement Analysis…………………………………………………………......19
3. 1. 2. Velocity Analysis………………………………………………………………......20
3. 1. 2. Acceleration Analysis……………………………………………………………....21
3. 2. Dynamic Modelling ………… ………..…….......…..............................................................22
3. 2. 1. Lagrange Formalism for Inverse Dynamics…………………………………….....22
3. 2. 2. Including Actuator Dynamics……………………………………………………...26
3. 2. 3. Including Contact Forces………………………………………………………......27
3. 2. 4. Discrete Time Dynamic Model……………………………………………………28
4. Multi-Objective Trajectory Planning ……………...…………………………….............................31
4. 1. Constraints Modelling ………… ………..…….......….......................................................31
viii
I. Introduction
In today’s fast paced control and automation world, high performance in flexibility,
agility, cost reducing, quick and accurate planning are increasingly in demand. To be
competitive, companies utilize tools, which quickly provide reliable and accurate information
about an application without the expensive and time consuming set up of a physical prototype.
Robotics is an archetype-engineering field where such tools are a must [1-4]. Indeed, a great
force of robots is their flexibility and ability to rearrange for new tasks. Utilization of robot's
flexibility presupposes effective programming. The ability to program offline and simulate a robot
trajectory prior to production might help eliminating the potential for collisions between robots, parts,
tools, and fixtures, and saving inevitably large amounts of non-productive time if manual programming
were used. In fact, several demanding tasks in manufacturing such as assembly, pick and place,
wilding and screwing are repetitive and there exist room for enhancing productivity while
decreasing production costs. Henceforth, if it is possible to capture the essential components that
govern robot, task and workspace interactions in a mathematical model, predictions regarding the
outcome of the experiment could be made using a computer, instead of a physical robot [5-7].
Moreover, this enables the user to identify the influence of some single parameters upon overall
performance, something that can not be done with a real robot (because there are never two truly
identical situations in the real world!). Therefore, robot off-line programming may be
encapsulated in the two basic levels (Fig. 1). The modeling and approach level, and the
simulation and testing level. In the first level one develops robot kinematics and dynamics,
including actuators models, task model, consisting in starting, ending and intermediate poses and
workspace model including obstacles, tools, humans, or other robots to avoid or to cooperate
with. A feasible approach could be satisfactory if required performances are not too tight.
Otherwise, one must get an optimal approach, and then test how is it robust to changes of the
10
dynamic parameters [6-8]. By including robot dynamics and forces optimization, one can set the
trajectory planning (TP) as an optimal control problem and solve it using advanced non-linear
programming techniques. For serial robots, this approach has been proved as highly effective.
Now, one would like to know how about its generalization to parallel kinematic structures. In this
report, we first outline the pros and cons of offline programming then review the literature
published on the broad subject of robot offline programming with a special emphasis on parallel
manipulators. The problem is then formulated in a variational calculus framework, based on
PKM dynamic model, including actuators and contact forces. The augmented Lagrangian is used
to solve the resulting non-linear and non-convex constrained optimal control problem.
This report contains six sections. In section 2, offline programming systems of robot manipulators
are reviewed . In section 3, PKM modeling is considered. First, kinematic models of the PKM are
developed. This is followed by the continuous time, then the discrete time dynamic models using Euler-
Lagrange formulation. In section 4, the Minimum Time-Energy Singularity-Free Trajectory Planning
(MTE-SF-TP) control problem is formulated in a non-linear programming framework under various robot,
workspace, and task constraints. After decoupling of the robot dynamics, the Augmented Lagrangian
technique is used to solve for the resulting linear and decoupled problem. A systematic implementation
procedure of the augmented lagrangian is also provided. Simulation experiments are presented and
discussed on a case study 2-DOF planar PKM in section 5. Finally, section 6 concludes this work and
gives short-term improvements and future trends.
2. Off-line Programming of Robotic Systems
2. 1. Approaches to Off-line Programming
Over the last two decades, an increasing number of researchers have been working on
computational methods to generate optimal control for general manipulator robots for both offline and
online programming. Robot motion planning had been essentially considered from two different points of
11
view. From computational kinematics and CAD standpoints, it consists merely to assimilate the robot,
workspace, and environment to that of a Windows application using a dedicated CAD/CAM graphics-
based interactive simulation system, with menus, toolbars and icons and implements advanced 3D
modeling, drawing, and simulation tools to obtain as accurate positioning results as possible in 3D space.
Examples of such software packages are CATIA-Robotics, IGRID, RobotMaster, and ROBCAD. The
defined task could then be completed with dynamic and effort forces information, for full success task
achievement [16-19].
From control systems standpoint, the problem consists in finding the optimal torques sequence to
achieve the displacement of the robot from a starting to an ending poses, while optimizing a cost function.
One school of thought to the trajectory planning is that of making the analysis and planning over the phase
space rather than the configuration space. From a state-space representation using a system of differential
equations the trajectory planning is solved by optimizing a performance index, and applying optimal
control theory and variational calculus techniques [7]. Doing so, one can introduce dynamic parameters of
the robot, several criteria and constraints to satisfy in the course of the trajectory planning process. Several
works had been published, especially those dealing with minimum time path planning of serial
manipulators. This is merely justified as to increase production by efficient use of the robot capacity,
which is demonstrated by executing tasks as fast as possible. However, minimum time control is
essentially of Bang-Bang type, which has several drawbacks [20-21]. Several other criteria had been
proposed, such as minimum energy planning, minimum time-energy planning and obstacle avoidance [20-
27].
2. 2. Advantages of Off-line Programming
- A programming process directly applied to a physical robot can be fastidious. One way to save time and
energy is to run the robot task in simulation for later downloading the most successful results, controls and
trajectories to the physical robot for further use [16-18],
- Production can continue while programming, and robots do not have to shut down for very long as the
program for the robot is already written and just needs to be uploaded,
12
- Effective usage of program logics and calculations with state-of-the-art debugging facilities,
- Effective programming by discovering locations and potential problems such as restrictions in the robot
workspace,
- The robot programs are verified in simulation and visualization and any errors are corrected,
- Well documented through simulation
structural stiffness and rigidity. Second, with such structure, it is possible to mount all drives on or near
the base. This results in large payloads capability and low inertia. Indeed, the ratio of payload to the robot
load is usually about 1/10 for serial robots, while only ½ for parallel ones. Despite these advantages,
PKMs are still rare in the industry. Among the major reasons for this gap are the small workspace,
complex transformations between joint and Cartesian space, and singularities as compared to their serial
counterparts Indeed, one of the significant contrasts between serial and parallel manipulators is the nature
of singularities. Serial manipulators have only kinematic singularities obtained by motion of the actuated
joints. These singularities are also called boundary singularities as they are encountered at the workspace
boundaries. For parallel manipulators, boundary singularities appear from the leg limits or the joint limits
depending on the type of manipulator. Moreover, parallel manipulators possess force singularities where
Fig.1. Off-line Programming Framework
they lose some degree of constraint, i.e. gain some degrees of freedom, and become uncontrollable. The
Design-Modeling and Simulation-Testing Levels with Validation by Production Target for a Robot Off-line Programming System
14
analytical condition for force singul larities of the force-transformation
the
is the under consideration of the dynamics of these machines [14].
Th
arities is determined to be the singu
matrices which map the actuator forces/torques at the legs to the output forces/moments at the end-
effector. These facts lead to a tremendous amount of research in PKMs design and customization [28].
On the other hand, due to differences in the areas of application of serial and parallel manipulators,
nature of path-planning problems and their requirements also differ in the two cases. For serial
manipulators, in a path-planning problem, the objective is to move the end-effector from one
configuration to another such that the path remains within the workspace and there is no collision with the
obstacles at any point. In case of parallel manipulators, in addition, the associated question of practical
importance is that of avoiding singularities in the planning of a path for execution of a task. The problem
of singularity avoidance can be posed in the form of a path-planning problem where it is necessary to
avoid singularities, whose information is available in the form of implicit functional relationship among
the joint and task space coordinates.
Another reason recently identified
e mentioned architecture-dependent performance associated with strong-coupled non-linear dynamics
makes the trajectory planning and control system design for PKMs more difficult, as compared to serial
machines. In fact, for serial robots, there is a plentiful literature published on the topics of offline and
online programming, from both stand points: computational geometry and kinematics, and optimal control
including robots dynamics [20-24]. For PKMs, a relatively large amount of literature is devoted to
computational kinematics and workspace optimization issues. The overwhelm criteria considered for
PKMs trajectory planning are essentially design-oriented. These include singularity avoidance and
dexterity optimization [27-35]. In [29], the authors had developed a clustering scheme to isolate and avoid
singularities and obstacles for a PKM path planning. A variational approach is reported in [11] for
planning a singularity-free minimum-energy path between two end-points for Gough-Stewart platforms.
15
Fig. 2. Example of a serial robot M16iB Series by Fanuc Robotics Fig. 3. Example of a parallel robot, F200iB
Courtesy by Fanuc Robotics (http://www.fanucrobotics.com)
Fig. 4. Example of Parallel Robot, Flight Simulator Series 500 by CAE
Courtesy by CAE (http://www.cae.com/www2004/Photo_Gallery/civil.shtml)
16
This method is based on a penalty optimization method. Penalty methods, however, have several
drawbacks [36]. Another major issue for off-line programming and practical use of PKMs in industry (in a
machining process, for example) is that for a prescribed tool path in the workspace, the control system
should guarantee the prescribed task completion within the workspace, for a given set up of the EE (i.e.,
for which limitations on actuator lengths and physical dimensions are not violated). This problem has been
recently considered in [37, 38], where design methodologies involving workspace limitations and actuator
forces optimization, using optimization techniques were introduced. However, none of these methods had
included actuator’s dynamics nor considered multi-objective planning including time, kinetic and electric
energy as well as end-effector passing through imposed positions and orientation constraints.
In this context, an integrated off-line programming approach is developed for Parallel Kinematic
Machines. A decoupling and linearizing approach to PKM multi-objective optimal control is introduced in
order to handle some intractable computation issues within the non linear and non decoupled formulation.
The multi-objective optimization procedure is performed within a proper balance between time and energy
minimization, singularity avoidance, actuators, sampling periods, link lengths and workspace limitations,
and task constraints. From a state space representation by a system of differential equations, the trajectory
planning is formulated using a variational calculus framework. The resulting constrained nonlinear
programming problem is solved using an augmented Lagrangian (AL) with decoupling technique.AL
algorithms have proven to be robust and powerful to cope with difficulties related to non-strictly convex
constraints [40-45] as compared to optimization methods employing only penalty. The decoupling
technique is introduced in order to solve difficult computations, related mainly to the co-states, in the
original coupled formulation. Another advantage of the proposed method is that one might introduce
several criteria and constraints to satisfy in the trajectory planning process.
17
3. Modelling
3. 1. Kinematic Modelling
The kinematic analysis for a PKM mechanism deals with the analysis of the end-effector and
actuating joint motions and the relationships between these two types of motion without considering the
torques or forces that cause these motions. Hence, there are two basic problems in kinematic modeling;
the forward kinematics is the determination of the EE motion from a given motion of the leg lengths,
while the inverse kinematics is the determination of the leg lengths motion from a given motion of EE.
Moreover kinematic analysis might be performed at displacement, velocity and acceleration levels. Note
that throughout this report, words end-effector and platform will be used interchangeably.
The PKM shown in Fig. 5 represents a full 6-Degrees-Of-Freedom (DOF) motion of its EE from its
articulated motion of its six leg lengths defining the joint coordinates by Tllllll ) , , , , ,( 654321=l
where for are the lengths of the six numbered links of the PKM. The pose, i.e., position and
orientation, of the EE, namely,
il 6,...,1 =i
TT ] [ ψθϕpq = can be expressed in B by p the position vector of the
origin of A relative to B, and a set of Euler angles ( ψθφ , , ) defining the orientation of the EE in B,
namely, , as used to uniquely determine the EE orientation. This rotation matrix is given as ABR
⎥⎥⎦
⎤
⎢⎢⎣
⎡−+−+
−−−=
θθψφψφθψφθφψψφθφψφθψφθφψψφθφψ
cscsscscccsssccscsscsccsssccc
))( ),( ),(( ψθϕ ZyZAB RRRR = , (1)
where c and s stand for the cosine and sine functions, respectively. Clearly, the orientation of A is
described with respect to B by a rotation matrix [ ]321 rrr=AB R , where and are, respectively,
the unit vectors along the axes of A.
3r21 , rr
1 3×
The velocity of the EE, namely, , can be obtained as a function of the time derivative of q , i.e., ⎥⎥⎦
⎤
⎢⎢⎣
⎡=
..
ωpq
18
where is the position vector of the ith attachment point Ai on the EE given with
reference to frame A. Once the position of the attachment point Ai is determined, the vector of link i
is obtained as:
iAa ) , ,(
iii aaazyx=
ia iL
(5) iii b aL −=
where is a known 3-vector that represents the coordinate of the base attachment point of link i with
reference to frame B. The length of the ith link is simply calculated as:
ib
il
iiil LL = (6)
ii
i lLe 1
=Furthermore, the leg axis unit vector is obtained in terms of the displacement vector as:ie .
Equations (4-6) constitute the inverse position kinematic model of the PKM, and determine the six link
length for a given state q in Cartesian space, representing the position and orientation of the EE.
3. 1. 2. Velocity Analysis
(9) ω⎥⎦⎤
⎢⎣⎡ +++= T
iiA
AB
iA
ABT
iiA
AB )- ( x )- ( baR aR baR ppp &ii ll &
For , Eq. (7) can be written in the following matrix form: ,..., i 61=
(10) lq && BA =
TTAA
BTAA
BTAA
B
⎟⎟⎠
⎞⎜⎜⎝
⎛−−−×××
=662211
662211 ) ( ... ) ( ) (
bababaeaReaReaR
A
with , , ⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜
⎝
⎛
=.
p
ωq& ) ,..., ,( 621 llldiag=B
is the actuated leg length velocity vector, and is a unit vector along the ith joint axis. Tlll ] , ... , ,[ 6.
2.
1..
=l ie
Equation (10) might be written in a compact form as:
1 qJl && −= (11)
where is the 6x6 inverse Jacobian matrix given as:
1−J
AB 11 −− =J (12)
One notices that since B is a diagonal matrix it is always invertible insofar none of leg lengths is zero.
3. 1. 3. Acceleration Analysis
ω&&& and pFor acceleration analysis, let denote the Cartesian and angular velocities of the EE. Then if we
define as the time derivative of the twist of the EE, upon differentiating both sides of eq. (11),
we obtain the equation governing the relation between joint acceleration and EE acceleration
⎥⎦
⎤⎢⎣
⎡=
p&&&
&&ω
q
l&& q&&
qJ
qJl &&&&& )( 1
1
dtd
−
− += (13)
21
Note that unlike the inverse kinematic problem, the forward kinematics is more challenging for general
PKMs. In fact, there is a full duality between serial manipulators and Gough-Stewart parallel
manipulators. In the formers, the robot Jacobian is completely on the joint or actuation space side, whereas
for Gough-Stewart platforms it is completely on the Cartesian space side. However, for a general PKM,
the number of solutions depends on the number of configurations the mechanism can be assembled into,
for a given set of link lengths. Equation (11) representing the inverse kinematic solution can not be
inverted to find q for a given l, because q does not explicitly occur in (11). Numerical methods are
generally used to solve the forward kinematic problem [47-49]. In this study, a Newton method is used.
3. 2. Dynamic Modelling
3. 2. 1. Lagrange Formalism for Inverse Dynamics
Likewise to kinematic modeling, there are two basic problems in dynamic modeling [9, 10, 46]: the
forward dynamics and inverse dynamics. The latter consists of finding the joint force/torque from a given
EE motion. The former is to find the EE motion from a given joint input force/torque and initial position,
velocity and acceleration conditions. For optimal control and trajectory planning purposes, the dynamic
equations are derived using Euler-Lagrange formalism, as it allows finding in terms of Cartesian
displacement, velocity and acceleration the corresponding motion to a given input force/torque wrenches
and some initial position and velocity conditions. This is performed through energy assessment as follows:
qqτ
∂∂
+∂∂
−∂∂
∂∂
=PKK
t
q& (14)
where andK P are the total kinetic and potential energies of the mechanism, and define the robot
state.
) ,( q&q
Kinetic Energy:
The kinetic energy of the EE is due to its rotational and translational motions. A general expression for
this energy is given by:
22
AA
AATg
TgEE ImK ωω
21
21 += pp && (15)
where is the mass of EE, is a 3-dimentional vector of linear velocity of the EE center of mass, gp&Em
A ω is a 3-dimentional vector of angular velocity of the EE resolved along the axes of the frame A, and
AAI is the mass of moment of inertia tensor of the EE center of mass (COM), within the frame A, is
related to the velocity p of the origin of the frame A and to the angular velocity
gp&
Aω by the relationship: &
) ( rR AAA
Bg ×+= ωpp && (16)
rA being the 3-D vector of the center of the EE with reference to frame A , which is given by:
TA zyx ) , ,( =r
The kinetic energy of the struts might be expressed in terms of the EE translation and orientation
coordinates q as we did for the kinetic energy of the EE. The following equation is derived for the strut
kinetic energy:
qMqqTeeTTTq &&&& LiT
iTii
Tiii
Tii
TLi mmK
21 ) (
21 21 =+= (17)
The total kinetic energy is the sum of kinetic energy of the EE and kinetic energy of the struts.
qMq && )(21
6
1iqT
LiE KKK =+= ∑=
(18)
MSimilarly, the total inertia matrix is defined as the sum of inertia matrix of the EE and the inertia
matrices of the links
EM
LiM
(19) ∑=
+=6
1i LiE MMM
23
Potential Energy:
The potential energy of the EE is given by: EP
) ( ) ). ( 232221 zRyRxRygmygmP EjA
AB
EE +++=+= erR (20)
Where is the jth element of the second row of , and is the unit vector along the y-axis ABR jejR2
y define the displacement of the EE along the y-axis
The potential energy of the strut i is given by: iL
P
jiiLi glmllgmP ee . ) ) (( 2211 +−= (21)
PThe total potential energy of the robot is the sum of the potential energy of the EE and the potential
energy of the struts:
EP
∑=
+=6
1i LiE PPP
At present, we proceed to find the dynamic model using equation (14).
The partial derivative of the total kinetic energy with respect to is: q&
qMq
&&
)( q=∂∂K
(22)
Differentiating (22) with respect to time gives:
qMqMq
&&&&
)( )( .
qq +=∂∂
∂∂ Kt
(23)
The partial derivative of the total kinetic energy with respect to q is:
∑∑=
== ∂
∂=∂
∂=∂∂ 6
6,...,1
6 ....)(2
1 ))(21(
ljk
k
nj
Innj
T MKq
qqq qqqMq (24)
PNow, differentiating the potential energy with respect to q , gives the expression of the force of
gravity as: )(qG
)( qq G=∂∂P (25)
24
Finally, substituting the equations (22)-(25) in (14) gives the second order canonical form of the dynamic
equations as:
)( ) ,( )( qqq τ GqqNqM ++= &&&& (26)
With
)(21 )( ) ,(
6
6,...,1
66
6,...,1
6
∑∑∑∑=
===
== ∂
∂−
∂
∂=
ljk
k
nj
Innj
ljk
n
kj
Innj
MMqq
q qqqqqqN &&&&&& (27)
and
6,...,1,
6
6,...,1,
6
) 21 (( )
21 (( ) ,( =
==
= ∂
∂−
∂
∂=
∂
∂−
∂
∂= ∑∑ jk
k
nj
n
kj
Innnk
k
nj
n
kj
ljj
MMMMqqqq
q qqqN &&& (28)
)(qM is the inertia matrix and it is obtained from kinetic energy, represents the Coriolis and
centrifugal wrench, and is obtained from equations (27) and (28). The gravity force vector is
obtained by differentiating the potential energy
) ,( qN &q
)(qG
P with respect to . The EE gravity wrench is
given by:
q )(qEG
⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜
⎝
⎛
−−+−−−−+−
+−−+−=
)( )( )( )(
)( )(010
)(
yscccsxcccsszccyccsxscs
zssycscscxsscccgmEE
ψφθψφψφθψφφθψφθψφθ
φθψφθψφψφθψφqG (29)
The gravity wrench of the strut i is obtained from equation (21) as: )(qiL
G
)) . (( ) . ( ) ( )( 11122 jijiLi Lgmnglmglm eeGqq
q∂∂
+∂∂
−= (30)
Ti
qL )1 ,0 ,0(
3=
∂∂Ti
qL )0 ,0 ,1(
1=
∂∂ Ti
qL )0 ,1 ,0(
2=
∂∂ , , (31)
iA
j
AB
j
iL aR
qq ∂
∂=
∂∂ 6 ,5 ,4 =j for (32)
) ( 1 1 2
j
ii
j
i
ij
i
i
i
j
i
ij
i lLl
llLL
l qqqqq ∂∂
−∂∂
=∂∂
−∂∂
=∂∂ ee
(33)
25
The expressions of link lengths derivatives are not detailed here.
3. 2. 2. Including Actuator Dynamics
It has been shown that actuator dynamics are significant and cannot be neglected for trajectory planning
and control [9, 10, 35]. The dynamic equations of actuators yield:
(34) maaa τ=++ FKlVlM &&&
where
662
66 )( 2M ×× +== IIM msaa JnJnpπ
662
66 )( 2V ×× +== IIV msaa bnbnpπ (35)
6666 2 K ×× == IIK np
aa π
with the parameters described in the nomenclature.
Continuous-Time Joint Space Dynamic Model Including Actuators Model
Since the actuator’s dynamics is written in joint space, in order to included it, it is necessary to
transform the motion eq. (26) to joint space, using eq. (11)
(36) FGqqNlM )( ),( )( =++ qqq jjj &&&&
where
JMJM )( )( qq cT
j = ,
)(
)( ) ,( ) ,(1
dtd
cT
cT
j
−
−=J
JMJqNJqN qqq && , and (37)
)( )( qq cT
j GJG =
with being the Jacobian, and J F the 6-directional column of the input forces.
The combination of eqs (34) and (36) gives the dynamic model including the actuators in joint space as
26
mjjj τqqq )( ),( )( =++ GqqNlM &&&& (38)
acT
aj MJMJKqM )( )( += qWith ,
11
) )(
)( ( ) ,( ) ,( −−
−+= JJJ
JMJKVqNJKqNdt
dc
Taac
Taj qqq && ,
)( )( qq cT
aj GJKG = (39)
The bars over uppercase boldface letters are used to include both manipulator and actuator elements.
Continuous-Time Cartesian Dynamic Model
Going back to Cartesian space, the overall dynamic equation of PKM in the Cartesian space is written as :
mccc τqqq )( ) ,( )( =++ GqqNqM &&&& (40)
with
1 )( )( −+= JMMJKM acT
ac qq ,
) )(
)( ( ) ,( ) ,(1
1
dtdT
aacT
ac
−− ++=
JMJVqNJKqN qqq && ,
)( )( qq cT
ac GJKG =
3. 2. 3. Including Contact Forces
It is noteworthy that in the proposed approach one might include contact effort models. Among
such models, there are friction and other application-specific forces. Such inclusion is very useful in many
practical cases as deflashing and screwing, where the contact forces are important. The knowledge of
such a force may be important for full success task achievement, as it allows avoiding actuator saturation
and improving the trajectory planning performance. The Cartesian space dynamic equations including
contact forces become:
eccc FJGqqNqM 1 )( ) ,( )( −+++= qqq τ &&&& (41)
27
1−J is the robot inverse Jacobian, and is the effort forces and torques. eF
Strut Friction Model
A friction model is included in the proposed off-line programming system. Viscous and Coulomb (or
dry) friction are two classic models of friction and it is reasonable to model joint friction as a linear
combination of friction forces since both are likely to be encountered in practice. The linear combination
of the two friction models constitutes the prismatic joint friction force , which is easily added to the
developed dynamic model by subtracting the modeled friction force from the frictionless dynamic
model given by (40). This results in a new dynamic model that includes frictional effects. First, these two
friction models can be summarized by the following two equations:
frictF
frictF
, (42) l FF &Vviscfrict = , )sgn( , lFF &
Ccoulfrict =
Where and are the viscous and Coulomb friction constants, respectively. VF CF
The resulting viscous plus Coulomb friction model takes the form:
(43) )sgn( lFlFF &&CVfrict +=
As illustrated in equation (43), is the only state needed to fully characterize the friction parameters
and . The PKM dynamic model including actuators and friction models in the Cartesian space is
finally obtained as:
.
l VF
CF
(44) τqqq )sgn()() ()( 11 =++++ −−−−−
qJFqJFGqqNqM &&&&&& cvccc ,
3. 2. 4. Discrete Time Dynamic Model
The approximate state-space discrete-time model of the PKM is deducted from a state-space form of the
continuous-time dynamic model. Without loss of generality and to alleviate the calculations, in the
remainder of this report, contact forces are not considered, and the time index and the contact forces are
28
omitted. So, the approximate state-space discrete-time model of the PKM is deducted from a state-space
form of the continuous-time dynamic model. First, one might express eq. (19) as:
[ ] )( ) ,( )(1
qGqqqNqM ccc +−
&&−=−
)( 1
mc τqMq&& (45)
By noting the robot state as , (with and being defined in the nomenclature), eq. (23) is
transformed as:
[ TTT21 , xxx= ] 1x 2x
mccc τ )( ),( )( 122121 =++ xGxxxNxxM & (46)
In turns, one might rewrite eq. (24) as follows:
[ ] mcccc
τ )(
)( ) ,( )(
1
166
12211
116
6666
6666⎥⎦
⎤⎢⎣
⎡+⎥
⎦
⎤⎢⎣
⎡
+−⎥
⎦
⎤⎢⎣
⎡= −
×−
×
××
××
xMO
xGxxxNxM0
xOOIO
x& (47)
with
)( )()( )( 11
111 xJMxMxJKxM −+= acTac (48)
and
)))((
)( ( )( ) ,( 11
11
121 dtd
aaT
acxJ
MxJVxJKxxN−
− ++= (49)
In order to derive the discrete-time dynamic model of the robot, eq. (25) is written in the form:
τ)( )( xBxDFxx +−=& (50)
with
[ ]⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+
=−
×
)( ),( )( )(
122111
16
xGxxxNxM
0xD
ccc
,⎥⎦
⎤⎢⎣
⎡=
××
××
6666
6666
OOIO
F , and (51)
(52) ⎥⎥⎦
⎤
⎢⎢⎣
⎡= −−
×
)( )(
1
166
xM
OxB
c
Now, let’s define the sampling time , with T being the total T and , such that ,1
01 =∑≤≤
−
=+
N
kkkkk hhthh
29
4. Multi-Objective Trajectory Planning
4. 1. Constraints Modeling
Simulating a robotics task requires taking into account several constraints; structural and geometric
constraints, kinematic and dynamic parameter nominal values, such as limits on link lengths, velocities,
accelerations, and nominal torques supported by the actuators. Some of these constraints are defined in
joint space, while others are in task space.
4. 1. 1. Robot Constraints
Dynamic state equations: These consists of eq. (36), which is rewritten for later easy use as:
, (59) ) , ,(1 kkkdk hk
τxfx =+
where encapsulates the second member of eq.(36) kd
f
Link intermediate length limits:
N,...,,k 2 0 = , with max min lll ≤≤ k , and (60) )( maxmax xl Θ=
Singularity avoidance:
Singularities are particular poses in which the robot becomes uncontrollable. Therefore, they are crucial
for a successful trajectory planning system. The conditions characterizing singularities are difficult to find
analytically for a general PKM, since an analytical expression for the determinant of is not available.
Several studies had been dealt with the problem and many singularity avoidance algorithms were
proposed [9-13, 16, 17, 31-33]. A common kinematic performance index related to singularity avoidance
is the manipulability measure [27]. Accordingly, by defining the manipulability measure as
1−J
))()(det( )( 111 kkk xJxJx T=w (61)
The following singularity avoidance function can be used:
))()(det(
1 )( 11
1kk
kxJxJ
xT
=ϖ (62)
31
Torque limits:
Non-violation of control torque limits is another major issue for trajectory planning. The required leg
forces must continuously be checked for possible violation of the limits as the manipulator moves close to
a singular pose. As soon as any leg actuator crosses its limit, the optimal planning procedure has to
determine an alternate leg actuation strategy leading to another path on which the actuator forces/torques
would be constrained within the limits. In this paper, the robot torques is assumed to belong to a compact
and bounded set C , expressed as: N6 ℜ⊂
{ }10 , :such that , maxmin6 −=≤≤ℜ∈= ,...,Nkk
Nk ττττ C (63)
Sampling period limits:
If the overall robot traveling time T is too small, there may be no admissible solution to the optimal
control problem, since the torque constraints bound indirectly the path traversal time. On the other hand,
the sampling period must be smaller than the system smallest time mechanical constant in order to
prevent the system from being uncontrollable between two control times. The determination of such a
time mechanical constant and limits of sampling periods is out of the scoop of this paper – They are
assumed to be available through a prior study and analysis of the system’s dynamics – From these limits,
a tradeoff is made in this paper through variation of the sampling period within an admissible domain
defined as:
kh
Η
{ }maxmin :such that , hhhh kk ≤≤ℜ∈= + (64) Η
4. 1. 2. Task and Workspace Constraints
Task and workspace constraints are basically geometric and kinematic, and allow the determination of
the size and shape of the manipulator workspace, which defines the set of poses that can be reached by the
EE without singularity or link interference [19, 20]. These constraints are expressed by imposing to the EE
to pass through a set of specified poses (Fig. 3). These poses are quantified by a set of L pairs (pl, Rl) with
pl referring to the Cartesian position, and Rl to the orientation of the lth imposed pose on the EE, such that:
32
Way-configuration Z z y x
X Y Passage tolerance
xs
xT
Fig. 6. Illustration of EE passage through imposed poses (positions and orientations)
_____ optimal path, ---- feasible path, xs Starting pose, , xT Target pose,
[ ] [ ]∑−
=
+=1
0 ,
N
kkkNd LFE τxx (69)
The first term in the right-hand side of equality (P1) is a cost associated to the final state (i.e at time
for a sampling period h), whereas the second term, it is related to the instantaneous state and
control input variables (i.e at time ). The robot state and input vector and are related by the
discrete dynamic model represented by equation (53). Criterion (P1) is said to be stationary if it does not
contain an explicit appearance of time index k. Otherwise, it is said to be non-stationary, in which case
is written as
, NhtN =
kx kτkhtk =
dE
[ ]+= Nd FE x [ ]∑−
=
1
0, ,
N
kkk kL τx (70)
4. 2. 1. Minimum Error on Final State Attainability Control Problem
This case considers a distance-like cost function that penalizes the difference between the actual reached
state and the desired target state , while starting from a fixed initial state ; In this case the
performance index becomes:
Nx Sxx =0Tx
[ ] [ ]TN
T
TNF xxxx −−= 0 =L, (71)
Such a criterion might be expressed as:
34
)(Min nOrientatioPositiond EEE += (72)
Some authors [26] have suggested this criterion as a benchmark for test-trajectory accuracy for
parallel robots and as a measure of the tracking error in the Cartesian space, by defining the distance-like
error function between the reached Cartesian position , and the target position : Np Tp
222 ) () () ( TNTNTNNTPosition zzyyxxE −+−+−=−= pp (73)
However the ending orientation of the EE is also a relevant issue for proper task execution. This constraint
is considered here through minimisation of the error between the actual (computed) and the target
(desired) orientations as follows:
) vect( TTN RR=nOrientatioE (74)
4. 2. 2. Optimal Servo-Control Problem
The problem now is to bring the robot from a starting point to a target point , while
following a set of pre-defined points . This problem can be formulated at both levels; position and
orientation. At the position level, the objective function might be defined as a distance-like function that
penalizes the difference between the instantaneous reached positions with pre-specified desired
positions .
Sx Tx
kη
kp
Nk ,..,0 =kη
[ ] [ ]∑−
=
+=1
0 ,
N
kkkNd LFE τxx (75)
[ ] [ ]TNT
TN pppp −− [ ] [ ] [ ]kkT
kkkkL ηpηpτ , −−=x[ ]= NF x , and (76) with
4. 2. 3. Minimum Time Control Problem
The Minimum Time Control of robotic systems corresponds to 0 =F , in the mentioned
criterion (P1). This case had been widely considered by several authors. This is of obvious interest
considering production targets in industrial mass production process. Different authors had applied this
1 =L
35
control strategy to serial manipulator robots [20-24]. However, the major drawback of this control method
is its Bang-Bang character, which produces non smooth trajectories, which fastens the mechanical fatigue
of the machine. The sampling periods are defined such that the overall robot traveling time is
∑=−
=
1
0
N
kkhT , (77)
where is the robot traveling time between two successive discrete configurations k and k
h 1 +k ,
. There are two basic approaches to the minimum time control problem: 1,...,0 −= Nk
1st Approach: Consists of taking a fixed sampling period h and search for a minimum number N
of discretisations of the trajectory. This is equivalent to bring the robot from an initial configuration to
a final imposed one , within a minimum number of steps. For the unconstrained case, the time
optimal control is basically bang-bang with singularities occurring at the vicinity of the switching
function. For strongly non linear and coupled mechanical systems (like for the PKM at hand), this is
simply impractical, even by using symbolic calculations.
Sx
Tx N
2nd Approach: Consists of taking a fixed number of discretisations and vary the sampling time
. This is equivalent to bring the robot from an initial configuration to a final imposed one ,
within a fixed number of steps while varying (minimizing) the sampling periods .
N
Sxkh Tx
khN
4. 2. 4. Minimum Energy Control Problem
The problem in this situation is to bring the robot from a starting point to a target point ,
while minimizing an electric energy cost. Hence, one has
Sx Tx
0 =F and , (78) ∑−
=
=1
0
N
kk
T
k RL ττ
36
with such a criterion, or more generally, with a quadratic criterion, like kinetic-energy criterion,
( , is the velocity vector), the obtained trajectory is smoother, as it averts the very
discontinuous trajectories such as those obtained with the application of a Bang-Bang like control. Several
authors had been choosing it. In [7] one of the authors had applied this criterion to serial manipulator
robot. This criterion has been adopted for trajectory planning of parallel robots as well. In [48, 49], the
authors consider the minimization of a kinetic energy, under singularity avoidance constraints for a
parallel robot.
∑−
=
=1
0
N
kk
T
k RL vv v
4. 2. 5. Redundancy Resolution and Singularity Avoidance Control Problem
In the case of redundant robots, the Jacobian J is not a square matrix. The kinematic redundancy might
be used to solve the inverse kinematics, by optimizing a secondary criterion. Several approaches had been
proposed [51]. Among these, the gradient projection is widely used to devise a general solution to the
inverse kinematics problem, which is eventually expressed as:
(79) zJJIxJ )( ⊥⊥ −+= &&q
Where is a generalized inverse of the Jacobian matrix. One of the more popular generalized inverses is
the pseudo-inverse (or Moor-Penrose inverse), which is defined as
⊥J
1)( −⊥= TT JJJJ . The pseudo-inverse
solution in the first term of above equation is known to minimize the two-norm of the joint velocity
vector, whereas the second term is called the homogeneous or null-space solution. The null-space solution
does not contribute to the end-effector motion yet determines the joint pose. However, this approach might
lead to robot singularities at very high demands in joint velocities. A weighted inertia pseudo-inverse had
been proposed to ensure dynamic consistency as compared to simple pseudo-inverse [51]. It is given by:
111 )( −−−⊥= TT JJDJDJD (80)
where D is the robot inertia matrix.
37
To include a secondary task criterion by a performance index , z is chosen to be
)(qg
)( qz gk∇±= (81)
where is a positive real number and k )(qg∇ the gradient of , with a positive sign indicating that
the criterion is to be maximized; a negative sign indicating minimization.
)(qg
Many criteria might be derived. Each related to a specific choice of g, e.g., it could be chosen in to prevent
joint limits, speed limits, avoid obstacles, or a multi-objective including several of theses criteria. A
related criterion to singularity avoidance is to maximize a measure of manipulability (MOM) given in eq.
(57), or equivalently to minimize eq. (58). Note that this last criterion is applicable for both cases
redundant or non-redundant manipulators.
4. 2. 6. Multi-Objective Quadratic Optimal Control Problem
This is a general multi-criteria optimal control problem including all criteria mentioned above,
while associating a weigh to each criterion. The cost function in this case might be written as:
k
N
kk
T
kkkT
kkTNT
TNd hRQFE ∑−
=
++−−+−−=1
0 ] [(] [(
21 ] [( ] [(
21 μττηη xxxxxx (82)
Where is a positive scalar function, and are matrices with the following properties;
(which are of dimensions ( ), is the state dimension) must be positive semi definite.
RQF , , QF ,dE
) ( mmR ×nn × n ,
is the dimension of the control inputs vector and must be positive definite, because the computing of
the optimal control necessitates the existence of the inverse of
m
R μ, and is a positive scalar number
serving to weigh the time criterion.
4. 2. 7. The Objective Functional for the Considered Problem
The performance index considered in this report, relates energy consumption, traveling time, and
singularity avoidance. For energy criterion, both electric and kinetic energies are optimized. For time
criterion, as noted above, there are two basic ways to perform optimization: The first one fixes the
38
sampling period and searches for a minimum number N of discretisations. The second one fixes the
number of discretisations and varies the sampling periods . For the reasons mentioned earlier, in this
report, the number of sampling periods is guessed from an initial feasible kinematic solution. Then the
sampling periods and the actuator torques are taken as control variables. For singularity avoidance, it is
included through maximization of criterion (58). In continuous-time, the constrained optimal control
problem can be stated as follows:
h
khN
∈)(tτAmong all admissible control sequences C and Η∈h , that allow the robot to move from an
initial state to a final state , find those that minimize the cost functionTTt xx )( = : St xx =)( 0 E
∫ +++∈
Η∈
=⎪⎭
⎪⎬⎫
⎪⎩
⎪⎨⎧
⎥⎦
⎤⎢⎣
⎡TTT
t
tdtttttt
ttt
E
T
0
122 1 ))(( )()(2
1 )()( )( ,
Min
0
xQxxU ϖδιτττ C
(83)
Subject to constraints (54)-(63).
with C , and being, respectively, the set of admissible torques, the set of admissible sampling
periods, electric energy, kinetic energy, and time weights, and a weight factor for singularity avoidance.
The corresponding discrete-time optimal control problem consists of finding the optimal
sequences and , allowing the robot to move from an initial state
to a target state , while minimizing the cost :
QU , ι δΗ ,
Sxx 0 =) ,..., ,( 120 −Nτττ ),..., ,( 120 −Nhhh
dETN xx =
(84) ⎭⎬⎫
⎩⎨⎧
+++= ∑−
=∈Η∈
1
0122
)]( [Min
N
kkk
Tkk
Tkkd hE
h
xQxxU ϖδιτττ C
Subject to: 10 ), , ,(1 ,..., N- k hf kkkdkk
==+ τxx
10 ,J1 , ,..., N- k ,..,j ==0 ) , ,( ≤kkkj hτxg
,..., N k 0 , =L, ..., ii 210 )( k == , xs
39
4. 3. Non linear programming formulation
4. 3. 1. Augmented Lagrangian Approach
There are two basic approaches to solving the stated Minimum Time-Energy Singularity-Free
Trajectory Planning (MTE-SF-TP) n constrained on-linear control problem (65); dynamic programming
and variational calculus through the Maximum principle of Pontryagin. With the former approach, a
global optimal control might be found using dynamic programming [56]. The optimal feedback control
can also be characterized by solving a partial differential equation (PDE) for a so-called value
function through Hamilton-Jacobi-Bellman partial differential equations (HJB-PDE) [57]. For linear-
quadratic regulator problems, the HJB-PDE can be solved analytically or numerically by solving either an
algebraic or dynamic matrix Riccati equation. For a general case, however, the PDE can be solved
numerically for very small state dimension only. This is a common problem to large-scale systems
identified as the curse of dimensionality problem [56]. If inequality constraints on state and control
variables are added, this makes the problem harder. We propose to use the second approach [58] to solve
this problem. The Augmented Lagrangean (AL) is used to solve the resulting non linear and non convex
constrained optimal control problem. The AL function transforms the constrained problem into a non-
constrained one, where the degree of penalty for violating the constraints is regulated by penalty
parameters. This method was originated independently by Powell and Hestens [43, 44]. It was
subsequently improved by several authors [39-42]. It basically relies on quadratic penalty methods, but
reduces the possibility of ill conditioning of the sub-problems that are generated with penalization by
introducing explicit Lagrange multipliers estimates at each step into the function to be minimized, which
results in a super linearly convergence iterates. Furthermore, while the ordinary Lagrangean is generally
non convex (in the presence of non convex constraints like for the considered problem), AL might be
convexified to some extent with a judicious choice of the penalty coefficients [45]. This procedure had
been previously implemented by the first author on several cases of robotic systems [5]. An outline of AL
implementation procedure for the case at hand is given at the end of this section. The AL function
) (* tx,τ
40
transforming the constrained optimal control problem into an unconstrained one is written as:
[ ] k
N
kkk
Tkk
Tk h )(
1122∑
=
+++ xQxxU ϖδιττ { }+−+∑−
=++ ),,((
1
011
N
kkkkdk
Tk h
kτλ xfx= ) , , , , ,( σρλτμ hL x
+∑ (85) ⎥⎦
⎤Φ∑
=)) , ,( ,(
J
1kkkj
jk
jh
gτρμ xg+⎢⎣
⎡ Ψ∑ ∑∑−
= =
−
= ))( ,(
1
0
2
1
1
1
N
kk
li
ik
i
L
lk
Sh xsσμ
=Ψ
2
1))( ,(
SiN
Li
iNNh xsσμ
where the function is defined by the discrete state eq. (54) at the sampling time k, is the
total sampling number, designates the ajoint (or co-state) obtained from the adjunct equations
associated to state equations, are Lagrange multipliers with appropriate dimensions, associated to
equality and inequality constraints and are the corresponding penalty coefficients. The penalty
functions adopted here combine penalty and dual methods. This allows relaxation of the inequality
constraints as soon as they are satisfied. Typically, these penalty functions are defined by:
),,( kkkd hk
τxf N
N 12 R∈λ
σ ρ,
, Sg μμ
{ }22 ) ,0(Max
21 ) ,( ababa −+=Φ g
ggμ
μμbbaba ss
T)2 ( ) ,( μμ +=Ψ and (86)
where and b refer respectively to Lagrange multipliers and the left hand side of equality and inequality
constraints.
a
The Karush-Kuhn-Tucker first order optimality necessary conditions require that for , kkk h ,,τx ,...,Nk 0 =
to be solution to the problem, there must exist some positive Lagrange multipliers , unrestricted
sign multipliers , and finite positive penalty coefficients such that :
) ,( kk ρλ
kσ ) ,( sg μμ
0 =∂∂xμL 0 =
∂∂τμL 0 =
∂∂λμL0 =
∂∂
hLμ 0 =
∂∂σμL0 =∂
∂ρμL, , , , , , and
0 ) ( =hgTk ,x,τρ 0 )( =xsT
kσ 0 ) ( ≤h,g τx,, , (87)
The development of these conditions enables us to derive the iterative formulas to solve the optimal
control problem by adjusting control variables, Lagrange multipliers as well as penalty coefficients and
tolerances. However, in equation (54), contains the inverse of the total inertia matrix ) , ,( kkkd hk
τxf
41
)(1
xM−−c of the PKM, including struts and actuators, as well as their Coriolis and centrifugal
wrenches . These might take several pages long to display. In developing the first necessary
optimality conditions and computing the co-states , one has to determine the inverse of the mentioned
inertia matrix and its derivatives with respect to state variables. This results in an intractable complexity
even by using symbolic calculation.
) ,( 21 xxN c−
kλ
4. 3. 2. Constrained Linear-Decoupled Formulation
The major computational difficulty mentioned earlier cannot be solved by performing with the original
non linear formulation. Instead, it is solved using a linear-decoupled formulation [62].
Theorem:
Under the invertiblity condition of the inertia matrix, the control law defined in the Cartesian space as
(88) )( ) ,( )( 12211 xGxxxNvxMu ccc−−−
++=
allows the robot to have a linear and decoupled behavior with a dynamic equation:
, (89) vx 2 =&
where is an auxiliary input v
This follows simply by substituting the proposed control law (69) into the dynamic model (40). One gets
(90) vxMxxM )( )( 121 cc−−
=&
Since is invertible, it follows that )(xM c−
vx 2 =&
This brings the robot to have the decoupled and linear behavior described by the following linear
dynamic equation written in discrete form as:
(91) ) , , ( ))(( 1 kkkD
dkkkdkk hhk
vxfvBxFx =+=+
42
{ }I ..., 1, )( k , 0 ∈= iDi xs ,..., N k 0 , =
4. 3. 3. Augmented Lagrangian for the Decoupled Formulation
Now, mutatis mutandis, the augmented Lagrangian associated to the decoupled formulation (P) is (after
removing bars and c indexes for writing simplicity):
[ ][[{∑−=
++1
012211 )( ) ,( )(
N
k
Tkkkkkk UxGxxxNvxM= ) , , , ,(D σ ρ,μ λvx hL
{ }∑−
=++ −
1
011 ) , ,( (
N
kkkk
Ddk
Tk h
kvxfxλ[ ]])() ,( )( 12211 kkkkkk xGxxxNvxM ++ ] }++++ )( 122 kkk
Tk hxQxx ϖ δι
+∑ (97)
where the function is defined by eq. (71) at time k, is the total sampling number, other
parameters appearing in (75) are defined above.
⎥⎦
⎤Φ∑
=)) , ,( ,(
J
1kkk
Dj
jk
jh
gτρμ xg+⎢⎣
⎡ Ψ∑ ∑∑−
= =
−
= ))( ,(
1
0
2
1
1
1
N
kk
Dli
ik
i
L
lk
Sh xsσμ
=Ψ
2
1))( ,(
iN
DLi
iNNh xs
Sσμ
),,( kkkD
d hk
τxf N
Again, the development of the first order Karush-Kuhn-Tucker optimality necessary conditions require
that for , to be solution to the problem (P), there must exist some positive Lagrange
multipliers , unrestricted sign multipliers , and finite positive penalty coefficients
such that equations (68) are satisfied for the decoupled formulation.
,...,Nk 0 =kkk h , , vx
) ,( kk ρλ kσ ) ,( sg μμμ=
The co-states are determined by backward integration of the adjunct state equation yielding: kλ
Ux
xGxxxNvxM
k
kckkkckkc
kk h∂
⎥⎦⎤
⎢⎣⎡ ++∂
−=
−−−
−
)() ,()( 2
12211
1λ ⎥⎦
⎤⎢⎣
⎡ ++−−−
)( ) ,( )( 12211 kckkkckkc xGxxxNvxM
−− 2 2 kkhQx )( 11
kk
xx ϖ∇δ - (98) ⎥⎦
⎤⎢⎣
⎡Φ∇∑
=
J
1)), ,(,(
jkkk
Dj
jkk hh
gkvxg x ρμ−⎥⎦
⎤⎢⎣⎡ Ψ∇∑∑
=
−
= ))( ,(
2
1
1
1k
Dli
ik
i
L
lk
Skh xsx σμ−kT
d λF
The gradient of the Lagrangian with respect to sampling period variables is
44
UxGxxxNvxM
T
kckkkckkcDhkL⎢⎢
⎣
⎡
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡ −+
−+
−∇ = )() ,()( 1221 μ ⎥⎦
⎤⎢⎣
⎡ ++−−−
)() ,()( 12211 kckkkckkc xGxxxNvxM
+ (99) ⎥⎦
⎤+++ )( 122 kkT
k xQxx ϖδι ∑=Φ
J
1)) ,(,(
jkk
Dj
jk vxg
gρμ+Ψ∑∑
=
−
= ))( ,(
2
1
1
1k
Dli
ik
i
L
lxs
Sσμ
The gradient of the Lagrangian with respect to acceleration variables is
kTkkkkkkkk
Tk
Dk hL cccc λμ Zxxxxvxx GNMUMv +
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡++∇−−−−
= )() ,( )( 122111 )(2 + (100) ⎥⎦
⎤⎢⎣
⎡Φ∇∑
=
J
1)), ,( ,(
jkkk
Dj
jkk hh
gkvxgv ρμ
[ ] [ ]k
k
kk
kk
h
hh vIIxIO
IIZ 2
66
662
6666
6666
⎥⎥⎦
⎤
⎢⎢⎣
⎡+⎥⎦
⎤⎢⎣⎡=
×
×
××
×× 120 −= ,...,N,kwhere ,
4. 4. Implementation issues
4. 4. 1. Initial solution:
To fasten convergence of AL algorithm ─ although it converges even if it starts from an unfeasible
solution ─ a kinematic-feasible solution is defined. It is based on a velocity trapezoidal profile. This
solution is divided into three zones, acceleration zone with duration T1. In this zone, the actuators are
assumed to supply an initial force to accelerate the EE until the maximum velocity is reached. Then a
constant velocity zone of duration T2 is achieved. Finally a deceleration zone of duration T3= T1 finishes
the cycle (Fig. 7). It is obvious that must be greater than 2T
1T , otherwise the trajectory will be achieved
only on two phases, the acceleration and the deceleration phase, in which case a maximum value of v will
be reached at time = T
zzv f
f
)(2 0−=2
T , which implies . 1T
The initial time discretisation is assumed an equidistant grid for convenience, i.e.
Ntttth f
kkk0
1 −=−= + 121 −= ,...,N,k, (101)
45
(a) position profile (b) velocity profile (c) acceleration profile
Position
time time
Velocity
time
Acceleration
Fig. 7 Trapezoidal profile for a kinematic trajectory in the operation space.
At the calculation of the inertia matrix and Coriolis and centrifugal dynamics components, we can
use the approach developed initially for serial robots by Walker and Orin [63] and based on the
application of Newton-Euler model of the robot dynamics. This method is straightforwardly
generalisable to the case of PKM robots [35].
4. 4. 2. Search Direction Update
Because the considered problem is of large scale type, to solve for the minimization step at the primal
level of AL, a limited-memory quasi-Newton-like method is used at each iteration of the optimization
process. This method allows the computing of the approximate Hessian matrix by using only the first
derivative information, and without need to storing of this approximated Hessian matrix. It performs the
second order BFGS (Broyden-Fletcher-Goldfarb-Shano) search technique. It is briefly outlined bellow.
For more details, the reader is referred to [36]. At the )1( +k th iteration, set as the update of
the control variable
kkk vv 1 −= +α
v , the update of the gradient and the approximation of the
inverse of the Hessian. The inverse of the approximate Hessian can be obtained using the BFGS
update formula:
kkk LL vv ∇−∇= + 1β 1−kH
1−kH
kTk
TkkT
kkkk αβααVHVH 11 += −− )()( k
Tk
Tkkk αβαβIV −=, with (102)
46
The following pseudo-code describes the BFGS two-loop iterative procedure used to compute the search
direction efficiently by using the last pairs of : kk Lv∇−1H ) ,( kk βαm
kLv∇← s
mkkki ,...,2 ,1 −−−= For
;/ iTi
Tii αβsαγ ←
; iiβγss −←
End (for) (103)
;)( 10 sHr −← k
1 ,..., , −−−= kmkmki For
;/ iTi
Tii αβrβδ ←
;) ( iii αδγrr −+←
End (for)
Stop with result rH 1 =∇− kk Lv
where is the initial approximation of the inverse of the Hessian matrix. One can set it as:
, with I is the identity matrix of appropriate dimension, and
10)( −kH
)()( 1111 −−−−= kTkk
Tkk βββαςIςH kk )( 10 =−
4. 4. 3. Overall Solution Procedure:
A systematic procedure flowchart for the augmented Lagrangian implementation appears in Fig. 8. In
this procedure, after selecting robot parameters, task definition, (such as starting, intermediate and final
poses), workspace limitations and simulation parameters (block 1), the kinematic unit (block 2) defines a
feasible solution satisfying initial and final poses. Then the inner optimization loop (block 3) solves for the
ALD minimization with respect to sampling periods and acceleration control variables to give the robot
dynamic state. This state is then tested within against feasibility tolerances. The feasibility is done by
47
further to the dual part of ALD (block 4), to test for constraints satisfaction and update multipliers, penalty
and tolerance parameters. If the constraints are satisfied with respect to a first tolerance level (judged as
good, though not optimal), then the multipliers are updated without decreasing penalty. If the constraints
are violated with respect to a second tolerance level, then keep multipliers unchanged and decrease
penalty to ensure that the next sub-problem will place more emphasis on reducing the constraints
violations. In both cases the tolerances are decreased to force the subsequent primal iterates to be
increasingly accurate solutions of the primal problem.
5. Simulation Case Study
5. 1. Description of the 2-DOF Parallel Manipulator Case Study
A simulation program has been implemented using Matlab [38] to test the proposed multi-objective
trajectory planning approach on a PKM case study reported in [39]. Preliminary results are encouraging.
This PKM consists of two degrees of freedom planar parallel manipulator. The robot kinematic and
dynamic models considered have been developed in [37]. A schematic of the manipulator is depicted in
Figure 9, where the base is labeled 1 and the EE is labeled 2. The EE is connected to the base by two
identical legs. Each leg consists of a planar four-bar parallelogram: links 2, 3, 4, and 5 for the first leg and
links 2, 6, 7, and 8 for the second leg. Prismatic actuators actuate the link 3 and 8, respectively. Motions of
the EE are achieved by combination of movements of links 3 and 8 that can be transmitted to the EE by
the system of the two parallelograms. Due to its structure, the manipulator can position a rigid body in a
2D space with a constant orientation.
5. 2. Kinematic and Dynamic Analysis
As illustrated in Figure 9, a reference frame is attached to the EE, and a reference frame
is attached to the robot base, where is the origin of frame
)' ,' ,'( : yxOA
) , ,( : yxOB A'O and the origin of B. To
characterize the planar four-bar parallelogram, the chains and are considered as shown in
O
11 BA 22BA
49
Figure 8. Vectors and define the positions of points in frames A and B respectively.
Vectors define the position of points in frame B. The geometric parameters of the
manipulator are , and
)2 ,1 ( =iAiA BiA iA
)2 ,1 ( =iBib iB
LBA ii = rAA 2 21 = RBB 2 21 =)2 ,1 ( =i
The position of point in the fixed frame B is defined by the vector . The kinematic equations
of this manipulator are given by:
Tyx ) ,( 'O
(104) ⎥⎦
⎤⎢⎣
⎡=⎥⎦⎤
⎢⎣⎡
yx
yy
xl &&
&&
2
1 JJ
where and are respectively, the xJlJ 22× inverse and forward Jacobian matrices of the manipulator,
which can be expressed as
, (105) 00
2
1⎥⎦⎤
⎢⎣⎡
−−= yy
yylJ
2
1⎥⎦⎤
⎢⎣⎡
−+−−++= yyRrx
yyRxrxJ
If is non-singular, the Jacobian matrix of the manipulator can be obtained as lJ
(106) ⎥⎦⎤
⎢⎣⎡=⎥⎦
⎤⎢⎣⎡
−+−−−+== −
2221
1211
2
11 1 ) )/( (
1 ) )/( ( JJJJ
yyRrxyyRxr
xl JJJ
Accordingly, it is clear that singularity occurs when one of the following cases holds [12, 13]:
0 =lJ 0 ≠xJ1st case: and . This case is known as the type one singularity, and corresponds to the
situation where or , i.e., the first or the second leg is parallel to the x-axis. 1yy= 2yy=
0 ≠lJ 0 =xJ2nd case: and . This case is known as the type two singularity. It corresponds to the
pose where four bars of the parallelogram in one of the two legs are parallel to each other. It is analytically
expressed by the equality for the first leg when x is positive, andRrx =+ rRx =+ for the second leg when x
is negative.
0 =lJ 0 =xJ3rd case: and . This corresponds to the type three singularity for which the two legs are
both parallel to the x-axis. This is mainly a design issue as it is characterized by a geometric parameters
condition given by:
50
RrL =+ (107)
The robot dynamic model in the task space of the PKM is obtained using Lagrange formalism as follows:
+⎥⎥⎦
⎤
⎢⎢⎣
⎡
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
++++
++++++=⎥
⎦
⎤⎢⎣
⎡
38 2 ) ()
32 (
) ()32( ) ()
3 (2 )
34 (
2111
2111221
211
2
1
yx
mmmJJmm
JJmmJJmmmm
lspls
lsl
slp
&&
&&
ττ
+
⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢
⎣
⎡
⎥⎥⎦
⎤
⎢⎢⎣
⎡
+−+
−+⎥⎥
⎦
⎤
⎢⎢
⎣
⎡
+++
⎥⎥⎦
⎤
⎢⎢⎣
⎡
+−+
−+⎥⎥
⎦
⎤
⎢⎢
⎣
⎡+−+
+
2/32121
2/31111
22
22
2/32121
2/3111122
22
22
)(
)( )
32(
)(
)( )
32(
RrxJJ
RrxJJ
yxyxmmxL
RrxJJ
RrxJJ
yxmmyx
xL
ls
ls
&&
&&&
&&&&
&
(108)
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
⎥⎥⎦
⎤
⎢⎢⎣
⎡+++
+
−
+⎥⎥⎦
⎤
⎢⎢⎣
⎡++
+−
)()()(
)
)( )2
()(
)((
21112/322
2
21112/322
lp
ls
mmgJJyx
xyxxy
JJmmgyx
yxyxy
&&
&&&&&&&
&&
&&&&&&&
More details on the derivation of the dynamic model might be found in [37].
Following the streamline developed in previous sections, a discrete-time state-space model associated to
of the state eq. (70) is:
[ ]kkckkkc
k
k
kckk
k
h
h
Ih
τ-)(),(2 )( 1221
22
22
2
1
1
2222
22221 xGxxxN
I
IxMxO
IIx +
⎥⎥
⎦
⎤
⎢⎢
⎣
⎡−⎥
⎦
⎤⎢⎣
⎡=
×
×−
××
××+ (109)
The optimal control problem consists to minimize criterion (47) subject to dynamic eq. (71) and equality
and inequality constraints (37-44), and the following specific constraints.
Workspace limitations:
MaxkMin xxx ≤≤ MaxkMin yyy ≤≤ ,...,N,k 20 =, for (110)
Singularity avoidance:
In our considered case study, the first type singularity constraint might be expressed by:
1)2- )1- (( ε≥kkkk yyyy , (111)
51
whereas the second it is given as:
2 ) ))(sgn(( ε≥+ r-Rkxkx (112)
represent small positive tolerances. 21 and εε
The third singularity type concerns mainly the geometric parameters . These parameters are
chosen at the design level, such that the equality
RrL , , and
RrL =+ does not hold. The required passage poses is
reduced to positioning ones, insofar a constant orientation is assumed during task execution. Typically,
one might have:
0 T )( PassTh =−−=pllppxs (113)
The augmented Lagrangian and the associated decoupled formulation are obtained along with various
gradients. These calculations are quite long. The reader is referred to [37] for further details.
5. 3. Simulation Data and Scenario
The following numeric values are used: The EE mass is =200.0 , that of each leg is =
570.5 , and that of the slider is =100 . The platform radius is
kg lmEEm
kg kgsm mr 750.0 = , and the
strut length . Table. 1 shows the limits of the workspace, actuator torques and sampling
periods. For ALD, the following parameter values had been taken , , ,
, ,
mR 2030.1 =
mL 9725.1 =
0.5=sω 0.5=sη 4.0== ηααw
4.0== ηββw =0w 2100 10 −==ηη =*w 5*1* 10 −==ηη , , , 0.25 1=γ 1.2 2=γ 01.0=υ , . The initial
Lagrange multipliers components are set to zero. The singularity weight is
3.0=−υ
1 =δ , 00 ρσ . The maximum
value for , and the minimum value for . 42Max 01 =δ -42
Min 01 =δ
The simulated scenario consists of a straight-line trajectory from an initial Cartesian state
position , to a final position , (in meters). The initial and final
linear and angular velocities are equal to zero. The maximum velocity is 0.2m/sec and maximum
7.0 =Tx7.0 0 −=x 1.0 0 −=y 6.1 −=Ty
52
acceleration is 2 m/sec2. The maximum allocated time for this trajectory is 10 sec. In the presented
simulations, the focus is on time-energy constrained trajectory planning by the augmented Lagrangian,
more kinematic related performance evaluation and design for a similar case study might be found in [37,
39]. Typically, four simulation objectives are considered:
Compare robot trajectories for different values of the weights . δι and, 1 , QU
Assess the effects of the dynamic parameters changes on the augmented Lagrangian sensitivity
and on the behavior of the PKM.
At which precision vs. time consumption, the augmented Lagrangian achieves passage satisfaction
through imposed poses?
To what extent the number of inner and outer iterations of AL impacts PKM performance vs. CPU
Time?
To start, Figure 10 shows the velocity profile used to initialize the augmented Lagrangian. Figures 11
and 12 show the simulation results for both initial kinematic and augmented Lagrangian solutions. In part
(a) of these figures, the first plot from the top shows the displacement along x-axis of the end-effector
point of operation. The second plot shows the displacement along y-axis of the end-effector point of
operation. The third shows the instantaneous values of consumed time to achieve the trajectory. In part
(b), the first and second plots from the top show the instantaneous variations of joint torques, while the
third one shows the instantaneous values of the consumed energy. It is noteworthy that although the initial
solution is kinematically feasible, when the corresponding torques is computed considering the dynamic
model and forces, one gets shortly torque values outside the admissible domain resulting in high values for
the energy cost. With the augmented Lagrangian, however, with four inner and 7 outer iterations, the
variations of the energy consumption are increasing smoothly and monotonously. Figure 13 displays the
simulation outcomes for the only energy criterion (i.e. the time weight is set to zero, so the sampling
period is kept constant). One gets a 34% faster trajectory with time-energy criterion (Fig. 12), as compared
to a trajectory computed with only minimum-energy criterion (Fig. 9). As for imposed passages through
pre-specified poses, the same scenario as above is simulated, while constraining the EE to pass through
53
the following positions: (0.0, -1.4), (0.4, -1.1), (0.5, -1.0), all in meters. Figure 14 shows the trajectory
corresponding to passage through imposed poses. With 10 dual iterations and 7 primal ones, one gets a
precision of 7.10-4, which confirms the well-known constraints satisfaction performance of ALD for
constrained optimization problems as compared to its counterparts like penalty methods. Furthermore, we
observe that the proposed variational approach has not only been successful in finding singularity-free
trajectory, but also the obtained trajectories are optimal in time and energy minimization. To analyze with
respect to AL parameters, Table 2 shows comparison of results for different simulation parameters of AL,
where NDisc is the number of discretisations, NPrimal is the number of inner optimization loops, NDual
is the number of outer optimization loops, is the total traveling time, Energy =
is the consumed electric and kinetic energy, and APEq and APIneq for Achieved
Precision for Equality and Inequality constraints satisfaction, respectively. The values shown for the total
traveling time tT, Energy, and AP correspond to those computed for the last outer iteration.
∑=−
=
1
0
N
kkT ht
∑ +−
=
1
022 )][(
N
k
Tkk
Tkk QxxUττ
2R
2r
Fig. 9. A geometric representation of a 2-DOF planar parallel manipulator
54
Table 1. Limits of workspace, actuator torques and sampling periods
Parameter x-coordinate (m) y-coordinate (m) (N) (N) h (sec) 1τ 2τ
max 0.8 -0.720 550 700 0.7
min -0.8 -1.720 -550 -700 0.005
Table 2. Convergence history of Minimum Time-Energy Planning with Augmented Lagrangian
NDisc NPrimal NDual CPU(sec) tT (sec) Energy(J) APEq APIneq
10 4 7 12.62 9.50 6874.37 3.10-3 3.10-3
20 4 7 29.25 8.01 5156.28 10-3 10-3
20 5 10 35.54 7.31 4910.39 5.10-4 4.10-4
30 5 10 69.83 6.07 4010.23 2.10-5 3.10-5
Fig. 10. Initial solution, velocity profile
(a) (b) Fig. 11. Kinematic simulation results
(a)- Variations of x, y, coordinates of the EE, and sampling periods (b)- Variations of torques , , and energy 1τ 2τ
55
(a) (b) Fig. 12. Simulation results with the ALD
(a)- Variations of x, y, coordinates of the EE, and sampling periods (b)- Variations of torques , , and energy 1τ 2τ
(a) (b)
Fig. 13. ALD (Minimum Energy) (a)- Variations of x, y, coordinates of the EE, and fixed sampling periods
(b)- Variations of torques , and energy 1τ 2τ
(a) (b)
Fig. 14. ALD Minimum Time-Energy with imposed passage through Cartesian positions (0.0, -1.4), (0.4, -1.1), and (0.5, -1.0)
(a) Variations of x, y, coordinates of the EE, and sampling periods (b) - Variations of torques , and energy 1τ 2τ
56
5. 4. Sensitivity Analysis
The optimal time-energy control considered so far is dependent on the values of the dynamic parameters
of the PKM. As PKMs are strongly non-linear and coupled mechanical systems, several of these
parameters such as inertial parameters are known only approximately or may change. So, a sensitivity
analysis [40] is necessary to assess how robust the proposed approach to the parameter changes is.
This is performed through varying the value of the EE mass. On Figure 15, it is shown the ALD
simulation with modified EE mass as =300.0 . One notices that the needed actuator torques and
necessary energy and time to achieve the same task are higher, especially at the beginning.
kgEEm
(a) (b) Fig. 15. ALD Simulation with disturbed trajectory (Min Time-Energy), (Modified mass of EE, =30.0 kg) EEm
(a)- Variations of x, y, coordinates of the EE, and sampling periods (b)- Variations of torques , and energy 1τ 2τ
6. Conclusions and Discussions
In this report, the offline programming problem of parallel robots is studied. The basic contribution is the
formulation and resolution of the trajectory planning problem of parallel kinematic machines using a variational
calculus framework. This is performed by considering robot kinematic and dynamic models, while optimizing
time and energy necessary to achieve the trajectory, avoiding singularities, and satisfying several constraints
related to the robot, task and workspace. The robot dynamic model includes the EE, struts and actuators models.
The augmented Lagrangian algorithm is used to solve the resulting non-linear and non-convex optimal control
problem. This optimization technique is used along with a decoupled and linearized formulation of the original
problem, permitting the ultimate benefit of easing the computation of the co-states and other variables necessary
57
to perform optimization. Although it is task and algorithm parameter settings dependent, the computational time is
drastically reduced when the decoupled and linearized formulation is used. It has been shown that the proposed
approach performs better in optimizing traveling time and actuator torques than kinematic only based schemes.
Moreover, it produces smoother trajectories as compared to minimum-time based-control. Furthermore, the
proposed trajectory planning is robust to dynamic parameters changes. This in fact is due to the ability of the
Augmented Lagrangian to cope with numerical ill-conditioning problems, as compared to other optimization
techniques like penalty methods. Moreover, a major advantage of this approach is that one can introduce any type
of constraints related to the robot, task or environment, like obstacles or link interference avoidance, by deriving
the corresponding constraint expressions and adding them naturally in the Lagrangian in order to have them
included in the trajectory planning system.
The short time perspectives for this work are twofold: The first one is to incorporate obstacle and link
interference avoidance strategies and other specific task requirements within the optimization process. Second,
from algorithmic point of view, this work will be extended by considering and comparing the augmented
Lagrangian technique, developed in this report, by hybridizing it with a projection algorithm. This might be
achieved by projecting on the tangent space of the final state constraints rather than solving these constraints
through penalty. This allows the fateful advantage over penalty only approaches in final state attainability
satisfaction with any given precision, by generating a feasible solution, at each iteration. Furthermore, the
decoupled linear formulation allows easing the projection operator computation, with a reasonable complexity.
Acknowledgment
The authors gratefully thank the Natural Science and Engineering Research Council of Canada
(NSERC) for supporting this work under grants ES D3-317622, RGPIN-203618, RGPIN-105518 and
STPGP-269579.
58
[15] M. Vukobratovic, D. Stotic, (1985) ''Is Dynamic Control Needed in Robotics System? and If So to
What Extent? '', Int’l Jou. Of Robotics Research Vol.2 N°2 pp : 18-35, Summer 1983.
[16] T. DeFanti, M. Brown, C. De Latt: ''iGrid 2002: The international virtual laboratory'', Future
Generation Computer Systems, v 19, n 6, p 803-804, 2003.
[17] Available at: http://robotmaster.com
[18] Simulation software saves time - ROBCAD and DYNAMO software from Tecnomatix; Canadian
capsule machine maker dominates global market, Design Engineering (Toronto), v 45, n 1, Jan-Feb, 1999.
[19] Available: www.3ds.com/products-solutions/brands/CATIA
[20] B. Cao, G. I. Dodds, and G. W. Irwin, “Time-Optimal and Smooth Constrained Path Planning for
Robot Manipulators”, Proc. IEEE Int. Conf. Robotics and Automation, pp.1853-1858, 1994.
[21] Z. Shiller and W. Lu, “Computation of Path Constrained Time Optimal Motion with Dynamic
Singularities”, Trans. ASME, vol. 114, pp.34-41, 1992.
[22] K. N. Shin and N. D. Mckay : ''Minimum-Time Trajectory Planning For Industrial Robots With
General Torque Constraints'', Proceedings of the IEEE International Conference on Robotics and
Automation, San Francisco, CA, USA pp: 412-417, 1986.
[23] J. S. Choi and B. K. Kim: ''Near Minimum Time Direct Voltage Control Algorithms for Wheeled
Mobile Robots with Current and Voltage Constraints'', Robotica, 19, pp: 29-39, 2001.
[24] O. Dahl, “Path-Constrained Robot Control with Limited Torques-Experimental Evaluation”, IEEE
Trans. Robot. Automat., vol. 10, pp.658-669, Oct.1994.
[25] V. E. Gough, Contribution to discussion of papers on research in automobile stability, control and
type performance, in Proc. Auto Div. Inst. Mech. Eng. Part D (J. Automob. Eng.), p 392-395, 1956.
[26] D. Stewart, A platform with six degree-of freedom, in Proc. Inst. Mech. Eng., v 180, p 371-
386, 1965.
[27] K. J. Waldron & K. H. Hunt, “Series-Parallel Dualities in Actively Coordinated Mechanisms”,
International Journal of Robotics Research, v 10, n 5, p 473-480 Oct. 1991
60
[28] B. Dasgupta, T.S. Mruthyunjaya, ''The Stewart platform manipulator, A review'', Mechanism and
Machine v 35, n 1 pp. 15-40, 2000.
[29] Dash, A.K.; I-Ming Chen; Song Huat Yeo; Guilin Yang: ''Singularity-free path planning of parallel
manipulators using clustering algorithm and line geometry'', Proceedings of the ICRA '03, IEEE
International Conference on Robotics and Automation, Volume 1, 14-19 pp: 761 – 766, Sept. 2003.
[30] C. Innocenti and V. Parenti-Castelli: ''Singularity-free evolution from one configuration to another in
serial and fully-parallel manipulators'', Journal of Mechanical Design, Transactions of the ASME, v 120, n
1, pp: 73-79, Mar, 1998.
[31] O. Ma, J. Angeles: ''Architecture Singularities of Parallel Manipulators'', Int’l Journal of Robotics and
Automation, vol. 7, No 1, pp: 23-29, 1992.
[32] C. Gosselin: ''Parallel Computational Algorithms for the Kinematics and Dynamics of Plannar and
Spatial Parallel Manipulators'', ASME, J. of Dynamic Systems, Measurements and Control, vol. 118 no. 1,
pp: 22-28, 1996.
[33] D. N. Nenchev, S. Bhattacharya, et M. Uchiyama: '' Dynamic Analysis of Parallel Manipulators
under Singularity-Consistent Parameterization'', Robotica, vol. 15, pp: 375-384, 2003.
[34] L. Baron: ''Workspace-Based Design of Parallel Manipulators of Star Topology with a Genetic
Algorithm”, Proceedings of the ASME Design Engineering Technical Conference, Pittsburgh,
Pennsylvania, USA, September 9-12, 2001.
[35] K. Harib, K. Srinivasan: ''Kinematic and Dynamic Analysis of Stewart platform-based Machine Tool
Structures'', Robotica, vol. 21, pp: 541-551, 1997.
[36] D. P. Bertsekas: ''Non Linear Programming'', Athena Scientific, 1995.
[37] M Hay, J. A. Snyman, methodologies for the optimal design of parallel manipulators, Inl J. for
Numerical Methods in Engineering, Vol. 59, No. 1, p 131-152, 2004.
[38] M Hay, J. A. Snyman, A multi-level optimization methodology for determining the dexterous
workspaces of planar parallel manipulator, workspaces of planar parallel manipulator, Structural and
Multidisciplinary Optimization, Vol. 30, No. 6, p 422-427, 2005.
61
[39] J.B.Hiriart-Urruty and C. Lemaréchal: ''Convex Analysis and Minimization Algorithms'', Springe
Verlag, Berlin, New York, 1993.
r
[40] G. Cohen, D. L. Zhu: ''Decomposition-Coordination Methods in Large Scale Optimization Problems:
: 974-1002, 2001.
The non Differential Case and the Use of Augmented Lagrangean'', Advances in Large Scale Systems,
Vol. 1, No. 2, JAI Press 1984
[41] N. Gould, D. Orban, A. Sartenear, and P. Toint: ''Super Linear Convergence of Primal-Dual interior
Points Algorithms for Non Linear Programming'', in SIAM Jour on Optimization. Mathematical
Programming, B, v. 11, n 4, pp
[42] Available: www.cse.clrs.ac.uk/Activity/LANCELOT
[43] M. J. D. Powell, ''A Method for Nonlinear Constraints in Minimization Problems'', In ; Optim
(R. Fletcher, ed.) pp. 283-298, Academic Press Lo
ization
ndon, 1969.
Optimization Theory and Applications 4, 303-
M Review 35, pp: 183-238, 1993.
ukhi, L. Baron, M. Balazinski: “Programmation Dynamique Time-Énergie Minimum des
3
g
th M
l
[50] R. Longchamp, ''Commande Numérique des Systèmes Dynamiques'', Presse Polytechniques
Universitaires Romandes EPFL, Lausanne, 1995.
[44] M. R. Hestens: ''Multiplier and Gradient Methods'', J.
320, 1969.
[45] T. Rockafellar: ''Lagrange Multipliers and optimality'', SIA
[46] Craig J. J. : ''Introduction to robotics: mechanics and control'', 3rd Ed, Addison-Wesley, 2005.
[47] A. Kho
Robots Parallèles par Lagrangien Augmenté”, CCToMM SM , May 26-27, 2005, Canadian Space
Agency, Saint Hubert QC, Canada, pp:147-148.
[48] A. Khoukhi, L. Baron, M. Balazinski, “A Decoupled Approach to Time-Energy Trajectory Plannin
of Parallel Kinematic Machines”, CISM Courses and Lectures No. 487, Proc. of 16 CISM-IFFToM
Symposium, Robot Design, Dynamics and Control, T. Zielinska, C. Zielinski Eds, p 179-186, Springer
Wien N.Y, 2006.
[49] A. Khoukhi, L. Baron, M. Balazinski: “Constrained Multi-Objective Trajectory Planning of Paralle
Kinematic Machines”, Accepted Robotics and Computer Integrated Manufacturing, 2007.
62
[51] Y. Nakamura, Advanced Robotics: Redundancy and Optimization, Addison-Wesley, 1991.
[52] S. G. P. Saramago, V. Jr. Steffen: ''Optimal Trajectory Planning of Robot Manipulators in the
Presence of Moving Obstacles'', Mechanism and Machine Theory, Vol. 35, No. 8, pp: 1079-1094, 2000
[53] M. Niv, D. M. Auslander: ''Optimal Control of a Robot with Obstacles'', American Control
Conference. San Diago, California, 1984.
.
[54] E. G. Gilbert and D. W. Johnson: ''Distance Function and Their Application to Robot Path Planning
85.
J.
amilton-Jacobi Equations, Pittman, 1982.
ts, I. Plander Ed. Elsevier Science Publishers, BV, (North-Holland),
ing for Mobile
sign for Robot Dynamic Control”, Power Electronics and Drive
coupling for Non Linear Systems'', Syst. Cont. Letters, pp: 242-248, 1982. [63]
yst
[64] X. J. Liu, Q. M. Wang, J. Wang, Kinematics, Dynamics and dimentional synthesis of a novel 2- dof
translational manipulator, J. of Intelligent and Robotic Systems, v 41, p 205-224, 2004.
in the Presence of Obstacles'', IEEE Trans. On Robotics and Automation, Vol. 1, n1, pp: 21-30, 19
[55] C. J. Ong and E. G. Gilbert: ''Robot Path Planning with Penetration Growth Distance'',
Robot. Syst. 15(2), 57-74, 1998.
[56] P. E. Bellman, Dynamic Programming, Princeton University Press, 1957.
[57] P. L. Lions, Generalized Solutions of H
[58] L. S. Pontryagin, V. G. Boltyanski, R. V. Gamkrelidge, E. F. Miscenko, the Mathematical Theory of
Optimal Processes, Wiley, 1962.
[59] A. Khoukhi,: “Constrained Dynamic Navigation for a Mobile Robot”, in Artificial Intelligence and
Information-Control System of Robo
pp: 335-344, 1989.
[60] A. Khoukhi: “Dynamic Modelling and Optimal Time-Energy Off-Line Programm
Robots, A Cybernetic Problem”, Kybernetes, vol. 31 No 5-6 pp: 731-735, 2002.
[61] A. Khoukhi: “An optimal De
Systems, PEDS’99, Hong-Kong, Proceedings of the IEEE 1999 International Conference, vol. 1, pp 382-
387, 27-29 July, 1999.
[62] D. Claude: ''De
Walker M. W. Orin D. E.: ''Efficient Dynamic Computer Simulation Of Robotic Mechanisms'', J Dyn S
Meas Control Trans ASME, v 104, n 3, pp: 205-211, Sep, 1982.
63
[65] A. Saltelli, K. Chan, E. M. Scott (Eds.): Sensitivity Anaslysis, J. Wiley & Sons, 2000.
64