Give a desired trajectory, and leave joint movements to controller.

EE 473 Introduction to Robotics

Lecture 8: Robot Control

G. Ablay

G. Ablay, EE473 1
 Contents
• Robot control
– Linear & nonlinear controllers
• Linear control – PID
• Computed torque control
– Matlab simulations

G. Ablay, EE473 2
 • Robotic systems
System = robotic arm
Controller = computer, or microcontroller
Actuators = motors
Sensors = position and force sensors

Motor4
Motor3
End-effector
position
Motor2 Sensor4 p ( x, y )
Sensor3

Sensor2

Position setpoint Controller

Speed setpoint (Computer, PLC, Motor1 Sensor1
Force setpoint Microcontroller)

Position setpoint
y p ( x, y )

G. Ablay, EE473 3
 Architecture of an Industrial-robot Controller

Probably for economic reasons, present-day industrial manipulators are controlled with
very simple control laws that generally are completely error driven and are implemented
in architectures such as independent joint control.
• The gravity terms will tend to cause static positioning errors, so some robot
• manufacturers include a gravity model, G(q), in the control law.

G. Ablay, EE473 4
 Architecture of an Industrial-robot Controller
Unimation PUMA 560 industrial robot:
The hardware architecture is that of a two-level hierarchy, with a DEC LSI-11 computer
serving as the top-level "master" control computer passing cormmands to six Rockwell
6503 microprocessors.
• Each of these microprocessors controls an individual joint with a PID control law
• Each joint of the PUMA 560 is instrumented with an incremental optical encoder
• Actuators are DC torque motors

Hierarchical computer architecture of the PUMA 560 robot-control system.

G. Ablay, EE473 5
• Robot Control Methods

G. Ablay, EE473 6
 Purpose of Robotic Control

• Direct control of forces or positions of a manipulator

• Path planning and navigation (mobile robots)
• Compensate for robot’s dynamic properties (inertia,
damping, etc.)
• Avoid internal/external obstacles

G. Ablay, EE473 7
 Method of Control
A teach pendant device is needed to
There are two basic types of control systems: control an industrial robot remotely.
(1)Point-to-point control system
(2)Continuous path control system

• Point-to-point robots:
• A point-to-point robot can be taught a discrete set
of points but there is no control on the path of the
end-effector in between taught points.
• Such robots are usually taught a series of points
with a teach pendant.
• The points are then stored and played back.
• Point-to-point robots are severely limited in their
range of applications.
• Point-to-point control systems work well in
machine loading and unloading, hole drilling,
boring, reaming, tapping and punching.

G. Ablay, EE473 8
• Continuous path robots:
• The entire path of the end-effector can be controlled with a continuous path.
• In addition, the velocity and/or acceleration of the end-effector can often be
controlled.
• In the working cycle, the robot follows the points to reproduce the desired
path.
• These are the most advanced robots and require the most sophisticated
computer controllers and software development.
• Continuous path control applications include welding, painting, gluing,
finishing (e.g., grinding, sanding, and deburring).

Path of linear interpolation

G. Ablay, EE473 9
 Robot Control Algorithms
Robots are nonlinear dynamical systems, and there is no general method for designing
a nonlinear controller to be suitable for every robot in every mission. However, there
are a variety of alternative and complementary methods, each best applicable to
particular class of robots in a particular mission. The most important control methods
are as follows:
• Linear Control Techniques:
• The simplest technique for controlling robots is to design a linear controller
based on the linearization of the equations of motion about an operating point.
• The linearization technique locally determines the stability of the robot.
Proportional, integral, and derivative, or any combination of them, are the most
practical linear control techniques.
• Computed Torque Control Technique (Feedback Linearization):
• In computed torque control technique, we define a control law to obtain a
linear differential equation for error command, and then use the linear control
design techniques. The feedback linearization technique can be applied to
robots successfully, however, it does not guarantee robustness according to
parameter uncertainty or disturbances.
• This technique is a model-based control method, because the control law is
designed based on a nominal model of the robot.
G. Ablay, EE473 10
 Robot Control Algorithms
