SM Slides

CONTROL OF ROBOT MANIPULATORS IN
JOINT SPACE
Rafael Kelly
Centro de Investigacion Cientfica y de Educacion Superior de Ensenada
Mexico
Vctor Santiban
ez
Instituto Tecnologico de la Laguna
Mexico
Antonio Lora
Centre National de la Recherche Scientifique
France
Springer Berlin Heidelberg New York Hong Kong London Milan Paris Tokyo
Control of Robot Manipulators in Joint Space
Part I
PRELIMINARIES
R. Kelly, V. Santiba
nez and A. Loria
Introduction
Introduction
Robots occupy a privileged place in the modernization of numerous industrial

sectors.
The word robot finds its origins in robota which means work in
Slavic languages.
Was introduced by the Czech science fiction writer Karel Capek.

The term robot is nowadays used to denote autonomous machines.
nez and A. Loria
Introduction
These machines may be roughly classified as follows:

Robot manipulators
Ground robots Wheeled robots

Legged robots
Mobile robots
Submarine robots
Aerial robots.
This book is exclusively devoted to robot manipulators.
nez and A. Loria
Introduction
Robotics
Term coined by the science fiction writer Isaac Asimov
Science devoted to the study of robots
Incorporates a variety of fields:
Electric engineering, mechanical engineering, industrial engineering,
computer science and applied mathematics
Automatic control of robot manipulators (spine of robotics).
nez and A. Loria
Introduction
The International Federation of Robotics (norm ISO/TR 8373) defines:
A manipulating industrial robot is an automatically controlled,

reprogrammable, multipurpose manipulator programmable in
three or more axes, which may be either fixed in place or mobile
for use in industrial automation applications.
nez and A. Loria
Introduction
The number of joints of a manipulator determines as well, its number of

degrees of freedom (DOF ) typically 6 DOF .
3 determine the position of the end of the last link in the Cartesian space
3 more specify its orientation.
q1
q2
q3
Figure 1: Robot manipulator.

nez and A. Loria
Introduction
In the present textbook:

A robot manipulator or simply manipulator is a mechanical
articulated arm that is constituted of links interconnected through
hinges or joints that allow a relative movement between two
consecutive links.
nez and A. Loria
Introduction
The joint positions of the robot are collected in the vector q, i.e.,

q1
q2
q :=
.. .
qn
The joint velocities are: q :=
d
dt q
1
2
The torques and forces are collected in the vector: :=

.. .
n
nez and A. Loria
Ch. 1. What does control of robots consists in?
Ch. 1. What does control of robots

consists in?
Robot control consists in studying how to make a robot manipulator perform

a task.
Control design may be divided roughly in the following steps:
Familiarization with the physical system under consideration,
Modeling.
Control specifications.
nez and A. Loria
Familiarization with the physical system under

consideration
Figure 2: Freely moving robot.

For this robot, the outputs y, are the positions q and joint velocities q
or the position and orientation of the end effector.
nez and A. Loria
10
Figure 3: Robot interacting with its environment.
In this case, the output y may include the torques and forces f exerted
by the end tool over its environment.
nez and A. Loria
11
Image
Camera
Figure 4: Robotic system: fixed camera.

In this system, the output y may correspond to the coordinates associated
to each of the marks with reference to a screen on a monitor.
nez and A. Loria
12
Image
Camera
Figure 5: Robotic system: camera in hand.

Output y may correspond to the coordinates of the screen.
nez and A. Loria
13
ROBOT
q
Figure 6: Input-output representation of a robot.
The input variables, are basically the torques and forces ,
The outputs are the joint positions and velocities:

