BÀI TẬP LỚN KỸ THUẬT ROBOT ĐỀ 18
Câu 1:
a) Xây dựng hệ tọa độ cho các thanh nối
Từ sơ đồ cánh tay ta gắn các hệ trục tọa độ như sau:
• Vị trí gốc tọa độ:
- Khâu số 0 thân Robot là khâu cố định có gốc tọa độ O được đặt tại khớp 1
- Khung tọa độ 1 có gốc O1 được gắn vào khớp thứ 2
- Khung tọa độ 2 có gốc O2 được gắn vào khớp thứ 3
- Khung tọa độ 3 có gốc O3 được gắn vào khâu tác động cuối
• Chiều của các khung tọa độ :
- Khung tọa độ số 0 có trục Z0 trùng với trục của khớp 1, trục X0 được chọn vuông
góc với mặt giấy
- Khung tọa độ số 1 có trục Z1 vuông góc với mặt phẳng giấy và trục X1 trùng với
trúc của khớp thứ 2
- Khung tọa độ số 2 có trục Z2 trùng với trục của khớp số 3 và trục X2 được chọn
vuông góc với mặt giấy
- Khung tọa độ số 3 có trục Z3 trùng với phương của trục Z2 và trục X3 vuông góc
với mặt giấy
- Trục Y0, Y1, Y2, Y3 được xác định theo quay tắc bàn tay phải.
Từ đó ta xây dựng được hệ trục tọa độ như sau:
1
o
0
1
0
2
03
K
1
K2
K

mô tả quan hệ giữa khâu 2 và khâu 1, ma trận
2
A
3
mô tả quan hệ giữa khâu 3 và khâu 2.
Dạng tổng quát của ma trận
i-1
A
i
như sau:
2
i-1
A
i
=
. . .
. . .
0
0 0 0 1
C i S i C i S i S i ai C i
S i C i C i C i S i ai S i
S i C i di
θ θ α θ α θ
θ θ α θ α θ
α α
−
 
 
−
 

2 2
2
0 0
0 0
0 1 0
0 0 0 1
C S
S C
a
θ θ
θ θ
−
 
 
 
 
−
 
 
2
A
3
=
3
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
d
 

θ θ θ θ θ θ θ θ
θ θ θ
− − −
 
 
− − −
 
 
 
 
c) Giải thích ý nghĩa của ma trận T:
Ma trận T có dạng tổng quát như sau:
3
T=
0 0 0 1
x x x x
y y y y
z z z z
n o a p
n o a p
n o a p
 
 
 
 
 
 
Trong đó:
- Vector
n

C30 .C30 -S30 C30 .S30 0,3.S30 0,1.C30 .C30
S30 .C30 C30 S30 .S30 0,3.C30 0,1.S30 .S30
S30 0 C30 0,1.C30
0 0 0 1
o
 
− −
 
− − −
 
 
 
 
=
3 1 3 0,3
4 2 4 4
3 3 1 0,6. 3 0,1
4 2 4 4
1 3 0,1. 3
0
2 2 2
0 0 0 1
 
− −
 
 
 
+
− −
 

a) Xác định momen khớp quay và lực tổng ở các khớp tịnh tiến khi Robot ở cuối hành
trình chuyển động
Ta có mô hình của Robot như sau:
Để xây dựng được các phương trình động lực học của Robot θ-rđơn giản chúng ta giả
thuyết:
- Khối lượng m
1
của xilanh tập trung tại điểm cuối của xilanh tức là điểm A
- Khối lượng m
2
của pittong tập trung ở bàn tay Robot tức là tập trung tại điểm B
- Mômen quán tính ở khớp J
i
= 0
5
Trình tự xây dựng phương trình động lực học như sau:
 Xác định hàm lagrange
Phương trình lagrange của một cơ cấu được xác định như sau:
Trong đó:
- K: Tổng động năng của hệ thống
- P: Tổng thế năng của hệ thống
 Với khớp quay ta có phương trình động lực học như sau:
M =
d L L
dt
θ θ
∂ ∂
 
−
 ÷

1
= r
1
. cos
θ
Y
1
= r
1.
sin
θ
Đạo hàm hai vế theo thời gian thu được thành phần vận tốc theo các trục:
1 1
.sin .X r
θ θ
= −
&
&
1 1
.cos .Y r
θ θ
=
&
&
Vận tốc điểm A:
2 2 2 2 2
1 1 1 1
v x y r
θ
= + =

2
cos . sin .y r r
θ θ θ
= +
&
& &
Vận tốc của B:
2 2 2 2 2 2
2 2 2
v x y r r
θ
= + = +
&
& & &
 Động năng của khớp 2:
2 2 2
2 2
1
( )
2
K m r r
θ
= +
&
&
• Xác định thế năng của hệ thống
- Thế năng của khớp 1:
1 1 1
sinP m gr
θ

= −
 ÷
∂ ∂
 
&
Ta có:
2 2
1 1 2
2 2
1 1 2 2
2
L
m r m r
d L
m r m r m rr
dt
θ θ
θ
θ θ θ
θ
∂
= +
∂
∂
 
