ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ – BỘ MÔN CƠ ĐIỆN TỬ oOo ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ Đề bài THIẾT KẾ, CHẾ TẠO ROBOT DÒ LINE PHÂN PHỐI HÀNG HOÁ THEO MÀ[.]
ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ – BỘ MƠN CƠ ĐIỆN TỬ -oOo - ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ Đề bài: THIẾT KẾ, CHẾ TẠO ROBOT DỊ LINE PHÂN PHỐI HÀNG HỐ THEO MÀU SẮC GVHD: PGS TS Nguyễn Duy Anh SVTH: Nguyễn Tấn Dũng Lâm Hiệp Hưng Ngô Trần Tuấn Đạt Hoàng Thanh Thế TP HCM, tháng 12 năm 2022 DANH SÁCH THÀNH VIÊN VÀ PHÂN CÔNG CÔNG VIỆC Họ tên MSSV Phân công công việc Nguyễn Tấn Dũng Thiết kế khí Lâm Hiệp Hưng Lập trình điều khiển, giải thuật Ngơ Trần Tuấn Đạt 1911009 Phân tích động học, điều khiển, mơ hình hố mơ Hoàng Thanh Thế 1910555 Thiết kế hệ thống điện, cảm biến, thiết kế giao diện i LỜI CẢM ƠN Chúng em xin gửi lời cám ơn đến PGS TS Nguyễn Duy Anh tận tình hướng dẫn, trợ giúp nhóm hoàn thành đồ án Thiết kế hệ thống Cơ Điện Tử Những kiến thức tiếp thu từ môn học hành trang vững cho thành viên nhóm để vững bước đường học tập phát triển nghiệp sau Xin chân thành cám ơn Thầy! Nhóm sinh viên thực TP HCM, tháng 12 năm 2022 ii MỤC LỤC DANH MỤC HÌNH ẢNH vii DANH MỤC BẢNG BIỂU xi TỔNG QUAN 1.1 Mục tiêu thiết kế 1.2 Tổng quan robot phân loại hàng hoá 1.2.1 Giới thiệu robot phân phối hàng hoá 1.2.2 Tình hình nghiên cứu ngồi nước 1.2.3 Các thiết kế khí robot tự hành 1.2.4 Các thiết kế phần điện 1.2.5 Các thiết kế điều khiển 13 1.3 Bài toán thiết kế thông số đầu vào 16 LỰA CHỌN PHƯƠNG ÁN 19 2.1 Lựa chọn phương án khí 19 2.1.1 Lựa chọn phương án nguyên lý xe .19 2.1.2 Lựa chọn phương án động dẫn động 20 2.1.3 Lựa chọn phương án động rẽ hướng 20 2.1.4 Lựa chọn phương án vật liệu bánh xe dẫn động 20 2.2 Lựa chọn phương án điện 21 2.2.1 Lựa chọn phương án cảm biến 21 2.2.2 Lựa chọn nguồn điện 22 2.3 Lựa chọn phương án điều khiển 23 2.3.1 Lựa chọn cấu trúc điều khiển 23 2.3.2 Lựa chọn thuật toán điều khiển 23 iii 2.4 Tổng hợp lựa chọn phương án thiết kế 23 TÍNH TỐN THIẾT KẾ CƠ KHÍ 25 3.1 Tính tốn kích thước xe 25 3.1.1 Khoảng cách bánh xe trục 25 3.1.2 Kích thước xe theo chiều dọc ngang 25 3.1.3 Kích thước cấu khâu lề 26 3.2 Tính tốn động dẫn động 27 3.3 Tính tốn động rẽ hướng .30 3.3.1 Số bậc tự cấu 30 3.3.2 Phân tích lực 30 3.3.3 Chọn động rẽ hướng: 35 3.4 Lựa chọn dung sai 35 3.5 Tổng quan thiết kế khí 35 THIẾT KẾ HỆ THỐNG ĐIỆN 38 4.1 Sơ đồ khối hệ thống điện 38 4.2 Hệ thống cảm biến dò line 39 4.2.1 Yêu cầu thiết kế 39 4.2.2 Lựa chọn cảm biến dò line 39 4.2.3 Tính tốn giá trị điện trở 40 4.2.4 Xác định độ cao đặt cảm biến 42 4.2.5 Xác định cách bố trí cảm biến 44 4.2.6 Xác định số lượng khoảng cách cảm biến 45 4.2.7 Thiết kế mạch dò line 47 4.2.8 Calib cảm biến 47 iv 4.3 Lựa chọn mạch Driver cho động 49 4.3.1 Tiêu chí lựa chọn mạch Driver 49 4.3.2 Lựa chọn mạch Driver 49 4.4 Lựa chọn cảm biến màu sắc .50 4.4.1 Tiêu chí lựa chọn cảm biến màu sắc 50 4.4.2 Lựa chọn cảm biến màu sắc 51 4.5 Lựa chọn module thu phát Bluetooth 51 4.5.1 Tiêu chí lựa chọn module thu phát Bluetooth 51 4.5.2 Lựa chọn mạch thu phát Bluetooth 52 4.6 Lựa chọn mạch giảm áp 52 4.6.1 Tiêu chí lựa chọn mạch giảm áp .52 4.6.2 Lựa chọn mạch giảm áp 53 4.7 Lựa chọn Pin 54 4.7.1 Tiêu chí lựa chọn Pin 54 4.7.2 Tính toán dung lượng cần thiết 54 4.7.3 Lựa chọn Pin 54 4.8 Sơ đồ nguyên lý mạch tổng 55 MƠ HÌNH HỐ 58 5.1 Xây dựng phương trình động học 58 5.2 Mơ hình tốn xác định sai số 59 5.3 Mơ hình hố động 60 5.3.1 Xác định thông số lấy mẫu động 60 5.3.2 Kiểm tra quan hệ tuyến tính động – driver với tín hiệu đầu vào 61 v 5.3.3 Xây dựng hàm truyền động 61 5.4 Thiết kế điều khiển 63 5.4.1 Tiêu chí thiết kế điều khiển 63 5.4.2 Xây dựng điều khiển mơ hình tốn 65 5.5 Mô chuyển động xe .69 5.6 Nhận xét kết mô 71 THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 73 6.1 Lưu đồ giải thuật điều khiển 73 6.2 Thiết kế giao diện điều khiển 77 THỰC NGHIỆM 79 vi DANH MỤC HÌNH ẢNH Hình 1.1 Các phương pháp sử dụng để dẫn hướng cho AGV Hình 1.2 Sơ đồ nguyên lý sử dụng robot nghiên cứu Hình 1.3 Robot Q3-600C Hình 1.4 Sơ đồ nguyên lý Q3-600C .4 Hình 1.5 Robot vận chuyển cho dây chuyền lắp ráp SMT Hình 1.6 Sơ đồ nguyên lý robot vận chuyển cho dây chuyền lắp ráp SMT Hình 1.7 Kết cấu xe bánh gồm bánh dẫn động bánh bị động Hình 1.8 Sơ đồ nguyên lý xe bánh rẽ hướng chủ động .8 Hình 1.9 Cơ cấu điều khiển rẽ hướng bốn khâu lề cho sơ đồ (a) Hình 1.10 Điều khiển rẽ hướng sơ đồ (b) Hình 1.11 Các loại cảm biến nhận diện đường dẫn sử dụng Hình 1.12 Các phương pháp bố trí cảm biến 12 Hình 1.13 Phương pháp so sánh 12 Hình 1.14 Thuật toán phát đường line theo phương pháp 13 Hình 1.15 Sơ đồ giải thuật PID 15 Hình 1.16 Bộ điều khiển fuzzy .15 Hình 1.17 Sơ đồ sa bàn cho robot phân phối hàng hố theo màu sắc 18 Hình 2.1 Sơ đồ nguyên lý xe bánh 19 Hình 2.2 Sơ đồ nguyên lý xe bánh 19 Hình 2.3 Sơ đồ nguyên lý lựa chọn .24 Hình 3.1 Sơ đồ nguyên lý dẫn hướng sử dụng cấu Ackermann 25 Hình 3.2 Mơ hình xe tiến hành vào cua với cấu ackermann lý tưởng 26 Hình 3.3 Sơ đồ nguyên lý cấu khâu lề sử dụng 27 vii Hình 3.4 Phân tích lực bánh xe 28 Hình 3.5 Thành phần lực ma sát cản trở lên bánh dẫn hướng 31 Hình 3.6 Cơ cấu tác động trực tiếp lên bánh xe rẽ hướng 31 Hình 3.7 Cơ cấu điều khiển rẽ hướng 32 Hình 3.8 Khâu dẫn .32 Hình 3.9 Nhóm tĩnh định 32 Hình 3.10 Hoạ đồ véc tơ nhóm tĩnh định 33 Hình 3.11 Khâu dẫn (khâu điều khiển) 33 Hình 3.12 Nhóm tĩnh định cấu điều khiển 34 Hình 3.13 Hoạ đồ véc tơ nhóm tĩnh định 34 Hình 3.14 Mơ hình 3D robot phân phối hàng hố theo màu sắc 36 Hình 3.15 Mơ hình cấu rẽ hướng 37 Hình 4.1 Sơ đồ khối hệ thống điện .38 Hình 4.2 Kích thước cảm biến TCRT5000 39 Hình 4.3 Nguyên lý hoạt động TCRT5000 40 Hình 4.4 Đường đặc tuyến �� với �� 41 Hình 4.5 Đường đặc tuyến ��� với �� 41 Hình 4.6 Giá trị analog theo chiều cao gá đặt cảm biến 43 Hình 4.7 Giá trị analog so với vị trí tâm line độ cao h=12 mm 44 Hình 4.8 Các cách bố trí cảm biến 44 Hình 4.9 Vùng hoạt động cảm biến 45 Hình 4.10 Sơ đồ vùng quét cảm biến đường line 45 Hình 4.11 Khoảng cách ngắn đầu thu cảm biến 46 Hình 4.12 Bố trí dãy cảm biến theo chiều ngang line 47 viii Hình 4.13 Sơ đồ nguyên lý mạch cảm biến dò line 47 Hình 4.14 Sai lệch vị trí thực tế với vị trí xấp xỉ phương pháp trung bình trọng số 49 Hình 4.15 Driver TB6612 .50 Hình 4.16 Cảm biến màu sắc TCS3200 51 Hình 4.17 Module Bluetooth HC-05 52 Hình 4.18 Mạch giảm áp DC-DC BUCK LM2596 3A 53 Hình 4.19 Pin Lipo 2200mAh 55 Hình 4.20 Khối điều khiển 55 Hình 4.21 Khối nguồn 56 Hình 4.22 Các khối cảm biến bluetooth 56 Hình 4.23 Khối Driver Động 57 Hình 5.1 Mơ hình tốn xe dị line 58 Hình 5.2 Đồ thị mối quan hệ %PWM số vòng quay động - driver JGB37 TB6612 61 Hình 5.3 Import liệu lấy vào System Identification 62 Hình 5.4 Kết tính tốn từ System Identification 63 Hình 5.5 Sơ đồ khối hệ thống .65 Hình 5.6 Sơ đồ khối điều khiển động – driver 65 Hình 5.7 Sơ đồ khối điều khiển PID động – driver 65 Hình 5.8 Sử dụng PID Tuner Matlab Simulink 66 Hình 5.9 Thơng số đáp ứng hệ thống 67 Hình 5.10 Kết mơ hình hố robot bám theo line trường hợp line 69 Hình 5.11 Kết sai số e2 trường hợp rẽ line 69 Hình 5.12 Kết vận tốc góc trường hợp rẽ line 70 ix Hình 5.5 Sơ đồ khối hệ thống 5.4.2 Xây dựng điều khiển mô hình tốn Mơ hình tốn điều khiển PID cho khối động – driver JGB37 – TB6612 Ta có hàm truyền động – driver JGB37 – TB6612: � � = Sơ đồ khối điều khiển PID: 3,841 � + 0,16536 Hình 5.6 Sơ đồ khối điều khiển động – driver Tiến hành mô sơ đồ khối hàm truyền điều khiển PID cho động – driver Matlab Simulink: Hình 5.7 Sơ đồ khối điều khiển PID động – driver 65 Với tiêu chí thiết kế điều khiển PI chọn từ trước ta sử dụng công cụ PID Tuner để tìm hệ số thoả mãn: �� = 4,3916 , �� = 7,2438 Hình 5.8 Sử dụng PID Tuner Matlab Simulink 66 Hình 5.9 Thông số đáp ứng hệ thống Hệ số điều khiển PI cho động thay đổi đơi chút q trình lập trình nhúng cho vi điều khiển Mơ hình tốn điều khiển PID cho tốn bám line robot Ta có phương trình sai số sau: �1 = �� −� + �2 � �2 = �� �3 − (�1 + �)� �3 = �� − � Coi �� nhiễu với khơng xét giá trị �1 đề không yêu cầu đến ta rút gọn hệ dạng: �2 �� �2 −� = + � �3 0 �3 −1 Hàm truyền hệ đưa dạng: � � = � �� − � Với ma trận đặc trưng hệ là: −1 � 67 �= Ta có hàm truyền hệ: 0 �� −� ,�= ,� = 0 −1 � � =− �� � − �2 � Chọn điều khiển PD để khử chattering �� � = �� + �� � Theo tiêu chí chọn ta có sai số xác lập theo chuẩn 10% thời gian xác lập cần 1s, từ đó: � =− ln % �� 100 �2 + ln2 % �������� ���� = Hàm truyền hệ thống có dạng: � � = �� 100 ≈ 0,5 = ↔ �� = �� � � � �� (�) + � � �� (�) Phương trình đặc trưng hệ thống: Tương đương: + � � �� � = �� − �� d�2 − �� �� + �� � s + �� �� = Đồng hệ số với phương trình: (� + �)(�2 + 2�� �� + �2� ) = Với �� = 300 ��/� � = 200 �� Ta có: 68 ⇔ �� ≈− 0,033 �� ≈− 0,2 5.5 Mô chuyển động xe Sau tinh chỉnh, hệ số PID cuối xác định: �� =− 0,28 �� =− 0,47 Kết mơ Robot dị line theo sa bàn với thời gian lấy mẫu 0,02s với hệ số �� =− 0,28 �� =− 0,47 Xe chạy với tốc độ mong muốn �� = 300 ��/� Trường hợp line Hình 5.10 Kết mơ hình hố robot bám theo line trường hợp line Hình 5.11 Kết sai số e2 trường hợp rẽ line 69 Hình 5.12 Kết vận tốc góc trường hợp rẽ line Trường hợp line Hình 5.13 Kết mơ hình hố robot bám theo line trường hợp line 70 Hình 5.14 Kết sai số e2 trường hợp rẽ line Hình 5.15 Kết vận tốc góc trường hợp rẽ line 5.6 Nhận xét kết mô Kết mô khối JGB37 – TB6612 71 - - Thời gian xác lập sai số: �������� ���� ≈ 1� , đáp ứng đến sai số 10% Độ vọt lố: 6.06% < 15% Sai số xác lập: 1,4 ���/min < 10% �ủ� 400 ���/min = 40 → Kết mơ thỏa mãn tiêu chí thiết kế điều khiển Kết mô chuyển động Robot: - Sai số bám line: có giá trị tuyệt đối ln nhỏ 6�� < 16�� - được, có tượng bẻ bánh qua lại �2 đổi dấu song chấp nhận Đáp ứng RC servo: nằm vùng mà phần cứng xe đáp ứng Mô xe bám line tốt → Kết mơ thỏa mãn tiêu chí thiết kế điều khiển 72 THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 6.1 Lưu đồ giải thuật điều khiển Robot sử dụng điều khiển tập trung để điều khiển chức riêng biệt Trong vi điều khiển STM32F446RET6 đảm nhiệm tất nhiệm vụ gồm có: - Đọc xử lý giá trị sensor - Đọc giá trị vận tốc từ Encoder - Điều khiển động thông qua driver điều khiển PID - Cấp xung PWM cho động RC DC Sau tín hiệu trả từ sensor dạng analog vi điều khiển tính tốn sai số �2 (bỏ qua sai số �3 mơ hình bỏ qua �3 ) Vận tốc động vi điều khiển xử lý truyền tín hiệu điều khiển dạng xung PWM cho driver tương ứng Vi điều khiển nhận xung encoder trả áp dụng giải thuật PID để đảm bảo vận tốc xe đáp ứng giá trị tính tốn Lưu đồ giải thuật 73 Hình 6.1 Lưu đồ giải thuật chương trình 74 Hình 6.2 Chương trình thực bám line 75 Hình 6.3 Chương trình đọc cảm biến Giải thích lưu đồ giải thuật Khi bật công tắc vi điều khiển đọc giá trị dãy cảm biến, vi điều khiển xử lý tín hiệu nhận để tính tốn khoảng cách so với đường tâm x(mm), nhận giá trị x(mm) vi điều khiển tính tốn vận tốc v cho động DC vận tốc � cho động RC, robot bắt đầu bám line Khi tới chỗ nhận hàng, vi điều khiển nhận giá trị analog cảm biến trả line đen robot truyền tín hiệu dừng động để nhận hàng Cảm biến 76 màu sắc nhận diện màu khối hàng truyền tín hiệu đến vi điều khiển nhận hàng Sau nhận hàng, vi điều khiển trả giá trị � v tương ứng để tiếp tục bám line Khi tới ngã ba, nhận diện màu sắc từ trước vi điều khiển cấp xung PWM cho RC servo để quẹo theo giá trị � mong muốn Nếu khối hàng màu đỏ quẹo trái ngược lại màu xanh quẹo phải Sau đó, robot trả giá trị � v tương ứng để tiếp tục thực bám line chạy hết sa bàn, cảm biến nhận line trắng dừng lại 6.2 Thiết kế giao diện điều khiển Sử dụng ngôn ngữ C# Winform để thực thiết kế giao diện giao tiếp với robot qua mãy tính Các thành phần giao diện bao gồm: - Nút start: bắt đầu cho robot hoạt động - Nút stop: dừng hoạt động robot - Nút reset: bắt đầu lại hệ thống - Khung hiển thị màu: hiển thị màu đọc từ khối hàng - Khung hiển thị vận tốc: hiển thị vận tốc đọc robot - Các nút kết nối ngắt kết nối với bluetooth Nguyên lý hoạt động - Sau cấp nguồn cho robot, đặt robot lên sa bàn nhấn nút start robot bắt đầu thực trình bám line - Khi tới chỗ nhận hàng robot dò line dừng lại, ta đặt khối hàng lên robot robot thực đọc màu sắc từ cảm biến - Sau nhận tín hiệu màu sắc, vi điều khiển thực giao tiếp với khối chức bluetooth gửi tín hiệu máy tính, lúc chương trình hiển thị màu sắc nhận lên hình - Sau có tín hiệu màu trả về, robot tiếp tục thực trình bám line hết sa bàn 77 - Trong trình bám line, vận tốc robot vi điều khiển truyền hiển thị giao diện - Tốc độ động điều khiển tăng giảm nhờ vào nút nhấn Tăng tốc Giảm tốc - Khi bấm nút reset chương trình bắt đầu lại từ đầu - Khi bấm nút stop, robot dừng lại nhận lệnh Hình 6.4 Giao diện bluetooth máy tính 78 THỰC NGHIỆM 79