(Đề tài NCKH) xe hai bánh tự cân bằng

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH CƠNG TRÌNH NGHIÊN CỨU KHOA HỌC SINH VIÊN XE HAI BÁNH TỰ CÂN BẰNG MÃ SỐ:SV2018-62 SKC006729 Tp Hồ Chí Minh, tháng 07/2018 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM BÁO CÁO TỔNG KẾT ĐỀ TÀI NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN XE HAI BÁNH TỰ CÂN BẰNG SV2018-62 Thuộc nhóm ngành khoa học: Kỹ Thuật TP Hồ Chí Minh, 7/2018 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM BÁO CÁO TỔNG KẾT ĐỀ TÀI NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN XE HAI BÁNH TỰ CÂN BẰNG SV2018-62 Thuộc nhóm ngành khoa học: Kỹ thuật SV thực hiện: Phạm Ngọc Thái Nam, Nữ: Nam Dân tộc: Kinh Lớp, khoa:141511, Khoa Điện- Điện Tử Năm thứ: /4 Ngành học: Cơng nghệ kĩ thuật điều khiển tự động hóa Người hướng dẫn: PGS TS Nguyễn Minh Tâm TP Hồ Chí Minh, 7/2018 i CHƯƠNG 1: TỔNG QUAN 1.1Đặt vấn đề 1.2Mục tiêu đề tài 1.3Nội dung đề tài 1.4Giới hạn đề tài 1.5Ý nghĩa thực tiễn khoa học đề t CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1Con lắc ngược 2.2Mơ hình toán học 2.3Thuật toán điều khiển CHƯƠNG 3: TÍNH TỐN VÀ THIẾT KẾ 3.1 Mô mơ hình Matlab Simulink CHƯƠNG 4: THI CÔNG HỆ THỐNG 4.1Phần cứng 4.1.5 Cầu H Hbr-H 200W (bộ điều kh 4.1.8 Mạch hạ áp DC-DC (LM2596) 4.2 Sơ đồ nối dây 4.3 Lưu đồ điều khiển 4.4 Phần mềm 4.5 Mơ hình thực tế CHƯƠNG 5: KẾT QUẢ 5.1 Kết mô 5.2 Kết mơ hình thực tế CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 6.1 Kết luận 6.2 Giới hạn 6.3 Hướng phát triển: TÀI LIỆU THAM KHẢO PHỤ LỤC iii LIỆT KÊ HÌNH VẼ Hình 2.1 Con lắc ngược Hình 2.2 Mơ hình xe hai bánh tự cân Hình 2.3 Quy trình hoạt động lọc Kalman .9 Hình 2.4 Mơ tả đơn giản hệ thống điều khiển 12 Hình 2.5 Bộ diều khiển LQR 14 Hình 3.1 Sơ đồ khối robot tự cân 15 Hình 3.2 Sơ đồ điều khiển LQR cho robot tự cân 20 Hình 4.1 Bộ điều khiển hệ thống 18 Hình 4.2 Pin Lipo 21 Hình 4.3 Arduino Mega 2560 19 Hình 4.4 Sơ đồ khối họ MPU-6000 20 Hình 4.5 Gia tốc kế 23 Hình 4.6 MPU 6050 21 Hình 4.7 Module HBR – 200W 25 Hình 4.8 Bánh xe 26 Hình 4.9 Động NF5475e 22 Hình 4.10 Mạch hạ áp 23 Hình 4.11 Mạch thu phát HC-05 28 Hình 4.12 Sơ đồ nối dây 29 Hình 4.13 Lưu đồ hệ thống 30 iv Hình 4.14 Giao diện Editor Matlab 31 Hình 4.15 Giao diện Simulink 31 Hình 4.16 Giao diện Arduino IDE 28 Hình 4.17 Giao diện điều khiển thiết kế web AppInventor 28 Hình 4.18 Code điều khiển AppInventor (1) 29 Hình 4.19 Code điều khiển AppInventor (2) 29 Hình 4.20 Mơ hình thực tế 30 Hình 5.1 Đồ thị mơ góc tới θ 31 Hình 5.2 Đồ thị mơ góc nghiêng ψ 31 Hình 5.3 Đồ thị mơ góc xoay φ 31 Hình 5.4 Đồ thị thực tế θ 32 Hình 5.5 Đồ thị thực tế ψ 33 Hình 5.6 Đồ thị thực tế φ 33 Hình 5.7 Đồ thị thực tế PWM 34 v LIỆT KÊ BẢNG Bảng 2.1 Thơng số mơ hình xe hai bánh tự cân Bảng 3.1 Bảng thông số thực tế xe 15 Bảng 4.1 Thông số kĩ thuật Arduino Mega 2560 22 Bảng 4.2 Thông số kĩ thuật MPU 6050 24 Bảng 4.3 Thông số động NF5475E 26 vi BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM THÔNG TIN KẾT QUẢ NGHIÊN CỨU CỦA ĐỀ TÀI Thông tin chung: - Tên đề tài: Xe hai bánh tự cân - SV thực hiện: Phạm Ngọc Thái - Lớp: - SV thực hiện: Nguyễn Thành Phát - Lớp: - Người hướng dẫn: PGS TS Nguyễn Minh Tâm Mục tiêu đề tài: Từ yêu cầu thực tế đào tạo Nhà trường phát triển khoa học công nghệ, đổi công tác đào tạo, nâng cao chất lượng đào tạo, nhìn thấy tầm quan trọng mơi trường nghiên cứu với đầy đủ trang thiết bị, đề tài thức giúp cho Lab có thêm mơ hình để thí nghiệm đồng thời tài liệu để bạn sinh viên khóa sau tiếp tục nghiên cứu phát triển Hồn thành mơ hinh̀ xe hai bánh tự cân điều khiển Bluetooth làmucc̣ tiêu đềtài Tính sáng tạo: Cùng với phát triển giới khoa học công nghệ từ dẫn chứng tình hình nghiên cứu ngồi nước, ta thấy tầm quan việc nghiên cứu, phát triển ứng dụng đề tài khoa học vào sống cần thiết sinh viên Hiện phịng thí nghiệm bơ c̣mơn điều khiển tự động hóa Trường Đại học Sư Phạm Kỹ Thuật có nhiều thí nghiệm gồm: Bộ điều khiển tốc độ động cơ, vị trí động cơ, hệ lò nhiệt, hệ bồn nước, Pendubot, Pendulum, Nhưng chưa có thí nghiệm xe bánh tự cân điều khiển thông qua Bluetooth vii Nhận thấy điều đó, nhóm định thực đề tài nghiên cứu khoa học đề tài Kết nghiên cứu: Hồn thành mơ hình xe hai bánh tự cân thời gian qui định Mơ hình hóa hệ thống, mơ điều khiển mơ hình xe hai bánh tự cân xử dụng điều khiển LQR Matlab Simulink Xe tự cân điều khiển từ xa mà trì trạng thái cân Nghiên cứu biết thêm giải thuật LQR lọc Kalman Vì kiến thức chun mơn chưa sâu nên xe cịn gặp số hạn chế: Chưa đồng liệu từ Matlab tới vi điều khiển giải thuật LQR Điều khiển tới lui chưa thật mượt hạn chế phần cứng Xe bị dao động mạnh tăng tải Đóng góp mặt giáo dục đào tạo, kinh tế - xã hội, an ninh, quốc phòng khả áp dụng đề tài: Ứng dụng phịng thí nghiệm mơn cơng nghệ kĩ thuật điều khiển tự động hóa trường Đại Học Sư Phạm Kỹ Thuật Tp HCM Công bố khoa học SV từ kết nghiên cứu đề tài (ghi rõ tên tạp chí có) nhận xét, đánh giá sở áp dụng kết nghiên cứu (nếu có): Mơ hình hoạt động tốt, ổn định ứng dụng để chứng minh giải thuật điều khiển tự động cho sinh viên nghiên cứu khoa học Ngày tháng năm SV chịu trách nhiệm thực đề tài (kí, họ tên) viii CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 6.1 Kết luận Đề tài hoàn thành thời gian qui định Xe tự cân điều khiển từ xa mà trì trạng thái cân Nghiên cứu biết thêm giải thuật LQR lọc Kalman Chưa đồng liệu từ Matlab tới vi điều khiển giải thuật LQR Vì kiến thức chuyên mơn chưa sâu nên xe cịn gặp số hạn chế: Khi chịu tác động ngoại lực lớn, xe dễ cân ngã Điều khiển tới lui chưa thật mượt hạn chế phần cứng Xe cịn bị dao động mạnh tăng tải 6.2 Giới hạn - Robot mang vật nặng - Robot làm việc tốt ngã tác động lực mạnh - Robot không phản hồi 6.3 Hướng phát triển: Trong tương lai, chúng em dự định phát triển robot theo hướng: - Tối ưu điều khiển LQR thơng số mơ hình để robot làm việc tốt - Phát triển thêm giải thuật khác (PID, Fuzzy…) - Phát triển thêm khả khác xe tránh, dừng gặp vật cản - Tích hợp thêm nhiều cảm biến xử lý ảnh, camera, GPS… để bám định vị đối tượng BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 40 TÀI LIỆU THAM KHẢO TÀI LIỆU THAM KHẢO [1] Kalman filter Retrieved November 23, 2009, from Wikipedia: http://en.wikipedia.org/wiki/Kalman_filter 69 Wikipedia (2009, September 13) [2] Nguyễn Minh Tâm, Nguyễn Văn Đông Hải, Nguyễn Phong Lưu, Lê Văn Tuấn, “Modelling and Optimal Control for Two-wheeled Self-Balancing Robot”, Trường Đại Học Sư Phạm Kĩ Thuật, 12-2015 [3] PGS.TS Huỳnh Thái Hoàng, “Lý thuyết điều khiển nâng cao”, Bộ Môn Điều khiển tự động, Đại học Bách Khoa TPHCM, 15-1-2014 [4] Rich Chi Ooi, “Balancing a Two-Wheeled Autonomous Robot”, The University of Western Australia, 2003 BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 41 PHỤ LỤC PHỤ LỤC Phụ lục I: Code điều khiển robot: gồm: - Vịng lặp Các hàm điều khiển động cơ, hàm ngắt dùng đọc xung encoder bánh trái phải - Hàm tính tốn LQR - Hàm nhận tín hiệu điều khiển từ Bluetooth /* * Final thesis: Two-wheeled balancing robot using LQR control * Control via Bluetooth */ #include // Support I2C library #include "Kalman.h" // Kalman library #define ToRad PI/180 #define ToDeg 180/PI kalman kalman; // kalman filter define: kalman getAngle(pitch, gyrorate, dt); #define factortheta PI*10 // Set point for angle forward of the robot #define factorphi PI // Set point for angle rotate of the robot int inChar; // Read data send from board Bluetooth uint32_t timerloop, timerold; // Create sampling cycle for theta, psi, phi // Motor control Pin // / #define leftpwm #define leftdir / Control PWM of the left Motor Control direction left Motor BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 42 PHỤ LỤC #define rightpwm 10 / Control PWM of the right Motor //10 #define rightdir / Control direction right Motor volatile long leftencoder; / Read and save value of left encoder volatile long rightencoder; / Read and save value of right encoder #define leftencoder_a / Read state of encoder channel A, LOW or HIGH / Read state of encoder channel A, LOW or HIGH #define leftencoder_b 11 #define rightencoder_a #define rightencoder_b // MPU6050 Data // float mpudata; float / Contain psi angle ( Y axis) AcX, AcZ; float / The values of Gyroscope following X axis and Z axis Gyro; long offset; / Angular acceleration following Y axis / Offset value of MPU6050 uint32_t timer; uint8_t i2cData[14]; // Save values read from MPU // LQR data // long PWML, PWMR; float k1, k2, k3, k4, k5, k6; // the factor of K matrix bool falldown = 0; float theta, psi, phi; float thetadot, psidot, phidot; float thetaold, psiold, phiold; float leftvolt; BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 43 PHỤ LỤC float rightvolt; // output Voltage of right Motor in LQR (u matrix) float addtheta; // Save setpoint value which help robot move and cling to it float addphi; // Save setpoint value which help robot move and cling to it int ForwardBack; // -> Forward; -1 -> Back; int LeftRight; -> Stop and Balacing // -> Turnleft; -1 -> Turnright; -> Stop and Balacing //////////////////////////////////////////////////// ////////////////// SERIAL BEGIN //////////////////// //////////////////////////////////////////////////// void setup() { kalman.setQangle(0.001); kalman.setQbias(0.003); kalman.setRmeasure(0.03); // ouput pin control motor left and right // pinMode(leftpwm, OUTPUT); pinMode(leftdir, OUTPUT); pinMode(rightpwm, OUTPUT); pinMode(rightdir, OUTPUT); // intput pin read encoder // pinMode(leftencoder_a, INPUT_PULLUP); pinMode(rightencoder_a, INPUT_PULLUP); pinMode(leftencoder_b, INPUT_PULLUP); pinMode(rightencoder_b, INPUT_PULLUP); Serial.begin(115200); // Connect to PC via USB port BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 44 PHỤ LỤC Serial3.begin(9600); // Data MPU6050 // Wire.begin(); //setupMPU(); TWBR = ((F_CPU / 400000UL) - 16) / 2; i2cData[0] = 7; i2cData[1] = 0x00; i2cData[2] = 0x00; i2cData[3] = 0x00; while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at the same time while (i2cWrite(0x6B, 0x01, true)); disable while (i2cRead(0x75, i2cData, 1)); if (i2cData[0] != 0x68)// Read "WHO_AM_I" register { Serial.print(F("Error reading sensor")); while(true); } delay(100); while (i2cRead(0x3B, i2cData, 6)); AcX = (int16_t)((i2cData[0] 0) { inChar = Serial3.read(); Serial.println(4); } else inChar = 0; serialEvent1(); addtheta = addtheta + ForwardBack*factortheta; // For/Back // theta la rad addphi = addphi + LeftRight*factorphi; // Left/Right getlqr(theta + addtheta, thetadot, psi, psidot, phi + addphi, phidot); motorcontrol(PWML, PWMR, (mpudata + offset), falldown) } } // left motor encoder interrupt // void left_isr() BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG 47 PHỤ LỤC { if(digitalRead(leftencoder_b) == 1) leftencoder++; else leftencoder ; } / right motor encoder interrupt// void right_isr() { if(digitalRead(rightencoder_b) == 1) rightencoder ; else rightencoder++; } / Read psi // void readMPU6050() { while (i2cRead(0x3B, i2cData, 14)); AcX = (int16_t)((i2cData[0]