Điều khiển chuyển động cho robot trụ xoay

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
Báo cáo bài tập lớn
Môn điều khiển robot công nghiệp
Giáo viên hướng dẫn : PGS.Nguyễn Phạm Thục Anh


Báo cáo bài tập lớn
MỤC LỤC
PHẦN 1: GIỚI THIỆU CHUNG VÀ LỰA CHỌN PHƯƠNG ÁN
1. Khái niệm:
Thuật ngữ robot xuất hiện lần đầu tiên năm 1920 trong một vở kịch của nahf
viết kịch người Tiệp Karel Capek.
Robot có nhiều cách định nghĩa khác nhau:
- Viện Robot (Mỹ): Robot là cơ cấu đa chức năng được lập trình trước với các cảm
biến bên ngoài để thực hiện các nhiệm vụ khác nhau như: lắp ráp, vận chuyển…
- Trong Bách khoa toàn thư mới: Robot có thể định nghĩa là một thiết bị tự điều
khiển hoàn toàn bao gồm các bộ phận điện, điện tử và cơ khí,…
- Trong kĩ thuật: Robot là 1 cánh tay vài bậc tự do được điều khiển bằng máy tính
có thể thực hiện các chuyển động như tay người, còn được gọi là tay máy.
Cấu tạo robot gồm 3 bộ phận chính:
2
Báo cáo bài tập lớn
- Cơ cấu cơ khí.
- Cơ cấu điều khiển.
- Cơ cấu cảm biến.
2. Lựa chọn phương án:
Từ những yêu cầu của đề bài nhóm chúng em lựa chọn phương án thiết kế
như sau:
- Cơ cấu robot: Robot có 1 trục quay và 2 trục tịnh tiến.
- Xây dưng phần mềm tính toán bọn em sử dụng Gui trong chương trình Mathlab
.

PHẦN 2: TÍNH TOÁN CÁC THAM SỐ CỦA ROBOT
1. Phương trình động học ngược:
- Cơ cấu robot:
3
Báo cáo bài tập lớn
- Bảng D-H :
a
i
α
i
d
i
θ
i
1 0 0 0 θ
1
2 0 -90
0
d
1
0
3 0 0 d
2
0
- Ma trận biểu diễn quan hệ giữa hai khung tọa độ i-1 và i :
4
Báo cáo bài tập lớn
A
i
=







−
−






1000
cossin0
sinsincoscoscossin
coscossincossincos
iii
iiiiiii
iiiiiii
d
a
a
αα
θαθαθθ
θαθαθθ
- Ta có:
0
A

1
=






−






1000
0100
00cossin
00sincos
11
11
θθ
θθ
;
1
A
2
=







−






1000
010
0100
0001
1
d
;
2
A
3
=













1000
100
0010
0001
2
d
;
- Suy ra:
RT
H
=
0
A
1

1
A
2
2
A
3
=







−






1000
0100
00cossin
00sincos
11
11
θθ
θθ
x






−







1000
010
0100
0001
1
d
x












1000
100
0010
0001
2
d
RT
H
=







−
−−






1000
010
coscos0sin
sinsin0cos
1
1211
1211
d
d
d
θθθ
θθθ
5
Báo cáo bài tập lớn
- Sau khi giải phương trình động học ngược ta có:










