Scribd
Upload
179 views

Compute Torque Control

This document discusses computed torque control for robotic systems. It begins by deriving the Lagrange-Euler dynamics equations of motion for a two-link planar robotic arm. It then describes how computed torque control uses feedback linearization to make the tracking error dynamics linear, even though the actual system dynamics are nonlinear. The control input is defined as the desired acceleration plus a transformation of the nonlinear terms. A PD controller is selected to stabilize the linearized error system. Simulation results are shown for a two-link arm tracking a desired trajectory using computed torque control with PD feedback.

Uploaded by

nhozcaibang
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
179 views

Compute Torque Control

This document discusses computed torque control for robotic systems. It begins by deriving the Lagrange-Euler dynamics equations of motion for a two-link planar robotic arm. It then describes how computed torque control uses feedback linearization to make the tracking error dynamics linear, even though the actual system dynamics are nonlinear. The control input is defined as the desired acceleration plus a transformation of the nonlinear terms. A PD controller is selected to stabilize the linearized error system. Simulation results are shown for a two-link arm tracking a desired trajectory using computed torque control with PD feedback.

Uploaded by

nhozcaibang
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 12

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 =

mv 2
mrw 2 mr 2
r

linear velocity : v w r

coriolis force : Fcor 2mwo v

1
2

kinetic energy (k) : k mv 2


1
2

rotational kinetic energy : k rot I 2


potential energy (p) : P mgh
Dynamics of a Two-Link Planar Elbow Arm
1

1) kinetic energy & potential energy


1
2

For link1 : K1 m(a11 ) 2

P1 m1 ga1 s i n
1

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

velocity squared :
v2 x2 y 2 a1 1 a2 (1 2 ) 2 2a1a2 (1 12 ) cos 2
2

1
1
1
2
2
2
P2 m2 g y2 m2 g[a1 sin 1 a2 sin(1 2 )]

K 2 m2 v 2 2 m2 a1 2 1 2 m2 a 2 2 (1 2 ) 2 m2 a1 a 2 (1 2 12 ) cos 2

2) Lagrange's equation

d L L

)
dt q q




q 1 , q 1 , 1

2
2
2

L K P ( K1 K 2 ) ( P1 P2 )

1
1
2
2
2
2
(m1 m2 )a1 1 m2 a2 (1 2 ) 2 m2 a1a2 (1 12 ) cos 2
2
2
(m1 m2 ) ga1 sin 1 m2 ga2 sin(1 2 )

L
2
2
( m1 m2 ) a1 1 m2 a2 (1 2 ) m2 a1a2 ( 21 2 ) c o s 2

d L
2
2
( m1 m2 ) a1 1 m2 a2 (1 2 ) m2 a1a2 ( 21 2 ) c o s 2

dt 1
2
m2 a1a2 (212 1 ) sin 2

L
( m1 m2 ) ga1 cos1 m2 ga2 cos(1 2 )
1

L
2
m2 a2 (1 2 ) m2 a1a21 cos 2

2
d L
2
m2 a2 (1 2 ) m2 a1a21 cos 2 m2 a1a212 sin 2

dt 2
L
2
m2 a1a2 (1 12 ) sin 2 m2 ga2 cos(1 2 )
2

1 [( m1 m2 )a12 m2 a2 2 2m2 a1a2 c o s 2 ]1


2
2
[m2 a2 m2 a1a2 cos 2 ]2 m2 a1a2 (212 1 ) sin 2

(m1 m2 ) ga1 cos1 m2 ga2 cos(1 2 )

2 [m2 a2 2 m2 a1a2 cos 2 ]1 m2 a2 22 m2 a1a212 sin 2


m2 ga2 cos(1 2 )

(m1 m2 )a12 m2 a2 2 2m2 a1a2 c o s 2

2
m2 a2 m2 a1a2 c o s 2

2
m2 a2 m2 a1a2 c o s 2 1

2
m2 a 2
2

m2 a1a2 (212 12 ) sin 2 (m1 m2 ) ga1 cos1 m2 ga2 cos(1 2 )

2 sin
m2 ga2 cos(1 2 )
m
a
a

2 1 2 1
2



1
2

Manipulator in the standard form

Mx

cx

M (q)q
Inertia
matrix

kx

V (q, q )

G (q)

Coriolis/centripetal
vector

Gravity
vector
3

* 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 qd q
e qd M 1 ( N d )

M (qd e) ( N d )
qd e M 1 ( N d )
e qd M 1 ( N d )

Defining control input u and disturbance function w ,


u qd M 1 ( N )

---

w M 1 d

then e u w
Defining state x :
e
x
e

Then, tracking error dynamics :


0
d e 0 I e 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
M (qd 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 :
u

Kd e

Kp e

Then, equation becomes

M ( qd

Kd e

K p e)

Kp e

Now, we want to find gains for u.


Closed-loop error dynamics
e

Kd e

Kp e

Kp e

Kd e

e u w
():

Kd e

Kd e

Kp e

in state-space form :
d e
dt e

Kp

Kd e

Closed-loop characteristic equation :


Using | sI A | 0
c (s) S 2 K v S K p 0

Where, Kv diag{Kvi } ,

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(2t / T )
2d g2 cos(2t / T )

,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;
6

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

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

## 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 (qd u) N ),


M ( qd

Kp e

Ki

K d e)

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


dynamic system.
e
K p e Ki
Kd e w
d
e
dt
e

Ki

Kp

Kd e

0w
1

Closed-loop error dynamics characteristic equation :


c (s)

By Routh test

S3

Kd S 2

Kp S

Ki

K ii K vi K pi
9

Gain selection: integral gain should not be too large.

10

11

12

You might also like