= + +
 ÷
∂
 
& &

&& &
&
9
 Tính lực khớp tịnh tiến (khớp 2)
Lực tác động lên khớp tịnh tiến đưuọc tính như sau:
d L L
F
dt r r
∂ ∂
 
= −
 ÷
∂ ∂
 
&
Trong đó:
2
2
2
2 2
sin
L
m r
r
d L
m r
dt r
L
m r m g
r

r
có hai bậc tự do gồm khớp 1 là khớp quay, khớp thứ 2 là khớp tịnh tiến.
Giả thuyết chuyển động của các khớp này là chuyển động đều. Đồng thời thời gian để khớp
quay quay hết quỹ đạo yêu cầu bằng thời gian mà khớp tịnh tiến thực hiện hết hành trình làm
việc của mình.
Thời gian để cơ cấu tịnh tiến chuyển động từ r
1
= 0.5(m) tới vị trí r
max
=1.0(m) với vận
tốc r = 0.125(m/s) là:
max 1
1.0 0.5
4( )
0.125
r r
t s
r
−
−
= = =
Mặt khác cơ cấu Robot lại quay từ 0
0
với tốc độ
16
π
θ
=
&
và thời gian khớp quay đúng

=

thay vào các công thức đã xây dựng ở trên ta được:
- Mômen của khớp quay:
2 2
1 1 2 2 1 1 2
2 2
1 1 2 2 1 1 2
2 2
2 ( ) cos
( ) 2 ( ) cos
(2,5.0,5 2,5.1,0 ).0 2.2,5.1,0.0,125. (2,5.0,5 2,5.1,0).9,81. os 26,13( )
16 4
M m r m r m rr m r m r g
m r m r m rr m r m r g
c Nm
θ θ θ θ
θ θ θ
π π
= + + + +
= + + + +
= + + + + =
&& && &
&
&& &
&
- Lực tổng của khớp tịnh tiến:
2
2 2 2
2

&&
Hay viết dưới dạng ma trận:
(2-1)
Trong đó:
M
M
F
 
=
 
 
- mô men hoặc lực của khớp
2 2
1 1 2
2
0
0
m r m r
H
m
 
+
=
 
 
- ma trận mô men hoặc lực tỉ lệ với gia tốc
11
ĐK1 ĐK2
R
2

phương trình động lực học của Robot và phân ly đặc tính động học của các thanh nối.Kết
quả nhận được một hệ thống tuyến tính từ đó dễ dàng sử dụng các phương pháp thiết kế kinh
điển của hệ thống tuyến tính.
Phương trình luật điều khiển có dang:
dk P D
M K= ε +Κ ε
&
(24)
Với:
d
q q
ε
= −
là vector sai số vị trí của khớp robot;
P
K
=
diag
1 2
( , )
p p
K K
- ma trận đường chéo các kệ số khuếch đại
D
K =
diag
1 2
( , )
d d
K K

4
n
qd
t
ω
ξ
=
- tần số dao động.
Cân bằng phương trình (2-5) và (2-6) thu được:
2
2
4 8
2 2 .
4
di n
qd qd
pi n
qd
K
t t
K
t
ξω ξ
ξ
ω
ξ
= = =
 
= =
 ÷

Suy ra các hệ số bộ điều khiển:
8 0
0 8
32,65 0
0 32,65
d
p
K
K
 
=
 
 
 
=
 
 
c) Mô phỏng hệ thống
• Thiết kế quỹ đạo chuyển động cho hai khớp dạng 2 – 1 – 2
Để đảm bảo tay Robot di chuyển từ vị trí ban đầu A(x
0
, y
0
) đến vị trí cuối cùng là
B(x
c
, y
c
) trong khoảng thời gian t
c

0
1
1
; .
2 2
. . . . 0
1
.
2
2 4
c c
m m
m
c c
m
c c c
t q q
t q
q q
q q t q t q t t q q
t t
q q q t
t t q q
t
q

+
= =



t t q q
t
q
−
= − − −
&&
Biểu thức (2-7) có nghĩa khi và chỉ khi :
2
0
1
0
1 1
2
4
4.
(2 8)
c c
c
cp
c
t q q
q
q q
q q
t
−
−
−
⇒ < ≤ −
&&



− = − −


&&
&&
&&
Trong đó:
- Chọn
1
q
&&
nằm trong khoảng theo biểu thức (2-8).
- Biết
1
q
&&
, tc, qc, q0 ta tính được t1 theo (2-7).
• Kết quả mô phỏng
Mô men và lực của 2 khớp
0 1 2 3 4 5 6
0
2
4
6
8
10
12
14