−=
−=
=
θ
θ
sin
)tan(
2
2
21
x
y
x
P
d
P
P
a
Pd
2. Tính ma trân Jacoby
2.1 Tính theo cách trực tiếp
Ta có : X= [
12

sin
θ
d

12
cos
θ
d
d
1
0 0
1
θ
]
T
Ma trận Jacoby:
J=





















−
−−
001
000
000
010
cos0sin
sin0cos
112
112
θθ
θθ
d
d
2.2 Tính ma trận Jacoby theo phương pháp
H
J
A
3
=













1000
100
0010
0001
2
d
;
1
3
T
=













−
1000
010
100
0001
1
2
d
d
6
Báo cáo bài tập lớn
0
3
T
= A
1
x A
2
x

A
3
=







−
−−






1000
010
coscos0sin
sinsin0cos
1
1211
1211
d
d
d
θθθ
θθθ
Tính ma trận
H
J:
H
J=





















−
−
−
000
001
000
100
010
00
2
d
Ma trận Jacoby:
J=





















−
−−
001
000
000
010
cos0sin
sin0cos
112
112
θθ

θθ
d
d
Phần 3 : Xây dựng phần mềm tính toán cho động lực thuận vị trí và ma trận
Jacoby
1. Giới thiệu về công cụ thiết kế
Ở bài tập lớn này chúng em sử dụng GUI trong Matlab để giải quyết bài toán. Ưu
điểm của GUI là tạo ra một giao diện thân thiện. Thuận lợi cho việc kiểm chứng kết
quả.
Với Gui rất dễ lập trính các phép tính toán.
7
Báo cáo bài tập lớn
2. Xây dựng phần mềm tính toán cho động lực thuận vị trí
Như đã tính toán ở trên ta tính được ma trận truyền là:
RT
H
=






−
−−







1000
010
coscos0sin
sinsin0cos
1
1211
1211
d
d
d
θθθ
θθθ
Với nhập đầu vào là Q= [PhyZ1 d
1
d
2
]
T
Ta sẽ thu được ma trận truyền
R
T
H
3. Xây dựng phần mềm tính toán cho ma trận Jacoby
Ta vẫn sử dụng nhập số liệu vào là Q= [PhyZ1 d
1
d
2
]
T

Kết quả cho ra là ma trận Jacoby và mà trân
H
J. Ở đây em cho cả hai phương pháp
trên một màn hình hiển thị để so sánh kết quả nhận được của 2 phương pháp.
8
Báo cáo bài tập lớn
Phần 4 Thiết kế quỹ đạo trong không gian khớp
1. Thiết kế quỹ đạo chuyển động cho robot.
- Ta có, tại thời điểm ban đầu:





=
=
=
0)0(
0)0(
0)0(
2
1
0
d
d
θ
- Chọn thời gian t = 5 s, kết thúc quá trình chuyển động, robot có các thông số:






=
=
=
md
md
1)0(
1)0(
45)0(
2
1
0
0
θ
Do quỹ đạo chuyển động theo phương trình bậc 3 không thể kiểm soát được
gia tốc, mặt khác quỹ đạo 2-1-2 chỉ ứng dụng trong trường hợp điều khiển khi có
vật cản nên ta chọn quỹ đạo chuyển động theo phương trình bậc 5.
9
Báo cáo bài tập lớn
+ Quỹ đạo một khớp quay :
5
5
4
4
3
3
2
210
)( tatatatataat +++++=

θ
⇒
Quỹ đạo tốc độ :
4
5
3
4
2
321
5432)( tatatataat ++++=
θ

⇒
Quỹ đạo gia tốc :
3
5
2
432
201262)( tatataat +++=
θ

+ Từ các ràng cuộc của bài cho, ta có:
















−−+−−
=
−+++−
=
−−+−−=
=
=
=
5
2
000
5
4
2
000
4
2
0003
0
2
01
00
2

)()(6)(12
2
)23()1614()(30
)3()128()(20
2
f
fffff
f
fffff
fffff
t
tt
a
t
tt
a
tta
a
a
a
θθθθθθ
θθθθθθ
θθθθθθ
θ
θ
θ






- Từ đó ta tính được phương trình quỹ đạo cho các khớp như sau:













+−=
+−=
+−=
=
6
5
4
4
3
3
1
6
5
4
4

3
3
1
6
5
1
4
4
1
3
3
11
61510
61510
61510
)(5
fff
d
fff
d
fff
d
f
t
t
x
t
t
x
t

t
xq
t
t
y
t
t
y
t
t
yq
t
t
t
t
t
t
q
st
θθθ
10
Báo cáo bài tập lớn
2. Các thông số của robot.
- Khâu trụ xoay có dạng hình hộp chữ nhật, kích thước: X*Y*Z = 0,1*0,4*1,7 (m);
khối lượng 14 kg.
⇒











=+=+=
=+=+=
=+=+=
)(3,2)4,11,0(14
12
1
)(
12
1
)(045,0)1,017,0(14
12
1
)(
12
1
)(32,2)17,04,1(14
12
1
)(
12
1
22222
3
22222

1
22222
1
kgmYXmI
kgmXZmI
kgmZYmI
⇒











=
3,200
0045,00
0032,2
TX
I
Tương tự ta tính được các ma trận I của khâu đế, khâu tịnh tiến và khâu tác
động cuối.
- Khâu đế có kích thước: X*Y*Z = 1*0,08*0,5 (m); khối lượng 14 kg.
⇒











=
17,100
046,10
003,0
Đ
I
- Khâu tịnh tiến có kích thước: X*Y*Z = 0,1*1,2*0,17 (m); khối lượng 12 kg.
⇒










=
45,100
004,00
0047,1

TT
I
- Khâu tác động cuối có kích thước: X*Y*Z = 1,2*0,1*0,17 (m); khối lượng 12 kg.
11
Báo cáo bài tập lớn
⇒










=
45,100
047,10
0004,0
EE
I
3.Bộ điều khiển PID phản hồi đầu ra:
- Bộ điều khiển khớp xoay θ
1
500
0
200
k
i

d
P
P
P
=


=


=

- Bộ điều khiển khớp tịnh tiến Qd
1
1000
700
500
k
i
d
P
P
P
=


=


=


- Bộ điều khiển khớp tịnh tiến Qd
2
12
Báo cáo bài tập lớn
500
0
200
k
i
d
P
P
P
=


=


=

Hình 2.1.1: Mô hình điều khiển robot.
Hình 2.1.2: Bộ điều khiển PID.
13
Báo cáo bài tập lớn
Hình 2.1.3: Mô hình robot.
Hình 2.1.4: Đồ thị vị trí của robot.
14
Báo cáo bài tập lớn

Hình 2.1.5: Đồ thị vận tốc của robot.
Hình 2.1.6: Đồ thị gia tốc của robot.
15
Báo cáo bài tập lớn
PHẦN 5: KẾT LUẬN
Sau một quá trình học tập, nghiên cứu môn học “Điều khiển robot công
nghiệp”, cùng với sự hướng dẫn của cô Nguyễn Phạm Thục Anh và sự cố gắng của
cả nhóm, chúng em đã hoàn thành bài tập lớn: Điều khiển chuyển động cho robot
trụ xoay.
Bài tập đã thực hiện được các nhiệm vụ:
• Xây dựng phần mềm tính toán sử dụng Gui trong Mathlab.
• Thiết kế bộ điều khiển PID điều khiển robot di chuyển đến điểm
mong muốn.
• Thực hiện mô phỏng với kết quả tương đối chính xác.
Trong quá trình thực hiện, chắc chắn nhóm em không thể tránh được những
thiếu sót, vì vậy chúng em rất mong nhận được ý kiến đóng góp của cô và các bạn
để bài tập lớn này hoàn thiện hơn.
Chúng em xin chân thành cảm ơn!
16
Báo cáo bài tập lớn
TÀI LIỆU THAM KHẢO
1. Điều khiển robot công nghiệp – TS.Nguyễn Mạnh Tiến, NXB Khoa học và kỹ
thuật.
2. MATLAB và Simulink dành cho kĩ sư điều khiển tự động - GS.TSKH.Nguyễn
Phùng Quang, NXB Khoa học và kỹ thuật.
3. Introduction to Robotics – John J.Craig, Prentice Hall.
4. Simmechanic user’s guide – MathWorks.
17