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