q
f) =
y = y(q, q,
q
nez and A. Loria
14
Derivation of the dynamic model
The systems mathematical model is obtained typically via one of the two
following techniques.
Analytic. Physics laws of the systems motion.
Experimental. Experimental data collected from the system itself.
nez and A. Loria
15
Some interesting topics related with modelling are:

Robustness. Faculty of a control system to cope with errors due to
neglected dynamics.
Parametric identification. The objective is to obtain the numeric values
of different physical parameters.
The dynamic model of robot manipulators
is derived in the analytic form using basically the laws of mechanics.
an n DOF system (multivariable nonlinear system).
nez and A. Loria
16
Control specifications
Definition of control objectives:

Stability
Regulation
Trajectory tracking (motion control)
Optimization.
nez and A. Loria
17
Stability. Consists in the property of a system by which it goes on

working at certain regime or closely to it for ever.
Lyapunov stability theory.
input-output stability theory.
In the case when the output y corresponds to the joint position q and
velocity q.
Regulation Position control in joint coordinates
Trajectory tracking Tracking control in joint coordinates
nez and A. Loria
18
Motion control of robot manipulators
Point-to-point. Determines a series of points in the manipulators

workspace, by which the end effector is required to pass.
Figure 7: Point-to-point motion specification.

nez and A. Loria
19
(Continuous) trajectory. The control problem consists in making the

end-effector follow a trajectory as closely as possible.
Figure 8: Trajectory motion specification.

Set-point control problem. The specified trajectory is simply a point in
the workspace.
nez and A. Loria
20
Robot navigation problem consists in solving, in one single step, the following
subproblems.
Path planning. Determines a curve in the state space of the desired
posture
Trajectory generation. Parameterizes in time above curve
Control design. Solves the control problem
nez and A. Loria
21
Ch. 2. Introduction to Lyapunov stability theory
Basic notation
Throughout the text we employ the following symbols.
for all
there exists
belong(s) to
=
implies
is equivalent to or if and only if
tends to
:= and =: is defined as and equals by definition to .
nez and A. Loria
22
Preliminaries: Linear algebra
Vectors
IRn denotes the real Euclidean space of dimension n
Vector x of dimension n
x1
x2
= [x1 x2 xn]T
x=
.
.
xn
Denoted by bold small letters, either Latin or Greek.
nez and A. Loria
23
Euclidean norm
The Euclidean norm kxk of a vector x IRn is defined as
v
u n
uX
2
t
xi = xTx
kxk :=
i=1
Matrices
IRnm denotes the set of real matrices A of dimension n m
a11 a12 a1m

a21 a22 a2m
A = {aij } =
..
..
...
.
..
an1 an2 anm
nez and A. Loria
24
Eigenvalues
For each square matrix A IRnn there exist n eigenvalues (in general,
complex numbers)
denoted by 1{A}, 2{A}, , n{A}.
satisfy: det [i{A}I A] = 0,
for i = 1, 2, , n
For the case of a symmetric matrix A = AT IRnn,

1{A}, 2{A}, , n{A} IR.
theorem of RayleighRitz establishes that for all x IRn
2
Max{A} kxk xTAx min{A} kxk .
nez and A. Loria
25
Spectral norm
The spectral norm kAk of a matrix A IRnm is defined as

q
kAk = Max{ATA},
nez and A. Loria
26
Fixed points
Consider a continuous function f : IRn IRn. The vector x IRn is a

fixed point of f (x) if
f (x) = x.
If x is a fixed point of the function f (x), then x is a solution of
f (x) x = 0.
The Contraction Mapping Theorem provides a sufficient condition for
the existence and unicity of fixed points.
nez and A. Loria
27
Theorem 2.1 Contraction Mapping.

following continuous function
Consider IRm and the
n
f : IRn
IR

x
7 f (x, )
where stands for a vector of parameters.

Assume that there exists a non-negative constant k such that
kf (x, ) f (y, )k k kx yk for all x, y IRn and all .
If k < 1, then for each , the function f (x, ) possesses a unique
fixed point x IRn.
nez and A. Loria
28
Lyapunov stability
Second method of Lyapunov or direct method of Lyapunov.

Dynamical systems described by
x = f (t, x),
x IRn, t IR+ ,
(1)
where
x corresponds to the state.
We will use x(t) to denote a solution to (1) in place of x(t, t, x(t)).
nez and A. Loria
29
We assume that f : IR+ IRn IRn is continuous in t and x and is such

that
it has a unique solution corresponding to each initial condition t, x(t)
and
the solution x(t, t, x(t)) depends continuously on the initial conditions
t, x(t).
If f (t, x) = f (x) then, is said to be autonomous.
we can safely consider that t = 0.
f (t, x) = A(t)x + u(t) is linear differential equation
In the opposite case it is nonlinear
nez and A. Loria
30
The concept of equilibrium
Definition 2.1 A constant vector xe IRn is an equilibrium or equilibrium

state of the system x = f (t, x), if
f (t, xe) = 0
t 0.
Typically x = 0 IRn is an equilibrium of x = f (t, x). If this is not the

case, this may be translated to the origin.
nez and A. Loria
31
x
62
x(0) = xe
x1

1
x(t)

............................................................................................
..........................................................................
.
.
.
.

.... .
..
. ....

.... ...
...

..

x
62
x(0) = xe
t
.... .... .... .... ....

...
...
...
.
x1
Figure 9: Equilibrium
If the initial state x(t) IRn is an equilibrium (x(t) = xe IRn) then,
x(t) = xe
x(t)
=0
t t 0
t t 0.
nez and A. Loria
32
Example 2.2
Consider a pendulum whose dynamic model is
J q + mgl sin(q) = (t)
l
g
m
Figure 10: Pendulum.

nez and A. Loria
33
where
m is the mass of the pendulum
J is the total moment of inertia about the joint axis
l is the distance from its axis of rotation to the center of mass
q is the angular position.
In terms of the state [q q]
T , the dynamic model is given by

q
q
d
.
=
dt
q
J 1 [ (t) mgl sin(q)]
nez and A. Loria
34
If (t) = 0,
the equilibrium states are given by
[q q]
T = [n 0]T for n = , 2, 1, 0, 1, 2,
since mgl sin(n) = 0.
If (t) = such that | | > mgl,
there does not exist any equilibrium since there is no q IR such that
= mgl sin(q ).
nez and A. Loria
35
Definitions of stability
x(t )
x1
x2
1

................ .... ....
.... .... .... .... .... .... .... .... .... .... ....
.... ....
... .... .... ....6
.. .
.... ........ ........ ...... .... .... .... .... .
...
.

.
.
.
.
.
.
.
.
.
... .
..
.... ...............
.... .... ....

...
...
...
....
.....
....
....
. ... ..................
...................
..
.. ............................
..

.....
...x(t)
... ...
. ...
......... ... .......
. .... .........
...
.
.
..
.
.
.
.
.
...
.....
...
... ...
....
...
.
.
.
.
.
.
.
.
.
.
.
.
.
.
... ... x
.
.
......
..
.
..
.. ............ ..
e
... ....... ............
.. ..
...
...
...
..
..
.
. .............

.........
..
........ ..
... .
... ..
... ..
.. ..
... ..
....... ... ..
.
.
.
.

.
. ..
.... ....
.
.
t
.
.
.... ....
.
.
.
..
.. ...
.
.
.
.
...
. .
.
.
. .. ....
.
...
...
...
...
... ....
. . ...
.
..
...
.... . . .... . .......
.... . . .... .
...
...
... ...
... ...
...
...
....
....
.
.
.
.
.
.
.
.
....
.
.... ...................... .... .... .... .... .... .... .... .... .... .... .... .... .... .... .... .... .... ....... ........... .... ....
x(t )
x2
.... ....
@.... .... .... ....6
....
....
.... @
....
...........
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
...
R
... ...@
.. ........ .......
..
..... ...........
.
.
.
...
.
.
.
......
.
..
...
. .....
........
...
... ... ...................
.....
.
.
.
.....
...
.. ...
..
.....
... ..
.
.
.
.
.
......... ..
...
.
..
.
.
.
.
.
.
......
...... .
x1
.
.
.
.
...
.
.
.
.
....
....
..
........ .......
...
....
............
...
.... .... .... ....
...
...
....
..
.
.
....
.... .... .... .... ....
....
Figure 11: Stability

Definition 2.2 Stability.
The origin is a stable equilibrium (in the sense of Lyapunov) of Equation (1)
if, for each pair of numbers > 0 and t 0, there exists = (, t) > 0
such that
kx(t)k < = kx(t)k <
nez and A. Loria
t t 0 .
(2)
36
Example 2.3
..
.
...................
.
.
.
.
.
.
.
.
.
.
.
.
.
.......
.
......
.
....
..... ............................ ........
... ..
. ..
..... ..... ........ ..... ......... ............. ............ ........... ....
. ... ..
... ...x
... .........e
............... .....
.....
........ .. .........
.......................
..
..
x2
x1
Figure 12: Harmonic oscillator: Phase plane

System described by the equations:
x 1 = x2
x 2 = x1
The origin is the unique equilibrium point and is stable.
nez and A. Loria
37
Definition 2.3 Uniform stability.

The origin is a uniformly stable equilibrium (in the sense of Lyapunov) of
Equation (1) if for each number > 0 there exists = () > 0 such that
(2) holds.
nez and A. Loria
38
x2
6
... .... .... ..
x(0)
x1
.. ...
....0.... .
. ..
1
..

.... .... .... .... .... .... .... .... .... ....
.... .... .... .... .... ............... .... .
....
... ....................... .... ........
.
...

.
.
.
..
..
....
.
....
..

... ... .... .... .... ..... ..
....
...

.
... ... ... .
... .
.
.

.
.
.
...
..............................
.
.. .. ...
......
.
.
.
.
.
...... ...... ................................... x(t)
...
.....
.. .. .. x

.....
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. .......
.
....
e
.. .....
..
..
... ..... .. ..
.......
.....................
.
.
.
.
.
... ...
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.

.
... ....
... ..... . .. .....
. ...........
...
..
.
.
.
.
.
.
...
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
..
...
.
t
..
... ..
..
. ...
...
..
.
... ....... .. ..
... ... ...
...
........... .
..
.
..
...
.... .... .... .... .... ...
....
..
...
.
..
....
...
... ....... .... ...... .... .... .... .... .... .... .... .... .... .... .... .... .... .... .... ........ ...... ..... .... ...
.
.
.
.
.
.
.
.
.
.
.
.
.
....
....
....
....
.... ...
. .... .... ....
Figure 13: Asymptotic stability

Definition 2.4 Asymptotic stability.
The origin is an asymptotically stable equilibrium of (1) if
1. the origin is stable and
2. the origin is attractive, i.e., for each t, there exists 0 = 0(t) > 0 :
kx(t)k < 0
nez and A. Loria
kx(t)k 0 as
t , t 0.
(3)
39
..
..
..................................................................... .............
.
.
.
.
.
.
.
.
. ..
.. .
.
................ .. ........................ .........
.
........... ........................ .... ............ ........
.. .. ...... ....... .................. ....
.. ..
.
.
.
..... ..... ..... ..... ..... ............... ..... ....... ..... .
........... ........... ..... ......................................... .... .... .... ..
... ... ..... ... ...............................
.
. ...
.... ...... ................................. ......... .... ...
.. .... .
.
...... .......
.................................................................................................
...........................................
.
..
..
x2
x1
Figure 14: Attractive but unstable equilibrium (x1 = rcos() and

x2 = r sin())
Example 2.4
5
r =
r(1 r)
100
= sin2(/2)
[0, 2).
Equilibria at the origin [r ]T = [0 0]T and at [r ]T = [1 0]T .
nez and A. Loria
40
Definition 2.5 Uniform asymptotic stability.

The origin is an uniformly asymptotically stable equilibrium of (1) if
1. the origin is uniformly stable and
2. the origin is uniformly attractive, that is, there exists a number 0 > 0
such that (3) holds with a rate of convergence independent of t.
nez and A. Loria
41
Definition 2.6 Global asymptotic stability.

The origin is a globally asymptotically stable equilibrium
1. the origin is stable and
2. the origin is globally attractive, that is,
kx(t)k 0 as t ,
nez and A. Loria
x(t) IRn, t 0.
42
Definition 2.7 Global uniform asymptotic stability.

The origin is a globally uniformly asymptotically stable equilibrium of
Equation (1) if:
1. the origin is uniformly stable with () in (2) which satisfies ()
as (uniform boundedness) and
2. the origin is globally uniformly attractive, i.e., for all x(t) IRn and all
t 0,
kx(t)k 0 as t
with a convergence rate that is independent of t.
nez and A. Loria
43
Definition 2.8 Global exponential stability.

The origin is a globally exponentially stable equilibrium of (1) if there exist
positive constants and , independent of t, such that
kx(t)k < kx(t)k e(tt),
nez and A. Loria
t t 0,
x(t) IRn .
44
..
.........
.................................................................
.
.
.
.
........
.
.
.
.
.
......
....................
.
.
.
.
.
.
.....
.
.
.
.
............ ...................
.
.
.
.
......
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
....
...... .... ... .. ... 0
.
..... ..... ..... ...... ..... ...... ..... ....... ..... .......... ....... .... ..... ..... ..... ............ ..... .................. ..... ..... ..
... .... .. . ...... ..
.... ..................
... .......... ................. ...... ................................ ....
................. ......... ........
...
.
...
.. ........
.............. .......................
.............. ..
.
x2
x1
Figure 15: Van der Pol: Phase plane.

Definition 2.9 Instability.
The origin of Equation (1) is unstable if it is not stable.
Example 2.6 Van der Pol system,
x 1 = x2,
x 2 = x1 + (1 x21)x2.
nez and A. Loria
45
Lyapunov functions
Definition 2.10 Locally and globally positive definite function.
A continuous function W : IRn IR+ is said to be locally positive definite
if
1. W (0) = 0,
2. W (x) > 0
for small
kxk 6= 0.
It is said to be globally positive definite (or simply positive definite) if

1. W (0) = 0,
2. W (x) > 0
x 6= 0 .
nez and A. Loria
46
W (x) is said to be (locally) negative definite if W (x) is (locally)

positive definite.
A function V : IR+ IRn IR+ is (resp. locally) positive definite if
there exists a positive definite function W : IRn IR+ such that
1. V (t, 0) = 0 t 0
2. V (t, x) W (x), t 0,
nez and A. Loria
x IRn (resp. for small kxk).
47
Definition 2.11 Radially unbounded function and decrescent

function.
A continuous function W : IRn IR is said to be radially unbounded if
W (x)
as
k x k .
V (t, x) is radially unbounded if V (t, x) W (x) for all t 0.

A continuous function V : IR+ IRn IR is (resp. locally) decrescent if
there exists a (resp. locally) positive definite function W : IRn IR+ such
that
V (t, x) W (x)
t0
x IRn (resp. for small kxk).
If V (t, x) = V (x) then V (x) is decrescent.

nez and A. Loria
48
Example 2.7
Consider the graphs of the functions Vi(x) with i = 1, . . . , 4 as depicted in

Figure 16.
V1(x) is locally positive definite but is not (globally) positive definite.
V2(x) is locally positive definite and (globally) positive definite. Also it
is radially unbounded.
V3(x) is locally positive definite and (globally) positive definite but it is
not radially unbounded.
V4(x) is positive definite and radially unbounded.
nez and A. Loria
49
.
...
..
... (x) ...
.
V
.
... 1
..
..
...
...
...
..
...
..
..
...
.
.
.
...
... ......... ... ......... ...
... .. ... . .. ... ..
... .. ... .. .. ... ..
.. .
. . .. . .
..... ..... .... ........ ....... .... .... ....... ..... ..... ........ ........ ..... ..... ...0
.
... .
... .
..
.
.....
.....
...
..
...V2.. (x) ..
.... ...
... .. ...
.
.
.
.
... .. ...
. ... ..
... . ... . ... . .. ..... ...
... .... ...... . ..... .. ..
... .. ... .. ... ... ...
... .. .... .. .. ... .
... . .. . ... ..
..... ...... .. ......
. ......
.... .... .... .... ..... ..... ..... ........ ..... ..... ..... ..... ..... ..... ....
..
..
x
0
..
..
..
........................... ... ...........................
... . .
... .. ..
... . ..
... ... ..
... .. ..
..... ..... .... .... .... .... .... ........ ..... ..... ..... ..... ..... ..... ...0
.
..
..
...V4 (x) ...

.
..
.
...
...
...
..
...
.
..
...
..
.
.
...
..
..
...
.
.
.
....
.... ... ......
... . ...
.... .... .... .... ..... ................................. ..... ..... ..... ..... ..... ....
..
...
V3 (x)
x
0
x
0
Figure 16: Examples

nez and A. Loria
50
Definition 2.12 Lyapunov function candidate.

A function V : IR+ IRn IR+ is said to be a Lyapunov function candidate
for the equilibrium x = 0 IRn of the equation x = f (t, x) if
1. V (t, x) is locally positive definite,
2.
V (t,x)
t
is continuous with respect to t and x,
3.
V (t,x)
x
is continuous with respect to t and x .
nez and A. Loria
51
Definition 2.13 Time derivative of a Lyapunov function candidate.

Let V (t, x) be a Lyapunov function candidate for the equation x = f (t, x).
The total time derivative of V (t, x) along the trajectories of (1), denoted
by V (t, x), is given by
T
V (t, x) V (t, x)
d
+
f (t, x).
V (t, x) := {V (t, x)} =
dt
t
x
nez and A. Loria
52
Definition 2.14 Lyapunov function.

A Lyapunov function candidate V (t, x) for Equation (1) is a Lyapunov
function for (1) if its total time derivative along the trajectories of (1)
satisfies
V (t, x) 0
t0
and for small kxk .
nez and A. Loria
53
Lyapunovs direct method

Theorem 2.2 Stability and uniform stability.
The origin is a stable equilibrium of Equation x = f (t, x), if there exists
V (t, 0) = 0 t 0
V (t, x) W1(x) > 0,
V (t, x) 0,
t0
t0
locally
for small kxk .
If moreover V (t, x) W2(x), t 0 (decrescent) for small kxk,

then the origin is uniformly stable.
W1(x) and W2(x) are positive definite functions.
nez and A. Loria
54
Theorem 2.3 (Uniform) boundedness of solutions plus uniform

stability.
The origin is a uniformly stable equilibrium of x = f (t, x) and the solutions
x(t) are uniformly bounded for all initial conditions (t, x(t)) IR+ IRn
if there exists
V (t, x) W1(x) > 0,
t 0 x IRn
and V (t, 0) = 0 t 0 (positive definite condition)

as
k x k (radially unbounded)
with W1(x)
V (t, x) W2(x)
V (t, x) 0
t0
t t 0
x IRn (decrescent)
x IRn .
W1(x) and W2(x) are positive definite functions.

nez and A. Loria
55
Example 2.11
Consider the dynamic model of an ideal pendulum without friction
J q + mgl sin(q) = 0
with
q(0), q(0)
IR
or, in the state-space form

x 1 = x2
mgl
sin(x1),
x 2 =
J
where x1 = q and x2 = q.
Autonomous nonlinear equation
nez and A. Loria
56
It has multiple equilibria,

at [q q]
T = [n 0]T for n = , 2, 1, 0, 1, 2,
the origin is an equilibrium
x2
......
.....
......
.............................
.............................
.............................
.
.
.
.
.
.
.
.
.
.... ...
.... ...
.
.
.
.. .... .........
... ....
... ....
..
..
.. . ... ........... .......... ... ... ........... ......... ... ... ........... ..........
....... .. .. ..... .... ..... ........ ... ... ..... .... .... .......... ... ... .... .... .....
.. . ..
.. . .
... .... ....
. . ... .... .... .. ... .
.... ..... ...... .... ... .
.... ..... ..... .. ... ....... ....... ........ ... ... ...
.
... ... ... .. .. .. ... .. ... .. .. .. ... ... ... .. .. ..
............ ........... .. .. ........... ........... .. .. ........... ........... .. ..
... ...
... ...
... ...
.
... .... ........
... .... ........
... .... ........
.... .............. ....
.... ............. ....
.... ................
....... ......
....... ......
....... ......
....
...
...
x1
Figure 17: Pendulum: Phase plane
nez and A. Loria
57
Lyapunov function candidate

x22
.
V (x1, x2) = mgl [1 cos(x1)] + J
2
which is locally positive definite
Total time derivative of V (x1, x2)
V (x1, x2) = mgl sin(x1)x 1 + Jx2x 2
= 0.
According with Theorem 2.2 the origin is a stable equilibrium.
nez and A. Loria
58
Theorem 2.4 Global (uniform) asymptotic stability

The origin of x = f (t, x) is globally asymptotically stable if there exists
V (t, x) W1(x) > 0,
t 0 x IRn
and V (t, 0) = 0 t 0 (positive definite condition)

with W1(x)
as
k x k (radially unbounded)
V (t, x) W3(x) < 0
t0
x IRn .
and V (t, 0) = 0 t 0 (negative definite condition)

If moreover V (t, x) W2(x)
t 0 x IRn (decrescent), the
origin is globally uniformly asymptotically stable.
W1(x), W2(x) and W3(x) are positive definite functions.
nez and A. Loria
59
Theorem 2.5 Global exponential stability.

The origin of x = f (t, x) is globally exponentially stable if there exists a
Lyapunov function candidate V (t, x) and positive constants , , and
p 1 such that:
p
kxk V (t, x) kxk ,

p
V (t, x) kxk
t t 0
x IRn .
If all the above conditions hold only for small kxk then we say that the
origin is an exponentially stable equilibrium.
nez and A. Loria
60
Theorem 2.6
Consider the differential equation
x = f (t, x),
x = f (x),
x IRn,
t IR+
x IRn.
The unicity of an existing equilibrium point is necessary for the following

properties (or, in other words, the following properties imply the unicity of
an equilibrium)
Global asymptotic stability,
Global exponential stability.
nez and A. Loria
61
Theorem 2.7 La Salle

Consider the autonomous differential equation x = f (x), whose origin
x = 0 IRn is an equilibrium. Assume that there exists
a globally positive definite and radially unbounded Lyapunov function
candidate V (x), such that
V (x) 0
x IRn.
o
n
Define the set = x IRn : V (x) = 0 . If
x(0) = 0 is the only initial state in , such that x(t) for all t 0,
then the origin x = 0 IRn is globally asymptotically stable.
nez and A. Loria
62
Corollary 2.1 Simplified La Salle.

Consider the set of autonomous differential equations
x = f x(x, z) ,
x IRn
z IRm .
= f z (x, z) ,
where f x(0, 0) = f z (0, 0) = 0. That is, the origin is an equilibrium

point. Let V : IRn IRm IR+ be globally positive definite and radially
unbounded in both arguments. Assume that there exists a globally positive
definite function W : IRm IR+ such that
V (x, z) = W (z) .
If x = 0 is the only solution of f z (x, 0) = 0 then the origin [xT z T ]T =
0 is globally asymptotically stable.
nez and A. Loria
63
Lemma 2.2 Consider the continuously differentiable functions x : IR+

IRn, z : IR+ IRm, h : IR+ IR+ and P : IR+ IR(n+m)(n+m). Assume
P (t) = P (t)T > 0 for each t IR+.
T

x
x
Define: V (t, x, z, h) = P (t) + h(t) 0 .
z
z
T
x
Q(t)
If V (t, x, z, h) =
0
z

x
0
0
0
z
for all t IR+, x IRn, z IRm and h IR+, where Q(t) = Q(t)T > 0
for all t 0, then,
nez and A. Loria
64
1. x(t), z(t) and h(t) are bounded for all t 0 and

2. x(t) is square-integrable, i.e.,
Z
kx(t)k dt < .
If moreover x is also bounded then we have that

lim x(t) = 0 .
nez and A. Loria
65
Ch. 3. Robot dynamics
We consider robot manipulators formed by an open kinematic chain.

q3
z0
z2
link 2
joint 2
link 1
joint 1
q1
joint n
z3
q2
z1
zn
link n
qn
x1
x.2
x= .
.
xm
y0
x0
Figure 18: Abstract diagram of an n-DOF robot manipulator.

nez and A. Loria
66
The generalized joint coordinate qi is the angular displacement around zi

(revolute) or linear displacement along zi (prismatic).
z3
z1
z0
z2
q3
z4
q2
q1
q4
"x #
x=
y0
1
x2
x3
x0
Figure 19: Example of a 3-DOF robot.

nez and A. Loria
67
The vector of joint positions q has n elements

q1
q2
n
q=
IR
.
..
qn
The position and orientation of the robots end-effector, are collected in the
vector x of operational positions .

x1
x2
x=
..
xm
where m n.
nez and A. Loria
68
The direct kinematic model describes the relation betwwen q and x. It is a

function : IRn IRm such that
x = (q).
The inverse kinematic model consists on the inverse relation of the direct
kinematic model
q = 1(x).
The computation may be highly complex.
nez and A. Loria
69
The dynamic model of a robot consists of an ordinary differential equation.

In general, these are second order nonlinear models
q , ) = 0 ,
f EL(q, q,
(4)
x, ) = 0 .
f C (x, x,
(5)
stands for the forces and torques applied.

The dynamic model (4) is called joint dynamic model,
while (5) corresponds to the operational dynamic model.
We focus on the joint dynamic model.
nez and A. Loria
70
Lagranges equations of motion

Now we describe how to derive the dynamic model of a robot.
Consider the robot manipulator with n links depicted in Figure 20.
q3
z0
z2
link 2
joint 2
link 1
joint 1
q1
joint n
z3
q2
z1
zn
link n
qn
x1
x.2
x= .
.
xm
y0
x0
Figure 20: Abstract diagram of an n-DOF robot manipulator.

nez and A. Loria
71
is defined as
The Lagrangian L(q, q)
= K (q, q)
U(q) .
L (q, q)
(6)
where
is the kinetic energy function
K (q, q)
U(q) is the potential energy function (we assume only conservative
forces)
The total energy E of a robot manipulator of n DOF is
= K (q, q)
+ U(q)
E (q, q)
where q = [q1, , qn]T .
nez and A. Loria
72
The equations of motion of Lagrange for a manipulator of n DOF, are given

by

d L(q, q)
L(q, q)
= ,
dt
q
q
or in the equivalent form by

d L(q, q)
L(q, q)
= i,
dt
qi
qi
i = 1, , n
(7)
where
i correspond to the external as well as to other non conservative forces
and torques at each joint.
nez and A. Loria
73
The use of Lagranges equations in the derivation of the robot dynamics

can be reduced to four main stages:
.
1. Computation of the kinetic energy function K(q, q)
2. Computation of the potential energy function U(q) .
.
3. Computation of the Lagrangian (6) L(q, q)
4. Development of Lagranges equations (7).
nez and A. Loria
74
Example 3.3
Consider the robot manipulator with 2 DOF shown in Figure 21.

z
1
x
l1
q1
I1
m1
lc1
q2
lc2
m2
I2
Figure 21: 2-DOF robot.

nez and A. Loria
75
where,
l1 is the length the first link
m1 and m2 are the masses of the links
lc1 and lc2 are the distances of the rotating axes to the centers of their
respective mass
I1 and I2 denote the moments of inertia of the links
q = [q1 q2]T defines the vector of joint positions
Notice that the center of mass of link 2 may be physically placed out of
the link itself! This is determined by the value of the constant angle
nez and A. Loria
76
may be decomposed by
The kinetic energy function K(q, q)
+ K2(q, q),
= K1(q, q)
K(q, q)
where
is the kinetic energy associated to the mass m1
K1(q, q)
K2(q, q)
with
2 2
= 12 m1lc1
K1(q, q)
q1 + 12 I1q12
=
K2(q, q)
nez and A. Loria
m2 2 2
m2 2 2
l
q
+
2 1 1
2 lc2 q2
+ 12 I2q22 .
+ m2l1lc2 cos(q1 q2 + )q1q2
77
= K(q, q)
U(q) , takes the form
The Lagrangian L(q, q)
+ K2(q, q)
= K1(q, q)
L(q, q)
1
1
2
2 2
2 2
=
(m1lc1 + m2l1 )q1 + m2lc2
q2
2
2
+ m2l1lc2 cos(q1 q2 + )q1q2
1 2 1 2
+ I1q1 + I2q2 .
2
2
nez and A. Loria
78
By using Lagranges equations

we have
1 =
d
dt
2
m1lc1
)
L(q ,q
qi
m2l12
)
L(q ,q
qi
= i,
i = 1, , n
+ I1 q1
+ m2l1lc2 cos(q1 q2 + )
q2
+ m2l1lc2 sin(q1 q2 + )q22
and,
q1
2 = m2l1lc2 cos(q1 q2 + )
2
+ [m2lc2
+ I2]
q2
m2l1lc2 sin(q1 q2 + )q12 .

nez and A. Loria
79
Denoting C21 = cos(q2 q1), S21 = sin(q2 q1), one obtains

2
2
1 = m1lc1 + m2l1 + I1 q1
+ [m2l1lc2 cos()C21 + m2l1lc2 sin()S21]
q2
+ [m2l1lc2 cos()S21 + m2l1lc2 sin()C21]q22
(8)
and
q1
2 = [m2l1lc2 cos()C21 + m2l1lc2 sin()S21]
2
+ [m2lc2
+ I2]
q2
+ [m2l1lc2 cos()S21 m2l1lc2 sin()C21]q12

(9)
nez and A. Loria
80
which are of the form (4) with

q , ) =
f EL(q, q,
1 RHS(8)
2 RHS(9)
where RHS(8) and RHS(9) denote the terms on the right hand side of (8)
and (9) respectively.
nez and A. Loria
81
Example 3.4
Consider the 3-DOF Cartesian robot manipulator shown in Figure 22.
z0
z0
q2
m3
m2
m1
q3
q1
q1
y0
x0
x0
q2
y0
q3

=
K(q, q)
1
2
m1q32
+ [m1 +
m2]q22
+ [m1 + m2 +
m3]q12
U(q) = [m1 + m2 + m3]gq1 .

nez and A. Loria
82
We obtain the Lagrangian as:
U(q)
L(q, q)
= K(q, q)

1
2
2
2
m1q3 + [m1 + m2]q2 + [m1 + m2 + m3]q1
=
2
[m1 + m2 + m3]gq1 ,
hence, the dynamic equations result

q1 + [m1 + m2 + m3]g = 1
[m1 + m2 + m3]
[m1 + m2]
q2 = 2
m1q3 = 3
nez and A. Loria
83
In terms of the state vector [q1 q2 q3 q1 q2 q3] , above equations

may be expressed as
q1
q1
q2
q2
3
q3
=
.
1
dt
[
[m
+
m
+
m
]g]
1
1
2
3
q
1 m1 + m2 + m3
q2

m
+
m
1
2

1
q3
3
m1
nez and A. Loria
84
The necessary and sufficient condition for the existence of equilibria is

1 = [m1 + m2 + m3]g,
2 = 0 and
3 = 0
Actually we have an infinite number of them:
T
[q1 q2 q3 q1 q2 q3] = [q1 q2 q3 0 0 0]
with q1, q2, q3 IR.

nez and A. Loria
85
Dynamic model in compact form

Consider a robot manipulator of n DOF composed of rigid links
interconnected by frictionless joints.
= 12 q TM (q)q
K(q, q)
expresses the kinetic energy
where
? M (q) = M (q)T > 0 for all q IRn is the n n inertia matrix.
U(q) denotes the potential energy
The Lagrangian results
1
= q TM (q)q U(q) .
L(q, q)
2
nez and A. Loria
86
The Lagranges equations of motion

L(q, q)
d L(q, q)
= ,
dt
q
q
may be written as

d 1 T
q M (q)q
dt q 2

1 T
U(q)
=.
q M (q)q +
q 2
q
which takes the compact form,

q + g(q) =
M (q)
q + C(q, q)
nez and A. Loria
87
where

1 T
q = M (q)q
C(q, q)
q M (q)q
2 q
U(q)
.
g(q) =
q
q is a vector of dimension n called vector of centrifugal and
C(q, q)
Coriolis forces,
g(q) is a vector of dimension n of gravitational forces or torques and
is a vector of dimension n called the vector of external forces
nez and A. Loria
88
IRnn may be not unique, but C(q, q)

q is indeed unique.
C(q, q)
is through the Christoffel symbols
One way to obtain C(q, q)
cijk (q) =
1 Mkj (q) Mki(q) Mij (q)

+
.
2
qi
qj
qk
is given by
Ckj (q, q),
The kjth element of the matrix C(q, q),
c1jk (q)
c2jk (q)
q .
=
Ckj (q, q)
..
cnjk (q)
nez and A. Loria
89
q + g(q) = may be viewed as

The model M (q)
q + C(q, q)
q
ROBOT
q
Figure 23: Input-output representation of a robot.
nez and A. Loria
90
Example 3.6
z
1
x
l1
q1
I1
m1
lc1
q2
lc2
m2
I2
nez and A. Loria
91
The dynamic model, for this robot, in compact form is

C11(q, q)
M11(q) M12(q)
C12(q, q)
q +
q
C22(q, q)
M21(q) M22(q)
C21(q, q)
{z
}
{z
}
|
|
M (q)
C(q, q)
= (t)
where
M11(q) =
2
m1lc1
m2l12
+ I1
M12(q) = [m2l1lc2 cos()C21 + m2l1lc2 sin()S21]

M21(q) = [m2l1lc2 cos()C21 + m2l1lc2 sin()S21]
2
+ I2 ]
M22(q) = [m2lc2
nez and A. Loria
92
C11(q, q)
= 0
= [m2l1lc2 cos()S21 + m2l1lc2 sin()C21]q2

C12(q, q)
C21(q, q)
= [m2l1lc2 cos()S21 m2l1lc2 sin()C21]q1
C22(q, q)
= 0
That is, the gravitational forces vector is zero.
nez and A. Loria
93
q + g(q) = in terms of the state

The dynamic model M (q)
q + C(q, q)
vector [q T q T ]T can be expressed as

q
q
d
.
=
dt
q g(q)]
q
M (q)1 [ (t) C(q, q)
(10)
The necessary and sufficient condition for the existence of equilibria is

? (t) be constant (say, )
? and there exist a solution q IRn to g(q ) = .
The equilibria are given by [q T q T ]T = [q T 0T ]T IR2n.
nez and A. Loria
94
When 0, the possible equilibria of (10) are given by [q T q T ]T =

[q T 0T ]T
where
? q is a solution of g(q ) = 0
U (q )
? As g(q) = q , we have that q corresponds to the vectors where
the potential energy possesses extrema.
nez and A. Loria
95
Dynamic model of robots with actuators

q2
v2
v1
q1

On a real robot manipulator the torques vector , is delivered by actuators
electromechanical,
pneumatic or
hydraulic.
nez and A. Loria
96
They have their own dynamics,
v2
q2
q1
v1

nez and A. Loria
97
Actuators with linear dynamics

Ka , Kb , Ra
Jm
v
fm
1:r
Figure 27: Diagram of a DC motor.

Simplified linear dynamic model of a DC motor
Jmq + fmq +
KaKb
Ka
q + 2 =
v
Ra
r
rRa
where:
nez and A. Loria
98
Jm : rotors inertia [kg m2],

Ka : motor-torque constant [N m/A],
Ra : armature resistance [],
Kb : back emf [V s/rad],
fm : rotors friction coefficient with respect to its hinges [N m],
: net torque applied after the set of gears at the loads axis [N m],
q : angular position of the loads axis [rad],
r : gear reduction ratio (in general r 1),
v : armature voltage [V].
nez and A. Loria
99
We can obtain
+ B q + R = Kv
Jq
with
J = diag {Jmi }

KaKb
B = diag fmi +
Ra i

1
R = diag 2
ri

Ka 1
K = diag
Ra i ri
nez and A. Loria
100
The complete dynamic model of a manipulator (considering friction in the

joints) including its actuators is
+ R C(q, q)
q + R g(q) + R f (q)
+ B q = Kv . (11)
(R M (q) + J) q
ACTUATORS
ROBOT
q
Figure 28: Block-diagram of a robot with its actuators.
nez and A. Loria
101
Example 3.8
Consider the pendulum depicted in Figure 29.
fL
lb
mb , J
Jm
v
fm
Ka , Kb , Ra
1:r
m
q
Figure 29: Pendular device with a DC motor.

The equation of motion for this device including its load is given by
J q + fLq + [mblb + ml] g sin(q) =
nez and A. Loria
102
where
J : arms inertia without load (i.e., with m=0),
mb : arms mass (without load),
lb : distance from the rotating axis to the arms center of mass (without
load),
m : loads mass (assumed to be punctual),
l : distance from the rotating axis to the load m,
g : gravity acceleration,
: applied torque at the rotating axis,
fL : friction coefficient of the arm and its load.
nez and A. Loria
103
The equation above may also be written in the compact form

JLq + fLq + kL sin(q) =
where
JL = J + ml2
kL = [mblb + ml]g .
nez and A. Loria
104
The complete dynamic model of the pendular device may be obtained as:

Jm +
JL
fL KaKb
Ka
kL
+
+
sin(q)
=
v,
q
+
f
q
+
m
2
2
2
r
r
Ra
r
rRa
where, we identify
M (q) = JL
1
R =
r2
KaKb
B = fm +
Ra
C(q, q)
= 0
nez and A. Loria
= Jm
Ka
K =
rRa
f (q)
= fLq
g(q) = kL sin(q) .
105
When the gear ratios ri are sufficiently large, the robot-with-actuators

equation (11) may be approximated by
+ B q Kv .
Jq
In the opposite case, the equation (11) may be rewritten as

q + g(q) + f (q)
+ R1B q = R1Kv
M 0(q)
q + C(q, q)
where M 0(q) = M (q) + R1J .
nez and A. Loria
106
we have
In terms of the state vector [q q]

q
q
d
.
=

dt
q g(q) f (q)
q
M 0(q)1 R1 Kv R1 B q C(q, q)
The problem of motion control is:
given q d, q d and q d,
determine v, to be applied to the motors,
in such a manner that q follow precisely q d .
nez and A. Loria
107
Ch. 4. Properties of the dynamic model

Dynamic model for n-DOF robots
q + g(q) = .
M (q)
q + C(q, q)
These properties, may be classified as follows.
Properties of the inertia matrix M (q),
Properties of the centrifugal and Coriolis forces matrix C(q, q),

Properties of the gravitational forces and torques vector g(q),
Residual dynamics.
nez and A. Loria
108
Properties of the inertia matrix

Property 4.1 Inertia matrix M (q)
The inertia matrix M (q) = M (q)T > 0 IRnn, satisfies the following
properties:
There exists a real positive number such that

M (q) I
1.
q IRn
where I denotes the identity matrix of dimension n n. The

matrix M (q)1 exists and is positive definite.
nez and A. Loria
109
For robots having only revolute joints there exists a constant > 0
such that
Max{M (q)} q IRn .
One way of computing is
2.

n max |Mij (q)|
i,j,q
where Mij (q) stands for the ijth element of the matrix M (q).
nez and A. Loria
110
For robots having only revolute joints there exists a constant

kM > 0 such that
3.
kM (x)z M (y)zk kM kx yk kzk

for all vectors x, y, z IRn. One simple way to determine kM is
as follows.

Mij (q)
2

.
kM n
max

i,j,k,q
qk
nez and A. Loria
111
For robots having only revolute joints there exists a number

0
kM
> 0 such that
4.
0
kyk
kM (x)yk kM
for all x, y IRn.
nez and A. Loria
112
The centrifugal and Coriolis forces matrix
Property 4.2 Coriolis matrix C(q, q)

IRnn satisfies the
The centrifugal and Coriolis forces matrix C(q, q)
following.
1.
may be not unique

For a given manipulator, the matrix C(q, q)
q is unique.
but the vector C(q, q)
2.
C(q, 0) = 0 for all vectors q IRn.
nez and A. Loria
113
For all vectors q, x, y, z IRn and scalars we have that

3.
C(q, x)y = C(q, y)x

C(q, z + x)y = C(q, z)y + C(q, x)y .
nez and A. Loria
114
The vector C(q, x)y may be written in the form
xTC1(q)y
T
C(q, x)y =
x C2(q)y
..
xTCn(q)y
4.
where
for all
matrix
ofh the
1
2
Ck (q) are symmetric matrices of dimension n n

k = 1, 2, , n. The ij-th element Ckij (q) of the
Ck (q) corresponds to the so-called Christoffel symbol
first kind cjik (q) andi which is defined as cijk (q) =
Mkj (q )
Mij (q )
Mki (q )
.
+
qi
qj
q
nez and A. Loria
115
For robots having exclusively revolute joints, there exists a number

kC1 > 0 such that
5.
kC(q, x)yk kC1 kxk kyk

for all q, x, y IRn.
nez and A. Loria
116
For robots having exclusively revolute joints, there exist numbers

kC1 > 0 and kC2 > 0 such that
6.
kC(x, z)w C(y, v)wk kC1 kz vk kwk

+kC2 kx yk kwk kzk
for all vector v, x, y, z, w IRn.
nez and A. Loria
117
defined using the Christoffel symbol is related to

The matrix C(q, q),
the inertia matrix M (q) by the expression
xT
7.

1
x=0
M (q) C(q, q)
2
x IRn
q, q,
is skewsymmetric. Equivalently,
and as a matter of fact, 12 M (q)C(q, q)
is antisymmetric, and it is also true that
the matrix M (q) 2C(q, q)
+ C(q, q)
T.
M (q) = C(q, q)
nez and A. Loria
118
is derived, it always
Independently of the way in which C(q, q)
satisfies
q
1
q = 0
M (q) C(q, q)
2
nez and A. Loria
q, q IRn .
119
The vector of gravitational torques

Property 4.3 Gravity vector g(q)
The gravitational torques vector g(q) IRn is continuous and therefore
bounded for each bounded q and also satisfies the following.
The vector g(q) and the velocity vector q are correlated as

1.
g(q(t))Tq(t)
dt = U(q(T )) U(q(0))
for all T IR+ .
nez and A. Loria
120
2.
For robots having only revolute joints there exists a number kU

such that
Z T
g(q(t))Tq(t)
dt + U(q(0)) kU
0
for all T IR+ and where kU = min {U(q)}.

q
nez and A. Loria
121
For robots having only revolute joints, the vector g(q) is Lipschitz, that
is, there exists a constant kg > 0 such that
kg(x) g(y)k kg kx yk
3.
for all x, y IRn. A simple way to compute kg is by evaluating its partial
derivative

gi(q)
.
kg n max
i,j,q
qj
nez and A. Loria
122
Furthermore, kg satisfies

g(q)
g(q)

.
kg
q Max
q
nez and A. Loria
123
4.
For robots having only revolute joints there exists a constant k0

such that
kg(q)k k0
for all q IRn.
nez and A. Loria
124
Residual dynamics
The residual dynamics h(

q , q ) is defined as follows.
h(
q , q ) = [M (q d) M (q d q )] q d
h
i
+ C(q d, q d) C(q d q , q d q ) q d
+ g(q d) g(q d q ),
and with an abuse of notation it may be written as
q d + g(q d) g(q).
h(
q , q ) = [M (q d) M (q)] q d + [C(q d, q d) C(q, q)]
nez and A. Loria
125
Definition 4.1 Vectorial tangent hyperbolic function
tanh(x1)
..
tanh(x) :=
tanh(xn)
(12)
tanh(x)
1.0
0.5
0.0
0.5
1.0
.
..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ....... ..... ..... ..... ........................................................................
.
....
.. ..........
.. ....
.. ......
.
..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ................ ..... ..... ..... ..... ..... ..... ..... .... .... .... ..
... .
.. .
... ...
.
.
.
... .
.....
.
.
.
.
.
.
.
.
.
.
.................................................................. ..... ..... ..... ....... ..... ..... ..... ..... ..... ..... ..... ..... .... .... .... ..
.
4 3 2 1
3 x 4
Figure 30: Tangent hyperbolic tanh(x).

nez and A. Loria
126
The first partial derivative of tanh(x) is given by

tanh
(x) =: Sech2(x) = diag{sech2(xi)}
x
where
sech(xi) :=
nez and A. Loria
1
.
x
x
i
i
e e
127
The vectorial tangent hyperbolic function satisfies the following properties.

For any x, x IRn
ktanh(x)k 1 kxk
ktanh(x)k 2
2
ktanh(x)k 3 tanh(x)T x

2

Sech (x)x 4 kxk

where 1,
, 4 > 0. With tanh(x) defined as in (12), the constants
1 = 1, 2 = n, 3 = 1, 4 = 1.
nez and A. Loria
128
Property 4.4 Vector residual dynamics h(

q , q )
The vector of residual dynamics h(
q , q ) of n 1 depends on
, q
,
q
q d, q d, and q d (supposed to be bounded)
q dkM the supreme values over the norms
We denote by kq dkM and k
of the desired velocity and acceleration.
In addition, h(
q , q ) has the following property:
nez and A. Loria
129
There exist constants kh1, kh2 0 such that the norm of the
residual dynamics satisfies
1.

q , q ) kh1kq k + kh2 ktanh(
q )k
h(
IRn, where tanh(
q ) is the vectorial tangent
for all q , q
hyperbolic function introduced in the latter Definition.
nez and A. Loria
130
where
s2
,
kh2
s2
tanh
s1
kC1 kq dkM kh1 ,

with
q d kM +
s1 = kg + kM k
and
2
kC2 kq dkM
0
s2 = 2 k0 + kM
k
q d kM +
nez and A. Loria
kC1 kq dk2M
.
131
Ch. 5. Case study: The Pelican prototype robot
Figure 31: Pelican: experimental prototype from CICESE, Robotics lab.

The prototype is a
vertical planar manipulator
two links connected to revolute joints.
nez and A. Loria
132
The links are direct driven by two brushless DC motors.

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2
Figure 32: Diagram of the Pelican prototype robot with 2 degrees of

freedom.
nez and A. Loria
133
Table 1: Physical parameters of Pelican prototype manipulator.

Description
Notation
Value
Units
Length of Link 1
l1
0.26
Length of Link 2
l2
0.26
Distance to the center of mass (Link 1)
lc1
0.0983
Distance to the center of mass (Link 2)
lc2
0.0229
Mass of Link 1
m1
6.5225
kg
Mass of Link 2
m2
2.0458
kg
Inertia rel. to center of mass (Link 1)
I1
0.1213
kg m2
Inertia rel. to center of mass (Link 2)
I2
0.0116
kg m2
Gravity acceleration
9.81
m/sec2
nez and A. Loria
134
The chapter is organized as follows:

Direct kinematics.
Inverse kinematics.
Dynamic model.
Model properties of the dynamic model.
Reference trajectories.
nez and A. Loria
135
Direct kinematics
For this case, the problem consists on expressing

x
= (q1, q2) ,
y
where : IR2 IR2.
The direct kinematic model is given by
x = l1sin(q1) + l2sin(q1 + q2)
y
nez and A. Loria
= l1cos(q1) l2cos(q1 + q2) .
136
It may also be obtained the relation between the velocities,

q1
x
l1cos(q1) + l2cos(q1 + q2) l2cos(q1 + q2)
=
l1sin(q1) + l2sin(q1 + q2) l2sin(q1 + q2)
q2
y

q
= J(q) 1
q2
where
(q)
IR22 is called Jacobian matrix or, Jacobian of the
J(q) =
q
robot,
nez and A. Loria
137
and the Relation of accelerations

d
q1
q1
x
=
J(q)
+ J(q)
,
y
q2
q2
dt
where we have defined
Pn (q)
d
J(q) :=
qi.
i =1 q
dt
i
nez and A. Loria
138
Inverse kinematics
It has the form
q1
= 1(x, y), where 1 : IR2 and IR2.
q2
y
qd
1
qd
2
Figure 33: Two solutions to the inverse kinematics problem.

It may have multiple solutions or no solution at all!
nez and A. Loria
139
qd1
x
Figure 34: No solution to the inverse kinematics problem.

y
qd2
x
qd1
Figure 35: No solution to the inverse kinematics problem.

nez and A. Loria
140
A practical interest relies on its utility to

T
=
[q
q
]
from specified desired positions xd and yd.
define
q
d
d
1
2

d
q d1
= 1(x, y).
q d2

xd
l2sin(qd2 )
tan1
qd1 = tan1
yd
l1 + l2cos(qd2 )
2

2
2
2
+
y
l
x
1
2
d
d
qd2 = cos1
.
2l1l2
The desired joint velocities may be obtained as

x
qd1
= J 1(q d) d
qd2
y d
nez and A. Loria
141
and the desired joint accelerations as

qd1
qd2
where
= J
|
d
x
d
x d
1
1
J(q d) J (q d)
(q d)
+ J (q d)
yd
yd
dt
{z
}

d 1
J (q d)
dt
S12
C12
l
S
l
S
1
2
1
2
1
J (q d) =
l1S1 l2S12 l1C1 + l2C12 ,
l1l2S2
l1l2S2
and
l1S1qd1 l2S12(qd1 + qd2 ) l2S12(qd1 + qd2 )

d
,
[J(q d)] =
dt
l1C1qd1 + l2C12(qd1 + qd2 )
l2C12(qd1 + qd2 )
nez and A. Loria
142
where for simplicity

S1 = sin(qd1 ), S2 = sin(qd2 ), C1 = cos(qd1 ),
S12 = sin(qd1 + qd2 ), C12 = cos(qd1 + qd2 ) .
Typically, singular configurations are those in which the end-effector of
the robot is located at the physical boundary of the workspace
y
qd2
x
qd1
Figure 36: Bentover singular configuration.
nez and A. Loria
143
Dynamic model
Lagrangian equations
We start by writing the kinetic energy:
= K1(q, q)
+ K2(q, q)
K(q, q)
where
K1(q, q)
is associated to the mass m2
K2(q, q)
nez and A. Loria
144
with
K1(q, q)
=
=
1
1 2
T
m1v 1 v 1 + I1q1
2
2
1
1 2
2 2
m1lc1q1 + I1q1 .
2
2
and
=
K2(q, q)
=
1
1
T
m2v 2 v 2 + I2(q1 + q2)2
2
2

m2 2 2 m2 2 2
2
l q +
l q + 2q1q2 + q2
2 1 1
2 c2 1

2
+ m2l1lc2 q1 + q1q2 cos(q2)
1
+ I2(q1 + q2)2.
2
nez and A. Loria
145
Similarly the potential energy is given by

U(q) = U1(q) + U2(q)
where, assuming that the potential energy is zero at y = 0, we obtain that
U1(q) = m1lc1g cos(q1)
U2(q) = m2l1g cos(q1) m2lc2g cos(q1 + q2) .
nez and A. Loria
146
The Lagrangian is
U(q)
L(q, q)
= K(q, q)
2

1
1
2
2 2
2
2
[m1lc1 + m2l1 ]q1 + m2lc2 q1 + 2q1q2 + q2
=
2
2

2
+ m2l1lc2 cos(q2) q1 + q1q2
+ [m1lc1 + m2l1]g cos(q1)
+ m2glc2 cos(q1 + q2)
1 2 1
+ I1q1 + I2[q1 + q2]2.
2
2
Applying the Lagranges equations

that
nez and A. Loria
d
dt
L
qi
L
q
= i
i
i = 1, 2 we get
147
1 =
2
m1lc1
m2l12
2
m2lc2
2
+ m2lc2
+ m2l1lc2
+ 2m2l1lc2 cos(q2) + I1 + I2 q1

cos(q2) + I2 q2
2m2l1lc2 sin(q2)q1q2 m2l1lc2 sin(q2)q22

+ [m1lc1 + m2l1]g sin(q1)
+ m2glc2 sin(q1 + q2)
and
2 =
2
m2lc2
2
+ m2l1lc2 cos(q2) + I2 q1 + [m2lc2
+ I2]
q2
+ m2l1lc2 sin(q2)q12 + m2glc2 sin(q1 + q2)
nez and A. Loria
148
Model in compact form

C12(q, q)
C11(q, q)
g (q)
M11(q) M12(q)
q +
q + 1
= ,
M21(q) M22(q)
C21(q, q)
g2(q)
C22(q, q)
{z
}
{z
}
|
|
| {z }
M (q)
C(q, q)
g(q)
nez and A. Loria
149
where
M11(q) =
2
m1lc1
m2 l12
2
M12(q) = m2 lc2
+ l1lc2
2
M21(q) = m2 lc2 + l1lc2
2
lc2
+ 2l1lc2 cos(q2) + I1 + I2

cos(q2) + I2

cos(q2) + I2
2
M22(q) = m2lc2
+ I2
C11(q, q)
= m2l1lc2 sin(q2)q2
C12(q, q)
= m2l1lc2 sin(q2) [q1 + q2]
= m2l1lc2 sin(q2)q1
C21(q, q)
C22(q, q)
= 0
g1(q) = [m1lc1 + m2l1] g sin(q1) + m2lc2g sin(q1 + q2)
g2(q) = m2lc2g sin(q1 + q2) .
nez and A. Loria
150
In terms of these state variables, the dynamic model of the robot may be
written as
q1
q1
q2 =
q2
dt
1
1
q g(q)]
M
(q)
[ (t) C(q, q)
q2
The formulas and numeric values of the constants Max{M }, kM , kC1 , kC2
and kg are summarized in Tables below
nez and A. Loria
151
Table 2: Parameters for the Pelican prototype.

Max {M }

max Mij (q)
i,j,q
kM

Mij (q)

max
i,j,k,q
qk
k C1
k C2
kg
nez and A. Loria
n2

max Ckij (q)
i,j,q

!
Ck (q)
ij

max

i,j,k,l,q
ql
n3

!

gi (q)
max

i,j,q qj
152
Table 3: Numeric values of the parameters for the CICESE prototype.
nez and A. Loria
Parameter
Value
Units
Max {M }
0.7193
kg m2
kM
0.0974
kg m2
k C1
0.0487
kg m2
k C2
0.0974
kg m2
kg
23.94
kg m2 /sec2
153
Desired reference trajectories
We have selected the following reference trajectories in joint space:
qd1
qd2
2.0 t3
b1[1 e
b2[1 e2.0
2.0 t3
] + c1[1 e
] + c2[1 e2.0
] sin(1t)
] sin(2t)
[rad]
(13)
where
b1 = /4 [rad], c1 = /9 [rad] and 1 = 4 [rad/sec], and
b2 = /3 [rad], c2 = /6 [rad] and 2 = 3 [rad/sec].
nez and A. Loria
154
The figure depicts the graphs of above reference trajectories against time.
2.0
[rad]
q
1.5
1.0
0.5
0.0
......
......
......
......d2
.. .....
.. ....
.. ....
... ....
.
.
.
qd1....... ...... ..... ... ........
.. .... ..... ....... ..... ....
.
.
.
.
.
.
.
.
.
.
... . .... .... ... . . .... . .. ... .... .. ..... .....
... ..... ... ... ..... ....... ....... ... .......... ...... ..... ..... ... ... ..... .......
.. ..... .. ... .. ... ..... .. ............... .... ............. ..... ... .......
......
......
.................... ........ ..... ........
.....
......
..
............ ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
10
t [sec]
Figure 37: Desired reference trajectories.
nez and A. Loria
155
||qd (t)|| [rad]

2.0
1.5
1.0
0.5
0.0
....
.........
.. .....
.
.
.
.......
.. ...
...........
.. ...
.... ...
... .....
... .....
... .....
.... .....
......... ..
.........
... ..
... ..
..
... .
... ..
... ..
....
....... ...
... ..
... ..
. . .
... .
.....
.
... .....
....
..
..
...
.
.....
10
t [sec]
Figure 38: Norm of the desired reference positions.
nez and A. Loria
156
||q d(t)|| [ rad

sec ]
.
2.4
.
....
1.8
1.2
0.6
0.0
..
.. ...
.....
.....
.....
... .....
....... ... ..... .......
....... ... ..... .......
....... ... .....
.. ... .. .... .. ... .. ... .. .... .. ... .. ... .. ....
.. ....
... ..... ........... ..... ... .... ... ................ ..... ... .... ... ............... ..... ... ....
... .. ... ..
... .. ... ..
... .. ...
.. ... ...
... . ... .
... .. ... ..
... .. ...
.
... ..... ...
.
... . ... .
... . ... .
... . ... .
.
.
.. ... ..
.
.
.
.
... . .. .
... . .. .
... .. ... ..
.. .....
..... .....
..... .....
..... .....
. ..
. ..
. ..
... ......
..
..
10
t [sec]
Figure 39: Norm of the desired reference velocities vector.

2 2.0 t3
qd1 = 6b1t e
2 2.0 t3
qd2 = 6b2t e
2 2.0 t3
+ 6c1t e
2 2.0 t3
+ 6c2t e
sin(1t) + [c1 c1e
2.0 t3
sin(2t) + [c2 c2e
in [ rad/sec ].
nez and A. Loria
2.0 t3
] cos (1t)1,
] cos (2t)2 .
(14)
157
||
qd (t)|| [ rad
sec ]
10
.....
8
6
4
2
0
.....
... .....
.
.... ....
.... ....
.... ....
. .. ..
....... ... ..... .... ... ..... ... ..... .... .... ... ..... ... ..... .... .... ... ..... ... ..
.. .... .. .... .. .. ... .. ... .. ... .. ... .. ... .. ... .. ... .. ... .. ... .. ... ..
... ........ ... .... ..... ... ..... ... ..... ... ... ..... ... ..... ... ..... ... ... ..... ... ..... ...
..... ... ..
..... ... .. .....
..... ... .. .....
.. .....
.. ... ..
.. ... .. ..
.. ... .. ..
... .......
... .
... .
... .
..
.
.
.....
....
....
..
.....
.....
.....
..
.
..
..
10
t [sec]
Figure 40: Norm of the desired reference accelerations vector.
nez and A. Loria
158
Reference accelerations:
2.0 t3
qd1 = 12b1te
4 2.0 t3
36b1t e
4 2.0 t3
36c1t e
qd2 = 12b2te
4 2.0 t3
4 2.0 t3
[c2 c2e2.0
sen(1t)
2 2.0 t3
36b2t e
36c2t e
+ 12c1te
sen(1t) + 12c1t e
cos (1t)1

t3
2
2
]sen(1t)1
rad / sec ,
[c1 c1e2.0
2.0 t3
2.0 t3
2.0 t3
+ 12c2te
sin (2t)
2 2.0 t3
sin (2t) + 12c2t e

cos (2t)2

t3
2
2
] sin (2t)2
rad / sec .
(15)
nez and A. Loria
159
From above figures we deduce the following upperbounds on the norms:
kq dkMax 1.92 [rad]

kq dkMax 2.33 [rad/sec]
2
k
q dkMax 9.52 [rad/sec] .
nez and A. Loria
160
Part II
SET-POINT CONTROL
nez and A. Loria
161
Introduction
Introduction
Motion controllers are clasified
Set-point controllers (q d(t) = q d, is a constant vector),
and tracking controllers (q d(t) is a time variant vector).
q +
Consider the dynamic model of a robot manipulator M (q)
q + C(q, q)
g(q) = , which can be written as:

q
q
d
=
dt
q g(q)]
q
M (q)1 [ (t) C(q, q)
nez and A. Loria
162
Introduction
where
M (q) IRnn is the inertia matrix,
q IRn is the vector of centrifugal and Coriolis forces,
C(q, q)
g(q) IRn is el vector of gravitational forces and torques and
IRn is a vector of external forces and torques applied at the joints.
q IRn denote the position, velocity and joint
The vectors q, q,
acceleration respectively.
nez and A. Loria
163
Introduction
The objective of set-point control consists on finding such that

lim q(t) = q d
where q d IRn is a given constant vector which represents the desired

set-point.
It is convenient to rewrite the set point control objective as
lim q (t) = 0
where q IRn stands for the joint position errors

q (t) := q d q(t) .
nez and A. Loria
164
Introduction
In general, a control law may be expressed as

q , q d, M (q), C(q, q),
g(q)) .
= (q, q,
However, for practical purposes, it is desirable that the controller does not
.
depend on the joint acceleration q
qd
q d
q d
CONTROLLER
ROBOT
q
q
Figure 41: Set-point control: closed-loop system.

nez and A. Loria
165
Introduction
A methodology to analyze the stability may be summarized in:

1. Derivation of the closed loop dynamic equation.
2. Representation of the closed loop equation in the state-space form, i.e.,

d qd q
g(q)) .
q d, M (q), C(q, q),
= f (q, q,
q
dt
qd
Figure 42:
Set-point
representation.
nez and A. Loria
CONTROLLER
+
ROBOT
control
closed-loop
q
q
system:
Input-output
166
Introduction
3. Study of the existence and possible unicity of the equilibrium for the
closed loop equation.
4. Proposition of a Lyapunov function candidate to study the stability
5. Alternatively to step 4), determine the qualitative behavior of the
solutions of the closed loop equation.
nez and A. Loria
167
Introduction
The controllers that we present may be called conventional (commonly

used in industrial robots).
Velocity feedback Proportional control and Proportional Derivative (PD)
control
PD control with gravity compensation
PD control with precalculated gravity compensation
Proportional Integral Derivative (PID) control
nez and A. Loria
168
Ch. 6. Velocity feedback proportional control and PD control
Ch. 6. Velocity feedback proportional control and PD

