Báo cáo đồ án thiết kế hệ thống cơ điện tử

ĐẠ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Ử BÁO CÁO ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD PGS TS Nguyễn Tấn Tiến SVTH Lê Hải Đăng 21300834 Phan Đình.

ĐẠ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Ử BÁO CÁO ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: PGS.TS Nguyễn Tấn Tiến SVTH: Lê Hải Đăng 21300834 Phan Đình Khánh 21301797 Trần Đại Nhân 21302733 Hạ Xn Kỳ 21301977 Hồng Vũ Bình 21300299 TP.Hồ Chí Minh, tháng 1/2017 MỤC LỤC MỤC LỤC i DANH MỤC HÌNH ẢNH iii DANH MỤC BẢNG BIỂU v CHƢƠNG TỔNG QUAN 1.1 Giới thiệu 1.2 Yêu cầu đặt đồ án 1.2.1 Mục tiêu đồ án 1.2.2 Yêu cầu kỹ thuật 1.3 Cấu trúc xe dị line thơng dụng 1.3.1 Phần khí 1.3.2 Phần cảm biến 13 1.3.3 Phần điều khiển 15 1.4 Đặt toán 16 CHƢƠNG LỰA CHỌN PHƢƠNG ÁN 17 2.1 Phƣơng án khí 17 2.2 Phƣơng án cảm biến 17 2.3 Phƣơng án điều khiển 18 2.4 Kế hoạc thực 18 CHƢƠNG THIẾT KẾ CƠ KHÍ 19 3.1 Chọn bánh xe 19 3.2 Tính tốn chọn động 19 3.3 Tính toán ổn định cho xe di chuyển bán kính cong 21 3.4 Thiết kế phận khí 22 3.4.1 Chuỗi kích thƣớc 22 3.4.2 Chuỗi kích thƣớc 24 CHƢƠNG THIẾT KẾ ĐIỆN 26 4.1 Xây dựng sơ đồ khối nguyên lý hệ thống điện 26 4.2 Thiết kế cảm biến 26 4.2.1 Lựa chọn cảm biến 26 4.2.2 Xác định chiều cao đặt cảm biến khoảng cách hai cảm biến 27 4.2.3 Tuyến tính hóa cảm biến 31 4.3 Lựa chọn driver 35 i 4.4 Lựa chọn vi điều khiển 36 CHƢƠNG MƠ HÌNH HĨA 38 5.1 Mơ hình hóa cấu lái 38 5.2 Xây dựng mơ hình động học hệ thống 40 5.3 Mơ hình hóa hệ thống kết hợp sai số 41 CHƢƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN 43 6.1 Thành lập luật điều khiển 43 6.2 Mô điều khiển 43 6.3 Thành lập điều khiển PID cho động dẫn động 46 6.4 Giải thuật điều khiển 48 TÀI LIỆU THAM KHẢO 50 ii DANH MỤC HÌNH ẢNH Hình 1.1.Sa bàn di chuyển robot Hình 1.2 Chariot Hình 1.3 Usain Volt 2.0 Hình 1.4 Fireball (2010) Hình 1.5 CartisX04 (2014) 10 Hình 1.6 FH Westküste 11 Hình 1.7 Xe đua mơ hình HSP 94107TOP-10707 12 Hình 1.8 Xe đua mơ hình HSP 94123-12309 13 Hình 1.9 Các phƣơng pháp xếp cảm biến 14 Hình 1.10 Tín hiệu đƣợc xử lý giải thuật so sánh 15 Hình 1.11 Giải thuật xấp xỉ giá trị 15 Hình 1.12 Sơ đồ line vị trí giao 16 Hình 3.1 Mơ hình phân tích lực 19 Hình 3.2 Mơ hình tính tốn xe rẽ hƣớng 21 Hình 3.3 Mơ hình bánh truyền động 23 Hình 3.4 Chuỗi kích thƣớc 23 Hình 3.5 Mơ hình hóa cấu treo bánh trƣớc 25 Hình 4.1 Sơ đồ khối nguyên lý hệ thống điện 26 Hình 4.2 Sơ đồ nguyên lý 27 Hình 4.3 Mơ hình cảm biến TCRT5000 28 Hình 4.4 Đồ thị biểu diễn thay đổi giá trị ADC chiều cao thay đổi − 20𝑚𝑚 29 Hình 4.5 Đồ thị biếu diễn thay đổi giá trị ADC 30 Hình 4.6 Đồ thị biếu diễn thay đổi giá trị ADC 30 Hình 4.7 Khoảng cách cảm biến đặt nằm dọc theo đƣờng line 31 Hình 4.8 Phƣơng pháp tính giá trị trung bình xấp xỉ theo trọng số 32 Hình 4.9 Đồ thị quan hệ vị trí thực vị trí tính tốn 33 Hình 4.10 Đồ thị mối quan hệ tâm đƣờng line sai số tuyệt đối 33 Hình 4.11 Đồ thị biểu điễn sai số hàm nội suy giá trị đọc 34 Hình 4.12 Đồ thị biểu diễn hiệu sai số giá trị đọc giá trị thực tâm cảm biến 34 Hình 4.13 Đồ thị test độ tuyến tính vận tốc driver TB6612 36 Hình 5.1 Mơ hình cấu lái 38 iii Hình 5.2 Mơ hình hóa hệ thống 40 Hình 5.3 Mơ hình động học kèm sai số 41 Hình 5.4 Đồ thị quan hệ góc lái góc quay động RC servo 42 Hình 6.1 Kết bám line 44 Hình 6.2 Sai số theo trục 𝑥 44 Hình 6.3 Sai số theo trục 𝑦 45 Hình 6.4 Sai số góc quay 45 Hình 6.5 Vận tốc hoạt động 46 Hình 6.6 Góc lái servo 46 Hình 6.7 Đồ thị đáp ứng vận tốc theo thời gian 47 Hình 6.8 Đồ thị đáp ứng vận tốc theo thời gian áp dụng điều khiển PID thực tế 48 Hình 6.9 Giải thuật điều khiển xe dò line sa bàn 49 iv DANH MỤC BẢNG BIỂU Bảng 1.1.Thông số kỹ thuật xe Chariot Bảng 1.2 Thông số kỹ thuật Usain Volt 2.0 Bảng 1.3 Thông số kỹ thuật Fireball 10 Bảng 1.4.Thông số kỹ thuật CartisX04 10 Bảng 1.5 Thông số kỹ thuật FH Westküste 11 Bảng 1.6 Thông số kỹ thuật xe đua HSP 94107TOP-10707 12 Bảng 1.7 Thông số kỹ thuật xe đua HSP 94107TOP-10707 13 Bảng 3.1 Thơng số đầu vào tính tốn chọn động 21 Bảng 3.2 Thơng số đầu tính tốn chọn động 21 Bảng 3.3 Thông số động DC Mitsumi 21 Bảng 4.1 Thông số kỹ thuật cảm biến TCRT5000 26 Bảng 4.2 Thông bảng giá trị thay đổi 𝑑1, 𝑑2, 𝑋𝑑 theo chiều cao cảm biến 28 Bảng 6.1 Thông số mô 43 v Chƣơng CHƢƠNG TỔNG QUAN 1.1 Giới thiệu Moblie platform loại robot di chuyển từ nơi đến nơi khác cách tự chủ Không giống nhƣ robot cơng nghiệp, mobile platform di chuyển tự không gian làm việc để đạt đƣợc mục đích mong muốn Từ giúp cho mobile platform có nhiều ứng mơi trƣờng khác Cùng với đa dạng hình thức di chuyển nhƣ chạy, nhảy, bơi, lặn, mobile platform đa dạng phƣơng thức dẫn động nhƣ: bánh xe (WMRs), chân (LMRs), dẫn động chân vịt (AUVs) dẫn động cánh quạt (UAVs) Trong thực tế, WMRs đƣợc sử dụng phổ biến, thích hợp với ứng dụng thông thƣờng yêu cầu phát triển xã hội Một ứng dụng cụ thể WMRs xe dò line, đƣợc sử dụng phổ biến công nghiệp đời sống nhƣ: AGV (Automated Guided Vehicle) số nhà cung cấp: Daifuku, Swisslog, AGVE Group, Egemin Automation, để vận chuyển, xếp hàng hóa hệ thống cảng biển, kho hàng tự động (AS/RS), ô-tô tự hành vận chuyển hành khách tự động [1] Ngồi ra,xe dị line cịn đề tài nghiên cứu kỹ thuật nhận diện, điều khiển nhƣ làm đề tài cho thi kỹ thuật giới (Robot Challenge,Renesas MCU Car Rally, Cosmobot 2012, All Japan Micro Mouse, ).Ở nƣớc, robot dò line trở thành đề tài cho sân chơi kỹ thuật trƣờng đại học dƣới hình thức đua tính thời gian (cuộc thi Bkit Car Rally trƣờng Đại học Bách Khoa TPHCM, thi ITCar trƣờng Đại học Công nghệ thông tin- ĐHQG TPHCM, thi "Xe đua lập trình MCR Sƣ phạm Kỹ thuật TPHCM", ) Trong phạm vi đề tài, nhóm hƣớng đến thiết kế xe đua bám đƣờng hoàn thành quỹ đạo đƣờng định sẵn đảm bảo yêu cầu đặt đồ án môn học 1.2 Yêu cầu đặt đồ án 1.2.1 Mục tiêu đồ án Thiết kế chế tạo xe dò line di chuyển với tốc độ cao sa bàn có đặc điểm nhƣ hình sau Hình 1.1.Sa bàn di chuyển robot Chƣơng Khi bắt đầu, robot đƣợc đặt vị trí START (điểm A), sau robot chạy theo thứ tự qua điểm nút quy định lần lƣợt nhƣ sau: (START) A  B  C  D  E F  C  G  A  C  E (END) Màu sắc đƣờng line: đen Màu nền: Trắng Bề rộng đƣờng line: 26𝑚𝑚 1.2.2 Yêu cầu kỹ thuật Tốc độ di chuyển robottối thiểu: 0,2 𝑚/𝑠 Số lƣợng bánh xe robot (bao gồm bánh xe dẫn động bánh xe bị động) đƣợc chọn tùy thuộc vào thiết kế nhóm Trên robot đƣợc trang bị hệ thống cảm biến để giúp robot nhận biết đƣờng line bề mặt sàn/mặt đất di chuyển bám theo đƣờng line Nhóm sinh viên tự chọn loại cảm biến phù hợp.Điều kiện ràng buộc: Đƣờng kính bánh xe: 𝑑 ≤ 200𝑚𝑚 Số lƣợng bánh xe (chủ động + bị động): tùy chọn Kích thƣớc tối đa chiều robot (dài × rộng × cao): 300𝑚𝑚×220𝑚𝑚×300𝑚𝑚 1.3 Cấu trúc xe dị line thơng dụng Xe dị line dạng mobile platform tự hành, di chuyển quỹ đạo cho trƣớc, thông thƣờng line (tùy vào điều kiện làm việc mà line từ line màu) Về bản, xe dò line gồm ba phần là: khí, cảm biến điều khiển 1.3.1 Phần khí Đối với xe dị line, phần khí đóng vai trị khung, nâng đỡ thiết bị chịu tải Không vậy, phần khí cịn ảnh hƣởng đến việc điều khiển định đặc tính kỹ thuật sau thiết kế nhƣ: tốc độ tối đa, bán kính cong, khối lƣợng chịu tải Có nhiều sơ đồ nguyên lý áp dụng cho xe đua dị line, chủ yếu sơ đồ gồm bánh, số bánh chủ động thƣờng hai bánh Ngồi ra, cấu vi sai đƣợc áp dụng vào sơ đồ, nhiên làm cho hệ thống trở nên phức tạp Sau số cấu xe thƣờng đƣợc sử dụng: 1.3.1.1 Loại ba bánh với hai bánh chủ động bánh bị động Đại diện cho loại xe dò line Chariot[2] Chariot tham gia thi LVBots line following competition câu lạc robot LVBots Las Vegas, Nevada tổ chức Chƣơng a b Hình 1.2 Chariot a Hình dáng bên ngồi b Sơ đồ ngun lý Bảng 1.1.Thông số kỹ thuật xe Chariot Thông số Dài × Rộng × Cao Vận tốc trung bình, 𝑣𝑡𝑏 Khối lƣợng, 𝑚 Giá trị 145 × 90 × 70 1,17 210 Đơn vị 𝑚𝑚 𝑚/𝑠 𝑔 Chariot sử dụng cấu lái vi sai (differential drive), xe điều khiển góc quay dựa việc thay đổi vận tốc hai bánh chủ động Ƣu điểm: kết cấu truyền động, mơ hình động học đơn giản, đảm bảo bánh xe đồng phẳng cho phép xe di chuyển bán kính cong nhỏ, chí quay chỗ[3] Nhƣợc điểm: xe bị trƣợt theo phƣơng pháp tuyến vào đoạn line có bán kính nhỏ với tốc độ cao Để đảm bảo xác xe di chuyển đoạn đƣờng thẳng, đòi hỏi điều khiển đồng hai động độc lập cho hai bánh xe quay với vận tốc 1.3.1.2 Loại bốn bánh với hai bánh độc lập chủ động hai bánh bị động Usain Volt 2.0[4] tham gia thi LVBots line following competition câu lạc robot LVBots Las Vegas, Nevada tổ chức Chƣơng b a Hình 1.3 Usain Volt 2.0 a Hình dáng bên ngồi b Sơ đồ nguyên lý Bảng 1.2 Thông số kỹ thuật Usain Volt 2.0 Thơng số Dài × Rộng × Cao Vận tốc trung bình, 𝑣𝑡𝑏 Khối lƣợng, 𝑚 Đơn vị 𝑚𝑚 𝑚/𝑠 𝑔 Giá trị 182 × 132 × 60 1,07 295 Ƣu điểm: kết cấu truyền động, mơ hình động học đơn giản, di chuyển bán kính cong nhỏ độ cứng vững cao mơ hình ba bánh, khắc phục đƣợc cân xảy vào đoạn bán kính cong với tốc độ cao, đặc biệt có chở thêm tải Nhƣợc điểm: cần đảm bảo tiếp xúc củabốn bánh xe với mặt đƣờng khó khăn điều khiển hai động đồng (tƣơng tự mô hình 1.3.1.1) 1.3.1.3 Loại bốn bánh chủ động độc lập Fireball[5] tham gia thi Bot Brawl (Peoria, Illinois), sau Chibots Summer 2010 Contest câu lạc robot Chicago tổ chức a b Hình 1.4 Fireball (2010) a Hình dáng bên ngồi b Sơ đồ nguyên lý Chƣơng Hình 4.13 Đồ thị test độ tuyến tính vận tốc driver TB6612 Nhận xét: Quan hệ RPM PWM gần nhƣ tuyến tính lần đo 4.4 Lựa chọn vi điều khiển 36 Chƣơng Dựa yêu cầu phƣơng án thiết kế chọn cho robot, vi điều khiển cần sử dụng vi xử lí trung tâm để thực trình bám line, đọc cảm biến, tính tốn giá trị, hiệu chỉnh truyền tín hiệu xuống cấu chấp hành Hiện có nhiều loại vi điều khiển, nhiên ta cần lựa chọn loại vi điều khiển có đầy đủ yêu cầu đáp ứng hệ thông điện thiết kế nhƣ: cần có ADC, PWM, QEI, giao tiếp UART cách điều khiển nhóm điều khiển tập trung nên yêu cầu có vi điều khiển đủ mạnh để đáp ứng đƣợc yêu cầu kể nhóm định sử dụng vi điều khiển 32 bits TM4C123GH6PM Thời gian dọc encoder: 20 ms Thời gian xử lý pid: 10 ms Thời gian đọc ADC: 10ms 37 Chƣơng CHƢƠNG MƠ HÌNH HĨA Mơ hình hóa việc xây dựng phƣơng trình tốn học đặc tả hệ thống dựa đặc tính vật lý sẳn có hệ thống Các phƣơng trình vừa tìm đƣợc sở để thiết kế điều khiển sau 5.1 Mơ hình hóa cấu lái 𝜑𝑟 𝑇 𝐺 𝑥 𝑥′ 𝑑5 𝐸 𝑑6 𝑑4 𝐷 𝐹 𝐷 𝑦′ 𝑑2 𝐶 𝑑1 𝑑3 𝐴 𝐴 𝐸 𝐷 𝐵 𝑑5 𝑑6 𝜑3 𝜑1 𝑑 𝑑1 𝐵 𝑦 𝐶 𝑑2 𝜑𝑟 − 𝜋 𝐺 𝜑2 𝐹 𝑑4 a b Hình 5.1 Mơ hình cấu lái a Cơ cấu lái Ackerman hoàn chỉnh b Cơ cấu bốn khâu lề Đối với hệ thống lái Ackerman, thời điểm ln đảm có tâm vận tốc tức thời đặc tính khơng thay đổi Vì vậy, việc mơ hình hóa xét đến quan hệ góc quay servo góc lái phải −𝜑𝑟 Để đơn giản việc trình bày q trình mơ hình hóa ta chia cấu Ackerman thành hai cấu bốn khâu lề: A-B-C-D D-E-F-G Mơ hình hóa cấu D-E-F-G: Trong hệ tọa độ 𝑥𝐺𝑦 gọi 𝐷 𝑥1 , 𝑦1 : 𝐹 = 𝑑5 cos 𝜑𝑟 − 𝜋 , 𝑑5 sin 𝜑𝑟 − 𝜋 = 𝑚1 , 𝑛1 𝐸 = 𝑥1 + 𝑑4 cos 𝜑3 , 𝑦1 + 𝑑4 sin 𝜑3 Khoảng cách 𝐵𝐶: 𝐸𝐹 = 𝑥1 + 𝑑4 cos 𝜑3 − 𝑚1 + 𝑦1 + 𝑑4 sin 𝜑3 − 𝑛1 Đặt: 𝑝1 = 𝑥1 − 𝑚1 𝑞1 = 𝑦1 − 𝑛1 38 = 𝑑62 (5.1) Chƣơng Phƣơng trình (5.1) trở thành: 𝑝2 + 𝑞 + 𝑑42 − 𝑑62 + 2𝑝1 𝑑4 cos 𝜑3 + 2𝑞1 𝑑4 sin 𝜑3 = (5.2) Đặt: 𝑎1 = 2𝑞1 𝑑4 𝑏1 = 2𝑝1 𝑑4 𝑐1 = 𝑑62 − 𝑑42 − 𝑞12 − 𝑝12 Thay vào 5.2 ta đƣợc: (5.3) 𝑎1 sin 𝜑3 + 𝑏1 cos 𝜑3 = 𝑐1 Đặt: 𝛼 = atan2 𝑎1 𝑎12 + 𝑏12 , 𝑏1 𝑎12 + 𝑏12 Suy ra: 𝑐1 𝜑3 = acos 𝜑3 = − acos 𝑎12 + 𝑏12 𝑐1 +𝛼 𝑎12 + 𝑏12 +𝛼 Với phƣơng trình quan hệ lƣợng giác, ta thu đƣợc tối thiểu hai nghiệm phân biệt Vì vậy, để đƣa kết luận nghiệm dựa vào vị trí trƣớc góc quay, từ biện luận đƣa nghiệm hợp lý Giải tƣơng tự cho cấu A-B-C-D, ta thu đƣợc: 𝑐2 𝜑1 = acos 𝜑1 = − acos 𝑎22 + 𝑏22 𝑐2 +𝛽 𝑎22 + 𝑏22 +𝛽 Điều kiện: 𝜑2 = 𝜑3 − 90° Giới hạn hệ thống đạt đƣợc A,B,C thẳng hang, góc giới hạn servo tính đƣợc nhƣ sau: 𝑑1 + 𝑑3 cos 𝜑1𝑚𝑎𝑥 = 𝑥2 + 𝑑2 cos 𝜑2 𝑑1 + 𝑑3 sin 𝜑1𝑚𝑎𝑥 = 𝑦2 + 𝑑2 sin 𝜑2 Với 𝐷(𝑥2 , 𝑦2 ) hệ tọa độ 𝑥′𝐴𝑦′ Giải hệ phƣơng trình (4) ta thu đƣợc góc giới hạn 𝜑1𝑚𝑎𝑥 39 Chƣơng 5.2 Xây dựng mơ hình động học hệ thống Từ phần lựa chọn phƣơng án, xe sử dụng cấu lái Ackerman bánh, dƣới sơ đồ nguyên lý đƣợc đề 𝑌 𝜑𝑙 𝐿 𝐼 𝜑𝑡 𝑇 𝑦 𝜑𝑟 𝑅 𝑥 𝐶 2𝑏 𝑙 𝑑 𝜑 𝑂 𝑋 Hình 5.2 Mơ hình hóa hệ thống Gọi điểm tâm xe 𝐶(𝑥, 𝑦), 𝑠 quãng đƣờng chuyển động, 𝑣 vận tốc tính tâm xe 𝜔 vận tốc góc xe, 𝜌 bán kính cong chuyển động tính tâm trắc Ta có phƣơng trình động học tâm xe: 𝑥 sin 𝜑 𝑦 = cos 𝜑 𝜑 0 𝑣 𝜔 Đặt vào hệ thống hệ tọa độ nhƣ hình 5.2, ta có: 𝐼𝐶 = 𝑥 + 𝑏 + 𝑙 cot 𝜑𝑙 sin 𝜑 , 𝑦 + 𝑏 + 𝑙 cot 𝜑𝑙 cos 𝜑 Vận tốc góc xe: 0|𝜑𝑙 = 𝜔= 𝑣𝐶 𝐼𝐶 |𝜑𝑙 ≠ Vận tốc điểm tham chiếu 𝑇 : 40 (5.4) Chƣơng 𝑣𝑇 = 𝑣𝐶 + 𝐼𝑇 ∧ 𝜔 (5.5) Từ phƣơng trình 5.4 , (5.5): 𝑥𝑇 cos 𝜑 −𝑑 sin 𝜑 𝑣 𝑦𝑇 = sin 𝜑 𝑑 cos 𝜑 𝜔 𝜑 Phƣơng trình điểm tham chiếu: 𝑥𝑅 = 𝜈𝑅 𝑐𝑜𝑠 𝜑𝑅 𝑦𝑅 = 𝜈𝑅 𝑠𝑖𝑛 𝜑𝑅 𝜑𝑅 = 𝜔 𝑅 5.3 Mơ hình hóa hệ thống kết hợp sai số e3 𝑌 φR R yR φ e2 𝜑𝑙 e1 𝐿 𝐼 yW 𝑥 𝜑𝑡 𝑇 𝜑𝑟 𝑦 𝑅 𝐶 2𝑏 𝑙 𝑑 𝜑 xW 𝑂 xR 𝑋 Hình 5.3 Mơ hình động học kèm sai số Gọi 𝑒1 , 𝑒2 , 𝑒3 sai số hệ thống Theo hình 3.3, sai số 𝑒 xác định sau: 𝑒1 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 𝑥𝑅 − 𝑥𝑊 𝑒2 = −𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 𝑦𝑅 − 𝑦𝑊 𝑒3 0 𝜑𝑅 − 𝜑𝑊 41 Chƣơng 𝑒1 𝑐𝑜𝑠𝜑(𝑥𝑅 − 𝑥𝑊 ) + 𝑠𝑖𝑛𝜑(𝑦𝑅 − 𝑦𝑊 ) 𝑒2 = −𝑠𝑖𝑛𝜑(𝑥𝑅 − 𝑥𝑊 ) + 𝑐𝑜𝑠𝜑(𝑦𝑅 − 𝑦𝑊 ) 𝑒3 𝜑𝑅 − 𝜑𝑊 Lấy đạo hàm hai vế theo thời gian: 𝑒1 −𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 𝑥𝑅 − 𝑥𝑊 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 𝑥𝑅 − 𝑥𝑊 𝑒2 = 𝜑 −𝑐𝑜𝑠𝜑 −𝑠𝑖𝑛𝜑 𝑦𝑅 − 𝑦𝑊 + −𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 𝑦𝑅 − 𝑦𝑊 𝑒3 0 𝜑𝑅 − 𝜑 𝑊 0 𝜑𝑅 − 𝜑𝑊 𝑒1 𝑒2 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 𝜈𝑅 𝑐𝑜𝑠 𝜑𝑅 − 𝑣𝑐𝑜𝑠𝜑 + 𝜔𝑑𝑠𝑖𝑛𝜑 𝑒2 = 𝜑 −𝑒1 + −𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 𝜈𝑅 𝑠𝑖𝑛 𝜑𝑅 − 𝑣𝑠𝑖𝑛𝜑 − 𝜔𝑑𝑐𝑜𝑠𝜑 𝑒3 𝜔𝑅 − 𝜔 0 𝑒1 𝑒2 𝜈𝑅 𝑐𝑜𝑠 𝜑𝑅 − 𝜑 − 𝑣 𝑒2 = 𝜑 −𝑒1 + 𝜈𝑅 𝑠𝑖𝑛 𝜑𝑅 − 𝜑 − 𝜔𝑑 𝑒3 𝜔𝑅 − 𝜔 𝑒1 𝜈𝑅 𝑐𝑜𝑠 𝑒3 + 𝑒2 𝜔 − 𝑣 → 𝑒2 = 𝜈𝑅 𝑠𝑖𝑛 𝑒3 − 𝑒1 𝜔 − 𝜔𝑑 𝑒3 𝜔𝑅 − 𝜔 𝑒1 𝜈𝑅 𝑐𝑜𝑠 𝑒3 → 𝑒2 = 𝜈𝑅 𝑠𝑖𝑛 𝑒3 𝑒3 𝜔𝑅 −1 𝑒2 𝑣 + −𝑒1 − 𝑑 𝜔 −1 (5.6) 40 30 20 Goc Servo 10 -10 -20 -30 -40 -50 -30 -20 -10 10 20 30 Goc lai Hình 5.4 Đồ thị quan hệ góc lái góc quay động RC servo 42 Chƣơng CHƢƠNG THIẾT KẾ BỘ ĐIỀU KHIỂN Trình bày bƣớc thành lập nên điều khiển hệ thống, từ phƣơng án đề chƣơng 2, điều khiển đƣợc chia thành hai phần: luật điều khiển Lyapunov điều khiển PID động 6.1 Thành lập luật điều khiển Chọn hàm Lyapounov: 1 − 𝑐𝑜𝑠 𝑒3 𝐿 = 𝑒1 + 𝑑 + 𝑒2 + ≥ 0, ∀𝑘2 > 2 𝑘2 Lấy đạo hàm theo thời gian: 𝑒3 𝑠𝑖𝑛𝑒3 𝐿 = 𝑒1 + 𝑑 𝑒1 + 𝑒2 𝑒2 + 𝑘2 Với sai số 𝑒1 , 𝑒2 , 𝑒3 có đƣợc từ phần mơ hình hóa theo cơng thức (5.6): 𝑠𝑖𝑛 𝑒3 𝜔𝑅 − 𝜔 𝐿 = 𝑒1 + 𝑑 𝜈𝑅 𝑐𝑜𝑠 𝑒3 − 𝑣 + 𝑒2 𝜔 + 𝑒2 (𝜈𝑅 𝑠𝑖𝑛 𝑒3 − 𝑒1 𝜔 − 𝜔𝑑 ) + 𝑘2 Chọn: 𝑣 = 𝜈𝑅 𝑐𝑜𝑠 𝑒3 + 𝑘1 𝑒1 + 𝑑 𝜔 = 𝑘2 𝑒2 𝜈𝑅 + 𝜔𝑅 + 𝑘3 𝑠𝑖𝑛 𝑒3 Thế vào, ta đƣợc : 𝐿 ≤ 0, ∀𝑘𝑖 > 0, 𝑖 = .3 Vậy hệ thống ổn định với luật điều khiển chọn 6.2 Mô điều khiển Thông số mô phỏng: Bảng 6.1 Thông số mô Thông số Giá trị Đơn vị Khoảng cách tâm track tâm xe, 𝑑 300 𝑚𝑚 Chiều dài xe, 𝑙 250 𝑚𝑚 Bề rộng xe, 2𝑏 200 𝑚𝑚 Vận tốc tham khảo, 𝑣 0,25 𝑚/𝑠 Vận tốc tối đa, 𝑣𝑚𝑎𝑥 𝑚/𝑠 Thời gian lấy mẫu, 𝑑𝑡 0,02 𝑠 Kết mô phỏng: 43 Chƣơng Reference Model 1.5 y axis (m) 0.5 -0.5 0.5 1.5 2.5 x axis (m) Hình 6.1 Kết bám line -0.05 -0.1 e1 (m) -0.15 -0.2 -0.25 -0.3 -0.35 -0.4 10 12 Time (s) Hình 6.2 Sai số theo trục 𝑥 44 14 16 Chƣơng 0.08 0.06 e2 (m) 0.04 0.02 -0.02 -0.04 -0.06 10 12 14 16 Time (s) Hình 6.3 Sai số theo trục 𝑦 30 20 e3 (degree) 10 -10 -20 -30 10 12 Time (s) Hình 6.4 Sai số góc quay 45 14 16 Chƣơng 1.5 V (m/s) 0.5 -0.5 -1 -1.5 -2 10 12 14 16 14 16 Time (s) Hình 6.5 Vận tốc hoạt động 200 150 p1 (degree) 100 50 -50 -100 -150 -200 10 12 Time (s) Hình 6.6 Góc lái servo 6.3 Thành lập điều khiển PID cho động dẫn động Cấ p mô ̣t giá tri PWM nằ m khoảng điề u khiể n mong muố n (khoảng 1364 vg/ph) ta ̣ có da ̣ng đờ thi ̣vâ ̣n tớ c theo thời gian là: 46 Chƣơng Hình 6.7 Đồ thị đáp ứng vận tốc theo thời gian (𝑇𝑠𝑎𝑚𝑝𝑙𝑒 = 10𝑚𝑠, 𝑃𝑊𝑀 = 90) Dựa vào đồ thi ̣trên , ta xấ p xỉ cả ̣ thố ng (vi điề u khiể n , driver, đô ̣ng cơ) hệ qn tính bâ ̣c có da ̣ng hàm truyề n : K TF = + Ts Sử dụng công cụ Identification System Toolbox, ta đƣợc hàm truyền: G(s) = 17.3762 + 0.026149s Tiêu chí điều khiển: - Settling time: Ts < 0.05s - Steady state error: ess = 5% - Overshoot: M < 7% Dùng PID Tuner của Matlab ta có ̣ số : 𝐾𝑝 = 0,25447; 𝐾𝑖 = 19; 𝐾𝑑 = 0,001 Dựa PID mô phỏng, kết hợp với việc hiệu chỉnh thông số theo thực tế, thông số PID đƣợc sử dụng thực tế 𝐾𝑝 = 0,005; 𝐾𝑖 = 0,002; 𝐾𝑑 = 0,001  - Nhận xét đáp ứng: Settling time: Ts = 0,04318s < 0,05𝑠 Steady state error: ess = 4% < 5% Overshoot: M = 1% 47 Chƣơng Hình 6.8 Đồ thị đáp ứng vận tốc theo thời gian áp dụng điều khiển PID thực tế 6.4 Giải thuật điều khiển 48 Chƣơng Start Cài thông số k ứng với đoạn thằng F – G Cài thông số k ứng với đoạn thằng A – B S Gặp điểm G Đ S Gặp điểm B Cài thông số k ứng với đoạn thằng G – A Đ Cài thông số k ứng với đoạn thằng B – D S Gặp điểm A Đ S Gặp điểm D Robot quay 90 độ Cài thông số k ứng với đoạn thằng A – E Đ Cài thông số k ứng với đoạn thằng D – F S Gặp điểm E Đ S Gặp điểm F Dừng xe Đ Kết thúc Hình 6.9 Giải thuật điều khiển xe dị line sa bàn 49 TÀI LIỆU THAM KHẢO [1] Tzafestas, Spyros G (2013).Introduction to Mobile Robot Control,1st Edition.Elsevier [2] BRANDON (2015) Brandon's line following robot: The Chariot Pololu Blog Disponivel em: [3] Bruno Siciliano & Oussama Khatib (2008) Springer handbook of robotics,chapter 17.2.2-17.2.6.Springer [4] JON (2015).Jon's line following robot: Usain Volt 2.0 Pololu Blog Disponivel em: [5] BROOKS, R (2010) FireBall Disponivel em: [6] ANIKI (2014).CartisX04 Disponivel em: [7] M Zafri Baharuddin et al (2005) Analysis of Line Sensor Configuration for the Advanced Line Follower Robot Universiti Tenaga Nasional [8] Juing-Huei Su et al (2010) An intelligent line-following robot project for introductory robot courses World Transactions on Engineering and Technology Education, Vol.8, No.4, pp 455-461 [9] Nguyễn Hữu Cẩn et al (2005) Lý thuyết ô tô máy kéo Nhà Xuất Bản Khoa Học Và Kỹ Thuật, Hà Nội [10] Trịnh Chất-Lê Văn Uyển Tính tốn thiết kế hệ dẫn động khí, Tập 1, Tập Nhà xuất giáo dục Việt Nam [11] Ninh Đức Tốn Dung sai lắp ghép Nhà xuất giáo dục Việt Nam [12] Vishay - TCRT5000 Datasheet [13] Xiaoling Wu et al An Improved Hardware Design and Navigation Optimization Algorithm for Line Following Robot [14] Richard T Vannoy II, M.S.I.T., and B.S.E.E.T (2008) Designing and Building a Line Following ITT 50 ... (