Up till now

§ Lecture 1
Ÿ Rigid body motion
Robot control Ÿ Representation of rotation
Ÿ Homogenous transformation
§ Lecture 2
Ÿ Kinematics
• Position
Lecture 4 • Velocity via Jacobian
Mikael Norrlöf • DH parameterization
§ Lecture 3
Substantial contribution by Stig Moberg Ÿ Dynamics
Ÿ Lagrange’s equation Next lecture 15/1
Ÿ Newton Euler 10-12 Algoritmen

PhDCourse Robot modeling and control

Lecture 4

Robot control - examples Outline

§ Fanta challenge § Robot Motion Control Overview

http://www.youtube.com/watch?v=PSKdHsqtok0 § Current and Torque Control
§ Control Methods for Rigid Robots
§ Fanta challenge II § Control Methods for Flexible Robots
http://www.youtube.com/watch?v=SOESSCXGhFo
§ Interaction with the environment

Chapters 6, 8, 9

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Robot motion control Torque and Current Control
Models Simple Model of 3-Phase PM Motor
• Kinematic Model
• Rigid Dynamic Model
R
• Flexible Dynamic Model
dI
V= L + RI
V dt
L
Models τ=K I
User Program Robot t
x1 v
MoveL x1, v1
u(t) 2 x2 I
Trajectory v1
MoveL x2, v2 Controller
xi,vi Interpolator Simplified PI + FFW Control
x1 v Zd(t) y(t)
v1
2 x2 dI
L + RI
dt
τ ref
Trajectory Interpolator
• Path Zd(p)
K t-1 PI τ Torque
• Timing law p(t) gives Zd(t) Controller
• Time Optimal Trajectory • Position Control (Tracking and Regulation)
acting
• Limited d/dt (spd, acc, jerk, ...) • Force and Compliance Control I measured on rotor

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Torque and Current Control Torque and Current Control

q& m ref FeedForward

I
q Calculation
τ ref Kt
Current Rotor Sensor Gear Link
ref

Torque PI
U q
to Control
Current Motor
measured Trend: Reduced Size => Lower Inertia Position
I q d-q
FeedForward to
Measurement Disturbances
Calculation stator PWM Torque Ripple Disturbances Differentiated deterministic disturbance
Gain (Kt) uncertainty 12.05
1
stator

Speed [rad/s]
0.8 Reference Torque
ref 12.04
Motor Torque 0.5
to ref
=0 U 0.7
Torque Constant [Nm/A]

12.03

I
0
d
d-q d PI 0.6
-0.5

Torque [Nm]
12.02

Control 0.5
12.01
-1
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5

0.4
12 Differentiated stochastic disturbance
Measured measured

Speed [rad/s]
0.3

I 11.99 5

Currents d
qm 0.2
11.98
0

qm
0.1
11.97

0 -5
5 10 15 20 25 30 35 40 45 50 11.96
0 1 2 3 4 5 6 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5
Motor Number Motor Angular Position [rad] Time [s]

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 The Robot Control Problem The Robot Control Problem
Max Path Following Error 3.40 mm
0
Ref Pos
-10 Pos
Max Error
-20

-30
0.1
-40

Pos [rad]
Y [mm]
-50

-60

-70 0.05

-80

-90

-60 -40 -20 0 20 40 60 0

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
X [mm]

Typical Requirements:
q Settling Time 0.1 s
q Path Error < 0.1 mm @ 20 mm/s
The user-specified path Zd must be followed with specified precision even
q Path Error 1 mm @ 1000 mm/s
under the influence of different uncertainties. These uncertainties are disturbances acting
q Speed Accuracy 5 %
on the robot and on the measurements, as well as uncertainties in the models used by
the motion control system. q Absolute Accuracy 1 mm
q Repeatability < 0.1 mm

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Independent Joint PD Control Cascade Controller

u = K p (qd - q ) + K v (q& d - q& ) Inner Loop: Speed (PI) Controller

òK i

q& d Kv Robot
- q
d
dt
q Integral part added in most cases
q Speed normally not measured, estimated from position
q D-part on reference can be removed (gives overshoot if I-part is added!)

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Cascade Controller Feedforward + PD Control

Outer Loop: Position Controller (P) – Inner Loop: Speed (PI) Controller u ffw = M (qd )q&&d + c(qd , q& d ) + g (qd )

qd q& d u = u ffw + K p (qd - q) + K v (q& d - q& )

òK i

Kp Kv Robot
- - q
d
dt

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Feedback Linearization + PD Control Feedback Linearization + PD Control

v = q&&d + K p (qd - q ) + K v (q& d - q& ) M(q)&q& + c(q, q& ) + g(q) = u

u = M (q)v + c(q, q& ) + g ( q) M̂(q)v + ĉ(q, q& ) + ĝ(q) = u