• Adaptive Control Technique:
• Adaptive control is a technique for controlling uncertain or time-varying
robots.
• Adaptive control technique is more effective for low DOF robots.
• Robust and Adaptive Control Technique:
• In the robust control method, the controller is designed based on the nominal
model plus some uncertainty.
• Uncertainty can be in any parameter, such as the load carrying by the end-
effector. For example, we develop a control technique to be effective for loads in
a range of 1 − 10 kg.
• Gain-Scheduling Control Technique:
• Gain-scheduling is a technique that tries to apply the linear control techniques
to the nonlinear dynamics of robots.
• In gain-scheduling, we select a number of control points to cover the range of
robot operation.
• Then at each control point, we make a linear time-varying approximation to
the robot dynamics and design a linear controller.
• The parameters of the controller are then interpolated or scheduled between
control points.
G. Ablay, EE473 11
1- Linear Control Techniques

G. Ablay, EE473 12
1- Linear Control Techniques

PID Control Design

-
Position PI Control
+
reference e Kp, Ki q
General PID robot control Robot
q
Velocity e Kv
reference +
-

Acceleration is not used in the control design due to high noise issue.

A robotic manipulator with n-DOF (rigid links)

NOTE: Linearization, sin( q)  q, cos( q)  1
M ( q ) q  C ( q, q ) q  G ( q )  T NOTE: State model,

 q  M 1 T  Cq  G   x1  q
 
 x1  x2

 x2  M T  Cx2  G 
1
 x2  q

Position control problem: finding appropriate T such that position error goes to zero:
Tracking error: e  qd  q
lim  qd  q   0
t 
G. Ablay, EE473 13
1- Linear Control Techniques

PD Control
-
Position
+ Kp
reference e q
T
+ Robot
q
Velocity e Kv
reference +
-

PD control law is
T  K p (qd  q )  K v (qd  q ) T = torque

If qd is constant, then - T q
qd  0 +
e Kp + Robot
qd -
q
K p , Kv are diagonal matrices
Kv

G. Ablay, EE473 14
1- Linear Control Techniques

PD Control
Analysis of PD control:
T  K p (qd  q )  K v (qd  q ) , qd  constant  qd  qd  0
M ( q ) q  C ( q , q ) q  G ( q )  K p ( qd  q )  K v ( q d  q )
 qd  q  qd  M 1  K p e  K v q  Cq  G 
 q  M 1  K p e  K v q  Cq  G 
Lyapunov stability analysis:
T
1 e K p 0  e 1 T 1 T
V         q Mq  e K pe
2 q   0 M  q  2 2
Since M and Kp are positive definite, V is positive definite. The derivative of V
yields
1 T
 V  q Mq  q Mq  eT K p e
T

2
G. Ablay, EE473 15
1- Linear Control Techniques

PD Control
Lyapunov stability analysis:
T
1 e K p 0  e 1 T 1 T
V         q Mq  e K pe  0
2 q   0 M  q  2 2
1
 V  qT Mq  qT Mq  eT K p e M (q)q  C (q, q )q  G (q )  K (q  q )  K (q  q )
p d v d

2
 q  K p e  K v q  Cq  G   q Mq  eT K p e
T 1 T
2
1 T
 q K p e  q K v q  q Cq  q G  q Mq  eT K p e
T T T T

2
T 1 
 q K p e  e K p e  q Kv q  q G  q  M  C  q
T T T T

2 
 qT K p e  eT K p q  qT K v q  qT G =0 qd  constant  qd  qd  0
 e  q
 q K v q  q G
T T

 min ( K v ) q  G q
2
Thus, the error e is bounded, but not zero.
G. Ablay, EE473 16
1- Linear Control Techniques

Equilibrium point:
PD Control
q   M 1  K p e  K v q  Cq  G   0
 K p e*  G  0
1
 e*  G
Kp

• Thus, there might be multiple equilibria.

• The error e is bounded, but
• PD control does not achieve control objective under G(q) since e is not going to
zero.
• If G(q) = 0, then e→0.

• There will be steady-state error under PD controller.

G. Ablay, EE473 17
1- Linear Control Techniques

PD Control with gravity compensation

