1

COMPUTED TORQUE CONTROL

Lagrange-Euler Dynamics

i) Derive kinetic and potential energy

ii) Use Lagrange's equations of motion

Force, inertia, and Energy

Centripetal force : F cent = 222

mrmrwr

mv

linear velocity : rwv

coriolis force : vwmF ocor

2

kinetic energy (k) : 22

1mvk

rotational kinetic energy : 22

1Ikrot

potential energy (p) : mghP

Dynamics of a Two-Link Planar Elbow Arm

2

1) kinetic energy & potential energy

For link1 : 2111 )(2

1amK 1111 s i n gamP

For link2 : )cos(cos 212212 aax

)cos()(cos

)sin()(sin

)sin(sin

21212112

21212112

212212

aay

aax

aay

velocity squared :

2212

121

2

21

2

2

2

1

2

1

2

2

2

2

2

2 cos)(2)( aaaayxv

2212

1212

2

21

2

22

2

1

2

12

2

222 cos)()(2

1

2

1

2

1 aamamamvmK

)]sin(sin[ 212112222 aagmygmP

2) Lagrange's equation )(

q

L

q

L

dt

d

2

1

2

1

2

1,,

qq

)()( 2121 PPKKPKL

2212

1212

2

21

2

22

2

1

2

121 cos)()(2

1)(

2

1 aamamamm

)sin(sin)( 21221121 gamgamm

221212212

221

2

121

1

c o s)2()()(

aamamamm

L

3

221212212

221

2

121

1

c o s)2()()(

aamamamm

L

dt

d

22

121212 sin)2( aam

)cos(cos)( 212211211

gamgamm

L

)cos(sin)(

sincos)(

cos)(

2122221

2

1212

2

2212122121221

2

22

2

2121221

2

22

2

gamaamL

aamaamamL

dt

d

aamamL

122122

22

2

1211 ]c o s2)[( aamamamm

22

12121222212

2

22 sin)2(]cos[ aamaamam

)cos(cos)( 21221121 gamgamm

22

12122

2

2212212

2

222 sin]cos[ aamamaamam

)cos( 2122 gam

2

1

2

222212

2

22

2212

2

222212

2

22

2

121

c o s

c o sc o s2)(

amaamam

aamamaamamamm

)cos(

)cos(cos)(

sin

sin)2(

2122

21221121

2

2

1212

2

2

121212

gam

gamgamm

aam

aam

2

1

Manipulator in the standard form

( ) ( , ) ( )

Mx cx kx F

M q q V q q G q

Inertia matrix

Coriolis/centripetal

vector

Gravity

vector

4

* M(q) is symmetric.

Computed-Torque Control

( feedback linearization of nonlinear system)

Consider robot arm dynamics :

dqqNqqM ),()( ---

,where d : disturbance/noise, : control torque

Define tracking error as )()()( tqtqte d

then,

qqe

qqe

d

d

)(1 dd NMqe

Defining control input u and disturbance function w ,

)(1 NMqu d ---

dMw 1

then wue

Defining state x :

e

ex

Then, tracking error dynamics :

0 0 0

0 0

e I edu w

e e I Idt

---

linear error system

The dynamic sytem itself is nonlinear, but the tracking error

)()( dd NeqM )(1 dd NMeq )(

1 dd NMqe

5

dynamic is linear.

looks like feedback linearizing transformation

NuqM d )( ---

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 :

d pu K e K e

Then, equation becomes ( )d d pM q K e K e N

Now, we want to find gains for u.

Closed-loop error dynamics

d pe K e K e w ():

p de K e K e w

d pe K e K e w

in state-space form :

0

p d

Ie e odw

K Ke e Idt

Closed-loop characteristic equation :

Using 0|| AsI

0)( 2 pvc KSKSs

Where, }{ viv KdiagK , }{ pip KdiagK

Error system is asymptotically stable if viK and piK are all positive (using Routh-Hurwitz table)

wue

d pK e K e w

6

[Ex] For the robot dynamic equation derived before, desired

trajectory )(tqd has the components:

)/2sin(11 Ttgd

)/2cos(22 Ttgd

,where T=2s , amplitudes 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.

[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

temp=[0];

for i=0:500

ts=i*0.01;

tf=(i+1)*0.01;

7

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;

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);

8

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

];

9

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

torques

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

p i du K e K K e ( )edt

Substituting u into computed-torque control ( NuqM d )( ),

( )d p i dM q K e K K e N

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

dynamic system.

p i de K e K K e w

0 1 0 0

0 0 1 0

1i p d

de e w

dte K K K e

Closed-loop error dynamics characteristic equation :

3 2( )c d p is S K S K S K

By Routh test piviii KKK

10

Gain selection: integral gain should not be too large.

11

12