Control of industrial robots

Notes1

Roberto Bochet

December 4, 2021

This work is licensed under a Creative Commons “Attribution-

ShareAlike 4.0 International” license.
This work is realized in LATEX, you can find the source code on
https://github.com/RobertoBochet/cir-notes

1
the whole document is based on [1] and [2]
Bibliography

[1] Paolo Rocco. Slides of the PoliMi course “Control of industirial robots”. https:
//rocco.faculty.polimi.it/cir/index.html. 2020.
[2] Bruno Siciliano et al. Robotica. Modellistica, pianificazione e controllo. it. 3rd ed.
College. McGraw-Hill Education, 2008. isbn: 978-88-386-6322-2.

1
Contents

1 Kinematic redundancy 6
1.1 Inverse kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.1.1 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.1.2 Finding a generic solution . . . . . . . . . . . . . . . . . . . . 7
1.1.3 Finding a solution exploiting redundant . . . . . . . . . . . . . 8
1.1.3.1 Choosing q̇ 0 . . . . . . . . . . . . . . . . . . . . . . . 8
1.1.4 Possible issues with redundant robots . . . . . . . . . . . . . . 9
1.2 Kinematic control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2 Robot dynamics 11
2.1 Model exploiting Euler-Lagrange equation . . . . . . . . . . . . . . . 11
2.1.1 Kinetics energy . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1.2 Potential energy . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.1.3 The Lagrangian . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.1.3.1 The matrix C . . . . . . . . . . . . . . . . . . . . . . 17
2.1.4 The non conservative forces . . . . . . . . . . . . . . . . . . . 17
2.1.5 Skew-symmetry of matrix Ḃ − 2C . . . . . . . . . . . . . . . 18
2.1.6 Summary model . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.1.7 Linearity in dynamic parameters . . . . . . . . . . . . . . . . 20
2.1.7.1 Parameters identification . . . . . . . . . . . . . . . . 20
2.2 Newton-Euler formulation . . . . . . . . . . . . . . . . . . . . . . . . 21
2.2.1 The recursive algorithm . . . . . . . . . . . . . . . . . . . . . 22
2.2.1.1 Optimization . . . . . . . . . . . . . . . . . . . . . . 23
2.3 Euler-Lagrange vs Newton-Euler . . . . . . . . . . . . . . . . . . . . . 23

2
 2.3.1 Computation of direct dynamics . . . . . . . . . . . . . . . . . 23
2.3.2 Computation of inverse dynamics . . . . . . . . . . . . . . . . 24

3 Motion planning 25

4 Decentralized control 26
4.1 Simplified dynamic model . . . . . . . . . . . . . . . . . . . . . . . . 26
4.2 Electrical motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.2.1 Control of the current . . . . . . . . . . . . . . . . . . . . . . 29
4.3 Rigid transmission approximation . . . . . . . . . . . . . . . . . . . . 29
4.3.1 P/PI control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
4.3.1.1 PI speed control design . . . . . . . . . . . . . . . . 31
4.3.1.2 P position control design . . . . . . . . . . . . . . . . 31
4.3.1.3 Speed feed-forward . . . . . . . . . . . . . . . . . . . 32
4.3.1.4 Position measure only . . . . . . . . . . . . . . . . . 32
4.4 Elastic transmission . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

5 Centralized control 34
5.1 Control in joint space . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
5.1.1 Open loop . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
5.1.2 PD + gravity compensation . . . . . . . . . . . . . . . . . . . 35
5.1.3 Inverse dynamics control . . . . . . . . . . . . . . . . . . . . . 37
5.1.3.1 Taking into account the uncertainty . . . . . . . . . 38
5.1.3.2 Adaptive control . . . . . . . . . . . . . . . . . . . . 42
5.2 Control in operational space . . . . . . . . . . . . . . . . . . . . . . . 46
5.2.1 PD + gravity compensation . . . . . . . . . . . . . . . . . . . 46
5.2.2 Inverse dynamics control . . . . . . . . . . . . . . . . . . . . . 48

6 Control of the interaction 50

6.1 Forces in statics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
6.2 The concept of impedance . . . . . . . . . . . . . . . . . . . . . . . . 51
6.2.1 Dynamics model with external forces . . . . . . . . . . . . . . 52
6.2.2 Admittance control . . . . . . . . . . . . . . . . . . . . . . . . 54

3
 6.3 Natural constraints and artificial constraints . . . . . . . . . . . . . . 54
6.4 Explicit control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
6.5 Implicit control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

7 Control with vision sensors 59

7.1 Camera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
7.1.1 Perspective projection . . . . . . . . . . . . . . . . . . . . . . 59
7.1.2 Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
7.1.3 Camera configuration . . . . . . . . . . . . . . . . . . . . . . . 60
7.1.3.1 Eye-to-hand . . . . . . . . . . . . . . . . . . . . . . . 60
7.1.3.2 Eye-in-hand . . . . . . . . . . . . . . . . . . . . . . . 60
7.2 Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
7.2.1 Measure: Position based vs image based . . . . . . . . . . . . 61
7.2.1.1 Position based . . . . . . . . . . . . . . . . . . . . . 61
7.2.1.2 Image based . . . . . . . . . . . . . . . . . . . . . . . 61
7.2.2 Actuation: Dynamic look and move vs visual servoing . . . . . 61
7.2.2.1 Dynamic look and move . . . . . . . . . . . . . . . . 61
7.2.2.2 Visual servoing . . . . . . . . . . . . . . . . . . . . . 61
7.2.3 Image based schema . . . . . . . . . . . . . . . . . . . . . . . 62
7.2.3.1 Interaction matrix . . . . . . . . . . . . . . . . . . . 64
7.2.3.2 Image Jacobian . . . . . . . . . . . . . . . . . . . . . 64

8 Summary 66
8.1 Centralized control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
8.1.1 Control in joint space . . . . . . . . . . . . . . . . . . . . . . . 66
8.1.1.1 PD + gravity compensation . . . . . . . . . . . . . . 66
8.1.1.2 Inverse dynamic control . . . . . . . . . . . . . . . . 66
8.1.2 Control in operational space . . . . . . . . . . . . . . . . . . . 67
8.1.2.1 PD + gravity compensation . . . . . . . . . . . . . . 67
8.1.2.2 Inverse dynamic control . . . . . . . . . . . . . . . . 67
8.2 Control of the interaction . . . . . . . . . . . . . . . . . . . . . . . . 67
8.2.1 Impedance control . . . . . . . . . . . . . . . . . . . . . . . . 67

4
 8.2.2 Admittance control . . . . . . . . . . . . . . . . . . . . . . . . 68

Index 69

5
Chapter 1

Kinematic redundancy

We let x = k(q) where x is the pose of the end effector in the task space, q the
joint configuration. The kinematic function k(·) provides the transformation from
the joint space Rn to the task space Rm .
A robot is kinematically redundant if n > m.

n.b. task space might be a subset of the operational space, so the dimension of the
second one might be less than the first, that is why the redundancy is a relative
concept

1.1 Inverse kinematics

q = k −1 (x)

When a robot is kinematically redundant the inversion of the kinematics gives in-
finite solutions and there are internal motion that not affect the pose of the end
effector.
To solve this issue the problem is usually addressed in the speed domain.

ẋ = J (q)q̇ (1.1)

Where J (q) is the Jacobian of the robot associated to the task space.

1.1.1 Jacobian
For a redundant robot the Jacobian are a rectangular matrix with the shape of
(m × n) where n > m as we saw before.

6
 dim(im(J )) = m dim(ker(J )) = n − m

The null space exists only for a redundant robot.

So we can compose a movement as

q̇ = q̇ ∗ + P q̇ 0 where im(P ) ≡ ker(J )

with P q̇0 that does not contribute to the movement of the end effector, in fact if we
multiply the equation to J from left we get

J q̇ = J q̇ ∗ + J P q̇ 0 = J q̇ ∗ = ẋ

where JP q̇ 0 = 0 for each q̇ 0 .

1.1.2 Finding a generic solution

We can approach to the problem like a optimization one, as first thing we define a
cost function in the joint space

1
g(q̇) = q̇ T W q̇
2
W is a symmetrical weight matrix (n × n) and W > 0
we want to find the optimal solution under the constraint ẋ = W (q)q̇ so we can
use the Lagrange multiplier

1
L(q̇, λ) = q̇ T W q̇ + λT (ẋ − J q̇)
2
 T  T
∂L ∂L
=0 =0
∂ q̇ ∂λ

from these we get

W q̇ − J T λ = 0 ẋ − J q̇ = 0

to combine the two equation we can find λ

ẋ = J W −1 J T λ =⇒ λ = (J W −1 J T )−1 ẋ

and with a recombination with the first equation we get the solution

7
 q̇ = W −1 J T (J W −1 J T )−1 ẋ
special case W = I =⇒ q̇ = J † ẋ

The matrix J † = J T (J J T )−1 is called right pseudo inverse matrix. With dualism,
we can define the matrix J †W = W −1 J T (J W −1 J T )−1 calling weighted pseudo
inverse matrix.

1.1.3 Finding a solution exploiting redundant

As we saw before if q̇ ∗ is a valid solution to inverse problem also q̇ ∗ + P q̇ 0 is it. So,
we can modify the general approach saw above to find a solution that exploiting the
redundant. We consider a new cost function

1
g ′ (q̇) = (q̇ − q̇ 0 )T (q̇ − q̇ 0 )
2
To find a solution under the constraint 1.1 close to q̇ 0 .

1
L′ (q̇, λ) = (q̇ − q̇ 0 )T (q̇ − q̇ 0 ) + λT (ẋ − J q̇)
2
∂L′
From ∂ q̇
= 0 and the constraint 1.1 we can get λ

λ = (J J T )−1 (ẋ − J q̇ 0 )

then we get the solution

q̇ = J † ẋ + (I − J † J )q̇ 0 (1.2)

So we found (I − J † J ) a solution for the matrix P .

1.1.3.1 Choosing q̇ 0

Now we can choose q̇ 0 to improve the behaviour of the robot exploiting its redun-
dancy. A typical choice of q̇ 0 called projected gradient has the shape

 T
∂w(q)
q̇ 0 = k0
∂q

Choosing w(q) as a cost function to maximize to increase a specific performance:

8
 • Manipulability measure to maximize the distance from singularities
q
w(q) = det(J (q)J T (q))

• Distance form joint limits

n  2
1 X qi − q̄i
w(q) =
2n i=1 qiM − q̄im

• Distance from the closet obstacle

w(q) = min∥x(q) − o∥
x,o

k0 > 0 can be arbitrary chosen.

1.1.4 Possible issues with redundant robots

Mainly two possible issues

• A close trajectory in the task space can be mapped as a open trajectory in the
joint space