(Principle of the active stiffness control scheme)

To achieve control objective, the gravity term is added to PD control:

T  K p (qd  q )  K v (qd  q )  G (q)

G(qd) replaces G(q)

G(q)
-
Position
+ Kp q
reference e T
+ Robot
q
Velocity e
+ Kv
reference
-

By choosing control gains sufficiently large, the position error approaches zero:

e  0 as t  .
G. Ablay, EE473 18
1- Linear Control Techniques

PID Control
An integral term is included to drive the position error to zero:

T  K p (qd  q )  K v (qd  q )  K i  (qd  q )dt

T  K p (qd  q )  K v (qd  q )  K i 

  qd  q
 M (q )q  C (q, q )q  G (q )  K p (qd  q )  K v (qd  q )  K i 

  qd  q
   e 
  
   d
e q  M 1
 K
 p e  K v e  K i   Cq  G 
 

Ki/s
-
Position
+ Kp q
reference e T
+ Robot
q
Velocity e Kv
reference +
- G. Ablay, EE473 19
1- Linear Control Techniques

Example 1: Pendulum
An integral term is included to drive the position error to zero:

J q  mgl sin(q )  T

y=l cos(q)
 e  qd  J 1 T  mgl sin(q ) 
J  1, mgl  1, qd   / 2 q
mg
Controller : k p  0.25, kv  0.5 x=l sin(q)

NOTE: Linearization, sin( q)  q, cos( q)  1

NOTE: State model,
 x1  q  x1  x2
 
 x2  J T  mgl sin(q) 
1
 x2  q

G. Ablay, EE473 20
1- Linear Control Techniques

Example 2: 2-DOF system

A planar arm with two links connected through revolute joints, i.e. it possesses
2 DOF. The links are driven by two electrical motors located at the “shoulder”
(base) and at the “elbow”.
 M 11 M 12   q1   C11 C12   q1   g1   1 
M   
 12 M 22   q2  C21 C22   q2   g 2   2 
M 11  m1lc21  m2  l12  lc22  2l1lc 2 cos(q2 )   I1  I 2
M 12  m2  lc22  l1lc 2 cos(q2 )   I 2
C11  m2 l1lc 2 sin(q2 )q2
C12  m2 l1lc 2 sin(q2 )  q1  q2 
C21  m2 l1lc 2 sin(q2 )q1
C22  0
g1   m1lc1  m2 l1  g sin(q1 )  m1lc 2 g sin(q1  q2 )
l1  0.26m, l2  0.26m
g 2  m2 lc 2 g sin(q1  q2 )
lc1  0.0983m, lc 2 00.0229m
m1  6.5225kg , m2  2.0458kg
System parameters I1  0.1213kgm2 , I 2  0.0116kgm 2
G. Ablay, EE473 g  9.81m / s 2 21
1- Linear Control Techniques

Example 2: 2-DOF system (continued)

 / 10 
Control objective: q d    rad
 / 30 
30 0  7 0 
Controller: K p    Nm / rad , K v   0 3 Nms / rad
 0 30   

State model, System parameters

l1  0.26m, l2  0.26m
 x1  q1
x  q  x1  x3 lc1  0.0983m, lc 2 00.0229m
 2  m1  6.5225kg , m2  2.0458kg
  x2  x4
2

 3x  q  I1  0.1213kgm2 , I 2  0.0116kgm 2
1
 T  C ( x, x )  G ( x ) 
1
 x4  q2 [ x3 x 4 ]T
 M g  9.81m / s 2

Simulate the system for the given parameter and control values.

G. Ablay, EE473 22
2- Computed-Torque Control

G. Ablay, EE473 23
2- Computed-Torque Control

Computed-Torque Control
• These generally perform well when the robot arm
parameters are known fairly accurately.
• These schemes involve the decomposition of the
control design problem into an inner-loop design and
an outer-loop design.
• Computed torque, at the same time, is a special
application of feedback linearization of nonlinear
systems, which has gained popularity in modern
systems theory.
• Computed-torque-like controls appear in robust
control, adaptive control, learning control, and so on.
G. Ablay, EE473 24
2- Computed-Torque Control

