o

o
AiBi
PRPaR
RRPaR
 E xyz

c c c s s
E c s s s c c s s s s s c
s s c s c s c c s s c c

x y z

E E
 x y z

i i

AiBi

F k
k k F k
 a

A
a

a
 x y z

x,y,z x y z x y z

x y z

x y z x y z
 2

1
1
0

2 2

2 2

1 1
1 1
0 0
 2 2

2 2

1 1
1 1
0 0

2
2

2
2
1
1
1 0
1
0
 2 2

2 2

1 1
1 1
0 0

1
1
0
 2

1
1
0

1
1
0
 2

1
1
0

1
1
0
 f

f n
 f

f
f

1
1
0
compliance matrix
Bi

AiBi

Ai
 l

l
l

l S

l
S
l
S
l

l
S

S
S
 l

l
l

l
 N
i
Ni
ki
k ij
j

Kq k kN
i
Ni
j
i kij Kx J Kq J

e e

e
e

X X
 e e

e
e
Dynamics of robot
 x
ma
Jz

m
Jx Jy a L
y z
Jx Jy Jz mL
f f
r f f
r r
r
r
r
 f f
r
r

f f
r
r

r
a1
 a1 5r

B
m2, JC2
y0 g
x d a2 C2
A
u1 C1 x0 B
O
m2, JC2
m1 u2 y0 g
x d a2 C2
A
u1 C1 x0
O
m1 u2
 B
m2, JC2
y0 g
x d a2 C2
A
u1 C1 x0
O
m1 u2

B
m2, JC2
y0 g
x d a2 C2
A
u1 C1 x0
O
m1 u2
 y q y y q y
z z
z z
u m
m
u m
m
z
q
x
z
q
u
x

u
 y q y y
z z x
m J
u m g
m x
q lC C
x
q
z y
q
q
x z u C
q 2
m u
u

y y
x x
m J m J
g g
x x
q lC C q lC C
x x
y y
q q
z u C z u C
m u m u
 y
x
m J
g
x
q lC C
x
y
q
z u C
m u
 2
2 2
2
2
2
2 C
2 C 0 1
0 1 2
2
1
1 1
1 1
1 u
u u
u 1
1 0
0 0
0

2
2

2
2 C
0 1
2
1
1
1
u
1
u
0
0
 2 function q2dot = tinhq2dot(ins)
2

2 % ins = [1 2 3 1 2 3]';
2 C
0 1
n=2;
2
1
1
1
% state variables
u
1
u
0 qdot = ins(1:n);
0
q = ins(n+1:2*n);
u = ins(2*n+1:3*n);
q1_dot=qdot(1);
q2_dot=qdot(2);
q1=q(1);
q2=q(2);

2
2
% Damping matrix % mass matrix of system
2 m11 = J1+J2+m1*a1^2+m2*l1^2+m2*a2^2+2*m2*l1*a2*cos(q2);
2 C % system parameter, see page 16,
0 1 PhD Thesis of Renwei Hu m12 = J2+m2*a2^2+m2*l1*a2*cos(q2);
2
1 % body 1 m21 = m12;
1
1 m1=21.20; %[kg] m22 = J2+m2*a2^2;
u
1
u J1=0.21; %[kg m2]
0
l1=0.25; %[m] M=[m11, m12; m21, m22];
0
a1=0.15; %[m] % Coriolis forces
c11 = -2*m2*l1*a2*sin(q2)*q2_dot; c12 = m2*l1*a2*sin(q2)*q2_dot;
% body 2 c21 = m2*l1*a2*sin(q2)*q1_dot; c22 = 0;

m2=15.22; %[kg] c11 = -m2*l1*a2*q2_dot*sin(q2)’;

c12 = -m2*l1*a2*(q1_dot+q2_dot)*sin(q2);
J2=0.18; %[kg m2]
c21 = m2*l1*a2*q1_dot*sin(q2);
l2=0.45; %[m] c22 = 0;
a2=0.19; %[m] C = [c11, c12; c21, c22];
gr=9.81; % gravitational forces
g = gr*[m1*a1*sin(q1)+m2*(l1*sin(q1)+a2*sin(q1+q2));
m2*a2*sin(q1+q2)];
% Damping matrix
D = 60*diag([0.1, 0.1]);

% B matrix

B = [1, 0; 0, 1];

% q2dot

q2dot=inv(M)*(B*u-C*qdot-D*qdot-g);
 s L
z

zs
s
s
s
t=0 t0 tf t s
O ys
y
s=0 s= L s xs

x
t = [ 0 2 4 6 8 10];
q = [-10 20 0 30 40 40];
qv = [ 0 10 -10 10 0 0];
t = [ 0 2 4 6 8 10];
q = [ 10 20 0 30 40 40];
qv = [ 0 -10 20 3 0 0];
qa = [ 0 5 10 3 0 0];
a
 s(t) s(t)
y y

x x
 qt

t
s L
z

zs
s
s
s
t=0 t0 tf t s
O ys
y
s=0 s= L s xs

x
 s L
z

zs
s
s
s
s
O ys
y s L
xs
s
x
s

s
s s

s(t)

x
 s

z s L

zs
s
t=0 t0 tf t s
s
s

s O ys
s=0 s= L
y
xs

x
r
n,s,a

i, i, i
f, f, f
 , ,

, ,
t
 n
f

y q
O
z

q
O
x
 n
f

y q
O
z

q
O
x
 Ra,i La,i i

Ui

m,i
Jm,i

Ra,i La,i i

Ui
m,i
Jm,i
 U

U
U
U
x f x,u
 x
x
x fx

x
fx