Dynamic Equations of Motion for a

Two-Link
Two
Link Robotic Manipulator

Mechanical Systems - Translational motion

x(t)
K
M

f(t)

Mass - kinetic energy

Stiffness - potential energy
Damper , Friction

Equations of Mechanical Systems

Differential equation

d 2 x(t )
dx(t )
f (t ) M

Kx(t )
2
dt
dt
Dynamic equation

Mx(t ) x (t ) Kx(t ) f (t )
Transfer function

X (s)
1

2
F ( s ) Ms s K

Two-link manipulator with point masses at distal end of links

A simple two
two-rigid-link
rigid link robot manipulator shown in
Figure, and modelled as a set of nonlinear coupled
differential equations to use as a vehicle for
confirming
fi i
allll th
the algorithms
l
ith
developed
d
l
d in
i this
thi book.
b k
Although it is a simple two-link manipulator,
the dynamic equations of the manipulator are
multi-variable, highly non-linear, and contain
/ time-varying
y g
frictions,, disturbances,, unknown and/or
Paremeters.
The model illustrates all of the necessary features,
without
itho t being very
er complicated,
complicated for testing the
control algorithms developed in this book.
We use these equations in simulations throughout the
book.
4

Dynamic Equations
For simplicity, we assume that all mass exists as a point mass at the distal end of each link.

m1 , m2

masses of each link

l1 , l2

lengths of each link

The equations of motion for this manipulator are

m2l22 (q1 q2 ) m2l1l2 c2 (2q1 q2 ) (m1 m2 )l12 q1

m2l1l2 s2 q22 2m2l1l2 s2 q1q2 m2l2 qs12 (m1 m2 )l1 gs1
v1q1 k1sgn(q1 )

where

m2l22 (q1 q2 ) m2l1l2 c2 q1 m2l1l2 s2 q12 m2l2 gs12

v2 q2 k2 sgn(q2 )

c1 : cos(q1 ) ,

The parameters

vi

and

s12 : sin(q1 q2 )
ki

, etc.
etc .

are viscous and Coulomb friction coefficients, respectively.

Form of Dynamic Equations

The equation can be written in the form of

Mq Vq G Fq d
where

M
M 11
M 21

hq2
V
hq1

M 12
, with
M 22

M 11 m1l12 m2 [l12 l22 2l1l2 cos (q2 )]

M 12 m2l1l2 cos (q2 ) m2l22
M 22 m2l22

hq1 hq2
, with h m2l1l2 sin(q2 )
0

G m l gcos(q1 ) m2 g[l2 cos(q1 q2 ) l1cos(q1 )]

G 1 11

m2l2 gcos(q1 q2 )
G2

F v q
F 1 1 1
F2 v2 q2

k sgn(q1 )
d d1 1

k
sgn
(
q
)
2
d2 2

n actuator torque vector

q, q , q n position, velocity, acceleration
M (q ) nn mass matrix
V (q, q ) n centrifugal and Coriolisforcevector
G (q ) n gravity torque vector
F ( q ) n joint friction torque vector

d n

uncertainty torque vector

Computed Torque Control

of Robot Manipulators

Mechanical Systems - Translational motion

x(t)
K
M

f(t)

Mass - kinetic energy

Stiffness - potential energy
Damper , Friction

open-loop dynamic equation

mx bx kx f

(2)

Computed Torque Control Design Step 1

Two-Step Design

Step 1
Asume that m, b, k are known
model based control law
m,b,k are obatained by real-time calculation
The system is simplified to mass-only system
Apply a model-based control law to Eq. (2)

f mf s bx kx

(3)

Structure of Contpued Torque Control

where

fs

is the new-input of the system as shown in the figure.

y
will be simplyfied
py
as mass-onlyy second-order system
y
Then, the system

x fs

((4))

fs

.. .
f=mx+bx+kx

bx+kx

10

Step 2 of Computed Torque Control

Step 2
Step 1: Simplify the system
Step 2: Apply classical control law. Ex. PID, Pole-placement

Apply servo contorl law to (4)

For example,

f s
xd K v e K p e :
xd u