• Trajectories with the same initial and final task positions may end with dif-
ferent joint configurations

A possible solution to these problems is the method called repeatable or cyclic

The source of the problems above is the non repeatability of the kinematic inversion
methods
A way to solve the kinematic inversion problem is based on the augmented Jaco-
bian, it consists in add an auxiliary task in order to completely bound the kinematic
inversion problem; where the main task is expressed by x = f (q) and the auxiliary
task y = h(q). Now we can introduce an enlarged version of the Jacobian that
taking in account the auxiliary task.
   
ẋ J (q)
ẋaug = = ∂h q̇ = J aug (q)q̇
ẏ ∂q

where J aug is the augmented Jacobian. In this way the inverse kinematics prob-
lem is reduced to

q̇ = J −1
aug (q)ẋaug

9
if now consider y constant we can write

 −1  
J (q) ẋ
q̇ = ∂h
∂q 0

so we introduced additional holonomic constraints to the system, so the augmented

Jacobian make the kinematic inversion repeatable.
The holonomic constraints can be selected as for the conditions for constrained
optimality of a given objective function.

n.b. the inversion of the augmented Jacobian may introduce new singularities
called algorithmic singularities

1.2 Kinematic control

Exploiting the Equation 1.2 we can design a control law in the kinematic space (the
robot dynamics is ignored) for a redundant robot. We can design a control law for
the input ẋ of the inverted kinematic model, a simple control law could be

˙ + K(x̄ − x̂)
ẋ = x̄

where x̂ = k(q). The overall system is

q̇ = J † (x̄
˙ + K(x̄ − x̂)) + I − J † (q)J (q) q̇ 0

the implemented scheme is shown in Figure 1.1.

q̇ 0

˙
x̄
I − J † (·)J (·)

x̄ ẋ q̇ 1
q
K J † (·) s
−

k(·)
x̂

Figure 1.1: Kinematic control for a redundant robot

10
Chapter 2

Robot dynamics

To develop advance system of control and trajectory planning we need a dynamics

representation of the robot, the bound between torques and the robot’s motions.

2.1 Model exploiting Euler-Lagrange equation

We can exploit the Euler-Lagrange equation to derive a model fo the robot’s dy-
namics.

 T  T
d ∂L ∂L
− =ξ L=T −U
dt ∂ q̇ ∂q

where T is the kinetics energy, U is the potential energy and ξ the generalized forces
associated to the vector q.

2.1.1 Kinetics energy

n
X
T = (Tli + Tmi )
i=1

This is the sum of the kinetics contributions of all the robot’s links, in particular Tli
is the contribution from the i-th link and Tmi from the i-th joint motor.
The contribution for the i-th link are defined as
Z
1
Tli = ṗ∗T ∗
i ṗi ρdV (2.1)
2 Vi

where ṗ∗i is the speed of a generic point of the link, and it can be expressed as

11
 ṗ∗i = ṗli + ω i × r i = ṗli + S(ω i )r i (2.2)

pli is the position of the center of mass of the i-th link and r i the vector that define
the position of generic point respect the link’s center of mass.
Z
1
pli = p∗i ρdV (2.3)
mi Vi

Combining Equation 2.1 and Equation 2.2 we get

Z
1
T

Tli = ṗli + S(ω i )r i ṗli + S(ω i )r i ρdV
2 Vi
Z
1
ṗTli ṗli + ṗTli S(ω i )r i + r Ti S T (ω i )ṗli + r Ti S T (ω i )S(ω i )r i ρdV


=
2 Vi
Z Z Z
1 1
= T
ṗ ṗ ρdV + T
ṗli S(ω i )r i ρdV + r Ti S T (ω i )S(ω i )r i ρdV
2 Vi li li Vi 2 Vi

We can identify in this equation three contributes:

Translation Z Z
1 1 1
ṗTli ṗli ρdV = ṗTli ṗli ρdV = mi ṗTli ṗli
2 Vi 2 Vi 2

Mutual
Z Z Z
ṗTli S(ω i )r i ρdV = ṗTli S(ω i ) r i ρdV = ṗTli S(ω i ) (p∗i − pli )ρdV = 0
Vi Vi Vi

Because from Equation 2.3 we can notice that

Z Z Z
p∗i ρdV = pl i ρdV =⇒ (p∗i − pli )ρdV = 0
Vi Vi Vi

Rotational
Z Z 
1 1
r Ti S T (ω i )S(ω i )r i ρdV = ω Ti T
S (r i )S(r i )ρdV ωi
2 Vi 2 Vi

because of S(ω i )r i = −S(r i )ω i .

Let us define

12
 Z
I li = S T (r i )S(r i )ρdV
Vi

we call it inertia tensor referred to the center of mass of the i-th link expressed in
the base frame.
The inertia tensor is a symmetrical matrix 3 × 3 and its components are expressed
as

2
R 2
R R 
(riy + riz )ρdV −
R 2 rix riy ρdV − R rix riz ρdV
2
I li =  ∗ (rix + riz )ρdV R− 2 riy riz2 ρdV

∗ ∗ (rix + riy )ρdV

We have to note that the inertia tensor expressed in the base frame is dependent
by robot configuration, so it would be better if we can express it in the joint frame.
Exploiting the transformation of the rotation speed ω remembering that ω ii = RTi ω i
we can write an equivalent tensor expressed in the joint frame as I i = Ri I ii RTi .
So, we can write the rotation contributes of i-th joint as
Z
1 1 1
r Ti S T (ω i )S(ω i )r i ρdV = ω Ti I i ω i = ω Ti Ri I ii RTi ω i
2 Vi 2 2

Now we can finally write the contribution of the i-th joint to the kinematics energy
as

1 1
Tli = mi ṗTli ṗli + ω Ti Ri I ii RTi ω i
2 2
The velocity can easily write as
" #
  (l )
ṗli J Pi
= (l ) q̇ = J (li ) q̇
ωi J Oi

n.b. J (li ) for a link i include only the contribution of the joints 1, . . . , i, so the column
i + 1, . . . , n are equal to 0
the single non-null column of J (li ) is defined as
" #
 z j−1

 for prismatic joint
 0
" # 

(l ) 
j Pji
(l ) =
j Oij  " #
z × (p − p )


 j−1 li j−1

 for rotational joint
z j−1


13
 1 (l )T (l ) 1 (l )T (l )
Tli = mi q̇ T J P i J P i q̇ + q̇ T J Oi Ri I ii RTi J Oi q̇
2 2
Now we can compute the sum for the kinetics energy (we consider Tmi = 0 for each
motor for simplicity, but include motors contributions is an easy task)

n n n
X 1 1 XX
T = Tli = q̇ T B(q)q̇ = q̇i q̇j bij (q)
i=1
2 2 i=1 j=1

where B(q) is called inertia matrix and it is defined as

n 
X 
(l )T (l ) (l )T (l )
B(q) = mi J P i J P i + J Oi Ri I ii RTi J Oi
i=1

it is symmetrical, > 0 and it depends upon the robot configuration q

2.1.2 Potential energy

Also for the potential energy we compute the total energy as the sum of the contri-
bution of each link

n
X
U= Ui
i=1

The potential energy for a link is given by the gravitational force in accordig to this
formula
Z
Ui = − g T0 p∗i ρdV = −mi g T0 pi
Vi

where g 0 is the gravity acceleration vector expressed in the base frame.

n
X
U =− mi g T0 pi
i=1

2.1.3 The Lagrangian

Now, we have T and U, so we can finally compute the Lagrangian function and
solve the Lagrangian equation

14
 L(q, q̇) = T (q, q̇) − U(q)
n
1 X
= q̇ T B(q)q̇ + mi g T0 pi
2 i=1
n n n
1 XX X
= q̇i q̇j bij (q) + mi g T0 pi (q)
2 i=1 j=1 i=1

Let us consider the Lagrangian equation referring to a generic joint

d ∂L ∂L
− = ξi
dt ∂ q̇i ∂qi
d ∂T (q, q̇) ∂T (q, q̇) ∂U(q)
− + = ξi (2.4)
dt ∂ q̇i ∂qi ∂qi

Let us solve the three parts that compose the Lagrangian equation

n n
!
d ∂T (q, q̇) d ∂ 1 XX
= q̇j q̇k bjk (q)
dt ∂ q̇i dt ∂ q̇i 2 j=1 k=1
n
!
d X
= q̇j bij (q)
dt j=1
n n
X X dbij (q)
= q̈j bij (q) + q̇j
j=1 j=1
dt
n n n
X X X ∂bij (q)
= q̈j bij (q) + q̇j q̇k
j=1 j=1 k=1
∂qk

n n
!
∂T (q, q̇) ∂ 1 XX
= q̇j q̇k bjk (q)
∂qi ∂qi 2 j=1 k=1
n n
1 XX ∂bjk (q)
= q̇j q̇k
2 j=1 k=1 ∂qi

15
 n
!
∂U(q) ∂ X
=− mj g T0 pj
∂qi ∂qi j=1
n
X ∂pj
=− mj g T0
j=1
∂qi
n
(l )
X
=− mj g T0 j Pij (q)
j=1

= gi (q)

So the Lagrange equation 2.4 became

n n n n n
X X X ∂bij (q) 1 X X ∂bjk (q)
q̈j bij (q) + q̇j q̇k − q̇j q̇k + gi (q) = ξi
j=1 j=1 k=1
∂qk 2 j=1 k=1 ∂qi

and if we rearrange the terms we get

n n X
n  
X X ∂bij (q) 1 ∂bjk (q)
q̈j bij (q) + q̇j q̇k − + gi (q) = ξi
j=1 j=1 k=1
∂qk 2 ∂qi

and defined

∂bij (q) 1 ∂bjk (q)

hijk (q) = −
∂qk 2 ∂qi

we get the result

n
X n X
X n
q̈j bij (q) + q̇j q̇k hijk (q) + gi (q) = ξi
j=1 j=1 k=1

where we can recognize

bii : inertial moment as ”seen” from the axis of the i-th joint
bij : effect of the acceleration of j-th joint on the i-th joint
hijj q̇j2 : centrifugal effect induced at i-th joint by the velocity of the j-th joint
hijk q̇j q̇k : Coriolis effect induced at i-th joint by the velocity of the joints j and k

Now, we can write the Lagrangian equations for all the joints in a matrix form as

B(q)q̈ + C(q, q̇)q̇ + g(q) = ξ (2.5)

16
2.1.3.1 The matrix C

The matrix C is not unique, and its elements must satisfy the equation

n
X n X
X n
cij q̇j = hijk q̇j q̇k
j=1 j=1 k=1

A possible definition of C can be found with this process

