Robotics 1

Kinematic control
Prof. Alessandro De Luca

Robotics 1

Robot motion control

need to actually realize a desired robot motion task

... despite the presence of

external disturbances and/or unmodeled dynamic effects

initial errors (or arising later due to disturbances) w.r.t. desired task
discrete-time implementation, uncertain robot parameters, ...

we use a general control scheme based on

regulation of pose/configuration (constant reference)

trajectory following/tracking (time-varying reference)

feedback (from robot state measures, to impose asymptotic stability)

feedforward (nominal commands generated in the planning phase)

the error driving the feedback part of the control law can be
defined either in Cartesian or in joint space

Robotics 1

control action always occurs at the joint level (where actuators drive
the robot), but performance has to be evaluated at the task level
2

Kinematic control of robots

a robot is an electro-mechanical system driven by actuating

torques produced by the motors
it is possible, however, to consider a kinematic command (most
often, a velocity) as control input to the system...
...thanks to the presence of low-level feedback control at the
robot joints that allow imposing commanded reference velocities
(at least, in the ideal case)
these feedback loops are present in industrial robots within a
closed control architecture, where users can only specify
reference commands of the kinematic type
in this way, performance can be very satisfactory, provided the
desired motion is not too fast and/or does not require large
accelerations

Robotics 1

An introductory example
..

a mass M in linear motion: M x = F

.
low-level feedback: F = K(u x), with u = reference velocity
.
equivalent scheme for K: x u
in practice, valid in a limited frequency bandwidth K/M
u

1/M

..
x

.
x

Robotics 1

Frequency response
of the closed-loop system

Bode diagrams of sx(s)/u(s) for K/M = 0.1, 1, 10, 100

- 3dB

actual
velocity

.
x
u

commanded
reference
velocity

Robotics 1

Time response

setting K/M = 10 (bandwidth), we show two possible time responses

to unit sinusoidal velocity reference commands at different
1
0.6
-7.5 dB attenuation

= 2 rad/s

= 20 rad/s

actually realized velocities

Robotics 1

A more detailed example

including nonlinear dynamics

single link (a thin rod) of mass m, center of mass at d from joint

axis, inertia M (motor + link) at the joint, rotating in a vertical plane
(the gravity torque at the joint is configuration dependent)

Im

d
q

dynamic model

Il

(I

+ Il + md 2 ) q + mg0 d sinq =

g0 = 9.81[m /s2 ]
m = 10 [kg]

d = = 0.2 [m]
2
1
Il = m 2 = 0.1333 [kg m 2 ]
12
Im = 0.5333 [kg m 2 ] (= Il + md 2 )

mg0
M = 1.0667 [kg m ]

fast low-level feedback control loop based on a PI action on the

velocity error + an approximate acceleration feedforward

kinematic control loop based on a P feedback action on the position
error + feedforward of the velocity reference
evaluation of tracking performance for rest-to-rest motion tasks with
increasing dynamics = higher accelerations
2

Robotics 1

A more detailed example

differences between the ideal and real case

Simulink scheme

real behavior

trajectory generation
(a cubic position profile)

ideal behavior

Robotics 1

A more detailed example

robot with low-level control

Simulink scheme

low-level control =
PI velocity feeedback loop
+ acceleration feedforward

robot
dynamics

actuator
saturation at 100 [Nm]

Robotics 1

Simulation results

rest-to-rest motion from downward to horizontal position

in T = 1 s

in T = 0.25 s

bad
tracking of
reference
trajectory

max error
5.5

Robotics 1

position error

max error
0.2

position tracking

very good
tracking of
reference
trajectory

in T = 0.5 s

10

Simulation results

rest-to-rest motion from downward to horizontal position

in T = 1 s

in T = 0.5 s

in T = 0.25 s
high-level velocity command

similar to
the profile of
reference
velocity!

torque for static

balance of gravity
(provided by the
integral term)
dominated by
gravity torque evolution

Robotics 1

saturation!

low-level torque command

similar to reference
acceleration profile!

11

Simulation results

rest-to-rest motion from downward to horizontal position

in T = 1 s

in T = 0.5 s

in T = 0.25 s

max error
0.2

max error
5.5

real position errors increase when reducing too much motion time
( too high accelerations)
while ideal position errors
(based only on kinematics)
remain always the same!!

here, 0 because of the initial matching

between robot and reference trajectory
Robotics 1

12

Control loops in industrial robots

analog loop of large bandwidth on motor current ( torque)

analog loop on velocity (Gvel(s), typically a PI)
digital feedback loop on position, with velocity feedforward
this scheme is local to each joint (decentralized control)

.
qd(t)
qd(t)

seen as a simple integrator

+
-

+
P, PI,
or PID +

digital part
Robotics 1

+
-

Gvel(s)

actuator

robot
(joint i)

.
q

analog part
13

Kinematic control of joint motion

.
pd

J-1(qd)

qd(0)

.
qd

qd + e

reference generator
(off-line computation of J-1)

e = qd - q
ep = pd - p

K > 0 (often
diagonal)

q(0)

robot
model

not used
for control

f(q)

