Robot Control Kelly 2005-1
Robot Control Kelly 2005-1
Robot Control Kelly 2005-1
Robot Dynamics
q3
z0 zn
z2 link 2
joint n
joint 2 z3
link n
q2 qn
z1
link 1 ⎡ ⎤
x1
joint 1 ⎢x2⎥
x = ⎣ .. ⎦
q1 .
xm
y0
x0
are numbered in such a way that the ith joint connects the ith to the (i − 1)th
link. Each joint is independently controlled through an actuator, which is usu-
ally placed at that joint and the movement of the joints produces the relative
movement of the links. We temporarily denote by zi , the ith joint’s axis of
motion. The generalized joint coordinate denoted by qi , corresponds to the
angular displacement around zi if the ith joint is revolute, or to the linear
displacement along zi if the ith joint is prismatic. In the typical case where
the actuators are placed at the joints among the links, the generalized joint
coordinates are named joint positions. Unless explicitly said otherwise, we
assume that this is the case.
z3
z1
z0 z2 q3
z4
q2
q1 q4
⎡ ⎤
x1
x = ⎣ x2 ⎦
y0 x3
x0
The joint positions corresponding to each joint of the robot, and which
are measured by sensors adequately placed at the actuators, that are usually
located at the joints themselves, are collected for analytical purposes, in the
vector of joint positions q. Consequently, for a robot with n joints, that is,
with n DOF (except for special cases, such as elastic-joints or flexible-link
robots), the vector of joint positions q has n elements:
3 Robot Dynamics 61
⎡ ⎤
q1
⎢ q2 ⎥
q=⎢ ⎥
⎣ .. ⎦
.
qn
i.e. q ∈ IRn . On the other hand it is also of great interest, specifically from a
practical viewpoint, to determine the position and orientation (posture) of the
robot’s end-effector since it is the latter that actually carries out the desired
task. Such position and orientation are expressed with respect to the reference
frame placed at the base of the robot (e.g. a Cartesian frame {x0 , y0 , z0 }) and
eventually in terms of the so-called Euler angles. Such coordinates (and angles)
are collected in the vector x of operational positions1
⎡ ⎤
x1
⎢ x2 ⎥
x=⎢ ⎥
⎣ ... ⎦
xm
where m ≤ n. In the case when the robot’s end-effector can take any position
and orientation in the Euclidean space of dimension 3 (e.g. the room where
the reader is at the moment), we have m = 6. On the other hand, if the
robot’s motion is on the plane (i.e. in dimension 2) and only the position of
the end-effector is of interest, then m = 2. If, however, the orientation on the
plane is of concern then, m = 3.
The direct kinematic model of a robot, describes the relation between the
joint position q and the position and orientation (posture) x of the robot’s
end-effector. In other words, the direct kinematic model of a robot is a function
ϕ : IRn → IRm such that
x = ϕ(q) .
Although lengthy, computation of the direct kinematic model, x = ϕ(q),
is methodical and in the case of robots of only few DOF, it involves simple
trigonometric expressions.
The inverse kinematic model consists precisely in the inverse relation of
the direct kinematic model, that is, it corresponds to the relation between the
operational posture x and the joint position q, i.e.
q = ϕ−1 (x).
The vector τ stands for the forces and torques applied at the joints by
the actuators. The dynamic model (3.1) is the dynamic model in joint space,
while (3.2) corresponds to the dynamic model in operational space. In this
text, we focus on the dynamic model in joint space and, for simplicity, we
omit the words “in joint space”.
Kinematics as much as dynamics, is fundamental to plan and carry out
specific tasks for robot manipulators. Both concepts are dealt with in consid-
erable detail in a large number of available textbooks (see the list of references
at the end of this chapter). However, for the sake of completeness we present
in this chapter the basic issues related to robot dynamics. In the next two
chapters we present properties of the dynamic model, relevant for control. In
Chapter 5 we present in detail the model of a real 2-DOF lab prototype which
serves as a case study throughout the book.
Besides the unquestionable importance that robot dynamics has in control
design, the dynamic models may also be used to simulate numerically (with a
personal computer and specialized software), the behavior of a specific robot
before actually being constructed. This simulation stage is important, since it
allows us to improve the robot design and in particular, to adapt this design
to the optimal execution of particular types of tasks.
One of the most common procedures followed in the computation of the
dynamic model for robot manipulators, in closed form (i.e. not numerical), is
the method which relies on the so-called Lagrange’s equations of motion. The
use of Lagrange’s equations requires the notion of two important concepts
with which we expect the reader to be familiar: kinetic and potential energies.
We describe next in some detail, how to derive the dynamic model of a
robot via Lagrange’s equations.
to use Lagrange’s equation of motion. The latter are named after the French
mathematician Joseph Louis de La Grange (today spelled “Lagrange”) which
first reported them in 1788 in his celebrated work “Mécanique analytique”2 .
Consider the robot manipulator with n links depicted in Figure 3.1. The
total energy E of a robot manipulator of n DOF is the sum of the kinetic and
potential energy functions, K and U respectively, i.e.
where q = [q1 , · · · , qn ]T .
The Lagrangian L(q, q̇) of a robot manipulator of n DOF is the difference
between its kinetic energy K and its potential energy U, that is,
where τi correspond to the external forces and torques (delivered by the ac-
tuators) at each joint as well as to other (nonconservative) forces. In the class
of nonconservative forces we include those due to friction, the resistance to
the motion of a solid in a fluid, and in general, all those that depend on time
and velocity and not only on position.
Notice that one has as many scalar dynamic equations as the manipulator
has degrees of freedom.
The use of Lagrange’s equations in the derivation of the robot dynamics
can be reduced to four main stages:
In the rest of this section we present some examples that illustrate the
process of obtaining the robot dynamics by the use of Lagrange’s equations
of motion.
z0 z0 m2
l2
m1 ϕ
l1
x0
x0 y0
y0 q1
Figure 3.3. Example of a 1-DOF mechanism
influence on the robot dynamics. This should not surprise the reader; note
that the potential energy for this robot is constant since its mass does not
move in the vertical direction.
A particularity of this example, is that the actuators that deliver the
torques are not physically located at the joints themselves, but one of them
transmits the motion to the link through belts. Such types of transmission are
fairly common in industrial robots. The motivation for such configurations is
to lighten the weight (henceforth the inertia) of the arm itself by placing the
actuators as close to the base as possible.
z
τ1
x
l1 q1
I1 lc1
m1 y
τ2
q2 δ
m2
I2
lc2
respective centers of mass and that are parallel to the z axis. The joint
positions associated to the angles q1 and q2 are measured between each
respective link and an axis which is parallel to the x axis. Both angles
are taken to be positive counterclockwise. The motion is transmitted
to link 2 by a belt since the corresponding actuator is placed at the
base of the robot. The vector of joint positions q is defined as
q = [q1 q2 ]T .
The kinetic energy function K(q, q̇) for this arm may be decom-
posed into the sum of two parts: K(q, q̇) = K1 (q, q̇) + K2 (q, q̇), where
K1 (q, q̇) and K2 (q, q̇) are the kinetic energies associated with the
masses m1 and m2 respectively. In turn, the kinetic energy includes the
linear and angular motions. Thus, we have K1 (q, q̇) = 12 m1 v12 + 12 I1 q̇12 ,
where v1 is the speed3 of the center of mass of link 1. In this case,
1 1
K1 (q, q̇) = 2 2
m1 lc1 q̇1 + I1 q̇12 . (3.6)
2 2
On the other hand, K2 (q, q̇) = 21 m2 v22 + 12 I2 q̇22 , where v2 is the
speed of the center of mass of link 2. This speed squared, i.e. v22 , is
given by
v22 = ẋ22 + ẏ22
where ẋ2 and ẏ2 are the components of the velocity vector of the
center of mass of link 2. The latter are obtained by evaluating the
time derivative of the positions x2 and y2 of the center of mass of link
2, i.e.
x2 = l1 cos(q1 ) + lc2 cos(q2 − δ)
y2 = l1 sin(q1 ) + lc2 sin(q2 − δ) .
Using the trigonometric identities, cos(θ)2 + sin(θ)2 = 1 and
sin(q1 )sin(q2 − δ) + cos(q1 )cos(q2 − δ) = cos(q1 − q2 + δ), we finally get
∂L
= −m2 l1 lc2 sin(q1 − q2 + δ)q̇1 q̇2 ,
∂q1
∂L
= m2 lc2
2
q̇2 + m2 l1 lc2 cos(q1 − q2 + δ)q̇1 + I2 q̇2 ,
∂ q̇2
' (
d ∂L
= m2 lc2
2
q̈2 + m2 l1 lc2 cos(q1 − q2 + δ)q̈1
dt ∂ q̇2
− m2 l1 lc2 sin(q1 − q2 + δ)q̇1 (q̇1 − q̇2 ) + I2 q̈2 ,
∂L
= m2 l1 lc2 sin(q1 − q2 + δ)q̇1 q̇2 .
∂q2
Thus, the dynamic equations that model the robot manipulator
may be obtained by using Lagrange’s equations (3.4),
+ ,
τ1 = m1 lc1
2
+ m2 l12 + I1 q̈1
+ m2 l1 lc2 cos(q1 − q2 + δ)q̈2
+ m2 l1 lc2 sin(q1 − q2 + δ)q̇22
and,
3.1 Lagrange’s Equations of Motion 69
and
z0 q2 z0
m3 m2
q3 m1
q1
q1
y0 y0
q2
x0 x0 q3
1+ ,
K(q, q̇) = m1 q̇32 + [m1 + m2 ]q̇22 + [m1 + m2 + m3 ]q̇12 . (3.10)
2
On the other hand, the potential energy is given by
So we have
' (
∂L d ∂L
= [m1 + m2 + m3 ]q̇1 = [m1 + m2 + m3 ]q̈1
∂ q̇1 dt ∂ q̇1
' (
∂L d ∂L
= [m1 + m2 ]q̇2 = [m1 + m2 ]q̈2
∂ q̇2 dt ∂ q̇2
' (
∂L d ∂L
= m1 q̇3 = m1 q̈3
∂ q̇3 dt ∂ q̇3
∂L ∂L ∂L
= =0 = −[m1 + m2 + m3 ]g.
∂q2 ∂q3 ∂q1
The dynamic equations that model this robot may be obtained by
applying the Lagrange’s equations (3.4) to obtain
3.2 Dynamic Model in Compact Form 71
where τ1 , τ2 and τ3 are the external forces applied at each joint. Notice
that in this example Equations (3.12)–(3.14) define a set of linear
autonomous differential equations.
In terms of the state vector [q1 q2 q3 q̇1 q̇2 q̇3 ] , the equations
(3.12), (3.13) and (3.14) may be expressed as
⎡ ⎤
⎡ ⎤ q̇1
q1
⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ q̇2 ⎥
⎢ q2 ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ q̇3 ⎥
⎢ q3 ⎥ ⎢ ⎥
d ⎢ ⎥ ⎢ ⎥
⎢ ⎥=⎢ 1 ⎥.
dt ⎢ ⎥ ⎢ [τ − [m + m + m ]g] ⎥
⎢ q̇1 ⎥ ⎢ m1 + m2 + m3 1 1 2 3 ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ 1 ⎥
⎢ ⎥ ⎢ τ ⎥
⎢ q̇2 ⎥ ⎢ 2 ⎥
⎣ ⎦ ⎢ m 1 + m 2 ⎥
⎣ ⎦
1
q̇3 τ3
m1
The necessary and sufficient condition for the existence of equilib-
ria is τ1 = [m1 + m2 + m3 ]g, τ2 = 0 and τ3 = 0 and actually we have
an infinite number of them:
T T
[q1 q2 q3 q̇1 q̇2 q̇3 ] = [q1∗ q2∗ q3∗ 0 0 0]