Foundations of Robotics: 2018-I

Control of Motion
Prof. Oscar E. Ramos, Ph.D.
 Temas

1. General Concepts

2. Control in the Joint Space

- PD Control
- PD Control + Gravity Compensation
- Inverse Dynamics Control

3. Control in the Operational Space

- Control of Robots -
Prof. Oscar E. Ramos, Ph.D. 2
 Dynamic Model in the State Space

• Dynamics of a Robot:
  C (q, q )q  g(q)  u
M (q)q u  τ tot
Control signal
- State variables:

 x1  q   x 1  q 
x   x      
 x 2  q   x 2  q
 

- Equations in the state space

 x 1   x2   0 
 x     M 1 (x )  C (x , x )x  g(x )     M 1 (x )  u
 2  1 1 2 2 1   1 

f ( x) G(x1 )

x  f (x)  G(x1 )u Affine system with

respect to the
State space equation control

3
 Stability of Dynamic Systems
Types of Stability (for x=0)

• Stability (in the sense of Lyapunov)

- Idea: the system is always within a region R, if it

starts close to the equilibrium point x = 0
r
R x0 - More formally:

R  0, r  0,‖x(0)‖ r ‖x(t )‖ R, t  0

• Asymptotic stability
- If the system starts sufficiently close to the
equilibrium point, it will go back to the equilibrium
point
ro
x0
- More formally:
1. It is stable in the sense of Lyapunov
2. r0  0,‖x (0)‖ r0  x(t )  0 when t  

Note: we consider that x = 0 is an equilibrium point 4

 Control Methods

• According to the control variable

- Position control
- Velocity control
- Torque control

• According to the coupling level

- Decentralized control (single-joint)
• Each joint is separately controlled
• It can implement position, velocity or torque control
- Centralized control (multiple-joints)
• It considers all the dynamic model of the robot
• It typically implements torque control

• According to the result of the control

- Motion control
- Force control
- Hybrid control (motion+force)
5
 Control Methods

• Decentralized control (single-joint)

- It considers n independent systems (joints): SISO control
- It uses the dynamic model of each joint (motor + transmission) separately

q1d q1
Control Joint 1

q2d q2
Control Joint 2
Kinematics

qd
Inverse

xd q

qnd qn
Control Joint n

- Coupling effects: they are considered as noise

6
 Control Methods

• Decentralized control (single-joint)

- Kinematic control using: “resolved motion rate control”
Δq1 q1
Control Joint 1

Δq2 q2
Control Joint 2
xd Δx Δq
J# q
x

Δqn qn
Control Joint n

Forward
Kinematics

- There are other forms of kinematic control

• Integrate ̇ to obtain q and enter it as reference for each controller
7
 Control Methods

• Centralized control (multiple-joints)

- It considers all the joints at the same time (considering joint coupling)
- It uses the complete model of the robot:
  C (q, q )q  g(q)  u
M (q)q
- It can be specified in:
a) Joint space
– The control loop uses joint variables
– The input to the controller is based on q

b) Operational space
– The control loop uses operational space variables (position, orientation, etc.)
– The input to the controller is based on x

8
 Temas

1. General Concepts

2. Control in the Joint Space

- PD Control
- PD Control + Gravity Compensation
- Inverse Dynamics Control

3. Control in the Operational Space

- Control of Robots -
Prof. Oscar E. Ramos, Ph.D. 9
 Control in the Joint Space
Introduction

• It is a centralized control method (multiple-joint)

- It considers all the joints simultaneously

• According to the control objective:

- Set point control: qd(t) = qd
- Trajectory following control: time-varying qd(t)

• Usual methods:
- PD (Proportional-derivative)
- PD + Gravity compensation
- Inverse dynamics (computed torque)

10
 Control in the Joint Space
Set Point Control

• Objective:
- Find torques such that lim q(t )  q d
t 

• Notation:
- Joint errors: e(t )  q d  q(t )

• Regulation problem: lim e(t )  0

t 

• General control law:

u  f (q, q , q d , q d , q
 d , M (q), C (q, q ), g (q))

Controller u
Robot

11
 Control in the Joint Space
PD Control

• Dynamic model of a robot:

  C (q, q )q  g(q)  u
M (q)q n degrees of freedom

- State variables:
e  Joint position error: e = qd - q
x 
q  Joint velocities

• Objective: reach qd with q d  0

• Control law (PD):

u  K p (q d  q)  K d (q d  q ) K p , K d   n n
Positive definite
u  K p (q d  q)  K d q

- Note: Kp, Kd are usually diagonal matrices

12
 Control in the Joint Space
PD Control

• Example: RR planar robot (2 dof)

- Dynamic model:   C (q, q )q  g (q)  u

M (q)q
ŷ

m m12   hq h(q1  q2 ) 

M (q)   11  C (q, q )   2 
 m21 m22   hq1 0  lc2
l1 q2

g(q)  
 1

 m1lc  m2l1 c1  m2lc c12 
2
g
lc1
q1
 m2lc2 c12  x̂

 