n
X n X
X n
cij q̇j = hijk q̇j q̇k
j=1 j=1 k=1
n X n  
X ∂bij 1 ∂bjk
= q̇j q̇k −
j=1 k=1
∂qk 2 ∂qi
n n   n n  
1 XX ∂bij 1 XX ∂bik ∂bjk
= q̇j q̇k + q̇j q̇k −
2 j=1 k=1 ∂qk 2 j=1 k=1 ∂qj ∂qi

so the elements of C can be defined as

n
X
cij = cijk q̇k (2.6)
k=1

with  
1 ∂bij ∂bik ∂bjk
cijk = + −
2 ∂qk ∂qj ∂qi

cijk are called Christoffel symbols of the first kind

2.1.4 The non conservative forces

The last thing we have to do is include the forces acting on the system expressed as
ξ in the Equation 2.5.
We can identify tree types of forces:

• joint torques τ

• viscous friction torques −F ν q̇

with F ν a diagonal matrix of viscous friction coefficients

• static friction torques −f s (q, q̇)

with f s function that models the static friction at the joints

17
If we include all these three forces in the Equation 2.5 we get

B(q)q̈ + C(q, q̇)q̇ + F ν q̇ + f s (q, q̇) + g(q) = τ (2.7)

2.1.5 Skew-symmetry of matrix Ḃ − 2C

The derivation of the matrix C seen in the subsubsection 2.1.3.1 allows us to derive
a singularity property of the Equation 2.7.

N (q, q̇) = Ḃ(q) − 2C(q, q̇)

this matrix satisfies the following property

wT N (q, q̇)w = 0

To prove it, we take into account Equation 2.6

n  
1X ∂bij ∂bik ∂bjk
cij = + − q̇k
2 k=1 ∂qk ∂qj ∂qi
n n  
1 X ∂bij dqk 1 X ∂bik ∂bjk
= + − q̇k
2 k=1 ∂qk dt 2 k=1 ∂qj ∂qi
n  
1 1 X ∂bik ∂bjk
= ḃij + − q̇k
2 2 k=1 ∂qj ∂qi

So, we take the generic element

n  
X ∂bik ∂bjk
nij = ḃij − 2cij = − − q̇k
k=1
∂qj ∂qi

So, we can notice that nij = −nji , then the property is demonstrated.
As direct consequence we can state

q̇ T N (q, q̇)q̇ = 0 (2.8)

for any choice of the matrix C.

Let us try to find a physical interpretation to this property, let us consider the
principle of energy conservation

18
 1d T
(q̇ B(q)q̇) = q̇ T (τ − F ν q̇ − f s (q, q̇) − g(q)) (2.9)
2 dt
we consider the left side member, and we try to solve it

1d T 1
(q̇ B(q)q̇) = q̇ T Ḃ(q)q̇ + q̇ T B(q)q̈
2 dt 2
1 T
using 2.7 = q̇ Ḃ(q)q̇ + q̇ T (τ − C(q, q̇)q̇ − F ν q̇ − f s (q, q̇) − g(q))
2
1 T 
= q̇ Ḃ(q) − 2C(q, q̇) q̇ + q̇ T (τ − F ν q̇ − f s (q, q̇) − g(q))
2
1 T
= q̇ N (q, q̇)q̇ + q̇ T (τ − F ν q̇ − f s (q, q̇) − g(q))
2

We can notice that this equation coincides with the Equation 2.9 only if the Equa-
tion 2.8 is satisfied. In conclusion, we can state that Equation 2.8 is satisfied for
each matrix C because it is a direct consequence of the principle of energy con-
servation of the system.

2.1.6 Summary model

So, we summarize how to define a dynamics robot model

B(q)q̈ + C(q, q̇)q̇ + g(q) = τ

let’s try to define a procedure to write the equation’s parameters

1. Compute B(q)
(l ) (l )
1.1. Compute the matrices J P i , J Oi
1.2. Compute B(q) as
n 
X 
(l )T (l ) (l )T (l )
B(q) = mi J P i J P i + J Oi Ri I ii RTi J Oi
i=1

2. Compute C(q, q̇)

2.1. Compute the Christoffel symbols cijk for all combinations of i, j, k in

{1, . . . , n}3 , (remembering cijk = cikj ), as
 
1 ∂bij ∂bik ∂bjk
cijk = + −
2 ∂qk ∂qj ∂qi

19
 2.2. Compute the C(q, q̇) element by element as
n
X
cij = cijk q̇k
k=1

3. (Optionally) As validation step you can check that Ḃ − 2C is antisymmetric

4. Compute g(q) elements with

n
(l )
X
gi (q) = − mj g T0 j Pij (q)
j=1

2.1.7 Linearity in dynamic parameters

If we assume a simplified function for the static friction function

f s (q, q̇) = F s sgn(q̇)

it is possible to demonstrate that the dynamic model of the manipulator is linear

about a suitable set of dynamic parameters. In particular the Lagrangian can be
written in the form

n
X
β TTi − β TUi π i


L=
i=1

where π i is a vector of constant parameters associated to i-th joint (mass, iner-

tia, static friction, . . . ). If we solve the Lagrangian equations with the above
Lagrangian we find that the linearity assumption is not lost, and we can write

τ = Y (q, q̇, q̈)π

where π is the vector get by the concatenation of π i and Y (q, q̇, q̈) is a matrix
called regression matrix.

2.1.7.1 Parameters identification

It may be useful to develop a system to estimate the parameters vector π from

experimental data due to the complexity of compute it via analytic method. Given
a set of measures τ̄ 1 , . . . , τ̄ m and the corresponding matrix Ȳ 1 , . . . , Ȳ m (gotten from
kinematics parameters and joints values q, q̇, q̈, measured or estimated). If we define

20
    
τ̄ 1 Ȳ 1
τ̄ =  ...  , Ȳ =  ... 
   
τ̄ m Ȳ m

we can write

τ̄ = Ȳ π

this equation due to noise measures does not admit solution for π which it can be
estimated with the linear least squares method1 as
 −1
T T †
π̂ = Ȳ Ȳ Ȳ τ̄ = Ȳ τ̄

The trajectories used to generate τ̄ and Ȳ must be enough rich (good conditioning
T
of matrix Ȳ Ȳ ): they need to involve all components in the dynamic model.

2.2 Newton-Euler formulation

An alternative way to derive a dynamics model of a manipulator is exploiting
Newton-Euler equations. As opposed to Lagrange formulation that is based
on balance of energies, this kind of formulation is based on balances of forces and
moments on each link.
We have to solve the equations in a recursive way, propagating the velocity and
acceleration from the base to the end effector and the forces and moments in the
opposite way.
For each joint we can find the following equations

f i − f i+1 + mi g 0 = mi p̈Ci
µi + f i × r i−1,Ci − µi+1 − f i+1 × r i,Ci = I i ω̇i + ωi × I i ωi

To include the generalized forces at joint we can write

We have to define generalized forces at joint, velocity and acceleration for prismatic
and rotational joint

Prismatic
1
https://en.wikipedia.org/wiki/Linear_least_squares

21
 • generalized forces
τ i = f Ti z i−1 + Fvi d˙i + fsi

• velocities

ω i = ω i−1
ṗi = ṗi−1 + d˙i z i−1 + ω i × r i−1,i

• accelerations

ω̇ i = ω̇ i−1
p̈i = p̈i−1 + d¨i z i−1 + 2d˙i ω i × z i−1 + ω̇ i × r i−1,i + ω i × (ω i × r i−1,i )

Rotational

• generalized forces
τ i = µTi z i−1 + Fvi θ̇i + fsi

• velocities

ω i = ω i−1 + θ̇i z i−1

ṗi = ṗi−1 + ω i × r i−1,i

• accelerations

ω̇ i = ω̇ i−1 + θ̈i z i−1 + θ̇i ω i−1 × z i−1

p̈i = p̈i−1 + ω̇ i × r i−1,i + ω i × (ω i × r i−1,i )

The terms Fvi and fsi that appear in the generalized forces are respectively the
coefficient of viscous friction, and the function that characterize the static friction.
For both the joints types the acceleration of the center of mass is given by

p̈Ci = p̈i + ω̇ i × r i,Ci + ω i × (ω i × r i,Ci )

2.2.1 The recursive algorithm

As we said above the Newton-Euler formulation is based on two kind of algorithm
for the term propagation

Forward recursion from the base to the end effector, we propagate the velocities
and accelerations. The initial condition is given by ω o , p̈0 −g 0 , ω̇ 0 , then the elements
ω i , ω̇ i , p̈i , p̈Ci are calculated recursively until the end effector.

22
Backward recursion from the end effector to the base, we propagate the forces
and moments. The initial condition is given by f n+1 = f e , µn+1 = µe , the elements
f i and µi are calculated recursively until the base as

f i = f i+1 + mi p¨Ci
µi = −f i × (r i−1,i + r i,Ci ) + µi+1 + f i+1 × r i,Ci + I i ω̇ i + ω i × I i ω i

2.2.1.1 Optimization

Note that the entities r i−1,i , r i,Ci and I i being referred in the base frame are functions
of the robot’s pose, so they are not constant, so to improve the efficient of the
algorithm is a good idea refer them to the link frame so as to make them constant.
WARNING Missing examples

2.3 Euler-Lagrange vs Newton-Euler

Euler-Lagrange formulation

• It is a systematic and easy to understand.

• The returned formula is in a form in which the physical terms are separated
(inertia matrix, Coriolis, centrifugal and gravitational). All these elements are
useful to design a controller.

• It lends itself to the introduction into the model of more complex effects (like
joint or link deformation).

Newton-Euler formulation

• It is a computational efficient recursive method.

2.3.1 Computation of direct dynamics

In the computation direct dynamics problem given the joint torques τ (t) we have
to calculate the acceleration of the joint q̈(t) and eventually compute via integration
methods also the speeds q(t) and positions q(t). This is essential to simulate the
model.
The Euler-Lagrange formulation can give an easy solution to this problem. We
need to compute and invert this formula

B(q)q̈ + n(q, q̇) = τ

23
where

n(q, q̇) = C(q, q̇)q̇ + g(q) + F ν q̇ + f s (q, q̇)

so, inverting the function we get

q̈ = B −1 (q) (τ − n(q, q̇)) (2.10)

Also exploiting Newton-Euler formulation we can easily find the matrix B(q)
and the vector n(q, q̇) to solve the problem:
Given the implementation of the Newton-Euler script τ = N E(q, q̇, q̈), set q̈ = 0
and use the current values of q and q̇ the computed τ is exactly the searched vector
n(q, q̇).
Then we set g 0 = 0 into the script and q̇ = 0 in order to eliminate gravitation,
Coriolis, centrifugal and friction effects. With n interaction of the algorith we can
compute the matrix B(q) column by column imposing q̈i = 1 and q̈j = 0, i ̸= j.
So we can use always the Equation 2.10 with the computed parameters.