. .
.
.
.
ep = pd - p = J(qd)qd - J(q)(qd + K(qd - q))
ep J(q)e

feedback from q

.
.
.
.
.
e = qd - q = qd - (qd + K(qd - q)) = - K e

q qd

Robotics 1

+
+

.
q

J(q)

.
p

ei 0 (i=1,,n)
exponentially,
e(0)

.
ep - J(q)K J-1(q) ep
14

Kinematic control of Cartesian motion

pd(0)

.
pd

pd +

reference
generator

ep = pd - p

ep

Kp

+
+

Kp >0 (often
diagonal)

J-1(q)

.
q

q(0)

robot
model

J(q)

.
p

f(q)

feedback from p

.
. .
.
.
-1
ep = pd - p = pd - J(q) J (q) (pd + Kp(pd - p)) = - Kp ep

ep,i 0 (i=1,,m) exponentially, ep(0)

needs on-line computation of the inverse(*) J-1(q)
real-time + singularities issues
(*)

Robotics 1

or pseudoinverse if m<n

15

Simulation

features of kinematic control laws

desired reference
trajectory:
two types of tasks
1.straight line
2.circular path
both with
constant speed
numerical
integration method:
fixed step
Runge-Kutta
at 1 msec

Robotics 1

robot:
planar 2R
lengths l1=l2=1

Simulink block diagram

16

Simulink blocks
calls to Matlab functions
k(q)=dirkin (user)
J(q)=jac (user)
J-1(q)=inv(jac) (library)

a saturation (for task 1.)

or a sample and hold (for task 2.)
added on joint velocity commands
system initialization of kinematics
data, desired trajectory, initial state,
and control parameters (in init.m file)

never put numbers inside the blocks !

Robotics 1

17

Matlab functions

init.m
script

(for task 1.)

Robotics 1

18

Simulation data for task 1

straight line path with constant velocity

large initial error on end-effector position

xd(0) = 0.7 m, yd(0) = 0.3 m; vy,d = 0.5 m/s, for T = 2 s

q(0) = [-45 90]T ep(0) = [-0.7 0.3]T m

control gains
K = diag{20,20}
(a) without joint velocity command saturation
(b) with saturation ...
vmax,1 = 120/s, vmax,2 = 90/s

Robotics 1

19

Results for task 1a

straight line: initial error, no saturation

initial
transient
phase
(about 0.2 s)

p(0)

pd(0)

path executed by the

robot end-effector
(actual and desired)
Robotics 1

stroboscopic view of motion

(start and end configurations)
trajectory
following
phase
(about 1.8 s)

20

Results for task 1a

(cont)
straight line: initial error, no saturation

errors converge independently

and exponentially to 0

px, py actual and desired

Robotics 1

. .
control inputs qr1, qr2

21

Results for task 1b

straight line: initial error, with saturation

initial
transient
phase
(about 0.5 s)

p(0)
pd(0)

path executed by the

robot end-effector
(actual and desired)
Robotics 1

stroboscopic view of motion

(start and end configurations)
trajectory
following
phase
(about 1.5 s)

22

Results for task 1b

(cont)
straight line: initial error, with saturation

errors eventually converge

once out of saturation!

px, py actual and desired

Robotics 1

. .
control inputs qr1, qr2
(saturated at vmax,1, vmax,2)

23

Simulation data for task 2

circular path with constant velocity

zero initial error on Cartesian position (match)

centered at (1.014,0) with radius R = 0.4 m;

v = 2 m/s, performing two rounds T 2.5 s
q(0) = [-45 90]T ep(0) = 0

(a) ideal continuous case (1 kHz), even without feedback

(b) with sample and hold (ZOH) of Thold = 0.02 s (joint
velocity command updated at 50 Hz), but without feedback
(c) as before, but with Cartesian feedback using the gains
K = diag{25,25}

Robotics 1

24

Results for task 2a

circular path: no initial error, continuous control (ideal case)

px, py actual and desired

zero tracking
error is kept
at all times

Robotics 1

control inputs qr1, qr2

joint variables q1, q2

final configuration
(after two rounds)
coincides with
initial configuration

25

Results for task 2b

circular path: no initial error, ZOH at 50 Hz, no feedback

px, py actual and desired

a drift occurs
along the path
due to the
linearization
error along the
path tangent
Robotics 1

control inputs qr1, qr2

joint variables q1, q2

final configuration
(after two rounds)
differs from
initial configuration

26

Results for task 2c

circular path: no initial error, ZOH at 50 Hz, with feedback

px, py actual and desired

(almost) the same

performance of
the continuous
case is recovered!!

Robotics 1

control inputs qr1, qr2

joint variables q1, q2

note however that
larger P gains will
eventually lead to
unstable behavior
(see: stability
problems for
discrete-time
control systems)
27

3D simulation
video

kinematic control of Cartesian motion of Fanuc 6R (Arc Mate S-5) robot

simulation and visualization in Matlab
Robotics 1

28

Kinematic control of KUKA LWR

video

kinematic control of Cartesian motion with redundancy exploitation

velocity vs. acceleration level
Robotics 1

29