m11  m1lc21  m2 l12  2l1lc2 c2  lc22  I zz1  I zz2 m22  m2lc22  I zz2
m12  m21  m l l
2 1 c2 2 
c  lc22  I zz2 h  m2l1lc2 s2

- PD control:
 k p1 0   q1d  q1   kd 1 0   q1 
u  Open: controldin.m
0 k p 2   q2 d  q2   0 kd 2   q2 

13
 Control in the Joint Space
PD Control

• Example: RR planar robot (2 dof)

- Desired value: qd = [0.5, 0.5]
- Use PD control with: kp1 = kp2 y kd1 = kd2
- Find adequate gain values

- Is there a steady-state error?

- What is the value in steady state?
• What expression defines the steady-state value? (Hint: analyze the closed-loop
model in space state)

14
 (Equilibrium Points)

• Equilibrium point:
- Given a system x  h(x, u)
- The equilibrium points lead to: x  h(x, u)  0

• For a robot:
- Dynamic model:
x  f (x)  G(x)u

q   q   0   x1  q 
q
     M 1  Cq  g     M 1  u x  
       x 2  q 

- (forced) Equilibrium points satisfy:

q  0
x  f (x)  G(x)u  0
u(q, q )  g (q)

- All the equilibrium points have a null velocity

- At equilibrium, the joint torques must balance gravity
15
 Control in the Joint Space
PD Control

• Example: RR planar robot (2 dof)

- Desired value: qd = [0.5, 0.5]
- Use PD control with: kp1 = kp2 y kd1 = kd2
- Find adequate gain values

- Is there a steady-state error?

- What is the value in steady-state?
• What expression defines the steady-state value? (Hint: analyze the closed-loop
model in state space)
q  q d  K p1g(q)

- Remove gravity from the model, ¿what is the steady state error?

• In general, PD control can be used for models where no gravity is

considered
16
 Control in the Joint Space
PD Control + Gravity Compensation

• Dynamic model of a robot:

  C (q, q )q  g(q)  u
M (q)q n degrees of freedom

- State variables:
e  joint position error: e = qd - q
x 
q  joint velocities

• Objective: reach qd with q d  0

• Control law (PD + gravity):
u  g (q)  K p (q d  q)  K d q K p , K d   n n
Positive definite*

- Test with the model of the RR robot

 e  0
- This law achieves asymptotic stability for: x   q  qd
q  0
- How to verify asymptotic stability?
• Lyapunov’s criterion
*típically diagonal 17
 Control in the Joint Space
PD Control + Gravity Compensation
• Mechanic Interpretation: u  g  K p e  K d q
For diagonal matrices Kp, Kd (with positive elements):
- The elements of Kp represent the stiffness of the virtual springs
- The elements of Kd represent the viscosity of the virtual dampers

Stiffness kpi > 0 qd (each component defined

Viscosity kdi > 0 with respect to the previous
one)

Current
configuration q
Desired
configuration qd

18
 Control in the Joint Space
Inverse Dynamics Control

• Problem Formulation:
  q
u*  arg min‖q *‖2
u
subject to
  C (q, q )q  g(q)  u
M (q)q
• Solution:
  M 1 (q)  u  C (q, q )q  g(q) 
q

u*  arg min‖M 1  u  Cq  g   q

*‖2
u
u*  arg min‖M 1u  M 1  Cq  g   q
*‖2
u

M 1u  M 1  Cq  g   q
*

u*  Cq  g  Mq
*
Control law
* : reference
q

19
 Control in the Joint Space
Inverse Dynamics Control

• Dynamic model of the robot

  C (q, q )q  g(q)  u
M (q)q

• Control using inverse dynamics:

- It compensates for the robot dynamics → it “linearizes” the system
u  M (q)y  C (q, q )q  g(q) Control law

- It decouples the elements

  y
q yi only affects qi

- It computes the inverse dynamics of the robot

- Problem:
• Specify y to obtain the desired motion
• Stabilize the control law y

  C (q, q )q  g(q)  M (q)y  C (q, q )q  g(q)

M (q)q   M (q)y
M (q)q
20
 Control in the Joint Space
Inverse Dynamics Control
  y
q

• Alternative for y:
- PD + feedforward (sometimes called “computed torque”)
 d  K d (q d  q )  K p (q d  q)
y q
- Replacing:
error
 d  q
q   K d (q d  q )  K p (q d  q)  0 e  qd  q

e  K d e  K p e  0
Dynamics of the position error when tracking a trajectory:
Second order system

12 0 0   21 0 
0
    Critically
Kp       Kd       damped
 0 0 n2   0 0 2n 
  
ωi: natural frequency (determines the velocity of the response)
21
 Control in the Joint Space
Inverse Dynamics Control
  y
q

• Alternative for y:
- PID + feedforward
d  K d (q d  q )  K p (q d  q)  K I  (q d  q)dt
y q
• More robust to uncertainties
• More complex to be implemented in real time

• Summary: control laws

- Linearization + PD + feedforward

u  M (q)  q
 d  K d (q d  q )  K p (q d  q)   C (q, q )q  g (q)