2.3.2 Computation of inverse dynamics

In the inverse dynamics problem the joint accelerations q̈(t), speeds q̇(t) and
position q(t) are given, and we have to compute the law of τ (t) to produce that
motion. This bound is useful in the trajectory planning and model based
control.
An approach based on Newton-Euler formulation can be used to solve this prob-
lem efficiently.

24
Chapter 3

Motion planning

25
Chapter 4

Decentralized control

Once a desired robot trajectory is designed, the next step is to guarantee that the
real trajectory as close as possible to it. Practically we have to design a control
strategy in order to q d (t) ≈ q(t).
The main way adopted in the control of industrial robot is the independent joint
control approach; it is a decentralized control approach where each joint is con-
trolled independently of each other.
WARNING Missing evaluation of control performance

4.1 Simplified dynamic model

Let us consider a simplified dynamics model of the robots

B(q)q̈ + C(q, q̇)q̇ + g(q) = τ (4.1)

In this model τ , q, q̇, q̈ are referred to the joints, but in a real system we cannot act
directly on the joints variables, instead we have to consider that we can act only
on the motors that are bound to the joints via a transmission. So we introduce a
dynamics model for a generic rigid transmission and joint motor

Jmi q̈mi + Dmi q̇mi = τmi − τlmi

τlmi = τli /ni

where

26
 Jmi : moment of inertial of the motor
Dmi : friction viscous coefficient of the motor
τlmi : load torque on the axis of the motor
τli : load torque on the axis of the joint
ni : transmission reduction rate between motor and joint

to simplify we considered just the effect related to its spinning around its own axis
and a rigid transmission.
Let us introduce the square matrix N = diag{ni } so we can write

qm = N q τ lm = N −1 τ

Before we consider the entire model seen from the motor point of view, we observe
that the matrix B(q) can be seen as the sum of two contributes, the first of average
inertia and the other of the residual as function of the pose q

B(q) = B̄ + ∆B(q)

Now we can combine the models of motors/transmissions and robots

J m q̈ m + D m q̇ m = τ m − τ lm
−1
J m q̈ m + D m q̇ m + N (B(q)q̈ + C(q, q̇)q̇ + g(q)) = τ m

replace the joints variables with motors ones

J m q̈ m + D m q̇ m + N −1 B(q)N −1 q̈ m +
N −1 C(q, q̇)N −1 q̇ m + N −1 g(q) = τ m

introduce the decomposition of B(q)

J m q̈ m + D m q̇ m + N −1 B̄ + ∆B(q) N −1 q̈ m +

N −1 C(q, q̇)N −1 q̇ m + N −1 g(q) = τ m

(J m + N −1 B̄N −1 )q̈ m + D m q̇ m + N −1 ∆B(q)N −1 q̈ m +
N −1 C(q, q̇)N −1 q̇ m + N −1 g(q) = τ m

introduce the vector d and the matrix B̄ r = N −1 B̄N −1

J m + B̄ r q̈ m + D m q̇ m + d = τ m

27
where

d = N −1 ∆B(q)N −1 q̈ m + N −1 C(q, q̇)N −1 q̇ m + N −1 g(q) (4.2)

We got a dynamics model where the motor torques τ m are the input, and the vector
d is a non-linear function that we can treat as noise, you can see it in the Figure 4.1.

n.b. all the non-linearities are moved in the noise d.

n.b. the bigger are the reduction ratios ni , the less relevant the noise d is.
So, the decentralized control approach treat each joint as a SISO linear model subject
to a noise. The control loop for each joint is independent of each others.
This method heavily relies on the assumption that the reduction ratios ni values
are large (generally true for industrial robotics manipulators), to can ignore the
nonlinear contribution d in the controller design.

N −1 g(q)

N −1 C(q, q̇)N −1

N −1 ∆B(q)N −1

d
τm −
−1 q̈ m 1
q̇ m 1
qm
J m + B̄ r s s
−

Dm

Figure 4.1: motor-transmission-joint dynamics model

n.b. the arrows for the dependencies of noise functions are been omitted

4.2 Electrical motor

Let’s consider a simple DC motor

28
 V (t) = Ri(t) + Li̇(t) + E(t)
E(t) = K q̇m (t)
τm (t) = Ki(t) = Jm q̈m (t)

E
V − i τm 1
q̇m qm
1 1
sL+R K Jm s s

Figure 4.2: DC motor model

4.2.1 Control of the current

We can close a high frequency (thousands rad /s) loop for the current as it can be
seen in Figure 4.3. E is considered to change as a slowly varying disturbance, that
the controller can effectively reject. From the point of view of the mechanical system
the current loop is so fast to consider the current change is instantaneous. So for
the mechanical control we can consider to control directly the motor torque

τm (t) = Ki(t) ≈ K ī(t)

E
ī − i τm 1
q̇m qm
1 1
Ri (s) sL+R K Jm s s
−

Figure 4.3: motor current control

4.3 Rigid transmission approximation

As we saw above, for a rigid transmission we have the following system

29
 Jm q̈m + dm q̇m = τm − τlm
Jl q̈l = nτlm − τl
qm = nql

The first and the second equation can be merged exploiting the third equation
 
Jl τl
Jm + 2 q̈m + dm q̇m = τm −
n n

The result model can be seen in the Figure 4.4

1 1/dm
Gv (s) = Jl

= Jm +Jl /n2
dm + Jm + n2
s 1+ dm
s

dm is an uncertain small parameter, since dm give a real stable pole we can consider
the most conservative situation in which dm = 0, so we get

1
Gv (s) =
Jm + nJ2l s

τl
1/n

τm − q̇m 1
qm
Gv (s) s

Figure 4.4: Mechanical system with rigid transmission

4.3.1 P/PI control

In order to control position of the motor we can close in cascade two loop (also to
the current loop). The first with a PI regulator on the motor speed and the second
with an P regulator on the motor position as you can see in Figure 4.5.

n.b. in Figure 4.5 the current loop is omitted because, as we said before, the current
loop has a much higher bandwidth than the speed loop, so we can assume τ̄m = τm .

30
 τl
1/n

q̄m τm − q̇m 1
qm
RP (s) RP I (s) Gv (s) s
− −

Figure 4.5: Control scheme P/PI with a rigid transmission

4.3.1.1 PI speed control design

As we stated the P/PI design is done with cascade approach, so as first step we can
design the speed loop.

1 + tiv s kpv 1 + tiv s

RP I (s) = kpv =⇒ Lv (s) = Jl


s Jm + n2 s2

We impose an ωcv with kpv

kpv
ωcv =
Jm + nJ2l

and to guarantee that the ωcv desired coincide with the effective one we have to put
the zero in low frequency respect it

1
tiv ≈
(0.1 ÷ 0.3)ωcv

With this regulator we get a close loop function for the speed loop

1
Fv (s) ≈ 1
1+ ωcv
s

4.3.1.2 P position control design

We can now design the RP (s) controller taking in account the speed closed loop
designed above.

1
RP (s) = kpp =⇒ Lp (s) = kpp  
s 1 + ω1cv s

if we choose ωcp ≪ ωcv is enough to impose kpp = ωcp

31
The overall closed loop on the position guarantee null static error and a bandwidth
of ωcp .

4.3.1.3 Speed feed-forward

A possible improvement of the P/PI scheme is the speed feed-forward scheme Fig-
ure 4.6. The block kf f s improves the speed of the response of the position control.
The kf f coefficient is chosen between 0 and 1.

τl
kf f s
1/n

q̄m τm − q̇m 1
qm
RP (s) RP I (s) Gv (s) s
− −

Figure 4.6: Control scheme P/PI with a rigid transmission and speed feed-forward

4.3.1.4 Position measure only

If only available the position measure but not the speed, it can be derived with a
process of differentiation on the position. The control scheme became as Figure 4.7
that are equivalent to Figure 4.8 where all the controller block are substituted with
a single PID on position loop.

τl
s
1/n

q̄m τm − q̇m 1
qm
RP (s) RP I (s) Gv (s) s
−
−
s

Figure 4.7: Control scheme with only position measurement

32
 τl
1/n

q̄m τm − q̇m 1
qm
RP ID (s) Gv (s) s
−

Figure 4.8: Control scheme PID with only position measurement

4.4 Elastic transmission

If we look only to the rigid transmission model we do not find significant limitations
in the bandwidth of speed control, but from experimental data we can clearly find
several limitations mainly bound to vibration, we have to improve the rigid model
to take in account these limitations.
If we consider an elastic transmission we can write the following system

Jm q̈m + dm q̇m = τm − τlm

Jl q̈l = nτlm − τl
τlm = kel (qm − nql ) + del (q̇m − nq̇l )

The Gv (s) now is composed of one real pole, two complex poles and two complex
zeros; in particular we can find the parameter ωz of the complex zeros as
s
kel
ωz =
Jl /n2

and a reasonable choice for the speed closed loop bandwidth is ωcv ≈ 0.7ωz

33
Chapter 5

Centralized control

As opposed of the decentralized approach where each joint is controlled indepen-

dently of each others, we can implement a control law based on centralized ap-
proach where the manipulator is controlled exploiting its overall model. This ap-
proach required obviously a model of the robot, but it generally guarantees better
performance than the decentralized one.

n.b. as we saw in section 4.1 for a decentralized control we stated that a high
reduction ratios in transmissions between motors and joints is a fundamental re-
quirement because this reduces the magnitude of the noise d, if this decoupling effect
is not guaranteed we must use the centralized approach.
The centralized control approach allows us to develop several control schemes
both in the joint space that in the operational one.

5.1 Control in joint space

In joint space control the design of the controller is done directly on the joints state.

5.1.1 Open loop

A first naive approach can be seen as an extension of a decentralized controller
designed in the previous chapter.
The idea is to compensate the noise d adding to the decentralized scheme an open
˙ ¨q̄ . Yuo can see the scheme
loop controller fed with the desired joints functions q̄, q̄,
in Figure 5.1.
The function of the feedforward controller can be designed from Equation 4.2 using
as input the desired state

34
 d̂ = N −1 ∆B(q̄)N −1 ¨q̄ m + N −1 C(q̄, q̄)N
˙ −1 ˙
q̄ m + N −1 g(q̄)

The calculation of d̂ is generally computationally expensive, so it is preferred to

precalculate it offline d̂ if it is possible (i.e in the repeated trajectories).

N −1 g(q)

¨q̄ m N −1 C(q, q̇)N −1

q̄˙ m Centralized
feedforward
action
N −1 ∆B(q)N −1

d̂ d
q̄ m Decentralized τm −
−1 q̈ m 1
q̇ m 1
qm
J m + B̄ r s s
controller
− −

Dm

Figure 5.1: open loop feedforward d compensation

5.1.2 PD + gravity compensation

