(Đồ án tốt nghiệp) thiết kế và điều khiển cánh tay robot ba bậc tự do

Chúng em cũng xin chân thành cảm ơn các thầy cô giáo trong trường ĐH SưPhạm Kỹ Thuật TP Hồ Chí Minh nói chung, các thầy cô trong Bộ môn Tự ĐộngHóa Điều Khiển nói riêng đã dạy dỗ cho em k

KHÓA LUẬN TỐT NGHIỆP

NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA

THIẾT KẾ VÀ ĐIỀU KHIỂN CÁNH TAY ROBOT BA BẬC TỰ DO

GVHD: ThS NGUYỄN PHONG LƯU SVTH: NGUYỄN VĂN TÂY

MSSV: 11151064 SVTH: KIỀU QUANG PHÚC MSSV: 11151156

SKL004114

Tp Hồ Chí Minh, tháng 7/2015

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM

Ngành: Công Nghệ Kỹ Thuật Điều Khiển và Tự Động Hóa Lớp: 11151CL1

Giảng viên hướng dẫn: Th.S Nguyễn Phong Lưu ĐT:

Ngày nhận đề tài: 15/3/2015 Ngày nộp đề tài: 25/7/2015

1 Tên đề tài: Thiết kế và điều khiển cánh tay robot ba bậc tự do

2 Các số liệu và tài liệu ban đầu:

- Sách giáo trình “Kỹ thuật robot” của trường Đại Học Sư Phạm Kỹ Thuật Thành Phố Hồ Chí Minh

- Tài liệu về thư viện Waijung từ trang web “http://waijung.aimagin.com.”

- Tính toán các phương trình động học

- Viết phương trình điều khiển cho cánh tay robot

- Nhận các giá trị trả về từ cánh tay robot để có thể giám sát

- Quy hoạch quỹ đạo bằng thuật toán Bézier

- Mô hình cánh tay robot ba bậc tự do

i

CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT

NAM Độc lập – Tự do – Hạnh phúc

Tp Hồ Chí Minh , ngày 25 tháng 7 năm 2015

PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN

Họ và tên sinh viên: Nguyễn Văn Tây MSSV:11151064

Ngành: Công Nghệ Kỹ Thuật Điều Khiển và Tự Động Hóa

Tên đề tài: Thiết kế và điều khiển cánh tay robot ba bậc tự do

Họ và tên Giáo viên hướng dẫn: Th.s Nguyễn Phong Lưu

Tp Hồ Chí Minh, ngày 25 tháng 7 năm 2015

Giáo viên hướng dẫn

(Ký & ghi rõ họ tên)

ii

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM

Độc lập – Tự do – Hạnh phúc

Tp Hồ Chí Minh , ngàytháng 7 năm 2015

PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN

Tp Hồ Chí Minh, ngày tháng 7 năm 2015

Giáo viên phản biên

(Ký & ghi rõ họ tên)

iii

Chúng em cũng xin chân thành cảm ơn các thầy cô giáo trong trường ĐH SưPhạm Kỹ Thuật TP Hồ Chí Minh nói chung, các thầy cô trong Bộ môn Tự ĐộngHóa Điều Khiển nói riêng đã dạy dỗ cho em kiến thức về các môn đại cương cũngnhư các môn chuyên ngành, giúp em có đuợc cơ sở lý thuyết vững vàng và tạo điềukiện giúp đỡ em trong suốt quá trình học tập.

Cuối cùng, em xin chân thành cảm ơn gia đình và bạn bè, đã luôn tạo điều kiện,quan tâm, giúp đỡ, động viên em trong suốt quá trình học tập và hoàn thành đồ ántốt nghiệp

Mặc dù chúng em đã cố gắng hoàn thành đề tài nhưng cũng không thể tránh đượckhỏi nhiều sai sót nhất định do về mặt thời gian và kiến thức, chúng em rất mongđược sự thông cảm và chia sẻ quý báu từ các thầy cô và các bạn

Chúng em xin gửi lời chúc sức khỏe và thành đạt tới tất cả các quý thầy cô cùngcác bạn

TP.Hồ Chí Minh, tháng 07 năm 2015

Nhóm thực hiện đề tàiNguyễn Văn Tây – Kiều Quang Phúc

iv

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

TÓM TẮT

Trên thế giới hiện nay, ngành công nghiệp có những phát triển vượt trội và đadạng, bên cạnh đó nhằm tăng tính cạnh tranh và hạ giá thành sản phẩm đó là một bàitoán đặt ra cho hầu hết các doanh nghiệp Nhằm đáp ứng như cầu đó thì ngành côngnghiệp robot được nghiên cứu và phát triển

Robot có thể hoạt động với những điều kiện khắc nghiệt và làm việc với tấn suấtcao Robot ngoài việc đáp ứng trong công nghiệp với những phát triển vượt bậcrobot còn đi sâu vào đời sống của con người, trong các ngành y tế, chăm sóc conngười, trong quân sự, quốc phòng

Có thể thấy sự phát triển của ngành công nghệ robot, nhóm chúng em chọn đề tài

“Thiết kế và điều khiển cánh tay robot ba bậc tự do.” có thể vận dụng những kiếnthức cơ bản nhất của kỹ thuật robot và điều khiển tự động để thiết kế nên một cánhtay robot ba bậc tự do dơn giản Điều khiển qua phần mềm Matlab thông qua kitSTM32F4 Discovery và thư viện hỗ trợ Waijung