control
qd
Kp
ROBOT
q
q
Kv
Figure 43: Velocity feedback proportional control.

where
Control law given by = Kpq Kv q,
Kp, Kv IRnn are symmetric positive definite matrices
q d IRn corresponds to the desired joint position,
= q d q IRn is called position error.
q
nez and A. Loria
169
Kv
q d
qd
ROBOT
q
q
Kp
Figure 44: PD Control.

, where
The PD control law is given by: = Kpq + Kv q
Kp, Kv IRnn are also symmetric positive definite
When q d, is constant, then, q = q (both control laws are identical).
nez and A. Loria
170
We assume a constant q d, then, the closed loop equation may be rewritten

q
q
d
.
=
dt
q g(q)]
q
M (q)1 [Kpq Kv q C(q, q)
It may have multiple equilibria given by
T T T
= [sT 0T ]T where s IRn is solution of
? q q
Kps g(q d s) = 0 .
nez and A. Loria
171
Robots without gravity term

Dynamic model given by
q = .
M (q)
q + C(q, q)
Assuming that q d is constant, the closed loop equation becomes

q
q
d
=
dt
q]
q
M (q d q )1 [Kpq Kv q C(q d q , q)
It represents an autonomous differential equation (since q d is constant).
The origin q

T T
nez and A. Loria
= 0 is the only equilibrium of this equation.

172
To study the stability of the equilibrium, consider

V (
q , q)
=
T
q
K
1 p
2
0
q
0
M (q d q )

q

q
1 T
1 T
Kpq .
q M (q)q + q
2
2
is positive definite since M (q) > 0 and Kp > 0.

? V (
q , q)
nez and A. Loria
173
results
The total derivative of V (
q , q)
V (
q , q)
= q TKv q

T
0 0
q
q
0,
=
0 Kv
q
q
By Theorem 2.3 we conclude that the origin is stable and,
the solutions q (t) and q(t)

are bounded.
By applying the La Salles theorem (Theorem 2.7) we have that
nez and A. Loria
174
the set is given by

n
o
2n
=
x IR : V (x) = 0

q
=0
q , q)
=
x=
IR2n : V (
q
= {
q IRn, q = 0 IRn} ,

T T
where q (0) q(0)

= 0 IR2n is the only initial condition in
for which x(t) for all t 0.
This is enough to establish global asymptotic stability of the origin,
T T T
= 0 IR2n.
q q
T
nez and A. Loria
175
Robots with gravity term

The closed loop equation is

q
q
d
.
=
dt
q g(q)]
q
M (q)1 [Kpq Kv q C(q, q)
(16)
The study of this section is limited to robots having only revolute joints.
It may have multiple equilibria given by
T T T
= [sT 0T ]T where s IRn is solution of
? q q
Kps g(q d s) = 0 .
nez and A. Loria
176
Example 6.1
Consider the model of an ideal pendulum
J q + mgl sin(q) = .
l
g
m

nez and A. Loria
177
The equilibria expression takes the form

kps mgl sin(qd s) = 0 .
For the sake of illustration consider the following numeric values
J =1
mgl = 1
kp = 0.25 qd = /2.
The closed loop system under PD control has the equilibria

q
1.25
2.13
3.56
,
,
.
q
0
0
0
nez and A. Loria
178
Unicity of the equilibrium. The equilibria of the closed loop equation

(16) satisfy
T T T
T T T
q q
= s 0
, where
? s IRn is solution of s = Kp1g(q d s) = f (s, q d) .
For all vectors x, y, q d IRn, we get that
kf (x, q d) f (y, q d)k
kg
kx yk
min{Kp}
If min{Kp} > kg we have (by contraction mapping theorem)

T T h T T iT
? the only equilibrium is q q = s
0
.
nez and A. Loria
179
Arbitrarily bounded position and velocity error. Case where

Kp is not restricted to satisfy min{Kp} > kg ,
but it is enough that Kp be positive definite.
Define the following non-negative function
1
1
= q TM (q)q + q TKpq
+ U(q) kU 0 .
V (
q , q)
2
2
where kU = minq {U(q)}
nez and A. Loria
180
yields
The time derivative of V (
q , q)
1
+ q T g(q) ,
V (
q , q)
= q TM (q)
q + q TM (q)q + q TKpq
2
Q
z}|{
T
= q Kv q

T
0 0
q
q
0.
=
0 Kv
q
q
Invoking Lemma 2.2, we conclude
? q(t)
and q (t) are bounded for all t, and
R
2
? the velocities vector is square integrable, that is, 0 kq(t)k

dt < .
nez and A. Loria
181
? Moreover, the exact explicit bounds hold, for all t 0:
2V (
q (0), q(0))
k
q (t)k
min{Kp}
2
TM (q(0))q(0)
+ q (0) Kpq (0) + 2U(q(0)) 2kU
q(0)
=
min{Kp}
(17)
2V (
q (0), q(0))
kq(t)k
min{M (q)}
2
T
TM (q(0))q(0)
q(0)
+ q (0) Kpq (0) + 2U(q(0)) 2kU
=
min{M (q)}
(18)
nez and A. Loria
182
Example 6.2
Consider again the ideal pendulum from Example 6.1
J q + mgl sin(q) = ,
l
g
m

nez and A. Loria
183
The potential energy function is U(q) = mgl(1 cos(q)) and the constant
kU is zero.
Consider next the numeric values
J =1
mgl = 1
kp = 0.25 kv = 0.50
qd = /2.
Assume that we apply the PD controller to drive it from the initial
conditions q(0) = 0 and q(0)
=0
nez and A. Loria
184
According to the bounds (17) and (18), we get

q2(t) q2(0) = 2.46 rad2
q(t)2 [rad2 ]
3
2
1
t0
.....
...
...
...
...
............
...... ................................................................................................................
...
.
.
.
.
....... ........
..
0
0
10
15
20
t [sec]
Figure 47: Graph of q(t)2 .

nez and A. Loria
185
and
kp 2
q (0) = 0.61
q (t)
ml2
2
0.04
0.02
0.00
rad
sec
2
t0
2
q(t)
2 [( rad
sec ) ]
0.08
0.06
.....
... .....
.. ...
... .....
.. ....
... .....
.. ...
... ..... ........
... .. ...
..
... .. .... .............
...
........ .....................................................................................................
..
10
15
20
t [sec]
Figure 48: Graph of q(t)

2
nez and A. Loria
186
Is interesting to observe
lim
q(t)
1.25
=
q(t)
That is, the solutions tend to one of the three equilibria determined in
Example 6.1
nez and A. Loria
187
Example 6.3
Consider the 2-DOF prototype robot studied in Chapter 5,
y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

freedom.
nez and A. Loria
188
whose vector of gravitational torques g(q) is

g1(q) = (m1lc1 + m2l1)g sin(q1) + m2glc2 sin(q1 + q2)
g2(q) = m2glc2 sin(q1 + q2).
The control objective consists on making that
lim q(t) = q d =
/10
/30
[rad].
It may be verified that g(q d) 6= 0

T T T
= 0 IR4 of the closed loop equation with the
The origin q q
PD controller, is not an equilibrium.
nez and A. Loria
189
Consider the PD controller

= Kpq Kv q
with the following numeric values
Kp =
30 0
,
0 30
Kv =
7 0
.
0 3
The initial conditions are fixed to q(0) = 0 and q(0)

= 0.
nez and A. Loria
190
0.4
0.3
0.2
0.1
0.0
[rad]
.....
... q
....1
....
......
..........
.......................................................................................................0.1309
.........................
..... q
...... 2
.................
0.0174
...
..... ..... ..... ..... .................................................................................................................................................................................................................................
0.1
0.0
0.5
1.0
1.5
2.0
t [sec]
Figure 50: Position errors q1 and q2.

One may appreciate that limt q1(t) = 0.1309 and limt q2(t) =
0.0174
therefore, as it was expected, the control objective is not achieved
nez and A. Loria
191
Ch. 7. PD control with gravity compensation

g(q)
ROBOT
q
q
Kv
q d
qd
Kp
Figure 51: PD control with gravity compensation.

The control law is given by = Kpq + Kv q + g(q) where
Kp, Kv IRnn are positive definite symmetric matrices.
nez and A. Loria
192
Global asymptotic stability by La Salles theorem
Considering q d as constant, the closed loop equation may be written as

q
q
d
=
dt
q]
q
M (q d q )1 [Kpq Kv q C(q d q , q)
The origin q

T T
nez and A. Loria
= 0 IR2n is the unique equilibrium.
193
To study the stability of the origin, consider

V (
q , q)
=
T
q
K
1 p
2
0
q

q
0

q
M (q d q )
1 T
1 T
q M (q)q + q Kpq
2
2
is positive definite since M (q) > 0 and Kp > 0

? V (
q , q)
nez and A. Loria
194
Its total derivative with respect to time is
V (
q , q)
= q TKv q

T
0 0
q
q
0
=
0 Kv
q
q
0 because Kv > 0
? V (
q , q)
Consequently, by Theorem 2.3 the origin is stable and all the solutions
(t) and q(t)
q
are bounded.
nez and A. Loria
195
By applying the La Salles theorem (Theorem 2.7) we have that

the set is given by
n
o
=
x IR2n : V (x) = 0

q
=0
q , q)
=
x=
IR2n : V (
q
= {
q IRn, q = 0 IRn} ,

T T

where q (0) q(0)
This is enough to establish global asymptotic stability of the origin,
T T T
= 0 IR2n.
q q
Hence, limt q (t) = 0 and limt q(t)

= 0 (the set-point control
objective is achieved).
T
nez and A. Loria
196
Example 7.1
Consider the Pelican robot studied in Chapter 5

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2
Figure 52: Diagram of the Pelican robot.

nez and A. Loria
197
The components of the vector of gravitational torques g(q) are given by

g1(q) = (m1lc1 + m2l1)g sin(q1) + m2lc2g sin(q1 + q2)
g2(q) = m2lc2g sin(q1 + q2) .
Consider the PD control with gravity compensation, where
Kp = diag{kp} = diag{30} [Nm/rad]
Kv
= diag{kv } = diag{7, 3} [Nm sec/rad] .
The components of , are given by

1 = kpq1 kv q1 + g1(q)
2 = kpq2 kv q2 + g2(q) .
nez and A. Loria
198
The initial conditions are chosen as

q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0 ,
and the desired joint positions as
qd1 = /10, qd2 = /30 [rad] ,
hence, the initial state is taken to be
0.3141
/10
q (0)
/30 0.1047
=
0 = 0 .
q(0)
0
0
nez and A. Loria
199
0.4
0.3
0.2
0.1
0.0
[rad]
.....
...
... q
.... 1
....
....
......
........
..... q
.................
...... 2
...................................................................................0.0620
..........................
..............
.............................................................................................................0.0144
..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ................................................
0.1
0.0
0.5
1.0
1.5
2.0
t [sec]

From Figure above, steady state position errors due to to unmodelled
friction phenomenon, can be seen.
nez and A. Loria
200
Lyapunov function for global asymptotic stability

It is convenient to cite some properties of the vectorial function.
tanh(x) = [ tanh(x1) tanh(x2) tanh(xn)]T
tanh(x)
1.0
0.5
0.0
0.5
1.0
..
..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ...... ..... ..... ..... .......................................................................
..
....
. ...........
.. ....
.. .....
. ..
..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... .............. ..... ..... ..... ..... ..... ..... ..... .... .... .... ..
.
..
... ...
.
.
.. .
.... ..
.
.
.
.
.
......
.................................................................... ..... ..... ..... ....... ..... ..... ..... ..... ..... ..... ..... ..... .... .... .... ..
.
4 3 2 1
Figure 54: Tangent hyperbolic function, tanh(x)

where tanh(x) =
ex ex
ex +ex
nez and A. Loria
.
201
|x| | tanh(x)|, and 1 | tanh(x)| for all x IR therefore,

?
n
kxk
IR
k tanh(x)k
n
x IRn
? tanh(x) = 0 if and only if x = 0.
? For a symmetric positive definite matrix A, it has
1 T
1
q min{A} k tanh(
q )k2
q A
2
2
IRn
q
? If moreover, A is diagonal, then

q min{A} k tanh(
q )k2
tanh(
q )T A
nez and A. Loria
q IRn .
202
We present now an alternative stability analysis.

Consider the Lyapunov function candidate
1 T
1 T
= q M (q)q + q
Kpq tanh(
V (
q , q)
q )T M (q)q
2
2
(19)
where
? > 0 is a constant sufficiently small so as to satisfy simultaneously,
min{Kp}min{M }
2
>
2Max{M }
(20)
4min{Kp}min{Kv }
>.
(21)
2Max{Kv } + 4min{Kp}[ n kC1 + Max{M }]
? There always exists > 0 arbitrarily small satisfying (20) and (21).
> 0 because Kp > 0 and satisfies (20).
? V (
q , q)
nez and A. Loria
203
The time derivative of the Lyapunov function candidate yields (19).

= q T Kv q + q T Sech2(
q )T M (q)q tanh(
q )T Kpq
V (
q , q)
T q ,
q )T C(q, q)
+ tanh(
q )T Kv q tanh(
? and is upperbounded by

ktanh(
q )k

V (
q , q)
kqk
T
ktanh(
q )k
kqk
where
Q=
min{Kp}
12 Max{Kv }
nez and A. Loria
12
Max {Kv }
1
n kC1
min {Kv }

Max{M }
204
is a negative definite function because

V (
q , q)
min{Kp} > 0
and
4min{Kp}min{Kv }
> .
2
Max{Kv } + 4min{Kp}[ nkC1 + Max{M }]
Finally, Theorem 2.4 allows to establish global asymptotic stability of the

origin.
nez and A. Loria
205
Ch. 8. PD control with desired gravity compensation

g(q d )
Kv
q d
qd
ROBOT
q
q
Kp
Figure 55: PD Control with desired gravity compensation.

Control law given by = Kpq + Kv q + g(q d), where
Kp, Kv IRnn are symmetric positive definite matrices
g(q d) may be evaluated offline, it is not necessary anymore to evaluate
g(q) in real time.
nez and A. Loria
206
Considering the desired position q d to be constant, the closed-loop equation

may be written as

q
q
d
=
dt
Kv q C(q, q)
q + g(q d) g(q)]
q
M (q)1 [Kpq
(22)
T T T
q q
= 0 IR2n is an equilibrium point.
There are as many equilibria as solutions in q , may have the equation
Kpq = g(q d q ) g(q d) .
If Kp > > 0 (sufficiently ), then q = 0 IRn is the unique solution.
nez and A. Loria
207
Example 8.1
Consider the model of an ideal pendulum
J q + mgl sin(q) =
l
g
m

nez and A. Loria
208
where g(q) = mgl sin(q).

The equilibria equation takes the form:
kpq = mgl [sin(qd q) sin(qd)] .
Consider the following numeric values,
J =1
mgl = 1
kp = 0.25 qd = /2 .
Either via a graphical method or numeric algorithms, one may verify the
equilibria,

q
0
0.51
4.57
,
,
.
q
0
0
0
nez and A. Loria
209
Consider now a larger value for kp (sufficiently large), e.g.,

kp = 1.25
In this scenario the origin is the unique equilibrium, i.e.,

q
0
=
IR2 .
q
0
nez and A. Loria
210
Boundedness of position and velocity errors, q and q

Assume that
Kp and Kv are positive definite (without assuming that Kp is sufficiently
large).
Then
For a desired constant position q d,
the closed loop equation has an equilibrium at the origin, but there might
also be other equilibria.
(t) and q(t)
In spite of this, q
remain bounded for all initial conditions.
nez and A. Loria
211
Define the following non-negative function:

1
+ U(q) kU + q TKpq
V (
q , q)
= K(q, q)
2
1
T
+ q g(q d) + g(q d)TKp1g(q d)

2
where
= 12 q T M (q)q denotes the kinetic energy
K(q, q)
U(q) denotes the potential energy
and kU = minq {U(q)} .
nez and A. Loria
212
along the trajectories of the closed loop

q , q)
system results
Q
z}|{
= q T Kv q .
V (
q , q)
By invoking Lemma 2.2, we conclude that
? q(t)
and q (t) are bounded
? q(t),
is square integrable, that is,
Z
kq(t)k
dt < .
nez and A. Loria
213
? The explicit upper-bounds on the position and velocity errors are:
k
q (t)k
q
2
q (0), q(0))
kg(q d)k + kg(q d)k + 2min{Kp}V (
min{Kp}
(23)
for all t 0, and

2
kq(t)k
2V (
q (0), q(0))
min{M (q)}
(24)
for all t 0.
nez and A. Loria
214
Example 8.2
Consider again the model of the ideal pendulum
J q + mgl sin(q) = ,
l
g
m

nez and A. Loria
215
The potential energy function is U(q) = mgl[1 cos(q)] and kU = 0.

Consider the numeric values
J =1
mgl = 1
kp = 0.25 kv = 0.50
qd = /2 .
Assume that we use the PD control with desired gravity compensation
to control it from the initial conditions q(0) = 0 and q(0)
= 0.
It is easy to verify that
g(qd) = mgl sin(/2) = 1
1
1 2
kpq (0) + mgl
q (0) +
(mgl)2 = 3.87 .
V (
q (0), q(0))
=
2
2kp
nez and A. Loria
216
According to the bounds (23) and (24), we get
2
q
2
2
mgl
+
[mgl
+
k
q
(0)]
+
(mgl)
p
2
q2(t)
117.79 [ rad ]
kp
35
30
25
20
15
10
5
0
q(t)2 [rad2 ]
.
.......
... ....
.. .....
...........
... ...... ..........................................................................
...
.........
..
...
..
...
..
........ ...............
..............
10
20
30
t [sec]
Figure 58: Graph of position errors, q(t)2.

nez and A. Loria
217
and
q2(t)
2 kp 2
1
q (0) + mgl
q (0) +
(mgl)2 7.75
J 2
2kp
1
0
rad
sec
2
2
q(t)
2 [( rad
sec ) ]
3
2
.....
... ....
.. ....
... .....
.. ...
... .....
. ..
.......
.. .... .....
... ....
.
... .. ..
. ...... ......
... . ... ......
.......
...
... ....... ......................................................................................
10
20
30
t [sec]
Figure 59: Graph of velocities, q(t)

2.
nez and A. Loria
218
Evidence from simulation shows

lim
q(t)
4.57
=
q(t)
The solutions tend precisely to one among the three equilibria which do
not correspond to the origin.
PD control with desired gravity compensation may fail to verify the
set-point control objective.
nez and A. Loria
219
Unicity of equilibrium
For robots having only revolute joints and considering the closed-loop
equation (22), we have.
The equilibria satisfy q

T T
= q

T T
IR2n where
q , q d) .
? q IRn is solution of q = Kp1 [g(q d q ) g(q d)] = f (
For all vectors x, y IRn, we have that
kf (x, q d) f (y, q d)k
kg
kx yk ,
min{Kp}
If min{Kp} > kg we have (by contraction mapping theorem)

T T T T T T
? the unique equilibrium is the origin, q q
= 0 0
IR2n.
nez and A. Loria
220
Global asymptotic stability

To study stability of the equilibrium of closed loop equation, consider
min{Kp} > kg (the origin is the unique equilibrium)
=
V (
q , q)
1 T
) U(q d)
q M (q d q )q + U(q d q
2
1 T
T
+ q Kpq .
+ g(q d) q
2
is radially unbounded and positive definite because M (q) > 0

? V (
q , q)
and min{Kp} > kg .
nez and A. Loria
221
results in V (
= q T Kv q 0
q , q)
q , q)
By applying the La Salles Theorem 2.7, we have
The set is given by
o
n
=
x IR2n : V (x) = 0

