Một phương pháp nâng cao chất lượng hệ điều khiển tay máy 3 thanh nối

tả toán học hệ điều điều khiển tay máy 3 thanh [3]: 
Xét tay máy có sơ đồ động học như hình 1. Hệ có 3 
thanh nối: Thanh I quay quanh trục Z0 ≡ Z1 (coi là khối trụ 
tròn có bán kính R, khối lượng m1); thanh II có chiều dài l2, 
khối lượng m2, tham gia hai chuyển động: Chuyển động 
quay theo quanh trục Z2 và chuyển động quay tương đối 
quanh trục Z1 ; thanh III có chiều dài l3, khối lượng m3, tham 
gia các chuyển động: Chuyển động quay theo quanh trục Z3 
và hợp chuyển động tương đối hay còn gọi là chuyển động 
song phẳng tương đối quanh trục Z2 và Z1. 
Chọn hệ tọa độ góc để biểu diễn hệ ĐKCĐ nhiều 
trục như trên hình 1. Hệ tọa độ X0Y0Z0 được gắn cố định với 
nền móng, hệ tọa độ X1Y1Z1 được gắn với tâm của chuyển 
động quay khớp I( 1ϕ ) với trục quay là Z1 trùng với trục Z0, 
hệ tọa độ X2Y2Z2 được gắn với chuyển động quay khớp 
II( 2ϕ ) với trục quay là :trục Z2, hệ tọa độ X3Y3Z3 được gắn 
với tâm của chuyển động quay khớp III ( 3ϕ ) với trục Z3. 
Hệ phương trình Lagrange 2 mô tả các chuyển động là: 
2 2
11 1 1 2 2 13 3 11 1 1 2 1 12 1 3 1 13 2 3 1 12 2 1 13 3 1
2 2 2
2 1 1 2 2 2 23 3 21 2 1 3 2 13 2 3 21 1 1 2 12 2 3 1 3 3 2 2
31 1 32 2 33 3 3 12 1 3 31 1 1 3
M M M N N N H H Q
M M M N N H H H G Q
M M M N H
ϕ + ϕ + ϕ + ϕ ϕ + ϕ ϕ + ϕ ϕ + ϕ + ϕ =
ϕ + ϕ + ϕ + ϕ ϕ + ϕ ϕ + ϕ + ϕ + ϕ + =
ϕ + ϕ + ϕ + ϕ ϕ + ϕ ϕ
ɺɺ ɺɺ ɺɺ ɺ ɺ ɺ ɺ ɺ ɺ ɺ ɺ
ɺɺ ɺɺ ɺɺ ɺ ɺ ɺ ɺ ɺ ɺ ɺ
ɺɺ ɺɺ ɺɺ ɺ ɺ ɺ ɺ
2 2 2
31 1 1 31 2 2 31 3 3 3 3H H H G Q




