Bài tập nhóm mô phỏng hệ cơ điện tử

TIẾN ĐỘ HOÀN THÀNH VÀ CHƯA HOÀN THÀNH1 Động học Thuận Quy luật quỹ đạo điểm cuối Yes Mô phỏng số Đặc biệt và bất kì Yes Quỹ đạo tròn thẳng theo thời gian Yes Ngược Tính được các mô men đ

MỤC LỤC

MỤC LỤC 1

THÀNH VIÊN VÀ PHÂN CÔNG CÔNG VIỆC: 3

CHƯƠNG 1: PHÂN TÍCH, THIẾT KẾ CẤU TRÚC 3D 5

1.1 Phân tích cấu trúc 5

1.2 Thiết kế 3D Robot trên Solidworks 7

1.2.1 Thiết kế từng khâu và đặt hệ trục 7

1.2.2 Lắm ghép và xuất file 15

CHƯƠNG 2: PHẦN ĐỘNG HỌC 17

2.1 Động học thuận 17

2.1.1 Tình các phương trình động học cần thiết 17

2.1.2 Phương trình quỹ đạo điểm cuối, các vận tốc, gia tốc 19

2.1.3 Mô phỏng động học thuận 24

2.2 Động học ngược 40

2.2.1 Giải bài toán động học ngược bằng phương pháp số (Newton-raphson) 40 2.2.2 Kết quả tính toán các biến khớp, vận tốc khớp, giá tốc khớp 45

2.2.3 Kiểm tra tính chính xác của bài toán ngược: 49

CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO 52

3.1 Thiết kê quỹ đạo hình học đơn giản: 52

3.2 Thiết kế quỹ đạo đi qua 5 điểm: 53

CHƯƠNG 4: MÔ PHỎNG OPEN GL 63

4.1 Giới thiệu chung về OpenGL 63

4.2 Mô phỏng quỹ đạo đường tròn: 76

CHƯƠNG 5: ĐỘNG LỰC HỌC 77

5.1 Động lực học tổng quát 77

5.3 Mô phỏng số bài toán động lực học ngược: 81

5.4 Mô phỏng bài toán động lực học thuận: 87

CHƯƠNG 6: THIẾT KẾ BỘ ĐIỀU KHIỂN 90

6.1 Chọn luật điều khiển PID tổng quát 90

6.2 Mô phỏng PID bằng Simulink 92

CHƯƠNG 7: KẾT QUẢ VÀ ĐỒ THỊ 95

7.1 Mô tả vị trí các code tính toán 95

7.2 Phần động học tổng quát : 96

7.3 Động học thuận 98

7.4 Động học ngược 105

7.5 Thiết kế quỹ đạo 111

7.6 Động lực học ngược 113

PHỤ LỤC 116

THÀNH VIÊN VÀ PHÂN CÔNG CÔNG VIỆC:

code maple, matlab) +động lực học (tổngquát+ngược) + thiết kế quỹđạo

10/10Tích cực

phỏng Simulink (sửa các lỗixuất, trục, body), open GL

10/10Tích cực

maple) + động lực học( hỗtrợ sửa lỗi, sửa code)

10/10Tích cực

PID+ động lực học ( thamgia hỗ trợ giải, sửa lỗi)

10/10Tích cực

+PID +động lực học (viết

thuận+ngược)

10/10Tích cực

Simulink + hỗ trợ sửa lỗicode

10/10Tích cực

NOTE: - Các kết quả tính toán quá dài sẽ được trình bày trong phụ lục.

TIẾN ĐỘ HOÀN THÀNH VÀ CHƯA HOÀN THÀNH

1 Động học Thuận Quy luật quỹ đạo điểm cuối Yes

Mô phỏng số (Đặc biệt và bất kì) Yes

Quỹ đạo tròn thẳng theo thời gian

Yes

Ngược Tính được các mô men đối vơi Yes

Thuận Sử dụng kết quả tính ngược tính

ngược lại các biến khớp

Yes

Chọn hệ số