Inner loop/outer control architecture:

Linearized System

Trajectory
qd
Outer-loop ad Inner-loop τ
Robot
Planner Controller Controller

G. Ablay, EE473 25
2- Computed-Torque Control

Derivation of Inner Feedforward Loop

The robot arm dynamics are

M (q)q  C (q, q )q  G (q )  Td  T

Suppose that a desired trajectory qd(t) has been selected for the arm motion,
Define an output or tracking error as
e(t )  qd (t )  q(t )
e  qd  q
e  qd  q  qd  M 1  Cq  G  Td  T 

Defining the control input function the disturbance function

u  qd  M 1  Cq  G  T  w  M 1Td

G. Ablay, EE473 26
2- Computed-Torque Control

Derivation of Inner Feedforward Loop

The tracking error dynamics

 e  0 I  e  0 0
e   0 0 e    I  u   I  w
        

This is a linear error system in Brunovsky canonical form consisting of n pairs of

double integrators 1/s2, one per joint. It is driven by the control input u(t) and the
disturbance w(t).

The feedback linearizing transformation may be inverted to yield

T  M  qd  u   Cq  G computed-torque control law

It is important to realize that computed-torque depends on the inversion of the robot

dynamics, and indeed is sometimes called inverse dynamics control .
The rigid arm dynamics are minimum phase.

G. Ablay, EE473 27
2- Computed-Torque Control

PD Outer-Loop Design
The outer-loop signal u(t) can be chosen using many approaches, including robust
and adaptive control techniques

u   K p e  Kv e
Overall robot dynamics
T  M  qd  u   Cq  G
 M  qd  K p e  K v e   Cq  G
Closed-loop error dynamics

e  qd  M 1  Cq  G  Td  T 


 qd  M 1 Cq  G  Td  M  qd  K p e  K v e   Cq  G 
  K p e  Kv e  w

 e  Kv e  K p e  w G. Ablay, EE473 28
2- Computed-Torque Control

PD Outer-Loop Design
Choice of PD gains:
K p  diag{k pi } , K v  diag{kvi }

Then the characteristic equation of the error dynamics

    s 2  kvi s  k pi 
n

i 1

Closed-loop error dynamics are asymptotically stable as long as the control gains are
positive. The disturbance term must be bounded.
How to choose PD gains:
p ( s )  s 2  kvi s  k pi  s 2  2n s  n2
 k pi  n2 , kvi  2n
kvi2
For critical damping:   1  k pi 
4
G. Ablay, EE473 29
2- Computed-Torque Control

Example: Two-link planar elbow arm

(m1  m2 )a12  m2 a22  2ms a1 a2 cos  2 m2 a22  m2 a1 a2 cos  2  1 
  
 m2 a22  m2 a1 a2 cos  2 m2 a22   2 
 m2 a1 a2 (21 2   22 ) sin  2  (m1  m2 ) ga1 cos 1  m2 ga2 cos(1   2 )   T1 
    
 m2 a1 a21 sin  2
2
  m2 ga2 cos(1   2 )  T2 

System parameters
mi  1, ai  1, g  9.8 (𝑥2 , 𝑦2 )
𝑦 𝑚2
Desired trajectory:
𝑎2 𝜃2
1d  g1 sin(2 t / T ) ,
 2 d  g 2 sin(2 t / T ), T  2, gi  0.1
𝑎1
Controller design 𝑚1
𝜃1
Desired time-constant:   0.1 s 𝑥
𝑜
1
  n  10
n
Controller : K p  n2  100 , K v  2n  20
G. Ablay, EE473 30
2- Computed-Torque Control

Computed-Torque Control
Computed-torque control scheme, showing inner and PD outer loops:

- G(q)
qd + Kp
e u q
T
qd + M (q ) + Robot
q
qd +
e Kv C ( q, q )
-

G. Ablay, EE473 31
2- Computed-Torque Control

PID Outer-Loop Design

The outer-loop signal u(t) using PID controller
u   K p e  K v e  K i 

  e

Overall robot dynamics

T  M  qd  u   Cq  G  M  qd  K p e  K v e  K i    Cq  G

