Hcmute 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 SKC 0 Tp Hồ Chí Minh, tháng 07/2018 Luan van 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 Luan van 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 Luan van MỤC LỤC CHƯƠNG 1: TỔNG QUAN 1.1 Đặt vấn đề 1.2 Mục tiêu đề tài 1.3 Nội dung đề tài 1.4 Giới hạn đề tài .2 1.5 Ý nghĩa thực tiễn khoa học đề tài .2 CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1 Con lắc ngược 2.2 Mơ hình tốn học 2.3 Thuật toán điều khiển 2.3.1 Bộ lọc Kalman 2.3.2 Lý thuyết điều khiển 12 CHƯƠNG 3: TÍNH TỐN VÀ THIẾT KẾ 15 3.1 Mơ mơ hình Matlab Simulink 15 CHƯƠNG 4: THI CÔNG HỆ THỐNG 21 4.1 Phần cứng 21 4.1.1 Bộ điều khiển hệ thống 21 4.1.2 Nguồn .21 4.1.3 Vi điều khiển 21 4.1.4 Cảm biến gia tốc .23 4.1.5 Cầu H Hbr-H 200W (bộ điều khiển động cơ) 24 ii Luan van 4.1.6 Bánh xe .25 4.1.7 Động NF5475e 26 4.1.8 Mạch hạ áp DC-DC (LM2596) 27 4.1.9 Mạch thu phát Bluetooth HC-05 28 4.2 Sơ đồ nối dây .28 4.3 Lưu đồ điều khiển 29 4.4 Phần mềm 30 4.4.1 Matlab Simulink .30 4.4.2 Arduino IDE .31 4.4.3 MIT App Inventor 32 4.5 Mô hình thực tế 34 CHƯƠNG 5: KẾT QUẢ .35 5.1 Kết mô 35 5.2 Kết mô hình thực tế .36 CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN .40 6.1 Kết luận 40 6.2 Giới hạn 40 6.3 Hướng phát triển: .40 TÀI LIỆU THAM KHẢO 41 PHỤ LỤC 42 iii Luan van 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 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 Luan van 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 Luan van 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 Luan van 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: 141511 Khoa: Điện- Điện Tử Mã số SV: 14151093 Năm thứ: - SV thực hiện: Nguyễn Thành Phát - Lớp: 141511 Khoa: Điện- Điện Tử Số năm đào tạo: Mã số SV: 14151077 Năm thứ: Số năm đào tạo: - 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 Hoàn thành mơ hình xe hai bánh tự cân điều khiển Bluetooth là mu ̣c tiêu của đề tài này 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 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ơ ̣ 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 Luan van 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 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 cịn 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 Luan van CHƯƠNG KẾT QUẢ - Hướng lên trên: xe chạy theo chiều dương - Hướng xuống dưới: xe chạy theo chiều âm BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG Luan van 39 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 chun 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 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 Luan van 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 Hồ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 Luan van 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 Luan van 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 #define leftencoder_b 11 #define rightencoder_a // Read state of encoder channel A, LOW or HIGH #define rightencoder_b // MPU6050 Data // float mpudata; // Contain psi angle ( Y axis) float AcX, AcZ; // The values of Gyroscope following X axis and Z axis float Gyro; // Angular acceleration following Y axis long offset; // Offset value of MPU6050 uint32_t timer; // Update and calculate the sample time and read sample time and data from MPU as well, Timer for kalman filter psi angle uint8_t i2cData[14]; // Save values read from MPU // LQR data // long PWML, PWMR; // PWM output for H-Brigde float k1, k2, k3, k4, k5, k6; // the factor of K matrix bool falldown = 0; // Run = true; Stop = false, define to control the motor float theta, psi, phi; // Angle float thetadot, psidot, phidot; // Angular velocity of these variable float thetaold, psiold, phiold; // Value of these variable which was created period before float leftvolt; // output Voltage of left Motor in LQR (u matrix) BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG Luan van 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 Luan van 44 PHỤ LỤC Serial3.begin(9600); // Serial connect to bluetooth // Data MPU6050 // Wire.begin(); //setupMPU(); TWBR = ((F_CPU / 400000UL) - 16) / 2; i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz i2cData[1] = 0x00; // Disale FSYNC and set 260Hz Accelerator filtering, 256 Hz Gyro filtering, 8Khz sampling i2cData[2] = 0x00; // Set Gyro full scale range to +/-250deg/s i2cData[3] = 0x00; // Set Gyro full scale range to +/-2g while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at the same time while (i2cWrite(0x6B, 0x01, true)); disable // PLL with X Axis Gyroscope reference and sleep mode 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 Luan van 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]