q
=0
q , q)
=
x=
IR2n : V (
q
= {
q IRn, q = 0 IRn} .

T T
q (0) q(0)
= 0 IR2n is the unique initial condition in for
which x(t) for all t 0.
This is enough to guarantee global asymptotic stability of the origin
T T T
q q
= 0 IR2n.
T
nez and A. Loria
222
Example 8.3
Consider the 2-DOF prototype robot studied in Chapter 5,

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

freedom.
nez and A. Loria
223
whose components of the gravitational torques vector g(q) are given by

g1(q) = [m1lc1 + m2l1]g sin(q1) + m2lc2g sin(q1 + q2)
g2(q) = m2lc2g sin(q1 + q2) .
The constant kg may be obtained as

kg
= n

gi(q)

max i,j,q
qj
= n [[m1lc1 + m2l1]g + m2lc2g]

i
h
2
2
= 23.94 kg m /sec .
nez and A. Loria
224
Consider the PD controller with desired gravity compensation where

min{Kp} > kg .
In particular, we pick
Kp = diag{kp} = diag{30} [Nm/rad] ,
Kv
= diag{kv } = diag{7, 3} [Nm sec/rad] .
The components of the control input are given by

1 = kpq1 kv q1 + g1(q d) ,
2 = kpq2 kv q2 + g2(q d) .
nez and A. Loria
225
The initial conditions corresponding to the positions and velocities, are

set to
q1(0) = 0, q2(0) = 0 ,
q1(0) = 0, q2(0) = 0 .
The desired joint positions are chosen as
qd1 = /10 [rad] qd2 = /30 [rad] ,
hence, the initial state is set to
0.3141
/10
q (0)
/30 0.1047
[ rad ] .
=
=
0 0
q(0)
0
0
nez and A. Loria
226
Figure shows the experimental results

0.4
0.3
0.2
0.1
0.0
[rad]
....
... q
... 1
...
...
...
....
..... q
....
....
2
.......... .............................................................................................................0.0359
..........................
..
..... ..... ..... ................................................................................................................................................................................................................................................
0.0138
0.1
0.0
0.5
1.0
1.5
2.0
t [sec]
Figure 61: Position errors q1 and q2

The position errors do not vanish due to nonmodeled friction effects.
nez and A. Loria
227
Lyapunov function for global asymptotic stability
We show now a global asymptotic stability analysis without using La Salles

theorem
Consider the following Lyapunov function candidate,
=
V (
q , q)
nez and A. Loria
P
}|
z
T
{
0
2
1+ q M (q)
q
q
2 Kp
k k
1

2
1+ 0q M (q)
M (q)
q
q
k k
1 T
T
,
+ U(q) U(q d) + g(q d) q + q Kpq
1
{z
}
|
)
f (q
228
which can be rewritten as

=
V (
q , q)
1 T
q M (q)q + U(q) U(q d) + g(q d)Tq
2

1
1
0
T
Kpq
+
q
+
q TM (q)q
1 2
1 + k
qk
where 0 > 0, 1 > 2 and 2 > 2 are chosen so that

2min{Kp}
> 1 > 2
kg
2 =
s
21
>2
1 2
2min{Kp}
> 0 > 0 with ( Max{M (q)} ) .
2
nez and A. Loria
229
To show positive definitness, we rearrange
V (
q , q)
=
with =
1
2
1
[q +
q ]T M (q) [q +
q ] + q T
Kp 2M (q) q
2
2
2
1 T
T
Kpq ,
+ U(q) U(q d) + g(q d) q + q
1
{z
}
|
)
f (q
0
k.
1+kq
is positive
hV (
q , q)
i definite because M (q) > 0, and
22 Kp 2M (q) > 0 and min{Kp} > kg .
nez and A. Loria
230
The time derivative of the above Lyapunov function candidate takes the
form
q TKpq +
V (
q , q)
= q TKv q + q TM (q)q
q TKv q
q
q T [g(q d) g(q)]
q TM (q)q ,
q TC(q, q)
which is upperbounded by
V (
q , q)

k
qk
kqk
T z

}|
min{Kp} kg
12 Max{Kv }
12 Max{Kv }
1
20 min {Kv }
k
qk
kqk
1
2.
[min{Kv } 20(kC1 + 2)] kqk
{z
}
2|
nez and A. Loria
231
The matrix Q is positive definite if it holds that