Thông qua giải thuật Bézier để xây dựng quỹ đạo của cánh tay có thể tránh cácvật cảng trên quỹ đạo đi của cánh tay

Xây dựng mô hình cánh tay robot đơn giản

TP.Hồ Chí Minh, tháng 07 năm 2015

Nhóm thực hiện đề tàiNguyễn Văn Tây – Kiều Quang Phúc

v

MỤC LỤC

NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP i

PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN ii

PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN iii

LỜI CẢM ƠN iv

TÓM TẮT v

MỤC LỤC vi

MỤC LỤC HÌNH x

MỤC LỤC BẢNG xii

CHƯƠNG 1 TỔNG QUAN ĐỀ TÀI 1

1.1 Đặt vấn đề 1

1.2 Tính cấp thiết của đề tài 2

1.3 Mục tiêu của đề tài 2

1.4 Nhiệm vụ của đề tài 2

1.5 Đối tượng và phạm vi của đề tài 3

1.6 Phương pháp nghiên cứu 3

1.7 Bố cục của Đồ án 3

CHƯƠNG 2 CƠ SỞ LÝ THUYẾT 4

2.1 Lý thuyết phần động lực học robot:[1] 4

2.1.1 Biểu diễn Denavit – Hartenberg của bài toán động học robot: 4

2.1.2 Động học thuận 6

2.1.3 Động học nghịch: 7

2.1.4 Jacobi và động học vận tốc: 10

2.2 Bộ điều khiển PID: 14

2.2.1 Thành phần tỷ lệ (P): 15

2.2.2 Thành phần tích phân (I) 15

vi

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

2.2.3 Thành phần vi phân (D) 16

2.2.4 Bộ điều khiển PID số 16

2.3 Giới thiệu về kit STM32F4 Discovery 17

2.3.1 Giới thiệu chung 17

2.3.2 Tính năng 18

2.3.3 Dãy công cụ phát triển: 19

2.3.4 Giới thiệu về dòng vi điều khiển STM32 19

2.3.5 Phần cứng 22

2.4 Thư viện Waijung Blockset [3] 24

2.4.1 Khối Target Setup 25

2.4.2 Khối Digital Output 26

2.4.3 Khối UART Setup 27

2.4.4 Khối UART Rx 28

2.4.5 Khối UART Tx 29

2.4.6 Khối Encoder Read 30

2.4.7 Khối Basic PWM 31

2.4.8 Khối Host Serial Setup 33

2.4.9 Khối Host Serial Tx 34

2.4.10 Khối Host Serial Rx 35

2.5 PL-2303 UART 36

2.5.1 Truyền thông nối tiếp không đồng bộ 36

2.5.2 Truyền thông nối tiếp không đồng bộ với STM32F4 Discovery (UART) 40 2.6 Đường cong Bézier 41

2.6.1 Đường cong Bézier toàn phương hay bậc hai (quadratic) 41

2.6.2 Định nghĩa đệ quy 42

2.6.3 Đường cong bậc hai 43

vii

CHƯƠNG 3NỘI DUNG THỰC HIỆN ĐỀ TÀI 44

3.1 Phương trình động học robot 44

3.1.1 Đặt tọa độ và bảng D – H 44

3.1.2 Động học thuận robot 45

3.1.3 Động học ngược robot 46

3.1.4 Ma trận Jacobi 46

3.2 Giải thuật điều khiển 48

3.3 Bộ điều khiển cơ bản trên Matlab 49

3.3.1 Bộ Init system 49

3.3.2 Bộ PID 50

3.3.3 Encoder 50

3.3.3 Motor 51

CHƯƠNG 4 THIẾT KẾ CÁNH TAY ROBOT 52

4.1 Thiết kế các khâu cho cánh tay Robot 52

4.1.1 Khớp 1 52

4.1.2 Khớp 2 53

4.1.3 Khớp 3 53

4.2 chọn động cơ cho từng khâu cánh tay Robot 54

4.2.1 Khớp 1 54

4.2.2 Khớp 2 54

4.2.3 Khớp 3 55

4.3 Chọn encoder cho từng khâu cánh tay Robot 56

4.4 Mạch cầu H cho từng động cơ của từng khâu cánh tay Robot 57

CHƯƠNG 5KẾT QUẢ THỰC HIỆN ĐỀ TÀI 58

5.1 Cánh tay robot ba bậc tự do 58

5.2 Các thông số PID cho hệ thống 59

viii

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

5.3 Quy hoạch quỹ đạo cánh tay robot 59

5.3.1 Điểm cuối [150 150 100] và điểm ảo [150 150 0] 59

5.3.2 Điểm cuối [150 150 100] và điểm ảo [150 0 150] 60

5.3.3 Điểm cuối [150 150 100] và điểm ảo [0 150 150] 61

5.3.4 Điểm cuối [150 150 100] và điểm ảo [150 0 0] 62

5.3.5 Điểm cuối [150 150 100] và điểm ảo [0 150 0] 63

CHƯƠNG 6KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 65

6.1 Kết luận 65

6.1.1 Ưu điểm 65

6.1.2 Nhược điểm 65

6.2 Hướng phát triển của đề tài 65

TÀI LIỆU THAM KHẢO 66

ix

MỤC LỤC HÌNH

Hình 2.1: Bộ PID cơ bản 14