Từ bài toán động lực học thuận

ko có PID so sánh độ lệch đồ thị (tính toán, thực) tìm các hệ số.

Yes

Mô phỏng

Mô phỏng PID trên simulink Yes

phỏng

Open GL

Chạy thành công quỹ đạo thẳng tròn

Yes

Mô phỏng trên Matlab

Chạy thành công quỹ đạo thẳng tròn trên matlab Mô hình có gắn PID và sử dụng kết quả tính toán động lực học.

Yes

6 Báo cáo Words Hoàn thành báo cáo chi tiết Yes

Power point

DANH MỤC HÌNH ẢNH

Hình 1 Mô hình Robot và hệ trục Dh tương ứng 8

Hình 2 Minh họa về phương pháp đặt hệ trục D-H: 9

Hình 3 Khâu đế 13

Hình 4 Cách đặt Hệ trục khâu đế 13

Hình 5 Khâu 1 14

Hình 6 Cách đặt hệ trục khâu 1 14

Hình 7 Khâu 2 15

Hình 8 Cách đặt hệ trục khâu tịnh tiến ( khâu 2) 15

Hình 9 Khâu 3 16

Hình 10 Cách đặt hệ trục khâu quay (Khâu 3) 16

Hình 11 Khâu 4 17

Hình 12 Cách đặt hệ trục khâu quay (khâu 4) 17

Hình 13 Miền làm việc theo phương X-Y 25

Hình 14 Miền làm việc trong không gian Y-Z 26

Hình 15 Miền làm việc trong không gian X-Z 26

Hình 16 Miền làm việc vẽ trong không gian 3D 27

Hình 17 Quỹ đạo điểm cuối bài toán thuận kiểm tra 29

Hình 18 Quỹ đạo mô phỏng thuận 30

Hình 19 Đồ thị vận tốc, gia tốc điểm E theo phương x 31

Hình 20 Đồ thị vận tốc, gia tốc điểm E theo phương y 31

Hình 21 Đồ thị vận tốc gia tốc điểm E theo phương z 31

Hình 22 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo x 32

Hình 23 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo y 33

Hình 24 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo z 33

Hình 26 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo y 34

Hình 27 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo z 35

Hình 28 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x 35

Hình 29 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo y 36

Hình 30 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo z 36

Hình 31 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x 37

Hình 32 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo y 37

Hình 33 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo z 38

Hình 34 Vận tốc gia tốc góc khâu 1 theo x,y,z 39

Hình 35 Vận tốc gia tốc góc khâu 2 theo x,y,z 40

Hình 36 Vận tốc gia tốc góc khâu 1 theo x,y,z 41

Hình 37 Vận tốc gia tốc góc khâu 1 theo x,y,z 42

Hình 38 Quỹ đại điểm cuối theo quy luật cho trước 44

Hình 39 Đồ thị kiểm tra 45

Hình 40 Vẽ hai đồ thị trên cùng một miền 45

Hình 41 Đồ thị quy luật cho trước 46

Hình 42 Đồ thị kiểm tra 47

Hình 43 Vẽ hai đồ thị trên cùng một miền 47

Hình 44 Đồ thị q1 48

Hình 45 Đồ thị q2 49

Hình 46 Đồ thị q3 50

Hình 47 Đồ thị q4 51

Hình 48 Đồ thị vận tốc thực và vận tốc kiểm tra phương x 52

Hình 49 Đồ thị vận tốc thực và vận tốc kiểm tra phương y 53

Hình 50 Đồ thị vận tốc thực và vận tốc kiểm tra phương z 53

Hình 51 Quỹ đạo đoạn 1 59

Hình 52 Quỹ đạo đoạn 2 60

Hình 53 Quỹ đạo đoạn 3 60

Hình 54 Quỹ đạo đoạn 4 61

Hình 55 Quỹ đọa đi qua 5 điểm 61

Hình 56 Đồ thị vận tốc theo phương x 63

Hình 57 Đồ thị vận tốc theo phương y 64

Hình 58 Đồ thị vận tốc theo phương z 65

