CHAPTER 2: COMPUTED TORQUE CONTROL

Lagrange-Euler Dynamics
i) Derive kinetic and potential energy
ii) Use Lagrange's equations of motion

Force, inertia, and Energy

mv 2
Centripetal force : F cent =  mrw2  mr 2
r

  
linear velocity : v  w  r
  
coriolis force : Fcor  2mwo  v

1
kinetic energy (k) : k  mv 2
2
1
rotational kinetic energy : k rot  I 2
2

potential energy (p) : P  mgh

1
Dynamics of a Two-Link Planar Elbow Arm

1) kinetic energy & potential energy

1
For link1 : K1  m1 (a11 ) 2 P1  m1 ga1  s i n1
2
For link2 : x2  a1  cos 1  a2 cos(1   2 )
y2  a1  sin 1  a2 sin(1   2 )
x2  a1 1  sin 1  a2 (1  2 ) sin(1   2 )
y 2  a1  1  cos 1  a2 (1  2 ) cos(1   2 )
velocity squared :

v2 2  x 2 2  y 2 2  a12  12  a2 2 (1  2 ) 2  2a1a2 (12  12 ) co

∴
1 1 1
K 2  m2 v2  m2 a1  1  m2 a2 (1  2 ) 2  m2 a1a2 (1  12 ) cos 2
2 2 2 2 2

2 2 2
P2  m2  g  y2  m2  g[a1  sin 1  a2  sin(1   2 )]

d L L      
2) Lagrange's equation (  ) q   1 , q   1 ,   1 
dt q q  2  
 2   2 

2
 L  K  P  ( K1  K 2 )  ( P1  P2 )
1 1
 (m1  m2 )a1 1  m2 a2 (1  2 ) 2  m2 a1a2 (1  12 ) cos 2
2 2 2 2

2 2
 (m1  m2 ) ga1  sin 1  m2 ga2  sin(1   2 )
L
 (m1  m2 )a1 1  m2 a2 (1  2 )  m2 a1a2 (21  2 ) cos 2
2 2

1
d L
 (m1  m2 )a1 1  m2 a2 (1  2 )  m2 a1a2 (21  2 ) cos 2
2 2

dt 1

 m2 a1a2 (212  1 ) sin  2

2

L
 (m1  m2 ) ga1  cos1  m2 ga2  cos(1   2 )
1

L  K  P  ( K1  K 2 )  ( P1  P2 )
1 1
 (m1  m2 )a1 1  m2 a2 (1  2 ) 2  m2 a1a2 (1  12 ) cos 2
2 2 2 2

2 2
 (m1  m2 ) ga1  sin 1  m2 ga2  sin(1   2 )

L
 m2 a2 (1  2 )  m2 a1a21 cos 2
2

 2
d L
 m2 a2 (1  2 )  m2 a1a21 cos 2  m2 a1a212 sin  2
2

dt 2
L
  m2 a1a2 (1  12 ) sin  2  m2 ga2  cos(1   2 )
2

 2

d L L d L L
  1  2
dt 1 1 dt 2  2

∴  1  [(m1  m2 )a12  m2 a2 2  2m2 a1a2  c o s 2 ]1

 [m2 a2  m2 a1a2  cos  2 ]2  m2 a1a2 (212  1 ) sin  2

2 2

 (m1  m2 ) ga1  cos 1  m2 ga2  cos(1   2 )

 2  [m2 a2 2  m2 a1a2  cos  2 ]1  m2 a2 22  m2 a1a212 sin  2

 m2 ga2  cos(1   2 )

3
 (m1  m2 ) a12  m2 a2 2  2m2 a1a2  c o s 2 m2 a2  m2 a1a2  c o s 2  1 
2

    
m2 a2  m2 a1a2  c o s 2   2 
2 2
 m2 a 2

 m2 a1a2 (212  12 ) sin  2  (m1  m2 ) ga1  cos 1  m2 ga2  cos(1   2 )

 
m2 a1a21 sin  2  m2 ga2  cos(1   2 )
2
 

 
  1
 2 

Manipulator in the standard form

M (q )q  V (q, q )  G (q )  

Inertia Coriolis/centripetal Gravity

matrix vector vector

* M(q) is symmetric.

4
Computed-Torque Control
(⇒ feedback linearization of nonlinear system)
Consider robot arm dynamics :

M (q )q  N (q, q )   d   ---①

,where d : disturbance/noise,  : control torque

Define tracking error as e(t )  qd (t )  q (t )

then,
e  qd  q
e  qd  q M (qd  e)  ( N   d   )
qd  e   M 1 ( N   d   )
①⇒ e  qd  M 1 ( N   d   )  e  qd  M 1 ( N   d   )

Defining control input u and disturbance function w ,

u  qd  M 1 ( N   ) ---②
w  M 1 d
then e  u  w
Defining state x :
e 
x 
e
Then, tracking error dynamics :
d e  0 I  e   0  0
        u    w ---③ ????