min{Kp} > kg ,
2min{Kv }(min{Kp} kg )
2Max{Kv }
> 0 ,
and we have that > 0 if

min{Kv }
> 0 .
2[kC1 + 2]
nez and A. Loria
232
Under this scenario, we get

i
h
0
2
2
2,
V (
q , q)

kqk
min{Q} k
q k + kqk
1 + k
qk
2
k
q k2
2,
0min{Q}
kqk
1 + k
qk 2
which is a negative definite function.
We conclude that the origin q q

asymptotically stable (Theorem 2.4).
nez and A. Loria

T T
= 0 IR2n is a globally
233
Ch. 9. PID control
Ch. 9. PID control
Kv
q d
qd
Kp
q
q
ROBOT
Rt
Ki
0
Figure 62: PID control.

+ Ki
The PID control law is given by = Kpq + Kv q
Rt
0
q () d, where
Kp, Kv , Ki IRnn, are symmetric positive definite matrices.
nez and A. Loria
234
Ch. 9. PID control
The control law may be expressed via the two following equations,
= Kpq + Kv q + Ki
= q .
=
q
dt

h
i
q g(q)
+ Ki C(q, q)
q d M (q)1 Kpq + Kv q
nez and A. Loria
235
Ch. 9. PID control
If the desired position q d(t) is constant, the equilibrium is

1
Ki g(q d)
q =
IR3n .
0

q
0
This equilibrium may be translated to the origin (via change of variable)
z = Ki1g(q d) .
nez and A. Loria
236
Ch. 9. PID control
The corresponding closed loop equation is

z
q
d
.
q
q =
dt
q g(q)]
M (q)1 [Kpq Kv q + Kiz + g(q d) C(q, q)
q
(25)
Above equation is autonomous and its unique equilibrium is the origin
T T T T
z q q
= 0 IR3n.
nez and A. Loria
237
Ch. 9. PID control
For the sequel,we adopt the following global change of variables,
w
I
q = 0
q
0
nez and A. Loria
I
I
0

0
z
with > 0.
0q
I
q
238
Ch. 9. PID control
The closed loop equation (25) may be expressed as
d
q =
dt

q
q
q
q
M (q)
Kp
1
Ki
nez and A. Loria
q Kv q +
1
Ki w
q g(q)
+ g(q d) C(q, q)
(26)
239
Ch. 9. PID control
Above equation is autonomous

The origin of the state space, w

equilibrium.

T T
= 0 IR3n is the unique
If Kp and Kv are sufficiently large and Ki sufficiently small in the

following sense
min{M }min{Kv }
Max{Ki}
,
>
2
{K
}
k
Max {M }
min
p
g
and moreover
min{Kp} > kg ,
then, the set-point control objective is achieved locally.
nez and A. Loria
240
Ch. 9. PID control

A positive definite Lyapunov function candidate is
w) =
V (
q , q,
1
Ki
w
w
0
0
1

M (q) q
q 0
Kv
2
0
M (q)
M (q)
q
q

1
1 T
+ q Kp Ki q + U(q d q ) U(q d) + q T g(q d)
2
? U(q) denotes as usual, the robots potential energy, and

? is a positive constant satisfying
Max{Ki}
min{M }min{Kv }
>
>
.
2
min{Kp} kg
Max {M }
nez and A. Loria
241
Ch. 9. PID control
Time derivative of the Lyapunov function candidate

It may be written as
w) = q T [Kv M (q)] q q T [Kp Ki] q
V (
q , q,
T q

q T C(q, q)
q T [g(q d) g(q)]
It can be upperbounded by

k
qk
w)
V (
q , q,
kqk
T
Q11
0
Q22(q)

k
qk
kqk
where
Q11 = [min{Kp} kg ] Max{Ki},
.
= min{Kv } [Max{M } + kC1 kqk]
Q22(q)
nez and A. Loria
242
Ch. 9. PID control
It is possible to prove that there exists a ball D
w IRn
D := q , q,

:
q <
q
w) is negative semidefinite.
on which V (
q , q,
Therefore, according with Theorem 2.2, the origin of the closed loop
equation (26), is a stable equilibrium.
nez and A. Loria
243
Ch. 9. PID control
Asymptotic stability
We may use La Salles theorem (Theorem 2.7).
The set is given by
n
o
3n
=
x IR : V (x) = 0
w) = 0
q , q,
=
x = q IR3n : V (
q
= 0 IRn, q = 0 IRn} .
= {w IRn, q

T T
? w(0)
q (0) q(0)
? We conclude that the origin is locally asymptotically stable.
T
nez and A. Loria
244
Ch. 9. PID control
Tuning procedure
The preceding stability analysis allows to extract a simple tuning procedure.
Max{Ki} min{Ki} > 0
Max{Kp} min{Kp} > kg
Max{Ki}
Max2{M }
.
Max{Kv } min{Kv } >
min{Kp} kg min{M }
It requires the knowledge of the structure of M (q) and g(q).
Nonetheless, it is sufficient to have upper bounds on Max{M (q)}
and kg , and a lower bound for min{M (q)}.
nez and A. Loria
245
Ch. 9. PID control
Example 9.2
Consider the 2-DOF prototype robot showed in Figure 63.
y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2
Figure 63: Diagram of the Pelican prototype robot.
nez and A. Loria
246
Ch. 9. PID control
The elements of the inertia matrix M (q) are

M11(q) =
2
m1lc1
m2 l12
2
M12(q) = m2 lc2
+ l1lc2
2
M21(q) = m2 lc2
+ l1lc2
2
lc2
+ 2l1lc2 cos(q2) + I1 + I2

cos(q2) + I2

cos(q2) + I2
2
M22(q) = m2lc2
+ I2 .
The components of the gravitational torques vector g(q), are given by

g2(q) = m2lc2g sin(q1 + q2).
nez and A. Loria
247
Ch. 9. PID control
Firstly, we compute the value of kg using the numeric values listed in

Table 5.1

gi(q)

kg = n Max i,j,q
qj
= n(m1lc1 + m2l1 + m2lc2)g

2
2
= 23.94 kg m /sec
We proceed now to compute numerically min{M (q)} and Max{M (q)}

2
min{M (q)} = 0.011 kg m ,

2
Max{M (q)} = 0.361, kg m
which correspond to q2 = 0.
nez and A. Loria
248
Ch. 9. PID control
By following the tuning procedure, we finally determine the following

matrices,
Ki = diag{1.5} [Nm / (rad sec)] ,
Kp = diag{30} [Nm / rad] ,
Kv
nez and A. Loria
= diag{7, 3} [Nm sec / rad] .
249
0.4
0.3
0.2
0.1
0.0
Ch. 9. PID control
[rad]
...
...
...
...
...
... q
..........................
...................1
............
...
............q
....2...............................................................................................................................................................................
.
..... ..... ..... ..... ..... ..... ..... ..... ..... ..... .. ... .... . .... ...............................................................
0.1
0.0
12.5
25.0
37.5
50.0
t [sec]

From Figure we may conclude that the transient response is slower
This is due to the fact that the tuning procedure limits min{Ki} by a
relatively small upperbound.
nez and A. Loria
250
0.4
0.3
0.2
0.1
0.0
Ch. 9. PID control
[rad]
.....
...
... q
... 1
...
...
....
..
..... q
.....2 ...........
........
.....
..... ..... .............................................................................................................................................................................................................................
0.1
0.0
0.5
1.0
1.5
2.0
t [sec]

If the tuning procedure is violated the performance of the PID controller
improves up.
The latter results have been obtained increasing the values of Ki to
Ki = diag{70, 100} [Nm / (rad sec)] .
nez and A. Loria
251
Part III
TRACKING CONTROL
nez and A. Loria
252
Introduction
Introduction
Consider the dynamic model

q + g(q) = .
M (q)
q + C(q, q)
In terms of the state vector q

T T
gives

q
q
d
=
dt
q g(q)]
q
M (q)1 [ (t) C(q, q)
nez and A. Loria
253
Introduction
where
M (q) IRnn is the inertia matrix,
q IRn is the vector of centrifugal and Coriolis forces,
C(q, q)
g(q) IRn is el vector of gravitational forces and torques and
IRn is a vector of external forces and torques applied at the joints.
q IRn denote the position, velocity and joint
The vectors q, q,
acceleration respectively.
nez and A. Loria
254
Introduction
Given a set of vectorial bounded functions q d, q d and q d referred to as

desired joint positions, velocities and accelerations.
The objective of tracking control consists on finding such that
lim q (t) = 0
where
IRn stands for the joint position errors vector
q
q (t) := q d(t) q(t) ,
(t) = q d(t) q(t)

stands for the velocity error.
q
nez and A. Loria
255
Introduction
In general, a control law may be expressed as

g(q)) .
q , q d, q d, q d, M (q), C(q, q),
= (q, q,
For practical purposes it is desirable that the controller does not depend on
the joint acceleration q .
qd
q d
q d
CONTROLLER
ROBOT
q
q
Figure 66: Tracking control: closed loop system.
nez and A. Loria
256
Introduction
A methodology to analyze the stability may be summarized in:

1. Derivation of the closed loop dynamic equation.
2. Representation of the closed loop equation in the state-space form,

d qd q
g(q)) .
q d, q d, q d, M (q), C(q, q),
= f (q, q,
dt q d q
qd
q d
d
q
Figure 67:
Tracking
representation.
nez and A. Loria
CONTROLLER
+
ROBOT
control
closed-loop
system:
Input-output
257
Introduction
3. Study of the existence and possible unicity of the equilibrium

4. Proposition of a Lyapunov function candidate to study the stability of
any equilibrium of interest
5. Alternatively to step 4), determine the qualitative behavior of the
solutions of the closed loop equation.
nez and A. Loria
258
Introduction
The controllers that we consider are, in order,

Computed torque control and Computed torque+ control.
PD control with compensation and PD+ control.
Feedforward control and PD plus feedforward control.
nez and A. Loria
259
Ch. 10. Computed-torque control and Computed-torque+ control
Ch. 10. Computed-torque control and

Computed-torque+ control
In this chapter we study two controllers that do not present explicitly the
linear PD term
Computedtorque control,
Computedtorque+ control.
nez and A. Loria
260
Computed-torque control
g(q)
q d
Kv
q d
M (q)
Kp
ROBOT
q
q
C(q, q)
qd
Figure 68: Computed-torque control.

The corresponding equation to computed-torque control is given by
h
i
q + g(q) ,
= M (q) q d + Kv q + Kpq + C(q, q)
(27)
where Kp and Kv are symmetric positive definite matrices.

nez and A. Loria
261

h
d + Kv q + Kpq .
M (q)
q = M (q) q
Above equation reduces to
q + Kv q + Kpq = 0
h
which in turn, may be expressed in terms of the state vector q T q

q
q
0
d
=
=
dt
q
Kpq Kv q
Kp

q
I
,
Kv
q
i
T T
as
(28)
where I is the identity matrix of dimension n.

nez and A. Loria
262
It is important to remark that

the closed loop equation (28) is represented by a linear autonomous
differential equation,
h
whose unique equilibrium point is given by q T q
nez and A. Loria
i
T T
= 0 IR2n.
263
Consider next
the globally positive definite Lyapunov function candidate
V (
q , q ) =
q
q
K + Kv I
1 p

2
I
I
q
q
i 1
iTh

1h
T
2
(29)
q + q Kp + Kv I q
q +
q q +
2
2
where the constant satisfies,

Kv I > 0 .
Kp + Kv 2I > 0 .
nez and A. Loria
264
Evaluating the total time derivative of V (

q , q ) we get that
T
V (
q , q ) = q [Kv I] q
q TKpq

T
q
0
Kp
q
.
=
q
0
Kv I
q
(30)
? V (
q , q ) in (30) is globally negative definite.
nez and A. Loria
265
In view of Theorem 2.4, we get that the origin q T q

globally uniformly asymptotically stable and therefore
i
T T
= 0 IR2n is
lim q (t) = 0
lim q (t) = 0 .
For practical purposes, we can choose,
nez and A. Loria
Kp

2
2
= diag 1 , , n
Kv
= diag {21, , 2n} .
266
Example 10.2
Consider the Pelican prototype robot studied in Chapter 5

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2
Figure 69: Diagram of the Pelican prototype.

nez and A. Loria
267
Consider the Computed-torque control (27) on this robot for tracking

control.
The desired reference trajectory, q d(t), is given by Equation (13).
2.0
[rad]
q
1.5
1.0
0.5
0.0
......d2
......
......
......
... .....
.. .....
.. .....
.. .....
.
.
.
qd1....... ..... ..... ... ........
.. ..... ..... ....... ..... .....
.
.
.
.
.
.
.
.
.
.
... ... . . .... . .. ... ... .. ..... .....
... . ...
... ..... ... ... ..... ........ ....... ... .......... ...... .... ..... ... ... ..... ......
.. ..... .. ... .. ... ..... .. ............... ..... ............ ..... ... ........
......
......
.................... ........ ..... ........
.....
......
..
............ ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
10
t [sec]

nez and A. Loria
268
qd1
qd2
2.0 t3
b1[1 e
b2[1 e2.0
2.0 t3
] + c1[1 e
] + c2[1 e2.0
] sin(1t)
] sin(2t)
[rad]
where
? b1 = /4 [rad], c1 = /9 [rad] and 1 = 4 [rad/sec], and
? b2 = /3 [rad], c2 = /6 [rad] and 2 = 3 [rad/sec].
q d(t) and q d(t), were analytically found, and they correspond to
Equations (14) and (15), respectively.
nez and A. Loria
269
The symmetric positive definite matrices Kp and Kv are chosen as

Kp = diag{12, 22} = diag{1500, 14000} [1 / sec]

2
1 / sec ,
Kv = diag{21, 22} = diag{77.46, 236.64}
where we used 1 = 38.7 [rad / sec] and 2 = 118.3 [rad / sec].
q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0 .
nez and A. Loria
270
[rad]
0.02
0.01
0.00
0.01
q
...
.
........
.......
....1
................. ... ....................... .... .................................... . .................... .... ....................... ..... ......
............................................... ........ ..... ............ ...... ........ ..................................................... ............. .............. .... ..
..... .. ................ ................ ..... ....... ....... .. .............. ..
..........
............
............
..........
........
q2
0.02
0
10
t [sec]
Figure 71: Position errors.

The steady state position errors are not zero due to the friction effects
of the actual robot which nevertheless, are neglected in the analysis.
nez and A. Loria
271
Computed-torque+ control
g(q)
M (q)
q d
1
p+
ROBOT
q
q
C(q, q)
Kv
Kp
p
p+
q d
qd
Figure 72: Computed-torque+ control.

nez and A. Loria
272
The equation corresponding to the computed-torque+ controller is given by

h
i
q + g(q) C(q, q)
d + Kv q + Kpq + C(q, q)
= M (q) q
(31)
where
Kv and Kp are symmetric positive definite design matrices,
IRn is obtained by filtering the errors of position q and velocity q ,
that is,
i
b h
bp
Kv q + Kpq ,
(32)
q
=
p+
p+
? p is the differential operator (i.e., p :=
? , b are positive design constants.
nez and A. Loria
d
dt )
273
The computed-torque+ control law is dynamic The expression (32) in the

state space form is a linear autonomous system given by

I
0
1
Kp Kv
d 1

+
=
dt
2
2
0 I
q
0
I

q
1
= [ I I ] [ 0 I ]
2
q
(33)
(34)
where 1, 2 IRn are the new state variables.
nez and A. Loria
274
Now, we have the following closed loop equation

h
i

1
q M (q) C(q, q)
1 + 2 + q Kv q Kpq
d
,
=
dt
1 + Kpq + Kv q

2
2 q
h
the origin q
nez and A. Loria
T1
T2
iT
(35)
= 0 IR4n is an equilibrium point.
275
The study of global asymptotic stability of the origin of the closed loop
equation (35)
is actually an open problem.
Nevertheless, using Lemma 2.2 and Corollary A.2 we will show that
the functions q (t), q (t) and (t) are bounded, and
that the motion control objective is verified.
nez and A. Loria
276
Toward this end, we can get an equivalent closed loop equation

= 0.
M (q) [ + ] + C(q, q)
Consider now the following non-negative function
1
V (t, , q ) = TM (q d q ) 0
2 | {z }
P
The derivative with respect to time of V (, q ) is given by
V (, q ) = TM (q) 0
| {z }
Q
nez and A. Loria
277
We conclude
? Ln Ln2
? and
2V ((0), q (0)) 2t
e
(t)T(t)
.
| {z }
2
k (t)k
This means that that (t) 0 exponentially.
Making use of the latter and of Corollary A.2 we get that

q , q Ln2 Ln
lim q (t) = 0 ,
nez and A. Loria
278
Example 10.3
Consider the 2-DOF prototype robot studied in Chapter 5

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2
Figure 73: Diagram of the Pelican prototype.

nez and A. Loria
279
Consider the Computed-torque+ control described by (31), (33) and (34)

applied to this robot.
q d(t), q d(t) and q d(t) are those used in the previous example.
2.0
[rad]
q
1.5
1.0
0.5
0.0
..... d2
......
......
......
... .....
.. .....
.. .....
.. .....
.
.
.
....... ..... ....... ..... .....
qd1........ ...... ...... ... .........
.
.
.
.
.
.
.
... .. . . .... . .. ... ... .. ...... ..... .
... . ...
... ..... ... ... ..... ....... ........ ... .......... ...... ..... ..... ... ... .... .......
.. ..... .. ... .. ... .... .. ........ ...... .... ........ ... ..... ... .....
....... ...
........
...... ..
..................... ......... ...... .........
......
...
........... ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
10
t [sec]

nez and A. Loria
280
Kp and Kv , and the constant are taken as

Kp = diag{12, 22} = diag{1500, 14000} [1 / sec]

2
1 / sec
Kv = diag{21, 22} = diag{77.46, 236.64}
= 60 .
The initial conditions of the controller state variables are fixed at
1(0) = 0, 2(0) = 0 .
The initial conditions of the actual positions and velocities are set to
q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0 .
nez and A. Loria
281
[rad]
0.02
0.01
0.00
0.01
q. ... ......... ..
...... ...... ..........
.......... .......1
..... ...
. .. . .
.
.
.
.
.
.
..... ..... . .......... ........ ... .......... ................ ....................................... ....... ............ ....... ... ....
........................................... ....... ..... ............ .... ....... ..... ............ .................... ..... ...... ............... .... ..
.... . ............... .............. .... ....... ....... .. ............... ..
..........
............
...........
...........
........
q2
0.02
0
10
t [sec]

It is interesting to remark that the plots obtained with the Computedtorque control, present a considerable similarity to those of figure 75.
nez and A. Loria
282
Ch. 11. PD+ Control and PD Control with Compensation
Ch. 11. PD+ Control and PD Control with

Compensation
We present two controllers whose control laws are based on the dynamic
equations of the system but which also involve certain nonlinearities that
are evaluated along the desired trajectories.
PD control with compensation
PD+ control.
nez and A. Loria
283
PD Control with Compensation

g(q)
q d
M (q)
ROBOT
q
q
Kp
Kv
C(q, q)
q d
qd
Figure 76: PD control with compensation.

The PD control law with compensation may be written as
i
h
[q d +
= Kpq + Kv q + M (q) q d + q + C(q, q)
q ] + g(q),
(36)
where Kp = KpT > 0, Kv = KvT > 0 IRnn and = Kv 1Kp .

nez and A. Loria
284
The equation of closed loop is

h
i
h
i
q
+
M (q) q + q + C(q, q)
q = Kpq Kv q ,
h
which may be expressed in terms of the state vector q T q
i
T T
as
q
q
d
,
h
h
ii
=
dt
q +
M (q)1 Kpq Kv q C(q, q)
q q
q
It is non-autonomous and
iT
h
T
= 0 IR2n as an equilibrium point.
has the origin q T q
nez and A. Loria
285
The stability analysis may be carried out by considering