Hình 59 Mô phỏng quỹ đạo tròn 79

Hình 60 Mô hình động học Robotác thông số đầu vào: 81

Hình 61 Mô tả sumlink giải động học ngược 85

Hình 62 Đồ thị MoMen khâu 1 85

Hình 63 Đồ thị Lực khâu 2 86

Hình 64 Đồ thị Momen khâu 3 86

Hình 65 Đồ thị Momen khâu 4 87

Hình 66 Đồ thị Momen khớp 1 87

Hình 67 Đồ thị Lực khớp 2 88

Hình 68 Đồ thị Momen khớp 3 88

Hình 69 Đồ thị Momen khớp 4 89

Hình 70 Mô tả simulink giải động học thuận 90

Hình 71 Đồ thị tính toán và so sánh q1 91

Hình 72 Đồ thị tính toán và so sánh q2 91

Hình 73 Đồ thị tính toán và so sánh q3 92

Hình 74 Đồ thị tính toán và so sánh q4 92

Hình 75 Mô hình điều khiển tổng quát 93

Hình 76 Mô hình điều khiển PID thiết kế Thiết kế: 93

Hình 77 Toàn bộ sơ đồ PID 95

Hình 78 Tín hiệu đặt 96

Hình 80 Khối tính các biến khớp thực tế theo lực, mô men điều khiển 97

Hình 81 Khối so sánh lực thực tế và tính toán 97

Hình 82 Khối PID 97

Hình 83 quỹ đạo điểm cuối bài toán thuận theo quỹ đạo tùy ý bên trên 102

Hình 84 Đồ thị vận tốc gia tốc điểm cuối 103

Hình 85 Vận tốc gia tốc góc khâu 1 theo x,y,z 104

Hình 86 Vận tốc gia tốc góc khâu 2 theo x,y,z 105

Hình 87 Vận tốc gia tốc góc khâu 1 theo x,y,z 106

Hình 88 Vận tốc gia tốc góc khâu 1 theo x,y,z 107

Hình 89 Kiểm tra quỹ đạo thẳng 108

Hình 90 Kiểm tra quỹ đạo tròn 109

Hình 91 Kiểm tra khớp 1 110

Hình 92 Kiểm tra khớp 2 110

Hình 93 Kiểm tra khớp 3 111

Hình 94 Kiểm tra khớp 4 111

Hình 95 kiểm tra vận tốc phương x 112

Hình 96 kiểm tra vận tốc phương y 112

Hình 97 Kiểm tra vận tốc phương z 113

Hình 98 Đồ thị quỹ đạo và các điểm đã chọn 115

CHƯƠNG 1: PHÂN TÍCH, THIẾT KẾ CẤU TRÚC 3D

1.1 Phân tích cấu trúc.

Hình 1 Mô hình Robot và hệ trục Dh tương ứng

Sử dụng phương pháp DH :

Theo DH tại mỗi khớp của robot đặt một hệ chục tọa độ quy ước về việc đặt

hệ trục tọa độ này như sau:

-Trục zi được đặt tại khớp i+1, chiều của trục zi tùy ý.

Nếu hai trục i và i+1 cắt nhau xi được xác định dựa vào zi và zi+1 theo quy tắc bàn tay phải

-Trục yi xác định dựa vào xi và zi theo quy tắc bàn tay phải

Hình 2 Minh họa về phương pháp đặt hệ trục D-H:

- Lập ma trận Denavit – Hartenberg

Các thông số động học Denavit – Hartenberg được xác định như sau:

 d i: khoảng cách Oi-1 và Oi theo trục zi-1.

Cách đặt hệ trục mong muốn trong solidword:

1 Chọn Move/copy , bôi đen toàn bộ khâu

2 Chọn tọa độ điểm đầu và điểm cần đặt hệ trục:

3 Chọn để hiển thị hệ trục:

- Hình ảnh thiết kế 3D ( từng khâu):

Hình 3 Khâu đế

-Hệ trục đặt tại khâu 0 khớp 1: O0x0y0z0

