Thiết kế và mô phỏng điều khiển robot 3 bậc tự do RRR

Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
VIỆN CƠ KHÍ

ĐỒ ÁN MÔN HỌC: THIẾT KẾ HỆ THỐNG CƠ ĐIỆN

Mã HP: ME4336

TỬ
Họ và tên sinh viên: Phạm Duy Trọng

MSSV: 20134134 – KT. CĐT 02

Giảng viên hướng dẫn: TS. Nguyễn Danh Trường
Bộ môn: Cơ học vật liệu và kết cấu

NHIỆM VỤ ĐỒ ÁN MÔN HỌC
1. Đề Tài: Thiết kế và mô phỏng điều khiển robot 3 bậc tự do RRR
2. Cài công cụ cần thiết: Phần mềm Matlab, Solidworks
3. Các tài kiệu tham khảo
- Các tài liệu tham khảo phần mềm Matlab, công cụ Simulink, công cụ
Simmechanics.
- Các tài liệu về lí thuyết điều khiển.
- Sách Robot công nghiệp, Động lực học hệ nhiều vật, …
4. Nội dung các phần thuyết minh và tính toán.
- Vẽ và thiết kế Robot trên Solidworks.
- Tính toán động học thuận và ngược.
- Thiết kế điều khiển và mô phỏng trên Matlab.

- Kết quả đồ án

Sinh viên thực hiện: Phạm Duy Trọng

Page | 1


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

MỤC LỤC
Chương I: Thiết kế robot 3DOF – RRR trên phần mềm Solidwork
............................................................................................................... 4
1.

2.

Thiết kế và đặt hệ trục tọa độ cho mỗi khâu ........................ 4
1.1.

Thiết kế giá đỡ (khâu 0) .................................................. 4

1.2.

Thiết kế khâu 1............................................................... 15

1.3.

Thiết kế khâu 2............................................................... 16

1.4.

Thiết kế khâu 3............................................................... 16

Lắp ráp các khâu .................................................................. 17

Chương II: Tính toán động học và động lực học cho robot ......... 22
1.

Thiết lập bảng thông số D-H cho robot 3DOF ................... 22

2.

Giải bài toán động học thuận ............................................... 24

3.

2.1.

Ma trận D-H ................................................................... 24

2.2.

Vận tốc góc các khâu ..................................................... 26

2.3.

Gia tốc góc của khâu ..................................................... 30

Giải bài toán động học ngược .............................................. 32

CHƯƠNG III: Thiết kế mạch mô phỏng điều khiển và GUI ....... 39
1.

Thiết kế mô phỏng điều khiển và GUI với version First

Generation ..................................................................................... 39
1.1.

Giới thiệu các khối của robot ....................................... 41

1.2.

Thêm các khối điều khiển ............................................. 42

Sinh viên thực hiện: Phạm Duy Trọng

Page | 2


Thiết kế và mô phỏng điều khiển robot

1.3.

GVHD: TS. Nguyễn Danh Trường

Kết quả chạy mô phỏng sau khi thêm các khối điều

khiển 44

1.4.

Lập trình giao diện GUI trong matlab ........................ 45

1.4.1. Thiết kế giao diện GUI.............................................. 45
1.4.2. Lập trình trong giao diện file (.m) ........................... 48
2.

Thiết kế mô phỏng điều khiển và GUI với version Second

Generation ..................................................................................... 53
2.1.

Giới thiệu các khối của robot ....................................... 53

2.2.

Thêm các khối điều khiển: PID, Step, S-PS, PS-S ..... 54

2.3.

Lập trình giao diện GUI trong matlab ........................ 57

2.3.1. Thiết kế giao diện GUI.............................................. 57
2.3.2. Lập trình GUI trong file (.m) ................................... 63
Kết luận: ............................................................................................. 69

Sinh viên thực hiện: Phạm Duy Trọng

Page | 3

Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

Chương I: Thiết kế robot 3DOF – RRR trên phần mềm Solidwork

Khớp quay 2

Khớp quay 3

Khâu 2
Khâu 3

Khâu 1
Khớp quay 1

Giá đỡ

1. Thiết kế và đặt hệ trục tọa độ cho mỗi khâu
Do trình tự thiết kế và đặt hệ trục tọa độ các khâu tương tự nhau lên sau đây em
xin trình bày rõ ràng thiết kế 1 khâu, còn các khâu khác em xin phép được lấy
hình.
1.1.

Thiết kế giá đỡ (khâu 0)

Sinh viên thực hiện: Phạm Duy Trọng