Hình2.2: Hình ảnh thực tế Kit STM32F4 Discovery 18

Hình 2.3: Sơ đồ khối phần cứng kit STM32F4 Discovery 22

Hình 2.4: Sơ đồ kit STM32F4 23

Hình 2.5: Chức năng của các chân 24

Hình 2.6: Hình ảnh khối Targat Setup trong Simulink 25

Hình 2.7: Cấu hình cho Target Setup 25

Hình 2.8: Hình ảnh khối Digital Output trong Simulink 26

Hình 2.9: Cấu hình cho khối Digital Output 26

Hình 2.10: Hình ảnh khối UART Setup trong Simulink 27

Hình 2.11: Cấu hình cho khối UART Setup 28

Hình 2.12: Hình ảnh khối UART Rx trong Simulink 28

Hình 2.13: Cấu hình cho khối UART Rx 29

Hình 2.14: Hình ảnh khối UART Tx trong Simulink 29

Hình 2.15: Cấu hình cho khối UART Tx 30

Hình 2.16: Hình ảnh khối Encoder Read trong Simulink 30

Hình 2.17: Cấu hình cho khối Encoder Read 31

Hình 2.18: Hình ảnh khối Basic PWM trong Simulink 31

Hình 2.19: Cấu hình của khối Basic PWM 32

Hình 2.20: Hình ảnh khối Host Serial Setup trong Simulink 33

Hình 2.21: Cấu hình khối Host Serial Setup 33

Hình 2.22: Hình ảnh khối Host Serial Tx trong Simulink 34

Hình 2.23: Cấu hình cho khối Host Serial Tx 34

Hình 2.24: Hình ảnh khối Host Serial Rx trong Simulink 35

Hình 2.25: Cấu hình cho khối Host Serial Rx 35

x

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

Hình 2.26: UART PL2303 36

Hình 2.27: Tín hiệu tương đương của UART và RS232 37

Hình 2.28: Truyền 8 bit phương thức nối tiếp 38

Hình 2.27: Một đường cong Bézier bậc hai 43

Hình 3.1: hệ trục tọa độ cánh tay 44

Hình 3.2: Giải thuật điều khiển 48

Hình 3.3: Bộ Init system 49

Hình 3.4: Bộ PID 50

Hình 3.5: Bộ Encoder 50

Hình 3.5: Bộ Motor 51

Hình 4.1:Hướng 1 điểm cuối [150 150 100] và điểm ảo [150 150 0] 59

Hình 4.2:Hướng 2 điểm cuối [150 150 100] và điểm ảo [150 150 0] 60

Hình 4.3:Hướng 1 điểm cuối [150 150 100] và điểm ảo [150 0 150] 60

Hình 4.4:Hướng 2 điểm cuối [150 150 100] và điểm ảo [150 0 150] 61

Hình 4.5:Hướng 1 điểm cuối [150 150 100] và điểm ảo [0 150 150] 61

Hình 4.6:Hướng 2 điểm cuối [150 150 100] và điểm ảo [0 150 150] 62

Hình 4.7:Hướng 1 điểm cuối [150 150 100] và điểm ảo [150 0 0] 62

Hình 4.8:Hướng 2 điểm cuối [150 150 100] và điểm ảo [150 0 0] 63

Hình 4.9:Hướng 1 điểm cuối [150 150 100] và điểm ảo [0 150 0] 63

Hình 4.10:Hướng 2 điểm cuối [150 150 100] và điểm ảo [0 150 0] 64

xi

MỤC LỤC BẢNG

Bảng 3.1: Bảng D – H của cánh tay robot 44

Bảng 4.1: Các thông số PID 59

xii

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

CHƯƠNG 1 TỔNG QUAN ĐỀ TÀI

1.1 Đặt vấn đề

Hiện nay, đời sống đã và đang được thay đổi ngày càng văn minh và hiện đạihơn bỡi sự ứng của khoa học kỹ thuật vào đời sống lẫn trong sản xuất Ngànhđiện tử của thế giới có sự phát triển vượt bậc nên đã tạo ra những thiết bị có tínhchất: tốc độ cao, độ chính xác cao, sự đáp ứng nhanh tăng năng suất lao độngcủa các ngành công nghiệp

Ngành công nghiệp robot ra đời đã áp dụng trong nhiều lĩnh vực của conngười nhằm thay thế sức lao động và tăng tính hiệu quả trong công việc Mụctiêu ứng dụng của robot góp phần nâng cao năng xuất của dây chuyền sản xuất,giảm giá thành sản phẩm, cải thiện điều khiện lao động, ….Robot có thể hoạtđộng những môi trường mà con người không thể hoạt động và có tính năngvượt trội: làm việc liên tục trong thời gian dài, trong môi trường phóng xạ, môitrường độc hại, nhiệt độ cao,…

Hiện nay, trên thế giới đã xuất hiện các dây chuyền tự động gồm các máyCNC kết hợp với robot công nghiệp, với mức tự hóa cao, linh hoạt cao,….Bêncạnh đó không chỉ trong các phân xưởng, nhà máy, kỹ thuật robot còn được sửdụng trong việc khai thác thềm lục địa, trong y khoa, quốc phòng, ngoài vũ trụ,

…

Rõ ràng là khả năng làm việc của robot trong một số điều khiện vượt hơn khảnăng của con người Do đó, nó là phương tiện hữu hiệu để tự động hóa, nângcao năng suất lao động, giảm nhẹ cho con người trong các công việc nặng nhọc