+ ϕ + ϕ + ϕ + = ɺ ɺ ɺ
Trong đó : 2
1 1 2 3 2 2 3 2
1 1M A ( B E ) c o s C c o s ( ) D c o s c o s ( )
3 2
= + + ϕ + ϕ − ϕ − ϕ ϕ − ϕ 
2
12 2 3 2 2 3 2
1 1M ( B E ) cos C cos( ) D cos cos( )
3 2
= + ϕ + ϕ − ϕ − ϕ ϕ − ϕ 
l1 
l2 
2ϕ 
3ϕ 
Z0 ≡ Z1 
Z2 
Zi 
Z3 I3 
Ri 
Yi 
Y0 ≡ Y1 
Xi 
1ϕ 
X0 ≡ X1 
O
I
II
III
Hình 1: Sơ đồ động học tay máy 3 thanh 
T¹p chÝ Khoa häc & C«ng nghÖ - Sè 3(43)/N¨m 2007 
45
13 3 2 3 2 3 3 2 2
1 1 3M C cos( ) cos D( cos( ) cos( ) E cos
2 4 4
= ϕ − ϕ ϕ − ϕ + ϕ + ϕ − ϕ + ϕ 
2 2
21 2 3 2 3 2 2
1 1M ( B E) cos Ccos ( ) D cos( )cos
3 2
= + ϕ + ϕ − ϕ − ϕ − ϕ ϕ 
2 2
2 2 2 3 2 3 2 2
2 3 3 2 3 3 2 2 3 2
3 1 3 2 3 3 2 2 3 2
3 2 3 2 3 3 2
1 1M ( B E ) c o s C co s ( ) D co s ( ) c o s
3 2
1 3 1M C co s ( ) c o s D [ co s ( ) co s ( ) 2 E c o s
2 4 4
1 3 1M C c o s( ) co s D [ co s ( ) c o s( ) E c o s
2 4 4
1 3M C co s ( ) c o s D [ co s (
2 4
= + ϕ + ϕ − ϕ − ϕ − ϕ ϕ
= ϕ − ϕ ϕ − ϕ − ϕ + ϕ + ϕ + ϕ
= ϕ − ϕ ϕ − ϕ − ϕ + ϕ + ϕ + ϕ
= ϕ − ϕ ϕ − ϕ − ϕ 2 3 2
2
3 3 3 3
1) co s ( ) E c o s
4
1M C c o s D co s E
2
+ ϕ + ϕ + ϕ
= ϕ − ϕ +
1 1 1 2 3 2 3 2
1 1 2 3 2 3 2 2
1 1N ( B E ) s in 2 C s in ( ) D s in ( )
3 2
1N [ C s in 2 ( ) D s in ( ) co s
2
= − + ϕ + ϕ − ϕ − ϕ − ϕ
= ϕ − ϕ + ϕ − ϕ ϕ
113 3 2 3 3 2 2 3 3 2 3 2 2 2
1 1 1 3N Csin( ) cos Csin 2( ) D[ sin( ) sin( ) sin( ) cos ] 2E sin
2 2 4 4
= ϕ − ϕ ϕ − ϕ − ϕ + ϕ + ϕ − ϕ − ϕ + ϕ − ϕ ϕ − ϕ
112 3 2 2 3 3 2
113 3 2 2 3 3 2
1 1 3H C sin(2 ) D[ sin( ) sin( )]
2 4 4
1 1 3H C sin(2 ) D[ sin( ) sin( )
2 4 4
= ϕ − ϕ + ϕ + ϕ + ϕ − ϕ
= ϕ − ϕ + ϕ + ϕ + ϕ − ϕ
212 3 2 3 2 3 3 2 2
3 2 3 2 2
213 3 2 3 2 2
1 1N C sin 2( ) C sin ( ) cos D[sin ( ) cos
2 2
1 3
sin ( ) sin ( )] E sin
4 4
1N C sin 2( ) D[sin ( ) cos ]
2
= − ϕ − ϕ − ϕ − ϕ ϕ + ϕ − ϕ ϕ
− ϕ + ϕ + ϕ − ϕ + ϕ
= − ϕ − ϕ + ϕ − ϕ ϕ
2 1 1 2 3 2 3 2
2 1 2 2 3 2 3 2
2 1 3 3 2 3 2 3 2
1 1 1 1H ( B E ) s in 2 C s in 2 ( ) D s in ( )
6 2 4 2
1 1 1 1H ( B E ) s in 2 C s in 2 ( ) D s in ( )
6 2 4 2
1 1 3H C s in ( 2 ) D [ s in ( ) s in ( ) ]
2 4 4
= + ϕ − ϕ − ϕ + ϕ − ϕ
= − + ϕ + ϕ − ϕ − ϕ − ϕ
= − ϕ − ϕ + ϕ − ϕ + ϕ + ϕ
2 2 2 3 2 2 3 3 3 2
312 3 2 3 3 2 2 3
3 2 2 2 3 2
G (m gd m gl ) cos m gd cos( )
1 3 1N [ C sin( ) cos D[ sin ( ) sin ( )
2 4 4
1
sin ( ) cos ] E cos C sin 2( )
2
= − + ϕ + ϕ − ϕ
= + ϕ − ϕ ϕ − ϕ − ϕ − ϕ + ϕ +
+ ϕ − ϕ ϕ − ϕ + ϕ − ϕ
T¹p chÝ Khoa häc & C«ng nghÖ - Sè 3(43)/N¨m 2007 
46 
311 3 2 3 2 2
1 1H C sin 2( ) D sin ( ) cos
4 4
= ϕ − ϕ − ϕ − ϕ ϕ 
312 3 2 3 3 2 2 3 3 2 2 2 3 2
1 3 1 1 1H Csin( )cos D[ sin( ) sin( ) sin( )cos ] Ecos Csin2( )
2 4 4 2 4
= ϕ −ϕ ϕ − ϕ −ϕ − ϕ +ϕ + ϕ −ϕ ϕ − ϕ + ϕ −ϕ
313 3 3 3 3 3 3 3H C sin 2 Dsin ;G m gd cos= ϕ − ϕ = ϕ 
3. Cấu trúc điều khiển mờ thích nghi:[2] 
Để điều khiển các chuyển động của tay máy ta dùng các bộ điều khiển mờ thích 
nghi có cấu trúc như hình 2. 
Bộ điều khiển mờ thích nghi 
có thể có nhiều đầu vào gồm 
sai lệch và các đạo hàm của 
chúng, trong bài báo này ta chỉ 
thiết kế bộ điều khiển mờ 
thích nghi có 2 đầu vào e và 
e’. Bộ điều khiển mờ có nhiệm 
vụ tạo ra tín hiệu điều khiển 
u, sao cho quĩ đạo đầu ra của 
đối tượng (y) bám theo quĩ đạo 
cho trước (yd), cho dù có sự 
thay đổi thông số và cấu trúc 
của đối tượng. 
Mỗi đầu vào có 7 hàm liên thuộc rải đều trên miền [ ]jmaxjmin α,α với j = 1, 2 là số đầu 
vào: 
( ) ( )j1jj1j1 ej1j1jjA e1
11,;e)e(
α+δ−
+
−=αδµ=µ ; ( ) ( )2ejpjpjjA jpjjpjp e,;eµ)e(µ α−δ−=αδ= 
Với p = 2, 3 ... , Nj -1
, còn ( )





 α−δ−
+
=αδ= j
jNj
j
jN
jjjN e
j
N
j
NjjA
e1
1
,;eµ)e(µ
 Trong đó: jmax
j
N
j
1N
j
2
j
1
j
min α=α<α<<α<α=α −⋯ 
- Bộ điều khiển mờ có 7x7 = 49 luật điều khiển với luật n1 iiRu  là: 
n1n21 ii
n
in
2
i2
1
i1 ButhenAeandandAeandAeif ⋯⋯ ==== 
Trong đó i1 = 1, 2 ... , N1 ; ... in = 1, 2, ... , Nn là số hàm liên thuộc cho mỗi biến đầu 
vào;
n1 iiB ⋯ là tập mờ đầu ra sẽ được xác định. 
Sử dụng luật hợp thành PROD, mờ hoá theo đường singleton và giải mờ bằng phương 
pháp trung bình trọng tâm ta thu được bộ điều khiển mờ: 
Hình 2: Sơ đồ cấu trúc bộ điều khiển mờ thích nghi 
- 
⋮
e 
u y yd 
dt
d
Đối tượng 
Bộ điều khiển mờ 
u = u(e,θ) = θTξ(e) 
Luật thích nghi 
θ’ = γeTpnξ(e) 
T¹p chÝ Khoa häc & C«ng nghÖ - Sè 3(43)/N¨m 2007 
47
( )
( )
( )∑ ∑ ∏
∑ ∑ ∏
= = =
= = =






µ





 µ
=θ=
1
1
n
n j
ji
1
1
n
n
j
ji
n1
N
1i
N
1i
n
1j jA
N
1i
N
1i
n
1j jAii
e
ey
,euu
⋯
⋯
⋯
 (1) 
( )eu Tξθ= (2) 
Trong đó: ξ(e) là tập hợp hàm mờ cơ sở đã biết. 
( )
( )
( )∑ ∑ ∏
∏
= = =
=





 µ
µ
=ξ
1
1
n
n
j
ji
j
ji
N
1i
N
1i
n
1j jA
n
1j jA
e
e
e
⋯
 (3) 
n1 iiy ⋯ là điểm trọng tâm của n1 iiB ⋯ , chúng sẽ được chỉnh định theo luật thích nghi 
cho phù hợp với đối tượng. 
θ là một véc tơ gồm tập hợp các 
n1 iiy ⋯ với i1 =1 ... N1;... i2 = 1 ... N2 
Các thông số θ được chỉnh định nhờ sử dụng luật thích nghi sau: 
)e(Pe nT ξγ=θɺ (4) 
Trong đó γ là 1 hằng số dương xác định tốc độ của thuật toán còn pn là cột cuối cùng 
của ma trận P, với P là nghiệm của phương trình Lyapunov. 
Dat vi tri 1
Dat vi tri 2
Dat vi tri 3
Scope
Mux
Fi1''
Fi1'
Fi1
Fi2''
Fi2'
Fi2
Fi3''
Fi3'
Fi3
MK1 
Khop1
Fi1''
Fi1'
Fi1
Fi2''
Fi2'
Fi2
Fi3''
Fi3'
Fi3
MK3 
Khop 3
Fi1''
Fi1'
Fi1
Fi2''
Fi2'
Fi2
Fi3''
Fi3'
Fi3
MK2 
Khop 2
In1 Out1
DKMTN3
In1 Out1
DKMTN2
In1 Out1
DKMTN1
DA DK
Mo men
Fi''
Fi'
Fi
DC3
DA DK
Mo men
Fi''
Fi'
Fi
DC2
Momen
DA DK
Fi''
Fi'
Fi
DC1
Hình 3: Sơ đồ mô phỏng hệ điều khiển tay máy 3 thanh nối trên MATLAB 
T¹p chÝ Khoa häc & C«ng nghÖ - Sè 3(43)/N¨m 2007 
48 
Sau khi thiết kế được các bộ điều khiển mờ thích nghi, ta ghép chúng vào đối tượng điều 
khiển và tiến hành mô phỏng trên MATLAB với các thông số như sau: 
L1 = 0,06m; m1 = 20kg; d1 = 0,3m; L2 = 0,6m; m2 = 12kg; d2 = 0,2m; L3 = 0,5m; 
m3 = 12kg; d3 = 0,2m. 
Kết quả mô phỏng được thể hiện trên hình từ hình 4 đến hình 6. Trong đó hình 4 đến 
hình 6 là quĩ đạo của các chuyển động 1 khi dùng PID kinh điển và dùng bộ điều khiển mờ, hình 
7 là quĩ đạo không gian của 3 chuyển động. 
4. Kết luận 
Từ các kết quả mô phỏng ta thấy rằng khi dùng bộ điều khiển mờ thích nghi, quĩ đạo của 
các chuyển động bám quĩ đạo đặt sát hơn so với khi dùng bộ điều khiển kinh điển. Bộ điều 
khiển có cấu trúc như trên có thể sử dụng để điều khiển các hệ nhiều chuyển động trong thực tế. 
0 0.5 1 1.5 2 2.5 3
0
0.5
1
1.5
2
2.5
3
3.5
Hình 4: Quĩ đạo của khớp 1 khi dùng PID 
kinh điển và Điều khiển mờ thích nghi 
PID kinh điển 
Điều khiển mờ thích nghi 
0 0.5 1 1.5 2 2.5 3
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
PID kinh điển 
Điều khiển mờ thích nghi 
Hình 5: Quĩ đạo của khớp 2 khi dùng PID 
kinh điển và Điều khiển mờ thích nghi 
0 0.5 1 1.5 2 2.5 3
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3
PID kinh điển 
Điều khiển mờ thích nghi 
Hình 6: Quĩ đạo của khớp 3 khi dùng PID 
kinh điển và Điều khiển mờ thích nghi 
0
1
2
3
4
-2
0
2
4
-1
-0.5
0
0.5
1
1.5
Hình 7: Quĩ đạo không gian của 3 
chuyển động 
T¹p chÝ Khoa häc & C«ng nghÖ - Sè 3(43)/N¨m 2007 
49
Tóm tắt 
Nâng cao chất lượng hệ điều khiển nhiều trục là một vấn đề khó đang được nhiều nhà 
khoa học quan tâm. Bài báo đề xuất giải pháp dùng bộ điều khiển thích nghi mờ để điều khiển 
tay máy 3 thanh nối. Các kết quả mô phỏng cho thấy, chất lượng của hệ thống đã được nâng lên 
nhiều so với hệ thống điều khiển kinh điển. 
Summary 
Advance quality for control system of 3 joints is a diffucult matter that a number of 
scientists concern. This article proposes a solution of using adaptive fuzzy controller for robot 
arm of 3 joints. The modelling results proves that the its quality is higher than the traditional 
one. 
TÀI LIỆU THAM KHẢO 
[1] Phan Xuân Minh, Nguyễn Tiến Hiếu (2005), “Điều khiển thích nghi tay máy trên cơ 
sở hệ mờ”, Tuyển tập các báo cáo khoa học hội nghị tự động hóa toàn quốc lần thứ 6 năm 2005 , 
trang 370-375. 
[2] Lại Khắc Lãi (2003), “Một số phương pháp tổng hợp bộ điều khiển trên cơ sở logic mờ 
và thích nghi”, Luận án tiến sĩ kỹ thuật. 
[3] Nguyễn Như Hiển (2003), “Nghiên cứu nâng cao chất lượng hệ chuyển động nhiều 
trục”, Luận án tiến sĩ kỹ thuật. 
[4] H. Yamamoto & T. Furuhashi (1999), New fuzzy Inference method for system Using 
symbolic stability analysis of fuzzy control, The fourth Asian Fuzzy System Synposium, May 31 - June, 
Tsukuba, Japan, pp.450-455.