Ki / s
- G(q)
qd + Kp
e u q
T
qd + M (q ) + Robot
q
qd +
e Kv C ( q, q )
-
G. Ablay, EE473 32
2- Computed-Torque Control

PID Outer-Loop Design

Closed-loop error dynamics

e  qd  M 1  Cq  G  Td  T 


 qd  M 1 Cq  G  Td  M  qd  K p e  K v e  K i    Cq  G 
  K p e  Kv e  Ki  w
   0 0    0 
e  K v e  K p e  K i   w
 I
   
  e   0 0 I   e   0  w

  e
  e    Ki
   K p  K v   e   I 

Then the characteristic equation of the error dynamics

  s3 I  K v s 2  K p s  Ki
How to choose PID gains (using Routh’s criteria):

Ki  K p K v
G. Ablay, EE473 33
2- Computed-Torque Control

PID Outer-Loop Design

Choice of PID gains:
K p  diag{k pi } , K v  diag{kvi } , K i  diag{kii }

Then the characteristic equation of the error dynamics

    s 3  kvi s 2  k pi s  kii 
n

i 1

Closed-loop error dynamics are asymptotically stable as long as the control gains are
positive. The disturbance term must be bounded.
How to choose PID gains:

kii  k pi kvi

G. Ablay, EE473 34
2- Computed-Torque Control

Example: Two-link planar elbow arm

(m1  m2 )a12  m2 a22  2ms a1 a2 cos  2 m2 a22  m2 a1 a2 cos  2  1 
  
    2 
2 2
 m a
2 2 m a a
2 1 2 cos 2 m a
2 2

 m2 a1 a2 (21 2   22 ) sin  2  (m1  m2 ) ga1 cos 1  m2 ga2 cos(1   2 )   T1 

    T 
 m a a 
2 1 2 1
2
sin  2   m 2 ga 2 cos(1   2 )   2

System parameters
(𝑥2 , 𝑦2 )
mi  1, ai  1, g  9.8 𝑦 𝑚2
Desired trajectory: 𝑎2 𝜃2
1d  g1 sin(2 t / T ) ,
 2 d  g 2 sin(2 t / T ), T  2, gi  0.1
𝑎1
𝑚1
Controller design
𝜃1
Desired time-constant:   0.1 s 𝑥
𝑜
1
  n  10
n
Controller : K p  n2  100 , K v  2n  20 , K i  10?
G. Ablay, EE473 35
• Finding Model of a Robot

G. Ablay, EE473 36
 Step 1: Identify Model Mechanics
Example: 2-link robotic arm

Source: Peter R. Kraus, 2-link arm dynamics

G. Ablay, EE473 37
Step 2: Identify Parameters

• For each link, find or calculate

– Mass, mi
– Length, li m1
– Center of gravity, lCi
– Moment of Inertia, Ii

I1=m1l12 / 3

G. Ablay, EE473 38
Step 3: Formulate Lagrangian

• Lagrangian L defined as difference

between kinetic and potential energy:
L ( q, q )  K  U
 L is a scalar function of q and dq/dt
 L requires only first derivatives in time

G. Ablay, EE473 39
 Kinetic and Potential Energies
 Kinetic energy of individual links in an n-link arm
n
1 1 T
K i  mi vCi vCi  i I i i
T
K   Ki
2 2 i 1

 Potential energy of individual links

U i  mi lCi g sin(i )h0i Height of

link end

G. Ablay, EE473 40
 Energy Sums (2-Link Arm)
sin 2 q  cos 2 q  1
sin q1  sin(q1  q2 )  cos q1  cos(q1  q2 )  cos q2

• K = sum of kinetic energies:

K

• U = sum of potential energies:

U

G. Ablay, EE473 41
Step 4: Equations of Motion

• Calculate partial derivatives of L wrt qi, dqi/dt

and plug into general equation:

Non-conservative Forces
Inertia Conservative (damping, inputs)
(d2qi/dt2) Forces