T
T
T
) M (q d q )
q
q
2Kp + M (q d q
1
.
, q ) =
V (t, q
2
M (q d q )
q
M (q d q )
q
We rewrite it in the following form
h
h
iT
i
1
+
V (t, q , q ) = q +
q M (q) q
q + q TKpq
2
nez and A. Loria
(37)
286
which is equivalent to
1
V (t, q , q ) =
2
q
q
T
|
I
0 I

Kp
0
0 M (q)
{z
B T AB

I 0
I

}
q
q
It is easy to see
i
h
1
T
2
qk
V (t, q , q ) Min{B AB} kq k + k
2
It is positive definite (because Kp > 0, Kv > 0 and hence B T AB > 0)
and radially unbounded.
nez and A. Loria
287
Correspondingly, since the inertia matrix is bounded uniformly in q,

we have that
2

1

2
q + Max{Kp} k
V (t, q , q ) Max{M } q +
qk
2
) is also decrescent.
hence, V (t, q , q
nez and A. Loria
288
The time derivative of the Lyapunov function candidate (37) is
V (t, q , q ) = q Kv q q TTKv
q

T T
Kv 0
q
q
.
=
0
Kv
q
q
Globally negative definite function.

From Theorem 2.4 we conclude immediately global uniform asymptotic
iT
h
T T
q
stability of the equilibrium q
= 0 IR2n.
nez and A. Loria
289
Example 11.1
Consider the Pelican robot presented in Chapter 5

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

nez and A. Loria
290
Consider this robot under PD control with compensation (36).

It is desired that the robot tracks the trajectories q d(t), q d(t) and q d(t)
represented by Equations (13)(15).
2.0
[rad]
q
1.5
1.0
0.5
0.0
......d2
......
......
.....
... ....
.. ....
.. .....
.. .....
.
.
.
qd1......... ...... ...... .... .......... .......... ..... ......... ...... ......
. .
.
..
. .
.
.
..... .. ..... .... ... .. .. ...... .... ... ... .. ..... ....
... ..... ... ... ..... ........ ....... ... .......... ...... ..... ..... ... ... ..... ........
... ..... .. ... .. ....
.. .... .. ... .. ... ...... .. .............
...... ...
...... ..
....
.................... ...... .... ......
....
...
............ ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
10
t [sec]
nez and A. Loria
291
qd1
qd2
2.0 t3
b1[1 e
b2[1 e2.0
2.0 t3
] + c1[1 e
] + c2[1 e2.0
] sin(1t)
] sin(2t)
[rad]
where
? b1 = /4 [rad], c1 = /9 [rad] and 1 = 4 [rad/sec], and
? b2 = /3 [rad], c2 = /6 [rad] and 2 = 3 [rad/sec].
q d(t) and q d(t), were analytically found, and they correspond to
Equations (14) and (15), respectively.
nez and A. Loria
292
The symmetric positive definite matrices Kp and Kv are chosen so that

Kp = diag{200, 150} [N m / rad] ,
Kv
= diag{3} [N m sec / rad] ,
and therefore = Kv1Kp = diag{66.6, 50} [1/sec].

The initial conditions corresponding a the positions and velocities are
q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0 .
nez and A. Loria
293
0.02
0.01
0.00
0.01
[rad]
q
1
........ ......
...
...
....
.....
....
.... ......... ..... ....
................... .... ............... ...... ..... .............. ......
...... ...
.... ... .. ... .... ... .. ....... .. .... ... .................... ... ........... ....... ... .....
.... ..... ......................... ........ ......... ..... ............... ..... ....... ........ ............ ............... ....... ..... ...................... .... ...
.... . ............ ..... ......... ..... ..............
............. ...... ............ .....
...........
...........
.......
..........
..........
q2
0.02
0
10
t [sec]

(t), by virtue of
The experimental steady state tracking position errors q
friction phenomena in the actual robot, are not zero.
nez and A. Loria
294
PD+ control
g(q)
q d
q d
qd
M (q)
C(q, q)
Kv
ROBOT
q
q
Kp
Figure 80: PD+ control.

The control law of PD+ control is given by
q d + g(q)
= Kpq + Kv q + M (q)
q d + C(q, q)
(38)
where Kp, Kv IRnn are symmetric positive definite matrices

nez and A. Loria
295
The closed loop equation may be written as
q
q
d
h
i.
=
dt
, q d q )q
M (q d q )1 Kpq Kv q C(q d q
q
Nonlinear nonautonomous differential equation
h
The only equilibrium point is the origin q T q
nez and A. Loria
i
T T
= 0 IR2n.
296
To analyze the stability of the origin consider now

V (t, q , q ) =
z
T
K
q
1 p
2
0
q
P
}|
{
0
q

M (q d q )
q
1 T
1
q M (q)q + q TKpq ,
2
2
(39)
which is positive definite since M (q) > 0 and Kp > 0.
nez and A. Loria
297
Taking the time derivative of (39) we obtain

) =
V (t, q , q
Q
T z}|{
q Kv q
T
0
q
=
0
q

0
q
0
Kv
q
From Theorem 2.3 we conclude stability of the origin.

La Salles theorem cannot be used to conclude global asymptotic stability.
Alternatively, we may use Lemma 2.2
position and velocity errors are bounded and
2
R

the velocity error is square-integrable ( 0 q (t) dt < . )
nez and A. Loria
298
Example 11.3

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

nez and A. Loria
299
Consider the application of the PD control+ (38) on this robot.

The joint desired trajectories of position, velocity and acceleration: q d(t),
d(t), are given by Equations (13)(15).
q d(t) and q
2.0
1.5
1.0
0.5
0.0
[rad]
q
...
...
...
... d2
... ...
... ...
... ...
.... .....
... .....
... .....
... .....
.. ...
qd1...... ... ..... .. ......
.. ..... ... ...... ... .....
...... ... ........ ........ ..... ... ... .......... ...... ..... ..... ... ........ ........
.. ... .. .. ... ..... .... .. ...... .... ... ... .. .. ... .....
... ............. ..... ... ............... ... ............... ..... ............ ..... ... .......
......
.....
.....
.....
.....
..............
......
..
............ ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
0
10
t [sec]
nez and A. Loria
300
The symmetric positive definite matrices Kp and Kv are chosen as

Kp = diag{200, 150} [N m / rad]
Kv
= diag{3} [N m sec / rad] .

fixed as
q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0 .
nez and A. Loria
301
0.02
0.01
0.00
0.01
[rad]
q
.
..... ..
.... ..
.... ...
.... .. .... .1
.... ..
.... ..
.
..................... ................ . ................... .................... ................... ................... ....................
....... ....... .. ................... .. . .... .................. .... ..................... .. .................... .. ....
.
.
. .
.
..
.... ..... ................................................. ..... ...................... ....... ....... ........... ................. ........................... ...... ............................ .... ......
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
....... . .............. ........... ..... ............. ............. . .............. ...
....................... ......... .. ...............
............. .......... .. .................... ... ...................
..... q
..... .
.....
...
.....
...
....
..
..
..
.
2 .
.
0.02
0
10
t [sec]

(t), by virtue of
The experimental steady state tracking position errors q
friction phenomena (neglected in the analysis) in the actual robot, are
not zero.
nez and A. Loria
302
Lyapunov function for asymptotic stability

We present an alternative stability analysis. Consider now,
Lyapunov function candidate,
V (t, q , q ) =
T
Kp
q
1
0
2
q
k M (q)
1+kq
= W (t, q , q ) +

0
M
(q)
q
k
1+kq
M (q)
(40)
q
0
q T M (q)q
1 + k
qk
| {z }
(
q)
T
where W (t, q , q ) = 12 q M (q)q + 12 q TKpq is the Lyapunov function
(39).
nez and A. Loria
303
The
s positive constant 0 is chosen so as to satisfy simultaneously
min{Kp}
> 0 > 0
Max{M (q)}
min{Kv }
> 0 > 0
2 (kC1 + 2Max{M (q)})

2min{Kp}min{Kv }
2 > 0 > 0
(Max{Kv } + kC1 kq dkMax)
This, implies that the matrix Kp 2M (q) > 0
The function (40) may be rewritten as
h
h
iT
i 1

1
T
2
+
V (t, q , q ) = q
q M (q) q +
q + q Kp M (q) q
2
2
which is positive definite since so is M (q) as well as Kp 2M (q).
nez and A. Loria
304
Notice that V (t, q , q ) satisfies

"
q k
1 k
V (t, q , q )

2 q
#T
0 Max{M }
Max{Kp}
0 Max{M }
Max{M }
"
q k
k
, (41)
q
The matrix on the right hand side of inequality (41) is positive definite
in view of the condition on 0,
s
Max{Kp}
> 0 > 0
Max{M (q)}
q IRn
Thus, the function (40) is positive definite, radially unbounded and

decrescent.
nez and A. Loria
305
The time derivative of the Lyapunov function candidate (40) is given by
T
T
T
q
V (
q , q ) = q Kv q + (
q )q M (q)q + (
q )q C(q, q)
{z
}
|
a(
q , q )
h
i
T
+ Kv q + (
(
q )
q Kpq
q )
q TM (q)q .
{z
}
|
)
b(
q, q
nez and A. Loria
306
It may be upperbounded in the following manner
k
qk
k
qk
V (t, q , q ) Q

q
q
|
{z
}
h(k
q k , kq k)
2
1

[min{Kv }20 (kC1 + 2Max{M })] q ,
{z
}
2|
nez and A. Loria
307
where the symmetric matrix Q is given by
Q=
min{Kp}
12
12 (Max{Kv } + kC1 kq dk)
(Max{Kv } + kC1 kq dk)

1
20 min {Kv }
The function V (t, q , q ) is negative definite

h
q
Thus, using Theorem 2.4 we conclude that the origin q
T
i
T T
=0
IR2n is globally uniformly asymptotically stable.
nez and A. Loria
308
Ch. 12. Feedforward control and PD plus feedforward control
Ch. 12. Feedforward control and PD plus feedforward

control
Practical implementation of controllers is via digital technology.
Sampling of the joint position q and of the velocity q,

computation of the control action from the control law,
the order to apply this control action is sent to the actuators.
d(t),
Control strategies using precomputed terms based on q d(t), q d(t) and q
has advantages:
Reduction in the time of computation of
Higher processing frequency (larger potential for fast tasks)
nez and A. Loria
309
Feedforward control
q d
M (q d )
C(q d , q d )
g(q d )
ROBOT
q
q
q d
qd
Figure 84: Feedforward control.

The feedforward controller is given by
q d + C(q d, q d)q d + g(q d) .
= M (q d)
nez and A. Loria
(42)
310
The behavior of the control system is described by
q
q
d
,
=
dt
q d + Cdq d C q + g d g]
q
M 1 [(Md M )
where
Cd = C(q d, q d)
M = M (q), Md = M (q d), C = C(q, q),
g = g(q) and g d = g(q d).
h
i
T T
The origin q q
= 0 IR2n is an equilibrium point of the previous
equation but in general, it is not the only one.
T
nez and A. Loria
311
Example 12.3

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

nez and A. Loria
312
Consider the application of feedforward control (42) on this robot.

The desired trajectory is given by q d(t) which is defined in (13).
2.0
1.5
1.0
0.5
0.0
[rad]
q
... d2
...
...
...
.... .....
... ...
... ...
... ...
.. ...
... .....
... .....
... .....
qd1....... ... ..... .. ......
.. ..... ... ...... ... .....
...... ... ........ ....... ..... ... ... .......... ...... ..... ..... ... ........ ....... .
.. .... .. .. ... ...... ..... .. ...... .... ..... ..... ... ... ..... ........
... ..... ... .. .. ...
... ............ ..... ... .............. ... ...............
...... ..
..... .
.....
.....
.....
...............
....
..
............ ..... .... .... .... .... .... .... .... .... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..
0
10
t [sec]

q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0 .
nez and A. Loria
313
[rad]
2.0
1.5
1.0
0.5
0.0
0.5
.........
..........
..........
... .....
... ....
... ....
.. ...
...
...
...
...
.
..
... ...
... ...
.
...
... ...
.
.
.......... ...
... .
... .
...
... ..
.
.
.. .... ...
.......
.......
....
.......
.
.
..
..
....
......
.....
......
.....
.
.
.
.
.
.
.
.
.
.....q
.
.
.
.
.
.
.
.
1 ..
..... .......... ...... .... ... .... ... ..... ... .... ... .... ...
..... ..... ..... .... ..... ....... ..... ....... ..... ....... ............ ..... ...... ....... ..... .................. ..... ....... ....... ..... ..
......
......
.......
... .
......
....
q2..............
10
t [sec]
Figure 87: Position errors

(t) tend to an oscillatory behavior.
Figure shows the position errors q
Naturally, this behavior is far from satisfactory.
A rigorous generic analysis of stability or instability seems to be an
impossible task.
nez and A. Loria
314
PD plus feedforward control

q d
M (q d )
C(q d , q d )
g(q d )
Kv
ROBOT
q
q
Kp
q d
qd
Figure 88: PD plus feedforward control.

Control law is given by
q d + C(q d, q d)q d + g(q d),
= Kpq + Kv q + M (q d)
where Kp = KpT > 0, Kv = KvT > 0 IRnn.
nez and A. Loria
315
The closed loop equation may be written as:

q
d
h
i,
=
dt
q h(
M (q)1 Kpq Kv q C(q, q)
q , q )
(43)
where the so-called residual dynamics, is given by

q d + g(q d) g(q) .
h(
q , q ) = [M (q d) M (q)]
q d + [C(q d, q d) C(q, q)]
The origin [e
q
q T ]T = 0 IR2n of the state space is an equilibrium.

e
However, the number of equilibria depends on the proportional gain Kp.

nez and A. Loria
316
Unicity of the equilibrium

The equilibria are the constant vectors
T
e ]T = [
[e
qT q
q T 0T ]T IR2n,
q , 0) = 0 .
where q IRn is a solution of Kpq + h(
It always is satisfied by the trivial solution q = 0 IRn
Explicit conditions to ensure unicity of the equilibrium are presented next.

Define k(
q ) = Kp1h(
q , 0) .
For all vectors x, y IRn, we have
1

kk(x) k(y)k
Kp [h(x, 0) h(y, 0)]
1
Max Kp kh(x, 0) h(y, 0)k .
nez and A. Loria
317
On the other hand, we have that

dk
kh(x, 0) h(y, 0)k k[M (q d y) M (q d x)] q
+ k[C(q d y, q d) C(q d x, q d)] q dk
+ kg(q d y) g(q d x)k .
q d kM +
kh(x, 0) h(y, 0)k kg + kM k
nez and A. Loria
2
kC2 kq dkM
kx yk .
318
We get that
i
h
1
2
kg + kM k
q dkM + kC2 kq dkM kx yk .
kk(x) k(y)k
min {Kp}
Invoking the contraction mapping theorem, we conclude that
2
q dkM + kC2 kq dkM

min {Kp} > kg + kM k
It is a sufficient condition for k(
q ) to have a unique fixed point, and
therefore,
for the origin of the state space to be the unique equilibrium of the
system in closed loop (43).
nez and A. Loria
319
Global uniform asymptotic stability
We assume that given a constant > 0,

Kv is chosen sufficiently large in the sense that
Max{Kv } min{Kv } > kh1 + b ,
and so is Kp but in the sense that
Max{Kp} min{Kp} > 3
nez and A. Loria
"
[2 a + kh2]
+ kh2
4 [min{Kv } kh1 b]
320
so that
2 2
2 1 Max {M }
Max{Kp} min{Kp} >
min{M }
(44)
where kh1 and kh2 are defined in Chapter 4

while the constants a and b are given by
1
a =
[Max{Kv } + kC1 kq dkM + kh1] ,
2
b = 4 Max{M } + 2 kC1 .
nez and A. Loria
321
Lyapunov function candidate. To carry out the stability analysis, consider

1
1 T
M (q)q + q T Kpq + tanh(
V (t, q , q ) = q
q )T M (q)q
2
2
(45)
where > 0 is a given constant and
tanh(x1)
..
tanh(x) =
tanh(xn)
with x IRn.
nez and A. Loria
322
The Lyapunov function candidate (45) satisfies the following inequality

"
q k
1 k
V (t, q , q )

2 q
#T
1 Max{M }
min{Kp}
1 Max{M }
min{M }
"
q k
k

q
It is positive definite and radially unbounded since

Kp is positive definite min{Kp} > 0,
and it is chosen so as to satisfy (44).
Max{Kp} min{Kp} >
nez and A. Loria
2
2 1
2Max{M }
.
min{M }
323
One may also show

"
k
q k
1

V (t, q , q )

2 q
#T
Max{Kp}
1 Max{M }
1 Max{M }
Max{M }
"
k
q k

q
whose right hand side is positive definite and radially unbounded since
the condition
Max{Kp} > 212 Max{M },
is satisfied under hypothesis (44) on Kp.
This means that V (t, q , q ) is decrescent.
nez and A. Loria
324
Time derivative.
The time derivative of the Lyapunov function candidate yields
T
T
, q ) = q Kv q
q
Sech2(
V (t, q
q )T M (q)q tanh(
q )T Kpq
T q
tanh(
q )T Kv q + tanh(
q )T C(q, q)
T
q , q ) tanh(
q )T h(
q , q ) .
q h(
nez and A. Loria
325
which can be upperbounded by

, q)