và làm việc trong môi trường độc hại, nó có thể hoạt động trong lĩnh vực y tếchăm sóc bệnh nhân, thăm do các bộ phận trong cơ thể người …

Dựa trên những lý do trên nên nhóm em chọn đề tài “Thiết kế và điều khiểncánh tay robot ba bậc tự do” trên cơ sở những kiến thức căn bản mà chúng emhọc trong bốn năm và những kiến thức mới có từ bên ngoài chúng em thiết kếtcánh tay robot có thể vận dụng trong một số hoạt động sản xuất công nghiệpđơn giản

1

1.2 Tính cấp thiết của đề tài

Nước ta là một nước đang phát triển nên vấn đề làm thế nào để tăng năngsuất lao động Bên cạch đó, Ngành công nghiệp robot trên thế giới đã và đangphát triển một cách mạnh mẽ, đi sâu vào sản xuất và đời sống của con người.Robot được đưa vào sản suất thay thế con người trong một số công việc nhằmtăng năng suất và hạ giá thành sản phẩm tăng tính cạnh tranh của sản phẩm.Nắm được nhu cầu đó và có thể vận dụng những kiến thức căn bản đã học vềrobot, nhóm chọn đề tài “Thiết kế và điều khiển cánh tay robot ba bậc tự do”

Đề tài hướng tới có thể tạo ra một cánh tay robot đơn giản và có thể đáp ứngđược nhu cầu của ngành công nghiệp ở nước ta Bên cạnh đó có thể áp dụng cácgiải thuật điều khiển đơn giản PID vào bộ điều của cánh tay, áp dụng giải thuậtBézier để quy hoạch quỹ đạo, sử dụng phần mềm Matlab để điều khiển và đọcnhững dữ liệu cần thiết Dùng cho hệ thông này là hệ thống nhúng

1.3 Mục tiêu của đề tài

Rèn luyện khả năng tìm và đọc hiểu tài liệu

Vận dụng những lý thuyết đã học trên giảng đường vào đề tài

Biết được một cách tổng quát về phần cứng, phần mềm để có thể tạo nên mộtcánh tay robot ba bậc tự do

Vận dụng thiết kế một mô hình cánh tay robot ba bậc tự do

1.4 Nhiệm vụ của đề tài

Xây dựng hệ thống nhúng điều khiển cho cánh tay robot trên phần mềmMatlab thông qua kit STM32F4 và thông qua UART để đọc về các tín hiệu cầnthiết để giám sát

Thiết kế và lắp ráp phần cứng của cánh tay robot

2

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

1.5 Đối tượng và phạm vi của đề tài

Đối tượng của đề tài là các phương trình động học của cánh tay robot và đưa

ra thuật điều khiển thích hợp tối ưu cho cánh tay robot, phần mềm MatLab,STM32F4 Discovery, phần cơ khí cho cánh tay robot

Cơ sở khoa học và thực tiễn về chế tạo, điều khiển robot

Cách thức giao tiếp giữa STM32F4 với máy tính

Các phần mềm hỗ trợ của MatLab

Thiết kế và thi công phần cứng cánh tay robot ba bậc tự do

1.6 Phương pháp nghiên cứu

Trên cơ sở lý thuyết có thể áp dụng vào thực hiện cánh tay robot theo sựhướng dẫn và tìm hiểu của nhóm

1.7 Bố cục của Đồ án

Chương 1: Tổng quan đề tài

Chương 2: Cơ sở lý thuyết

Chương 3: Nội dung thực hiện đề tài

Chương 4: Kết quả thực hiện đề tài

Chương 5: Kết luận và hướng phái triển

3

CHƯƠNG 2 CƠ SỞ LÝ THUYẾT

2.1.1 Biểu diễn Denavit – Hartenberg của bài toán động học robot:

Mô hình hóa Denavie – Hartenberg (viết tắt là phương pháp D – H) là cáchbiểu diễn đơn giản mô hình hóa các khâu và khớp của robot và có thể sử dụngcho bất kỳ cấu hình robot nào từ đơn giản cho tới phức tạp Chúng có thể biểudiễn cho bất kỳ hệ trục tọa độ nào: Descartes, trụ, cầu, Euler, RPY, … Thêmvào đó, nó có thể sử dụng để biểu diễn cho tất cả Robot mà chúng ta có thể gặptrong công nghiệp và robot dịch vụ, robot biến hình Mặc dù, chúng ta có thểgiải các bài toán đơn giản bằng phương pháp ma trận thuần nhất hay hình họcnhưng với việc biểu diễn D – H chúng ta có thể sử dụng kết quả của nó cho cácbài toán như Jacobi, phân tích lực, độ cứng, ….[1]

Giả sử robot là một chuỗi các khâu và khớp nối tiếp Các khớp này có thể làkhớp trượt (tịnh tiến) hay khớp trụ (khớp quay), có thể xếp theo bất kỳ thứ tựnào, nằm trong bất kỳ mặt phẳng nào Khâu cũng có bất kỳ chiều dài nào, co thểxoắn hoặc cong Vì vậy, một tập hợp của các khâu và khớp đều có thể tạo mộtcấu hình robot và giải quyết các vấn đề bằng cách mô hình hóa nó [1]