1
(*ˆ ) = (*) Þ &q& = v Þ q = v
s2

Decoupled
system of
double
integrators!

Feedback Linearization is sometimes called Computed

Torque or Inverse Dynamics, so is Feedforward Control!
PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Simulation Example – Simple Robot Simulation Example – Simple Robot

qd Kp qd Kp
- 1 1 - 1 1
Js s q& d Js s
Kv Kv
- q& q - q& q

J =1 J =1
Kp = 158 Kp = 158
Kv = 25 Kv = 25

Closed Loop Closed Loop Poles

Poles -12.6, -12.6 -12.6, -12.6
Zero -6
i.e. damping = 1
=> no overshoot

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Simulation Example – Simple Robot Simulation Example – Simple Robot

qd Kp &&d
q Ĵ
- 1 1 qd Kp
q& d - 1 1
Kv Js s q& d
- q& q Kv Js s
J =1
- q& q
Jˆ = 1
J =1 Kp = 158
Kp = 158 Kv = 25
Kv = 126

Closed Loop
Poles -124, -1.3 ”2-DOF” Controller:
Zero -1.3 Tracking &
Regulation/Robustness
”decoupled”

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Simulation Example – Simple Robot Robustification of Feedback Linearization
&&d
q Ĵ Robust outer loop design (Second Method of Lyapunov)
qd Kp 1 1 M(q)&q& + c(q, q& ) + g(q) = u
-
q& d Js s M̂(q)aq + ĉ(q, q& ) + ĝ(q) = u
Kv J = 0.7
- q& q e = qd - q
Jˆ = 1
Kp = 158 aq = &q&d + K p e + K v e&
Kv = 25
(*ˆ ) ¹ (*) Þ &q& = aq + η(v, e, e& )
Worst case estimation of η => discontinuous control term Δa
can be added to outer loop control:

aq = &q&d + K p e + K v e& + Δa (e, e& , t)

q High sample rate required
q Must know max acceleration & worst case error in M, c, and g
q Approximate continuous version

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Passivity Based Robust Control Passivity Based Adaptive Control

Control Input: u = M̂(q)a + ĉ(q, q& )v + ĝ(q) + Kr

v = q& d + Λ(q d - q) Control Input: u = Y(q, q& , a, v)θ̂ + Kr
a = v& = &q&d + Λ(q& d - q& )
r = q& d - q& + Λ(q d - q)
q K and Λ are diagonal matrices.
q Closed loop system is coupled and nonlinear &
q Linear parametrization of dynamic model is used Parameter update: θ̂ = -Γ -1Y T (q, q& , v, a)r
q Bound ρ does not depend on trajectory or state

θ0 - θ £ ρ
u = Y(q, q& , a, v)(θ 0 + δθ) + Kr
ì YT r
ïρ T ; Y r > ε
T

ï Y r
δθ = í
ï ρ Y r ; YTr £ ε
T

ïî ε
PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Robustified Feedback Linearization (Lyap 2) Robustified Feedback Linearization (Lyap 2)

&& d
q Δa
qd Kp
- 1 1
q& d Ĵ
Kv Js s
- q& q

w = p 21e + p 22e
J = 0.7 ì w ; w ¹0
ï- (γ1 + γ1 e + e& )
2 2

Ĵ = 1 Δa = í w
Kp = 158 ïî 0 ; w =0
Kv = 25
q p21, p22,γ 1 , γ 2, computed from J uncertainty, Kv, Kp, and &q&dmax
q Lyapunov equation => p21, p22
Discontinuous Δa
PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Robustified Feedback Linearization (Lyap 2) Path Accuracy: Circle r = 50 mm @ 1 s

XY rigid robot with PD (fdb) P (ref) control

Max Error 44.36 mm, Bandwidth 1 Hz (x) 1 Hz (y)
0
Ref Pos
-10 Pos
Max Error
-20

-30

-40

Y [mm]
-50

-60

-70

-80

-90

Continuous Δa -60 -40 -20 0

X [mm]
20 40 60

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Path Accuracy: Circle r = 50 mm @ 1 s Path Accuracy: Circle r = 50 mm @ 1 s

XY rigid robot with PD (fdb) P (ref) control XY rigid robot with PD (fdb) P (ref) control
Max Error 2.01 mm, Bandwidth 15 Hz (x) 20 Hz (y)
Max Error 0.84 mm, Bandwidth 15 Hz (x) 15 Hz (y)
0
0
Ref Pos Ref Pos
-10 -10 Pos
Pos
Max Error Max Error
-20 -20

-30 -30

-40 -40

Y [mm]
Y [mm]

-50 -50

-60 -60

-70 -70

-80 -80

-90 -90