0.8
0.9
1
1.1
1.2
Nhận xét: Góc quay của các biến khớp là θ và r bám sát quỹ đạo 2-1-2 cho trước với
sai số nhỏ và các khớp thực hiện tới đúng điểm dừng cuối.
Chương trình mô phỏng
%======================================
% CHUONG TRINH CHINH
%=======================================
% khai bao gia tri bo dieu khien
% goi ham RobotThetaR
[At1,Aq1,qdd1,Aq2,qdd2,Adq1,Adq2,AM1,AM2,AeTheta,Aer] = RobotThetaR()
% ========================================
% do thi tin hieu thu duoc
%=========================================
% ve mo men va luc cua robot tren cung do thi
plot(At1,AM1,'r') % mo mem khop quay (khop 1)
hold on
plot(At1,AM2) % luc cua khop tinh tien (khop 2)
grid
% ve sai so goc quay va tinh tien tren cung do thi
19
plot(At1,AeTheta,'r') % mo mem khop quay (khop 1)
hold on
plot(At1,Aer) % luc cua khop tinh tien (khop 2)
grid
% ve goc quay khop 1
plot(At1,Aq1)

%thoi gian giam toc ve 0. Tuc la thoi gian tang va giam toc
%deu bang t11/2
t12 = tc/2 - sqrt((tc^2*ddq2-4*(qc(2)-q0(2)))/ddq2)/2; %Thoi gian tang toc
t22 = tc - t12;
Tk = 0.01; %Sau Tk(s) ta se tinh toan cac tham so cua robot
%Dieu kien ban dau (So kien)
q = q0; dq = [0;0];
X0 = [q0(1);dq(1);q0(2);dq(2)]; %So kien bien trang thai X
20
file1 = fopen('RobotThetaR.txt','w');
i = 0; %Bien dung de dem
for t = 0:0.001:tc;
i = i+1;
At1(i)=t; %Lay thoi gian de ve do thi
%Tinh toan gia tri dat cho cac khop trong tung khoang thoi gian chuyen
%dong
[qd1, dqd1] = quiDaoKhopThetaR(q0(1),qc(1),ddq1,t11,t21,tc,t); %dqd1 la
van
%toc cua khop 1, ddq1 la gia toc khop 1
[qd2, dqd2] = quiDaoKhopThetaR(q0(2),qc(2),ddq2,t12,t22,tc,t);
qd = [qd1; qd2];
dqd = [dqd1; dqd2];
G11 = (m1*r1 + m2*X0(3))*9.81*cos(X0(1));
G12 = m2*9.81*sin(X0(1));
G1 = [G11;G12];
v1 = 2*m2*X0(3)*X0(4)*X0(2);
v2 = -m2*X0(4)*X0(1)*X0(1);
V1 = [v1;v2];
H11 = m1*r1^2 + m2*X0(3)^2; %r = x21
H12 = 0;

function[q,dq] = RobotModel(M,X0,Tk)
%
M01 = M(1); %Mo men dieu khien cho khop quay
F02 = M(2); %Luc dieu khien cho khop tinh tien
x11 = X0(1); %Goc Theta1
x12 = X0(2); %Toc do goc khop quay
x21 = X0(3); %Gia tri r
x22 = X0(4); %Toc do khop tinh tien
%
%Cac thong so cua Robot
m1 = 2.5 ; %Khoi luong thanh 1
m2 = 2.5 ; %Khoi luong thanh 2
r1 = 0.5; %Chieu dai thanh noi 1
%
C1 = cos(x11);
S1 = sin(x11);
%
%Ma tran quan tinh
H11 = m1*r1^2 + m2*x21^2; %r = x21
H12 = 0;
H21 = 0;
H22 = m2;
H = [H11, H12; H21, H22];
%Momen nhot va momen huong tam
v1 = 2*m2*x21*x22*x12;
v2 = -m2*x22*x12*x12;
V = [v1;v2];
%Momen trong luc
G1 = (m1*r1 + m2*x21)*9.81*C1;
G2 = m2*9.81*S1;

q = qc - ddq*(t-tc)^2/2;
dq = ddq*t1 - ddq*(t-t2);
end
end
+ hàm ControllerPD
function[M,errorTheta, errordTheta] =
ControllerPD(sd,dsd,theta,dtheta,Kp,Kd,G,V,H)
%Cai dat tham so bo dieu khien vi tri phan hoi PD
kp1 = Kp; kp2 = Kp; kd1 = Kd; kd2 = Kd;
%Tinh sai lech goc quay va dao ham
errorTheta = sd - theta;
errordTheta = dsd - dtheta;
%Momen dieu khien
Kp = [kp1,0; 0,kp2];
Kd = [kd1,0; 0,kd2];
%M = Kp * errorTheta - Kd * dtheta + G;
U = dsd+Kp*errorTheta+Kd*errordTheta;
M=H*U+V+G;
23
Tài liệu tham khảo:
[1]. TS. Nguyễn Mạnh Tiến . Điều Khiển Robot Công Nghiệp . Nhà xuất bản Khoa Học và
Kỹ Thuật. Hà Nội 2006.
[2]. TS. Nguyễn Mạnh Tiến . Bài Giảng Robot Công Nghiệp .
[3]. GS.TSKH . Nguyễn Thiện Phúc . Robot Công Nghiệp . Nhà xuất bản Khoa Học và Kỹ
Thuật. Hà Nội 2006.
24