Giả sử robot được cấu hình với một lượng khâu và khớp tùy ý, bố trì bất kỳ

Để mô hình hóa D – H, đầu tiên gắn cho mỗi khớp một hệ tọa độ tham chiếu địaphương Chúng ta phải gắn cho mỗi khớp trục z và trục x, không nhất thiết phảigắn trục y (có thể xác định theo tam diện thuận, biểu diễn D – H không cần thiếtdùng trục y) [1]

Tất cả các khớp đều được biểu diễn theo trục z Nếu khớp là khớp quay thìtrục z sẽ được xác định theo hướng chuyển tạo do sự quay đó theo quy tắc bàntay phải, khớp là tịnh tiến thì trục z sẽ dọc theo hướng di chuyển Với quy tắcnày ta thuận tiện gắn trục z lên các khớp Khớp quay thì góc quay quanh trục z

sẽ là biến khớp (θ hoặc α hoặc β …), đối với khớp tịnh tiến thì chiều dài củakhâu dọc theo trục z biểu diễn bỡi thông số d là biến khớp [1]

Việc mô hình hóa không nhất thiết các khớp phải song song hay phải giaonhau Trong mô hình hóa đưa ra thì trục z là những đường chéo nhau, vì vậy sẽ

4

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

tồn tại một đường thằng vuông góc chung giữa các trục z này với nhau (đây

chính là khoảng cách ngắn nhất giữa hai trục z) Hệ trục tham chiếu địa phương

luôn được gắn theo hướng đường vuông góc này Nếu hai trục z mà song song

với nhau thì có vô số đường vuông góc chung nên lấy đường vuông góc chung

của khớp trước, hai trục z mà giao nhau thì không tồn tại đường vuông góc

chung (chiều dài bằng 0) chúng ta sẽ gắn trục x dọc theo đường vuông góc với

mặt phẳng hình thành bởi hai trục này Điều này có nghĩa là đường vuông góc

chung là đường thẳng vuông góc với mặt phẳng chứa hai trục z và có hướng là

tích hữu hướng của hai trục z [1]

Như vậy, θ biểu diễn sự quay quanh trục z, d biểu diễn khoảng cách trên trục

z giữa hai đường vuông góc chung, a biểu diễn chiều dài của đoạn vuông góc

chung còn gọi là đoạn dịch chuyển của khớp, α biểu diễn góc giữa hai trục z kế tiếp nhau

Thường chỉ có θ và d là biến khớp [1]

Ma trận A biểu diễn bốn di chuyển bằng cách nhân bốn ma trận biểu diễn

từng chuyển động, kết quả như sau: [1]

Giải pháp gắn hệ trục lên robot: đánh số từ 0 cho tới n (n là số khâu của

robot) và số 0 cho bệ robot Mỗi khâu sẽ được gắn với mỗi hệ trục và tuân theo

quy tắt bàn tay phải [1]

Bảng tham số Denavit – Hartenberg:

5

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

θi: góc quay từ trục xi-1 tới xi xung quanh trục zi-1

αi: góc quay của trục zi-1 tới zi xung quanh trục xi

2.1.2 Động học thuận

Động học thuận là quá trình tính toán vị trí và hướng của cơ cấu chấp hànhcuối hay tay gắp khi biết tất cả các giá trị biến khớp Để tính toán vị trí vàhướng từ tập hợp các giá trị biến khớp thì các tham số của bảng Denavit –Hartenberg phải được biết Mỗi hệ trục mỗi khâu được mô tả đầy đủ bỡi matrận biểu diễn vị trí và hướng so với hệ trục của khâu trước theo chuỗi của robotcũng là thứ tự của các ma trận và là sự kệt hợp giữa các chuyển vị thuần nhất.Như vậy, động học thuận để tính vị trí và hướng của tay gắp hay là cơ cấu chấphành cuối so với hệ tọa độ tham chiếu của robot [1]

Như vậy ở hệ trục thứ i sẽ mô tả ma trận vị trí và hướng so với hệ trục thứ i-1như sau:

Chúng ta tính toán tọa độ của vector u so với hệ trục i-2 và sau đó sẽ dẫn

đến hệ trục tham chiếu toàn cục Chúng ta có sự chuyển đổi giữa các khâu như

Mục đích của động học thuận là tính toán vị trí và hướng P của cơ cấu chấp

hành so với hệ tham chiếu

2.1.3 Động học nghịch:

Đóng vài trò quan trọng trong việc điều khiển robot nên bài toán động học

nghịch được nghiên cứu từ những năm 1960 Sự phức tạp của bài toán phục

thuộc vào cấu trúc của robot Số bậc càng tăng thì độ phức tạp càng tăng, bên

cạch đó sự sắp xếp các khớp cũng ảnh hưởng không nhỏ đến sự phức tạp của

bài toán động học nghịch Một số bài toán co động học thuận đơn giản thì cóthể giải động học nghịch bằng phương pháp đại số [1]

7

Động học thuận nhằm giải quyết vấn đề về vị trí và hướng của khớp cuối hay

là cơ cấu chấp hành khi đã biết tất cả các biến khớp Còn bài toán nghịch thì tìmtập hay nhiều tập nghiệm của khớp được gọi là cấu hình robot trong không giankhớp Giả sử, tay máy có n khớp thì tập giá trị khớp là ma trận [n x 1] Độnghọc nghịc vị trí là trọng tâm của vấn đề điều khiển tay máy Với vị trí tay gắp(cơ cấu chấp hành cuối) trong hệ Descartes sẽ có một tập giá trị khớp đáp ứngmỗi khớp robot điều khiển độc lập [1]