-60 -40 -20 0 20 40 60 -60 -40 -20 0 20 40 60

X [mm] X [mm]
PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Path Accuracy: Circle r = 50 mm @ 1 s Flexible Joint Model

XY rigid robot with PD (fdb) P (ref) control Complete Model:

0
Max Error 7.88 mm, Bandwidth 50 Hz (x) 10 Hz (y) 0 = M a (q a )q
&& a + Sq && m + c1 (q a , q& a , q& m ) + g(qa ) + K(q a - q m ) + D(q& a - q& m )
-10
Ref Pos τ = M mq &&m + S q T
&& m + c 2 (q a , q& a ) - K(q a - q m ) - D(q& a - q& m ) + f(q& m )
Pos
Max Error
-20
Simplified Model I (high gear ratio):
0 = M a (q a )q
&& a + c(q a , q& a ) + g(qa ) + K(q a - q m ) + D(q& a - q& m )
-30
Bandwidth must be sufficiently
-40
high relative frequency content of τ = M mq &&m - K(q a - q m ) - D(q& a - q& m ) + f(q& m )
Y [mm]

-50
trajectory

-60 All channels should be equal! Simplified Model II (friction and damping low):
-70
0 = M a (q a )q
&& a + c(q a , q& a ) + g(q a ) + K(q a - q m )
-80 τ = M mq m - K(q a - q m )
&&
-90

-60 -40 -20 0 20 40 60 For N links and N motors: 2N d.o.f.

X [mm]

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Linear One Axis Model PD Control

k
τ Jm Ja
qd
-
Kp

qm d qa Kv
- q& m qm
s 2 2ς z s
+ +1
ω 2z ω z
G τ ®q m =
æ s 2 2ς s ö
s 2 (J a + J m )ç 2 + p + 1 ÷
çω ÷
è p ωp ø
sd + k Closed Loop Bandwidth:
G τ ®q a =
æ s 2 2ς s ö 10 Hz
s 2 (J a + J m )ç 2 + p + 1 ÷
çω ÷ Mechanical Resonance:
è p ωp ø 6 Hz
d 1 d Ja + Jm k k(J a + J m )
ςz = ςp = ωz = ωp =
2 kJ a 2 kJ a J m Ja JaJm

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

PD Control – Motor Feedback PD + FFW Control – Motor Feedback

qd Kp && d
q Ĵ
- qd Kp
Kv -
- q& d
q& m qm Kv
- q& m qm

Closed Loop Bandwidth: Same PD Tuning as on

3.5 Hz previous slide
Mechanical Resonance: (Poles @ 3.5 Hz)
6 Hz

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 PD Control – Arm Feedback PD Control – Arm Feedback

qd Kp qd Kp
- -
Kv Kv
- q& a qa - q& a qa

Same PD Tuning as on Low Gain Controller

previous slide => unstable

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

PD Control – Arm Pos / Motor Speed Feedback LQ Control / State Feedback – Full State Measurement

qd Kp
- qd L0
-
Kv
-
q& m
x = [q m q a q& m q& a ]
qa L
Ref Position 0.1
Motor Position
0.09 Ref Position
Arm Position
Motor Position
0.08 Arm Position
Motor Feedback preferred 0.1 0.07

for flexible joint robot if only 0.06

Pos [rad]

Pos [rad]
two states are measured 0.05

0.04
0.05
0.03

0.02

0.01
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0
0 0.05 0.1 0.15 0.2 0.25

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Control of a flexible robot arm: A benchmark problem Swedish Open Championships in Robot Control 2005

q What is the limit for control using motor measurements only?

q Design of a robust digital controller for optimal disturbance rejection

q One axis uncertain model of an industrial robot

www.robustcontrol.org

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Nonlinear MIMO Flexible Joint Control Suggested Control Methods

Feedback Linearization
Passivity-Based Control
Backstepping
Adaptive Control
Neural Networks
Singular Perturbations
Composite Control
Input Shaping
Robust Control based on Lyaponov 2nd Method
Sliding Mode Control
Iterative Learning Control
Feedforward Control
Linear MIMO Control (Pole Placement, LQG, H infinity, …)
Linear Diagonal Control (PID, Pole Placement, …)
…
…
Goal: Perfect tracking, i.e Z(t) = Z d (t)

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Compact Gear Transmission Nonlinear Flexibility

Elasticity with hysteresis in rotational direction

1000

800

600

400

Torque [Nm]
200

-200

-400

-600

-800

-1000
-1.5 -1 -0.5 0 0.5 1 1.5
Deflection [arcmin]

Friction
4
Measured Friction
3 Model

Friction [Nm]
1

-1

-2
Bearing Elasticity
Torque Ripple -3