- Linearization + PID + feedforward

 
d  K d (q d  q )  K p (q d  q)  K I  (q d  q)dt  C (q, q )q  g(q)
u  M (q) q

22
 Control in the Joint Space
Other Control Methods

• Robust control
- When the real model differs from the theoretical model
- The maximum variations are known
- It is based on the uncertainty and the design uses the “second” Lyapunov
method

• Adaptive control
- It adapts the control law according to the uncertainty
- It uses the linear parameterization of the robot
- It is non-linear dynamic control

23
 Temas

1. General Concepts

2. Control in the Joint Space

- PD Control
- PD Control + Gravity Compensation
- Inverse Dynamics Control

3. Control in the Operational Space

- Control of Robots -
Prof. Oscar E. Ramos, Ph.D. 24
 Introduction

• Control in the joint space

- The trajectory of each joint is controlled
- Disadvantage:
• Motion is (almost always) specified in the operational space
• Inverse kinematics is required (operational → joint)
- Advantages:
• It is not computationally expensive

• Control in the operational space

- The trajectory in the operational space is controlled
- Advantages:
• It does not require explicit inverse kinematics (more direct control)
• Direct implementation of motion and force control (environmental constraints)
- Disadvantages
• It requires more computational power

25
 Control in the Operational Space

• Conventional methods
- PD control + gravity compensation
- Inverse dynamics control
- Robust control
- Adaptive control

• More recent methods

- Optimal control (MPC) based on QPs
- Redundancy control
• Using inverse dynamics (and projections on the nullspace)
• Using a stack of tasks (and QPs)
• Control based on weighted tasks
• Trajectory optimization (numerical optimal control: iLQR, DDP)

Notes:
- MPC: Model-based predictive control
- QP: Quadratic Program
- Task: control objective (e.g. move the end effector, the elbow)
- DDP: Differential Dynamic Programming
26
 Control in the Operational Space
PD Control + Gravity Compensation

• Dynamic model of a robot:

  C (q, q )q  g(q)  u
M (q)q n degrees of freedom

- State variables:
e  error in the operational space: e = xd - x
x 
q  joint velocitites, with: x  J Aq

• Objective: reach xd with q d  0

• Control law (PD + gravity):
Pose:
u  g (q)  J K p (x d  x)  K d q
T
A
- Desired: xd
- Current: x

K p , K d   nn : positive definite (typically diagonal)

J A : analytic Jacobian associated with x
 e  0
- With this law we reach asymptotic stability for: x   x  xd
q  0 
27
 Control in the Operational Space
PD Control + Gravity Compensation

• Alternative for the regulation problem:

- PD (in the operational space) + gravity compensation (in the joint space)

u  g (q)  J AT  K p (x d  x)  K d x 

• Mechanical interpretation:

Stiffness in the operational space, Stiffness and damping in the

damping in the joint space operational space

u  g (q)  J AT K p (x d  x)  K d q u  g (q)  J AT  K p ( x d  x)  K d x 

28
 Control in the Operational Space
Inverse Dynamics

• Robot dynamic model:

  C (q, q )q  g(q)  u
M (q)q
• Inverse dynamics control:
- It compensates for the robot dynamics → “linearizes” the system
u  M (q)y  C (q, q )q  g(q)
- In closed loop (replacing u):
  y
q we want to represent y in the
operational space
  J A y
J Aq
x  J q  J y
 A A
Relation between y and the
acceleration
- We choose y to minimize the error

x d  J Aq  K d e  K p e)
y must follow the desired
y  J A1 ( trajectory xd(t)

x  J Aq x  J Aq  J Aq
  x  J Aq
  
J Aq xd : desired acceleration
29
 Control in the Operational Space
Inverse Dynamics

• Dynamics of the error:

x  J Aq
J A y  
x  J Aq  
 xd  J Aq  K d e  K p e
x d  J Aq  K d e  K p e)
y  J A1 (
 x  K d e  K p e  0
x d  

e  K d e  K p e  0 Asymptotically stable if Kd > 0, Kp > 0

- We get a second order system.

- Relation for being critically damped: K d  2 K p (term by term)

• Summary of the control law:

x d  J Aq  K d e  K p e)  C (q, q )q  g(q)

u  M (q) J A1 (

• Note:
- In the operational space the analytic Jacobian is always needed
- It is more complex than in the joint space

30
 Summary

• Two control approaches:

- In the joint space
- In the operational space

• Control in the joint space requires inverse kinematics but it is

computationally cheaper

• Control in the operational space includes inverse kinematics but it is

more computationally expensive

• Inverse dynamics linearizes the system, but it is computationally

expensive

31
 References

• B. Siciliano, L. Sciavicco, L. Villani, y G. Oriolo. Robotics: modelling,

planning and control. Springer Science & Business Media, 2010
(Chapter 8)

• M.W. Spong, S. Hutchinson, y M. Vidyasagar. Robot Modeling and

Control. John Wiley & Sons, 2006 (Chapter 8)

• de Wit, Carlos Canudas, Bruno Siciliano, and Georges Bastin,

eds. Theory of robot control. Springer Science & Business Media,
1996 (Chapter 2)

32