G. Ablay, EE473 42
Equations of Motion – Structure
• M – Inertia Matrix
– Positive Definite
– Configuration dependent
– Non-linear terms: sin(θ), cos(θ)
• C – Coriolis forces
– Non-linear terms: sin(θ), cos(θ),
(dθ/dt)2, (dθ/dt)*θ
• Fg – Gravitational forces
– Non-linear terms: sin(θ), cos(θ)

G. Ablay, EE473 43
 Equations of Motion for 2-Link Arm,
Relative coordinates
M- Inertia matrix

Conservative forces
Coriolis forces, c(θi,dθi/dt)
(gravity)

Source: Peter R. Kraus, 2-link arm dynamics

G. Ablay, EE473 44
 Alternate Form: Absolute Joint Angles
• If relative coordinates are written as θ1’,θ2’, substitute θ1=θ1’ and θ2=θ2’+θ1’

 Advantages:
 M matrix is now symmetric
 Cross-coupling of 𝜃 eliminated from C, 𝜃 from F matrices
 Simpler equations (easier to check/solve)
G. Ablay, EE473
45
 Matlab Code
function xdot= robot_2link_abs(t,x)
global T
%parameters
g = 9.8;
m = [10, 10];
l = [2, 1];%segment lengths l1, l2
lc =[1, 0.5]; %distance from center
i = [m(1)*l(1)^2/3, m(2)*l(2)^2/3]; %moments of inertia i1, i2, need to validate coef's
c=[100,100];
xdot = zeros(4,1);

%matrix equations
M= [m(2)*lc(1)^2+m(2)*l(1)^2+i(1), m(2)*l(1)*lc(2)^2*cos(x(1)-x(2));
m(2)*l(1)*lc(2)*cos(x(1)-x(2)),+m(2)*lc(2)^2+i(2)];

C= [-m(2)*l(1)*lc(2)*sin(x(1)-x(2))*x(4)^2;
-m(2)*l(1)*lc(2)*sin(x(1)-x(2))*x(3)^2];

Fg= [(m(1)*lc(1)+m(2)*l(1))*g*cos(x(1));
m(2)*g*lc(2)*cos(x(2))];

T =[0;0]; % input torque vector

tau =T+[-x(3)*c(1);-x(4)*c(2)]; %input torques,

xdot(1:2,1)=x(3:4);
xdot(3:4,1)= M\(tau-Fg-C);

G. Ablay, EE473 46
 Matlab Code

t0=0;tf=20;

x0=[pi/2 0 0 0];

[t,x] = ode45('robot_2link_abs',[t0 tf],x0);

figure(1)

plot(t,x(:,1:2))

Title ('Robotic Arm Simulation for x0=[pi/2 0 0 0]and T=[sin(t);0] ')

legend('\theta_1','\theta_2')

G. Ablay, EE473 47
 Open Loop Model Validation
Zero State/Input

 Arm falls down and settles in that position

G. Ablay, EE473 48
 Open Loop - Static Equilibrium

x0= [-pi/2 pi/2 0 0]

x0= [-pi/2 –pi/2 0 0]

x0= [pi/2 pi/2 0 0]

x0= [pi/2 -pi/2 0 0]

Arm does not change its position- Behavior is as expected

G. Ablay, EE473 49
 Open Loop - Step Response
Torque applied to second joint
Torque applied to first joint

 When torque is applied to the first joint, second joint falls down

 When torque is applied to the second joint, first joint falls down

G. Ablay, EE473 50
 Input (torque) as Sine function
Torque applied to first joint
Torque applied to first joint

 When torque is applied to the first joint, the first joint oscillates and
the second follows it with a delay
 When torque is applied to the second joint, the second joint oscillates
and the first follows it with a delay

G. Ablay, EE473 51
Units:
Quantities SI unit
Time – t second (s)
Mass – m kilogram (kg)
Length – l meter (m)
Force – f Newton (N)
Energy – w Joule (J) [W-s, N-m]
Power – p Watt (W) [J/s]
Spring Constant – k (N/m)
Friction Coefficient – b (N-s/m)
Velocity – v (m/s)
Acceleration – a (m/s2)
Inertia – J (kg-m2)
Angular position – θ (rad)
Angular velocity – ω (rad/s)

G. Ablay, EE473 52