-4
-300 -200 -100 0 100 200 300
Speed [rad/s]

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Extended Flexible Joint Model Extended Flexible Joint Model

For N links, N motors and M

unactuated joints: 2N + M d.o.f.

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Feedforward Control of Extended Flexible Joint Nonlinear Simulation Model

M a (q a )v& a = c(q a , v a ) + g(q a ) + τ a (q a , q m , v a , v m )

M m v& m = τ m (q a , q m , v a , v m ) + u
q& a = v a
q& m = v m Inverse
Dynamics
Z d (t) = Γ(q a )

q Perfect tracking & Point-To-Point

q A High Index DAE must be solved
q Solution for M=2, N=3 & M=3, N=9 § 2 actuated DOF (N = 2)
q Non-minimum Phase? § 1 unactuated DOF (M = 1)
§ 5 DOF

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Simulation Model – The Equations Non-minimum Phase

• Extended Flexible Joint & Flexible Link is normally NMP for

perfect tracking
• Example: 5 DOF 2 Axis Model:
Non-causal solution, movement starts at t = 0.1 s
u1

5000

u1 [Nm]
0

-5000

0 0.05 0.1 0.15 0.2 0.25 0.3

Time [s]
u2

4000

2000

u2 [Nm]
0

-2000

-4000
0 0.05 0.1 0.15 0.2 0.25 0.3
Time [s]

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Control when in contact with the environment Force and torque when in contact with environment

§ Position and speed control Virtual displacement (again)

not enough
§ Why? dX = J (q)dq

§ Sensor configuration virtual work

Ÿ Wrist torque/force sensor
Ÿ Joint torque/force sensor Figure 9.2 in Spong etal
dw = F T dX - t T dq
Ÿ Tactile
(
= F T J (q) - t T dq)

Þ t = J T (q) F

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Complete dynamics Coordinate frames and constraints

évù
x=ê ú
Let the velocity (Twist) be defined ëw û
M (q )q&& + C (q, q& )q& + g (q ) + J (q ) Fe = u
T

éfù
and the force (Wrenche) F=ê ú
ënû

Reciprocity condition x T F = vT f + w T n = 0

Figure 9.2 in Spong etal

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Task constraints Impedance control

Natural constraints. Constraints imposed by the task.

M&x& = u - F
x F = v x f x + v y f y + v z f z + w x nx + w y n y + w z n z = 0
T

Let
Example: M
u = -( + 1) F
m
Natural constraints Artificial constraints
vx = 0 fx = 0
vy = 0 fy = 0 The closed loop system is
fz = 0 v z = vd
wx = 0 nx = 0 M
wy = 0 ny = 0
M&x& = -( + 1) F + F Þ m&x& = - F
nz = 0 wz = 0
Figure 9.3 in Spong etal m

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Force Control and SoftMove Force Control and SoftMove

General idea – mechanical compliance Current important applications

§ Define compliant behaviour in one (or § Force Control

several) Cartesian directions Ÿ Machining
• Define soft direction relative to tool or work • Milling, grinding, fettling, polishing, etc.
object • FC SpeedChange functionality can be used to control path speed

» The robot is stiff in other directions Ÿ Assembly

• Complex, multi-stage assembly operations
Ÿ Product testing

SoftMove § SoftMove
FC Assembly
Machining
Ÿ Ejector machines, extracting (die casting)
Search Ÿ Handling workpiece variations
Movement direction
directions

F/T sensor
Uneven
surface Soft direction
directions Pressure direction

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4
 Force Control Assembly Force Control Machining

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4

Force Control technology

SoftMove
Force feedback

§ Speed reference is generated in the soft

directions based on current deviation from
desired force
• Damping and LP-filter (“inertia”) can be
programmed

+ -

F/T sensor

PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Lecture 4 Force
 SoftMove technology Force Control and SoftMove
Servo controller modification Important aspects to remember

§ Make servo control “softer” in a chosen Cartesian direction • Forces are unpredictable
Ÿ No force sensor needed No feed-forward or path planning to improve performance
• Forces can not be controlled • A robot in contact behaves very differently depending on
• More sensitive to friction contact stiffness and geometry
» Friction can be compensated by adding a force offset
• “Cartesian Soft Servo”
» Compare ordinary (joint) Soft Servo functionality

Force
PhDCourse Robot modeling and control PhDCourse Robot modeling and control
Lecture 4 Programmed point Lecture 4

Summary

§ Robot Motion Control Overview

Ÿ Inner/outer loop control architecture
§ Current and Torque Control
§ Control Methods for Rigid Robots
Ÿ Computed torque
Ÿ Feedback linearization
§ Control Methods for Flexible Robots
Ÿ Feed-forward control
Ÿ State feedback
§ Interaction with the environment

PhDCourse Robot modeling and control

Lecture 4