Thiết kế bộ Điều khiển trượt cho tay máy Robot 2 bậc tự do và mô phỏng trên Matlab – Simulink

) 
với H(q) là ma trận quán tính xác định dương, đối xứng. 
Chúng ta giả sử rằng các giá trị ước lượng )(ˆ qH và ),(ˆ qqh & quan hệ 
với giá trị thực )(qH và ),( qqh & bởi bất đẳng thức sau: 
)()()(ˆ 1 qqHqH β≤− (3.41) 
và ),(),(ˆ),( max qqhqqhqqh &&& Δ≤− (3.42) 
với )(qβ và ),(max qqh &Δ là những hàm đã biết. 
Viết lại biểu thức động lực học dưới dạng: 
τ)(),( qBqqfq += &&& (3.43) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 61
Với ),()(),( 1 qqhqHqqf && −−= (3.44) 
)()( 1 qHqB −= (3.45) 
Nhiệm vụ của điều khiển là tìm mô men thích hợp τ sao cho vector 
vị trí q của tay máy bám theo quỹ đạo mong muốn qd. 
Chúng ta định nghĩa sai lệch trạng thái e và mặt trượt như sau: 
qqe d −= (3.46) 
0; >=+= TCCeCeS & (3.47) 
Rõ ràng rằng S=0 thì )()( tqtq q→ . Quả thực với S=0 ta có thể viết lại 
như sau: 
CeeeCeS −=⇒=+⇒= && 00 
Như vậy hệ thống ổn định tiệm cận nếu có e = 0 và theo đó điều kiện 
bám )()( tqtq q→ sẽ được đảm bảo. 
Do vậy vấn đề điều khiển là phải tìm mô men τ thích hợp sao cho 
vector trạng thái của hệ thống có thể bám được trên mặt trượt. Hay phải tìm 
τ thỏa mãn điều kiện trượt. Điều kiện trượt có thể xác định theo tiêu chuẩn 
Lyapunov. 
Chúng ta định nghĩa hàm Lyapunov như sau: 
0
2
1 >= SSV T (3.48) 
Đạo hàm của (3.48) có dạng: 
SSV T && = (3.49) 
Như vậy, nếu 0<V& thì với 0→V dẫn tới 0→S và 0→e 
Do vậy, điều kiện đủ của điều kiện trượt là: 
0<SS T & (3.50) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 62
Khi đó điều kiện trượt đảm bảo cho hệ kín ổn định toàn cục, tiệm cận 
và điều kiện bám được thực hiện mặc dù mô hình không chính xác, 
nhiễu, 
Nếu điều kiện trượt có thể thỏa mãn theo đó: 
∑
=
=>≤−≤
n
i
i
T SSSSS
1
2;0;0 αα& (3.51) 
Tiếp đó, mặt phẳng trượt S=0 sẽ đạt được với thời gian giới hạn nhỏ 
hơn T0 ở đó: 
))0((
2
1
0 qST α= (3.52) 
Biểu thức trên được chứng minh như sau: 
Từ (29) ta có: 
α−≤
S
SS T & (3.53) 
Thay SSV T && = và 21)2( VS = vào (3.53) sau đó tích phân hai vế với 
t=0→treach , S(q(treach))=0 ta có: 
[ ] 0
0 0 2
))0((
2
))0((
2
1
)2(
2
1
2
1 T
qS
tt
qS
Vdt
V
V
reachreach
t treach reach
=≤→−≤==∫ αα
&
 (3.54) 
Bây giờ chúng ta tìm đầu vào bộ điều khiển τ thỏa mãn điều kiện 
trượt. 
Lấy đạo hàm biểu thức (3.47) ta có: 
dqqeCS &&&&&& −+= (3.55) 
 Thay biểu thức (3.39) vào ta có: 
dqqBqqfeCS &&&&& −++= τ)(),( (3.56) 
Do đó tín hiệu điều khiển có dạng 
[ ])(ˆ 1 sKSgnB eq −= − ττ (3.57) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 63
với: 
[ ]Tn
eq
sSgnsSgnsSgnsSgn
qqfeCq
)(),...,(),()(
),(ˆ
21=
−−= &&&&τ
 (3.58) 
K>0, K là ma trận khuyếch đại nxn. 
Ma trận khuyếch đại K phải chọn đủ lớn để điều kiện trượt được thỏa 
mãn mặc dù có tham số không rõ, nhiễu, 
Trong trường hợp ước lượng chính xác ffBB == ˆ,ˆ thì điều kiện 
trượt được viết lại như sau: 
SsKSgnSSS TT α−≤−= )(& (3.59) 
Nếu chọn αββ >≥ ;IK (3.60) 
và SSSSSSS
m
i
i
m
i
i
T αββββ −≤−=−≤−=−= ∑∑
== 1
2
1
& (3.61) 
thì chế độ trượt xảy ra. 
Ta nhận thấy rằng, đầu vào điều khiển được gián đoạn qua s(t) như 
cho ở biểu thức (3.57). Hiện tượng chattering xảy ra. Bởi vì trong thực tế, 
sự chuyển đổi là không lý tưởng. Trong trường hợp sai số ước lượng là 
không đủ nhỏ thì việc chọn K là không đơn giản như biểu thức trên. 
Trong trường hợp đó S& cho dưới dạng: 
)(ˆ)(ˆ)(),( 11 sKSgnBqBBqBqqfeCqS eqd
−− −+++−= τ&&&&& (3.62) 
đặt 1ˆ)();ˆ(ˆ −=−+= BqBRffff dẫn tới: 
 )()ˆ()( sRKSgnffIRS eq −−+−= τ& (3.63) 
Từ đây, điều kiện trượt là: 
{ } )()()()( sSgnSsRKSgnffIRSSS TeqTT ατ −≤−−+−= && (3.64) 
Do vậy, nếu chọn K để: 
{ } )()ˆ()()( sSgnSffIRSsRKSgnS TeqTT ατ +−+−≥ (3.65) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 64
thì điều kiện trượt như ở trên 0<SS & được thỏa mãn và điều kiện 
trượt đạt được. 
 )(ˆ 11 qBBR β≤= −− (3.66) 
Từ biểu thức (3.41) và (3.42) ta có bất đẳng thức: 
 )(ˆ 11 qBBR β≤= −− (3.67) 
 max1 ˆ)ˆ(ˆ)ˆ( hBhhBffR Δ≤−=−− (3.68) 
Từ đây, ta có thể chọn ma trận K thoả mãn điều kiện trượt như sau: 
 IhBIK eq βατβ +Δ+−≥ maxˆ)1( (3.69) 
III.4.3. Ứng dụng Điều khiển trượt cho tay máy Robot 2 bậc tự 
do: 
III.4.3.1. Phương trình động lực học tay máy hai bậc tự do toàn 
khớp quay: 
Bộ thông số tay máy: m1 = m2 = 1 kg 
 l1 = l2 = 1 m 
Phương trình động lực học: 
⎥⎦
⎤⎢⎣
⎡+⎥⎦
⎤⎢⎣
⎡
⎥⎦
⎤⎢⎣
⎡+⎥⎦
⎤⎢⎣
⎡
⎥⎦
⎤⎢⎣
⎡=⎥⎦
⎤⎢⎣
⎡
2
1
2
1
2221
1211
2
1
2221
1211
2
1
g
g
cc
cc
hh
hh
F
F
θ
θ
θ
θ
&
&
&&
&&
 (3.70) 
Trong đó F1, F2 là lực được tạo ra ở các khớp động, ma trận H là ma 
trận xác định dương và đối xứng, ma trận C là ma trận lực ly tâm, G là ma 
trận lực trọng trường. 
 Giá trị của các ma trận khi thay giá trị được xác định như sau: 
- Ma trận H: 
1
cos2/31
cos35
22
22112
211
=
+==
+=
h
hh
h
θ
θ
 (3.71) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 65
- Ma trận C: 
0
sin3
sin2/3
sin3
22
2121
2212
2211
=
−=
−=
−=
c
c
c
c
θθ
θθ
θθ
&
&
&
 (3.72) 
- Ma trận G: 
)cos(15
)cos(15cos15
212
2111
θθ
θθθ
+=
+−=
g
g (3.73) 
III.4.3.2. Mô hình động lực học tay máy hai bậc tự do: 
Chúng ta đặt các biến trạng thái là tín hiệu góc quay và vận tốc của 
các khớp tay máy: 
Khớp 1: 
⎪⎩
⎪⎨
⎧
=
==
=
112
11112
111
θ
θ
θ
&&&
&&
x
xx
x
 (3.74) 
Khớp 2: 
⎪⎩
⎪⎨
⎧
=
==
=
222
22122
221
θ
θ
θ
&&&
&&
x
xx
x
 (3.75) 
Tín hiệu vào u: 
⎩⎨
⎧
=
=
22
11
Fu
Fu (3.76) 
Từ biểu thức (3.70) ta có: 
⎥⎦
⎤⎢⎣
⎡−⎥⎦
⎤⎢⎣
⎡
⎥⎦
⎤⎢⎣
⎡−⎥⎦
⎤⎢⎣
⎡=⎥⎥⎦
⎤
⎢⎢⎣
⎡ −−−
2
11
2
1
2221
12111
2
11
1
1
g
g
H
cc
cc
H
F
F
H θ
θ
θ
θ
&
&
&&
&&
 (3.77) 
trong đó H-1 là ma trận nghịch đảo của ma trận H. 
Tính H-1 và kết hợp tất cả các phương trình trên và thay vào (3.77) để 
tính được: 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 66
Khớp 1 
11 12
12 1 22 12 22 21 11 212
21
2
21 2 12 21 11 21
1 3
( (2 )sin 15(cos cos ))
4 9 4 cos 2
3 3
(1 cos )( sin 15 cos( ))
2 2
x x
x u x x x x x x
x
x u x x x x
⎧⎪ =⎪⎪ = + + − − −⎨ −⎪⎪− + − − +⎪⎩
&
&
 (3.78) 
Khớp 2: 
21 22
22 21 1 22 12 22 21 11 212
21
2
21 2 12 21 11 21
1 3 3
( (1 cos )( (2 )sin 15(cos cos )))
4 9 4cos 2 2
3
(5 3cos )( sin 15cos( )))
2
x x
x x u x x x x x x
x
x u x x x x
⎧⎪ =⎪⎪ = − + + + − − +⎨ −⎪⎪+ + − − +⎪⎩
&
&
(3.79) 
III.4.3.3. Thiết kế bộ điều khiển trượt cho tay máy 2 bậc tự do: 
Xác định bậc tương đối cho khớp 1 và khớp 2: 
Từ phương trình trạng thái của các khớp (3.78), (3.79) và biểu thức 
(3.24) ta có được ngay là trong trường hợp này là p=n hay bậc tương đối 
của từng khớp p=2. 
Xây dựng mặt trượt cho từng khớp: 
2222
1111
eeS
eeS
&
&
λ
λ
+=
+= (3.80) 
ở đây: 
222
111
rd
rd
xxe
xxe
−=
−= (3.81) 
λ1, λ2 là những số thực dương. 
Điều kiện để xảy ra chế độ trượt cho hệ trên: 
0<= SSV && (3.82) 
Xây dựng bộ điều khiển: 
Từ (3.78) và (3.79) nếu đặt: 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 67
⎩⎨
⎧
+=
=
),()( 1112
1211
uxgxfx
xx
&
&
 (3.83) 
và: 
⎩⎨
⎧
+=
=
),()( 2222
2221
uxgxfx
xx
&
&
 (3.84) 
Ta có: 
)(),())((
)),()((
111111111
1111111111111111
ShKuxgxfxe
uxgxfxexxeeeS
d
dd
−=−−+=
+−+=−+=+=
λλλ
λλλλλ
&&&
&&&&&&&&&&&& (3.85) 
1
1111111
1
))(()(
),( λ
λλ xfxeShKuxg d −++=⇒ &&& 
 Tương tự ta cũng có: 
2
2222222
2
))(()(
),( λ
λλ xfxeShKuxg d −++=⇒ &&& (3.86) 
 Theo (3.77) ta có: 
 uH
uxg
uxg 1
2
1
),(
),( −=⎥⎦
⎤⎢⎣
⎡ (3.87) 
⎥⎦
⎤⎢⎣
⎡−⎥⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡−=⎥⎦
⎤⎢⎣
⎡ −−
2
11
22
12
2221
12111
2
1
)(
)(
g
g
H
x
x
cc
cc
H
xf
xf (3.88) 
 Chú ý: ),(11 uxgg ≠ 
 Từ (3.58) ta có được: 
⎥⎥
⎥⎥
⎦
⎤
⎢⎢
⎢⎢
⎣
⎡
−++
−++
=⎥⎦
⎤⎢⎣
⎡
)(
)(
)(
)(
2
2
22222
1
1
11111
2
1
xf
xeShK
xf
xeShK
H
u
u
d
d
λ
λ
λ
λ
&&&
&&&
 (3.89) 
Thay (3.88) vào (3.89) ta có được bộ điều khiển: 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 68
⎥⎦
⎤⎢⎣
⎡+⎥⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡+
⎥⎥
⎥⎥
⎦
⎤
⎢⎢
⎢⎢
⎣
⎡
++
++
=⎥⎦
⎤⎢⎣
⎡
2
1
22
12
2221
1211
2
22222
1
11111
2
1
)(
)(
g
g
x
x
cc
cc
xeShK
xeShK
H
u
u
d
d
λ
λ
λ
λ
&&&
&&&
 (3.90) 
III.4.3.4. Tính toán giá trị đặt θi cho tay máy hai bậc tự do: 
Để tính toán giá trị đặt cho tay máy hai bậc tự do chúng ta cần giải 
bài toán động học ngược, từ đó tính toán giá trị đặt cho các khớp: 
⎥⎥
⎥⎥
⎦
⎤
⎢⎢
⎢⎢
⎣
⎡
+
+−
==
1000
0100
)(0
)(0
1121212
1121212
2
1
1
0
2
0 SSlCS
CClSC
AAA (3.91) 
theo cách biến đổi toạ độ ta có được: 
⎥⎥
⎥⎥
⎦
⎤
⎢⎢
⎢⎢
⎣
⎡
=
⎥⎥
⎥⎥
⎦
⎤
⎢⎢
⎢⎢
⎣
⎡
11
2
2
2
2
0
0
0
0
z
y
x
A
z
y
x
 (3.92) 
Tuy nhiên, trong bài toán này có x2, y2, z2=0 vì ta chỉ quan tâm tới 
chuyển động của tâm bàn kẹp do vậy từ (3.91) và (3.92) ta có: 
⎩⎨
⎧
+=
+=
)(
)(
1120
1120
SSly
CClx
 (3.93) 
⎟⎟⎠
⎞
⎜⎜⎝
⎛ −+=⇒
=−+⇒
+=+⇒
2
22
0
2
0
2
22
22
0
2
0
22
2
0
2
0
2
2
arccos
)cos(
2
2
)cos(22
l
lyx
l
lyx
l
yx
θ
θ
θ
 (3.94) 
 Từ (3.93) ta có: 
⎪⎩
⎪⎨⎧ =−
=−⇒
⎩⎨
⎧
=−
=−
2
12
2
10
2
12
2
10
1210
1210
)()(
)()(
lSlSy
lClCx
lSlSy
lClCx (3.95) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 69
12
0
2
0
0
12
0
2
0
0
2
0
2
0
1010
2
0
2
0
2
0)(2
S
yx
y
C
yx
x
l
yx
SyCxlyx
+
+
+
=+⇒
=+−+⇒
 (3.96) 
Đặt 
⎪⎪⎩
⎪⎪⎨
⎧
+
=
+
=
2
0
2
0
0
2
0
2
0
0
)sin(
)cos(
yx
y
yx
x
α
α
 (3.97) 
Chọn αθ >1 ta có: 
αθ +⎟⎟⎠
⎞
⎜⎜⎝
⎛ +=
l
yx
2
arccos(
2
0
2
0
1 (3.98) 
 Như vậy, nếu yêu cầu của bài toán là điều khiển tâm bàn kẹp đi 
theo một quỹ đạo đã được định trước và được xác định bởi: 
⎪⎩
⎪⎨
⎧
==
=
=
0)(
)(
)(
tzz
tyy
txx
thì giá trị đặt cho các khớp phải là: 
⎪⎪
⎪
⎩
⎪⎪
⎪
⎨
⎧
⎟⎟⎠
⎞
⎜⎜⎝
⎛ −+=
+⎟⎟⎠
⎞
⎜⎜⎝
⎛ +=
2
22
0
2
0
2
2
0
2
0
1
2
2
arccos
2
arccos
l
lyx
l
yx
θ
αθ
 (3.99) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 70
CHƯƠNG IV: MÔ PHỎNG QUÁ TRÌNH CHUYỂN ĐỘNG CỦA ROBOT 
DÙNG BỘ ĐIỀU KHIỂN TRƯỢT TRÊN NỀN MATLAB AND SIMULINK: 
IV.1. Tổng quan về Matlab-Simulink: 
Matlab là một bộ chương trình phần mềm lớn của lĩnh vực toán số. 
Tên của bộ chương trình chính là từ viết tắt của từ Matrix Laboratory, thể 
hiện định hướng chính của chương trình là các phép tính vectơr và ma trận. 
Phần cốt lõi của chương trình bao gồm một số hàm toán, các chức năng 
xuất nhập cũng như các khả năng điều khiển chu trình mà nhờ đó ta có thể 
dựng nên các Scripts. 
Thêm vào phần cốt lõi, có thể dùng các bộ công cụ Toolbox với 
phạm vi chức năng chuyên dụng mà người sử dụng cần. Simulink là một 
Toolbox có vai trò đặc biệt quan trọng: vai trò của một bộ công cụ mạnh 
phục vụ mô hình hoá và mô phỏng các hệ thống kĩ thuật - Vật lý, trên cơ sở 
sơ đồ cấu trúc dạng khối. 
Giao diện đồ họa trên màn hình của Simulink cho phép thể hiện hệ 
thống dưới dạng sơ đồ tín hiệu với các khối chức năng quen thuộc. 
Simulink cung cấp cho người dùng một thư viện rất phong phú, có sẵn với 
số lượng lớn các khối chức năng cho các hệ tuyến tính, phi tuyến và gián 
đoạn. Hơn thế người sử dụng có thể tạo nên các khối riêng cho mình. 
 Sau khi đã xây dựng mô hình của hệ thống cần nghiên cứu, bằng 
cách ghép các khối cần thiết, thành sơ đồ cấu trúc của hệ, ta có thể khởi 
động quá trình mô phỏng. Trong các quá trình mô phỏng ta có thể trích tín 
hiệu hiện tại vị trí bất kì của sơ đồ cấu trúc và hiển thị đặc tính của tín hiệu 
đó trên màn hình. Hơn thế nữa, nếu có nhu cầu ta còn có thể cất giữ các 
đặc tính đó vào môi trường nhớ. Việc nhập hoặc thay đổi tham số của tất cả 
các khối cũng có thể thực hiện được rất đơn giản bằng cách nhập trực tiếp 
hay thông qua matlab. Để khảo sát hệ thống, ta có thể sử dụng thêm các 
Toolbox như Signal Processing (xử lý tín hiệu), Optimization (tối ưu) hay 
Control System (hệ thống điều khiển). 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 71
IV.2. Các thao tác thực hiện mô phỏng: 
Khớp 1: 
11 12
12 1 22 12 22 21 11 212
21
2
21 2 12 21 11 21
1 3
( (2 )sin 15(cos cos ))
4 9 4cos 2
3 3
(1 cos )( sin 15cos( ))
2 2
x x
x u x x x x x x
x
x u x x x x
⎧⎪ =⎪⎪ = + + − − −⎨ −⎪⎪− + − − +⎪⎩
&
&
Khớp 2: 
21 22
22 21 1 22 12 22 21 11 212
21
2
21 2 12 21 11 21
1 3 3
( (1 cos )( (2 )sin 15(cos cos )))
4 9 4cos 2 2
3
(5 3cos )( sin 15cos( )))
2
x x
x x u x x x x x x
x
x u x x x x
⎧⎪ =⎪⎪ = − + + + − − +⎨ −⎪⎪+ + − − +⎪⎩
&
&
Ta có mô hình Simulink của Robot 2 bậc tự do: 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 72
Khối Subsytem: 
 Ở đây ta sử dụng các khối: 
+ intergrator: khối tích phân với các tham số của khối mặc định cho 
trước 
+ khối mux: chập tín hiệu đơn thành tín hiệu tổng hợp của nhiều tín 
hiệu 
+ khối input, ouput: đầu vào và đầu ra của tín hiệu. 
+ khối hàm: biểu diễn 1 hàm toán học khi có tín hiệu đi vào là các 
biến, tín hiệu ra thu được là hàm cần biểu diễn: 
.
12x =(u[5]+1.5*u[4]*sin(u[3])*(2*u[2]+u[4])-15*(cos(u[1])-cos(u[3]))-
(1+1.5*cos(u[3]))*(u[6]-1.5*u[2]*u[2]*sin(u[3])-15*cos(u[1]+u[3])))/(4-
2.25*cos(u[3])*cos(u[3])) 
Và:
.
22x =(-(1+1.5*cos(u[3]))*(u[5]+1.5*u[4]*sin(u[3])*(2*u[2]+u[4])-
15*(cos(u[1])-cos(u[3])))+(5+3*cos(u[3]))*(u[6]-1.5*u[2]*u[2]*sin(u[3])-
15*cos(u[1]+u[3])))/(4-2.25*cos(u[3])*cos(u[3])) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 73
+ Các khối scope (thuộc thư viện con sinks): Hiển thị các tín hiệu 
của quá trình mô phỏng theo thời gian. Nếu mở cửa sổ Scope sẵn từ trước 
khi bắt đầu mô phỏng ta có thể theo dõi trực tiếp diễn biến của tín hiệu. 
Ta sử dụng nguồn tín hiệu u1, u2 là 1(t) 
Các hàm x’12, và x’22 là : 
X’12 = (u[5]+1.5*u[4]*sin(u[3])*(2*u[2]+u[4])-15*(cos(u[1])-
cos(u[3]))-(1+1.5*cos(u[3]))*(u[6]-1.5*u[2]*u[2]*sin(u[3])-
15*cos(u[1]+u[3])))/(4-2.25*cos(u[3])*cos(u[3])). 
X’22 = (-(1+1.5*cos(u[3]))*(u[5]+1.5*u[4]*sin(u[3])*(2*u[2]+u[4])-
15*(cos(u[1])-cos(u[3])))+(5+3*cos(u[3]))*(u[6]-1.5*u[2]*u[2]*sin(u[3])-
15*cos(u[1]+u[3])))/(4-2.25*cos(u[3])*cos(u[3])) 
với u[1], u[2], u[3], u[4], u[5], u[6] tương ứng là các vị trí thứ tự trên 
khối Mux. u[1] = x11, u[2] = x12, u[3] = x21, u[4] = x22, u[5] = u1, u[6] = u2. 
Sau khi mô phỏng ta có đồ thị các đường đặc tính của các biến trạng 
thái x11, x12, x21,x22 là : 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 74
Mô phỏng dạng hàm điều khiển: 
Từ các công thức: 
⎥⎦
⎤⎢⎣
⎡+⎥⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡+
⎥⎥
⎥⎥
⎦
⎤
⎢⎢
⎢⎢
⎣
⎡
++
++
=⎥⎦
⎤⎢⎣
⎡
2
1
22
12
2221
1211
2
22222
1
11111
2
1
)(
)(
g
g
x
x
cc
cc
xeShK
xeShK
H
u
u
d
d
λ
λ
λ
λ
&&&
&&&
- Ma trận H: 
1
cos2/31
cos35
22
22112
211
=
+==
+=
h
hh
h
θ
θ
- Ma trận C: 
0
sin3
sin2/3
sin3
22
2121
2212
2211
=
−=
−=
−=
c
c
c
c
θθ
θθ
θθ
&
&
&
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 75
- Ma trận G: 
)cos(15
)cos(15cos15
212
2111
θθ
θθθ
+=
+−=
g
g 
Ta có: 
U1=h11 1 1 1 1 1
1
( ) dK h S e xλ
λ
+ +& && +h12 2 2 2 2 2
2
( ) dK h S e xλ
λ
+ +& &&
+c11x12+c12x22+15cos(x11) 
-15sos(x11+x21) 
U2=h21 1 1 1 1 1
1
( ) dK h S e xλ
λ
+ +& && +h22 2 2 2 2 2
2
( ) dK h S e xλ
λ
+ +& && +c21x12+c22x22 
+15cos(x11+x21) 
Chuyển về dạng hàm của sơ đồ Simulink: 
U1=u[5]*(5+cos(u[3]))+(1+1.5*cos(u[3]))*u[6]-3*u[2]*u[3]*sin(u[3]) 
-1.5*u[3]*u[4]*sin(u[3])+15*cos(u[1])-15*cos(u[1]+u[3]) 
U2=(1+1.5*cos(u[3]))*u[5]+u[6]-3*u[1]*u[2]*sin(u[3])+15*cos(u[1]+u[3]) 
với u[1]=x11, u[2]=x12, u[3]=x21, u[4]=x22. 
U[5]= 1 1 1 1 1
1
( ) dK h S e xλ
λ
+ +& && 
U[6]= 2 2 2 2 2
2
( ) dK h S e xλ
λ
+ +& && 
Theo công thức kinh nghiệm ta chọn: K1=K2=500, 1λ = 2λ =0,156. 
Trường hợp này ta dùng hàm H(S1),H(S2) là các hàm giới hạn đầu 
vào trong khoảng giá trị upper và giá trị lower. 
từ đó ta có: 
u[5]= 1
1
K
λ H(S1)+ 
.
1
1
1
eλ + 1dx&& ( Khối subsytem) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 76
u[6]= 2
2
K
λ H(S2)+ 
.
2
2
1
eλ + 2dx&& ( Khối subsytem2) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 77
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 78
Ta chọn khoảng thời gian mô phỏng là từ 0 ->30 s , tức giá trị stop 
time = 30 ở trong Congiguration Parameters. 
Khi chay sơ đồ Simulink ta được các kết quả đường đặc tuyến của 2 
hàm điều khiển U1, U2, và sai số 1e& , 2e& là: 
Ta có các giá trị đặt xd1 và xd2 là: 
Từ các công thức: 
⎩⎨
⎧
+=
+=
)(
)(
1120
1120
SSly
CClx
⎪⎪⎩
⎪⎪⎨
⎧
+
=
+
=
2
0
2
0
0
2
0
2
0
0
)sin(
)cos(
yx
y
yx
x
α
α
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 79
⎪⎪
⎪
⎩
⎪⎪
⎪
⎨
⎧
⎟⎟⎠
⎞
⎜⎜⎝
⎛ −+=
+⎟⎟⎠
⎞
⎜⎜⎝
⎛ +=
2
22
0
2
0
2
2
0
2
0
1
2
2
arccos
2
arccos
l
lyx
l
yx
θ
αθ
=>xd1= 
acos(sqrt((cos(u[1]+u[3])+cos(u[1]))^2+(sin(u[1]+u[3])+sin(u[1]))^2)/2)+ 
acos((cos(u[1]+u[3])+cos(u[1]))/sqrt((cos(u[1]+u[3])+cos(u[1]))^2+(sin(u[1])+sin(u[1]
+u[3]))^2)) 
xd2 = acos(((cos(u[1]+u[3])+cos(u[1]))^2+(sin(u[1]+u[3])+sin(u[1]))^2-2)/2) 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 80
Kết luận 
Các vấn đề đã được giải quyết : 
Các vấn đề còn tồn tại : 
Tài liệu tham khảo : 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 81
1. Nguyễn Thiện Phúc: Robot công nghiệp. NXB KHKT 2004. 
2. Nguyễn Thiện Phúc: Người máy công nghiệp và sản xuất tự 
động linh hoạt. NXB KHKT 1991. 
3. Nguyễn Phùng Quang: Điều khiển Robot công nghiệp - 
Những vấn đề cần biết. Tạp chí Tự động hoá ngày nay - số tháng 4, 5, 6 / 
2006. 
4. Nguyễn Thiện Phúc: Robot - Thế giới công nghệ cao của 
bạn. NXB KHKT 2005. 
5. Đào Văn Hiệp: Kỹ thuật Robot. NXB KHKT 2002. 
6. Đinh Gia Tường, Tạ Khánh Lâm: Nguyên lý máy. NXB GD 
2003. 
7. Nguyễn Hồng Thái: Xây dựng thuật toán điều khiển và mô 
phỏng động Robot nhiều bậc tự do. Thư viện ĐHBK HN 2002. 
8. Lê Huy Tùng: Điều khiển trượt thích nghi động cho đối 
tượng phi tuyến có mô hình không tường minh. Thư viện ĐHBK HN 
2003. 
9. Nguyễn Doãn Phước, Phan Xuân Minh, Hán Thành Trung: Lý 
thuyết điều khiển phi tuyến. NXB KHKT 2003. 
10. Nguyễn Doãn Phước, Phan Xuân Minh: Hệ phi tuyến. NXB 
KHKT 2000. 
11. Nguyễn Thương Ngô: Lý thuyết điều khiển tự động hiện 
đại. NXB KHKT 2003. 
12. Nguyễn Thương Ngô: Lý thuyết điều khiển thông thường 
và hiện đại. NXB KHKT 2002. 
13. Nguyễn Doãn Phước: Lý thuyết điều khiển tuyến tính. 
NXB KHKT 2002. 
Đồ án tốt nghiệp Thiết kế bộ Điều khiển trượt cho Robot 2 bậc tự do 
 82
14. Nguyễn Phùng Quang: Matlab & Simulink dành cho kỹ sư 
điều khiển tự động. NXB KHKT 2004. 
15. Nguyễn Hoàng Hải: Lập trình Matlab. NXB KHKT 2003. 
16. Solomon: Stability of nonlinear control systems. 1965. 
17. Applied Asymptotic Methods in Nonlinear Oscillitions. 
Thư viện ĐHBK HN 1994. 
18. Harry: Nonlinear Modulation Theory. 1971. 
19. Jakub Mozaryn, Jerzt E.Kurek: Design of the Sliding Mode 
Control for the Puma 560 Robot. Institute of Automatic Control and 
Robotics Warsaw university of Technology. 
20. Martin Ansbjerg Kjaer: Sliding Mode Control. Sweden 
February 6th 2004. 
21. C.Abdallah, D.Dawson, P.Dorato, and M.Jamshidi: Survey of 
Robust Control for Rigid Robots . 
22. Mark W.Spong: Motion control of Robot Manipulators. The 
coordinated Science Laboratory, University of Illinois at Urbana-
Champaign, 1308 W. Main St, Urbana, III. 61801 USA. 
23. . Andre` Jaritz and Mark W. Spong: An Experimental 
Comparison of Robust control Algorithms on a Direct Drive 
Manipulator .IEEE Transaction on Control Systems Technology, Vol. 4, 
No. 6, November 1996. 
24. Austin Blaquie`re: Nonlinear system analysis. Academic 
Press New York and London 1966.