Page | 4


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Trên mặt phẳng Top vẽ sketch với biên dạng là đường tròn với
D=300mm

Sinh viên thực hiện: Phạm Duy Trọng

Page | 5


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Sử dụng lệnh extrude

• Sử dụng lệnh chamfer:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 6


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Trên mặt phẳng đáy vẽ sketch có biên dạng sau:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 7


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Sử dụng lệnh extrude:

• Dùng lệnh chamfer

Sinh viên thực hiện: Phạm Duy Trọng

Page | 8


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Vẽ sketch trên mặt phẳng sau:

Mặt phẳng

• Sau đó extrude:

• Tương tự vẽ sketch trên mặt phẳng sau và extrude:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 9


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Sử dụng lệnh mirror qua mặt phẳng front:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 10


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Sử dụng lệnh cirpattern quanh trục y or tâm đường tròn

• Tạo plane1:

• Sau đó trên plane1 vẽ sketch với D = 200mm, và dưới mặt phẳng
face1 vẽ đưởng tròn với D=220mm

Sinh viên thực hiện: Phạm Duy Trọng

Page | 11


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Dùng lệnh Loft để nối 2 đường tròn lại:

• Vẽ sketch trên mặt phẳng sau để tạo khớp quay:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 12


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

 Ta được hình sau

• Do chi tiết có gốc không theo D-H nên ta phải xoay và dịch
chuyển chi tiết đến vị trị phù hợp với thông số D-H

• Chọn 1 điểm point tại khớp quay và sử dụng lệnh dịch chuyển
gốc tọa độ về điểm point đó:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 13


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Đặt hệ tọa độ vào điểm point:

 Kết quả thu được chi tiết khâu 0:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 14


Thiết kế và mô phỏng điều khiển robot

1.2.

GVHD: TS. Nguyễn Danh Trường

Thiết kế khâu 1

Sinh viên thực hiện: Phạm Duy Trọng

Page | 15

Thiết kế và mô phỏng điều khiển robot
1.3.

Thiết kế khâu 2

1.4.

Thiết kế khâu 3

Sinh viên thực hiện: Phạm Duy Trọng

GVHD: TS. Nguyễn Danh Trường

Page | 16


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

2. Lắp ráp các khâu
• Trên môi trường assembly ta lấy chi tiết khâu đế ra (chi tiết đầu
tiên bị lấy ra sẽ là chi tiết cố định)

• Sau đó phải chuột vào chi tiết và chọn float để khối có thể di
chuyển được.
• Tiếp theo ràng buộc hệ trục tọa độ gốc trong assembly với chi
tiết để chi tiết cố định tại gốc tọa độ đó.

Sinh viên thực hiện: Phạm Duy Trọng

Page | 17


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Ràng buộc bề mặt trùng nhau:

• Ràng buộc bề mặt song song:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 18


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

 Chi tiết khâu 0 cố định:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 19

Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Tiếp tục lần lượt lấy các chi tiết khâu 1,2,3 vào môi trường để lắp ráp chúng lại
với nhau:

• Ràng buộc đồng tâm hay đồng trục và ràng buộc mặt của khâu 1 với khâu 0 lại
với nhau ta được:

• Tương tự ràng buộc khâu 2 với khâu 1

Sinh viên thực hiện: Phạm Duy Trọng

Page | 20


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Ràng buộc khâu 3 với khâu 2

Sinh viên thực hiện: Phạm Duy Trọng

Page | 21


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

Chương II: Tính toán động học và động lực học cho robot

1. Thiết lập bảng thông số D-H cho robot 3DOF

Bảng tham số DH của robot 3DOF-RRR
Khâu

i

di

ai

i

1

1

900

0

90

2

2

0

650

0

3

3

0

400

0

• Robot 3DOF – RRR gồm giá đỡ nối với 3 khâu bằng khớp trụ. Khâu thứ 1 là
khớp quay, khâu thứ 2 quay, khâu thứ 3 quay, chuyển động linh hoạt trên mặt
phẳng nằm ngang.

Sinh viên thực hiện: Phạm Duy Trọng

Page | 22


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

• Với các thông số biết trước như sau:
✓ Khâu 1: d1 = 900mm, khớp quay o0 với

=(-π,π)

1

✓ Khâu 2: a2 = 650mm, khớp quay o1 với

2

✓ Khâu 3: a3 = 400mm, khớp quay o2 với

3

=(

(

3

, )
3

, )
3 3