dt e  0 0 e   I  I 

↖linear error system

★ The dynamic sytem itself is nonlinear, but the tracking error

dynamic is linear.
② looks like feedback linearizing transformation

5
 ②⇒   M (qd  u )  N ---④

↘called "Computed-torque control law."

★ If we selects u(t) stabilizing ③ so that e(t) goes to zero, llow

roboajectory.
(1) Selecting control input u(t) as PD :

  M (qd  u )  N
u   K p  e  K v  e
Then, equation ④ becomes   M (qd  K v  e  K p  e)  N
Now, we want to find gains for u.
Closed-loop error dynamics
e  K v  e  K p  e  w e  u  w   K v  e  K p  e  w
(←③):
e  K v  e  K p  e  w
e   K p  e  K v  e  w
 in state-space form :
d e  0 I  e o
 
dt e  K p  K v  e  I 
w

Closed-loop characteristic equation :

Using | sI  A | 0
 c ( s)  S 2  K v S  K p  0
Where, K v  diag{K vi} , K p  diag{K pi }
∴ Error system is asymptotically stable if K vi and K pi are all positive
(using Routh-Hurwitz table)

6
[Ex] Calculate the robot dynamic equation of the two elbow arm,
desired trajectory qd (t ) has the components:
1d  g1 sin( 2t / T )

 2 d  g 2 cos( 2t / T )

,where T=2s , amplitutes g1=g2=0.1 rad, kp=100, kv=20,

m1=1, m2=1, a1=1, a2=1,
initial condition, x=[0 0 0 0]:theta1, theta2, dtheta1,dtheta2
Simulate PD computed torque control.

7
(2) Selecting control input u(t) as PID :

u   K v  e  K p  e  K i   (  e)
Substituting u into computed-torque control (   M (qd  u )  N ),
  M (qd  K v  e  K p  e  K i   )  N

Now, in order to find Kv and Kp, we consider closed-loop error

dynamic system.
e   K v  e  K p  e  K i    w

   0 1 0    0
d    
e  0 0 1   e   0 w
dt   
 e   K i  Kp  K v   e  1

Closed-loop error dynamic’s characteristic equation :

 c ( s)  S 3  K v S 2  K p  S  K i
By Routh test K ii  K vi  K pi

∴ Gain selection: integral gain should not be too large.

(3) Approximate computed-torque controller

In computed-torque control law :
 c  M (qd  u )  N ---①
But, in reality, M is not known exactly (ex. payload mass 
unknown)
Then, best estimate Mˆ  M (q), Nˆ  N (q.q )

We can avoid computing

M,N at every sample time

∴  c  Mˆ (qd  u )  Nˆ ---②
(FACT) Even if Mˆ  M and Nˆ  N the performance can be quite
good if outer-loop gains (PID or PD) are selected large enough.

8
(4) PD-plus-Gravity controller
When Mˆ  I , Nˆ  G (q )  qd , u   K v  e  K p  e  ②

Then,  c  Mˆ (qd  K v  e  K p  e)  Nˆ
 qd  K v  e  K p  e  G (q)  qd
⇒  c  K v  e  K p  e  G (q)
(FACT) • When arm is at rest, the only nonzero terms in the
dynamics are gravity G(q), disturbance. The PD-gravity controller  c

include G(q), so that we would expect good performance for set-point

tracking, that is, when a constant q d is given so that q d  0 .

(5) Classical Joint Control

: A simple controller in practice
If we select, Mˆ  I , Nˆ  qd from ②

then,  c  u ( u i depend on joint variable i, ∴ independent joint

control)
* do not involve nonlinear robot dynamics.
i) PD control
  K v  e  K p  e
u   K v e  K p e

ii) PID control

  K v  e  K p  e  K i   (  e)
u   K v e  K p e  K i

Theorem
Suppose that PD-gravity control is used in arm dynamics:
M (q )q  N (q, q)   d   ,

and  d  0 , q d  0 . Then, steady-state tracking error e  qd  q is

9
zero.
(Proof) F.L.Lewis, “Control of Robot Manipulators,” Macmillan
publishing company.
Theorem
If classical PD joint control is applied to each joint and e(0)=0,
e (0)=0, the position and velocity tracking errors are bounded within a
1
ball whose radius decreases approximately (for large K v ) as .
Kv
Note
PD joint control is often suitable for tracking control. However,
steady-state error exists due to gravity. This can be removed using PID
joint control law.

Actuator Saturation and Integrator Windup.

• Implementing PID control on actual robot, there may be limits on
the voltages and torques
• Problems called with integral control due to so called "integrator
windup"

ex)
  Ki   , where  : intergrator output

10
11
ex)

Feedback system with actuator saturation

When a big overshoot and a long transient response happen, we can

solve the problem by shutting down the integral action in the controller.
⇒ integrator anti-windup.

∴ In case of PI controller
• If actuator saturated, input to the integral controller of PI
controller: ei  0

• If actuator not saturated, use normal PI controller.

12
13
14
15