y
x
J2
1
l1
lg1
lg2
2
m2
J1
m1
l2
z
Hình 1: Cơ cấu Robot
Bài số 2
Cho một cơ cấu robot 2DOF như hình vẽ
Sử dụng matlab- Simulink
1. Xây dựng mô hình toán học robot
2. Thiết kế bộ điều khiển PID bù trọng trường. Chạy mô phỏng
Số liệu của robot:
Chiều dài thanh nối 1: l
1
= 0,5 m
Chiều dài thanh nối 2: l
2
= 0,4 m
Khối lượng thanh nối 1: m
1
= 1 Kg
Khối lượng thanh nối 2: m
2

2
tác dụng giữa thanh nối 1 và 2; trọng lực có hướng theo
trục y
Ký hiệu động năng và thế năng của thanh nối i là K
i
và P
i
(i=1,2)
Ta có:
Động năng thanh nối 1:
1
2 2
. .
2
1 1
1 1 1
1 1
2 2
g
K m l J
θ θ
= +
Thế năng thanh nối 1:
1 1 1 1
sin
g
P m gl
θ
=
Tọa độ tâm khối thanh nối 2:

y l c l
θ θ θ θ θ θ
θ θ θ θ θ θ

= − − +



= + +

→Bình phương tốc độ dài tâm thanh nối 2:
2 2 2 2
. . . . . . . .
2 2 2 2
2 1 1 2 1 1 2
2 1 2 1 2 22
( ) 2 os ( )
g g
v x y l l l l c
θ θ θ θ θ θ θ
= + = + + + +
Động năng thanh nối 2:
. .
2 2
2
2 2 2 2
1
1 1
( )
2 2

1 1
2 2
. . . . . .
2 2 2
1 1 2 1 1 2
2 1 2 1 2 2
. .
2
2
2 1 1 1 2 1 1 2 1 21
1 1
2 2
1
[ ( ) 2 os ( )]
2
1
( ) sin [ sin sin( )]
2
g
g g
g g
m l J
m l l l l c
J m gl m g l l
θ θ
θ θ θ θ θ θ θ
θ θ θ θ θ θ
= + +
+ + + + + +
+ + − − + +

g g
m l J m l l l l c J
m l l l c J m l l
m gl c m g l l c
θ θ
θ θ θ θ θ θ
θ θ θ θ
= + + + + +
+ + + − +
+ + + +
Mômen của khớp 2:
2
.
2
2
( ) ( )
d L L
M
dt
θ
θ
∂ ∂
= −
∂
∂
.

2 2 2
2 2 1 2 2 2 1 2 2 2 2 2 1 2 2 1
2 2 1 2

Ta có mô hình toán học robot:
.
( ) ( , ) ( )M H q q V q q G q= + +
Trong đó:
11 12
21 22
H H
H
H H
 
=
 
 
: ma trận quán tính
Với:
1
2 2 2
11 1 1 2 1 2 1 2 2 2
2
12 21 2 2 1 2 2 2
2
22 2 2 2
( 2 os )
( os )
g
g g
g g
g
H m l J m l l l l c J
H H m l l l c J

− +
 
=
 
 
 
: thành phần momen nhớt và hướng
tâm
1
2
g
G
g
 
=
 
 
: thành phần momen trọng lực
Với:
1 1 1 1 2 1 1 2 1 2
2 2 2 1 2
os [( os os( )]
os( )
g g
g
g m gl c m g l c l c
g m gl c
θ θ θ θ
θ θ
= + + +

% ma tran V
V1 = -m2*l1*lg2*sin(q(2))*[dq(2)^2+2*dq(1)*dq(2)];
V2 = m2*l1*lg2*sin(q(2))*dq(1)^2;
y = [V1;V2];
- Khối
1
( )H q
−
Hàm tính
1
( )H q
−
function y = H(q,M)
% thong so robot
m1 = 1; m2 = 1;
l1 = 0.5; l2 = 0.4;
g = 9.8;
J1 = 0.5; J2 = 0.5;
lg1 = l1/2; lg2 = l2/2;
% bien khop
theta1=q(1);
theta2=q(2);
% ma tran H(q)
H11=m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(theta2))+J2;
H12=m2*(lg2^2+l1*lg2*cos(theta2))+J2;
H21=H12;
H22=m2*lg2^2+J2;
H = [H11 H12;H21 H22];
% ma tran nghich dao H^(-1)(q)
Hinv = inv(H);
2
2
0
0
D
K
K
K
 
=
 
 
Tạo khối:
Mô hình bộ điều khiển trên matlab:
/3
/ 2
d
q
π
π
 
=
 
 
0
0
/6
q