Let us consider again the simplified dynamics model Equation 4.1 and we try to
define a control law based on it to hold the given pose q̄. To design a proper control
law we will exploit the Lyapunov method Let us start defining the error function
as

q̃(t) = q̄ − q(t)

and the Lyapunov function

1 1
V (q̃, q̇) = q̇ T B(q)q̇ + q̃ T K P q̃
2 2
with K P > 0 and symmetrical.

35
 1
V̇ (q̇, q̃) = q̇ T B(q)q̈ + q̇ T Ḃ(q)q̇ − q̇ T K P q̃
2
1
using 4.1 = q̇ T (τ − C(q, q̇)q̇ − g(q)) + q̇ T Ḃ(q)q̇ − q̇ T K P q̃
2
1 T 
= q̇ Ḃ(q) − 2C(q, q̇) q̇ + q̇ T (τ − g(q) − K P q̃)
2
as we saw in subsection 2.1.5 the first term is equal to zero

= q̇ T (τ − g(q) − K P q̃)

if we impose the control law

τ = g(q) + K P q̃ − K D q̇ (5.1)

with K D > 0 and symmetrical, so V̇ became

V̇ (q̇, q̃) = −q̇ T K D q̇

the function V̇ (q̇, q̃) is semi-definite negative, but not definite negative (see the
possible state q̇ = 0, q̃ ∈ R), so we have to study furthermore the asymptotically
stability of the equilibrium q̇ = 0. We consider the dynamic system from Equa-
tion 4.1 at the equilibrium q̇ = 0 with the control law from Equation 5.1

B(q)q̈ + C(q, q̇)q̇ + g(q) = g(q) + K P q̃ − K D q̇

and we get

K P q̃ = 0 =⇒ q̃ = 0

then for the Krasowski - La Salle Lemma we can state that the system with
the control law from Equation 5.1 is globally asyntotically stable with the unique
equilibrium in q = q̄. This result is valid only if gravity contribution g(q) is perfectly
compensated from the designed control law.
You can see the implemented control system in the Figure 5.2.

n.b. with this kind of regulator we do not have control on the time history with
which the error q̃(t) goes to zero, so this way is not practicable if we want track a
trajectory

36
 KD

− q̇
q̄ τ
KP Robot q
−

g(·)

Figure 5.2: Closed loop with PD and gravity compensation

5.1.3 Inverse dynamics control

Let us try to solve the problem of the trajectory tracking. we rewrite the Equa-
tion 4.1 in the form

B(q)q̈ + n(q, q̇) = τ

with

n(q, q̇) = C(q, q̇)q̇ + g(q)

If B(q) is full rank for each configuration of the manipulator we can define the
control law called inverse dynamics

τ = B(q)y + n(q, q̇) (5.2)

so, if the system knowledge is perfect, this law impose the dynamics q̈ = y; from
the extern the overall system appears as a double integrator.
Now we can design a control law for the function y, a valid choice can be a decou-
pled PD controller

y = K P q̃ + K I q̃˙ + ¨q̄ (5.3)

that imposes the error dynamics

¨q̃ + K D q̃˙ + K P q̃ = 0

then, the error q̃ is characterized by a second order dynamics that can be arbitrarily
assigned by suitable choice of the parameters of the diagonal matrices K P and K I .
This allows us to use this technique to track an assigned trajectory.

37
You can see the implementation of this control in Figure 5.3.

n.b. this technique requires perfect knowledge of the dynamic model, which might be
difficult in practice

¨q̄

q̇
q̄˙ q̃˙ y τ
KD B(·) Robot
− q
q̄ q̃
KP
− n(·, ·)

Figure 5.3: Closed loop with inverse dynamics control

5.1.3.1 Taking into account the uncertainty

As we saw in above the inverse dynamics control required perfect knowledge of the
dynamics model of the robot, this is unrealistic in real robots, so we have to take
into account the model uncertainty.
Let us consider a more realist control law

τ = B̂(q)y + n̂(q, q̇)

so the compensated system become

B(q)q̈ + n(q, q̇) = B̂(q)y + n̂(q, q̇)

Let us define the uncertainty as

B̃(q) = B̂(q) − B(q), ñ(q, q̇) = n̂(q, q̇) − n(q, q̇)

still under the assumption that B(q) is invertible, for q̈ we can write

q̈ = y − η

where
 
η = I − B −1 B̂ y − B −1 ñ(q, q̇)

38
If we adopt the same control law we saw before in Equation 5.3, the error dynamic
become

¨q̃ + K D q̃˙ + K P q̃ = η

the system still nonlinear, so we have to add in addiction to the PD controller a

nonlinear term, function of the error to improve the robustness of the final system.
Let us define the second order derivative of the error

¨q̃ = ¨q̄ − q̈ = ¨q̄ − y + η

we define a new system whose state are the errors

q̃ T
 
ξ = ˙T
q̃

and the dynamics is defined by

ξ̇ = Hξ + D (¨q̄ − y + η)

with
  
0 I 0
H= ∈ R2n×2n , D= ∈ R2n×n
0 0 I

Before we go any further, we need to characterize the uncertainty with three as-
sumptions

• supt≥0 ∥¨q̄ ∥ < QM < ∞ ∀¨q̄

This assumption requires that the required acceleration is not infinite. It is
obviously always verified, because a planned trajectory will never require an
unlimited acceleration.

• ∥I − B −1 B̂∥ ≤ α < 1 ∀q
A matrix B is definite positive and it is lower and upper bound, so the following
equation is always valid

0 < Bm ≤ ∥B −1 ∥ ≤ BM < ∞

then, we can impose

2
B̂ = I
BM + Bm

39
 that always satisfy the assumption
BM − Bm
∥I − B −1 B̂∥ ≤ <1
BM + Bm

n.b. B̂ = B =⇒ α = 0

• ∥ñ∥ ≤ Φ(∥ξ∥) < ∞ ∀q, q̇

We can choose the form

Φ(∥ξ∥) = α0 + α1 ∥ξ∥ + α2 ∥ξ∥2

We add a term to the control law Equation 5.3

y = K P q̃ + K I q̃˙ + ¨q̄ + w

now we consider the error dynamics with this control law

ξ̇ = Hξ + D η − K P q̃ − K I q̃˙ − w

= H̃ξ + D (η − w)
 
defining K = K P K D with
 
0 I
H̃ = (H − DK) =
−K P K D

n.b. all the eigenvalues of H̃ are negative

Now, we need to design a control law for w, and to do this we will use the Lyapunov
method. Consider the following Lyapunov function candidate

V (ξ) = ξ T Qξ > 0, ∀ξ ÷ 0

where Q is symmetric positive definite matrix.

T
V̇ = ξ̇ Qξ + ξ T Qξ̇
T
= (ξ T H̃ + (η − w)T D T )Qξ + ξ T Q(H̃ξ + D (η − w))
 T 
= ξ T H̃ Q + QH̃ ξ + (η − w)T D T Qξ + ξ T QD (η − w)

40
transposing the second element because it is a scalar
 T 
= ξ T H̃ Q + QH̃ ξ + 2ξ T QD (η − w)
T
set z = D T Qξ and H̃ Q + QH̃ = −P

= −ξ T P ξ + 2z T (η − w)

n.b. because all eigenvalues of H̃ are negative for any P positive definite symmetrical
matrix also the solution Q is a positive definite symmetrical matrix
Let us analyze the derivative Lyapunov function, the first element −ξ T P ξ is always
negative, so we need to analyze the second term. If ξ ∈ ker(D T Q), then z = 0, so
the system is asymptotically stable, in the other case we have to define a control
function w that make the second term negative. From

z
w=ρ
∥z∥
we get

zTz
 
T T z
z (η − w) = z η − ρ = zTη − ρ ≤ ∥z∥∥η∥ − ρ∥z∥ = ∥z∥(∥η∥ − ρ)
∥z∥ ∥z∥

so, if ρ > ∥η∥ the globally asymptotically stability for the system is guaranteed.
Looking for define a function ρ exploiting the assumptions

∥η∥ ≤ ∥I − B −1 B̂∥(∥¨q̄ ∥ + ∥K∥∥ξ∥ + ∥w∥ + ∥B −1 ∥∥ñ∥)

≤ αQM + α∥K∥∥ξ∥ + αρ + BM Φ(∥ξ∥) < ρ

so

1
ρ≥ (αQM + α∥K∥∥ξ∥ + BM Φ(∥ξ∥))
1−α
under the assumption that Φ(∥ξ∥) has the form α0 + α1 ∥ξ∥ + α2 ∥ξ∥2 we can chose

ρ(∥ξ∥) = β0 + β1 ∥ξ∥ + β2 ∥ξ∥2

with

αQM + α0 BM α∥K∥ + α1 BM α2 BM
β0 ≥ , β1 ≥ , β2 ≥
1−α 1−α 1−α

41
this control law guarantees a globally asymptotically stability of the system.
So the overall control law is composed by three terms

• B̂(q)y + n̂(q, q̇)

approximately compensated for the nonlinear terms

• K P q̃ + K D q̃˙ + ¨q̄
stabilizes the nominal dynamic system in the error

• w = ρ(∥ξ∥) ∥z∥
z

gives robustness, counteracting the uncertainty

A further improvement can be done, in order to avoid high frequency switching of