V (t, q
"

T min{Kp}
kh2
ktanh(
q )k
3
kqk
a 1 k2h2
|
1 kh2
2
#
a
ktanh(
q )k
1
kqk
[min {Kv } kh1] b
{z
}
R()
? where
1
2
b = 4 Max{M } + 2 kC1 .
a =
? R() is positive definite

? therefore, V (t, q , q ) is globally negative definite.
Theorem 2.4 concludes global uniform asymptotic stability of the origin.
nez and A. Loria
326
Tuning procedure. It can be summarized as

Derivation of the dynamic robot model to be controlled. Particularly,
and g(q) in closed form.
computation of M (q), C(q, q)
0
Computation of the constants Max{M (q)}, min{M (q)}, kM , kM
,kC1 ,
kC2 , k0 and kg . For this, it is suggested to use the information given in
Table 4.1.
Computation of k
q dkMax, kq dkMax from the specification of a given task
to the robot.
nez and A. Loria
327
Computation of the constants s1 and s2 given by

h
q d kM +
s1 = kg + kM k
and
2
kC2 kq dkM
0
s2 = 2 k0 + kM
k
q d kM +
2
kC1 kq dkM
Computation of kh1 and kh2 given by

? kh1 kC1 kq dkM ,
s2
.
? kh2
tanh ss21
nez and A. Loria
328
Computation of the constants a and b given by

1
2
b = 4 Max{M } + 2 kC1 ,
a =
where 2 =
n, 4 = 1.
nez and A. Loria
329
Select > 0 and determine the design matrices Kp and Kv so that their
smallest eigenvalues satisfy
? min{Kv } > kh1"+ b,
[2 a + kh2]
+ kh2 ,
4 [min{Kv } kh1 b]
2 2
2 1 Max {M }
,
? min{Kp} >
min{M }
? min{Kp} > 3
with 1 = 1, 3 = 1.
nez and A. Loria
330
Example 12.5
Consider the 2-DOF prototype robot showed in the Figure

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

nez and A. Loria
331
The elements of the inertia matrix M (q) are

M11(q) =
2
m1lc1
m2 l12
2
M12(q) = m2 lc2
+ l1lc2
2
M21(q) = m2 lc2
+ l1lc2
2
lc2
+ 2l1lc2 cos(q2) + I1 + I2

cos(q2) + I2

cos(q2) + I2
2
M22(q) = m2lc2
+ I2 .
nez and A. Loria
332
are given
The elements of the centrifugal and Coriolis forces matrix C(q, q)
by
C11(q, q)
= m2l1lc2 sin(q2)q2
C12(q, q)
= m2l1lc2 sin(q2) (q1 + q2)
= m2l1lc2 sin(q2)q1
C21(q, q)
C22(q, q)
= 0.
The elements of the vector of gravitational torques g(q) are
g2(q) = m2lc2g sin(q1 + q2) .
nez and A. Loria
333
Using the numeric values of the constants given in Table 5.1 as well as
the formulas on Table 4.1, we get
kM
kC1
kC2
kg
= 0.0974 kg m ,

2
= 0.0487 kg m ,

2
= 0.0974 kg m ,

2
2
= 23.94 kg m /sec ,
0
kM
= Max{M (q)} = 0.3614 kg m

2
min{M (q)} = 0.011 kg m .
Numerically, we get: k0 = 7.664
[N m] , and
q dkMax = 9.52 [ rad/sec2] .

kq dkMax = 2.33 [ rad/sec] and, k
nez and A. Loria
334
Using this information and the definitions of the constants from the
tuning procedure, we get that
s1 = 25.385 [N m] ,
s2 = 22.733 [N m] ,

2
kh1 = 0.114 kg m /sec ,
kh2 = 31.834 [N m] ,

2
a = 1.614 kg m /sec ,

2
b = 0.43 kg m .
Finally, we set = 2 [sec1],

nez and A. Loria
335
An appropriate choice of the gains is

Kp = diag{200, 150} [N m] ,
Kv
= diag{3} [N m sec/rad] .

chosen as
q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0 .
nez and A. Loria
336
0.02
0.01
0.00
0.01
[rad]
q...1
.
.... ..
... .
.... ..
.... .
.... ..
..
.
......................... ................... .. ..................... ..................... .................... ..................... .. .....................
.. .... . ........ ..... . . .. ............. ... ............... . . ....... ..... . .. ..
....... ..... ........................................ ........... ..... .................. ...... ........ ......... ............... ................ ...... ................................... .... .......
........ .. ........ ........ .......... ... .. ... ........ ........ .......... ... .. .. ........ ......... ...
.............. ................... ................... ..... ................ ................ ..... .................... ......
..........
..... .......... ... .. ....... ..
..... .... ....... ..... ...
....
...
.....
.....
....
.
....
..... q
2
..
.
.
.
.
.
0.02
0
10
t [sec]
Figure 90: Position errors

In contrast to example 12.3 where the controller did not carry the PD
term, the behavior abtained here is satisfactory.
nez and A. Loria
337
Part IV
ADVANCED TOPICS
nez and A. Loria
338
Introduction to Part IV
Introduction to Part IV
We deal with a variety of topics:

Control without velocity measurements
Control under model uncertainty.
Specifically:
PD control with gravity compensation and PD control with desired
gravity compensation
Introduction to adaptive robot control
PD control with adaptive gravity compensation
PD control with adaptive compensation.
nez and A. Loria
339
Ch. 13. PD Control
Ch. 13. PD Control with gravity compensation and

PD Control with precalculated gravity compensation
The interest to count on controllers without measurement of velocity, is

twofold:
no poor quality for certain bands of operation
suppression of velocity sensors (no tachometers and resolvers)
reduction in the production cost
robot lighter.
nez and A. Loria
340
Ch. 13. PD Control
The design of controllers that do not require velocity measurements to

control robot manipulators
is a topic of investigation broached in the decade of the 1990s
to date, many questions remain open.
the common idea: to propose state observers to estimate the velocity.
In this chapter we present an alternative:
by the filtering of q
substituting q,
through a first order system of zero relative degree,
whose output is denoted, by .
nez and A. Loria
341
Ch. 13. PD Control
Specifically, IRn is given by:

bip
= diag
q
p + ai
d
, ai and bi are arbitrary strictly positive real constants,
? where, p = dt
for i = 1, 2, , n.
A state-space representation of above Equation is:

x = Ax ABq
= x + Bq
? where, x IRn represents the state vector of the filters,
? A = diag{ai} and B = diag{bi}.
nez and A. Loria
342
Ch. 13. PD Control
In this chapter we present the study of the proposed modification for the
following controllers:
PD control with gravity compensation
PD control with desired gravity compensation.
the derivative part of both controllers is no longer proportional to the
derivative of the position error q
this motivates the quotes in D in the name of the controller.
nez and A. Loria
343
Ch. 13. PD Control
PD Control with gravity compensation

Control law:
+ Kv [q d ] + g(q)
= Kpq
x = Ax ABq
= x + Bq
where
Kp, Kv IRnn are diagonal positive definite matrices,
A = diag{ai} and B = diag{bi} and ai and bi are arbitrary real strictly
positive constants for i = 1, 2, , n.
nez and A. Loria
344
Ch. 13. PD Control
g(q)
Kv
q d
ROBOT
B
Kp
(pI +A)1AB
qd
Figure 91: PD Control with gravity compensation.

When q d is a constant vector
the control law becomes
Kv diag
= Kpq
nez and A. Loria
bip
q + g(q).
p + ai
345
Ch. 13. PD Control
it verifies the set-point control objective: limt q(t) = q d (with

q d IRn constant)
The closed loop equation may be rewritten as
A + AB
q
q
q =
dt
, q)
q]
M (q d q )1 [Kpq Kv [ B
q ] C(q d q
q
? which is an autonomous differential equation
? = x + Bq d.
iT
h
? the origin T q T q T = 0 IR3n is the unique equilibrium.
nez and A. Loria
346
Ch. 13. PD Control
Stability of the origin,

1 T
1
= K(q, q)
+ q Kpq + ( B
q )T Kv B 1 ( B
q)
V (, q , q)
2
2
where
= 12 q TM (q)q is the kinetic energy function
? K(q, q)
? Kv B 1 is positive definite.
is globally positive definite.
? Hence, V (, q , q)
nez and A. Loria
347
Ch. 13. PD Control

1 T
T
TKpq
V (, q , q)
= q M (q)
q + q M (q)q + q
2
h
i
.
+ [ B
q ]T Kv B 1 B q
? Using the closed loop equation we obtain
V (, q , q)
= [ B
q ]T Kv B 1A [ B
q]
T
1
Kv B A Kv A 0
BKv A 0
= q Kv A
0
0
0
q
q
q
is globally negative semidefinite.

? V (, q , q)
The origin is asymptotically stable (use the La Salles Theorem).
nez and A. Loria
348
Ch. 13. PD Control
Example 13.1
Consider the Pelican robot

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

nez and A. Loria
349
Ch. 13. PD Control

g2(q) = m2lc2g sin(q1 + q2) .
Consider the PD controller with gravity compensation with

Kv
= diag{kv } = diag{7, 3} [Nm sec/rad] ,
A = diag{ai} = diag{30, 70} [1/sec] ,

B
= diag{bi} = diag{30, 70} [1/sec] .
nez and A. Loria
350
Ch. 13. PD Control

1 = kpq1 kv 1 + g1(q)
2 = kpq2 kv 2 + g2(q)
x 1 = a1x1 a1b1q1
x 2 = a2x2 a2b2q2
1 = x1 + b1q1
2 = x2 + b2q2 .
Initial conditions
q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0
x1(0) = 0, x2(0) = 0 .
nez and A. Loria
351
Ch. 13. PD Control
The desired joint positions are chosen as
qd1 = /10, qd2 = /30 [rad] .
In terms of the state vector of the closed loop equation, the initial state
is
b1/10
9.423

(0) b2/30 7.329
/10 0.3141
q (0) = /30 = 0.1047 .

0
0
q(0)
0
0
nez and A. Loria
352

0.4
0.3
0.2
0.1
0.0
Ch. 13. PD Control
[rad]
.....
... q
... 1
...
....
......
.
.... q ....................
...... 2
.............................................................................................0.0587
.........................
................
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ..... ...............................................
0.0151
0.1
0.0
0.5
1.0
1.5
2.0
t [sec]
Figure 93: Position errors q1(t) and q2(t).

Figure presents the experimental results,
? q (t) tends asymptotically to a constant nonzero value (due to the
non-modeled friction effects).
nez and A. Loria
353
Ch. 13. PD Control
PD Control with desired gravity compensation

Control law
+ Kv [q d ] + g(q d)
= Kpq
x = Ax ABq
= x + Bq
where
Kp, Kv IRnn are diagonal positive definite matrices,
A = diag{ai} and B = diag{bi} with ai and bi arbitrary real strictly
positive constants for all i = 1, 2, , n.
nez and A. Loria
354
Ch. 13. PD Control
g(q d )
Kv
q d
qd
Kp

ROBOT
(pI +A)1AB
Figure 94: PD Control with desired gravity compensation.

When q d is a constant vector
the control law may be expressed by

bip
= Kpq Kv diag
q + g(q d),
p + ai
nez and A. Loria
355
Ch. 13. PD Control
If min{Kp} > kg , then

it verifies the set-point control objective that is, limt q(t) = q d
The closed loop equation may rewritten as:
A + AB
q
q
q =
dt
qg(q
M (q)1 [Kpq Kv [B
q]+g(q d)C(q d
q , q)
q )]
q
d
? which is an autonomous differential equation.
? = x + Bq d.
iT
h
T
T
T
? the origin q q
= 0 IR3n is the unique equilibrium
nez and A. Loria
356
Ch. 13. PD Control
Stability of the origin

Consider the Lyapunov function candidate
1
T
+ f (
= K(q d q , q)
q ) Kv B 1 ( B
q ) + ( B
q)
V (, q , q)
2
where
= 12 q T M (q d q )q
? K(q d q , q)
? f (
q ) = U(q d q ) U(q d) + g(q d)T q + 12 q TKpq .
Since Kv B 1 is positive definite and
min{Kp} > kg
is also globally positive definite.
Consequently, V (, q , q)
nez and A. Loria
357
Ch. 13. PD Control
The time derivative of the Lyapunov function candidate yields

1 T
T
T
= q M (q)
V (, q , q)
q + q M (q)q q g(q d q ) + g(q d)T q
2
h
i
.
q ]T Kv B 1 B q
+ q TKpq + [ B
? Using the closed loop equation we obtain
T
V (, q , q)
= ( B
q ) Kv B 1A ( B
q)
is a globally semidefinite negative function.

? V (, q , q)
The origin is global asymptotically stable (use La Salles Theorem).
nez and A. Loria
358
Ch. 13. PD Control
Example 13.2

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

nez and A. Loria
359
Ch. 13. PD Control

g2(q) = m2lc2g sin(q1 + q2) .
The constant kg may be obtained as (see Property , 4.3):
kg

= n max

gi(q)

i,j,q
qj
= n((m1lc1 + m2l1)g + m2lc2g)

h
i
2
2
= 23.94 kg m /sec .
nez and A. Loria
360
Ch. 13. PD Control
Consider the PD control with desired gravity compensation satisfying:

min{Kp} > kg .
In particular, these matrices are taken to be

Kv
= diag{kv } = diag{7, 3} [Nm sec/rad] ,
A = diag{ai} = diag{30, 70} [1/sec] ,

B
= diag{bi} = diag{30, 70} [1/sec] .
nez and A. Loria
361
Ch. 13. PD Control

1 = kpq1 kv 1 + g1(q)
2 = kpq2 kv 2 + g2(q)
x 1 = a1x1 a1b1q1
x 2 = a2x2 a2b2q2
1 = x1 + b1q1
2 = x2 + b2q2 .
Initial conditions
q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0
x1(0) = 0, x2(0) = 0 .
nez and A. Loria
362
Ch. 13. PD Control
The desired joint positions are
qd1 = /10, qd2 = /30 [rad] .
is
b1/10
9.423

(0) b2/30 7.329
/10 0.3141
q (0) = /30 = 0.1047 .

0
0
q(0)
0
0
nez and A. Loria
363

0.4
0.3
0.2
0.1
0.0
Ch. 13. PD Control
[rad]
....
... q
... 1
...
...
...
.
..... q2...........
..... .....................
.........................................................................................0.0368
........
..........................
..... ..... ........................................................................................................................................................................................................................................................
0.0145
0.1
0.0
0.5
1.0
1.5
2.0
t [sec]
Figure 96: Position errors q1(t) and q2(t)

Figure shows the experimental results,
q (t) tends asymptotically to a constant nonzero value (due to the
friction effects in the actual robot).
nez and A. Loria
364
Ch. 14. Introduction to adaptive robot control
Ch.14. Introduction to adaptive robot control

The implicit assumptions in the preceding chapters are:
The model is accurately known
The constant physical parameters are accurately known.
Two general techniques deal with the absence of above considerations:
Robust control aims at controlling, with a small error, a class of robot
manipulators (model is not accurately known) with the same controller.
Adaptive control deals with
uncertainty in the systems parameters (unknown constant parameters)
it requires the precise knowledge of the structure of the system.
nez and A. Loria
365
In the subsequent chapters we describe and analyze two adaptive controllers

for robots.
PD control with feedforward gravity compensation.

PD control with adaptive compensation.
nez and A. Loria
366
Parameterization of the dynamic model

The dynamic model of robot manipulators as we know, is given by
q + g(q) =
M (q)
q + C(q, q)
To emphasize the dependence of the dynamic model on the dynamic
parameters, we write
)q + g(q, ) = .
M (q, )
q + C(q, q,
(46)
where
? IRm is the m vector of parameters
? m is some known constant
? , do not necessarily correspond to the physical parameters of the
robot
nez and A. Loria
367
Example 14.1
Consider the example of an ideal pendulum
Dynamic model
ml2q + mgl sin(q) =
where its mass m is concentrated at the tip,
at a distance l from its axis of rotation.
We identify:
2
q, and g(q, ) = mgl sin(q).
? M (q,) = ml

ml2
? =
(assuming that both parameters are unknown)
mgl
? it is a nonlinear vectorial function of the physical parameters.
nez and A. Loria
368
Linearity in the Dynamic Parameters

Above example also shows that
the dynamic model is linear in the parameters .
ml2q + mgl sin(q)
[
q
sin(q)]
ml
mgl
=: (q, q) .
? contains nonlinear terms of the state, and
? is the vector of dynamic parameters.
Property known as
parameterization.
nez and A. Loria
linearity
in
the
parameters
or
linear
369
Property 14.1. Linearity in the dynamic parameters.

For all u, v, w IRn it holds that
M (q, )u+C(q, w, )v+g(q, ) = (q, u, v, w)+(q, u, v, w)
1.
where (q, u, v, w) is a vector of n 1, (q, u, v, w) is a matrix
of n m and the vector IRm depends only on the dynamic
parameters of the manipulator and its load.
2.
Moreover, if q, u, v, w Ln then (q, u, v, w) Lnm

.
nez and A. Loria
370
The Nominal Model
The dynamics (46), may be expressed as

M (q, )u + C(q, w, )v + g(q, ) =
(q, u, v, w) + M0(q)u + C0(q, w)v + g 0(q),
where
the nominal model or nominal part of the model is,
(q, u, v, w) = M0(q)u + C0(q, w)v + g 0(q) .
nez and A. Loria
371
M0(q), C0(q, w) and the vector g 0(q) are the parts of

and g(q)
? M (q), C(q, q)
? do not depend on (unknown dynamic parameters).
IRm, we have
Given a vector
=
(q, u, v, w)
M (q, )u+C(q,
w, )v+g(q,
)M
0(q)uC0(q, w)vg 0(q).
A particular case is when u = v = w = 0 IRn. In this scenario
g(q, ) = (q, 0, 0, 0) + g 0(q) .
nez and A. Loria
372
Example 14.7

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

nez and A. Loria
373
Dynamic model

C11(q, q)
g1(q)
M11(q) M12(q)
C12(q, q)
=
q+
q+
C22(q, q)
M21(q) M22(q)
C21(q, q)
g2(q)
{z
}
{z
}
|
|
| {z }
)
M (q )
g (q )
C(q ,q
where
M11(q) =
2
m1lc1
m2 l12
2
M12(q) = m2 lc2
+ l1lc2
2
M21(q) = m2 lc2 + l1lc2
2
lc2
+ 2l1lc2 cos(q2) + I1 + I2

cos(q2) + I2

cos(q2) + I2
2
M22(q) = m2lc2
+ I2
C11(q, q)
= m2l1lc2 sin(q2)q2
nez and A. Loria
374
C12(q, q)
= m2l1lc2 sin(q2) [q1 + q2]
= m2l1lc2 sin(q2)q1
C21(q, q)
C22(q, q)
= 0
g2(q) = m2lc2g sin(q1 + q2) .
We have selected as parameters of interest,
? m2, I2 and lc2
We define the vectors

u1
u=
,
u2
nez and A. Loria
v=
v1
v2
w=
w1
w2
.
375
The parameterization leads to

M (q, )u + C(q, w, )v + g(q, ) =

1
11 12 13
2 + M0(q)u + C0(q, w)v + g 0(q),
21 22 23
3
where
11 = l12u1 + l1g sin(q1)
12 = 2l1 cos(q2)u1 + l1 cos(q2)u2 l1 sin(q2)w2v1
l1 sin(q2)[w1 + w2]v2 + g sin(q1 + q2)
13 = u1 + u2
21 = 0
nez and A. Loria
376
22 = l1 cos(q2)u1 + l1 sin(q2)w1v1 + g sin(q1 + q2)

23 = u1 + u2

m2
1
= 2 = m2lc2
2
3
m2lc2
+ I2

2
m1lc1 + I1 0
M0(q) =
0
0

0 0
C0(q, w) =
0 0

m1lc1g sin(q1)
.
g 0(q) =
0
depends exclusively on the parameters of interest m2, I2 and lc2.
nez and A. Loria
377
The Adaptive Robot Control Problem

Consider the dynamic equation
)q + g(q, ) =
M (q, )
q + C(q, q,
or equivalently,
q + g 0(q) = .
q)
+ M0(q)
q + C0(q, q)
(q, q , q,
IRnn and
, q,
q)
IRnm, M0(q), C0(q, q)
Assume that (q, q
g 0(q) IRn are known
but IRm is unknown
Given a set of vectorial bounded functions q d, q d and q d,
we seek to design controllers that achieve the position or the motion
control objectives.
nez and A. Loria
378
Parameterization of the controller
The control laws may be written in the functional form

g(q)) .
q d, q d, q d, M (q), C(q, q),
= (q, q,
Giving a little more structure we have
q d, q d, q d) + M (q)u + C(q, w)v + g(q),
= 1(q, q,
where
nez and A. Loria
379
q d, q d, q d), usually corresponds to linear control

? The first term 1(q, q,
terms of PD type, i.e.,
q d, q d, q d) = Kp[q d q] + Kv [q d q]
1(q, q,
where
Kp is the gain matrix of position
Kv is the velocity (or derivative gain).
It does not depend explicitly on the dynamic model
? In the second term u, v, w IRn depend on q, q and on q d, q d and q d.
It depends explicitly on the dynamic model
nez and A. Loria
380
In general an adaptive controller is formed by two main parts:

Control law or controller.
Adaptation law.
Control law in the generic form
+ C(q, w, )v
+ g(q, )
q d, q d, q d) + M (q, )u
= 1(q, q,
where
is the vector of adaptive parameters
?
nez and A. Loria
381
An adaptation law (integral law or gradient type)

=
(t)
q , q d, q d, q d) ds + (0)
(s, q, q,
where
?
?
?
?
IRm are design parameters

= T IRmm and (0)
is usually diagonal and positive definite (adaptation gain)
is a vectorial function to determine, of dimension m.
(0)
is an arbitrary vector
in practice, we choose it as the best approximation available on .
nez and A. Loria
382
qd
q d
q d
q d , q
d , )
(t, q, q,
ROBOT
qd
q
q
, q d , q d , q
d ) ds
(s, q, q,
0
Figure 98: Adaptive control of robots: block-diagram.

An equivalent representation of the adaptation law
q , q d, q d, q d) .
(t)
= (s, q, q,
It is desirable from a practical viewpoint,
that the control law as well as the adaptation law, do not depend explicitly
.
on q
nez and A. Loria
383
Stability and Convergence of Adaptive Control Systems

An adaptive system guarantees the parametric convergence if
? the limit of (t)

when t exists and is such that
=
lim (t)
? Parametric convergence is not an intrinsic characteristic of an adaptive

controller.
Stability analysis is based on Lyapunov theory,
? with the inclusion of
=
nez and A. Loria
(parametric errors vector)

384
General form of the closed loop equation

q

q = f t, q, q,
q d, q d, q
d,
dt
the origin is an equilibrium point.

In general, is not the only equilibrium point
We study only stability and convergence of the position errors:
limt q (t) = 0 .
Certainty equivalence (the achievement of the control objective under
parameter uncertainty)
nez and A. Loria
385
Ch. 15. PD control with adaptive desired gravity compensation
Ch.15. PD Control with Adaptive Desired Gravity

Compensation
We consider the scenario where all the joints of the robot are revolute.
The Control and Adaptive Laws
IRm and x IRn
Making use of (47), we have that for any vector
= (x, 0, 0, 0)
+ g (x) .
g(x, )
0
(47)
For simplicity, in the sequel we use

g (x) = (x, 0, 0, 0) .
nez and A. Loria
386
The PD control with adaptive desired gravity compensation is described
= Kpq Kv q + g(q d, )
+ g (q ),
= Kpq Kv q + g (q d)
0 d
and
= g (q )T
(t)
d
Z t
0
0
q q
1 + k
qk
ds + (0),
? where Kp, Kv IRnn and IRmm are

symmetric positive definite design matrices
? 0 is a suitable positive constant.
? It was used (47) with x = q d.
nez and A. Loria
387
Design parameters
? Only Kp and 0 must be chosen carefully.
To that end, we start by defining Max{M }, kC1 and kg as
? Max{M (q, )} Max{M }
q IRn, IRm
)k kC1 kqk
? kC(q, q,
q, q IRn, IRm
? kg(x, ) g(y, )k kg kx yk x, y IRn, IRm.

The constants Max{M }, kC1 and kg are considered known.
), g(q, ) and ,
It is necessary to dispose of M (q, ), C(q, q,
but one does not require to know .
In practice the set is determined from upper and lowerbounds on
the dynamic parameters
nez and A. Loria
388
Kp and 0 are chosen so that

C.1)
C.2)
C.3)
C.4)
smin{Kp} > kg ,
2min{Kp}
> 0 ,
2Max{M }
2min{Kv }[min{Kp} kg ]
> 0 ,
2
Max{Kv }
min{Kv }
> 0
2 [kC1 + 2Max{M }]
where 2 and 1 are defined so that

21
2 =
1 2
2min{Kp}
> 1 > 2 .
kg
nez and A. Loria
389
=
.
We define the parametric errors vector as:
It is introduced only for analytical purposes (not used by the controller)
It may be verified that
= g (q )
+ g (q )
g (q d)
d
d
+ g(q , ) g (q ),
= g (q d)
d
0 d
The control law may be written as
+ g(q , ) .
d
Using above control law in the robot model, we obtain
+ g(q , ) g(q, ) .
)q = Kpq Kv q + g (q d)
M (q, )
q + C(q, q,
d
nez and A. Loria
390
= ,
we get that
As
= g (q )T
0
q q .
1 + k
qk
From all the above we have that the closed loop equation is formed by

q
q
n
o

M (q)1 K q K q+
(q
)
C(q,
q)
q+g(q
)
g(q)
p
v
g d
q =
d
dt

g (q d) 1+ 0q q q
k k
Autonomous differential equation (the origin is an equilibrium point).
nez and A. Loria
391
Stability analysis
=
)
V (t, q , q,
P
}|
z
T
q
0 M (q, )
2

K
k
2
1+kq

1
q