• Cách chọn hệ trục tọa độ cho từng khâu theo D-H như sau:
✓ Trục zi được chọn dọc theo hướng của trục khớp động thứ i+1
✓ Trục xi được chọn dọc theo đường vuông góc chung của 2 trục zi

1

và

zi , hướng từ trục zi 1 đến zi . Nếu 2 trục zi 1 và zi cắt nhau thì hướng
của trục xi được chọn tùy ý miễn là vuông góc với với trục zi . Nếu 2
trục zi

1

và zi song song với nhau, thì chọn trục xi hướng theo pháp

tuyến chung nào cũng được
✓ Trục yi được chọn sao cho hệ (Oxyz )i là hệ quy chiếu thuận (khi quay
trục Ox dương đến Oy dương và trục Oz còn lại dương thì gọi là hệ quy
chiếu thuận)
✓ Gốc tọa độ oi được chọn tại giao điểm của xi và zi
✓ Đối với hệ tọa độ (Oxyz ) 0 theo quy ước trên ta chọn được trục z0 , còn

x0 được chọn tùy ý sao cho chúng vuông góc với nhau
• Cách xác định các thông số trong bảng D-H:
✓

là góc quay quanh trục zi

i

1

để trục xi

1

chuyển đến trục

xi ( xi //

xi )
✓ d i là khoảng cách giữa 2 trục xi

1

và xi

✓ ai là khoảng cách giữa 2 trục zi

1

và zi

✓

i

là góc quay quanh trục xi sao cho trục

zi

1

( zi

1

// zi ) chuyển đến

trục z

Sinh viên thực hiện: Phạm Duy Trọng

Page | 23


Thiết kế và mô phỏng điều khiển robot

GVHD: TS. Nguyễn Danh Trường

2. Giải bài toán động học thuận
2.1.
-

Ma trận D-H

Ma trận D-H tổng quát:

Ai

Rot ( z , i ).Trans (0,0, di ).Trans (ai ,0,0).Rot ( x,

cos

i

sin

i

sin i cos
cos i cos

0
0
-

i

cos i sin

i

cos
0

i

i

ai cos

i

ai sin

i

di
1

i

i

di

ai

1

1

d1 =900

a1 = 0

2

2

3

3

d2 = 0
d3 = 0

a2 = 650
a3 = 400

-

)

Bảng thông số D-H
i

1

=

2

2

=0

3

=0

Với quy ước:

C1

cos 1 , S1

sin 1 , C2

C3

cos 3 , S3

sin 3 , C12

C123

A3

sin
0

sin i sin

Khâu

-

A1

i

i

cos(

1

2

3

cos 2 , S2
cos(

), S123

sin(

sin

1

), S12

2
1

2

2

sin(
3

1

2

)

)

Ma DH của các khâu:

C1 0

S1

0

S1

0

C1

0

0
0

1
0

0
0

d1
1

,

C3

S3

0 a3C3

S3

C3

0 a3 S3

0
0

0
0

1
0

A2

C2

S2

0 a2C2

S2

C2

0 a2 S2

0
0

0
0

1
0

0
1

,

0
1

Sinh viên thực hiện: Phạm Duy Trọng

Page | 24


Thiết kế và mô phỏng điều khiển robot

Ma trận T – Phương trình động học thuận của 3 khâu:

-

• Khâu 1:

T1

A1

C1 0

S1

0

S1

0

C1

0

0
0

1
0

0
0

d1
1

 Tọa độ điểm cuối khâu 1

• Khâu 2:

T2

GVHD: TS. Nguyễn Danh Trường

T2

A1. A2

A

0;0; d1

C1 0

S1

0 C2

S1

0

C1

0

0
0

1
0

0
0

d1
1

C1C2

C1S2

S1

a2C1C2

S1C2

S1S2

C1

a2 S1C2

C2
0

0
0

S2
0

.

S2

0 a2C2

S2

C2

0 a2 S2

0
0

0
0

1
0

0
1

d 1 a2 S2
1

 Tọa độ điểm cuối khâu 2 là

B

a2C1C2 ; a2 S1C2 ; d1

a2 S2

• Khâu 3:

T3

T3

A1. A2 . A3

T2 . A3

C 1 C23

C1S23

S1

C1 (a3C23

a2C2 )

S1C23

S1S23

C1

S1 (a3C23

a2C2 )

C23

0

0
0

a3 S23

S23
0

a2 S2
1

d1

T3 là phương trình động học thuận của robot
 Tọa độ điểm cuối của khâu 3 là:

Sinh viên thực hiện: Phạm Duy Trọng

Page | 25