COMPUTED TORQUE CONTROL

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

Force, inertia, and Energy

mv2
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

Dynamics of a Two-Link Planar Elbow Arm

Assoc. Prof. Dr Tuong Quan Vo 1

1) kinetic energy & potential energy
1
For link1 : K1  m(a11 ) 2 P1  m1 ga1  s i n1
2

For link2 : x2  a1  cos  2  a2 cos(1   2 )

y2  a1  sin  2  a2 sin(1   2 )
x 2  a1    sin 1  a2 (1  2 ) sin(1   2 )
y  a    cos  a (   ) cos(   )
2 1 1 2 1 2 1 2

velocity squared :
v2  x2  y 2  a1  1  a2 (1  2 ) 2  2a1a2 (1  12 ) cos  2
2 2 2 2 2 2 2

1 1 1
∴ K 2  m2 v2 2  m2 a1 2  1 2  m2 a 2 2 (1  2 ) 2  m2 a1a 2 (1 2  12 ) cos 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 
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

Assoc. Prof. Dr Tuong Quan Vo 2

 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
 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

∴  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 )

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

2

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

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

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

 
  1
 2 

Assoc. Prof. Dr Tuong Quan Vo 3

Manipulator in the standard form

Mx cx kx F
M ( q ) q V ( q, q ) G (q)

Inertia Coriolis/centripetal Gravity

matrix vector vector

* M(q) is symmetric.

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

Assoc. Prof. Dr Tuong Quan Vo 4

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

u  qd  M 1 ( N   ) ②

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

↘called "Computed-torque control law."

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

trajectory.
(1) Selecting control input u(t) as PD :
④    M (qd  uPD )  N
u PD Kp e Kd e
Then, equation ④ becomes M (qd Kd e K p e) N
Now, we want to find gains for u.
Closed-loop error dynamics
e Kd e Kp e w e  u  w
(←③):
Kd e Kp e w
e Kp e Kd e w
e Kd e Kp e w
 in state-space form :
d e 0 I e o
w
dt e Kp Kd e I

Closed-loop characteristic equation :

Assoc. Prof. Dr Tuong Quan Vo 5

 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)

[Ex] For the robot dynamic equation derived before, desired trajectory
qd (t ) has the components:

1d  g1 sin( 2t / T )

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

,where T=2s , amplitudes g1=g2=0.1 rad, kp=100, kd=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.
[Example of MATLAB code]
%FILE NAME=ct_pd.m
%Computed torque method for 2-D robot arm
global m_inv;
global n;
global torq;

g=9.806;
g1=0.1;
g2=0.1;
fact=3.14;
kp=100;
kv=20;
m1=1;m2=1;a1=1;a2=1;
x=[0 0.1 0 0]; %theta1, dtheta1, theta2,dtheta2

Assoc. Prof. Dr Tuong Quan Vo 6

temp=[0];

for i=0:500
ts=i*0.01;
tf=(i+1)*0.01;
rowx_out=size(x,1);
x_end=x(rowx_out,:);

th_d=[g1*sin(fact*ts) g2*cos(fact*ts)];
dth_d=[g1*fact*cos(fact*ts) -g2*fact*sin(fact*ts)];
ddth_d=[-g1*fact^2*sin(fact*ts) -g2*fact^2*cos(fact*ts)];

e=[th_d(1)-x_end(1) th_d(2)-x_end(2)];
de=[dth_d(1)-x_end(3) dth_d(2)-x_end(4)];

m=[(m1+m2)*a1^2+m2*a2^2+2*m2*a1*a2*cos(x_end(2))
m2*a2^2+m2*a1*a2*cos(x_end(2))
m2*a2^2+m2*a1*a2*cos(x_end(2)) m2*a2^2];

m_inv=inv(m);
temp=-m2*a1*a2*(2*x_end(3)*x_end(4)+x_end(4)^2)*sin(x_end(2));

n=[temp+(m1+m2)*g*a1*cos(x_end(1))+m2*g*a2*cos(x_end(1)+x_end(2
))

m2*a1*a2*x_end(3)^2*sin(x_end(2))+m2*g*a2*cos(x_end(1)+x_end(2))]'
;

s=[ddth_d(1)+kv*de(1)+kp*e(1) ddth_d(2)+kv*de(2)+kp*e(2)];

torq=[m(1,1)*s(1)+m(1,2)*s(2)+n(1) m(1,2)*s(1)+m(2,2)*s(2)+n(2)];

[t,x_o]=ode45('xdot',[ts tf],x_end);
rowx_o=size(x_o,1);
x(rowx_out+1,:)=x_o(rowx_o,:);

theta(i+1,:)=th_d;
ee(i+1,:)=e;

Assoc. Prof. Dr Tuong Quan Vo 7

tq(i+1,:)=torq;
subplot(3,1,1);
plot(theta), title('position and velocity');
subplot(3,1,2);
plot(ee), title('position error');
subplot(3,1,3);
plot(tq), title('control torque');

end

%% FILE NAME=xdot.m
function x_dot=xdot(t,x)
global m_inv;
global n;
global torq;
x_dot=[
x(3)
x(4)
m_inv(1,1)*(-n(1)+torq(1))+m_inv(1,2)*(-n(2)+torq(2))
m_inv(1,1)*(-n(1)+torq(1))+m_inv(2,2)*(-n(2)+torq(2))
];

Assoc. Prof. Dr Tuong Quan Vo 8

 ## Desired trajectories/Errors for angle and rate angle/Control
torques

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

u Kp e Ki Kd e ( edt )
Substituting u into computed-torque control (   M (qd  u )  N ),
M (qd Kp e Ki K d e) N

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

dynamic system.
e K p e Ki Kd e w

0 1 0 0
d
e 0 0 1 e 0w
dt
e Ki Kp Kd e 1

Closed-loop error dynamic’s characteristic equation :

c ( s) S3 Kd S 2 Kp S Ki
By Routh test K ii  K vi  K pi

Assoc. Prof. Dr Tuong Quan Vo 9

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

Assoc. Prof. Dr Tuong Quan Vo 10

Assoc. Prof. Dr Tuong Quan Vo 11
Assoc. Prof. Dr Tuong Quan Vo 12