Động học cánh tay máy thường là sự chuyển đổi giữa hai không gian: khônggian Descartes và không gian khớp [1]

Ta có:

Với Ai = Ai(qi) là hàm số biến qi là ma trận chuyển từ hệ trục i-1 tới i và P là

ma trận vị trí và hướng của cơ cấu chấp hành Chúng ta co phương trình mô tảtương đương vị trí và hướng như sau: [1]

Đầu tiên ta phải giải bài toán động học thuận liên hệ với vị trí và hướng của

cơ cấu chấp hành Các biến khớp với khớp quay qi = i và khớp tịnh tiến qi =

di Với robot có n khớp thì phương trình động học thuận là: [1]

P  f q1 , q2 , , q n 

Khi giải được các phương trình động học thuận phải xem xét để xác địnhbiến nào có thể tính toàn từ các phương trình này Sử dụng vài phương trình đểgiải bằng phương pháp đại số, mục đích là đưa ra các phương trình giải cácbiến khớp một cách đơn giản nhất Nhiều phương trình toán học có dạng: [1]

Với a, b, d là các giá trị đã biết hoặc là hằng số và si, ci là giá trị sin, cos của biến khớp  i Ngoài ra chúng ta thường chuyển các biến đơn giản như sau:

8

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

2.1.4 Jacobi và động học vận tốc:

Khi các khớp robot kích hoạt thì cơ cấu chấp hành sẽ di chuyển trong khônggian hoạt động Vector vị trí của cơ cấu chấp hành là một ma trận r [6 x 1] baogồm ba tọa độ (px, py, pz) và ba góc định hướng (α, β, γ) vì vậy r = [px, py, pz, α,

β, γ]T [1]

Vector vận tốc tổng quát V với hai vận tốc như sau vận tốc thẳng là v và vậntốc góc là ω:

 v  V    Với

Ma trận Jacobi của robot:

Ma trận Jacobi là biểu diễn về hình học của các phần tử của cơ cấu trong đơn

vị thời gian Nó cho phép chuyển từ di chuyển vi phân hoặc vận tốc của từngkhâu riêng hay vận tốc của điểm mà chúng ta quan tâm [1]

Giả sử chúng ta có phương trình Yi với các biến xi là:

10

Phương trình trên có thể viết dưới dạng ma trận và nó biểu diễn mối liên hệ

vi phân giữa các biến và các hàm riêng biệt với nhau Ma trận biểu diễn này

được gọi là ma trận Jacobi [1]

Bằng cách đạo hàm các phương trình vị trí của robot, chúng ta có thể viết

được phương trình sau, liên hệ di chuyển vi phân của khớp và di chuyển vi

phân của cánh tay: [1]



Với dx, dy, dz là di chuyển vi phân của cánh tay trong ba trục x, y, z và δx, x,

δx, y, δx, z là các góc quay quanh các trục x, y, z [Dθ] biểu diễn di chuyển vi phân

11

của các khớp Cả hai ma trận chia cho dt thì sẽ được vận tốc thay cho di chuyển

vi phân [1]

Với robot có n khớp thì vector vận tốc cơ cấu chấp hành cuối và vận tốc

khớp thì được biểu thị dưới dạng: [1]

là vector khớp tỷ lệ Cho khớp I, khi khớp I quay

hệ Descartes với các khớp trong không gian Công thức (20) thường được táchlàm hai công thức tính toán vận tốc dài và vận tốc góc: [1]

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

Giải động học thuận ta có được ma trận vị trí

Vận tốc góc của cơ cấu chấp hành cuối:

Vận tốc góc của cơ cấu cuối của robot co n khớp:

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

Trong lĩnh vực điều khiển, bộ điều khiển PID được xem như một giải pháp

đa năng cho các ứng dụng điều khiển tương tự hay điều khiển số Hơn 90% các

bộ điều khiển trong công nghiệp được sử dụng là bộ điều khiển PID Nếu đượcthiết kế tốt, bộ điều khiển PID có khả năng điều khiển hệ thống đáp ứng tốt cácchỉ tiêu chất lượng như đáp ứng nhanh, thời gian quá độ ngắn, độ vọt lỗ thấp,triệt tiêu được sai lệch tĩnh [6]

Luật điều khiển PID đươc định nghĩa:

Trong đó u là tín hiệu điều khiển và e là sai lệch điều khiển e  y sp  y 

Tín hiệu điểu khiển là tổng của ba thành phần: tỉ lệ, tích phân và vi phân [6]

Hàm truyền của bộ điều khiển PID: [6]

14

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

Các tham số của bộ điều khiển KP, KI,

Thành phần P có ưu điểm là tác động nhanh và đơn giản Hệ số tỉ lệ Kp cànglớn thì tốc độ đáp ứng càng nhanh, do đó thành phần P có vai trò lớn trong gianđoạn đầu của của quá trình quá độ [6]

Tuy nhiên, khi hệ số tỉ lệ Kp càng lớn thì sự thay đổi của tín hiệu điều khiểncàng mạnh dẫn đến dao động lớn, đồng thời làm hệ nhạy cảm hơn với nhiễu.Hơn nữa, đối với đối tượng không có đặc tính tích phân thì sử dụng bộ P vẫntồn tại sai lệch tĩnh [6]

