Đồ án 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

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.