We see that

u K v e K p e

(5)

is the PD(position-defivative) feedback loop.

11

Structure of Computed Torque Control

Plant: Eq.(2)
Model-based Control law: Eq.(3)
Servo-contorl law: Eq. (5)
= Computed Torque Control System

..

fs

xd

.. .
f=mx+bx+kx

Kp

bx+kx

Kv

xd
xd

12

Computed torque control of manipulators

Dynamic equations of robot manipulator

M (q )q V (q, q ) G (q )

(10)

13

PD feedback loop Computed Torque Control

Model-based control law of Eq.(10)

M (q )(qd u ) V (q, q ) G (q )
Design

u (t )

(11)

with PD(propotional-plus-derivative) feedback loop

u K v e K p e

(12)

Apply Eq.(12)
Eq (12) to Eq.
Eq (11),
(11) the control law will be

M (q )(qd K v e K p e) V (q, q ) G (q )

(13)

14

PD feedback loop Computed Torque Contorl

Apply the control law (13) to the plant (10) will result the system closed-loop error
dynamic as

e K v e K p e 0
State-space form

d e 0

dt e K p

I e
K v e

Close-loop characteristic polynomial

c ( s ) | s 2 I K v s K p |
Total system

15

PD feedback loop computed torque control

Total system

..

qd

M(q)

.
M(q)+V(q,q)+G(q)

q
Ki
1
s

Kp

V(q,q)+G(q)
V(q,q)
G(q)

Kv

qd
qd

[Figure] Computed-torque controller

(a) Task space PD controller
p
PID controller
(b) Joint space
16

PID feedback loop computed torque control

PD computed-torque control is usful if
system parameters are known, no disturbance, --- which are impossible
exist steady
steady-state
state error
-> feedforwaard loop + integrator = PID computed-torque controller

-> Eq. (12) will become

(15)

u K v e K p e K i
Total control law

M (q )(qd K v e K p e K i ) V (q, q ) G (q )

(t )

= intgral of tracking error

(16)

e(t )

17

PID feedback loop Computed torque control

state-space form of closed loop error dynamics

0
d
e 0
dt
e K i

I
0
K p

I e

K v e

Closed -loop characteristic polynomial

c ( s ) | s 2 I K v s K p K i |
diagonal control gain

K d , K p , Ki

18

PID feedback loop computed torque control

Apply the control law to the plant, the the total system block diagram will be

..

qd

M(q)

M(q)+V(q,q)+G(q)

q
Ki
1
s

Kp

V(q,q)+G(q)

Kv

qd
qd

[Figure] Computed-torque controller

Joint space PID controller

19

Computed-Torque Robot controller Characteristics

Robot dynamics

M (q )q V (q, q ) G (q )

Tracking error

e(t ) qd (t ) q (t )

PD Computed Torque control law and closed-loop error dynamics (state space form)

M (q )(qd K v e K p e) V (q, q ) G (q )
d e 0

d e K p
dt

I e
K v e

PD-Gravity Control

K v e K p e G (q )
Classical PD Control

K v e K p e

20

Computed-Torque Robot controller Characteristics

PID Computed Torque control law

e
M (q )(qd K v e K p e K i ) V (q, q ) G (q )
closed-loop error dynamics (state space form)

0
d
e 0

dt
e K i

I
0
K p

I e

K v e

Classical PID Control

e
K v e K p e K i

21

Computed-Torque PID Robot Control

MATLAB Simulation
S u at o
function [XP,XD,E,DE,B]=ctorq2(T1,T)
% Computed torque control
% CTORQ(simulation_time, sampling_time)
mas=[2;2];amas=mas;ma1=mas;l1=1;l2=1;GRAV=9.8;
t=linspace(0,T1,T1/T);
nu=length(t);
% PID Gains
Kv=-[22.7 0;0 41.2];
Kp=-[14.6 0;0 14.8];
Ki=-[100 0;0 100];
% initial state
xx=[1;0.8;.2*pi;0];
e=[0;0]; ei=[0;0];
tt=0;tt1=0;i=1;

22