Hình 4 Cách đặt Hệ trục khâu đế

Hình 5 Khâu 1

-Hệ trục đặt tại khớp 2 khâu 1: O1x1y1z1

Hình 6 Cách đặt hệ trục khâu 1

Hình 7 Khâu 2

-Hệ trục đặt tại khớp 3 khâu 2: O2x2y2z2

Hình 8 Cách đặt hệ trục khâu tịnh tiến ( khâu 2)

Hình 9 Khâu 3

-Hệ trục đặt tại khớp 4 khâu 3 : O3x3 y3z3

Hình 10 Cách đặt hệ trục khâu quay (Khâu 3)

Hình 11 Khâu 4

-Hệ trục đặt tại điểm lắp dụng cụ khâu 4: O4x4y4z4

Hình 12 Cách đặt hệ trục khâu quay (khâu 4)

1.2.2 Lắm ghép và xuất file.

- Hình ảnh lắm ghép ( từng khâu)

- Các bước xuất ra file dùng để mô phỏng:

CHƯƠNG 2: PHẦN ĐỘNG HỌC

2.1 Động học thuận.

Mục tiêu: - Khảo sát chuyển động khâu cuối phụ thuộc vào quy luật biến khớp

cho trước và tìm miền làm việc của Robot

- Ứng dụng trong thực tế: Từ các quan hện giữa quỹ đạo điểm cuối và biến

khớp, giúp thiết lập miền không gian làm việc phù hợp, thiết kế chiều dài cáckhâu phù hợp với yêu cầu làm việc

2.1.1 Tình các phương trình động học cần thiết.

T40=T10.T21.T32.T43

Ma trận biến đổi tọa độ thuần nhất Denavit – Hartenberg:

T i i−1=