Nhược điểm của thành phần tích phân là do phải mất một khoảng thời gian

để e(t) về 0 nên đặc tính tác động của bộ điều khiển sẽ chậm hơn Ngoài ra,thành phần tích phân đôi khi con làm xấu đi đặc tính tác động học của hệ thống,thậm chí có thể làm mất ổn định [6]

Người ta thương sử dụng bộ PI hoặc PID thay vì bộ I đờn thuẩn vửa để cảithiện tốc độ đáp ứng, vừa đảm bảo yêu cầu động học của hệ thống

15

2.2.3 Thành phần vi phân (D)

dt

(2.4)

Mục đích của thành phần vi phân là cải thiện sự ổn định của hệ kín Do động

học của quá trình, nên sẽ tồn tại một khoảng thời gian trể làm bộ điều khiển

chậm so với sự thay đổi của sai lệch e(t) và đầu ta y(t) của quá trình Thành

phần vi phân đóng via trò dự đoán đầu ra của quá trình và đưa ra phản ứng thích

hợp dựa trên chiều hướng và tốc độ thay dổi của sai lệch e(t), làm tăng tốc độ

đáp ứng của hệ [6]

Một ưu điểm nữa là thành phần vi phân giúp ổn định một số quá trình mà

bình thương không ổn định được với các bộ P hay PI [6]

Nhược điểm của thành phần vi phân là rất nhạy cảm với nhiễu đó hay các giá

trị đặt do tính đáp ứng nhanh nêu ở trên [6]

2.2.4 Bộ điều khiển PID số

Trong thực tế công nghiệp, các bộ điềuh kiển PID có thể được cấu thành từ

các thành phần mạch tương tự hoặc các cơ cấu chấp hành Tuy nhiên với yêu

cầu cao về độ chính xác với sự phát triển của các ứng dụng nhúng hay trên nền

vi xử lý, thì điều khiển số cũng là một lĩnh vực quan trọng Các bộ điều khiển

được số hóa để có thể thực thi với tốc độ cao và chính xác hơn Đồng thời việc xây

dựng các bộ điều khiển trên nền máy tính hay vi điều khiển cũng đơn gian hơn nhiều

[6]

Dưới đây là trình bày về việc xấp xỉ bộ PID trên miền thời gian sang dạng

PID số Việc chọn lựa tham số cho bộ PID số cũng tương như như trên miền

thời gian Ngoài ra ta cần quan tâm đến một tham số quan trọng là chu kì lấy

mẫu của vi điều khiển [6]

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

Ta sử dụng các công thức xấp xỉ tích phân lùi (backward integral

approximation) và sử dụng vi phân lùi (backward diffenrence apporixumation)

với chu kì lấy mẫu T [6]

2.3 Giới thiệu về kit STM32F4 Discovery

2.3.1 Giới thiệu chung

STM32F4 Discovery là board vi điều khiển dành cho người mới học hoặc

dành cho lập trình viên chuyên nghiệp trong lĩnh vực điều khiển tự động hóa

STM32F4 giúp khám phá STM32F407/417 dể phát triển các ứng dụng một

cách dễ dàng Nó bao gồm tất cả mọi thứ để người mới học bắt đầu một cách

nhanh chóng như : một công cụ debug ST-LINK/V2, hai ST MEMS gia tốc kỹ

thuật số và micro kỹ thuật số, một bộ DAC âm thanh co tích hợp trình điều

khiển loa lớp D, đèn LED và các nút bấm và một cổng kết nối USB OTG

Board được tích hợp chip ARM Cortex-M4 cùng với bộ tính toán số thực(FPU),

hoạt động với tần số rất cao 168 MHz, tỷ suất DMIPS/MHZ cao 1.25 giúp cho

hệ thống có thể đạt được hiệu năng 210 DMIPS, board rất thích hợp cho các

ứng dụng với yêu cầu tính toán xử lý nhanh, ví dụ như DSP, điều khiển robot…

Với STM32F4 Discovery, chúng ta sẽ không cần phải lo lắng và không cần

phải bỏ tiền ra để mua mạch nạp đắt tiền như các loại MCU thông thường

Điểm nổi bật nhất của board là có nhiều tính năng trong khi giá thành

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

rất rẻ Đó cũng là lý do mà nhóm thực hiện đề tài lựa chọn kit này để sử dụng.[5]

Hình2.2: Hình ảnh thực tế Kit STM32F4 Discovery

2.3.2 Tính năng

Kit STM32F4 Discovery có các tính năng như sau: [5]

- Vi điều khiển có bộ nhớ Flash 1 MB, bộ nhớ RAM là 192 KB trong gói LQFP100

- Có thể lựa chọn để sử dụng bộ ST-LINK/V2 (kết nối SWD để lập trình và

gỡ lỗi)

- Điện cung cấp cho board : có thể sử dụng thông qua cổng USB hoặc cấp điện áp 5V cho board

- Nguồn cung cấp cho ứng dụng bên ngoài: 3V và 5V

- Có LIS302DL, cảm biến chuyển động ST MEMS, 3 trục đo gia tốc đầu ra

kỹ thuật số

- MP45DT02, cảm biến âm thanh ST MEMS, micro kỹ thuật số đẳng hướng

18