Computed-Torque PID Robot Control

MATLAB Simulation
S u at o
for i=1:nu
Xp = xx(1:2);
dXp= xx(3:4);
q1=xx(1); q2=xx(2); dq1=xx(3); dq2=xx(4);
t1=pi*tt;
s1=sin(t1); c1=cos(t1);
Xd=[1+.2*s1;1-.2*c1];
dXd=[.2*pi*c1;.2*pi*s1];
ddXd=[-.2*pi*pi*s1;.2*pi*pi*c1];
e=Xd-Xp;
de=dXd-dXp;
dei=e*T;
ei=ei+dei;
c1=cos(q1);c12=cos(q1+q2);
c2=cos(q2);s2=sin(q2);
i
M=[mas(1)+mas(2)*(2+2*c2),mas(2)*(1+c2);
mas(2)*(1+c2),mas(2)];
V=[-dq2, -dq1-dq2; dq1, 0]*[dq1;dq2]*mas(2)*s2;
G = [mas(1)*GRAV*c1+mas(2)*GRAV*(c12+c1);mas(2)*GRAV*c12];
1 G
1
2 G
12 1
2 G
12
23

Computed-Torque PID Robot Control

MATLAB Simulation
S u at o
b=M*(ddXd-Kv*de-Kp*e-Ki*ei)+V+G;

end
d

if tt >= 2;
amas(2)=mas(2)+1;
end
xx0=xx;
db=[0;0];
db=[1;1]*sin(4*pi*tt)*5; % Disturbance
k1=cplt(b,xx,amas,db)*T;
xx=xx0+k1*0.5;
k2=cplt(b,xx,amas,db)*T;
xx=xx0+k2*0.5;
db=[1;1]*sin(4*pi*(tt+T))*5;
k3=cplt(b,xx,amas,db)*T;
xx=xx0+k3;
k4=cplt(b,xx,amas,db)*T;
xx = xx0 + (k1 + 2 * k2 + 2 * k3 + k4) / 6;
XP(:,i)=Xp;
i
XD(:,i)=Xd;
i
d B(:,i)=b;
i b E(:,i)=e;
i
DE(:,i)=de;
i d
[e(1),e(2),norm(e)*10,norm(b),tt]
i=i+1;
tt=tt+T;

24

Computed-Torque PID Robot Control

MATLAB Simulation
S u at o
figure(1)
plot(t,E(1,:),t,E(2,:),'-.');grid
title('Position errors')
pause
figure(2)
plot(t,DE(1,:),t,DE(2,:),'-.');grid
title('Velocity errors')
pause
figure(3)
plot(t,XP(1,:),t,XD(1,:),'-.');grid
title('Link-1 positions');
pause
figure(4)
plot(t,XP(2,:),t,XD(2,:),'-.');grid
title('Link-2 positions');
pause
figure(5)
plot(t,B(1,:),t,B(2,:),'-.');grid
title('Torques')
il
pause

25

Cplt.m
function k1=cplt(b,xx,amas,db)
GRAV=9.8;
q1=xx(1); q2=xx(2); dq1=xx(3); dq2=xx(4);
c1=cos(q1);c12=cos(q1+q2);
c2=cos(q2);s2=sin(q2);
M=[amas(1)+amas(2)*(2+2*c2),amas(2)*(1+c2);
amas(2)*(1+c2),amas(2)];
V=[-dq2, -dq1-dq2; dq1, 0];
G = [amas(1)*GRAV*c1+amas(2)*GRAV*(c12+c1);amas(2)*GRAV*c12];
torq =b+db;
k1=[dq1;dq2;inv(M)*(torq-V*[dq1;dq2]*amas(2)*s2-G)];
end

26

Computed-Torque Robot Control PID via

MATLAB S
Simulation
u at o

function [XP,XD,E,DE,B]=ctorq_hi(T1,T,beta, gamma)

% Computed torque control - H_infinity
% CTORQ(simulation_time, sampling_time, beta, gamma)
mas=[2;2];amas=mas;ma1=mas;l1=1;l2=1;GRAV=9.8;
p
( , , / );
t=linspace(0,T1,T1/T);
nu=length(t);
tt=0;tt1=0;i=1;
% initial state
xx=[1;0.8;.2*pi;0];
e=[0;0]; ei=[0;0];