[

sin(q1)=s1, sin(q2)=s2,sin(q4)=s4

cos(q1)=c1, cos(q2)=c2, cos(q4)=c4

Khâu 0 sang khâu 1:T10=

[

s1 c10 a1∗s1

0 0

01 00

d1

1

]

Khâu 1 sang khâu 2:T21=

[

0 0

Note: Kết quả chi tiết ở phụ lục

2.1.2 Phương trình quỹ đạo điểm cuối, các vận tốc, gia tốc.

Ta giải quỹ đạo điểm cuối bằng cách chọn giải 3 trong tổng số 12 phương trìnhlập được ở mục 2.1.1

Vận tốc gia tốc khối tâm khâu:

Tọa độ khối tâm khâu trong hệ tọa độ gắn với khâu: (các thông số để dạng chữ Số

=

[

211∗sin(q1)∗(203∗cos ⁡(q 3 ) ¿ +30∗cos ⁡(q1 )) ¿ 203∗sin(q3)+600+q2

r C 40 = ¿

¿Vận tốc khối tâm khâu-ma trận jacobi tịnh tiến khối tâm khâu

Note: kết quả chi tết ở phụ lục

Miền làm việc của robot:

- Từ giới hạn từng khâu và phương trình quỹ đạo điểm cuối ta tính toán miền

Sử dụng tính toán lập trình trên matlab ta có các đồ thị miền làm việc:

Hình 13 Miền làm việc theo phương X-Y

Hình 14 Miền làm việc trong không gian Y-Z

Hình 15 Miền làm việc trong không gian X-Z

Hình 16 Miền làm việc vẽ trong không gian 3D

2.1.3 Mô phỏng động học thuận.

1 Mô phỏng bằng quy luật đặc biệt ( kiểm tra bài toán thuận):

Chọn các quy luật biến khớp đặc biệt: ( thuộc giới hạn biến khớp)

so sánh 2 kết quả để kiểm tra

Tọa độ điểm cuối dễ dàng được xác định bằng phương pháp hình học:

U B=

{

3)+400=950(mm)¿y E=0(mm)

z E=d1+q2+a3∗sin(q3)=770+50+400∗sin

(

)

Tọa độ điểm cuối được tính bằng phương pháp giải số:

Hình 17 Quỹ đạo điểm cuối bài toán thuận kiểm tra

Kết luận: Ta thấy kết quả tính toán hình học và kết quả tính toán bằng ma trận

DH trên maple hoàn toàn trùng nhau nên kết quả giải động học thuận là đúng.

2 Thiết kế mô phỏng theo quỹ đạo bất kì:

Vận tốc, gia tốc điểm E:

Hình 19 Đồ thị vận tốc, gia tốc điểm E theo phương x

Hình 20 Đồ thị vận tốc, gia tốc điểm E theo phương y

Hình 21 Đồ thị vận tốc gia tốc điểm E theo phương z

Đồ thị vận tốc, gia tốc khối tâm các khâu:

Khâu 1

Hình 23 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo y

Hình 24 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo z.

Khâu 2

Hình 25 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo x.

Hình 27 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo z

Khâu 3

Hình 28 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x

Hình 29 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo y

Khâu 4

Hình 31 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x

Hình 32 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo y

Hình 33 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo z

Đồ thị vận tốc góc các khâu:

Khâu 1:

Hình 34 Vận tốc gia tốc góc khâu 1 theo x,y,z

Khâu 2:

Hình 35 Vận tốc gia tốc góc khâu 2 theo x,y,z

Khâu 3:

Hình 36 Vận tốc gia tốc góc khâu 1 theo x,y,z

Khâu 4:

Hình 37 Vận tốc gia tốc góc khâu 1 theo x,y,z

2.2Động học ngược

Mục tiêu: - Khảo sát quy luật biến khớp phụ thuộc vào quy luật chuyển động

điểm cuối cho trước Nhằm phục vụ bài toán thiết kế quỹ đạo

- Ứng dụng trong thực tế: Giúp quá trình mô phỏng chuyển động và khảo sát

các biến khớp khi cho chuyển động theo quỹ đạo mong muốn

2.2.1 Giải bài toán động học ngược bằng phương pháp số (Newton-raphson).

Lý thuyết tính toán:

Trong giải tích số, phương pháp Newton-Rapshon là một phương pháp tìm nghiệm xấu xỉ gần đúng của một hàm số có tham số thực dựa vào tính hội tụ và ý nghĩa hình học của hàm số:

Phương pháp được thực hiện như sau:

trình giải góc Cardan (rotx roty rotz) mà ta đã chọn Tính các đạo hàm của 3phương trình trên thực hiện đáp ứng ta tính được x1 là 1 xấp xỉ tốt hơn x0:

x1=x0−f(x0)

f '(x0).(2.9)

x0 là nghiệm đầu ta giải được tại t=0

Lặp đi lặp lại quá trình trên ta được xấp xỉ :x n=x n−1− f(x n−1)

- Đồ thị thật: Đồ thị được vẽ bằng phương trình quỹ đạo đặt cho trước.

Hình 38 Quỹ đại điểm cuối theo quy luật cho trước

- Đồ thị bài toán ngược: sau khi gải bài toán ngược được các giá trị biếnkhớp tương ứng, thay lại vào bài toán thuận để vẽ đồ thị quỹ đạo điểmcuối

Mô phỏng số bài toán ngược (đường tròn):

- Đồ thị thật: Đồ thị được vẽ bằng phương trình quỹ đạo đặt cho trước

Hình 41 Đồ thị quy luật cho trước

- Đồ thị bài toán ngược: sau khi gải bài toán ngược được các giá trị biếnkhớp tương ứng, thay lại vào bài toán thuận để vẽ đồ thị quỹ đạo điểmcuối.

Hình 42 Đồ thị kiểm tra

- Kiểm tra chính xác về mặt quỹ đạo t vẽ chung 2 đồ thị trên cùng mộtmiền

Hình 43 Vẽ hai đồ thị trên cùng một miền

Kết luận: Bài toán ngược chính xác về mặt quỹ đạo.

2.2.2 Kết quả tính toán các biến khớp, vận tốc khớp, giá tốc khớp

Để kiểm tra bài toán ngược ta có các điều khiện sau:

1 Tại các tọa độ cực trị của đồ thị biến khớp q ( q chuyển từ tăng thành giảm hoặc ngược lại) thì vận tốc của khớp tại thời điểm đó phải bằng 0

2 Tại các điểm cực trị của vận tốc khớp ( vận tốc chuyển từ tang thành giảm hoặc ngược lại) thì gia tốc tại thời điểm đó phải bằng 0

a) Đồ thị đối với q1

0 1 2 3 4 5 6 7 8 9 10

s

-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1

Hình 44 Đồ thị q1

Kiểm tra:

- Tại các điểm A , B trên đồ thị khi đồ thị q1 đổi chiều thì vận tốc v q 1=0.

- Tại các điểm C trên đồ thị khi đồ thị v q 1 đổi chiều thì vận tốc a q 1=0

b) Đồ thị đối với q2

0 1 2 3 4 5 6 7 8 9 10

s -200

Van toc =0 thi q cuc tri

Gia toc =0 thi van toc cuc tri

Hình 45 Đồ thị q2

Kiểm tra:

- Tại A , B , Đồ thị q2 đặt cực trị ( đổi chiều tăng giảm) thì v q 2=0.

- Tại C Đồ thị v q 2đặt cực trị ( đổi chiều tăng giảm) thì a q 2=0.

c) Đồ thị đối với q3

s -0.5

0 0.5 1 1.5 2

Van toc = 0 , q cuc tri

gia toc = 0 van toc cuc tri

Hình 46 Đồ thị q3

Kiểm tra:

- Tại A Đồ thị q3 đặt cực trị ( đổi chiều tăng giảm) thì v q 3=0.

- Tại B ,C Đồ thị v q 3đặt cực trị ( đổi chiều tăng giảm) thì a q 3=0.

Kết luận: Bài toán ngược giải ra q3 đúng

gia toc =0 thi van toc cuc tri van toc =0 q cuc tri

Hình 47 Đồ thị q4

Kiểm tra:

- Tại A Đồ thị q4 đặt cực trị ( đổi chiều tăng giảm) thì v q 4=0.

- Tại B ,C Đồ thị v q 4đặt cực trị ( đổi chiều tăng giảm) thì a q 4= 0.

Kết luận: Bài toán ngược giải ra q4 đúng

2.2.3 Kiểm tra tính chính xác của bài toán ngược:

Phương pháp:

- Tính ma trận Jacobi đối với khâu cuối

- Vận tốc điểm E tính theo Jacobi có công thức:

Hình 49 Đồ thị vận tốc thực và vận tốc kiểm tra phương y

Nhận xét: Các kết quả đồ thị nhận được tương đối chính xác với mức sai tối ta trên đồ thị là 0.6 mm/s.

Kết luận chung:

Bài toán ngược giải bằng phương pháp số trên là chính xác

CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO

3.1Thiết kê quỹ đạo hình học đơn giản:

Mục tiêu: thiết kế các quỹ đạo chuyển động khâu cuối theo thời gian phù hợp

với chuyển động, miền làm việc của Robot

- Ứng dụng trong thực tế: Áp dụng cho bài toán động học ngược hoặc bài

Chiếu lên 2 trục : x=a 1∗t+b1 , z=a 2∗t+b 2

Thay các điều kiện: t=0 : x=600 , z=300 t=6 : x=900 , z=900

Được quy luật điểm cuối: x Et=100∗t+600 ; y Et=0 ; z Et=100∗t+300.

Đường tròn đi tâm I ( 600,0 , 200) và bán kính R =100 chuyển động trong mặt phẳng song song Oxz.

Phương trình đường thẳng đi qua A, B: x Et=100∗cos (φ) +600.

y Et=100∗sin (φ), z Et=200. Thay các điều kiện: t=0 : x=500 , y=0 , z=200.

t=5 : x=700 , y=0 , z=200.

Được quy luật điểm cuối: x Et=100∗cos

(

)

1