- CS43L22, âm thanh với bộ chuyển đổi DAC có tích hợp trình điều khiển loa lớp D.

Tám đèn LED :

+ LD1 (màu đỏ/màu xanh lá cây) để giao tiếp USB

+ LD2 (màu đỏ) báo điện áp 3.3V

+ Bốn LED báo sử dụng : LD3 (màu cam, PD13), LD4 (màu xanh lá cây,

PD12), LD5 (màu đỏ, PD14) và LD 6 (màu xanh dương, PD15)

+ 2 USB OTG LED: LD7( màu xanh lá) VBus và LD8 (màu đỏ) quá dòng

- Hai nút nhấn (user và reset hệ thống)

- OTG USB với bộ kết nối micro-AB

2.3.3 Dãy công cụ phát triển:

- IAR EWARW (IAR Embedded Workbench)

- GCC dựa trên IDE (ARM @ Atollic@ True STUDIO@, )

2.3.4 Giới thiệu về dòng vi điều khiển STM32

- Sự tinh vi

Thoạt nhìn thì các ngoại vi của STM32 cũng giống như những vi điều khiểnkhác, như hai bộ chuyển đổi ADC, timer, I2C, SPI, CAN, USB và RTC Tuynhiên mỗi ngoại vi trên đều có rất nhiều đặc điểm thú vị Ví dụ như bộ ADC12-bit có tích hợp một cảm biến nhiệt độ để tự động hiệu chỉnh khi nhiệt độthay đổi và hỗ trợ nhiều chế độ chuyển đổi Mỗi bộ định thời có 4 khối capturecompare (dùng để bắt sự kiện với tính năng input capture và tạo dạng sóng ởngõ ra với output compare), mỗi khối định thời có thể liên kết với các khối địnhthời khác để tạo ra một mảng các định thời tinh vi hơn Một bộ định thời caocấp chuyên hỗ trợ điều khiển động cơ, với 6 đầu ra PWM với dead time(khoảng thời gian được chèn vào giữa hai đầu tín hiệu xuất PWM bù nhau trongđiều khiển mạch cầu H) lập trình được và một đường break input (khi phát hiệnđiều kiện dừng khẩn cấp) sẽ buộc tín hiệu PWM sang một trạng thái an toàn đã

19

Đ ÁN T T NGHI P Ồ ÁN TỐT NGHIỆP ỐT NGHIỆP ỆP

được cài sẵn Ngoại vi nối tiếp SPI có một khối kiểm tổng (CRC) bằng phầncứng cho 8 và 16 word hỗ trợ tích cực cho giao tiếp thẻ nhớ SD hoặc MMC [5]

STM32 có hỗ trợ thêm tối đa 12 kênh DMA (Direct Memory Access) Mỗikênh có thể được dùng để truyền dữ liệu đến các thanh ghi ngoại vi hoặc từ cácthanh ghi ngoại vi đi với kích thước từ (word) dữ liệu truyền đi có thể là 8/16hoặc 32-bit Mỗi ngoại vi có thể có một bộ điều khiển DMA (DMA controller)

đi kèm dùng để gửi hoặc đòi hỏi dữ liệu như yêu cầu Một bộ phân xử bus nội(bus arbiter) và ma trận bus (bus matrix) tối thiểu hoá sự tranh chấp bus giữatruy cập dữ liệu thông qua CPU (CPU data access) và các kênh DMA Điều đócho phép các đơn vị DMA hoạt động linh hoạt, dễ dùng và tự động điều khiểncác luồng dữ liệu bên trong vi điều khiển [5]

STM32 là một vi điều khiển tiêu thụ năng lượng thấp và đạt hiệu suất cao

Nó có thể hoạt động ở điện áp 2V, chạy ở tần số 72MHz và dòng tiêu thụ chỉ có36mA với tất cả các khối bên trong vi điều khiển đều được hoạt động Kết hợpvới các chế độ tiết kiệm năng lượng của Cortex, STM32 chỉ tiêu thụ 2μA khi ởA khi ởchế độ Standby Một bộ dao động nội RC 8MHz cho phép chip nhanh chóngthoát khỏi chế độ tiết kiệm năng lượng trong khi bộ dao động ngoài đang khởiđộng Khả năng nhanh đi vào và thoát khỏi các chế độ tiết kiệm năng lượng làmgiảm nhiều sự tiêu thụ năng lượng tổng thể [5]

- Sự an toàn

Ngày nay các ứng dụng hiện đại thường phải hoạt động trong môi trườngkhắt khe, đòi hỏi tính an toàn cao, cũng như đòi hỏi sức mạnh xử lý và càngnhiều thiết bị ngoại vi tinh vi Để đáp ứng các yêu cầu khắt khe đó, STM32cung cấp một số tính năng phần cứng hỗ trợ các ứng dụng một cách tốt nhất.Chúng bao gồm một bộ phát hiện điện áp thấp, một hệ thống bảo vệ xung Clock

và hai bộ Watchdogs Bộ đầu tiên là một Watchdog cửa sổ (windowedwatchdog) Watchdog này phải được làm tươi trong một khung thời gian xácđịnh Nếu nhấn nó quá sớm, hoặc quá muộn, thì Watchdog sẽ kích hoạt Bộ thứhai là một Watchdog độc lập (independent watchdog), có bộ dao động bênngoài tách biệt với xung nhịp hệ thống chính Hệ thống bảo vệ xung nhịp có thể

20