27

Computed-Torque Robot Control PID via

MATLAB S
Simulation
u at o

% PID Gain find using H_infinity theory

% 1. make stable system with some gains
Kv=[-4 0;0 -4]; Kp=[-5 0;0 -5];
% 2. get K using H_infty method
beta2=beta*beta;
gamma2=gamma*gamma;
A = [eye(2)*0, eye(2), eye(2)*0;
Kp, Kv, eye(2)*0;
eye(2),eye(2)*0,eye(2)*0];
B1= [eye(2)*0;eye(2);eye(2)*0];
B2= [eye(2)*0;eye(2);eye(2)*0];
C1 = [eye(2)*0, eye(2)*0, eye(2)];
C = [1 1 1 1];
Q=eye(6);
%[k,P]=lqr(A, B1, eye(4), beta2); %for H_2
%Q=C'*C;
P=lqgf(A,
l f
B2*B2'/beta2
b
- B1*B1'/gamma2, Q);
if min(eig(P)) < 1e-10;error('r too small, stop'),end
P,pause

28

Computed-Torque Robot Control PID via

MATLAB S
Simulation
u at o

p0=P(1:2,1:2);
p1=P(1:2,2+1:2+2);
p11=P(1:2,2+2+1:2+2+2);
l1 = beta2; r1=gamma2;
K1 = -inv(l1) * eye(2)' * p0 ;
K2 = -inv(l1) * eye(2)' * p1 ;
K3 = -inv(l1) * eye(2)' * p11;
%K0 = -inv(l1) * B2' * inv(A' - P*(B2*B2'/l1-B1*B1'/r1)) * P * B1 ;
Kp=Kp+K1;
Kv=Kv+K2;
Ki=K3;
Kp,Kv,Ki,pause

29

Computed-Torque Robot Control PID via

MATLAB S
Simulation
u at o
for i=1:nu

Xp = xx(1:2);
dXp= xx(3:4);
q1=xx(1); q2=xx(2); dq1=xx(3); dq2=xx(4);
t1=pi*tt;
s1=sin(t1); c1=cos(t1);
Xd=[1+.2*s1;1-.2*c1];
dXd=[.2*pi*c1;.2*pi*s1];
ddXd=[-.2*pi*pi*s1;.2*pi*pi*c1];
e=Xd-Xp;
de=dXd-dXp;
dei=e*T;
ei=ei+dei;
GRAV=9.8;
c1=cos(q1);c12=cos(q1+q2);
c2=cos(q2);s2=sin(q2);
M=[mas(1)+mas(2)*(2+2*c2),mas(2)*(1+c2);
mas(2)*(1+c2),mas(2)];
V=[-dq2, -dq1-dq2; dq1, 0]*[dq1;dq2]*mas(2)*s2;
G = [mas(1)*GRAV*c1+mas(2)*GRAV*(c12+c1);mas(2)*GRAV*c12];

30

Computed-Torque Robot Control PID via

MATLAB S
Simulation
u at o

b=M*(ddXd-Kv*de-Kp*e-Ki*ei)+V+G;
if tt >= 2;
amas(2)=mas(2)+1;
end
xx0=xx;
db=[0;0];
% db=[1;1]*sin(4*pi*tt)*5; % Disturbance
k1=cplt(b,xx,amas,db)*T;
xx=xx0+k1*0.5;
k2=cplt(b,xx,amas,db)*T;
xx=xx0+k2*0.5;
% db=[1;1]*sin(4*pi*(tt+T))*5;
k3=cplt(b,xx,amas,db)*T;
xx=xx0+k3;
k4=cplt(b,xx,amas,db)*T;
xx = xx0 + (k1 + 2 * k2 + 2 * k3 + k4) / 6;

end
d

XP(:,i)=Xp;
i
XD(:,i)=Xd;
i
d B(:,i)=b;
i b E(:,i)=e;
i
DE(:,i)=de;
i d
[e(1),e(2),norm(e)*10,norm(b),tt]
i=i+1;
tt=tt+T;