2
M (q, )
1+0q M (q, )
k k
0
0
{
q
1 T
+ U(q, ) U(q d, ) + g(q d, ) q + q Kpq
1
{z
}
|
f (
q)
T
nez and A. Loria
392
? Equivalent form
=
, q,
)
V (t, q
1 T
q M (q, )q + U(q, ) U(q d, ) + g(q d, )Tq
2

1
1
0
T
+
+
q TM (q, )q
q Kpq
1 2
1 + k
qk
1 T 1
+ ,
2
nez and A. Loria
393
The constants 0 > 0, 1 > 2 and 2 > 2 are chosen so that

2min{Kp}
> 1 > 2
kg
(48)
21
2 =
1 2
(49)
2min{Kp}
> 0 > 0 .
2Max{M }
(50)
? Condition (48) guarantees that f (

q ) is a positive definite function
? (50) ensures that P is a positive definite matrix
? Finally (49) implies that 11 + 12 = 12 .
nez and A. Loria
394
Define as
= (k
q k) :=
0
.
1 + k
qk
Inequality (50) implies that the matrix

2
Kp
2
nez and A. Loria
0
1 + k
qk
2
2
M (q, ) = Kp 2M (q, ) > 0
2
395
The Lyapunov function candidate may be rewritten as,

1
T
[q +
q ] M (q, ) [q +
q]
2

1 T 2
+ q
Kp 2M (q, ) q
2
2
1 T 1

+
2
=
)
V (t, q , q,
1
+ U(q, ) U(q d, ) + g(q d, )Tq + q TKpq ,
1
{z
}
|
)
f (q
which is obviously a positive definite function since
M (q, ) > 0, 22 Kp 2M (q, ) > 0, > 0, and
f (
q ) is also a positive definite function (since min{Kp} > kg )
nez and A. Loria
396

= q TKv q + q TM (q)q
)
q TKpq +
q TKv q
V (t, q , q,
q
q T [g(q d) g(q)]
q TC(q, q)

q TM (q)q .
It can be upper bounded, getting:

, q,
)
V (t, q
k
qk
kqk
T z

Q
}|
min{Kp} kg
12 Max{Kv }
12 Max{Kv }
1
20 min {Kv }
1
2.
[min{Kv } 20(kC1 + 2Max{M })] kqk
{z
}
2|
nez and A. Loria
397
k
qk
kqk
Q is positive definite if
min{Kp} > kg
2min{Kv }(min{Kp} kg )
2Max{Kv }
> 0
while we have that > 0 if

min{Kv }
> 0 .
2(kC1 + 2Max{M })
nez and A. Loria
398
Finally we obtain that

i
h
0
2
2

2
, q,
)
kqk
min{Q} k
q k + kqk
V (t, q
1 + k
qk
2
2
k
qk
2.
kqk
0min{Q}
1 + k
qk 2
It is a globally negative semidefinite function.
> 0 (globally)
)
Since moreover V (t, q , q,
the origin of the closed loop equation is stable, and
Lm .
its solutions are bounded, that is: q , q Ln and
nez and A. Loria
399
Because:
2
d
k
q
(t)k
(t), q(t),
V (t, q
(t))
0min{Q}
,
dt
1 + k
q (t)k
we have
V0
k
q (t)k
dt ,
1 + k
q (t)k
0min{Q}
0
where V0 := V (0, q (0), q(0),

(0)).
q
Ln2 .
That is: q
k
1+kq
Ln2
Using Lemma A.7 we obtain that: q
, q Ln and q Ln2 , and Lemma A.5 we obtain

Thus, from q
lim q (t) = 0 IRn (control objective achieved).
nez and A. Loria
400
Example 15.2

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

nez and A. Loria
401
Dynamic model:

C12(q, q)
M11(q) M12(q)
C11(q, q)
g (q)
=
q +
q + 1
M21(q) M22(q)
C21(q, q)
g2(q)
C22(q, q)
{z
}
{z
}
|
|
| {z }
)
M (q )
g (q )
C(q ,q
where
M11(q) =
2
m1lc1
m2 l12
2
M12(q) = m2 lc2
+ l1lc2
2
M21(q) = m2 lc2 + l1lc2
2
lc2
+ 2l1lc2 cos(q2) + I1 + I2

cos(q2) + I2

cos(q2) + I2
2
M22(q) = m2lc2
+ I2
C11(q, q)
= m2l1lc2 sin(q2)q2
nez and A. Loria
402
C12(q, q)
= m2l1lc2 sin(q2) [q1 + q2]
= m2l1lc2 sin(q2)q1
C21(q, q)
C22(q, q)
= 0
g2(q) = m2lc2g sin(q1 + q2) .
We consider parametric uncertainty in m2, I2 and lc2;
the numeric values of these constants are not known exactly.
Nevertheless, we assume to know their upper-bounds:
m2, I2 and lc2, that is,
m2 m2;
I2 I2 ;
lc2 lc2 .
The control problem: limt q (t) = 0 (with a constant q d).

nez and A. Loria
403
Dynamic parameters vector IR3,
m2
1
= 2 = m2lc2 .
2
3
m2lc2
+ I2
PD control with adaptive desired gravity compensation.
+ g (q )
0 d

Z t
(t)
= g (q d)T
q q ds + (0)
1 + k
qk
0
? Kp, Kv IRnn and IRmm are symmetric positive definite design

matrices
? 0 is a suitable positive constant.
nez and A. Loria
404
Vector g 0(q d)
g 0(q d) =
m1lc1g sin(qd1)
0
Matrix g (q d)
g (q d) = (q d, 0, 0, 0)

l1g sin(qd1) g sin(qd1 + qd2) 0
.
=
0
g sin(qd1 + qd2) 0
We first need to determine the numeric values of
Max{M (q, )} Max{M }
q IRn,
)k kC1 kqk
kC(q, q,
q, q IRn,
kg(x, ) g(y, )k kg kx yk x, y IRn, .

nez and A. Loria
405
It is necessary to characterize the set IR3, to which , as:

=
x1
2
x2 IR3 : |x1| m2; |x2| m2lc2; |x3| m2lc2 + I2 .
x3
Expressions for Max{M }, kC1 and kg for the case of parametric
uncertainty are:
Max{M }
2
m1lc1
m2 l12
+ 2lc2 + 3lc1lc2 + I1 + I2
kC1 n2m2l1lc2

kg n m1lc1 + m2l1 + m2lc2 g .
nez and A. Loria
406
Fixing the following values for the bounds

m2 = 2.898
I2 = 0.0125
lc2 = 0.02862
[kg]

2
kg m
[m],
and considering the numeric values showed in table 5.1 we finally

obtain:
Max{M } = 0.475
kC1 = 0.086
kg
nez and A. Loria
= 28.99

2
kg m

2
kg m

2
2
kg m /sec .
407
The next step consists on calculating Kp, Kv and 0 and 2.

? Condition C.1: min{Kp} > kg
As, kg = 28.99, hence Kp = diag{kp} = diag{30}.
? Kv is chosen arbitrarily but symmetric positive definite.
We may fix it to Kv = diag{kv } = diag{7, 3}.
? We chose 1 according with
2min{Kp}
> 1 > 2,
kg
so an appropriate value is 1 = 2.01.
? 2 is determined from
21
,
2 =
1 2
so we get 2 = 402 .
nez and A. Loria
408
Using above information, it is immediate to verify that

s
2min{Kp}
2Max{M }
= 0.561
2min{Kv }[min{Kp} kg ]
2Max{Kv }
= 0.124
min{Kv }
2 [kC1 + 2Max{M }]
= 1.448
According to conditions C.2 through C.4, 0 must be strictly smaller

than the previous quantities. Therefore, we choose 0 = 0.12.
must be symmetric positive definite.
A choice is e.g., = diag{1, 2} = diag{500, 10}.
nez and A. Loria
409
The vector of initial adaptive parameters is arbitrary, and here it is taken
to be: (0)
= 0.
In summary, the control law may be written as
1 = kpq1 kv q1 + l1g sin(qd1)1 + g sin(qd1 + qd2)2
+ m1lc1g sin(qd1)
2 = kpq2 kv q2 + g sin(qd1 + qd2)2 .
Notice that the control law does not depend on 3.
nez and A. Loria
410
Consequently, the adaptation law only has the following two components:
Z t
0
q1 q1 ds + 1(0)
1(t) = l1g sin(qd1)
1 + k
qk
0

Z t
0
2(t) = g sin(qd1 + qd2)

q1 q1 ds
1
+
k
q
k
0

Z t
0
q2 q2 ds + 2(0) .
+ g sin(qd1 + qd2)
1 + k
qk
0
Laboratory experimental results.

Initial conditions:
q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0 .
nez and A. Loria
411
Desired joint positions:

qd1 = /10, qd2 = /30
[rad] .
is
0.3141
/10
q (0)
/30 0.1047
[rad] .
=
=
0 0
q(0)
0
0
nez and A. Loria
412
0.4
[rad]
0.3
.....
.... q1
.....
......
......
....
2 .................
.........q
.
............
.................. ...............................
.............................................................................................
..... ..... ..... ..... ..... ..... ..... ..... ..... .....
..........................
........................................
0.2
0.1
0.0
0.1
0.0
12.5
25.0
37.5
50.0
t [sec]

(t) tend
Above Figure shows that the components of the position error q
asymptotically to zero in spite of the non-modeled friction phenomenon.
nez and A. Loria
413

10.0
7.5
5.0
2.5
0.0
2.5
5.0
7.5
10.0
1
3.2902
......................................................................
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
........
................................................................................................................................................................0.1648
............................
.
.
.
.
.
.
.
.
2 ........
...
.....
.
.
.
.
....
...
0.0
12.5
25.0
37.5
50.0
t [sec]
Figure 101: Estimated parameters 1 and 2.

From Figure we appreciate that both parameters tend to values which
are relatively near of the unknown values of 1 and 2, i.e.,

(t)
3.2902
m2
2.0458
lim 1
=
1 =
=
.
0.1648
0.047
t 2(t)
2
m2lc2
In general, the parameters do not converge to their true values (persistency
of excitation is not verified)
nez and A. Loria
414
If instead of limiting the value of 0 we use the same gains as for the
latter controllers, the performance is improved.
0.4
0.3
0.2
0.1
0.0
[rad]
....
... q
... 1
...
....
.....
..
... q .........
..........2
.......... ................
.....
......
..... ..... ..... ..... .........................................................................................................................................................................
0.1
0.0
0.5
1.0
1.5
2.0
t [sec]
nez and A. Loria
415
For this, we set the gains to

Kp =
Kv
30
0
7
0
500
0
0
30
0
3
10
10
[Nm / rad] ,
[Nm sec / rad] ,

[Nm / (rad sec)] ,
and 0 = 5, i.e., Kp and Kv have the same values as for the PD

controllers.
nez and A. Loria
416
10.0
7.5
5.0
2.5
0.0
2.5
5.0
7.5
10.0
.....
........
.. ....
... .... ...

...........................
..................1...................................................2.9053
... ..... ... ............................................
.
.
.
....................................................0.1942
..........................
........................................................................................2
... ..
.....
.
0.0
0.5
1.0
1.5
2.0
t [sec]
Figure 103: Estimated parameters 1 and 2.
nez and A. Loria
417
Ch. 16. PD Control with Adaptive Compensation
Ch.16. PD Control with Adaptive Compensation
The control and adaptation laws

Firstly we recall that the PD controller with compensation is given by
i
h
[q d +
d + q + C(q, q)
= Kpq + Kv q + M (q) q
q ] + g(q),
where Kp, Kv IRnn are symmetric positive definite design matrices,
= q d q denotes the position error,
q
and is defined as
= Kv 1Kp .
nez and A. Loria
418
Now we recall the following property

Parameterization of the dynamic model
M (q, )u + C(q, w, )v + g(q, ) =
(q, u, v, w) + M0(q)u + C0(q, w)v + g 0(q)
where
? (q, u, v, w) IRnm, M0(q) IRnn, C0(q, w) IRnn, g 0(q)
IRn and IRm.
? contains elements that depend on the dynamic parameters.
? M0(q), C0(q, w) and g 0(q) represent parts of
and g(q) that do not depend on .
M (q), C(q, q)
nez and A. Loria
419
IRm, we obtain
For any vector
h
i
=
q + q + C(q, q,
[q +
)
q ] + g(q, )
M (q, )
d
d
h
i
+ M0(q) q + q

q , q)
(q, q d + q , q d +
d
[q d +
q ] + g 0(q) .
+C0(q, q)
(51)
where we defined
u = q d + q
q
v = q d +
w = q .
.
d + q , q d +
q , q)
In the sequel we use the abbreviation: = (q, q
nez and A. Loria
420
We are now ready to study the PD control with adaptive compensation

Control law
h
i
q + q + C(q, q,
[q +
)
= Kpq + Kv q + M (q, )
q]
d
d
+g(q, )
(52)
+ M0(q) q
[q d +
d + q + C0(q, q)
q]
= Kpq + Kv q +
(53)
+g 0(q),
and
=
(t)
T q +
q
ds + (0),
Kp, Kv IRnn and IRmm are symmetric positive definite.

The pass from (52) to (53) follows by using (51).
nez and A. Loria
421
=
.
We define the parametric errors vector as:
Introduced only with analytic purposes (it is not used by the controller).
is unknown since it is a function of .

It may be verified that
=
+
h
i
+ M (q, ) q + q + C(q, q,
) [q d +
=
q ] + g(q, )
d
h
i
[q d +
q ] g 0(q)
M0(q) q d + q C0(q, q)
nez and A. Loria
422
The control law takes the form
= Kpq + Kv q +
i
h
) [q d +
q ] + g(q, ) .
+M (q, ) q d + q + C(q, q,
Now the robot model can be expressed as:
h
i
h
i
.

) q +
M (q, ) q + q + C(q, q,
q = Kpq Kv q
Considering these facts we have that
h
i
T
= q +
q .
nez and A. Loria
423
Finally, the closed-loop equation, may be written as:
q
q
h
h
ii
d
1
) q +
Kpq Kv q C(q, q,
q q
q = M (q, )
dt
h
i
q
T q +
which is a nonautonomous differential equation and of which the origin,

q

q = 0 IR2n+m , is an equilibrium point.

nez and A. Loria
424
Stability analysis

T
2Kp + TM (q, ) TM (q, )

q

1
V (t, q , q , ) = q
M (q, )
M (q, )
0
0

q
0

0
q .

which is positive definite. This, may be more clear when rewriting it as

h
h
iT
i
1
1 T 1
T
Kpq + .
V (t, q , q , ) = q +
q M (q, ) q +
q +q
2
2
nez and A. Loria
425
The time derivative of the Lyapunov function candidate becomes

h
h
h
iT
i
iT
i 1h
+
q +
q M (q, ) q + q + q +
q
q M (q, ) q
2
T 1
T
.
+ 2
q Kpq +
=
V (t, q , q , )
It may be reduced to
= q TKv q q TTKv
V (t, q , q , )
q
TKv is symmetric positive definite (because, is a non-singular

matrix while Kv is a symmetric positive definite matrix, cf. Lemma
2.1)
is a globally negative semidefinite function.
, )
Therefore, V (t, q , q
nez and A. Loria
426
Above result, and since, the Lyapunov function candidate is globally

positive definite, radially unbounded and decrescent,
Theorem 2.3 guarantees that the origin of the closed-loop equation is
uniformly stable, and
all the solutions are bounded, that is,
q , q Ln,
Lm .
nez and A. Loria
427
Because
d
(t), q (t), (t))

V (t, q
q (t)

q (t)TT Kv
dt
q (t)k2
min{T Kv } k
we have
V0
T
min{ Kv }
(0), q (0), (0))

where V0 := (0, q
That is: q Ln2 .
k
q (t)k dt
, q Ln and q Ln2 , and Lemma A.5 we obtain

Thus, from q
lim q (t) = 0 IRn (control objective achieved).
nez and A. Loria
428
Example 16.3

y
g
Link 1
l1
lc1
x
I1 m1
Link 2
m2
I2
q1
q2
lc2
l2

nez and A. Loria
429
Dynamic model:
M11(q) M12(q)
C11(q, q)
g (q)
C12(q, q)
=
q +
q + 1
C22(q, q)
M21(q) M22(q)
C21(q, q)
g2(q)
{z
}
{z
}
|
|
| {z }
)
M (q )
g (q )
C(q ,q
where
M11(q) =
2
m1lc1
m2 l12
2
M12(q) = m2 lc2
+ l1lc2
2
M21(q) = m2 lc2 + l1lc2
2
lc2
+ 2l1lc2 cos(q2) + I1 + I2

cos(q2) + I2

cos(q2) + I2
2
M22(q) = m2lc2
+ I2
nez and A. Loria
430
C11(q, q)
= m2l1lc2 sin(q2)q2
C12(q, q)
= m2l1lc2 sin(q2) [q1 + q2]
= m2l1lc2 sin(q2)q1
C21(q, q)
C22(q, q)
= 0
g2(q) = m2lc2g sin(q1 + q2) .
Unknown parameters: m2, I2 and lc2.

We wish to design a controller, such that: limt q = 0.
We use the PD Control with Adaptive Compensation.
nez and A. Loria
431
Parameterization of the dynamic model:

M (q, )u + C(q, w, )v + g(q, ) =

1
11 12 13
=
2
21 22 23
3
+M0(q)u + C0(q, w)v + g 0(q)
where
11 = l12u1 + l1g sin(q1)
12 = 2l1 cos(q2)u1 + l1 cos(q2)u2 l1 sin(q2)w2v1
l1 sin(q2)[w1 + w2]v2 + g sin(q1 + q2)
13 = u1 + u2
nez and A. Loria
432
21 = 0
22 = l1 cos(q2)u1 + l1 sin(q2)w1v1 + g sin(q1 + q2)
23 = u1 + u2

m2
2.0458
1
= 2 = m2lc2 = 0.047
2
0.0126
3
m2lc2
+ I2

2
m1lc1 + I1 0
M0(q) =
0
0

0 0
C0(q, w) =
0 0

m1lc1g sin(q1)
.
g 0(q) =
0
nez and A. Loria
433
Particularly:
i
) [q d +
q ] + g(q, ) =
M (q, ) q d + q + C(q, q,
h
i
d + q + g 0(q),
+ M0(q) q
where

u1
q + 11q1 + 12q2
u = q d + q =
= d1
u2
qd2 + 21q 1 + 22q 2

v1
qd1 + 11q1 + 12q2
q=
v = q d +
=
v2
qd2 + 21q1 + 22q2

q
w1
= 1 .
w = q =
w2
q2
nez and A. Loria
434
The control law becomes
11
+
= Kpq + Kv q
21
+
2
(m1lc1
12
22

1
13
2
23
3

+ I1)u1
m1lc1g sin(q1)
,
+
0
0
while the adaptation law is
11[v1 q1]
Z t
1(t)
[v
]
+
[v
]
=
ds
+
12
1
1
22
2
2
2(t)
0
13[v1 q1] + 23[v2 q2]
3(t)
nez and A. Loria
1(0)
.
2(0)
3(0)
435
Where,
Kp = diag{200, 150} [N m / rad] ,
= diag{3} [N m sec / rad] ,

2
2
2
2
2
= diag{1.6 kg sec /m , 0.004 kg sec , 0.004 kg m sec },

11 12
:= Kv1Kp = diag{66.6, 50} [1/[sec]].
and therefore, =
21 22
Kv
Initial conditions, are chosen as:

q1(0) = 0, q2(0) = 0
q1(0) = 0, q2(0) = 0
1(0) = 0, 2(0) = 0
3(0) = 0 .
nez and A. Loria
436
0.02
0.01
0.00
0.01
[rad]
......... .... q
... ..... ... ..... 1
. ... . . .
........ ............ ................... .......... ....................... .................... ........................ ..........
.
... .... .. ... .... .. ....... .. .... .... .... ....... .. ... .... .... .. ....
.... ..... ....... ................. ....... .... ........ .... ..... ...... ....... ............ ............... ..... ..... ...... ............ ..... ....
........... ... ...... ..... .... .... ... ........ ........ ... ..... ... ........ .......
... .......... ... ..... .........
......... ........... ......... ................
q2
..
..
0.02
0
10
t [sec]

Figure 105 shows the steady state tracking position errors q (t),
by virtue of friction phenomena in the actual robot, are not zero.
nez and A. Loria
437
3.0
2.5
2.0
1.5
1.0
0.5
0.0
1
.
.
......
.......
.......
..
.......
.....
... ..... ... ..... .... ..... ... ..... ... ..... ...
.... ..
.... ..
.... ..
.... ..
.. ....... ...
....
....
.....
.....
..
...
..
..
...
...
.. ..
.
.. .. ... ... .............................................
.
.
....2
..............................................................................
............. .... ..... ....
3
0
2
4
6
8
10
t [sec]
Figure 106: Estimated parameters 1, 2, and 3

Figure 106 shows the evolution in time of the adaptive parameters.
These parameters were arbitrarily assumed to be zero at the initial instant.
We did not suppose having any knowledge a priori, about .
nez and A. Loria
438
Thanks a lot for your attention!
nez and A. Loria
439
http://www.springer.com/978-1-85233-994-4

SM Slides

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

SM Slides

Cargado por

Copyright:

Formatos disponibles

CONTROL OF ROBOT MANIPULATORS IN

Control of Robot Manipulators in Joint Space

Control of Robot Manipulators in Joint Space

Robots occupy a privileged place in the modernization of numerous industrial

Was introduced by the Czech science fiction writer Karel Capek.

Control of Robot Manipulators in Joint Space

These machines may be roughly classified as follows:

Ground robots Wheeled robots

Control of Robot Manipulators in Joint Space

Control of Robot Manipulators in Joint Space

The International Federation of Robotics (norm ISO/TR 8373) defines:

A manipulating industrial robot is an automatically controlled,

Control of Robot Manipulators in Joint Space

The number of joints of a manipulator determines as well, its number of

Figure 1: Robot manipulator.

Control of Robot Manipulators in Joint Space

In the present textbook:

Control of Robot Manipulators in Joint Space

The torques and forces are collected in the vector: :=

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Ch. 1. What does control of robots

Robot control consists in studying how to make a robot manipulator perform

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Familiarization with the physical system under

Figure 2: Freely moving robot.

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Figure 3: Robot interacting with its environment.

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Figure 4: Robotic system: fixed camera.

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Figure 5: Robotic system: camera in hand.

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Derivation of the dynamic model

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Some interesting topics related with modelling are:

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Definition of control objectives:

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Stability. Consists in the property of a system by which it goes on

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Motion control of robot manipulators

Point-to-point. Determines a series of points in the manipulators

Figure 7: Point-to-point motion specification.

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

(Continuous) trajectory. The control problem consists in making the

Figure 8: Trajectory motion specification.

Control of Robot Manipulators in Joint Space

Ch. 1. What does control of robots consists in?

Control of Robot Manipulators in Joint Space

Ch. 2. Introduction to Lyapunov stability theory