the control variable, called chattering, the third term can be changed to
(
z
ρ(∥ξ∥) ∥z∥ ∥z∥ ≥ ϵ
w=
ρ(∥ξ∥) zϵ ∥z∥ < ϵ

z z
ρ(∥ξ∥) ∥z∥
DT Q

¨q̄

w
q̇
q̄˙ q̃˙ y τ
KD B̂(·) Robot
− q
q̄ q̃
KP
− n̂(·, ·)

Figure 5.4: Closed loop with inverse dynamics robust control

5.1.3.2 Adaptive control

In additional to the inverse dynamics robust control (subsubsection 5.1.3.1) we can

consider an adaptive control to compensate the uncertain dynamic parameters. As
we saw in subsection 2.1.7 the dynamic model can write as

B(q)q̈ + C(q, q̇)q̇ + g(q) = Y (q, q̇, q̈)π = τ

42
where π is a suitable constant vector of uncertain dynamic parameters.
Let us consider the control law

τ = B(q)q̈ r + C(q, q̇)q̇ r + g(q) + K D σ

where

q̇ r = q̄˙ + Λq̃ q̈ r = ¨q̄ + Λq̃˙

with Λ symmetrical positive define matrix (usually diagonal). If we impose

σ = q̇ r − q̇ = q̃˙ + Λq̃

we get as overall system

B(q)q̈ + C(q, q̇)q̇ + g(q) = B(q)q̈ r + C(q, q̇)q̇ r + g(q) + K D σ

B(q)q̈ + C(q, q̇)q̇ = B(q)q̈ r + C(q, q̇)q̇ r + K D σ
B(q) (q̈ − q̈ r ) + C(q, q̇) (q̇ − q̇ r ) = K D σ
−B(q)σ̇ − C(q, q̇)σ = K D σ
B(q)σ̇ + C(q, q̇)σ + K D σ = 0 (5.4)

So let us check the stability of this system using the Lyapunov function

1 1
V (σ, q̃) = σ T B(q)σ + q̃ T M q̃
2 2
M is (as always) a positive definite matrix, so the derivative is

1
V̇ = σ T B(q)σ̇ + σ T Ḃ(q)σ + q̃ T M q̃˙
2
using B(q)σ̇ from Equation 5.4
1
= σ T (−C(q, q̇) − K D ) σ + σ T Ḃ(q)σ + q̃ T M q̃˙
2
1  
= −σ T K D σ + σ T Ḃ(q) − 2C(q, q̇) σ + q̃ T M q̃˙
2
remembering the property of the matrix Ḃ(q) − 2C(q, q̇) seen in subsection 2.1.5
= −σ T K D σ + q̃ T M q̃˙
= −(q̃˙ + Λq̃)T K D (q̃˙ + Λq̃) + q̃ T M q̃˙
= −q̃˙ T K D q̃˙ − q̃ T ΛK D Λq̃ − 2q̃ T ΛK D q̃˙ + q̃ T M q̃˙

43
choosing M = 2ΛK D

= −q̃˙ T K D q̃˙ − q̃ T ΛK D Λq̃

V̇ is negative define thus the system is globally asymptotically stable with the equi-
librium q̃ = 0, q̃˙ = 0
Now let us consider a control law where the parameters are estimated and exploiting
the formulation with the parametric vector we saw above

τ = B̂(q)q̈ r + Ĉ(q, q̇)q̇ r + ĝ(q) + K D σ

= Y (q, q̇, q̇ r , q̈ r )π̂ + K D σ

the system dynamic model Equation 5.4 become

B(q)σ̇ + C(q, q̇)σ + K D σ = −B̃(q)q̈ r − C̃(q, q̇)q̇ r − g̃(q)

= −Y (q, q̇, q̇ r , q̈ r )π̃

where B̃, C̃, g̃ are the residual error given by the estimate of the model parameters,
and thus the Y (q, q̇, q̇ r , q̈ r )π̃ can be seen as the model residual error.
So consider a modified version of the Lyapunov function saw before

1 1
V (σ, q̃, π̃) = σ T B(q)σ + q̃ T ΛK D q̃ + π̃ T K π π̃
2 2
where M is already substituted and K π is a positive define matrix

1
V̇ = σ T B(q)σ̇ + σ T Ḃ(q)σ + 2q̃ T ΛK D q̃˙ + π̃ T K π π̃
˙
2
using B(q)σ̇ from uncertain dynamics model
1
= σ T (−C(q, q̇) − K D ) σ − σ T Y (q, q̇, q̇ r , q̈ r )π̃ + σ T Ḃ(q)σ
2
+ 2q̃ T ΛK D q̃˙ + π̃ T K π π̃
˙
1  
= −σ T K D σ + σ T Ḃ(q) − 2C(q, q̇) σ − σ T Y (q, q̇, q̇ r , q̈ r )π̃
2
+ 2q̃ T ΛK D q̃˙ + π̃ T K π π̃
˙
= −σ T K D σ − σ T Y (q, q̇, q̇ r , q̈ r )π̃ + 2q̃ T ΛK D q̃˙ + π̃ T K π π̃
˙
= −σ T K D σ + 2q̃ T ΛK D q̃˙ + π̃ T (K π π̃
˙ − Y T (q, q̇, q̇ r , q̈ r )σ)

44
using σ

= −(q̃˙ + Λq̃)T K D (q̃˙ + Λq̃) + 2q̃ T ΛK D q̃˙ + π̃ T (K π π̃

˙ − Y T (q, q̇, q̇ r , q̈ r )σ)
= −q̃˙ T K D q̃˙ − q̃ T ΛK D Λq̃ − 2q̃ T ΛK D q̃˙ + 2q̃ T ΛK D q̃˙
˙ − Y T (q, q̇, q̇ r , q̈ r )σ)
+ π̃ T (K π π̃
= −q̃˙ T K D q̃˙ − q̃ T ΛK D Λq̃ + π̃ T (K π π̃
˙ − Y T (q, q̇, q̇ r , q̈ r )σ)

˙ = K −1 Y T (q, q̇, q̇ r , q̈ r )σ = π̃
if we update the parameters’ estimate as π̂ ˙
π

= −q̃˙ T K D q̃˙ − q̃ T ΛK D Λq̃

V̇ < 0 thus, the system is globally asymptotically stable with the equilibrium q̃ =
0, q̃˙ = 0
So, in summary the overall control law (which you can see in the Figure 5.5) is
described by the system

˙ = K −1 Y T (q, q̇, q̇ , q̈ )(q̃˙ + Λq̃)

π̂ π r r

τ = Y (q, q̇, q̇ r , q̈ r )π̂ + K D (q̃˙ + Λq̃)

in which we can identify three terms

• Y (q, q̇, q̇ r , q̈ r )π̂

can be interpreted as an approximate inverse dynamic control

• K D (q̃˙ + Λq̃)
has the behaviour of a PD acting on the tracking error

• K −1 T ˙
π Y (q, q̇, q̇ r , q̈ r )(q̃ + Λq̃)
update the estimate of parameters in according to a gradient technique. The
matrix K π defines the speed of the convergence of the estimate to its asymp-
totic value.

n.b. asymptotically we will not get π̂ → π, but only Y (q, q̇, q̇ r , q̈ r )(π̂ − π) → 0
In conclusion the adaptive control offers worse performance than the robust
control in presence of model errors, but this first guarantees a more regular control
action than the robust control, which are characterized by potentially dangerous
high frequency switching.

45
 ¨q̄ ¨q̃ r

q̄˙ q̃˙
Λ
q̇
− q̇ r σ ˙
π̂ π̂ τ
1
K −1
π Y
T
s Y Robot
−
q̄ q̃ q
Λ
−
KD

Figure 5.5: Adaptive control scheme

5.2 Control in operational space

In opposite to the control in the joints space, we can implement the control directly
in the operational space (a Cartesian space).
As first difference by the control in the joints space, we will not apply the kine-
matic inversion, but the measures are actually the result of the direct kinematics
computations based (generally) on the joint measures.

5.2.1 PD + gravity compensation

In a cartesian space we can define the error as

x̃ = x̄ − x

Let us consider a Lyapunov function

1 1
V (x̃, q̇) = q̇ T B(q)q̇ + x̃T K P x̃
2 2
with K P symmetric positive definite matrix

1 ˙ T K P x̃
V̇ = q̇ T B(q)q̈ + q̇ T Ḃ(q)q̇ + x̃
2
˙ = ẋ = −J A (q)q̇
exploiting the analytic Jacobian we have x̃
1
= q̇ T B(q)q̈ + q̇ T Ḃ(q)q̇ − q̇ T J TA (q)K P x̃
2

46
replacing Bq̈ with the Equation 4.1
1
= q̇ T (τ − C(q, q̇)q̇ − g(q)) + q̇ T Ḃ(q)q̇ − q̇ T J TA (q)K P x̃
2
1 T 
= q̇ Ḃ(q) − 2C(q, q̇) q̇ + q̇ T τ − g(q) − J TA (q)K P x̃


2
with the property we saw in subsection 2.1.5 we can eliminate the first term

= q̇ T τ − g(q) − J TA (q)K P x̃

consider the control law τ = g(q) + J TA (q)K P x̃ − J TA (q)K D J A (q)q̇

= q̇ T g(q) + J TA (q)K P x̃ − J TA (q)K D J A (q)q̇ − g(q) − J TA (q)K P x̃

= −q̇ T J TA (q)K D J A (q)q̇

we can state V̇ ≤ 0 (V̇ = 0 for q̇ = 0, ∀x̃ ∈ R) so let us check the system trajectory.
The dynamics model (Equation 4.1) with the control law become

B(q)q̈ + C(q, q̇)q̇ = J TA (q)K P x̃ − J TA (q)K D J A (q)q̇

evaluated in the equilibrium q̇ = 0, q̈ = 0

J TA (q)K P x̃ = 0

so, if the Jacobian is full rank the only solution is x̃ = 0, thus the system is globally
asymptotically stable with the equilibrium x̄ = x. You can see the corresponding
scheme in Figure 5.6

KD J A (·)
ẋ

− q̇
x̄ x̃ τ
KP JT
A (·) Robot
− q

g(·)

k(·)
x̂

Figure 5.6: PD and gravity compensation in the operational space

47
5.2.2 Inverse dynamics control
To tracking a trajectory the PD + gravity compesation is not usable (it is
˙ = 0), so we have to implement a different control system, in
constrained by x̄
particular we can adapt the inverse dynamics control designed to the control in
the joints space saw in subsection 5.1.3.
As we saw if we impose the control law in Equation 5.2 we get that q̈ = y, so we
have to design a new input y in such a way to allow the trackinf of the trajectory
x̄(t). A way to do this is notice that

d d
ẍ = ẋ = (J A (q)q̇) = J A (q)q̈ + J̇ A (q, q̇)q̇
dt dt
so we can invert the function and get
 
−1
q̈ = J A (q) ẍ − J̇ A (q, q̇)q̇

this suggests that a possible control law for y can be

 
y = J −1
A (q) ¨
x̄ + K D
˙
x̃ + K P x̃ − J̇ A (q, q̇)q̇ (5.5)

so the overall control law become

 
τ = B(q)J −1
A (q) ¨
x̄ + K D
˙
x̃ + K P x̃ − J̇ A (q, q̇)q̇ + n(q, q̇)

Let us find the error dynamics beginning from the dynamic model subject the above
control law

 
B(q)q̈ + n(q, q̇) = B(q)J −1 ¨ ˙
x̄ + K D x̃ + K P x̃ − J̇ A (q, q̇)q̇ + n(q, q̇)
A (q)
 
B(q)q̈ = B(q)J −1
A (q) ¨
x̄ + K D
˙
x̃ + K P x̃ − J̇ A (q, q̇)q̇
˙ + K P x̃ − J̇ A (q, q̇)q̇ − J A (q)q̈
¨ + K D x̃
0 = x̄

substitute q̈
˙ + K P x̃ − J̇ A (q, q̇)q̇ − ẍ − J̇ A (q, q̇)q̇
¨ + K D x̃
0 = x̄
˙ + K P x̃ − ẍ
¨ + K D x̃
0 = x̄

¨ = x̄
recognize x̃ ¨ − ẍ

¨ + K D x̃
0 = x̃ ˙ + K P x̃

48
So the dynamics of the error in the operative space is characterized by a second order
function with K P , K D arbitrarly assigned. You can see the schema in Figure 5.7.

n.b. to compute the control law we have to invert the Jacobian J −1

A , thus this method
can not been used for the control of the redundant robots or for robots in singular
configurations

¨
x̄

q̇
˙
x̄ ˙
x̃ y τ
KD J −1
A (·) B(·) Robot
− − q
x̄ x̃
KP
n(·, ·)
−
J̇ A (·, ·)
˙
x̂
J A (·)
x̂
k(·)

Figure 5.7: Inverse dynamics control in the operational space

49
Chapter 6

Control of the interaction

In this chapter we will analyze how the robot interact with the environment, and
we will develop techniques to perform this task based on the control of the forces
acting on the robot (generally on the end effector).
This kind of study is essential for the robots will interact with humans (collaborative
robots).

Passive control of compliance In some tasks to the robots are requested a

certain degree of flexibility in contrast to the request of the trajectory requested is
strictly followed (x(t) = x̄(t)), for example in an assembly task where it is required
to insert an element in a hole (the alignment of end effector with the hole might
be not perfect), in this case the problem can be bypassed equipping the robot with
an Remote Center of Compliance1 . The RCC placed between the robot’s wrist
and the gripper introduces a form of compliance for the axis perpendicular to the
approach one, correcting misalignment in a passive way.
More flexibility behaviour can be gotten with an active control.

Forces measurements The measurements of forces and moments are provided

by forces sensors that return the measurements of these alone the three axis bound
on a local frame (generally in the proximity of the end effector).
The forces sensors are generally based on strain gauges2 (a device that changes its
conductance in function of strain). The strain gauges are suitable mounted in way
to allow the measure of the six component of forces and moments.
1
https://en.wikipedia.org/wiki/Remote_Center_Compliance
2
https://en.wikipedia.org/wiki/Strain_gauge

50
6.1 Forces in statics
Let us start from the study of the robots’ statics subjected to forces (and moments)
acting on the end-effector. We will use the principle of the virtual works. For the
torques joints we find the contribution as

dWτ = τ T dq

and for the forces on the end effector

dWγ = f T dpe + µT ω e dt

with f and µ are respectively the resulting force and moment act on the end effector.
Exploiting the geometrical Jacobian we can write dWγ as a function of q

dWγ = f T J P (q)dq + µT J O (q)dq = γ T J (q)dq

T
where the resulting force and moment are written in the vector γ = f T µT .


The elementary movement and the virtual movement coincide, so we can write

δWτ = τ T δq
δWγ = γ T J (q)δq

The robot is in a static state if δWτ = δWγ , that is satisfied with

τ = J T (q)γ

n.b. the static relation shows a duality with the robot kinetics that can be written in
the form v e = J (q)q̇ (kineto-static duality)

n.b. if γ ∈ ker(J T ), then it does not require any joint torque to balance γ

6.2 The concept of impedance

With a generalized approach we can state that for a dynamical system a power
flow tent to change a generalized effort. We can easily see this behaviour in an
electrical system, where the flow is the current and the effort is the voltage, the
relation between these two measure is expressed by the impedance. In a mechanical
system we can find this duality with force (flow) and the position (effort). So we
can define the mechanical impedance and design a control system based on it.

51
Let us consider a 1 dof mass (M ) on which acting two forces (u, f ) (where a is the
mass’ acceleration)

Ma = u + f

we introduce a couple spring-dumper acting on the mass through u

u = −kd v − Ke p

where we indicted with v, p respectively the mass’ speed and position. So the system
become

M a + kd v + K e p = f

So we defined a relation between the force and the position (and its derivation) in
a mass-spring-damper system; we will call this relation mechanical impedance.

6.2.1 Dynamics model with external forces

Let us try to extend the concept of mechanical impedance to the robots control.
We consider the dynamical system with a γ force (and moment) acting on the end
effector

B(q)q̈ + C(q, q̇)q̇ + g(q) = τ − J T (q)γ

and we consider the control law based on the inverse dynamics

τ = B(q)y + C(q, q̇)q̇ + g(q)

if we substitute it we get

q̈ = y − B −1 (q)J T (q)γ

now, assume for y the control law we saw for the inverse dynamic control in operative
space (Equation 5.5), for the closed loop we get

 
q̈ = J −1
A (q)
¨ ˙
x̄ + K D x̃ + K P x̃ − J̇ A (q, q̇)q̇ − B −1 (q)J T (q)γ
˙ + K P x̃ − J A (q)B −1 (q)J T (q)γ
¨ + K D x̃
J A (q)q̈ + J̇ A (q, q̇)q̇ = x̄

52
recognizing J A q̈ + J̇ A (q, q̇)q̇ = ẍ
˙ + K P x̃ − J A (q)B −1 (q)J T (q)γ
¨ + K D x̃
ẍ = x̄
¨ + K D x̃
x̃ ˙ + K P x̃ = J A (q)B −1 (q)J T (q)γ

defining J T (q)γ = J TA (q)γ A

¨ + K D x̃
x̃ ˙ + K P x̃ = J A (q)B −1 (q)J TA (q)γ A

and introducing B A (q) = J −T −1

A (q)B(q)J A (q)

¨ + K D x̃
x̃ ˙ + K P x̃ = B −1
A (q)γ A

we get an impedance relation, which is coupled and only partially assignable. But
if we have also the forces’ measurement we can change the control laws to exploit
them

τ = B(q)y + C(q, q̇)q̇ + g(q) + J T (q)γ

 
−1 −1 ¨ ˙
y = J A (q)M d M d x̄ + D d x̃ + K d x̃ − M d J̇ A (q, q̇)q̇ − γ A

with M d , D d , K d diagonal positive definite matrices; the closed loop become

¨ + D d x̃
M d x̃ ˙ + K d x̃ = γ A

so, a completely decoupled system.

This form defines a mechanical impedance between the forces ( and moments)
and the position error in the operational space.

n.b. the mechanical impedance have the same shape of a mass-spring-damper

system with M d mass, D d damping and K d stiffness
Unluckily this method impose several constraints:

• it requires a complete knowledge of the dynamic model, it does not guarantee

a compensation of the model errors

• it requires complete access to the robot, the control law is design directly on
joints torques

• the system become inherently compliant to external disturbances, which is

conflicting with the typical stiffness required to the industrial robots

53
6.2.2 Admittance control
Defining the admittance filter function (the inverse of the impedance)

1
H(s) =
Md s2 + Dd s + K d
we can make the control system in Figure 6.1. In which

1
x̄ − x ≈ f
Md s2 + D d s + K d ext

H(s)

f ext

x̄ High gain x
motion Robot
− controller

Figure 6.1: Admittance control

6.3 Natural constraints and artificial constraints

It is generally impossible to assign simultaneously an arbitrary position and force
to a system, but in some task a control both on position and force is required, to
handle this requirement we define a new concept based on constraints.
In the 3d space (with reference to a frame) we can identify 6 dof for the generalized
movement velocity, 3 for the lineal speed (vx , vy , vz ) and 3 for the rotational speed
(ωx , ωy , ωz ) to which we can associate 6 generalized forces (or moments), 3 linear
forces (fx , fy , fz ) and 3 torques (µx , µy , µz ).
So, we can define two kinds of constraints on this 12 variables:
We will call natural constraints ones that impose a limitation on a generalized
movement velocity and forces, these are the constraints imposed by the environment.
e.g. a rigid plane impose a limit on the linear speed vn in the direction defines by the
plane normal, instead the absence of an obstacle in the generic direction of vector
unit u (that is opposed to the motion) can not allow the imposition of the associated
linear force fu .
We will call artificial constraints ones we can impose to the system arbitrarily
e.g. a pipe in a hole allows to impose an artificial constraints on the rotation ve-

54
locity ωl of the pipe in the hole (where l is vector unit in the direction of the pipe
longitudinal axis)
It is evident that the artificial and natural constraints are complementary, and they
suggest the control approach where the desire functions are defined only for the
artificial constraints, while the state subject to the natural constraints are free
to evolve.
So we can define the useful diagonal matrix Σ (6 × 6) with only zeros or ones as
elements called selection matrix.
The Σ matrix is defined to weight the error on the reference for controlled forces
the element (i,i) of the matrix is 1 for the others 0.
An approximate scheme of the control based on this approach can see in Figure 6.2, it
is called hybrid force/position control and it is characterized by a high flexibility.
It is possible identify some inconsistencies can appear due to nominal model used:

• a detected force in nominally free direction caused by a friction at the contact

• a detected movement speed in nominally constrained direction caused by com-

pliance in the robot structure or contact

• uncertainty in the environment geometry at the contact

The first two are automatically filtered by the select matrix, the third can be miti-
gated by real time estimation process.

Σ
γ̂

γ̄ Force
Σ
control
τ

˙
x̄ Motion
I −Σ
control

I −Σ
˙
x̂

Figure 6.2: Hybrid force/position control

55
6.4 Explicit control
Consider a 1dof system with a mass m subject to 2 forces f, u subjected to the
relation (where x is the position of the mass)

w + u = mẍ

if we introduce a spring with law −kx on w we get

−kx + u = mẍ

in the Laplace form

1
x(s) = u(s)
ms2 +k
so if we evaluated the reaction force on the other side f of the spring we can get the
relation

k
f (s) = u(s)
ms2 +k
an equivalent block diagram can be seen in the Figure 6.3

−
u 1 x f
ms2 k

Figure 6.3: Spring-mass-environment interaction equivalent block diagram

F (s)
The transfers function U (s)
can be also written as

k
m ωn2
Gf (s) = k
=
s2 + m s2 + ωn2
p
highlighting it presents 2 imaginary poles in ±i k/m and for k → ∞ then Gf (s) →
1
So we can close a loop around the force to regulate it (Figure 6.4)
a good choice for Rf (s) is a pure integrator

Kif
Rf (s) =
s
so we can directly assign ωcf changing Kif ≈ ωcf if ωcf < ωn ; with this choice the
resonant frequency ωn is out of the system bandwidth.

56
 −
f¯ u 1 x f
Rf (s) ms2 k
−

Figure 6.4: Explicit force control block diagram

6.5 Implicit control

Now let us start to analyze the same system saw for the explicit control when a
control loop on position is already closed (Figure 6.5).

−
x̄ u x f
R(s) G(s) k
−

Figure 6.5: Closed loop on position

The corresponding closed loop is

f (s) kR(s)G(s)
=
x̄(s) 1 + kG(s) + R(s)G(s)
f (s)
which for infinitely contact stiffness (k → ∞) become x̄(s)
= R(s) and f ≈ u

x̄ u f
R(s) ≈1

Figure 6.6: Closed loop on position with infinite contact stiffness

now, let us try to close a loop on the force with another controller. In particular,
we can design a regulator composed by two action, one to compensate the position
regulator influence (C(s)), and another one to impose the desired dynamics to f
(Rf (s)), you can see this system in Figure 6.7.
Under the hypothesis that the position regulator is a PID

1 kD s2 + kP s + kI
R(s) = kP + kI + kD s =
s s
we can design the compensator as

1
C(s) =
kD s2 + kP s + kI

57
 f¯ x̄ u f
Rf (s) C(s) R(s) ≈1
−

Figure 6.7: Closed loop on position with infinite contact stiffness

so the open loop function become (Figure 6.8)

1
Lf (s) = Rf (s)C(s)R(s) = Rf (s)
s

f¯ 1
f
Rf (s) s
−

Figure 6.8: Compensated implicit force control

We can take Rf (s) as PI controller

1
Rf (s) = kPf + kIf
s
so finally the open loop function become

skPf + kIf
Lf (s) =
s2
the ωcf can be set as ωcf ≈ kPf

58
Chapter 7

Control with vision sensors

7.1 Camera
A camera performs a 2D projection of a 3D space, this projection involves an in-
formation loss. To determinate coordinates of a 3D point from the 2D projection
additional information are needed.

7.1.1 Perspective projection

If we identify a 3D point in the space with the tuple (X, Y, Z) and the corresponding
point in the projection space with the tuple (x, y) we can identify the following
relations

λ λ
x= X y= Y (7.1)
Z Z
Where λ is the focal length(in pixel) of the camera.

n.b. the above relations is valid only if 3D space and projection space share the same
reference frame

7.1.2 Calibration
Before using the camera its need to be calibrated; we need to find two kinds of
parameters

• Intrinsic parameters
The parameters that characterize the camera as the focal length, and some
kind of aberration in the lens that have to be considered in computer vision

59
 task. All these parameters are fixed for the camera, and it is not necessary to
recalculate when the task changes.

• Extrinsic parameters
All the other parameters used to map the 3D point into the projective space,
like camera’s orientation and position respect the global reference frame. If
the camera is moved the extrinsic parameters have to be recalculated.

7.1.3 Camera configuration

There are two main camera configuration in vision control of manipulator

7.1.3.1 Eye-to-hand

The camera is fixed to the global reference frame, pointed toward the task space.

• Advantages

– The movements of the robot does not affect the camera field of view
– The geometric relation between projective space and the 3D space does
not change

• Disadvantages

– The movements of the robot may occlude the camera field of view

7.1.3.2 Eye-in-hand

The camera is attached to the robot, usually on the robot wrist.

• Advantages

– The camera field of view are fixed on the end effector

– The camera field of view is never occluded by the robots

• Disadvantages

– The geometric relation between projective space and the 3D space changed
with the robot movements
– The camera field of view continuously change even for small movements

60
7.2 Control
The main control schemes based on computer vision are given by the combination
of two measure methods and two actuation approaches.

7.2.1 Measure: Position based vs image based

The camera image can be use in two-way to produce measurements for the control

7.2.1.1 Position based

From the camera images a partial 3D representation of the task space is recreated
exploiting technics of computer vision. Algorithms to estimate the pose are com-
putably intense and sensitive to errors in camera calibration.

7.2.1.2 Image based

No transformation between projective space of the camera image to the real 3D

worlds is attempted, the image is directly used to provide the measurements to
the control system. The error for the controller is defined on a quantities directly
measurable from the image.

7.2.2 Actuation: Dynamic look and move vs visual servoing

The action control based on the visual can be applied to different level of the control
stack

7.2.2.1 Dynamic look and move

The visual information are used at high level of the control; the speed control is not
based on visual data and only the position control exploiting the visual measure-
ments. The camera can work at low framerate without compromising the overall
performance of the position control system. Generally, in industrial application is
the only way to go because the only accessible control variable on the robot is the
position set point.

7.2.2.2 Visual servoing

The visual information are used also at the low level control; so, the access to the
actuators input of the robots is necessary to exploiting the visual control to achieve

61
 x̄d Cartesian Axis
Actuators Robot
controller controller
−

Image
Pose
feature
x̂ estimator ξ video
extraction

Figure 7.1: Dynamic look and move position based

ξ̄ d Image
Axis
space Actuators Robot
controller
− controller

Image
feature
ξ extraction video

Figure 7.2: Dynamic look and move image based

high control performances. It is also required a high framerate to achieve high

performances.

7.2.3 Image based schema

Design an image based control is a critical task; we need to relate motion of the
camera with motion of the features in the image.
Let us consider a fixed point P in the space and the camera frame c, we can state

P w = Ow w c
c (t) + Rc (t)P (t)

differentiating in time

x̄d Cartesian
Actuators Robot
controller
−

Image
Pose
feature
x̂ estimator ξ video
extraction

Figure 7.3: Visual servoing position based

62
 ξ̄ d Image
space Actuators Robot
− controller

Image
feature
ξ extraction video

Figure 7.4: Visual servoing image based

w w c
0 = Ȯ c + Ṙc P c + Rw
c Ṗ
c
and find the solution for Ṗ

c c c
Ṗ = −Ȯ c − Ṙc P c

remembering that ṘP = ω × P we get

c c
Ṗ = −Ȯ c − ω cc × P c

where all the vector are expressed in the camera frame.

Define
     
X ωx Ȯx
c c c
P = Y
 ω c = ωy 
 Ȯ c = Ȯy 

Z ωz Ȯz

we can write

Ẋ = Y ωz − Zωy − Ȯx
Ẏ = Zωx − Xωz − Ȯy
Ż = Xωy − Y ωx − Ȯz

remembering Equation 7.1 we get

x
Ẋ = Zωz − Zωy − Ȯx
λ
y
Ẏ = Zωx − Zωz − Ȯy
λ
x y
Ż = Zωy − Zωx − Ȯz
λ λ

63
Differentiating in time projective coordinate from Equation 7.1

dX ẊZ − X Ż
ẋ = λ =λ
dt Z Z2
dY Ẏ Z − Y Ż
ẏ = λ =λ
dt Z Z2

now we can finally write the relation between motion of the camera to the motion
of the image’s features
   c
ẋ Ȯ c
= L(λ, x, y, Z)
ẏ ω cc

where
" 2 2
#
xy
− Zλ 0 x
Z λ
− λ +x
λ
y
L(λ, x, y, Z) = y λ +y 2
2
0 − Zλ Z λ
− xy
λ
−x

and it is called interaction matrix.

7.2.3.1 Interaction matrix

The interaction matrix is a 2 × 6 matrix, it depends on variable values x, y and

Z. If it decomposed
 
ẋ c
= LO (λ, x, y, Z)Ȯ c + Lω (λ, x, y)ω cc
ẏ

we can see that the rotation part does not depend on depth Z.
Due to the shape of it, we can state that exist an associated null space with dimension
of 4, so there are four types of camera movement that not produce any change of
the feature in the projective image.

7.2.3.2 Image Jacobian

If we try to bind camera speed we write

 c
Ȯ c c
c = T w (q)J (q)q̇
ωc

where T cw (q) is the coordinate transformation between world and camera frame, and
J (q) is the robot geometrical Jacobian. So, we can write

64
  
ẋ
= L(λ, x, y, Z)T cw (q)J (q)q̇ = J I (λ, x, y, Z, q)q̇
ẏ

where J I is the image Jacobian.

The Z parameter is clearly not available, but it can be estimated based on apriori
information.
Now the image Jacobian can be used to design the control law in the image based
scheme.
   
q̇ = J †I ˙ξd + k(ξd − ξ)x + I − J † J I q̇
I 0

65
Chapter 8

Summary

8.1 Centralized control

8.1.1 Control in joint space

8.1.1.1 PD + gravity compensation

Control law
τ = g(q) + K P q̃ − K D q̇
Lyapunov’s function
1 1
V (q̃, q̇) = q̇ T B(q)q̇ + q̃ T K P q̃
2 2
Closed loop
B(q)q̈ + C(q, q̇)q̇ = K P q̃ − K D q̇
Block diagram in Figure 5.2

8.1.1.2 Inverse dynamic control

Control law

τ = B(q)y + n(q, q̇)

y = K P q̃ + K I q̃˙ + ¨q̄

Open loop
q̈ = y
Closed loop
¨q̃ + K D q̃˙ + K P q̃ = 0
Block diagram in Figure 5.3

66
8.1.2 Control in operational space
8.1.2.1 PD + gravity compensation

Control law
τ = g(q) + J TA (q)K P x̃ − J TA (q)K D J A (q)q̇
Lyapunov’s function
1 1
V (x̃, q̇) = q̇ T B(q)q̇ + x̃T K P x̃
2 2
Closed loop
B(q)q̈ + C(q, q̇)q̇ = J TA (q)K P x̃ − J TA (q)K D J A (q)q̇
Block diagram in Figure 5.6

8.1.2.2 Inverse dynamic control

Control law
τ = B(q)y + n(q, q̇)
 
y = J −1
A (q) ¨
x̄ + K D
˙
x̃ + K P x̃ − J̇ A (q, q̇)q̇

Open loop
q̈ = y
Closed loop
¨ + K D x̃
x̃ ˙ + K P x̃ = 0
Block diagram in Figure 5.7

n.b. needs inversion of J A , then it cannot be used with redundant robots

8.2 Control of the interaction

8.2.1 Impedance control

System
B(q)q̈ + C(q, q̇)q̇ + g(q) = τ − J T (q)γ
Control law
τ = B(q)y + C(q, q̇)q̇ + g(q) + J T (q)γ
 
y = J −1
A (q)M −1
M d ¨
x̄ + D d
˙
x̃ + K d x̃ − M J̇
d A (q, q̇)q̇ − γ A
d

Closed loop
¨ + D d x̃
M d x̃ ˙ + K d x̃ = γ A

67
8.2.2 Admittance control
Dynamics
1
x̄ − x ≈ f
Md s2 + D d s + K d ext
Block diagram in Figure 6.1

n.b. requires a high bandwidth position controller

68
Index

algorithmic singularities, 10 Lyapunov method, 35

analytic Jacobian, 46
augmented Jacobian, 9 model
uncertainty, 38
centralized control approach, 34
chattering, 42 Newton-Euler equations, 21
Christoffel symbols, 17 Newton-Euler script, 24
decoupled PD controller, 37
principle of energy conservation, 18
direct dynamics, 23
independent joint control, 26 regression matrix, 20
inertia Remote Center of Compliance, 50
matrix, 14
tensor, 13 selection matrix, 55
inverse dynamics, 24, 37 strain gauges, 50

kineto-static duality, 51 uncertain dynamic parameters, 42

69