31

Computed-Torque Robot Control PID via

MATLAB S
Simulation
u at o

figure(1)
plot(t,E(1,:),t,E(2,:),'-.');grid
title('Position errors')
pause
figure(2)
plot(t,DE(1,:),t,DE(2,:),'-.');grid
title('Velocity errors')
pause
figure(3)
plot(t,XP(1,:),t,XD(1,:),'-.');grid
title('Link-1 positions');
pause
figure(4)
plot(t,XP(2,:),t,XD(2,:),'-.');grid
title('Link-2 positions');
pause
figure(5)
plot(t,B(1,:),t,B(2,:),'-.');grid
title('Torques')
il
pause

32

Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u

33

Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u

clear all;
home;
global m1 m2 L1 L2;
% Mass
Link2

[Kg]
m1 = 1; % at the end-point of gravity of Link1
m2 = 2;
% at the end-point of gravity of

% Length [m]
L1 = 1;
L2 = 1;

% of Link1
% of Link2

% Threshold of Friction Function [Nm]

Th1 = 5.50;
% of Motor1
Th2 = 0.90;
% of Motor2
disp('Initialization is done');
34

Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u

35

Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u

36

Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u
function torq = mvg(fbsig)
global

m1 m2 L1 L2
ddqs= [fbsig(1);fbsig(2)];
x(1)=fbsig(3);
x(2)=fbsig(4);
x(3)=fbsig(5);
x(4)=fbsig(6);
d11 = m1*L1*L1+m2*(L1*L1+L2*L2+2*L1*L2*cos(x(2)));
d12 = m2*L1*L2*cos(x(2))+m2*L2*L2;
( ( ))
;
d21 = d12;
d22 = m2*L2*L2;
D = [d11 d12; d21 d22];
h=m2*L1*L2*sin(x(2));
h=m2
L1 L2 sin(x(2));
v1 = [-h*x(4) -h*x(3)-h*x(4)];
v2 = [h*x(3) 0];
g1=m1*L1*9.8*cos(x(1))+m2*9.8*(L2*cos(x(1)+x(2))+L1*cos(x(1)));
g2=m2*L2*9.8*cos(x(1)+x(2));
2
2*L2*9 8* ( (1) (2))
G = [g1;g2];
torq = D*ddqs+[v1; v2]*[x(3); x(4)]+G;

end
37

Computed-Torque PD
Robot Control
Co t o Simulation
S u at o with
w t S
Simulink
u
function [sys, x0] = tlplnt(t, x, u, flag)
global
if flag == 0

m1 m2 L1 L2 Th1 Th2
mp1=m1; mp2=m2;

sys = [ 4, 0, 2, 2, 0, 0 ];
x0 = zeros(4, 1);
elseif flag == 3
sys = [x(1); x(2)];
elseif flag == 1
d11 = mp1*L1*L1+mp2*(L1*L1+L2*L2+2*L1*L2*cos(x(2)));
d12 = mp2*L1*L2*cos(x(2))+mp2*L2*L2;
d21 = d12;
d22 = mp2*L2*L2;
D = [d11 d12; d21 d22];
h=mp2*L1*L2*sin(x(2));
v1 = [-h*x(4) -h*x(3)-h*x(4)];
v2 = [h*x(3) 0];
V = [v1; v2]*[x(3); x(4)];
g1=mp1*L1*9.8*cos(x(1))+mp2*9.8*(L2*cos(x(1)+x(2))+L1*cos(x(1)));
g2=mp2*L2*9.8*cos(x(1)+x(2));
G = [g1;g2];
f x = inv(D)
f_x
inv(D)*(-V
( V - G);
b_x = inv(D)*u;
sys(1) = x(3);
sys(2) = x(4);
sys(3) = f_x(1) + b_x(1);
sys(4) = f_x(2)
f x(2) + b_x(2);
b x(2);
else
sys = [];
End

38

Computed-Torque PD
Robot Control
Co t o Simulation
S u at o Result
Resu t

39