0
⎥ ⎢λ ⎥
31
3
⎣
⎦
⎣
⎦ ⎣ ⎦
Differentiation equation 30 with respect to time yields
θ
⎡ ⎤
⎡ x⎤
⎡ x⎤
11
⎢ ⎥
⎢ ⎥ d
θ
Γ
y
⎢
y⎥
= Γ
+
⎢
21 ⎥
⎢ ⎥ dt ⎢ ⎥
θ
⎢ ⎥
⎢ z⎥
⎢ z⎥
⎣ 31⎦
⎣ ⎦
⎣ ⎦
Substituting into equation 34 yields
⎡λ ⎤
⎡ x⎤
⎡ x⎤
⎡−cosθ
⎤
1
11
⎢ ⎥
⎢ ⎥
d
(35)
λ
Γ
2 B
y
2 B
⎢ y⎥ C ⎢ sinθ ⎥
=
Γ
+
+
2
21
⎢ ⎥
⎢ ⎥
dt ⎢ ⎥
⎢
⎥
⎢λ ⎥
⎢ z ⎥
⎢ z⎥
⎢
0
⎥
⎣ 3 ⎦
⎣ ⎦
⎣ ⎦
⎣
⎦
3.2.9 Taking the derivatives of the Lagrange function with respect to X
d
L
∂
L
∂
(
) = 2 Ax ,
= 0
dt
x
∂
x
∂
3
d
L
∂
L
∂
f
∂
(
) −
= ∑(
i
λ
) + Q
i
4
dt
x
∂
x
∂
∂
i 1
=
x
280
Parallel Manipulators, Towards New Applications
2 Ax = F − Γ λ − Γ λ − Γ λ
(36)
x
11 1
21 2
31 3
where Fx , Fx and Fx are the forces applied by the actuator for the first, second and third
limbs. Γ is the (i, j) element of the Γ matrix.
ij
3.2.10 Taking the derivatives of the Lagrange function with respect to Y
d
L
∂
L
∂
(
) = 2 Ay ,
= 0
dt
y
∂
y
∂
3
d
L
∂
L
∂
f
∂
(
) −
= ∑(
i
λ
) + Q
i
5
dt
y
∂
y
∂
∂
i 1
=
y
2 Ay = F − Γ λ − Γ λ − Γ λ
(37)
y
12 1
22 2
32 3
3.2.11 Taking the derivatives of the Lagrange function with respect to Z
∂
d
L
(
) = 2 Az , L
∂ = E
dt
z
∂
z
∂
3
d
L
∂
L
∂
f
∂
(
) −
= ∑(
i
λ
) + Q
i
6
dt
z
∂
z
∂
∂
i 1
=
z
2 Az − E = F − Γ λ − Γ λ − Γ λ
(38)
z
13 1
23 2
33 3
Rearrangement of equations 36, 37, and 38 produces:
⎡ F ⎤
⎡ x ⎤ ⎡ 0 ⎤
⎡λ ⎤
x
1
⎢ F ⎥ 2 A ⎢ y ⎥ ⎢
0 ⎥
T ⎢λ ⎥
=
−
+ Γ
y
2
⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥
⎢ F ⎥
⎢ z ⎥ ⎢ E ⎥
⎢λ ⎥
⎣ z ⎦
⎣ ⎦ ⎣ ⎦
⎣ 3 ⎦
Substituting into equation 35 yields
⎡ F ⎤
⎡ 0 ⎤
⎡− cosθ ⎤
⎡ x⎤
⎡ x ⎤
x
11
⎢ ⎥
⎢ ⎥
⎢
⎥
⎢ ⎥
d Γ
F
0
T C sinθ
(2
T
AI +
2 B )
T
y +
2 B
⎢ y ⎥
= −
+ Γ
+
Γ
Γ
Γ
y
21
⎢ ⎥
⎢ ⎥
⎢
⎥
⎢ ⎥
dt ⎢ ⎥
⎢ F ⎥
⎢ E ⎥
⎢
0
⎥
⎢ z⎥
⎢ z ⎥
⎣ z ⎦
⎣ ⎦
⎣
⎦
⎣ ⎦
⎣ ⎦
The dynamic equation of the whole system can be written as
F = M ( q) q+ G( q, q) q + K ( q)
(39)
Where
⎡ F ⎤
⎡ x⎤
⎡ x ⎤
⎡ x ⎤
Γ
x
,
d
⎢ ⎥ ,
⎢ ⎥ ,
,
( ) = 2
T
M q
AI + Γ 2 B Γ , G ( q, q)
T
= Γ 2 B
, and
F
⎢ F ⎥
=
q = y
q = y
q
⎢ y ⎥
=
y
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎢ ⎥
dt
⎢ F ⎥
⎣
⎢ z⎥
⎢ z ⎥
⎢ z ⎥
z ⎦
⎣ ⎦
⎣ ⎦
⎣ ⎦
Cartesian Parallel Manipulator Modeling, Control and Simulation
281
⎡ 0 ⎤
⎡−cosθ ⎤
11
( )
⎢ 0 ⎥ T
K q
C ⎢ sinθ ⎥
= −
+ Γ
21
⎢ ⎥
⎢
⎥
⎢ E ⎥
⎢
0
⎥
⎣ ⎦
⎣
⎦
where q is the 3×1 vector of joint displacements, q is the 3×1 vector of joint velocities, F is the 3×1 vector of applied force inputs, M(q) is the manipulator inertia matrix, G( q, q) is the manipulator centripetal and coriolis matrix which is the 3×3 matrix of centripetal and
coriolis forces, and K(q) is the vector of gravitational forces which is the 3×1 vector of
gravitational forces due to gravity. The Lagrangian dynamics equation, equation 39, possess
important properties that facilities analysis and control system design. Among theses are
(Lewis et al., 1993): the M(q) is a 3×3 symmetric and positive definite matrix and the matrix
W ( q, q) = M ( q) − 2 G( q, q) is a skew symmetric matrix.
4. Controller design
4.1 Introduction
The control problem for robot manipulators is the problem of determining the time history
of joint inputs required to cause the end-effector to execute a commanded motion. The joint
inputs may be joint forces or torques depending on the model used for controller design.
Position control and trajectory tracking are the most common tasks for robot manipulators;
given a desired trajectory, the joint force is chosen so that the manipulator follows that
trajectory. The control strategy should be robust with respect to initial condition errors,
sensor noise, and modeling errors. The primary goal of motion control in joint space is to
make the robot joints q track a given time varying desired joint position qd. Rigorously, we
say that the motion control objective in joint space is achieved provided that lim (
e t) = 0
t→∞
where e(t)=qd(t) - q(t) denotes the joint position error. Although the dynamics of the
manipulator‘s equation is complicated, it nevertheless is an idealization, and there are a
number of dynamic effects that are not included in this equation. For example, friction at the
joints is not accounted for in this equation and may be significant for some manipulators.
Also, no physical body is completely rigid. A more detailed analysis of robot dynamics
would include various sources of flexibility, such as elastic deformation of bearings and
gears, deflection of the links under load, and vibrations.
4.2 PID control versus model based control
The PID controller is a single-input/single-output (SISO) controller that produces a control
signal that is a sum of three terms. The first term is proportional (P) to the positioning error,
the second term is proportional to the integral (I) of the error, and the third is proportional
to the derivative (D) of the error. The PID (or PD) type is usually employed in industrial
robot manipulators because it is easy to implement and requires little computation time
during real time operation. This approach views each actuator of the manipulator
independently, and essentially ignores the highly coupled and nonlinear nature of the
manipulator. The error between the actual and desired joint position is used as feedback to
control the actuator associated with each joint. However, independent joint controllers can
not achieve a satisfactory performance due to their inherent low rejection of disturbances
and parameter variations. Because of such limitation, model based control algorithms were
proposed (Sciavicco et al., 1990) that have the potential to perform better than independent
282
Parallel Manipulators, Towards New Applications
joint controllers that do not account for manipulator dynamics. However, the difficulty with
the model based controller is that it requires a good model of the manipulator inverse
dynamics and good estimates of the model parameters, making this controller more
complex and difficult to implement than the non-model based controller. The model based
control scheme was intensively experimentally tested for example the experimental
evaluation of computed torque control was presented in (Griffiths et al., 1989).
4.3 PD control with position and velocity reference
The first PD control law is based on the position and velocity error of each actuator in the
joint space. To implement the joint control architecture, the values for the joint position error
and the joint rate error of the closed chain system are used to compute the joint control force
F (Spong & Vidyasagar, 1989).
F = K e + K e
(40)
P
D
Where e = q − q , which is the vector of position error of the individual actuated joints, and
d
e = q − q , which is the vector of velocity error of the individual actuated joints. Where
d
q and q are the desired joint velocities and positions, and KD and KP are 3 ×3 diagonal
d
d
matrices of velocity and position gains. Although this type of controller is suitable for real
time control since it has very few computations compared to the complicated nonlinear
dynamic equations, there are a few downsides to this controller. It needs high update rate to
achieve reasonable accuracy. Using local PD feedback law at each joint independently does
not consider the couplings of dynamics between robot links. As a result, this controller can
cause the motor to overwork compared to other controllers presented next.
4.4 PD Control with gravity compensation
This is a slightly more sophisticated version of PD control with a gravitational feedforward
term. Consider the case when a constant equilibrium posture is assigned for the system as
the reference input vector qd. It is desired to find the structure of the controller which
ensures global asymptotic stability of the above posture. The control law F is given by
(Spong & Vidyasagar, 1989):
F = K e + K e + K( q )
(41)
P
D
d
It has been shown (Spong, 1996) that the system is asymptotically stable but it is only
proven with constant reference trajectories. Although with varying desired trajectories, this
type of controller cannot guarantee perfect tracking performance. Hence, more dynamic
modeling information is needed to incorporate into the controller.
4.5 PD control with full dynamics feedforward terms
This type of controller augments the basic PD controller by compensating for the
manipulator dynamics in the feedforward way. It assumes the full knowledge of the robot
parameters. The key idea for this type of controller is that if the full dynamics is correct, the
resulting force generated by the controller will also be perfect. The controller is given by
(Gullayanon , 2005)
F = M ( q ) q + G( q , q ) q + K ( q ) + K e + K e
(42)
d
d
d
d
d
d
P
D
Cartesian Parallel Manipulator Modeling, Control and Simulation
283
If the dynamic knowledge of the manipulator is accurate, and the position and velocity error
terms are initially zero, the applied force F = M ( q ) q + G ( q , q ) q + K ( q ) d
d
d
d
d
d
is sufficient to maintain zero tracking error during motion. This controller is very similar to
the computed torque controller, which is presented next. The difference between these two
controllers is the location of the position and velocity correction terms. This controller is less
sensitive to any mass changes in the system. For example, if the robot picks up a heavy load
in the middle of its operation, this controller is likely to respond to this change slower
compared to computed torque controller. The advantage of this controller is that once the
desired trajectory for a given task has been specified, then the feedforward terms relying on
the robot dynamics M ( q ) q + G ( q , q ) q + K ( q ) can be computed offline to reduce the d
d
d
d
d
d
computational burden.
4.6 Computed torque control
This controller is developed for the manipulator to examine if it is possible to improve the
performance of the trajectory tracking of the manipulator by utilizing a more complete
understanding of the manipulator dynamics in the controller design. This controller
employs a computed torque control approach, and it uses a model of the manipulator
dynamics to estimate the actuator forces that will result in the desired trajectory. Since this
type of controller takes into account the nonlinear and coupled nature of the manipulator,
the potential performance of this type of controller should be quite good. The disadvantage
of this approach is that it requires a reasonably accurate and computationally efficient
model of the inverse dynamics of the manipulator to function as a real time controller. The
controller computes the dynamics online, using the sampled joint position and velocity data.
The key idea is to find an input vector, F, as a function of the system states, which is capable
to realize an input/output relationship of linear type. It is desired to perform not a local
linearization but a global linearization of system dynamics obtained by means of a nonlinear
state feedback. Using the computed torque approach with a proportional-derivative (PD)
outer control loop, the applied actuator forces are calculated at each time step using the
following force law as described by Lewis, 1993:
F = M ( q)[ q + K e + K e] + G( q, q) q + K( q) (43)
d
D
p
where F is the force applied to input links, KD is the diagonal matrix of the derivative gains,
KP is the diagonal matrix of the proportional gains, and e is the vector of the position errors
of the input links, e = q − q . To show that the computed torque control scheme linearizes
d
the controlled system, the force computed by equation 43 is substituted into equation 39,
yielding: M ( q) q = M ( q)