Thiết kế điều khiển mô hình Drone 4 động cơ, sử dụng bộ điều khiển PID cho hệ thống, dùng bộ lọc kalman để lọc nhiễu cảm biến góc nghiêng.
BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP.HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN ROBOT THIẾT KẾ VÀ ĐIỀU KHIỂN MÁY BAY KHÔNG NGƯỜI LÁI (DRONE) Giảng viên hướng dẫn: TS Trần Đức Thiện Nhóm: SVTH: Bùi Tấn Tài Phạm Văn Thắng , tháng… năm…… 19151013 19151175 CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc *** -Tp Hồ Chí Minh, ngày 27 tháng 12 năm 2022 NHIỆM VỤ BÁO CÁO ĐỒ ÁN ROBOT Họ tên sinh viên: Bùi Tấn Tài Phạm Văn Thắng MSSV: 19151013 MSSV: 19151175 Ngành: Công nghệ kỹ thuật Điều khiển & Tự động hóa Lớp: 19151CL2B Giảng viên hướng dẫn: TS Trần Đức Thiện ĐT: Ngày nhận đề tài: Ngày nộp đề tài: Tên đề tài: THIẾT KẾ VÀ ĐIỀU KHIỂN MÁY BAY KHÔNG NGƯỜI LÁI (DRONE) Các số liệu, tài liệu ban đầu: Nội dung thực đề tài: Sản phẩm: TRƯỞNG NGÀNH GIẢNG VIÊN HƯỚNG DẪN i ĐỒ ÁN ROBOT ii ĐỒ ÁN ROBOT ii ĐỒ ÁN ROBOT PHÂN CÔNG NHIỆM VỤ Thành viên Bùi Tấn Tài Nhiệm vụ Tìm tài liệu, thi cơng phần cứng, làm Đã hoàn word, làm powerpoint Phạm Văn Thắng thành Viết code, thi cơng phần cứng, hỗ trợ Đã hồn word thành ii ĐỒ ÁN ROBOT Tiến độ NHẬN XÉT ĐÁNH GIÁ Giảng viên hướng dẫn TS Trần Đức Thiện iii ĐỒ ÁN ROBOT LỜI CẢM ƠN Nhóm chúng em xin cảm ơn sâu sắc đến thầy Ts Trần Đức Thiện giảng viên môn Thực tập robot Thầy nhiệt tình hỗ trợ nhóm mặt suốt trình thực đề tài Thầy cung cấp kiến thức quý giá để nhóm hồn thành đề tài với kết tốt Em xin cảm ơn thầy cô môn Điều khiển tự động trường Đại học Sư phạm Kỹ thuật TP.HCM trang bị cho chúng em kiến thức tảng để chúng em thực đồ án Do kiến thức hạn chế nên báo cáo nhóm có vài điểm thiếu sót Chúng em mong nhận bảo, đóng góp ý kiến từ thầy để chúng em bổ sung hoàn thiện báo cáo tốt Nhóm em xin chân thành cảm ơn iii ĐỒ ÁN ROBOT MỤC LỤC LỜI CẢM ƠN v DANH MỤC HÌNH ẢNH vi DANH MỤC BẢNG viii CHƯƠNG TỔNG QUAN 1.1 ĐẶT VẤN ĐỀ 1.1.1 Giới thiệu máy bay không người lái 1.1.2 Các ứng dụng máy bay không người lái 1.2 MỤC TIÊU 1.3 NỘI DUNG NGHIÊN CỨU .6 1.4 GIỚI HẠN 1.5 BỐ CỤC CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1 PHÂN TÍCH HOẠT ĐỘNG CỦA MƠ HÌNH DRONE 2.1.1 Nguyên lý hoạt động chung drone .7 2.1.2 Lý thuyết điều khiển Drone 2.2 MƠ HÌNH TỐN CỦA DRONE .9 2.3 LÝ THUYẾT VỀ MPU6050 11 2.3.1 Ước tính vị trí 11 2.3.2 Ước tính gia tốc 11 2.4 ARDUINO IDE VÀ GIẢI THUẬT ĐIỀU KHIỂN 13 2.4.1 Giới thiệu phần mềm Arduino IDE 13 2.4.2 Giải thuật điều khiển 13 CHƯƠNG 3: THIẾT KẾ MƠ HÌNH 23 3.1 SƠ ĐỒ KHỐI 23 3.2 GIỚI THIỆU VÀ LỰA CHỌN THIẾT BỊ .23 3.2.1 Arduino 23 3.2.2 Động pha himodel 25 3.2.3 Cặp cánh quạt Azure Power Johnny 4838 .26 3.2.4 Mạch điều khiển động ESC Blhheli 30A 27 3.2.5 Mạch nguồn Matek 28 3.2.6 Cảm biến góc nghiêng MPU 6050 29 3.3 THỐNG KÊ CHI PHÍ THIẾT BỊ 32 3.4 THIẾT KẾ KHUNG ĐỠ CHO DRONE 34 3.4.1 Bản vẽ 2D 34 3.4.3 Hình vẽ 3D .34 iv ĐỒ ÁN ROBOT CHƯƠNG 4: THI CÔNG HỆ THỐNG .36 4.1 LẮP RÁP PHẦN CỨNG 36 4.2 LẬP TRÌNH ĐIỀU KHIỂN HỆ THỐNG .39 4.2.1 Lưu đồ giải thuật 39 4.2.2 Tìm lực nâng 40 4.2.3 Chương trình Arduino 41 4.2.4 Đáp ứng mơ hình hoạt động .46 CHƯƠNG 5: KẾT QUẢ NHẬN XÉT ĐÁNH GIÁ 54 TÀI LIỆU THAM KHẢO 55 PHỤ LỤC 56 v ĐỒ ÁN ROBOT DANH MỤC HÌNH ẢNH Hình 1: Máy bay khơng người lái giao hàng Hình 2: Taxi Drone Hình 3: Drone cứu hộ Hình 4: Drone nông nghiệp Hình 1: Chuyển động Drone .8 Hình 2: Hệ quy chiếu A B với chiều dài trục L, tổng khối lượng mơ hình m Hình 3: Mơ hình momen quay 10 Hình 4: Phần mềm Arduino IDE 13 Hình 5: Hình minh họa cho lọc thông thấp 14 Hình 6: Sơ đồ lọc thơng thấp 15 Hình 2.7: Thanh ghi 0x1A MPU 6050 .16 Hình 8: Sơ đồ khối lọc Kalman 17 Hình 9: Thuật toán lọc Kalman 18 Hình 10: Sơ đồ lọc Kalman 18 Hình 11: Sơ đồ khối điều khiển PID cho Drone 20 Hình 12: Luật định PID 21 Hình 1: Sơ đồ khối hệ thống 21 Hình 2: Sơ đồ chức chân Arduino uno R3 .23 Hình 3: Động pha himodel 23 Hình 4: Cánh quạt Azure Power Johnny 4838 25 Hình 5: ESC Blhheli 30A 26 Hình 6: Mạch phân nguồn .27 Hình 7: Cảm biến góc nghiêng 28 Hình 8: Khung drone .28 Hình 9: Pin Spider Lipo 3S 2200 mah 25C 29 Hình 10: Bản vẽ 2D trục xoay test bench 32 Hình 11: Bản vẽ 2D Test bench 32 Hình 12: Test bench drone 33 Hình 1: Kết nối động với ESC 34 Hình 2: Kết nối ESC với mạch phân nguồn 34 Hình 3: Kết nối Pin với mạch phân nguồn 35 Hình 4: Kết nối MPU 6050 với Arduino .35 Hình 5: Kết nối dây tín hiệu ESC với Arduino .36 Hình 6: Mơ hình drone hồn chỉnh .36 Hình 7: Sơ đồ kết nối dây 37 Hình 14: Lưu đồ giải thuật chương trình .38 Hình 15: Đáp ứng lực đẩy động 39 Hình 16: Đáp ứng hệ thống Kp, Kd Ki 44 Hình 17: Đáp ứng hệ thống Kp=2 45 Hình 18: Đáp ứng hệ thống Kp=5 45 Hình 19: Đáp ứng hệ thống Kp=2, Kd=1 45 Hình 20: Đáp ứng hệ thống Kp=5, Kd=0.85 46 Hình 21: Đáp ứng hệ thống Kp=5, Kd=0.85, Ki=0.01 46 vi ĐỒ ÁN ROBOT CHƯƠNG 4: THI CÔNG HỆ THỐNG Hình 9: Đáp ứng lực đẩy động Với tổng khối lượng drone 640g nên nhóm e chọn tốc độ quay cho động giống với tốc độ cấp xung 1320us 4.2.3 Chương trình Arduino #include #include #include #include #define RESTRICT_PITCH #define runEvery(t) for (static uint16_t _lasttime;\ (uint16_t)((uint16_t)millis()- _lasttime) >= (t);\ _lasttime += (t)) Kalman kalmanX; Kalman kalmanY; /* IMU Data */ double accX, accY, accZ; double gyroX, gyroY, gyroZ; int16_t tempRaw; double gyroXangle, gyroYangle; double compAngleX, compAngleY; double kalAngleX, kalAngleY; Servo Servo Servo Servo ESC_R1; ESC_L1; ESC_R2; ESC_L2; 43 ĐỒ ÁN ROBOT CHƯƠNG 4: THI CÔNG HỆ THỐNG float float float float PID, pwmLeft, pwmRight, error, previous_error; pid_p=0; //6 1350 pid_i=0; pid_d=0; /////////////////PID CONSTANTS///////////////// double kp=6;//17 double ki=0.02;//0.35 double kd=0.85;// 0.92 /////////////////////////////////////////////// double MaxSpeed = 1500; double MinSpeed = 1000; double throttle = 1320; //initial value of throttle to the motors float desired_angle = 0; //This is the angle in which we whant the //balance to stay steady uint32_t timer; uint8_t i2cData[14]; void setup() { Serial.begin(115200); Wire.begin(); #if ARDUINO >= 157 Wire.setClock(400000UL); #else TWBR = ((F_CPU / 400000UL) - 16) / 2; #endif i2cData[0] = 7; i2cData[1] = 0x00; i2cData[2] = 0x00; i2cData[3] = 0x00; while (i2cWrite(0x19, i2cData, 4, false)); while (i2cWrite(0x6B, 0x01, true)); while (i2cWrite(0x1A, 0x03, true)); while (i2cRead(0x75, i2cData, 1)); if (i2cData[0] != 0x68) { Serial.print(F("Error reading sensor")); while (1); } delay(100); /* Set kalman and gyro starting angle */ while (i2cRead(0x3B, i2cData, 6)); accX = (int16_t)((i2cData[0] MaxSpeed)pwmLeft=MaxSpeed; ESC_L1.writeMicroseconds(pwmLeft); ESC_R1.writeMicroseconds(pwmRight); ESC_L2.writeMicroseconds(pwmLeft); ESC_R2.writeMicroseconds(pwmRight); previous_error = error; } } 4.2.4 Đáp ứng mơ hình hoạt động 4.2.4.1 Kiểm tra góc Roll Ban đầu ta cho tất Kp, Kd Ki chạy chương trình 48 ĐỒ ÁN ROBOT CHƯƠNG 4: THI CÔNG HỆ THỐNG Hình 10: Đáp ứng hệ thống Kp, Kd Ki Sau ta tăng dần Kp sai số giao động Hình 11: Đáp ứng hệ thống Kp=2 Hình 12: Đáp ứng hệ thống Kp=5 49 ĐỒ ÁN ROBOT CHƯƠNG 4: THI CÔNG HỆ THỐNG Sau thấy giao động ta bắt đầu thêm Kd vào hệ thống sai số có giao động nhỏ Hình 13: Đáp ứng hệ thống Kp=2, Kd=1 Hình 14: Đáp ứng hệ thống Kp=5, Kd=0.85 Và cuối thêm Ki để hệ thống chỉnh sửa lại sai số nhỏ Hình 15: Đáp ứng hệ thống Kp=5, Kd=0.85, Ki=0.01 50 ĐỒ ÁN ROBOT CHƯƠNG 4: THI CÔNG HỆ THỐNG Sau ta thử đáp ứng hệ thống Hình 16: Đáp ứng hệ thống tác động sang trái Hình 17: Đáp ứng hệ thống tác động sang trái Ảnh hưởng lọc đến đáp ứng hệ thống: Hình 18: Đáp ứng cảm biến khơng có lọc 51 ĐỒ ÁN ROBOT CHƯƠNG 4: THI CƠNG HỆ THỐNG Đáp ứng tính hiệu cảm biến khơng sử dụng lọc có nhiễu làm ổn định hoạt động hệ thống 52 ĐỒ ÁN ROBOT CHƯƠNG 4: THI CÔNG HỆ THỐNG Hình 19: Đáp ứng cảm biến có lọc Kalman Hình 20: Đáp ứng chạy động khơng lọc Kalman Hình 22: Đáp4.ứng cóđộng lọc thơng khơng lọc Kalman Hình 21:khi Đápchạy ứngđộng chạy có thấp, lọc Kalman 53 ĐỒ ÁN ROBOT CHƯƠNG 4: THI CÔNG HỆ THỐNG Nhận xét : Sau dùng phương pháp thử sai để tìm thơng số PID cho hệ thống sử dụng hai lọc thơng thấp kalman lọc nhiễu cho tín hiệu cảm biến tìm thơng số PID cho góc roll drone hoạt động cân bằng, có tác động drone đáp ứng tốt trở vị trí cân với thời gian xác lập khoảng 1s sai số xác lập 2-3 độ nghiêng độ vọt lố thấp 4.2.4.2 Kiểm tra góc Pitch Drone tự hoạt động cân Kp=8, Ki=0.02, Kd=0.78 Hình 24: Đáp hệ thống Kp=8, Ki=0.02, Kd=0.78 Sau drone tự hoạt động vị trí cân ta tác động bên trái phải để xem đáp ứng hệ thống tốt khơng Hình 25: Đáp hệ thống Kp=8,54 Ki=0.02, Kd=0.78 tác động sang trái ĐỒ ÁN ROBOT CHƯƠNG 4: THI CÔNG HỆ THỐNG Nhận xét : Sau dùng phương pháp thử sai để tìm thơng số PID cho hệ thống sử dụng hai lọc thông thấp kalman lọc nhiễu cho tín hiệu cảm biến tìm thơng số PID cho góc pitch drone hoạt động cân bằng, có tác động drone đáp ứng tốt trở vị trí cân với thời gian xác lập độ vọt lố thấp 4.2.4.1 Kiểm tra hệ thống ballhead kết hợp hai trục pitch roll Hình 27: Đáp ứng hệ thống kết hợp hai trục pitch roll Nhận xét: Hệ thống hoạt động cân kết hợp hai trục pitch roll với với sai sô xác lập thời gian xác lập thấp 55 ĐỒ ÁN ROBOT CHƯƠNG 5: KẾT QUẢ NHẬN XÉT ĐÁNH GIÁ CHƯƠNG 5: KẾT QUẢ NHẬN XÉT ĐÁNH GIÁ Đánh giá, nhận xét kết đạt được: Sau trình nghiên cứu, tìm tịi, tham khảo tài liệu thơng số kỹ thuật Nhóm thực thành cơng đề tài, q trình nhóm nghiệm thu số kết sau: Thiết kế, vẽ phác họa thành cơng mơ hình máy bay khơng lái tự cân phần mềm chuyên dụng Solidworks Sử dụng thành thạo điều khiển PID Biết công dụng lọc thông thấp, lọc bù lọc Kalman Được hiểu thêm mã lập trình arduino Tìm thông số PID cho hệ drone hoạt động cân hai trục Roll, Pitch, Yaw cân Hướng phát triển: - Phát triển cho drone chạy cân lúc trục Roll, Pitch Yaw - Nghiên cứu phát triển cho drone bay được, tích hợp định vị GPS xử lý ảnh vào mơ hình Nhận xét : Sau dùng phương pháp thử sai để tìm thơng số PID cho hệ thống sử dụng hai lọc thông thấp kalman lọc nhiễu cho tín hiệu cảm biến tìm thơng số PID cho góc roll tối ưu cho hệ drone tự cân với thời gian xác lập khoảng 1s sai số xác lập 2-3 độ nghiêng 56 ĐỒ ÁN ROBOT TÀI LIỆU THAM KHẢO TÀI LIỆU THAM KHẢO [1] Drone Technology: What Is a Drone?, https://builtin.com/drones [2] The Future of Drones – Areas To Watch, https://yourdronereviews.com/future-ofdrones [3] Trends In Drone Development – Uses And Future Of Drones, https://tecsmash.com/trends-in-drone-development/ [4] Cấu tạo nguyên lý hoạt động Drone, https://by.com.vn/HQP2d [5] MesiDas, Tổng quan bo mạch vi điều khiển Arduino, https://by.com.vn/y2C68 [6] Lâm Ngọc Tâm, Trần Xuân Tùy (2012) Thiết kế chế tạo mơ hình máy bayQuadrocopter Tuyển tập Báo cáo Hội nghị Sinh viên Nghiên cứu Khoa học lần thứ Đại học Đà Nẵng [7] Nguyên lý hoạt động quadcoper, https://dien.donga.edu.vn/chi-tiet-bai-viet/nguyen-ly-hoat-dong-cua-mot-quadcoper-16637 [8] Giới thiệu Arduino UNO R3, https://thanhtukgc.blogspot.com/2020/11/bai-1-gioithieu-ve-arduino-uno-r3.html [9] What Are The Benefits of Brushless Motors for Drones, https://www.writersevoke.com/what-are-the-benefits-of-brushless-motors-for-drones/ #What_is_the_Brushless_Motor [10] Bùi Đình Cường, Nguyễn Khánh Hưng (2012) Tìm hiểu lý thuyết ứng dụng lọc Kalman Trường Đại học Bách Khoa Hà Nội Viện công nghệ thông tin truyền thông [11] Bộ lọc Kalman, https://tailieumienphi.vn/doc/bo-loc-kalman-3d9ptq.html [12] Đặng Lâm Tùng (2019) Kalman filter hay Complementary filter [13] H.-Q.-T Ngo, T.-P Nguyen, V.-N.-S Huynh, T.-S Le and C.-T Nguyen Experimental Comparison of Complementary Filter and Kalman Filter Design for Low-Cost Sensor in Quadcopter 57 ĐỒ ÁN ROBOT